//=========================================================================
// 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...");
    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::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::startSubsystem( Reson6046IF::ResonSonarSystems sonarSystem )
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Sending start subsystem %d command...", sonarSystem);
    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);
    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);
    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);
    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);
    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;
}

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::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 );

    // 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 );

    // 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::status()
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;
    dwrite(2)("Requesting driver status...");
    return _output->data.status;
}