/******************************************************************
 * PrIPBoardSTG.cpp
 *
 * This hardware interface is designed to control a Puma Arm
 * thru a servoToGo model 2 Board.
 * Contains board/amp independant stuff
 *
 ******************************************************************/

#include <stdio.h>

#include "PrIPBoardSTG.h"
#include "PrDriverIntf.h"

#include "PrSTGBoard.h"
#include "PrForceSensorZebra.h"
#include "PrForceSensorATI.h"
#include "PrForceSensorJr3PCI.h"
#include "PrForceSensorJr3ISA.h"
#include "PrAccelSensorJr3PCI.h"
#include "PrRobotDefn.h"
#include "ServoToGo.h"
#include "matrix/PrVector.h"
#include "matrix/PrMatrix.h"
#include "Filter.h"

PrIPBoardSTG::PrIPBoardSTG(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 || baseID_ != PR_EMPTY_ID)
    {
      printf("Error: invalid baseID = %d && armID = %d\n",baseID,armID);
      flag_ = PR_IPBOARDINTF_ERROR;
    }
  else
    {
      flag_ = Initialize();

      if (flag_ != PR_IPBOARDINTF_ERROR)
	{
	  jointPositionArm(qOld_);
	  if (gripperID_ != PR_EMPTY_ID)
	    gripperPosition(qOld_+6);
	}
    }
}


PrIPBoardSTG::~PrIPBoardSTG() {
    if (armID_ != PR_EMPTY_ID || gripperID_ != PR_EMPTY_ID)
        delete boardSTG_;

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


    // error checking for constructor
int PrIPBoardSTG::flag() 
{ 
  return flag_; 
}

int PrIPBoardSTG::powerOn()
{
  return PR_DRIVER_OK;
}

int PrIPBoardSTG::powerOff()
{
  return PR_DRIVER_OK;
}

    // hardware reset
int PrIPBoardSTG::zero() 
{
    assert(baseID_ != PR_EMPTY_ID || armID_ != PR_EMPTY_ID);

    if (armID_ != PR_EMPTY_ID || gripperID_ != PR_EMPTY_ID) {
        filter_qdArm = new Filter;
        // Init & Zero Arm velocity filter
        PrVector q0(6);
        filter_qdArm->init_dLowPass( q0 , 1.0/tick_, 16.0 );

	boardSTG_->zero();
    }


    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;
}


int PrIPBoardSTG::thermostat() 
{ printf("PrIPBoardSTG::thermostat() NOT IMPLEMENTED\n");
  return PR_DRIVER_ERROR; 
}

int PrIPBoardSTG::handOpen()
{
  if (boardSTG_->handOpen() != PR_STGBOARD_OK)
      return PR_IPBOARDINTF_ERROR;
  return PR_IPBOARDINTF_OK; 
}

int PrIPBoardSTG::handClose()
{
  if (boardSTG_->handClose() != PR_STGBOARD_OK)
      return PR_IPBOARDINTF_ERROR;
  return PR_IPBOARDINTF_OK; 
}

int PrIPBoardSTG::calibrationCheck()
{ printf("PrIPBoardSTG::calibrationCheck() NOT IMPLEMENTED\n");
  return PR_DRIVER_ERROR; 
}

int PrIPBoardSTG::calibrationSet()
{ printf("PrIPBoardSTG::calibrationSet() NOT IMPLEMENTED\n");
  return PR_DRIVER_ERROR; 
}


    // set torques(force) for base and arm
int PrIPBoardSTG::jointForce(Float *tau) 
{
  if (armID_ == PR_EMPTY_ID || baseID_ != PR_EMPTY_ID)
    return PR_IPBOARDINTF_ERROR;
  else
    return jointForceArm(tau);
}

    // set torques for arm
int PrIPBoardSTG::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]);

  boardSTG_->jointForce(tau);

  return PR_IPBOARDINTF_OK;
}

    // set accel force for base
int PrIPBoardSTG::jointForceBase(Float *tau) 
{
  // DUMMY, NOT IMPLEMENTED YET
  Float temp = tau[0];
  printf("PrIPBoardSTG::jointForceBase() NOT IMPLEMENTED\n");
  return PR_DRIVER_ERROR;
}

    // set accel force and external force for base 
int PrIPBoardSTG::jointForceBase(Float *tau , Float *tauExt)
{
  // DUMMY, NOT IMPLEMENTED YET
  Float temp = tau[0];
  temp = tauExt[0];
  printf("PrIPBoardSTG::jointForceBase() NOT IMPLEMENTED\n");
  return PR_DRIVER_ERROR;
}

    // set force for gripper
int PrIPBoardSTG::gripperForce(Float *tau) 
{
// DUMMY, NOT IMPLEMENTED YET
  Float temp = tau[0];
  return PR_DRIVER_ERROR;
}


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

    if (armID_ != PR_EMPTY_ID)
      if (jointPositionArm(q) != PR_IPBOARDINTF_OK)
	return PR_IPBOARDINTF_ERROR;
      else return PR_IPBOARDINTF_ERROR;
    return PR_IPBOARDINTF_OK;
}

    // read angles for arm
int PrIPBoardSTG::jointPositionArm(Float *q){

  if (armID_ == PR_EMPTY_ID)
    return PR_IPBOARDINTF_ERROR;

  boardSTG_->readPos(q);

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


int PrIPBoardSTG::jointPositionBase(Float *q) 
{ 
// DUMMY, NOT IMPLEMENTED YET
  Float temp = q[0];
  return PR_DRIVER_ERROR; 
}


// read distance for gripper
int PrIPBoardSTG::gripperPosition(Float *q) 
{
// DUMMY, NOT IMPLEMENTED YET
  q[0] = 0.0;
  return PR_DRIVER_ERROR;
}

int PrIPBoardSTG::jointVelocity(Float *q,Float *dq)
{
  if (armID_ != PR_EMPTY_ID)
    if (jointVelocityArm(q,dq) 
	!= PR_IPBOARDINTF_OK)
      return PR_IPBOARDINTF_ERROR;
  
  return PR_IPBOARDINTF_OK;
}


int PrIPBoardSTG::jointVelocityArm(Float *q,Float *dq)
{
  static PrVector q_in(6),qd_out(6);
  int i;
  
  if (armID_ == PR_EMPTY_ID)
    return PR_IPBOARDINTF_ERROR;
  
  for( i=0; i<6 ; ++i)
    { 
      q_in[i]=q[i];
    }
  filter_qdArm->filt(q_in,qd_out);
  for( i=0; i<6 ; ++i)
    { 
      dq[i]=qd_out[i];
    }
  
  return PR_IPBOARDINTF_OK;
}


int PrIPBoardSTG::jointVelocityBase(Float *q,Float *dq)
{
//DUMMY, NOT IMPLEMENTED YET
  Float temp1 = q[0];
  Float temp2 = dq[0];
  return PR_DRIVER_ERROR;
}


int PrIPBoardSTG::gripperVelocity(Float *q,Float *dq) 
{
  q[0] = dq[0] = 0.0;
  return PR_DRIVER_ERROR;
}


// read force from the force sensor
int PrIPBoardSTG::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 PrIPBoardSTG::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 PrIPBoardSTG::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 PrIPBoardSTG::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 PrIPBoardSTG::jointPositionVelocityBase(Float *q,Float *dq) 
{
  // DUMMY, NOT IMPLEMENTED YET
  Float temp = q[0];
  temp = dq[0];
  return PR_IPBOARDINTF_ERROR;
}

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

    fs_ = NULL;
    accel_ = NULL;

    if (armID_ != PR_EMPTY_ID || gripperID_ != PR_EMPTY_ID) {
        boardSTG_ = new PrSTGBoard();
        if ( boardSTG_->flag() != PR_STGBOARD_OK ) {
            armID_ = PR_EMPTY_ID;
            return PR_IPBOARDINTF_ERROR;
        }
        filter_qdArm = new Filter;
        // Init & Zero Arm velocity filter
        PrVector q0(6);
        filter_qdArm->init_dLowPass( q0 , 1.0/tick_, 16.0 );
    }

    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 || fs_->zero() != 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 || 
	     accel_->zero() != PR_DRIVER_OK ) {
            accelID_ = PR_EMPTY_ID;
            return PR_IPBOARDINTF_ERROR;
        }
    }

    return PR_IPBOARDINTF_OK; 
}

ServoToGo* PrIPBoardSTG::GetBoard() 
{ if( boardSTG_ ) return boardSTG_->GetBoard(); 
  else return NULL;
}
