#include <signal.h>
#include <conio.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include "Syslog.h"
#include "SerialParameters.h"
#include "System.h"
#include "EchoSounderIF.h"
#include "EchoSounderDriver.h"
#include "TimeP.h"

void signalHandler(int signalNo);

pid_t echoSounderPID;

void signalHandler(int signalNo)
{
  kill(echoSounderPID, SIGTERM);
  kill(0, SIGQUIT);
  exit(0);
}

int main(int argc, char **argv)
{
  //
  SerialParameters serialParams(&argc, argv);
  char argString[100];
  EchoSounderIF::Data data;
  DeviceIF::Status status;

  sprintf(argString, "-serial %s", serialParams.string());
  signal(SIGQUIT, signalHandler);
  printf("Starting echoSounderServer ...\n");

  if ( (echoSounderPID = System::spawn("echoSounderServer", argString)) == -1) {
    fprintf(stderr, "System::spawn(echoSounderServer) failed\n");
  }
  //
  // Now get a handle to the server IF
  //
  EchoSounderIF *echoSounder;
  try 
  {
    echoSounder = new EchoSounderIF("EchoSounder");
  }
  catch (Exception e) 
  {
    Syslog::write(
        "echoSounderTest -- Caught exception on creation of EchoSounderIF:%s\n",
		  e.msg);
    exit( -1 );
  }

  TimeIF::TimeSpec updateTime;
  Boolean echoReceived;
  double range;

  //echoSounder->set_beacon("V4,1");  

  Boolean First = True;
  
  char buf[256];
  char *helpStr = "Valid commands are:\n"
                  "   help   - Help\n"
                  "   start  - Start\n"
                  "   stop   - Stop\n"
                  "   move degrees   - move the head to the specified value.\n"
                  "   sector degrees - Set the scan sector angle.\n"
                  "   isScanning - Reports if the sonar is a fixed-beam "
                  "or scanning model.\n"
                  "   read   - Display the output.\n"
                  "   quit   - Quit.\n";
  while (True) 
  {
     if( First )
     {
	//
	// Start with "read" set.
	First = False;
	strcpy( buf, "re" );
     }
     else
     {
	// 
	// Read from the command line:
	printf("Test> ");
	if (gets(buf) == 0) continue;  // Some error occurred
     }

     if ( !strncmp(buf, "help", 1) || !strncmp(buf, "Help", 1) || 
	  !strncmp(buf, "" , 1) )
     {
	printf("%s",helpStr);
     }
     else if (!strncmp(buf, "start", 3 )) 
     {
	echoSounder->enable();
     }
     else if (!strncmp(buf, "stop", 3 )) 
     {
	echoSounder->disable();
     }
     else if (!strncmp(buf, "isScanning", 3 )) 
     {
	if( echoSounder->isScanning() )
	   printf(" Yes.\n");
	else
	   printf(" No.\n");
     }
     else if (!strncmp(buf, "move", 4 )) 
     {
	char tempstr[32];
	short trainAngle;
	sscanf( buf, "%s %d", tempstr, &trainAngle);
	printf("%s\n%s\n%d\n",buf, tempstr, trainAngle);
	printf("Move to %d degrees.\n", trainAngle);
	echoSounder->headAngleCmd(trainAngle*PI/180.);
     }
     else if (!strncmp(buf, "sector", 4 )) 
     {
	char tempstr[32];
	short sectorAngle;
	sscanf( buf, "%s %d", tempstr, &sectorAngle);
	printf("%s\n%s\n%d\n",buf, tempstr, sectorAngle);
	printf("Scan a %d degree sector.\n", sectorAngle);
	echoSounder->scanSectorCmd(sectorAngle*PI/180.);
     }
     else if (!strncmp(buf, "read", 2 )) 
     {
	//
	// The two variables below are used to determine when there is new
	// data.
	//
	long   lastSeconds = 0;
	long   lastNanoSeconds = 0;
	unsigned long time, timeLast;
	double dtrainArray[EchoSounderIF::ScanArraySize];
	char *asterisk = "*";
	char *dash     = "-";
	char *space    = " ";
	char *symbol;
	short tindx;

	timeLast = Time::milliseconds();
	
	for( tindx = 0; tindx<EchoSounderIF::ScanArraySize; tindx++ ) dtrainArray[tindx]=-1.;

	while(!kbhit())
	{
	   status = echoSounder->get( &data );

	   if( data.updateTime.seconds     != lastSeconds ||
	       data.updateTime.nanoSeconds != lastNanoSeconds )
	   {
	      //
	      // New data!
	      if( status == DeviceIF::Ok )
	      {
		 double dtrain = ((double)(data.train-trainShift)) * 
		    trainDegPerStep*PI/180.;
		 double sectorEng = 3*PI/180.*data.sector;
		 dtrainArray[data.tindx]=dtrain;
		 printf("\n\n");
		 printData( data.header, data.headID, data.serialStatus, 
			    dtrain,
			    sectorEng,
			    data.maxRangeCfrm, data.range, data.nBytes, 
			    data.nBytesRead, data.freq, 
			    data.crashCtr, data.echoData);
		 //if( data.sector != 0 )
		 {
		    printf("\n\n");
		    printf(" scanArray = \n");
		    //for( int i=0; i<EchoSounderIF::ScanArraySize; i++ )
		    for( short i=data.tindxMin; i<=data.tindxMax; i++ )
		    {
		       dtrainArray[i] = echoSounder->getHeadAngle(i);
		       symbol = space;
		       //if( data.ranges[i] != -2 )
		       {
			  if( i == data.tindxMax ) symbol = dash;
			  if( i == data.tindxMin ) symbol = dash;
			  if( i == data.tindx    ) symbol = asterisk;
			  printf(" %s %4d %7.2f %7d  %ld\n", symbol, 
			         i, 
			         dtrainArray[i]*180./PI, 
			         data.ranges[i],
			         data.times[i]);
		       }
		    }
		    printf("\n\n");
		 }
		 lastSeconds     = data.updateTime.seconds;
		 lastNanoSeconds = data.updateTime.nanoSeconds;
		 timeLast = Time::milliseconds();
	      }
	      else
		 printf("echoSounderTest - Error; Status = %d\n", status);
	   }
	   else
	      printf("echoSounderTest - No response.\n");
	   sleep(1);
	}
     }
     else if ( !strncmp(buf, "q", 1 ) || !strncmp(buf, "Q", 1 ) ) break;
     else printf(" Error: Command unrecognized.\n %s", helpStr);
  }

  // Just wait for termination signal
  signalHandler(0);
  return 0;
}