/****************************************************************************/
/* Copyright (c) 2005 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : DockOnBoardServer.cc                                          */
/* Author   : rsm                                                           */
/* Project  :                                                               */
/* Version  :                                                               */
/* Created  : 9/5/2006                                                      */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <process.h>
#include "DockServer.h"

DockServer::DockServer(SerialParameters *serialParamsVehicle,
		       SerialParameters *serialParamsDock) 
  : DockIF_SK()
{
  _serialParamsVehicle = serialParamsVehicle;
  _serialParamsDock    = serialParamsDock;
//
  _dockOutput = new DockOutput(SharedData::ReadWrite);
  _dockOutput->data.deviceReady = False;
  _dockOutput->write();
  _dockCommand = new DockCommand( MessageQueue::ReadWrite );
//
  _vehicleOutput = new VehicleOutput(SharedData::ReadWrite);
  _vehicleOutput->data.deviceReady = False;
  _vehicleOutput->write();
  _vehicleCommand = new VehicleCommand( MessageQueue::ReadWrite );
}


DockServer::~DockServer()
{
  delete _dockOutput;
  delete _dockCommand;
  delete _vehicleOutput;
  delete _vehicleCommand;
}

DeviceIF::Status DockServer::request_dock_status()
{
   //
   //PicCmd is an enum   
   DockCommand::DockCommands PicCmd;   
   //
   //Set it to the value ReadStatus.
   PicCmd = DockCommand::ReadStatus;   
   //
   //Construct a "command" object that contains the enum.  
   DockCommand::Command cmd(PicCmd);   
   //
   //Write the command to the driver.
   _dockCommand->write(&cmd);
  
  return DeviceIF::Ok;
}

DeviceIF::Status DockServer::set_dock_state( DockIF::String32 State )
{
   //
   //PicCmd is an enum   
   DockCommand::DockCommands PicCmd;   
   //
   //Set its value:
   PicCmd = DockCommand::SetState;   
   //
   //Construct a "command" object that contains the enum.  
   DockCommand::Command cmd(PicCmd, State);   
   //
   //Write the command to the driver.
   _dockCommand->write(&cmd);
  
  return DeviceIF::Ok;
}

DeviceIF::Status DockServer::read_dock_PIC( DockIF::String512 pic_status, 
					       TimeIF::TimeSpec *sampleTime )
{
   _dockOutput->read();
   strcpy(pic_status,  _dockOutput->data.rs_response);
   sampleTime->seconds = _dockOutput->data.updateTime.seconds;
   sampleTime->nanoSeconds = _dockOutput->data.updateTime.nanoSeconds;
  return DeviceIF::Ok;
}

DeviceIF::Status DockServer::request_vehicle_status()
{
   //
   //PicCmd is an enum   
   VehicleCommand::VehicleCommands PicCmd;   
   //
   //Set it to the value ReadStatus.
   PicCmd = VehicleCommand::ReadStatus;   
   //
   //Construct a "command" object that contains the enum.  
   VehicleCommand::Command cmd(PicCmd);   
   //
   //Write the command to the driver.
   _vehicleCommand->write(&cmd);
  
  return DeviceIF::Ok;
}

DeviceIF::Status DockServer::read_vehicle_PIC(
   DockIF::String512 pic_status, TimeIF::TimeSpec *sampleTime)
{
   _vehicleOutput->read();
   strcpy(pic_status,  _vehicleOutput->data.rs_response);
   sampleTime->seconds = _vehicleOutput->data.updateTime.seconds;
   sampleTime->nanoSeconds = _vehicleOutput->data.updateTime.nanoSeconds;
  return DeviceIF::Ok;
}

DeviceIF::Status DockServer::query_resolver()
{
   //
   //PicCmd is an enum   
   DockCommand::DockCommands PicCmd;   
   //
   //Set it to the correct value:
   PicCmd = DockCommand::QueryResolver;   
   //
   //Construct a "command" object that contains the enum.  
   DockCommand::Command cmd(PicCmd);   
   //
   //Write the command to the driver.
   _dockCommand->write(&cmd);
  
  return DeviceIF::Ok;
}

DeviceIF::Status DockServer::read_resolver(
	DockIF::String32 status, TimeIF::TimeSpec *sampleTime)
{
   _dockOutput->read();
   strcpy(status,  _dockOutput->data.resolverState);
   sampleTime->seconds = _dockOutput->data.updateTime.seconds;
   sampleTime->nanoSeconds = _dockOutput->data.updateTime.nanoSeconds;
   return DeviceIF::Ok;
}

DeviceIF::Status DockServer::query_probe()
{
   //
   //PicCmd is an enum   
   DockCommand::DockCommands PicCmd;   
   //
   //Set it to the correct value:
   PicCmd = DockCommand::QueryProbe;   
   //
   //Construct a "command" object that contains the enum.  
   DockCommand::Command cmd(PicCmd);   
   //
   //Write the command to the driver.
   _dockCommand->write(&cmd);
  
  return DeviceIF::Ok;
}

DeviceIF::Status DockServer::read_probe(
	DockIF::String32 status, TimeIF::TimeSpec *sampleTime)
{
   _dockOutput->read();
   strcpy(status,  _dockOutput->data.probeState);
   sampleTime->seconds = _dockOutput->data.updateTime.seconds;
   sampleTime->nanoSeconds = _dockOutput->data.updateTime.nanoSeconds;
   return DeviceIF::Ok;
}

DeviceIF::Status DockServer::move_probe( DockIF::String32 probeCmd )
{
   //
   //PicCmd is an enum   
   DockCommand::DockCommands PicCmd;   
   //
   //Set it to the correct value:
   PicCmd = DockCommand::MoveProbe;   
   //
   //Construct a "command" object that contains the enum.  
   DockCommand::Command cmd(PicCmd, probeCmd);   
   //
   //Write the command to the driver.
   _dockCommand->write(&cmd);
  
  return DeviceIF::Ok;
}

DeviceIF::Status DockServer::open_port()
{
   //
   //PicCmd is an enum   
   DockCommand::DockCommands PicCmd;   
   //
   //Set it to the correct value:
   PicCmd = DockCommand::OpenPort;
   //
   //Construct a "command" object that contains the enum.  
   DockCommand::Command cmd(PicCmd);
   //
   //Write the command to the driver.
   _dockCommand->write(&cmd);
  
  return DeviceIF::Ok;
}

DeviceIF::Status DockServer::ping( short timeOut )
{
   //
   //PicCmd is an enum   
   DockCommand::DockCommands PicCmd;   
   //
   //Set it to the correct value:
   PicCmd = DockCommand::Ping;
   //
   //Construct a "command" object that contains the enum.  
   DockCommand::Command cmd(PicCmd, timeOut);
   //
   //Write the command to the driver.
   _dockCommand->write(&cmd);
  
  return DeviceIF::Ok;
}

DeviceIF::Status DockServer::ping_result(
	short *pingError, double *sampleTime)
{
   _dockOutput->read();
   *pingError = _dockOutput->data.pingError;
   *sampleTime = _dockOutput->data.updateTimeSecs;
   return DeviceIF::Ok;
}

DeviceIF::Status DockServer::close_port()
{
   //
   //PicCmd is an enum   
   DockCommand::DockCommands PicCmd;   
   //
   //Set it to the correct value:
   PicCmd = DockCommand::ClosePort;
   //
   //Construct a "command" object that contains the enum.  
   DockCommand::Command cmd(PicCmd);
   //
   //Write the command to the driver.
   _dockCommand->write(&cmd);
  
  return DeviceIF::Ok;
}

DeviceIF::Status DockServer::set_continuous_query( Boolean query,
												   short pingTimeOut)
{
   //
   //PicCmd is an enum   
   DockCommand::DockCommands PicCmd;   
   //
   //Set it to the correct value:
   if( query ) PicCmd = DockCommand::ContinuousQueryOn;
   else PicCmd = DockCommand::ContinuousQueryOff;
   //
   //Construct a "command" object that contains the enum.  
   DockCommand::Command cmd(PicCmd, pingTimeOut);
   //
   //Write the command to the driver.
   _dockCommand->write(&cmd);
  
  return DeviceIF::Ok;
}

DeviceIF::Status DockServer::read_continuous_query( 
   Boolean *networkPresent,
   DockIF::String32 probeStatus, 
   DockIF::String32 resolverStatus, 
   double *sampleTime)
{
   _dockOutput->read();
    int pingError = _dockOutput->data.pingError;
	if( pingError ) *networkPresent = False;
	else            *networkPresent = True;
	strcpy(probeStatus,  _dockOutput->data.probeState);
	strcpy(resolverStatus,  _dockOutput->data.resolverState);
	*sampleTime = _dockOutput->data.updateTimeSecs;
  
	return DeviceIF::Ok;
}

void DockServer::name(DeviceIF::Name name)
{
  strcpy(name, "Dock");
}

Boolean DockServer::ready()
{
  _dockOutput->read();
  return _dockOutput->data.deviceReady;
}

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


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


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


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


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


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


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


DeviceIF::Status DockServer::enable()
{
  return DeviceIF::Ok;
}


DeviceIF::Status DockServer::disable()
{
  return DeviceIF::Ok;
}


int DockServer::spawnAuxTasks()
{
  char *dockProgram = "dockDriver";
  char *vehicleProgram = "vehicleDriver";
  char errorBuf[256];
  char buf[256];
 
  pid_t pid;
 
  switch ((pid = fork())) {
     
  case 0:
    // In child
    // Execute server dockProgram
    execlp(dockProgram, dockProgram, "-serial", 
	   _serialParamsDock->string(), 0);

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

    perror(errorBuf);
 
    break;

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

  switch ((pid = fork())) {
     
  case 0:
    // In child
    // Execute server vehicleProgram
    execlp(vehicleProgram, vehicleProgram, "-serial", 
	   _serialParamsVehicle->string(), 0);

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

    perror(errorBuf);
 
    break;

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

  return 0;
}