//////////////////////////////////////////////////////////////////////
//Filename:     AGTailConeTest.cc
//Author:       Amanda Green
//Date:         7/7/00
//
//Description:  First attempt at talking to the tailcone.
//
//Currently, the rudder and elevator are working
//On 6/26, I commented out all range checking... have yet to see if it works
//on 6/26, changed "mode" to int 0 or 1.
//On 7/7: Set up to print Propeller values being sent to tailcone to 
//        screen first, in order to see if sending funky data.
//to run: AGTailConeTest -dev /dev/ser2
/////////////////////////////////////////////////////////////////////

#include <stdio.h>
#include <iostream.h>
#include <string.h>
#include <errno.h>
#include <stdlib.h>
#include <process.h>
#include <signal.h>
#include "TailConeIF.h"
#include "Exception.h"
#include "StringConverter.h"
#include "MathP.h"
#include "System.h"
#include "Task.h"
#include "Syslog.h"
#include "SerialParameters.h"

class AGTailConeTest : public Task{

public:

  AGTailConeTest(SerialParameters *serialParams);
  virtual ~AGTailConeTest();

  virtual void run();

protected: 

  TailConeIF *_agTailCone;
  pid_t _tailConeServer;
  pid_t _vehicleConfigServer;

};


//constructor//////////////////////////////////////////////////////
AGTailConeTest::AGTailConeTest(SerialParameters *serialParams):Task("tailConeTest")
{
  char errorBuf[100];
  const char *driverProgram = "odysseyTailCone";

  //Start vehicle configuration server (needed by tailcone server)
  if ((_vehicleConfigServer = System::spawn("vehicleConfigurationServer", "")) == -1)
  {
    throw Exception("System::spawn(vehicleConfigurationServer) failed");
  }

  //Start tailcone Server
  char argString[100];
  sprintf(argString, "-dev %s", serialParams->portName);
  if ((_tailConeServer = System::spawn(driverProgram, argString)) == -1)
    sprintf(errorBuf, "System::spawn(%s) failed", driverProgram);

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

  fprintf(stderr, "Initialize tailcone\n");
  if (_agTailCone->initialize() != TailConeIF::Ok)
    fprintf(stderr, "Tailcone initialization error\n");

  fprintf(stderr, "Ready to accept commands\n");
}//end constructor

//destructor//////////////////////////////////////////
AGTailConeTest::~AGTailConeTest()
{
  delete _agTailCone;
  kill(_tailConeServer, SIGTERM);
  kill(_vehicleConfigServer, SIGTERM);
}//end destructor


//run()///////////////////////////////////////////////
void AGTailConeTest::run()
{
  //initialize variables
  float inMode, inPropeller, inRudder, inElevator;

  do{
    //ask for input
    //eventually will be from serial connection
    printf("Enter mode: ");
    scanf("%f", &inMode);

    printf("\nEnter propeller speed (rpm): ");
    scanf("%f", &inPropeller);

    printf("\nEnter rudder: ");
    scanf("%f", &inRudder);
    
    printf("\nEnter elevator: ");
    scanf("%f", &inElevator);

    //check checksum

    //check ranges
    if((inPropeller > 300.0)||(inPropeller<-300.0))
      continue;
    if((inElevator > 1000.0)||(inElevator<-1000.0))
      continue;
    if((inRudder > 1000.0)||(inRudder<-1000.0))
      continue;
    //change values to degrees
    //1 rotation/min = 6 degrees/second
    inPropeller = inPropeller*6.0;
  
    inRudder = inRudder/100.0;
    cout<< inRudder<<endl;
    inElevator = inElevator/100.0;
    cout<< inElevator<<endl;

    //inPropeller = inPropeller*Math::RadsPerDeg;
    //inRudder = inRudder*Math::RadsPerDeg;
    //inElevator = inElevator*Math::RadsPerDeg;

    cout<<"Prop speed in rad per sec: "<<inPropeller<<endl;
    cout<<"Rudder in radians: "<<inRudder<<endl;
    cout<<"Elevator in radians: "<<inElevator<<endl;

    //send commands to tailcone
    //_agTailCone->setPropellerSpeed(inPropeller);
    //_agTailCone->setRudder(inRudder);
    //_agTailCone->setElevator(inElevator);
    _agTailCone->command(inPropeller*Math::RadsPerDeg, inElevator*Math::RadsPerDeg, inRudder*Math::RadsPerDeg);
   
  
  //repeat until mode = 1
  }while (inMode==1);

  _agTailCone->disableThruster();

}//end run()//////////////////////////////////////////////////////////////


//main///////////////////////////////////////////////////////////////////
int main(int argc, char **argv)
{ 
  Boolean debug = True;
  Boolean error = False;
  AGTailConeTest *test = 0;
  
  try {
    SerialParameters serialParams(&argc, argv);
    test = new AGTailConeTest(&serialParams);
    test->run();
  }
  
  catch (Exception e) {
    fprintf(stderr, "Caught exception: %s\n", e.msg);
    error = True;
  }

  catch (...) {
    fprintf(stderr, "Caught some exception...");
    error = True;
  }

  dprintf("delete AGTailConeTest object!");
  delete test;

  if (error)
    return 1;
  else 
    return 0;
}//end main///////////////////////////////////////////////////////