/****************************************************************************/
/* 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 "TailConeIF.h"
#include "Exception.h"
#include "StringConverter.h"
#include "Math.h"
#include "System.h"
#include "Task.h"
#include "Syslog.h"
#include "SerialParameters.h"

class TailConeTest : public Task {

public:

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

  virtual void run();


protected:

  enum CommandType {
    Elevator,
    Rudder,
    Propeller
  };

  TailConeIF *_tailCone;
  pid_t _tailConeServer;
  pid_t _vehicleConfigServer;
};


TailConeTest::TailConeTest(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);
  }

  _tailCone = new TailConeIF("tailCone");

  _tailCone->enableThruster();

  fprintf(stderr, "Initialize tailcone\n");
  if (_tailCone->initialize() != TailConeIF::Ok) {
    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;
  double value;
  struct timespec sleepTime;
  const int sleepMillisecs = NavigationPeriodMillisec;

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

  while (True) {

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

      continue;
    }

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

    if (!token)
      continue;

    if (!strncmp(token, "r", 1)) {
      // Rudder command
      commandType = Rudder;
    }
    else if (!strncmp(buf, "e", 1)) {
      // Elevator command
      commandType = Elevator;
    }
    else if (!strncmp(buf, "p", 1)) {
      // Propeller command
      commandType = Propeller;
    }
    else if (!strncmp(buf, "i", 1)) {
      _tailCone->initialize();
      continue;
    }
    else if (!strncmp(buf, "q", 1)) {
      // Quit
      break;
    }
    else {
      fprintf(stderr, "Unknown command: %s\n", buf);

      fprintf(stderr, "Valid commands:\n");
      fprintf(stderr, "'e value' - set elevator value (deg)\n");
      fprintf(stderr, "'r value' - set rudder value (deg)\n");
      fprintf(stderr, "'p value' - set propeller value (rpm)\n");
      fprintf(stderr, "'i' - initialize (assumes fins at zero degrees)");

      fprintf(stderr, "'q' - quit\n");
      continue;
    }

    char *valuePtr;

    if (strlen(token) > 1) {
      // Value is concatenated
      valuePtr = &token[1];
    }

    else if ((valuePtr = strtok(0, " \t")) == 0) {
      // Expecting numerical value
    }

    if (!StringConverter::isFloat(valuePtr)) {

      fprintf(stderr, 
	      "Invalid value: \"%s\" - must be floating point number\n",
	      valuePtr);

      continue;
    }

    value = atof(valuePtr);

    switch (commandType) {
    case Elevator:

      fprintf(stderr, "Setting elevator to %.2f deg (%.5f rad)...",
	      value, value * Math::RadsPerDeg);

      _tailCone->setElevator(value * Math::RadsPerDeg);
      break;

    case Rudder:

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

      _tailCone->setRudder(value * Math::RadsPerDeg);
      break;

    case Propeller:

      fprintf(stderr, 
	      "Setting propeller speed to %.2f rpm (%.5f rad/sec)\n",
	      value, value * 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(value * Math::RpmToRadps);
	nanosleep(&sleepTime, 0);
      }

      // fprintf(stderr, "Thruster watchdog will timeout in a few seconds...");
      //_tailCone->setPropellerSpeed(value * Math::RadsPerDeg);
      break;
    }
    fprintf(stderr, " Ok\n");
  }

  _tailCone->disableThruster();
}



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

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

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