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

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

#include "PrExtendedOpControl.h"
#include "matrix/PrVector.h"
#include "matrix/PrVector6.h"
#include "matrix/PrMatrix.h"
#include "matrix/PrQuaternion.h"
#include "matrix/PrVector3.h"
#include "matrix/PrTransform.h"
#include "robot/PrLink.h"
#include "robot/PrRobot.h"
#include "PrOpControl.h"
#include "PrJointControl.h"
#include "PrTreeRobotIntf.h"


PrExtendedOpControl::PrExtendedOpControl(PrJointControl *jtControl,PrTreeRobotIntf *robot)
{
  noj_ = jtControl->noj(); 
  jtControl_ = jtControl;
  robot_ = robot;

  opControl_ = new PrOpControl;
//  opControl_->link(robot_->robot()->endEffector());
	opControl_->link(robot_->robot()->lastLink());
  opControl_->servoRate((int)(1.0/jtControl->period()));
  opControl_->id(0);

  tau_ = new PrVector(noj_);
  J_ = new PrMatrix(6,noj_);
  Jt_ = new PrMatrix(noj_,6);

  Jddqn_ = new PrVector(6);
  fo_ = new PrVector(6);
  dx_ = new PrVector(6);

  //dqo=qo=0.0; //Philip's filter
}

PrExtendedOpControl::~PrExtendedOpControl()
{
  delete opControl_;
  delete tau_;
  delete J_;
  delete Jt_;

  delete Jddqn_;
  delete fo_;
  delete dx_;
}

PrOpControl &PrExtendedOpControl::opControl()
{
  return *opControl_;
}

void PrExtendedOpControl::update()
{
  UpdateFrame();
  UpdateJacobianMatrix();
  UpdateVelocity();
}

PrVector &PrExtendedOpControl::computedTorque(const PrVector &ddqn)
{
  // tau = Jt R L Rt (ddx - J ddqn) + A ddqn + c + g
  // tau' = Jt R L Rt fo'_vec6
  //      = Jt R L f'_vec6
  //      = Jt R f_vec6
  //      = Jt fo_vec6
  //      = Jt fo

  // Jddqn = J * ddqn

  J_->multiply(ddqn,*Jddqn_);

  int i;

  // fo'_vec6 = (ddx - Jddqn)

  fo_prime_vec6_ = opControl_->force();
  for(i=0;i<6;i++)
    fo_prime_vec6_.elementAt(i) -= Jddqn_->elementAt(i);
  
  // f'_vec6 = Rt fo'_vec6

  (~(opControl_->link()->globalTransform().rotation())).multiply(fo_prime_vec6_[0],f_prime_vec6_[0]);
  (~(opControl_->link()->globalTransform().rotation())).multiply(fo_prime_vec6_[1],f_prime_vec6_[1]);
  
  // Lambda f'_vec6 = f_vec6
  UpdateLambdaMatrix();
  Lambda_.multiply(f_prime_vec6_,f_vec6_);

      // fo_vec6 = R f_vec6
  opControl_->link()->globalTransform().rotation().multiply(f_vec6_[0],fo_vec6_[0]);
  opControl_->link()->globalTransform().rotation().multiply(f_vec6_[1],fo_vec6_[1]);

      // fo = fo_vec6
  // fo = fo_vec6
  for(i=0;i<6;i++)
    fo_->elementAt(i) = fo_vec6_.elementAt(i);

  // tau = tau' = Jt fo
  J_->transpose(*Jt_);
  Jt_->multiply(*fo_,*tau_);

  // tau += A ddqn + c + g
  *tau_ += robot_->computedTorque(ddqn); 
  return *tau_;
}


// private methods

void PrExtendedOpControl::UpdateLambdaMatrix()
{
  robot_->robot()->localOSIM(Lambda_);
  for (int i=1;i<6;i++)
    for (int j=0;j<i;j++)
      Lambda_.elementAt(i,j) = Lambda_.elementAt(j,i);

  //  Lambda_.display("L");

}

void PrExtendedOpControl::UpdateJacobianMatrix()
{
  PrLink *link = opControl_->link();
  PrVector3 &globalP = link->globalTransform().translation();
  while(link->parent() != NULL)
    {
      PrVector6 &Jj = link->globalJacobian(globalP);
      for(int j=0;j<6;j++)
	J_->elementAt(j,link->id()) = Jj.elementAt(j);
      link = link->parent();
    }
}

void PrExtendedOpControl::UpdateFrame()
{
  opControl_->currentFrame() = opControl_->link()->globalTransform();
}

void PrExtendedOpControl::UpdateVelocity()
{
  J_->multiply(jtControl_->currentVel(),*dx_);

  PrVector6 &dxdPrev = opControl_->desiredVel();

  for(int j=0;j<6;j++) {
    opControl_->currentVel().elementAt(j) = dx_->elementAt(j);
  }

}


