/****************************************************************************/
/* Copyright (c) 2004 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : BenthosModem.cc                                               */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 10/01/2004                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <unistd.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/uio.h>
#include <sys/socket.h>
#include <sys/select.h>
#include <i86.h>
#include <fcntl.h>
#include <errno.h>
#include <stdlib.h>
#include <iostream.h>
#include <time.h>
#include "TimeP.h"
#include <math.h>
#include <FloatAttribute.h>
#include <AttributeParser.h>
#include <StringAttribute.h>
#include <IntegerAttribute.h>
#include <string.h>
#include "MathP.h"
#include "BenthosModem.h"
#include "Syslog.h"

#define ONLINE_MODE  0
#define COMMAND_MODE 1
#define PROMPT ">"
#define COMMAND "+++"
#define ONLINE  "ATO\xd\xa"
#define READ_TIMEOUT 6000
#define EOL "\xd\xa"
#define MAX_RECORD_BYTES 200
#define RANGE_REQUEST "\x0a\x20range\x20request"

#include <string.h>
#include <stdlib.h> /* NULL macro */

char *BenthosModem::memstr(char *haystack, char *needle, int size)
{
	char *p;
	char needlesize = strlen(needle);

	for (p = haystack; p <= (haystack-needlesize+size); p++)
	{
		if (memcmp(p, needle, needlesize) == 0)
			return p; /* found */
	}
	return NULL;
}

BenthosModem::BenthosModem(SerialDevice *serialDevice, Boolean verbose)
  : _state(ONLINE_MODE), _verbose(verbose), _device(serialDevice), _nav(0),
    _interval(0), PeriodicTask("BenthosModem")
{
  Boolean debug = _verbose;

  if (initialize())
  {
    _command = new BenthosModemCommand( MessageQueue::ReadWrite );
    printf("done creating command %x\n", _command);
    addPeriodicCallback(1000, (CallbackMethod)BenthosModem::periodicCallback);
  } else {
    exit(-1);
  }
}


BenthosModem::~BenthosModem()
{
  if (_command) delete _command;
  if (_nav) delete _nav;
  if (_layeredControl) delete _layeredControl;
}


Boolean BenthosModem::initialize()
{
  Boolean debug = _verbose;

  _initialized = False;

  _device->flushReceiveBuf();
  _device->clearPort();
  _device->nonBlocking();
  _device->raw();
  char cmd[40];

  // Put the modem into command mode. If accepted, it's good to go.
  //
  int stat = commandMode();

  if (stat != 0) {
    dprintf("BenthosModem::initialize() - init failed!");
    return _initialized;
  }

  dprintf("Set verbose level");
  sprintf(cmd, "ATS13=03\xd\xa");  // So we'll look for the correct responses
  _device->write(cmd, strlen(cmd));
  delay(300);

  dprintf("Set write delay");
  sprintf(cmd, "ATS8=10\xd\xa");  // Delay is (0.5 sec) before writing
  _device->write(cmd, strlen(cmd));
  delay(300);

  dprintf("Set remote response waiting time");
  sprintf(cmd, "ATS7=10\xd\xa");  // Wait 5 secs for a response from remote
  _device->write(cmd, strlen(cmd));
  delay(300);

  dprintf("Turn on doppler correction");
  sprintf(cmd, "ATS1=1\xd\xa");  // Turn on dopplerCorrection
  _device->write(cmd, strlen(cmd));
  delay(300);

  try {
    _nav = new NavigationIF(NavigationIFServerName);
  }
  catch (Exception ne) {
    Syslog::write("BenthosModem::initialize() - no nav server");
    _nav = 0;
  }

  try {
   _layeredControl = new LayeredControlIF("layeredControl");
  }
  catch (Exception ne) {
    Syslog::write("BenthosMOdem::initialize() - no layeredControl server");
    _layeredControl = 0;
  }

  try {
   _fan = new FanIF("fan");
  }
  catch (Exception ne) {
    Syslog::write("BenthosModem::initialize() - no fan server");
    _fan = 0;
  }

  if (onlineMode() == 0) _initialized = True;
  _bytesInBuffer = 0;

  _output = new BenthosModemOutput(SharedData::ReadWrite);
  _output->data.status = DeviceIF::Initializing;
  _output->data.cmd = AcousticModemIF::INVALID_CMD;
  _output->data.newCmd = False;
  _output->write();

  Syslog::write("done initializing modem\n");
  return _initialized;
}

void BenthosModem::write(char* string)
{
  Boolean debug = _verbose;

  // If not in online mode, try putting modem in online mode
  if (_state != ONLINE_MODE)
    onlineMode();

  // Write the string to the modem
  //
  if (_state == ONLINE_MODE) {
    _device->write(string, strlen(string));
    dprintf("BenthosModem::write() - writing %s\n", string);
  }
}

Boolean BenthosModem::readPacket(BenthosModemPacket *packet)
{
  int nbytes;
  char *head, *field;

  if (_bytesInBuffer == sizeof(_buffer)) {
    _bytesInBuffer = 0;
  }

  try {
    nbytes = _device->read((char *)(_buffer+_bytesInBuffer),
			   sizeof(_buffer)-_bytesInBuffer,100);
  }
  catch(...) {
    nbytes = 0;
  }

  if (nbytes > 0) {
    printf("bytes read: %d bytes in buffer %d\n", nbytes, _bytesInBuffer);
    _bytesInBuffer += nbytes;
    head = memstr((char *)_buffer, "DATA", _bytesInBuffer);
    if (head) {
      printf("got DATA header\n");
      if (sscanf(head,"DATA(%d):", &(packet->nbytes)) == 1) {
	printf("packet length is %d\n", packet->nbytes);
	memcpy(packet->data,head+11, packet->nbytes);
	head += 11 + packet->nbytes;
      }
      if (strstr(head,"<EOP>")) { //we have the whole packet
	printf("got a whole packet\n");
	if (field = strstr(head,"Source")) {
	  sscanf(field,"Source:%2d", &(packet->src));
	}
	if (field = strstr(head,"Destination")) {
	  sscanf(field,"Destination:%2d", &(packet->dest));
	}
	if (field = strstr(head,"CRC")) {
	  if (!strncmp(field+4,"Pass", 4)) {
	    packet->crcPass = True;
	    printf("CRC passed\n");
	  } else {
	    packet->crcPass= False;
	    printf("CRC fails\n");
	  }
	}
	if (field = strstr(head,"MPD")) {
	  sscanf(field,"MPD:%f", &(packet->multiPathDelay));
	}
	if (field = strstr(head,"SNR")) {
	  sscanf(field,"SNR:%f", &(packet->snr));
	}
	if (field = strstr(head,"AGC")) {
	  sscanf(field,"AGC:%d", &(packet->agc));
	}
	if (field = strstr(head,"SPD")) {
	  sscanf(field,"SPD:%f", &(packet->spd));
	}
	if (field = strstr(head,"CCER")) {
	  sscanf(field, "CCERR:%d", &(packet->ccerr));
	}
	_bytesInBuffer = 0; //got the whole packet, flush everything in the buffer
	return packet->crcPass;
      }
    } 
      
  }
  return False;
}

void BenthosModem::sendStatus()
{
  SMAC_STAT_RECORD st;
  char buffer[120]; unsigned long tx_bytes;
  double lat, lon, depth, alt, hdg;
  SMAC_RECORD_HEADER hdr;
  FanIF::Data fanData;

    if (_nav) _nav->location(&lat, &lon, &depth, &alt);
    if (_nav) _nav->heading(&hdg);
    st.auvLat = Math::radToDeg(lat);
    st.auvLong = Math::radToDeg(lon);
    st.auvHeading = Math::radToDeg(hdg);
    st.auvDepth = depth;
    st.auvAlt = alt;
    st.auvVolt = 18.0;
    if (_fan) _fan->Get(&fanData);
    st.auvHumid = fanData.Humidity;
    st.auvBehavior = 3;
    st.auvAbort = 0;
    smac::encodePacket(&hdr, &st, buffer, &tx_bytes);
    _device->write(buffer, tx_bytes);
}

Boolean BenthosModem::dispatchPacket(BenthosModemPacket *packet)
{
  SMAC_SAN_RECORD san;
  SMAC_CMD_RECORD cmd;

  smac::printPacket(packet->data, packet->nbytes);
  if(!smac::decodeSanRecord(packet->data, packet->nbytes, &san)) {
    Syslog::write("stat is %d\n", stat);
    Syslog::write("got a SAN packet\n");
    Syslog::write("time:%f\n", san.wfTime);
    Syslog::write("lat:%.4f lon:%.4f\n", san.wfLat, san.wfLong);
    Syslog::write("slant range: %.2f\n", san.wfSlant);
    Syslog::write("SNR:%f AGC:%d\n", packet->snr, packet->agc);
    memcpy(&_output->data.sanRecord, &san, sizeof(SMAC_SAN_RECORD));
    _output->data.newFix = True;
    _output->write();
    sendStatus();
  }

  if (!smac::decodeCmdRecord(packet->data, packet->nbytes, &cmd)) {
    SMAC_RECORD_HEADER hdr;
    smac::decodeHeader(packet->data, packet->nbytes, &hdr);
    printf("command is %d\n", hdr.parameter);
    if (hdr.parameter == smac::REQ_STAT) {
      char buffer[120]; unsigned long tx_bytes;
      double lat, lon, depth, alt, hdg;
      printf("got request for status\n");
      sendStatus();
    } else if (hdr.parameter == smac::ABORT_MISSION) {
	Syslog::write("got ABORT from acoustic modem\n");
        if (_layeredControl) {
	    _layeredControl->abortMission();
        }
	sendStatus();
    } else if (hdr.parameter == smac::START_SURVEY)
    {
      _output->data.newCmd = True;
      _output->data.cmd = AcousticModemIF::START_SURVEY;
      _output->write();
      sendStatus();
    }
  }

  return True;

}

void BenthosModem::periodicCallback()
{
BenthosModemPacket packet;

  Boolean debug = _verbose;

  // First, process commands if any
  //
  BenthosModemCommand::Command cmdIn;
  float meters;
  while ((_command->read(&cmdIn)) > 0)
  {
    switch (cmdIn._cmd) {
      case BenthosModemCommand::Power:
	power(cmdIn._arg1);
	break;

      case BenthosModemCommand::Transpond:
	transpond(cmdIn._arg1);
	break;

      case BenthosModemCommand::SetRegister:
	setRegister(cmdIn._arg1, cmdIn._arg2);
	break;

      case BenthosModemCommand::Range:
	range(cmdIn._address, &meters);
	break;

      case BenthosModemCommand::DopplerCorrection:
	dopplerCorrection(cmdIn._arg1 == 1);
	break;

      case BenthosModemCommand::SendString:
	write(cmdIn.string);
	break;

      case BenthosModemCommand::SendStatus:
	sendStatus();
	break;

    }
  }

  // Now, read data from remote modems, if any
  //
  while (readPacket(&packet)) {
    dispatchPacket(&packet);
  }

}

int BenthosModem::dopplerCorrection(short on_off)
{
  Boolean debug = _verbose;

  if (commandMode() != 0) {
    return -1;
  }

  char cmd[40];
  sprintf(cmd, "ATS1=%d\xd\xa", (on_off == 0? 0 : 1));
  dprintf("BenthosModem::doppler() - %s", cmd);
  _device->write(cmd, strlen(cmd));
  delay(1000);

  onlineMode();

  return 0;
}

int BenthosModem::power(short powerLevel)
{
  Boolean debug = _verbose;

  if (powerLevel < 1 || powerLevel > 8) {
    dprintf("BenthosModem::power() - invalid power level:%d", powerLevel);
    return -1;
  }

  if (commandMode() != 0) {
    return -1;
  }

  char cmd[40];
  sprintf(cmd, "ATS6=%d\xd\xa", powerLevel);
  dprintf("BenthosModem::power() - %s", cmd);
  int stat = 0;
  _device->write(cmd, strlen(cmd));
  if (_device->confirm("OK\xd\xa", READ_TIMEOUT) != 0)
  {
    Syslog::write("BenthosModem::power() - timeout waiting for \"OK\"");
    stat = -2;
  }

  onlineMode();
  return stat;
}

int BenthosModem::setRegister(short reg, short value)
{
  Boolean debug = _verbose;

  if (commandMode() != 0) {
    return -1;
  }

  char eq;
  if (reg < 21)
    eq = '=';
  else
    eq = '*';

  char cmd[40];
  sprintf(cmd, "ATS%d%c%d\xd\xa", reg, eq, value);
  dprintf("BenthosModem::setRegister() - %s", cmd);
  int stat = 0;
  _device->write(cmd, strlen(cmd));
  if (_device->confirm(PROMPT, READ_TIMEOUT) != 0)
  {
    Syslog::write("BenthosModem::setRegister() - timeout waiting for \"OK\"");
    stat = -2;
  }

  onlineMode();
  return stat;
}

int BenthosModem::transpond(short freq)
{
  Boolean debug = _verbose;

  if (freq < 90 || freq > 140) {
    dprintf("BenthosModem::transpond() - invalid freq:%d", freq);
    return -1;
  }

  if (commandMode() != 0) {
    return -1;
  }

  char cmd[40];
  sprintf(cmd, "ATN%d\xd\xa", freq);
  dprintf("BenthosModem::transpond() - %s", cmd);
  int stat = 0;
  _device->write(cmd, strlen(cmd));
  if (_device->confirm(PROMPT, 12000) != 0)
  {
    Syslog::write("BenthosModem::transpond() - timeout waiting response");
    stat = -2;
  }

  onlineMode();
  return stat;
}

int BenthosModem::range(short address, float *meters)
{
  Boolean debug = _verbose;
  dprintf("BenthosModem::range() to %d ...", address);


  if (address < 0 || address > 31) {
    dprintf("BenthosModem::range() - invalid modem address:%d", address);
    return -1;
  }

  if (commandMode() != 0) {
    return -1;
  }

  char cmd[40];
  char resp[40];
  sprintf(cmd, "ATR%d\xd\xa", address);
  dprintf("BenthosModem::range() - %s", cmd);
  int stat = 0;

  try {
    _device->write(cmd, strlen(cmd));

    int nb;
    if ((nb = _device->readUntil(resp, sizeof(resp)-1, ">",
				 READ_TIMEOUT+2000)) != 0)
    {
      resp[nb] = '\0';
      char *endptr;
      *meters = (float)strtod(resp, &endptr);
      if (endptr == resp) {
	dprintf("BenthosModem::range() - no response from modem %d\n",
		address);
	stat = -2;
      }
      printf("Benthos says \"%s\"\n", resp);
    }
  }
  catch (Exception e) {
    Syslog::write("BenthosModem::range() Exception: %s", e.msg);
    stat = -1;
  }
    

  onlineMode();
  return stat;
}

int BenthosModem::commandMode()
{
  Boolean debug = _verbose;

  int stat = 0;
  char cmd[20];
  sprintf(cmd, COMMAND);

  _state = 0;
  while (strcmp(cmd, EOL) && _state != COMMAND_MODE) {
  if (stat != 0 ) {
    // Try just sending an EOL and see if we get the prompt
    //
    stat = 0;
    dprintf("BenthosModem::commandMode() - trying EOL...");
    sprintf(cmd, EOL);
  }
  try {
    if (_device->write(cmd, strlen(cmd)) == strlen(cmd))
    {
      dprintf("BenthosModem::commandMode() - confirming...");
      if (_device->confirm(PROMPT, READ_TIMEOUT) != 0)
      {
	Syslog::write("BenthosModem::commandMode() - no prompt after %s", cmd);
	stat = -2;
      }
      else
	_state = COMMAND_MODE; // Command state
    }
    else
    {
      dprintf("BenthosModem::commandMode() - %s not written to device", cmd);
      stat = -1;
    }
  }
  catch (Exception e)
  {
    Syslog::write("BenthosModem::commandMode() Exception: %s", e.msg);
    stat = -1;
  }

  }

  return stat;
}

int BenthosModem::onlineMode()
{
  Boolean debug = _verbose;

  char cmd[20];
  sprintf(cmd, ONLINE);

  try {
    if (_device->write(cmd, strlen(cmd)) == strlen(cmd))
    {
      dprintf("BenthosModem::onlineMode() - confirming...");
      if (_device->confirm("CONNECT", READ_TIMEOUT) != 0)
      {
	Syslog::write("BenthosModem::onlineMode() - no CONNECT after %s", cmd);
	return -1;
      }
      else {
	_device->confirm(EOL, READ_TIMEOUT);
	_state = ONLINE_MODE;
      }
    }
    else
    {
      dprintf("BenthosModem::onlineMode() - %s not written to device", cmd);
      return -1;
    }
  }
  catch (Exception e)
  {
    Syslog::write("BenthosModem::onlineMode() Exception: %s", e.msg);
    return -1;
  }

  return 0;
}