////////////////////////////////////////////////////////////////////////////////
//
// PURPOSE:  Dvl Driver.
// AUTHOR:   Rob McEwen
// DATE:     2012/10/1
// COMMENTS: 
//
////////////////////////////////////////////////////////////////////////////////
//
#include <process.h>
#include "DvlSideServer.h"

DvlSideServer::DvlSideServer(const char *driverName, SerialParameters *serialParams) 
  : DvlSideIF_SK() 
{
  _serialParams = serialParams;
  _output = new DvlSideOutput(SharedData::ReadWrite);
  _driverName = strdup(driverName);
  m_lastPingTime = 0.;
}



DvlSideServer::~DvlSideServer() 
{
  delete _output;
  free((void *)_driverName);
}


void DvlSideServer::name(DeviceIF::Name name)
{
  // Dummy implementation for now
  strcpy(name, "Dvl");
}


void DvlSideServer::serialNumber(DeviceIF::Name number)
{
  // Dummy implementation for now
  strcpy(number, "XXX");
}


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


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


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


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


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


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


DeviceIF::Status DvlSideServer::get(DvlSideIF::Data *data, Boolean *newData) 
//DeviceIF::Status DvlSideServer::get(DvlIF::Data *data)
//
// I would like to remove the now-useless (as of 03/11/24) argument *newData
// above.  After having removed it from all the relevant places (I can find)
// the linker still fails, giving a fairly weird error.  So, I had to put it
// back.
//
{
  _output->read();
  //
  // Write the data from shared memory back across the idl interface:
  //
  memcpy((void *)data, (void *)&_output->data, sizeof(DvlSideIF::Data));
  //
  // Return an error if the serial line has a problem:
  //
  if( _output->data.badComms != 0 ) return DeviceIF::Error;
  //
  // If the DVL has a valid bottom lock, return OK
  //
  if((_output->data.bottomStatus == 0) && 
     ((_output->data.dataStatus & BAD_BOTTOM_TRACK_VELOCITY) !=
                                  BAD_BOTTOM_TRACK_VELOCITY)  )
    return DeviceIF::Ok;
  //
  // Or, if the DVL is tracking water mass velocity, return OK
  //
  if((_output->data.waterStatus == 0) && 
     ((_output->data.dataStatus & BAD_WATER_MASS_VELOCITY) !=
                                  BAD_WATER_MASS_VELOCITY)  )
    return DeviceIF::Ok;
  //
  // Otherwise, tell Navigation that the DVL reading is in error.
  //
  //hjt - with the DVL in core sensors, this return status logic doesn't quite
  //      work. If the DVL doesn't get water track or bottom track, then this
  //      will look like an error to navigation, which will abort. Not sure of
  //      the best way to handle this. Maybe change default return status to Ok if we get this far. We've
  //      already checked for a comms error, and bottom status. Its up to the
  //      consumer to check specific flags to confirm the validity of the
  //      data
//  return DeviceIF::Error;
  return DeviceIF::Ok;

}


int DvlSideServer::spawnAuxTasks()
{
  char *program = (char *)_driverName;

  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;
}