/****************************************************************************/
/* Copyright (c) 2005 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : DockDriver.h                                             */
/* Author   : henthorn                                                      */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 11/15/2005                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <math.h>
#include "DockDriver.h"
//#include "DockLog.h"
#include "DockOutput.h"
#include "Syslog.h"
#include "System.h"
#include "TimeP.h"
#include <FloatAttribute.h>
#include <StringAttribute.h>
#include <IntegerAttribute.h>
#include <AttributeParser.h>
#include <i86.h>   //For the delay()

//#define RECORD_TERMINATOR "\xd\xa\0"
#define RECORD_TERMINATOR "\xa\0"
#define PICDELAY 200

Boolean debug_flag = True;

DockDriver::DockDriver(SerialDeviceExt *device, char* config)
   : PeriodicTask("dockDriver"), _config(config), _attributes(config),
     _period(400), _device(device), _timeout(400)
{

//_log = new DockLog(this, DataLog::BinaryFormat);
   _output = new DockOutput();
   _command = new DockCommand(MessageQueue::ReadWrite);

   //
   // Initialize:
   //
   init();
   addPeriodicCallback(_period, (CallbackMethod)DockDriver::callback);

   _continuousQuery = False;
   _pingTimeOut = 150;
   _pingCtr = 0;
}


DockDriver::~DockDriver()
{
// delete _log;
   delete _output;
   delete _command;
}

void DockDriver::init()
{
   Boolean debug = debug_flag;
   char cmd[128];
   try
   {
   }
   catch (Exception e)
   {
	  Syslog::write("DockDriver - caught exception on %s: %s", cmd, e.msg);
   }
  
   _output->data.deviceReady = True;
   _output->write();
}

void DockDriver::callback()
{
   Boolean debug = debug_flag;
   //
   // Check for any commands.  Execute them if requested.
   //
   DockCommand::Command cmdIn;
   int i;
   int nTries = 4;
   int rslen = sizeof( _output->data.resolverState );
   int pslen = sizeof( _output->data.probeState );
   int pingError;
   TimeIF::TimeSpec updateTime;
   
   while ((_command->read(&cmdIn)) > 0)
   {
	  strcpy(_response, "No response.");
	  Boolean done = False;
	  switch (cmdIn._cmd) 
	  {
		 case DockCommand::SetState:
			dprintf("Set State: %s\n", cmdIn.string);
			//
			//      cmd1  nlines, nTries  cmd2          nlines2
			PicCom( "ss", 3,      4,      cmdIn.string, 1 );
			break;

		 case DockCommand::SetPower:
			dprintf("SetPower not yet implemented.\n");
			break;

		 case DockCommand::ReadStatus:
			PicCom( "rs", 10 );
			break;

		 case DockCommand::MoveProbe:
			dprintf("Probe Command: %s\n", cmdIn.string);
			//
			//      cmd1  nlines, nTries  cmd2          nlines2
			PicCom( "sp", 2,      4,      cmdIn.string, 1 );
			break;

		 case DockCommand::QueryProbe:
			PicCom( "qp" );
			for( i = 0; i<pslen; i++ )
			{
			   if(_response[i] == '\xa' || _response[i] == '\xd')
				  _response[i] = '\0';
			}
			strncpy(_output->data.probeState, _response, pslen);
			break;

		 case DockCommand::QueryResolver:
			PicCom( "qr" );
			for( i = 0; i<rslen; i++ )
			{
			   if(_response[i] == '\xa' || _response[i] == '\xd')
				  _response[i] = '\0';
			}
			strncpy(_output->data.resolverState, _response, rslen);
			break;

		 case DockCommand::ContinuousQueryOn:
			_continuousQuery = True;
			_pingTimeOut = cmdIn.integer;
			break;

		 case DockCommand::ContinuousQueryOff:
			_continuousQuery = False;
			break;

		 case DockCommand::OpenPort:
			_device->openSD();
			break;

		 case DockCommand::ClosePort:
			_device->closeSD();
			break;

		 case DockCommand::Ping:
			pingError = _device->ping( cmdIn.integer );
			//printf("DockDriver: pingError = %d\n", pingError);
			_output->data.pingError = pingError;
			Time::gettime( &updateTime );
			_output->data.updateTimeSecs = Time::seconds( &updateTime );
			//printf("DockDriver: Ping time is %.2f\n", 
			//	   _output->data.updateTimeSecs);
			break;

		 case DockCommand::None:
			dprintf("DockDriver Command = None.\n");
			break;

		 case DockCommand::SetBeacon:
			strcpy(_beacon, cmdIn.string);
			// Set the interrogation string to use for continuous operation
			//
			strcpy(_interrogation, "POS ");
			strcat(_interrogation, _beacon);
			strcat(_interrogation, RECORD_TERMINATOR);
			dprintf("DockDriver - new interrogation string is %s",
					_interrogation);
			break;

		 default:
			Syslog::write("DockDriver::callBack: Bad Command.\n");
			dprintf("DockDriver::callBack: Bad Command.\n");
			break;
	  }  //end  switch (cmdIn._cmd) 
   }  //end  while ((_command->read(&cmdIn)) > 0)
   if( _continuousQuery )
   {
	  pingError = _device->ping( _pingTimeOut );
	  if( !pingError )
	  {
		  if( _pingCtr < 3 ) _pingCtr++;
	  }
	  else
		  _pingCtr = 0;
	  
		  
	  _output->data.pingError = pingError;
	  delay(500);
	  Syslog::write("DockDriver:: _pingCtr = %d.", _pingCtr);
	  if( _pingCtr == 3 )
	  {
		 if( !_device->isPortOpen() )
		 {
			_device->openSD();
			delay(500);
			Syslog::write("DockDriver:: Opened the TCP port.");
		 }
		 PicCom("qr");
		 for( i = 0; i<rslen; i++ )
		 {
			if(_response[i] == '\xa' || _response[i] == '\xd')
			   _response[i] = '\0';
		 }
		 strncpy(_output->data.resolverState, _response, rslen);
		 delay(500);

		 PicCom("qp");
		 for( i = 0; i<pslen; i++ )
		 {
			if(_response[i] == '\xa' || _response[i] == '\xd')
			   _response[i] = '\0';
		 }
		 strncpy(_output->data.probeState, _response, pslen);
		 delay(500);
	  }
   }
   _output->write();
//_log->write();
}

//
// Read and write to the PIC
//
int DockDriver::PicCom( char *cmd, int nlines, int nTries, 
						char *cmd2, int nlines2 )
{
   Boolean done = False;
   Boolean debug = False;
   int nBytes = 0;
   int i;
   //
   // First check to see if the _devices's buffer is empty.  If not, empty it.
   int bytesLeft =_device->nRecvdBytes();
   //while( bytesLeft > 1 )
   while( bytesLeft > 0 )
   {
	  printf("PicCom: The number of bytes left is %d.\n", bytesLeft);
	  nBytes = _device->read(_response,
				 sizeof(_response),
				 _timeout);
          printf("nBytes is %d\n", nBytes);
	  delay(PICDELAY);
	  for( int j=0; j<nBytes; j++ ) 
	  {
		 if(_response[j] == '\0') _response[j]=' ';
	  }
	  _response[nBytes] = '\0';
	  printf("PicCom: Read a line: %s\n", _response);
	  bytesLeft =_device->nRecvdBytes();
	  printf("PicCom: After flushing, the number of bytes left is %d.\n", 
			  bytesLeft);
   }
   //
   // Try nTries times to get through to the PIC:
   //
   for( i=0; i<nTries; i++ )
   {
      if( 0 == PicWriteRead2( cmd, nlines, cmd2, nlines2 ) ) 
      {
		 done = True;
		 Time::gettime(&(_output->data.updateTime));
		 _output->data.updateTimeSecs = 
			Time::seconds(&(_output->data.updateTime));
		 //
		 // Copy the Pic output to shared memory:
		 strcpy(_output->data.rs_response, _response);
		 break;
      }
	  dprintf("DockDriver::PicComm: i = %d.\n",i);
   }  // for( int i=0; i<nTries; i++ )
   if( !done ) 
   {
      dprintf("DockDriver - Error: Could not communicate with the "
			  "PIC after %d tries.\n",nTries);
      return -1;
   }

   return 0;
}
//
// Read and write to the PIC
//
int DockDriver::PicWriteRead2( char *cmd, int nlines, 
							   char *cmd2, int nlines2 )
{
   Boolean debug = False;
   int error = -1;
   try 
   {
      char *resp_ptr = _response;
	  char tempBuf[64];
	  int tempBufSize = sizeof( tempBuf );
	  dprintf("resp_ptr = %s", resp_ptr);
      int nBytes = 0;
	  int tBytes = 0;
	  int resp_size = sizeof(_response);
      sprintf(_cmd, "%s\xa", cmd);
      _device->write(_cmd, strlen(_cmd));
      //
      // The PIC will respond with nlines lines:
		//while( _device->nRecvdBytes() > 1 )
	  for( int i=0; i<nlines; i++ )
      {
#if 0
		 nBytes = _device->readUntil(resp_ptr,
		 							 resp_size,
		 							 RECORD_TERMINATOR, _timeout);
#endif
		 delay(PICDELAY);
		 nBytes = _device->readUntil(tempBuf,
									 tempBufSize,
		                             RECORD_TERMINATOR, _timeout);
//		 for( int k=0; k<tempBufSize; k++ ) resp_ptr[k]=tempBuf[k];
		 if( nBytes < 0 ) throw Exception("nBytes < 0.");
//		 if( !strcmp(_response, "Error") ) 
		 if( !strcmp(tempBuf, " Error") ) 
			throw Exception("PIC returned Error.");
		 //
		 // Replace the unwanted nulls with spaces, then add a final 
		 // null.
		 for( int j=0; j<nBytes; j++ ) 
		 {
//			if(resp_ptr[j] == '\0') resp_ptr[j]=' ';
			if(tempBuf[j]  == '\0') tempBuf[j] =' ';
		 }
//		 resp_ptr[nBytes] = '\0';
		 tempBuf[nBytes] = '\0';
		 for( int k=0; k<tempBufSize; k++ ) resp_ptr[k]=tempBuf[k];
//		 dprintf("PicWriteRead2 response:(%d) \n%s", nBytes, _response);
		 dprintf("PicWriteRead2 tempBuf :(%d) \n%s", nBytes, tempBuf);
		 resp_ptr += nBytes;
		 resp_size -=nBytes;
		 tBytes += nBytes;
		 if( resp_size <= 0 ) throw Exception("resp_size <= 0");
		 if( tBytes > sizeof(_response) ) throw Exception("tBytes too large.");
      }
      //
      // Now send it the second command:
      if( cmd2 != NULL )
      {
		 nBytes = 0;
		 sprintf(_cmd, "%s\xa", cmd2);
		 dprintf(" PicWriteRead2 - The second argument is %s\n", _cmd);
		 delay(PICDELAY);
		 _device->write(_cmd, strlen(_cmd));
		 //
		 // The PIC will respond with nlines2 lines:
		 for( int i=0; i<nlines2; i++ )
		 {
			delay(PICDELAY);
			nBytes = _device->readUntil(tempBuf,
										tempBufSize,
										RECORD_TERMINATOR, _timeout);
			if( nBytes < 0 ) throw Exception("nBytes < 0.");
			if( !strcmp(tempBuf, " Error") ) 
			   throw Exception("PIC returned Error.");
			//
			// Replace the unwanted nulls with spaces, then add a final 
			// null.
			for( int j=0; j<nBytes; j++ ) 
			{
			   if(tempBuf[j] == '\0') tempBuf[j]=' ';
			}
			tempBuf[nBytes] = '\0';
			for( int k=0; k<tempBufSize; k++ ) resp_ptr[k]=tempBuf[k];
			dprintf("PicWriteRead2 second response:(%d) \n%s", 
					nBytes, _response);
			resp_ptr += nBytes;
			resp_size -=nBytes;
			if( resp_size <= 0 ) throw Exception("resp_size <= 0");
		 }
      }  // if( cmd2 != NULL )
   } //try
   catch (Exception e)
   {
      Syslog::write("DockDriver - caught exception "
					"on %s: %s", _cmd, e.msg);
      printf("DockDriver - caught exception "
			  "on %s: %s", _cmd, e.msg);
      return error;
   }
   return 0;
}


//
// Create attributes:
//
void DockDriver::createCfgAttributes()
{
   _attributes.add( new IntegerAttribute("PollPeriod",
										 "Dock Polling Period", &_period, 200 ) );

}

// Load the config attributes and do some sanity checking
//
void DockDriver::loadConfigFile(const char* configFile)
{
   AttributeParser::parse(configFile, &_attributes); 
}

// Record the attributes in Syslog
//
void DockDriver::reportCfgAttributes()
{
   Syslog::write("Dock -- configuration:\n"
				 "\tPollPeriod = %d ms\n"
				 "\tBeacon     = %s\n",
				 _period, _beacon);
}