/****************************************************************************/
/* Copyright (c) 2001 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : Ips4.cc                                                       */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 04/09/2001                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <unistd.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/uio.h>
#include <sys/socket.h>
#include <i86.h>
#include <fcntl.h>
#include <errno.h>
#include <iostream.h>
#include <time.h>
#include <math.h>
#include <FloatAttribute.h>
#include <AttributeParser.h>
#include <StringAttribute.h>
#include <IntegerAttribute.h>
#include "Ips4.h"
#include "Syslog.h"

#define MaxRecordBytes 44
#define READ_TIMEOUT 3000
#define END_OF_RECORD "\xa"
#define AVERAGE_SOS  1450.0   // Average speed of sound used to calc range

Ips4::Ips4(SerialDevice *serialDevice, Boolean verbose, double pAtm)
     : SerialDeviceDriver("Ips4", serialDevice, MaxRecordBytes, END_OF_RECORD,
			  READ_TIMEOUT, 2)
{
  _pAtm = pAtm;
  _verbose = verbose;
  _output = new Ips4Output(SharedData::ReadWrite);
  _input  = new Ips4Input(SharedData::ReadWrite);
  _iceDraft = new icedraft_t;
  _avgSOS = 0.;
  _avgDens = 0.;
  _ipsRecord = (char*)malloc(41);
  strcpy(_ipsRecord, "");
  _log    = new Ips4Log(this, DataLog::BinaryFormat);
  _numberErrors = 0;
  _ctdServer = 0;
  _profileOn = False;
  _nCtdPoints = 0;
}


Ips4::~Ips4()
{
  stop();
  delete _output;
  delete _input;
  delete _iceDraft;
  delete _log;
  free(_ipsRecord);
}

// Initialize a connection to a Ctd server
void Ips4::connectToCtd()
{
  Syslog::write("Ips4:create interface to ctd...");
  try {
    _ctdServer = new CtdIF("ctdDriver", "ctdDriver", 2);
  } 
  catch(SharedObjectClient::MissingServer e) {
    _ctdServer = NULL;
    Syslog::write("Ips4: ctd server not found");
  }
}

// Initialize a CTD profile
void Ips4::startProfile()
{
  if (_profileOn)
  {
    Syslog::write("Ips4:startProfile() - already profiling");
    return;
  }

  if (_ctdServer == NULL)
  {
    connectToCtd();
  }

  if (_ctdServer != NULL)
  {
    _nCtdPoints = 0;
    Syslog::write("Ips4::Starting CTD profile");
    _profileOn = True;
  }
}

// Stop the CTD profile
void Ips4::stopProfile()
{
  _profileOn = False;
  Syslog::write("Ips4::Stopping CTD profile with %d points", _nCtdPoints);
}

DeviceIF::Status Ips4::initialize()
{
  char cmd[8];

  Syslog::write("Ips4:initializing Ips4...\n");

  connectToCtd();

  cmd[0] = 27;
  Syslog::write("Ips4:Cycle up to top menu using %c...", cmd[0]);
  for (int i =0; i < 4; i++)
  {
    delay(200);
    _device->write(cmd, 1);
  }

  _numberErrors = 0;
  sprintf(cmd, "g\r");

  _device->flushReceiveBuf();

  // Tell unit to start processing. If accepted, it's good to go.
  //
  Syslog::write("Ips4:Telling unit to begin processing");
  if (_device->write(cmd, strlen(cmd)) == strlen(cmd))
  {
    unsigned char record[MaxRecordBytes];
    int nbytes, attempts = 0;
    DeviceIF::Status stat;
    do
    {
      Syslog::write("Ips4:Attempting to read record from Ips");
      sleep(1);
      stat = readRecord((unsigned char*)record,
			MaxRecordBytes, END_OF_RECORD, 3000, &nbytes);
      if (stat == DeviceIF::Ok) break;
      attempts++;
      _device->write(cmd, strlen(cmd)) == strlen(cmd);
    } while (attempts < 4);

    if (stat == DeviceIF::Ok)
    {
      Syslog::write("Ips4:initialized\n");
      _output->data._deviceReady = True;
      startProfile();
      writeOutput("initialize() OK");
      return DeviceIF::Ok;
    }
  }

  Syslog::write("Ips4:initialize failure\n");
  _output->data._deviceReady = False;
  writeOutput("initialize() Error");
  return DeviceIF::Error;
}
 
////////////////////////////////////////////////////////////////////////////////
//
// This routine overrides the readRecord declared in the base class, because 
// we don't get a terminating character.  We just look for 20 bytes each time.
//
////////////////////////////////////////////////////////////////////////////////
//
DeviceIF::Status Ips4::readRecord(unsigned char *record,
					   int maxRecordBytes,
					   const char *recordTerminator,
					   unsigned readTimeout,
					   int *nBytesRead)
{
  Boolean debug = _verbose;
  Boolean readError = False;

  dprintf("Ips4:readRecord...");

  if (_numberErrors > 7) // Make sure #errors > #attempts in initialize
  {
    Syslog::write("Ips4::readRecord(): Too many consecutive errors, reinit");
    initialize();
  }

  try 
  {
    //
    // Read data record from device
    //
    dprintf("Ips4:recving data...");
    //
    // Changed back to readUntil() on Healy, 10/19/01
    //*nBytesRead =
    //  ::recv(_device->getFd(), record, MaxRecordBytes, O_NONBLOCK);
    //
        *nBytesRead = _device->readUntil((char *)record, MaxRecordBytes,
    				     (char *)recordTerminator, readTimeout);
  }
  catch (SerialDevice::TimedOut) {
    readError = True;
    dprintf("%s::readRecord() - Ips4 serial device timed out", name());
    Syslog::write("%s::readRecord() - record so far: %s", name(), record);
  }
  catch (SerialDevice::BufferFull) {
    readError = True;
    Syslog::write("%s::readRecord() - Ips4 serial device buffer full", name());
  }
  catch (Exception e) {
    readError = True;
    Syslog::write("%s::readRecord() - Ips4 driver caught exception:abort", name());
    throw;
  }

  dprintf("Ips4::readRecord(): %d bytes => %s", *nBytesRead, record);

  if (!readError)
  {
    //
    // Compute the checksum for the record that we just read in:
    //
    unsigned char  byteVal, checksum = 0;
    short  advChecksum;

    for (short byte = 0; byte < 40; byte++)
    {
      sscanf( (char *)( record + byte ), "%c", &byteVal );
      checksum += byteVal;
    }
    //
    // Now, read in the checksum that the Ips computed, and compare with the
    // one that we computed here.
    //    
    sscanf( (char *) (record + 40), "%2x", 
	    &advChecksum);
    dprintf("Ips4::readRecord(): Computed checksum = %d, Ips checksum = %d",
	    checksum, advChecksum);

    if (checksum != advChecksum)
    {
      readError = True;
      Syslog::write("Ips4::readRecord() ensemble checksum incorrect");
      dprintf("Computed checksum = %d / Ips checksum      = %d\n",
	      checksum, advChecksum);
    }
  }

  if (readError)
  {
    _output->data._badComms = True;
    writeOutput("readRecord() Error");
    _numberErrors++;
    return DeviceIF::Error;
  }
  else
  {
    dprintf("Good read");
    _numberErrors = 0;
    return DeviceIF::Ok;
  }

}

DeviceIF::Status Ips4::processRecord(unsigned char *record, int nBytes)
{
  Boolean debug = _verbose;

  int stat = decodeIPS(_iceDraft, (char*)record);
  if (stat < 0)
  {
    Syslog::write("Ips4::processRecord(): Bad status from decodeIPS: %d",
		  stat);
    return DeviceIF::Error;
  }

  // Use "standard" average SOS to calculate range
  //
  _iceDraft->Range = (_iceDraft->TravelTime/2.) * AVERAGE_SOS;

  dprintf("Ips4::processRecord(): IPS data:");
  dprintf(" %02d/%02d/%02d %02d:%02d:%02d",
	  _iceDraft->Year, _iceDraft->Month, _iceDraft->Day,
	  _iceDraft->Hour, _iceDraft->Minute, _iceDraft->Second);
  dprintf(" Pressure = %f Temperature = %f",
	  (float)_iceDraft->Pressure, (float)_iceDraft->Temperature);
  dprintf(" X = %f Y = %f TT = %f Range = %f",
	  (float)_iceDraft->TiltX, (float)_iceDraft->TiltY,
	  (float)_iceDraft->TravelTime, (float)_iceDraft->Range);
  dprintf(" Amp = %u Persist = %f", 
	  _iceDraft->Amplitude, (float)_iceDraft->Persistence);
  dprintf(" Press Age = %02d Flags = %2d Status = %d",
	  _iceDraft->PressureAge, _iceDraft->Flag, _iceDraft->Status);

  // Should we start/stop profiling?
  //
  _input->read();
  if (_input->data._profile == START_PROFILE)
  {
    startProfile();
    _input->data._profile = RESET;
    _input->write();
  }
  else if (_input->data._profile == STOP_PROFILE)
  {
    stopProfile();
    _input->data._profile = RESET;
    _input->write();
  }
  dprintf("Ips4:Profiling? = %d", _profileOn);

  // Calculate ice draft
  //
  if (_profileOn)
  {
    try {
      // Acquire next ctd point if profiling is active
      getCtdPoint();
    }
    catch(...) {
      Syslog::write("Ips4:ctdServer suddenly failed");
      stopProfile();
      _ctdServer = NULL;
    }
  }

  calculate_IceDraft(_pAtm, _nCtdPoints,   _cond,  _temp,  _pres,
                     &_iceDraft->Draft);
  _output->data._iceDraft = *_iceDraft;
  _output->data._badComms = False;    // If we got here, comms must be OK

  strncpy(_ipsRecord, (char*)record, (size_t)40);
  _ipsRecord[40] = '\0';

  writeOutput("processRecord()");

  // now write to log file
  _log->write();

  return DeviceIF::Ok;

}

////////////////////////////////////////////////////////////////////////////////
//
// This routine commands the Ips4 unit to stop processing. In this mode, the
// Ips4 will disable the sonar and suspend the real-time output in order to
// reduce power usage. However, pressure, temperature and tilt will continue
// to be sampled.
//
////////////////////////////////////////////////////////////////////////////////
//
DeviceIF::Status Ips4::stop()
{
  Syslog::write("Ips4:stopping Ips4...\n");

  char cmd[32];
  sprintf(cmd, "p\r");
  if (_device->write(cmd, strlen(cmd)) == strlen(cmd))
  {
    Syslog::write("Ips4:stopped\n");
    return DeviceIF::Ok;
  }
  else
  {
    Syslog::write("Ips4:stop failure\n");
    return DeviceIF::Error;
  }
}
 
////////////////////////////////////////////////////////////////////////////////
//
// This routine commands the Ips4 unit to start processing using the stored
// phase setup.
//
////////////////////////////////////////////////////////////////////////////////
//
DeviceIF::Status Ips4::start()
{
  Syslog::write("Ips4:starting Ips4...\n");

  char cmd[32];
  sprintf(cmd, "g\r");
  if (_device->write(cmd, strlen(cmd)) == strlen(cmd))
  {
    Syslog::write("Ips4:started\n");
    return DeviceIF::Ok;
  }
  else
  {
    Syslog::write("Ips4:start failure\n");
    return DeviceIF::Error;
  }
}

// Get current CTD data for profile
//
void Ips4::getCtdPoint()
{
  Boolean debug = _verbose;
  dprintf("Ips4:getCtdPoint()");
  if (_nCtdPoints >= MAX_CTD_POINTS)
  {
    stopProfile();
    return;
  }

  if (_ctdServer == NULL)
  {
    Syslog::write("Ips4::Cannot perform CTD profile - no server available");
    return;
  }

  if (_nCtdPoints < 0 )
  {
    _nCtdPoints = 0;
  }

  // Get temp and cond from ctd server
  //
  if (_ctdServer->ready())
  {
    float cond, temp, depth, tfreq, cfreq, v1, v2, v3, v4, v5, v6;
    TimeIF::TimeSpec condTime, tempTime;
    _ctdServer->get_CTD_data(&cond, &temp, &depth, &tfreq, &cfreq,
  			    &condTime, &tempTime, &v1, &v2, &v3, &v4, &v5, &v6);

    _cond[_nCtdPoints] = (double)cond;
    _temp[_nCtdPoints] = (double)temp;

    // Use pressure from Ips4
    //
    _pres[_nCtdPoints] = _iceDraft->Pressure*10.;

    _nCtdPoints++;
    dprintf("Ips4:getCtdPoint() - acquired point %d\n", _nCtdPoints);
  }
}

/////////////////////////////////////////////////////////////////////////
// calculate_IceDraft() - compute the icedraft using the following inputs
//
//	pAtm    - estimated value of the atmospheric pressure (in bars)
//    nPoints - number of elements in CTD array
//	cond    - pointer to conductivity array (C(S,T,0)/C(35,T,0))
//	temp    - pointer to temperature array (degress Celsius)
//	pres    - pointer to pressure array (absolute pressure in bars)
//
// output:
//      iceDraft- address of double to place calculated ice draft
//
// return:
//      non-zero Error code as defined in IceDraft.h
//      zero if completed normally
//
int Ips4::calculate_IceDraft(double pAtm, int nPoints,
			     double cond[/*nPoints*/], 
			     double temp[/*nPoints*/], 
			     double pres[/*nPoints*/],
			     double *computedIceDraft)
{
  double avgSOS, avgDens;
  int stat =0;

  Boolean debug = _verbose;
  dprintf("Ips4:Calculating Ice Draft");
  // Compute the average speed of sound and density with the given inputs
  //
  stat = calcAvgProp(pAtm, nPoints, cond, temp, pres, &avgSOS, &avgDens);
  if (stat != 0)
    return stat;

  _avgSOS  = avgSOS;
  _avgDens = avgDens;

  // Compute the ice draft and return
  //
  //_iceDraft->TravelTime = 0.0262; // For testing
  double waterLevel = 0;
  stat = IceDraft(_iceDraft, &waterLevel, pAtm, avgSOS, avgDens);
  if ((_iceDraft->Second % 8) == 0)
    Syslog::write("Ips4: range:%f, avgSOS:%f, waterlevel:%f, draft:%f",
                  _iceDraft->Range, _avgSOS, waterLevel, _iceDraft->Draft);

  if (stat != 0)
    return stat;

  *computedIceDraft = _iceDraft->Draft;
  return 0;
}

void Ips4::writeOutput(char *msg)
{
  try
  {
    _output->write();
  }
  catch (SharedData::AccessError error)
  {
    Syslog::write("Ips4::%s: SharedData::write() error", msg);
    fprintf(stderr, "Ips4::%s: SharedData::write() error", msg);
    exit(1);
  }
}