/****************************************************************************/
/* Copyright (c) 2015 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : Power Buoy Data Logger Crossbow AHRS data source class        */
/* Filename : Crossbow.hpp                                                  */
/* Author   : Henthorn                                                      */
/* Project  : Power Buoy                                                    */
/* Version  : 1.0                                                           */
/* Created  : 04/13/2015                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*                                                                          */
/* This class is the data source reader for the Crossbow instrument.        */
/* It is derived from the generic FileSource class.                         */
/*                                                                          */
/****************************************************************************/

#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <math.h>

#include "Crossbow.hpp"
#include "CSVWriter.hpp"

#define  XB_PREAMBLE_CHAR  0x55
#define  XB_PORT  "/dev/ttyO4"

// Contructor opens the serial port, initializes variables and flags
//
Crossbow::Crossbow(CSVWriter* w)
   : FileSource("crossbow"), _logger(w), _p(0),
   _in_msg(false), _preamble(false), _recvptr(0), _rlen(0)
{
   if ( (_fd = open(XB_PORT, O_RDONLY | O_NONBLOCK)) < 0 )
   {
      /* Could not open the port. */
      printf("Crossbow: Unable to open serial port %s", XB_PORT);
      perror(" ");
   }
   printf("Crossbow: Serial port %s opened, fd = %d, stdin = %d\n", XB_PORT, _fd, fileno(stdin));

   memset(_record, 0, sizeof(_record));
   _recvbuf[0] = _recvbuf[1] = 0x55;

}

Crossbow::~Crossbow()
{
}

char Crossbow::get_record(XB_Record *r)
{
   if (r)
   {
      memcpy(r, &_record[_p], sizeof(XB_Record));
      return 1;
   }
   else
      return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Reads data from a Crossbow instrument. Assumes a serial connection that
// can be bursty - only partial record data is available when performing
// non-blocking reads. So it probably will take multiple calls to get the
// full record.
// 
// Return 0 on error
// Return the (0 - SourceID) if a full record was read from that source
// Otherwise, return the number of bytes still needed for
// a full record.
// 
// int v = read_data(0);
// if (v > 0)
//   still some reading to do
// else if (v < 0)
//   complete record from source (0 - v)
// else
//   v == 0, error
//
int Crossbow::read_data(char verbose)
{
   int val = 0;     // return value

   _verbose = verbose;

   // Find the start of a msg if we're not already in the process
   // of reading a msg.
   // 
   if (!_in_msg)
   {
      find_start();   // Sets _in_msg to true if start was found and
                      // writes the starting bytes into the receive buffer

      //if (!_in_msg) printf("\n\t\t\tKeep looking next select()\n");
   }

   // Otherwise, we must be in the middle of reading a complete msg.
   //
   else
   {
      // Get what data is waiting, up to but not more than what is needed for a msg
      //
      int n = read(_fd, _recvbuf+_recvptr, XB_MSG_SIZE-_recvptr);

      //printf("\n\t\t\tPopulating receive buf starting at %d\n", _recvptr);
      //for (int i = 0; i < n; i++)
      //   printf("[%d]:%2x ", i, _recvbuf[_recvptr+i] & 0xff);

      _recvptr += n;    // Keeps track of how many msg bytes have been read so far
   }

   // Don't have the entire msg yet? Return the number of bytes needed
   //
   if (_recvptr < XB_MSG_SIZE)
   {
      val = (XB_MSG_SIZE - _recvptr);
   }
   else
   {
      //printf("Got full msg!\n");
      //for (int i = 0; i < _recvptr; i++) printf("%2x ", _recvbuf[i] & 0xff); printf("\n");

      // All the expected bytes received. Process and reset for next message.
      //
      process_data();    // Interprets data and sets values in a record structure
      _in_msg = false;   // No longer in a msg
      _recvptr = 0;      // Reset the tracker

      val = (0 - CrossbowID);          // indicates a complete message was read
   }

   return val;
}

// Define a bit of syntactic sugar
// 
#define  RECORD  _record[1-_p]

void Crossbow::process_data()
{
   // Assume there is a complete Crossbow message in the receive buffer.
   // Populate the data record according to Crossbow dox.
   //
   unsigned short crc, calc_crc = calcCRC();
   char *pcrc = (char*)&crc;
   memcpy(pcrc, _recvbuf+48, 1);
   memcpy(pcrc+1, _recvbuf+47, 1);

   if (calc_crc != crc)
      //printf("Crossbow: CRCs don't match\n");
      return;

   float  angle_scale = 360. / pow(2,16);
   float   rate_scale = 1260. / pow(2,16);
   float  accel_scale = 20. / pow(2,16);
   float    vel_scale = 512. / pow(2,16);
   float    gps_scale = 360. / pow(2,32);
   float    alt_scale = 1./4.;
   float   temp_scale = 200. / pow(2,16);

   // Angles
   RECORD.rollAngle   = i2_value(_recvbuf+5, angle_scale);
   if (_verbose) printf("roll %f  ", RECORD.rollAngle);
   RECORD.pitchAngle   = i2_value(_recvbuf+7, angle_scale);
   if (_verbose) printf("pitch %f  ", RECORD.pitchAngle);
   RECORD.yawAngleTrue = i2_value(_recvbuf+9, angle_scale);
   if (_verbose) printf("yaw %f  ", RECORD.yawAngleTrue);

   // Rates
   RECORD.xRateCorrected = i2_value(_recvbuf+11, rate_scale);
   if (_verbose) printf("xRate %f  ", RECORD.xRateCorrected);
   RECORD.yRateCorrected = i2_value(_recvbuf+13, rate_scale);
   if (_verbose) printf("yRate %f  ", RECORD.yRateCorrected);
   RECORD.zRateCorrected = i2_value(_recvbuf+15, rate_scale);
   if (_verbose) printf("zRate %f  ", RECORD.zRateCorrected);

   // Accellerations
   RECORD.xAccel = i2_value(_recvbuf+17, accel_scale);
   if (_verbose) printf("xAccel %f  ", RECORD.xAccel);
   RECORD.yAccel = i2_value(_recvbuf+19, accel_scale);
   if (_verbose) printf("yAccel %f  ", RECORD.yAccel);
   RECORD.zAccel = i2_value(_recvbuf+21, accel_scale);
   if (_verbose) printf("zAccel %f  ", RECORD.zAccel);

   // Velocities
   RECORD.nVelocity = i2_value(_recvbuf+23, vel_scale);
   if (_verbose) printf("nVelocity %f  ", RECORD.nVelocity);
   RECORD.eVelocity = i2_value(_recvbuf+25, vel_scale);
   if (_verbose) printf("eVelocity %f  ", RECORD.eVelocity);
   RECORD.dVelocity = i2_value(_recvbuf+27, vel_scale);
   if (_verbose) printf("dVelocity %f  ", RECORD.dVelocity);

   // GPS
   RECORD.longitude = i4_value(_recvbuf+29, gps_scale);
   if (_verbose) printf("longitude %f  ", RECORD.longitude);
   RECORD.latitude  = i4_value(_recvbuf+33, gps_scale);
   if (_verbose) printf("latitude %f  ", RECORD.latitude);

   // Altitude
   RECORD.altitude  = i2_value(_recvbuf+37, alt_scale);
   if (_verbose) printf("altitude %f  ", RECORD.altitude);

   // Temperature
   RECORD.xRateTemp = i2_value(_recvbuf+39, temp_scale);
   if (_verbose) printf("temp %f  ", RECORD.xRateTemp);

   if (_verbose) printf("itow %d\n", (int)u4_value(_recvbuf+41, 1.0));

   // Log data and toggle the record pointer - refer to the latest data
   // 
   if (_logger) _logger->update_xbow_record(&(RECORD));
   _p = 1 - _p;
}

//////////////////////////////////////////////////////////////////////////////
//
// Read bytes from the port into the read buffer until the preamble is found.
// The preamble is defined as 0x5555
//
// Then copy the preamble and any remaining message bytes from the
// read buffer into the start of the receive buffer. 
// Return the number of message bytes copied in the receive buffer.
// 
int Crossbow::find_start()
{
   _recvptr = 0;
   _in_msg = false;
   int preamble_start = 0;

   // Read from port and look for preamble sequence in the read buffer
   //
   int nbytes = read(_fd, _readbuf, sizeof(_readbuf));
   for (int i = 0; i < nbytes; i++, _rlen++)
   {
      //printf("[%d]:%2x ", i, _readbuf[i] & 0xff);
      // Look for the first preamble byte
      //
      if (_readbuf[i] == XB_PREAMBLE_CHAR)
      {
         if (!_preamble)
         {
            // First one
            _preamble = true;
         }
         else
         {
            //printf(" <-- last msg length = %d\n", _rlen);
            // Second one - bingo!
            preamble_start = i - 1;
            _rlen = 0;
            _in_msg = true;
            _preamble = false;
            break;
         }
      }
      else
      {
         _preamble = false;
      }
   }

   // We're here because the preamble was found.
   // Copy the preamble and any remaining message bytes from
   // the read buffer into the start of the receive buffer.
   //
   if (_in_msg)
   {
      //printf("\n\t\t\tCopying %d bytes into receive buffer\n", bytes_to_copy);
      int bytes_to_copy = nbytes - preamble_start;
      memcpy(_recvbuf, _readbuf+preamble_start, bytes_to_copy);
      _recvptr = bytes_to_copy;
      return _recvptr;
   }
   else
      return 0;
}

// CRC calculation as documented in Crossbow manual Appendix C
//
unsigned short Crossbow::calcCRC()
{
   unsigned short crc = 0x1D0F;

   for (int i = 2; i < XB_MSG_SIZE - 2; i++)
   {
      crc ^= _recvbuf[i] << 8;
      for (int j = 0; j < 8; j++)
      {
         if (crc & 0x8000) crc = (crc << 1) ^ 0x1021;
         else crc = crc << 1;
      }
   }

   return crc;
}

// Converts signed 2-byte value to a float
float Crossbow::i2_value(char* raw, float scale)
{
   short int val = (short int)256*raw[0] + raw[1]; 
   return scale * val;
}

// Converts unsigned 2-byte value to a float
float Crossbow::u2_value(char* raw, float scale)
{
   unsigned short int val = (unsigned short int)256*raw[0] + raw[1]; 
   return scale * val;
}

// Converts signed 4-byte value to a float
float Crossbow::i4_value(char* raw, float scale)
{
   float val = i2_value(raw+0,scale)*65536 + u2_value(raw+2,scale);
   return val;
}

// Converts unsigned 4-byte value to a float
float Crossbow::u4_value(char* raw, float scale)
{
   float val = u2_value(raw+0,scale)*65536 + u2_value(raw+2,scale);
   return val;
}