/*****************************************************************************
 * Copyright (c) 2002-2020 MBARI
 * Monterey Bay Aquarium Research Institute, all rights reserved.
 *****************************************************************************
 * @file    oa.cc
 * @authors r. henthorn
 * @date    11/23/2020
 * @brief   Main program for Obstacle Avoidance for LRAUV
 *
 * Project: LRAUV Obstacle Avoidance (oahu)
 * Summary: See @brief
 *****************************************************************************/
/*****************************************************************************
 * Copyright Information:
 * Copyright 2002-2020 MBARI
 * Monterey Bay Aquarium Research Institute, all rights reserved.
 *
 * Terms of Use:
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
 * Software Foundation; either version 3 of the License, or (at your option)
 * any later version. You can access the GPLv3 license at
 * http://www.gnu.org/licenses/gpl-3.0.html
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
 * more details (http://www.gnu.org/licenses/gpl-3.0.html).
 *
 * MBARI provides the documentation and software code "as is", with no
 * warranty, express or implied, as to the software, title, non-infringement
 * of third party rights, merchantability, or fitness for any particular
 * purpose, the accuracy of the code, or the performance or results which you
 * may obtain from its use. You assume the entire risk associated with use of
 * the code, and you agree to be responsible for the entire cost of repair or
 * servicing of the program with which you are using the code.
 *
 * In no event shall MBARI be liable for any damages,whether general, special,
 * incidental or consequential damages, arising out of your use of the
 * software, including, but not limited to,the loss or corruption of your data
 * or damages of any kind resulting from use of the software, any prohibited
 * use, or your inability to use the software. You agree to defend, indemnify
 * and hold harmless MBARI and its officers, directors, and employees against
 * any claim,loss,liability or expense,including attorneys' fees,resulting from
 * loss of or damage to property or the injury to or death of any person
 * arising out of the use of the software.
 *
 * The MBARI software is provided without obligation on the part of the
 * Monterey Bay Aquarium Research Institute to assist in its use, correction,
 * modification, or enhancement.
 *
 * MBARI assumes no responsibility or liability for any third party and/or
 * commercial software required for the database or applications. Licensee
 * agrees to obtain and maintain valid licenses for any additional third party
 * software required.
 *****************************************************************************/

/***********************************************************************
 * Headers
 ***********************************************************************/

#include <stdlib.h>
#include <unistd.h>
#include <stdio.h>
#include <math.h>
#include <getopt.h>
#include <libgen.h>
#include <sys/time.h>
#include <sys/stat.h>
#include <lcm/lcm-cpp.hpp>

#include "TethysLcmTypes/LrauvLcmMessage.hpp"
#include "LcmMessageReader.h"
#include "LcmMessageWriter.h"

#include "MathP.h"
#include "NavUtils.h"
#include "macrologger_main.h"
#include "LcmDeltaT.h"
#include "oaUtils.h"

#include "ObsAvoid.h"

/***********************************************************************
 * Macros
 ***********************************************************************/

#define IDT_PORT         4040
#define IDT_NAME         "lcmdeltat"
#define IDT_LOG_DATA     true
#define IDT_TIMEOUT      250  // ms

#define DEFAULT_TRACE    DEBUG_LEVEL
#define SYSLOG_FILENAME  "syslog.oa"
#define LOG_ENV_VAR      "AUV_LOG_DIR"

using namespace TethysLcmTypes;
using namespace lrauv_lcm_tools;

/***********************************************************************
 * Local structures and functions
 ***********************************************************************/

// hold the command line options here
struct long_options
{
    char*    ini;     // app configuration file
    uint32_t port;    // DeltaT port
    char*    bfcfg;   // beamformer configuration file
    char     trace;   // syslog trace level
    bool     syslog;  // syslogging to a file if true
    bool     lcmidt;  //
    bool     verbose; //
};

typedef struct _OaConfig_
{

} OaConfig;

// parse command line options. fill-out options object. return true if all good.
bool parseOptions(struct long_options *opt, int argc, char const *argv[]);
void syslogSetup(bool);

// setup AUV_LOG_DIR using setenv() for use by other components
// explain app usage to user.
void usage();

int loadINI(const char* inipath, OaConfig *cfg);
int loadINI(const char* inipath, OaConfig *cfg)
{
   // Check parameters
   if (NULL == cfg) {
      LOG_ERROR("NULL ptr to OaConfig object");
      return -1;
   }

   // If a INI file was specified, use it to populate the OaConfig object
   if (inipath) {
      LOG_INFO("Loading configuration from %s", inipath);
   }
   return 0;
}

/***********************************************************************
 * Code
 ***********************************************************************/

// File scoped variables
static OaVehState _vehState;
static Idt83pData _aftSonar, _fwdSonar;


// LCM message handler processes DeltaT and AHRS messages capturing
// multibeam ranges and vehicle state for use in OA computation
class OALcmMessageHandler
{

public:

   //*********************************************************************
   // Called when a message is received on the DeltaT channel
   // Use when running with a separate IDT driver or when replaying
   // logged LCM messages
   void handleDeltaT(const lcm::ReceiveBuffer* rbuf,
                     const std::string& chan,
                     const LrauvLcmMessage* msg);

   // *********************************************************************
   // Called when a message is received on the AHRS channel
   void handleAHRS(const lcm::ReceiveBuffer* rbuf,
                   const std::string& chan,
                   const LrauvLcmMessage* msg);

   // *********************************************************************
   // Called when a message is received on the Nav channel
   void handleNav(const lcm::ReceiveBuffer* rbuf,
                  const std::string& chan,
                  const LrauvLcmMessage* msg);

   // *********************************************************************
   // Called when a message is received on the DVL channel
   void handleDVL(const lcm::ReceiveBuffer* rbuf,
                  const std::string& chan,
                  const LrauvLcmMessage* msg);

   // *********************************************************************
   // Called when a message is received on the Depth channel
   void handleDepth(const lcm::ReceiveBuffer* rbuf,
                    const std::string& chan,
                    const LrauvLcmMessage* msg);

protected:
   LcmMessageReader _msg_reader;
};      // class OALcmMessageHandler


////////////////////////////////////////////////////////////////////////////
// Obstacle Avoidance main program
int main(int argc, char const *argv[])
{
   // =========== Application setup ===========
   // get command line options
   struct long_options options;
   if (!parseOptions(&options, argc, argv))
   {
      usage();
      exit(0);
   }
   // load config file settings
   LOG_INFO("oa: ini=%s, port=%d, bfcfg=%s, trace=%0x, verbose=%s, lcmidt=%s",
          options.ini, options.port, options.bfcfg, options.trace,
          options.verbose?"yes":"no", options.lcmidt?"yes":"no");

   // =========== Get the application configuration - load INI file ===========
   OaConfig oa_cfg;
   if (0 != loadINI(options.ini, &oa_cfg)) {
      LOG_ERROR("oa: App initialization failed");
      exit(0);
   }

   // =========== Logging setup ===========
   // setup AUV_LOG_DIR using setenv() for use by other components
   setLogDir(LOG_ENV_VAR, getenv(LOG_ENV_VAR));
   syslogSetup(options.syslog);

   setMLLevel(options.trace);
   setMLVerbose(options.verbose);

   // =========== Setup LCM channels and message handlers ===========
   OALcmMessageHandler handler;
   lcm::LCM lcm;
   if(!lcm.good())
   {
      LOG_ERROR("oa: LCM initialization failed");
      exit(0);
   }

   lcm.subscribe("AHRS_M2", &OALcmMessageHandler::handleAHRS, &handler);
   lcm.subscribe("DeadReckonUsingMultipleVelocitySources", &OALcmMessageHandler::handleNav, &handler);
   lcm.subscribe("Depth_Keller", &OALcmMessageHandler::handleDepth, &handler);
   lcm.subscribe("RDI_Pathfinder", &OALcmMessageHandler::handleDVL, &handler);

   // create depth_command item in lrauv LCM message writer
   Dim sdim(0,0);
   LcmMessageWriter<std::string> msgWriter;
   if (!msgWriter.addArray(Float,"_.depth_command","_.depth_command","meter",sdim))
      LOG_ERROR("LcmDeltaT::init83pState() - failed to add depth_command");


   // =========== Create LcmDeltaT with config object ===========
   // Option to launch DeltaT logger in a separate process.
   // For now, we'll simplify things with just one process.
   DeltaT::DeltaTConfig dtconfig =
               {IDT_NAME, options.port, IDT_LOG_DATA, NULL, IDT_TIMEOUT};
   LcmDeltaT *idt = NULL;
   if (options.lcmidt)
   {
      lcm.subscribe("DeltaT",  &OALcmMessageHandler::handleDeltaT, &handler);
   }
   else
   {
      // Either read and publish IDT data or subscribe to IDT LCM messages
      idt = new LcmDeltaT(dtconfig);
      if (options.bfcfg) DeltaT::configDeltaT(idt, options.bfcfg);
   }

   // pulled from Rob's oa.cc
   ObsAvoid *oa = new ObsAvoid();

   // Clear data sets
   memset(&_vehState, 0, sizeof(_vehState));
   memset(&_fwdSonar, 0, sizeof(_fwdSonar));
   memset(&_aftSonar, 0, sizeof(_aftSonar));

   // Track last sonar data timestamp
   double lastIdtPingtime = 0.;

   // =========== main loop ===========
   while (true)
   {
      // Some temporal cohesion between IDT and AHRS measurements might be
      // required before performing a computation. For now, compute if we get
      // an update on either.
      bool compute = false;

      // Read DeltaT beam ranges from local interface if configured for that
      // Otherwise the beam ranges arrive from LCM
      if (!options.lcmidt) {
         idt->getData(&_aftSonar);
      }

      // Get vehicle state
      int nMsgs = lcm.handleTimeout(dtconfig.readTimeout);

      LOG_DEBUG("%d LCM messages handled", nMsgs);
      LOG_DEBUG("Last idt time: %.2f\tLatest idt time: %.2f",
         lastIdtPingtime, _aftSonar.pingtime);

      if (_aftSonar.pingtime != lastIdtPingtime) compute = true;

      if (compute)
      {
         LOG_DEBUG("Computing OA");
         lastIdtPingtime = _aftSonar.pingtime;

         // Calculate
         // pulled from Rob's oaTest.cc
         // Do some of these values need to be pulled from a config file?
         double altitudeCmdIn = 10;
         double avoidRange = 40;
         double avoidAngle = 20*PI/180.;
         double depthCmd = 85;
         double depthCmdOut = -1;
         if (0 == oa->compute(&_vehState, &_aftSonar, &_fwdSonar, altitudeCmdIn,
                              avoidRange, avoidAngle, depthCmd, &depthCmdOut))
         {
            printf("Valid OA computation - publishing depthCmd %.2f\n",
                       depthCmdOut);
            // Set the commanded depth message items and publish
            msgWriter.set("_.depth_command", depthCmdOut);

            struct timeval spec;
            gettimeofday(&spec, NULL);
            double ms = (spec.tv_sec * 1000.) + (spec.tv_usec / 1000.);

            msgWriter.publish(lcm, "OA", (int64_t)ms);
         }
      }
   }

   delete oa;
   delete idt;
   return 0;
}


////////////////////////////////////////////////////////////////////////////
// command line options
static struct option long_options[] =
{
    {"ini"    , required_argument, NULL, 'i'},   // OA config file name (NULL)
    {"port"   , required_argument, NULL, 'p'},   // DeltaT port (DEFAULT_PORT)
    {"cfg"    , required_argument, NULL, 'c'},   // OA config file name (NULL)
    {"trace"  , required_argument, NULL, 't'},   // Trace level (DEFAULT_TRACE)
    {"syslog" , no_argument      , NULL, 's'},   // syslogging to file (false)
    {"verbose", no_argument      , NULL, 'v'},   // DEBUG to stderr (false)
    {"help"   , no_argument      , NULL, '?'},   // print usage
    {NULL, 0, NULL, 0}
};

// parse command line options. fill-out options object. return true if all good.
bool parseOptions(struct long_options *options, int argc, char const *argv[])
{
   // set the defaults first
   options->ini      = NULL;
   options->port     = IDT_PORT;
   options->bfcfg    = NULL;
   options->trace    = DEFAULT_TRACE;
   options->syslog   = false;
   options->lcmidt   = false;
   options->verbose  = false;         // no debug or info output to stderr

   // now get the options
   char *trace = NULL;
   int opt = 0, long_index = 0;
   while ((opt = getopt_long(argc, (char *const *)argv, "i:c:f:p:n:st:lv?",
                  long_options, &long_index )) != -1)
   {
      if (opt == 'p')
      {
         options->port = abs(atoi(optarg));
      }
      else if (opt == 'i')
      {
         options->ini = strdup(optarg);
      }
      else if (opt == 'c')
      {
         options->bfcfg = strdup(optarg);
      }
      else if (opt == 'v')
      {
         options->verbose = true;
      }
      else if (opt == 's')
      {
         options->syslog = true;
      }
      else if (opt == 'l')
      {
         options->lcmidt = true;
      }
      else if (opt == 't')
      {
         trace = strdup(optarg);
      }
      else if (opt == '?')
      {
         return false;
      }
      else
      {
         fprintf(stderr, "Unknown option '%c'\n", opt);
         return false;
      }
   }

   // encode the trace option to a macrologger log level
   if (trace)
   {
      bool bad = false;
      if (!strcasecmp(trace, "ERROR"))
         options->trace = ERROR_LEVEL;
      else if (!strcasecmp(trace, "INFO"))
         options->trace = INFO_LEVEL;
      else if (!strcasecmp(trace, "DEBUG"))
         options->trace = DEBUG_LEVEL;
      else if (!strcasecmp(trace, "NONE"))
         options->trace = NO_LOG;
      else
         bad = true;

      delete trace;

      if (bad) return false;
   }

   // all good
   return true;
}


////////////////////////////////////////////////////////////////////////////
// explain app usage to user.
void usage()
{
   fprintf(stderr,
    "Usage:\n"
    "  oa [OPTIONS]  LRAUV Obstacle Avoidance\n"
    "  Options\n"
    "    -?, --help              Print this help\n"
    "    -i, --ini               Use this INI file to configure OA\n"
    "    -p, --port     NUM      Use this port to get Delta T data (4040)\n"
    "    -c, --cfg      pathname Use this config file to init the beamformer\n"
    "    -l, --lcmidt            Use the DeltaT LCM channel instead of a beamformer connection\n"
    "    -s, --syslog            Divert syslogging to syslog.oa (no divert)\n"
    "    -v, --verbose           Show DEBUG and INFO messages on console (no)\n"
    "    -t, --trace    CHOICE   Use one of NONE, ERROR, INFO, DEBUG (INFO)\n\n"
    "  Example\n"
    "    oa -i oa.ini -p 4050 --trace DEBUG --syslog -v --cfg deltat.cfg"
    "    # use port 4050, trace level DEBUG to syslog.oa only, init using deltat.cfg\n"
    ""
    "    oa --trace DEBUG --syslog"
    "    # trace level DEBUG to syslog.idt only\n");
   return;
}

////////////////////////////////////////////////////////////////////////////
// open optional syslogging FILE
void syslogSetup(bool open_syslog)
{
   if (open_syslog)
   {
      // create the path to the syslog file
      const char* ald = getenv(LOG_ENV_VAR);
      if (NULL == ald) ald = ".";
      char* syslogfile = (char*)malloc(strlen(ald)+strlen(SYSLOG_FILENAME)+2);
      sprintf(syslogfile, "%s/%s", ald, SYSLOG_FILENAME);

      // open the syslog
      if (NULL == openMLOutstream(syslogfile))
      {
         LOG_ERROR("oa: could not open syslogging file %s", syslogfile);
      }
      else
      {
         LOG_INFO("Syslogging to %s", syslogfile);
      }
      free(syslogfile);
   }
   return;
}

void OALcmMessageHandler::handleDeltaT (const lcm::ReceiveBuffer* rbuf,
                                        const std::string& chan,
                                        const LrauvLcmMessage* msg)
{
   LOG_DEBUG("OALcmMessageHandler: received message: timestamp: %ld",
      msg->epochMillisec);

   // Use message reader from lrauv-lcmtypes to access LCM message internals
   _msg_reader.setMsg(msg);
   std::vector<const char*> names = _msg_reader.listNames();
   for( size_t i=0; i<names.size(); ++i )
      LOG_DEBUG("DeltaT msg item %d: %s", (int)i, names[i]);

   // Clear previous data set
   memset(&_aftSonar, 0, sizeof(_aftSonar));

   // Extract info about the ping
   const IntArray *ia = _msg_reader.getIntArray("valid");
   if (ia)  _aftSonar.valid = ia->data[0]; else LOG_ERROR("no valid");
//   const DoubleArray *da = _msg_reader.getDoubleArray("timestamp");
//   if (da)  _aftSonar.timestamp = da->data[0]; else LOG_ERROR("no timestamp");
   const DoubleArray *da = _msg_reader.getDoubleArray("pingtime");
   if (da)  _aftSonar.pingtime = da->data[0]; else LOG_ERROR("no pingtime");
   LOG_DEBUG("pingtime is %.2f", _aftSonar.pingtime);

   ia = _msg_reader.getIntArray("pingnumber");
   if (ia)  _aftSonar.pingnumber = ia->data[0]; else LOG_ERROR("no pingnumber");
   ia = _msg_reader.getIntArray("interval");
   if (ia)  _aftSonar.interval = ia->data[0]; else LOG_ERROR("no interval");

   // Extract info about the beams and ranges
   const FloatArray *fa = _msg_reader.getFloatArray("altitude");
   if (fa)  _aftSonar.altitude = fa->data[0]; else LOG_ERROR("no altitude");
   ia = _msg_reader.getIntArray("nbeams");
   if (ia)  _aftSonar.nbeams = ia->data[0]; else LOG_ERROR("no nbeams");

   // Now carefully get the ranges and intensities if available
   int dim = 0;
   fa = _msg_reader.getFloatArray("range");
   if (fa)
   {
      dim = fa->data.size();
      // Ensure that the index does not overflow array bounds
      for (int i=0; i<_aftSonar.nbeams && i<IDT_MAX_BEAMS && i<dim; i++)
         _aftSonar.range[i] = fa->data[i];
      LOG_DEBUG("OALcmMessageHandler: #ranges   : %d", dim);
   } else LOG_ERROR("no range");
   ia = _msg_reader.getIntArray("intense");
   if (ia)
   {
      dim = ia->data.size();
      // Ensure that the index does not overflow array bounds
      for (int i=0; i<_aftSonar.nbeams && i<IDT_MAX_BEAMS && i<dim; i++)
         _aftSonar.intense[i] = ia->data[i];
      LOG_DEBUG("OALcmMessageHandler: #intense  : %d", dim);
   } else LOG_ERROR("no intense");

   return;
}

// *********************************************************************
// Called when a message is received on the AHRS channel
void OALcmMessageHandler::handleAHRS (const lcm::ReceiveBuffer* rbuf,
                                      const std::string& chan,
                                      const LrauvLcmMessage* msg)
{
   LOG_DEBUG("OALcmMessageHandler: received message: timestamp: %ld",
      msg->epochMillisec);

   // Use message reader from lrauv-lcmtypes to access LCM message internals
   _msg_reader.setMsg(msg);
   std::vector<const char*> names = _msg_reader.listNames();
   for( size_t i=0; i<names.size(); ++i )
      LOG_DEBUG("AHRS msg item %d: %s", (int)i, names[i]);

   // Extract yaw, pitch, and roll
   const DoubleArray *da = _msg_reader.getDoubleArray("platform_orientation");
   if (da)  _vehState.yaw = da->data[0]; else LOG_ERROR("no orientation");
   da = _msg_reader.getDoubleArray("platform_pitch_angle");
   if (da)  _vehState.pitch = da->data[0]; else LOG_ERROR("no pitch");
   da = _msg_reader.getDoubleArray("platform_roll_angle");
   if (da)  _vehState.roll = da->data[0]; else LOG_ERROR("no roll");

   return;
}

// *********************************************************************
// Called when a message is received on the Nav channel
void OALcmMessageHandler::handleNav (const lcm::ReceiveBuffer* rbuf,
                                     const std::string& chan,
                                     const LrauvLcmMessage* msg)
{
   LOG_DEBUG("OALcmMessageHandler: received message: timestamp: %ld",
      msg->epochMillisec);

   // Use message reader from lrauv-lcmtypes to access LCM message internals
   _msg_reader.setMsg(msg);
   std::vector<const char*> names = _msg_reader.listNames();
   for( size_t i=0; i<names.size(); ++i )
      LOG_DEBUG("Nav msg item %d: %s", (int)i, names[i]);

   // Extract location in geo degrees, convert to radians, use to get UTM
   double lat = 0., lon = 0;
   const DoubleArray *da = _msg_reader.getDoubleArray("latitude");
   if (da)  lat = Math::degToRad(da->data[0]); else LOG_ERROR("no latitude");
   da = _msg_reader.getDoubleArray("longitude");
   if (da)  lon = Math::degToRad(da->data[0]); else LOG_ERROR("no longitude");
   NavUtils::geoToUtm(lat, lon, NavUtils::geoToUtmZone(lat, lon),
                      &_vehState.northings, &_vehState.eastings);

   return;
}

// *********************************************************************
// Called when a message is received on the DVL channel
void OALcmMessageHandler::handleDVL (const lcm::ReceiveBuffer* rbuf,
                                     const std::string& chan,
                                     const LrauvLcmMessage* msg)
{
   LOG_DEBUG("OALcmMessageHandler: received message: timestamp: %ld",
      msg->epochMillisec);

   // Use message reader from lrauv-lcmtypes to access LCM message internals
   _msg_reader.setMsg(msg);
   std::vector<const char*> names = _msg_reader.listNames();
   for( size_t i=0; i<names.size(); ++i )
      LOG_DEBUG("DVL msg item %d: %s", (int)i, names[i]);

   // Extract altitude
   const FloatArray *fa = _msg_reader.getFloatArray("height_above_sea_floor");
   if (fa)  _vehState.altitude = fa->data[0]; else LOG_ERROR("no height_above_sea_floor");

   return;
}

// *********************************************************************
// Called when a message is received on the Depth channel
void OALcmMessageHandler::handleDepth (const lcm::ReceiveBuffer* rbuf,
                                       const std::string& chan,
                                       const LrauvLcmMessage* msg)
{
   LOG_DEBUG("OALcmMessageHandler: received message: timestamp: %ld",
      msg->epochMillisec);

   // Use message reader from lrauv-lcmtypes to access LCM message internals
   _msg_reader.setMsg(msg);
   std::vector<const char*> names = _msg_reader.listNames();
   for( size_t i=0; i<names.size(); ++i )
      LOG_DEBUG("Depth msg item %d: %s", (int)i, names[i]);

   // Extract depth
   const FloatArray *fa = _msg_reader.getFloatArray("depth");
   if (fa)  _vehState.depth = fa->data[0]; else LOG_ERROR("no depth");

   return;
}