/***************************************************************
 * PrOpControl.cpp
 *
 * This class provides the operational space control impelementation.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 05/12/00: Tine lefebvre: changes to continuous tracking mode
 * 04/06/00: Philip Tsai modified for continuous tracking mode
 * 10/14/98: K.C. Chang: created.
 */

#include "PrOpControl.h"
#include <stdio.h>

PrOpControl::PrOpControl(int servoRate,int id)
{
  period_ = 1.0/servoRate;
  id_ = id;
  for(int i=0;i<4;i++)
    for(int j=0;j<4;j++)
      a_[i][j]=0.0;

  dtheta=ddtheta=0.0;
  //continuous=0; -- a hack for PrExtendedOpControl before ICRA00
}

PrVector6 &PrOpControl::floatLaw()
{
  f_prime_.zero();
  return f_prime_;
}

PrVector6 &PrOpControl::PDLaw(PrTransform *Tg,PrVector6 *dxg,PrVector6 *ddxg)
{
  // f' = ddxg -kv (dx - dxg) - kp (T - Tg)
  if (Tg == NULL) Tg = &Tg_;
  if (dxg == NULL) dxg = &dxg_;
  if (ddxg == NULL)
    {
      ddxg = &ddxd_;
      ddxg->zero();
    }
  T_.error(*Tg,error_);
// if(
// fabs( error_[1].elementAt(0) ) >1  ||
// fabs( error_[1].elementAt(1) ) >1  ||
// fabs( error_[1].elementAt(2) ) >1
// )
// {
// error_[1].display();
// puts("");
// }
  f_prime_ = *ddxg - kv_ * (dx_ - *dxg) - kp_ * error_;

  return f_prime_;
}

PrVector6 &PrOpControl::goalPositionLaw()
{
	// f' = -kv dx - kp (T-Tg)
	//        f' = -kv (dx - mu*dxg) 
	//        with mu=sat(vMax/|dxg|)
	//             dxg=kp/kv (Tg-T)
	//
	// Note: PrVector6 consists of two PrVector3
	//       which can be accessed by [0] and [1]

  PrVector6 dxg;
  Float normal;

  Tg_.error(T_,error_);
  for(int i=0;i<6;i++)
    {
      if (kv_.elementAt(i) != 0.0)
	dxg.elementAt(i) = kp_.elementAt(i)*error_.elementAt(i)/kv_.elementAt(i);
      else
	dxg.elementAt(i) = 0.0;
    }

  // maximum translational velocity
  normal = dxg[0].magnitude();
  if (normal > vMax_) 
  {
    normal = vMax_/normal;
    dxg[0]*=normal;
  }

  // maximum rotational velocity
  normal = dxg[1].magnitude();
  if (normal > wMax_) 
    {
      normal = wMax_/normal;
      dxg[1]*=normal;
    }

  f_prime_ = kv_*(dxg - dx_);

  return f_prime_;
}

PrVector6 &PrOpControl::sineLaw(Float time,Float wd)
{
  time = 0.0;
  wd = 0.0;
  f_prime_.zero();
  return f_prime_;
}


PrVector6 &PrOpControl::WaxOn(Float time)
{
  static PrVector6 dxg,ddxg;

  Float t = time - iTime_;

  Float  r = 0.1;
  Float wr = 3.1415;
  Float ramp = 1.0;

  Float xg[3];
  
  static Float x0[3];
  
   if(t < 0.0015)
   { x0[0] = T_.translation().elementAt(0);
     x0[1] = T_.translation().elementAt(1);
     x0[2] = T_.translation().elementAt(2);
   }

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

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

       xg[0]          =  x0[0];
      dxg.elementAt(0)=  0.0;
     ddxg.elementAt(0)=  0.0;

       xg[1]          =  r*      sr      + x0[1];
      dxg.elementAt(1)=  r*wr*   cr ;
     ddxg.elementAt(1)= -r*wr*wr*sr ;

       xg[2]          =  r*      cr - r  + x0[2];
      dxg.elementAt(2)= -r*wr*   sr ;
     ddxg.elementAt(2)= -r*wr*wr*cr ;

   }

  Tg_.translation().values( xg[0], xg[1], xg[2] );

  PDLaw( &Tg_, &dxg, &ddxg);

  return f_prime_;

}


PrVector6 &PrOpControl::trackLaw(Float time)
{
	// Note: PrVector6 consistes of two PrVector3
	//	     which can be accessed by [0] and [1]

  GetTrajectory(time);
  PDLaw(&Td_,&dxd_,&ddxd_);

  Setei();
  f_prime_ += ki_ * ei_;

  return f_prime_;
}

void PrOpControl::initTrackLaw(Float time)
{
// calculation of the constants a for the spline determining the motion
// constants: a_[i][j]  (see book Craig chapter 7)
//            i: 0-2: xyz, 3: rotation
//            j: 0-3: parameters (cubic) spline


	// Note: Td is of type PrTransfer (vector+Quaternion)
        //
	// dxd_ is PrVector6, consisting of two PrVector3: 
	// dxd_[0] and dxd_[1]: translation and rotation velocities
        // ddxd_ has a similar structure for the accelerations

  iTime_ = time;
  ei_.zero();
  ef_.zero();
  oef_.zero();
  e_.zero();
  oe_.zero();

  // theta, axis_ will be the rotation angle and axis
  // between the current (T_) and the goal frame (Tg_)
  // 
  // ~Ri_ represents the inverse of the rotation Ri_
  //
  // the * of two quaternions produces a new quaternion
  // that represents the 'sum' of the two rotations
  //
  // also the structure here (~ and *) gives the quaternion
  // that represents the angle rotation between both frames
  //
  Ri_ = T_.rotation();
  Float theta;
  ((~Ri_)*Tg_.rotation()).axisAngle(axis_,theta);

  Float tfi,tvel,tacc,tf=0.0;
  int i;

  //figure out which translation requires the most time
  for (i=0;i<3;i++)
    {
      tvel = 1.5*fabs(Tg_.translation()[i]-T_.translation()[i])/vMax_;
      tacc = sqrt(6*fabs(Tg_.translation()[i]-T_.translation()[i])/dvMax_);
      tfi = (tvel > tacc) ? tvel:tacc;
      if (tfi > tf) tf=tfi;
    }
  //see if orientation takes longer
  tvel = 1.5*fabs(theta)/wMax_;
  tacc = sqrt(6*fabs(theta)/dwMax_);
  tfi = (tvel > tacc) ? tvel:tacc;
  if (tfi > tf) tf=tfi;

  fTime_ = tf;
  // ::printf("\ninitTrackLaw: duration=%f\n",fTime_);

  //assuming zeros for initial and final velocities (7.6)
  for(i=0;i<3;i++)
    {
      a_[i][0] = T_.translation()[i];
      a_[i][1] = 0.0;
      a_[i][2] = 3*(Tg_.translation()[i]-T_.translation()[i])/(tf*tf);
      a_[i][3] = -2*(Tg_.translation()[i]-T_.translation()[i])/(tf*tf*tf);
    }
  i=3;
  a_[i][0] = 0.0;
  a_[i][1] = 0.0;
  a_[i][2] = 3*(theta)/(tf*tf);
  a_[i][3] = -2*(theta)/(tf*tf*tf);
}  

void PrOpControl::initContinuousTrackLaw(Float time)
{
// see comments in function initTrackLaw()
  iTime_ = time;
  ei_.zero();
  ef_.zero();
  oef_.zero();
  e_.zero();
  oe_.zero();

  //continuous=1; //tell PrExtendedOpControl's updateVelocity that this is cont. -- a hack before ICRA demo: attempting to smoothe velocity data

  Ri_ = T_.rotation();

  //theta needs to be current theta...
  Float theta;
  ((~Ri_)*Tg_.rotation()).axisAngle(axis_,theta);

  Float tfi,tvel,tacc,tf=0.0;
  int i;
  //::printf("\ntime=%f\n",time);
  //::printf("initContinuousTrackLaw: tf before calc.=%f\n",tf);

  //figure out which translation requires the most time
  for (i=0;i<3;i++)
    {
      float diffVel= dxd_[0][i]*dx_[0][i]>0 ? 0 : fabs(dxd_[0][i]-dx_[0][i]);
      //::printf("%d, diffVel=%f, diffPos=%f, Tg=%f, T=%f, dxd=%f, dx=%f, vMax=%f, dvMax=%f\n",i, diffVel, (Tg_.translation()[i]-T_.translation()[i]), Tg_.translation()[i], T_.translation()[i], dxd_[0][i], dx_[0][i], vMax_, dvMax_);

      tvel = 1.5*fabs(Tg_.translation()[i]-T_.translation()[i])/vMax_;
      tacc = sqrt(6*fabs(Tg_.translation()[i]-T_.translation()[i])/dvMax_);
      tfi = (tvel > tacc) ? tvel:tacc;

      tfi = dxd_[0][i]*dx_[0][i]>0 ? tfi : tfi*1.5;

      if (tfi > tf) tf=tfi;
    }
  //see if orientation takes longer -- since we aren't tracking orientation
  //it shouldn't be the case
  //::printf("theta=%f, dtheta=%f\n",theta, dtheta);
  tvel = 1.5*fabs(theta)/wMax_;
  tacc = sqrt(6*fabs(theta)/dwMax_);
  tfi = (tvel > tacc) ? tvel:tacc;
  if (tfi > tf) tf=tfi;

  //::printf("initContinuousTrackLaw: tf after calc.=%f\n",tf);

  if (fabs(tf) < 0.001) {
    for(i=0;i<=3;i++)
      a_[i][0] = a_[i][1] = a_[i][2] = a_[i][3] = 0.0;
    return;
  }

  //full calculation doesn't render a nice soln.  7 cases to consider
  //not worth the efforts right now -- Currently reflected in Line 247
  //tf *= 1.5;
  //tf = 5; -- experimental value

  fTime_ = tf; 
  ::printf("\ninitContinuousTrackLaw: estimated duration=%f\n",fTime_);

  // (7.9) assuming zero final velocity 
  for(i=0;i<3;i++)
    {
      a_[i][0] = T_.translation()[i];
      //::printf("dxd_[0][%d]=%f\n",i,dxd_[0][i]);
      //::printf("dx_[0][%d]=%f\n",i,dx_[0][i]);
      a_[i][1] = dx_[0][i];
      a_[i][2] = (3*(Tg_.translation()[i]-T_.translation()[i])/tf-2*dx_[0][i])
	/ tf;
      a_[i][3] = (-2*(Tg_.translation()[i]-T_.translation()[i])/tf+dx_[0][i])
	/(tf*tf);
    }
  i=3;
  //::printf("dtheta=%f\n\n", dtheta);

  // Tine: I suggest following code:
  // a_[i][0]=0;
  // a_[i][1]=-dtheta;
  // a_[i][2]=(3*theta/tf+2*dtheta)/tf;
  // a_[i][3]=(-2*theta/tf-dtheta)/(tf*tf);
  // Philip : Suggestions taken. Fixed.
  a_[i][0] = 0.0;
  a_[i][1] = -dtheta;
  a_[i][2] = (3*theta/tf + 2*dtheta)/tf;
  a_[i][3] = (-2*theta/tf - dtheta)/(tf*tf);


}  

void PrOpControl::GetTrajectory(Float time)
{
// see comments in function initTrackLaw

  Float t = time - iTime_;
  if (t > fTime_) 
    {
      Td_ = Tg_;
      dxd_.zero();
      ddxd_.zero();
    } 
  else 
    {
      int i;
      // (7.3) & (7.4)
      for(i=0;i<3;i++)
	{
	  Td_.translation()[i]=a_[i][0]+(a_[i][1]+(a_[i][2]+a_[i][3]*t)*t)*t;
	  dxd_[0][i]=a_[i][1] + (2*a_[i][2] + 3*a_[i][3]*t)*t;
	  ddxd_[0][i]=2*a_[i][2] + 6*a_[i][3]*t;
	}
      i=3;
      //change:  we need to keep track of dtheta and ddtheta for continuous
      //tracking with continuous velocity (strictly, only dtheta is needed)
      Float theta=a_[i][0]+(a_[i][1]+(a_[i][2]+a_[i][3]*t)*t)*t;
      dtheta=a_[i][1] + (2*a_[i][2] + 3*a_[i][3]*t)*t;
      ddtheta=2*a_[i][2] + 6*a_[i][3]*t;


	  // multiply Ri_ with PrQuaternion(axis_,theta)
	  // This means taking the 'sum' of both rotations
      // the result is stored in Td_.rotation()
	  Ri_.multiply(PrQuaternion(axis_,theta),Td_.rotation());
      /* Wo = R06 * W6 */
      Ri_.multiply(axis_*dtheta,dxd_[1]);
      Ri_.multiply(axis_*ddtheta,ddxd_[1]);
    }
}

/*
 * do a tustin approximation for wi/(s + wi)
 */
void PrOpControl::Setei()
{
  int wi=1;
  Float b = period_*wi*0.5; 
  
  Td_.error(T_,e_);
  ef_ = (e_ + oe_)*b - oef_*(b-1.0);

  oef_ = ef_;
  oe_ = e_;

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




