#include <math.h>
#include <i86.h>
#include <stdio.h>
#include "BuoyLauncherDriver.h"
#include "BuoyLauncherServer.h"
#include "Attributes.h"
#include "IntegerAttribute.h"
#include "FloatAttribute.h"
#include "BooleanAttribute.h"
#include "AttributeParser.h"
#include "System.h"
#include "TimeP.h"
#include "VehicleConfigurationIF.h"
#include "PeriodicTask.h"
#include "Syslog.h"
#include "MathP.h"
#include "LcXmodem.h"

BuoyLauncherDriver::BuoyLauncherDriver(SerialDevice *device,
				       int period)
  : PeriodicTask("buoyLauncherDriver")
{
  _period = period;
  _device = device;
  _statusCalls = True;

  // set up I/O
  _input = new BuoyLauncherInput(SharedData::ReadWrite);
  _output = new BuoyLauncherOutput(SharedData::Write);
  _lcx = new LcXmodem(_device);
  _input->read();
  _input->data._command = '#';
  _input->write();

  _lastStatusCheck = time((long*)0);

  initialize();

  //set up interfaces to periodic task
  addPeriodicCallback(period, 
		      (CallbackMethod )BuoyLauncherDriver::periodicCallback);


  //  _serverProxy = new BuoyLauncherIF("buoyLauncherServer");


  // Subscribe to events from server
  //  _eventService->subscribe(_serverProxy,
  //		   BuoyLauncherIF::Initialized,
  //		   (EventService::Callback )BuoyLauncherDriver::eventCallback);

}

BuoyLauncherDriver::~BuoyLauncherDriver()
{
  delete _input;
  delete _output;
  delete _lcx;
}

Boolean BuoyLauncherDriver::launchBuoy(char cmd)
{
  Syslog::write("BuoyLauncher::processing launch command");

  int i;
  int success;
  char ack[3];

  // Send time to LC if this is a "real" launch
  //
  time_t now = time((long*)0);
  if (cmd == 'L') sendTimeToLC(now);

  _output->data._lastLaunchDone = _output->data._inLaunchPosition = 0;
  _output->write();
  Syslog::write("BuoyLauncher::clearing launchDone");
  for (i = 0, success = 0; i < 6 && success < 2;  i++)
  {
    if (sendCmdToLC(cmd))
      success++;
    else if (i == 6)
    {
      Syslog::write("Launch command not received");
      return False;
    }
  }

  // Wait 60 seconds for ack which says buoy is in launch position
  if (readAckFromLC(60)) {
      Syslog::write("BuoyLauncher::Launch Buoy(): Launcher in Launch position");
      _output->data._inLaunchPosition = 1;
      _output->write();
  } else {
      Syslog::write("BuoyLauncher::LaunchBuoy(): no ack for launch position");
      _output->data._lastLaunchDone = 1;
      _output->write();
      return False;
  }

  // Wait 90 seconds for somekind of acknowledgement from the Launcher.
  //
  if (readAckFromLC(90))
  {
    Syslog::write("BuoyLauncher::LaunchBuoy(): recvd ACK");
    Syslog::write("BuoyLauncher::setting launchDone");
  }
  else
  {
    Syslog::write("BuoyLauncher::LaunchBuoy(): recvd NACK or timeout");
  }
  _output->data._lastLaunchDone      = 1;
  _output->write();

  return True;
}

void BuoyLauncherDriver::rotateBuoy()
{
  Syslog::write("BuoyLauncher::processing rotateBuoy command");

  if (sendCmdToLC('R'))
  {
    if (readAckFromLC(360))
    {
      Syslog::write("BuoyLauncherDriver::rotateBuoy: Got a ACK! Good buoy");
      _output->data._buoyResponsive = 1;
      _output->write();
    }
    else
    {
      Syslog::write("BuoyLauncherDriver::rotateBuoy: Got a NACK! No buoy");
      _output->data._buoyResponsive = 0;
      _output->write();
    }
  }
}

void BuoyLauncherDriver::rotateCarousel()
{
  Syslog::write("BuoyLauncher::processing rotateCarousel command");

  sendCmdToLC('B');
}

void BuoyLauncherDriver::xferData(char type)
{
  Syslog::write("BuoyLauncher::processing loadData (Argos) command");
  time_t now = time((long*)0);  // Send time to LC
  sendTimeToLC(now);

  // get the filename from the _input buffer
  BuoyLauncherIF::Filename fname;
  strcpy(fname, _input->data._dataFilename);

  // try to open and load the file
  //
  FILE *ifile = fopen(fname, "r");
  if (ifile)
  {
    // File opened, find the size and allocate some memory
    //
    fseek(ifile, 0L, 2);
    unsigned long filesize = ftell(ifile);
    printf("filesize=%ld\n", filesize);
    BYTE *buf = (BYTE*)malloc(sizeof(BYTE)*filesize);
    rewind(ifile);

    // Inform the LC that data is coming
    //
    sendCmdToLC(type);
    Syslog::write("BuoyLauncher::xferData(): transferring file %s", fname);

    // Send the data over
    //
    unsigned long nbytes = fread(buf, sizeof(BYTE), filesize, ifile);
    _lcx->sendXmodem(buf, filesize);
    free((char*)buf);
  }
  else
  {
    Syslog::write("BuoyLauncher::xferData():error opening xfer file %s",fname);
  }
}

void BuoyLauncherDriver::currentBuoy()
{
  Syslog::write("BuoyLauncher::processing currentBuoy");

  // Let the LC know we want a new "current" buoy
  //
  sendCmdToLC('N');

  // Send the buoy number over to LC
  //
  BYTE buoyNo = (BYTE)_input->data._currentBuoy;
  Syslog::write("BuoyLauncher::currentBuoy(): %d", (int)buoyNo);
  _lcx->sendXmodem((BYTE*)&buoyNo, sizeof(buoyNo));
}

void BuoyLauncherDriver::status(char cmd)
{
  if (!_statusCalls) return;

  Syslog::write("BuoyLauncher::processing getLauncherStatus");

  // Let the LC know we want a status check
  //
  sendCmdToLC(cmd);

  // Read the status from the LC
  //
  LauncherStatus ls;
  long numbytes = 0;
  _lcx->receiveXmodem((BYTE*)&ls, &numbytes);

  // If we received the LauncherStatus object, send the data back to the Server
  //
  Syslog::write("BuoyLauncher::recvd %ld bytes from LC, should be %d",
		numbytes, sizeof(ls));
  if (numbytes == sizeof(ls))
  {
    _output->data._launcherError     = ls.bLauncherError;
    _output->data._lastLaunchOK      = ls.bLastLaunchOK;
    _output->data._buoyResponsive    = ls.bBuoyResponsive;
    _output->data._nextBuoyReady     = ls.bNextBuoyReady;
    _output->data._currentBuoyNumber = ls.bCurrentBuoyNumber;
    _output->data._totalNumBuoys     = ls.bTotalNumBuoys;
    _output->data._numBuoysRemain    = ls.bNumBuoysRemain;
    _output->data._numBuoysLaunched  = ls.bNumBuoysLaunched;
    _output->data._timeSec           = ls.lTimeSecL;
    _output->data._argosBytesL       = ls.lARGOSBytesL;
    _output->data._archiveBytesL     = ls.lArchiveBytesL;
    _output->data._picoBytesL        = ls.lPicoBytesAvailL;
    _output->data._dataMemoryL       = ls.lDataMemoryL;

    _output->data._buoyNumber    = ls.Buoy.bBuoyNumber;
    _output->data._mainBattVolts = ls.Buoy.bMainBattVolts;
    _output->data._auxBattVolts  = ls.Buoy.bAuxBattVolts;
    _output->data._tempDegC      = ls.Buoy.bTempDegC;
    _output->data._timeSecB      = ls.Buoy.lTimeSecBuoy;
    _output->data._argosBytesB   = ls.Buoy.lARGOSBytesBuoy;
    _output->data._archiveBytesB = ls.Buoy.lArchiveBytesBuoy;
    _output->data._picoBytesB    = ls.Buoy.lPicoBytesAvailBuoy;
    _output->data._dataMemoryB   = ls.Buoy.lDataMemoryBuoy;
    _output->write();
  }
}

void BuoyLauncherDriver::periodicCallback()
{
  // Check for a command
  _input->read();
  time_t now;
  switch (_input->data._command)
    {
    case '#':
      break; // No command

    case 'L':  // real buoy
    case 'Y':  //dummy buoy
      launchBuoy(_input->data._command);
      break;

    case 'R':
      rotateBuoy();
      break;

    case 'X':
    case 'D':
      xferData(_input->data._command);
      break;

    case 'N':
      currentBuoy();
      break;

    case 'S':
    case 'G':
      status(_input->data._command);
      break;

    case 'C':  // upper case
    Syslog::write("BuoyLauncherDriver::status calls set true");
      _statusCalls = True;
      break;

    case 'c':  // lower case
    Syslog::write("BuoyLauncherDriver::status calls set false");
      _statusCalls = False;
      break;

    case 'B':
    Syslog::write("BuoyLauncherDriver::rotateCarousel() being called\n");
      rotateCarousel();
      break;

    default:
      Syslog::write("BuoyLauncherDriver::recvd unknow command: %c",
		    _input->data._command);
      break;
    };


  _input->data._command = '#'; // clear command character
  _input->write();

  if (time((long*)0) > _lastStatusCheck + 30)
  {
    Syslog::write("BuoyLauncherDriver::getting 30 second status");
    status('G');
    _lastStatusCheck = time((long*)0);
  }

}

DeviceIF::Status BuoyLauncherDriver::initialize()
{
  // Initialize contact with the LC. Sync it's time with ours
  // RH+ Don't send time until needed RH-
  //time_t now = time((long*)0);
  //sendTimeToLC(now);

  _output->data._buoyResponsive = 0;
  _output->write();

  return DeviceIF::Ok;
}

Boolean BuoyLauncherDriver::readAckFromLC(unsigned timeout_in_secs)
{
  char ack[2];
  try {
    _device->readNChars(ack, 1, timeout_in_secs*1000);
  }
  catch (SerialDevice::TimedOut) {
  Syslog::write("BuoyLauncherDriver::readAckFromLC() timeout (%d secs)",
		timeout_in_secs);
  return False;
  }

  if (ack[0] == ACK)
    return True;
  else
    return False;
}

Boolean BuoyLauncherDriver::sendCmdToLC(char command)
{
  char ack[8];
  char cmd[8];
  sprintf(cmd, "*%c", command);
  unsigned char chk = '*' + command;

  if ( (_device->write(cmd, strlen(cmd)) == strlen(cmd)) &&
       (_device->write((char*)&chk, 1) == 1) )
  {
    Syslog::write("BuoyLauncher::\"%s%x\" sent to LC", cmd, chk);

  }
  else
  {
    Syslog::write("BuoyLauncher::failed to send \"%s%x\" to LC", cmd, chk);
    return False;
  }

  const int MAX_ATTEMPTS = 3;
  for (int i = 0; i < MAX_ATTEMPTS; i++)
  {
    try {
      _device->readNChars(ack, 3,15000);
    }
    catch (SerialDevice::TimedOut) {
      Syslog::write("BuoyLauncherDriver::SendCmdToLC() timeout");
      continue;
    }
    ack[3] = '\0';
    Syslog::write("BuoyLauncher::recvd %s as response from LC", ack);
    chk = '#' + command;
    if (ack[1] == command && ack[2] == chk)
    {
      return True; // valid response, no ack wait
    }
    else
    {
      continue; // try again
    }
  }
  Syslog::write("BuoyLauncher::Launcher not responding to '%c'", command);
  return False;
}

Boolean BuoyLauncherDriver::sendTimeToLC(time_t newtime)
{
  if (sendCmdToLC('T'))
  {
    time_t now = time((long*)0);
    _lcx->sendXmodem((BYTE*)&now, sizeof(time_t));
  }
  else
  {
    Syslog::write("BuoyLauncher::sendTimeToLC(): Launcher not responding T");
    return False;
  }

  if (readAckFromLC(300))
  {
    return True;
  }
  else
  {
    Syslog::write("BuoyLauncher::sendTimeToLC(): Launcher not ACK-ing");
    return False;
  }
}

void BuoyLauncherDriver::eventCallback(TaskInterface *taskIF,
				       EventCode event)
{
    return;
}

void BuoyLauncherDriver::loadConfiguration()
{
}