#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 "SonardyneServer.h"

void signalHandler(int signalNo);

pid_t sonardynePID;

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

int main(int argc, char **argv)
{
  //
  SerialParameters serialParams(&argc, argv);
  char argString[100];

  Syslog::write("sonardyneTest.cc was last compiled on %s, %s\n",__DATE__, __TIME__);

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

  if ( (sonardynePID = System::spawn("sonardyneServer", argString)) == -1) {
    fprintf(stderr, "System::spawn(sonardyneServer) failed\n");
  }

  // Now get a handle to the server IF
  //
  UsblIF *sonardyne;
  try {
    sonardyne = new UsblIF("Sonardyne");
  }
  catch (Exception e) 
  {
    Syslog::write(
        "sonardyneTest -- Caught exception on creation of UsblIF:%s\n",
		  e.msg);
    exit( -1 );
  }

  float r, x, y, z;
  short tat;
  UsblIF::UsblDataStatus status;
  TimeIF::TimeSpec updateTime, lastUpdateTime;

  lastUpdateTime.seconds = lastUpdateTime.nanoSeconds = 0;


  Boolean First = True;
  
  char buf[256];
  char *helpStr = "Valid commands are:\n"
                  "   help   - Help\n"
                  "   address- Change beacon address\n"
                  "   read   - Display the outout.\n"
                  "   quit   - Quit.\n";



  while (True)
  {
     // 
     // 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, "address", 2 )) 
     {
	char address[32];
	char tempstr[32];
	sscanf( buf, "%s %s", tempstr, address);
	printf("%s\n%s\n%s\n",buf, tempstr, address);
	printf("Reset beacon address to %s.\n", address);
	sonardyne->set_beacon(address);  
     }
     else if (!strncmp(buf, "quit", 2 )) 
     {
        break;
     }
     else if (!strncmp(buf, "read", 2 )) 
     {
	sonardyne->start();
	while(!kbhit())
	{

	   if (sonardyne->ready())
	   {
	      sonardyne->get_range_data(&r, &x, &y, &z, &tat, &updateTime, &status);
	      if( (updateTime.seconds     != lastUpdateTime.seconds) |
		  (updateTime.nanoSeconds != lastUpdateTime.nanoSeconds) )
	      {
		 lastUpdateTime.seconds     = updateTime.seconds;
		 lastUpdateTime.nanoSeconds = updateTime.nanoSeconds;
		 printf("\n");
		 printf("\tRange = %f\tX = %f\tY = %f\tZ = %f\n", r, x, y, z);
		 printf("\tTurnaround Time = %d milliseconds\n\tStatus of data: ", tat);
		 switch(status) 
		 {
		    case UsblIF::AllGood:
		       printf("All Good\n");
		       break;
          
		    case UsblIF::RangeOnly:
		       printf("Range Only\n");
		       break;
          
		    case UsblIF::Stale:
		       printf("Beacon Not Detected\n");
		       break;
          
		    case UsblIF::Undefined:
		       printf("Undefined\n");
		       break;

		    default:
		       printf("Unexpected value of %d\n", status);
		       break;
		 } //end switch
	      } //end if( (updateTime...
	      r = x = y = z = tat = 0.;

	   } //end if (sonardyne->ready())
    
	   sleep(1);
	} //end while(!kbhit())
	sonardyne->stop();

     } //end if ( !strncmp ...
  } //end while (True)

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