/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : OdysseyFin.cc                                                 */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <ctype.h>
#include <errno.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>       
#include <time.h>
#include "OdysseyFin.h"
#include "Math.h"
#include "Syslog.h"
#include "Time.h"

double OdysseyFin::_elevatorHallCoeffs[4] = {0., 0., 0., 0.};
double OdysseyFin::_rudderHallCoeffs[4] = {0., 0., 0., 0.};

OdysseyFin::OdysseyFin(const char *name, SerialDevice *serialDevice, 
		       FinID finID,
		       double encoderCtsPerRad,
		       Boolean rezero)
{
  double *hallCoeffs;

  _rezero = rezero;
  _encoderCtsPerRadian = encoderCtsPerRad;
  _sailDevice =  new SailDevice(name, serialDevice, '\n');

  if (finID == ElevatorID) {
    _cmdPrefix = "#FE";
    hallCoeffs = _elevatorHallCoeffs;
  }
  else if (finID == RudderID) {
    _cmdPrefix = "#FR";
    hallCoeffs = _rudderHallCoeffs;
  }
  else {
    // Should never get here
    Syslog::write("Invalid FinID: %d", finID);
    exit(1);
  }
  
  _zeroHallAngle = 0.;

  for (int i = 0; i < 4; i++) {
    _hallCoeffs[i] = hallCoeffs[i];
  }

}


OdysseyFin::~OdysseyFin()
{
}


TailConeIF::Status OdysseyFin::initialize()
{
  char command[8];
  int unused;

  if (deglitch() == -1) {
    sprintf( _eventMsg, "unable to deglitch %s\n", _sailDevice->name());
    Syslog::write("%s", _eventMsg);
    return DeviceIF::Initializing;
  }

  if (_rezero && reZero(2) == -1 ) {    
    sprintf( _eventMsg, "unable to index %s\n", _sailDevice->name() );
    Syslog::write("%s", _eventMsg);
    return DeviceIF::Initializing;
  }

  int encoderCts;
  int hallCts;

  if ( control(0.0) == -1 )
    return DeviceIF::Initializing;

    Syslog::write("\tindexing %s ... ", _sailDevice->name() );

  sleep( 5 );
  Syslog::write("done\n");

  return TailConeIF::Ok;
}


int OdysseyFin::deglitch()
{
  char command[8];
  sprintf(command,"%sG", _cmdPrefix);
  _sailDevice->write(command,strlen(command));

  // Reply is exact echo of input
  try {
    _sailDevice->readUntil(command);
  }
  catch (Exception e) {

    Syslog::write("OdysseyFin::deglitch() - bad juju reading reply!\n%s",
		  e.msg);

    return -1;
  }

  return 1;
}


void OdysseyFin::buildCommand(double angle, char *commandBuf)
{
  Boolean debug = False;
  int pos;

  pos = _encoderCtsPerRadian * (angle - _zeroHallAngle);
  dprintf("OdysseyFin::buildCommand() - angle: %.4f, angle0: %.4f, pos: %d",
	  angle, _zeroHallAngle, pos);

  if ( pos < 0 )
    pos += 0xffffff;

  sprintf(commandBuf, "%sL%06X", _cmdPrefix, pos);

  return;
}


TailConeIF::Status OdysseyFin::control(double angle)
{
  Boolean debug = False;

  char command[16];

  buildCommand(angle, command);

  dprintf("OdysseyFin::control() - write command \"%s\"", command);

  _sailDevice->write(command, strlen(command));

  /* Reply format of 000000 7C 59 where 000000 is current location and 7C
     is the hall effect value and 59 is the FET block temperature */

  /* Read chars into a buf until either an ETX (ASCII 3) or timeout
     expires.  Parse buf to get location, he, and temp values */

  char reply[256];

  int nBytes;

  try {
    nBytes = _sailDevice->readUntil(reply, sizeof(reply), "\x03", 1000);
  }
  catch (SerialDevice::TimedOut e) {
    Syslog::write("OdysseyFin::control() - timed out reading serial port");
    return DeviceIF::Error;
  }
  catch (...) {
    Syslog::write("OdysseyFin::control() - bad juju reading reply!");
    return DeviceIF::Error;
  }

  /* check for glitch */
  for (int i = 0; i < nBytes; i++) {

    if ( reply[i] == 'X' ) {
      sprintf( _eventMsg, "%s glitch ... \n", _sailDevice->name() );
      Syslog::write(_eventMsg);
      //      return TailConeIF::FinGlitch;
      return DeviceIF::Error;
    }
  }

  /* check length of string */
  if ( nBytes < 14 ) {

    sprintf( _eventMsg, "%s short reply (%s)\n", 
	    _sailDevice->name(), reply );

    Syslog::write(_eventMsg);
    return TailConeIF::Error;
  }

  char c;
  int n = sscanf( reply, "%x %x%c", &_encoderCts, &_hallCts, &c );

  if ( n > 2 && isspace(c) ) {
    if ( _encoderCts > 0x800000 )
      _encoderCts -= 0xffffff;

    return TailConeIF::Ok;
  }
  else
    return TailConeIF::Error;
}


int OdysseyFin::reZero(int tries)
{
  Boolean debug = False;
  const int sleepMillisecs = 100;

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

  char command[8];
  char buf[100];
  char reply[32];
  int nBytes;

  int rawHE1, rawHE2, rawHE3;

  while ( tries-- > 0 ) {

    if ( checkGlitch() == -1) {
      sprintf( _eventMsg, "%s glitch reset failure, %d %s left\n",
	      _sailDevice->name(), tries, (tries == 1 ? "try" : "tries") );
      Syslog::write(_eventMsg);
      continue;
    }

    if ( getHall(&rawHE1) == -1 ) {
      sprintf( _eventMsg, "first %s HE query failed, %d %s left\n",
	      _sailDevice->name(), tries, (tries == 1 ? "try" : "tries") );
      Syslog::write(_eventMsg);
      continue;
    }

    // Wait a bit, then try again
    nanosleep(&sleepTime, 0);

    if ( getHall(&rawHE2) == -1 ) {
      sprintf( _eventMsg, "second %s HE query failed, %d %s left\n",
	      _sailDevice->name(), tries, (tries == 1 ? "try" : "tries") );

      Syslog::write(_eventMsg);
      continue;
    }

    /* make 3rd query if disagree */
    if ( abs(rawHE2 - rawHE1) > 1 ) {
      // Wait a bit, then try again
      nanosleep(&sleepTime, 0);

      if ( getHall(&rawHE3) == -1 ) {
	sprintf( _eventMsg, "third %s HE query failed, %d %s left\n",
		_sailDevice->name(), tries, (tries == 1 ? "try" : 
					     "tries") );
	Syslog::write(_eventMsg);
	continue;
      }

      /* 2 & 3 agree, use one of them */
      if ( abs(rawHE3 - rawHE2) <= 1 )
	rawHE1 = rawHE2;
      else {
	/* 2 & 3 disagree, make sure 3 & 1 do */
	if ( abs(rawHE3 - rawHE1) > 1 ) {
	  sprintf( _eventMsg, "bad triple %s HE, %d %s left\n",
		  _sailDevice->name(), tries, ( tries == 1 ? "try" :
					       "tries" ) );
	  Syslog::write(_eventMsg);
	  continue;
	}
      }
    }
    sprintf( _eventMsg, "\tzero changed from %.1f to ", _zeroHallAngle );
    _zeroHallAngle = hallAngle(rawHE1);
    sprintf( _eventMsg + strlen(_eventMsg), "%.1f deg\n", _zeroHallAngle );
    Syslog::write(_eventMsg);
    
    /* Rezero the 6811 */
    sprintf( command, "%sZ", _cmdPrefix );
    _sailDevice->write(command, strlen(command));

    // Reply terminated by ETX
    try {
      _sailDevice->readUntil("\x03");
    }
    catch (SerialDevice::TimedOut e) {
      sprintf( _eventMsg, "unable to zero %s, %d %s left\n",
	      _sailDevice->name(), tries, ( tries == 1 ? "try" : "tries" ) );

      Syslog::write(_eventMsg);
      continue;
    }
    catch (...) {
      Syslog::write("OdysseyFin::reZero() - bad juju while rezeroing 6811!");
      return -1;
    }

    return 0;

  }
  return -1;                        /* this signifies error */
}



int OdysseyFin::checkGlitch()
{
  char command[8];
  char reply[16];

  sprintf( command, "%s\n", _cmdPrefix );
  _sailDevice->write(command, strlen(command) );
  // Reply terminated by LF, or "X:"

  if (_sailDevice->read(reply, sizeof(reply)) == -1) {
    Syslog::write("OdysseyFin::checkGlitch() - read reply failed");
    return -1;
  }
  
  if (!strcmp(reply, "X:")) {

    sprintf( _eventMsg, "\n% ******* %s glitch *******\n\n",
	    _sailDevice->name());
    
    Syslog::write(_eventMsg);

    return resetGlitch();
  }
  return 0;
}


int OdysseyFin::getHall( int *rawHE )
{
  Boolean debug = False;
  int nBytes;
  char c, command[8], *reply, buf[128];
  sprintf(command, "%s?V", _cmdPrefix);
  _sailDevice->write(command, strlen(command));

  // Reply terminated by ETX
  try {
    nBytes = _sailDevice->readUntil(buf, sizeof(buf), "\x03");
  }
  catch (SerialDevice::TimedOut e) {
    Syslog::write("OdysseyFin::getHall() - reply timed out");
    return -1;
  }
  catch (...) {
    Syslog::write("OdysseyFin::getHall() - bad juju in reply");
    return -1;
  }

  dprintf("OdysseyFin::getHall() - %d bytes in reply: %s", nBytes, buf);

  if ( !(reply = strrchr(buf, 'V')) ) {
    sprintf( _eventMsg, 
	    "OdysseyFin::getHall() - bad reply to %s's \"%s\" command: %s\n", 
	    _sailDevice->name(), command, buf );

    Syslog::write(_eventMsg);
    return -1;
  }

  if ( sscanf(reply, "V %*x %*x %x%c", rawHE, &c) < 2 || !isspace(c) ) {
    sprintf( _eventMsg, 
	    "OdysseyFin::getHall() - error parsing reply \"%s\" from %s\n", 
	    reply, _sailDevice->name());

    Syslog::write(_eventMsg);
    return -1;
  }
  sprintf( _eventMsg, "%s rawHE 0x%x\n", _sailDevice->name(), *rawHE );
  Syslog::write(_eventMsg);

  return 0;                        /* no error detected */
}


double OdysseyFin::hallAngle( int raw )
{
  double deg = 
    ((_hallCoeffs[3]*raw + _hallCoeffs[2])*raw + _hallCoeffs[1])*raw + 
      _hallCoeffs[0];

  return (deg * Math::RadsPerDeg);
}


double OdysseyFin::encoderAngle()
{
  return (_encoderCts / _encoderCtsPerRadian);
}


double OdysseyFin::hallAngle()
{
  return hallAngle(_hallCts);
}


int OdysseyFin::resetGlitch()
{
  const int sleepMillisecs = 1000;

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

  char command[8];

  sprintf(command, "%sG", _cmdPrefix);
  
  _sailDevice->write(command, strlen(command) );
  // Reply terminated by '\0'?

  // Wait a bit, then confirm
  nanosleep(&sleepTime, 0);

  try {
    _sailDevice->readUntil(":\x3", SailTimeout);
  }
  catch (SerialDevice::TimedOut e) {

    sprintf( _eventMsg, "reset %s glitch: no reply from %s\n",
	    _sailDevice->name(), command);

    Syslog::write(_eventMsg);

    return -1;
  }
  catch (...) {
    Syslog::write("OdysseyFin::resetGlitch() - bad juju from %s cmd",
		  command);
    return -1;
  }

  return 0;
}