/***************************************************************
 * PrForceSensorZebra.cpp
 *
 * This is an implementation of the Zebra Force Sensor class.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 04/20/98: K.C. Chang: created.
 * 02/22/03: Weyn Ong  : Version for QNX 6.  I/O permissions must be enabled
 *                       for port access.  Check PrZebraDefn.h to verify that
 *                       PR_ZEBRA_ADDR_SPACE_SIZE is the correct value.
 */

#include "PrZebraDefn.h"
#include "PrForceSensorZebra.h"
#include "PrUtility.h"

#include <stdio.h>
#include <sys/mman.h>
#include <sys/neutrino.h>

static Float maxForce_G[3]={180.0,180.0,180.0};
static Float maxTorque_G[3]={2.5,2.5,4.0};

// kg, kg
/*                   
    {  -0.1813,   -0.1118,    0.3947,    2.6753,    0.0716,   -2.8484},
    {  -0.0560,   -3.2340,    0.1361,    1.6638,    0.1193,    1.5734},
    {  -1.7008,    0.0995,   -2.1834,    0.1525,   -2.1246,    0.0347},
    {   0.0073,   -0.6825,   -0.7331,    0.3899,    0.7419,    0.3251},
    {   0.7581,   -0.0367,   -0.4846,   -0.4657,   -0.4318,    0.5918},
    {  -0.0199,   -1.9859,   -0.1379,   -1.8185,   -0.1091,   -1.8088}
*/

// N, Nm
static Float calibrationMatrix_G[3][6][6] =
{
  {
    { -1.7784,   -1.0971,    3.8723,   26.2445,    0.7026,  -27.9429},
    { -0.5493,  -31.7260,    1.3351,   16.3219,    1.1705,   15.4353},
    {-16.6844,    0.9761,  -21.4195,    1.4956,  -20.8422,    0.3404},
    {  0.0047,   -0.4421,   -0.4749,    0.2526,    0.4806,    0.2106},
    {  0.4534,   -0.0220,   -0.2898,   -0.2785,   -0.2582,    0.3539},
    { -0.0041,   -0.4082,   -0.0283,   -0.3738,   -0.0224,   -0.3718}
  },
  {
    { -1.7784,   -1.0971,    3.8723,   26.2445,    0.7026,  -27.9429},
    { -0.5493,  -31.7260,    1.3351,   16.3219,    1.1705,   15.4353},
    {-16.6844,    0.9761,  -21.4195,    1.4956,  -20.8422,    0.3404},
    {  0.0047,   -0.4421,   -0.4749,    0.2526,    0.4806,    0.2106},
    {  0.4534,   -0.0220,   -0.2898,   -0.2785,   -0.2582,    0.3539},
    { -0.0041,   -0.4082,   -0.0283,   -0.3738,   -0.0224,   -0.3718}
  },
  {
    { -1.7784,   -1.0971,    3.8723,   26.2445,    0.7026,  -27.9429},
    { -0.5493,  -31.7260,    1.3351,   16.3219,    1.1705,   15.4353},
    {-16.6844,    0.9761,  -21.4195,    1.4956,  -20.8422,    0.3404},
    {  0.0047,   -0.4421,   -0.4749,    0.2526,    0.4806,    0.2106},
    {  0.4534,   -0.0220,   -0.2898,   -0.2785,   -0.2582,    0.3539},
    { -0.0041,   -0.4082,   -0.0283,   -0.3738,   -0.0224,   -0.3718}
  }
};

PrForceSensorZebra::PrForceSensorZebra(int sensorNum)
: PrForceSensorIntf(PR_ZEBRA_CHANNEL_COUNT,PR_ZEBRA_BIT_COUNT)
{
  assert( sensorNum==PR_ZEBRA_SENSOR_0 
	 || sensorNum==PR_ZEBRA_SENSOR_1 
	 || sensorNum==PR_ZEBRA_SENSOR_2 );
 
  sensorNum_ = sensorNum;

  utility_ = new PrUtility;

  Initialize();

  // Enable permissions for I/O access
  ThreadCtl( _NTO_TCTL_IO, 0 );
  mmap_device_io( PR_ZEBRA_ADDR_SPACE_SIZE, PR_ZEBRA_BASE );

  flag(zero());
}

PrForceSensorZebra::~PrForceSensorZebra()
{
  delete utility_;
}

Float PrForceSensorZebra::forceMax()
{
  return maxForce_G[sensorNum_];
}

Float PrForceSensorZebra::torqueMax()
{
  return maxTorque_G[sensorNum_];
}

int PrForceSensorZebra::values(Float *data)
{
  Float volt[6];
  Float val[6];

  register int k;
  for(k=0;k<6;k++)
    value(k,volt[k]);

  for (int i=0; i<6; i++)
    {
      val[i]=0.0;
      for(int j=0; j<6; j++)
	{
	  val[i] += calibrationMatrix_G[sensorNum_][i][j]*(volt[j]);
	  
	}
//		data[i]=val[i];
    }
	
  for(k=0;k<6;k++)
    data[k] = val[k]-bias()[k];

  return ( PR_DRIVER_OK );
}

int PrForceSensorZebra::value(int channel,Float &volt)
{
  /* is the channel 0-5 */ 
  if ( ( channel < 0 ) || ( channel > 5 ) )
    return ( PR_DRIVER_ERROR );
  
  /* select AD channel between 0-5 */ 
  /* invert the address bits */
  channel = (~channel & PR_ZEBRA_ADDR_MASK) << ch_OFFSET_[sensorNum_];

  /* write channel address */  
  hwIO().output1B( outPORT_[sensorNum_], channel );           

  int i;

  /* delay 10 us for signal to settle */
  //  utility_->microBlock(10);
  for(i=0;i<1000;i++);

  /* start AD conversion */
  /* raise start conversion bit */	    
  hwIO().output1B( outPORT_[sensorNum_], channel | conv_BIT_[sensorNum_] );  

  /* delay 250 ns for pulse width */
  //  utility_->nanoBlock(250);
  for(i=0;i<10;i++);

  /* lower start conversion bit */
  hwIO().output1B( outPORT_[sensorNum_], channel);              
  
  /* wait 30us for end of conversion */
  //  utility_->microBlock(30);
  for(i=0;i<10000;i++);

  // impossible: too noisy
  /*
    unsigned int status_flag;     
    int count = 0;
    while (1)        
    {
    status_flag = (inp(PORTC_1) & EOC1);
	
    if ((status_flag == EOC1))
    {
    count++;
    }
    else if ((count++) > 1000) 
    {
    printf("ad error !");
    break;
    }	
	
    }
  */
  /* read MSB */    
  int hi_byte = hwIO().input1B(inPORT_hi_[sensorNum_])&hi_MASK_[sensorNum_];   
  /* read LSB */
  int lo_byte = hwIO().input1B(inPORT_lo_[sensorNum_])&lo_MASK_[sensorNum_];   
  
  /* put 10 bits together and shift to measure values from center of range */
  int adValue = (hi_byte << hi_SCALE_[sensorNum_] 
		 | lo_byte >> lo_SCALE_[sensorNum_]) 
    - PR_ZEBRA_OFFSET_ZERO; 
  /* convert AD value to voltage */
  volt = PR_ZEBRA_VOLTS_PER_BIT*adValue;
    
  return ( PR_DRIVER_OK );
}

void PrForceSensorZebra::display()
{
  printf("calibration matrix:\n");
  // 6 x 6 
  for(int i=0; i<6; i++) 
    {
      for(int j=0; j<6; j++)
	printf("%f ",calibrationMatrix_G[sensorNum_][i][j]); 
      printf("\n");
    }
  printf("bias: ");
  for(int i=0; i<6; i++) 
    printf("%f ",bias()[i]);
  printf("\n");
  printf("max force         = %f\n",maxForce_G[sensorNum_]);
  printf("max torque        = %f\n",maxTorque_G[sensorNum_]);
}


void PrForceSensorZebra::Initialize()
{
  InitPorts();
}

/**************************************************************************
 * FUNCTION:     InitPorts
 * PURPOSE:      set input/output ports
 * ARGUMENTS:    none            
 *             
 * DESCRIPTION:  PORTA_1 in       PORTA_2 in       
 *		 PORTB_1 in       PORTB_2 in 
 *		 PORTC_1 out/out  PORTC_2 in/out
 *               
 * RETURN:        
 * VALUES:       
 * SIDE EFFECT:  
 * CALLS:        
 * CALLED BY:    
 *************************************************************************/
void PrForceSensorZebra::InitPorts ()
{
  /* set input/output ports */
  hwIO().output1B( PR_ZEBRA_PORTD_1, PR_ZEBRA_MODE_1);
  hwIO().output1B( PR_ZEBRA_PORTD_2, PR_ZEBRA_MODE_2);
  /* clear output ports */
  hwIO().output1B( PR_ZEBRA_PORTC_1, PR_ZEBRA_ZERO);
  hwIO().output1B( PR_ZEBRA_PORTC_2, PR_ZEBRA_ZERO);
  
  
  inPORT_lo_[0]   = PR_ZEBRA_PORTA_1;
  inPORT_hi_[0]   = PR_ZEBRA_PORTB_1;
  outPORT_[0]     = PR_ZEBRA_PORTC_1;
  conv_BIT_[0]    = PR_ZEBRA_CONV1;
  ch_OFFSET_[0]   = PR_ZEBRA_CH_OFFSET_1;
  lo_MASK_[0]     = PR_ZEBRA_LO_MASK_1; 
  hi_MASK_[0]     = PR_ZEBRA_HI_MASK_1; 
  hi_SCALE_[0]    = PR_ZEBRA_HI_SCALE_1;
  lo_SCALE_[0]    = PR_ZEBRA_LO_SCALE_1;

  inPORT_lo_[1]   = PR_ZEBRA_PORTA_2;
  inPORT_hi_[1]   = PR_ZEBRA_PORTB_2;
  outPORT_[1]     = PR_ZEBRA_PORTC_2;
  conv_BIT_[1]    = PR_ZEBRA_CONV2;
  ch_OFFSET_[1]   = PR_ZEBRA_CH_OFFSET_2;
  lo_MASK_[1]     = PR_ZEBRA_LO_MASK_2; 
  hi_MASK_[1]     = PR_ZEBRA_HI_MASK_2; 
  hi_SCALE_[1]    = PR_ZEBRA_HI_SCALE_2;
  lo_SCALE_[1]    = PR_ZEBRA_LO_SCALE_2;
      
  inPORT_lo_[2]   = PR_ZEBRA_PORTB_1;
  inPORT_hi_[2]   = PR_ZEBRA_PORTB_2;
  outPORT_[2]     = PR_ZEBRA_PORTC_1;
  conv_BIT_[2]    = PR_ZEBRA_CONV3;
  ch_OFFSET_[2]   = PR_ZEBRA_CH_OFFSET_3;
  lo_MASK_[2]     = PR_ZEBRA_LO_MASK_3; 
  hi_MASK_[2]     = PR_ZEBRA_HI_MASK_3; 
  hi_SCALE_[2]    = PR_ZEBRA_HI_SCALE_3;
  lo_SCALE_[2]    = PR_ZEBRA_LO_SCALE_3;
}

