#include <stdlib.h>
#include <stdio.h>
#include <sys/stat.h>
#include <dirent.h>
#include <unistd.h>
#include <sys/time.h>
#include <exception>
#include <lcm/lcm-cpp.hpp>

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

#include "NavUtils.h"
#include "DataLogReader.h"
#include "MathP.h"
#include "TimeP.h"
#include "TimeTag.h"
#include "FloatData.h"
#include "IntegerData.h"
#include "DataField.h"
#include "Exception.h"

#include "ObsAvoid.h"

#define SYSLOG_NAME    "syslog.lr"
#define LOG_ENV_VAR    "AUV_LOG_DIR"
#define NBEAMS         4

#include "macrologger_main.h"

using namespace TethysLcmTypes;
using namespace lrauv_lcm_tools;

// publish OaVehData on LCM using an LcmMessageWriters
void publishVehState(lcm::LCM *lcm, LcmMessageWriter<std::string> *vehWriter,
   LcmMessageWriter<std::string> *navWriter,
   LcmMessageWriter<std::string> *depthWriter,
   OaVehState *data);
// publish Idt83pData on LCM using an LcmMessageWriter
void publishDvl(lcm::LCM *lcm, LcmMessageWriter<std::string> *dvlWriter,
   Idt83pData *data, OaVehState *oa);
// setup the message writers for their respective data
void setupMsgWriters(LcmMessageWriter<std::string> *dvlWriter,
   LcmMessageWriter<std::string> *vehWriter,
   LcmMessageWriter<std::string> *depthWriter,
   LcmMessageWriter<std::string> *navWriter);

class nbeamExcept
{

public:
   nbeamExcept(const char *message, int cntr)
   {
      strncpy(str, message, sizeof(str));
      errorCntr = cntr;
   }
   char str[256];
   int errorCntr;
};


int main(int argc, char const *argv[])
{

   /*
     NO_LOG          0x00
     ERROR_LEVEL     0x01
     INFO_LEVEL      0x02
     DEBUG_LEVEL     0x03
   */
   setMLLevel(DEBUG_LEVEL);

   lcm::LCM *lcm0 = NULL;
   lcm0 = new lcm::LCM();
   if (!lcm0->good())
   {
      LOG_ERROR("LCM initialization failed!");
      exit(0);
   }
   LOG_INFO("LCM initialized!");

   OaVehState vehState;
   Idt83pData aftSonar;

   DataLogReader *trnlog = new DataLogReader("TerrainNav.log");

   LcmMessageWriter<std::string> vehWriter;
   LcmMessageWriter<std::string> navWriter;
   LcmMessageWriter<std::string> dvlWriter;
   LcmMessageWriter<std::string> depthWriter;
   setupMsgWriters(&dvlWriter, &vehWriter, &depthWriter, &navWriter);

   // 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_NAME)+2);
   sprintf(syslogfile, "%s/%s", ald, SYSLOG_NAME);

   // open the syslog
   if (NULL == openMLOutstream(syslogfile))
   {
      LOG_ERROR("lrTest: could not open syslogging file %s", syslogfile);
   }
   free(syslogfile);

   if (MLOUTSTREAM) LOG_INFO("There is an output file");

   /* Insert canned data for OA */

   /* Read the first line of the DeltaT log.  */
   try
   {
      trnlog->read();    /*QQ  Where does the row advance?*/
   }
   catch ( char *str )
   {
      LOG_ERROR("lrTest: Error - %s", str);
      return -1;
   }
   catch (...)
   {
      throw "lrTest: Error on the first reading of the trn.log";
      return -1;
   }

   /* Now begin looping*/
   /* Walk the nav log rows until the nav time stamp exceeds the Delta T.
      The nav log is sampled faster.*/

   DataField *f = NULL;

   try
   {
      double lastDepth = 0.0;
      int sub = 0;
      while( trnlog->read() != EOF )        /* This doesn't work.  Rely on exceptions. */
      {
          memset(&aftSonar, 0, sizeof(aftSonar));
          memset(&vehState, 0, sizeof(vehState));

         // n beams
         trnlog->fields.get(11, &f);
         aftSonar.nbeams = atoi(f->ascii());

         // n good beams
         trnlog->fields.get(12, &f);
         if (atoi(f->ascii()) > 1) aftSonar.valid = 1;

      	 trnlog->fields.get(10, &f);
      	 aftSonar.pingnumber = atof(f->ascii());

      	 for( int i=0; i<aftSonar.nbeams; i++ )
      	 {
      	    trnlog->fields.get(13+i, &f);
      	    aftSonar.range[i] = atof(f->ascii());
      	 }

      	 trnlog->fields.get(8, &f);
      	 aftSonar.pingtime = atof(f->ascii());
         aftSonar.pingtime -= ((sub % 10)*1.0)/100.;
         sub++;

      	 /* Read in the vehicle orientation from the navigation log.*/
      	 trnlog->fields.get(2, &f);
      	 vehState.northings = atof(f->ascii());
      	 trnlog->fields.get(3, &f);
      	 vehState.eastings = atof(f->ascii());
      	 trnlog->fields.get(4, &f);
      	 vehState.depth = atof(f->ascii());
          vehState.utm_zone = 10;

      	 trnlog->fields.get(5, &f);
      	 vehState.roll = atof(f->ascii());
      	 trnlog->fields.get(6, &f);
      	 vehState.pitch = atof(f->ascii());
      	 trnlog->fields.get(7, &f);
      	 vehState.yaw = atof(f->ascii());

         // Use dummy velocity values
         vehState.v_x = 0.88;
         vehState.v_y = -0.03;
         if (vehState.depth > lastDepth+0.05)
           vehState.v_z = 0.01;
         else if (vehState.depth < lastDepth-0.05)
           vehState.v_z = -0.01;
         else
           vehState.v_z = 0.0;

         lastDepth = vehState.depth;

          // Use omega_x to hold poseT timestamp
      	 trnlog->fields.get(1, &f);
      	 vehState.omega_x = atof(f->ascii());

          if (vehState.omega_x <= aftSonar.pingtime)
          {
             publishVehState(lcm0,&vehWriter, &navWriter, &depthWriter, &vehState);
             publishDvl(lcm0, &dvlWriter, &aftSonar, &vehState);
             usleep(250000);
          }
          else
          {
             publishDvl(lcm0, &dvlWriter, &aftSonar, &vehState);
             publishVehState(lcm0,&vehWriter,&navWriter,&depthWriter,&vehState);
             usleep(250000);
          }

      	 LOG_DEBUG("oaTest: Roll    = %.2f Deg",vehState.roll*180./PI);
      	 LOG_DEBUG("oaTest: Pitch   = %.2f Deg",vehState.pitch*180./PI);
         LOG_DEBUG("oaTest: Yaw     = %.2f Deg",vehState.yaw*180./PI);
         LOG_DEBUG("oaTest: Depth   = %.2f m",vehState.depth);
      	 LOG_DEBUG("oaTest: Time    = %.2f secs",vehState.omega_x);
         LOG_DEBUG("oaTest: Dvltime = %.2f secs",aftSonar.pingtime);
      	 LOG_DEBUG("oaTest: DvlValid= %s\n", aftSonar.valid? "true" : "false");
      }

   }
   catch ( char *str )
   {
      LOG_ERROR("oaTest: Error - %s", str);
      return -1;
   }
   catch ( nbeamExcept e )
   {
      LOG_ERROR("%s", e.str);
   }
   catch (Exception e)
   {
      LOG_INFO("oaTest: Caught end of file excpetion; %s",e.msg);
   }
   catch (...)
   {
      LOG_ERROR("oaTest: Uncaught error..");
      return -1;
   }


   return 0;
}

// setup the message writers for their respective data
void setupMsgWriters(LcmMessageWriter<std::string> *dvlWriter,
   LcmMessageWriter<std::string> *vehWriter,
   LcmMessageWriter<std::string> *depthWriter,
   LcmMessageWriter<std::string> *navWriter)
{
   // Idt message structure taken from LcmDeltaT
   Dim sdim(0,0);

   // yaw
   if (!vehWriter->addArray(Double, "platform_orientation", "platform_orientation", "radians", sdim))
      LOG_ERROR("failed to add platform_orientation");
   // pitch
   if (!vehWriter->addArray(Double, "platform_pitch_angle", "platform_pitch_angle", "radians", sdim))
      LOG_ERROR("failed to add platform_pitch_angle");
   // roll
   if (!vehWriter->addArray(Double, "platform_roll_angle", "platform_roll_angle", "radians", sdim))
      LOG_ERROR("failed to add platform_roll_angle");

   if (!navWriter->addArray(Double, "longitude", "longitude", "meters", sdim))
      LOG_ERROR("failed to add northings");
   if (!navWriter->addArray(Double, "latitude", "latitude", "meters", sdim))
      LOG_ERROR("failed to add northings");

   if (!depthWriter->addArray(Float, "depth", "depth", "meters", sdim))
      LOG_ERROR("failed to add depth");

   if (!dvlWriter->addArray(Float, "platform_x_velocity_wrt_ground", "platform_x_velocity_wrt_ground", "m/s", sdim))
      LOG_ERROR("failed to add xvel");
   if (!dvlWriter->addArray(Float, "platform_y_velocity_wrt_ground", "platform_y_velocity_wrt_ground", "m/s", sdim))
      LOG_ERROR("failed to add yvel");
   if (!dvlWriter->addArray(Float, "platform_z_velocity_wrt_ground", "platform_z_velocity_wrt_ground", "m/s", sdim))
      LOG_ERROR("failed to add zvel");
   if (!dvlWriter->addArray(Float, "Beam1Range", "Beam1Range", "meters", sdim))
      LOG_ERROR("failed to add beam1");
   if (!dvlWriter->addArray(Float, "Beam2Range", "Beam2Range", "meters", sdim))
      LOG_ERROR("failed to add beam2");
   if (!dvlWriter->addArray(Float, "Beam3Range", "Beam3Range", "meters", sdim))
      LOG_ERROR("failed to add beam3");
   if (!dvlWriter->addArray(Float, "Beam4Range", "Beam4Range", "meters", sdim))
      LOG_ERROR("failed to add beam4");
   if (!dvlWriter->addArray(Int, "BottomVelocityFlag", "BottomVelocityFlag", "", sdim))
      LOG_ERROR("failed to add valid");
}

// publish Idt83pData on LCM using an LcmMessageWriter
void publishDvl(lcm::LCM *lcm, LcmMessageWriter<std::string> *dvlWriter,
   Idt83pData *data, OaVehState *oa)
{
   if (data && lcm && lcm->good() && dvlWriter)
   {
      if (!dvlWriter->set("platform_x_velocity_wrt_ground", (float)oa->v_x)) LOG_ERROR("failed to set v_x");
      if (!dvlWriter->set("platform_y_velocity_wrt_ground", (float)oa->v_y)) LOG_ERROR("failed to set v_y");
      if (!dvlWriter->set("platform_z_velocity_wrt_ground", (float)oa->v_z)) LOG_ERROR("failed to set v_z");
      if (!dvlWriter->set("Beam1Range", (float)data->range[0])) LOG_ERROR("failed to set beam1");
      if (!dvlWriter->set("Beam2Range", (float)data->range[1])) LOG_ERROR("failed to set beam2");
      if (!dvlWriter->set("Beam3Range", (float)data->range[2])) LOG_ERROR("failed to set beam3");
      if (!dvlWriter->set("Beam4Range", (float)data->range[3])) LOG_ERROR("failed to set beam4");
      if (!dvlWriter->set("BottomVelocityFlag", (int)data->valid)) LOG_ERROR("failed to set valid");

      double ms = data->pingtime * 1000.;
      dvlWriter->publish(*lcm, "RDI_Pathfinder", (int64_t)ms);
   }
}

// publish OaVehData on LCM using an LcmMessageWriter
void publishVehState(lcm::LCM *lcm, LcmMessageWriter<std::string> *vehWriter,
   LcmMessageWriter<std::string> *navWriter,
   LcmMessageWriter<std::string> *depthWriter,
   OaVehState *data)
{
   if (data && lcm && lcm->good() && vehWriter && navWriter && depthWriter)
   {
      double ms = data->omega_x * 1000.;

      // AHRS channel
      if (!vehWriter->set("platform_orientation", data->yaw)) LOG_ERROR("failed to set yaw");
      if (!vehWriter->set("platform_pitch_angle", data->pitch)) LOG_ERROR("failed to set pitch");
      if (!vehWriter->set("platform_roll_angle", data->roll)) LOG_ERROR("failed to set roll");
      vehWriter->publish(*lcm, "AHRS_M2", (int64_t)ms);

      // Nav channel
      // Convert from UTM to geo radians, convert to degrees and publish
      double lat = 0., lon = 0.;
      if (data->northings > 0.1 && data->eastings > 0.1)
         NavUtils::utmToGeo(data->northings, data->eastings, data->utm_zone,
                            &lat, &lon);
      if (!navWriter->set("longitude", (double)Math::radToDeg(lon))) LOG_ERROR("failed to set longitude");
      if (!navWriter->set("latitude",  (double)Math::radToDeg(lat))) LOG_ERROR("failed to set latitude");
      navWriter->publish(*lcm, "DeadReckonUsingMultipleVelocitySources", (int64_t)ms);

      // Depth and DVL channels
      if (!depthWriter->set("depth", (float)data->depth)) LOG_ERROR("failed to set depth");
      depthWriter->publish(*lcm, "Depth_Keller", (int64_t)ms);
   }
}