//=========================================================================
// Summary  : Edgetech FSDW Server
// Filename : EdgetechFSDWServer.cc
// Author   : Reed Christenson reed@bluefinrobotics.com
// Project  : Edgetech FSDW Driver
// Revision : 0.0
// Created  : 2002.03.21
// Modified : 2002.03.21
//=========================================================================
// Description : Edgetech FSDW Server
//=========================================================================

#include "EdgetechFSDWServer.h"

EdgetechFSDWServer::EdgetechFSDWServer(const char *configFileName)
    : EdgetechFSDWIF_SK()
{
    dwrite(5)("Entering server constructor...");
    _output = new EdgetechFSDWOutput(True, SharedData::ReadWrite, True);
    _output->clear();
    _output->data.deviceReady = False;
    _output->write();
//    _command = new EdgetechFSDWCommand( MessageQueue::ReadWrite );

    _driver = new EdgetechFSDWDriver();

    debug = _output->data.debug;



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

}


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

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

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


// ############################################################
// ## Edgetech FSDW Specific Commands
// ############################################################

DeviceIF::Status EdgetechFSDWServer::startEdgetechStorage(Boolean bUpdate)
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;


    dwrite(1)("Sending start storage command...");
    //    EdgetechFSDWCommand::Command cmd( EdgetechFSDWCommand::StartLogging, bUpdate);
    //    _command->write( &cmd );

    if (_driver->Command_StartEdgetechStorage(bUpdate)>=0)
    	return DeviceIF::Ok;
    else {
    	Syslog::write("EdgetechFSDW is not available.");
    	return DeviceIF::Error;
    }

}


DeviceIF::Status EdgetechFSDWServer::stopEdgetechStorage(Boolean bUpdate)
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

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

    if (_driver->Command_StopEdgetechStorage(bUpdate)>=0 )
    	return DeviceIF::Ok;
    else
    {
    	Syslog::write("EdgetechFSDW is not available.");
    	return DeviceIF::Error;
    }

}


DeviceIF::Status EdgetechFSDWServer::startSubsystem( EdgetechFSDWIF::EdgetechFSDWSonarSystems sonarSystem, Boolean bUpdate )
{
    // 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 == EdgetechFSDWIF::Multibeam) {
    //      printf("EdgetechFSDWServer creating MBStartPing command\n");
    //      EdgetechFSDWCommand::Command mbcmd( EdgetechFSDWCommand::MBStartPing, sonarSystem );
    //      _command->write( &mbcmd );
    //    }
    //    else {
    //      EdgetechFSDWCommand::Command cmd( EdgetechFSDWCommand::StartSubsystem, sonarSystem , bUpdate);
    //      _command->write( &cmd );
    //    }

    if (_driver->Command_StartSubsystem(sonarSystem,bUpdate)>=0)
    	return DeviceIF::Ok;
    else  {
    	Syslog::write("EdgetechFSDW is not available.");
    	return DeviceIF::Error;
    }

}


DeviceIF::Status EdgetechFSDWServer::stopSubsystem( EdgetechFSDWIF::EdgetechFSDWSonarSystems sonarSystem, Boolean bUpdate )
{
    // Read in shared memory.
    _output->read();  //(&_output->data);
    debug = _output->data.debug;

    dwrite(1)("Sending stop subsystem %d command...", sonarSystem);

    if (_driver->Command_StopSubsystem(sonarSystem,bUpdate) >=0)
    	return DeviceIF::Ok;
    else {
    	return DeviceIF::Error;
    	Syslog::write("EdgetechFSDW is not available.");
    }

}


DeviceIF::Status EdgetechFSDWServer::pingStatus( EdgetechFSDWIF::EdgetechFSDWSonarSystems sonarSystem,
					      unsigned short *pingStatus, Boolean bUpdate )
{
    // Get device ping status.
    Syslog::write("ERROR: pingStatus needs code!");
    return DeviceIF::Error;
}


// Set pulse filename.
DeviceIF::Status EdgetechFSDWServer::setPulseFilename( EdgetechFSDWIF::EdgetechFSDWSonarSystems sonarSystem,
						    EdgetechFSDWIF::String32 pulseFilename , Boolean bUpdate)
{    
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

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

    if (_driver->Command_SetPulseFilename(sonarSystem,pulseFilename,bUpdate) >=0)
    	return DeviceIF::Ok;
    else {
    	Syslog::write("EdgetechFSDW is not available.");
    	return DeviceIF::Error;
    }

}


// Set Ping Range.
DeviceIF::Status EdgetechFSDWServer::setPingRange( EdgetechFSDWIF::EdgetechFSDWSonarSystems sonarSystem,
						float pingRange, Boolean bUpdate )
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

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

    if (_driver->Command_SetPingRange(sonarSystem,pingRange,bUpdate) >=0 )
    	return DeviceIF::Ok;
    else {
    	Syslog::write("EdgetechFSDW is not available.");
    	return DeviceIF::Error;
    }

}


// Set Ping rate.
DeviceIF::Status EdgetechFSDWServer::setPingRate( EdgetechFSDWIF::EdgetechFSDWSonarSystems sonarSystem,
					       float pingRate , Boolean bUpdate)
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

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

    if (_driver->Command_SetPingRate(sonarSystem,pingRate,bUpdate) >= 0)
    	return DeviceIF::Ok;
    else {
    	Syslog::write("EdgetechFSDW is not available.");
    	return DeviceIF::Error;
    }
}


// Set Ping Power.
DeviceIF::Status EdgetechFSDWServer::setPingPower( EdgetechFSDWIF::EdgetechFSDWSonarSystems sonarSystem,
						float pingPower, Boolean bUpdate )
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

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

    if (_driver->Command_SetPulsePower(sonarSystem,pingPower,bUpdate) >= 0 )
    	return DeviceIF::Ok;
    else {
    	Syslog::write("EdgetechFSDW is not available.");
    	return DeviceIF::Error;
    }
}


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

    dwrite(1)("Setting Time...");
    //    EdgetechFSDWCommand::Command cmd( EdgetechFSDWCommand::SetTime, bUpdate );
    //    _command->write( &cmd );

    if (_driver->Command_SetTime(bUpdate)>=0)
    	return DeviceIF::Ok;
    else {
    	Syslog::write("EdgetechFSDW is not available.");
    	return DeviceIF::Error;
        }

}

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

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

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

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

    dwrite(1)("Requesting Edgetech FSDW version...");
    //    EdgetechFSDWCommand::Command cmd( EdgetechFSDWCommand::GetVersion);
    //    _command->write( &cmd );

    if (_driver->Command_GetVersion() >= 0)
    	return DeviceIF::Ok;
    else {
    	Syslog::write("EdgetechFSDW is not available.");
    	return DeviceIF::Error;
    }
}

DeviceIF::Status EdgetechFSDWServer::getSonarStatus( EdgetechFSDWIF::EdgetechFSDWSonarSystems sonarSystem, Boolean bUpdate )
{
    // Read in shared memory.
    _output->read();
    debug = _output->data.debug;

    dwrite(1)("Requesting sonar status for system %d...",sonarSystem);
//    EdgetechFSDWCommand::Command cmd( EdgetechFSDWCommand::GetSonarStatus, sonarSystem, bUpdate);
//    _command->write( &cmd );
//	TODO CLEAN
    if (_driver->Command_GetSonarStatus(sonarSystem) >= 0)
    	return DeviceIF::Ok;
    else {
    	Syslog::write("EdgetechFSDW is not available.");
	return DeviceIF::Error;
    }


}

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

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

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

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

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

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

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

    if (_driver->Command_StartDataLogging() >=0)
    	return DeviceIF::Ok;
    else {
    	Syslog::write("EdgetechFSDW is not available.");
    	return DeviceIF::Error;
    }
}

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

    // Send command.
    //    EdgetechFSDWCommand::Command cmd( EdgetechFSDWCommand::DataLoggingOff);
    //    _command->write( &cmd );

    dwrite(1)("Sending stop logging data command...");
    if (_driver->Command_StopDataLogging() >=0)
    	return DeviceIF::Ok;
    else {
    	Syslog::write("EdgetechFSDW is not available.");
    	return DeviceIF::Error;
    }

}

DeviceIF::Status EdgetechFSDWServer::setSBPDuration(float duration_milli, Boolean bUpdate)
{
	//  EdgetechFSDWCommand::Command cmd( EdgetechFSDWCommand::SetSBPDuration,
	//                                 0, duration_milli, bUpdate);
	//  _command->write( &cmd );

	Syslog::write("EdgetechFSDW Server - setting SBP duration to %f", duration_milli);
	if (_driver->Command_SetSBPDuration(duration_milli,bUpdate) >= 0)
		return DeviceIF::Ok;
	else {
		Syslog::write("EdgetechFSDW is not available.");
		return DeviceIF::Error;
	}
}

DeviceIF::Status EdgetechFSDWServer::setSBPGain(float gain, Boolean bUpdate)
{
	//  EdgetechFSDWCommand::Command cmd( EdgetechFSDWCommand::SetSBPGain,
	//                                 0, gain);
	//  _command->write( &cmd );

	Syslog::write("EdgetechFSDW Server - setting SBP gain to %f", gain, bUpdate);

	if (_driver->Command_SetSBPGain(gain,bUpdate)>=0)
		return DeviceIF::Ok;
	else {
    	Syslog::write("EdgetechFSDW is not available.");
    	return DeviceIF::Error;
    }
}

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

DeviceIF::Status EdgetechFSDWServer::relayCompositeSituation(EdgetechFSDWIF::sCompositeSituation * compSit) {

	timespec ts;
	ts.tv_sec = compSit->timestamp.seconds;
	ts.tv_nsec= compSit->timestamp.nanoSeconds;

	int retval = _driver->Command_RelayCompositeSituation(compSit->flags,
			compSit->vDirections,
			ts,
			compSit->latitudeD,
			compSit->longitudeD,
			compSit->depthM,
			compSit->altitudeM,
			compSit->heaveM,
			compSit->velocity1MS,
			compSit->velocity2MS,
			compSit->velocityDownMS,
			compSit->pitchD,
			compSit->rollD,
			compSit->headingD,
			compSit->soundSpeedMS,
			compSit->waterTempC);
	if (retval >= 0 )
		return DeviceIF::Ok;
	else {
		Syslog::write("EdgetechFSDW is not available.");
		return DeviceIF::Error;
	}
}

DeviceIF::Status EdgetechFSDWServer::relaySituation(EdgetechFSDWIF::sSituation* situation) {

	timespec ts;
	ts.tv_sec = situation->timestamp.seconds;
	ts.tv_nsec= situation->timestamp.nanoSeconds;

	int retval = _driver->Command_RelaySituation(
			situation->flags,
			ts,
			situation->latitudeD,
			situation->longitudeD,
			situation->depthM,
			situation->headingD,
			situation->pitchD,
			situation->rollD,
			situation->xRelativePosM,
			situation->yRelativePosM,
			situation->zRelativePosM,
			situation->xVelocityMS,
			situation->yVelocityMS,
			situation->zVelocityMS,
			situation->northVelocityMS,
			situation->eastVelocityMS,
			situation->downVelocityMS,
			situation->xAngularRateDS,
			situation->yAngularRateDS,
			situation->zAngularRateDS,
			situation->xAccelerationMS,
			situation->yAccelerationMS,
			situation->zAccelerationMS,
			situation->latitudeStandardDeviationM,
			situation->longitudeStandardDeviationM,
			situation->depthStandardDeviationM,
			situation->headingStandardDeviationD,
			situation->pitchStandardDeviationD,
			situation->rollStandardDeviationD
	);

	if (retval >=0 )
		return DeviceIF::Ok;
	else {
		Syslog::write("EdgetechFSDW is not available.");
		return DeviceIF::Error;
	}
}

DeviceIF::Status EdgetechFSDWServer::relayPressureRecord(
		EdgetechFSDWIF::sPressureRecord* pressureRecord) {

	int retval =_driver->Command_RelayPressureRecord(
    		 pressureRecord->pressurePSI,
    		 pressureRecord->tempC,
    		 pressureRecord->salPSU,
    		 pressureRecord->condSM,
    		 pressureRecord->sosMS,
    		 pressureRecord->depthM,
    		 pressureRecord->flags
    );

	if (retval >=0 )
		return DeviceIF::Ok;
	else {
		Syslog::write("EdgetechFSDW is not available.");
		return DeviceIF::Error;
	}
}

DeviceIF::Status EdgetechFSDWServer::relayAltitudeRecord(
		EdgetechFSDWIF::sAltitudeRecord* altitudeRecord) {

	int retval = _driver->Command_RelayAltitude(
			altitudeRecord->flags,
    		altitudeRecord->altitudeM,
    		altitudeRecord->forwardVelocityMS,
    		altitudeRecord->crossTrackVelocityMS);
	if (retval >=0 )
		return DeviceIF::Ok;
	else {
		Syslog::write("EdgetechFSDW is not available.");
		return DeviceIF::Error;
	}
}

int EdgetechFSDWServer::spawnAuxTasks()
{
//  char *program = "edgetechFSDWDriver";
//  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;
//  }

	// NO More Aux tasks, server will run monotonically.
    return DeviceIF::Ok;
}