#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.oa"
#define LOG_ENV_VAR    "AUV_LOG_DIR"
#define NBEAMS         120

#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> *dvlWriter,
   LcmMessageWriter<std::string> *depthWriter,
   OaVehState *data);
// publish Idt83pData on LCM using an LcmMessageWriter
void publishIdt(lcm::LCM *lcm, LcmMessageWriter<std::string> *idtWriter,
   Idt83pData *data);
// setup the message writers for their respective data
void setupMsgWriters(LcmMessageWriter<std::string> *idtWriter,
   LcmMessageWriter<std::string> *vehWriter,
   LcmMessageWriter<std::string> *dvlWriter,
   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(INFO_LEVEL);

   // Use local ObsAvoid object or publish data on LCM?
   bool useLcm = false;
   if (argc > 1 && !strcasecmp(argv[1], "lcm")) useLcm = true;

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

   ObsAvoidLog::oaLog oaData;

   OaVehState vehState;
   Idt83pData aftSonar, fwdSonar;
   double altitudeCmdIn = 10;
   double avoidRange = 40;
   double avoidAngle = 20*PI/180.;
   double depthCmd = 85;
   double depthCmdOut;
   char error[128];
   int nbeamCntr = 0;
   const int nErrors = 5;

   DataLogReader *idtlog = new DataLogReader("idt.log");
   DataLogReader *navlog = new DataLogReader("navigation.log");

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

   /* Initialize milliseconds to now */
   long time0 = TimeP::milliseconds();

   LOG_DEBUG("oaTest: time0 = %ld msec.", time0);

   // 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("deltaTTest: 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
   {
      idtlog->read();    /*QQ  Where does the row advance?*/
      navlog->read();

      if (idtlog->timeTag()->value() < navlog->timeTag()->value() )
	 throw "First DeltaT timestamp precedes first navigation timestamp.";
   }
   catch ( char *str )
   {
      LOG_ERROR("oaTest: Error - %s", str);
      return -1;
   }
   catch (...)
   {
      throw "oaTest: Error on the first reading of the .log files.";
      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
   {
      while( idtlog->read() != EOF )        /* This doesn't work.  Rely on exceptions. */
      {
	 /* Synchronize the nav log with the deltat log.  Skip the in-between records.*/
	 do
	 {
	    navlog->read();
	 } while( idtlog->timeTag()->value() > navlog->timeTag()->value() );

    memset(&aftSonar, 0, sizeof(aftSonar));
    memset(&vehState, 0, sizeof(vehState));
	 //LOG_INFO("oaTest: idtlog->timeTag()->value() = %.2f", idtlog->timeTag()->value());
	 //LOG_INFO("oaTest: navlog->timeTag()->value() = %.2f", navlog->timeTag()->value());


	 idtlog->fields.get(1, &f);
	 aftSonar.altitude = atof(f->ascii());
	 LOG_INFO("oaTest: Altitude measured by the DeltaT. %s = %.2f", f->name(), aftSonar.altitude);

	 idtlog->fields.get(2, &f);
	 aftSonar.nbeams = atoi(f->ascii());

	 idtlog->fields.get(3, &f);
	 aftSonar.pingnumber = atof(f->ascii());
	 LOG_INFO("oaTest: Ping number = %d", aftSonar.pingnumber);

	 if( aftSonar.nbeams != NBEAMS )
	 {
	    nbeamCntr++;
	    sprintf( error,"oaTest: nbeams = %d at t = %.2f sec.",
		     aftSonar.nbeams, TimeP::milliseconds()/1000.);

	    LOG_INFO("%s",error);

	    throw nbeamExcept(error, nbeamCntr);
	 }
	 nbeamCntr = 0;
	 for( int i=0; i<NBEAMS; i++ )
	 {
	    idtlog->fields.get(4+i, &f);
	    aftSonar.range[i] = atof(f->ascii());
	 }

	 /* Read in the interrogation time*/

	 idtlog->fields.get(244, &f);
	 aftSonar.pingtime = atof(f->ascii());
	 /* This is zeroed in the simulator.  Add it here.*/
	 aftSonar.pingtime = idtlog->timeTag()->value();
    LOG_DEBUG("pingtime is %.2f", aftSonar.pingtime);


	 /* Read in the vehicle orientation from the navigation log.*/
	 navlog->fields.get(1, &f);
	 vehState.northings = atof(f->ascii());
	 navlog->fields.get(2, &f);
	 vehState.eastings = atof(f->ascii());
	 navlog->fields.get(3, &f);
	 vehState.depth = atof(f->ascii());
    navlog->fields.get(50, &f);
    double lat = atof(f->ascii());
    navlog->fields.get(51, &f);
    double lon = atof(f->ascii());
    vehState.utm_zone = NavUtils::geoToUtmZone(lat, lon);

	 navlog->fields.get(7, &f);
	 vehState.roll = atof(f->ascii());
	 navlog->fields.get(8, &f);
	 vehState.pitch = atof(f->ascii());
	 navlog->fields.get(9, &f);
	 vehState.yaw = atof(f->ascii());
	 navlog->fields.get(10, &f);
	 vehState.omega_x = atof(f->ascii());
	 navlog->fields.get(11, &f);
	 vehState.omega_y = atof(f->ascii());
	 navlog->fields.get(12, &f);
	 vehState.omega_z = atof(f->ascii());

	 navlog->fields.get(14, &f);
	 vehState.altitude = atof(f->ascii());

	 /* Calculate or publish - check a command line option */
    if (useLcm && NULL != lcm0)
    {
       publishVehState(lcm0, &vehWriter, &navWriter,
         &dvlWriter, &depthWriter, &vehState);
       usleep(12000);
       publishIdt(lcm0, &idtWriter, &aftSonar);
       usleep(12000);
    }

    if (!useLcm && NULL != oa)
    {
   	 oa->compute(&vehState, &aftSonar, &fwdSonar, altitudeCmdIn, avoidRange,
   		     avoidAngle, depthCmd, &depthCmdOut);
    }
	 /*Test logging*/

	 oaData.pingtime = aftSonar.pingtime;

	 oaData.roll  = vehState.roll;
	 oaData.pitch = vehState.pitch;
	 oaData.yaw   = vehState.yaw;
	 oaData.depthCmdOut = depthCmdOut;

	 LOG_DEBUG("oaTest: Roll  = %8.2f Deg.",oaData.roll*180./PI);
	 LOG_DEBUG("oaTest: Pitch = %8.2f Deg.",oaData.pitch*180./PI);
	 LOG_DEBUG("oaTest: Yaw   = %8.2f Deg.",oaData.yaw*180./PI);
	 LOG_DEBUG("oaTest: Idt pingtime   = %.2f Sec.",aftSonar.pingtime);
      }

   }
   catch ( char *str )
   {
      LOG_ERROR("oaTest: Error - %s", str);
      return -1;
   }
   catch ( nbeamExcept e )
   {
      LOG_ERROR("%s", e.str);
      if( e.errorCntr >= nErrors )
      {
	 LOG_ERROR("Read incorrect number of beams %d consecutive times; aborting,",
		   nErrors);
	 return -1;
      }
   }
   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> *idtWriter,
   LcmMessageWriter<std::string> *vehWriter,
   LcmMessageWriter<std::string> *navWriter,
   LcmMessageWriter<std::string> *dvlWriter,
   LcmMessageWriter<std::string> *depthWriter)
{
   // Idt message structure taken from LcmDeltaT
   Dim sdim(0,0);
   // scalar int value valid
   if (!idtWriter->addArray(Int, "valid", "valid", "", sdim))
      LOG_ERROR("failed to add valid");
   // epoch seconds double value pingtime
   if (!idtWriter->addArray(Double, "pingtime", "pingtime", "epoch", sdim))
      LOG_ERROR("failed to add pingtime");
   // epoch seconds doule value timestamp
   if (!idtWriter->addArray(Double, "timestamp", "timestamp", "epoch", sdim))
      LOG_ERROR("failed to add timestamp");
   // scalar int value pingnumber
   if (!idtWriter->addArray(Int, "pingnumber", "pingnumber", "", sdim))
      LOG_ERROR("failed to add pingnumber");
   // epoch seconds float value altitude
   if (!idtWriter->addArray(Float, "altitude", "altitude", "", sdim))
      LOG_ERROR("failed to add altitude");
   // int value interval
   if (!idtWriter->addArray(Int, "interval", "interval", "", sdim))
      LOG_ERROR("failed to add interval");
   // scalar int value nbeams
   if (!idtWriter->addArray(Int, "nbeams", "nbeams", "", sdim))
      LOG_ERROR("failed to add nbeams");
   //  beam ranges
   Dim cdim(1,IDT_MAX_BEAMS);    // 1-dimension
   if (!idtWriter->addArray(Float, "range", "range", "meter", cdim))
      LOG_ERROR("failed to add range");
   //  beam intensities
   if (!idtWriter->addArray(Int, "intense", "intense", "percentage", cdim))
      LOG_ERROR("failed to add intense");

   // 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, "height_above_sea_floor", "height_above_sea_floor", "meters", sdim))
      LOG_ERROR("failed to add height_above_sea_floor");
#if 0
   if (!vehWriter->addArray(Double, "v_x", "v_x", "m/s", sdim))
      LOG_ERROR("failed to add v_x");
   if (!vehWriter->addArray(Double, "v_y", "v_y", "m/s", sdim))
      LOG_ERROR("failed to add v_y");
   if (!vehWriter->addArray(Double, "v_z", "v_z", "m/s", sdim))
      LOG_ERROR("failed to add v_z");
   if (!vehWriter->addArray(Double, "omega_x", "omega_x", "rad/s/s", sdim))
      LOG_ERROR("failed to add omega_x");
   if (!vehWriter->addArray(Double, "omega_y", "omega_y", "rad/s/s", sdim))
      LOG_ERROR("failed to add omega_y");
   if (!vehWriter->addArray(Double, "omega_z", "omega_z", "rad/s/s", sdim))
      LOG_ERROR("failed to add omega_z");
#endif
}

// publish Idt83pData on LCM using an LcmMessageWriter
void publishIdt(lcm::LCM *lcm, LcmMessageWriter<std::string> *idtWriter,
   Idt83pData *data)
{
   if (data && lcm && lcm->good() && idtWriter)
   {
      if (!idtWriter->set("valid", data->valid)) LOG_ERROR("failed to set valid");
      if (!idtWriter->set("pingtime", data->pingtime)) LOG_ERROR("failed to set pingtime");
      if (!idtWriter->set("timestamp", data->timestamp)) LOG_ERROR("failed to set timestamp");
      LOG_DEBUG("pingtime is %.2f", data->pingtime);

      if (!idtWriter->set("pingnumber", (int)data->pingnumber)) LOG_ERROR("failed to set pingnumber");
      if (!idtWriter->set("altitude", data->altitude)) LOG_ERROR("failed to set altitude");
      if (!idtWriter->set("interval", data->interval)) LOG_ERROR("failed to set interval");
      if (!idtWriter->set("nbeams", data->nbeams)) LOG_ERROR("failed to set nbeams");
      for (int i = 0; i < IDT_MAX_BEAMS; i++)
      {
         if (!idtWriter->set("range", data->range[i], i)) LOG_ERROR("failed to set range");
         if (!idtWriter->set("intense", data->intense[i], i)) LOG_ERROR("failed to set intense");
      }
      struct timeval spec;
      gettimeofday(&spec, NULL);
      double ms = (spec.tv_sec * 1000.) + (spec.tv_usec / 1000.);
      idtWriter->publish(*lcm, "DeltaT", (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> *dvlWriter,
   LcmMessageWriter<std::string> *depthWriter,
   OaVehState *data)
{
   if (data && lcm && lcm->good() && vehWriter && navWriter && dvlWriter && depthWriter)
   {
      struct timeval spec;
      gettimeofday(&spec, NULL);
      double ms = (spec.tv_sec * 1000.) + (spec.tv_usec / 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.;
      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);

      if (!dvlWriter->set("range", (float)data->altitude)) LOG_ERROR("failed to set range");
      dvlWriter->publish(*lcm, "RDI_Pathfinder", (int64_t)ms);

   }
}