/*****************************************************************************
Copyright 1998 MBARI                                                      
******************************************************************************
Summary  : iView pan-tilt axis class member functions
Filename : Axis.C
Author   : Michael B. Matthews                                                
Project  : iView                                                        
Version  : 3.8                                                          
Created  : 11.2.98
Modified : 23.3.98
Notes    : Used to be part of device.C in iView version before V3.8
******************************************************************************/

#define __PROTOTYPE_5_0			// logMsg variable arg list

#include <vxWorks.h>            	      
#include <sysLib.h>   	         	      
#include <stdioLib.h>           	     
#include <stdlib.h>  	         	      
#include <math.h>              		
#include <semLib.h>            		         
#include <logLib.h>            		         
#include <wdLib.h>             		    
#include <systime.h>           		   
#include <strLib.h>			             
#include <stdlib.h>			
#include <fppLib.h>             	   
#include <errno.h>        	
#include <tickLib.h>        	

extern "C" 
{
#include <trig.h>      		           
#include <mbariTypes.h>  	            
#include <sensorDcon.h>		
#include <vmeIbc.h>		      
#include <sensorsDm.h>	                                    
#include <datamgr.h>   		    
#include <dm_errno.h>   	   
#include <usrTime.h>
#include <tiburon.h>			
#include <camera.h>			
#include <panTilt.h>			
#include <pantilt.h>	
#include <rovPriority.h>	
};

#include "Iview.h"
#include "Misc.h"		// misc functions
#include "String.h"		// homebrew string classes
#include "DataManager.h"	// data manager classes
#include "DmGroup.h"		// data manager classes
#include "uFilter.h"		// filter and model parameters		
#include "Buffer.h"		// homebrew buffer class
#include "Linear_System.h"	// linear system and Kalman filter classes
#include "Calibration.h"	// calibration table class
#include "Axis.h"		// device axis class			
#include "Device.h"		// device classes		
#include "ControlSource.h"	// control source classes
#include "SetPoint.h"				
#include "iview.h"				


class CalibrationTable;


/******************************************************************************
Function : Axis::Axis                                              
Purpose  : Axis constructor						      
Inputs   : cfg    - Axis configuration structure with necessary parameters 
         : dmName - base data manager name                  
Outputs  : 		                                                      
*******************************************************************************/

Axis::Axis(AxisConfig* cfg, String& dmName) : name(nameConversion(dmName))
{
  Int i,j;
  Flt32 p;

  index = 0;
  control = FALSE;
  standby = FALSE;
  stall = FALSE;
  stallDelayCount = 0;
  stallDetectionCount = 0;
  stallIntegrator = 0;
  integratorDump = FALSE;

  directWrite = FALSE;

  t = 0; u = 0;
  analogDegs = 0;
  analogDegsLast = 0;
  stepperDegs = 0;
  stepperDegsLast = 0;
  analogPosition = 0;
  position = 0;
  ccwLimitSwitch = FALSE;
  cwLimitSwitch = FALSE;
  posLimitState = FALSE;	
  negLimitState = FALSE;


  // load calibration table
  String string = IVIEW_DIRECTORY;
  string += name;
  string += "CalibrationTable";
  calibrationTable = new CalibrationTable(string);


  // load local parameters
  direction	= cfg->direction;
  offset      	= cfg->offset;
  panoramicOffsetDefault = cfg->panoramicOffset;
  panoramicOffset = 0;
  posLimit     	= cfg->posLimit;		// MBM write limit switch code
  negLimit    	= cfg->negLimit;
  maxVelocity 	= cfg->maxVelocity;
  minVelocity 	= cfg->minVelocity;
  pGain 	= cfg->pGain;
  dGain 	= cfg->dGain;
  iGain 	= cfg->iGain;
  fGain 	= cfg->fGain;
  velocityConversion = cfg->velocityConversion;
  bufferLength  = cfg->bufferLength;


  // create motor stall DM item as provider
  string = IVIEW_DM_PREFIX + dmName + IVIEW_MOTOR_STALL_DM; 
  Dmi.motorStall = new DmBooleanObject(string.str);
  Dmi.motorStall->startProvide(DM_ASYNC, ExclusiveProvider);
  Dmi.motorStall->write(&stall);

  // create position command DM item as provider
  string = PAN_TILT_DM_PREFIX + dmName + PT_POSITION_CMD_DM;
  Dmi.positionCmd = new DmFlt32Object(string.str);
  Dmi.positionCmd->startProvide(IVIEW_DM_PERIOD, ExclusiveProvider);

  // create velocity command DM item as provider
  string = PAN_TILT_DM_PREFIX + dmName + PT_VELOCITY_CMD_DM;
  Dmi.velocityCmd = new DmInt16Object(string.str);
  Dmi.velocityCmd->startProvide(IVIEW_DM_PERIOD, ExclusiveProvider);

  // create analog position request DM item as provider
  string = PAN_TILT_DM_PREFIX + dmName + "ANALOG_POSITION_REQUEST"; 
  Dmi.analogRequest = new DmEmptyObject(string.str);
  Dmi.analogRequest->startProvide(IVIEW_DM_PERIOD, ExclusiveProvider);

  // create stepper position request DM item as provider
  string = PAN_TILT_DM_PREFIX + dmName + "STEPPER_POSITION_REQUEST"; 
  Dmi.stepperRequest = new DmEmptyObject(string.str);
  Dmi.stepperRequest->startProvide(IVIEW_DM_PERIOD, ExclusiveProvider);

  // create stepper position request DM item as provider
  string = PAN_TILT_DM_PREFIX + dmName + "IVIEW_POSITION"; 
  Dmi.position = new DmFlt32Object(string.str);
  Dmi.position->startProvide(IVIEW_DM_PERIOD, ExclusiveProvider);

  // create control string DM item as provider
  string = PAN_TILT_DM_PREFIX + dmName + PT_CTRL_STRING_DM; 
  Dmi.ctlrString = new DmStringObject(string.str, 30);
  Dmi.ctlrString->startProvide(DM_ASYNC, ExclusiveProvider);


  // create comsumer group
  group = new DmGroup();

  // create positive limit DM item as consumer
  string = IVIEW_DM_PREFIX + dmName + IVIEW_POSITIVE_LIMIT_DM; 
  Dmi.positiveLimit = new DmFlt32Object(string.str);
  Dmi.positiveLimit->startConsume(DM_ASYNC);
  group->addItem(Dmi.positiveLimit);
  Dmi.positiveLimit->urgentOpen(DM_STATIC);
  p = RADS_TO_DEGS(posLimit);
  Dmi.positiveLimit->write(&p);
  Dmi.positiveLimit->urgentClose();

  // create positive limit DM item as consumer
  string = IVIEW_DM_PREFIX + dmName + IVIEW_NEGATIVE_LIMIT_DM; 
  Dmi.negativeLimit = new DmFlt32Object(string.str);
  Dmi.negativeLimit->startConsume(DM_ASYNC);
  group->addItem(Dmi.negativeLimit);
  Dmi.negativeLimit->urgentOpen(DM_STATIC);
  p = RADS_TO_DEGS(negLimit);
  Dmi.negativeLimit->write(&p);
  Dmi.negativeLimit->urgentClose();

  // create positive limit DM item as consumer
  string = IVIEW_DM_PREFIX + dmName + IVIEW_OFFSET_DM; 
  Dmi.offset = new DmFlt32Object(string.str);
  Dmi.offset->startConsume(DM_ASYNC);
  group->addItem(Dmi.offset);
  Dmi.offset->urgentOpen(DM_STATIC);
  p = RADS_TO_DEGS(offset);
  Dmi.offset->write(&p);
  Dmi.offset->urgentClose();

  // create panoramic offset DM item as consumer
  string = IVIEW_DM_PREFIX + dmName + IVIEW_PANORAMIC_OFFSET_DM; 
  Dmi.panoramicOffset = new DmFlt32Object(string.str);
  Dmi.panoramicOffset->startConsume(DM_ASYNC);
  group->addItem(Dmi.panoramicOffset);
  Dmi.panoramicOffset->urgentOpen(DM_STATIC);
  p = RADS_TO_DEGS(panoramicOffset);
  Dmi.panoramicOffset->write(&p);
  Dmi.panoramicOffset->urgentClose();

  // create proportional gain DM item as consumer
  string = IVIEW_DM_PREFIX + dmName + IVIEW_PGAIN_DM; 
  Dmi.pGain = new DmFlt32Object(string.str);
  Dmi.pGain->startConsume(DM_ASYNC);
  group->addItem(Dmi.pGain);
  Dmi.pGain->urgentOpen(DM_STATIC);
  Dmi.pGain->write(&pGain);
  Dmi.pGain->urgentClose();

  // create proportional gain DM item as consumer
  string = IVIEW_DM_PREFIX + dmName + IVIEW_IGAIN_DM; 
  Dmi.iGain = new DmFlt32Object(string.str);
  Dmi.iGain->startConsume(DM_ASYNC);
  group->addItem(Dmi.iGain);
  Dmi.iGain->urgentOpen(DM_STATIC);
  Dmi.iGain->write(&iGain);
  Dmi.iGain->urgentClose();

  // create proportional gain DM item as consumer
  string = IVIEW_DM_PREFIX + dmName + IVIEW_DGAIN_DM; 
  Dmi.dGain = new DmFlt32Object(string.str);
  Dmi.dGain->startConsume(DM_ASYNC);
  group->addItem(Dmi.dGain);
  Dmi.dGain->urgentOpen(DM_STATIC);
  Dmi.dGain->write(&dGain);
  Dmi.dGain->urgentClose();

  // create proportional gain DM item as consumer
  string = IVIEW_DM_PREFIX + dmName + IVIEW_FGAIN_DM; 
  Dmi.fGain = new DmFlt32Object(string.str);
  Dmi.fGain->startConsume(DM_ASYNC);
  group->addItem(Dmi.fGain);
  Dmi.fGain->urgentOpen(DM_STATIC);
  Dmi.fGain->write(&fGain);
  Dmi.fGain->urgentClose();

  // create stepper pan position DM item as consumer 
  string = PAN_TILT_DM_PREFIX + dmName + PT_CW_LIMIT_DM;
  Dmi.cwLimit = new DmBooleanObject(string.str);
  Dmi.cwLimit->startConsume(DM_ASYNC);
  group->addItem(Dmi.cwLimit);

  // create stepper pan position DM item as consumer 
  string = PAN_TILT_DM_PREFIX + dmName + PT_CCW_LIMIT_DM; 
  Dmi.ccwLimit = new DmBooleanObject(string.str);
  Dmi.ccwLimit->startConsume(DM_ASYNC);
  group->addItem(Dmi.ccwLimit);

  // create analog position DM item as consumer
  string = PAN_TILT_DM_PREFIX + dmName + PT_ANALOG_POS_DM; 
  Dmi.analogDegs = new DmFlt32Object(string.str);
  Dmi.analogDegs->startConsume(DM_ASYNC);
  group->addItem(Dmi.analogDegs);

  // create stepper pan position DM item as consumer 
  string = PAN_TILT_DM_PREFIX + dmName + PT_STEPPER_POS_DM; 
  Dmi.stepperDegs = new DmFlt32Object(string.str);
  Dmi.stepperDegs->startConsume(DM_ASYNC);
  group->addItem(Dmi.stepperDegs);

  // create positive limit DM item as consumer
  string = PAN_TILT_DM_PREFIX + dmName + PT_CW_LIMIT_POS_DM; 
  Dmi.cwLimitPos = new DmFlt32Object(string.str);
  Dmi.cwLimitPos->startConsume(DM_ASYNC);
  group->addItem(Dmi.cwLimitPos);

  // create positive limit DM item as consumer
  string = PAN_TILT_DM_PREFIX + dmName + PT_CCW_LIMIT_POS_DM; 
  Dmi.ccwLimitPos = new DmFlt32Object(string.str);
  Dmi.ccwLimitPos->startConsume(DM_ASYNC);
  group->addItem(Dmi.ccwLimitPos);


  // Initialize data buffers
  positionBuffer  = new IO_Buffer(bufferLength);

#ifdef IO_BUFFERING
  controlBuffer = new IO_Buffer(bufferLength);
  errorBuffer 	= new IO_Buffer(bufferLength);
  ierrorBuffer 	= new IO_Buffer(bufferLength);
  derrorBuffer 	= new IO_Buffer(bufferLength);
  pKalmanBuffer	= new IO_Buffer(bufferLength);	
  vKalmanBuffer = new IO_Buffer(bufferLength);	
  setPointBuffer  = new IO_Buffer(bufferLength);
  dsetPointBuffer = new IO_Buffer(bufferLength);
  velocityBuffer = new IO_Buffer(bufferLength);
  innovationsBuffer = new IO_Buffer(bufferLength);
#endif

  // Initialize integrator with external buffers 
  integrator = new IIR_Filter(bInt,2,aInt,2);

  // control input filter
  FltN** Fi = new FltN*[U_SYSTEM_ORDER];
  for (i = 0; i < U_SYSTEM_ORDER; i++){
    Fi[i] = new FltN[U_SYSTEM_ORDER];
    for (j = 0; j < U_SYSTEM_ORDER; j++){
      Fi[i][j] = F[i][j];
    }
  }
  FltN** Di = new FltN*[U_SYSTEM_ORDER];
  for (i = 0; i < U_SYSTEM_ORDER; i++){
    Di[i] = new FltN[U_INPUT_DIMENSION];
    for (j = 0; j < U_INPUT_DIMENSION; j++){
      Di[i][j] = D[i][j];
    }
  }
  FltN** Hi = new FltN*[U_OUTPUT_DIMENSION];
  for (i = 0; i < U_OUTPUT_DIMENSION; i++){
    Hi[i] = new FltN[U_SYSTEM_ORDER];
    for (j = 0; j < U_SYSTEM_ORDER; j++){
      Hi[i][j] = H[i][j];
    }
  }
  FltN** Ki = new FltN*[U_SYSTEM_ORDER];
  for (i = 0; i < U_SYSTEM_ORDER; i++){
    Ki[i] = new FltN[U_OUTPUT_DIMENSION];
    for (j = 0; j < U_OUTPUT_DIMENSION; j++){
      Ki[i][j] = K[i][j];
    }
  }
  FltN* x0 = new FltN[U_SYSTEM_ORDER]; 
  bzero((char*) x0, U_SYSTEM_ORDER * sizeof(FltN));
  uFilter = new Linear_System(Fi,U_SYSTEM_ORDER,
			      Di,U_INPUT_DIMENSION,
			      Hi,U_OUTPUT_DIMENSION,
			      x0);

  pFilter = new Kalman_Filter(Fi,U_SYSTEM_ORDER,
			      Di,U_INPUT_DIMENSION,
			      Hi,U_OUTPUT_DIMENSION,
			      Ki,x0);

  for (i = 0; i < U_SYSTEM_ORDER; i++)
    delete[] Fi[i];
  delete[] Fi;
  for (i = 0; i < U_SYSTEM_ORDER; i++)
    delete[] Di[i];
  delete[] Di;
  for (i = 0; i < U_OUTPUT_DIMENSION; i++)
    delete[] Hi[i];
  delete[] Hi;
  for (i = 0; i < U_SYSTEM_ORDER; i++)
    delete[] Ki[i];
  delete[] Ki;
  delete[] x0;    
}



/******************************************************************************
Function : Axis::~Axis                                              
Purpose  : Axis class destructor				      
Inputs   :                		                                      
Outputs  : 		                                                      
*******************************************************************************/

Axis::~Axis()
{
  delete group;
  delete Dmi.motorStall;
  delete Dmi.positiveLimit; 
  delete Dmi.negativeLimit; 
  delete Dmi.offset; 
  delete Dmi.panoramicOffset; 
  delete Dmi.pGain;
  delete Dmi.iGain;
  delete Dmi.dGain;
  delete Dmi.fGain;

  delete Dmi.analogRequest;
  delete Dmi.stepperRequest;
  delete Dmi.velocityCmd;
  delete Dmi.positionCmd;
  delete Dmi.analogDegs;
  delete Dmi.stepperDegs;
  delete Dmi.cwLimit;
  delete Dmi.ccwLimit;
  delete Dmi.ctlrString;

  delete integrator;
  delete uFilter;		
  delete pFilter;		
  delete positionBuffer;
  delete calibrationTable;

#ifdef IO_BUFFERING
  delete controlBuffer;
  delete errorBuffer;
  delete ierrorBuffer;
  delete derrorBuffer;
  delete setPointBuffer;
  delete dsetPointBuffer;
  delete pKalmanBuffer;		
  delete vKalmanBuffer;		
  delete velocityBuffer;	
  delete innovationsBuffer;	
#endif
}

  

/*****************************************************************************
Function : Axis::nameConversion                                             
Purpose  : Converts data manager name to usable filename		      
Inputs   : name - string with data manger item name
Outputs  : returns string of form "portCameraPan"   
Notes    : This routine is a cludge for the general problem of passing names
         : to member objects of lower hierarchy. Needs works.                
******************************************************************************/

String Axis::nameConversion(String& name)
{
  String r;

  if (name == "PORT.CAMERA.PAN.")
  {
    r = "portCameraPan";
    return r;
  }

  if (name == "PORT.CAMERA.TILT.")
  {
    r = "portCameraTilt";
    return r;
  }

  if (name == "PORT.LIGHT.PAN.")
  {
    r = "portLightPan";
    return r;
  }

  if (name == "PORT.LIGHT.TILT.")
  {
    r = "portLightTilt";
    return r;
  }

  if (name == "STBD.CAMERA.PAN.")
  {
    r = "stbdCameraPan";
    return r;
  }

  if (name == "STBD.CAMERA.TILT.")
  {
    r = "stbdCameraTilt";
    return r;
  }

  if (name == "STBD.LIGHT.PAN.")
  {
    r = "stbdLightPan";
    return r;
  }

  if (name == "STBD.LIGHT.TILT.")
  {
    r = "stbdLightTilt";
    return r;
  }
}



/*****************************************************************************
Function : Axis::Update                                             
Purpose  : Servos axis based on input setpoint position		      
Inputs   : p - axis setpoint for servoing
         : dp - axis setpoint velocity for servoing
         : link - controller comm flag
Outputs  : 		                                                      
******************************************************************************/

STATUS Axis::Update(FltN p, FltN dp, MBool link)
{
  FltN v = 0;

  // read all new DM items
  ReadDMItems();

  // if controller off send zero velocity and exit
  if (!link)
  {
    Velocity(v);	
    return ERROR;
  }

  // check positve limit setpoint
  if (p > posLimit)
  {
    p = posLimit;
    posLimitState = TRUE;
  }
  else
    posLimitState = FALSE;

  // check negative limit setpoint
  if (p < negLimit)
  {
    p = negLimit;
    negLimitState = TRUE;
  }
  else
    negLimitState = FALSE;


  // estimate position and velocity MBM
  positionBuffer->Update(analogPosition);
  pFilter->Update(&analogPosition);	
  position = pFilter->State(1);
  dposition = pFilter->State(2);


  // clip position estimate in [-180,180]
  if (position > PI) position = PI;
  if (position < -PI) position = -PI;


  // write of iview position estimate for GUI
  FltN positionDegs = RADS_TO_DEGS(position);
  Dmi.position->write(&positionDegs);


  // insert panoramic mode offset if nonzero
  p -= panoramicOffset;

 
  // update diagnostic buffers
#ifdef IO_BUFFERING
  setPointBuffer->Update(p);
  dsetPointBuffer->Update(dp);
  pKalmanBuffer->Update(position);  
  vKalmanBuffer->Update(dposition);
#endif


  // compute PID error terms
  error  = p - position;
  derror = dp - dposition;
  ierror = 0;

  // compute integral error if error below a threshold
  if (Fabs(error) < INTEGRATOR_DUMP_THRESHOLD)
  {
    if (!integratorDump)
    {
      integrator->Init();
      integratorDump = TRUE;
    }
    ierror = integrator->Compute(error);
  }
  else
    integratorDump = FALSE;

  // compute PID control input
  u = pGain * error + dGain * derror + iGain * ierror + fGain * dp;


  // check for closed loop control enable
  if (!control || standby)
    u = SMALL_NUMBER;


  // motor stall detector (MBM)
  if (Fabs(pFilter->Innovation()) > INNOVATIONS_THRESHOLD)
  {
    if (!stall)
    {
      logMsg("%s - stall detected\n", name.str);
      if (stallDetectionCount++ >= STALL_DETECTION_COUNT)
      {
	stall = TRUE;
	Dmi.motorStall->write(&stall);
	stallDetectionCount = 0;
	uFilter->Init();
	u = SMALL_NUMBER;
	logMsg("%s - stall\n", name.str);
      }
    }
    else
      u = STALL_VELOCITY * sign(u);
  }
  else
  {
    if (stall)
    {
      stall = FALSE;
      logMsg("%s - stall clear\n", name.str);
    }
    else
    {
      // logMsg("%s - stall count cleared\n", name.str);
      stallDetectionCount = 0;
    }
  }


  // update all error buffers
#ifdef IO_BUFFERING
  errorBuffer->Update(error);
  derrorBuffer->Update(derror);
  ierrorBuffer->Update(ierror);
#endif


  // clip commanded velocity
  if (Fabs(u) > maxVelocity) 
    u = sign(u) * maxVelocity;  

  // input velocity command to linear system model
  pFilter->Propagate(&u);

  // select velocity state from model and send velocity command
  v = pFilter->State(2);
  Velocity(v);	


#ifdef IO_BUFFERING
  controlBuffer->Update(u);
  innovationsBuffer->Update(pFilter->Innovation());
#endif

  return OK;
}



/******************************************************************************
Function : Axis::ReadDMItems                                                  
Purpose  : Reads in all new DM items               
Inputs   : None                                            
Outputs  : None                                                                
*******************************************************************************/

STATUS Axis::ReadDMItems()
{
  group->getChanges();

  if (group->itemChanged(Dmi.ccwLimit))		// limit switch states
    Dmi.ccwLimit->read(&ccwLimitSwitch);
 
  if (group->itemChanged(Dmi.cwLimit))
    Dmi.cwLimit->read(&cwLimitSwitch);
 
  if (group->itemChanged(Dmi.ccwLimitPos))	// limit switch thresholds
    Dmi.ccwLimitPos->read(&ccwLimitPos);
 
  if (group->itemChanged(Dmi.cwLimitPos))
    Dmi.cwLimitPos->read(&cwLimitPos);
 
  if (group->itemChanged(Dmi.positiveLimit))
  {
    Dmi.positiveLimit->read(&posLimit); 
    posLimit = DEGS_TO_RADS(posLimit);
  }

  if (group->itemChanged(Dmi.negativeLimit))
  {
    Dmi.negativeLimit->read(&negLimit); 
    negLimit = DEGS_TO_RADS(negLimit);
  }

  if (group->itemChanged(Dmi.offset))
  {
    Dmi.offset->read(&offset); 
    offset = DEGS_TO_RADS(offset);
  }

  if (group->itemChanged(Dmi.panoramicOffset))
  {
    Dmi.panoramicOffset->read(&panoramicOffset); 
    panoramicOffset = DEGS_TO_RADS(panoramicOffset);
  }

  if (group->itemChanged(Dmi.pGain))
    Dmi.pGain->read(&pGain); 

  if (group->itemChanged(Dmi.iGain))
    Dmi.iGain->read(&iGain); 

  if (group->itemChanged(Dmi.dGain))
    Dmi.dGain->read(&dGain); 

  if (group->itemChanged(Dmi.fGain))
    Dmi.fGain->read(&fGain); 

  // read position from pan-tilt axis feedback potentiometer
  if (group->itemChanged(Dmi.analogDegs))
    readAnalogPosition();

  if (group->itemChanged(Dmi.stepperDegs))
    readStepperPosition(); 

  return OK;
}



/******************************************************************************
Function : Axis::Velocity                                                  
Purpose  : Checks input limits and sends axis velocity command                 
Inputs   : v - velocity command                                              
Outputs  :                                                                 
*******************************************************************************/

STATUS Axis::Velocity(FltN v)
{
  static Int i = 0;
  FltN velocity = v;

  if (Fabs(velocity) > maxVelocity) 
    velocity = sign(velocity) * maxVelocity;  

#ifdef IO_BUFFERING
  velocityBuffer->Update(velocity);
#endif

  // convert to integer command in step units
  velocityCmd = (Int16) (direction * velocity * velocityConversion);

  if (directWrite)
    Dmi.velocityCmd->write(&velocityCmd);

  return OK;
}



/******************************************************************************
Function : Axis::urgentVelocity                                                  
Purpose  : Checks input limits and sends axis velocity command as urgent Dmi     
Inputs   : v - velocity command                                              
Outputs  : None   
Notes    : This routine can be used when a separate task references the Dmi's
         : within this object.                                                 
*******************************************************************************/

STATUS Axis::urgentVelocity(FltN v)
{
  static Int i = 0;
  FltN velocity = v;

  if (Fabs(velocity) > maxVelocity) 
    velocity = sign(velocity) * maxVelocity;  

#ifdef IO_BUFFERING
  velocityBuffer->Update(velocity);
#endif

  // convert to integer command in step units
  velocityCmd = (Int16) (direction * velocity * velocityConversion);

  // urgent write to Dmi
  Dmi.velocityCmd->urgentOpen(DM_STATIC);
  Dmi.velocityCmd->write(&velocityCmd);
  Dmi.velocityCmd->urgentClose();

  return OK;
}



/*****************************************************************************
Function : Axis::Position                                                  
Purpose  : Sends axis position command                                        
Inputs   : a - axis position                                                 
Outputs  : None                                                                
******************************************************************************/

STATUS Axis::Position(FltN a)
{
  FltN position = a * direction;
  Dmi.positionCmd->write(&position);
  return OK;
}



/*****************************************************************************
Function : Axis::requestAnalogPosition                                      
Purpose  : Requests analog position feedback measurement from pan-tilt micro    
Inputs   : None                                                                
Outputs  : None                                                                
******************************************************************************/

STATUS Axis::requestAnalogPosition()
{
  Dmi.analogRequest->write();
  return OK;
}



/******************************************************************************
Function : Axis::readAnalogPosition                                       
Purpose  : Read and process analog position input from pan-tilt daughterboard  
Inputs   : None                                                                
Outputs  : analogPosition updated    
Notes	 : The variable analogPosition contains the offset and direction 
         : adjusted analog reading.                                             
******************************************************************************/

STATUS Axis::readAnalogPosition()
{
  Dmi.analogDegs->read(&analogDegs);

  // check input range
  if ((analogDegs >= 180) || (analogDegs <= -180))
    analogDegs = analogDegsLast;

  analogDegsLast = analogDegs;
  analogPosition = calibrationTable->LinearPiecewiseInterpolationY(analogDegs);
  analogPosition = direction * DEGS_TO_RADS(analogPosition) - offset;

  return OK;
}



/*****************************************************************************
Function : Axis::positiveLimitSwitch                                              
Purpose  : Returns state of positive limit switch                               
Inputs   : None                                                 
Outputs  : None   
Notes    : This make the limit switches axis independent. Note that the 
         : pan-tilt controller hardware is such that tilt cw limit switch is
         : up and the convention for positive tilt is up; alternatively, the
         : pan cw limit switch is towards the left, however, the convention 
         : for positive pan is to the right.                                  
******************************************************************************/

MBool Axis::positiveLimitSwitch()
{
  if (direction < 0.0) 
    return ccwLimitSwitch | posLimitState; 
  else
    return cwLimitSwitch | posLimitState; 
}



/*****************************************************************************
Function : Axis::negativeLimitSwitch                                              
Purpose  : Returns state of negative limit switch                               
Inputs   : None                                                 
Outputs  : None   
Notes    : This make the limit switches axis independent. Note that the 
         : pan-tilt controller hardware is such that tilt cw limit switch is
         : up and the convention for positive tilt is up; alternatively, the
         : pan cw limit switch is towards the left, however, the convention 
         : for positive pan is to the right.                                  
******************************************************************************/

MBool Axis::negativeLimitSwitch()
{
  if (direction > 0.0) 
    return ccwLimitSwitch | negLimitState; 
  else
    return cwLimitSwitch | negLimitState; 
}



#if 0
/*****************************************************************************
Function : Axis::linearCorrection                                    
Purpose  : Performs linear interpolation of input value with calibration table
Inputs   : y - input value                                                         
Outputs  : x - interpolated value in interval [x1,x2]   
Notes    : If y falls outside of calibration table range, y is returned      
******************************************************************************/

FltN Axis::linearCorrection(FltN y)
{
  Int16 i,m;
  FltN x1,x2,y1,y2;

  m = (AXIS_CAL_NUM_STEPS - 1)/2;

  if ( Fabs(y) <= AXIS_CAL_STEP_SIZE * m )
  {
    // find index into table where y is in the interval [y1,y2]
    i = (Int16) (y/AXIS_CAL_STEP_SIZE + m);	

    // find step interval [x1,x2] corresponding to interval [y1,y2]
    x1 = AXIS_CAL_STEP_SIZE * (i - m); 
    x2 = x1 + AXIS_CAL_STEP_SIZE; 

    // index into calibration table to find calibration points at x1 and x2
    y1 = calibrationTable[i];
    y2 = calibrationTable[i+1];

    // compute linear interpolation
    return ( x1 + (y - y1) / (y2 - y1) * (x2 - x1) );
  }
  else
    return y;
}	
#endif



/*****************************************************************************
Function : Axis::requestStepperPosition                                    
Purpose  : Requests the controller stepper position                            
Inputs   : None                                                                
Outputs  : None                                                                
******************************************************************************/

STATUS Axis::requestStepperPosition()
{
  Dmi.stepperRequest->write();
  return OK;
}



/*****************************************************************************
Function : Axis::readStepperPosition                                    
Purpose  : Reads stepper position from controller                            
Inputs   : None                                                                
Outputs  : None 
Notes    : Stepper position is sent only after a requestStepperPosition call.
         : Requesting a stepper position causes the controller micro to pause
         : for 100mS; hence, this reading cannot be used in real-time.       
******************************************************************************/

STATUS Axis::readStepperPosition()
{
  Dmi.stepperDegs->read(&stepperDegs);
  stepperPosition = direction * DEGS_TO_RADS(stepperDegs);
  return OK;
}



/*****************************************************************************
Function : Axis::Log                                            
Purpose  : Logs axis positions to file                                         
Inputs   : filename                                                                
Outputs  :                                                                 
******************************************************************************/

STATUS Axis::Log()
{
  String p = IVIEW_LOG_DIRECTORY;
  p += name;
  p += "Log";
  logMsg("opening file       %s\n", p.str);
  if ((fp = fopen(p.str, "w")) == NULL)
  {
    logMsg("error opening file %s\n", p.str);
    return ERROR;
  }

  char s[200];
  sprintf(s, "%10.6f\n", pGain);
  fwrite((char*) s, strlen(s), 1, fp);
  sprintf(s, "%10.6f\n", dGain);
  fwrite((char*) s, strlen(s), 1, fp);
  sprintf(s, "%10.6f\n", iGain);
  fwrite((char*) s, strlen(s), 1, fp);
  sprintf(s, "%10.6f\n", maxVelocity);
  fwrite((char*) s, strlen(s), 1, fp);
  sprintf(s, "%10.6f\n", minVelocity);
  fwrite((char*) s, strlen(s), 1, fp);
  sprintf(s, "%10.6f\n", velocityConversion);
  fwrite((char*) s, strlen(s), 1, fp);
  sprintf(s, "%10.6f\n", offset);
  fwrite((char*) s, strlen(s), 1, fp);

#ifdef IO_BUFFERING
  Int k = positionBuffer->index;
  for (Int i = 1; i <= bufferLength; i++)
  {
    sprintf(s,
	  "%9.5f %9.5f %9.5f %9.5f %9.5f %9.5f %9.5f %9.5f %9.5f %9.5f %9.5f\n", 
	    (*positionBuffer)[k+i],
	    (*pKalmanBuffer)[k+i],		
	    (*vKalmanBuffer)[k+i],		
	    (*controlBuffer)[k+i],		
	    (*errorBuffer)[k+i],		
	    (*ierrorBuffer)[k+i],		
	    (*derrorBuffer)[k+i],
	    (*setPointBuffer)[k+i],
	    (*dsetPointBuffer)[k+i],
	    (*velocityBuffer)[k+i],
	    (*innovationsBuffer)[k+i]
	    );

    fwrite((char*) s, strlen(s), 1, fp);
  }
#endif

  fclose(fp);

  return OK;
}