/****************************************************************************/
/* 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 :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*                                                                          */
/* This class is the data source reader for the Boost Controller 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 <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>

#include "CanBus.hpp"
#include "LogWriter.hpp"

CanBus::CanBus(const char* cansock, LogWriter* w)
   : FileSource("CanBus"), _logger(w),
   _cansock(NULL), _curr_sc_seq(0)
{
   _cansock = strdup(cansock);
   _bc_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;
    }

    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);
}

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::createMask(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()
{
   printf("CanBus: reading data...\n");
   struct can_frame frame_rd;
   int recvbytes = 0;
   char buffer[200];
   unsigned seq_num,data_id,src_id;

   unsigned seq_mask  = createMask(0,8);
   unsigned data_mask = createMask(9,13);
   unsigned src_mask  = createMask(14,17);

   seq_num = data_id = src_id = 0;
   recvbytes = read(_fd, &frame_rd, sizeof(struct can_frame));
   if(recvbytes)
   {
      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  ",
         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 source ID
      //
      switch(src_id)
      {
        case BoostConID:
          handle_boost_msg(frame_rd);
          break;

        case PneumConID:
          break;

        default:
          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)
      printf("Big enough\n");
   else
      printf("Insufficient buffer\n");

   return 0;
}

// Extract Boost Controller data from a CANBus frame
//
int CanBus::handle_boost_msg(struct can_frame cf)
{
   int return_val = 1;
   unsigned seq_mask  = createMask(0,8);
   unsigned data_mask = createMask(9,13);
   unsigned src_mask  = createMask(14,17);

   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 != BoostConID)
      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_boost_record(&_bc_record);
      _bc_record.seq_num = seq_num;
      return_val = 0;         // Got a complete record
   }

   switch(data_id)
   {
      case 0:
         _bc_record.time = *(float*)&cf.data[0];
         _bc_record.voltage = *(float*)&cf.data[4];

         printf("time:%f  volt:%f\n",
          _bc_record.time, _bc_record.voltage);
         break;

      case 1:
         _bc_record.p1dc = *(short*)&cf.data[0];
         _bc_record.pmeas = *(short*)&cf.data[2];
         _bc_record.mindbar = *(short*)&cf.data[4];
         _bc_record.lag = *(short*)&cf.data[6];

         printf("P1DC:%d   pmeas:%d   minDBar:%d   lag:%d\n",
          _bc_record.p1dc, _bc_record.pmeas, _bc_record.mindbar, _bc_record.lag);
         break;

      case 2:
         _bc_record.itemp1 = *(short*)&cf.data[0];
         _bc_record.itemp2 = *(short*)&cf.data[2];
         _bc_record.itemp3 = *(short*)&cf.data[4];
         _bc_record.itemp4 = *(short*)&cf.data[6];

         printf("Temp1:%d   Temp2:%d   Temp3:%d   Temp4:%d\n",
          _bc_record.p1dc, _bc_record.pmeas, _bc_record.mindbar, _bc_record.lag);
         break;

      case 3:
         _bc_record.vtarget = *(float*)&cf.data[0];
         _bc_record.ctarget = *(float*)&cf.data[4];

         printf("vtarget:%f  ctarget:%f\n",
          _bc_record.vtarget, _bc_record.ctarget);
         break;

      case 4:
         _bc_record.ibridge = *(float*)&cf.data[0];
         _bc_record.iload = *(float*)&cf.data[4];

         printf("IBridge:%f  ILoad:%f\n",
          _bc_record.ibridge, _bc_record.iload);
         break;

      case 5:
         _bc_record.bridgedc = *(float*)&cf.data[0];
         _bc_record.loaddc = *(float*)&cf.data[4];

         printf("BridgeDC:%f  LoadDC:%f\n",
          _bc_record.bridgedc, _bc_record.loaddc);
         break;

      case 6:
         _bc_record.rpms = *(float*)&cf.data[0];
         _bc_record.torque = *(float*)&cf.data[4];

         printf("RPMs:%f  Torque:%f\n",
          _bc_record.rpms, _bc_record.torque);
         break;

      default:
         break;
   }

   return return_val;
}

// Extract Pneumatic Spring Controller data from a CANBus frame
//
int CanBus::handle_spring_msg(struct can_frame cf)
{
   int return_val = 1;
   unsigned seq_mask  = createMask(0,8);
   unsigned data_mask = createMask(9,13);
   unsigned src_mask  = createMask(14,17);

   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 != PneumConID)
      return -1;

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

   switch(data_id)
   {
      case 0:
         break;

      case 1:
         break;

      case 2:
         break;

      case 3:
         break;

      case 4:
         break;

      case 5:
         break;

      case 6:
         break;

      default:
         break;
   }

   return return_val;
}