// mskccRobot.h: interface for the mskccRobot class.
// $Id: mskccRobot.h,v 1.16 2006/08/02 15:54:20 pkaz Exp $
//////////////////////////////////////////////////////////////////////

#ifndef MSKCCROBOT_H
#define MSKCCROBOT_H

#include <iostream>
#include <stdexcept>
#include "windows.h"
#include "dmccom.h"

#include <cisstVector.h>
#include <cisstCommon.h>

// code required for the DLL generation
#ifdef mskccRobot_EXPORTS
#define CISST_THIS_LIBRARY_AS_DLL
#endif
#include <cisstCommon/cmnExportMacros.h>
#undef CISST_THIS_LIBRARY_AS_DLL
// end of code for the DLL generation

// Conditional compilation used because SWIG cannot handle nested class definitions
#ifndef SWIG
class CISST_EXPORT mskccRobot : public cmnGenericObject
{
#ifdef CMN_DECLARE_SERVICES_INSTANTIATION
CMN_DECLARE_SERVICES(CMN_NO_DYNAMIC_CREATION, 5);  // cisst-0.3.1
#else
CMN_DECLARE_SERVICES(mskccRobot, CMN_NO_DYNAMIC_CREATION);  // cisst-0.3.0
#endif

public:
#endif
   class RobotException : public std::runtime_error {
   public:
       explicit RobotException(const std::string& msg = "") : std::runtime_error(msg) {
		   CMN_LOG(1) << "RobotException: " << msg << std::endl;
	   }
   };

   class ExcpPowerOff : public RobotException {
   public:
       explicit ExcpPowerOff(const std::string& msg = "Motor Power is Off") : RobotException(msg) { }
   };

   class ExcpAmpError : public RobotException {
   public:
       explicit ExcpAmpError(const std::string& msg = "Amplifier Error") : RobotException(msg) { }
   };

   class ExcpMotorOff : public RobotException {
   public:
       explicit ExcpMotorOff(const std::string& msg = "Motor is Disabled") : RobotException(msg) { }
   };
      
   class ExcpNotHomed : public RobotException {
   public:
       explicit ExcpNotHomed(const std::string& msg = "Robot is not Homed") : RobotException(msg) { }
   };
   
   class ExcpForceProg : public RobotException {
   public:
       explicit ExcpForceProg(const std::string& msg = "Force Program Error") : RobotException(msg) { }
   };
   
   class ExcpLimitError : public RobotException {
   public:
       explicit ExcpLimitError(const std::string& msg = "Out of Limits") : RobotException(msg) { }
   };

   class ExcpSystemError : public RobotException {
   public:
       explicit ExcpSystemError(const std::string& msg = "System Error") : RobotException(msg) { }
   };

   class ExcpWaitMotion : public RobotException {
   public:
       explicit ExcpWaitMotion(const std::string& msg = "Error waiting for robot to stop") : RobotException(msg) { }
   };
#ifdef SWIG
class CISST_EXPORT mskccRobot : public cmnGenericObject
{
public:
#endif
   enum AXES { X, Y, Z1, Z2};
   enum {MAX_AXES = Z2+1};

private:
   HANDLEDMC   hDmc;
   double speed, accel, decel;  // shadow variables
   bool homed;                    // TRUE if robot homed

   double cnts_per_mm[MAX_AXES];  // joint scale factors
   double jt_home[MAX_AXES];      // joint home position
   double jt_ulim[MAX_AXES];      // joint upper limit
   double jt_llim[MAX_AXES];      // joint lower limit

   int amp_type[MAX_AXES];        // Amplifier gain (0 -> 0.4A/V, 1 -> 0.7A/V, 2 -> 1.0A/V)
   double amp_gain[3];            // Conversion factors for amplifier gain
   struct pid_gains {
      double kp, ki, kd;
   } pid[MAX_AXES];               // PID controller gains
   double curlim_avg[MAX_AXES];   // Average current (torque) limit


   // Internal functions

   // Read configuration file (configuration file name could be added as
   // a parameter)
   bool ReadConfigFile() throw();

   // Check status and raise exception if not valid.
   //    callerName -- name of calling function (for debug purposes)
   //    statusMask -- mask to indicate which status fields to check
   // Returns:  the robot status
   long VerifyStatus(const char *callerName, long statusMask) throw(RobotException);

   int GetForceState() throw(ExcpSystemError);

   // Returns true if goal is inside joint limits for specified axis
   bool InsideLimits(AXES axis, double goal) const
      { return ((goal >= jt_llim[axis]) && (goal <= jt_ulim[axis])); }

public:

   // Default Constructor
   mskccRobot() throw();
   // Copy constructor
   mskccRobot(const mskccRobot& rob) throw();
   // Destructor
   virtual ~mskccRobot();

   // Assignment operator
   mskccRobot &operator =(const mskccRobot& rob);

   // Initialize (e.g., establish communications with the controller)
   // and configure the robot.  Note that a configuration file name could
   // be added as a parameter.
   bool Init() throw(ExcpSystemError);

   // Enable/disable motor power
   void EnableMotorPower() throw(ExcpPowerOff, ExcpSystemError);
   void DisableMotorPower() throw(ExcpSystemError);

   // Stop robot motion (do not disable motor power)
   void Stop() throw(ExcpSystemError);

   // Home robot (initialize encoders)
   bool Home() throw(RobotException);

   // Unhome robot
   void UnHome() throw(ExcpSystemError);

   // Returns true if robot homed
   inline bool IsHomed() const throw () { return homed; }

    // Return joint limits
   inline double GetJointUpperLimit(AXES axis) const throw()
        { return jt_ulim[axis]; }
   inline double GetJointLowerLimit(AXES axis) const throw()
        { return jt_llim[axis]; }

   // Set joint limits (should be called after Init but before Home)
   inline void SetJointUpperLimit(AXES axis, double lim) throw()
        { jt_ulim[axis] = lim; }
   inline void SetJointLowerLimit(AXES axis, double lim) throw()
        { jt_llim[axis] = lim; }


   //*** Motion commands (all positions are in mm and are relative to
   //    the robot world coordinate system).

   // Move the first 3 axes (X, Y, Z1) to the specified xyz position
   void MoveXYZ1(double x, double y, double z1) throw(RobotException);
   void MoveXYZ1(const vctDouble3& goal) throw(RobotException)
        { MoveXYZ1(goal.X(), goal.Y(), goal.Z()); }

   // Move the 4th axis (Z2) to the specified goal position
   void MoveZ2(double goal) throw(RobotException);

   // Move the 4th axis (Z2) by the specified increment
   void AdvanceZ2(double incr) throw(RobotException);

   // Move all joints (z2 is not in world coordinate system)
   void MoveJoints(double x, double y, double z1, double z2) throw(RobotException);
   void MoveJoints(const vctDouble4& goal) throw(RobotException)
        { MoveJoints(goal[0], goal[1], goal[2], goal[3]); }

   //*** Position feedback (all positions are in mm and are relative to
   //    the robot world coordinate system, unless specified otherwise)

   // Returns the xyz position of the first 3 axes (X, Y, Z1)
   vctDouble3 GetPositionXYZ1() throw(ExcpNotHomed, ExcpSystemError);
   
   // Returns the position of the Z2 axis
   double GetPositionZ2() throw(ExcpNotHomed, ExcpSystemError);

   // Returns joint positions (x,y,z1,z2)
   vctDouble4 GetJointPosition() throw(ExcpNotHomed, ExcpSystemError);

   //*** Motion parameters, speeds are in mm/sec, accelerations/decelerations are
   //    in mm/sec**2

   void SetSpeed(double spd) throw(ExcpSystemError);
   void SetAccel(double acc) throw(ExcpSystemError);
   void SetDecel(double dec) throw(ExcpSystemError);

   double GetSpeed() const throw();
   double GetAccel() const throw();
   double GetDecel() const throw();

   //*** Force compliance

   void StartForceCompliance() throw(RobotException);
   void StopForceCompliance() throw(ExcpSystemError);

   // Returns X,Y,Z forces (units currently are volts)
   vctDouble3 GetForces() throw(ExcpSystemError);

   // Request Galil controller to bias forces (i.e., remove offset)
   void BiasForces() throw(ExcpSystemError);

   // Get current force bias values
   vctDouble3 GetForceBias() throw(ExcpSystemError);

   // Get current force control gains
   vctDouble3 GetForceGains() throw(ExcpSystemError);

   // Set force control gains
   void SetForceGains(const vctDouble3& fgains) throw(ExcpSystemError);

   //*** Other functions:

   // Wait for all motion to be complete.
   void WaitMotion() throw(ExcpWaitMotion, ExcpSystemError);

   // GetStatus:  32 bits -- 8 general + 6 per axis
   //
   // General (8):
   //    7  -- unused (0)
   //    6  -- 1 if robot is not homed, NOT_HOMED_MASK
   //    5  -- 1 if force threshold was exceeded, FSTOP_MASK
   //    4  -- 1 if motor power supply is off (red switch latched), ESTOP_MASK
   //    3  -- 1 for any other amplifier error (over temp, over voltage, over current), AMP_ERROR_MASK
   //    2  -- 1 if Galil program is not running, PROG_ERROR_MASK
   //    1  -- 1 if any axis in motion, MOTOR_MOVING_MASK
   //    0  -- 1 if any motor is off, MOTOR_OFF_MASK
   //
   // Per axis (6), N = 0,1,2,3:
   //    8+6*N+5 (bits 13,19,25,31) -- 1 if axis in motion, JT_MOTOR_MOVING_MASK
   //    8+6*N+4 (bits 12,18,24,30) -- 1 if axis error exceeds limit, JT_ERROR_LIMIT_MASK
   //    8+6*N+3 (bits 11,17,23,29) -- 1 if motor is off, JT_MOTOR_OFF_MASK
   //    8+6*N+2 (bits 10,16,22,28) -- 1 if at forward limit switch, JT_FWDLIM_MASK
   //    8+6*N+1 (bits  9,15,21,27) -- 1 if at reverse limit switch, JT_REVLIM_MASK
   //    8+6*N+0 (bits  8,14,20,26) -- 1 if at home switch, JT_HOME_MASK
   //
   // Note that the ANY_ERROR_MASK can be used to quickly check if there are any general problems:
   //    if (status&ANY_ERROR_MASK)
   //       cout << "There is an error, status = " << status << endl;
   // This mask looks for any set bits except the "motor moving" bit
   //
   enum { MOTOR_OFF_MASK=0x01, MOTOR_MOVING_MASK=0x02, PROG_ERROR_MASK=0x04, AMP_ERROR_MASK=0x08,
          ESTOP_MASK=0x10, FSTOP_MASK=0x20, NOT_HOMED_MASK=0x40, ANY_ERROR_MASK=0x7d };
   enum { JT_HOME_MASK=0x01, JT_REVLIM_MASK=0x02, JT_FWDLIM_MASK=0x04, JT_MOTOR_OFF_MASK=0x08,
          JT_ERROR_LIMIT_MASK=0x10, JT_MOTOR_MOVING_MASK=0x20 };
   long GetStatus() throw(ExcpSystemError);

   //*** Low-level functions that have been made public for use with the IRE:

   // Send command to Galil controller and return pointer to response buffer.
   const char *SendCommand(char *buffer) throw(ExcpSystemError);

   // Get vector (3D array) data from Galil controller
   vctDouble3 GetGalilVector(const char *name) throw(ExcpSystemError);
};

#ifndef SWIG
#ifdef CMN_DECLARE_SERVICES_INSTANTIATION
CMN_DECLARE_SERVICES_INSTANTIATION(mskccRobot); // cisst-0.3.1
#endif
#endif

#endif

// CVS Log, do not edit
// $Log: mskccRobot.h,v $
// Revision 1.16  2006/08/02 15:54:20  pkaz
// Allow it to compile with cisst version 0.3.0 and 0.3.1 (differences in CMN_DECLARE_SERVICES macros).
//
// Revision 1.15  2006/07/13 04:47:56  pkaz
// Updated for current version of cisst libraries.
//
// Revision 1.14  2005/04/08 17:20:58  pkaz
// Added inline functions to set joint limits.
//
// Revision 1.13  2005/04/07 21:59:37  pkaz
// Overloaded MoveXYZ1 and MoveJoints functions so that they both take scalar and vector parameters.
//
// Revision 1.12  2005/04/07 04:57:51  pkaz
// Added CMN_NO_DYNAMIC_CREATION to CMN_DECLARE_SERVICES.
//
// Revision 1.11  2005/01/24 22:08:41  alamora
// Modified to derive from the cmnGenericObject base class
// Now using the object register and logger
// Replaced "cout"s with logs
// Destructor is now virtual
// Removed IFDEF UNUSED code (see previous revision notes)
//
// Revision 1.10  2005/01/22 17:59:25  pkaz
// Removed "move" and "get" methods that refer to Cannula or RegProbe position because that functionality is provided in the application software (e.g., Slicer).  For now, code is removed by inserting #ifdef UNUSED.
//
// Revision 1.9  2005/01/20 23:04:26  pkaz
// Consolidated SendCommand and SendCommandWithResponse.
//
// Revision 1.8  2005/01/16 07:19:09  pkaz
// Added more exception specifications.
//
// Revision 1.7  2005/01/14 22:51:34  pkaz
// Added exceptions.  Not quite working with Python.
//
// Revision 1.6  2005/01/10 22:09:17  pkaz
// Fixed white-space -- no code changes.
//
// Revision 1.5  2004/12/21 22:48:28  pkaz
// Added UnHome.  Improved homing repeatability.
//
// Revision 1.4  2004/11/23 16:13:39  pkaz
// Added amplifier gain and torque limits; temp changes for replacement Z2 motor
//
// Revision 1.3  2004/11/03 18:18:38  pkaz
// Added setting of PID gains
//
// Revision 1.2  2004/10/27 15:55:54  anton
// Added CMakeLists.txt files and minor modifications to compile the code.
//
// Revision 1.1  2004/10/26 21:57:17  anton
// Imported code from Peter
//