/****************************************************************************/
/* Copyright (c) 2004 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : BenthosModemServer.cc                                         */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 10/07/2004                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <process.h>

#include <time.h>
#include "BenthosModemServer.h"
#include "BenthosModemOutput.h"
#include "AcousticModemIF.h"
#include "Syslog.h"

BenthosModemServer::BenthosModemServer(const char *name,
				       SerialParameters *serialParams)
  : AcousticModemIF_SK(name)
{
  _serialParams = serialParams;
  _command = new BenthosModemCommand( MessageQueue::ReadWrite );
  _output = new BenthosModemOutput(SharedData::ReadWrite);
  _output->data.status = DeviceIF::Ok;
  _output->write();
}


BenthosModemServer::~BenthosModemServer()
{
  delete _command;
  delete _output;
}


void BenthosModemServer::name(DeviceIF::Name name)
{
  strcpy(name, "BenthosModem");
}


void BenthosModemServer::serialNumber(DeviceIF::Name number)
{
  strcpy(number, "XXX");
}


DeviceIF::Status BenthosModemServer::initialize()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status BenthosModemServer::powerOn()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status BenthosModemServer::powerOff()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status BenthosModemServer::status()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}

DeviceIF::Status BenthosModemServer::writeString(AcousticModemIF::modemString ps)
{
  Syslog::write("Inside writeString.. %s", ps);
  BenthosModemCommand::Command cmd(ps);
  _command->write(&cmd);
  return DeviceIF::Ok;
}

DeviceIF::Status BenthosModemServer::dopplerCorrection(Boolean on_off)
{
  BenthosModemCommand::Command cmd(BenthosModemCommand::DopplerCorrection,
				   0, (on_off? 1 : 0));
  _command->write(&cmd);

  return DeviceIF::Ok;
}

DeviceIF::Status BenthosModemServer::range(short modemAddress, float *meters)
{
  if (modemAddress >= 0 &&  modemAddress <= 31) {
    BenthosModemCommand::Command cmd(BenthosModemCommand::Range, modemAddress);
    _command->write(&cmd);
  }
  return DeviceIF::Ok;
}

DeviceIF::Status BenthosModemServer::power(short powerLevel)
{
  if (powerLevel >= 1 && powerLevel <= 8) {
    BenthosModemCommand::Command cmd(BenthosModemCommand::Power,0, powerLevel);
    _command->write(&cmd);
  }
  return DeviceIF::Ok;
}

DeviceIF::Status BenthosModemServer::sendStatus()
{
    BenthosModemCommand::Command cmd(BenthosModemCommand::SendStatus);
    _command->write(&cmd);
    return DeviceIF::Ok;
}

DeviceIF::Status BenthosModemServer::transpond(short freq)
{
  if (freq >= 90 && freq <= 140) {
    BenthosModemCommand::Command cmd(BenthosModemCommand::Transpond,
				     0, freq);
    _command->write(&cmd);
  }
  return DeviceIF::Ok;
}

DeviceIF::Status BenthosModemServer::setRegister(short reg, short value)
{
  BenthosModemCommand::Command cmd(BenthosModemCommand::SetRegister,
				   0, reg, value);
  _command->write(&cmd);

  return DeviceIF::Ok;
}

DeviceIF::Status
BenthosModemServer::getSANFix(AcousticModemIF::SMAC_SAN_RECORD *fix, Boolean *newFix)
{
  //grab data from shared memory
  _output->read();
  memcpy(fix, &_output->data.sanRecord, sizeof(AcousticModemIF::SMAC_SAN_RECORD));
  *newFix = _output->data.newFix;

  //clear "new" flag so we don't load old data into INS
  _output->data.newFix = False;
  _output->write();
  
  return DeviceIF::Ok;
}

DeviceIF::Status
BenthosModemServer::getSMACCmd(AcousticModemIF::SMAC_CMD_TYPE *cmd, 
			       Boolean *newCmd)
{
  //grab data from shared memory
  _output->read();
  *newCmd = _output->data.newCmd;
  *cmd = _output->data.cmd;
  _output->data.newCmd = False;
  _output->write();
  
  return DeviceIF::Ok;
}

int BenthosModemServer::spawnAuxTasks()
{
  char *program = "benthosModem";
  char errorBuf[256];
  char buf[256];
 
  pid_t pid;
 
  switch ((pid = fork())) {
     
  case 0:
    // In child
    // Execute server program

    execlp(program, program, "-v", "-serial", _serialParams->string(), 0);

    sprintf(errorBuf, 
	    "Task::spawnServer() - execlp() of \"%s\" failed", program);

    perror(errorBuf);
 
    break;

  case -1:
    perror("Task::spawnServer() - fork() failed");
    return -1;
  }

  return 0;
}