/****************************************************************************/
/* Copyright (c) 2005 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : VehicleDriver.h                                               */
/* Author   : rsm                                                           */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 18 May 2006                                                   */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <math.h>
#include "VehicleDriver.h"
//#include "VehicleLog.h"
#include "VehicleOutput.h"
#include "Syslog.h"
#include "TimeP.h"
#include <FloatAttribute.h>
#include <StringAttribute.h>
#include <IntegerAttribute.h>
#include <AttributeParser.h>
#include <i86.h>

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

//Boolean debug_flag = True;

VehicleDriver::VehicleDriver(SerialDevice *device, char* config)
   : PeriodicTask("vehicleDriver"), _config(config), _attributes(config),
     _period(200), _device(device), _timeout(400)
{

//_log = new VehicleLog(this, DataLog::BinaryFormat);
  _output = new VehicleOutput();
  _command = new VehicleCommand(MessageQueue::ReadWrite);

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


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

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

void VehicleDriver::callback()
{
  Boolean debug = True;
  //
  // Check for any commands.  Execute them if requested.
  //
  VehicleCommand::Command cmdIn;
  int i;
  int nTries = 4;
  while ((_command->read(&cmdIn)) > 0)
  {
     strcpy(_response, "No response.");
     Boolean done = False;
     switch (cmdIn._cmd) 
     {
	case VehicleCommand::SetState:
	   dprintf("SetState not yet implemented.\n");
	   break;
	case VehicleCommand::SetPower:
	   dprintf("SetPower not yet implemented.\n");
	   break;
	case VehicleCommand::ReadStatus:
	   //
	   // Try four times to get through to the PIC:
	   //
	   for( i=0; i<nTries; i++ )
	   {
	      //
	      // getrs() reads the status from the PIC.
	      if( 0 == getrs() ) 
	      {
		 done = True;
		 Time::gettime(&(_output->data.updateTime));
		 strcpy(_output->data.rs_response, _response);
		 break;
	      }
	   }
	   if( !done ) 
	   {
	      dprintf("VehicleDriver - Error: Could not read the "
		      "PIC status after %d tries.\n",nTries);
	   }
	   break;

	case VehicleCommand::None:
	   dprintf("VehicleDriver Command = None.\n");
	   break;
	case VehicleCommand::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("VehicleDriver - new interrogation string is %s",
		   _interrogation);
	   break;

	default:
	   break;
     }  //end  switch (cmdIn._cmd) 
  }  //end  while ((_command->read(&cmdIn)) > 0)
  _output->write();
//_log->write();
  
}
//
// Read PIC status (rs):
//
int VehicleDriver::getrs()
{
   Boolean debug = False;
   int error = -1;
   try 
   {
      char *resp_ptr = _response;
      int nBytes = 0;
      int resp_size = sizeof(_response);
      sprintf(_cmd, "rs\xa");
      _device->write(_cmd, strlen(_cmd));
      delay(PICDELAY);
		//printf("After device->write() and before _device->readUntil()\n");
      //
      // The rs command responds with eight Ascii lines. 
      for( int i=0; i<8; i++ )
      {
	 nBytes = _device->readUntil(resp_ptr, 
				     resp_size,
				     RECORD_TERMINATOR, _timeout);
	  delay(PICDELAY);
	  if( nBytes < 0 ) throw Exception(" getrs : nBytes < 0.");
	 if( !strcmp(_response, "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]=' ';
	 }
	 resp_ptr[nBytes] = '\0';
	 dprintf("VehicleDriver - RS response %d:(%d) \n%s", i,nBytes, _response);
	 resp_ptr += nBytes;
	 resp_size -=nBytes;
	 if( resp_size <= 0 ) throw Exception("resp_size <= 0");
      }
   }  
   catch (Exception e)
   {
      Syslog::write("VehicleDriver - caught exception "
		    "on %s: %s", _cmd, e.msg);
      return error;
   }
   dprintf("getrs() - got a complete record\n");
   return 0;
}
//
// Create attributes:
//
void VehicleDriver::createCfgAttributes()
{
  _attributes.add( new IntegerAttribute("PollPeriod",
                   "Vehicle Polling Period", &_period, 200 ) );

}

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

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