/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : Parosci.cc                                                    */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <i86.h>
#include "Parosci.h"
#include "Syslog.h"
#include "WorkSiteIF.h"
#include "SerialDevice.h"

static short parosciPeriod = 200;

static char *INT_TIME_STRING ="20";
static char *INT_RESET_CONFIRM_STRING ="*0001PR=0020";
static char *UNIT_CONFIRM_STRING ="*0100UN\xd\xa";
static char *UNIT_CONFIRM_RESPONSE_STRING ="*0001UN=";

static char *UNIT_RESET_STRING ="*0100EW*0100UN=1\xd\xa";
static char *UNIT_RESET_RESPONSE ="*0001UN=1";

static char *MODE_CONFIRM_STRING ="*0100MD\xd\xa";
static char *MODE_CONFIRM_RESPONSE ="*0001MD=0";

static char *SANITY_CHECK_STRING ="*0100P3\xd\xa";

static char *GET_PRESSURE_STRING ="*0100P2\xd\xa";
static char *READ_PRESSURE_STRING ="*0100DB\xd\xa";
static char *STOP_PRESSURE_STRING = "*0100P1\xd\xa";
static char *GET_TEMP_PERIOD ="*0100Q1\xd\xa";

static char *prefix ="*0100";

typedef enum{ TC, C1, C2, C3, D1, D2, T1, T2, T3, T4, T5, 
		U0, Y1, Y2, Y3
		} parocoeff;

static const char *Parosci::coeffNameList[] = {
  "TC", "C1", "C2", "C3", "D1", "D2","T1","T2","T3","T4","T5",
  "U0","Y1","Y2","Y3"
};

static const double Parosci::kPsiToBar = 0.0689476;

//== The members =================================================

Parosci::Parosci(SerialDevice *device)
  : PeriodicTask("Parosci")
{
  _device = device;
  _output = new ParosciOutput(SharedData::ReadWrite);
  _output->data.deviceStatus = DeviceIF::Initializing;
  _output->write();
  init();
}

Parosci::~Parosci()
{
  delete _output;
  delete _log;
}

#define REPLY_LEN 32

int Parosci::init()
{
  char command[100];
  int val,i,serialNo;
  double pres_0;
  Boolean debug = True;

  // == Initialize class variables ================================
  site_latitude = 0.0;
  pressure=0.0; depth=0.0; 
  pres_period = -1.0; temp_period=-1.0;

  // == Configure the site latitude ===============================
  try {
    WorkSiteIF worksite("worksite");
    site_latitude = worksite.latitude();
  } 
  catch(SharedObjectClient::MissingServer e) {
    site_latitude = 45.0;
    Syslog::write("Parosci.cc -- Worksite server is not running. "
		  "Using default lat of %lf", site_latitude );
  }
  catch(...) {
    Syslog::write("Parosci.cc  failed on instantiation of WorkSiteIF");
  }

  // == Configure the ParosciOutput ===============================

  _output->data.depth = 0.00;
  _output->data.pressure = 0.00;
  _output->data.temp = 0.00;
  _output->data.updateTime.tv_sec = 0.0;
  _output->data.deviceStatus = DeviceIF::Ok;

  try
    {
      _output->write();
    }
  catch (SharedData::AccessError error)
    {
      fprintf(stderr, "%s",error.msg);
      exit(1);
    }


  // == Configure the periodic callback ===========================
  addPeriodicCallback( parosciPeriod, 
		       (CallbackMethod)Parosci::periodicParosciCallback );

  //== Set up logger ==============================================

  _log = new ParosciLog( this, DataLog::BinaryFormat );


  //== Initialize the serial port =================================

  _device->commsDebugMode(SerialDevice::DebugOff);
  //     _device->commsDebugMode(SerialDevice::DebugOn);

  // initialize serial port
  _device->setLineFormat(BAUD, DATA_BITS, STOP_BITS, PARITY);
  
  // clear serial port
  _device->clearPort();
  _device->flushReceiveBuf();

  //== Put into proper mode =======================================
  strcpy(command, "*0100EW*0100MD=0\xd\xa");
  _device->write(command, strlen(command));
  if( _device->confirm("*0001MD=0\xa",PARO_TIMEOUT) == -1)
    {
      //error
      Syslog::write("Parosci.cc -- Failed to set mode");
      throw Exception("Parosci.cc -- Failed to set mode");
    }
  else {
    dprintf("Parosci::init() - mode set okay");
  }
  //== Memory check ===============================================
  strcpy(command,"*0100MC");
  _device->write(command,strlen(command));
  if( _device->confirm("*0001MC=Y\xa",PARO_TIMEOUT) == -1)
    {
      //error
      Syslog::write("Parosci.cc -- Failed to check memory");
      throw Exception("Parosci.cc -- Failed to check memory");
    }
  
  //== Get serial no  
  strcpy(command, "*0100SN\xd\xa");
  _device->write(command,strlen(command));
  if (_device->readUntil(reply, REPLY_LEN, "\xa", PARO_TIMEOUT) != -1)
    //     if (_device->read(reply, REPLY_LEN, PARO_TIMEOUT) != -1)
    {
      if (sscanf(reply, "%*7s=%d",&serialNo) !=1)
	//error
	{
	  Syslog::write("Parosci.cc -- Failed to read serial number");
	  throw Exception("Failed to read serial numbre");
	}
    }
  else
    {
      Syslog::write("Parosci.cc -- Failed to read serial number");
      throw Exception("Failed to read serial numbre");
    }

  // Reduce integration times 

  strcpy(command, "*0100PR\xd\xa");
  _device->write(command, strlen(command));
  if (_device->confirm("*0001PR=",PARO_TIMEOUT) == -1)
    {
      //error
      Syslog::write("Parosci.cc -- Failed to read integration times");
      throw Exception("Failed to read integration times");
    }
  
  //     if (_device->read(reply, REPLY_LEN, PARO_TIMEOUT) != -1)
  if (_device->readUntil(reply, REPLY_LEN, "\xa", PARO_TIMEOUT) != -1)
    {
      if ( sscanf(reply,"%d",&val) !=1)
	{
	  //error
	  Syslog::write("Parosci.cc -- Failed to read integration times");
	  throw Exception("Failed to read integration times");
	}
      if (val != INT_TIME)
	{
	  Syslog::write("Parosci.cc -- Current integration time = %d",
			val );
	  Syslog::write("Parosci.cc --      setting to new time = %d",
			INT_TIME );

	  //reset integration time
	  strcpy(command, "*0100EW*0100PR=");
	  strcat(command, INT_TIME_STRING);
	  strcat(command, "\xd\xa");
	  _device->write(command, sizeof(command));
	  if (_device->confirm(INT_RESET_CONFIRM_STRING, PARO_TIMEOUT) == -1)
	    {
	      //error
	      Syslog::write("Parosci.cc -- Failed to set new integration times");
	      throw Exception("Failed to set new integration times");
	    }
	  
	}
    }
  
  //confirm units on comp. pressure are in PSI

  _device->write(UNIT_CONFIRM_STRING, strlen(UNIT_CONFIRM_STRING));
  if (_device->confirm(UNIT_CONFIRM_RESPONSE_STRING, PARO_TIMEOUT) == -1)
    {
      //error
      Syslog::write("Parosci.cc -- Failed to confirm units");
      throw Exception("Failed to confirm units");
    }
  
  //     if (_device->read(reply, REPLY_LEN, PARO_TIMEOUT) != -1)
  if (_device->readUntil(reply, REPLY_LEN, "\xa", PARO_TIMEOUT) != -1)
    {
      if (sscanf(reply, "%d",&val) !=1)
	{
	  Syslog::write("Parosci.cc -- Failed to confirm units");
	  throw Exception("Failed to confirm units");
	}
       
      if (val != UNITS_VAL)
	{
	  Syslog::write("Parosci.cc -- Current output units = %d",
			val );
	  Syslog::write("Parosci.cc --    Setting new units = %d",
			UNITS_VAL );
	  //reset units
	  _device->write(UNIT_RESET_STRING, strlen(UNIT_RESET_STRING));
	  if (_device->confirm(UNIT_RESET_RESPONSE, PARO_TIMEOUT) == -1)
	    {
	      //error
	      Syslog::write("Parosci.cc -- Failed to set new units");
	      throw Exception("Failed to set new units");
	    }
	}
    }

  // confirm MODE is 0
  _device->write(MODE_CONFIRM_STRING,strlen(MODE_CONFIRM_STRING));
  if (_device->confirm(MODE_CONFIRM_RESPONSE, PARO_TIMEOUT) == -1)
    {
      //error
      Syslog::write("Parosci.cc -- Failed to confirm mode");
      throw Exception("Failed to confirm mode");
    }

  // Retrieve conversion coefficients

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

  //  strcpy(msg_buf,"\t")
  for (i=0 ; i < sizeof(coeffNameList)/sizeof(char *) ; i++)
    //     for (i=0 ; i < 5 ; i++)
    {
      sprintf(command, "%s%s\xd\xa",prefix,coeffNameList[i]);
      _device->write(command,strlen(command));
      tcdrain(_device->getFd()); delay(10);
      // get the reply
      //	  if (_device->read(reply, REPLY_LEN, PARO_TIMEOUT) == -1 ||
      try {
	if (_device->readUntil(reply, REPLY_LEN, "\xa", 5000) == -1 ||
	    sscanf(reply,"%*7s=%lf",&pc[i]) !=1)
	  {
	    //error
	    Syslog::write("Parosci.cc -- Failed to read coefficient %d (%d): %02x", i, strlen(reply), reply[0] );
	    //	       throw Exception("Failed to read coefficient");
	  }
      }
      catch(...)
	{
	  Syslog::write("Parosci.cc -- could not get coefficient");
	}
    }

  delay(500);
     
  _device->clearPort();
  //sanity check: ask instrument for compensated pressure, then get
  // raw pres & temp periods and calculate compensated pressure
  // using above coeffs
  // (from Odyssey Code)

  _device->write(SANITY_CHECK_STRING,strlen(SANITY_CHECK_STRING));
  tcdrain(_device->getFd()); delay(10);
  if (_device->confirm("*0001",2*PARO_TIMEOUT) != -1
      && _device->read(reply, REPLY_LEN, PARO_TIMEOUT) > 0
      && sscanf(reply, "%lf",&pres_0) == 1)
    {
      Syslog::write("Parosci.cc -- Initial pressure = %lf",
		    psiToBar(pres_0) );
    }
  else
    {
      //error
      Syslog::write("Parosci.cc -- Failed to query pressure for "
		    "sanity check: %s",reply);
      throw Exception("Failed to query pressure for sanity check");
    }

  // now get pressure period
  _device->clearPort();
  _device->write(GET_PRESSURE_STRING,strlen(GET_PRESSURE_STRING));
  tcdrain(_device->getFd()); delay(10);
  _device->clearPort();
  _device->confirm("*0001", 2*PARO_TIMEOUT);
  _device->readUntil(reply, REPLY_LEN, "\xa", PARO_TIMEOUT);
  _device->confirm("*0001", 2*PARO_TIMEOUT);
  _device->readUntil(reply, REPLY_LEN, "\xa", PARO_TIMEOUT);
  if (sscanf(reply, "%lf", &pres_period) == 1)
    {
      Syslog::write("Parosci.cc -- Parosci initial pressure period = %lf",
		    pres_period );
    }
  else
    {
      //error
      Syslog::write("Parosci.cc -- Failed to query pressure period"
		    " for sanity check");
      throw Exception("Failed to query pressure period for sanity check");
    }
  _device->write(STOP_PRESSURE_STRING, strlen(STOP_PRESSURE_STRING));
  tcdrain(_device->getFd()); delay(10);
  _device->clearPort();

  // and temperature period
  _device->write(GET_TEMP_PERIOD, strlen(GET_TEMP_PERIOD));
  tcdrain(_device->getFd()); delay(10);
  _device->clearPort();
  if (_device->confirm("*0001",2*PARO_TIMEOUT) != -1
      && _device->readUntil(reply, REPLY_LEN, "\xa", PARO_TIMEOUT) > 0
      //	 && _device->read(reply, REPLY_LEN, PARO_TIMEOUT) > 0
      && sscanf(reply,"%lf",&temp_period) == 1)
    {
      Syslog::write("Parosci.cc -- Parosci initial temp period = %lf",
		    temp_period );
    }
  else
    {
      //error
      Syslog::write("Parosci.cc -- Failed to query temperature period for "
		    "sanity check");
      throw Exception("Failed to query temperature period for sanity"
		      " check");
    }

  // Calculating initial pressure

  init_temp = comp_temp( temp_period );
  init_pres = comp_press_bar(pres_period,temp_period);

  // Paroscientific initialization completed

  Syslog::write("Parosci.cc -- Paroscientific configured.");

  _output->data.deviceStatus = DeviceIF::Ok;
  _output->write();

  return OK;
} 


int Parosci::getParosci(void)
{
  int nconv;
  double value;
  static int parosci_count = 0; //number of times this routine is called
  static int temp_cycle = 10;
  static int last_read_was_temp = 0; //read temp every 10 cycles

  static int temp_reject_count=0, pres_reject_count=0;
  char c;
  int i;

  // next read command is either temp or
  // pressure, depending on cycle count

  if (parosci_count %10 == 0)
    {
      _device->write(GET_TEMP_PERIOD, strlen(GET_TEMP_PERIOD));
      delay(50);
      _device->clearPort();
      _device->write(GET_TEMP_PERIOD, strlen(GET_TEMP_PERIOD));
      last_read_was_temp = 1;
    }
  else
    {
      _device->clearPort();
      _device->write(GET_PRESSURE_STRING,strlen(GET_PRESSURE_STRING));
      _device->clearPort();
      _device->readUntil(reply, REPLY_LEN, "\xa", PARO_TIMEOUT);
      _device->readUntil(reply, REPLY_LEN, "\xa", PARO_TIMEOUT);
      last_read_was_temp = 0;
    }
  parosci_count++;
  
  if (_device->confirm("*0001",READ_TIMEOUT) == -1 && parosci_count)
    {
      //error no reply
      Syslog::write("Parosci.cc -- Error: failure to read from Parosci");
      return ERROR;
    }
  else {
    try{
      _device->readUntil(reply, REPLY_LEN, "\xa", PARO_TIMEOUT);
    }
    catch (Exception exception) {
      Syslog::write("Parosci.cc - caught Exception: %s", exception.msg);
      return ERROR;
    }
    catch(...) {
      Syslog::write("Parosci.cc -- Caught some exception");
      return ERROR;
    }
  }

  if (sscanf(reply,"%lf",&value) != 1)
    {
      //read error
      Syslog::write("Parosci.cc -- Failure to parse reply: %s", reply);
      return ERROR;
    }
  else
    {
      if (last_read_was_temp)
	{
	  // compare with last temperature to rule out spurious readings
	  if ((temp_reject_count<0)&&
	      ((fabs(temp_period-value)/temp_period)>0.1))
	    {
	      //temp rejected
	      temp_reject_count++;
	      Syslog::write("Parosci.cc -- Rejected temperature period "
			    "%lf, too far from previous %lf", 
			    value, temp_period );
	      return ERROR;
	    }
	  else
	    {
	      temp_period = value;
	      _output->data.temp = comp_temp( temp_period );
	      temp_reject_count = 0;
	    } 
	}
      else
	{
	  _device->write(STOP_PRESSURE_STRING, strlen(STOP_PRESSURE_STRING));
	  if ((pres_reject_count < 3) &&
	      ((fabs(pres_period-value)/pres_period)>0.01))
	    {
	      //reject
	      pres_reject_count++;
	      Syslog::write("Parosci.c -- Rejected pressure period %lf, "
			    "too far from previous %lf", 
			    value, pres_period );
	      Syslog::write("Parosci.c - bad string was %s\n", reply);
	      return ERROR;
	    }
	  else
	    {
	      pres_period = value;
	      if (temp_period != -1)
		{

		  pressure = comp_press_bar(pres_period, temp_period);
		      
		  if (pressure <MAX_PRESSURE && pressure > MIN_PRESSURE)
		    {
		      depth = corrected_depth (pressure - init_pres);
		      pressure = init_pres;
		    }
		  else {
		    //error bad pressure
		    Syslog::write("Parosci.cc -- Error:Bad pressure %f bar\n",
				  pressure);
		    Syslog::write("Parosci - Error: %f pres period %f temp\n",
				  pres_period, temp_period);
		    Syslog::write("Reply is %s\n", reply);
		      return ERROR;
		    }
		}
	      pres_reject_count = 0;
	    }
	}
    }
  
  clock_gettime(CLOCK_REALTIME,&readtime);
  
  return OK;
}


void Parosci::periodicParosciCallback( void )
{
  // == Read data first ===========================================
  if( getParosci() == OK) 
    {
	  
      //	  Syslog::write("Parosci.cc -- pressure = %lf; temp = %lf; "
      //			"depth = %lf", pressure, temp, depth );
	  
      _output->data.pressure = pressure;
      _output->data.depth = depth;
      _output->data.updateTime.tv_sec = readtime.tv_sec;
      _output->data.updateTime.tv_nsec = readtime.tv_nsec;
      _output->data.deviceStatus = DeviceIF::Ok;

      // -- Log the data -----------------------------------------
      _log->write();

      // -- and now make data available to server ----------------
      _output->write();

    }
  else {
    _output->data.deviceStatus = DeviceIF::Error;
    _output->write();
  }
}

//COMP_TEMP: calculates the temperature on the Parosci in degC
// using the algorithm described in appendix G of the Parosci
//Digiquartz Manual
// (from Odyssey Code)

double Parosci::comp_temp(double t_per)
{
  double U, Temp;

  U = t_per - pc[U0];

  Temp = (pc[Y1] + (pc[Y2] + pc[Y3]*U)*U)*U;

  //     printf("Given t_per = %lf, U = %lf, temp = %lf\n",
  //	    t_per, U, Temp );
  
  return Temp;

}

//== comp_press_bar =============================================
// comp_press_bar: calculates the temparature compensated pressure in bar
// using the method in Appendix G of the Parosci Digiquartz manual
//(from Odyssey Code parosci.c)

double Parosci::comp_press_bar(double p_per, double t_per)
{
  double U, Temp;
  double Tn, C, D, T0, psi;

  U = t_per - pc[U0];
  C = pc[C1] + (pc[C2] + pc[C3]*U)*U;
  D = pc[D1] + pc[D2]*U;
  T0 = pc[T1] + (pc[T2] + (pc[T3] + (pc[T4] + pc[T5]*U)*U)*U)*U;
  Tn = T0*T0/(p_per*p_per);
  psi = C*(1-Tn) * (1-D*(1-Tn));

  return psiToBar(psi);
}

double Parosci::corrected_depth(double bar)
{
  double corrected, dbar, C, gr;
  static double st2;
  static int first=1;
  if (first) // calc & store sin^2(latitude)
    {
      st2 = sin ( site_latitude);
      st2 *= st2;
      first = 0;
    }
  //convert to dbar
  dbar = 10*bar;
  C = (((-1.8200e-15*dbar + 2.279e-10)*dbar - 2.5212e-05)*dbar + 9.72659)
    * dbar;
  gr = 9.780318*((2.3600e-05*st2 + 5.2788e-3)*st2 + 1) + 1.092e-06*dbar;

  corrected = C/gr;
  return corrected;
}