/***************************************************************
 * PrOpControl.h
 *
 * This class provides the operational space control interface.
 *
 ****************************************************************/

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


	// Tine  May 12, 2000	
	// Note: PrVector6 consists of two PrVector3
        //       which can be accessed by [0] and [1]


#ifndef _PrOpControl_h
#define _PrOpControl_h

#include "PrGlobalDefn.h"
#include "matrix/PrVector3.h"
#include "matrix/PrVector6.h"
#include "matrix/PrMatrix6.h"
#include "matrix/PrQuaternion.h"
#include "matrix/PrTransform.h"

class PrLink;

class PrOpControl
{
 public:
  PrOpControl(int servoRate = 1,int id = -1);
  ~PrOpControl() {}

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

  void servoRate(int rate) { period_ = 1.0/rate; }
  Float period() const { return period_; }

  PrLink *link() { return link_; }
  void link(PrLink *n) { link_ = n; }

  PrMatrix6 &P() { return P_; }
  PrVector6 &force() { return f_prime_; }
  PrVector6 &floatLaw();
  PrVector6 &PDLaw(PrTransform *Tg=NULL,PrVector6 *dxg=NULL,
		   PrVector6 *ddxg=NULL);
  PrVector6 &goalPositionLaw();
  PrVector6 &sineLaw(Float time,Float wd=1.0);
  PrVector6 &trackLaw(Float time);
  void initTrackLaw(Float time);
  void initContinuousTrackLaw(Float time);

  PrTransform &currentFrame() { return T_; }
  PrVector6 &currentVel() { return dx_; }
  PrVector6 &desiredVel() { return dxd_; }

  PrTransform &goalFrame() { return Tg_; }
  PrVector6 &goalVel() { return dxg_; }
  //int isContinuous() { return continuous; }

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

  void vMax(Float vMax) { vMax_ = vMax; }
  Float vMax() const { return vMax_; }
  void wMax(Float wMax) { wMax_ = wMax; }
  Float wMax() const { return wMax_; }
  void dvMax(Float dvMax) { dvMax_ = dvMax; }
  Float dvMax() const { return dvMax_; }
  void dwMax(Float dwMax) { dwMax_ = dwMax; }
  Float dwMax() const { return dwMax_; }
  void integralMax(Float eiMax) { eiMax_ = eiMax; }
  Float integralMax() const { return eiMax_; }

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


  // BOB'S EXPERIMENTS
  PrVector6 &WaxOn(Float time);


 private:
  Float period_;
  int id_;
  PrLink *link_;

  PrMatrix6 P_;
  Float fTime_;
  Float iTime_;
  Float vMax_;
  Float dvMax_;
  Float wMax_;
  Float dwMax_;
  Float eiMax_;
  Float a_[4][4];

  PrQuaternion Ri_;
  PrVector3 axis_;

  PrTransform T_;
  PrVector6 dx_;

  PrTransform Tg_;
  PrVector6 dxg_;

  PrTransform Td_;
  PrVector6 dxd_;
  PrVector6 ddxd_;
  Float dtheta;
  Float ddtheta;
  //int continuous; -- a hack for ICRA demo, used in PrExtendedOpControl.cpp

  PrVector6 error_;
  PrVector6 f_prime_;

  PrVector6 kp_;
  PrVector6 kv_;
  PrVector6 ki_;

  PrVector6 ei_;
  PrVector6 ef_;
  PrVector6 oef_;
  PrVector6 e_;
  PrVector6 oe_;

  void GetTrajectory(Float time);
  void Setei();
  
  PrOpControl(const PrOpControl &) {}
};

#endif // _PrOpControl_h
