/***************************************************************
 * PrPuma560.cpp
 *
 * This class provides the PUMA560 robot impelementation.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 09/02/98: K.C. Chang: created.
 */

#include "PrPuma560.h"

#include "robot/PrRobot.h"
#include "robot/PrJointState.h"
#include "robot/PrJointIntegrator.h"
#include "driver/PrIPBoardSTG.h"
#include "driver/PrIPBoardTRC.h"

PrPuma560::PrPuma560(char *paramFile, int choice, int servoRate)
  : choice_(choice), servoRate_(servoRate), noj_(6)
{
  robot_ = NULL;
  hwInterface_ = NULL;
  integrator_ = NULL;

  if (choice_ == PR_PUMA560_ROBOT_REAL)
    {
      robot_ = new PrRobot(noj_,paramFile,PR_ROBOT_INV_DYNAMICS);
      hwInterface_ = new BOARD_TO_USE(servoRate_,PR_ARM_ID,
				      PR_GRIPPER_ID,
				      PR_FS_ID, 
                                      PR_ACCEL_ID,
				      PR_BASE_ID);
      assert(hwInterface_ != NULL);
      power(PR_PUMA560_POWER_ON);
    }
  else 
    {
      robot_ = new PrRobot(noj_,paramFile,PR_ROBOT_FWD_DYNAMICS);
      integrator_ = new PrJointIntegrator(noj_);
      assert(integrator_ != NULL);
    }
  assert(robot_ != NULL);
}

PrPuma560::~PrPuma560()
{
  power(PR_PUMA560_POWER_OFF);
  if (choice_ == PR_PUMA560_ROBOT_REAL)
    {
      delete hwInterface_;
    }
  else
    {
      delete integrator_;
    }
  delete robot_;
}

void PrPuma560::zero()
{
  if (choice_ == PR_PUMA560_ROBOT_SIM)
    {
      for(int i=0;i<noj_;i++)
	{
	  robot_->jointState()->q()[i]=0.0;
	  robot_->jointState()->dq()[i]=0.0;
	}
    }
}

void PrPuma560::jointPos(const PrVector6 &q)
{
  if (choice_ == PR_PUMA560_ROBOT_SIM)
    {
      for(int i=0;i<noj_;i++)
	{
	  robot_->jointState()->q()[i]=q.elementAt(i);
	}
    }
}

void PrPuma560::jointVel(const PrVector6 &dq)
{
  if (choice_ == PR_PUMA560_ROBOT_SIM)
    {
      for(int i=0;i<noj_;i++)
	{
	  robot_->jointState()->dq()[i]=dq.elementAt(i);
	}
    }
}

void PrPuma560::simulate(Float dt)
{
  if (choice_ == PR_PUMA560_ROBOT_SIM)
    {
//      integrator_->integrateRK4(robot_->jointState(),dt,UpdateState);
	integrator_->integrateEuler(robot_,dt);
    }
}

PrVector6 &PrPuma560::jointPos()
{
  if (choice_ == PR_PUMA560_ROBOT_REAL)
    {
      hwInterface_->jointPosition(robot_->jointState()->q());
      robot_->updateTransform();
    }
  for (int i=0;i<noj_;i++)
    q_.elementAt(i) = robot_->jointState()->q()[i];
  return q_;
}

PrVector6 &PrPuma560::jointVel()
{
  if (choice_ == PR_PUMA560_ROBOT_REAL)
    {
      hwInterface_->jointVelocity(robot_->jointState()->q(),
				 robot_->jointState()->dq());
    }
  for (int i=0;i<noj_;i++)
    dq_.elementAt(i) = robot_->jointState()->dq()[i];
  return dq_;
}

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

  robot_->invDynamics();

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

void PrPuma560::torque(const PrVector6 &tau)
{
  for(int i=0;i<noj_;i++)
    robot_->jointForce()[i] = tau.elementAt(i);

  if (choice_ == PR_PUMA560_ROBOT_REAL)
    {
	hwInterface_->jointForce(robot_->jointForce());
    }
}

void PrPuma560::gripper(int g)
{
  if (choice_ == PR_PUMA560_ROBOT_REAL)
    {
      if (g == PR_PUMA560_GRIPPER_OPEN)
	{
	  hwInterface_->handOpen();
	}
      else
	{
	  hwInterface_->handClose();
	}
    }
}

void PrPuma560::power(int p)
{
  if (choice_ == PR_PUMA560_ROBOT_REAL)
    {
      if (p == PR_PUMA560_POWER_ON)
	{
	  hwInterface_->powerOn();
	}
      else
	{
	  hwInterface_->powerOff();
	}
    }
}


