#include <ctype.h>
#include "SerialDevice.h"
#include "epiBatt.h"
#define READ_TIMEOUT 2500

#define EPIBATT_PERIOD 5000

epiBatt::epiBatt(SerialDevice *device, 
		 float abortVoltage,
		 float terminateVoltage,
		 Boolean exitFlag, const char *name)
  : _device(device), _abortVoltage(abortVoltage),
    _terminateVoltage(terminateVoltage),layeredControl(NULL),
    _exitFlag(exitFlag),
    PeriodicTask("epiBatt")
{

   if (init() != DeviceIF::Ok)
   {
      Syslog::write("epiBatt: init failed!\n");
      throw Exception("epiBatt -- init failed!\n");
   }

   sample();

   addPeriodicCallback(EPIBATT_PERIOD, (CallbackMethod)epiBatt::sample);
   _log = new epiBattLog(this, DataLog::BinaryFormat, name);

}


epiBatt::~epiBatt()
{
//  delete _output;
   delete _log;
}


DeviceIF::Status epiBatt::init()
{
  char buf[30];

//  _output = new epiBattOutput();
//  _output->data[0].state = epiBattOutput::Unknown;
//  _output->data[0].fault = epiBattOutput::Clear;
//  _output->data[0].voltage = 0.;
//  _output->data[0].current = 0.;
//  _output->data[0].temp = 0.;
//  _output->data[0].minvoltage = 0.;
//  _output->data[0].maxvoltage = 0.;
//  _output->data[0].waterleak = 0;
//  _output->data[0].capacity = 0.;
//  _output->data[0].sampleTime.tv_sec = 0;
//  _output->data[0].sampleTime.tv_nsec = 0;

  _device->commsDebugMode(SerialDevice::DebugOff);

  
  _device->clearPort();
  _device->flushReceiveBuf();

  _aborted = False;
  return DeviceIF::Ok;
} 


void epiBatt::sample(void)
{
  char cmd[16], reply[100], *ptr;
  int numBattsOnline = 0;
  float minCellVoltage = 1.0e6;
  float sumMinCellVoltage = 0.0;
  float meanMinCellVoltage = 0.0;
  short sval;
  char cval;
  int i, nbytes;
  Boolean abortOnCapacity;
  static int abortOnVoltage = 0;
  Boolean exitOnCapacity;
  static int exitOnVoltage = 0;
  float stackVoltage = 0.0;

//  _device->write("@AD", 3);
  _device->write("@RQ", 3);
  try {
      nbytes = _device->readNChars(reply, 100, 2000);
  }
  catch(...) {nbytes = 0;}
  printf("nbytes is %d\n", nbytes);
  if (nbytes != 77) return;
  
  ptr = reply;

  //elapsed usage, in amp-hours
  sscanf(ptr,"%4x", &sval); ptr+=4;
  _battState.gasGauge = (float)sval/100.0;

  //cell voltage, in volts
  for (i=0;i<8;i++) {
	sscanf(ptr,"%4x", &sval); ptr += 4; 
        _battState.cellVoltage[i] = (float)sval/1000.0;
        stackVoltage += _battState.cellVoltage[i];
  }

  //current, in amps
  sscanf(ptr,"%4x", &sval); ptr+=4;
  _battState.current = (float)sval/100.0;

  //temp, in degrees C
  for (i=0; i<9; i++) {
	sscanf(ptr, "%2x", &_battState.temp[i]); ptr+=2;
  }

  //relative humidity, %rH
  sscanf(ptr,"%2x", &_battState.rH); ptr+=2;
  sscanf(ptr,"%2x", &_battState.balance); ptr+=2;
  sscanf(ptr,"%4x", &_battState.tempAlertFlag); ptr+=4;
  sscanf(ptr,"%4x", &_battState.voltAlertFlag); ptr+=4;
  sscanf(ptr,"%4x", &_battState.batteryStateFlag);
  if (_battState.batteryStateFlag & (1<<9)) ++numBattsOnline;
 
  Syslog::write("epiBatt:gas gauge:%f, current:%f\n", 
                _battState.gasGauge, _battState.current);
  Syslog::write("epiBatt:V1:%1.3f V2:%1.3f V3:%1.3f V4:%1.3f\n",
		_battState.cellVoltage[0], _battState.cellVoltage[1],
	  	_battState.cellVoltage[2], _battState.cellVoltage[3]);
  Syslog::write("epiBatt:V5:%1.3f V6:%1.3f V7:%1.3f V8:%1.3f\n",
		_battState.cellVoltage[4], _battState.cellVoltage[5],
		_battState.cellVoltage[6], _battState.cellVoltage[7]);
  Syslog::write("epiBatt:T1:%d T2:%d T3:%d T4:%d\n",
		_battState.temp[0], _battState.temp[1],
		_battState.temp[2], _battState.temp[3]);
  Syslog::write("epiBatt:T5:%d T6:%d T7:%d T8:%d T9:%d\n",
		_battState.temp[4], _battState.temp[5],
		_battState.temp[6], _battState.temp[7], _battState.temp[8]);
  Syslog::write("epiBatt:temp alert: 0x%x, voltAlert: 0x%x\n",
		_battState.tempAlertFlag, _battState.voltAlertFlag);
  Syslog::write("epiBatt:battery state flag: 0x%x\n",
		_battState.batteryStateFlag);  
  Syslog::write("epiBatt:Rh:%d\n", _battState.rH);
  Syslog::write("epiBatt:balance:0x%2X", _battState.balance);

  _log->write();
  
  for (i=0; i<8;i++) {
	if (_battState.cellVoltage[i] < minCellVoltage)
		minCellVoltage = _battState.cellVoltage[i];
  }
   
  //check for abort conditions, abort missions if batteries get too low
  Syslog::write("batts online %d, min cell voltage %f, stack voltage %f\n", 
                numBattsOnline, minCellVoltage, stackVoltage);

  if (minCellVoltage <= _abortVoltage) {
	abortOnVoltage++;
  } else {
	abortOnVoltage = 0;
  }
  if (minCellVoltage <= _terminateVoltage) {
	exitOnVoltage++;
  } else {
	exitOnVoltage = 0;
  }

  if (_battState.gasGauge >= 139.0) {
//	abortOnCapacity = True;
  } else {
	abortOnCapacity = False;
  }
  if (_battState.gasGauge >= 143.0) {
//	exitOnCapacity = True;
  } else {
	exitOnCapacity = False;
  }

  if (_battState.rH > 40) {abortOnCapacity = True;}

  if ((abortOnCapacity || (abortOnVoltage > 10)) && _exitFlag) { 
      try {
	Syslog::write("epiBatt.cc: aborting mission because of low voltage\n");
	// Create an interface to LayeredControl
	if (!layeredControl) {
		layeredControl = new LayeredControlIF("layeredControl");
        }
	// LayeredControl is running; abort the mission
	if (!(layeredControl->abortingMission()))
		layeredControl->abortMission();
	_aborted = True;
	// Done with interface; delete it to avoid memory leaks
      }
      catch (...) {
	Syslog::write("epiBatt.cc: Couldn't abort mission -- Layered Control not running!\n");
	// LayeredControl isn't running 
      }   
  } 
  if (((exitOnVoltage > 10) || exitOnCapacity) && _exitFlag) {
        Syslog::write("epiBatt.cc: terminating mission because of really "
  		      "low voltage\n");
        throw Exception("epiBatt.cc: terminating mission because of really "
                        "low voltage\n");
  } //else

} // void Sample(void)