/****************************************************************************/
/* Copyright (c) 2003 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : FastCat.cc                                                    */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 09/17/2003                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <unistd.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/uio.h>
#include <fcntl.h>
#include <errno.h>
#include <iostream.h>
#include <math.h>

#include "SoundVelocity.h"
#include "FastCat.h"
#include "FastCatLog.h"
#include "FastCatOutput.h"
#include "Syslog.h"

#define BAUD 9600
#define PARITY "NONE"
#define STOP_BITS 1
#define DATA_BITS 8
#define READ_TIMEOUT 100

#define FastCatPERIOD 63

#define MaxRecordBytes 200

FastCat::FastCat(SerialDevice *device, Boolean verbose)
  : StreamSerialDriver("FastCat", device, MaxRecordBytes, "\xa",
		       READ_TIMEOUT )
{
  Syslog::write("FastCat: constructing...\n");
  _device = device;
  _device->commsDebugMode(SerialDevice::DebugOn);
  _output = new FastCatOutput();
  _verbose = verbose;

  try {
    _log = new FastCatLog( this, DataLog::BinaryFormat );
  }
  catch ( ... ) {
    throw Exception("FastCat:FastCat() - FastCatLog contructor failed\n");
  }

}


FastCat::~FastCat()
{
  delete _output;
}


/* Init function simply confirms that data is pending on the device port */

DeviceIF::Status FastCat::initialize(void)
{
  char buf[30];
  
  Syslog::write("FastCat: initializing...\n");

  if (isatty(_device->getFd())) {
        _device->raw(24,1);
//	_device->nonRaw();
  }

  sprintf(buf, "%c%c", 0x0d, 0x0a);
  printf("CRLF %d bytes\n", _device->write(buf, 2));
  perror("");
  sleep(1);
  printf("CRLF %d bytes\n", _device->write(buf, 2));
  perror("");


  int nbytes = 0;
  sleep(1);
  if ((nbytes = _device->nRecvdBytes()) <= 0)
  {
    Syslog::write("FastCat: data not streaming (%d)...\n", nbytes);
    return DeviceIF::Error;
  }
  else
  {
    Syslog::write("FastCat: data is streaming (%d)...\n", nbytes);
  }

  _output->data.deviceReady = True;
  try
  {
    _output->write();
  }
  catch (SharedData::AccessError error)
  {
    fprintf(stderr, "%s",error.msg);
    return DeviceIF::Error;
  }

  return DeviceIF::Ok;
} 

DeviceIF::Status FastCat::readRecord(unsigned char *record,
				     int maxRecordBytes,
				     const char *recordTerminator,
				     unsigned readTimeout,
				     int *nBytesRead)
{
    *nBytesRead = 0;
    int status;
    status = read(_device->getFd(), record, 24);

    if (record[23] == 0x0a) {
	*nBytesRead = status;
	return DeviceIF::Ok;
    }

    //try to resync
    Syslog::write("FastCat::resync'ing");
    do {
	status = read(_device->getFd(), record+(*nBytesRead), 1);
        if (status <= 0) return (DeviceIF::Error);
        *nBytesRead += status;
    } while ((record[*nBytesRead-1]) != 0x0a && *nBytesRead < maxRecordBytes); 
//    *nBytesRead = read(_device->getFd(), record, maxRecordBytes);
    if (*nBytesRead > 0 && record[*nBytesRead-1] == *recordTerminator) {
	return DeviceIF::Ok;
    } else {
	return DeviceIF::Error;
    }
}


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

  dprintf("FastCat::processRecord() - %s", record);

  char *reply = (char*)record;
  unsigned long rawvals[4];
  if (4 != sscanf(reply, "%6x%6x%6x%4x",
		  &rawvals[0],
		  &rawvals[1],
		  &rawvals[2],
		  &rawvals[3]))
  {
    Syslog::write("FastCat - data not scanned correctly!");
    return DeviceIF::Error;
  }

  timespec readtime;
  clock_gettime(CLOCK_REALTIME,&readtime);
  _output->data.temp_updateTime.tv_sec = readtime.tv_sec;
  _output->data.temp_updateTime.tv_nsec = readtime.tv_nsec;
  _output->data.cond_updateTime.tv_sec = readtime.tv_sec;
  _output->data.cond_updateTime.tv_nsec = readtime.tv_nsec;

  dprintf("FastCat::processRecord() - 22 chars scanned");

  /* Calculate temp, pressure, and conductivity */
  /* Need to put these calibration constants in
     a config file eventually */

  _output->data.tcounts = rawvals[0];
  _output->data.cfreq   = (double)rawvals[1] / 256.0;
  _output->data.pcounts = rawvals[2];
  _output->data.pvolts  = (double)rawvals[3] / 13107.0;
  _output->data.temperature         = calcTemp(rawvals[0]);
  _output->data.pressure            = calcPres(rawvals[2],
					       _output->data.pvolts);
  _output->data.conductivity        = calcCond(_output->data.cfreq,
					       _output->data.temperature,
					       _output->data.pressure);
  _output->data.salinity = calcSalinity(_output->data.conductivity*10,
					_output->data.temperature,
					_output->data.pressure);

  float sndVel = SoundVelocity::calcSndVel(_output->data.temperature,
					   _output->data.pressure,
					   0.,
					   _output->data.salinity);
  _output->data.sound_velocity = sndVel;

  _output->write();

  _log->write();

  return DeviceIF::Ok;
}

double FastCat::calcSalinity(double conductivity, // S/m * 10 (see PSS 1978 notes)
			     double temp,         // degrees C
			     double pressure)     // decibars
{
  double cond_35_15_0 = 42.914;
  double A1 = 0.0000207;
  double A2 = -6.370e-10;
  double A3 = 3.989e-15;
  double B1 = 0.03426;
  double B2 = 0.0004464;
  double B3 = 0.4215;
  double B4 = -0.003107;
  double c0 = 0.6766097;
  double c1 = 0.0200564;
  double c2 = 0.0001104259;
  double c3 = -6.9698e-7;
  double c4 = 1.0031e-9;

  double a0 = 0.0080;
  double a1 = -0.1692;
  double a2 = 25.3851;
  double a3 = 14.0941;
  double a4 = -7.0261;
  double a5 = 2.7081;
  double b0 = 0.0005;
  double b1 = -0.0056;
  double b2 = -0.0066;
  double b3 = -0.0375;
  double b4 = 0.0636;
  double b5 = -0.0144;
  double k = 0.0162;

  double R = conductivity / cond_35_15_0;
  double Rp = 1 + (pressure*(A1 + A2*pressure + A3*pressure*pressure))
                         / (1 + B1*temp + B2*temp*temp + B3*R + B4*R*temp);
  double rT = c0 + c1*temp + c2*temp*temp + c3*pow(temp,3) + c4*pow(temp,4);
  double Rt = R / (Rp*rT);
  double Rtx = sqrt(Rt);

//  double salA = a0*pow(Rt,0.5) + a1*pow(Rt,0.5) + a2*pow(Rt,0.5) + a3*pow(Rt,0.5)
                               + a4*pow(Rt,0.5) + a5*pow(Rt,0.5);
//  double salB = b0*pow(Rt,0.5) + b1*pow(Rt,0.5) + b2*pow(Rt,0.5) + b3*pow(Rt,0.5)
                               + b4*pow(Rt,0.5) + b5*pow(Rt,0.5);

   double salA = a0 +
                 a1*Rtx + 
                 a2*Rtx*Rtx + 
                 a3*Rtx*Rtx*Rtx + 
                 a4*Rtx*Rtx*Rtx*Rtx +
                 a5*Rtx*Rtx*Rtx*Rtx*Rtx;
   double salB = b0 +
		 b1*Rtx +
		 b2*Rtx*Rtx +
	 	 b3*Rtx*Rtx*Rtx +
		 b4*Rtx*Rtx*Rtx*Rtx +
		 b5*Rtx*Rtx*Rtx*Rtx;
  double t_factor = (temp - 15)/(1 + k*(temp - 15));
  double sal = salA + t_factor*salB;

  return sal;
}


double FastCat::calcPres(unsigned long presCounts, double thermistor)
{
  double pa0 = 1.052398e+00; //9.893233e-01; //1.533173;
  double pa1 = 3.116437e-02; //3.114406e-02; //0.03111118;
  double pa2 = -1.896517e-09; //-1.906641e-09; //-0.000000001882209;
  double ptempa0 = -7.333041e+01; //-7.290148e+01; //-67.94835;
  double ptempa1 = 5.860151e+01; //5.878786e+01; //52.47274;
  double ptempa2 = -2.346585e+00; //-2.611919e+00; //-0.6730714;
  double ptca0 = 5.239215e+05; //5.239244e+05; //523938.7;
  double ptca1 = 2.473526e+00; //2.417595e+00; //3.338685;
  double ptca2 = -4.064230e-02; //-3.625319e-02; //-0.04929293;
  double ptcb0 = 25.01600;
  double ptcb1 = 0.0002;
  double ptcb2 = 0.;

  double t = ptempa0 + ptempa1*thermistor + ptempa2*thermistor*thermistor;
  double x = presCounts - ptca0 - ptca1*t - ptca2*t*t;
  double n = x*ptcb0 / (ptcb0 + ptcb1*t + ptcb2*t*t);
  double pres = pa0 + pa1*n + pa2*n*n;

  pres *= (.6894757);
  pres -= 10.1325; //subtract off atmospheric pressure, per App note 14 in manual
  return pres;
}


double FastCat::calcTemp(unsigned long tempCounts)
{
  double a0 = 8.813980e-04; //8.631787e-04; //.0008426681;
  double a1 = 2.681933e-04; //2.740111e-04; //.0002763018;
  double a2 = -6.897948e-07; //-1.307042e-06; //-.000001575461;
  double a3 = 1.476427e-07; //1.694194e-07; //.0000001904065;

  double mv = (double)((double)tempCounts - 524288.0) / (double)1.6e7;
  double r  = (mv * (double)2.295e10 + (double)9.216e8)
                                     / ((double)6.144e4 - mv*(double)5.3e5);
  double ln_r = ::log(r);

  double temp = 1. / ( a0 + a1*ln_r + a2*pow(ln_r,2) + a3*pow(ln_r,3) )
                                                              - (double)273.15;
  return temp;
}

double FastCat::calcCond(double cFreq, double temp, double pressure)
{
  double g = -9.797017e-01; //-9.798803e-01; //-0.9789886;
  double h = 1.382482e-01; //1.382055e-01; //0.1379683;
  double i = -2.072005e-04; //-1.590867e-04; //-0.0001009654;
  double j = 3.823857e-05; //3.503165e-05; //0.00002966963;
  double cpcor = -0.000000095700;
  double ctcor = 0.0000032500;

  // pressure *= 1.45; // Convert to psia for this formula

  cFreq /= 1000.0; 
  double cond = (g + h*pow(cFreq,2) + i*pow(cFreq,3) + j*pow(cFreq,4))
                                          / (1 + ctcor*temp + cpcor*pressure);

  return cond;
}