/***************************************************************
 *  * PrTaskControl.cpp
 *   *
 *    * This class provides the Multi-tasks control impelementation.
 *     *
 *      ****************************************************************/

/*
 *  * modification history
 *   *----------------------
 *    *
 *     * 7/28/03: Jaeheung Park: created. "experimental version"
 *      */

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


PrTask::PrTask(int servoRate,PrTreeRobotIntf *robot,PrRobot* robotVirtual)
{
	servoRate_ = servoRate;
	robot_ = robot;
	robotVirtual_ = robotVirtual;
	noj_ = robot_->noj();

	Identity_ = new PrMatrix(noj_,noj_);

}

PrTask::~PrTask()
{
	delete J_;
	delete Jt_;
	delete JAinv_;
	delete JAinvt_;
	delete Lambda_;
	delete LambdaInv_;
	delete Nt_;
	delete x_;
	delete dx_;
}

void
PrTask::UpdateCoordinate(){
	// this will be implemeted in the inherited class
	printf("updateCoordinate\n");
}

void
PrTask::UpdateJacobianMatrix(){
	// this will be implemeted in the inherited class
	// it should update J_ and Jt_
}

void
PrTask::UpdateVelocity(PrVector dq){
	*dx_ = (*J_)*(dq);
}

void
PrTask::UpdateJAinv(){
	// use fwdDynamics
  for(int i=0;i<dof_;i++) {

	for(int j=0;j<noj_;j++) {
		robotVirtual_->jointForce()[j] = Jt_->elementAt(j,i);
	}
	robotVirtual_->fwdDynamics();
	for(j=0;i<noj_;j++) {
		JAinvt_->elementAt(j,i) 
		= robotVirtual_->jointAcceleration()[j];
	}

	}
  *JAinv_ = JAinvt_->transpose();
}

void
PrTask::UpdateLambdaMatrix(){
	(*LambdaInv_) = (*J_)*(*JAinvt_);

	// we need more stuff here
	// e.g. SVD
	LambdaInv_->inverse(*Lambda_);

	Lambda_->display("Lambda");
}

void
PrTask::UpdateNtMatrix(){
	// This has to be modified
	(*Nt_) = *Identity_ - (*Jt_)*(*Lambda_)*(*JAinv_);
}

void
PrTask::UpdateDesiredAccs(){
	// this will be implemeted in the inherited class
}

PrVector
PrTask::computedTorque(){

	this->UpdateCoordinate();
	UpdateJacobianMatrix();
	//UpdateVelocity(*(robot_->jointState()->dq());
	UpdateJAinv();
	UpdateLambdaMatrix();

	PrVector tau(noj_);
	return tau;
}
