///////////////////////////////////////////////////////////////
//ManualControl.cc
//
//Author:     Amanda Green
//Date:       7/12/00
//Version:    2
//
//Purpose:    As of 7/20/00, the program runs successfully 
///////////////////////////////////////////////////////////////

#include "ManualControl.h"
#include "DeviceIF.h"
#include "StreamSerialDriver.h"
#include "SerialDevice.h"
#include "SerialParameters.h"
#include "Attributes.h"
#include "FloatAttribute.h"
#include <string.h>
#include <stdio.h>
#include <iostream.h>
#include <stdlib.h>
#include <math.h>



#define MaxRecordBytes 50
#define READ_TIMEOUT 2000

ManualControl::ManualControl(SerialDevice *device, SerialParameters *serialParams) : StreamSerialDriver("agDriver", device, MaxRecordBytes, "*", READ_TIMEOUT)
{
       _device = device;
       _serialParams = serialParams;

	//CUT FROM AGTC constructor///////////////////////////////////////
  	char errorBuf[100];
  	const char *driverProgram = "tailConeServer";

        //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];
        //setting as ser2 for now
        sprintf(argString, "-dev %s", "/dev/ser2");
        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");


	// Get tailcone deadman timeout (seconds)
	_agTailCone->setDeadman(500);
	_agTailCone->deadman(&timeout);
	// Subtract a second from deadman, so that we timeout before
	// deadman kicks in
	timeout--;

	// Now set "read controller" timeout in milliseconds
	setReadTimeout(timeout);
}


ManualControl::~ManualControl()
{
	//CUT FROM AGTC DESTRUCTOR////////////////////////////////////////
	//not sure if diable thruster should go here...
	_agTailCone->disableThruster();
  	delete _agTailCone;
  	kill(_tailConeServer, SIGTERM);
  	kill(_vehicleConfigServer, SIGTERM);
  	//END CUT FROM AGTC DESTRUCTOR////////////////////////////////////
}

DeviceIF::Status ManualControl::initialize()
{
        char buf[30];

        _device->commsDebugMode(SerialDevice::DebugOff);

        //initialize serial port
        _device->setLineFormat(_serialParams);
	  //_device->nonRaw();

        //clear serial port
        //probably not necessary
        _device->clearPort();
        _device->flushReceiveBuf();

        return DeviceIF::Ok;
}

void ManualControl::handleReadTimeout()
{
  // Link to manual controller timed out; set prop speed to 0
  _agTailCone->setPropellerSpeed(0.);
}


DeviceIF::Status ManualControl::processRecord(unsigned char *record, int nRecordBytes)
{
	Boolean debug = False;
	char *reply = (char *)record;

	//variable declaration
	float mode = 1.0, propeller, rudder, elevator, checksum;
	
	//"_" represents break between each number, and "*" signifies
	//the end of the packet
	char seps[]   = "_*";
	char *token;
  	
	// Establish string and get the first token: 
	token = strtok( reply, seps );

      reply = 0;
	float x = 0.0; 
	int i = 0;

	while( token != NULL )
	{
	     // While there are tokens in "reply" 

	     //change the value to float
	     x = atof(token);

	     switch(i)
	     {
		case 0:
		    mode = x;
		    break;
		case 1:
		    propeller = x;
		    break;
		case 2:
		    elevator = x;
		    break;
		case 3:
		    rudder = x;
		    break;
	 	case 4:
		    checksum = x;
		    break;
	     }

	     i++;

	     // Get next token: 
	     token = strtok( NULL, seps );
        }

	//Do Stuff With the Data///////////////////////
	//Print data coming from joystick to screen to know that data is 
	//indeed coming in from joystick
	cout<<mode<<" "<<propeller<<" "<<elevator<<" "<<rudder<<" "<<checksum<<endl;


	//CUT FROM AGTC run////////////////////////////////
	
	//if checksum adds up, and within range, manipulate values and input
	//into tailcone
	//Range checking will soon be implemented in the tailcone prog.
	//itself, and will not need to be included here.
	if(checksum == (mode + fabs(propeller) + fabs(rudder) + fabs(elevator)))
	{
        	//change values to degrees/sec
        	//1 rotation/min = 6 degrees/second
        	propeller = propeller*6.0;
  
		//change values to degrees (from hundredths of degs)
 	      rudder = rudder/100.0;
        	elevator = elevator/100.0;

		_agTailCone->setDeadman(15000);
        	//send commands to tailcone
            _agTailCone->command(propeller*Math::RadsPerDeg, 
				 elevator*Math::RadsPerDeg, 
				 rudder*Math::RadsPerDeg);
		
		if(mode == 0)
	 	{
		 	exit(1);	
	 	}

	 }else {
			//If the values are not in range or checksum does not add
			//print out warning, and send 0,0,0 to tailcone.
			cout<<"Checksum/range error\n";
			_agTailCone->setPropellerSpeed(0);
            	_agTailCone->setRudder(0);
            	_agTailCone->setElevator(0); 
	}

	
	

        //END CUT FROM AGTC run////////////////////////////////////////////////
    
        return DeviceIF::Ok;
   
}//End processRecord//////////////////////////////////////////


//MAIN PROGRAM/////////////////////////////////////////////////
int main(int argc, char **argv)
{
        SerialDevice *device;
        ManualControl *test;

        try 
	{
             SerialParameters serialParams(&argc, argv);
             device = new SerialDevice(&serialParams);
             test = new ManualControl(device, &serialParams);
             test->run();
        }
        catch (Exception e) {
             fprintf(stderr, "Caught Exception: %s\n", e.msg);
        }

        return 0;
}