/****************************************************************************/
/* 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 <assert.h>
#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)
{
    memset(_pc_record, 0, sizeof(_pc_record));
    _pc_record[0].seq_num = _pc_record[1].seq_num = -1;
    memset(_bc_record, 0, sizeof(_bc_record));
    _bc_record[0].seq_num = _bc_record[1].seq_num = -1;
    memset(_sc_record, 0, sizeof(_sc_record));
    _sc_record[0].seq_num = _sc_record[1].seq_num = -1;
    memset(_rc_record, 0, sizeof(_rc_record));
    _rc_record[0].seq_num = _rc_record[1].seq_num = -1;

    memset(_tf_record, 0, sizeof(_tf_record));
    _tf_record[0].seq_num = _tf_record[1].seq_num = -1;

   // Initialize the pointers into the dual buffers
   // 
   _p = _b = _s = _r = _t= 0;

   _cansock = strdup(cansock);

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

}

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

void CanBus::decode_header(struct can_frame *cf)
{
   _src_id = _data_id = _seq_num = 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;
}

// 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
//
static float can_time;

int CanBus::read_data(char verbose)
{
   int val = 0;    // Return value
   _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");

#if 0
      // Grab the receive time of of the can message
      // 
      struct timeval tv;
      ioctl(_fd, SIOCGSTAMP, &tv);
      int err = can_time = float(tv.tv_sec); // + float(tv.tv_usec)/1000000.0;
      if (err) perror("ioctl:");
      printf("CAN time was: %.2f\n"); fflush(stdout);
#endif
      
      // Process message data depending on the controller ID
      //
      switch(src_id)
      {
        case PowerConID:
          val = handle_power_msg(frame_rd);
          assert(_p < 2);
          break;

        case SpringConID:
          val = handle_spring_msg(frame_rd);
          assert(_s < 2);
          break;

        case BatteryConID:
          val = handle_battery_msg(frame_rd);
          assert(_b < 2);
          break;

        case RudderConID:
          val = handle_rudder_msg(frame_rd);
          assert(_r < 2);
          break;

        case TrefoilConID:
          val = handle_trefoil_msg(frame_rd);
          assert(_t < 2);
          break;

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

      }
   }

   return val;
}

// Define a bit of syntactic sugar
// 
#define  PCR  _pc_record[1-_p]
#define  SCR  _sc_record[1-_s]
#define  BCR  _bc_record[1-_b]
#define  RCR  _rc_record[1-_r]
#define  TCR  _tf_record[1-_t]


// Extract Power Controller data from a CANBus frame
//
int CanBus::handle_power_msg(struct can_frame cf)
{
   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 0;            // Error condition

   // A change in sequence number means a new set of data
   // so update the logger and toggle the buffer offset
   //
   int return_val = 0;
   if (PCR.seq_num != seq_num)
   {
      if (_logger) _logger->update_power_record(&(PCR));
      _p = 1 - _p;

      PCR.seq_num = seq_num;
      return_val = PowerConID;         // Got a complete record
   }


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

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

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

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

         return_val = PC_0;
         break;

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

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

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

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

         return_val = PC_1;
         break;

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

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

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

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

         return_val = PC_2;
         break;

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

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

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

         return_val = PC_3;
         break;

      default:
         return_val = 0;
         break;
   }

   return return_val;
}

// Extract Spring Controller data from a CANBus frame
//
int CanBus::handle_spring_msg(struct can_frame cf)
{
   decode_header(&cf);
   unsigned seq_num = _seq_num, data_id = _data_id, src_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 0;            // Error condition

   // 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
   //
   int return_val = 0;
   if (SCR.seq_num != seq_num)
   {
      if (_logger) _logger->update_spring_record(&(SCR));
      _s = 1 - _s;

      SCR.seq_num = seq_num;
      return_val = SpringConID;                 // Got a complete record
   }

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

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

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

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

         return_val = SC_0;
         break;

      case 1:
         ultemp = *(long*)&cf.data[0];
         SCR.epoch = ultemp;
         if (_verbose) printf("seq:%d  raw:%ld  epoch:%ld sec\n", seq_num, ultemp,
                SCR.epoch);

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

         return_val = SC_1;
         break;

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

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

         return_val = SC_2;
         break;

      default:
         return_val = 0;
         break;
   }

   return return_val;
}



// Extract Battery Controller data from a CANBus frame
//
int CanBus::handle_battery_msg(struct can_frame cf)
{
   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 0;           // Error condition

   // A change in sequence number means a new set of data
   // so update the logger and toggle the buffer offset
   //
   int return_val = 0;
   if (BCR.seq_num != seq_num)
   {
      if (_logger) _logger->update_battery_record(&(BCR));
      _b = 1 - _b;

      BCR.seq_num = seq_num;
      return_val = BatteryConID;         // 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];
         BCR.voltage = (float)utemp / 100.;
         if (_verbose) printf("seq:%d  raw:%d  voltage:%.2f V\n", seq_num, utemp, BCR.voltage);

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

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

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

         return_val = BC_0;
         break;

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

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

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

         return_val = BC_1;
         break;

      default:
         return_val = 0;
         break;
   }

   return return_val;
}

int CanBus::handle_trefoil_msg(struct can_frame cf)
{
   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 != TrefoilConID)
      return 0;            // Error condition

   // A change in sequence number means a new set of data
   // so update the logger and toggle the buffer offset
   //
   int return_val = 0;
   if (TCR.seq_num != seq_num)
   {
      if (_logger) _logger->update_trefoil_record(&(TCR));
      _t = 1 - _t;

      TCR.seq_num = seq_num;
      return_val = TrefoilConID;         // Got a complete record
   }

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

         temp = *(short*)&cf.data[2];
         TCR.tvoltage = (float)temp / 100.;
         if (_verbose) printf("seq:%d  raw:%d  tV:%.2f V\n", seq_num, temp, TCR.tvoltage);

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

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

         return_val = TF_0;
         break;

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

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

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

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

         return_val = TF_1;
         break;

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

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

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

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

         return_val = TF_2;
         break;

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

         temp = *(short*)&cf.data[2];
         TCR.ang_rate[1] = (float)temp * 1000.;
         if (_verbose) printf("seq:%d  raw:%d ang_rate[1]:%.2f rad/sec\n", seq_num, temp, TCR.ang_rate[1]);

         temp = *(short*)&cf.data[4];
         TCR.ang_rate[2] = (float)temp * 1000.;
         if (_verbose) printf("seq:%d  raw:%d ang_rate[2]:%.2f rad/sec\n", seq_num, temp, TCR.ang_rate[2]);

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

         return_val = TF_3;
         break;

      case 4:
         temp = *(short*)&cf.data[0];
         TCR.accel[0] = (float)temp;
         if (_verbose) printf("seq:%d  raw:%d accel[0]:%.2f m/sec^2\n", seq_num, temp, TCR.accel[0]);

         temp = *(short*)&cf.data[2];
         TCR.accel[1] = (float)temp;
         if (_verbose) printf("seq:%d  raw:%d accel[1]:%.2f m/sec^2\n", seq_num, temp, TCR.accel[1]);

         temp = *(short*)&cf.data[4];
         TCR.accel[2] = (float)temp;
         if (_verbose) printf("seq:%d  raw:%d accel[2]:%.2f m/sec^2\n", seq_num, temp, TCR.accel[2]);

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

         return_val = TF_4;
         break;

      case 5:
         utemp = *(unsigned short*)&cf.data[0];
         TCR.mstatus = utemp;
         if (_verbose) printf("seq:%d  raw:%d  maxon stat:%d\n", seq_num, temp, TCR.mstatus);

         temp = *(short*)&cf.data[2];
         TCR.mcurr = temp;
         if (_verbose) printf("seq:%d  raw:%d  mcurr:%.3f A\n", seq_num, temp, TCR.spare2);

         ltemp = *(long*)&cf.data[4];
         TCR.encoder = ltemp;
         if (_verbose) printf("seq:%d  raw:%d  encoder:%ld\n", seq_num, temp, TCR.encoder);

         return_val = TF_5;
         break;

      default:
         return_val = 0;
         break;
   }

   if (src_id != TrefoilConID)
      return 0;            // Error condition

   return return_val;
}

int CanBus::handle_rudder_msg(struct can_frame cf)
{
   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 0;            // Error condition

   // 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
   //
   int return_val = 0;
   if (RCR.seq_num != seq_num)
   {
      if (_logger) _logger->update_rudder_record(&(RCR));
      _r = 1 - _r;

      RCR.seq_num = seq_num;
      return_val = RudderConID;               // Got a complete record
   }

   unsigned short utemp;
   short temp;
   unsigned long ultemp;
   char debug = 0;

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

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

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

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

         return_val = RC_0;
         break;

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

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

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

         RCR.b_cor1 = cf.data[3];
         if (debug || _verbose) printf("seq:%d  raw:%d  b_cor1:%d %%\n", seq_num, cf.data[3], RCR.b_cor1);

         RCR.b_cor2 = cf.data[4];
         if (debug || _verbose) printf("seq:%d  raw:%d  b_cor2:%d %%\n", seq_num, cf.data[4], RCR.b_cor2);

         RCR.b_cor3 = cf.data[5];
         if (debug || _verbose) printf("seq:%d  raw:%d  b_cor3:%d %%\n", seq_num, cf.data[5], RCR.b_cor3);

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

         return_val = RC_1;
         break;

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

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

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

         temp = *(short*)&cf.data[6];
         RCR.ram = (float)temp/100.;//32767.;
         if (debug || _verbose) printf("seq:%d  raw:%d  ram position:%.1f%%\n", seq_num, temp, RCR.ram);

         return_val = RC_2;
         break;

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

         return_val = RC_3;
         break;

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

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

         return_val = RC_4;
         break;

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

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

         return_val = RC_5;
         break;

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

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

         return_val = RC_6;
         break;

      // PID gains
      // 
      case 7:
         temp = *(short*)&cf.data[0];
         RCR.p_gain = (float)temp/32798.0;
         if (debug || _verbose) printf("seq:%d  raw:%d  pgain:%.2f \n", seq_num, temp, RCR.p_gain);

         temp = *(short*)&cf.data[2];
         RCR.i_gain = (float)temp/32798.0;
         if (debug || _verbose) printf("seq:%d  raw:%d  igain:%.2f \n", seq_num, temp, RCR.i_gain);

         utemp = *(unsigned short*)&cf.data[4];
         RCR.period = (float)temp/10.0;
         if (debug || _verbose) printf("seq:%d  raw:%d  period:%.2f \n", seq_num, temp, RCR.period);

         utemp = *(unsigned short*)&cf.data[5];
         RCR.period = temp;
         if (debug || _verbose) printf("seq:%d  raw:%d  ftc:%.2f \n", seq_num, temp, RCR.ftc);

         temp = *(short*)&cf.data[6];
         RCR.d_gain = (float)temp/32798.0;
         if (debug || _verbose) printf("seq:%d  raw:%d  dgain:%.2f \n", seq_num, temp, RCR.d_gain);

         return_val = RC_7;
         break;

      default:
         return_val = 0;
         break;
   }

   return return_val;
}

char CanBus::get_record(PC_Record *r, SourceID id)
{
   if (!r) return 0;

   // Return the published record
   // 
   if (id == PowerConID)
   {
      memcpy(r, &_pc_record[_p], sizeof(PC_Record));
      return id;
   }
   // Return the working copy record
   // 
   else if (id >= PC_0 && id <= PC_3)
   {
      memcpy(r, &_pc_record[1 - _p], sizeof(PC_Record));
      return id;
   }
   else
      return 0;
}

char CanBus::get_record(BC_Record *r, SourceID id)
{
   if (!r) return 0;

   // Return the published record
   // 
   if (id == BatteryConID)
   {
      memcpy(r, &_bc_record[_b], sizeof(BC_Record));
      return id;
   }
   // Return the working copy record
   // 
   else if (id >= BC_0 && id <= BC_1)
   {
      memcpy(r, &_bc_record[1 - _b], sizeof(BC_Record));
      return id;
   }
   else
      return 0;

}

char CanBus::get_record(SC_Record *r, SourceID id)
{
   if (!r) return 0;

   // Return the published record
   // 
   if (id == SpringConID)
   {
      memcpy(r, &_sc_record[_s], sizeof(SC_Record));
      return id;
   }
   // Return the working copy record
   // 
   else if (id >= SC_0 && id <= SC_2)
   {
      memcpy(r, &_sc_record[1 - _s], sizeof(SC_Record));
      return id;
   }
   else
      return 0;

}

char CanBus::get_record(RC_Record *r, SourceID id)
{
   if (!r) return 0;

   // Return the published record
   // 
   if (id == RudderConID)
   {
      memcpy(r, &_rc_record[_r], sizeof(RC_Record));
      return id;
   }
   // Return the working copy record
   // 
   else if (id >= RC_0 && id <= RC_7)
   {
      memcpy(r, &_rc_record[1 - _r], sizeof(RC_Record));
      return id;
   }
   else
      return 0;
}

char CanBus::get_record(TF_Record *t, SourceID id)
{
   if (!t) return 0;

   // Return the published record
   // 
   if (id == TrefoilConID)
   {
      memcpy(t, &_tf_record[_t], sizeof(TF_Record));
      return id;
   }
   // Return the working copy record
   // 
   else if (id >= TF_0 && id <= TF_5)
   {
      memcpy(t, &_tf_record[1 - _t], sizeof(TF_Record));
      return id;
   }
   else
      return 0;
}