#ifndef _TAILCONEDRIVER_H
#define _TAILCONEDRIVER_H

#include "CStepper.h"
#include "CProp.h"
#include "PeriodicTask.h"
#include "SerialDevice.h"
#include "TailConeInput.h"
#include "TailConeOutput.h"
#include "TailConeLog.h"
#include "TailConeIF.h"


class TailConeDriver : public PeriodicTask {

public:

  float currentSpeed() {
    return _currentSpeed;
  }

  int elevatorActualSteps() {
    return _elevator->pot1Steps();
  }

  double elevatorActualAngle() {
    return elevatorRadians(_elevator->pot1Steps());
  }

  int rudderActualSteps() {
    return _rudder->pot1Steps();
  }

  double rudderActualAngle() {
    return rudderRadians(_rudder->pot1Steps());
  }

  double rudderCmdAngle() {
    return _rudderpos;
  }

  double elevatorCmdAngle() {
    return _elevatorpos;
  }


  float propCurrent() {
    return _propeller->GetCurrent();
  }
#define AMP_STATUS_BITS (0x02|0x04|0x08|0x10)
  int propStatus() {
    return ((_elevator->GetInputs() & AMP_STATUS_BITS) >> 1);
  }

  long throttlePosition() {
	return _throttle_position;
  }

  long propTemp() {
	return _propTemp;
  }

  TailConeDriver(SerialDevice *device,
		 int period);

  virtual ~TailConeDriver();

protected:

  // Called on every cycle of control loop
  void periodicCallback();

  // Called when event is received from server
  void eventCallback(TaskInterface *taskIF, EventCode event);

  //load configuration data
  void loadConfiguration();

  TailConeInput *_input;
  TailConeOutput *_output;
  TailConeLog *_log;

  SerialDevice *_device;
  int _period;
  long _rudderBoardNum, _elevatorBoardNum, _propellerBoardNum;
  long _rudderStepDelay, _rudderMinStepDelay, _rudderHome, _rudderAccel,
    _rudderCntsToSteps;
  long _elevatorStepDelay, _elevatorMinStepDelay, _elevatorHome, _elevatorAccel,
    _elevatorCntsToSteps;
  double _propPitch;
  long _motorKp, _motorKi, _motorKd, _motorKmInv, _IntLimit;
  double _maxDeltaRpm;
  double _maxDeltaThrottle;
  float _goalSpeed, _currentSpeed, _Ierror, _Derror, _Perror;
  float _controlOutput, _pwm;
  long _logFrequency;
  short _deadman, _deadmanCntDown; //msec
  Boolean _isSimulated, debug;

  // Interface to server. Driver subscribes to Initialize, SetDeadman,
  // and other events from server
  TailConeIF *_serverProxy;

  DeviceIF::Status initialize();
  DeviceIF::Status setDeadman(short msec);

  short elevatorSteps(double theta_desired);
  short rudderSteps(double theta_desired);
  void initElevator(), initRudder(), initPropeller();
  void homeElevator(), homeRudder();
  DeviceIF::Status setRudderAngle(double desired_angle);
  DeviceIF::Status setElevatorAngle(double desired_angle);
  DeviceIF::Status setRudderCounts(short counts);
  DeviceIF::Status setElevatorCounts(short counts);
  double elevatorRadians(short steps);
  double rudderRadians(short steps);
  DeviceIF::Status getComputedRudderAngle(double *angle, 
					  TimeIF::TimeSpec *time);
  DeviceIF::Status getComputedElevatorAngle(double *angle, 
					    TimeIF::TimeSpec *time);
  DeviceIF::Status getRudderAngle(double *angle, 
				  TimeIF::TimeSpec *time);
  DeviceIF::Status getElevatorAngle(double *angle, 
				    TimeIF::TimeSpec *time);

  //control loop functions
  DeviceIF::Status rudderControlLoop(double desired_angle);
  DeviceIF::Status elevatorControlLoop(double desired_angle);
  DeviceIF::Status propellerControlLoop(double propOmega);
  DeviceIF::Status setPropellerSpeed(double speed);
  DeviceIF::Status setPropellerThrottle(short pos);
  DeviceIF::Status getPropellerThrottle(short *throttlepos);
  DeviceIF::Status getPropellerRpm(short *rpm);
  DeviceIF::Status getPropellerSpeed(double *radsPerSec);
  double TailConeDriver::Omega(short rpm);
  int Rpm(double omega);

  //tailcone state
  float _rudderpos, _elevatorpos;
  short _ruddersteps, _elevatorsteps;
  long _maxElevatorCounts, _maxRudderCounts;
  long _minElevatorCounts, _minRudderCounts;
  double _maxElevatorAngle, _maxRudderAngle; 
  double _minElevatorAngle, _minRudderAngle;
  long _minThrottlePos, _maxThrottlePos;
  long _throttle_position;
  int _propTemp;
  short _commandCount, _count;
  Boolean _propellerEnabled;
  Boolean propellerEnabled() {
    return _propellerEnabled;
  }

  void enablePropeller() {
    _propellerEnabled = True;
  }
  void disablePropeller() {
    _propellerEnabled = False;
  }

  //device handles for rudder, elevator & prop
  CPicServo *_picservo;
  CStepper *_elevator, *_rudder;
  CProp *_propeller;

  //tailcone kinematic parameters
  double _eL1, _eL1Sqrd, _eL2, _eL2Sqrd, _eR0, _eTheta0;
  double _rL1, _rL1Sqrd, _rL2, _rL2Sqrd, _rR0, _rTheta0;

};


#endif