/***************************************************************
 * PrTreeRobotReal.cpp
 *
 * This class provides the TreeRobotReal robot impelementation.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 11/17/98: K.C. Chang: modified to real robot only.
 * 09/02/98: K.C. Chang: created.
 *
 * 04/98: Bob modified to work with ServoToGo interface on XR4000
 */

#include "PrTreeRobotReal.h"

#include <string.h>
#include <stdio.h>

#include "robot/PrRobot.h"
#include "robot/PrJointState.h"
#include "matrix/PrVector.h"

#include "ServoToGo.h"

PrTreeRobotReal::PrTreeRobotReal(int noj,char *paramFilename,
				 int servoRate,int robotType)
  : servoRate_(servoRate)
{
  mode_ = -1;
  emode_ = -1;
  flag_ = PR_TREEROBOTREAL_ERROR;
  int armType = PR_EMPTY_ID;
//   if (robotType == PR_TREEROBOTREAL_SAMM0)
//     armType = PR_SAMMARM0_ID;
//   else if (robotType == PR_TREEROBOTREAL_SAMM1)
//     armType = PR_SAMMARM1_ID;
  armType = PR_XRARM0_ID;

  if (robotType != PR_TREEROBOTREAL_NONE)
    {

      hwInterface_ = new IPBoardStg(servoRate,
				    armType,
				    PR_EMPTY_ID,   //PR_GRIPPER_ID
				    PR_EMPTY_ID,   //PR_FS_ID
				    PR_XRBASE_ID,
				    XR_ARM_IRQ,
				    XR_ARM_ADDRESS,
				    XR_BASE_IRQ,
				    XR_BASE_ADDRESS);

//       hwInterface_ = new PrIPBoardTech80(servoRate_,armType,
// 					 PR_GRIPPER_ID,
// 					 PR_FS_ID,
// 					 PR_BASE_ID);

      if (hwInterface_->flag() != PR_IPBOARDINTF_OK)
	{
	  delete hwInterface_;
	}
      else
	{
	  robot_ = new PrRobot(noj,paramFilename,
			       PR_ROBOT_INV_DYNAMICS, PR_ROBOT_FILETYPE_GLOBAL);

	  q_ = new PrVector(noj);
	  dq_ = new PrVector(noj);
	  tau_ = new PrVector(noj);

	  power(PR_TREEROBOTREAL_POWER_ON);
	  //	  jointPos();
	  //	  gripperPos();
	  flag_ = PR_TREEROBOTREAL_OK;
	}
    }
}

PrTreeRobotReal::~PrTreeRobotReal()
{
  if (flag_ == PR_TREEROBOTREAL_OK)
    {
      power(PR_TREEROBOTREAL_POWER_OFF);
      delete q_;
      delete dq_;
      delete tau_;
      delete hwInterface_;
      delete robot_;
    }
}

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

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

void PrTreeRobotReal::updatePositionVelocity()
{
  hwInterface_->jointPositionVelocityBase(robot_->jointState()->q(),
					  robot_->jointState()->dq());

/* Test: park73
printf(" %10.3f %10.3f %10.3f\n",
robot_->jointState()->q()[0],
robot_->jointState()->q()[1],
robot_->jointState()->q()[2]);
*/

  hwInterface_->jointPositionArm(robot_->jointState()->q()+3);
  hwInterface_->jointVelocityArm(robot_->jointState()->q()+3,
				 robot_->jointState()->dq()+3);
  robot_->updateTransform();
}

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


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


PrVector &PrTreeRobotReal::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 PrTreeRobotReal::torque(const PrVector &tau)
{
  for(int i=0;i<robot_->noj();i++)
    robot_->jointForce()[i] = tau.elementAt(i);

  hwInterface_->jointForceBase(robot_->jointForce());
  hwInterface_->jointForceArm(robot_->jointForce()+3);
}


void PrTreeRobotReal::torque2(const PrVector &tau, const PrVector &tauExt)
{
static Float tauE[3];

  for(int i=0;i<robot_->noj();i++)
     robot_->jointForce()[i] = tau.elementAt(i);

  tauE[0] = tauExt[0];
  tauE[1] = tauExt[1];
  tauE[2] = tauExt[2];

  hwInterface_->jointForceBase(robot_->jointForce(), tauE);
  hwInterface_->jointForceArm(robot_->jointForce()+3);
}


Float PrTreeRobotReal::gripperPos()
{
  hwInterface_->gripperPosition(&robot_->jointState()->q()[robot_->noj()]);
  return robot_->jointState()->q()[robot_->noj()];
}


Float PrTreeRobotReal::gripperVel()
{
  hwInterface_->gripperVelocity(&robot_->jointState()->q()[robot_->noj()],
				&robot_->jointState()->dq()[robot_->noj()]);
  return robot_->jointState()->dq()[robot_->noj()];
}


void PrTreeRobotReal::gripperTorque(Float t)
{
  robot_->jointState()->ddq()[robot_->noj()] = t;
  
  hwInterface_->gripperForce(&t);
}

PrVector6 &PrTreeRobotReal::fsValue()
{
  hwInterface_->forceSensor(fs_);
  for(int i=0;i<6;i++)
    f_.elementAt(i) = fs_[i];
  return f_;
}

void PrTreeRobotReal::fsZero()
{
  hwInterface_->forceSensorZero();
}

void PrTreeRobotReal::power(int p)
{
  if (p == PR_TREEROBOTREAL_POWER_ON)
    {
      hwInterface_->powerOn();
    }
  else
    {
      hwInterface_->powerOff();
    }
}

