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

/*
* modification history
*----------------------
*
* 04/09/98: K.C. Chang: created.
* 11/16/98: Stef Sonck: fixed bug in setGatherMessage
* 03/02/00: N. Turro: PrStgPuma hardware interface
* 02/25/03: Weyn Ong: Added #define to select board.  Added accel sensor.
* 06/05/04: IrenaP: stripped out gathering, others for cs225 -
*                   functionality has been moved into user space.
*/

#include "puma.h"
#include "eompuma.h"
#include "libpuma.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 "driver/ServoToGo.h"
#include "matrix/PrVector6.h"
#include "matrix/PrMatrix6.h"
#include "matrix/PrTransform.h"
#include "robot/PrRobot.h"
#include "robot/PrJointState.h"
#include "robot/PrLink.h"
#include "GripperControl.h"

static PrTimer *timer;
static PrIPBoardIntf *hwInterface = NULL;
static int servoRate;
static PrRobot *realRobot=NULL;


static PrTransform inverseT,localT; // temporary 
static PrVector3 fsOffset;
static PrVector6 fsForce;
static GripperControl *gripper=NULL;


/*
* Function: pumaInitManipulator
* Usage:    pumaInitManipulator(clockrate,paramfile);
*
* input:    clockrate is in Hz: (ex)  clockrate = 500 Hz
*           if paramfile==NULL /local/pr/puma560.prm is used. 
* Initializes necessary variables and flags for PUMA 560.
* Also, sets the servo(sampling) rate to clockrate.
*/
void pumaInitManipulator(int clockrate, const char *paramfile)
{
   fsOffset[0]=0.0;
   fsOffset[1]=0.0;
   fsOffset[2]=PUMA_FS_OFFSET;

   servoRate = clockrate;

   if( paramfile != NULL )
      realRobot = new PrRobot(PUMA_NOJ,const_cast<char*>(paramfile),PR_ROBOT_INV_DYNAMICS);
   else
      realRobot = new PrRobot(PUMA_NOJ,"/local/pr/param/puma560.prm",
      PR_ROBOT_INV_DYNAMICS);

   hwInterface = new BOARD_TO_USE(servoRate,PR_ARM_ID,
      PR_GRIPPER_ID,
      PR_FS_ID,
      PR_ACCEL_ID,
      PR_BASE_ID);
   if ( hwInterface->flag() != PR_IPBOARDINTF_OK ) {
      fprintf( stderr, "Failed to initialize controller.\n" );
      exit( EXIT_FAILURE );
   }

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

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


/*
 * Function: pumaGetRobot
 * Usage:    pumaGetRobot()
 *
 * returns the robot object, which can be
 * queried for inv/fwd dynamics, force sensor,
 * gripper, etc.
 */
PrRobot* pumaGetRobot()
{
   return realRobot;
}

/*
* Sends a zero() signal to the hardware.
*/

void pumaEncoderReset( void ) 
{
   if ( hwInterface )
      hwInterface->zero();
}

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

/*
* Function: pumaWaitNextTick
* Usage:    pumaWaitNextTick();
*
* 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. 
*/
void pumaWaitNextTick()
{
   int m = timer->wait();
   if (m>0)
   {
      printf("servo rate too fast!: +%d ticks\n",m);
      pumaShutDown();
   }
}

/*
* Function: pumaGetPos
* Usage:    pumaGetPos(q);
*
* output:   q
* Puts the current joint positions to q.
* the unit is in radians.
*/
void pumaGetPos(PrVector6 &q)
{
   hwInterface->jointPosition(realRobot->jointState()->q());
   for (int i=0;i<PUMA_NOJ;i++)
      q.elementAt(i) = realRobot->jointState()->q()[i];
   realRobot->updateTransform();
}

/*
* Function: pumaGetVel
* Usage:    pumaGetVel(dq);
*
* output:   dq
* Puts the current estimated joint velocities to dq.
* the unit is in radians/second.
*/
void pumaGetVel(PrVector6 &dq)
{
   hwInterface->jointVelocity(realRobot->jointState()->q(),
      realRobot->jointState()->dq());
   for (int i=0;i<PUMA_NOJ;i++)
      dq.elementAt(i) = realRobot->jointState()->dq()[i];
}


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

/*
* Function: pumaSetOSFrameLocal
* Usage:    pumaSetOSFrameLocal(T)
*
* input:    
* define operational space frame in wrist frame
*/
void pumaSetOSFrameLocal(PrTransform &localT)
{
   realRobot->object()->homePosition(localT);
}

/*
* Function: pumaSetOSFrameGlobal
* Usage:    pumaSetOSFrameGlobal(T)
*
* input:    
* define operational space frame globally
*/
void pumaSetOSFrameGlobal(PrTransform &globalT)
{
   realRobot->object()->parent()->globalTransform().inverse(inverseT);
   inverseT.multiply(globalT,localT);
   realRobot->object()->homePosition(localT);
}

/*
* Function: pumaGetOSFrameLocal
* Usage:    pumaGetOSFrameLocal(T)
*
* input:    
* caculate operational space frame in wrist frame
*/
void pumaGetOSFrameLocal(PrTransform &localT)
{
   localT = realRobot->object()->localTransform();
}

/*
* Function: pumaGetOSFrameGlobal
* Usage:    pumaGetOSFrameGlobal(T)
*
* input:    
* caculate operational space frame in global frame
*/
void pumaGetOSFrameGlobal(PrTransform &globalT)
{
   globalT = realRobot->object()->globalTransform();
}

/*
* Function: pumaGetWristFrameGlobal
* Usage:    pumaGetWristFrameGlobal(T)
*
* input:    
* caculate wrist frame in global frame
*/
void pumaGetWristFrameGlobal(PrTransform &globalT)
{
   globalT = realRobot->object()->parent()->globalTransform();
}

/*
* Function: pumaGetFSFrameLocal
* Usage:    pumaGetFSFrameLocal(T)
*
* input:    
* caculate force sensor frame in wrist frame
*/
void pumaGetFSFrameLocal(PrTransform &localT)
{
   localT = realRobot->forceSensor()->localTransform();
   localT.translation()+= realRobot->forceSensor()->offset();
}

/*
* Function: pumaGetEEFrameLocal
* Usage:    pumaGetEEFrameLocal(T)
*
* input:    
* caculate end-effector frame in wrist frame
*/
void pumaGetEEFrameLocal(PrTransform &localT)
{
   localT = realRobot->endEffector()->localTransform();
   localT.translation()+= realRobot->endEffector()->offset();
}

/*
* Function: pumaGetOSIM
* Usage:    pumaGetOSIM(L);
*
* input:    
* caculate operational space inertia matrix
*/
void pumaGetOSIM(PrMatrix6 &Lambda)
{
   realRobot->globalOSIM(Lambda);
}

/*
* Function: pumaGetJacobian
* Usage:    pumaGetJacobian(J);
*
* input:    
* caculate Jacobian.
*/
void pumaGetJacobian(PrMatrix6 &J)
{
   PrLink *link = realRobot->base()->child();
   PrVector3 &P = realRobot->object()->globalTransform().translation();
   for(int j=0;j<PUMA_NOJ;j++)
   {
      PrVector6 &Jj = link->globalJacobian(P);
      for(int i=0;i<PUMA_NOJ;i++)
         J.elementAt(i,j) = Jj.elementAt(i);
      link = link->child();
   }
}

/*
* Function: pumaGetComputedTorque
* Usage:    pumaGetComputedTorque(t,tau);
*
* input:    t ---> tau
* caculate dynamically computed joint torques, tau.
*/
void pumaGetComputedTorque(PrVector6 &t, PrVector6 &tau)
{
   for(int i=0;i<PUMA_NOJ;i++)
      realRobot->jointAcceleration()[i] = t.elementAt(i);

   realRobot->invDynamics();

   for(int j=0;j<PUMA_NOJ;j++)
      tau.elementAt(j) = realRobot->jointForce()[j];
}


/*
* Function: pumaSetTorque
* Usage:    pumaSetTorque(t);
*
* input:    t
* Applys the caculated joint torques, t.
*/
void pumaSetTorque(PrVector6 &tau)
{
   //watchdog ping
   for(int i=0;i<PUMA_NOJ;i++)
      realRobot->jointForce()[i] = tau.elementAt(i);

   hwInterface->jointForceArm(realRobot->jointForce());
}

/*
* Function: pumaShutDown
* Usage:    pumaShutDown();
*
* Shuts the manipulator's power down.
*/
void pumaShutDown()
{
   delete gripper; gripper = NULL;
   hwInterface->powerOff();
   delete realRobot;
   delete timer;
   delete hwInterface;
   fprintf( stderr, "Puma shut down\n" );
}


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

/* 
* Function: pumaGetForce
* Usage:    error = pumaGetForce()
*
* This function returns the force values at the force sensor.
* The return value is PUMA_OK if reading was successful.
* NOTE: compression +
*/
int pumaGetForce (PrVector6 &forces) 
{
   if ( hwInterface == NULL )
      return PUMA_ERROR;
   Float f[PUMA_NOJ];
   if (hwInterface->forceSensor(f) != PR_IPBOARDINTF_OK)
      return PUMA_ERROR;
   for (int i=0;i<PUMA_NOJ;i++)
      forces.elementAt(i) = -f[i];
   return PUMA_OK;
}

/*
* Function: pumaZeroForce
* Usage:    error = pumaZeroForce()
*
* Zeroes the force sensor.
* The return value is PUMA_OK if reading was successful.
*/
int pumaZeroForce () 
{
   if ( hwInterface == NULL )
      return PUMA_ERROR;
   if (hwInterface->forceSensorZero() != PR_IPBOARDINTF_OK)
      return PUMA_ERROR;
   return PUMA_OK;
}

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

/* 
* Function: pumaGetAccel
* Usage:    error = pumaGetAccel()
*
* This function returns the accel values at the accel sensor.
* The return value is PUMA_OK if reading was successful.
* NOTE: compression +
*/
int pumaGetAccel (PrVector6 &accels) 
{
   if ( hwInterface == NULL )
      return PUMA_ERROR;
   Float f[PUMA_NOJ];
   if (hwInterface->accelSensor(f) != PR_IPBOARDINTF_OK)
      return PUMA_ERROR;
   for (int i=0;i<PUMA_NOJ;i++)
      accels.elementAt(i) = -f[i];
   return PUMA_OK;
}

/*
* Function: pumaZeroAccel
* Usage:    error = pumaZeroAccel()
*
* Zeroes the accel sensor.
* The return value is PUMA_OK if reading was successful.
*/
int pumaZeroAccel () 
{
   if ( hwInterface == NULL )
      return PUMA_ERROR;
   if (hwInterface->accelSensorZero() != PR_IPBOARDINTF_OK)
      return PUMA_ERROR;
   return PUMA_OK;
}


// NOTE: compression +
int pumaGetForceWrist(PrVector6 &Fw)
{
   int error = pumaGetForce(fsForce);
   if (error != PUMA_ERROR)
   {
      fsOffset.cross(fsForce[0],Fw[1]);
      Fw[0] = fsForce[0];
      Fw[1] += fsForce[1];
   }

   return error;
}

void pumaInitSymbolic(PrVector6 &q)
{
   eomInit(q);
}

void pumaGetOSIMSymbolic(PrMatrix6 &J,PrMatrix6 &A,PrMatrix6 &Lambda)
{
   eomGetOSIM(J,A,Lambda);
}

void pumaGetWristFrameSymbolic(PrMatrix3 &R,PrVector3 &P)
{
   eomGetWristFrame(R,P);
}

void pumaGetJacobianSymbolic(PrMatrix6 &J)
{
   eomGetJacobian(J);
}

void pumaGetJacobianDotSymbolic( PrMatrix6 &Jdot, const PrVector6& dq )
{
   eomGetJacobianDot( Jdot, dq );
}

void pumaGetGravitySymbolic(PrVector6 &G)
{
   eomGetGravity(G);
}

void pumaGetJSIMSymbolic(PrMatrix6 &M)
{
   eomGetJSIM(M);
}






