/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : Kearfott.cc                                                   */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  :                                                               */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*-----------------------------------------------------------------------*
  CLASS: Kearfott

  DESCRIPTION: Device interface for Kearfott SeaDevil INS

  $Id: Kearfott.cc,v 1.45 2010/12/01 21:51:57 rob Exp $
 *-----------------------------------------------------------------------*/

/*-----------------------------------------------------------------------*
  A NOTE ON COORDINATE SYSTEMS:

  This driver has been programmed to be consistent with the following 
  right-hand coordinate system:
  
  +X toward the nose of the AUV
  +Y to the starboard
  +Z down

  In this coord. system:

  Positive pitch is nose up
  Positive roll is toward the starboard (starboard down)
  Positive yaw is toward the starboard
 *-----------------------------------------------------------------------*/
#include "stdafx.h"
#include <memory.h>
#define _USE_MATH_DEFINES
#include <math.h>

#include "Kearfott.h"

double radToDeg(double rad) {return rad*180.0/M_PI;}
double degToRad(double deg) {return deg*M_PI/180.0;}

Kearfott::Kearfott(CSerial *device, BOOL extSensors)
{
     _device = device;
     _externalSensors = extSensors;

     // Initialize the device itself
     initialize();
}


Kearfott::~Kearfott()
{
}

BOOL Kearfott::initialize()
{
  //zero out raw nav data
  memset(&_rawNavData, 0, sizeof(KEARFOTT_RAW_NAV_TYPE));
  memset(&_opData, 0, sizeof(KEARFOTT_OP_TYPE));
  _opValid = _configValid = FALSE;
  _newUSBLFix = FALSE;
  _depthValid = FALSE;
  _salValid = FALSE;

  return TRUE;
}


BOOL Kearfott::sendOpData(KEARFOTT_OP_TYPE *data)
{
KEARFOTT_MSG_TYPE msg;
unsigned char *ptr;

    // save a copy for sending periodic updates
    memcpy(&_opData, data, sizeof(KEARFOTT_OP_TYPE));
    _opValid = TRUE;

    msg.cmdID = accept_operational; 
    msg.numBytes = 21;
    ptr = &(msg.data[0]);
    memcpy(ptr, &(data->validity), 1); ptr += 1;
    memcpy(ptr, &(data->modes), 1); ptr += 1;
    memcpy(ptr, &(data->init_lat), 4); ptr += 4;
    memcpy(ptr, &(data->init_lon), 4); ptr += 4;
    memcpy(ptr, &(data->idepth), 4); ptr += 4;
    memcpy(ptr, &(data->inithdg), 2); ptr += 2;
    memcpy(ptr, &(data->salinity), 2); ptr += 2;
    memcpy(ptr, &(data->cepos), 2); ptr += 2;
    memcpy(ptr, &(data->forcomm), 1); ptr += 1;
    msg.checksum = checksum(&msg);
    sendMsg(&msg);
 
    return TRUE;

}

BOOL Kearfott::getConfigData(KEARFOTT_CONFIG_TYPE *data)
{
KEARFOTT_MSG_TYPE msg;

  msg.cmdID = 6;
  msg.numBytes = 0;
  msg.checksum = checksum(&msg);
  sendMsg(&msg);
 
  return TRUE;
}

BOOL Kearfott::sendConfigData(KEARFOTT_CONFIG_TYPE *data)
{
KEARFOTT_MSG_TYPE msg;
unsigned char *ptr;

    memcpy(&_configData, data, sizeof(KEARFOTT_CONFIG_TYPE));
    _configValid = TRUE;
    
    msg.cmdID = accept_configuration; 
    msg.numBytes = 26;
    ptr = &(msg.data[0]);
    memcpy(ptr, &(data->valpha), 2); ptr += 2;
    memcpy(ptr, &(data->vbeta), 2); ptr += 2;
    memcpy(ptr, &(data->vgamma), 2); ptr += 2;
    memcpy(ptr, &(data->configBits), 1); ptr += 1;
    memcpy(ptr, &(data->satime), 1); ptr += 1;
    memcpy(ptr, &(data->vdelx), 2); ptr += 2;
    memcpy(ptr, &(data->vdely), 2); ptr += 2;
    memcpy(ptr, &(data->vdelz), 2); ptr += 2;
    memcpy(ptr, &(data->gpslx), 2); ptr += 2;
    memcpy(ptr, &(data->gpsly), 2); ptr += 2;
    memcpy(ptr, &(data->gpslz), 2); ptr += 2;
    memcpy(ptr, &(data->doplx), 2); ptr += 2;
    memcpy(ptr, &(data->doply), 2); ptr += 2;
    memcpy(ptr, &(data->doplz), 2); ptr += 2;
    msg.checksum = checksum(&msg);
    sendMsg(&msg);
 
    return TRUE;
 
}

BOOL Kearfott::parseConfigMessage(KEARFOTT_MSG_TYPE *msg)
{
  //printf("configuration message received\n");
  return TRUE;
}

BOOL Kearfott::parseNavigationMessage(KEARFOTT_MSG_TYPE *msg)
{
const char *ptr;
double dval;

  ptr = (const char *)(msg->data);
  memcpy(&_rawNavData.cycles, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.mode, ptr, 1); ptr += 1;
  memcpy(&_rawNavData.monitor, ptr, 1); ptr += 1;
  memcpy(&_rawNavData.lat, ptr, 4); ptr += 4;
  memcpy(&_rawNavData.lon, ptr, 4); ptr += 4;
  memcpy(&_rawNavData.depth, ptr, 4); ptr += 4;
  memcpy(&_rawNavData.bheight, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.roll, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.pitch, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.heading, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.vbodyx, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.vbodyy, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.vbodyz, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.accelx, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.accely, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.accelz, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.prate, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.qrate, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.rrate, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.utcTime, ptr, 4); ptr += 4;


  _scaledNavData.cycles = _rawNavData.cycles;
  _scaledNavData.mode = _rawNavData.mode;
  _scaledNavData.monitor = _rawNavData.monitor;
  _scaledNavData.lat = degToRad(_rawNavData.lat*0.00000008381903171539306640625);
  _scaledNavData.lon = degToRad(_rawNavData.lon*0.00000008381903171539306640625);
  _scaledNavData.depth = (double)_rawNavData.depth/(double)100.0;
  _scaledNavData.bheight = (double)_rawNavData.bheight/(double)100.0;
  _scaledNavData.roll = degToRad(_rawNavData.roll*0.0055); //comment in for kearfott z down
  _scaledNavData.pitch = degToRad(_rawNavData.pitch*0.0055);
  _scaledNavData.heading = degToRad(_rawNavData.heading*0.0055);
  _scaledNavData.vbodyx = _rawNavData.vbodyx/(double)1024.0;
  _scaledNavData.vbodyy = _rawNavData.vbodyy/(double)1024.0;
  _scaledNavData.vbodyz = _rawNavData.vbodyz/(double)1024.0;
  _scaledNavData.accelx = _rawNavData.accelx/(double)1024.0;
  _scaledNavData.accely = _rawNavData.accely/(double)1024.0;
  _scaledNavData.accelz = _rawNavData.accelz/(double)1024.0;
  _scaledNavData.prate = _rawNavData.prate/(double)8192.0;
  _scaledNavData.qrate = -_rawNavData.qrate/(double)8192.0;
  _scaledNavData.rrate = -_rawNavData.rrate/(double)8192.0;
  _scaledNavData.utcTime = _rawNavData.utcTime/(double)16384.0;

  //fill in the time when the data came in
  /*struct timespec currentTime;
  double deltaTime;
  clock_gettime(CLOCK_REALTIME, &currentTime);
  deltaTime = ((double)(currentTime.tv_sec+currentTime.tv_nsec/1.0e9))-
	      ((double)(_scaledNavData.sampleTime.seconds+_scaledNavData.sampleTime.nanoSeconds/1.0e9));
  if (deltaTime > .3) Syslog::write("Kearfott::deltaTime is %lf\n", deltaTime);
  _scaledNavData.sampleTime.seconds = currentTime.tv_sec;
  _scaledNavData.sampleTime.nanoSeconds = currentTime.tv_nsec;*/

  return TRUE;
}

void Kearfott::printRawNavType(KEARFOTT_RAW_NAV_TYPE *rawNav)
{
  printf("cntr: %d  mode: 0x%x  monitor: 0x%x\n",
	 rawNav->cycles, rawNav->mode, rawNav->monitor);
  printf("lat: %f  lon: %f depth: %f  height: %f\n",
	 rawNav->lat*8.3819e-8, 
	 rawNav->lon*8.3919e-8,
	 rawNav->depth/10.0, rawNav->bheight/100.0);
  printf("roll: %f pitch: %f hdg: %f VN: %f VE: %f VDOWN: %f\n",
	 rawNav->roll*0.0055, rawNav->pitch*0.0055, rawNav->heading*0.0055,
	 rawNav->vbodyx/1024.0,
	 rawNav->vbodyy/1024.0,
	 rawNav->vbodyz/1024.0);
  printf("ax: %f ay: %f az: %f pr: %f qr: %f rr: %f\n",
	 (float)((float)rawNav->accelx/1024.0),
	 (float)((float)rawNav->accely/1024.0),
	 (float)((float)rawNav->accelz/1024.0),
	 (float)((float)rawNav->prate/8192.0),
	 (float)((float)rawNav->qrate/8192.0),
	 (float)((float)rawNav->rrate/8192.0));
  printf("time %f sec\n", (float)((float)rawNav->utcTime/16384.0));
}

void Kearfott::printScaledNavType(KEARFOTT_SCALED_NAV_TYPE *scaledNav)
{
  printf("cntr: %d  mode: 0x%x  monitor: 0x%x\n",
	 scaledNav->cycles, scaledNav->mode, scaledNav->monitor);
  printf("lat: %f  lon: %f depth: %f  height: %f\n",
	 radToDeg(scaledNav->lat), 
	 radToDeg(scaledNav->lon),
	 scaledNav->depth, scaledNav->bheight);
  printf("roll: %f pitch: %f hdg: %f VN: %f VE: %f VDOWN: %f\n",
	radToDeg(scaledNav->roll),
	radToDeg(scaledNav->pitch),
	radToDeg(scaledNav->heading),
	 scaledNav->vbodyx,
	 scaledNav->vbodyy,
	 scaledNav->vbodyz);
  printf("ax: %f ay: %f az: %f pr: %f qr: %f rr: %f\n",
	 scaledNav->accelx,
	 scaledNav->accely,
	 scaledNav->accelz,
	 radToDeg(scaledNav->prate),
	 radToDeg(scaledNav->qrate),
	 radToDeg(scaledNav->rrate));
  printf("time %f sec\n",scaledNav->utcTime);
}

BOOL Kearfott::parseSlowTestMessage(KEARFOTT_MSG_TYPE *msg)
{
  return TRUE;
}

BOOL Kearfott::parseHighSpeedIMUData(KEARFOTT_MSG_TYPE *msg)
{
  return TRUE;
}

BOOL Kearfott::parseDopplerPD4(KEARFOTT_MSG_TYPE *msg)
{
const char *ptr;

  ptr = (const char *)(msg->data);
  ptr += 2;
  memcpy(&_dopplerPD4.dvlid, ptr, 1); ptr += 1;
  memcpy(&_dopplerPD4.datstr, ptr, 1); ptr += 1;
  memcpy(&_dopplerPD4.numbyte, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.sysconfig, ptr, 1); ptr += 1;
  memcpy(&_dopplerPD4.xvelbtm, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.yvelbtm, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.zvelbtm, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.evelbtm, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.bm1, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.bm2, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.bm3, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.bm4, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.botstat, ptr, 1); ptr += 1;
  memcpy(&_dopplerPD4.xvelref, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.yvelref, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.zvelref, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.evelref, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.refstrt, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.refend, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.refstat, ptr, 1); ptr += 1;
  memcpy(&_dopplerPD4.toping_hr, ptr, 1); ptr += 1;
  memcpy(&_dopplerPD4.toping_min, ptr, 1); ptr += 1;
  memcpy(&_dopplerPD4.toping_sec, ptr, 1); ptr += 1;
  memcpy(&_dopplerPD4.toping_hun, ptr, 1); ptr += 1;
  memcpy(&_dopplerPD4.bit, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.spdsnd, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.temp, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.checksum, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.ktnav, ptr, 2); ptr += 2;
  
  return TRUE;
}

void Kearfott::printDoppler(RDI_PD4_TYPE *pd4)
{
  printf("xvel:%d yvel:%d zvel:%d evel:%d\n",
	 pd4->xvelbtm, pd4->yvelbtm,
	 pd4->zvelbtm, pd4->evelbtm);
  printf("bm1:%d bm2:%d bm3:%d bm4:%d\n",
	 pd4->bm1, pd4->bm2,
	 pd4->bm3, pd4->bm4);
  printf("bottom status is 0x%x\n", pd4->botstat);
  printf("DOPPLER BIT results: 0x%x\n", pd4->bit);
  printf("DOPPLER temp: %f\n\n", pd4->temp/100.0);
}

BOOL Kearfott::parseGPS_GGA(KEARFOTT_MSG_TYPE *msg)
{
  //display GPS GGA string in GPS text box

 
  return TRUE;
}

BOOL Kearfott::parseGPS_GSA(KEARFOTT_MSG_TYPE *msg)
{
   //display GPS GSA string in GPS text box
   
   return TRUE;
}

BOOL Kearfott::parseGPS_VTG(KEARFOTT_MSG_TYPE *msg)
{
	//display GPS GSA string in GPS text box
	return TRUE;
}


BOOL Kearfott::sendMsg(KEARFOTT_MSG_TYPE *msg)
{
char buff[258];

 
    buff[0]= 0xaa; 
    buff[1]= msg->cmdID; 
    buff[2]= msg->numBytes;
    memcpy(&(buff[3]), msg->data, msg->numBytes);
    buff[msg->numBytes+3] = msg->checksum;
    _device->SendData(buff, msg->numBytes + 4); 

    return TRUE;
}

void Kearfott::logMsg(KEARFOTT_MSG_TYPE *msg)
{
char val;
unsigned char buf[259];
int cnt=0;

    buf[cnt++] = 0xaa;
    buf[cnt++] = msg->cmdID; 
    buf[cnt++] = msg->numBytes;
    if (msg->numBytes) {
	memcpy(buf+3, msg->data, msg->numBytes); cnt += msg->numBytes;
    }
    buf[cnt++] = msg->checksum; 

}


BOOL Kearfott::parseMsg(KEARFOTT_MSG_TYPE *msg)
{
   //
   // Add a terminating null in the data string to prevent strchr from running
   // past the end of the string in the parsing code in GpsUtils::parsexxx.
   //
   if( msg->numBytes < 0 || msg->numBytes > 255 )
   {
  /*    Syslog::write(" Kearfott::parseMsg: Error - "
		    "Message %d has an illegal number of bytes %d\n", 
		    msg->cmdID, msg->numBytes);*/
      return FALSE;
   }
   msg->data[msg->numBytes] = '\0';
   //
   switch (msg->cmdID)
   {
      case accept_configuration:
      case accept_operational:
      case return_configuration:
		return TRUE;
      case navigation_data:
		parseNavigationMessage(msg);
		return TRUE;
      case configuration_data:
		return parseConfigMessage(msg);
      case slow_test_message:
		return parseSlowTestMessage(msg);
      case high_speed_IMU_data:
		return parseHighSpeedIMUData(msg);
      case doppler_PD4:
		return parseDopplerPD4(msg);
      case gps_PVT_A:
      case gps_PVT_B:
		return TRUE;
      case gps_GGA:
	 //      printf("received gps gga data\n");
		return parseGPS_GGA(msg);
      case gps_GSA:
	 //      printf("received gps gsa data\n");
		return parseGPS_GSA(msg);
      case gps_VTG:
	 //      printf("received gps vtg data\n");
		return parseGPS_VTG(msg);
      default:
	 printf("unknown command ID %d\n", msg->cmdID);
	 printf("%s\n", msg->data);
	 return FALSE;
   }
}

BOOL Kearfott::readMsg(KEARFOTT_MSG_TYPE *msg)
{
	unsigned char buffer[256]; //most we should have to read until we see another packet
	BOOL readError = FALSE;
	unsigned char header[3];
	int nbytes;

	if	(_device) {	 
		do	{
			nbytes = _device->ReadData(header, 1);
			if (nbytes < 0)	return FALSE;
		} while (header[0]	!= 0xAA);
		nbytes = _device->ReadData(&(header[1]), 2);
		if (nbytes != 2) return FALSE;
		msg->cmdID = header[1]; msg->numBytes = header[2];
		nbytes = _device->ReadData(buffer, msg->numBytes+1);
		if (nbytes != msg->numBytes+1) {
			printf("only read %d bytes, expected %d bytes\n", nbytes, msg->numBytes+1);
			perror("readNChars");
			readError = TRUE;
		}
		memcpy(msg->data, buffer, msg->numBytes); //copy out data
		msg->checksum = buffer[msg->numBytes]; 
	}	
  if (readError) return FALSE;

  if (msg->cmdID == 1 && msg->numBytes != 46) {
 //   Syslog::write("nav message wrong length, expected 46, got %d\n", msg->numBytes);
    return FALSE;
  }
  if (msg->cmdID == 21 && msg->numBytes != 49) {
 //   Syslog::write("wierd PD4 message\n");
    return FALSE;

  }
  
  if ((msg->checksum != checksum(msg)) || readError == TRUE) {
        printf("checksum error, msg ID %d, byte cnt %d\n",
    	   msg->cmdID, msg->numBytes);
    return FALSE;
  } else {
    //logMsg(msg);
    return TRUE;
  }

}

unsigned char Kearfott::checksum(KEARFOTT_MSG_TYPE *msg)
{

     unsigned char i, sum=0;

     sum ^= msg->cmdID;
     sum ^= msg->numBytes;

     for (i = 0; i < msg->numBytes; i++) {
       sum ^= msg->data[i];
     }

     return sum;
}

void Kearfott::run()
{
KEARFOTT_MSG_TYPE inMsg;
time_t lastTime = time(NULL);
time_t lastCurrentDataTime = time(NULL);

	while (_device->ReadDataWaiting()) {
		if (readMsg(&inMsg) == TRUE){
			parseMsg(&inMsg);
		}
	}

/*
   if ((time(NULL) - lastCurrentDataTime) > 2) {
     lastCurrentDataTime = time(NULL);
     if (_opValid) sendOpData(&_opData);
   }
*/

   //if ((time(NULL) - lastTime) > 1) {
   //  lastTime = time(NULL);
   //  if (_externalSensors) {
   //    updateExternalSensors();
   //  } else if (_opValid) {
   //    sendOpData(&_opData);
   //  }
   //}

/* 
   if (_externalSensors) {
     if ((time(NULL) - lastTime) > 1) {
       lastTime = time(NULL);
       //Syslog::write("%s: updating with external sensors\n", name());
       updateExternalSensors();
     }
   }
*/
   //   delay(10);
}


void Kearfott::updateExternalSensors()
{
    // set initial values 
    _opData.validity = 0; 
    _opData.idepth = 0;
    //_opData.modes = GPSENAB|DOPPING|DOPENAB;
    _opData.forcomm = 0; //bottom track only mode 

    //update from USBL tracking system (looks like a GPS interface-wise
    if (_newUSBLFix) {
		_opData.init_lat = (int)(_usblLat/(8.381903171539307e-008));
		_opData.init_lon = (int)(_usblLon/(8.381903171539307e-008));
		_opData.cepos = 20000; //200m
		_opData.validity |= VPOS;
		_opData.forcomm = 0;
		_newUSBLFix = FALSE;
	}	

    if (_depthValid) {
		int ival;
		ival = (int)(_currentDepth * 100);
		_opData.idepth = ival;
		_opData.validity |= VDPTH|VERTDAMP_PRESS;
        if (_currentDepth > 10.0) _opData.modes |= DOPENAB;
		_depthValid = FALSE;
    }

    if (_salValid) {
      _opData.salinity = (int)(_currentSal * 100.0);
      _opData.validity |= VSALIN;
    } else {
      _opData.salinity = 3422;
      _opData.validity |= VSALIN;
    }

    sendOpData(&_opData);
 
}