/****************************************************************************/
/* Copyright (c) 2005 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : EchoSounderServer.cc                                          */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 11/28/2005                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <process.h>
#include "EchoSounderServer.h"
#include "EchoSounderDriver.h"
#include "Syslog.h"

EchoSounderServer::EchoSounderServer(SerialParameters *serialParams) 
  : EchoSounderIF_SK()
{
  _serialParams = serialParams;
  _output = new EchoSounderOutput(SharedData::ReadWrite);
  _command = new EchoSounderCommand( MessageQueue::ReadWrite );
  printf("Server Started with string %s.\n", _serialParams->string());
  //
  // Unfortunately the sonar-head return data does not distinguish between
  // the fixed-beam or the scanning version.  So the only way to tell
  // unambiguously is to check the IP address, which means maintaining a list
  // here of which IP goes with which type.  Fortunately MBARI has only two
  // instances.
  //
  // Parse out the IP address:
  char *protocol, *ip;
  char serverString[64];
  strcpy(serverString, _serialParams->string());
  protocol = strtok(serverString, ":\0");
  ip       = strtok(NULL, ":\0");
  strncpy( _ip, ip, sizeof(_ip) );
  _scanning = False;
  if( !strcmp(_ip, "134.89.32.119") )
  {
     Syslog::write("EchoSounderServer::Fixed beam sonar detected.");
//
//   Note: Can't put these here.  They may be overwritten by the Driver's
//   constructor if that one executes first.
//
//   _output->data.tindxMax = 0;
//   _output->data.tindxMin = 0;
//   _output->write();
  }
  else if( !strcmp(_ip, "134.89.32.127") )
  {
     Syslog::write("EchoSounderServer::Scanning sonar detected.");
     _scanning = True;
  }
  else
  {
     Syslog::write("EchoSounderServer::Unknown sonar.  Assume fixed beam.");
  }     
}


EchoSounderServer::~EchoSounderServer()
{
   Syslog::write("EchoSounderServer:: Destructor executing.");
  delete _output;
  delete _command;
}


void EchoSounderServer::name(DeviceIF::Name name)
{
  strcpy(name, "EchoSounder");
}

#if 0
DeviceIF::Status EchoSounderServer::set_beacon(UsblIF::String32 newBeaconName)
{
  EchoSounderCommand::Command cmd(newBeaconName);
  _command->write(&cmd);
  
  return DeviceIF::Ok;
}
#endif

DeviceIF::Status EchoSounderServer::range(Boolean *echoReceived, 
					  double *range, 
					  TimeIF::TimeSpec *sampleTime)
{
  _output->read();

//
// This instrument doesn't have an "echo not received" flag.  It just returns
// a high range instead. So echoReceived is set to false whenever
// DeviceIF::Status is set to Error, indicating a network or instrument
// problem.
//
  *echoReceived = _output->data.echoReceived;
  if( _output->data.error ) 
  {
     *echoReceived = False;
     return DeviceIF::Error;
  }
  *range = _output->data.range;
  sampleTime->seconds = _output->data.updateTime.seconds;
  sampleTime->nanoSeconds = _output->data.updateTime.nanoSeconds;
  
  return DeviceIF::Ok;      
}

DeviceIF::Status EchoSounderServer::get( EchoSounderIF::Data *data )
{

  _output->read();

  if( _output->data.error ) return DeviceIF::Error;

  memcpy((void *)data, (void *)&_output->data, sizeof(EchoSounderIF::Data));

  if( !_scanning ) data->train = 0.0;

  return DeviceIF::Ok;      
}

double EchoSounderServer::getHeadAngle( )
{
  _output->read();
      
  return ((double)(_output->data.train-trainShift))*trainDegPerStep*PI/180.;
}

double EchoSounderServer::getHeadAngle( short tindx )
{
   short train;

   if( tindx < 0 || tindx > EchoSounderIF::ScanArraySize )
   {
      Syslog::write("EchoSounderServer::getHeadAngle() Error.  "
      "Index tindx = %d out of range.", train);
      return -1;
   }
   train = tindx*8 + _output->data.toffset;
   return ((double)(train-trainShift))*trainDegPerStep*PI/180.;
}

void EchoSounderServer::xzAngle(double *radians)
{
  *radians = _output->data.xzAngle;
}

Boolean EchoSounderServer::isScanning(void)
{
   if(_scanning) return True;
   else          return False;
}


void EchoSounderServer::maxRange(double *meters)
{
// Rather than using the value from the .cfg file, use the echoed value in
// case it is different.  
//*meters = _output->data.maxRange;
   *meters = (double) _output->data.maxRangeCfrm;
}

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


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


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


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


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


DeviceIF::Status EchoSounderServer::dataLoggingOn()
{
  // Dummy for now
  return DeviceIF::Ok;
}


DeviceIF::Status EchoSounderServer::dataLoggingOff()
{
  // Dummy for now
  return DeviceIF::Ok;
}


DeviceIF::Status EchoSounderServer::enable()
{
   //
   // Declare an enum and initialize it.
   EchoSounderCommand::EchoSounderCommands cmdEnum;
   cmdEnum = EchoSounderCommand::Start;
   //
   // Construct an object and initialize it with the enum, set to "Start".
   EchoSounderCommand::Command cmdObj(cmdEnum);
   
   _command->write(&cmdObj);
   return DeviceIF::Ok;
}


DeviceIF::Status EchoSounderServer::disable()
{
   //
   // Declare an enum and initialize it.
   EchoSounderCommand::EchoSounderCommands cmdEnum;
   cmdEnum = EchoSounderCommand::Stop;
   //
   // Construct an object and initialize it with the enum, set to "Stop".
   EchoSounderCommand::Command cmdObj(cmdEnum);

   _command->write(&cmdObj);
   return DeviceIF::Ok;
}


DeviceIF::Status EchoSounderServer::headAngleCmd( double trainCmd)
{
   double trainCmdArg;
   trainCmdArg = trainCmd;
   //
   // Declare an enum and initialize it.
   EchoSounderCommand::EchoSounderCommands cmdEnum;
   cmdEnum = EchoSounderCommand::MoveHead;
   //
   // Construct an object and initialize it with the enum and 
   // corresponding argument.
   EchoSounderCommand::Command cmdObj(cmdEnum, trainCmdArg);

   _command->write(&cmdObj);
   return DeviceIF::Ok;
}

DeviceIF::Status EchoSounderServer::scanSectorCmd( double sector)
{
   double sectorArg;
   sectorArg = sector;
   //
   // Declare an enum and initialize it.
   EchoSounderCommand::EchoSounderCommands cmdEnum;
   cmdEnum = EchoSounderCommand::ScanSector;
   // Construct an object and initialize it with the enum and 
   // corresponding argument.
   EchoSounderCommand::Command cmdObj(cmdEnum, sectorArg);

   _command->write(&cmdObj);
   return DeviceIF::Ok;
}


int EchoSounderServer::spawnAuxTasks()
{
  char *program = "echoSounderDriver";
  char errorBuf[256];
  char buf[256];
 
  pid_t pid;
 
  switch ((pid = fork())) {
     
  case 0:
    // In child
    // Execute server program
    execlp(program, program, "-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;
}