#include "libcs225.h"
#include "matrix/PrVector3.h"
#include "matrix/PrMatrix3.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#define CLOCK_RATE      500
#define MESSAGE_SIZE    80

#define MBREAK 0
#define MFLOAT 1

static int cmode;

static float time;
static float q[3];
static float dq[3];
static float tau[3];

static float kp[3];
static float kv[3];

void InitGlobals();
void HandleUserMesg();
void HandleGatherMesg();
void HandleControlMode();

void BreakLaw();
void FloatLaw();

/* sample usage of matrix classes */
void MatrixTest();

/* 
 * servo loop 
 * runs forever until you type "exit".
 */
void main()
{
  float tick = 1.0/CLOCK_RATE;

  InitGlobals();

  InitManipulator(CLOCK_RATE);

  // sample matrix test routine
  // comment out the next line
  MatrixTest();

  while(1)
    {
      HandleGatherMesg();

      HandleUserMesg();

      time += tick; //time = GetTime();

      GetPos(q);

      GetVel(dq);

      HandleControlMode();

      SetTorque(tau);

      WaitNextTick();
    }
}

/*
 * make sure to initialize all global variables.
 */
void InitGlobals()
{
  time = 0.0;
  cmode=MFLOAT;

  for(int i=0;i<3;i++)
    {
      q[i]=0.0;
      dq[i]=0.0;
      tau[i]=0.0;
      kp[i]=16.0;
      kv[i]=8.0;
    }
}

/*
 * Function HandleUserMesg
 *
 * This is a simple example of a parser used to read command messages
 * from the user process. You are free to change this function in any
 * way you see fit.
 */
void HandleUserMesg()
{
  char mesg[MESSAGE_SIZE];

  memset(mesg,NULL,MESSAGE_SIZE);
  int n=RecvUserMessage(mesg,MESSAGE_SIZE);
  if (n<=0)
    return;

  int i;
  float k;

  switch(mesg[0])
    {
    case 'e':
      if (strncmp(mesg,"exit",4)==0)
	cmode = MBREAK;
      break;
    case 'f':
      if (strncmp(mesg,"float",5)==0)
	cmode = MFLOAT;
      break;
    case 'k':
      switch (mesg[1])
	{
	  /* Set gains kp & kv used in Lab 3 and on
	   * example: kp1 100.0
	   *
	   * sets kp[0], the positional gain on joint 1 to 100.0
	   */
	case 'p':
	  sscanf(mesg+2,"%d %f",&i,&k);
	  kp[i-1] = k;
	  break;
	case 'v':
	  sscanf(mesg+2,"%d %f",&i,&k);
	  kv[i-1] = k;
	  break;
	}
      // printf("%d %f\n",i,k);
      break;
    default:
      printf("error: unknown command\n");
      break;
    }
}

/*
 * Eventually you will want to add code to select how you will
 * gather information for output.
 */
void HandleGatherMesg()
{

}

/*
 * a simple selector used to select from a variety of implemented
 * control laws.
 */
void HandleControlMode()
{
  switch(cmode)
    {
    case MBREAK:
      BreakLaw();
      break;
    case MFLOAT:
      FloatLaw();
      break;
      /* Add more control modes here */
    default:
      printf("error: unknown mode %d\n",cmode);
      cmode = MFLOAT;
      break;
    }
}

void BreakLaw()
{
  ShutDown();
  exit(1);
}

void FloatLaw()
{
  /* lab2: you need to write this function */
  tau[0] = 0.0;
  tau[1] = 0.0;
  tau[2] = 0.0;
}

void MatrixTest()
{
  PrVector3 vec;
  vec[0]=1.0;
  vec[1]=2.0;
  vec[2]=3.0;

  vec.display("vec");
  printf("\n");

  PrMatrix3 mat;
  for(int i=0;i<3;i++)
    for(int j=0;j<3;j++)
      mat[i][j] = (float)(i+j);
  mat.display("mat");

  PrVector3 vec2;
  // vec2 = mat * vec
  mat.multiply(vec,vec2);

  vec2.display("vec2");
  printf("\n");
}

