#include <signal.h>
#include <conio.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <i86.h>
#include "Syslog.h"
#include "SerialParameters.h"
#include "System.h"
#include "DockIF.h"
#include "TimeP.h"

TimeIF::TimeSpec updateTime;
TimeIF::TimeSpec startTime;
TimeIF::TimeSpec now;
DockIF::String512 pic_status;
DockIF::String32 PicCmd;
DockIF *dock;
int maxTime = 10;         //Timeout for reading a PIC status.
//
// Cannot call these functions until after "dock" is initialized in main().
// Also you must set the startTime before calling these.
//
void readDockPic()
{
// Time::gettime(&startTime);
   Boolean timedOut = False;
   int waitTime = 0;
   while( !timedOut )
   {
	  delay(1000);
	  //
	  // Poll the Vehicle Server/Driver:
	  dock->read_dock_PIC(pic_status, &updateTime);
	  //printf("updateTime.seconds = %d.\n", updateTime.seconds);
	  //printf("startTime.seconds = %d.\n", startTime.seconds);
	  if( updateTime.seconds >= startTime.seconds)
	  {
		 printf("Dock PIC response:\n%s\n", pic_status);
		 break;
	  }
	  Time::gettime(&now);
	  waitTime = now.seconds-startTime.seconds;
	  printf("Waiting %d seconds for Dock PIC.\n", waitTime);
	  if( waitTime > maxTime )
	  {
		 printf("Timed out after %d seconds "
				"waiting for reply.\n", waitTime);
		 timedOut = True;
	  }
   }
}
void readVehiclePic()
{
// Time::gettime(&startTime);
   Boolean timedOut = False;
   int waitTime = 0;
   while( !timedOut )
   {
	  delay( 1000 );
	  //
	  // Poll the Vehicle Server/Driver:
	  dock->read_vehicle_PIC(pic_status, &updateTime);
	  //printf("updateTime.seconds = %d.\n", updateTime.seconds);
	  if( updateTime.seconds >= startTime.seconds)
	  {
		 printf("Vehicle PIC response:\n%s\n", pic_status);
		 break;
	  }
	  Time::gettime(&now);
	  waitTime = now.seconds-startTime.seconds;
	  printf("Waiting %d seconds for Vehicle PIC.\n", waitTime);
	  if( waitTime > maxTime )
	  {
		 printf("Timed out after %d seconds "
				"waiting for reply.\n", waitTime);
		 timedOut = True;
	  }
   }
}

void signalHandler(int signalNo);

pid_t dockPID;

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

int main(int argc, char **argv)
{
   double updateTimeSecs = 0.;
   double startTimeSecs = 0.;
   Boolean portOpen = False;
   printf("_dockTest.cc was last compiled on %s, %s\n",__DATE__, __TIME__);
   //
   // Assume that the command line contains two serial port specifications.
   // To get the SerialParameters object to work, we limit the number of
   // arguments to three, and for the second object, increment argv past the
   // first serial port specification.
   argc = 3;
   SerialParameters serialParamsVehicle(&argc, argv);
   //printf("_dockTest - Vehicle: %s\n", serialParamsVehicle.string() );

   SerialParameters serialParamsDock(&argc, argv+2);
   //printf("_dockTest - Dock: %s\n", serialParamsDock.string() );


   char argString[256];
   char argStringVehicle[128];
   char argStringDock[128];

   sprintf(argStringVehicle, "-serial %s", serialParamsVehicle.string());
   sprintf(argStringDock, "-serial %s", serialParamsDock.string());
   sprintf(argString, "%s %s", argStringVehicle, argStringDock);

   //printf("argString: %s\n", argString );


   signal(SIGQUIT, signalHandler);
   printf("Starting dockServer ...\n");

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

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


   char *helpStr = " Type: \n"
	  "    rvs    - read vehicle PIC status.\n"
	  "    rds    - read dock PIC status.\n"
	  "    sds    - set dock PIC state.  Arguments are\n"
	  "             (docked-nocharge, docked-charge, idle, auv-returning,\n"
	  "              failed-docked, failed-nondocked)\n"
	  "    qr     - query resolver.\n"
	  "    qp     - query probe.\n"
	  "    cq     - continuous probe and resolver query.\n"
	  "    dt     - run a simulated docking mission.\n"
	  "    op     - open the dock's Ethernet port.\n"
	  "    cp     - close the dock's Ethernet port.\n"
	  "    sp     - set (move) the probe.  Arguments are "
	  "(up, down, brake).\n"
	  "    ping   - ping across the WAPS to the dock.\n"
	  "    Help   - to repeat this screen.\n"
	  "    exit   - to exit this program.\n"
	  "    quit   - to exit this program.\n";

   printf( "%s", helpStr );

   enum NetworkStatusEnum
   {
	  Present, Absent
   };
   NetworkStatusEnum NetworkStatus = Absent;

   while( True )
   {
	  char buf[512];
	  short pingTimeOut = 150;          //milliseconds

	  printf( "dockTest>");
	  gets(buf);
	  if( !strncmp( buf, "h", 1) || !strncmp( buf, "", 1)) 
		 printf( "%s", helpStr );
	  else if( !strncmp( buf, "rvs", 3) ) 
	  {
		 Time::gettime(&startTime);
		 dock->request_vehicle_status();
		 readVehiclePic();
	  }
	  else if( !strncmp( buf, "rds", 3) ) 
	  {
		 if( portOpen )
		 {
			Time::gettime(&startTime);
			dock->request_dock_status();
			readDockPic();
		 }
		 else
			printf("Port not open.\n");
	  }
	  else if( !strncmp( buf, "sds", 3) ) 
	  {
		 if( portOpen )
		 {
			if( strlen( buf ) > 4 ) 
			{
			   strcpy(PicCmd, buf+4);       //Skip the "sds ".
			   Time::gettime(&startTime);
			   dock->set_dock_state( PicCmd );
			   readDockPic();
			}
			else
			   printf("Please enter an argument.\n");
		 }
		 else
			printf("Port not open.\n");
	  }
	  else if( !strncmp( buf, "qr", 2) ) 
	  {
		 if( portOpen )
		 {
			Time::gettime(&startTime);
			dock->query_resolver();
			readDockPic();
		 }
		 else
			printf("Port not open.\n");
	  }
	  else if( !strncmp( buf, "qp", 2) ) 
	  {
		 if( portOpen )
		 {
		 Time::gettime(&startTime);
		 dock->query_probe();
		 readDockPic();
		 }
		 else
			printf("Port not open.\n");
	  }
	  else if( !strncmp( buf, "ping", 3) ) 
	  {
		 short pingError, pingTimeOut = 200;
		 printf("Pinging the dock port.\n");
		 Time::gettime( &now );
		 startTimeSecs = Time::seconds( &now );
		 //
		 // Timeout in pingTimeOut ms.
		 dock->ping( pingTimeOut );
		 //
		 // Wait long enough for the ping to have timed out, then read
		 // the result from the server.
		 delay( pingTimeOut+500 );
		 dock->ping_result( &pingError, &updateTimeSecs );
		 printf("updateTime = %.2f.\n", 
		 		updateTimeSecs-startTimeSecs );
		 if( updateTimeSecs >= startTimeSecs )
			if( pingError == 1)  
			   printf("Ping timeout %d milliseconds.\n", pingTimeOut);
			else if( pingError < 0 )
			   printf("Ping error %d.\n", pingError);
			else
			   printf("Dock present!\n");
		 else
			printf("The server did not respond.\n");
	  }
	  else if( !strncmp( buf, "op", 2) ) 
	  {
		 printf("Opening the dock port.\n");
		 dock->open_port();
		 portOpen = True;
	  }
	  else if( !strncmp( buf, "cp", 2) ) 
	  {
		 printf("Closing the dock port.\n");
		 dock->close_port();
		 portOpen = False;
	  }
	  else if( !strncmp( buf, "sp", 2) ) 
	  {
		 if( portOpen )
		 {
			strcpy(PicCmd, buf+3);       //Skip the "sp ".
			Time::gettime(&startTime);
			dock->move_probe( PicCmd );
			readDockPic();
		 }
		 else
			printf("Port not open.\n");
	  }
	  else if( !strncmp( buf, "cq", 2) ) 
	  {
		 if( portOpen )
		 {
			DockIF::String32 probeStatus;
			DockIF::String32 resolverStatus;
			Boolean networkPresent = False;
			Time::gettime( &now );
			startTimeSecs = Time::seconds( &now );
			dock->set_continuous_query( True, pingTimeOut );
			while( !kbhit() )
			{
			   dock->read_continuous_query( &networkPresent,
											probeStatus, 
											resolverStatus, 
											&updateTimeSecs);
			   printf( "\n\n");
			   if(networkPresent) printf( " The network is present.\n");
			   else               printf( " The network is absent.\n");
			   printf( " Resolver:%s\n", resolverStatus );
			   printf( " Probe:   %s\n", probeStatus );
			   printf( " Update time:   %.2f\n", updateTimeSecs-startTimeSecs );
			   sleep(1);
			}
			dock->set_continuous_query( False, pingTimeOut );
		 }
		 else
			printf("Port not open.\n");
	  }
	  else if( !strncmp( buf, "dt", 2) ) 
	  {
		 //
		 // This is a docking test.  It assumes the vehicle is
		 // entering the tube, thus the probe must be down, even if it
		 // reports "unknowed".  Otherwise, the vehicle would hit the
		 // probe and the resolver would never read "good".
		 //
		 Boolean networkPresent = False;
		 DockIF::String32 probeString;
		 DockIF::String32 resolverString;
		 Time::gettime( &now );
		 startTimeSecs = Time::seconds( &now );
		 dock->set_continuous_query( True, pingTimeOut );
		 enum ProbeStateEnum 
		 {
			Down, Raising, Up, Lowering
		 };
		 ProbeStateEnum ProbeState = Down;
			
		 while( !kbhit() )
		 {
			dock->read_continuous_query( &networkPresent,
										 probeString,
										 resolverString,
										 &updateTimeSecs);

			if( !networkPresent )
			{
			   if( NetworkStatus == Present ) 
				  printf("ERROR Network connection terminated!\n");
			   NetworkStatus = Absent;
			}
			else
			{
			   //Moved the "open" to the driver.
			   //if( NetworkStatus == Absent ) dock->open_port();
			   if( NetworkStatus == Absent ) 
				  Syslog::write("dockTest:: Network present.");
			   NetworkStatus = Present;
			}

			if( NetworkStatus == Present )
			{
			   if( (    !strncmp( probeString, "down", 3 ) 
						|| !strncmp( probeString, "unk", 3 )   ) 
				   && ProbeState == Down )
			   {
				  printf("Got here.\n");
				  if( !strncmp( resolverString, "good", 3 ) )
				  {
					 printf("dockTest: Raising the Probe:");
					 dock->set_dock_state("docked-nocharge");
					 dock->move_probe( "up" );
					 ProbeState = Raising;
				  }
			   }
			   else if( !strncmp( probeString, "up", 2 ) )
			   {
				  // HOORAY!  The vehicle has docked.
				  printf("dockTest: The vehicle has docked successfully!\n");
				  ProbeState = Up;
			   }
			}
			
			printf( "\n\n");
			if(networkPresent) printf( " The network is present.\n");
			else               printf( " The network is absent.\n");
			printf( " Resolver:%s\n", resolverString );
			printf( " Probe:   %s\n", probeString );
			printf( " Update time:   %.2f\n", updateTimeSecs-startTimeSecs );
			sleep(1);
		 }
		 dock->set_continuous_query( False, pingTimeOut );
	  }
	  else if( !strnicmp( buf, "QU", 2) || !strnicmp( buf, "E", 1) ) 
	  {
		 printf("\n\nExiting dockTest. Have a nice day.\n\n");
		 break;
	  }
	  else
	  {
		 printf(" Command not recognized.  Please try again.\n");
		 printf( "%s", helpStr );
	  }
   }  //while( true )

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