/***************************************************************
 * PrTreeRobot.cpp
 *
 * This class provides the tree-like robot impelementation.
 *
 ****************************************************************/

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

#include "PrTreeRobot.h"

#include "robot/PrRobot.h"
#include "robot/PrJointState.h"
#include "robot/PrJointIntegrator.h"
#include "network/PrDataInput.h"
#include "matrix/PrTransform.h"
#include "matrix/PrVector.h"

PrTreeRobot::PrTreeRobot(int noj,char *paramFilename)
{
  mode_ = -1;
  emode_ = -1;

  printf("noj: %d\n", noj);
  q_ = new PrVector(noj);
  dq_ = new PrVector(noj);
  tau_ = new PrVector(noj);

				  
  integrator_ = new PrJointIntegrator(noj+1); // gripper

  robot_ = new PrRobot(noj,paramFilename,
		       PR_ROBOT_FWD_DYNAMICS,PR_ROBOT_FILETYPE_GLOBAL);
  assert(robot_ != NULL);
}

PrTreeRobot::~PrTreeRobot()
{
  delete integrator_;
  delete q_;
  delete dq_;
  delete tau_;
  delete robot_;
}

int PrTreeRobot::noj() const 
{ 
  return robot_->noj(); 
}

PrJointState *PrTreeRobot::jointState()
{
  return robot_->jointState();
}

void PrTreeRobot::zero()
{
  for(int i=0;i<robot_->noj();i++)
    {
      robot_->jointState()->q()[i]=0.0;
      robot_->jointState()->dq()[i]=0.0;
    }
  robot_->updateTransform();
}

void PrTreeRobot::jointPos(const PrVector &q)
{
  for(int i=0;i<robot_->noj();i++)
    {
      robot_->jointState()->q()[i]=q.elementAt(i);
    }
  robot_->updateTransform();
}

void PrTreeRobot::jointVel(const PrVector &dq)
{
  for(int i=0;i<robot_->noj();i++)
    {
      robot_->jointState()->dq()[i]=dq.elementAt(i);
    }
}

void PrTreeRobot::simulate(Float dt)
{
//  integrator_->integrateRK4(robot_,dt);
	integrator_->integrateEuler(robot_,dt);
}

PrVector &PrTreeRobot::jointPos()
{
  for (int i=0;i<robot_->noj();i++)
    (*q_).elementAt(i) = robot_->jointState()->q()[i];
  return *q_;
}

PrVector &PrTreeRobot::jointVel()
{
  for (int i=0;i<robot_->noj();i++)
    (*dq_).elementAt(i) = robot_->jointState()->dq()[i];
  return *dq_;
}

PrVector &PrTreeRobot::computedTorque(const PrVector &tau_prime)
{
  for(int i=0;i<robot_->noj();i++)
    robot_->jointAcceleration()[i] = tau_prime.elementAt(i);

  robot_->invDynamics();

  for(int j=0;j<robot_->noj();j++)
    (*tau_).elementAt(j) = robot_->jointForce()[j];
  return *tau_;
}

void PrTreeRobot::torque(const PrVector &tau)
{
  for(int i=0;i<robot_->noj();i++)
    robot_->jointForce()[i] = tau.elementAt(i);
}

void PrTreeRobot::display()
{
  robot_->display();
}

Float PrTreeRobot::gripperPos()
{
  return robot_->jointState()->q()[robot_->noj()];
}

Float PrTreeRobot::gripperVel()
{
  return robot_->jointState()->dq()[robot_->noj()];
}

void PrTreeRobot::gripperTorque(Float t)
{
  robot_->jointState()->ddq()[robot_->noj()] = t;
}

PrVector6 &PrTreeRobot::fsValue()
{
  return f_;
}

void PrTreeRobot::fsZero()
{
  f_.zero();
}



