/*
* File:   libcs225.h
*
* This file provides an interface to libcs225.cpp
*/
/*
* modification history
*----------------------
*
* 03/28/98: K.C. Chang: created.
*/
#ifndef _libcs225_h
#define _libcs225_h

#define CS225_OK                     1
#define CS225_ERROR                  0
#define CS225_HAND_OPEN              0
#define CS225_HAND_CLOSE             1
#define CS225_GATHER_SEC_MAX         90
#define CS225_GATHER_SEC_MIN         1

// Change the following #define to select which board/configuration
// to compile this library for

//#define BOARD_TO_USE PrIPBoardTRC
#define BOARD_TO_USE PrIPBoardSTG

#define CS225_PUMA_NOJ               6
#define CS225_ANGLE_OFFSET           ((float)0.0)//((float)0.046837113)


class PrForceSensorIntf;
class PrAccelSensorIntf;

extern "C"
{

   /*
   * 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);
   
   /* 
   * Function: EncoderReset
   * Usage:    EncoderReset();
   *
   * Warning !!!! Very dangerous!!!
   * This function sets all the encoder to zero.
   * 
   */
   void EncoderReset( void );

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

   /*
   * 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();

   /*
   * Function: GetPosAll
   * Usage:    GetPosAll(q,eq);
   *
   * output:   q[3], eq[3]
   * Puts the current joint positions to q[3].
   * qe[3] are the extended joints.
   * the unit is in radians.
   */
   void GetPos(float q[3], float eq[3]);

   /*
   * Function: GetVelAll
   * Usage:    GetVelAll(dq,edq);
   *
   * output:   dq[3], edq[3]
   * Puts the current estimated joint velocities to dq[3].
   * the unit is in radians/second.
   * edq is for the extended joints
   */
   void GetVel(float dq[3], float edq[3]);

   /*
    * Function: Gripper
    * Open or close the gripper. [0..78]
    */
  void pumaGripperCommand( int goal );  

   /*
    * Function: SetTorqueAll
    * Usage:  SetTorqueAll(t,et);
    *
    * input:  t[3], et[3]
    */
   void SetTorque(float t[3], float et[3]);

   /*
   * Function: ShutDown
   * Usage:  ShutDown();
   * Shuts the manipulator's power down.
   */
   void ShutDown();

   /*
   * 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);

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

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

   /*
   * 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);
 
   /* 
    * Function: GetAccel
    * Usage:  error = GetAccel()
    *
    * The return value is CS225_OK if reading was successful.
    */
   int GetAccel ( float accels[6] );

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

   /*
   * 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);

   /*
   * 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.
   * returns CS225_OK if it was successfull 
   * returns CS225_ERROR otherwise
   */
   //int SetGatherMessage(float time,float x[3],float xd[3]);

   /*
   * 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);

}
#endif 

/* libcs225.h */

