//=========================================================================
// Summary  : Test program for EdgetechFSDW
// Filename : _edgetechFSDWTest.cc
// Author   : Reed Christenson
// Project  : EdgetechFSDW Driver
// Revision : 0.0
// Created  : 2002.03.21
// Modified : 2002.03.21
//=========================================================================
// Description : EdgetechFSDW
//=========================================================================

#include "_edgetechFSDWTest.h"

class EdgetechFSDWTest;

EdgetechFSDWTest::EdgetechFSDWTest( int argc, char **argv)
    : Task(EdgetechFSDWTestName)
{
    Syslog::write("Entering constructor...");
    _server = NULL;
    //VehicleConfigurator::setName( EdgetechFSDWTestName );
    //System::findServerOrSpawn( EdgetechFSDWIFServerName, EdgetechFSDWServerName,"" );

    try {
	Syslog::write("edgetechFSDWTest::Spawning edgetechFSDWServer");
        if ((_serverPid = System::spawn("edgetechFSDWServer", "")) == -1)
          Syslog::write("edgetechFSDWTest::System.spawn() failed");
    }catch( Exception e ) {
	Syslog::write("edgetechFSDWTest::Couldn't launch edgetechFSDWServer");
	exit(-1);
    }

    try {
	_server = new EdgetechFSDWIF(EdgetechFSDWServerName);
	Syslog::write("Connected to %s",EdgetechFSDWIFServerName);
    }catch( SharedObjectClient::MissingServer e ) {
	Syslog::write("Couldn't connect to %s",EdgetechFSDWIFServerName);
	printf("Leaving constructor...");
	exit(-1);
    } catch(...) {
	Syslog::write("Caught exception while "" creating EdgetechFSDWIF");
    }

    // Initialize values.
    _seconds = 0;
    //_edgetechFSDWStatus = EdgetechFSDWIF::Standby;
    _callbackPeriod = 0;
    //strcpy(&_ipAddressString[0],"127.000.000.001");
    //_ipAddressPtr = &_ipAddressString[0];
    //_broadcastPort = 1111;
    //_broadcastTime.seconds = 0;
    //_broadcastTime.nanoSeconds = 0;

    // Let's not worry about getting info back all of the time.
    _defaultResponseRequest = false;

    Syslog::write("Leaving constructor...\n");
}

EdgetechFSDWTest::~EdgetechFSDWTest()
{
    Syslog::write("Entering destructor...\n");
    clean(_server);
    kill(_serverPid, SIGTERM);
    Syslog::write("Leaving destructor...\n");
}

void EdgetechFSDWTest::displayHelp()
{
    printf(" Q  : (Q)uit test\n");
    printf(" H  : this (H)elp\n");
    printf(" A  : (A)utomatic mode\n");
    printf(" M  : (M)anual mode\n");
    printf(" N  : driver logging o(N)\n");
    printf(" F  : driver logging of(F)\n");
    printf("--- + ---\n");
    printf(" l  : set state of sonar (l)ogging\n");
//    printf(" J	: set state of 7K center logging\n");
    printf(" m  : set ping (m)ode of devices\n");
    printf(" r  : set ping (r)ange\n");
    printf(" a  : set ping r(a)te\n");
    printf(" p  : set ping (p)ower\n");
    printf(" g  : set receiver (g)ain of SBP or MB\n");
    printf(" w  : set pulse (w)idth of SBP or MB\n");
//    printf(" b  : set (b)ottom detection filters parameters for Multibeam\n");
//    printf(" B  : set (B)ottom detection filters On/Off for Multibeam\n");
//    printf(" R  : set sp(R)eading loss for Multibeam\n");
//    printf(" V  : set sound (V)elocity for Multibeam\n");
//    printf(" O  : set abs(O)rption loss for Multibeam\n");
//    printf(" S  : set (S)nippet control On/Off and window size for Multibeam\n");
    printf(" f  : set ping pulse (f)ilename\n");
    printf(" t  : set (t)ime\n");
    printf("--- + ---\n");
    printf(" v  : (v)ersion\n");
    printf(" e  : v(e)rbose mode\n");
    printf(" s  : (s)tatus\n");
//    printf(" o  : get m(o)dules\n");
//    printf(" u  : get r(u)n list\n");
    //printf(" t  : (t)ime synchronize\n"); // needs to be automated.
}


void EdgetechFSDWTest::checkStatus(DeviceIF::Status status)
{
    char statusString[16];

    switch(status)
    {
    case DeviceIF::Offline:
	strcpy(&statusString[0],"Offline");
	break;
    case DeviceIF::Initializing:
	strcpy(&statusString[0],"Initializing");
	break;
    case DeviceIF::Ok:
	strcpy(&statusString[0],"Ok");
	break;
    case DeviceIF::Error:
	strcpy(&statusString[0],"Error");
	break;
    case DeviceIF::FailedInit:
	strcpy(&statusString[0],"FailedInit");
	break;
    case DeviceIF::FailedRead:
	strcpy(&statusString[0],"FailedRead");
	break;
	//case DeviceIF::Failed:
	//strcpy(&statusString[0],"Failed");
	//break;
    default:
	strcpy(&statusString[0],"Unknown Status");
	break;
    }
    printf("Status = %s.\n",statusString);
}

void EdgetechFSDWTest::runTest(int argc, char **argv)
{
    bool go = True;
    //bool continous = True;
    bool automatic = False;
    DeviceIF::Status status;
    char command, command2, command3, inBuf[32],  defaultCommand, lastCommand, cString[8];
    EdgetechFSDWIF::String32 pulseFilename;
    // char command2, inBuf[32];
    unsigned int count=0;
    float pingRange = 0.0;
    float pingRate = 0.0;
    float pingPower = 0.0;
    float recvGain = 0.0;
    float loss = 0.0;
    float mpers = 0.;
    float pulseWidth = 0.0;
    int windowSize;

    int result = 0;

    defaultCommand = 'H';
    lastCommand=defaultCommand;
    command=defaultCommand;
    command2 = '-';
    command3 = '-';

    displayHelp();
    do
    {
	if(automatic)
	{
	    if (kbhit())
            {
		command = getch();
		count = 0;
		// can check for extended commands here
		// Only look for second command for: logging, ping
		if((command == 'l') || (command == 'p'))
		{
		    command2 = '-';
		    command3 = '-';
		}
	    }
	    else
		command = '-';
	}
	else
	{
	    if( gets(cString) == 0 )
	    {
		// Some problem
		continue;
	    }
	    command=cString[0];
	    command2=cString[1];
	    command3=cString[2];
	    // can check for extended commands here
	}

	float minRange, maxRange, minDepth, maxDepth;
	switch( command )
	{
//	case 'b':
//	  printf("Enter min range, max range, min depth, and max depth\n");
//	  printf("separated by commas,  for example: 15.0, 14.0, 13.2, 200.1\n");
//	  printf(" : ");
//	  result = scanf("%f,%f,%f,%f",&minRange,&maxRange,&minDepth,&maxDepth);
//	  printf("Received filters of %f,%f,%f,%f, with result of %d.\n",
//		 minRange, maxRange, minDepth, maxDepth, result);
//	  if(result == 4)
//	  {
//	    if(minRange < 0.0)
//	    {
//	      printf("Invalid min range %f, aborting.\n", minRange);
//	      break; //case 'b'
//	    }
//	    else if(maxRange < 0.0)
//	    {
//	      printf("Invalid max range %f, aborting.\n", maxRange);
//	      break; //case 'b'
//	    }
//	    else if(minDepth < 0.0)
//	    {
//	      printf("Invalid min depth %f, aborting.\n", minDepth);
//	      break; //case 'b'
//	    }
//	    else if(maxDepth < 0.0)
//	    {
//	      printf("Invalid max depth %f, aborting.\n", maxDepth);
//	      break; //case 'b'
//	    }
//	    else
//	    {
//	      printf("Sending request for bottom detection filters. TODO NOT SUPPORTED\n");
////	      status = _server->setMBBottomDetectionFilters(minRange,maxRange,
////							    minDepth,maxDepth);
//	      break;
//	    }
//	  }
//	  break;
//
//	case 'B':
//		printf("TODO NOT SUPPORTED Enter Depth and Range detection on/off (example: 1,0): ");
//
////	  if (2 != (result = scanf("%c,%c",&command2,&command3)))
////	   break;
////
////    printf("Got %c and %c\n", command2, command3);
////	  if (command2 != '0' && command2 != '1')
////	  {
////		  printf("Unknown command2 (%c), aborting.\n",command2);
////		  break;
////		}
////
////	  if (command3 != '0' && command3 != '1')
////	  {
////		  printf("Unknown command (%c), aborting.\n",command3);
////		  break;
////		}
////
////    status = _server->setMBBottomDetectDepthRangeOn((command2 == '1'), (command3 == '1'));
//    break;
//
//  case 'S':
// 		printf("TODO NOT SUPPORTED Enable snippet control: 0=Off, 1=On: ");
////		if( gets(inBuf) == 0)
////		{
////		    // Some problem
////		    command2 = '2';
////		    //continue;
////		}
////		else
////		    command2=inBuf[0];
////
////	  if (command2 != '0' && command2 != '1')
////	  {
////		  printf("Unknown option (%c), aborting.\n",command3);
////		  break;
////		}
////
////	  printf("Enter #samples around bottom detection for each beam: ");
////	  result = scanf("%d",&windowSize);
////	  printf("Received window size of %d\n", windowSize);
////	  if(result != EOF)
////	  {
////	    if(windowSize <= 0)
////	    {
////	      printf("Invalid size %d, aborting.\n", windowSize);
////	      break; //case 's'
////	    }
////	    else
////	    {
////	      printf("Sending request to enable(%c) window size of %d\n", command3, windowSize);
////	      status = _server->setMBSnippetControl((command2 == '1'), windowSize);
////	      break;
////	    }
////	  }
//	  break;

	case 'g':
		printf("Enter subsystem: 0=SBP: "
//				", 1=MB (NOT SUPPORTED TODO): "
				);
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    command3 = '5';
		    //continue;
		}
		else
		    command3=inBuf[0];

	  if (command3 != '0' && command3 != '1')
	  {
		  printf("Unknown subsystem (%c), aborting.\n",command3);
		  break;
		}
		
	  printf("Enter receiver gain (Value is * 1000.0 (only 1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024,	and 2048 supported): ");
	  result = scanf("%f",&recvGain);
	  printf("Received gain of %f, with result of %d.\n", recvGain, result);
	  if(result != EOF)
	  {
	    if(recvGain < 0.0)
	    {
	      printf("Invalid gain %f, aborting.\n",recvGain);
	      break; //case 'g'
	    }
	    else
	    {
	      printf("Sending request for %.2f gain.\n", recvGain);
//	      if (command3=='1') status = _server->setRxGain(EdgetechFSDWIF::Multibeam, recvGain);
	      if (command3=='0') status = _server->setSBPGain(recvGain, true);
	      break;
	    }
	  }
	  break;

//	case 'R':
//	  printf("TODO NOT SUPPORTED Enter Spreading Loss in dB: ");
//	  result = scanf("%f",&loss);
//	  printf("Received loss of %f, with result of %d.\n", loss, result);
//	  if(result != EOF)
//	  {
//	    if(loss < 0.0 || loss > 60.)
//	    {
//	      printf("Invalid loss %f, aborting.\n", loss);
//	      break;
//	    }
//	    else
//	    {
//	      printf("Sending request for %.2f spreading loss.\n", loss);
//	      status = _server->setMBSpreadingLoss(loss);
//	      break;
//	    }
//	  }
//	  break;

//	case 'V':
//	  printf("TODO NOT SUPPORTED Enter new sound velocity in meters/sec: ");
//	  result = scanf("%f",&mpers);
//	  printf("Received velocity of %f, with result of %d.\n", mpers, result);
//	  if(result != EOF)
//	  {
//      printf("Sending request for %.2f sound velocity.\n", mpers);
//      status = _server->setMBSoundVelocity(mpers);
//      break;
//	  }
//	  break;

//	case 'O':
//	  printf("TODO NOT SUPPORTED Enter Absoprtion Loss in dB per km: ");
//	  result = scanf("%f",&loss);
//	  printf("Received loss of %f, with result of %d.\n", loss, result);
//	  if(result != EOF)
//	  {
//      printf("Sending request for %.2f absoprtion loss.\n", loss);
//      status = _server->setMBAbsorptionLoss(loss);
//      break;
//	  }
//	  break;

	case 'w':
		printf("Enter subsystem: 0=SBP: "
//				", 1=MB (TODO NOT SUPPORTED):"
				);
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    command3 = '5';
		    //continue;
		}
		else
		    command3=inBuf[0];

	  if (command3 != '0' && command3 != '1')
	  {
		  printf("Unknown subsystem (%c), aborting.\n",command3);
		  break;
		}
		
	  printf("Enter pulse width (milliseconds): ");
	  result = scanf("%f",&pulseWidth);
	  printf("Received width of %f, with result of %d.\n", pulseWidth, result);
	  if(result != EOF)
	  {
	    if(pulseWidth < 0.0)
	    {
	      printf("Invalid width %f, aborting.\n", pulseWidth);
	      break; //case 'w'
	    }
	    else
	    {
	      printf("Sending request for %.2fms pulseWidth.\n", pulseWidth);
//	      if (command3 == '1') status = _server->setPulseWidth(EdgetechFSDWIF::Multibeam, pulseWidth);
	      if (command3 == '0') status = _server->setSBPDuration(pulseWidth,true);
	      break;
	    }
	  }
	  break;

	case 'l': // Set Logging State
	    printf("Requesting start or stop of sonar data logging...\n");
	    if((command2 == '-') || ((command2 != 'n') && (command2 != 'f')))
	    {
		printf("Enter sonar logging preference: n=on, f=off: ");
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    //continue;
		    command2 = 'f';
		}
		else
		    command2=inBuf[0];
	    }
	    switch (command2)
	    {
	    case 'n':
		status = _server->startEdgetechStorage(_defaultResponseRequest);
		command = lastCommand;
		break;
	    case 'f':
		status = _server->stopEdgetechStorage(_defaultResponseRequest);
		command = lastCommand;
		break;
	    default:
		printf("Unknown parameter (%c), setting to (f).", command2);
		command2 = 'f';
	    }
	    break;
        case 'J': // Send Test KearfottRecord
        	printf("Sending test for all records...\n");
        	EdgetechFSDWIF::sCompositeSituation compSit;
        	compSit.flags=0;
        	compSit.vDirections=1;
        	compSit.timestamp.seconds=1;
        	compSit.timestamp.nanoSeconds=2;
        	compSit.latitudeD=3;
        	compSit.longitudeD=4;
        	compSit.depthM=5;
        	compSit.altitudeM=6;
        	compSit.heaveM=7;
        	compSit.velocity1MS=8;
        	compSit.velocity2MS=9;
        	compSit.velocityDownMS=10;
        	compSit.pitchD=11;
        	compSit.rollD=12;
        	compSit.headingD=13;
        	compSit.soundSpeedMS=14;
        	compSit.waterTempC=15;
        	status = _server->relayCompositeSituation(&compSit);


        	EdgetechFSDWIF::sSituation situation;

			situation.timestamp.seconds=1;
        	situation.timestamp.nanoSeconds=1000;
        	situation.latitudeD = 1;
			situation.longitudeD = 2;
			situation.depthM =3;
			situation.headingD = 4;
			situation.pitchD = 5;
			situation.rollD= 6;
			situation.xRelativePosM= 7;
			situation.yRelativePosM= 8;
			situation.zRelativePosM= 9;
			situation.xVelocityMS= 10;
			situation.yVelocityMS= 11;
			situation.zVelocityMS= 12;
			situation.northVelocityMS= 13;
			situation.eastVelocityMS= 14;
			situation.downVelocityMS= 15;
			situation.xAngularRateDS= 16;
			situation.yAngularRateDS= 17;
			situation.zAngularRateDS= 18;
			situation.xAccelerationMS= 19;
			situation.yAccelerationMS= 20;
			situation.zAccelerationMS= 21;
			situation.latitudeStandardDeviationM= 22;
			situation.longitudeStandardDeviationM= 23;
			situation.depthStandardDeviationM= 24;
			situation.headingStandardDeviationD= 25;
			situation.pitchStandardDeviationD= 26;
			situation.rollStandardDeviationD= 027;
			status = _server->relaySituation(&situation);

			EdgetechFSDWIF::sPressureRecord pressureRecord;

			pressureRecord.pressurePSI = 0;
			pressureRecord.tempC = 1;
			pressureRecord.salPSU = 2;
			pressureRecord.condSM = 3;
			pressureRecord.sosMS = 4;
			pressureRecord.depthM = 5;
			pressureRecord.flags = 6;
        	status = _server->relayPressureRecord(&pressureRecord);


			EdgetechFSDWIF::sAltitudeRecord altitudeRecord;

			altitudeRecord.flags = 0;
			altitudeRecord.altitudeM = 1;
			altitudeRecord.forwardVelocityMS = 2;
			altitudeRecord.crossTrackVelocityMS = 3;
			status = _server->relayAltitudeRecord(&altitudeRecord);

			command = lastCommand;

            break;
	case 'm': // Set Ping Mode
	    printf("Setting ping mode of device...\n");
	    if((command2 == '-') || ((command2 != 'n') && (command2 != 'f')))
	    {
		printf("Enter pinging preference: n=on, f=off: ");
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    command2 = 'f';
		    //continue;
		}
		else
		    command2=inBuf[0];
	    }
	    printf("Requesting subsystem...\n");
	    if((command3 == '-') || ((command3 != '0') && ( command3 != '1') && (command3 != '2') && (command3 != '3')))
	    {
		printf("Enter subsystem: 0=SBP; 1=LFSS, 2=HFSS, 3=ALL:");
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    command3 = '5';
		    //continue;
		}
		else
		    command3=inBuf[0];
	    }
	    switch (command2)
	    {
	    case 'n':
		switch (command3)
		{
		case '0':
		    status = _server->startSubsystem(EdgetechFSDWIF::SubBottom, _defaultResponseRequest);
		    break;
		case '1':
		    status = _server->startSubsystem(EdgetechFSDWIF::SideScanLow, _defaultResponseRequest);
		    break;
		case '2':
		    status = _server->startSubsystem(EdgetechFSDWIF::SideScanHigh, _defaultResponseRequest);
		    break;
//		case '4':
//		    status = _server->startSubsystem(EdgetechFSDWIF::Multibeam);
//		    break;
		case '3':
		    status = _server->startSubsystem(EdgetechFSDWIF::EdgetechAll, _defaultResponseRequest);
		    break;
		default:
		    printf("Unknown subsystem (%c), setting to (0) {SBP}.\n",command3);
		}
		command = lastCommand;
		break;
	    case 'f':
		switch (command3)
		{
		case '0':
		    status = _server->stopSubsystem(EdgetechFSDWIF::SubBottom, _defaultResponseRequest);
		    break;
		case '1':
		    status = _server->stopSubsystem(EdgetechFSDWIF::SideScanLow, _defaultResponseRequest);
		    break;
		case '2':
		    status = _server->stopSubsystem(EdgetechFSDWIF::SideScanHigh, _defaultResponseRequest);
		    break;
//		case '4':
//		    status = _server->stopSubsystem(EdgetechFSDWIF::Multibeam);
//		    break;
		case '3':
		    status = _server->stopSubsystem(EdgetechFSDWIF::EdgetechAll, _defaultResponseRequest);
		    break;
		default:
		    printf("Unknown subsystem (%c), setting to (0) {SBP}.\n",command3);
		}
		command = lastCommand;
		break;
	    default:
		printf("Unknown parameter (%c), setting to (f).", command2);
		command2 = 'f';
	    }
	    break;
	case 'r': // Set Range
	    printf("Setting sonar ping range...\n");
	    if((command2 == '-') || ((command2 != '1') && (command2 != '2')))
	    {
		printf("Enter subsystem: 1=LFSS, 2=HFSS: "
//				", 3=MB(TODO NOT SUPPORTED): "
				);
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    //continue;
		    command2 = '0';
		}
		else
		    command2=inBuf[0];
	    }
	    if((command2 == '1') || (command2 == '2') || (command2 == '3'))
	    {
		printf("Enter range in meters for subsystem %c: ",command2);
		result = scanf("%f",&pingRange);
		printf("Received range of %f, with result of %d.\n", pingRange, result);
		if(result != EOF)
		{
		    if(pingRange < 0.0)
		    {
			printf("Invalid range %f, setting to 0.0.\n",pingRange);
			pingRange = 0.0;
		    }
		    else
		    {
			printf("Sending request for %.2fm range for subsystem %c...\n",
			       pingRange, command2);
			switch(command2)
			{
			case '1':			    
			    status = _server->setPingRange(EdgetechFSDWIF::SideScanLow, pingRange, _defaultResponseRequest);
			    break;
			case '2':
			    status = _server->setPingRange(EdgetechFSDWIF::SideScanHigh, pingRange, _defaultResponseRequest);
			    break;
//			case '3':
//			    status = _server->setPingRange(EdgetechFSDWIF::Multibeam, pingRange);
//			    break;
			default:
			    printf("Invalid subsystem %c.\n",command2);
			    break;
			}
		    }
		}
		else
		    printf("Unable to get range.\n");
	    }
	    else
		printf("Invalid subsystem %c.\n",command2);
	    command = lastCommand;
	    break;
	case 'a': // Set Rate
	    printf("Setting sonar ping rate...\n");
	    if((command2 == '-') || ((command2 != '0') && (command2 != '1') && (command2 != '2')))
	    {
	    	printf("Enter subsystem: 0=SBP, 1=LFSS, 2=HFSS: "
//	    			", 3=MB(TODO NOT SUPPORTED): "
	    			); // Have to see if Reson supports 1 & 2!
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    //continue;
		    command2 = '4';
		}
		else
		    command2=inBuf[0];
	    }
	    if((command2 == '0') || (command2 == '1') || (command2 == '2') ||
	       (command2 == '3'))
	    {
		printf("Enter rate (pulse per second) for subsystem %c: ",command2);
		result = scanf("%f",&pingRate);
		printf("Received rate of %f, with result of %d.\n", pingRate, result);
		if(result != EOF)
		{
		    if(pingRate < 0.0)
		    {
			printf("Invalid rate %f, setting to 0.0.\n",pingRate);
			pingRate = 0.0;
		    }
		    else
		    {
			printf("Sending request for %.2fHz rate for subsystem %c...\n",
			       pingRate, command2);
			switch(command2)
			{
			case '0':
			    status = _server->setPingRate(EdgetechFSDWIF::SubBottom, pingRate, _defaultResponseRequest);
			    break;
			case '1':
			    status = _server->setPingRate(EdgetechFSDWIF::SideScanLow, pingRate, _defaultResponseRequest);
			    break;
			case '2':
			    status = _server->setPingRate(EdgetechFSDWIF::SideScanHigh, pingRate, _defaultResponseRequest);
			    break;
//			case '3':
//			    status = _server->setPingRate(EdgetechFSDWIF::Multibeam, pingRate);
//			    break;
			default:
			    printf("Invalid subsystem %c.\n",command2);
			    break;
			}
		    }
		}
		else
		    printf("Unable to get rate.\n");
	    }
	    else
		printf("Invalid subsystem %c.\n",command2);
	    command = lastCommand;
	    break;
	case 'p': // Set Power
	    printf("Setting sonar ping power...\n");
	    if((command2 == '-') || ((command2 != '0') && (command2 != '1') && (command2 != '2')))
	    {
		printf("Enter subsystem: 0=SBP, 1=LFSS, 2=HFSS, 3=ALL: "
//				", 4=MB(TODO NOT SUPPORTED: "
				);
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    //continue;
		    command2 = '5';
		}
		else
		    command2=inBuf[0];
	    }
	    if((command2 == '0') || (command2 == '1') || (command2 == '2') ||
	       (command2 == '3') || (command2 == '4'))
	    {
		printf("Enter ping power (%) for subsystem %c: ",command2);
		result = scanf("%f",&pingPower);
		printf("Received rage of %f, with result of %d.\n", pingPower, result);
		if(result != EOF)
		{
		    if(pingPower < 0.0)
		    {
			printf("Invalid ping power %f, setting to 0.0.\n",pingPower);
			pingPower = 0.0;
		    }
		    else
		    {
			printf("Sending request for %.2f power for subsystem %c...\n",
			       pingPower, command2);
			switch(command2)
			{
			case '0':
			    status = _server->setPingPower(EdgetechFSDWIF::SubBottom, pingPower, _defaultResponseRequest);
			    break;
			case '1':
			    status = _server->setPingPower(EdgetechFSDWIF::SideScanLow, pingPower, _defaultResponseRequest);
			    break;
			case '2':
			    status = _server->setPingPower(EdgetechFSDWIF::SideScanHigh, pingPower, _defaultResponseRequest);
			    break;
//			case '4':
//			    status = _server->setPingPower(EdgetechFSDWIF::Multibeam, pingPower);
//			    break;
			case '3':
			    status = _server->setPingPower(EdgetechFSDWIF::EdgetechAll, pingPower, _defaultResponseRequest);
			    break;
			default:
			    printf("Invalid subsystem %c.\n",command2);
			    break;
			}
		    }
		}
		else
		    printf("Unable to get ping power.\n");
	    }
	    else
		printf("Invalid subsystem %c.\n",command2);
	    command = lastCommand;
	    break;
	case 'f': // set sonar pulse filename
	    printf("Setting sonar pulse filename...\n");
	    if((command2 == '-') || ((command2 != '0') && (command2 != '1') && (command2 != '2')))
	    {
		printf("Enter subsystem: 0=SBP, 1=LFSS, 2=HFSS, 3=MB(TODO NOT SUPPORTED): ");
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    //continue;
		    command2 = '4';
		}
		else
		    command2=inBuf[0];
	    }
	    if((command2 == '0') || (command2 == '1') || (command2 == '2') ||
	       (command2 == '3'))
	    {
		printf("Enter Pulse Filename: ");
		gets(&pulseFilename[0]);
		printf("Setting subsystem %c to use pulse filename %s...\n", command2, pulseFilename);
		switch(command2)
		{
		case '0':
		    status = _server->setPulseFilename(EdgetechFSDWIF::SubBottom, pulseFilename, _defaultResponseRequest);
		    break;
		case '1':
		    status = _server->setPulseFilename(EdgetechFSDWIF::SideScanLow, pulseFilename, _defaultResponseRequest);
		    break;
		case '2':
		    status = _server->setPulseFilename(EdgetechFSDWIF::SideScanHigh, pulseFilename, _defaultResponseRequest);
		    break;
//		case '3':
//		    status = _server->setPulseFilename(EdgetechFSDWIF::Multibeam, pulseFilename);
//		    break;
		default:
		    printf("...\n");
		}
	    }
	    else
		printf("Unknown subsystem %c.\n",command2);
	    command = lastCommand;
	    break;
	case 's':
	    printf("Getting sonar status...\n");
	    if((command2 == '-') || ((command2 != '0') && (command2 != '1') && (command2 != '2') && (command2 != '3')))
	    {
		printf("Enter subsystem: 0=SBP, 1=LFSS, 2=HFSS: "
//				"3=PLC: "
				);
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    //continue;
		    command2 = '4';
		}
		else
		    command2=inBuf[0];
	    }
	    if((command2 == '0') || (command2 == '1') || (command2 == '2') || (command2 == '3'))
	    {
		switch(command2)
		{
		case '0':
		    status = _server->getSonarStatus(EdgetechFSDWIF::SubBottom, _defaultResponseRequest);
		    break;
		case '1':
		    status = _server->getSonarStatus(EdgetechFSDWIF::SideScanLow, _defaultResponseRequest);
		    break;
		case '2':
		    status = _server->getSonarStatus(EdgetechFSDWIF::SideScanHigh, _defaultResponseRequest);
		    break;
//		case '3':
//		    status = _server->getSonarStatus(EdgetechFSDWIF::PLC);
//		    break;
		default:
		    printf("...\n");
		}
	    }
	    else
		printf("Unknown subsystem %c.\n",command2);
	    command = lastCommand;
	    break;
	case 't':
	    printf("Setting sonar time to current time...");
	    status = _server->setTime(_defaultResponseRequest);
	    command = lastCommand;
	    break;
	case 'e': // Set Verbose Mode
	    printf("Setting verbose mode...\n");
	    if((command2 == '-') || ((command2 != 'n') && ( command2 != 'f')))
	    {
		printf("Set verbose mode to f=Off or n=On: ");
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    command2 = '-';
		    //continue;
		}
		else
		    command2=inBuf[0];
	    }
	    switch(command2)
	    {
	    case 'f':
		status = _server->setVerboseMode(0);
		break;
	    case 'n':
		status = _server->setVerboseMode(1);
		break;
	    default:
		printf("Invalid setting command %c.\n",command2);
	    }
	    command = lastCommand;
	    break;
	case 'v':
	    printf("Requesting Edgetech version...");
	    status = _server->getVersion();
	    command = lastCommand;
	    break;
//	case 'o':
//	    printf("Requesting Modules...");
//	    status = _server->getModules();
//	    command = lastCommand;
//	    break;
//	case 'u':
//	    printf("Requesting Run List...");
//	    status = _server->getRunList();
//	    command = lastCommand;
//	    break;
	case 'A':
	    printf("Switching to Automatic mode...\n");
	    automatic = True;
	    command = lastCommand;
	    break;
	case 'M':
	    printf("Switching to Manual mode...\n");
	    automatic = False;
	    command = lastCommand;
	    EdgetechFSDWTest::displayHelp();
	    break;
	case 'L': // Begin logging data.
	    printf("Requesting driver data logging on...\n");
	    if((command2 == '-') || ((command2 != 'n') && (command2 != 'f')))
	    {
		printf("Enter driver logging preference: n=on, f=off: ");
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    //continue;
		    command2 = 'f';
		}
		else
		    command2=inBuf[0];
	    }
	    switch (command2)
	    {
	    case 'n':
		status = _server->dataLoggingOn();
		break;
	    case 'f':
		status = _server->dataLoggingOff();
		break;
	    default:
		printf("Unknown parameter (%c), setting to (f).", command2);
		command2 = 'f';
	    }
	    command = lastCommand;
	    break;
// 	case 'F': Stop logging data.
// 	    printf("Requesting driver data logging off....\n");
// 	    status = _server->dataLoggingOff();
// 	    command = lastCommand;
// 	    break;
	case 'Q': // Quit test program.
	    printf("Quitting...\n");
	    go = False;
	    break;
	case '-':
	    break;
	case 'H': // Same for default.
	default:
	    EdgetechFSDWTest::displayHelp();
	    command = lastCommand;
	}
	printf("Return ");
	checkStatus(status);
	printf("Driver ");
	checkStatus(_server->status());

	if(automatic)
	{
	    sleep(1);
	}
	count++;
	if(count>999)
	{
	    count=0;
	}
    }while(go);
}


int main( int argc, char **argv)
{
    EdgetechFSDWTest test(argc, argv);
    test.runTest(argc, argv);

     return (0);
}