//=========================================================================
// Summary  : Remote Agent Driver.
// Filename : RemoteAgentDriver.cc
// Author   : bluefinrobotics.com
// Project  : Remote Agent Driver.
// Revision : 1
// Created  : 2002
// Modified : 2003.03.11 Reed: added sendSeabirdSBE25.
//=========================================================================
// Description :  Remote Agent Driver.
//=========================================================================

#include "RemoteAgentDriver.h"
#include "ModemMessage.h"
#include "DataElement.h"
#include "ourTypes.h"
#include "System.h"
#include "Math.h"
#include "TimeIF.h"
#include "Time.h"
#include "Attributes.h"
#include "DebugAttribute.h"
#include "StringAttribute.h"
#include "IntegerAttribute.h"
#include "AttributeParser.h"

#include "MMAhrs.h"
#include "MMMultiLeica.h"
#include "MMSeacat.h"
#include "MMSeabirdSBE25.h"
#include "MMAgentRequest.h"
#include "MMActiveBehavior.h"
#include "MMBehavior.h"
#include "MMDepthSensor.h"
#include "MMDvl.h"
#include "MMDriver.h"
#include "MMGps.h"
#include "MMNavigation.h"
#include "MMTailcone.h"
#include "MMOas.h"
#include "MMSvs.h"
#include "MMPower.h"
#include "MMAlarm.h"
#include "MMHoming.h"
#include "MMBluefinBattery.h"
#include "MMDynamicControl.h"
#include "MMDeviceInfo.h"
#include "MMString.h"
#include "MMNmea.h"
#include "MMRtcm.h"
#include "MMUSBL.h"
#include "MMServer.h"
#include "MMMissionInfo.h"
#include "MMMissionName.h"
#include "MMLN250.h"
#include "MMSas21.h"
#include "MMPhins.h"
#include "MMEnvironmental.h"
#include "MMFluorometer.h"

#include "BluefinPowerSystemIF.h"
#include "NavigationIF.h"
#include "LayeredControlIF.h"
#include "TailConeIF.h"
#include "BluefinTailconeIF.h"
#include "BpauvTailConeIF.h"
#include "PowerSystemIF.h"
#include "WaterAlarmIF.h"
#include "DynamicControlIF.h"
#include "VehicleConfigurationIF.h"
#include "AhrsIF.h"
#include "CrossbowIF.h"
#include "MicrostrainIF.h"
#include "MultiLeicaIF.h"
#include "SeacatSBE19IF.h"
#include "SBE25IF.h"
#include "DepthSensorIF.h"
#include "BluefinBatteryIF.h"
#include "MultiBatteryIF.h"
#include "RdiadcpIF.h"
#include "ApogeeHomingIF.h"
#include "GpsIF.h"
#include "ExternalPosFixIF.h"
#include "RtcmIF.h"
#include "SupervisorIF.h"
#include "RangeFinderIF.h"
#include "SVSIF.h"
#include "AvtrakIF.h"
#include "Ln250IF.h"
#include "LUSBLFilterIF.h"
#include "Sas21IF.h"
#include "PhinsIF.h"
#include "BluefinEnvIF.h"
#include "FluorometerIF.h"

#include "RemoteAgentIF.h"

#include "ModemSerial.h"
#include "ModemSocket.h"
#include "ModemBroker.h"

#include "VehicleConfigurator.h"

#include "TaskIFVersion.h"
#include "RemoteVersion.h"

#include <signal.h>
#include <time.h>
#include <unistd.h>
#include <sys/name.h>
#include <sys/kernel.h>
#include <sys/types.h>
#include <sys/disk.h>
#include <vector>
#include <string>

#define SLEEP_INTERVAL 		1 	//seconds
#define DEFAULT_PERIOD 		60	//seconds
#define PING_SERVER_PERIOD 	10	//seconds
#define BATTERY_FLAGS 		8 	//number of flags

#define DEFAULT_DEBUG_LEVEL 3
#define RemoteAgentDriverConfigFileName "remoteAgentDriver.cfg"

RemoteAgentDriver* RemoteAgentDriver::_gpRemoteAgentDriver = 0;


//////////////////////////////////////
// RemoteAgent implementation
RemoteAgentDriver::RemoteAgentDriver()
{
    _gpRemoteAgentDriver = this;

    // read the config file
    loadConfiguration();

    _command = NULL;
    _command = new RemoteAgentCommand(MessageQueue::Read);

    // Set counter
    m_count = 0;

    //set attribute change counter
    _attCounter = 1;
    _misCounter = 1;

    // Init server pointers
    m_navigation = NULL;
    m_layeredControl = NULL;
    m_tailcone = NULL;
    m_power = NULL;
    m_dynamicControl = NULL;
    m_vehicleConfiguration = NULL;
    m_ahrs = NULL;
    m_crossbow = NULL;
    m_microstrain = NULL;
    m_multiLeica = NULL;
    m_seacat = NULL;
    m_seabirdSBE25 = NULL;
    m_depthSensor = NULL;
    m_battery = NULL;
    m_dvl = NULL;
    m_homing = NULL;
    m_gps = NULL;
    m_externalPosFix = NULL;
    m_rtcm = NULL;
    m_supervisor = NULL;
    m_oas = NULL;
    m_svs = NULL;
    m_avtrak = NULL;
    m_ln250 = NULL;
    m_lusbl = NULL;
    m_sas21 = NULL;
    m_phins = NULL;
    m_env = NULL;
    m_flu = NULL;

    dwrite(5)("RAD: Nulled IFs");

    //initially, allow coms
    _allowComs = True;

    // We don't wait for children to die, and we don't
    // want any zombies either...
    signal(SIGCHLD, RemoteAgentDriver::signalChildHandler);

    signal(SIGINT, RemoteAgentDriver::signalHandler);
    signal(SIGTERM, RemoteAgentDriver::signalHandler);
    signal(SIGQUIT, RemoteAgentDriver::signalHandler);

    atexit(RemoteAgentDriver::cleanup);

    int i;

    for (i=0;i<66;i++) {
	m_requestFunctionList[i] = nullRequest;
    }
    m_requestFunctionList[ModemMessage::tACK] = sendAck;
    m_requestFunctionList[ModemMessage::tMMOas] = sendOas;
    m_requestFunctionList[ModemMessage::tMMActiveBehavior] = sendActiveBehavior;
    m_requestFunctionList[ModemMessage::tMMServerAlarm] = sendServerAlarmBatch;
    m_requestFunctionList[ModemMessage::tMMLogDirName] = sendLogDirName;
    m_requestFunctionList[ModemMessage::tMMControlExecute] = sendControlExecute;
    m_requestFunctionList[ModemMessage::tMMSvs] = sendSvs;
    m_requestFunctionList[ModemMessage::tMMLN250] = sendLN250;
    m_requestFunctionList[ModemMessage::tMMNavigationPosition] = sendNavigationPosition;
    m_requestFunctionList[ModemMessage::tMMNavigationAttitude] = sendNavigationAttitude;
    m_requestFunctionList[ModemMessage::tMMTailcone] = sendTailconeRemote;
    m_requestFunctionList[ModemMessage::tMMTailconeRaw] = sendTailconeRaw;
    m_requestFunctionList[ModemMessage::tMMCircuitBatch] = sendCircuitBatchRemote;
    m_requestFunctionList[ModemMessage::tMMCircuitCurrentBatch] = sendCircuitCurrentBatch;
    m_requestFunctionList[ModemMessage::tMMCircuitInfo] = sendCircuitInfo;
    m_requestFunctionList[ModemMessage::tMMGroundFaultCurrentBatch] =sendGroundFaultCurrentBatch;
    m_requestFunctionList[ModemMessage::tMMTemperatureBatch] = sendTemperatureBatch;
    m_requestFunctionList[ModemMessage::tMMWaterAlarmBatch] = sendWaterAlarmBatch;
    m_requestFunctionList[ModemMessage::tMMDropWeightReleaseBatch] = sendDropWeightReleaseBatchRemote;
    m_requestFunctionList[ModemMessage::tMMDynamicControlCommand] = sendDynamicControlCommandRemote;
    m_requestFunctionList[ModemMessage::tMMDeviceInfo] = sendDeviceInfo;
    m_requestFunctionList[ModemMessage::tMMAhrs] = sendAhrs;
    m_requestFunctionList[ModemMessage::tMMMultiLeica] = sendMultiLeica;
    m_requestFunctionList[ModemMessage::tMMSeacat] = sendSeacat;
    m_requestFunctionList[ModemMessage::tMMSeabirdSBE25] = sendSeabirdSBE25;
    m_requestFunctionList[ModemMessage::tMMDepthSensor] = sendDepthSensor;
    m_requestFunctionList[ModemMessage::tMMBluefinBatteryInfo] = sendBluefinBatteryInfo;
    m_requestFunctionList[ModemMessage::tMMBluefinBatteryGlobal] = sendBluefinBatteryGlobal;
    m_requestFunctionList[ModemMessage::tMMDvl] = sendDvl;
    m_requestFunctionList[ModemMessage::tMMDvlBeamVelocities] = sendDvlBeamVelocities;
    m_requestFunctionList[ModemMessage::tMMDvlBeamRanges] = sendDvlBeamRanges;
    m_requestFunctionList[ModemMessage::tMMDvlAttTemp] = sendDvlAttTemp;
    m_requestFunctionList[ModemMessage::tMMHoming] = sendHoming;
    m_requestFunctionList[ModemMessage::tMMGps] = sendGps;
    m_requestFunctionList[ModemMessage::tMMNmea] = sendNmea;
    m_requestFunctionList[ModemMessage::tMMDriver] = sendDriverStatus;
    m_requestFunctionList[ModemMessage::tMMAbortMission] = sendAbortMission;
    m_requestFunctionList[ModemMessage::tMMKillMission] = sendKillMission;
    m_requestFunctionList[ModemMessage::tMMSas21] = sendSas21;
    m_requestFunctionList[ModemMessage::tMMPhins] = sendPhins;
    m_requestFunctionList[ModemMessage::tMMEnvironmental] = sendEnvironmental;
    m_requestFunctionList[ModemMessage::tMMFluorometer] = sendFluorometer;


    dwrite(5)("RAS: set request function list");

    //
    for (i=0;i<66;i++) {
	m_handleFunctionList[i] = nullHandler;
    }

    m_handleFunctionList[ModemMessage::tMMAgentRequest] = handleRequest;
    m_handleFunctionList[ModemMessage::tMMBehavior] = handleBehavior;
    m_handleFunctionList[ModemMessage::tMMBehaviorArray] = handleBehaviorArray;
    m_handleFunctionList[ModemMessage::tMMJumpBehavior] = handleJumpBehavior;
    m_handleFunctionList[ModemMessage::tMMDriver] = handleDriverCommand;
    m_handleFunctionList[ModemMessage::tMMMissionName] = handleMissionName;
    m_handleFunctionList[ModemMessage::tMMTailcone] = sendTailconeLocal;
    m_handleFunctionList[ModemMessage::tMMCircuit] = sendCircuitLocal;
    m_handleFunctionList[ModemMessage::tMMCircuitBatch] = sendCircuitBatchLocal;
    m_handleFunctionList[ModemMessage::tMMDropWeightRelease] = sendDropWeightReleaseLocal;
    m_handleFunctionList[ModemMessage::tMMDynamicControlCommand] = sendDynamicControlCommandLocal;
    m_handleFunctionList[ModemMessage::tMMDynamicControlCommandFull] = sendDynamicControlCommandFullLocal;
    m_handleFunctionList[ModemMessage::tMMWatchdog] = sendWatchdogCommand;
    m_handleFunctionList[ModemMessage::tMMString] = handleString;
    m_handleFunctionList[ModemMessage::tMMNmea] = sendExternalPosFix;
    m_handleFunctionList[ModemMessage::tMMRtcm] = sendRtcmMessage;
    m_handleFunctionList[ModemMessage::tMMUSBLFix] = sendUSBLFixMessage;
    m_handleFunctionList[ModemMessage::tMMServer] = serverMessage;
    m_handleFunctionList[ModemMessage::tMMMissionInfo] = sendMissionInfo;

    dwrite(5)("RAS: set handle function list");

    m_speed = 0;
    resetMessageFlags(0); // 0 is slow
    resetMessageFlags(1); // 1 is fast

    //
    i = 0;
    m_serverNameList[i] = strdup(NavigationIFServerName);
    m_serverList[i++] = m_navigation;
    m_serverNameList[i] = strdup(LayeredControlIFServerName);
    m_serverList[i++] = m_layeredControl;
    m_serverNameList[i] = strdup(BluefinTailconeIFServerName);
    m_serverList[i++] = m_tailcone;
    m_serverNameList[i] = strdup(BluefinPowerSystemIFServerName);
    m_serverList[i++] = m_power;
    m_serverNameList[i] = strdup(DynamicControlIFServerName);
    m_serverList[i++] = m_dynamicControl;
    m_serverNameList[i] = strdup(VehicleConfigurationIFServerName);
    m_serverList[i++] = m_vehicleConfiguration;
    m_serverNameList[i] = strdup(AhrsIFServerName);
    m_serverList[i++] = m_ahrs;
    m_serverNameList[i] = strdup(CrossbowIFServerName);
    m_serverList[i++] = m_crossbow;
    m_serverNameList[i] = strdup(MicrostrainIFServerName);
    m_serverList[i++] = m_microstrain;
    m_serverNameList[i] = strdup(MultiLeicaIFServerName);
    m_serverList[i++] = m_multiLeica;
    m_serverNameList[i] = strdup(SeacatSBE19IFServerName);
    m_serverList[i++] = m_seacat;
    m_serverNameList[i] = strdup(SBE25IFServerName);
    m_serverList[i++] = m_seabirdSBE25;
    m_serverNameList[i] = strdup(DepthSensorIFServerName);
    m_serverList[i++] = m_depthSensor;
    m_serverNameList[i] = strdup(BluefinBatteryIFServerName);
    m_serverList[i++] = m_battery;
    m_serverNameList[i] = strdup(RdiadcpIFServerName);
    m_serverList[i++] = m_dvl;
    m_serverNameList[i] = strdup(ApogeeHomingIFServerName);
    m_serverList[i++] = m_homing;
    m_serverNameList[i] = strdup(GpsIFServerName);
    m_serverList[i++] = m_gps;
    m_serverNameList[i] = strdup(ExternalPosFixIFServerName);
    m_serverList[i++] = m_externalPosFix;
    m_serverNameList[i] = strdup(RtcmIFServerName);
    m_serverList[i++] = m_rtcm;
    m_serverNameList[i] = strdup(SupervisorIFServerName);
    m_serverList[i++] = m_supervisor;
    m_serverNameList[i] = strdup(RangeFinderIFServerName);
    m_serverList[i++] = m_oas;
    m_serverNameList[i] = strdup(SVSIFServerName);
    m_serverList[i++] = m_svs;
    m_serverNameList[i] = strdup(AvtrakIFServerName);
    m_serverList[i++] = m_avtrak;
    m_serverNameList[i] = strdup(Ln250IFServerName);
    m_serverList[i++] = m_ln250;
    m_serverNameList[i] = strdup(LUSBLFilterIFServerName);
    m_serverList[i++] = m_lusbl;
    m_serverNameList[i] = strdup(Sas21IFServerName);
    m_serverList[i++] = m_sas21;
    m_serverNameList[i] = strdup(PhinsIFServerName);
    m_serverList[i++] = m_phins;
    m_serverNameList[i] = strdup(BluefinEnvIFServerName);
    m_serverList[i++] = m_env;
    m_serverNameList[i] = strdup(FluorometerIFServerName);
    m_serverList[i++] = m_flu;

    dwrite(5)("RAS: set server name list finished");

    m_numServers = i;

    // Create the servers
    createServers();

}

void RemoteAgentDriver::nullRequest()
{
    fprintf(stderr,"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
    fprintf(stderr,"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
    fprintf(stderr,"serious bug in remoteAgentDriver request\n");
}

void RemoteAgentDriver::nullHandler(ModemMessage *mm)
{
    fprintf(stderr,"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
    fprintf(stderr,"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
    fprintf(stderr,"serious bug in remoteAgentDriver handler\n");
}

RemoteAgentDriver * RemoteAgentDriver::getRemoteAgentDriver()
{
    return _gpRemoteAgentDriver;
}


RemoteAgentDriver::~RemoteAgentDriver()
{
    clean(m_navigation);
    clean(m_layeredControl);
    clean(m_tailcone);
    clean(m_power);
    clean(m_dynamicControl);
    clean(m_vehicleConfiguration);
    clean(m_ahrs);
    clean(m_crossbow);
    clean(m_microstrain);
    clean(m_multiLeica);
    clean(m_depthSensor);
    clean(m_battery);
    clean(m_dvl);
    clean(m_homing);
    clean(m_gps);
    clean(m_externalPosFix);
    clean(m_rtcm);
    clean(m_supervisor);
    clean(m_oas);
    clean(m_svs);
    clean(m_ln250);
    clean(m_lusbl);
    clean(m_sas21);
    clean(m_phins);
    clean(m_env);
    clean(m_flu);

    clean(_modemBroker);
}

void RemoteAgentDriver::resetMessageFlags(int speed)
{
    // Flags + Period
    for (int i=0;i<ModemMessage::tEND;i++) {
	m_periodList[speed][i] = DEFAULT_PERIOD;
	m_flagList[speed][i] = MMAgentRequest::Stop;
    }
}

void RemoteAgentDriver::loadConfiguration()
{
    Attributes attributes("remoteAgentDriver");

    attributes.add(new StringAttribute("remoteSocketIP",
				       "IP address for a ModemSocket",
				       (char**)&_remoteSocketIP, NULL));

    attributes.add(new StringAttribute("port1",
				       "serial port for serial modem 1",
				       (char**)&_serialPort1,""));

    attributes.add(new IntegerAttribute("baud1",
					"baud rate for serial modem 1",
					&_baudRate1,9600));

    attributes.add(new IntegerAttribute("baud2",
					"baud rate for serial modem 2",
					&_baudRate2,9600));

    attributes.add(new StringAttribute("port2",
				       "serial port for serial modem 1",
				       (char**)&_serialPort2,""));

    attributes.add(new DebugAttribute("debug",
				      "Debug flag", &debug, DEFAULT_DEBUG_LEVEL));

    try {

	AttributeParser::parse(System::configurationFile(RemoteAgentDriverConfigFileName),
			       &attributes);
    } catch(...) {
	dwrite(0)("Attribute parse failed");
    }
}

int RemoteAgentDriver::init()
{
    _modemBroker = new ModemBroker();

    _modemBroker->addModem(new ModemSocket( "0" ));

    if (strlen(_serialPort1) > 0) {
	_modemBroker->addModem(new ModemSerial(_serialPort1,(int)_baudRate1) );
    }
    return true;
}


void RemoteAgentDriver::run()
{

    init();

    while (true) {
	System::milliSleep(10);

	Assert(_modemBroker);
	_modemBroker->runCycle();

	if (m_count != time(NULL)) {
	    m_count = time(NULL);

	    try {
		// Look for servers
		verifyServers();

		// Check up on connection speed
		long oldSpeed = _connectionSpeed;
		_connectionSpeed = _modemBroker->getBytesPerSecond();
		if (_connectionSpeed < 2000) {
		    m_speed = 0;
		} else {
		    m_speed = 1;
		}

		// Maintain event service
		if(_allowComs) {
		    sendMessages();
		}

		// Check if any servers have bounced back
		if (checkForSending(True,PING_SERVER_PERIOD)) createServers();

		//polling for layered control acknowledgements
		pollAcks();

		//polling layered control for controlModem information
		if(m_layeredControl != NULL)
		    _allowComs = m_layeredControl->getAllowComs();

		processRequests();

	    } catch (Exception e) {
		dwrite(0)("exception: %s",e.msg);
	    }
	}

    }
}

void RemoteAgentDriver::processRequests()
{

    RemoteAgentCommand::Command cmdIn;

    while( (_command->read( &cmdIn )) > 0 ) {

	switch( cmdIn.cmd ) {
	case RemoteAgentCommand::RequestCommand:
	    processRequestCommand(cmdIn._mm, cmdIn._tt, cmdIn._p);
	    break;

	case RemoteAgentCommand::PoinkerCommand:
	    processPoinkerCommand(cmdIn._joint);
	    break;
	}
    }
}



void RemoteAgentDriver::createServers()
{
    dwrite(4)("createServers");

    try {
	if (!m_navigation && System::findServer(NavigationIFServerName) > 0) {
	    m_navigation = new NavigationIF("remoteAgentDriver");
	    dwrite(4)("Got navigation server.");
	}
    } catch (Exception e) {
	dwrite(1)("Exception getting navigation server:\n\t%s",e.msg);
    }
    try {
	if (!m_layeredControl && System::findServer(LayeredControlIFServerName) > 0) {
	    m_layeredControl = new LayeredControlIF("remoteAgentDriver");
	    dwrite(4)("Got layered control server.");
	}
    } catch (Exception e) {
	dwrite(1)("Exception getting layered control server:\n\t%s",e.msg);
    }
    try {
	if (!m_tailcone && System::findServer(BluefinTailconeIFServerName) > 0) {
	    m_tailcone = new BluefinTailconeIF("remoteAgentDriver");
	    dwrite(4)("Got tailcone server.");
	}
    } catch (Exception e) {
	dwrite(1)("Exception getting tailcone server:\n\t%s",e.msg);
    }
    try {
	if (!m_power && System::findServer(BluefinPowerSystemIFServerName) > 0) {
	    m_power = new BluefinPowerSystemIF("remoteAgentDriver");
	    dwrite(4)("Got power server.");
	}
    } catch (Exception e) {
	dwrite(1)("Exception getting power system server:\n\t%s",e.msg);
    }
    try {
	if (!m_dynamicControl && System::findServer(DynamicControlIFServerName) > 0) {
	    m_dynamicControl = new DynamicControlIF("remoteAgentDriver");
	    dwrite(4)("Got dynamic control server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting dynamic control server:\n\t%s",e.msg);
    }
    try {
	if (!m_vehicleConfiguration && System::findServer(VehicleConfigurationIFServerName) > 0) {
	    m_vehicleConfiguration = new VehicleConfigurationIF("remoteAgentDriver");
	    dwrite(4)("Got vehicle configuration server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting vehicle configuration server:\n\t%s",e.msg);
    }
    try {
	if (!m_ahrs && System::findServer(AhrsIFServerName) > 0) {
	    m_ahrs = new AhrsIF("remoteAgentDriver");
	    dwrite(4)("Got ahrs server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting ahrs server:\n\t%s",e.msg);
    }
    try {
	if (!m_crossbow && System::findServer(CrossbowIFServerName) > 0) {
	    m_crossbow = new CrossbowIF("remoteAgentDriver");
	    dwrite(4)("Got crossbow server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting crossbow server:\n\t%s",e.msg);
    }
    try {
	if (!m_microstrain && System::findServer(MicrostrainIFServerName) > 0) {
	    m_microstrain = new MicrostrainIF("remoteAgentDriver");
	    dwrite(4)("Got microstrain server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting microstrain server:\n\t%s",e.msg);
    }
    try {
	if (!m_multiLeica && System::findServer(MultiLeicaIFServerName) > 0) {
	    m_multiLeica = new MultiLeicaIF("remoteAgentDriver");
	    dwrite(4)("Got multiLeica server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting multiLeica server:\n\t%s",e.msg);
    }

    try {
	if (!m_seacat && System::findServer(SeacatSBE19IFServerName) > 0) {
	    m_seacat = new SeacatSBE19IF("remoteAgentDriver");
	    dwrite(4)("Got seacat server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting seacat server:\n\t%s", e.msg);
    }

    try {
	if (!m_seabirdSBE25 && System::findServer(SBE25IFServerName) > 0) {
	    m_seabirdSBE25 = new SBE25IF("remoteAgentDriver");
	    dwrite(4)("Got Seabird SBE25 server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting Seabird SBE25 server:\n\t%s", e.msg);
    }

    try {
	if (!m_depthSensor && System::findServer(DepthSensorIFServerName) > 0) {
	    m_depthSensor = new DepthSensorIF("remoteAgentDriver");
	    dwrite(4)("Got depth sensor server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting depth sensor server:\n\t%s",e.msg);
    }
    try {
	if (!m_battery && System::findServer(BluefinBatteryIFServerName) > 0) {
	    m_battery = new BluefinBatteryIF("remoteAgentDriver");
	    dwrite(4)("Got bluefin battery server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting lipoly battery server:\n\t%s",e.msg);
    }
    try {
	if (!m_dvl && System::findServer(RdiadcpIFServerName) > 0) {
	    m_dvl = new RdiadcpIF("remoteAgentDriver");
	    dwrite(4)("Got rdi adcp server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting rdi adcp server:\n\t%s",e.msg);
    }
    try {
	if (!m_homing && System::findServer(ApogeeHomingIFServerName) > 0) {
	    m_homing = new ApogeeHomingIF("remoteAgentDriver");
	    dwrite(4)("Got apogee homing server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting apogee homing server:\n\t%s",e.msg);
    }
    try {
	if (!m_gps && System::findServer(GpsIFServerName) > 0) {
	    m_gps = new GpsIF("remoteAgentDriver");
	    dwrite(4)("Got gps server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting gps server:\n\t%s",e.msg);
    }
    try {
	if (!m_externalPosFix && System::findServer(ExternalPosFixIFServerName) > 0) {
	    m_externalPosFix = new ExternalPosFixIF("remoteAgentDriver");
	    dwrite(4)("Got externalposfix server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting externalposfix server:\n\t%s",e.msg);
    }
    try {
	if (!m_rtcm && System::findServer(RtcmIFServerName) > 0) {
	    m_rtcm = new RtcmIF("remoteAgentDriver");
	    dwrite(4)("Got rtcm server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting rtcm server:\n\t%s",e.msg);
    }
    try {
	if (!m_supervisor && System::findServer(SupervisorIFServerName) > 0) {
	    m_supervisor = new SupervisorIF("remoteAgentDriver");
	    dwrite(4)("Got supervisor server.");
	}
    } catch (Exception e) {
	dwrite(1)("Caught exception getting supervisor server:\n\t%s",e.msg);
    }
    try {
	if (!m_oas && System::findServer(RangeFinderIFServerName) > 0) {
	    m_oas = new RangeFinderIF("remoteAgentDriver");
	    dwrite(4)("Got oas server.");
	}
    } catch (Exception e){
	dwrite(1)("Caught exception getting oas server:\n\t%s",e.msg);
    }
    try {
	if (!m_svs && System::findServer(SVSIFServerName) > 0) {
	    m_svs = new SVSIF("remoteAgentDriver");
	    dwrite(4)("Got svs server.");
	}
    } catch (Exception e){
	dwrite(1)("Caught exception getting svs server:\n\t%s",e.msg);
    }
    try {
	if (!m_avtrak && System::findServer(AvtrakIFServerName) > 0) {
	    m_avtrak = new AvtrakIF("remoteAgentDriver");
	    dwrite(4)("Got avtrak server.");
	}
    } catch (Exception e){
	dwrite(1)("Caught exception getting avtrak server:\n\t%s",e.msg);
    }

    try {
	if (!m_ln250 && System::findServer(Ln250IFServerName) > 0) {
	    m_ln250 = new Ln250IF("remoteAgentDriver");
	    dwrite(4)("Got ln250 server.");
	}
    } catch (Exception e){
	dwrite(1)("Caught exception getting ln250 server:\n\t%s",e.msg);
    }

    try {
	if (!m_lusbl && System::findServer(LUSBLFilterIFServerName) > 0) {
	    m_lusbl = new LUSBLFilterIF("remoteAgentDriver");
	    dwrite(4)("Got lusbl server.");
	}
    } catch (Exception e){
	dwrite(1)("Caught exception getting ln250 server:\n\t%s",e.msg);
    }

    try {
	if(!m_sas21 && System::findServer(Sas21IFServerName) > 0) {
	    m_sas21 = new Sas21IF("remoteAgentDriver");
	    dwrite(4)("Got Sas21 server.");
	}
    } catch (Exception e){
	dwrite(1)("Caught exception getting sas21 server:\n\t%s",e.msg);
    }

    try {
        if(!m_phins && System::findServer(PhinsIFServerName) > 0) {
            m_phins = new PhinsIF("remoteAgentDriver");
            dwrite(4)("Got Phins server.");
        }
    } catch (Exception e){
        dwrite(1)("Caught exception getting Phins server:\n\t%s",e.msg);
    }

    dwrite(4)("pre-shizzle");

    try {
        if(!m_env && System::findServer(BluefinEnvIFServerName) > 0) {
            m_env = new BluefinEnvIF("remoteAgentDriver");
            dwrite(4)("Got BluefinEnv server.");
        }
    } catch (Exception e){
        dwrite(1)("Caught exception getting BluefinEnv server:\n\t%s",e.msg);
    }


    try {
        if(!m_flu && System::findServer(FluorometerIFServerName) > 0) {
            m_flu = new FluorometerIF("remoteAgentDriver");
            dwrite(4)("Got Fluorometer server.");
        }
    } catch (Exception e){
        dwrite(1)("Caught exception getting Fluorometer server:\n\t%s",e.msg);
    }


}

void RemoteAgentDriver::verifyServers()
{
	// PLEASE READ!!
	// Okay, so there's a super-ugly hack in the reloadVehicle() function
	// later in this module that depends heavily on the behavior of this
	// function.
	//
	// This function _should_ do more than just set the interfaces to
	// NULL if the server can't be found (since they were allocated with
	// the new operator, they should be destroyed with the delete operator).
	//
	// The handleString hack (for reloading the config) uses the fact that
	// this function just NULL's the pointer - if this function is ever
	// changed to do the delete(), the hack won't work.
	//
	// It's YOUR responsibility to change reloadVehicle() if you change this function!!


    for (int i=0;i<m_numServers;i++) {
	if (System::findServer(m_serverNameList[i]) < 1) m_serverList[i] = NULL;
    }

}

/////////////////////////////////////////////////////////////////////


void RemoteAgentDriver::send(ModemMessage* pMessage)
{
    Assert(_gpRemoteAgentDriver->_modemBroker);
    /*
    if (_gpRemoteAgentDriver->_modemBroker->getBytesPerSecond() < 2000) {
	_gpRemoteAgentDriver->m_speed = 0;
    } else {
	_gpRemoteAgentDriver->m_speed = 1;
    }
    */
    if (_gpRemoteAgentDriver->_modemBroker->hasConnection()) {
	_gpRemoteAgentDriver->_modemBroker->addMessage(pMessage);
	return;
    }
    delete pMessage;
}

////////////////////////////////////////////////////////////////////
void RemoteAgentDriver::receive(const ModemMessage* pMessage)
{
    ModemMessage * mm = new ModemMessage(pMessage);
    (RemoteAgentDriver::_gpRemoteAgentDriver->*m_handleFunctionList[pMessage->getType()])(mm);
    delete mm;
}

////////////////////////////////////////////////////////////////////
void RemoteAgentDriver::handleRequest(ModemMessage* mm)
{
    MMAgentRequest message(mm);
    m_flagList[m_speed][message.getRequestType()] = message.getRequest();
    m_periodList[m_speed][message.getRequestType()] = message.getPeriod();
    switch (message.getRequest()) {
    case MMAgentRequest::Stop:
	break;

    case MMAgentRequest::Start:
    case MMAgentRequest::Single:
	(RemoteAgentDriver::_gpRemoteAgentDriver->*m_requestFunctionList[message.getRequestType()])();
	break;

    case MMAgentRequest::OnUpdate:
	break;

    case MMAgentRequest::WithNav:
	break;

    default:
	dwrite(3)("Got an unrecognized agent request %d",
		  message.getRequestType());
	break;
    }
}

////////////////////////////////////////////////////////////////////
void RemoteAgentDriver::sendMessages()
{
    for (int i=0;i<ModemMessage::tEND;i++) {
	if ((m_flagList[m_speed][i] == MMAgentRequest::Start) && (fmod(m_count*SLEEP_INTERVAL,m_periodList[m_speed][i]) == 0)) {
	    (RemoteAgentDriver::_gpRemoteAgentDriver->*m_requestFunctionList[i])();
	}
    }
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendServerAlarm(int server)
{
    Boolean value;
    switch ((MMServerAlarm::Server)server) {
    case MMServerAlarm::Navigation:
	value = (m_navigation == NULL);
	break;
    case MMServerAlarm::LayeredControl:
	value = (m_layeredControl == NULL);
	break;
    case MMServerAlarm::TailCone:
	value = (m_tailcone == NULL);
	break;
    case MMServerAlarm::PowerSystem:
	value = (m_power == NULL);
	break;
    case MMServerAlarm::WaterAlarm:
	value = (m_power == NULL);
	break;
    case MMServerAlarm::DropWeight:
	value = (m_power == NULL);
	break;
    case MMServerAlarm::DynamicControl:
	value = (m_navigation == NULL);
	break;
    case MMServerAlarm::VehicleConfiguration:
	value = (m_vehicleConfiguration == NULL);
	break;
    case MMServerAlarm::Ahrs:
	value = (m_ahrs == NULL);
	break;
    case MMServerAlarm::DepthSensor:
	value = (m_depthSensor == NULL);
	break;
    case MMServerAlarm::Battery:
	value = (m_battery == NULL);
	break;
    case MMServerAlarm::Dvl:
	value = (m_dvl == NULL);
	break;
    case MMServerAlarm::Homing:
	value = (m_homing == NULL);
	break;
    case MMServerAlarm::Gps:
	value = (m_gps == NULL);
	break;
    case MMServerAlarm::ExternalPosFix:
	value = (m_externalPosFix == NULL);
	break;
    default:
	Assert(0);
	return;
    }
    MMServerAlarm alarm(server,value);
    send( new ModemMessage(&alarm) );
    dwrite(4)("sent server %d alarm %d",server,value);
}



void RemoteAgentDriver::reloadVehicle()
{
	Boolean debug = True;

	// HACK. How gross is this - verifyServers is gonna wipe things out,
    //       but we need to make an IF call _after_ they get wiped out,
	//       so essentially we cache this pointer here. Better make
	//       sure if someone ever fixes verifyServers() to actually
	//       delete() the IF's that this hack get's fixed.

	// Cache a pointer to the supervisorIF.
	SupervisorIF *supervisorIF = m_supervisor;

	//and  cache a remoteAgentIF
	RemoteAgentIF* heinous = new RemoteAgentIF("I_am_your_son");

	dprintf("RemoteAgentDriver::reloadVehicle() - Telling vehicleConfigurationServer to reload.");
	// First tell vcs to reload the config
	if(m_vehicleConfiguration != NULL )
		m_vehicleConfiguration->reloadTables();

	dprintf("RemoteAgentDriver::reloadVehicle() - Reregistering with vehicleConfigurationServer.");

	//We need to register our parent
	//Best way to communicate is through the IF
	//So we are doing this weird incestuous stuff to communicate with the server
	//it really really ain't purty
	heinous->reregisterServer();
	delete heinous;

	dprintf("RemoteAgentDriver::reloadVehicle() - Nulling interface list.");
	// Now make sure our list of IF's gets nulled out;
	verifyServers();
	createServers();

	dprintf("RemoteAgentDriver::reloadVehicle() - Telling supervisor to reload servers.");
	// Use the "cached" supervisorIF to reload the config.
	supervisorIF->reloadConfig();

	dprintf("RemoteAgentDriver::reloadVehicle() - Done reloading (though the SupervisorIF call may still be going).");
}

void RemoteAgentDriver::processRequestCommand(short mmtype, short timeType, short period)
{
    dwrite(4)("RAD -- processRemoteCommand called with mm: %d tt: %d period: %d\n", mmtype, timeType, period);

    m_flagList[m_speed][mmtype] = timeType;
    m_periodList[m_speed][mmtype] = period;

    switch (timeType) {
    case MMAgentRequest::Stop:
	break;

    case MMAgentRequest::Start:
    case MMAgentRequest::Single:
	fprintf(stderr,"n");
	(RemoteAgentDriver::_gpRemoteAgentDriver->*m_requestFunctionList[mmtype])();
	fprintf(stderr,"m");
	break;

    case MMAgentRequest::OnUpdate:
	break;

    case MMAgentRequest::WithNav:
	break;

    default:
	dwrite(1)("processRemoteCommand -- Got an unrecognized timeType %d",
		  timeType);
	break;
    }
}

void RemoteAgentDriver::processPoinkerCommand(char * joint)
{
    if (strcmp("packDone",joint) == 0) {
	dwrite(4)("POINKER: packDone!");
	MMString message("packDone");
	send(new ModemMessage(&message));
    }
}


void RemoteAgentDriver::handleString(ModemMessage *mm)
{
    //so this takes strings, and seperates the arguments
    //where the arguments are seperated by commas
    //make sure that the dashboard is constructing the messages right

    MMString message(mm);
    char str[256];
    message.getString(str);

    string hold;
    vector<string*> sepStrings;
    string* tempString;
    string::size_type  next;
    string::size_type cur = 0;
    string::size_type r;

    hold = str;

    while(1){
	next = hold.find(",");
	if(next == hold.npos)
	    next = hold.length();
	tempString = new string(hold.substr(0, next));
	sepStrings.push_back(tempString);
	if(next+1 >= hold.length()) break;
	hold = hold.substr(next+1, hold.length());
    }

    //write your own.  It's fun!

    for(int j = 0; j < sepStrings.size(); j++){
	dwrite(4)("RAS: handleString--arg %d is %s", j,sepStrings[j]->c_str());
    }

    //add your comparitor here
    if(!(strncmp(sepStrings[0]->c_str(), "calibrateAhrs",strlen("calibrateAhrs")))){
	if(m_ahrs != NULL)
	    m_ahrs->calibrate();
    } else if(!(strncmp(sepStrings[0]->c_str(), "requestVersions",
			strlen("requestVersions")))) {
	dwrite(4)("Got string command for requestVersions");

	char version[256];

	sprintf(version,"version,remoteLib_%s,taskIF_%s", REMOTEVERSION, TASKIFVERSION);

	dwrite(4)("Sending version info :%s\n", version);

	MMString * message = new MMString(version);

	send(new ModemMessage(message));

	delete message;

    } else if(!(strncmp(sepStrings[0]->c_str(), "avtrakTranspondMode",
			strlen("avtrakTranspondMode")))) {
	dwrite(4)("Got string command for avtrakTranspondMode");
	long v[4];
	v[0] = 0;
        v[1] = 1;
        v[2] = 2;
        v[3] = 3;
	if(m_avtrak != NULL)
	    m_avtrak->controlAvtrak(1,16,5,16,5,v,0,0.1,0);
    } else if(!(strncmp(sepStrings[0]->c_str(), "avtrakTransceiveMode",
			strlen("avtrakTransceiveMode")))) {
	dwrite(4)("Got string command for avtrakTransceiveMode");
	long v[4];
	v[0] = 0;
        v[1] = 1;
        v[2] = 2;
        v[3] = 3;
	if(m_avtrak != NULL)
	    m_avtrak->controlAvtrak(2,16,5,16,5,v,10,4,0);
    } else if(!(strncmp(sepStrings[0]->c_str(), "avtrakTrigger",
			strlen("avtrakTrigger")))) {
	dwrite(4)("Got string command for avtrakTrigger");
	if(m_avtrak != NULL)
	    m_avtrak->triggerSynchPulse();
    } else if(!(strncmp(sepStrings[0]->c_str(), "pack",
			strlen("pack")))) {
	dwrite(4)("Got string command to pack %s",sepStrings[1]->c_str());
	pack(sepStrings[1]->c_str());
    } else if(!(strncmp(sepStrings[0]->c_str(), "!",
			strlen("!")))) {
	dwrite(4)("Got string command to execute %s",sepStrings[1]->c_str());
	system(sepStrings[1]->c_str());
    } else if(!(strncmp(sepStrings[0]->c_str(), "dvlSlaveModeOn",
			strlen("dvlSlaveModeOn")))) {
	dwrite(4)("Got string command for dvlSlaveModeOn");
	if(m_dvl != NULL)
	    m_dvl->setSlaveModeOn();
    } else if(!(strncmp(sepStrings[0]->c_str(), "dvlSlaveModeOff",
			strlen("dvlSlaveModeOff")))) {
	dwrite(4)("Got string command for dvlSlaveModeOff");
	if(m_dvl != NULL)
	    m_dvl->setSlaveModeOff();
    } else if(!strncmp(sepStrings[0]->c_str(), "ln250FreeInertial",
		       strlen("ln250FreeInertial"))) {
	dwrite(4)("Got string command for ln250FreeInertial");
	if(m_ln250 != NULL)
	    m_ln250->forceInsNavStart();
    } else if(!strncmp(sepStrings[0]->c_str(), "ln250MotionAlign",
		       strlen("ln250MotionAlign"))) {
	dwrite(4)("Got string command for ln250MotionAlign");
	if(m_ln250 != NULL)
	    m_ln250->startAlign(Ln250IF::TransferAlign);
    } else if(!strncmp(sepStrings[0]->c_str(), "ln250StatAlign",
		       strlen("ln250StatAlign"))) {
	dwrite(4)("Got string command for ln250StatAlign");
	if(m_ln250 != NULL)
	    m_ln250->startAlign(Ln250IF::StationaryAlign);
    } else if(!strncmp(sepStrings[0]->c_str(), "reloadVehicle",
		       strlen("reloadVehicle"))) {
	dwrite(4)("Got string command for vehicle reload");
	reloadVehicle();
    } else if(!strncmp(sepStrings[0]->c_str(), "headingControlStart",
			  strlen("headingControlStart"))) {
  	int duration = atoi(sepStrings[1]->c_str());
  	int depth = atoi(sepStrings[2]->c_str());
  	int rpms = atoi(sepStrings[3]->c_str());
  	int temp = atoi(sepStrings[4]->c_str());
  	double heading = temp*1.0;
  	dwrite(4)("Got string heading control message: duration %d rpm %d depth %d heading %g\n",
  		  duration, rpms, depth, heading);

  	LayeredControlIF::NewData _newData;
  	_newData.state = 0;
  	//this very dependent on having a depth envelope
  	_newData.priorityNum = 2;
  	_newData.changeNum = -1;
  	memset(_newData.attName, 0, sizeof(_newData.attName));
  	memset(_newData.data, 0, sizeof(_newData.data));
  	sprintf(_newData.attName, "%s", "heading");
  	sprintf(_newData.data, "%g", heading);
  	if(m_layeredControl != NULL)
  	    m_layeredControl->changeAttData(&_newData);

  	memset(_newData.attName, 0, sizeof(_newData.attName));
  	memset(_newData.data, 0, sizeof(_newData.data));
  	sprintf(_newData.attName, "%s", "depth");
  	sprintf(_newData.data, "%d", depth);
  	if(m_layeredControl != NULL)
  	    m_layeredControl->changeAttData(&_newData);

  	memset(_newData.attName, 0, sizeof(_newData.attName));
  	memset(_newData.data, 0, sizeof(_newData.data));
  	sprintf(_newData.attName, "%s", "rpm");
  	sprintf(_newData.data, "%d", rpms);
  	if(m_layeredControl != NULL)
  	    m_layeredControl->changeAttData(&_newData);

  	memset(_newData.attName, 0, sizeof(_newData.attName));
  	memset(_newData.data, 0, sizeof(_newData.data));
  	sprintf(_newData.attName, "%s", "timer");
  	sprintf(_newData.data, "%d", duration);
  	if(m_layeredControl != NULL)
  	    m_layeredControl->changeAttData(&_newData);
      } else if(!strncmp(sepStrings[0]->c_str(), "headingControlStop",
  		       strlen("headingControlStop"))) {

  	LayeredControlIF::NewData _newData;
  	_newData.state = 0;
  	//this very dependent on having a depth envelope
  	_newData.priorityNum = 2;
  	_newData.changeNum = -1;
  	memset(_newData.attName, 0, sizeof(_newData.attName));
  	memset(_newData.data, 0, sizeof(_newData.data));
  	sprintf(_newData.attName, "%s", "timer");
  	sprintf(_newData.data, "%d", 0);
  	if(m_layeredControl != NULL)
  	    m_layeredControl->changeAttData(&_newData);
	/*
      } else if(!strncmp(sepStrings[0]->c_str(), "batteryAutoMode",
			 strlen("batteryAutoMode"))) {
	  dwrite(4)("Got string batteryAutoMode command");
	  if(m_battery != NULL) {
	      m_battery->setAutoMode(True);
	  }
      } else if(!strncmp(sepStrings[0]->c_str(), "batteryManualMode",
			 strlen("batteryManualMode"))) {
	  dwrite(4)("Got string batteryManualMode command");
	  if(m_battery != NULL) {
	      m_battery->setAutoMode(False);
	  }
	*/
      } else if(!strncmp(sepStrings[0]->c_str(), "batteryOn",
			 strlen("batteryOn"))) {
	  int on = atoi(sepStrings[1]->c_str());
	  dwrite(4)("Got string command battery on #%d", on);
	  if(m_battery != NULL)
	      m_battery->turnOn(on);
      } else if(!strncmp(sepStrings[0]->c_str(), "batteryOff",
			 strlen("batteryOff"))) {
	  int off = atoi(sepStrings[1]->c_str());
	  dwrite(4)("Got string command battery off #%d", off);
	  if(m_battery != NULL)
	      m_battery->turnOff(off);
      } else if(!strncmp(sepStrings[0]->c_str(), "waypointControlStart",
			 strlen("waypointControlStart"))) {
	  double num1 = atof(sepStrings[2]->c_str());
	  double num2 = atof(sepStrings[3]->c_str());
	  int depth = atoi(sepStrings[4]->c_str());
	  int circ = atoi(sepStrings[5]->c_str());
	  int duration = atoi(sepStrings[6]->c_str());
	  int rpm = atoi(sepStrings[7]->c_str());

	  if(m_layeredControl == NULL) return;

	  LayeredControlIF::NewData _newData;
	  _newData.state = 0;
	  //this very dependent on having a depth envelope
	  _newData.priorityNum = 2;
	  _newData.changeNum = -1;

	  memset(_newData.attName, 0, sizeof(_newData.attName));
	  memset(_newData.data, 0, sizeof(_newData.data));

	  if(!strcmp(sepStrings[0]->c_str(), "UTM")) {
	      sprintf(_newData.attName, "%s", "conNorthings");
	      sprintf(_newData.data, "%g", num1);
	      m_layeredControl->changeAttData(&_newData);

	      memset(_newData.attName, 0, sizeof(_newData.attName));
	      memset(_newData.data, 0, sizeof(_newData.data));
	      sprintf(_newData.attName, "%s", "conEastings");
	      sprintf(_newData.data, "%g", num2);
	      m_layeredControl->changeAttData(&_newData);
	  } else {
	      sprintf(_newData.attName, "%s", "conLatitude");
	      sprintf(_newData.data, "%g", num1);
	      m_layeredControl->changeAttData(&_newData);

	      memset(_newData.attName, 0, sizeof(_newData.attName));
	      memset(_newData.data, 0, sizeof(_newData.data));
	      sprintf(_newData.attName, "%s", "conLongitude");
	      sprintf(_newData.data, "%g", num2);
	      m_layeredControl->changeAttData(&_newData);
	  }

	  memset(_newData.attName, 0, sizeof(_newData.attName));
	  memset(_newData.data, 0, sizeof(_newData.data));
	  sprintf(_newData.attName, "%s", "depth");
	  sprintf(_newData.data, "%d", depth);
	  m_layeredControl->changeAttData(&_newData);

	  memset(_newData.attName, 0, sizeof(_newData.attName));
	  memset(_newData.data, 0, sizeof(_newData.data));
	  sprintf(_newData.attName, "%s", "captureRadius");
	  sprintf(_newData.data, "%d", circ);
	  m_layeredControl->changeAttData(&_newData);

	  memset(_newData.attName, 0, sizeof(_newData.attName));
	  memset(_newData.data, 0, sizeof(_newData.data));
	  sprintf(_newData.attName, "%s", "timer");
	  sprintf(_newData.data, "%d",duration);
	  m_layeredControl->changeAttData(&_newData);

	  memset(_newData.attName, 0, sizeof(_newData.attName));
	  memset(_newData.data, 0, sizeof(_newData.data));
	  sprintf(_newData.attName, "%s", "rpm");
	  sprintf(_newData.data, "%d",rpm);
	  m_layeredControl->changeAttData(&_newData);
      } else if(!strncmp(sepStrings[0]->c_str(), "waypointControlStop",
			 strlen("waypointControlStop"))) {
	  if(m_layeredControl == NULL) return;

	  LayeredControlIF::NewData _newData;
	  _newData.state = 0;
	  //this very dependent on having a depth envelope
	  _newData.priorityNum = 2;
	  _newData.changeNum = -1;

	  memset(_newData.attName, 0, sizeof(_newData.attName));
	  memset(_newData.data, 0, sizeof(_newData.data));
	  sprintf(_newData.attName, "%s", "timer");
	  sprintf(_newData.data, "%d", 0);

	  m_layeredControl->changeAttData(&_newData);
      } else if(!strncmp(sepStrings[0]->c_str(), "remoteDebug",
			 strlen("remoteDebug"))) {
	  RemoteLogIF* rem = new RemoteLogIF("ras");
	  int d = atoi(sepStrings[1]->c_str());
	  rem->setDebugLevel(d);

	  //Syslog::remoteWrite(3, "Changing remote debug level to %d", d);
      }

    //just to make sure memory gets freed
    for(int i=0; i < sepStrings.size(); i++){
	tempString = sepStrings[i];
	delete tempString;
    }
}

void RemoteAgentDriver::pollAcks()
{
    int i;

    if(m_layeredControl == NULL)
	return;

    if(m_ackMis){
	int r = m_layeredControl->getMissionAck(_misCounter);
	if(r != -1) {
	    Boolean b = ((r == 0) ? False : True);
	    MMMissionName ack(b, m_misType);
	    send( new ModemMessage(&ack) );
	    m_ackMis = False;
	    _misCounter++;
	}
    }

    if(m_ackSingle || m_ackArray){
	m_layeredControl->getChangeAttAck(_ackData);
	if(m_ackSingle){
	    //scroll through looking for just that one
	    //NUMACK hard coded in layeredControlIF
	    for(i = 0; i < 15; i++){
		//if(_ackData[i].changeNum != -1)
		//fprintf(stdout, "ack %d sing %d\n", _ackData[i].changeNum, _singleAck);

		if(_ackData[i].changeNum == _singleAck) {
		    MMBehavior b_ack(_ackData[i].success);
		    send( new ModemMessage(&b_ack) );
		    m_ackSingle = False;
		}
	    }
	}
	if(m_ackArray){
	    Boolean done = True;
	    //this is just to figure out when we are done
	    for(int j=0; j < _ackArraySize; j++){
		if(_attDone[j] == -1) {
		    done = False;
		    for(int k= 0; k < 15; k++){
			if(_ackData[k].changeNum == _attNums[j]) {
			    _attDone[j] = _ackData[k].success;
			    dwrite(4)("In RAS: changeNum: %d success: %d", _attNums[j], _attDone[j]);
			    break;
			}
		    }
		}
	    }

	    //now we can send the batch messages back
	    if(done && _allowComs) {
		MMBehaviorArray ba(_ackArraySize, _attDone);
		send( new ModemMessage(&ba) );
		m_ackArray = False;
	    }
	}
    }
}

void RemoteAgentDriver::handleBehavior(ModemMessage * mm)
{
    MMBehavior message(mm);
    LayeredControlIF::NewData _newData;
    _newData.state = message.getStateNum();
    _newData.priorityNum = message.getPriorityNum();
    memset(_newData.attName, 0, sizeof(_newData.attName));
    memset(_newData.data, 0, sizeof(_newData.data));
    message.getName(_newData.attName);
    message.getData(_newData.data);
    _newData.changeNum = _attCounter;
    dwrite(4)("RAS: Single ChangeData getting called with %s %d %s %d", _newData.attName,
	      _newData.priorityNum, _newData.data, _newData.changeNum);

    if(m_layeredControl != NULL) {
	m_layeredControl->changeAttData(&_newData);

	//set up stuff for ack
	m_ackSingle = True;
	_singleAck = _attCounter++;
    }
}

void RemoteAgentDriver::handleBehaviorArray(ModemMessage *mm)
{
    LayeredControlIF::NewData _newData;
    MMBehaviorArray message(mm);
    if(m_layeredControl != NULL) {

	//setting up acks
	_ackArraySize = message.getNumChanges();
	_attNums = new short[_ackArraySize];
	_attDone = new short[_ackArraySize];
	m_ackArray = True;

	for(int i = 0; i < message.getNumChanges(); i++) {
	    _newData.state = message.getSingleState(i);
	    _newData.priorityNum = message.getSinglePriority(i);
	    memset(_newData.attName, 0, sizeof(_newData.attName));
	    memset(_newData.data, 0, sizeof(_newData.data));

	    message.getSingleName(i,_newData.attName);
	    message.getSingleData(i,_newData.data);

	    _newData.changeNum = _attCounter;

	    dwrite(4)("Sending ChangeData request %d with %s %s %d",
		      i, _newData.attName, _newData.data, _newData.changeNum);
	    m_layeredControl->changeAttData(&_newData);

	    _attNums[i] = _attCounter++;
	    _attDone[i] = -1;

	    //dwrite(3)("Success in attribute change %s: %d\n", _newData.attName, suc);
	}

	delete[] _attNums;
	delete[] _attDone;
    }
}

void RemoteAgentDriver::handleMissionName(ModemMessage *mm)
{
    char n[128];
    memset(n,0,128);
    MMMissionName message(mm);
    //if(m_layeredControl != NULL) {
    short mT = message.getMissionType();

    int s = message.getMissionNameSize();
    message.getMissionName(n);
    short j = message.getJumpNum();

    if(mT == MMMissionName::Normal) {
	if(m_layeredControl != NULL)
	    m_layeredControl->changeNormalMission(n,_misCounter, j);
	m_misType = MMMissionName::Normal;
    } else if(mT == MMMissionName::Abort) {
	if(m_layeredControl != NULL)
	    m_layeredControl->changeAbortMission(n,_misCounter, j);
	m_misType = MMMissionName::Abort;
    }
    m_ackMis = True;
}

void RemoteAgentDriver::handleJumpBehavior(ModemMessage *mm)
{
    MMJumpBehavior message(mm);
    short j = message.getJumpNum();
    if(m_layeredControl != NULL)
	m_layeredControl->jumpBehavior(j);
}

void RemoteAgentDriver::handleDriverCommand(ModemMessage *mm)
{
    char n[256];
    memset(n, 0, 256);
    MMDriver message(mm);
    message.getDriverName(n);
    dwrite(4)("Driver name: %s", n);
    Boolean b = message.getDriverOn();
    if (m_supervisor != NULL) {
	dwrite(4)("Supervisor->setDriverCommand");
	m_supervisor->setDriverCommand(n, b);
    }
}


////////////////////////////////////////////////////
void RemoteAgentDriver::sendServerAlarmBatch()
{
    if(!_allowComs) return;
    MMServerAlarmBatch alarm(MMServerAlarm::numServers);
    alarm.setValue(MMServerAlarm::Navigation,m_navigation == NULL);
    alarm.setValue(MMServerAlarm::LayeredControl,m_layeredControl == NULL);
    alarm.setValue(MMServerAlarm::TailCone,m_tailcone == NULL);
    alarm.setValue(MMServerAlarm::PowerSystem,m_power == NULL);

    alarm.setValue(MMServerAlarm::DynamicControl,m_dynamicControl == NULL);
    alarm.setValue(MMServerAlarm::VehicleConfiguration,m_vehicleConfiguration == NULL);
    alarm.setValue(MMServerAlarm::Ahrs,m_ahrs == NULL);
    alarm.setValue(MMServerAlarm::DepthSensor,m_depthSensor == NULL);
    alarm.setValue(MMServerAlarm::Battery,m_battery == NULL);
    alarm.setValue(MMServerAlarm::Dvl,m_dvl == NULL);
    alarm.setValue(MMServerAlarm::Homing,m_homing == NULL);
    alarm.setValue(MMServerAlarm::Gps,m_gps == NULL);
    alarm.setValue(MMServerAlarm::ExternalPosFix,m_externalPosFix == NULL);
    send( new ModemMessage(& alarm ) );
    dwrite(4)("sent server alarm batch");
}

////////////////////////////////////////////////////////////////////
int RemoteAgentDriver::getTime()
{
    int currentTime = 0;
    if ((m_layeredControl && System::findServer(LayeredControlIFServerName)) > 0)
	currentTime = Math::Rnd(m_layeredControl->elapsedMissionTime());
    return currentTime;
}

////////////////////////////////////////////////////////////////////
Boolean RemoteAgentDriver::checkForSending(Boolean flag, int period)
{
    return flag && (fmod(m_count*SLEEP_INTERVAL,period) == 0);
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendAck()
{
    if(!_allowComs) return;
    MMAgentRequest message(MMAgentRequest::Single,ModemMessage::tACK,-1);
    send( new ModemMessage(&message) );
    dwrite(4)("sent agent ack");
}

//////////////////////////////////////////////////////
void RemoteAgentDriver::sendActiveBehavior()
{
    if(!_allowComs) return;
    if (checkServer(LayeredControlIFServerName) < 1) return;

    short curState;
    short curBehavior;
    short misState;

    misState = m_layeredControl->getMissionState();
    curState = m_layeredControl->currentState();
    curBehavior = m_layeredControl->currentBehavior();

    MMActiveBehavior act(curState, curBehavior, misState);
    //fprintf(stdout, "Sending active behavior: %d %d\n", act.getState(), act.getBehavior());
    //act.sendRemote();

    send(new ModemMessage(&act));

    dwrite(4)("send active behavior message. Type: %d",ModemMessage::tMMActiveBehavior);
}

/////////////////////////////////////////////////////////////
void RemoteAgentDriver::sendLogDirName()
{
    if(!_allowComs) return;
    if (checkServer(SupervisorIFServerName) < 1) return;

    char logName[128];
    char toSend[128];

    memset(logName, 0, 128);
    memset(toSend, 0, 128);

    m_supervisor->getCurrentLogDirectoryName(logName);

    sprintf(toSend, "SupervisorLogDir, %s",logName);

    dwrite(4)("sending log name: %s\n", toSend);

    MMString* message = new MMString(toSend);
    send(new ModemMessage(message));
    delete message;
}

/////////////////////////////////////////////////////////////
void RemoteAgentDriver::sendOas()
{
    if(!_allowComs) return;
    TimeIF::TimeSpec tempTime;
    if(checkServer(RangeFinderIFServerName) < 1) return;

    double raw, mean, max;
    unsigned char er;

    m_oas->rawRange(&er, &raw, &tempTime);
    m_oas->range(&er, &mean,&tempTime);
    m_oas->maxRange(&max);

    MMOas oas(raw,mean,max);
    send(new ModemMessage(&oas));

    dwrite(4)("sent oas message");
}

void RemoteAgentDriver::sendSvs()
{
    if(!_allowComs) return;

    TimeIF::TimeSpec tempTime;
    if(checkServer(SVSIFServerName) < 1) return;

    long tof;
    long sMm;
    double sM;

    tof = m_svs->raw_TOF();
    sMm = m_svs->speed_in_mm();
    sM = m_svs->speed_in_meters();

    MMSvs svs(tof, sM, sMm);
    send(new ModemMessage(&svs));
    dwrite(4)("sent svs message");
}

void RemoteAgentDriver::sendLN250()
{
    if(!_allowComs) return;

    if(checkServer(Ln250IFServerName) < 1) return;

    Ln250IF::InsFix iD;
    Ln250IF::InsFix hD;
    short ln250mode;

    m_ln250->getInsOnlyFix(&iD);
    m_ln250->getHybridFix(&hD);
    m_ln250->getLn250Mode(&ln250mode);

    if (iD.systemTimer != 0) {
	MMLN250 insMM(MMLN250::InsOnly, iD.systemTimer, iD.validity,
		      iD.latitude, iD.longitude, iD.altitude,
		      iD.northVel, iD.eastVel, iD.upVel, iD.corrGyroBiasX,
		      iD.corrGyroBiasY, iD.heading,
		      iD.pitch, iD.roll, iD.yaw, iD.pitchRate,
		      iD.rollRate, iD.yawRate, ln250mode, iD.type, getTime());

	dwrite(4)("Ins only: mode %d sysTime %g lat %g lon %g alt %g\n nvel %g evel %g vvel %g pitch %g roll %g yaw %g\nbiasx %g biasy %g\n",
		  iD.mode, iD.systemTimer,iD.latitude, iD.altitude, iD.northVel,
		  iD.eastVel, iD.upVel, iD.pitch, iD.roll, iD.yaw,
		  iD.corrGyroBiasX, iD.corrGyroBiasY);

	if(m_ln250 != NULL)
	    send( new ModemMessage(&insMM));
    }

    if (hD.systemTimer != 0) {
	MMLN250 hyMM(MMLN250::Hybrid, hD.systemTimer, hD.validity,
		     hD.latitude, hD.longitude, hD.altitude,
		     hD.northVel, hD.eastVel, hD.upVel, hD.corrGyroBiasX,
		     hD.corrGyroBiasY, hD.heading, hD.pitch, hD.roll, hD.yaw,
		     hD.pitchRate,  hD.rollRate, hD.yawRate, ln250mode, hD.type, getTime());

	dwrite(4)("Hybrid: mode %d sysTime %g lat %g lon %g alt %g\nnvel %g evel %g vvel %g pitch %g roll %g yaw %g\nbiasx %g biasy %g\n",
		  hD.mode, hD.systemTimer,
		  hD.latitude, hD.altitude,
		  hD.northVel, hD.eastVel, hD.upVel, hD.pitch, hD.roll, hD.yaw,
		  hD.corrGyroBiasX, hD.corrGyroBiasY);

	if(m_ln250 != NULL)
	    send( new ModemMessage(&hyMM));
    }
}

void RemoteAgentDriver::sendSas21()
{
    if(!_allowComs) return;
    if(checkServer(Sas21IFServerName) < 1) return;

    bool leak;
    int temp, du, br;
    int be, we;

    leak = m_sas21->leak();
    temp = m_sas21->temperature();
    du = m_sas21->diskUsage();
    br = m_sas21->blocksReceived();
    be = m_sas21->blockErrors();
    we = m_sas21->writeErrors();

    MMSas21 mes(leak,temp,du,br,be,we);

    dwrite(4)("Sas21 message sent -- %d %d %d %d %d %d\n", leak,temp,du,br,be,we);

    if(m_sas21 != NULL)
	send( new ModemMessage(&mes));
}



////////////////////////////////////////////////////
void RemoteAgentDriver::sendTailconeLocal(ModemMessage* mm)
{
    MMTailcone* message = new MMTailcone(mm);
    if(!_allowComs) return;
    if (checkServer(BluefinTailconeIFServerName) < 1) return;
    if(m_tailcone == NULL) return;
    float speed = message->getPropellerRpm()*Math::RpmToRadps;
    m_tailcone->command(speed,message->getElevatorAngle(),
			message->getRudderAngle());
    dwrite(4)("sent tailcone command %f %f %f.",message->getPropellerRpm(),
	      message->getElevatorAngle(),message->getRudderAngle());
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendTailconeRemote()
{
    if(!_allowComs) return;
    if (checkServer(TailConeIFServerName) < 1) return;
    if(m_tailcone == NULL) return;
    TimeIF::TimeSpec t, t2;
    double propRadPs, elevAngle, rudderAngle;
    m_tailcone->actual(&propRadPs,&elevAngle,&rudderAngle,&t);
    short compLevel;
    m_tailcone->getCompLevel(&compLevel, &t2);
    MMTailcone message(propRadPs/Math::RpmToRadps,elevAngle,rudderAngle,compLevel,getTime());
    send( new ModemMessage(&message) );
    dwrite(4)("sent tailcone message.");
}

void RemoteAgentDriver::sendTailconeRaw()
{
    if(!_allowComs) return;
    if (checkServer(BluefinTailconeIFServerName) < 1) return;
    if(m_tailcone == NULL) return;
    TimeIF::TimeSpec t;
    short propA2D, elevatorA2D, rudderA2D, throttle;
    if (m_tailcone != NULL) {
	m_tailcone->readA2D(&propA2D,&elevatorA2D,&rudderA2D,&t);
	m_tailcone->getPropellerThrottle(&throttle,&t);
    } else {
	dwrite(1)("Unable to complete tailcone raw request");
	return;
    }
    MMTailconeRaw message(propA2D,elevatorA2D,rudderA2D,throttle,getTime());
    send( new ModemMessage(&message) );
    dwrite(4)("sent tailcone raw message prop=%d elev=%d rudd=%d",
	      propA2D, elevatorA2D, rudderA2D);
}

 ////////////////////////////////////////////////////
BluefinPowerSystemIF::Weight RemoteAgentDriver::getBluefinPowerSystemIF(MMDropWeightRelease::Weight weight)
{
    BluefinPowerSystemIF::Weight out = BluefinPowerSystemIF::Unassigned;
    switch (weight) {
    case (MMDropWeightRelease::Descend):
	out = BluefinPowerSystemIF::Descend;
	break;
    case (MMDropWeightRelease::Ascend):
	out = BluefinPowerSystemIF::Ascend;
	break;
    case (MMDropWeightRelease::Emergency):
	out = BluefinPowerSystemIF::Emergency;
	break;
    default:
	dwrite(1)("Unknown MMDropWeightRelease::Weight");
	break;
    }
    return out;
}

////////////////////////////////////////////////////
MMDropWeightRelease::Weight RemoteAgentDriver::getDropWeightMM(BluefinPowerSystemIF::Weight weight)
{
    MMDropWeightRelease::Weight out = MMDropWeightRelease::numWeights;
    switch (weight) {
    case (BluefinPowerSystemIF::Descend):
	out = MMDropWeightRelease::Descend;
	break;
    case (BluefinPowerSystemIF::Ascend):
	out = MMDropWeightRelease::Ascend;
	break;
    case (BluefinPowerSystemIF::Emergency):
	out = MMDropWeightRelease::Emergency;
	break;
    default:
	dwrite(1)("Unknown BluefinPowerSystemIF::Weight");
	break;
    }
    return out;
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendDropWeightReleaseLocal(ModemMessage* mm)
{
    MMDropWeightRelease* message = new MMDropWeightRelease(mm);
    if (checkServer(BluefinPowerSystemIFServerName) < 1) return;
    BluefinPowerSystemIF::Weight weight = getBluefinPowerSystemIF((MMDropWeightRelease::Weight)
						  message->getNumber());
    if (message->getValue()) {
	m_power->fire(weight);
	dwrite(4)("sent drop weight release command %d",weight);
    } else {
	m_power->unfire(weight);
	dwrite(4)("stopping drop weight release %d", weight);
    }
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendDropWeightReleaseBatchRemote()
{
    if(!_allowComs) return;
    if (checkServer(BluefinPowerSystemIFServerName) < 1) return;
    MMDropWeightReleaseBatch message(MMDropWeightRelease::numWeights);
    message.setValue(getDropWeightMM(BluefinPowerSystemIF::Ascend),
		     m_power->isFiring((short)BluefinPowerSystemIF::AscendIndex));
    message.setValue(getDropWeightMM(BluefinPowerSystemIF::Descend),
		     m_power->isFiring((short)BluefinPowerSystemIF::DescendIndex));
    message.setValue(getDropWeightMM(BluefinPowerSystemIF::Emergency),
		     m_power->isFiring((short)BluefinPowerSystemIF::EmergencyIndex));
    send( new ModemMessage(&message) );
    dwrite(4)("sent drop weight release batch message %d %d %d",
	      m_power->isFiring((short)BluefinPowerSystemIF::DescendIndex),
	      m_power->isFiring((short)BluefinPowerSystemIF::AscendIndex),
	      m_power->isFiring((short)BluefinPowerSystemIF::EmergencyIndex));
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendCircuitLocal(ModemMessage* mm)
{
    MMCircuit* message = new MMCircuit(mm);
    if (checkServer(PowerSystemIFServerName) < 1) return;
    if (message->getValue()) {
	m_power->netOn(message->getNumber());
    } else {
	m_power->netOff(message->getNumber());
    }
    dwrite(4)("local circuit %d command %d.",message->getNumber(),
	      message->getValue());
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendCircuitBatchLocal(ModemMessage* mm)
{
    MMCircuitBatch* message = new MMCircuitBatch(mm);
    if (checkServer(PowerSystemIFServerName) < 1) return;
    for (int i=0; i<message->getNumber(); i++) {
	if (message->getValue(i)) m_power->netOn(i);
	else m_power->netOff(i);
    }
    dwrite(4)("local circuit batch command");
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendCircuitRemote(int circuit)
{
    if(!_allowComs) return;
    if (checkServer(PowerSystemIFServerName) < 1) return;
    if(m_power == NULL) return;
    PowerSystemIF::CircuitInfo temp;
    m_power->getCircuitInfo(circuit,&temp);
    MMCircuit message(circuit,temp.status);
    send( new ModemMessage(&message) );
    dwrite(4)("sent circuit %d message %d",circuit,temp.status);
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendCircuitBatchRemote()
{
    if(!_allowComs) return;
    if (checkServer(PowerSystemIFServerName) < 1) return;
    if(m_power == NULL) return;
    PowerSystemIF::CircuitInfo temp;
    short nCircuits;
    m_power->getCircuitCount(&nCircuits);
    MMCircuitBatch message(nCircuits);
    for (short i=0; i<message.getNumber(); i++) {
	m_power->getCircuitInfo(i,&temp);
	message.setValue(i,temp.status);
	dwrite(4)("Circuit: %d value: %d", i, temp.status);
    }
    send( new ModemMessage(&message) );
    dwrite(4)("sent circuit batch message");

    //just throwing voltages here
    int count = 0;
    char volts[128];
    float v;
    for (i = 0; i<message.getNumber(); i++) {
	if(m_power->circuitHasVoltage(i)) count++;
    }
    sprintf(volts, "voltage,%d", count);
    for (i = 0; i<message.getNumber(); i++) {
	if(m_power->circuitHasVoltage(i)) {
	    m_power->voltage(i, &v);
	    //voltage a possible number
	    if(v > 100 || v < 0) v = 0.0;
	    sprintf(volts, "%s,%g", volts, v);
	}
    }
    MMString str(volts);
    send( new ModemMessage(&str));
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendCircuitCurrent(int circuit)
{
    if (checkServer(PowerSystemIFServerName) < 1) return;
    if(m_power == NULL) return;
    float value;
    m_power->current(circuit,&value);
    MMCircuitCurrent current(circuit,value);
    dwrite(4)("sent circuit %d current %f message",circuit,value);
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendCircuitCurrentBatch()
{
    if(!_allowComs) return;
    if (checkServer(PowerSystemIFServerName) < 1) return;
    if(m_power == NULL) return;
    float value;
    short nCircuits;
    m_power->getCircuitCount(&nCircuits);
    MMCircuitCurrentBatch message(nCircuits);
    for (int i=0; i<message.getNumber(); i++) {
	m_power->current(i,&value);
	message.setValue(i,value);
	dwrite(4)("circuit current %d = %f",i,value);
    }
    send( new ModemMessage(&message) );
    dwrite(4)("sent circuit current batch message");

}

///////////////////////////////////////////////////////
void RemoteAgentDriver::sendCircuitInfo()
{
    if(!_allowComs) return;
    if (checkServer(PowerSystemIFServerName) < 1) return;
    if(m_power == NULL) return;
    short nCircuits;
    PowerSystemIF::CircuitInfo temp;
    m_power->getCircuitCount(&nCircuits);
    for(int i = 0; i < nCircuits; i++) {
	try {
	    m_power->getCircuitInfo(i,&temp);
	    MMCircuitInfo circ(temp.number, temp.devices);
	    send( new ModemMessage(&circ));
	    dwrite(4)("Sent circuit info message %d %d %s\n", i, temp.number,temp.devices);
	} catch(...) {
	    dwrite(1)("Circuit count %d does not exist", i);
	}
    }
    dwrite(4)("sent circuit info messages");
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendGroundFaultCurrent(int circuit)
{
    if (checkServer(PowerSystemIFServerName) < 1) return;
    if(m_power == NULL) return;
    float value;
    m_power->groundFault(circuit,&value);
    MMGroundFaultCurrent current(circuit,value/1000);
    dwrite(4)("sent ground fault %d current %f message",circuit,value);
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendGroundFaultCurrentBatch()
{
    if(!_allowComs) return;
    if (checkServer(PowerSystemIFServerName) < 1) return;
    float value;
    short nFaults;
    if(m_power == NULL) return;
    m_power->getGroundFaultCount(&nFaults);
    if(nFaults == 0) return;
    MMGroundFaultCurrentBatch message(nFaults);
    for (int i=0; i<message.getNumber(); i++) {
	m_power->groundFault(i,&value);
	message.setValue(i,value/1000);
	dwrite(4)("ground fault %d = %f",i,value);
    }
    send( new ModemMessage(&message) );
    dwrite(4)("sent ground fault current batch message");
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendTemperature(int temperature)
{
    if (checkServer(PowerSystemIFServerName) < 1) return;
    float value;
    if(m_power == NULL) return;
    m_power->temperature(temperature,&value);
    MMTemperature message(temperature,value);
    dwrite(4)("sent temperature %d value %f message",temperature,value);

    //I'm way to lazy to write another function for pressure
    m_power->pressure(&value);
    char temp[32];
    sprintf(temp, "pressure, %lf", value);
    MMString str(temp);
    send( new ModemMessage(&str));

    //Ditto with the emergency power system (EPS)
    short epsValue;
    m_power->epsState(&epsValue);
    sprintf(temp, "eps, %d", epsValue);
    MMString str2(temp);
    send( new ModemMessage(&str2) );
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendTemperatureBatch()
{
    if(!_allowComs) return;
    if (checkServer(PowerSystemIFServerName) < 1) return;
    if(m_power == NULL) return;
    float value;
    short nTemperatures;
    m_power->getTempCount(&nTemperatures);
    MMTemperatureBatch message(nTemperatures);
    for (int i=0; i<message.getNumber(); i++) {
	m_power->temperature(i,&value);
	message.setValue(i,value);
	dwrite(4)("temperature %d = %f",i,value);
    }
    send( new ModemMessage(&message) );
    dwrite(4)("sent temperature batch message");

    //I'm way to lazy to write another function for pressure
    m_power->pressure(&value);
    char temp[32];
    sprintf(temp, "pressure, %lf", value);
    MMString str(temp);
    send ( new ModemMessage(&str));

    //Ditto with the emergency power system (EPS)
    short epsValue;
    m_power->epsState(&epsValue);
    sprintf(temp, "eps, %d", epsValue);
    MMString str2(temp);
    send( new ModemMessage(&str2) );
}

////////////////////////////////////////////////////
// Water alarms are numbered from 1
void RemoteAgentDriver::sendWaterAlarm(int number)
{
    if(!_allowComs) return;
    //if (checkServer(WaterAlarmIFServerName) < 1) return;
    if (checkServer(BluefinPowerSystemIFServerName) < 1) return;
    if(m_power == NULL) return;
    Boolean value;
    MMWaterAlarm alarm(number,m_power->getWaterAlarmStatus(number,&value));
    send( new ModemMessage(&alarm) );
    dwrite(4)("sent water alarm %d message %d",number,value);
}

////////////////////////////////////////////////////
// Water alarms are numbered from 1
void RemoteAgentDriver::sendWaterAlarmBatch()
{
    if(!_allowComs) return;
    if (checkServer(BluefinPowerSystemIFServerName) < 1) return;
    if(m_power == NULL) return;
    MMWaterAlarmBatch alarm(NUM_WATER_ALARMS);
    Boolean value;
    for (int i=0; i<alarm.getNumber(); i++) {
	m_power->getWaterAlarmStatus(i+1,&value);
	alarm.setValue(i,value);
	if (value) alarm.setPriority(pURGENT);
	dwrite(4)("water alarm %d = %d",i,value);
    }
    send( new ModemMessage(&alarm) );
    dwrite(4)("sent water alarm batch message");
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendDynamicControlCommandFullLocal(ModemMessage* mm)
{
    MMDynamicControlCommandFull* message = new MMDynamicControlCommandFull(mm);
    if (checkServer(DynamicControlIFServerName) < 1) return;
    DynamicControlIF::Command command;
    command.speedMode = (DynamicControlIF::SpeedMode) message->getSpeedMode();
    command.verticalMode = (DynamicControlIF::VerticalMode) message->getVerticalMode();
    command.horizontalMode = (DynamicControlIF::HorizontalMode) message->getHorizontalMode();
    command.speed = message->getSpeed();
    command.vertical = message->getVertical();
    command.horizontal = message->getHorizontal();
    m_dynamicControl->setCommand(&command);
    dwrite(4)("local dynamic control command full command %f %f %f",command.speed,
	      command.vertical,command.horizontal);
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendDynamicControlCommandLocal(ModemMessage* mm)
{
    MMDynamicControlCommand* message = new MMDynamicControlCommand(mm);
    if (checkServer(DynamicControlIFServerName) < 1) return;
    DynamicControlIF::Command command;
    command.speedMode = DynamicControlIF::Rpm;
    command.verticalMode = DynamicControlIF::Elevator;
    command.horizontalMode = DynamicControlIF::Rudder;
    command.speed = message->getPropellerRpm();
    command.vertical = message->getElevatorAngle();
    command.horizontal = message->getRudderAngle();
    m_dynamicControl->setCommand(&command);
    dwrite(4)("local dynamic control command command %f %f %f",command.speed,
	      command.vertical,command.horizontal);
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendDynamicControlCommandRemote()
{
    if(!_allowComs) return;
    if (checkServer(DynamicControlIFServerName) < 1) return;
    DynamicControlIF::LogData data;
    m_dynamicControl->getLogData(&data);
    MMDynamicControlCommand message(data.propOmegaCmd/Math::RpmToRadps,
				    data.rudderCmd,data.elevatorCmd,getTime());
    send( new ModemMessage(&message) );
    dwrite(4)("sent dynamic control command message.");
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendNavigationPosition()
{
    if(!_allowComs) return;
    if (checkServer(NavigationIFServerName) < 1) return;

    // Get the navigation state
    NavigationIF::PositionStruct position;
    NavigationIF::AttitudeStruct attitude;
    m_navigation->state(&position,&attitude);
    float speed = m_navigation->lvlBotVelNorm();

    // Create the outgoing message
    MMNavigationPosition pos(Math::radToDeg(position.latitude),
			     Math::radToDeg(position.longitude),position.northingRate,
			     position.eastingRate,speed,position.depth,position.altitude,getTime());
    send( new ModemMessage(&pos) );
    dwrite(4)("sent position message.");
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendNavigationAttitude()
{
    if(!_allowComs) return;
    if (checkServer(NavigationIFServerName) < 1) return;

    // Get the navigation state
    NavigationIF::PositionStruct position;
    NavigationIF::AttitudeStruct attitude;
    m_navigation->state(&position,&attitude);

    // Create the outgoing message
    MMNavigationAttitude att(attitude.roll,attitude.pitch,attitude.yaw,
			     getTime());
    send( new ModemMessage(&att) );
    dwrite(4)("sent attitude message.");
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendDeviceInfo()
{
    if(!_allowComs) return;
    short nProcess = m_vehicleConfiguration->processCount();
    VehicleConfigurationIF::DeviceInfo dev;
    for (int i=0; i<nProcess; i++)
	{
	    m_vehicleConfiguration->deviceInfo(i,&dev);
	    MMDeviceInfo mmDev(dev.name,dev.port,dev.driver,dev.IFName,
			       dev.priority,dev.pid,dev.powerCircuit,
			       dev.debug,0); //dev.autoStart
	    send( new ModemMessage(&mmDev) );
	    dwrite(4)("sent device info message %d %s",i, dev.name);
	}

    char temp[32];
    long free_blocks, tot_blocks;
    int fildes = open("/", O_RDONLY);
    disk_space( fildes, &free_blocks, &tot_blocks ); 
    sprintf(temp, "disk, %ld %ld", free_blocks, tot_blocks );
    MMString str(temp);
    send( new ModemMessage(&str) );

    dwrite(4)("sent device info messages");
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendAhrs()
{
    if(!_allowComs) return;
    if (checkServer(AhrsIFServerName) < 1) return;
    AhrsIF::Attitude att;
    TimeIF::TimeSpec t;

    //first checking crossbow
    if(m_crossbow != NULL && checkServer(CrossbowIFServerName) >= 1) {
	m_crossbow->getAttitude(&att, &t);
	MMAhrs message(0, att.roll,att.pitch,att.yaw,att.rollRate,att.pitchRate,att.yawRate,
		       getTime());
	send( new ModemMessage(&message) );
	dwrite(4)("sent crossbow message %g %g %g %g %g %g",att.roll,att.pitch,att.yaw,att.rollRate,att.pitchRate,att.yawRate);
    }

    //then checking microstrain
    if(m_microstrain != NULL && checkServer(MicrostrainIFServerName) >= 1) {
	m_microstrain->getAttitude(&att, &t);
	MMAhrs message(1, att.roll,att.pitch,att.yaw,att.rollRate,att.pitchRate,att.yawRate,
		       getTime());
	send( new ModemMessage(&message) );
	dwrite(4)("sent microstrain message %g %g %g %g %g %g",att.roll,att.pitch,att.yaw,att.rollRate,att.pitchRate,att.yawRate);
    }

    //if neither, send whatever
    if(m_crossbow == NULL && m_microstrain == NULL) {
	m_ahrs->getAttitude(&att, &t);
	MMAhrs message(1, att.roll,att.pitch,att.yaw,att.rollRate,att.pitchRate,att.yawRate,
		       getTime());
	send( new ModemMessage(&message) );
	dwrite(4)("sent generic ahrs message %g %g %g %g %g %g",att.roll,att.pitch,att.yaw,att.rollRate,att.pitchRate,att.yawRate);
    }
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendPhins()
{
    if(!_allowComs) return;
    if (checkServer(PhinsIFServerName) < 1) return;
    PhinsIF::Attitude att;
    TimeIF::TimeSpec t;
    m_phins->getAttitude(&att,&t);
    MMPhins message(att.roll,att.pitch,att.yaw,att.rollRate,att.pitchRate,att.yawRate,
                    getTime());
    send( new ModemMessage(&message) );
    dwrite(4)("sent phins message %g %g %g %g %g %g",att.roll,att.pitch,att.yaw,att.rollRate,att.pitchRate,att.yawRate);
}

void RemoteAgentDriver::sendMultiLeica()
{
    if(!_allowComs) return;
    if (checkServer(MultiLeicaIFServerName) < 1) return;

    //first send average
    LeicaCompassIF::LeicaAttitude attAv;
    LeicaCompassIF::LeicaAccel accAv;
    m_multiLeica->getAttitude(&attAv);
    m_multiLeica->getAccel(&accAv);
    MMMultiLeica average(MultiLeicaIF::CompassAverage,
			 attAv.roll,
			 attAv.elevation,
			 attAv.azimuth,
			 accAv.accelX,
			 accAv.accelY,
			 accAv.accelZ,
			 getTime());

    send ( new ModemMessage(&average));

    dwrite(4)("Leica average: %g %g %g %g %g %g",
	      Math::radToDeg(attAv.roll),
	      Math::radToDeg(attAv.elevation),
	      Math::radToDeg(attAv.azimuth),
	      accAv.accelX, accAv.accelY, accAv.accelZ);

    /*
      //then send compass one
      LeicaCompassIF::LeicaAttitude attOne;
      LeicaCompassIF::LeicaAccel accOne;
      m_multiLeica->getAttitude(&attOne, MultiLeicaIF::CompassOne);
      m_multiLeica->getAccel(&accOne, MultiLeicaIF::CompassAverage);
      MMMultiLeica one(MultiLeicaIF::CompassAverage,
      attOne.roll, attOne.elevation, attOne.azimuth,
      accOne.accelX, accOne.accelY, accOne.accelZ,getTime());

      send ( new ModemMessage(&one));

      //finally send compass two
      LeicaCompassIF::LeicaAttitude attTwo;
      LeicaCompassIF::LeicaAccel accTwo;
      m_multiLeica->getAttitude(&attTwo, MultiLeicaIF::CompassTwo);
      m_multiLeica->getAccel(&accTwo, MultiLeicaIF::CompassAverage);
      MMMultiLeica two(MultiLeicaIF::CompassAverage,
      attTwo.roll, attTwo.elevation, attTwo.azimuth,
      accTwo.accelX, accTwo.accelY, accTwo.accelZ,getTime());

      send ( new ModemMessage(&two));
    */

    dwrite(4)("sent leica messages");
}

void RemoteAgentDriver::sendSeacat()
{
    if(!_allowComs) return;
    if (checkServer(SeacatSBE19IFServerName) < 1) return;
    TimeIF::TimeSpec t;
    double cond, temp, depth, pressure, obs, flu, sal, sv;
    m_seacat->conductivity(&cond, &t);
    m_seacat->temperature(&temp, &t);
    m_seacat->depth(&depth, &t);
    m_seacat->pressure(&pressure, &t);
    m_seacat->obs(&obs, &t);
    m_seacat->fluorometer(&flu, &t);
    m_seacat->salinity(&sal, &t);
    m_seacat->soundVelocity(&sv, &t);
    MMSeacat message(cond, temp, pressure, depth, obs, flu, sal, sv);
    //MMSeacat message(12.0, 35.0, 1000, 40, 10, 6, 19.5, 1486.77);
    send( new ModemMessage(&message) );
    dwrite(4)("sent seacat message");
}

void RemoteAgentDriver::sendSeabirdSBE25()
{
    if(!_allowComs) return;
    if (checkServer(SBE25IFServerName) < 1) return;
    TimeIF::TimeSpec timeStamp;
    double conductivity, temperature, depth, pressure, salinity, soundVelocity, extVoltage[7];
    short numberOfExternalVoltages, i;

    for(i=0;i<7;i++)
	extVoltage[i]=0.0;

    m_seabirdSBE25->conductivity(&conductivity, &timeStamp);
    m_seabirdSBE25->temperature(&temperature, &timeStamp);
    m_seabirdSBE25->depth(&depth, &timeStamp);
    m_seabirdSBE25->pressure(&pressure, &timeStamp);
    m_seabirdSBE25->salinity(&salinity, &timeStamp);
    m_seabirdSBE25->soundVelocity(&soundVelocity, &timeStamp);
    m_seabirdSBE25->getNumberOfExternalVoltages(&numberOfExternalVoltages, &timeStamp);
    for(i=0;i<numberOfExternalVoltages;i++)
	m_seabirdSBE25->getVoltage(i, &extVoltage[i], &timeStamp);
    MMSeabirdSBE25 message(conductivity, temperature, depth, pressure, salinity, soundVelocity,
			  numberOfExternalVoltages,
			  extVoltage[0],
			  extVoltage[1],
			  extVoltage[2],
			  extVoltage[3],
			  extVoltage[4],
			  extVoltage[5],
			  extVoltage[6]);
    //MMSeacat message(12.0, 35.0, 1000, 40, 10, 6, 19.5, 1486.77);
    send( new ModemMessage(&message) );
    dwrite(4)("sent seabirdSBE25 message");
}



////////////////////////////////////////////////////
void RemoteAgentDriver::sendDepthSensor()
{
    if(!_allowComs) return;
    if (checkServer(DepthSensorIFServerName) < 1) return;
    TimeIF::TimeSpec t;
    double depth, temperature, pressure;
    m_depthSensor->depth(&depth,&t);
    m_depthSensor->temp(&temperature,&t);
    m_depthSensor->pressure(&pressure,&t);
    MMDepthSensor message(depth,temperature,pressure,getTime());
    send( new ModemMessage(&message) );
    dwrite(4)("sent depth sensor message");
}

/////////////////////////////////////////////////////
void RemoteAgentDriver::sendWatchdogCommand(ModemMessage* mm)
{
    MMWatchdog* message = new MMWatchdog(mm);
    if ( checkServer(PowerSystemIFServerName) < 1) return;
    // this call needs to be serviced by DwpPowerSystemIF
    PowerSystemIF* dwp = NULL;
    try { dwp = (PowerSystemIF *) m_power; } catch (...) {}
    if (dwp) {
	DeviceIF::Status status;
	MMWatchdog::Watchdog dog = (MMWatchdog::Watchdog) message->getNumber();
	switch (dog) {
	case (MMWatchdog::MVC):
	    if (message->getValue()) status = dwp->enableSystemWatchdog();
	    else status = dwp->disableSystemWatchdog();
	    dwrite(4)("Sent MVC watchdog %d",message->getValue());
	    break;
	case (MMWatchdog::Tailcone):
	    if (message->getValue()) status = dwp->enableTailconeWatchdog();
	    else status = dwp->disableTailconeWatchdog();
	    dwrite(4)("Sent tailcone watchdog %d",message->getValue());
	    break;
	default:
	    dwrite(1)("Bad watchdog enumeration");
	    break;
	}
    } else {
	dwrite(1)("Can not send watchdog command");
    }
}

void RemoteAgentDriver::sendBluefinBatteryInfo()
{
    if(checkServer(BluefinBatteryIFServerName) < 1) return;
    short numBats;
    double volts, amps, temp;
    double minV, maxV;
    char state, errorState;
    short comms, errorLevel;
    short ser;
    short master;

    // new: count up to max number of batteries
    numBats = m_battery->getMaxBatteryIndex();
    dwrite(4)("max battery index is %d", numBats);
    for(int i = 0; i < numBats; i++) {
      // new make sure battery exists
      if (m_battery->batteryExists(i)) {
	dwrite(4)("getting info for battery %d", i);
	ser = m_battery->getBatterySerial(i);
	volts = m_battery->getBatteryVolts(i);
	amps = m_battery->getBatteryAmps(i);
	temp = m_battery->getBatteryTemp(i);
	minV = m_battery->getBatteryMinVolts(i);
	maxV = m_battery->getBatteryMaxVolts(i);
	//capacity = m_battery->getBatteryCapacity(i);
	master = m_battery->getBatteryMaster(i);
	state = m_battery->getBatteryState(i);
	errorState = m_battery->getBatteryErrorState(i);
	comms = m_battery->getBatteryCommsFailures(i);
	errorLevel = m_battery->getBatteryErrorLevels(i);

	MMBluefinBatteryInfo bat(i, ser, volts, amps, minV, maxV, master,
				temp, state, errorState, comms,
				errorLevel);

	dwrite(4)("Battery Info num: %d serial: %d volts: %g amps: %g minV: %g\nmaxV: %g master: %d temp: %g\nstate: %c errorState:%c comms:%d errorLevel: %d\n",
		  i,ser,volts,amps,minV,maxV,master,temp,state,errorState,comms,errorLevel);
	send( new ModemMessage(&bat) );
      }
    }
    dwrite(4)("send battery info messages");
}

void RemoteAgentDriver::sendBluefinBatteryGlobal()
{
    if(checkServer(BluefinBatteryIFServerName) < 1) return;
    short numBats, numOn, numAlive;
    double volts, amps, capacity;
    short errorLevel;

    numBats = m_battery->getNumBatteries();
    numOn = m_battery->getNumBatteriesOn();
    numAlive = m_battery->getNumBatteriesAlive();
    volts = m_battery->getGlobalVolts();
    amps = m_battery->getGlobalAmps();
    capacity = m_battery->getGlobalCapacity();
    errorLevel = m_battery->getGlobalErrorLevel();

    MMBluefinBatteryGlobal global(numBats, numOn, numAlive,volts,
				 amps, capacity, errorLevel);

    dwrite(4)("Battery Global numBats:%d numOn: %d numAlive: %d\nvolts: %g amps:%g capacity:%g errorLevel: %d\n",
	      numBats,numOn,numAlive,volts,amps,capacity,errorLevel);

    send( new ModemMessage(&global) );
    dwrite(4)("send battery global message");
}

void RemoteAgentDriver::sendDvl()
{
	if (checkServer(RdiadcpIFServerName) < 1) return;
	//	Boolean echo;
	double vx,vy,vz,range,errBdyWatVel;
	TimeIF::TimeSpec t;
	DeviceIF::Status status = m_dvl->bdyBotVel(&vx,&vy,&vz,&errBdyWatVel,&t);
	status = m_dvl->bdyBotRng(&range, &t);

	if (fabs(vx) > 10.0) vx = vy = vz = errBdyWatVel = 0.0;

	MMDvl *message = new MMDvl(vx,vy,vz,range,getTime());
	dwrite(4)("SendDvl() -- vx: %g vy: %g vz: %g range: %g", vx,vy,vz,range);
	send( new ModemMessage(message) );
	dwrite(4)("sent dvl message");
	delete message;
}

////////////////////////////////////////////////////////
void RemoteAgentDriver::sendDvlBeamVelocities()
{
	if (checkServer(RdiadcpIFServerName) < 1) return;
	Boolean echo;
	double fwdBdyBotVel, stbBdyBotVel, dwnBdyBotVel, errBdyBotVel;
	TimeIF::TimeSpec t;
	DeviceIF::Status status = m_dvl->bdyBotVel(&fwdBdyBotVel,
						   &stbBdyBotVel,
						   &dwnBdyBotVel,
						   &errBdyBotVel,
						   &t);
	if(fabs(fwdBdyBotVel) > 10.0)
	    fwdBdyBotVel = stbBdyBotVel = dwnBdyBotVel = errBdyBotVel = 0.0;

	MMDvlBeamVelocities *message = new MMDvlBeamVelocities(fwdBdyBotVel,
							       stbBdyBotVel,
							       dwnBdyBotVel,
							       errBdyBotVel);
	send( new ModemMessage(message) );

	dwrite(4)("sent dvl beam velocities message %g %g %g %g",
		  fwdBdyBotVel,stbBdyBotVel,dwnBdyBotVel,
		  errBdyBotVel);
	delete message;
}

////////////////////////////////////////////////////////
void RemoteAgentDriver::sendDvlBeamRanges()
{
	if (checkServer(RdiadcpIFServerName) < 1) return;
	double r1,r2,r3,r4;
	TimeIF::TimeSpec t;
	DeviceIF::Status status = m_dvl->bdyBotBemRng(&r1, &r2, &r3, &r4, &t);
	MMDvlBeamRanges *message = new MMDvlBeamRanges(r1,r2,r3,r4);
	send( new ModemMessage(message) );
	dwrite(4)("sent dvl beam ranges message %g %g %g %g",r1,r2,r3,r4);
	delete message;
}

void RemoteAgentDriver::sendDvlAttTemp()
{
    if(!_allowComs) return;
    if(checkServer(RdiadcpIFServerName) < 1) return;
    double yaw,pitch,roll,temp;
    TimeIF::TimeSpec t;
    DeviceIF::Status status = m_dvl->heading(&yaw, &t);
    status = m_dvl->pitch(&pitch, &t);
    status = m_dvl->roll(&roll, &t);
    status = m_dvl->temperature(&temp, &t);
    MMDvlAttTemp *message = new MMDvlAttTemp(yaw, pitch, roll, temp, getTime());
    send( new ModemMessage(message) );
    dwrite(4)("sent dvl attitude/temp message");
    delete message;
}


/////////////////////////////////////////////////////////
void RemoteAgentDriver::sendHoming()
{
    if(!_allowComs) return;
    if (checkServer(ApogeeHomingIFServerName) < 1) return;
    double azimuth, rangerate;
    TimeIF::TimeSpec t;
    DeviceIF::Status status = m_homing->getUpdate(&azimuth,&rangerate,&t);
    MMHoming *message = new MMHoming(azimuth,rangerate,getTime());
    send( new ModemMessage(&*message) );
    dwrite(4)("sent apogee homing message");
    delete message;
}

/////////////////////////////////////////////////////////
void RemoteAgentDriver::sendGps()
{
    if(!_allowComs) return;
    if (checkServer(GpsIFServerName) < 1) return;
    GpsIF::Fix fix;
    DeviceIF::Status status = m_gps->getFix(&fix);
    Boolean differential = fix.quality == GpsIF::Differential;
    MMGps *message = new MMGps(fix.latitude,fix.longitude,differential,
			       fix.nSatellites,fix.hdop,fix.sampleTime.seconds);
    send( new ModemMessage(&*message) );
    dwrite(4)("sent gps message");
    delete message;
}

/////////////////////////////////////////////////////////
void RemoteAgentDriver::sendNmea()
{
    if(!_allowComs) return;
    if (checkServer(NavigationIFServerName) < 1) return;
    time_t t;
    time_t temp = time(&t);

    // Get the navigation state
    NavigationIF::PositionStruct position;
    NavigationIF::AttitudeStruct attitude;
    m_navigation->state(&position,&attitude);

    // Create the outgoing message
    MMNmea message(Math::radToDeg(position.latitude),
		   Math::radToDeg(position.longitude));
    send( new ModemMessage(&message) );
    dwrite(4)("sent nmea message");
}

/////////////////////////////////////////////////////////
void RemoteAgentDriver::sendExternalPosFix(ModemMessage* mm)
{
    MMNmea* message = new MMNmea(mm);
    if (checkServer(ExternalPosFixIFServerName) < 1) return;
    double latitude, longitude;
    int secs, nanoSecs;
    TimeIF::TimeSpec t;
    message->parseNmea(&latitude, &longitude, &secs, &nanoSecs);
    // dwrite(3)("fix time= %d",secs);
    t.seconds = secs;
    t.nanoSeconds = nanoSecs;
    m_externalPosFix->setFix(latitude,longitude,&t);
    dwrite(4)("set externalposfix");
}

/////////////////////////////////////////////////////////
void RemoteAgentDriver::sendAbortMission()
{
    if (m_supervisor && System::findServer(SupervisorIFServerName) > 0)
	m_supervisor->abortMission();
    dwrite(4)("sent abort mission command");
}

void RemoteAgentDriver::sendKillMission()
{
    if (m_supervisor && System::findServer(SupervisorIFServerName) > 0)
	m_supervisor->killMission();
    dwrite(4)("sent kill mission command");
}

/////////////////////////////////////////////////////////
void RemoteAgentDriver::sendRtcmMessage(ModemMessage* mm)
{
    MMRtcm* message = new MMRtcm(mm);
    if (m_rtcm && System::findServer(RtcmIFServerName) > 0) {
	char buf[96];
	long len;
	len = message->getRtcm(buf);
	m_rtcm->sendRtcm(buf, len);
	dwrite(4)("sent rtcm");
    }
}

/////////////////////////////////////////////////////////
void RemoteAgentDriver::sendUSBLFixMessage(ModemMessage* mm)
{
    MMUSBLFix* message = new MMUSBLFix(mm);
    static char buf[256];
    message->toString(buf);
    dwrite(4)(buf);
    LUSBLFilterIF::SurfaceDataStruct * SurfaceData = new LUSBLFilterIF::SurfaceDataStruct();
    SurfaceData->xgps = message->getEastings();
    SurfaceData->ygps = message->getNorthings();
    SurfaceData->psis = message->getRxHeading();
    SurfaceData->thetas = message->getRxPitch();
    SurfaceData->phis = message->getRxRoll();
    SurfaceData->l = message->getDcl();
    SurfaceData->m = message->getDcm();
    SurfaceData->reset_flag = message->getReset();
    SurfaceData->fixnum = message->getFixNum();
    if (m_lusbl) {
	dwrite(4)("Sending to lusbl server");
	m_lusbl->setSurfaceData(SurfaceData);
    }
    dwrite(4)("sent USBL fix message");
    delete SurfaceData;
}

/////////////////////////////////////////////////////////
void RemoteAgentDriver::serverMessage(ModemMessage* mm)
{
    MMServer* message = new MMServer(mm);
    short nProcess = m_vehicleConfiguration->processCount();
    VehicleConfigurationIF::DeviceInfo dev;
    /*
    for (int i=0; i<nProcess; i++) {
	m_vehicleConfiguration->deviceInfo(i,&dev);
	int port;
	sscanf(dev.port,"/dev/ser%d",&port);
	int j = message->getNumber();
	if (port == message->getNumber()) {
	    DeviceIF *server = new DeviceIF("remoteAgentDriver",0,dev.IFName);
	    if (message->getValue()) {
		server->TSstartAuxTasks();
	    } else {
		server->TSstopAuxTasks();
	    }
	    delete server;
	}
    }
    */
}

void RemoteAgentDriver::sendDriverStatus()
{
    VehicleConfigurationIF::Name processName;
    VehicleConfigurationIF::Name ifName;

    if(!_allowComs) return;
    short nProcess = m_vehicleConfiguration->processCount();
    VehicleConfigurationIF::DeviceInfo dev;
    for (int i=0; i<nProcess; i++)
	{
	    //this returns 0 on success
	    if (m_vehicleConfiguration->getProcessName(i, processName) != 0) {
		dwrite(1)("Error: driverStatus - getProcessName : %d",i );
		continue;
	    };

	    //total hack for leica
	    //	    fprintf(stdout, "Process name: %s\n", processName);
	    if(!strcmp("leicaCompass2", processName)) {
		MMDriver mmDev("leicaCompass2",(char)m_multiLeica->statusTwo());
		send( new ModemMessage(&mmDev) );
		continue;
	    }

	    // if bluefinBatteryDriverN
	    int battGroup = 0;

	    if (strncmp(processName, "bluefinBattery", strlen("bluefinBattery")) == 0 &&
		sscanf(processName, "bluefinBattery%d", &battGroup) ) {
	      dwrite(4)("found multibattery driver %s (%d)", processName, battGroup);
	      // create multibattery IF
	      MultiBatteryIF* mbat = NULL;
	      if (System::findServer(MultiBatteryIFServerName) > 0) {
		dwrite(4)("found multi battery server");
		mbat = new MultiBatteryIF("remoteAgentDriver");
		MMDriver mmDev(processName,(char)mbat->groupStatus(battGroup-1));
		send( new ModemMessage(&mmDev) );
		delete mbat;
		continue;
	      } else {
		dwrite(4)("didn't find multi battery server");
	      }
	    }

	    if (!m_vehicleConfiguration->hasInterface(processName, "/bluefin/DeviceIFServer")) {
	      dwrite(4)("process named %s did not have an interface.", processName);
	      continue;
	    }

	    //this returns 0 on success
	    if (m_vehicleConfiguration->getIFName(i, ifName) != 0) {
		dwrite(1)("Error: driverStatus - getIFName");
		continue;
	    }

	    DeviceIF * serverIF = new DeviceIF("remoteAgentDriver",0,(const char *)ifName);

	    m_vehicleConfiguration->deviceInfo(i,&dev);
	    //fprintf(stdout, "Driver Name %s\n", dev.driver);
	    MMDriver mmDev(dev.name,(char)serverIF->status());
	    send( new ModemMessage(&mmDev) );
	    dwrite(4)("sent device status message %d %s: %d",i, dev.name,serverIF->status());
	    delete serverIF;
	}
    dwrite(4)("sent device status messages");
    //Syslog::remoteWrite(4, "Sending device status");
}

void RemoteAgentDriver::sendControlExecute()
{
    char mes[64];

    if(!_allowComs) return;
    if (checkServer(LayeredControlIFServerName) < 1) return;

    short temp = m_layeredControl->getControlExecute();
    if(temp == 0) sprintf(mes, "controlExecute, not");
    else if(temp == 1) sprintf(mes, "controlExecute, initializing");
    else if(temp == 2) sprintf(mes, "controlExecute, waiting");
    else if(temp == 3) sprintf(mes, "controlExecute, executing");
    else if(temp == 4) sprintf(mes, "controlExecute, goal");

    dwrite(4)("Sending control execute: %d\n", temp);

    MMString str(mes);
    send(new ModemMessage(&str));
}

/////////////////////////////////////////////////////////
void RemoteAgentDriver::sendMissionInfo(ModemMessage* mm)
{
    MMMissionInfo* message = new MMMissionInfo(mm);
    if (checkServer(SupervisorIFServerName) < 1) return;
    m_supervisor->setMission((char *)message->getMissionFileName(),(char *)message->getAbortFileName());
    if (message->getStartMission()) m_supervisor->startMission();
    dwrite(4)("got missionInfo, m: %s, a: %s, sm: %d",message->getMissionFileName(),
	      message->getAbortFileName(), message->getStartMission());
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendEnvironmental()
{
    if(!_allowComs) return;
    if (checkServer(BluefinEnvIFServerName) < 1) return;
    TimeIF::TimeSpec t;
    double humidity, temperature, pressure;
    Boolean leak;
    m_env->humidity(&humidity,&t);
    m_env->temperature(&temperature,&t);
    m_env->pressure(&pressure,&t);
    m_env->leak(&leak,&t);
    MMEnvironmental message(temperature,humidity,pressure,leak);
    send( new ModemMessage(&message) );
    dwrite(4)("sent environmental message");
}

////////////////////////////////////////////////////
void RemoteAgentDriver::sendFluorometer()
{
    if(!_allowComs) return;
    if (checkServer(FluorometerIFServerName) < 1) return;
    TimeIF::TimeSpec t;
    double blueScatter, redScatter, fluorometry, temperature;
    m_flu->blueScatter(&blueScatter,&t);
    m_flu->redScatter(&redScatter,&t);
    m_flu->fluorometry(&fluorometry,&t);
    m_flu->temperature(&temperature,&t);
    MMFluorometer message(blueScatter, redScatter, fluorometry, temperature);
    send( new ModemMessage(&message) );
    dwrite(4)("sent fluorometer message");
}

/////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////

int RemoteAgentDriver::checkServer(char *serverIFName)
{
    // If the server doesn't exist, spawn it and then recognize it with createServers()
    /*
    VehicleConfigurationIF::Driver driver;
    VehicleConfigurationIF::DeviceInfo dev;
    short nProcess = m_vehicleConfiguration->processCount();

    */
    createServers();
    return System::findServer(serverIFName);
}

void RemoteAgentDriver::signalChildHandler( int sigNo )
{
    // createServers();
    return;
}


void RemoteAgentDriver::signalHandler(int signalNo)
{
  Boolean debug = False;
  dprintf("RemoteAgentDriver::signalHandler() %s - call cleanup()\n",
	 "remoteAgentDriver" );

  // Call exit(), which will invoke RemoteAgentDriver::cleanup()
  exit(1);
}


void RemoteAgentDriver::cleanup()
{
  Boolean debug = False;

  if (!_gpRemoteAgentDriver)
    // Doesn't exist
    return;

  dprintf("RemoteAgentDriver::cleanup() - delete 0x%x (named %s)\n",
	  _gpRemoteAgentDriver, "remoteAgentDriver");

  delete _gpRemoteAgentDriver;

  dprintf("RemoteAgentDriver::cleanup() - done deleting 0x%x\n",
	  _gpRemoteAgentDriver);

  dprintf("RemoteAgentDriver::cleanup() - done\n");
  return;
}

void RemoteAgentDriver::enableModem()
{

}

void RemoteAgentDriver::disableModem()
{

}

// packer $(AUV_LOG_DIR)/[session]/[mission]
//
//
// tar -c $(AUV_LOG_DIR)/[session]/[mission] | gzip > $(AUV_LOG_DIR)/[session]/[mission].tar
// poinker remoteAgentServer packDone
void RemoteAgentDriver::pack(const char * target)
{
    char cmd[128];
    sprintf(cmd,"$AUV_BIN_DIR/packer %s &",target);
    dwrite(4)("Pack command: %s\n",cmd);
    system(cmd);
}