//=========================================================================
// Summary  : Main file for Reson 6046
// Filename : Reson6046Driver.cc
// Author   : Reed Christenson
// Project  : Reson 6046 Driver
// Revision : 0.0
// Created  : 2002.02.26
// Modified : 2002.02.26
//=========================================================================
// Description : Reson 6046 Driver
//=========================================================================

#include "Reson6046Driver.h"

Reson6046Driver::Reson6046Driver(void) :
    PeriodicTask(Reson6046DriverName)
{
    int status = 0;
    int returnVal = 0;

    Time::gettime(&_timeStamp);

    //dwrite(5)("Entering driver constructor...");
    Syslog::write("--> Starting Reson 6046 Driver....");

    // Create output shared memory
    //dwrite(5)("Creating output (shared memory)...");
    _output = new Reson6046Output(False, SharedData::ReadWrite, False);
    // Initialize some values.
    _output->data.deviceReady = True;
    _output->data.status = DeviceIF::Initializing;
    _output->data.initErrorCount = 0;
    _output->data.errorCount = 0;
    _output->write();

    // Read attributes from cfg file and save in shared memory.
    dwrite(5)("Loading configuration...");
    loadConfiguration(ConfigurationFile);

    // Renew local copy of configured shared memory.
    _output->read();

    dwrite(5)("Creating connection to commands...");
    _resonPayCmds = NULL;
    _resonPayCmds = new Reson6046Commands();

    // Create command message queue
    dwrite(5)("Creating command message queue...");
    _command = NULL;
    try 
    {
	_command = new Reson6046Command( MessageQueue::ReadWrite );
	_command->flush();
    } catch( Exception e ) {
	Syslog::write("Caught exception while opening command queue: %s", e.msg );
    } catch(...) {
	Syslog::write("Caught exception while opening message queue");
    }

    // Create log writer
    //_log = new Reson6046Log(_data);


    // Find the configuration file
    dwrite(5)("Contacting vehicle configuration server...");
    VehicleConfigurationIF::DeviceInfo devInfo;
    if( VehicleConfigurator::getConfig(Reson6046DriverName, &devInfo ) )
    {
	Syslog::write("Reson6046Driver() - Can not find %s in config files.",
		      Reson6046DriverName);
	exit(-1);
    }
    else
    {
	dwrite(2)("Reson6046Driver() - Contacted vehicle configuration server.");
    }

    // Set task priority
    dwrite(5)("Setting task priority to %d...",devInfo.priority);
    if (System::setTaskPriority(devInfo.priority) == -1) exit (-1);

    _log = new Reson6046Log(_output, True);


    // Create an instance of the simple socket driver.
    _simpleSocket = NULL;
    _simpleSocket = new SimpleSocket();

    // Make a socket connection
    while(!_simpleSocket->ready())
    {
	returnVal = _simpleSocket->createConnectedStreamClient( _output->data.reson6046CommandPort,
								_output->data.reson6046IpAddressString);
	if(_simpleSocket->ready())
        {
	    _output->data.deviceReady = True;
	    _output->write();
	    resetInitError();
	}
	else
	{
	    _output->data.deviceReady = False;
	    _output->write();
	    onInitError();
	    sleep(1);
	}
    }

    returnVal = Command_StopLogging();
    returnVal = Command_StopSubsystem(Reson6046IF::EdgetechAll);
    // Setup sonar from configuration file.
    //sleep(2);
    returnVal = Command_SetPulseFilename(Reson6046IF::SubBottom, _output->data.sbpPulseFilename);
    returnVal = Command_SetPulsePower(Reson6046IF::SubBottom, _output->data.sbpPulsePower);
    returnVal = Command_SetPingRate(Reson6046IF::SubBottom, _output->data.sbpPingRate);
    //sleep(2);
    returnVal = Command_SetPulseFilename(Reson6046IF::SideScanLow, _output->data.sslPulseFilename);
    returnVal = Command_SetPulsePower(Reson6046IF::SideScanLow, _output->data.sslPulsePower);
    returnVal = Command_SetPingRange(Reson6046IF::SideScanLow, _output->data.sslPingRange);
    //sleep(2);
    returnVal = Command_SetPulseFilename(Reson6046IF::SideScanHigh, _output->data.sshPulseFilename);
    returnVal = Command_SetPulsePower(Reson6046IF::SideScanHigh, _output->data.sshPulsePower);
    returnVal = Command_SetPingRange(Reson6046IF::SideScanHigh, _output->data.sshPingRange);
    //sleep(2);

    // Set up the periodic callback
    dwrite(5)("Setting up the periodic callback to %d ms...",_output->data.callbackPeriod);
    addPeriodicCallback( _output->data.callbackPeriod, 
			 (CallbackMethod)Reson6046Driver::periodicCallback );

    // Save all new values to shared memory.
    _output->write();

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

/// Reson 6046 Driver Destructor.
Reson6046Driver::~Reson6046Driver(void)
{
    dwrite(5)("Entering Reson 6046 Driver destructor...");

    // Clean up 6046 commands.
    clean(_resonPayCmds);

    // Close UDP socket.
    clean(_simpleSocket);

    // Clean up command queue.
    clean(_command);

    // Clean up the log.
    clean(_log);

    // Inform shared memory of offline status.
    _output->data.deviceReady = False;

    //healthy states
    if(_output->data.status == DeviceIF::Ok ||
       _output->data.status == DeviceIF::Initializing ||
       _output->data.status == DeviceIF::Error) 
    {
	_output->data.status = DeviceIF::Offline;
    }
    _output->write();

    dwrite(5)("Leaving driver destructor.");
}

/// Periodic Callback
void Reson6046Driver::periodicCallback(void)
{
    int returnVal;
    unsigned char receivedMessage[640];
    U32 sizeNetworkFrameAndData;

    dwrite(5)("Entering driver periodicCallback...");
    // Read status from shared memory.
    _output->read();

    // If a socket doesn't exists, make one.
    if(!_simpleSocket->exists())
    {
	returnVal = _simpleSocket->createConnectedStreamClient( _output->data.reson6046CommandPort,
							        _output->data.reson6046IpAddressString);
	if(_simpleSocket->ready())
	{
	    _output->data.deviceReady = True;
	    resetError();
	}
	else
	{
	    _output->data.deviceReady = False;
	    onError();
	}
    }

    // If connection is ready, receive data.
    if(_simpleSocket->ready())
    {
	dwrite(5)("Receiving message...");

 	receivedMessage[0]='\0';
 	returnVal = _simpleSocket->receiving(&receivedMessage[0], sizeof(receivedMessage));
	if(returnVal < 0)
	    onError();
	else
	{
	    if(returnVal > 0)
	    {
		Syslog::write("DEBUG: Received:");
		for(int i=0;i<returnVal;i++)
		    printf("%02X:",receivedMessage[i]);
		printf("\n");
	    }
	    resetError();
	}
	dwrite(5)("Received message, size of %d.", returnVal);
    }

//     Syslog::write("DEBUG: Command_GetModules...");
//     returnVal = Command_GetModules();
//     Syslog::write("DEBUG: Command_GetRunList...");
//     returnVal = Command_GetRunList();
//     Syslog::write("DEBUG: CommandReserved1()...");
//     returnVal = Command_Reserved1();
//     Syslog::write("DEBUG: CommandReserved2()...");
//     returnVal = Command_Reserved2();

    _output->write();
    dwrite(5)("Leaving driver periodicCallback.");
}


/// Process Command
DeviceIF::Status Reson6046Driver::processCommand(void)
{
    dwrite(5)("Entering driver processCommand...");
    DeviceIF::Status returnStatus = DeviceIF::Ok;
    Reson6046Command::Command cmdIn;
    int returnVal;
    char buffer[32];
    
    // Make sure _command exists.
    if (!(_command)) 
    {
	Syslog::write("processCommand() - _command doesn't exist!\n");
	return DeviceIF::Error;
    }
    else
    {
	// Read in shared memory to get latest status.
	_output->read();

	// Process all commands in queue.
	while( (_command->read( &cmdIn )) > 0 )
	{
	    dwrite(5)("processCommand() --- Processing command: %d\n", cmdIn.request);
	    if (!(DeviceDriver::processCommand_Skel(cmdIn.request))) 
	    {
		switch( cmdIn.request )
		{
		case Reson6046Command::None :
		    // If not sending a command to sonar, keep same status.
		    returnVal = 0;
		    break;
		case Reson6046Command::DataLoggingOn :
		    dwrite(2)("Received start logging data command.");
		    returnVal = 0;
		    break;
		case Reson6046Command::DataLoggingOff :
		    dwrite(2)("Received stop logging data command.");
		    returnVal = 0;
		    break;
		case Reson6046Command::StartSubsystem:  // PLC Command ID  0
		    dwrite(2)("Starting subsystem %d...",cmdIn.iVal);
		    returnVal = Command_StartSubsystem(cmdIn.iVal);
		    break;
		case Reson6046Command::StopSubsystem:  // PLC Command ID  0
		    dwrite(2)("Starting subsystem %d...",cmdIn.iVal);
		    returnVal = Command_StopSubsystem(cmdIn.iVal);
		    break;
		case Reson6046Command::SetPulseFilename:  // PLC Command ID  0
		    dwrite(2)("Setting subsytem %d's pulse file to %s...",cmdIn.iVal,cmdIn.sVal);
		    returnVal = Command_SetPulseFilename(cmdIn.iVal, cmdIn.sVal);
		    break;
		case Reson6046Command::SetPingRange:  // PLC Command ID  0
		    dwrite(2)("Setting subsystem %d's ping range to %6.2f...",cmdIn.iVal,cmdIn.fVal);
		    returnVal = Command_SetPingRange(cmdIn.iVal, cmdIn.fVal);
		    break;
		case Reson6046Command::SetPingRate:  // PLC Command ID  0
		    dwrite(2)("Setting subsystem %d's ping rate to %6.2f...",cmdIn.iVal,cmdIn.fVal);
		    returnVal = Command_SetPingRate(cmdIn.iVal, cmdIn.fVal);
		    break;
		case Reson6046Command::SetPingPower:  // PLC Command ID  0
		    dwrite(2)("Setting subsystem %d's ping transmit power to %6.2f...",
			      cmdIn.iVal,
			      cmdIn.fVal);
		    returnVal = Command_SetPulsePower(cmdIn.iVal, cmdIn.fVal);
		    break;
		case Reson6046Command::StartLogging :  // PLC Command ID  1
		    dwrite(2)("Start sonar logging...");
		    returnVal = Command_StartLogging();
		    break;
		case Reson6046Command::StopLogging :  // PLC Command ID  1
		    dwrite(2)("Stop sonar logging...");
		    returnVal = Command_StopLogging();
		    break;
		case Reson6046Command::GetVersion:  // PLC Command ID  4
		    dwrite(2)("Getting sonar version...");
		    returnVal = Command_GetVersion();
		    break;
		case Reson6046Command::GetSonarStatus:  // PLC Command ID 10
		    dwrite(2)("Getting sonar status for subsystem %d...",
			      cmdIn.iVal);
		    returnVal = Command_GetSonarStatus(cmdIn.iVal);
		    break;
		case Reson6046Command::SetVerboseMode:  // PLC Command ID 11
		    dwrite(2)("Setting sonar verbose mode to %d...", cmdIn.iVal);
		    returnVal = Command_SetVerboseMode(cmdIn.iVal);
		    break;
		case Reson6046Command::GetModules:  // PLC Command ID 14
		    dwrite(2)("Requesting sonar modules...");
		    returnVal = Command_GetModules();
		    break;
		case Reson6046Command::GetRunList:  // PLC Command ID 15
		    dwrite(2)("Requesting sonar run list...");
		    returnVal = Command_GetRunList();
		    break;
		case Reson6046Command::SetTime:  // PLC Command ID 20
		    dwrite(2)("Setting sonar time...");
		    returnVal = Command_SetTime();
		    break;
		default:
		    Syslog::write("Ignoring unknown command %d.", cmdIn.request);
		    returnVal = -1;
		}
		if(returnVal < 0)
		    onError();
		else
		    resetError();
		returnStatus = _output->data.status;
	    }
	}
    }
    dwrite(5)("Leaving driver processCommand.");
    return returnStatus;
}


/// Create Command
DeviceIF::Status Reson6046Driver::createCommand(DriverCommand **cmd)
{
    dwrite(5)("Entering driver createCommand...");
    *cmd = new Reson6046Command(MessageQueue::ReadWrite);
    if (cmd) 
    {
	dwrite(5)("Leaving driver createCommand.");
    }
    else 
    {
	Syslog::write("createCommand() --- couldn't create commands\n");
	return DeviceIF::Error;
    }
    return DeviceIF::Ok;
}


/// Load Configuration
void Reson6046Driver::loadConfiguration(char *configFile)
{
    dwrite(5)("Entering driver loadConfiguration...");

    Boolean displayConfiguration = False;

    // Get latest copy of memory to update.
    _output->read();

    Attributes attributes(Reson6046DriverName);

    attributes.add(new DebugAttribute("debug",
				      "Debug Flag",
				      &_output->data.debug,
				      1));
    attributes.add(new IntegerAttribute("initLimit",
					"Seconds to keep trying to initialize",
					(long *)&_output->data.initErrorCountLimit,
					10));
    attributes.add(new IntegerAttribute("timeOutCount",
					"Allowed number of bad reads and writes in a row",
					(long *)&_output->data.errorCountLimit,
					10));
    attributes.add(new IntegerAttribute("callbackPeriod",
					"CallbackPeriod (milliseconds)",
					(long *)&_output->data.callbackPeriod,
					100));
    attributes.add(new BooleanAttribute("displayConfiguration",
					"Display Configuration",
					&displayConfiguration,
					False));

    // Reson 6046 Network Parameters
    attributes.add(new StringAttribute("Reson6046IPAddressString",
				       "Reson IP Address xxx.xxx.xxx.xxx",
				       &_output->data.reson6046IpAddressString,
				       "127.0.0.1"));
    attributes.add(new IntegerAttribute("Reson6046CommandPort",
					"Reson 6046 Command Port",
					(long *)&_output->data.reson6046CommandPort,
					6046));
    attributes.add(new IntegerAttribute("Reson6046ShutdownPort",
					"Reson 6046 Shutdown Port",
					(long *)&_output->data.reson6046ShutdownPort,
					8123));

    // EdgeTech FS-AU Network Parameters
    attributes.add(new BooleanAttribute("EdgeTechVisibleOnNetwork",
					"EdgeTech Visible On Network",
					&_output->data.edgetechVisibleOnNetwork,
					False));
    attributes.add(new StringAttribute("EdgeTechIpAddressString",
				       "IP Address xxx.xxx.xxx.xxx",
				       &_output->data.edgetechIpAddressString,
				       "127.0.0.1"));
    attributes.add(new IntegerAttribute("EdgeTechCommandPort",
					"EdgeTech Command Port",
					(long *)&_output->data.edgetechCommandPort,
					1800));
    attributes.add(new IntegerAttribute("EdgeTechDataPort",
					"EdgeTech Data Port",
					(long *)&_output->data.edgetechDataPort,
					1801));
    attributes.add(new IntegerAttribute("EdgeTechShutdownPort",
					"EdgeTech Shutdown Port",
					(long *)&_output->data.edgetechShutdownPort,
					8123));
    // Sub-Bottom Profiler Settings
    attributes.add(new StringAttribute("sbpPulseFile",
				       "SBP Pulse Filename",
				       &_output->data.sbpPulseFilename,
				       "216Pulse_9k_14k_20ms.spf"));
    attributes.add(new FloatAttribute("sbpPingRate",
				      "SBP Ping Rate in Hz",
				      &_output->data.sbpPingRate,
				      4.0));
    attributes.add(new FloatAttribute("sbpPulsePower",
				      "SBP Pulse Power Percentage",
				      &_output->data.sbpPulsePower,
				      0.0));
    // Sidescan Low Settings
    attributes.add(new StringAttribute("sslPulseFile",
				       "SSL Pulse Filename",
				       &_output->data.sslPulseFilename,
				       "ss75_5k_9k_20ms.spf"));
    attributes.add(new FloatAttribute("sslPingRange",
				      "SSL Ping Range in Meters",
				      &_output->data.sslPingRange,
				      100.0));
    attributes.add(new FloatAttribute("sslPulsePower",
				      "SSL Pulse Power Percentage",
				      &_output->data.sslPulsePower,
				      0.0));
    // Sidescan High Settings
    attributes.add(new StringAttribute("sshPulseFile",
				       "SSH Pulse Filename",
				       &_output->data.sshPulseFilename,
				       "ss75_5k_9k_20ms.spf"));
    attributes.add(new FloatAttribute("sshPingRange",
				      "SSH Ping Range in Meters",
				      &_output->data.sshPingRange,
				      100.0));
    attributes.add(new FloatAttribute("sshPulsePower",
				      "SSH Pulse Power Percentage",
				      &_output->data.sshPulsePower,
				      0.0));

    dwrite(5)("loadConfiguration: parse attributes...");
    AttributeParser::parse(System::configurationFile(configFile),&attributes);

    // Save values in shared memory.
    _output->write();

    if(displayConfiguration)
    {
	Syslog::write("-------Reson6046 Configuration-------");
	Syslog::write("Debug Level                 = %d.", debug);
	Syslog::write("Init Error Count Limit      = %d.", _output->data.initErrorCountLimit);
	Syslog::write("Error Count Limit           = %d.", _output->data.errorCountLimit);
	Syslog::write("Callback Period             = %d milliseconds.", _output->data.callbackPeriod);
	Syslog::write("Reson 6046 IP Address       = %s.", _output->data.reson6046IpAddressString);
	Syslog::write("Reson 6046 Command Port     = %d.", _output->data.reson6046CommandPort);
	Syslog::write("Reson 6046 Shutdown Port    = %d.", _output->data.reson6046ShutdownPort);
	Syslog::write("EdgeTech Visible on Network = %s.", _output->data.edgetechVisibleOnNetwork ? "True": "False");
	Syslog::write("EdgeTech IP Address         = %s.", _output->data.edgetechIpAddressString);
	Syslog::write("EdgeTech Command Port       = %d.", _output->data.edgetechCommandPort);
	Syslog::write("EdgeTech Data Port          = %d.", _output->data.edgetechDataPort);
	Syslog::write("EdgeTech Shutdown Port      = %d.", _output->data.edgetechShutdownPort);
	Syslog::write("SBP Pulse Filename          = %s.", _output->data.sbpPulseFilename);
	Syslog::write("SBP Ping Rate               = %6.2fHz.", _output->data.sbpPingRate);
	Syslog::write("SBP Pulse Power             = %6.2f%%.", _output->data.sbpPulsePower);
	Syslog::write("SSL Pulse Filename          = %s.", _output->data.sslPulseFilename);
	Syslog::write("SSL Ping Range              = %6.2fm.", _output->data.sslPingRange);
	Syslog::write("SSL Pulse Power             = %6.2f%%.", _output->data.sslPulsePower);
	Syslog::write("SSH Pulse Filename          = %s.", _output->data.sshPulseFilename);
	Syslog::write("SSH Ping Rate               = %6.2fm.", _output->data.sshPingRange);
	Syslog::write("SSH Pulse Power             = %6.2f%%.", _output->data.sshPulsePower);
	Syslog::write("-------------------------------------------");
    }
    dwrite(5)("Leaving driver loadConfiguration.");
}


int Reson6046Driver::sendCommand(U32 recordTypeIdentifier,
				 U32 sensorIndex,
				 U32 commandID,
				 U32 action,
				 char *command)
{
    int returnVal = -1;

    if(_simpleSocket->ready())
    {
	if(debug >= 4)
	    _resonPayCmds->displayPlcCommandFrame();
	returnVal = _resonPayCmds->setPlcCommandFrame( sensorIndex,
						       commandID,
						       action,
						       command);
	if(debug >= 4)
	    _resonPayCmds->displayPlcCommandFrame(Reson6046Commands::ShowFrameAndData);

	if(debug >= 4)	
	    _resonPayCmds->displayDataRecordFrame();
	returnVal = _resonPayCmds->setDataRecordFrame(recordTypeIdentifier);
	if(debug >= 4)
	    _resonPayCmds->displayDataRecordFrame(Reson6046Commands::ShowFrameAndData);

	if(debug >= 4)
	    _resonPayCmds->displayNetworkFrame();
	_resonPayCmds->setNetworkFrame();
	if(debug >= 4)
	    _resonPayCmds->displayNetworkFrame(Reson6046Commands::ShowFrameAndData);

	dwrite(5)("Sending message...");
	returnVal = _simpleSocket->sending(_resonPayCmds->getPtrNetworkFrameAndData(),
					   _resonPayCmds->getSizeNetworkFrameAndData());
	dwrite(5)("Send return value = %d.",returnVal);
    }

    return(returnVal);
}


int Reson6046Driver::sendCommand(U32 recordTypeIdentifier,
				 U32 sensorIndex,
				 U32 commandID,
				 U32 action,
				 char *command,
				 U32 parameter)
{
    int returnVal = -1;

    dwrite(5)("sendCommand(U32): command = %s, parameter = %d.", command, parameter);
    sprintf(&_buffer[0], command, parameter);
    dwrite(5)("sendCommand(U32): _buffer = %s (%02X).", _buffer, _buffer[0]);
    returnVal = sendCommand(recordTypeIdentifier,
			    sensorIndex,
			    commandID,
			    action,
			    &_buffer[0]);

    return(returnVal);
}


int Reson6046Driver::sendCommand(U32 recordTypeIdentifier,
				 U32 sensorIndex,
				 U32 commandID,
				 U32 action,
				 char *command,
				 F32 parameter)
{
    int returnVal = -1;

    dwrite(5)("sendCommand(F32): command = %s, parameter = %d.", command, parameter);
    sprintf(&_buffer[0], command, parameter);
    dwrite(5)("sendCommand(F32): _buffer = %s.", _buffer);
    returnVal = sendCommand(recordTypeIdentifier,
			    sensorIndex,
			    commandID,
			    action,
			    &_buffer[0]);

    return(returnVal);
}

int Reson6046Driver::sendCommand(U32 recordTypeIdentifier,
				 U32 sensorIndex,
				 U32 commandID,
				 U32 action,
				 char *command,
				 char *parameter)
{
    int returnVal = -1;

    dwrite(5)("sendCommand(char): command = %s, parameter = %s.", command, parameter);
    sprintf(&_buffer[0], command, parameter);
    dwrite(5)("sendCommand(char): _buffer = %s.", _buffer);
    returnVal = sendCommand(recordTypeIdentifier,
			    sensorIndex,
			    commandID,
			    action,
			    &_buffer[0]);

    return(returnVal);
}

int Reson6046Driver::sendCommand(U32 recordTypeIdentifier,
				 U32 sensorIndex,
				 U32 commandID,
				 U32 action,
				 char *command,
				 U32 parameter1,
				 U32 parameter2)
{
    int returnVal = -1;

    dwrite(5)("sendCommand(U32, U32): command = %s, parameters = %u, %u ", 
              command, parameter1, parameter2);
    sprintf(&_buffer[0], command, parameter1, parameter2);
    dwrite(5)("sendCommand(U32, U32): _buffer = %s.", _buffer);
    returnVal = sendCommand(recordTypeIdentifier,
			    sensorIndex,
			    commandID,
			    action,
			    &_buffer[0]);

    return(returnVal);
}

int Reson6046Driver::sendCommand(U32 recordTypeIdentifier,
				 U32 sensorIndex,
				 U32 commandID,
				 U32 action,
				 char *command,
				 U32 parameter1,
				 U32 parameter2,
				 U32 parameter3,
				 U32 parameter4,
				 U32 parameter5,
				 F32 parameter6)
{
    int returnVal = -1;

    dwrite(5)("sendCommand(U32*5,F32): command = %s, parameters = %u, %u, %u, %u, %u, %f", 
              command, parameter1, parameter2, parameter3, parameter4, parameter5, parameter6);
    sprintf(&_buffer[0], command, parameter1, parameter2, parameter3, parameter4, parameter5, parameter6);
    dwrite(5)("sendCommand(U32*5,F32): _buffer = %s.", _buffer);
    returnVal = sendCommand(recordTypeIdentifier,
			    sensorIndex,
			    commandID,
			    action,
			    &_buffer[0]);

    return(returnVal);
}


// ##############################
// ####### RESON COMMANDS #######
// ##############################

// PLC Command ID  0
int Reson6046Driver::Command_StartSubsystem(int subSystem)
{
    return( Command_StartStopSubsystem(subSystem, 1));
}

int Reson6046Driver::Command_StopSubsystem(int subSystem)
{
    return( Command_StartStopSubsystem(subSystem, 0));
}

int Reson6046Driver::Command_StartStopSubsystem(int subSystem, int action)
{
    int returnVal1 = 0, returnVal2 = 0, returnVal3 = 0, errorCount = 0;

    switch(subSystem)
    {
    case Reson6046IF::SubBottom:
    case Reson6046IF::SideScanLow:
    case Reson6046IF::SideScanHigh:
	if(action < 0)
	{
	    Syslog::write("Unknown subsystem state %d, setting subsystem %d to 0 (off)!", action, subSystem);
	    action = 0;
	}
	if(action > 1)
	{
	    Syslog::write("Unknown subsystem state %d, setting subsystem %d to 1 (on)!", action, subSystem);
	    action = 1;
	}

	returnVal1 = sendCommand(Reson6046Commands::PayloadControllerCommand,
				 U32(subSystem),
				 Reson6046Commands::PLC_Command_Generic,
				 Reson6046Commands::PLC_Action_Set,
				 "Ping = %d",
				 U32(action));
	if(returnVal1 < 0)
	    errorCount++;
	break;
    case Reson6046IF::SideScanBoth:
	returnVal2 = Command_StartStopSubsystem(Reson6046IF::SideScanLow, action);
	if(returnVal2 < 0)
	    errorCount++;
	returnVal3 = Command_StartStopSubsystem(Reson6046IF::SideScanHigh, action);
	if(returnVal3 < 0)
	    errorCount++;
	break;
    case Reson6046IF::EdgetechAll:
	returnVal1 = Command_StartStopSubsystem(Reson6046IF::SubBottom, action);
	if(returnVal1 < 0)
	    errorCount++;
	returnVal2 = Command_StartStopSubsystem(Reson6046IF::SideScanLow, action);
	if(returnVal2 < 0)
	    errorCount++;
	returnVal3 = Command_StartStopSubsystem(Reson6046IF::SideScanHigh, action);
	if(returnVal3 < 0)
	    errorCount++;
	break;
    default:
	Syslog::write("Unable to stop unknown subsystem %d!",action);
	errorCount++;
    }

    // Sum up all errors.
    if(errorCount < 1)
	return( returnVal1 + returnVal2 + returnVal3 );
    else
	return(-1);
}

int Reson6046Driver::Command_SetPulseFilename(int subSystem, char *pulseFilename)
{
    return( sendCommand(Reson6046Commands::PayloadControllerCommand,
					    U32(subSystem),
					    Reson6046Commands::PLC_Command_Generic,
					    Reson6046Commands::PLC_Action_Set,
					    "Pulse = %s",
					    pulseFilename) );
}

int Reson6046Driver::Command_SetPingRange(int subSystem, float range)
{
    int returnVal1 = 0, returnVal2 = 0, errorCount = 0;

    switch(subSystem)
    {
    case Reson6046IF::SideScanLow:
    case Reson6046IF::SideScanHigh:
	returnVal1 = sendCommand(Reson6046Commands::PayloadControllerCommand,
				 U32(subSystem),
				 Reson6046Commands::PLC_Command_Generic,
				 Reson6046Commands::PLC_Action_Set,
				 "Range = %.2f",
				 range);
	if(returnVal1 < 0)
	    errorCount++;
	break;
    case Reson6046IF::SideScanBoth:
	returnVal1 = Command_SetPingRange(Reson6046IF::SideScanLow, range);
	if(returnVal1 < 0)
	    errorCount++;
	returnVal2 = Command_SetPingRange(Reson6046IF::SideScanHigh, range);
	if(returnVal2 < 0)
	    errorCount++;
	break;
    default:
	Syslog::write("Unable to set ping range for invalid subsystem %d!", subSystem);
	errorCount++;
    }
    // Sum up all errors.
    if(errorCount < 1)
	return( returnVal1 + returnVal2 );
    else
	return(-1);
}

int Reson6046Driver::Command_SetPingRate(int subSystem, float rate)
{
    int returnVal1 = 0, returnVal2 = 0, returnVal3 = 0, errorCount = 0;

    switch(subSystem)
    {
    case Reson6046IF::SubBottom:
    case Reson6046IF::SideScanLow:
    case Reson6046IF::SideScanHigh:
	// Need to and range check on rate.
	// Need to verify that rates can be set for sidescan sonars.
	returnVal1 = sendCommand(Reson6046Commands::PayloadControllerCommand,
				 U32(subSystem),
				 Reson6046Commands::PLC_Command_Generic,
				 Reson6046Commands::PLC_Action_Set,
				 "Rate = %.2f",
				 rate);
	if(returnVal1 < 0)
	    errorCount++;
	break;
    case Reson6046IF::SideScanBoth:
	returnVal2 = Command_SetPingRate(Reson6046IF::SideScanLow, rate);
	if(returnVal2 < 0)
	    errorCount++;
	returnVal3 = Command_SetPingRate(Reson6046IF::SideScanHigh, rate);
	if(returnVal3 < 0)
	    errorCount++;
	break;
    case Reson6046IF::EdgetechAll:
	returnVal1 = Command_SetPingRate(Reson6046IF::SubBottom, rate);
	if(returnVal1 < 0)
	    errorCount++;
	returnVal2 = Command_SetPingRate(Reson6046IF::SideScanLow, rate);
	if(returnVal2 < 0)
	    errorCount++;
	returnVal3 = Command_SetPingRate(Reson6046IF::SideScanHigh, rate);
	if(returnVal3 < 0)
	    errorCount++;
    default:
	Syslog::write("Unable to set ping rate for invalid subsystem %d!", subSystem);
	errorCount++;
    }
    // Sum up all errors.
    if(errorCount < 1)
	return( returnVal1 + returnVal2 + returnVal3 );
    else
	return(-1);
}

int Reson6046Driver::Command_SetPulsePower(int subSystem, float percentage)
{
    int returnVal1 = 0, returnVal2 = 0, returnVal3 = 0, errorCount = 0;

    switch(subSystem)
    {
    case Reson6046IF::SubBottom:
    case Reson6046IF::SideScanLow:
    case Reson6046IF::SideScanHigh:
	if(percentage < 0.0)
	{
	    Syslog::write("Invalid power level, setting to 0.0 percent!");
	    percentage = 0.0;
	}
	if(percentage > 100.0)
	{
	    Syslog::write("Invalid power level, setting to 100.0 percent!");
	    percentage = 100.0;
	}
	returnVal1 = sendCommand(Reson6046Commands::PayloadControllerCommand,
				 U32(subSystem),
				 Reson6046Commands::PLC_Command_Generic,
				 Reson6046Commands::PLC_Action_Set,
				 "TxPower = %.2f",
				 percentage);
	if(returnVal1 < 0)
	    errorCount++;
	break;
    case Reson6046IF::SideScanBoth:
	returnVal2 = Command_SetPulsePower(Reson6046IF::SideScanLow, percentage);
	if(returnVal2 < 0)
	    errorCount++;
	returnVal3 = Command_SetPulsePower(Reson6046IF::SideScanHigh, percentage);
	if(returnVal3 < 0)
	    errorCount++;
	break;
    case Reson6046IF::EdgetechAll:
	returnVal1 = Command_SetPulsePower(Reson6046IF::SubBottom, percentage);
	if(returnVal1 < 0)
	    errorCount++;
	returnVal2 = Command_SetPulsePower(Reson6046IF::SideScanLow, percentage);
	if(returnVal2 < 0)
	    errorCount++;
	returnVal3 = Command_SetPulsePower(Reson6046IF::SideScanHigh, percentage);
	if(returnVal3 < 0)
	    errorCount++;
	break;
    default:
	Syslog::write("Unable to stop unknown subsystem %d!",subSystem);
	errorCount++;
    }

    // Sum up all errors.
    if(errorCount < 1)
	return( returnVal1 + returnVal2 + returnVal3 );
    else
	return(-1);
}

// PLC Command ID  1
int Reson6046Driver::Command_StartLogging()
{
    return( sendCommand(Reson6046Commands::PayloadControllerCommand,
			Reson6046Commands::SensorIndexPLC,
			Reson6046Commands::PLC_Command_Logging,
			Reson6046Commands::PLC_Action_Set,
			"%d",
			(U32)1) );
}

int Reson6046Driver::Command_StopLogging()
{
    return( sendCommand(Reson6046Commands::PayloadControllerCommand,
			Reson6046Commands::SensorIndexPLC,
			Reson6046Commands::PLC_Command_Logging,
			Reson6046Commands::PLC_Action_Set,
			"%d",
			(U32)0) );
}

// PLC COMMAND ID  4
int Reson6046Driver::Command_GetVersion()
{
    return( sendCommand(Reson6046Commands::PayloadControllerCommand,
			Reson6046Commands::SensorIndexPLC,
			Reson6046Commands::PLC_Command_Version,
			Reson6046Commands::PLC_Action_Get,
			"") );
}

// PLC Command ID  9
int Reson6046Driver::Command_SetAlarm(int alarmID, int action)
{
    if(action < 0)
    {
	Syslog::write("Invalid action %d, setting to 0 (disabled)!", action);
	action = 0;
    }
    if(action > 1)
    {
	Syslog::write("Invalid action %d, setting to 1 (enabled)!", action);
	action = 1;
    }
    return( sendCommand(Reson6046Commands::PayloadControllerCommand,
			Reson6046Commands::SensorIndexPLC,
			Reson6046Commands::PLC_Command_Status,
			Reson6046Commands::PLC_Action_Get,
			"%d, %d",
			U32(alarmID),
			U32(action)) );
}


// PLC Command ID 10
int Reson6046Driver::Command_GetSonarStatus(int subSystem)
{
    return( sendCommand(Reson6046Commands::PayloadControllerCommand,
			Reson6046Commands::SensorIndexPLC,
			Reson6046Commands::PLC_Command_Status,
			Reson6046Commands::PLC_Action_Get,
			"%d",
			U32(subSystem)) );
}

// PLC Command ID 11
int Reson6046Driver::Command_SetVerboseMode(int action)
{
    if(action < 0)
    {
	Syslog::write("Invalid action %d, setting to 0 (disabled)!", action);
	action = 0;
    }
    if(action > 1)
    {
	Syslog::write("Invalid action %d, setting to 1 (enabled)!", action);
	action = 1;
    }
    return( sendCommand(Reson6046Commands::PayloadControllerCommand,
			Reson6046Commands::SensorIndexPLC,
			Reson6046Commands::PLC_Command_Verbose,
			Reson6046Commands::PLC_Action_Set,
			"%d",
			U32(action)) );
}

// PLC Command ID 14
int Reson6046Driver::Command_GetModules()
{
    return( sendCommand(Reson6046Commands::PayloadControllerCommand,
			Reson6046Commands::SensorIndexPLC,
			Reson6046Commands::PLC_Command_Modules,
			Reson6046Commands::PLC_Action_Get,
			"") );
}

// PLC Command ID 15
int Reson6046Driver::Command_GetRunList()
{
    return( sendCommand(Reson6046Commands::PayloadControllerCommand,
			Reson6046Commands::SensorIndexPLC,
			Reson6046Commands::PLC_Command_RunList,
			Reson6046Commands::PLC_Action_Get,
			"") );
}

// PLC Command ID 18
int Reson6046Driver::Command_Reserved1()
{
    return( sendCommand(Reson6046Commands::PayloadControllerCommand,
			Reson6046Commands::SensorIndexPLC,
			Reson6046Commands::PLC_Command_Reserved1,
			Reson6046Commands::PLC_Action_Get,
			"") );
}

// PLC Command ID 19
int Reson6046Driver::Command_Reserved2()
{
    return( sendCommand(Reson6046Commands::PayloadControllerCommand,
			Reson6046Commands::SensorIndexPLC,
			Reson6046Commands::PLC_Command_Reserved2,
			Reson6046Commands::PLC_Action_Get,
			"") );
}

// PLC Command ID 20
int Reson6046Driver::Command_SetTime()
{
    do
    {
	Time::gettime(&_timeStamp);
	dwrite(5)("sec = %12d nano = %12d.",_timeStamp.seconds,_timeStamp.nanoSeconds);
    }while(_timeStamp.nanoSeconds < 750000000);
    Time::splitTime(&_fullTime);
			
    return( sendCommand(Reson6046Commands::PayloadControllerCommand,
			Reson6046Commands::SensorIndexPLC,
			Reson6046Commands::PLC_Command_TimeSync,
			Reson6046Commands::PLC_Action_Set,
			"%04u/%02u/%02u,%02u:%02u:%8.5f",
			_fullTime.year,
			_fullTime.mon+1,
			_fullTime.mday,
			_fullTime.hour,
			_fullTime.min,
			(F32)_fullTime.sec) );
}

// PLC Command ID 21
int Reson6046Driver::Command_GetHealth(int subSystem)
{
    return( sendCommand(Reson6046Commands::PayloadControllerCommand,
			Reson6046Commands::SensorIndexPLC,
			Reson6046Commands::PLC_Command_Health,
			Reson6046Commands::PLC_Action_Get,
			"%d",
			U32(subSystem)) );
}


// ####### Error Handling #######
void Reson6046Driver::resetInitError()
{
    _output->data.initErrorCount = 0;
    _output->data.status = DeviceIF::Ok;
    _output->write();
}

void Reson6046Driver::onInitError()
{
    //just leave status Initializing
    ++_output->data.initErrorCount;
    _output->write();
    if(++_output->data.initErrorCount >= _output->data.initErrorCountLimit) {
	onInitFailure();
    }
}

void Reson6046Driver::onInitFailure()
{
    _output->data.status = DeviceIF::FailedInit;
    _output->write();
    exit(1);
}

void Reson6046Driver::resetError()
{
    _output->data.errorCount = 0;
    _output->data.status = DeviceIF::Ok;
    _output->write();
}

void Reson6046Driver::onError()
{
    _output->data.status = DeviceIF::Error;
    _output->data.errorCount++;
    _output->write();
    if( _output->data.errorCount >= _output->data.errorCountLimit) {
	onFailure();
    }
}

void Reson6046Driver::onFailure()
{
    _output->data.status = DeviceIF::FailedRead;
    _output->write();
    exit(1);
}