/* file robot.h */
#ifndef _Robot_H
#define _Robot_H
#include <impl/robot1.h>
#include <impl/robot2.h>
#include <impl/conveyer1.h>
#include <stdbool.h>

class CRobot {
    private:
      int m_robot;  // identify the robot object 
      bool m_simul; // specify running mode
      
      int SetupRobot1();
      int SetupRobot2();
      int SetupConveyer();
      int DacsRobot1();
      int DacsRobot2();
      int DacsConveyer();
      int CalibrateRobot1();
      int CalibrateRobot2();
      int DisableRobot1();
      int DisableRobot2();
      int DisableConveyer();
      int InverseKinematicsRobot1(array double 
         trans[4][4],jointold[:],joint[:],int conf);
      int InverseKinematicsRobot2(array double 
         trans[4][4],jointold[:],joint[:],int conf);
      int DriveRobot1(array double disp[]);
      int DriveRobot2(array double disp[]);
      int DriveConveyer(array double disp[]);
      int Robot1GripperClose();
      int Robot2GripperClose();
      int Robot1GripperOpen();
      int Robot2GripperOpen();
      int Robot1MoveHome();
      int Robot2MoveHome();
      int Robot1MoveReady();
      int Robot2MoveReady();
      int Robot1MoveWait();
      int Robot2MoveWait();
      int ConveyerMoveWait();
      int Robot1MoveZero();
      int Robot2MoveZero();
      int Robot1JointSpeed(int speed[]);
      int Robot2JointSpeed(int speed[]);
      int ConveyerJointSpeed(int speed[]);

    public:
      CRobot(int robotnum,...);
      ~CRobot();

      int Calibrate();
      int Drive(array double disp[]);
      int GripperClose();
      int GripperOpen();
      int InverseKinematics(array double trans[4][4], 
         array double joint[:],jointold[:],int conf);
      int MoveHome();
      int MoveReady();
      int MoveWait();
      int MoveZero();
      int JointSpeed(int speed[]);
};

#pragma importf <CRobot.chf>

#endif   /*  _Robot_H */