// *******************************************************************
// CSimPuma.h
//
// This implementation of IRobotDriver simulates a puma.  See also
// CPuma.h and CPumaModel.h.
//
// modification history
// --------------------
//
// 07/07/04: Dan Merget: created
// *******************************************************************
#ifndef C_SIM_PUMA_H
#define C_SIM_PUMA_H

#include "IRobotDriver.h"
class PrJointIntegrator;
class PrRobot;

class CSimPuma : public IRobotSimDriver
{
public:
  CSimPuma( int clockRate );
  ~CSimPuma();
  virtual void getPos( PrVector& dest );
  virtual void getVel( PrVector& dest );
  virtual void setTorque( const PrVector& rhs );
  virtual int  waitNextTick( Float simSpeed );

  virtual void setFriction( bool val );
  virtual void setContactPoint( int linkNum, const PrVector3& contactPoint,
    const PrVector& pos );
  virtual void applyContactForce( Float springk,
    const PrVector3& destPoint );
  virtual void removeContactForce();

  virtual void setState( const PrVector& pos, const PrVector& vel );
  virtual void fwdDynamics( PrVector& acc, const PrVector& torque );
  virtual void invDynamics( PrVector& torque, const PrVector& acc );
  virtual void getEndEffectorFrame( PrTransform& dest );
  virtual void getMassMatrix( PrMatrix& dest );
  virtual void getGravityVector( PrVector& dest );

  virtual void getJacobianAtContactPoint( PrMatrix& jacobian );
  virtual void getJacobianAt( int linkID, const PrVector3& localPos, PrMatrix& jacobian );
  virtual bool getGlobalTransformOf( int linkID, PrTransform& globalTransform );

private:
  void applyContactTorque();
  PrRobot*           m_simRobot;        // Underlying simulator
  PrJointIntegrator* m_jointIntegrator; // Used by waitNextTick()
  PrVector6          m_tau;             // Applied torque, w/out contact force
  PrVector3          m_friction[6];     // Cached values of friction per node

  bool               m_applyContactForce; // true if contact force is applied
  int                m_contactLink;       // link that gets the contact force
  Float              m_contactGain;       // Gain (springk) for contact force
  PrVector3          m_contactPoint;      // Contact point, in link coords
  PrVector3          m_contactDest;       // Destination point, in base coords
};

#endif // C_SIM_PUMA_H
