/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : tailConeTest.cc                                               */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <stdlib.h>
#include <process.h>
#include <signal.h>
#include <stdlib.h>
#include <conio.h>
#include <time.h>
#include "DoradoTailConeIF.h"
#include "Exception.h"
#include "StringConverter.h"
#include "MathP.h"
#include "System.h"
#include "Task.h"
#include "Syslog.h"
#include "System.h"

class TailConeTest : public Task {


public:

  TailConeTest(int argc, char **argv);
  virtual ~TailConeTest();

  virtual void run();


  struct Options {

    Options() {
      serverProgram = 0;
      serialPort = 0;
    }

    ~Options() {
      free((void *)serverProgram);
      free((void *)serialPort);
    }

    char *serverProgram;
    char *serialPort;
  };

protected:

  enum CommandType {
    Rudder,
    RudderCounts,
    Elevator,
    ElevatorCounts,
    Propeller,
    Tach,
    Pontech,
    DeadMan,
    FullCommand
  };

  DoradoTailConeIF *_tailCone;
  pid_t _tailConeServer;
  pid_t _vehicleConfigServer;
};


TailConeTest::TailConeTest(int argc, char **argv) 
  : Task("tailConeTest")
{
  char errorBuf[100];

  char *serverProgram = "newTailConeServer";

  // Don't specify by default
  char *serialPort = strdup("");

  Boolean error = False;

  // Get options from command line
  for (int i = 1; i < argc; i++) {

    if (!strcmp(argv[i], "-dev") && i < argc - 1) {
      free((void *)serialPort);
      serialPort = strdup(argv[++i]);
    }
    else {
      fprintf(stderr, "Invalid argument: %s\n", argv[i]);
      error = True;
    }
  }

  if (!serverProgram || !serialPort) {
    // Error; must specify serial port and server program
    error = True;
  }

  if (error) {
    fprintf(stderr, "Usage: %s [-dev device]\n", argv[0]);
    exit(1);
  }

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

  Boolean debug = True;
  dprintf("spawn: %s %s\n", serverProgram, serialPort);

  // Start tailcone server
  char argString[100];
  sprintf(argString, "-dev %s", serialPort);
  printf("argstring is %s\n", argString);
  if ((_tailConeServer = System::spawn(serverProgram, argString)) == -1) {
    sprintf(errorBuf, "System::spawn(%s) failed", serverProgram);
  }

  _tailCone = new DoradoTailConeIF("tailCone", TailConeIFServerName);

  _tailCone->enableThruster();

  fprintf(stderr, "Initialize tailcone\n");
  if (_tailCone->initialize() != TailConeIF::Initializing) {
    fprintf(stderr, "Tailcone initialization error\n");
    fprintf(stderr, "But keep going...\n");
  }
  fprintf(stderr, "Ready to accept commands\n");
}


TailConeTest::~TailConeTest()
{
  delete _tailCone;
  kill(_tailConeServer, SIGTERM);
  kill(_vehicleConfigServer, SIGTERM);
}


void TailConeTest::run()
{
  char buf[256];
  CommandType commandType;
  const int sleepMillisecs = NavigationPeriodMillisec;
  struct timespec sleepTime;
  DeviceIF::Status status;

  sleepTime.tv_sec = 0;
  sleepTime.tv_nsec = sleepMillisecs * 1000000;

  long longValue;
  TimeIF::TimeSpec t;

  while (True) {

    if (gets(buf) == 0) {
      // Some error occurred

      continue;
    }

    char *token = strtok(buf, " \t");

    //
    // If strtok returns a null pointer it means that buf is empty.  Jump to 
    // the bottom of the while loop and try again.
    //
    if (!token)                          //if(NULL)
      continue;

    if (!strcmp(token, "r" )) {
      // Rudder command
      commandType = Rudder;
    }
    else if (!strcmp(token, "rc" )) {
      // Rudder command
      commandType = RudderCounts;
    }
    else if (!strcmp(token, "e" )) {
      // Elevator command
      commandType = Elevator;
    }
    else if (!strcmp(token, "ec" )) {
      // Elevator command
      commandType = ElevatorCounts;
    }
    else if (!strcmp(token, "p" )) {
      // Propeller command
      commandType = Propeller;
    }
    else if (!strcmp(token, "pc" )) {
      // Pontech counts: pass directly to the motor controller board
      commandType = Pontech;
    }
    else if (!strcmp(token, "a")) {
      // Full-Monty command
      commandType = FullCommand;
    }
    else if (!strcmp(token, "d")) {
      commandType = DeadMan;
    }

    else if (!strcmp(token, "c" )) {
      short elevatorCurrent, rudderCurrent;
      short motorCurrent1, motorCurrent2;
      _tailCone->currents(&elevatorCurrent, &rudderCurrent,
			  &motorCurrent1, &motorCurrent2, &t);

      fprintf(stderr, "Currents (mA): elevator=%d, rudder=%d\n",
	      elevatorCurrent, rudderCurrent);

      fprintf(stderr, "               motor1=%d, motor2=%d\n",
	      motorCurrent1, motorCurrent2);

      continue;
    }
    else if (!strcmp(token, "t" )) {
      // Read the tachometer at 5 Hz
      double speed[1];
      double elevator, rudder;
      TimeIF::TimeSpec time[1];
      const double GearRatio = 10.;        //Motor-to-prop gear ratio

      fprintf(stderr, "Strike any key to terminate.\n");
      while( !kbhit() )
      {
#if 0
	status = _tailCone->propellerSpeed( speed, time );

	fprintf(stderr, "  %.2f rpm;    %d seconds;  status = %d \n", 
		*speed/Math::RpmToRadps*GearRatio, time->seconds, status);
#endif

	status = _tailCone->actual( speed, &elevator, &rudder, time );

	fprintf(stderr, "  %.2f rpm, elev=%.2f deg, rudder=%.2f;    "
		"%d seconds;  status = %d \n", 
		*speed/Math::RpmToRadps*GearRatio, 
		Math::radToDeg(elevator), Math::radToDeg(rudder),
		time->seconds, status);


	nanosleep(&sleepTime, 0);
      }
      commandType = Tach;
      continue;
    }
    else if (!strcmp(token, "h" )) {

      fprintf(stderr, "HELP:\n");
      fprintf(stderr, "Valid commands are:\n");
      fprintf(stderr, "'c' Read currents in milliAmps\n");
      fprintf(stderr, "'t' Read rpm at 1Hz and display value in rpm.\n");
      fprintf(stderr, "    Type any key to terminate.\n");
      fprintf(stderr, "'i' Initialize. (assumes fins at zero degrees)\n");
      fprintf(stderr, "'q' Quit\n");
      fprintf(stderr, "\nThe following commands require a value.\n");
      fprintf(stderr, "'e  value' - set elevator value (deg)\n");
      fprintf(stderr, "'ec value' - set elevator actuator (counts)\n");
      fprintf(stderr, "'r  value' - set rudder value (deg)\n");
      fprintf(stderr, "'rc value' - set rudder actuator (counts)\n");
      fprintf(stderr, "'p  value' - set propeller value (rpm)\n");
      fprintf(stderr, "    This works ONLY when PDEBUG is 0 in tailcone.c\n");
      fprintf(stderr, "'pc value' - set Pontech Counts; for Debugging.\n");
      fprintf(stderr, "    This works ONLY when PDEBUG is 1 in tailcone.c\n");
      fprintf(stderr, "'a  speed  elev  rudder'- Set veh (m/s), elev (deg),");
      fprintf(stderr, " and rudder (deg).\n"); 
      continue;

    }
    else if (!strcmp(token, "i" )) {
      _tailCone->initialize();
      continue;
    }
    else if (!strcmp(token, "q" )) {
      // Quit
      break;
    }
    else {
      fprintf(stderr, "Unknown command: %s\n", token);
      fprintf(stderr, "Type h for help.\n");
      continue;
    }

    
#define MaxParams 10

    double params[MaxParams];
    int nParams = 0;
    while ((token = strtok(0, " \t")) != 0) {
      if (!StringConverter::isFloat(token)) {
	fprintf(stderr, "Parameters must be floating point/integers\n");
	continue;
      }
      if (nParams >= MaxParams) {
	fprintf(stderr, "Too many parameters!\n");
	continue;
      }
      params[nParams] = atof(token);
      nParams++;
    }

    short timeout;
    short counts;
    int cnt;

    switch (commandType) {

    case Elevator:

      if (nParams != 1) {
	fprintf(stderr, "Parameter required\n");
	continue;
      }
      fprintf(stderr, "Setting elevator to %.2f deg (%.5f rad)\n",
	      params[0], params[0] * Math::RadsPerDeg);

      _tailCone->setElevator(params[0] * Math::RadsPerDeg);
      break;

    case ElevatorCounts:
      if (nParams != 1) {
	fprintf(stderr, "Parameter required\n");
	continue;
      }
      counts = (short) params[0];

      fprintf(stderr, "Setting elevator actuator to %d counts\n",
	      counts);

      _tailCone->setElevator(counts);
      break;

    case Rudder:
      if (nParams != 1) {
	fprintf(stderr, "Parameter required\n");
	continue;
      }

      fprintf(stderr, "Setting rudder to %.2f deg (%.5f rad)...",
	      params[0], params[0] * Math::RadsPerDeg);

      _tailCone->setRudder(params[0] * Math::RadsPerDeg);
      break;

    case RudderCounts:
      if (nParams != 1) {
	fprintf(stderr, "Parameter required\n");
	continue;
      }
      counts = (short) params[0];

      fprintf(stderr, "Setting rudder actuator to %d counts\n",
	      counts);

      _tailCone->setRudder(counts);
      break;

    case Propeller:
      if (nParams != 1) {
	fprintf(stderr, "Parameter required\n");
	continue;
      }

      fprintf(stderr, 
	      "Setting propeller speed to %.2f rpm (%.5f rad/sec)\n",
	      params[0], params[0] * Math::RpmToRadps);
      fprintf(stderr, " Strike any key to resume ...\n");

      while( !kbhit() )
      {
	//
	// TailConeServer expects it's input in rads/sec.  It converts to rpm
	// for the call to SET_MOTOR_SPEED.
	//
	_tailCone->setPropellerSpeed( params[0] * Math::RpmToRadps);
	nanosleep(&sleepTime, 0);
      }
      break;

    case FullCommand:
      if (nParams != 3) {
	fprintf(stderr, "Must supply rpm elevator rudder parameters");
	continue;
      }

      fprintf(stderr, 
	      "Setting propeller speed to %.2f rpm (%.5f rad/sec)\n",
	      params[0], params[0] * Math::RpmToRadps);
      fprintf(stderr, " Strike any key to resume ...\n");

      while( !kbhit() ) {
	//
	//
	if ((status = 
	     _tailCone->command( params[0] * Math::RpmToRadps,
				 params[1] * Math::RadsPerDeg,
				 params[2] * Math::RadsPerDeg)) 
	    != DeviceIF::Ok) {
	  Syslog::write("status %d from _tailCone->command()", status);
	}
	nanosleep(&sleepTime, 0);
      }
      break;

    case DeadMan:
      if (nParams != 1) {
	fprintf(stderr, "Parameter required\n");
	continue;
      }
      timeout = (short) params[0];

      fprintf(stderr, "Setting deadman timeout to %d seconds\n",
	      timeout);

      _tailCone->setDeadman(timeout);

      break;

    case Pontech:
      if (nParams != 1) {
	fprintf(stderr, "Parameter required\n");
	continue;
      }

      fprintf(stderr, 
	      "Setting Pontech to %d counts\n", (long) params[0] );

      _tailCone->setMotorCounts( (long) params[0] ); 
      break;

    case Tach:
      //
      // The calc is above inside the "if" statement.  It's only necessary to
      // have code here if it takes a value.
      //
      break;

    default:
      fprintf(stderr, " Default: Unknown Command.\n");
      break;
    } /*End Switch*/
    fprintf(stderr, " Please enter another command.\n");
  }/*End While */  

   fprintf(stderr, " Exiting tailConeTest\n");
}



int main(int argc, char **argv)
{
  Boolean debug = True;
  Boolean error = False;
  TailConeTest *test = 0;

  try {
    test = new TailConeTest(argc, argv);
    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 TailConeTest object...");
  delete test;

  dprintf("Kill child processes...");
  kill(0, SIGTERM);

  if (error)
    return 1;
  else
    return 0;
}