//=========================================================================
// Summary  : Reson 6046 Server
// Filename : Reson6046Server.cc
// Author   : Reed Christenson reed@bluefinrobotics.com
// Project  : Reson 6046 Driver
// Revision : 0.0
// Created  : 2002.03.21
// Modified : 2002.03.21
//=========================================================================
// Description : Reson 6046 Server
//=========================================================================

#include "Reson6046Server.h"

Reson6046Server::Reson6046Server(const char *configFileName)
    : Reson6046IF_SK()
{
    dwrite(5)("Entering server constructor...");
    _output = new Reson6046Output(True, SharedData::ReadWrite, True);
    _output->clear();
    _output->data.deviceReady = False;
    _output->write();
    _command = new Reson6046Command( MessageQueue::ReadWrite );

    debug = _output->data.debug;

    dwrite(5)("Leaving server constructor.");
}


Reson6046Server::~Reson6046Server()
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(5)("Entering server destructor...");
   // Syslog::write("Reson6046Server - killing driver");
   // kill(_driverPid, SIGKILL);

    //VehicleConfigurator::clearPID();
    //VehicleConfigurator::clearIFName();
    delete _output;
    dwrite(5)("Leaving server destructor.");
}


// ############################################################
// ## Reson 6046 Specific Commands
// ############################################################

DeviceIF::Status Reson6046Server::startLogging()
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Sending start logging command...");
    Reson6046Command::Command cmd( Reson6046Command::StartLogging );
    _command->write( &cmd );

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::start7KCenterLogging()
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Sending start logging command...");
    Reson6046Command::Command cmd( Reson6046Command::Start7KCenterLogging );
    _command->write( &cmd );

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
        Syslog::write("Reson6046 is not available.");
        return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::stopLogging()
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Sending stop logging command...");
    Reson6046Command::Command cmd( Reson6046Command::StopLogging );
    _command->write( &cmd );

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::stop7KCenterLogging()
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Sending stop logging command...");
    Reson6046Command::Command cmd( Reson6046Command::Stop7KCenterLogging );
    _command->write( &cmd );

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
        Syslog::write("Reson6046 is not available.");
        return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::startSubsystem( Reson6046IF::ResonSonarSystems sonarSystem )
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Sending start subsystem %d command...", sonarSystem);
    // Use different command for Multibeam
    //
    if (sonarSystem == Reson6046IF::Multibeam) {
      printf("Reson6046Server creating MBStartPing command\n");
      Reson6046Command::Command mbcmd( Reson6046Command::MBStartPing, sonarSystem );
      _command->write( &mbcmd );
    }
    else {
      Reson6046Command::Command cmd( Reson6046Command::StartSubsystem, sonarSystem );
      _command->write( &cmd );
    }

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}


DeviceIF::Status Reson6046Server::stopSubsystem( Reson6046IF::ResonSonarSystems sonarSystem )
{
    // Read in shared memory.
    _output->read();  //(&_output->data);
    debug = _output->data.debug;

    dwrite(1)("Sending stop subsystem %d command...", sonarSystem);
    // Use different command for Multibeam
    //
    if (sonarSystem == Reson6046IF::Multibeam) {
      printf("Reson6046Server creating MBStopPing command\n");
      Reson6046Command::Command mbcmd( Reson6046Command::MBStopPing, sonarSystem );
      _command->write( &mbcmd );
    }
    else {
      Reson6046Command::Command cmd( Reson6046Command::StopSubsystem, sonarSystem );
      _command->write( &cmd );
    }

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}


DeviceIF::Status Reson6046Server::pingStatus( Reson6046IF::ResonSonarSystems sonarSystem, 
					      unsigned short *pingStatus )
{
    // Get device ping status.
    Syslog::write("ERROR: pingStatus needs code!");
    return DeviceIF::Error;
}


// Set pulse filename.
DeviceIF::Status Reson6046Server::setPulseFilename( Reson6046IF::ResonSonarSystems sonarSystem,
						    Reson6046IF::String32 pulseFilename )
{    
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Setting pulse filename for subsystem %d to %s...", sonarSystem, pulseFilename);
    Reson6046Command::Command cmd( Reson6046Command::SetPulseFilename, sonarSystem, pulseFilename );
    _command->write( &cmd );

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}


// Set Ping Range.
DeviceIF::Status Reson6046Server::setPingRange( Reson6046IF::ResonSonarSystems sonarSystem,
						float pingRange )
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Setting ping range for subsystem %d to %f...", sonarSystem, pingRange);

    // Use different command for Multibeam
    //
    if (sonarSystem == Reson6046IF::Multibeam) {
      printf("Reson6046Server creating SetMBRange command\n");
      Reson6046Command::Command mbcmd( Reson6046Command::SetMBRange, sonarSystem, pingRange );
      _command->write( &mbcmd );
    }
    else {
      Reson6046Command::Command cmd( Reson6046Command::SetPingRange, sonarSystem, pingRange );
      _command->write( &cmd );
    }


    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}


// Set Ping rate.
DeviceIF::Status Reson6046Server::setPingRate( Reson6046IF::ResonSonarSystems sonarSystem,
					       float pingRate )
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Setting ping rate for subsystem %d to %f...", sonarSystem, pingRate);

    // Use different command for Multibeam
    //
    if (sonarSystem == Reson6046IF::Multibeam) {
      printf("Reson6046Server creating SetMBPingRate command\n");
      Reson6046Command::Command mbcmd( Reson6046Command::SetMBPingRate, sonarSystem, pingRate );
      _command->write( &mbcmd );
    }
    else {
      Reson6046Command::Command cmd( Reson6046Command::SetPingRate, sonarSystem, pingRate );
      _command->write( &cmd );
    }

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}


// Set Ping Power.
DeviceIF::Status Reson6046Server::setPingPower( Reson6046IF::ResonSonarSystems sonarSystem,
						float pingPower )
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Setting ping power for subsystem %d to %f...", sonarSystem, pingPower);

    // Use different command for Multibeam
    //
    if (sonarSystem == Reson6046IF::Multibeam) {
      printf("Reson6046Server creating SetMBTxPower command\n");
      Reson6046Command::Command mbcmd( Reson6046Command::SetMBTxPower, sonarSystem, pingPower );
      _command->write( &mbcmd );
    }
    else {
      Reson6046Command::Command cmd( Reson6046Command::SetPingPower, sonarSystem, pingPower );
      _command->write( &cmd );
    }

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}

// Set MB receiver gain.
DeviceIF::Status Reson6046Server::setRxGain( Reson6046IF::ResonSonarSystems sonarSys,
					     float gain )
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Setting receiver gain for subsystem %d to %f...", sonarSys, gain);

    printf("Reson6046Server creating SetMBRxGain command\n");
    Reson6046Command::Command mbcmd( Reson6046Command::SetMBRxGain, sonarSys, gain );
    _command->write( &mbcmd );

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}

// Set MB pulse width.
DeviceIF::Status Reson6046Server::setPulseWidth( Reson6046IF::ResonSonarSystems sonarSys,
						 float width )
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Setting pulse width for subsystem %d to %f...", sonarSys, width);

    printf("Reson6046Server creating SetMBPulseWidth command\n");
    Reson6046Command::Command mbcmd( Reson6046Command::SetMBPulseWidth, sonarSys, width );
    _command->write( &mbcmd );

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::setMBBottomDetectionFilters(float minRange,
							      float maxRange,
							      float minDepth,
							      float maxDepth)
{
    printf("Reson6046Server creating SetMBBottomDetectionFilters command\n");
    Reson6046Command::Command mbcmd( Reson6046Command::SetMBBottomDetectionFilters,
				     minRange, maxRange, minDepth, maxDepth );
    _command->write( &mbcmd );

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::setTime()
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Setting Time...");
    Reson6046Command::Command cmd( Reson6046Command::SetTime );
    _command->write( &cmd );

        // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::setVerboseMode( unsigned short flag )
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Setting verbose mode to %s...", flag ? "ON" : "OFF");
    Reson6046Command::Command cmd( Reson6046Command::SetVerboseMode, flag);
    _command->write( &cmd );

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::getVersion()
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Requesting Reson 6046 version...");
    Reson6046Command::Command cmd( Reson6046Command::GetVersion);
    _command->write( &cmd );

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::setMBSnippetControl(Boolean on_off, unsigned short int window_size)
{
  Reson6046Command::Command cmd( Reson6046Command::SetMBSnippetControl,
                                 (on_off == True), float(window_size));
  Syslog::write("Reson Server - setting MB snippets: enable = %d, window size = %d", on_off, window_size);
  _command->write( &cmd );
  
  return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::getSonarStatus( Reson6046IF::ResonSonarSystems sonarSystem )
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Requesting sonar status for system %d...",sonarSystem);
    Reson6046Command::Command cmd( Reson6046Command::GetSonarStatus, sonarSystem);
    _command->write( &cmd );

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }

    return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::getModules()
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Requesting sonar modules...");
    Reson6046Command::Command cmd( Reson6046Command::GetModules);
    _command->write( &cmd );

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::getRunList()
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Requesting sonar run list...");
    Reson6046Command::Command cmd( Reson6046Command::GetRunList);
    _command->write( &cmd );

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::sonarReboot()
{
    // Tell sonar to reboot.
    Syslog::write("ERROR: sonarReboot needs code!");
    return DeviceIF::Error;
}

DeviceIF::Status Reson6046Server::sonarSystemReboot()
{
    // Tell sonar system to reboot.
    Syslog::write("ERROR: sonarSystemReboot needs code!");
    return DeviceIF::Error;
}

DeviceIF::Status Reson6046Server::sonarShutdown()
{
    // Tell sonar to shutdown.
    Syslog::write("ERROR: sonarShutdown needs code!");
    return DeviceIF::Error;
}

DeviceIF::Status Reson6046Server::sonarSystemShutdown()
{
    // Tell sonar system to shutdown.
    Syslog::write("ERROR: sonarSystemShutdown needs code!");
    return DeviceIF::Error;
}

DeviceIF::Status Reson6046Server::sonarSystemPowerOn()
{
    // Tell sonar system to power on.
    Syslog::write("ERROR: sonarSystemPowerOn needs code!");
    return DeviceIF::Error;
}
DeviceIF::Status Reson6046Server::sonarSystemPowerOff()
{
    // Tell sonar system to pwoer off.
    Syslog::write("ERROR: sonarSystemPowerOff needs code!");
    return DeviceIF::Error;
}

DeviceIF::Status Reson6046Server::dataLoggingOn()
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Sending start logging data command...");
    Reson6046Command::Command cmd( Reson6046Command::DataLoggingOn );
    _command->write( &cmd );

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::dataLoggingOff()
{
    // Read in shared memory.
    _output->read();  //(&_output->data);
    debug = _output->data.debug;

    // Send command.
    dwrite(1)("Sending stop logging data command...");
    Reson6046Command::Command cmd( Reson6046Command::DataLoggingOff );
    _command->write( &cmd );

    // Check to see if the Reson6046 is running.
    if (!_output->data.deviceReady)
    {
	Syslog::write("Reson6046 is not available.");
	return DeviceIF::Offline;
    }
    return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::setSBPDuration(float duration_milli)
{
  Reson6046Command::Command cmd( Reson6046Command::SetSBPDuration,
                                 0, duration_milli);
  Syslog::write("Reson Server - setting SBP duration to %f", duration_milli);
  _command->write( &cmd );
  
  return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::setSBPGain(float gain)
{
  Reson6046Command::Command cmd( Reson6046Command::SetSBPGain,
                                 0, gain);
  Syslog::write("Reson Server - setting SBP gain to %f", gain);
  _command->write( &cmd );
  
  return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::setMBSoundVelocity(float m_per_s)
{
  if (m_per_s < -0.1) return DeviceIF::Ok;
  Reson6046Command::Command cmd( Reson6046Command::SetMBSoundVelocity,
                                 0, m_per_s);
  Syslog::write("Reson Server - setting MB Sound Velocity to %f", m_per_s);
  _command->write( &cmd );
  
  return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::setMBAbsorptionLoss(float loss)
{
  Reson6046Command::Command cmd( Reson6046Command::SetMBAbsorptionLoss,
                                 0, loss);
  Syslog::write("Reson Server - setting MB Absorption loss to %f", loss);
  _command->write( &cmd );
  
  return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::setMBSpreadingLoss(float loss)
{
  Reson6046Command::Command cmd( Reson6046Command::SetMBSpreadingLoss,
                                 0, loss);
  Syslog::write("Reson Server - setting MB Spreading loss to %f", loss);
  _command->write( &cmd );
  
  return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::setMBBottomDetectDepthRangeOn(Boolean d_on_off, Boolean r_on_off)
{
  int action = 0;
  if (d_on_off) action += 1;
  if (r_on_off) action += 2;
  
  Reson6046Command::Command cmd( Reson6046Command::UseDepthRangeMBBottomDetect,  action);
  _command->write( &cmd );
  return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::setMBBottomDetectRecalc()
{
  Reson6046Command::Command cmd( Reson6046Command::RecalcParams);
  _command->write( &cmd );
  return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::setMBBottomDetectRecalcInterval(short seconds)
{
  Reson6046Command::Command cmd( Reson6046Command::RecalcInterval, seconds);
  _command->write( &cmd );
  return DeviceIF::Ok;
}

DeviceIF::Status Reson6046Server::status()
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;
    dwrite(2)("Requesting driver status...");
    return _output->data.status;
}

int Reson6046Server::spawnAuxTasks()
{
  char *program = "reson6046Driver";
  char errorBuf[256];
 
  switch ((_driverPid = fork())) {
     
  case 0:
    // In child
    // Execute server program

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

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

    perror(errorBuf);
 
    break;

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

  default:
    return _driverPid;
  }
    
    return 0;
}