/****************************************************************************/
/* Copyright (c) 2015 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : Power Buoy logger app entry point.                            */
/* Filename : pblog.cpp                                                     */
/* Author   : Henthorn                                                      */
/* Project  : Power Buoy                                                    */
/* Version  : 1.0                                                           */
/* Created  : 04/13/2015                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*                                                                          */
/* (1) Creates new data directory and data file.                            */
/* (2) Sets up the Logger and the data sources.                             */
/* (3) Kicks-off a continous loop that:                                     */
/*     (a) Selects on the file sources to see if any data is pending        */
/*     (b) Calls read_data() on the file sources with data pending          */
/*     (c) After 60 minutes, closes the current data file and opens another */
/*                                                                          */
/****************************************************************************/

#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>

#include <nanomsg/nn.h>
#include <nanomsg/reqrep.h>

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

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

// The url used for nanomsg IPc comms
// 
#define  NN_URL   "ipc:///tmp/pbcon.ipc"

static CSVWriter *w;

enum Modes {RANGE_CONTROL, DEPTH_CONTROL, POWER_CONTROL};


// Structure contains the options:
//   verbose :    Be chatty?
//
struct LogOptions
{
   char    verbose;
};

static char _verbose;

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

void usage()
{
   printf("usage:\n");
   printf(" pbcon [-v ]\n");
   printf("   -v        Verbose     - be chatty\n");
}

void cleanup(int sig)
{
   if (w) delete w;
   exit(0);
}

///////////////////////////////////////////////////////////////////////////////
//
int main(int argc, char **argv)
{
   w = NULL;

   unsigned int deci = 0;

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

   struct LogOptions options;

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

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

   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


   int cmd_sock = nn_socket(AF_SP, NN_REP);
   int       rv = nn_bind(cmd_sock, NN_URL);
   printf("NN_REP cmd_sock = %d  %d\n", cmd_sock, rv);


   ////////////////////////////////////////////////////////////////////////////
   ////////////////////////////  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.);
   while(1)
   {
      Modes mode = RANGE_CONTROL;
      CanBus::SC_Record   sc_rec;
      Crossbow::XB_Record xb_rec;
      double pid_cmd = 0., range_target = 40., depth_target = 30.;

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

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

      if ( (fd = xbow.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))
         {
            if (can.read_data(_verbose) == (0 - (SpringConID)))
            {
               // New Spring controller data - trigger control process
               //
               can.get_record(&sc_rec);
               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)\n", argv[0], argv[1]);
               set_rpm(argc, argv);

               double xh0, xh1;
               range_estimator.GetxHat(&xh0, &xh1);
               //if (0 == deci++%10) 
                //      printf("New spring controller record: seq=%d range=%.2f "
                 //     "upsi=%.2f lpsi=%.2f z=%.3f cmd=%.3f error=%.3f xHat(0)=%.3f xHat(1)=%.3f\n",
                       
            }
         }

         // 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(_verbose))
            {
               // New Crossbow data - trigger process
               //
               xbow.get_record(&xb_rec);
            }
         }

      }

      // 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)
      {
         fprintf(stderr, "recvd request %s\n", buf);
         nn_freemsg(buf);
         nn_send(cmd_sock, "OK", 3, 0);
      }

   }

   return OK;
}


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

///////////////////////////////////////////////////////////////////////////////
//
// Parse command line, populate the options struct
//
int parse_options(int argc, char** argv, LogOptions *opt)
{
   opt->verbose = false;

   int o, nopts = 0;
   while ( (o = getopt(argc, argv, "v")) != -1)
   {
      switch(o)
      {
         case 'v':
            opt->verbose = true;
            nopts++;
            break;

         default:
            return -1;
      }
   }

   // If there is one more argument than options, use it as
   // the base directory
   //
   return 0;
}