/*-----------------------------------------------------------------------*
  Copyright (C) 1994-1998, Massachusetts Institute of Technology.
  Proprietary to Sea Grant AUV Laboratory.  All rights reserved.
  $Id: OdysseyThruster.cc,v 1.12 2000/09/05 16:50:09 oreilly Exp $
 *-----------------------------------------------------------------------*/

#include <math.h>               /* ANSI */
#include "Math.h"               /* utils subdirectory */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include "OdysseyThruster.h"
#include "Syslog.h"

#define ResetCmd "#THR:"

OdysseyThruster::OdysseyThruster(SerialDevice *serialDevice)
{
  _sailDevice = new SailDevice("thruster", serialDevice, '\n');

  _initialized = False;

  disable();
}

OdysseyThruster::OdysseyThruster(SerialDevice *serialDevice, int mytimeout)
{

  _sailDevice = new SailDevice("thruster", serialDevice, '\n');
  
  _thrusterTimeout = mytimeout;

  _initialized = False;

  disable();
}

OdysseyThruster::~OdysseyThruster()
{
  delete _sailDevice;
}


DeviceIF::Status OdysseyThruster::initialize(Boolean busMonitor)
{
  Boolean debug = False;

  if (!enabled()) {
    return DeviceIF::Offline;
  }

  _initialized = True;

  /* First send a reset */
  dprintf("OdysseyThruster::initialize() - send reset command");
  if (_sailDevice->write(ResetCmd, strlen(ResetCmd)) == -1) {
    Syslog::write("write() of reset cmd \"%s\" failed", ResetCmd);
    return DeviceIF::Error;
  }

  // Reply just echoes input

  dprintf("OdysseyThruster::initialize() - read reply from reset");
  try {
    _sailDevice->readUntil(_reply, sizeof(_reply), "\x0a",_thrusterTimeout);
  }
  catch (SerialDevice::TimedOut e) {
    Syslog::write("OdysseyThruster::initialize() - "
		  "Timed out reading reply from thruster reset cmd");

    return DeviceIF::Error;
  }
  catch (...) {
    Syslog::write("OdysseyThruster::intialize() - bad juju reading reply "
		  "to thruster reset cmd!");

    return DeviceIF::Error;
  }

  // Looks like this is necessary following reset, before sending more
  // commands - sorry about the kludge!
  sleep(1);

  /* enable control commands either with
     or without sail bus monitor active */
  sprintf( _command, "#TH%c:", busMonitor ? 'B' : 'O' );

  dprintf("OdysseyThruster::initialize() - write cmd %s", _command);

  if (_sailDevice->write(_command, strlen(_command)) == -1) {
    Syslog::write("write of thruster command \"%s\" failed", _command);
    return DeviceIF::Error;
  }

  // Reply terminated with LF
  try {
    _sailDevice->readUntil(_reply, sizeof(_reply), "\x0A",_thrusterTimeout);
  }
  catch (SerialDevice::TimedOut e) {
    Syslog::write("OdysseyThruster::initialize() - "
		  "Timed out reading reply to bus monitor cmd");

    return DeviceIF::Error;
  }
  catch (...) {
    Syslog::write("OdysseyThruster::intialize() - bad juju reading reply "
		  "to bus monitor cmd!");

    return DeviceIF::Error;
  }

  if (strncmp(_reply, _command, strlen(_command)) ||
	      !strstr(_reply, busMonitor ? "*THB" : "*THO")) {

    Syslog::write("OdysseyThruster::initialize() - Unexpected reply to "
		  "thruster command \"%s\":\n%s", _command, _reply);

    return DeviceIF::Error;
  }

  return DeviceIF::Ok;
}


DeviceIF::Status OdysseyThruster::control(double current)
{
  Boolean debug = False;
  char *ptr, direction, busmon, fault;
  int cmd_val, motor_units, motor_current;
  int i, j, nconv, error=0, chksum=0;

  /* transform thruster_current to integer units for motor current reference
     voltage */

  /* I = Kcop*Vref */
  const double Kcop = 3.4773;
                                /* motor_units are 8bits from 0-5V */
  motor_units = current/Kcop * 255/5;
  dprintf("OdysseyThruster::control() - NEW model");

  dprintf("OdysseyThruster::control() - motor_units=%d\n", motor_units);

  /* format command, fwd or rev */
  if ( motor_units )
    sprintf( _command, "#TH%c%02X:", motor_units < 0 ? 'V' : 'F',
	     abs(motor_units) );
  else
    strcpy( _command, "#THD:" );

  /* send command & confirm response */
  dprintf("OdysseyThruster::control() - write command %s", _command);
  if (_sailDevice->write(_command, strlen(_command)) == -1) {
    Syslog::write("write() of thruster command \"%s\" failed", _command);
    return DeviceIF::Error;
  }

  // Reply terminated with LF
  try {
    _sailDevice->readUntil("*TH", _thrusterTimeout);
    _sailDevice->readUntil(_reply, sizeof(_reply), "\x0A",_thrusterTimeout);
  }
  catch (SerialDevice::TimedOut e) {
    Syslog::write("OdysseyThruster::control() - "
		  "Timed out reading reply to cmd \"%s\"", _command);
  }

  //  if (strncmp(_reply, "#THD:*", strlen("#THD:*"))) {
  // Syslog::write("confirmation of %s command failed", _command );
  // return DeviceIF::Error;
  // }

  if ( ptr = strchr(_reply, '\xd') )
    *ptr = '\0';

  nconv = sscanf( _reply, "%*c%c%c%2x %6x %2x %6x:%2x", &fault,
		  &direction, &cmd_val, &_raw16usPeriod,
		  &motor_current, &_rawBatt, &chksum );
                                /* ignoring checksum for now */
  if ( nconv >= 6 )  {

                                /* correct counter rollover */
    if ( _raw16usPeriod >= 0x8000 )
      _raw16usPeriod = 0x10000 - _raw16usPeriod;
                                /* convert period in 16us ticks to motor
                                   rpm and divide by 5.54 gear ratio to get
                                   propeller speed in rpm; then mult. by
                                   PI/30 to cvt to rad/sec. */
    if ( _raw16usPeriod == 0 )
      _raw16usPeriod = HUGE_VAL;
    _propSpeed = (60. / (16e-6*_raw16usPeriod)) / 5.54 * PI / 30.;
                                /* 0.04 amps/count, see above */
    _current = -0.04*motor_current;
    //
    // Flip the sign on propeller speed, and current, depending on the dirn.
    // I've assumed that current should be positive when propSpeed is.
    //
    if ( direction == 'V' )
    {
      _propSpeed *= -1.;
    }
    else
    {
      _current   *= -1.;
    }
  }
  return DeviceIF::Ok;
}


void OdysseyThruster::enable()
{
  _enabled = True;
}


void OdysseyThruster::disable()
{
  Boolean debug = False;
  dprintf("OdysseyThruster::disable()");

  if (_initialized)
    // Stop propeller
    control(0.);

  _enabled = False;
}


Boolean OdysseyThruster::enabled()
{
  return _enabled;
}


double OdysseyThruster::propSpeed()
{
  return _propSpeed;
}


double OdysseyThruster::current()
{
  return _current;
}