#include <math.h>
#include "AcousticMsgHandler.h"
#include "AcousticTelemetry.h"
#include "StringConverter.h"
#include "WhoiUam.h"
#include "Syslog.h"
#include "Exception.h"
#include "LayeredControlIF.h"
#include "Math.h"

#define CmdMnemonicLen 3

AcousticMsgHandler::AcousticMsgHandler(int maxMessageBytes)
  : StringMsgHandler(),
    _charData("char"),
    _navPacket(maxMessageBytes),
    _statusPacket(maxMessageBytes),
    _ctdPacket(maxMessageBytes),
    _commandPacket(maxMessageBytes),
    _log(DataLog::BinaryFormat)
{
  Boolean debug = True;

  // We don't know if other servers are running, so don't try to
  // connect at time of interface creation.
  _navigation = new NavigationIF("nav", DoNotConnect);
  _navigation->setConnectTimeout(1);
  _layeredControl = new LayeredControlIF("layeredControl", DoNotConnect);
  _layeredControl->setConnectTimeout(1);
  _battery = new MetraByteIF("battery", DoNotConnect);
  _battery->setConnectTimeout(1);
  _ctd = new CTDIF("ctd", DoNotConnect);
  _ctd->setConnectTimeout(1);

  _autoCmd = AcousticMessage::Unknown;
}


AcousticMsgHandler::~AcousticMsgHandler()
{
  delete _navigation;
}


void AcousticMsgHandler::process(StringMessage *input, StringMessages *output)
{
  Boolean debug = True;
  char errorBuf[256];

  // sequenceNo will be echoed back in response to command
  short sequenceNo;

  output->clear();

  // Extract command mnemonic
  char cmdMnemonic[CmdMnemonicLen + 1];
  AcousticMessage::Type cmd;

  input->get(cmdMnemonic, sizeof(cmdMnemonic));

  AcousticTelemetry *telemetry = 0;

  // Process specific commands
  if (!strcmp(cmdMnemonic, UAM_TelemetryRequestMnem)) {
    // Telemetry request from UAM

    // Autonomous telemetry alternates between command types 
    switch (_autoCmd) {

    case AcousticMessage::Navigation:
      _autoCmd = AcousticMessage::MissionState;
      break;

    case AcousticMessage::MissionState:
      _autoCmd = AcousticMessage::Unknown;
      // Don't respond to UAM; let it transmit its own diagnostic
      Syslog::write("AcousticMsgHandler::process() - don't respond");
      return;

    case AcousticMessage::Unknown:
      _autoCmd = AcousticMessage::Navigation;
      break;
    }

    cmd = _autoCmd;

    // Sequence no. of 0 indicates autonomous telemetry packet
    sequenceNo = 0;
  }
  else {
    // Command from user
    _commandPacket.setBuffer(input->buffer());

    try {
      cmd = _commandPacket.decodeData();
      sequenceNo = _commandPacket.data.sequenceNo;
    }
    catch (Exception e) {

      Syslog::write("AcousticMsgHandler::process() - caught exception: %s",
		    e.msg);

      sprintf(errorBuf, 
	      "AcousticMsgHandler::process() - unknown command: \"%s\"", 
	      cmdMnemonic);

      throw Exception(errorBuf);
    }

  }

  dprintf("AcousticMsgHandler::process() - \"%s\"", input->buffer());

  dprintf("AcoustMsgHandler::process() - command=%d (%s), seqNo=%d", 
	  cmd, AcousticMessage::typeMnemonic(cmd), sequenceNo);

  _log.cmd.setValue((char *)AcousticMessage::typeMnemonic(cmd));
  _log.cmdPacket.setValue(input->buffer());
  _log.write();

  if (cmd == AcousticMessage::ChangeState) {

    // For now, Abort is the only other state
    try {
      _layeredControl->abortMission();
    }
    catch (Exception e) {
      Syslog::write("AcousticMsgHandler::process() - caught exception: %s",
		    e.msg);
    }

    // Wait a sec for abort to kick in, then send back a status packet
    sleep(1);
    cmd = AcousticMessage::MissionState;
  }


  NavigationIF::Position position;
  NavigationIF::Attitude attitude;
  double depth, altitude;

  switch (cmd) {

  case AcousticMessage::Navigation:

    try {
      _navigation->location(&_navPacket.data.latitude, 
			    &_navPacket.data.longitude,
			    &depth,
			    &altitude);

      dprintf("latitude=%.4f deg, longitude=%.4f deg, d=%.0f m, alt=%.0f",
	      _navPacket.data.latitude * Math::DegsPerRad,
	      _navPacket.data.longitude * Math::DegsPerRad,
	      depth,
	      altitude);

      _navigation->state(&position, &attitude);
      _navPacket.data.depth = position.z;
      _navPacket.data.altitude = position.altitude;
      _navPacket.data.pitch = attitude.pitch;
      _navPacket.data.heading = attitude.yaw;
      _navPacket.data.speed =  sqrt(pow(position.xRate, 2.) + 
				    pow(position.yRate, 2.) + 
				    pow(position.zRate, 2.));

      telemetry = &_navPacket;

      dprintf("depth=%.1f, alt=%.1f, pitch=%.1f deg",
	      position.z, position.altitude, 
	      attitude.pitch * Math::DegsPerRad);

      dprintf("lat=%.2f lon=%.2f dpt=%.1f alt=%.1f hdg=%.1f speed=%.1f p=%.1f",
	      _navPacket.data.latitude * Math::DegsPerRad,
	      _navPacket.data.longitude * Math::DegsPerRad,
	      _navPacket.data.depth,
	      _navPacket.data.altitude,
	      _navPacket.data.heading * Math::DegsPerRad,
	      _navPacket.data.speed,
	      _navPacket.data.pitch * Math::DegsPerRad);

      dprintf("hdg=%.1f", attitude.yaw * Math::DegsPerRad);

    }
    catch (Exception e) {
      Syslog::write("AcousticMsgHandler::process() - caught exception: %s",
		    e.msg);
      break;
    }
    break;

  case AcousticMessage::MissionState:

    try {
      _layeredControl->state(&_statusPacket.data.elapsedMissionTime,
			     &_statusPacket.data.state,
			     _statusPacket.data.behaviorName,
			     &_statusPacket.data.behaviorIndex);


      dprintf("time=%.1f, state=%d, behaviorIndex=%d, behaviorName=%s",
	      _statusPacket.data.elapsedMissionTime,
	      _statusPacket.data.state,
	      _statusPacket.data.behaviorIndex,
	      _statusPacket.data.behaviorName);

      telemetry = &_statusPacket;
    }
    catch (Exception e) {
      Syslog::write("AcousticMsgHandler::process() - caught exception: %s",
		    e.msg);
    }

    try {
      long sampleTime;

      _battery->IandV(&_statusPacket.data.batteryCurrent, 
		      &_statusPacket.data.batteryVoltage,
		      &sampleTime);

      telemetry = &_statusPacket;
    }
    catch (Exception e) {
      Syslog::write("AcousticMsgHandler::process() - caught exception: %s",
		    e.msg);

      _statusPacket.data.batteryCurrent = 
	_statusPacket.data.batteryVoltage = 0.;
    }
    break;
 
  case AcousticMessage::Ctd:

    TimeIF::TimeSpec timeSpec;
    float dummy;

    try {
      _ctd->get_CTD_data(&_ctdPacket.data.conductivity,
			 &_ctdPacket.data.temperature,
			 &_ctdPacket.data.depth, 
			 &dummy, &dummy, 
			 &timeSpec, &timeSpec,
			 &dummy, &dummy, &dummy, &dummy, &dummy, &dummy);
    }
    catch (Exception e) {
      Syslog::write("AcousticMsgHandler::process() - caught exception: %s",
		    e.msg);
    }

    telemetry = &_ctdPacket;

    break;

  default:
    
    sprintf(errorBuf, 
	    "AcousticMsgHandler::process() - unknown command %d", cmd);

    throw Exception(errorBuf);
  }

  if (telemetry) {
    telemetry->reset();
    telemetry->encodeData(cmd, sequenceNo);
    StringMessage *msg = telemetry;
    output->add(&msg);
  }
}