#include <math.h>
#include <i86.h>
#include "TailConeDriver.h"
#include "TailConeServer.h"
#include "Attributes.h"
#include "IntegerAttribute.h"
#include "FloatAttribute.h"
#include "BooleanAttribute.h"
#include "AttributeParser.h"
#include "System.h"
#include "TimeP.h"
#include "VehicleConfigurationIF.h"
#include "PeriodicTask.h"
#include "Syslog.h"
#include "MathP.h"
#include "PicServo.h"
#include "CProp.h"
#include "CStepper.h"

const double in2m = .0254;        // meters / inch
const double m2s = 94488.189*2.0;     // steps / meter, number of
                                  // steps to move linear actuators a meter.   

TailConeDriver::TailConeDriver(SerialDevice *device, int period)
  : PeriodicTask("tailConeDriver")
{
  _period = period;
  _log = NULL;


  Syslog::write("TailConeServer -- "
		"Maximum Elevator Actuator Excursion is %d Counts", 
		_maxElevatorCounts);

  Syslog::write("TailConeServer:: -- "
		"Minimum Elevator Actuator Excursion is %d Counts", 
		_minElevatorCounts);

  Syslog::write("TailConeServer -- "
		"Maximum Rudder Actuator Excursion is %d Counts", 
		_maxRudderCounts);

  Syslog::write("TailConeServer -- "
		"Minimum Rudder Actuator Excursion is %d Counts", 
		_minRudderCounts);

  _commandCount = _count = 0;
  _device = device;
  _device->setLineFormat(19200, 8, 1, "NONE");
  //  _device->raw();

  //set up interfaces to periodic task
  _input = new TailConeInput(SharedData::Read);
  _output = new TailConeOutput(SharedData::Write);
  _input->read();
  _input->data.rudder = _input->data.elevator = _input->data.propOmega = 0;

  //create the log
  _log = new TailConeLog(this, DataLog::BinaryFormat);

  //grab device handles for rudder, elevator, and thruster
  _picservo = new CPicServo();
  _picservo->NmcInit(_device, 19200);

  //put check here for appropriate modules found

  _rudder = new CStepper();
  _elevator = new CStepper();
  _propeller = new CProp();
  loadConfiguration();

  _rudderpos = _elevatorpos = 0.0;
  _maxElevatorCounts = elevatorSteps( _maxElevatorAngle );
  _minElevatorCounts = elevatorSteps( _minElevatorAngle );
  _maxRudderCounts = rudderSteps( _maxRudderAngle );
  _minRudderCounts = rudderSteps( _minRudderAngle );

  //since we can't create the control surface objects before
  //we load the config, we have to copy over some things here
  _rudder->Pot1Home(_rudderHome);
  _rudder->Pot1CntsToSteps(_rudderCntsToSteps);
  _elevator->Pot1Home(_elevatorHome);
  _elevator->Pot1CntsToSteps(_elevatorCntsToSteps);

  //initialize control surfaces and prop
  initElevator();
  homeElevator();
  initRudder();
  homeRudder();
  _propeller->Initialize(_picservo, 3);
  _propeller->EnableAmp();

  //set up deadman;
  _deadman = 0; _deadmanCntDown = 0;

  addPeriodicCallback(period, 
		      (CallbackMethod )TailConeDriver::periodicCallback);

  _serverProxy = new TailConeIF("tailConeServer");

  // Subscribe to events from server
  _eventService->subscribe(_serverProxy,
			   TailConeIF::Initialized,
			   (EventService::Callback )TailConeDriver::eventCallback);
  

  _eventService->subscribe(_serverProxy,
			   TailConeIF::SetDeadman,
			   (EventService::Callback )TailConeDriver::eventCallback);

  printf("done starting driver\n");
}


static int iabs(int i)
{
  if(i < 0)
    i = -1*i;
  return i;
}

TailConeDriver::~TailConeDriver()
{
  
  _propeller->SetSpeed(0);
  delete _propeller;
  setElevatorAngle(0.0);
  delete _elevator;
  setRudderAngle(0.0);
  delete _rudder;

  if (_log) {
    delete _log;
  }

  delete _input;
  delete _output;
}

void TailConeDriver::periodicCallback()
{
static int i = 0, cnt = 0;
static unsigned int reindex = 0;
static unsigned int logWrite = 0;
struct timespec startTime, endTime;
double loopTime;

  clock_gettime(CLOCK_REALTIME, &startTime);

  // Read fin and prop commands from server (via shared memory)
  _input->read();

  //send new commands to boards and read back state  
  rudderControlLoop(_input->data.rudder);
  elevatorControlLoop(_input->data.elevator);

  //toggle propeller watchdog and execute rudder control loop
  if (_deadman && (_deadmanCntDown-- == 0)) {
    _deadmanCntDown = 0;
    _input->data.propOmega = 0;
  }

  int rpmSpeed = (_input->data.propOmega*60.0)/(2.0*M_PI);
  _propeller->SetSpeed(rpmSpeed);
  _propeller->Update();
  _currentSpeed = (_propeller->GetCurSpeed()/60.0)*(2.0*M_PI);
  _propeller->RefreshWD();

  clock_gettime(CLOCK_REALTIME, &endTime);
  loopTime = (double)(endTime.tv_sec+endTime.tv_nsec/1.0e9) -
    (double)(startTime.tv_sec+startTime.tv_nsec/1.0e9);
  //  printf("looptime %f\n", loopTime);


  _log->write();

  // Fill in output structure
  _output->data.elevator =
    rudderRadians(_rudder->pot1()*_rudder->Pot1CntsToSteps());
  _output->data.rudder = 
    elevatorRadians(_elevator->pot1()*_elevator->Pot1CntsToSteps());
  _output->data.propOmega = _currentSpeed;
  _output->data.status = DeviceIF::Ok;
  _output->data.propCurrent1 = 0;

  //reset deadman on propeller
  Time::gettime(&_output->data.sampleTime);

  // Make output data available to server (via shared memory)
  _output->write();
}

DeviceIF::Status TailConeDriver::setDeadman(short msec)
{
  _deadman = msec;
  _deadmanCntDown = msec/_period+1;
  return DeviceIF::Ok;
}

DeviceIF::Status TailConeDriver::initialize()
{
  return DeviceIF::Ok;
}

void TailConeDriver::eventCallback(TaskInterface *taskIF,
				   EventCode event)
{
  switch (event) {

  case TailConeIF::Initialized:
    _output->data.status = initialize();
    break;

  case TailConeIF::SetDeadman:
    _input->read();
    if ((_output->data.status = setDeadman(_input->data.deadmanMillisec)) ==
	DeviceIF::Ok)
      // Successfully set deadman to input value
      _output->data.deadmanMillisec = _input->data.deadmanMillisec;

    break;

  default:
    return;
  }

  // Make output, status available to server
  _output->write();
}


////////////////////////////////////////////////////////////////////////////
// *************** BEGIN ELEVATOR CONTROL FUNCTIONS ***********************
void TailConeDriver::initElevator()
{
  //initialize rudder hardware
  _elevator->Initialize(_picservo, _picservo->nummod); 
  _elevator->EnableAmp();
}

void TailConeDriver::homeElevator()
{

  int pos1 = 0;
  int pos2 = 0;  
  int pot1 = 0;
  int pot = 0;
  int done = 0;

  // some high error values
  int poterr = 128;
  int poserr = 10000;
  int k = 23;
  int moveto = 0;
  int failures = 0;
  time_t t;
  do {
    try {
      _elevator->Update();
      pot1 = _elevator->A2D();
      pos1 = _elevator->currentSteps();
      poterr = _elevatorHome - pot1;
      moveto = _elevator->Pot1CntsToSteps()*poterr;
      _elevator->SetDestPos(pos1 + moveto); delay(10);
      t = time(NULL);
      // wait till we get to commanded position
      // should use RT command here
      do {
	_elevator->Update();
	if(difftime(time(NULL),t) > 10.0)
	  throw PicException("TailConeServer::homeElevator -- Failure to reach commanded position in 10 sec");
      } while(_elevator->currentSteps() != _elevator->goalSteps()); 
    }    
    catch(PicException e)
      {
	failures++;
	Syslog::write(e.msg);
        // make sure we don't exit loop if err happens to be zero
        poterr = 128;	
      }
    
  }while((iabs(poterr) > 1 )&&(failures < 10));
  
  if(failures == 10)
    {
      Syslog::write("BpauvTailConeServer::homeElevator -- Failed to initialize elevator");
      exit(-1);
    }

  _elevatorsteps = 0;
  try {_elevator->Update();}
  catch(...) {
	printf("weird address error happened in elevator initializtion\n");
  }

  _elevator->offset(_elevator->currentSteps());


}


/////////////////////////////////////////////////////////////////////////
// function to set elevator angle. Commanded angle is bounded by
// the maximum and minimum specified in the tailcone config file
// [input] desired elevator angle in radians

DeviceIF::Status TailConeDriver::setElevatorAngle(double desired_angle)
{
  Boolean debug = False;
  static long count = 0;
  dprintf("TailConeServer::setElevator()");
  TimeIF::TimeSpec time;
  double potSteps;  
  int errorSteps;

  errorSteps = elevatorSteps(desired_angle) -
    (_elevatorsteps+_elevator->offset());
  if (iabs(errorSteps) > 5) {
    _elevatorsteps = elevatorSteps(desired_angle);
    _elevatorpos = desired_angle;
    _elevator->SetDestPos(_elevatorsteps + _elevator->offset());
  } 

  /*
  if( _isSimulated  && _simulator ) {
       _simulator->command( _prevSpeed, desired_angle, _rudderpos );
  }
  */


  return DeviceIF::Ok;
}


DeviceIF::Status TailConeDriver::setElevatorCounts(short counts)
{
  Boolean debug = False;
  dprintf("setElevatorCounts()");

  _elevator->SetDestPos((int)counts);
  return DeviceIF::Ok;
}



DeviceIF::Status TailConeDriver::getComputedElevatorAngle(double *angle, 
					  TimeIF::TimeSpec *time)
{
  int pos;
  try
    {
     pos =_elevator->currentSteps();
    } 
  catch(Exception e)
    {
      Syslog::write("TailConeServer::getComputedElevatorAngle : caught excepetion %s",e.msg); 
    }
  
  
  *angle = elevatorRadians(pos);
  
  return DeviceIF::Ok;
}


DeviceIF::Status TailConeDriver::getElevatorAngle(double *angle, 
					  TimeIF::TimeSpec *time)
{
  try
    {
      _elevator->Update();
      *angle = elevatorRadians((_elevator->A2D()-_elevatorHome)*
			       _elevator->Pot1CntsToSteps());     
    }
  catch(PicException e)
    {
      Syslog::write("TailConeServer::getElevatorAngle : caught excpetion %s ",e.msg);
    }
  
  return DeviceIF::Ok;
}



/////////////////////////////////////////////////////////////////////////
// convert stp100  steps from home position to an angle in Radians

double TailConeDriver::elevatorRadians(short steps)
{
  double dR = (double)steps/m2s;           // how far from home position?
  double theta = 0.0;

  // should be ok here as long as wacky number of steps never gets input
  // positive angles occur when actuator gets shorter
  try{
    theta = _eTheta0 - acos((_eL1Sqrd+_eL2Sqrd - (_eR0+dR)*(_eR0+dR)) / (2*_eL1*_eL2) );
  } 
  catch(...){
    Syslog::write("TailConeServer: Caught exception in elevatorSteps");
    return 0;
  }

  return theta;
}

/////////////////////////////////////////////////////////////////
// function to compute the position to move to from the desired 
// angle specified for the BPAUV tailcone elevator

short TailConeDriver::elevatorSteps(double theta_desired)
{
  if(theta_desired > _maxElevatorAngle)
    theta_desired = _maxElevatorAngle;
  else if(theta_desired < _minElevatorAngle)
    theta_desired = _minElevatorAngle;
  
  
  double theta_command = _eTheta0 + theta_desired;
  double R_desired = 0.0;
  
  try{        
    R_desired = sqrt( _eL1Sqrd + _eL2Sqrd - (2.0*_eL1*_eL2*cos(theta_command)));
  }
  catch(...){
    Syslog::write("TailConeServer: Caught exception in elevatorSteps");
    return 0;
  }
  
  return ((short)( m2s * ( R_desired - _eR0 )));   // positive theta_desired vals cause actuator to get shorter
}

DeviceIF::Status TailConeDriver::elevatorControlLoop(double desired_angle)
{
  _elevator->Update();
  _elevator->ClosePosLoop();
  setElevatorAngle(desired_angle);

  return DeviceIF::Ok;
}

//////////////////////////////////////////////////////////////////
// *************** BEGIN RUDDER CONTROL FUNCTIONS ***************
void TailConeDriver::initRudder()
{
  int stepdelay = 0;
  int mindelay = 0;
  int accel = 0;
  int mode = 0;
  Boolean tryagain = False;
  int count = 0;

  //initialize hardware
  _rudder->Initialize(_picservo, _picservo->nummod); 
  _rudder->EnableAmp();

}

/////////////////////////////////////////////////////////////////////////
// convert stp100 steps from home position to an angle in Radians

double TailConeDriver::rudderRadians(short steps)
{
  double dR = (double)steps/m2s;           // how far from home position?
  double theta = 0.0;

  // should be ok here as long as wacky number of steps never gets input
  try{
    theta = acos( (_rL1Sqrd+_rL2Sqrd - (_rR0+dR)*(_rR0+dR)) / (2*_rL1*_rL2) ) - _rTheta0;
  }
  catch(...){
    Syslog::write("TailConeServer: Caught exception in rudderRadians");
    return 0.0;
  }
  
  return theta;

}

void TailConeDriver::homeRudder()
{

  int pos1 = 0;
  int pos2 = 0;  
  int pot1 = 0;
  int pot = 0;
  int done = 0;

  // some high error values
  int poterr = 128;
  int poserr = 10000;
  int moveto = 0;
  int failures = 0;
  time_t t;
  do {
 
    try
      {
	pot1 = _rudder->A2D();
	pos1 = _rudder->currentSteps();
	poterr = _rudderHome - pot1;
	moveto = _rudder->Pot1CntsToSteps()*poterr;
	_rudder->SetDestPos(pos1+moveto);
	t = time(NULL);
	// wait till we get to commanded position
	do{
	  _rudder->Update();
          if(difftime(time(NULL),t) > 10.0)
	    throw PicException("TailConeServer::homeRudder -- Failure to reach commanded position in 10 sec");
	}while(_rudder->currentSteps() != _rudder->goalSteps()); 

      }    
    catch(PicException e)
      {
	failures++;
	Syslog::write(e.msg);
        // make sure we don't exit loop if err happens to be zero
        poterr = 128;	      }
    
  }while((iabs(poterr) > 1 )&&(failures < 10));
  
  if(failures == 10)
    {
      Syslog::write("BpauvTailConeServer::homeRudder -- Failed to initialize rudder");
      exit(-1);
    }

  try {_rudder->Update();}
  catch(...) {
	printf("weird address happened in rudder initialization\n");
  }
  _rudder->offset(_rudder->currentSteps());

  
}
/////////////////////////////////////////////////////////////////////////
// function to set rudder angle. Commanded angle is bounded by
// the maximum and minimum specified in the tailcone config file
// [input] desired elevator angle in radians

DeviceIF::Status TailConeDriver::rudderControlLoop(double desired_angle)
{
  _rudder->Update();
  _rudder->ClosePosLoop();
  setRudderAngle(desired_angle);
  return DeviceIF::Ok;
}

DeviceIF::Status TailConeDriver::setRudderAngle(double desired_angle)
{
  Boolean debug = False;
  dprintf("TailConeDriver::setRudder()");
  static long count = 0;
  TimeIF::TimeSpec time;
  double current_angle;
  int errorSteps;

  errorSteps = rudderSteps(desired_angle) -
    (_ruddersteps+_rudder->offset());
  if (iabs(errorSteps) > 5) {
    _ruddersteps = rudderSteps(desired_angle);
    _rudderpos = desired_angle;
    _rudder->SetDestPos(_ruddersteps + _rudder->offset());
  }

  return DeviceIF::Ok;
}


///////////////////////////////////////////////////////////////////////////
// function to set rudder motor position. Commanded movement is bounded by
// the maximum and minimum angles specified in the tailcone config file
// [input] desired linear motor position in steps from the zero
//         / home position.

DeviceIF::Status TailConeDriver::setRudderCounts(short counts)
{
  Boolean debug = False;
  dprintf("setRudderCounts()");

  _rudder->SetDestPos((int)counts);
  return DeviceIF::Ok;
}

//////////////////////////////////////////////////////////////////////
// Returns the current rudder angle in radians
// angle computed from the number of steps controller says the motor is 
// from the home position 

DeviceIF::Status TailConeDriver::getComputedRudderAngle(double *angle, 
					TimeIF::TimeSpec *time)
{
  int pos;
  try {
    pos = _rudder->currentSteps();
  }
  catch(Exception e) {
    Syslog::write("TailConeServer::getComputedElevatorAngle : caught excepetion %s",e.msg);            
  }
  
  *angle = rudderRadians(pos);

  return DeviceIF::Ok;
}


DeviceIF::Status TailConeDriver::getRudderAngle(double *angle, 
						TimeIF::TimeSpec *time)
{
  try {
    _rudder->Update();
    *angle = rudderRadians((_rudder->A2D() - _rudderHome)*
			   _rudder->Pot1CntsToSteps());
  }
  catch(PicException e) {
    Syslog::write("TailConeServer::getRudderAngle : exception caught, %s",
		  e.msg);
  }
  
  return DeviceIF::Ok;
}

/////////////////////////////////////////////////////////////////
// function to compute the position to move to from the desired 
// angle specified for the BPAUV tailcone rudder 

short TailConeDriver::rudderSteps(double theta_desired)
{
  if(theta_desired > _maxRudderAngle)
    theta_desired = _maxRudderAngle;
  else if(theta_desired < _minRudderAngle)
     theta_desired = _minRudderAngle;

  
  double theta_command = _rTheta0 + theta_desired;
  double R_desired = 0.0;
  
  try{
    R_desired = sqrt( _rL1Sqrd + _rL2Sqrd - (2.0*_rL1*_rL2*cos(theta_command)));
  }  
  catch(...){
    Syslog::write("TailConeServer: Caught exception in rudderSteps");
    return 0;
  }
  
 
  return (short)( -m2s * (R_desired - _rR0));  // positive theta_desired vals cause actuator to get longer

}

void TailConeDriver::loadConfiguration()
{
  long ival;

  Attributes attributes("doradoTailCone");

  attributes.add(new IntegerAttribute("rudder_board_num",
				      "Board num of the pic rudder controller",
				      (long *)&(_rudder->m_nNodeNum)));    

  attributes.add(new FloatAttribute("rudder_L1",
				    "Rudder Actuator Parameter L1",
				    &_rL1,  0.333));     // meters

  attributes.add(new FloatAttribute("rudder_L2",
				    "Rudder Actuator Parameter L2",
				    &_rL2,  0.075));     // meter

  attributes.add(new FloatAttribute("rudder_R0",
				    "Rudder Actuator Parameter R0",
				    &_rR0,  0.302));     // meters

  attributes.add(new FloatAttribute("rudder_Theta_zero",
				    "Rudder Actuator Parameter theta_zero",
				    &_rTheta0,  1.045));   // radians

  attributes.add(new FloatAttribute("maxRudderAngle",
				    "Maximum Rudder Angle",
				    &_maxRudderAngle, 0.262)); // radians

  attributes.add(new FloatAttribute("minRudderAngle",
				    "Minimum Rudder Angle",
				    &_minRudderAngle, -0.262));  // radians

  attributes.add(new IntegerAttribute("rudderAccel",
				      "rudder acceleration",
				      (long *)&(_rudder->m_params.accel)));

  attributes.add(new IntegerAttribute("rudderHoldCurrent",
				      "rudder hold current",
				      (long *)&(_rudder->m_params.holdcur)));
  
  attributes.add(new IntegerAttribute("rudderRunCurrent",
				      "rudder run current",
				      (long *)&(_rudder->m_params.runcur)));

  attributes.add(new IntegerAttribute("rudderSpeed",
				      "rudder speed",
				      (long *)&(_rudder->m_params.speed)));

  attributes.add(new IntegerAttribute("rudderMinSpeed",
				      "rudder minimum speed",
				      (long *)&(_rudder->m_params.minspeed)));
  
  attributes.add(new IntegerAttribute("rudderHome",
				      "home pot val",
				      &_rudderHome));    

  attributes.add(new IntegerAttribute("rudderPot1CntsToSteps",
				      "pot to step scale factor",
				      &_rudderCntsToSteps));    
  
  attributes.add(new IntegerAttribute("elevator_board_num",
				    "Board number of the stp100 elevator controller",
				      (long *)&(_elevator->m_nNodeNum)));    

  attributes.add(new FloatAttribute("elevator_L1",
				    "Elevator Actuator Parameter L1",
				    &_eL1,  0.35));      // meters

  attributes.add(new FloatAttribute("elevator_L2",
				    "Elevator Actuator Parameter L2",
				    &_eL2,  0.076));     // meters

  attributes.add(new FloatAttribute("elevator_R0",
				    "Elevator Actuator Parameter R0",
				    &_eR0,  0.311));     // meters

  attributes.add(new FloatAttribute("elevator_Theta_zero",
				    "Elevator Actuator Parameter theta_zero",
				    &_eTheta0,  0.936));   // meters

  attributes.add(new FloatAttribute("maxElevatorAngle",
				    "Maximum Elevator Angle",
				    &_maxElevatorAngle, 0.262));   // radians

  attributes.add(new FloatAttribute("minElevatorAngle",
				    "Minimum Elevator Angle",
				    &_minElevatorAngle, -0.262));  // radians

  attributes.add(new IntegerAttribute("elevatorAccel",
				      "elevator acceleration",
				      (long *)&(_elevator->m_params.accel)));

  attributes.add(new IntegerAttribute("elevatorHoldCurrent",
				      "elevator hold current",
				     (long *)&(_elevator->m_params.holdcur)));
  
  attributes.add(new IntegerAttribute("elevatorRunCurrent",
				      "elevator run current",
				      (long *)&(_elevator->m_params.runcur)));
  

  attributes.add(new IntegerAttribute("elevatorMinSpeed",
				      "elevator minimum speed",
			      (long *)&(_elevator->m_params.minspeed)));

  attributes.add(new IntegerAttribute("elevatorSpeed",
				      "elevator speed",
				      (long *)&(_elevator->m_params.speed)));
  
  attributes.add(new IntegerAttribute("elevatorHome",
				      "elevator home",
				      &_elevatorHome));    

  attributes.add(new IntegerAttribute("elevatorPot1CntsToSteps",
				     "elevator pot counts to steps",
				      &_elevatorCntsToSteps));    


  attributes.add(new IntegerAttribute("propeller_board_num",
				    "Board number of the sv203 propeller controller",
				      (long *)(&_propeller->m_nNodeNum)));    

  attributes.add(new FloatAttribute("propAccel",
				      "prop acceleration",
				      &(_propeller->m_params.accel)));


  attributes.add(new IntegerAttribute("propDeadband",
				      "propeller deadband",
				      (long *)&(_propeller->m_params.deadband)));    

  attributes.add(new FloatAttribute("propPitch",
				    "Dist traveled per radian of prop turn",
				    &_propPitch));

  attributes.add(new IntegerAttribute("propKp",
				      "",
				      (long *)&(_propeller->m_params.Kp)));

  attributes.add(new IntegerAttribute("propKi",
				      "",
				      (long *)&(_propeller->m_params.Ki)));

  attributes.add(new IntegerAttribute("propIntLimit",
				      "",
				      (long *)&(_propeller->m_params.Ilim)));

  attributes.add(new IntegerAttribute("propKd",
				      "",
				      (long *)&(_propeller->m_params.Kd)));

  attributes.add(new IntegerAttribute("propErrorLimit",
				      "",
				      (long *)&(_propeller->m_params.pos_err_lim)));

  attributes.add(new IntegerAttribute("propPWMLimit",
				      "",
				      (long *)&(_propeller->m_params.pwm_lim)));

  attributes.add(new IntegerAttribute("propServoRate",
				      "",
				      (long *)&(_propeller->m_params.rate)));

  attributes.add(new IntegerAttribute("logFrequency",
				      "Read and log telemetry every nth "
				      "command",
				      &_logFrequency, 1));

  attributes.add(new BooleanAttribute("simulated",
				      "",
				       &_isSimulated,
				       False ) );

  attributes.add(new BooleanAttribute("debug",
				      "",
				      &debug) );

  AttributeParser::parse(System::configurationFile("doradoTailCone.cfg"),
			 &attributes, debug);


  _eL1Sqrd = _eL1*_eL1;
  _eL2Sqrd = _eL2*_eL2;
  _rL1Sqrd = _rL1*_rL1;
  _rL2Sqrd = _rL2*_rL2;

}