/***************************************************************
 * PrJointControl.cpp
 *
 * This class provides the joint control impelementation.
 *
 ****************************************************************/

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

#include "PrJointControl.h"
#include "matrix/PrVector.h"
#include <stdio.h>

PrJointTrajectory::PrJointTrajectory(int noj)
  : noj_(noj)
{
  q_ = new PrVector(noj_);
  dq_ = new PrVector(noj_);
  tf_ = new PrVector(noj_);
}

PrJointTrajectory::~PrJointTrajectory() 
{
  delete q_;
  delete dq_;
  delete tf_;
}

void PrJointTrajectory::values(float *data)
{
  for(int i=0;i<noj_;i++)
    {
      q_->elementAt(i) = data[i];
      dq_->elementAt(i) = data[i+noj_];
      tf_->elementAt(i) = data[i+noj_+noj_];
    }
}

PrJointControl::PrJointControl(int noj,int servoRate) 
  : noj_(noj), period_(1.0/servoRate) 
{
	a_ = new Float[noj_][4];
	for(int i=0;i<noj_;i++)
	  for(int j=0;j<4;j++)
		a_[i][j]=0.0;

	q_ = new PrVector(noj_);
	dq_ = new PrVector(noj_);

	qg_ = new PrVector(noj_);
	dqg_ = new PrVector(noj_);

	qd_ = new PrVector(noj_);
	dqd_ = new PrVector(noj_);
	ddqd_ = new PrVector(noj_);

	dqMax_ = new PrVector(noj_);
	ddqMax_ = new PrVector(noj_);
	eiMax_ = new PrVector(noj_);

	qHigh_ = new PrVector(noj_);
	qLow_ = new PrVector(noj_);

	int k;
	for (k=0;k<noj_;k++)
	  {
	    dqMax_->elementAt(k) = 0.1;
	    ddqMax_->elementAt(k) = 1.0;
	    eiMax_->elementAt(k) = 1.0;
	    qHigh_->elementAt(k) = 0.7;
	    qLow_->elementAt(k) = -0.7;
	  }

	dqMaxee_ = 0.1;
	
	tau_prime_ = new PrVector(noj_);

	kp_ = new PrVector(noj_);
	kv_ = new PrVector(noj_);
	ki_ = new PrVector(noj_);

	ei_ = new PrVector(noj_);
	ef_ = new PrVector(noj_);
	oef_ = new PrVector(noj_);
	e_ = new PrVector(noj_);
	oe_ = new PrVector(noj_);

	headSegments_ = new PrVector(noj_);
	tf_ = new PrVector(noj_);
	ti_ = new PrVector(noj_);

	for(k=0;k<PR_JOINTCONTROL_JGOALS_MAX;k++)
	  jointTrajectory_[k] = new PrJointTrajectory(noj_);
	numSegments_ = 0;
}

PrJointControl::~PrJointControl()
{
  delete[] a_;

  delete q_;
  delete dq_;

  delete qg_;
  delete dqg_;

  delete qd_;
  delete dqd_;
  delete ddqd_;

  delete dqMax_;
  delete ddqMax_;
  delete eiMax_;

  delete qHigh_;
  delete qLow_;

  delete tau_prime_;

  delete kp_;
  delete kv_;
  delete ki_;

  delete ei_;
  delete ef_;
  delete oef_;
  delete e_;
  delete oe_;

  delete headSegments_;
  delete tf_;
  delete ti_;

  for(int i=0;i<PR_JOINTCONTROL_JGOALS_MAX;i++)
    delete jointTrajectory_[i];
}

PrVector &PrJointControl::floatLaw()
{
  tau_prime_->zero();
  return *tau_prime_;
}

PrVector &PrJointControl::PDLaw(PrVector *qg,PrVector *dqg,PrVector *ddqg)
{
  // t' = ddqg -kv (dq - dqg) - kp (q - qg)
  if (qg == NULL) qg = qg_;
  if (dqg == NULL) dqg = dqg_;
  if (ddqg == NULL)
    {
      ddqg = ddqd_;
      ddqg->zero();
    }
  *tau_prime_ = *ddqg - *kv_ * (*dq_ - *dqg) - *kp_ * (*q_ - *qg);
  return *tau_prime_;
}

PrVector &PrJointControl::goalPositionLaw()
{
  Float dqg;

  for(int i=0;i<noj_;i++)
    {
      if (kv_->elementAt(i) != 0.0)
	{
	  dqg = -kp_->elementAt(i)*
	    (q_->elementAt(i) - qg_->elementAt(i))/kv_->elementAt(i);

	  if (dqg > dqMax_->elementAt(i)) 
	    dqg = dqMax_->elementAt(i);
	  else if (dqg < -dqMax_->elementAt(i)) 
	    dqg = -dqMax_->elementAt(i);

	  tau_prime_->elementAt(i) = -kv_->elementAt(i)*
	    (dq_->elementAt(i) - dqg);
	}
      else
	{
	  tau_prime_->elementAt(i) = -kp_->elementAt(i)*
	    (q_->elementAt(i) - qg_->elementAt(i));
	}
      /*
      if (tau_prime_->elementAt(i) > ddqMax_->elementAt(i))
	tau_prime_->elementAt(i) = ddqMax_->elementAt(i);
      else if (tau_prime_->elementAt(i) < -ddqMax_->elementAt(i))
	tau_prime_->elementAt(i) = -ddqMax_->elementAt(i);
      */
    }
  return *tau_prime_;
}

PrVector &PrJointControl::sineLaw(Float time,Float wd)
{
  Float u = (sin(wd*time)+1.0)*0.5;
  Float udot = wd*cos(wd*time)*0.5;
  Float uddot = -wd*wd*sin(wd*time)*0.5;
  for(int i=0;i<noj_;i++)
    {
      qd_->elementAt(i) = u*qLow_->elementAt(i)+(1.0-u)*qHigh_->elementAt(i);
      dqd_->elementAt(i) = udot*(qLow_->elementAt(i)-qHigh_->elementAt(i));
      ddqd_->elementAt(i) = uddot*(qLow_->elementAt(i)-qHigh_->elementAt(i));
    }
  return PDLaw(qd_,dqd_,ddqd_);
}

PrVector &PrJointControl::trackLaw(Float time)
{
  if (initializedEi_ == 1)
      Setei(qd_);
  else
    initializedEi_ = 1;

  GetTrajectory(time);
  PIDLaw(qd_,dqd_,ddqd_);

  return *tau_prime_;
}


PrVector &PrJointControl::bobWaveLaw(Float time)
{
  if (initializedEi_ == 1)
      Setei(qd_);
  else
    initializedEi_ = 1;

  GetTrajectory(time);

  Float t = time - iTime_;

  Float wd = 6.0; // HACK
  Float a =  0.3; // HACK
  Float u     =    (1-cos(wd*t))*a;
  Float udot  =     wd*sin(wd*t)*a;
  Float uddot =  wd*wd*cos(wd*t)*a;
  for(int i=3;i<4;i++)
    {
       qd_->elementAt(i) = u;
       dqd_->elementAt(i) = udot;
       ddqd_->elementAt(i) = uddot;
    }

  PDLaw(qd_,dqd_,ddqd_);

  return *tau_prime_;
}





PrVector &PrJointControl::HulaBase(Float time)
{
  Float t = time - iTime_;

  Float  r = 0.075;
  Float wr = 3.0;
  Float  a = 0.0;
  Float wa = 1.5;
  Float ramp = 1.0;

  static Float q0[3];

   if(t < 0.0015 )
   { q0[0] = currentPos()[0];
     q0[1] = currentPos()[1];
     q0[2] = currentPos()[2];
   }

   if( t<ramp )
   { wr = sqrt(t/ramp) * wr;
     wa = sqrt(t/ramp) * wa;
   }

     Float cr = cos( wr*t );
     Float sr = sin( wr*t );

       qd_->elementAt(0)=  r*      cr - r + q0[0];
      dqd_->elementAt(0)= -r*wr*   sr ;
     ddqd_->elementAt(0)= -r*wr*wr*cr ;

       qd_->elementAt(1)=  r*      sr     + q0[1];
      dqd_->elementAt(1)=  r*wr*   cr ;
     ddqd_->elementAt(1)= -r*wr*wr*sr ;

     Float ca = cos( wa*t );
     Float sa = sin( wa*t );

       qd_->elementAt(2)=  a*   (1-ca)    + q0[2];
      dqd_->elementAt(2)=  a*wa*   sa ;
     ddqd_->elementAt(2)=  a*wa*wa*ca ;

  PDLaw(qd_,dqd_,ddqd_);

  // ONLY CONTROL **BASE** IN NULL SPACE...
  tau_prime_->elementAt(3) = 0.0;
  tau_prime_->elementAt(4) = 0.0;
  tau_prime_->elementAt(5) = 0.0;

  tau_prime_->elementAt(6) = 0.0;
  tau_prime_->elementAt(7) = 0.0;
  tau_prime_->elementAt(8) = 0.0;

  return *tau_prime_;
}




PrVector &PrJointControl::HulaWave(Float time)
{

  // TRACK TO MOVE ARM TO GOAL POSTURE
  if (initializedEi_ == 1)
      Setei(qd_);
  else
    initializedEi_ = 1;

  GetTrajectory(time);

  Float t = time - iTime_;

  // HULA AND SHIMMY THE BASE
  Float  r = 0.1;
  Float wr = 2.0;
  Float  a = 0.5;
  Float wa = 1.2;
  Float ramp = 1.0;

  static Float q0[3];

   if(t < 0.0015 )
   { q0[0] = currentPos()[0];
     q0[1] = currentPos()[1];
     q0[2] = currentPos()[2];
   }

   if( t<ramp )
   { wr = sqrt(t/ramp) * wr;
     wa = sqrt(t/ramp) * wa;
   }

     Float cr = cos( wr*t );
     Float sr = sin( wr*t );

       qd_->elementAt(0)=  r*      cr - r + q0[0];
      dqd_->elementAt(0)= -r*wr*   sr ;
     ddqd_->elementAt(0)= -r*wr*wr*cr ;

       qd_->elementAt(1)=  r*      sr     + q0[1];
      dqd_->elementAt(1)=  r*wr*   cr ;
     ddqd_->elementAt(1)= -r*wr*wr*sr ;

     Float ca = cos( wa*t );
     Float sa = sin( wa*t );

       qd_->elementAt(2)=  a*   (1-ca)    + q0[2];
      dqd_->elementAt(2)=  a*wa*   sa ;
     ddqd_->elementAt(2)=  a*wa*wa*ca ;


  // WAVE THE ARM
  Float wwd = 5.0; // HACK
  Float wwa =  0.3; // HACK
  Float u     =      (1-cos(wwd*t))*wwa;
  Float udot  =      wwd*sin(wwd*t)*wwa;
  Float uddot =  wwd*wwd*cos(wwd*t)*wwa;
  for(int i=3;i<4;i++)
    {
       qd_->elementAt(i) = u;
       dqd_->elementAt(i) = udot;
       ddqd_->elementAt(i) = uddot;
    }

  PDLaw(qd_,dqd_,ddqd_);

  return *tau_prime_;
}





void PrJointControl::initTrackLaw(Float time)
{
  iTime_ = time;

  Float tfi,tvel,tacc,tf=0.0;
  int i;
qg_->display("itl:qg_");

  for (i=0;i<noj_;i++)
    {
      tvel = 1.5*fabs(qg_->elementAt(i)-q_->elementAt(i))/dqMax_->elementAt(i);
      tacc = sqrt(6*fabs(qg_->elementAt(i)-q_->elementAt(i))/ddqMax_->elementAt(i));
      tfi = (tvel > tacc) ? tvel:tacc;
      if (tfi > tf) tf=tfi;
    }
  fTime_ = tf;
  ::printf("duration=%f\n",fTime_);

  for(i=0;i<noj_;i++)
    {
      a_[i][0] = q_->elementAt(i);
      a_[i][1] = 0.0;
      a_[i][2] = 3*(qg_->elementAt(i)-q_->elementAt(i))/(tf*tf);
      a_[i][3] = -2*(qg_->elementAt(i)-q_->elementAt(i))/(tf*tf*tf);
    }
}  


void PrJointControl::initIntegral()
{
  ei_->zero();
  ef_->zero();
  oef_->zero();
  e_->zero();
  oe_->zero();

  initializedEi_ = 0;
}

void PrJointControl::GetTrajectory(Float time)
{
  Float t = time - iTime_;
  if (t > fTime_) 
    {
      *qd_ = *qg_;
      dqd_->zero();
      ddqd_->zero();
    } 
  else 
    {
      for(int i=0;i<noj_;i++)
	{
	  qd_->elementAt(i)=a_[i][0]+(a_[i][1] + (a_[i][2] + a_[i][3]*t)*t)*t;
	  dqd_->elementAt(i)=a_[i][1] + (2*a_[i][2] + 3*a_[i][3]*t)*t;
	  ddqd_->elementAt(i)=2*a_[i][2] + 6*a_[i][3]*t;
	}
    }
}

void PrJointControl::PDLawBase(Float *qg,Float *dqg,Float *ddqg,Float *tau_prime)
{
  // t' = ddqg -kv (dq - dqg) - kp (q - qg)

  for(int i=0;i<3;i++)
    tau_prime[i] = ddqg[i] 
      - kv_->elementAt(i) * (dq_->elementAt(i) - dqg[i]) 
      - kp_->elementAt(i) * (q_->elementAt(i)  - qg[i]);
}

void PrJointControl::goalPositionLawBase(Float *qg,Float *tau_prime)
{
  Float dqg;

  for(int i=0;i<3;i++)
    {
      if (kv_->elementAt(i) != 0.0)
	{
	  dqg = -kp_->elementAt(i)*
	    (q_->elementAt(i) - qg[i])/kv_->elementAt(i);

	  if (dqg > dqMax_->elementAt(i)) dqg = dqMax_->elementAt(i);
	  else if (dqg < -dqMax_->elementAt(i)) dqg = -dqMax_->elementAt(i);

	  tau_prime[i] = -kv_->elementAt(i)*(dq_->elementAt(i) - dqg);
	}
      else
	{
	  tau_prime[i] = -kp_->elementAt(i)*(q_->elementAt(i) - qg[i]);
	}
    }
}

Float PrJointControl::PDLawGripper()
{
  return ( -kpee_*(qee_-qgee_) - kvee_*dqee_);
}

Float PrJointControl::goalPostionLawGripper()
{
  Float dqg = 0.0;
  Float tau = 0.0;

  if (kvee_ != 0.0)
    {
      dqg = -kpee_*(qee_ - qgee_)/kvee_;
      
      if (dqg > dqMaxee_) 
	dqg = dqMaxee_;
      else if (dqg < -dqMaxee_) 
	dqg = -dqMaxee_;

      tau = -kvee_*(dqee_ - dqg);
    }
  else
    {
      tau = -kpee_*(qee_ - qgee_);
    }
  return tau;
}

/*
 * do a tustin approximation for wi/(s + wi)
 */
void PrJointControl::Setei(PrVector *qd)
{
  //  int wi=1;
  //  Float b = period_*wi*0.5; 
  
  *e_ = *qd - *q_;

  //  *ef_ = (*e_ + *oe_)*b - *oef_*(b-1.0);

  *ef_ = *e_;

  *oef_ = *ef_;
  *oe_ = *e_;

  *ei_ += *ef_;

  for(int i=0;i<noj_;i++)
    {
      if (ei_->elementAt(i) > eiMax_->elementAt(i)) 
	ei_->elementAt(i) = eiMax_->elementAt(i);
      else if (ei_->elementAt(i) < -eiMax_->elementAt(i)) 
	ei_->elementAt(i) = -eiMax_->elementAt(i);
    }
}

/*
 * do a tustin approximation for wi/(s + wi)
 */
void PrJointControl::SeteiBase(Float *qd)
{
  //  int wi=1;
  //  Float b = period_*wi*0.5; 
  
  for(int i=0;i<3;i++)
    {
      e_->elementAt(i) = qd[i] - q_->elementAt(i);

      //      ef_->elementAt(i) = (e_->elementAt(i) + oe_->elementAt(i))*b - oef_->elementAt(i)*(b-1.0);

      ef_->elementAt(i) = e_->elementAt(i);

      oef_->elementAt(i) = ef_->elementAt(i);
      oe_->elementAt(i) = e_->elementAt(i);

      ei_->elementAt(i) += ef_->elementAt(i);

      if (ei_->elementAt(i) > eiMax_->elementAt(i)) 
	ei_->elementAt(i) = eiMax_->elementAt(i);
      else if (ei_->elementAt(i) < -eiMax_->elementAt(i)) 
	ei_->elementAt(i) = -eiMax_->elementAt(i);
    }
}

PrJointTrajectory &PrJointControl::jointTrajectory(int num)
{
  assert(num>=0 && num<numSegments_);
  return *jointTrajectory_[num];
}

void PrJointControl::numSegments(int num)
{ 
  if (num > PR_JOINTCONTROL_JGOALS_MAX)
    num = PR_JOINTCONTROL_JGOALS_MAX;
  numSegments_ = num; 
}

PrVector &PrJointControl::trackLaw2(Float time)
{
  if (initializedEi_ == 1)
      Setei(qd_);
  else
    initializedEi_ = 1;

  GetTrajectory2(time);
  PIDLaw(qd_,dqd_,ddqd_);

  return *tau_prime_;
}

void PrJointControl::initTrackLaw2(Float time)
{
  //  initIntegral();

  for(int j=0;j<noj_;j++)
    ti_->elementAt(j) = time;

  headSegments_->zero();

  *qg_ = jointTrajectory_[0]->q();
  *dqg_ = jointTrajectory_[0]->dq();
  *tf_ = jointTrajectory_[0]->tf();

  for(int i=0;i<noj_;i++)
    initTrajectory2(i);
}  

void PrJointControl::initTrajectory2(int i)
{
  Float tf = tf_->elementAt(i);

  if (tf > 0)
    {
      a_[i][0] = q_->elementAt(i);
      a_[i][1] = dq_->elementAt(i);
      a_[i][2] = 3*(qg_->elementAt(i) - q_->elementAt(i))/(tf*tf)
	- 2*dq_->elementAt(i)/tf
	- dqg_->elementAt(i)/tf;
      a_[i][3] = -2*(qg_->elementAt(i) - q_->elementAt(i))/(tf*tf*tf)
	+ (dqg_->elementAt(i) + dq_->elementAt(i))/(tf*tf); 
    }
  else
    {
      tf_->elementAt(i) = 0.0;
      a_[i][0] = q_->elementAt(i);
      a_[i][1] = dq_->elementAt(i);
      a_[i][2] = 0.0;
      a_[i][3] = 0.0;
    }
}

void PrJointControl::GetTrajectory2(Float time)
{
  for(int i=0;i<noj_;i++)
    {
      Float t = time - ti_->elementAt(i);
      if (t > tf_->elementAt(i))
	{
	  int num = headSegments_->elementAt(i);
	  if ( num < numSegments_)
	    {
	      headSegments_->elementAt(i) = num+1;
	      ti_->elementAt(i) = time;
	      qg_->elementAt(i) = jointTrajectory_[num]->q().elementAt(i);
	      dqg_->elementAt(i) = jointTrajectory_[num]->dq().elementAt(i);
	      tf_->elementAt(i) = jointTrajectory_[num]->tf().elementAt(i);
	      initTrajectory2(i);
	    }
	 else
	   { 
	     qd_->elementAt(i) = qg_->elementAt(i);
	     dqd_->elementAt(i) = 0.0;
	     ddqd_->elementAt(i) = 0.0;
	   }
	}
      else
	{
	  qd_->elementAt(i)=a_[i][0]+(a_[i][1] + (a_[i][2] + a_[i][3]*t)*t)*t;
	  dqd_->elementAt(i)=a_[i][1] + (2*a_[i][2] + 3*a_[i][3]*t)*t;
	  ddqd_->elementAt(i)=2*a_[i][2] + 6*a_[i][3]*t;
	}
    }
}

// private
PrVector &PrJointControl::PIDLaw(PrVector *qg,PrVector *dqg,PrVector *ddqg)
{
  // t' = ddqg -kv (dq - dqg) - kp (q - qg) + ki(ei)
  PDLaw(qg,dqg,ddqg);
  *tau_prime_ += *ki_ * *ei_;
  return *tau_prime_;
}

void PrJointControl::PIDLawBase(Float *qg,Float *dqg,Float *ddqg,Float *tau_prime)
{
  // t' = ddqg -kv (dq - dqg) - kp (q - qg) + ki(ei)
  PDLawBase(qg,dqg,ddqg,tau_prime);
  for(int i=0;i<3;i++)
    tau_prime[i] += ki_->elementAt(i) * ei_->elementAt(i);
}





