/****************************************************************************/
/* 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 <math.h>

#include "Crossbow.hpp"
#include "LogWriter.hpp"

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

// Contructor opens the serial port, initializes variables and flags
//
Crossbow::Crossbow(LogWriter* w)
   : FileSource("crossbow"), _logger(w),
   _in_msg(false), _preamble(false), _recvptr(0), _rlen(0)
{
   if ( (_fd = open(XB_PORT, O_RDONLY | O_NOCTTY)) < 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\n", XB_PORT, _fd);
   _recvbuf[0] = _recvbuf[1] = 0x55;
}

Crossbow::~Crossbow()
{

}

// Return 0 if full msg was read
// Otherwise, return something like the number of bytes needed for
// a full msg.
// Return < 0 on error.
//
int Crossbow::read_data()
{
   // 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
   {
      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;
   }

   // Don't have the entire msg yet? Return the number of bytes needed
   //
   if (_recvptr < XB_MSG_SIZE)
   {
      return 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();
      _in_msg = false;
      _recvptr = 0;

      return 0;          // 0 indicates a complete message was read
   }
}

float Crossbow::i2_value(char* raw, float scale)
{
   short int val = (256*raw[0] + raw[1]); 
   return scale * val;
}

float Crossbow::i4_value(char* raw, float scale)
{
   int val = (4096*raw[0] + 256*raw[1] + 16*raw[2] + raw[3]);
   return scale * val;
}

float Crossbow::u4_value(char* raw, float scale)
{
   unsigned int val = (4096*raw[0] + 256*raw[1] + 16*raw[2] + raw[3]);
   return scale * val;
}

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("CRCs match\n");
   else
      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);
   printf("roll %f  ", _record.rollAngle);
   _record.pitchAngle   = i2_value(_recvbuf+7, angle_scale);
   printf("pitch %f  ", _record.pitchAngle);
   _record.yawAngleTrue = i2_value(_recvbuf+9, angle_scale);
   printf("yaw %f  ", _record.yawAngleTrue);

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

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

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

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

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

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

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

   _logger->update_xbow_record(&_record);


}

// Return the latest data record in the buffer supplied by the caller.
// Return the number of bytes written to the buffer (0 or more).
// Return < 0 on error.
//
int Crossbow::get_record(const char *buffer)
{
   if (buffer && sizeof(buffer) > 100)
      printf("Big enough\n");
   else
      printf("Insufficient buffer\n");

   return 0;
}

// Read from the port into the read buffer until the preamble is found.
// Then copy the preamble and any fresh message bytes remaining in the
// read buffer into the start of the receive buffer. 
// Return the number of message bytes 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 n = read(_fd, _readbuf, sizeof(_readbuf));
   for (int i = 0; i < n; i++, _rlen++)
   {
      //printf("[%d]:%2x ", i, _readbuf[i] & 0xff);
      if (_readbuf[i] == XB_PREAMBLE_CHAR)
      {
         // Found a preamble character
         if (!_preamble)
         {
            // First one
            _preamble = true;
         }
         else
         {
            // Second one
            preamble_start = i - 1;
            //printf(" <-- %d last msg length = %d\n", start, _rlen);
            _rlen = 0;
            _in_msg = true;
            _preamble = false;
            break;
         }
      }
      else
      {
         _preamble = false;
      }
   }

   // Found the preamble. Copy it and the rest of the bytes from
   // the read buffer into the start of the receive buffer.
   //
   if (_in_msg)
   {
      int bytes_to_copy = n - preamble_start;
      //printf("\n\t\t\tCopying %d bytes into receive buffer\n", bytes_to_copy);
      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;
}