/***************************************************************
 * PrIPBoardTRC.cpp
 *
 * This is an implementation of the hardware TRC IP board class.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 02/25/03: Weyn Ong  : Added accel sensor.
 * 02/24/03: Weyn Ong  : Added JR3 force sensor.
 * 03/31/98: K.C. Chang: added handOpen and handClose.
 * 12/18/97: K.C. Chang: modified to multi-dof base.
 * 12/18/97: K.C. Chang: got rid of globals.
 * 12/12/97: K.C. Chang: modified to TRCBoard.
 * 11/26/97: K.C. Chang: modified to no base and no gripper.
 * 10/26/97: K.C. Chang: created.
 */

#include "PrIPBoardTRC.h"
#include <assert.h>
#include "PrRobotDefn.h"

#include "PrTRCBoard.h"
#include "PrEncoderPC7166.h"
#include "PrForceSensorATI.h"
#include "PrForceSensorZebra.h"
#include "PrForceSensorJr3PCI.h"
#include "PrForceSensorJr3ISA.h"
#include "PrAccelSensorJr3PCI.h"

// constructor sets flag
// flag() should be called before using
PrIPBoardTRC::PrIPBoardTRC(int servoRate,int armID,int gripperID,
			     int fsID, int accelID, int baseID) 
: tick_(1.0/servoRate),armID_(armID),gripperID_(gripperID),
  fsID_(fsID),accelID_(accelID), baseID_(baseID)
{
    if (armID_ == PR_EMPTY_ID)
      {
	  printf("Error: invalid armID = %d\n",armID);
	  flag_ = PR_IPBOARDINTF_ERROR;
      }
    else
      {
	  flag_ = Initialize();
	  
	  for(int i=0;i<PR_ROBOT_NOJ+PR_GRIPPER_NOJ;i++)
	    dqOld_[i] = 0.0;
	  if (flag_ != PR_IPBOARDINTF_ERROR)
	    {
		jointPosition(qOld_);
		if (gripperID_ != PR_EMPTY_ID)
		  gripperPosition(qOld_+PR_GRIPPER_JOINT_START);
	    }
      }
}

PrIPBoardTRC::~PrIPBoardTRC()
{
    if (armID_ != PR_EMPTY_ID)
      {
	  delete boardTRC_;

	  if (gripperID_ != PR_EMPTY_ID || baseID_ != PR_EMPTY_ID)
	    delete encoder2_;

	  if (fsID_ != PR_EMPTY_ID)
	    delete fs_;

          if ( accelID_ != PR_EMPTY_ID )
            delete accel_;
      }
}

// hardware reset
int PrIPBoardTRC::zero()
{
    assert(armID_ != PR_EMPTY_ID);

    if (gripperID_ != PR_EMPTY_ID || baseID_ != PR_EMPTY_ID)
      {
	  if (boardTRC_->calibrationCheck() != PR_TRCBOARD_OK)
	    if (encoder2_->zero() != PR_DRIVER_OK)
	      return PR_IPBOARDINTF_ERROR;
      }

    if (boardTRC_->zero() != PR_TRCBOARD_OK)
      return PR_IPBOARDINTF_ERROR;

    if (fsID_ != PR_EMPTY_ID)
      {
	  if (fs_->zero() != PR_DRIVER_OK)
	    return PR_IPBOARDINTF_ERROR;
      }

    if ( accelID_ != PR_EMPTY_ID )
        if ( accel_->zero() != PR_DRIVER_OK )
            return PR_IPBOARDINTF_ERROR;

    return PR_IPBOARDINTF_OK;
}

// set torques(force) for base and arm
int PrIPBoardTRC::jointForce(Float tau[PR_ROBOT_NOJ])
{
    if (baseID_ != PR_EMPTY_ID)
      if (jointForceBase(tau+PR_BASE_JOINT_START) != PR_IPBOARDINTF_OK)
	return PR_IPBOARDINTF_ERROR;

    if (armID_ != PR_EMPTY_ID)
      if (jointForceArm(tau+PR_ARM_JOINT_START) != PR_IPBOARDINTF_OK)
	return PR_IPBOARDINTF_ERROR;

    return PR_IPBOARDINTF_OK;
}

// set torques for arm
int PrIPBoardTRC::jointForceArm(Float *tau)
{
    if (armID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    tau[3] -= (PR_ARM_COUPLING34*tau[4]
	       -(PR_ARM_COUPLING34*PR_ARM_COUPLING45-PR_ARM_COUPLING35)*tau[5]);
    tau[4] -= (PR_ARM_COUPLING45*tau[5]);

    for(int i=0;i<PR_ARM_NOJ;i++)
      {
	  Float volt = tau[i]*motorScale_[i+PR_ARM_JOINT_START];
	  if (boardTRC_->valueDAC(PR_ARM_DAC_CHANNEL_START+i,volt) 
	      != PR_TRCBOARD_OK)
	    return PR_IPBOARDINTF_ERROR;
      }

    return PR_IPBOARDINTF_OK;
}

// set force for base
int PrIPBoardTRC::jointForceBase(Float *tau)
{
    if (baseID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    for(int i=0;i<PR_BASE_NOJ;i++)
      {
	  Float volt = tau[i]*motorScale_[i+PR_BASE_JOINT_START];
	  if (boardTRC_->valueDAC(PR_BASE_DAC_CHANNEL_START+i,volt) 
	      != PR_TRCBOARD_OK)
	    return PR_IPBOARDINTF_ERROR;
      }
    return PR_IPBOARDINTF_OK;
}

// set force for gripper
int PrIPBoardTRC::gripperForce(Float *tau)
{
    if (gripperID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    for(int i=0;i<PR_GRIPPER_NOJ;i++)
      {
	  Float volt = tau[i]*motorScale_[i+PR_GRIPPER_JOINT_START];
	  if (boardTRC_->valueDAC(PR_GRIPPER_DAC_CHANNEL_START+i,volt) 
	      != PR_TRCBOARD_OK)
	    return PR_IPBOARDINTF_ERROR;
      }
    return PR_IPBOARDINTF_OK;
}

// read angles(distance) for base and arm
int PrIPBoardTRC::jointPosition(Float q[PR_ROBOT_NOJ])
{
    if (baseID_ != PR_EMPTY_ID)
      if (jointPositionBase(q+PR_BASE_JOINT_START) != PR_IPBOARDINTF_OK)
	return PR_IPBOARDINTF_ERROR;

    if (armID_ != PR_EMPTY_ID)
      if (jointPositionArm(q+PR_ARM_JOINT_START) != PR_IPBOARDINTF_OK)
	return PR_IPBOARDINTF_ERROR;

    return PR_IPBOARDINTF_OK;
}

// read angles for arm
int PrIPBoardTRC::jointPositionArm(Float *q)
{
    if (armID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    if (boardTRC_->valuesENC(q) != PR_TRCBOARD_OK)
      return PR_IPBOARDINTF_ERROR;

    for(int i=0;i<PR_ARM_NOJ;i++)
      q[i] /= gearRatio_[i+PR_ARM_JOINT_START];

    q[4] -= (q[3]*PR_ARM_COUPLING34);
    q[5] -= (q[3]*PR_ARM_COUPLING35 + q[4]*PR_ARM_COUPLING45);

    for(int j=0;j<PR_ARM_NOJ;j++)
      q[j] += jointPositionAtNest_[j+PR_ARM_JOINT_START];

    return PR_IPBOARDINTF_OK;
}

// read distance for base
int PrIPBoardTRC::jointPositionBase(Float *q)
{
    if (baseID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;
    
    for(int i=0;i<PR_BASE_NOJ;i++)
      {
	  if (encoder2_->value(PR_BASE_ENCODER_CHANNEL_START+i,q[i]) 
	      != PR_DRIVER_OK)
	    return PR_IPBOARDINTF_ERROR;
	  //    q[i] /= gearRatio_[i+PR_BASE_JOINT_START]; // not needed
      }
    for(int j=0;j<PR_BASE_NOJ;j++)
      q[j] += jointPositionAtNest_[j+PR_BASE_JOINT_START];

    return PR_IPBOARDINTF_OK;
}

// read distance for gripper
int PrIPBoardTRC::gripperPosition(Float *q)
{
    if (gripperID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;
    
    for(int i=0;i<PR_GRIPPER_NOJ;i++)
      {
	  if (encoder2_->value(PR_GRIPPER_ENCODER_CHANNEL_START+i,q[i]) 
	      != PR_DRIVER_OK)
	    return PR_IPBOARDINTF_ERROR;
	  //    q[i] /= gearRatio_[i+PR_GRIPPER_JOINT_START]; // not needed
      }
    for(int j=0;j<PR_GRIPPER_NOJ;j++)
      q[j] += jointPositionAtNest_[j+PR_GRIPPER_JOINT_START];

    return PR_IPBOARDINTF_OK;
}

int PrIPBoardTRC::jointVelocity(Float q[PR_ROBOT_NOJ],Float dq[PR_ROBOT_NOJ])
{
    if (baseID_ != PR_EMPTY_ID)
      if (jointVelocityBase(q+PR_BASE_JOINT_START,dq+PR_BASE_JOINT_START) 
	  != PR_IPBOARDINTF_OK)
	return PR_IPBOARDINTF_ERROR;

    if (armID_ != PR_EMPTY_ID)
      if (jointVelocityArm(q+PR_ARM_JOINT_START,dq+PR_ARM_JOINT_START) 
	  != PR_IPBOARDINTF_OK)
	return PR_IPBOARDINTF_ERROR;

  return PR_IPBOARDINTF_OK;
}

int PrIPBoardTRC::jointVelocityArm(Float *q,Float *dq)
{
    if (armID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    return VelocityFilter(PR_ARM_NOJ,qOld_+PR_ARM_JOINT_START,q,
			  dqOld_+PR_ARM_JOINT_START,dq);
}

int PrIPBoardTRC::jointVelocityBase(Float *q,Float *dq)
{
    if (baseID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    return VelocityFilter(PR_BASE_NOJ,qOld_+PR_BASE_JOINT_START,q,
			  dqOld_+PR_BASE_JOINT_START,dq);
}

int PrIPBoardTRC::gripperVelocity(Float *q,Float *dq)
{
    if (gripperID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    return VelocityFilter(PR_GRIPPER_NOJ,qOld_+PR_GRIPPER_JOINT_START,q,
			  dqOld_+PR_GRIPPER_JOINT_START,dq);
}

int PrIPBoardTRC::forceSensor(Float *f)
{
    if (fsID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    if (fs_->values(f) != PR_DRIVER_OK)
      return PR_IPBOARDINTF_ERROR;

    return PR_IPBOARDINTF_OK;
}

  // zero the force sensor
int PrIPBoardTRC::forceSensorZero()
{
    if (fsID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    if (fs_->zero() != PR_DRIVER_OK)
      return PR_IPBOARDINTF_ERROR;

    return PR_IPBOARDINTF_OK;
}

int PrIPBoardTRC::accelSensor(Float *f)
{
    if (accelID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    if (accel_->values(f) != PR_DRIVER_OK)
      return PR_IPBOARDINTF_ERROR;

    return PR_IPBOARDINTF_OK;
}

  // zero the accel sensor
int PrIPBoardTRC::accelSensorZero()
{
    if (accelID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    if (accel_->zero() != PR_DRIVER_OK)
      return PR_IPBOARDINTF_ERROR;

    return PR_IPBOARDINTF_OK;
}

int PrIPBoardTRC::powerOn()
{
    if (armID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    if (boardTRC_->powerOn() != PR_TRCBOARD_OK)
      return PR_IPBOARDINTF_ERROR;
    return PR_IPBOARDINTF_OK;
} 

int PrIPBoardTRC::powerOff()
{
    if (armID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    if (boardTRC_->powerOff() != PR_TRCBOARD_OK)
      return PR_IPBOARDINTF_ERROR;
    return PR_IPBOARDINTF_OK;
}

int PrIPBoardTRC::calibrationCheck()
{
    if (armID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    if (boardTRC_->calibrationCheck() != PR_TRCBOARD_OK)
      return PR_IPBOARDINTF_ERROR;
    return PR_IPBOARDINTF_OK;
} 

int PrIPBoardTRC::calibrationSet()
{
    if (armID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    if (boardTRC_->calibrationCheck() != PR_TRCBOARD_OK)
      return PR_IPBOARDINTF_ERROR;
    return PR_IPBOARDINTF_OK;
} 

int PrIPBoardTRC::thermostat()
{
    if (armID_ == PR_EMPTY_ID)
      return PR_IPBOARDINTF_ERROR;

    if (boardTRC_->thermostat() != PR_TRCBOARD_OK)
      return PR_IPBOARDINTF_ERROR;
    return PR_IPBOARDINTF_OK;
}

int PrIPBoardTRC::handOpen()
{
  if (boardTRC_->handOpen() != PR_TRCBOARD_OK)
      return PR_IPBOARDINTF_ERROR;
    return PR_IPBOARDINTF_OK;
}

int PrIPBoardTRC::handClose()
{
  if (boardTRC_->handClose() != PR_TRCBOARD_OK)
      return PR_IPBOARDINTF_ERROR;
    return PR_IPBOARDINTF_OK;
}

// private methods

int PrIPBoardTRC::VelocityFilter(int noj,Float *qo,Float *q,
				  Float *dqo,Float *dq)
{
  // do a tustin approximation for vfw*s/(s+vfw)
  Float vfw = 2.5*40.0;
  Float dt = tick_;
  
  for(int i=0;i<noj;i++)
    {
      dq[i] = (2.0*vfw/(vfw*dt + 2.0))*(q[i]-qo[i])
	+ ((2.0 - vfw*dt)/(2.0 + vfw*dt))*dqo[i];

      qo[i] = q[i];
      dqo[i] = dq[i];
    }
  return PR_IPBOARDINTF_OK;
}

// initialize all hardware
int PrIPBoardTRC::Initialize()
{
    assert(armID_ != PR_EMPTY_ID);

    fs_ = NULL;
    accel_ = NULL;
    encoder2_ = NULL;

    InitializeArm();
    boardTRC_ = new PrTRCBoard;
    if (boardTRC_->flag() != PR_TRCBOARD_OK) {
      armID_ = PR_EMPTY_ID;
      return PR_IPBOARDINTF_ERROR;
    }

    if (gripperID_ != PR_EMPTY_ID || baseID_ != PR_EMPTY_ID)
      {
	  encoder2_ = new PrEncoderPC7166;
	  if (encoder2_->flag() != PR_DRIVER_OK) {
            gripperID_ = PR_EMPTY_ID;
            baseID_ = PR_EMPTY_ID;
	    return PR_IPBOARDINTF_ERROR;
          }
	  if (gripperID_ != PR_EMPTY_ID)
	    InitializeGripper();
	  if (baseID_ != PR_EMPTY_ID)
	    InitializeBase();
      }

    if (fsID_ != PR_EMPTY_ID)
      {
        switch ( fsID_ ) {
            case PR_ATI_FS_ID :
                fs_ = new PrForceSensorATI();
                break;
            case PR_JR3_ISA_FS_ID :
                fs_ = new PrForceSensorJr3ISA();
                break;
            case PR_JR3_PCI_FS_ID :
                fs_ = new PrForceSensorJr3PCI();
                break;
            default :
                fs_ = new PrForceSensorZebra( fsID_ );
                break;
        }   

	if (fs_->flag() != PR_DRIVER_OK) {
          fsID_ = PR_EMPTY_ID;
	  return PR_IPBOARDINTF_ERROR;
        }
      }

    if ( accelID_ != PR_EMPTY_ID ) {
        assert( accelID_ == PR_JR3_PCI_ACCEL_ID );
        accel_ = new PrAccelSensorJr3PCI();
        if ( accel_->flag() != PR_DRIVER_OK ) {
            accelID_ = PR_EMPTY_ID;
            return PR_IPBOARDINTF_ERROR;
        }
    }

    return zero();
}

void PrIPBoardTRC::InitializeArm()
{
    const Float motorScale[] = PR_ARM_MOTORSCALE;
    const Float gearRatio[] = PR_ARM_GEARRATIO;
    const Float jointPositionAtNest[] = PR_ARM_POSITION_AT_NEST;

    for(int i=0;i<PR_ARM_NOJ;i++)
      {
	  motorScale_[i+PR_ARM_JOINT_START] = motorScale[i];
	  gearRatio_[i+PR_ARM_JOINT_START] = gearRatio[i];
	  jointPositionAtNest_[i+PR_ARM_JOINT_START] = jointPositionAtNest[i];
      }

}

void PrIPBoardTRC::InitializeBase()
{
    const Float motorScale[] = PR_BASE_MOTORSCALE;
    const Float gearRatio[] = PR_BASE_GEARRATIO;
    const Float jointPositionAtNest[] = PR_BASE_POSITION_AT_NEST;
    
    for(int i=0;i<PR_BASE_NOJ;i++)
      { 
	  motorScale_[i+PR_BASE_JOINT_START] = motorScale[i];
	  gearRatio_[i+PR_BASE_JOINT_START] = gearRatio[i];
	  jointPositionAtNest_[i+PR_BASE_JOINT_START] = jointPositionAtNest[i];
      }
}

void PrIPBoardTRC::InitializeGripper()
{
    const Float motorScale[] = PR_GRIPPER_MOTORSCALE;
    const Float gearRatio[] = PR_GRIPPER_GEARRATIO;
    const Float jointPositionAtNest[] = PR_GRIPPER_POSITION_AT_NEST;
    
    for(int i=0;i<PR_GRIPPER_NOJ;i++)
      { 
	  motorScale_[i+PR_GRIPPER_JOINT_START] = motorScale[i];
	  gearRatio_[i+PR_GRIPPER_JOINT_START] = gearRatio[i];
	  jointPositionAtNest_[i+PR_GRIPPER_JOINT_START]
	    = jointPositionAtNest[i];
      }
}

