/***************************************************************
 * PrJointControl.h
 *
 * This class provides the joint control interface.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 10/07/98: K.C. Chang: created.
 */

#ifndef _PrJointControl_h
#define _PrJointControl_h

#include "PrGlobalDefn.h"

#define PR_JOINTCONTROL_JGOALS_MAX 4

class PrVector;

class PrJointTrajectory 
{
public:
  PrJointTrajectory(int noj);
  ~PrJointTrajectory(); 

  void values(float *data);
  PrVector &q() { return *q_; }
  PrVector &dq() { return *dq_; }
  PrVector &tf() { return *tf_; }

private:
  const int noj_;
  PrVector *q_;
  PrVector *dq_;
  PrVector *tf_;

  PrJointTrajectory(const PrJointTrajectory &) : noj_(0) {}
};

class PrJointControl
{
 public:
  PrJointControl(int noj,int servoRate);
  ~PrJointControl();

  int noj() const { return noj_;}
  Float period() const { return period_; }

  PrVector &floatLaw();
  PrVector &PDLaw(PrVector *qg=NULL,PrVector *dqg=NULL,PrVector *ddqg=NULL);
  PrVector &goalPositionLaw();
  PrVector &sineLaw(Float time,Float wd=1.0);
  PrVector &trackLaw(Float time);
  PrVector &bobWaveLaw(Float time);
  void initTrackLaw(Float time);

  void PDLawBase(Float *qg,Float *dqg,Float *ddqg,Float *tau_prime);
  void goalPositionLawBase(Float *qg,Float *tau_prime);

  PrVector &currentPos() { return *q_; }
  PrVector &currentVel() { return *dq_; }

  PrVector &goalPos() { return *qg_; }
  PrVector &goalVel() { return *dqg_; }

  PrVector &highPos() { return *qHigh_; }
  PrVector &lowPos() { return *qLow_; }

  PrVector &gainPos() { return *kp_; }
  PrVector &gainVel() { return *kv_; }
  PrVector &gainInt() { return *ki_; }

  PrVector &velocityMax() { return *dqMax_; }
  PrVector &accelerationMax() { return *ddqMax_; }
  PrVector &integralMax() { return *eiMax_; }

  Float &currentPosGripper() { return qee_; }
  Float &goalPosGripper() { return qgee_; }
  Float &currentVelGripper() { return dqee_; }
  Float &gainPosGripper() { return kpee_; }
  Float &gainVelGripper() { return kvee_; }
  Float &velocityMaxGripper() { return dqMaxee_; }
  Float PDLawGripper();
  Float goalPostionLawGripper();
  void initIntegral();

  PrJointTrajectory &jointTrajectory(int num);

  void numSegments(int num);
  int numSegments() { return numSegments_; }

  PrVector &trackLaw2(Float time);
  void initTrackLaw2(Float time);
  void initTrajectory2(int i);

  void initTime(Float time){ iTime_ = time; }

  // BOB'S EXPERIMENTAL
  PrVector &desPos() { return   *qd_; }
  PrVector &desVel() { return  *dqd_; }
  PrVector &desAcc() { return *ddqd_; }
  PrVector &HulaBase(Float time);
  PrVector &HulaWave(Float time);



 private:
  const int noj_;
  const Float period_;

  Float fTime_;
  Float iTime_;
  Float (*a_)[4];

  int numSegments_;
  PrVector *headSegments_;
  PrVector *tf_;
  PrVector *ti_;
  PrJointTrajectory *jointTrajectory_[PR_JOINTCONTROL_JGOALS_MAX];

  PrVector *q_;
  PrVector *dq_;

  PrVector *qg_;
  PrVector *dqg_;

  PrVector *qd_;
  PrVector *dqd_;
  PrVector *ddqd_;

  PrVector *dqMax_;
  PrVector *ddqMax_;
  PrVector *eiMax_;

  PrVector *qHigh_;
  PrVector *qLow_;

  PrVector *tau_prime_;

  PrVector *kp_;
  PrVector *kv_;
  PrVector *ki_;

  int initializedEi_;
  PrVector *ei_;
  PrVector *ef_;
  PrVector *oef_;
  PrVector *e_;
  PrVector *oe_;

  // end-effector
  Float qee_;
  Float dqee_;
  Float qgee_;
  Float kpee_;
  Float kvee_;
  Float dqMaxee_;

  void GetTrajectory(Float time);
  void GetTrajectory2(Float time);
  void Setei(PrVector *qd);
  void SeteiBase(Float *qd);
  
  PrVector &PIDLaw(PrVector *qg=NULL,PrVector *dqg=NULL,PrVector *ddqg=NULL);
  void PIDLawBase(Float *qg,Float *dqg,Float *ddqg,Float *tau_prime);

  PrJointControl(const PrJointControl &) : noj_(0), period_(0) {}
};

#endif // _PrJointControl_h






