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

#include "_reson6046Test.h"

class Reson6046Test;

Reson6046Test::Reson6046Test( int argc, char **argv)
    : Task(Reson6046TestName)
{
    Syslog::write("Entering constructor...");
    _server = NULL;
    VehicleConfigurator::setName( Reson6046TestName );
    System::findServerOrSpawn( Reson6046IFServerName, Reson6046ServerName,"" );

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

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

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

Reson6046Test::~Reson6046Test()
{
    Syslog::write("Entering destructor...\n");
    clean(_server);
    Syslog::write("Leaving destructor...\n");
}

void Reson6046Test::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(" 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(" 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 Reson6046Test::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 Reson6046Test::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];
    Reson6046IF::String32 pulseFilename;
    // char command2, inBuf[32];
    unsigned int count=0;
    float pingRange = 0.0;
    float pingRate = 0.0;
    float pingPower = 0.0;
    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
	}

	switch( command )
	{
	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->startLogging();
		command = lastCommand;
		break;
	    case 'f':
		status = _server->stopLogging();
		command = lastCommand;
		break;
	    default:
		printf("Unknown parameter (%c), setting to (f).", command2);
		command2 = 'f';
	    }
	    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 = '4';
		    //continue;
		}
		else
		    command3=inBuf[0];
	    }
	    switch (command2)
	    {
	    case 'n':
		switch (command3)
		{
		case '0':
		    status = _server->startSubsystem(Reson6046IF::SubBottom);
		    break;
		case '1':
		    status = _server->startSubsystem(Reson6046IF::SideScanLow);
		    break;
		case '2':
		    status = _server->startSubsystem(Reson6046IF::SideScanHigh);
		    break;
		case '3':
		    status = _server->startSubsystem(Reson6046IF::EdgetechAll);
		    break;
		default:
		    printf("Unknown subsystem (%c), setting to (0) {SBP}.\n",command3);
		}
		command = lastCommand;
		break;
	    case 'f':
		switch (command3)
		{
		case '0':
		    status = _server->stopSubsystem(Reson6046IF::SubBottom);
		    break;
		case '1':
		    status = _server->stopSubsystem(Reson6046IF::SideScanLow);
		    break;
		case '2':
		    status = _server->stopSubsystem(Reson6046IF::SideScanHigh);
		    break;
		case '3':
		    status = _server->stopSubsystem(Reson6046IF::EdgetechAll);
		    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: ");
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    //continue;
		    command2 = '0';
		}
		else
		    command2=inBuf[0];
	    }
	    if((command2 == '1') || (command2 == '2'))
	    {
		printf("Enter range 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(Reson6046IF::SideScanLow, pingRange);
			    break;
			case '2':
			    status = _server->setPingRange(Reson6046IF::SideScanHigh, 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: "); // Have to see if Reson supports 1 & 2!
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    //continue;
		    command2 = '3';
		}
		else
		    command2=inBuf[0];
	    }
	    if((command2 == '0') || (command2 == '1') || (command2 == '2'))
	    {
		printf("Enter rate 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(Reson6046IF::SubBottom, pingRate);
			    break;
			case '1':
			    status = _server->setPingRange(Reson6046IF::SideScanLow, pingRate);
			    break;
			case '2':
			    status = _server->setPingRange(Reson6046IF::SideScanHigh, 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: ");
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    //continue;
		    command2 = '4';
		}
		else
		    command2=inBuf[0];
	    }
	    if((command2 == '0') || (command2 == '1') || (command2 == '2') || (command2 == '3'))
	    {
		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(Reson6046IF::SubBottom, pingPower);
			    break;
			case '1':
			    status = _server->setPingPower(Reson6046IF::SideScanLow, pingPower);
			    break;
			case '2':
			    status = _server->setPingPower(Reson6046IF::SideScanHigh, pingPower);
			    break;
			case '3':
			    status = _server->setPingPower(Reson6046IF::EdgetechAll, pingPower);
			    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 pusle filename
	    printf("Setting sonar pulse filename...\n");
	    if((command2 == '-') || ((command2 != '0') && (command2 != '1') && (command2 != '2')))
	    {
		printf("Enter subsystem: 0=SBP, 1=LFSS, 2=HFSS: ");
		if( gets(inBuf) == 0)
		{
		    // Some problem
		    //continue;
		    command2 = '3';
		}
		else
		    command2=inBuf[0];
	    }
	    if((command2 == '0') || (command2 == '1') || (command2 == '2'))
	    {
		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(Reson6046IF::SubBottom, pulseFilename);
		    break;
		case '1':
		    status = _server->setPulseFilename(Reson6046IF::SideScanLow, pulseFilename);
		    break;
		case '2':
		    status = _server->setPulseFilename(Reson6046IF::SideScanHigh, pulseFilename);
		    break;
		default:
		    printf("...\n");
		}
	    }
	    else
		printf("Unknown subsystem %c.\n",command2);
	    command = lastCommand;
	    break;
	case 's':
	    printf("Setting sonar ping rate...\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(Reson6046IF::SubBottom);
		    break;
		case '1':
		    status = _server->getSonarStatus(Reson6046IF::SideScanLow);
		    break;
		case '2':
		    status = _server->getSonarStatus(Reson6046IF::SideScanHigh);
		    break;
		case '3':
		    status = _server->getSonarStatus(Reson6046IF::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();
	    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;
	    defualt:
		printf("Invalid setting command %c.\n",command2);
	    }
	    command = lastCommand;
	    break;
	case 'v':
	    printf("Requesting PLC 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;
	    Reson6046Test::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:
	    Reson6046Test::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)
{
    Reson6046Test test(argc, argv);
    test.runTest(argc, argv);

     return (0);
}