/****************************************************************************/
/* Copyright (c) 2015 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : Implementation of the CANBus power buoy controller commands.  */
/* Filename : cmd_code.c                                                    */
/* Author   : Henthorn                                                      */
/* Project  : Power Buoy                                                    */
/* Version  : 1.0                                                           */
/* Created  : 08/10/2015                                                    */
/* Modified : 10/25/2016   Added Rudder Controller commands                 */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*                                                                          */

#include <math.h>
#include "pbcmd.h"

// 
//                                       ID   CMD    SEQ #
#define BC_ID 0x80001400;     // binary 0000 01010 000000000
#define SC_ID 0x80005400;     // binary 0001 01010 000000000
#define PC_ID 0x80009400;     // binary 0010 01010 000000000
#define RC_ID 0x8000D400;     // binary 0011 01010 000000000
#define RESET 255

int fire(struct can_frame *f)
{
   short param;

   // Send the command (fire-and-forget)
   //
   // Some debug code
   // Display Power Buoy codes in the can_id
   //
#if 1
   unsigned int cmdid = 0, ctlid = 0;
   unsigned char op = f->data[0];
   unsigned char p1 = f->data[1];
   unsigned char p2 = f->data[2];
   unsigned short *p3 = (unsigned short*)&f->data[1];

   cmdid = f->can_id & 0x3E00;
   cmdid >>= 9;
   ctlid = f->can_id & 0x3C000;
   ctlid >>= 14;

   printf("CTLR  CMD   OP    PARAM\n");
   printf("====  ====  ====  =======\n");
   printf("%-4d  %-4d  %-4d  ", ctlid, cmdid, op);
   //if (f->can_dlc > 1) printf("%-#2x ", p1);
   //if (f->can_dlc > 2) printf("%-#2x ", p2);
   if (f->can_dlc > 1) printf("%-7d", *p3);
   printf("\n");
#endif
   if (OK != (param = send_frame(f)) )
   {
      perror("CAN comms failure");
      return param;
   }

   return OK;
}

/******************************************************************/
/* bender command:   */
/******************************************************************/
int bender_usage()
{
   printf("usage:\n"
          " bender [off | on | auto]\n");

   return 0;
}

int bender(int argc, char** argv)
{
   unsigned short param;

   if (argc < 2 || argc > 2)
      return bender_usage();

   if (!strcmp("on", argv[1]))
      param = 1;
   else if (!strcmp("off", argv[1]))
      param = 0;
   else if (!strcmp("auto", argv[1]))
      param = 2;
   else
      return bender_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = BC_ID; 
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 1;         // cmd byte 1=>bender
   frame.data[1] = param;     // parameter

   printf("bender: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}


/******************************************************************/
/* reset_bc command:   */
/******************************************************************/
int reset_bc_usage()
{
   printf("usage:\n"
          " reset_bc\n");

   return 0;
}

int reset_bc(int argc, char** argv)
{
   if (argc > 1)
      return reset_bc_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = BC_ID; 
   frame.can_dlc = 1;        // 2 bytes
   frame.data[0] = RESET;    // cmd byte 255=>reset

   printf("reset_bc: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}


/******************************************************************/
/* valve command:   */
/******************************************************************/
int pump_usage()
{
   printf("usage:\n"
          " pump [off | on]\n");

   return 0;
}

int pump(int argc, char** argv)
{
   unsigned short param = 0;

   if (argc < 2 || argc > 2)
      return pump_usage();

   if (!strcmp("on", argv[1]))
      param = 1;
   else if (!strcmp("off", argv[1]))
      param = 0;
   else
      return pump_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = SC_ID;     // binary 00 01010 000000000
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 11;        // cmd byte 11=>pump
   frame.data[1] = param;     // parameter

   printf("pump: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}


/******************************************************************/
/* valve command:   */
/******************************************************************/
int valve_usage()
{
   printf("usage:\n"
          " valve [off | 1..255 seconds]\n");

   return 0;
}

int valve(int argc, char** argv)
{
   unsigned short param = 0;

   if (argc < 2 || argc > 2)
      return valve_usage();

   if (!strcmp("off", argv[1]))
      param = 0;
   else if ( (param = atoi(argv[1])) == 0 || param > 255)
      return valve_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = SC_ID; 
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 12;        // cmd byte 12=>valve
   frame.data[1] = param;     // parameter

   printf("valve: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}


/******************************************************************/
/* reset_sc command:   */
/******************************************************************/
int reset_sc_usage()
{
   printf("usage:\n"
          " reset_sc\n");

   return 0;
}

int reset_sc(int argc, char** argv)
{
   if (argc > 1)
      return reset_sc_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = SC_ID;     // binary 00 01010 000000000
   frame.can_dlc = 1;         // 1 bytes
   frame.data[0] = RESET;     // cmd byte 255=>reset

   printf("reset_sc: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}


/******************************************************************/
/* scale command:   */
/******************************************************************/
int scale_usage()
{
   printf("usage:\n"
          " scale [0.4,0.5 .. 1.2]\n");

   return 0;
}

int scale(int argc, char** argv)
{
   unsigned char param;
   float f = -1.;

   if (argc < 2 || argc > 2)
      return scale_usage();

   if ( (f = atof(argv[1])) < 0.4 || f > 1.201)
      return scale_usage();

   f += 0.01;
   param = (unsigned char)(f*100.);

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc =  2;         // 2 bytes
   frame.data[0] = 21;         // cmd byte 21=>set scale
   frame.data[1] = param;

   printf("scale: Sending val of %d...\n", param);
   return fire(&frame);
}


/******************************************************************/
/* retract command:   */
/******************************************************************/
int retract_usage()
{
   printf("usage:\n"
          " retract [0.4,0.5 .. 1.0]\n");

   return 0;
}

int retract(int argc, char** argv)
{
   unsigned char param;
   float f = -1.;

   if (argc < 2 || argc > 2)
      return retract_usage();

   if ( (f = atof(argv[1])) < 0.4 || f > 1.001)
      return retract_usage();

   f += 0.01;
   param = (unsigned char)(f*100.);

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 22;        // cmd byte 22=>set retract
   frame.data[1] = param;

   printf("retract: Sending param of %d...\n", param);
   return fire(&frame);
}


/******************************************************************/
/* vmax command:   */
/******************************************************************/
int vmax_usage()
{
   printf("usage:\n"
          " vmax [0.0 .. 655.35]\n");

   return 0;
}

int vmax(int argc, char** argv)
{
   unsigned short param;
   float f = -1.;

   if (argc < 2 || argc > 2)
      return vmax_usage();

   if ( (f = atof(argv[1])) < 0.0 || f > 655.35)
      return vmax_usage();

   param = (int)(f*100.);

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 23;        // cmd byte 23=>set vmax
   unsigned short *p = (unsigned short*)&frame.data[1];
   *p = param;

   printf("vmax: Sending command of %d...\n", *p);
   return fire(&frame);
}


/******************************************************************/
/* imax command:   */
/******************************************************************/
int imax_usage()
{
   printf("usage:\n"
          " imax [0.0 .. 131.068]\n");

   return 0;
}

int imax(int argc, char** argv)
{
   unsigned short param;
   float f = -1.;

   if (argc < 2 || argc > 2)
      return imax_usage();

   if ( (f = atof(argv[1])) < 0.0 || f > 131.068)
      return imax_usage();

   param = (int)(f*500.);

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 24;        // cmd byte 24=>set imax
   unsigned short *p = (unsigned short*)&frame.data[1];
   *p = param;

   printf("imax: Sending command of %d...\n", *p);
   return fire(&frame);
}


/******************************************************************/
/* bus command:                                                   */
/******************************************************************/
int bus_usage()
{
   printf("usage:\n"
          " bus [off | on | auto]\n");

   return 0;
}

int bus(int argc, char** argv)
{
   unsigned short param;

   if (argc < 2 || argc > 2)
      return bus_usage();

   if (!strcmp("on", argv[1]))
      param = 1;
   else if (!strcmp("auto", argv[1]))
      param = 2;
   else if (!strcmp("off", argv[1]))
      param = 0;
   else
      return bus_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 25;        // cmd byte 25=>bus
   frame.data[1] = param;     // parameter

   printf("bus: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}

/******************************************************************/
/* scm command:                                                   */
/******************************************************************/
int scm_usage()
{
   printf("usage:\n"
          " stopchargemode [off | on]\n");

   return 0;
}

int scm(int argc, char** argv)
{
   unsigned short param;

   if (argc < 2 || argc > 2)
      return scm_usage();

   if (!strcmp("on", argv[1]))
      param = 1;
   else if (!strcmp("off", argv[1]))
      param = 0;
   else
      return scm_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 26;        // cmd byte 25=>stopchargemode
   frame.data[1] = param;     // parameter

   printf("stopchargemode: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}

/******************************************************************/
/* gain command:   */
/******************************************************************/
int gain_usage()
{
   printf("usage:\n"
          " gain (0 - 65535)\n"
          " A 0 value turns the Gain Schedule Mode Off\n");

   return 0;
}

int gain(int argc, char** argv)
{
   unsigned short param;
   float f = -1.;

   if (argc < 2 || argc > 2)
      return gain_usage();

   if ( (param = atoi(argv[1])) < 0 || atoi(argv[1]) > 65535)
      return target_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 3;         // 2 bytes
   frame.data[0] = 27;        // cmd byte 27->gain
   unsigned short *p = (unsigned short*)&frame.data[1];
   *p = param;

   printf("gain: Sending command of %d...\n", *p);
   return fire(&frame);
}

/******************************************************************/
/* target command:   */
/******************************************************************/
int target_usage()
{
   printf("usage:\n"
          " target (0 - 65535)\n");

   return 0;
}

int target(int argc, char** argv)
{
   unsigned short param;

   if (argc < 2 || argc > 2)
      return target_usage();

   if ( (param = atoi(argv[1])) < 0 || atoi(argv[1]) > 65535)
      return target_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 3;         // 2 bytes
   frame.data[0] = 28;        // cmd byte 28->target
   unsigned short *p = (unsigned short*)&frame.data[1];
   *p = param;

   printf("target: Sending command of %d...\n", *p);
   return fire(&frame);
}

/******************************************************************/
/* reset_pc command:   */
/******************************************************************/
int reset_pc_usage()
{
   printf("usage:\n"
          " reset_pc\n");

   return 0;
}

int reset_pc(int argc, char** argv)
{
   if (argc > 1)
      return reset_pc_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 1;         // 1 byte
   frame.data[0] = RESET;     // cmd byte 255=>reset

   printf("reset_pc: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}

/******************************************************************/
/* rc mode command:   */
/******************************************************************/
int rcmode_usage()
{
   printf("usage:\n"
          " rcmode (heading | rudder)\n");

   return 0;
}

int   rcmode(int argc, char** argv)
{
   short param;

   if (argc < 2 || argc > 2)
      return rcmode_usage();

   // Heading mode: param == 0
   // Rudder mode : param == 1
   // 
   if (!strcasecmp(argv[1], "heading"))
      param = 0;
   else if (!strcasecmp(argv[1], "rudder"))
      param = 1;
   else
      return rcmode_usage();
   
   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 31;        // cmd byte 31->rcmode
   frame.data[1] = (char)param;

   printf("rcmode: Sending command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* rc position command:   */
/******************************************************************/
int rcpos_usage()
{
   printf("usage:\n"
          " rcpos (-1.0 .. 1.0)\n");

   return 0;
}

int    rcpos(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return rcpos_usage();

   if ( (f = atof(argv[1])) < -1.0 || f > 1.0)
      return rcpos_usage();

   param = (int)roundf(f * 32767);
   if (param > 32767) param = 32767;
   else if (param < -32768) param = -32768;

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 32;     // cmd byte 32=>rcpos
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rcpos: Sending command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* rc target heading command:   */
/******************************************************************/
int rctarget_usage()
{
   printf("usage:\n"
          " rctarget (0.0 .. 359.5)\n");

   return 0;
}

int rctarget(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return rctarget_usage();

   if ( (f = atof(argv[1])) < 0.0 || f > 359.5)
      return rctarget_usage();

   param = (int)roundf(f);
   if (param > 359) param = 359;
   else if (param < 0) param = 0;

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 33;        // cmd byte 33=>rctarget
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rctarget: Sending command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* rc default heading command:   */
/******************************************************************/
int rchdg_usage()
{
   printf("usage:\n"
          " rchdg (0.0 .. 359.5)\n");

   return 0;
}

int    rchdg(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return rchdg_usage();

   if ( (f = atof(argv[1])) < 0.0 || f > 359.5)
      return rchdg_usage();

   param = (int)roundf(f);
   if (param > 359) param = 359;
   else if (param < 0) param = 0;

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 34;        // cmd byte 34=>rchdg
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rchdg: Sending command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* rc heading filter command:   */
/******************************************************************/
int rcfilter_usage()
{
   printf("usage:\n"
          " rcfilter (0..1000)\n");

   return 0;
}

int rcfilter(int argc, char** argv)
{
   short param;

   if (argc < 2 || argc > 2)
      return rcfilter_usage();

   if ( (param = atoi(argv[1])) < 0 || param > 1000)
      return rcfilter_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 35;        // cmd byte 35=>rcfilter
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rcfilter: Sending command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* rc period command:   */
/******************************************************************/
int rcperiod_usage()
{
   printf("usage:\n"
          " rcperiod (0..100)\n");

   return 0;
}

int rcperiod(int argc, char** argv)
{
   short param;

   if (argc < 2 || argc > 2)
      return rcperiod_usage();

   if ( (param = atoi(argv[1])) < 0 || param > 100)
      return rcperiod_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 36;        // cmd byte 36=>rcperiod
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rcperiod: Sending command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* rc prop gain command:   */
/******************************************************************/
int rcpgain_usage()
{
   printf("usage:\n"
          " rcpgain (0.0 .. 1.0)\n");

   return 0;
}

int  rcpgain(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return rcpgain_usage();

   if ( (f = atof(argv[1])) < 0.0 || f > 1.0)
      return rcpgain_usage();

   param = (int)roundf(f*32767.);
   if (param > 32767) param = 32767;
   else if (param < 0) param = 0;

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 37;        // cmd byte 37=>rcpgain
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rcpgain: Sending command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* rc int gain command:   */
/******************************************************************/
int rcigain_usage()
{
   printf("usage:\n"
          " rcigain (0.0 .. 1.0)\n");

   return 0;
}

int  rcigain(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return rcigain_usage();

   if ( (f = atof(argv[1])) < 0.0 || f > 1.0)
      return rcigain_usage();

   param = (int)roundf(f*32767.);
   if (param > 32767) param = 32767;
   else if (param < 0) param = 0;

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 38;        // cmd byte 38=>rcigain
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rcigain: Sending command of %d...\n", param);
   return fire(&frame);
}

int  rcreset(int argc, char** argv)
{
   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 1;         // 3 bytes
   frame.data[0] = RESET;     // cmd byte 255=>rcreset

   printf("reset_rudder: Sending command...\n");
   return fire(&frame);
}