/****************************************************************************/
/* Copyright (c) 2015 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : Power Buoy Data Logger CANBus instrument reader class         */
/* Filename : CanBus.hpp                                                    */
/* Author   : Henthorn                                                      */
/* Project  : Power Buoy                                                    */
/* Version  : 1.0                                                           */
/* Created  : 04/20/2015                                                    */
/* Modified : 10/19/2016  RGH integrated Rudder Controller data             */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*                                                                          */
/* This class is the data source reader for the CANBus instruments on the   */
/* Power Buoy. 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 <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>

#include "CanBus.hpp"
#include "CSVWriter.hpp"

CanBus::CanBus(const char* cansock, CSVWriter* w)
   : FileSource("CanBus"), _logger(w),
   _cansock(NULL), _curr_sc_seq(0)
{
   _cansock = strdup(cansock);
   _pc_record.seq_num = -1;

    struct ifreq ifr;
    struct sockaddr_can addr;

    /* open socket */
    _fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (_fd < 0)
    {
        perror("CanBus: Unable to open socket");
        return;
    }

    // Set up for CanSOCK
    //
    addr.can_family = AF_CAN;
    strcpy(ifr.ifr_name, _cansock);

    if (ioctl(_fd, SIOCGIFINDEX, &ifr) < 0)
    {
        perror("CanBus: Unable to establish CANBus socket");
        return;
    }

    addr.can_ifindex = ifr.ifr_ifindex;

    fcntl(_fd, F_SETFL, O_NONBLOCK);

    if (bind(_fd, (struct sockaddr *)&addr, sizeof(addr)) < 0)
    {
        perror("CanBus: Unable to bind CANBus socket");
        return;
    }

    printf("CanBus: socket opened, fd = %d\n", _fd);

    memset(&_pc_record, 0, sizeof(_pc_record));
    memset(&_bc_record, 0, sizeof(_bc_record));
    memset(&_sc_record, 0, sizeof(_sc_record));
    memset(&_rc_record, 0, sizeof(_rc_record));
}

CanBus::~CanBus()
{
   if (_cansock) delete _cansock;
}

// Simple function to create a 32-bit masking agent from bit a to bit b.
// Logical AND the result to a unsigned integer.
//
unsigned CanBus::create_mask(unsigned a, unsigned b)
{
   unsigned i, r = 0;
   for (i=a; i<=b; i++)
       r |= 1 << i;

   return r;
}

// Return 0 if full record was read
// Otherwise, return something like the number of bytes needed for
// a full record.
// Return < 0 on error.
//
int CanBus::read_data(char verbose)
{
   _verbose = verbose;

   struct can_frame frame_rd;
   int recvbytes = 0;
   char buffer[200];
   unsigned seq_num,data_id,src_id;

   seq_num = data_id = src_id = 0;
   recvbytes = read(_fd, &frame_rd, sizeof(struct can_frame));
   if(recvbytes == sizeof(struct can_frame))
   {
      make_masks();

      seq_num = _seq_mask & frame_rd.can_id;
      data_id = _data_mask & frame_rd.can_id; data_id >>= 9;
      src_id  = _src_mask & frame_rd.can_id;  src_id  >>= 14;

      //printf("dlc = %d, srcID = %d, dataID = %d, seq# = %d  \n",
      //frame_rd.can_dlc, src_id, data_id, seq_num);

      //for (int i = 0; i < 8; i++) printf("%2x ", frame_rd.data[i] & 0xff);
      //printf("\n");

      // Process message data depending on the controller ID
      //
      switch(src_id)
      {
        case PowerConID:
          handle_power_msg(frame_rd);
          break;

        case SpringConID:
          handle_spring_msg(frame_rd);
          break;

        case BatteryConID:
          handle_battery_msg(frame_rd);
          break;

        case RudderConID:
          handle_rudder_msg(frame_rd);
          break;

        default:
          printf("pblog: Unknown CANBus message %d|%d|%d\n",
            src_id, data_id, seq_num);
          break;

      }
   }

   return 1;
}

// 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 CanBus::get_record(const char *buffer)
{
   if (buffer && sizeof(buffer) > 100)
      if (_verbose) printf("Big enough\n");
   else
      printf("Insufficient buffer\n");

   return 0;
}

// Extract Power Controller data from a CANBus frame
//
int CanBus::handle_power_msg(struct can_frame cf)
{
   int return_val = 1;

   unsigned seq_num, data_id, src_id;
   seq_num = data_id = src_id = 0;
   seq_num = _seq_mask & cf.can_id;
   data_id = _data_mask & cf.can_id; data_id >>= 9;
   src_id  = _src_mask & cf.can_id;  src_id  >>= 14;

   if (src_id != PowerConID)
      return -1;

   // A change in sequence number means a new set of data
   // so update the logger
   //
   if (_pc_record.seq_num != seq_num)
   {
      _logger->update_power_record(&_pc_record);
      _pc_record.seq_num = seq_num;
      return_val = 0;         // Got a complete record
   }


   unsigned short utemp;
   short temp;
   switch(data_id)
   {
      case 0:
         temp = *(short*)&cf.data[0];
         _pc_record.rpm = (float)temp / 2.;
         if (_verbose) printf("seq:%d  raw:%d  rpm:%.1f\n", seq_num, temp, _pc_record.rpm);

         temp = *(short*)&cf.data[2];
         _pc_record.sd_rpm = (float)temp / 2.;
         if (_verbose) printf("seq:%d  raw:%d  sd_rpm:%.1f\n", seq_num, utemp, _pc_record.sd_rpm);

         utemp = *(unsigned short*)&cf.data[4];
         _pc_record.voltage = (float)utemp / 100.;
         if (_verbose) printf("seq:%d  raw:%d  voltage:%.2f V\n", seq_num, utemp, _pc_record.voltage);

         temp = *(short*)&cf.data[6];
         _pc_record.power = (float)temp / 2.;
         if (_verbose) printf("seq:%d  raw:%d  power:%.2f W\n", seq_num, temp, _pc_record.power);

         break;

      case 1:
         temp = *(short*)&cf.data[0];
         _pc_record.bcurrent = (float)temp / 500.;
         if (_verbose) printf("seq:%d  raw:%d  bcurrent:%.3f A\n", seq_num, temp, _pc_record.bcurrent);

         temp = *(short*)&cf.data[2];
         _pc_record.lcurrent = (float)temp / 500.;
         if (_verbose) printf("seq:%d  raw:%d  lcurrent:%.3f A\n", seq_num, temp, _pc_record.lcurrent);

         utemp = *(unsigned short*)&cf.data[4];
         _pc_record.torque = (float)utemp / 1000.;
         if (_verbose) printf("seq:%d  raw:%d  torque:%.2f mV\n", seq_num, utemp, _pc_record.torque);

         utemp = *(unsigned short*)&cf.data[6];
         _pc_record.diff_press = (float)utemp / 10000.;
         if (_verbose) printf("seq:%d  raw:%d  diff_press:%.2f PSI\n", seq_num, utemp, _pc_record.diff_press);

         break;

      case 2:
         utemp = *(unsigned short*)&cf.data[0];
         _pc_record.bridgedc = (float)utemp * (1./40.96);
         if (_verbose) printf("seq:%d  raw:%d  bridge dc:%.2f%%\n", seq_num, utemp, _pc_record.bridgedc);

         utemp = *(unsigned short*)&cf.data[2];
         _pc_record.loaddc = (float)utemp * (1./40.96);
         if (_verbose) printf("seq:%d  raw:%d  loaddc:%.2f%%\n", seq_num, utemp, _pc_record.loaddc);

         utemp = *(unsigned short*)&cf.data[4];
         _pc_record.scale = (float)utemp / 100.;
         if (_verbose) printf("seq:%d  raw:%d  scale:%.4f\n", seq_num, utemp, _pc_record.scale);

         utemp = *(unsigned short*)&cf.data[6];
         _pc_record.retract = utemp / 100.;
         if (_verbose) printf("seq:%d  raw:%d  retract:%.4f\n", seq_num, utemp, _pc_record.retract);

         break;

      case 3:
         utemp = *(unsigned short*)&cf.data[0];
         _pc_record.target_v = (float)utemp / 100.;
         if (_verbose) printf("seq:%d  raw:%d  target_v:%.2f V\n", seq_num, utemp, _pc_record.target_v);

         utemp = *(unsigned short*)&cf.data[2];
         _pc_record.status = utemp;
         if (_verbose) printf("seq:%d  raw:%d  status:%u\n", seq_num, utemp, _pc_record.status);

         temp = *(short*)&cf.data[4];
         _pc_record.sd_rpm_target = (float)temp / 2.;
         if (_verbose) printf("seq:%d  raw:%d  sd_rpm_target:%.1f\n", seq_num, temp, _pc_record.sd_rpm_target);

         break;

      default:
         break;
   }

   return return_val;
}

// Extract Spring Controller data from a CANBus frame
//
int CanBus::handle_spring_msg(struct can_frame cf)
{
   int return_val = 1;

   unsigned seq_num, data_id, src_id;
   seq_num = data_id = src_id = 0;
   seq_num = _seq_mask & cf.can_id;
   data_id = _data_mask & cf.can_id; data_id >>= 9;
   src_id  = _src_mask & cf.can_id;  src_id  >>= 14;

   if (src_id != SpringConID)
      return -1;

   // A change in sequence number means a new set of data has
   // begun to come in, so update the logger with the record we have
   //
   if (_curr_sc_seq != seq_num)
   {
      _logger->update_spring_record(&_sc_record);
      _curr_sc_seq = seq_num;
      return_val = 0;         // Got a complete record
   }

   unsigned short utemp;
   short temp;
   unsigned long ultemp;

   switch(data_id)
   {
      case 0:
         temp = *(short*)&cf.data[0];
         _sc_record.load = temp;
         if (_verbose) printf("seq:%d  raw:%d  load:%d lbs\n", seq_num, temp, _sc_record.load);

         utemp = *(unsigned short*)&cf.data[2];
         _sc_record.range = (float)utemp / 10.;
         if (_verbose) printf("seq:%d  raw:%d  range:%.2f in\n", seq_num, utemp, _sc_record.range);

         utemp = *(unsigned short*)&cf.data[4];
         _sc_record.upsi = (float)utemp / 10.;
         if (_verbose) printf("seq:%d  raw:%d  up psi:%.2f PSI\n", seq_num, utemp, _sc_record.upsi);

         utemp = *(unsigned short*)&cf.data[6];
         _sc_record.lpsi = (float)utemp / 10.;
         if (_verbose) printf("seq:%d  raw:%d  low psi:%.2f PSI\n", seq_num, utemp, _sc_record.lpsi);

         break;

      case 1:
         ultemp = *(long*)&cf.data[0];
         //ultemp = (&cf.data[0]<<0) | (&cf.data[1]<<8) | (&cf.data[2]<<16) |
         //         (&cf.data[3]<<24);
         _sc_record.epoch = ultemp;
         if (_verbose) printf("seq:%d  raw:%ld  epoch:%ld sec\n", seq_num, ultemp,
                _sc_record.epoch);

         ultemp = *(long*)&cf.data[4];
         //ultemp = (&cf.data[4]<<0) | (&cf.data[5]<<8) | (&cf.data[6]<<16) |
         //         (&cf.data[7]<<24);
         _sc_record.salinity = (float)ultemp / 1000000.;
         if (_verbose) printf("seq:%d  raw:%ld  salinity:%.2f\n", seq_num, ultemp,
                _sc_record.salinity);

         break;

      case 2:
         temp = *(short*)&cf.data[0];
         _sc_record.temperature = ((float)temp) / 1000.;
         if (_verbose) printf("seq:%d  raw:%d  temperature:%.3f\n", seq_num, temp,
                 _sc_record.temperature);

         temp = *(short*)&cf.data[2];
         _sc_record.status = temp;
         if (_verbose) printf("seq:%d  raw:%d  status:%d\n", seq_num, temp, _sc_record.status);

         break;

      default:
         break;
   }

   return return_val;
}



// Extract Battery Controller data from a CANBus frame
//
int CanBus::handle_battery_msg(struct can_frame cf)
{
   int return_val = 1;

   unsigned seq_num, data_id, src_id;
   seq_num = data_id = src_id = 0;
   seq_num = _seq_mask & cf.can_id;
   data_id = _data_mask & cf.can_id; data_id >>= 9;
   src_id  = _src_mask & cf.can_id;  src_id  >>= 14;

   if (src_id != BatteryConID)
      return -1;

   // A change in sequence number means a new set of data
   // so update the logger
   //
   if (_bc_record.seq_num != seq_num)
   {
      _logger->update_battery_record(&_bc_record);
      _bc_record.seq_num = seq_num;
      return_val = 0;         // Got a complete record
   }

#if 0
   printf("BD data: %u %u %u %u %u %u %u %u\n",
	  cf.data[0], cf.data[1], cf.data[2], cf.data[3], 
	  cf.data[4], cf.data[5], cf.data[6], cf.data[7]);
#endif

   unsigned short utemp;
   short temp;
   switch(data_id)
   {
      case 0:
         utemp = *(unsigned short*)&cf.data[0];
         _bc_record.voltage = (float)utemp / 100.;
         if (_verbose) printf("seq:%d  raw:%d  voltage:%.2f V\n", seq_num, utemp, _bc_record.voltage);

         temp = *(short*)&cf.data[2];
         _bc_record.ips = (float)temp / 1000.;
         if (_verbose) printf("seq:%d  raw:%d  ips:%.2f A\n", seq_num, temp, _bc_record.ips);

         utemp = *(unsigned short*)&cf.data[4];
         _bc_record.vbalance = (float)utemp/1000.;
         if (_verbose) printf("seq:%d  raw:%d  vbalance:%.2f V\n", seq_num, utemp, _bc_record.vbalance);

         utemp = *(unsigned short*)&cf.data[6];
         _bc_record.vstopcharge = (float)utemp/1000.;
         if (_verbose) printf("seq:%d  raw:%d  vstopcharge:%.2f V\n", seq_num, utemp, _bc_record.vstopcharge);

         break;

      case 1:
         utemp = *(unsigned short*)&cf.data[0];
         _bc_record.gfault = (float)utemp / 1000.;
         if (_verbose) printf("seq:%d  raw:%d  gfault:%.2f mA\n", seq_num, utemp, _bc_record.gfault);

         temp = *(short*)&cf.data[2];
         _bc_record.hydrogen = (float)temp / 1000.;
         if (_verbose) printf("seq:%d  raw:%d  hydrogen:%.2f mV\n", seq_num, temp, _bc_record.hydrogen);

         utemp = *(unsigned short*)&cf.data[4];
         _bc_record.status = utemp;
         if (_verbose) printf("seq:%d  raw:%d  status:%u\n", seq_num, utemp, _bc_record.status);

         break;

      default:
         break;
   }

   return return_val;
}

int CanBus::handle_rudder_msg(struct can_frame cf)
{
   int return_val = 1;

   unsigned seq_num, data_id, src_id;
   seq_num = data_id = src_id = 0;
   seq_num = _seq_mask & cf.can_id;
   data_id = _data_mask & cf.can_id; data_id >>= 9;
   src_id  = _src_mask & cf.can_id;  src_id  >>= 14;

   if (src_id != RudderConID)
      return -1;

   // A change in sequence number means a new set of data has
   // begun to come in, so update the logger with the record we have
   //
   if (_curr_rc_seq != seq_num)
   {
      _logger->update_rudder_record(&_rc_record);
      _curr_rc_seq = seq_num;
      _rc_record.seq_num = seq_num;
      return_val = 0;         // Got a complete record
   }

   unsigned short utemp;
   short temp;
   unsigned long ultemp;

   switch(data_id)
   {
      // X, Y, and Z velocities  pressure values
      // 
      case 0:
         temp = *(short*)&cf.data[0];
         _rc_record.x_vel = (float)temp/1000.;
         if (1 || _verbose) printf("seq:%d  raw:%d  x_vel:%.2f m/s\n", seq_num, temp, _rc_record.x_vel);

         temp = *(short*)&cf.data[2];
         _rc_record.y_vel = (float)temp/1000.;
         if (1 || _verbose) printf("seq:%d  raw:%d  y_vel:%.2f m/s\n", seq_num, temp, _rc_record.y_vel);

         temp = *(short*)&cf.data[4];
         _rc_record.z_vel = (float)temp/1000.;
         if (1 || _verbose) printf("seq:%d  raw:%d  z_vel:%.2f m/s\n", seq_num, temp, _rc_record.z_vel);

         utemp = *(unsigned short*)&cf.data[6];
         _rc_record.pressure = (float)utemp * 0.002;
         if (1 || _verbose) printf("seq:%d  raw:%d  pressure:%.2f psi\n", seq_num, utemp, _rc_record.pressure);

         break;

      // Beam amplitudes and corrections, target heading
      // 
      case 1:
         _rc_record.b_amp1 = cf.data[0];
         if (1 || _verbose) printf("seq:%d  raw:%d  b_amp1:%d\n", seq_num, cf.data[0], _rc_record.b_amp1);

         _rc_record.b_amp2 = cf.data[1];
         if (1 || _verbose) printf("seq:%d  raw:%d  b_amp2:%d\n", seq_num, cf.data[1], _rc_record.b_amp2);

         _rc_record.b_amp3 = cf.data[2];
         if (1 || _verbose) printf("seq:%d  raw:%d  b_amp3:%d\n", seq_num, cf.data[2], _rc_record.b_amp3);

         _rc_record.b_cor1 = cf.data[3]*100./255.;
         if (1 || _verbose) printf("seq:%d  raw:%d  b_cor1:%.2f %%\n", seq_num, cf.data[3], _rc_record.b_cor1);

         _rc_record.b_cor1 = cf.data[4]*100./255.;
         if (1 || _verbose) printf("seq:%d  raw:%d  b_cor2:%.2f %%\n", seq_num, cf.data[4], _rc_record.b_cor2);

         _rc_record.b_cor1 = cf.data[5]*100./255.;
         if (1 || _verbose) printf("seq:%d  raw:%d  b_cor3:%.2f %%\n", seq_num, cf.data[5], _rc_record.b_cor3);

         temp = *(short*)&cf.data[6];
         _rc_record.target = (float)temp*360./65535.;
         if (1 || _verbose) printf("seq:%d  raw:%d  target hdg:%.2f deg\n", seq_num, temp, _rc_record.target);

         return 0;

         break;

      // Measured and filtered headings, rudder and ram positions
      // 
      case 2:
         temp = *(short*)&cf.data[0];
         _rc_record.measured = (float)temp*360./65535.;
         if (1 || _verbose) printf("seq:%d  raw:%d  measured hdg:%.2f deg\n", seq_num, temp, _rc_record.measured);

         temp = *(short*)&cf.data[2];
         _rc_record.filtered = (float)temp*360./65535.;
         if (1 || _verbose) printf("seq:%d  raw:%d  filtered hdg:%.2f deg\n", seq_num, temp, _rc_record.filtered);

         temp = *(short*)&cf.data[4];
         _rc_record.rudder = temp;
         if (1 || _verbose) printf("seq:%d  raw:%d  rudder:%d counts\n", seq_num, temp, _rc_record.rudder);

         temp = *(short*)&cf.data[2];
         _rc_record.ram = (float)temp*.001;
         if (1 || _verbose) printf("seq:%d  raw:%d  ram position:%.2f inches\n", seq_num, temp, _rc_record.filtered);

         break;

      // Qtns
      // 
      case 3:
         for (int i = 0; i < 4; i++)
         {
            temp = *(short*)&cf.data[i*2];
            _rc_record.qtn[i] = (float)temp/32768.;
            if (1 || _verbose) printf("seq:%d  raw:%d  qtn[%d]:%.2f \n", seq_num, temp, i, _rc_record.qtn[i]);
         }

         break;

      // Mags, and controller status
      // 
      case 4:
         for (int i = 0; i < 3; i++)
         {
            temp = *(short*)&cf.data[i*2];
            _rc_record.mag[i] = (float)temp/32768.;
            if (1 || _verbose) printf("seq:%d  raw:%d  mag[%d]:%.2f gauss\n", seq_num, temp, i, _rc_record.mag[i]);
         }

         utemp = *(unsigned short*)&cf.data[6];
         _rc_record.status = utemp;
         if (1 || _verbose) printf("seq:%d  raw:%d  status:0x%04x\n", seq_num, utemp, _rc_record.status);

         break;

      // Angular rates, and vpe status
      // 
      case 5:
         for (int i = 0; i < 3; i++)
         {
            temp = *(short*)&cf.data[i*2];
            _rc_record.ang_rate[i] = (float)temp*0.001;
            if (1 || _verbose) printf("seq:%d  raw:%d  ang_rate[%d]:%.2f rad/sec\n", seq_num, temp, i, _rc_record.ang_rate[i]);
         }

         utemp = *(unsigned short*)&cf.data[6];
         _rc_record.vpe_status = utemp;
         if (1 || _verbose) printf("seq:%d  raw:%d  vpe status:0x%04x\n", seq_num, utemp, _rc_record.vpe_status);

         break;

      // Accelerations
      // 
      case 6:
         for (int i = 0; i < 3; i++)
         {
            temp = *(short*)&cf.data[i*2];
            _rc_record.accel[i] = (float)temp*0.001;
            if (1 || _verbose) printf("seq:%d  raw:%d  accel[%d]:%.2f m/sec^2\n", seq_num, temp, i, _rc_record.accel[i]);
         }

         utemp = *(unsigned short*)&cf.data[6];
         if (1 || _verbose) printf("seq:%d  raw:%d  \n", seq_num, utemp);

         printf("Done with %d\n", data_id);
         break;

      default:
         break;
   }

   return return_val;
}