/****************************************************************************/
/* Copyright (c) 2018 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : Power Buoy controller app entry point.                        */
/* Filename : pblog.cpp                                                     */
/* Author   : Henthorn                                                      */
/* Project  : Power Buoy                                                    */
/* Version  : 1.0                                                           */
/* Created  : 05/13/2018                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*                                                                          */
/*                                                                          */
/****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <unistd.h>
#include <string.h>
#include <time.h>
#include <fcntl.h>
#include <dirent.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>

#define USE_NN 1

#if USE_NN
#include <nanomsg/nn.h>
#include <nanomsg/reqrep.h>
// The url used for nanomsg IPC comms
// 
#define  NN_URL   "ipc:///tmp/pbcon.ipc"
#endif

#include "Crossbow.hpp"
#include "CanBus.hpp"
#include "RangeEstimator.hpp"
#include "PID.hpp"

// Making use of existing pbcmd commands
// 
extern "C"
{
#include "pbcmd.h"
}


#define  OK         0
#define  BAD_SOCK  -1      // Socket comms failed
#define  BAD_OPTS  -2      // Inconsistent option values
#define  DEFAULT_BASE_DIR  "/home/buoy/Logs"
#define  DEFAULT_INTERVAL  60*60
#define  DEFAULT_HIGH_LIMIT  70.0
#define  DEFAULT_LOW_LIMIT   50.0

#define  MODE_CMD  "mode "
#define  ACK       "ACK"
#define  NACK      "NACK"

#define  RAM_LIMIT  80.0
#define  HI_TARGET_REPS  3

#define HYDROGEN_LIMIT 10.0


enum Modes {MODE_INVALID, RANGE_CONTROL, DEPTH_CONTROL, POWER_CONTROL};

// Taken from Andy
// 
struct TF_StatusBits
{
   unsigned int ChargeMode         : 2; // offset = 0 
   unsigned int ChargeEnabled      : 1; // offset = 2 
   unsigned int MaxonPwrMode       : 2; // offset = 3 
   unsigned int MaxonPower         : 1; // offset = 5 
   unsigned int MaxonEnabled       : 1; // offset = 6 
   unsigned int DoorPositionTarget : 1; // offset = 7 
   unsigned int DoorPositionActual : 1; // offset = 8 
   unsigned int State              : 4; // offset = 9
   unsigned int Maxon_ComError     : 2; // offset = 13
};

typedef union
{
   unsigned int status;
   struct TF_StatusBits bits; 
} TF_Status;


// Structure contains the options:
//   verbose :    Be chatty?
//
struct ConOptions
{
   char     verbose;
   char     execute;   // When true, execute the set_rpm command
   char     watchdog;  // When true, feed watchdog every 60-70 seconds
   char     open_only; // Open-only: never send a command to close the doors
   Modes    mode;      // Controller mode
   double   hi_limit;  // Ram range hi limit
   double   lo_limit;  // Ram range lo limit
   unsigned hyst_time; // Required time SC range must be below low point 
};

int parse_options(int argc, char **argv);

// Returns Mode according to the string (range, depth, power)
// Returns the Mode value if successful. < 0 if invalid
Modes get_mode(const char* mode_str);

void usage()
{
   printf("usage:\n");
   printf(" pbcon [-v -x -m mode]\n");
   printf("   -v        Verbose        - Be chatty\n");
   printf("   -x        Execute        - Execute controller commands (default is NOT to execute, just report)\n");
   printf("   -w        Watchdog       - Feed the watchdog every minute or so (default: no watchdog\n");
   printf("   -o        Open-only      - Never send the command to close the doors (default is execute close)\n");
   printf("   -h        High limit     - Ram range (inches) at which to automatically open doors (%.1f)\n",
      DEFAULT_HIGH_LIMIT);
   printf("   -l        Low limit      - Ram range (inches) at which to automatically close doors (%.1f)\n",
      DEFAULT_LOW_LIMIT);
   printf("   -t        Low limit time - Time (seconds) range less than low point before closing doors (%d)\n",
      DEFAULT_INTERVAL);
}

void cleanup(int sig)
{
   exit(0);
}

static struct ConOptions _options = {false, false, false, false,
                                     RANGE_CONTROL, DEFAULT_HIGH_LIMIT,
                                     DEFAULT_LOW_LIMIT, DEFAULT_INTERVAL};
static int  _cmd_sock, _rv;

// Send command to open or close the heave cone doors.
// 0 == close doors, anything else means open.
// If the cmd is zero, the close command will be sent.
// Otherwise, open. We do not attempt to hold the state of
// the doors here, since a command to open/close can
// be sent outside of this app.
// 
char _opened = 0;
char _closed = 1;

void open_doors(int cmd)
{
   char *argv[2];
   char zc[10];
   argv[0] = (char*)"tf_SetPos";
   argv[1] = zc;
   int argc = 2;

   // Do not close doors if the open-only option is enabled.
   // Return now if the command is to close the doors.
   // 
   if (_options.open_only && cmd == 0) return;

   fprintf(stdout, "%s(%s) (%s really executing)\n",
      argv[0], argv[1], _options.execute ? "" : "Not");

   if (cmd != 0)
      sprintf(zc, "%d", 0);
   else
      sprintf(zc, "%d", 1);

   printf("%s %s\n", argv[0], argv[1]);
   
   if (_options.execute) tf_setpos(argc, argv);
   else {_opened = cmd; _closed = !_opened;}
   sleep(2);
}

///////////////////////////////////////////////////////////////////////////////
//
int main(int argc, char **argv)
{
   unsigned int deci = 0;

   signal(SIGINT, cleanup);
   signal(SIGQUIT, cleanup);
   signal(SIGKILL, cleanup);

   ////////////////////////////////////////////////////////////////////////////
   ///////////////////////  HIGH-LEVEL SETUP  /////////////////////////////////

   // Check cmdline options and optional base directory argument
   //
   if (parse_options(argc, argv) != 0)
   {
      usage();
      return -1;
   }

   char verbose = _options.verbose;
   int max_fd = 0, fd;

   ////////////////////////////////////////////////////////////////////////////
   ////////////////////////////  SETUP Readers ////////////////////////////////

   // Create the data readers, one for each data source.
   //
   Crossbow xbow(NULL);
   if ( (fd = xbow.get_fd()) < 0)
      perror("pblog: Failed to open Crossbow port: ");
   else
      if (fd > max_fd) max_fd = fd;     // Track maximum fd for FD_SET

   CanBus   can("can0", NULL);
   if ( (fd = can.get_fd()) < 0)
      perror("pblog: Failed to open CanBus port: ");
   else
      if (fd > max_fd) max_fd = fd;     // Track maximum fd for FD_SET

#if USE_NN
   _cmd_sock = nn_socket(AF_SP, NN_REP);
   _rv       = nn_bind(_cmd_sock, NN_URL);
   printf("NN_REP cmd_sock = %d  %d\n", _cmd_sock, _rv);
#endif


   ////////////////////////////////////////////////////////////////////////////
   ////////////////////////////  MAIN LOOP ////////////////////////////////////


   // loop (forever)
   //    if (there is data for the readers to read)
   //       let readers read new data
   //    endif
   // end loop
   //
   RangeEstimator range_estimator;
   float z;
   PID range_pid(100.0, 0., 0.);
   PID depth_pid(0.5, 0., 0.);

   double pid_cmd = 0.0, depth_target = 30.;
   double range_htarget = _options.hi_limit, range_ltarget = _options.lo_limit;

   time_t now, doors_closed = 0, doors_opened = 0, hyst_time = time_t(_options.hyst_time);
   unsigned int hi_count = 0, wd_count = 0;;

   // first, get the latest TF data with the status of the doors
   // 

   bool haveTF = false;
   time_t last_hydro_warning = 0;

   //   while (1)
   //{
   //   if (can.read_data(_options.verbose) == TF_2) break;
   //}
   //can.get_record(&tf_rec, TF_2);

   while(1)
   {
      Modes mode = RANGE_CONTROL;
      CanBus::SC_Record   sc_rec;
      CanBus::BC_Record   bc_rec;
      CanBus::PC_Record   pc_rec;
      CanBus::TF_Record   tf_rec;
      Crossbow::XB_Record xb_rec;

      struct timeval timeout = {0, 25000L};   // 25 ms period (40Hz)
      fd_set readset;
      FD_ZERO(&readset);

      char slackcmd[500];
      strcpy(slackcmd, "");

      // Specifiy the ports to select on
      //
      if ( (fd = can.get_fd()) > 0 )
         FD_SET(fd, &readset);

      // See if any ports have data waiting
      //
      if (select((max_fd + 1), &readset, NULL, NULL, &timeout) > 0)
      {
         // Get CanBus data
         // A return value < 0 signifies that a new data record has completed
         // 
         if (FD_ISSET(can.get_fd(), &readset))
         {
            // Feed the watchdog every so often.
            // Normally, the canbus delivers 1000 packets
            // in about 7 seconds. Let's wait for around a minute.
            // 
            if (++wd_count > 10000)
            {
               wd_count = 0;
               if (_options.watchdog)
               {
                  tf_watchdog(0, NULL);
                  time_t tt = time(NULL);
                  printf("Watchdog @ %s", ctime(&tt)); fflush(stdout);
               }
            }

#if 0
            // New Spring controller range data (SC_RANGE)? Trigger control process
            //
	    if (can.read_data(_options.verbose) == SC_RANGE)
            {
               can.get_record(&sc_rec, SC_RANGE);
               z = (float) range_estimator.RLS_Range((double) sc_rec.range,
                                                     (double) sc_rec.upsi,
                                                     (double) sc_rec.lpsi);
               
               pid_cmd = range_pid.CommandSignal(range_target, z);

               char *argv[2];
               char zc[10];
               sprintf(zc, "%.4f", (pid_cmd));
               argv[0] = (char*)"set_rpm";
               argv[1] = zc;
               int argc = 2;
               fprintf(stderr, "%s(%s) (%s really executing)\n",
                  argv[0], argv[1], _options.execute ? "" : "Not");
               if (_options.execute) set_rpm(argc, argv);

               double xh0, xh1;
               range_estimator.GetxHat(&xh0, &xh1);
            }
#endif
            int rec_type = can.read_data(_options.verbose);

            // Get latest trefoil record when available
            //
            if (rec_type == TF_2)
            {
               can.get_record(&tf_rec, TF_2);
               haveTF = true;
            }

            // If available, read new Battery controller hydrogen data to Trigger control process
            //
            else if (rec_type == BC_HYDROGEN)
            {
               can.get_record(&bc_rec, BC_HYDROGEN);
               if (bc_rec.hydrogen > HYDROGEN_LIMIT && (last_hydro_warning + 300) < time(0))
               {
                  last_hydro_warning = time(0);
                  sprintf(slackcmd, "echo \"%s %.1f > %.1f @ %s\" | slacktee.sh",
                                    "WARNING: Battery hydrogen limit exceeded!",
                  bc_rec.hydrogen, HYDROGEN_LIMIT, ctime(&last_hydro_warning));
                  printf("%s\n", slackcmd);
            }
         }
         // If available, read Spring controller range data to Trigger control process
         //
         else if (rec_type == SC_RANGE && haveTF)
         {
            can.get_record(&sc_rec, SC_RANGE);

            // What is the status of the doors?
            // 
            TF_Status tf_stat;
            tf_stat.status = tf_rec.status;
            char opened  = tf_stat.bits.DoorPositionActual == 0 &&
                           tf_stat.bits.DoorPositionTarget == 0;
            char opening = tf_stat.bits.DoorPositionActual == 1 &&
                           tf_stat.bits.DoorPositionTarget == 0;
            char closed  = tf_stat.bits.DoorPositionActual == 1 &&
                           tf_stat.bits.DoorPositionTarget == 1;
            char closing = tf_stat.bits.DoorPositionActual == 0 &&
                           tf_stat.bits.DoorPositionTarget == 1;

            // Used for testing, ignored when executing
            // 
            if (!_options.execute)
            {
               closed = _closed;
               opened = _opened;
            }

            // 
            if (closed && !opening && 
                  ( (sc_rec.range > range_htarget) &&
                    (sc_rec.range < RAM_LIMIT)
                  )
               )
            {
               hi_count++;
               // Open the doors only after a number of hits in a row.
               // Remember when the doors were opened.
               //
               if (hi_count > HI_TARGET_REPS)
               {
                  open_doors(1);
                  doors_closed = 0;
                  doors_opened = time(NULL);
                  fprintf(stdout, "Doors open on range value of %.2f @ %s",
                          sc_rec.range, ctime(&doors_opened));
                  fflush(stdout);
                  // range_htarget = 70.;  // dummy value used for testing
                  hi_count = 0;

                  sprintf(slackcmd, "echo \"%s %.2f @ %s\" | slacktee.sh",
                     "Doors open on range value of ", sc_rec.range, ctime(&doors_opened));
               }
            }
            else if (!_options.open_only &&
                    (sc_rec.range < range_ltarget) && opened)
            {
               // Close the doors if we're in normal range for
               // a fair amount of time (hyst_time)
               // 
               time_t now = time(NULL);
               if (hyst_time < (now - doors_opened))
               {
                  doors_opened = 0;
                  doors_closed = now;
                  fprintf(stdout, "Doors close on range value of %.2f @ %s",
                          sc_rec.range, ctime(&doors_closed));
                  fflush(stdout);
                  if (!_options.open_only) open_doors(0);
                  // range_htarget = 3.4; // dummy value used for testing
               }
               hi_count = 0;
            }
            else if ((sc_rec.range >= range_ltarget) && opened)
            {
               // Reset the lo limit timer countdown
               // 
               doors_opened = time(NULL);
               hi_count = 0;
            }

         }

      }

#if 0
      // Don't need Crossbow data in the controller yet.
      if ( (fd = xbow.get_fd()) > 0)
         FD_SET(fd, &readset);

         // Get Crossbow data
         // A return value < 0 signifies that a new data record has completed
         //
         if (FD_ISSET(xbow.get_fd(), &readset))
         {
            //fprintf(stderr, "reading xbow data\n");
            if (0 > xbow.read_data(options.verbose))
            {
               // New Crossbow data - trigger process
               //
               xbow.get_record(&xb_rec);
            }
         }

      }
#endif

#if USE_NN
      // Non-blocking check for commands. Execute and respond.
      //
      char *buf = NULL;
      int bytes = nn_recv(_cmd_sock, &buf, NN_MSG, NN_DONTWAIT);
      if (bytes > 0)
      {
         if (!strncasecmp(buf, MODE_CMD, strlen(MODE_CMD)))
         {
            fprintf(stderr, "pbcon: recvd mode request:%s\n", buf+strlen(MODE_CMD));
            Modes m = get_mode(buf+strlen(MODE_CMD));
            if (m != MODE_INVALID)
            {
               _options.mode = m;
               nn_send(_cmd_sock, ACK, strlen(ACK)+1, 0);
            }
         }
         else
         {
            fprintf(stderr, "pbcon: recvd unknown request %s\n", buf);
            nn_send(_cmd_sock, NACK, strlen(NACK)+1, 0);
         }

         nn_freemsg(buf);
      }
#endif

        // If there is a slack message, let it fly
        //
        if (strlen(slackcmd) > 0) system(slackcmd);
      }
   }
   return OK;
}


///////////////////////////////////////////////////////////////////////////////
///////////////////////  HELPER FUNCTIONS  ////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
//
// Parse command line, populate the options struct
//
int parse_options(int argc, char** argv)
{
   int o;
   Modes m;
   while ( (o = getopt(argc, argv, "vxowh:l:t:m:")) != -1)
   {
      switch(o)
      {
         case 'v':
            _options.verbose = true;
            break;

         case 'x':
            _options.execute = true;
            break;

         case 'w':
            _options.watchdog = true;
            break;

         case 'o':
            _options.open_only = true;
            break;

         case 'h':
            _options.hi_limit = atof(optarg);
            break;

         case 'l':
            _options.lo_limit = atof(optarg);
            break;

         case 't':
            _options.hyst_time = atoi(optarg);
            break;

         case 'm':
            m = get_mode(optarg);
            if (MODE_INVALID == m)
            {
               fprintf(stderr, "Unknown mode: %s\n", optarg);
               return -1;
            }
            else
               _options.mode = m;

            break;

         default:
            return -1;
      }
   }

   time_t now = time(NULL);
   printf("Starting @ %s", ctime(&now));
   if (_options.open_only)
   {
      _options.lo_limit = 0;
   }
   else if (_options.lo_limit >= _options.hi_limit)
   {
      printf("Inconsistent limits! (%.2f >= %.2f) - using defaults\n\n",
             _options.lo_limit, _options.hi_limit);
      usage();
      return -1;
   }

   printf("Execute       : %d...  \n", _options.execute);
   printf("Open-only     : %d...  \n", _options.open_only);
   printf("Watchdog      : %d...  \n", _options.watchdog);
   printf("High limit    : %.2f...\n", _options.hi_limit);
   printf("Low  limit    : %.2f...\n", _options.lo_limit);
   printf("Low limit time: %d...  \n", _options.hyst_time);
   fflush(stdout);
   return 0;
}

// Returns Mode according to the string (range, depth, power)
// Returns the Mode value if successful. < 0 if invalid
Modes get_mode(const char* mode_str)
{
   if (NULL == mode_str)
      return MODE_INVALID;

   if (!strcasecmp(mode_str, "range"))
      return RANGE_CONTROL;
   else if (!strcasecmp(mode_str, "depth"))
      return DEPTH_CONTROL;
   else if (!strcasecmp(mode_str, "power"))
      return POWER_CONTROL;
   else
      return MODE_INVALID;

}

