/***************************************************************
 * PrRobot.h

 *
 * This class provides the robot interface.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 11/11/97: K.C. Chang: created.
 */

#ifndef _PrRobot_h
#define _PrRobot_h

#include "PrGlobalDefn.h"
#include "matrix/PrVector3.h"
#include "matrix/PrMatrix3.h"
#include "matrix/PrMatrix6.h"
#include "matrix/PrTransform.h"

#define PR_ROBOT_OK    1
#define PR_ROBOT_ERROR 0

#define PR_ROBOT_INV_DYNAMICS 0
#define PR_ROBOT_FWD_DYNAMICS 1

#ifdef  PR_DOUBLE_PRECISION
#define PR_ROBOT_MASS_LARGE 100000000.0
#else // PR_DOUBLE_PRECISION
#define PR_ROBOT_MASS_LARGE 100000.0
#endif // PR_DOUBLE_PRECISION 
#define PR_ROBOT_INERTIA_LARGE (PR_ROBOT_MASS_LARGE/100.0)

#define PR_ROBOT_FS_ID 888
#define PR_ROBOT_EE_ID 999

#define PR_ROBOT_FILETYPE_DH  0
#define PR_ROBOT_FILETYPE_GLOBAL 1

class PrRobotDynamicsParamDH
{
  public:
    int id;
    int type;
    Float DH[4];
    Float mass;
    PrVector3 com;
    Float Ixx,Iyy,Izz;
    Float Imotor; 
    Float viscousFriction;
    Float dynamicCoulombFriction;
    Float staticCoulombFriction;
    Float qMin;
    Float qMax;
    PrVector3 offset;
}; 

class PrRobotDynamicsParamGlobal
{
  public:
    int id;
    int type;
    int axis;
    PrTransform homeT;
    PrVector3 com;
    Float mass;
    PrVector3 inertia;
    Float qMin;
    Float qMax;
};


class PrJointState;
class PrLink;

class PrRobot
{
 public:
    PrRobot(int noj,char *paramFilename,
	    int dynamicsType=PR_ROBOT_INV_DYNAMICS,int fileType=PR_ROBOT_FILETYPE_DH);
    ~PrRobot();
    
    int flag() const { return flag_; }

    int noj() { return noj_; }

    void id(int num) { id_ = num; }
    int id() const { return id_; }

    PrLink *object() { return object_; }
    PrLink *lastLink() { return links_[noj_-1]; }
    PrLink *endEffector() { return ee_; }
    PrLink *forceSensor() { return fs_; }
    PrLink *base() { return base_; }

    PrJointState *jointState() { return jointState_; }

    Float *jointForce() { return tau_; }
    Float *jointAcceleration();

    // PrTransform &fwdKinematics();
    // { return object_->globalTransform(); }
    //    Float *invKinematics() 
    //    { return q_; }

    void updateTransform();
    void invDynamics();
    void fwdDynamics();

    void globalOSIM(PrMatrix6 &Lambda);
    void localOSIM(PrMatrix6 &Lambda);

    void display() const;

 private:
    int flag_;
    int id_;
    const int noj_;
    const int dynamicsType_;
    PrLink *base_;
    PrLink **links_;
    PrLink *object_;
    PrLink *ee_;
    PrLink *fs_;
    Float *tau_;
    PrJointState *jointState_;

    PrMatrix3 tmpM31_,tmpM32_,tmpM33_;
    PrMatrix6 tmpM61_;

    int InitializeDH(char *paramFilename);
    int ReadDynamicsParamDH(char *paramFilename, PrVector3 &gravity,
			  PrRobotDynamicsParamDH *param,int *fs_ee);

    int InitializeGlobal(char *paramFilename);
    int ReadDynamicsParamGlobal(char *paramFilename, PrVector3 &gravity, 
				PrTransform &globalT, PrRobotDynamicsParamGlobal *param);

    void SimilarityRform6(const PrMatrix6 &localLambda,PrMatrix6 &globalLambda);

    // copy is not allowed
    PrRobot(const PrRobot &) : noj_(0), dynamicsType_(PR_ROBOT_INV_DYNAMICS) {}
};

#endif // _PrRobot_h





