/*
* File:   libcs225.cpp
*
* This file implements libcs225.h
*/

/*
* modification history
*----------------------
*
* 07/29/98: K.C. Chang: changed gather message.
* 03/28/98: K.C. Chang: created.
*/

//#include "cs225.h"
#include "libcs225.h"

#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include "PrGlobalDefn.h"
#include "PrNetworkDefn.h"
#include "driver/PrTimer.h"
#include "driver/PrIPBoardTRC.h"
#include "driver/PrIPBoardSTG.h"
#include "network/PrSocket.h"
#include "network/PrServerSocket.h"
#include "network/PrDataInput.h"
#include "network/PrDataOutput.h"
#include "GripperControl.h"

static PrTimer *timer;
//static PrIPBoardSTG *hwInterface;
static PrIPBoardIntf *hwInterface = NULL;
static Float jpos[CS225_PUMA_NOJ];
static Float jvel[CS225_PUMA_NOJ];
static Float tau[CS225_PUMA_NOJ];
static int servoRate;
static GripperControl *gripper=NULL;

/*
static PrServerSocket *serverUser,*serverGather;
static PrSocket *clientUser,*clientGather;
static PrDataInput *inUser,*inGather;
static PrDataOutput *outUser,*outGather;

static GatherMesg *gatherData;
static int gatherIndex=0;
static int gatherRequest=0;
static int gatherCount=0;

static int gatherDataReadyToWrite = 0;
static int gatherDataReadyToRead = 0;
*/

/*
* Function: InitManipulator
* Usage:    InitManipulator(clockrate);
*
* input:    clockrate is in Hz: (ex)  clockrate = 500 Hz
* Initializes necessary variables and flags for PUMA 560.
* Also, sets the servo(sampling) rate to clockrate.
*/
void InitManipulator(int clockrate)
{
   servoRate = clockrate;
   hwInterface = new BOARD_TO_USE(servoRate,PR_ARM_ID,
      PR_GRIPPER_ID,
      PR_FS_ID,
      PR_ACCEL_ID,
      PR_BASE_ID);

   /*
   serverUser = new PrServerSocket(CS225_USER_SERVO_TCP_PORT);
   if (serverUser->flag() != PR_SERVERSOCKET_OK)
      perror("server: can't create serverUser error");

   clientUser = serverUser->accept();
   inUser = new PrDataInput(clientUser);
   outUser = new PrDataOutput(clientUser);

   serverGather = new PrServerSocket(CS225_GATHER_SERVO_TCP_PORT);
   if (serverGather->flag() != PR_SERVERSOCKET_OK)
      perror("server: can't create serverGather error");

   clientGather = serverGather->accept();
   inGather = new PrDataInput(clientGather);
   outGather = new PrDataOutput(clientGather);

   gatherData = new GatherMesg[CS225_GATHER_SEC_MAX * servoRate];
   */

   timer = new PrTimer(servoRate);

   assert( hwInterface->GetBoard() != NULL ); 
   gripper = new GripperControl( ((PrIPBoardSTG*)(hwInterface))->GetBoard() ); 

   timer->start();

   //calibration check
   //watchdog init
   hwInterface->powerOn();
}

/*
* Function: EncoderReset
* Usage:    EncoderReset()
*
* Warning !!!!!
* This function set all the encoders to zero.
*
*/
void EncoderReset( void )
{
   if(hwInterface)
      hwInterface->zero();
}


/*
* Function: GetTime
* Usage:    time = GetTime();
*
* Returns the current clock(machine) time(sec) from 
* the beginning of this session.
*/
float GetTime()
{
   return timer->time();
}

/*
* Function: WaitNextTick
* Usage:    WaitNextTick();
*
* Comes back after a unit tick period.
* If the servo rate is faster than the actual time needed by your program,
* this function shuts the manipulator's power down and writes an error
* message. 
*/
bool WaitNextTick()
{
   //HandleGatherMessage();
   int m = timer->wait();
   if (m>0)
   {
      printf("servo rate too fast!: +%d ticks\n",m);
      ShutDown();
   }
   return true;
}

/*
* Function: GetPos
* Usage:    GetPos(q,eq);
*
* output:   q[3], eq[3]
* Puts the current joint positions to q[3].
* the unit is in radians.
* eq[3] is for the extended joints
*/
void GetPos(float q[3],float eq[3])
{
   hwInterface->jointPosition(jpos);
   q[0] = jpos[1];
   q[1] = jpos[2] - M_PI_2 - CS225_ANGLE_OFFSET;
   q[2] = jpos[4] + CS225_ANGLE_OFFSET;

   eq[0] = jpos[0];
   eq[1] = jpos[3];
   eq[2] = jpos[5];
}



/*
* Function: GetVel
* Usage:    GetVel(dq,edq);
*
* output:   dq[3] ,edq[3]
* Puts the current estimated joint velocities to dq[3].
* the unit is in radians/second.
* edq[3] is for the extended joints
*/
void GetVel(float dq[3],float edq[3])
{
   hwInterface->jointVelocity(jpos,jvel);
   dq[0] = jvel[1];
   dq[1] = jvel[2];
   dq[2] = jvel[4];

   edq[0] = jvel[0];
   edq[1] = jvel[3];
   edq[2] = jvel[5];
}

void pumaGripperCommand( int goal )
{
   int val = (goal >= 0) ? goal : 0;
 
   gripper->gripperCommand( val );
}


/*
* Function: SetTorque
* Usage:    SetTorque(t,et);
*
* input:    t[3], et[3]
* Applys the caculated joint torques, t[3].
* et[3] is for the extended joints
*/
void SetTorque(float t[3], float et[3])
{
   //watchdog ping
   tau[1] = t[0];
   tau[2] = t[1];
   tau[4] = t[2];

 
   tau[0] = et[0];
   tau[3] = et[1];
   tau[5] = et[2];
   
   // comment out above three lines, and comment in the following lines
   // to enable manipulation of all joints

   // Enforcing planar motion
   //tau[0] = -1600.0*jpos[0] - 80.0*jvel[0];
   //tau[3] = -900.0*jpos[3]  - 60.0*jvel[3];
   //tau[5] = -400.0*jpos[5]  - 40.0*jvel[5];    

   hwInterface->jointForce(tau);
}

/*
* Function: ShutDown
* Usage:    ShutDown();
*
* Shuts the manipulator's power down.
*/
void ShutDown()
{
   delete gripper; gripper = NULL;

   hwInterface->powerOff();

   delete timer;
   delete hwInterface;

   /*
   delete inUser;
   delete outUser;
   delete clientUser;
   serverUser->close();
   delete serverUser;

   delete inGather;
   delete outGather;
   delete clientGather;
   serverGather->close();
   delete serverGather;

   delete[] gatherData;
   */
}

/*
* Function: RecvUserMessage
* Usage:    nbyte = RecvUserMessage(mesg,maxlen);
*
* Returns a message from the user process.
* Any keyboard input from the terminal is a message.
*/
/*int RecvUserMessage(char *mesg, int maxlen)
{
   if (maxlen > PR_NETWORK_BUFFER_SIZE)
      maxlen = PR_NETWORK_BUFFER_SIZE;

   int n = inUser->ready();
   if (n<0) 
      return 0;
   switch(inUser->type())
   {
   case CS225_TYPE_CHAR:
      if (n >= maxlen)
         n = maxlen-1;
      if (inUser->readChar(mesg,n) != n)
      {
         printf("read error\n");
         n=0;
      }
      mesg[n]='\0';
      break;
   default:
      printf("unknow type\n");
      n=0;
      break;
   }
   return n;
}*/

/*
* Function: HandleGatherMessage
* Usage:    HandleGatherMessage()
*
* handles messages from and to the gather process.
* called in WaitNextTick()
*/
/*void HandleGatherMessage()
{
   if (!gatherDataReadyToRead)
   {
      int n = inGather->ready();
      if (n<0) 
         return;
      switch(inGather->type())
      {
      case CS225_TYPE_ACTION:
         int action;
         inGather->readInt(&action,1);
         switch(action)
         {
         case CS225_ACTION_GATHER_END:
            gatherIndex = 0;
            gatherRequest = 0;
            gatherCount = 0;
            gatherDataReadyToWrite = 0;
            gatherDataReadyToRead = 0;
            printf("gather done!\n");
            break;
         default:
            printf("unknown action\n");
            break;
         }
         break;
      case CS225_TYPE_GATHER_DATA:
         gatherDataReadyToRead = 1;
         //printf("i=%d, r=%d, c=%d\n", gatherIndex,gatherRequest,gatherCount);
         //
         //if (gatherRequest >= CS225_GATHER_SEC_MAX*servoRate)
         //{
         //gatherRequest = 0;
         //printf("error: HandleGatherMessage(): overflow\n");
         //return;
         //}
         //n = outGather->writeFloat(gatherData[gatherRequest].data,
         //CS225_GATHER_DATA_NUM);
         //if (outGather->flush(CS225_TYPE_GATHER_DATA)!=n)
         //{
         //printf("HandleGatherMessage:write error\n");
         //}
         //gatherRequest++;
         //gatherDataReadyToRead = 0;
         //
         break;
      default:
         printf("unknown type\n");
      }
   }
   if (gatherDataReadyToRead && gatherDataReadyToWrite)
   {
      if (gatherRequest >= CS225_GATHER_SEC_MAX*servoRate)
      {
         gatherRequest = 0;
         printf("error: HandleGatherMessage(): overflow\n");
         return;
      }
      int n = outGather->writeFloat(gatherData[gatherRequest].data,
         CS225_GATHER_DATA_NUM);
      if (outGather->flush(CS225_TYPE_GATHER_DATA)!=n)
      {
         printf("HandleGatherMessage:write error\n");
      }
      gatherRequest++;
      gatherDataReadyToWrite = 0;
      gatherDataReadyToRead = 0;
   }
}*/

/*
* Function: SetGatherMessage
* Usage:    flag = SetGatherMessage(time,q,qd);
*
* input:    time,q[3],qd[3]
* Saves (time,q[3], and qd[3]) in a file through the gather process.
*/
/*int SetGatherMessage(float time,float x[3],float xd[3])
{
   if (gatherIndex >= gatherCount)
   {
      gatherCount = 0;
      return CS225_ERROR;
   }

   if (gatherIndex >= CS225_GATHER_SEC_MAX*servoRate)
   {
      gatherIndex = 0;
      printf("error: SetGatherMessage(): overflow\n");
      return CS225_ERROR;
   }
   if (!gatherDataReadyToWrite)
   {
      gatherData[gatherIndex].data[0] = time;
      for(int i=0;i<3;i++)
      {
         gatherData[gatherIndex].data[i+1] = x[i];
         gatherData[gatherIndex].data[i+4] = xd[i];
      }
      gatherIndex++;
      gatherDataReadyToWrite = 1;
   }
   return CS225_OK;
}*/

/*
* Function: GatherStart
* Usage:    GatherStart(5,"output.mat");
*
* input:    
* Opens a output file with the name given.
* Starts the gather process.
* Note: the name of the output file should end with ".mat"
*       in order to save the data in Matlab format. 
*/
/*void GatherStart(int sec,char *outfile)
{
   if (sec<CS225_GATHER_SEC_MIN || sec>CS225_GATHER_SEC_MAX)
   {
      printf("error: GatherStart(): invalid gather duration(%d)\n",sec);
      printf("error: %d<= duration <= %d\n",
         CS225_GATHER_SEC_MIN,CS225_GATHER_SEC_MAX);
      return;
   }
   int action = CS225_ACTION_GATHER_START;
   int n = outGather->writeInt(&action,1);
   int num = sec*servoRate;
   n += outGather->writeInt(&num,1);
   n += outGather->writeChar(outfile,strlen(outfile)+1);
   if (outGather->flush(CS225_TYPE_ACTION)!=n)
   {
      printf("error: GatherStart(): write error\n");
   }
   gatherIndex = 0;
   gatherRequest = 0;
   gatherCount = sec*servoRate;
}
*/


/* Function: GetForceSensor
* Usage:    GetForceSensor()
* 
* This function returns an interface to the force sensor.
* Do a dynamic_cast<> to get a specific derived type.
*/
PrForceSensorIntf * GetForceSensor(void) 
{
   if ( hwInterface == NULL )
      return NULL;
   return hwInterface->forceSensor();
}

/* 
* Function: GetForce
* Usage:    error = GetForce()
*
* This function returns the force along x and y and the moment about z.
* The return value is CS225_OK if reading was successful.
*/
int GetForce ( float forces[6] )
{
   if ( !hwInterface )
      return CS225_ERROR;

   Float f[6];
   if (hwInterface->forceSensor(f) != PR_IPBOARDINTF_OK)
      return CS225_ERROR;

   /*
   forces[0] = f[2]; // force along x <-- force along z
   forces[1] = f[0]; // force along y <-- force along x
   forces[2] = f[4]; // torque about alpha <-- torque about y
   */

   forces[0] = f[0];
   forces[1] = f[1];
   forces[2] = f[2];
   forces[3] = f[3];
   forces[4] = f[4];
   forces[5] = f[5];

   return CS225_OK;
}

/*
* Function: ZeroForce
* Usage:    error = ZeroForce()
*
* Zeroes the force sensor.
* The return value is CS225_OK if reading was successful.
*/
int ZeroForce ()
{
   if ( !hwInterface )
      return CS225_ERROR;

   if (hwInterface->forceSensorZero() != PR_IPBOARDINTF_OK)
      return CS225_ERROR;
   return CS225_OK;
}


/* Function: GetAccelSensor
* Usage:    GetAccelSensor()
* 
* This function returns an interface to the accel sensor.
* Do a dynamic_cast<> to get a specific derived type.
*/
PrAccelSensorIntf * GetAccelSensor(void) 
{
   if ( hwInterface == NULL )
      return NULL;
   return hwInterface->accelSensor();
}

/* 
* Function: GetAccel
* Usage:    error = GetAccel()
*
* The return value is CS225_OK if reading was successful.
*/
int GetAccel ( float accels[6] )
{
   if ( !hwInterface )
      return CS225_ERROR;

   Float acc[6];
   if (hwInterface->accelSensor(accels) != PR_IPBOARDINTF_OK)
      return CS225_ERROR;

   accels[0] = acc[0];
   accels[1] = acc[1];
   accels[2] = acc[2];
   accels[3] = acc[3];
   accels[4] = acc[4];
   accels[5] = acc[5];


   return CS225_OK;
}

/*
* Function: ZeroAccel
* Usage:    error = ZeroAccel()
*
* Zeroes the accel sensor.
* The return value is CS225_OK if reading was successful.
*/
int ZeroAccel ()
{
   if ( !hwInterface )
      return CS225_ERROR;

   if (hwInterface->accelSensorZero() != PR_IPBOARDINTF_OK)
      return CS225_ERROR;
   return CS225_OK;
}







