#include "PeriodicTask.h"
#include "TailConeIF.h"
#include "MathP.h"
#include "DeviceUtils.h"

class TailConeCommander : public PeriodicTask {

public:

  TailConeCommander(double propOmega, double elevator, double rudder, 
		    int period);

  ~TailConeCommander();

protected:

  void callback();

  TailConeIF *_tailCone;
  double _propOmega;
  double _elevator;
  double _rudder;
};


TailConeCommander::TailConeCommander(double propOmega,
				     double elevator,
				     double rudder,
				     int period)
  : PeriodicTask("tailConeCommander")
{
  _propOmega = propOmega;
  _elevator = elevator;
  _rudder = rudder;

  _tailCone = new TailConeIF("tailCone");
  _tailCone->enableThruster();

  addPeriodicCallback(period, 
		      (CallbackMethod )TailConeCommander::callback);
}


TailConeCommander::~TailConeCommander()
{
  delete _tailCone;
}


void TailConeCommander::callback()
{
  DeviceIF::Status status = _tailCone->command(_propOmega,
					       _elevator,
					       _rudder);

  if (status != DeviceIF::Ok) {
    fprintf(stderr, "TailConeCommander::callback() - status=%s\n", 
	    DeviceUtils::statusMnem(status));
  }
}



int main(int argc, char **argv)
{
  Boolean error = False;
  
  if (argc != 5) {
    fprintf(stderr, "Usage: %s rpm elevator rudder period\n", argv[0]);
    return 1;
  }

  double propOmega = atof(argv[1]) * Math::RpmToRadps;
  double elevator = atof(argv[2]) * Math::RadsPerDeg;
  double rudder = atof(argv[3]) * Math::RadsPerDeg;
  int period = atoi(argv[4]);

  TailConeCommander *commander = 0;

  try {
    commander = new TailConeCommander(propOmega, elevator, rudder, period);
    commander->run();
  }
  catch (Exception e) {
    fprintf(stderr, "%s: caught exception: %s\n", argv[0], e.msg);
  }
  catch (...) {
    fprintf(stderr, "%s: caught some kinda exception\n", argv[0]);
  }

  delete commander;

  return 0;
}