/****************************************************************************/
/* Copyright (c) 2016 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : Power Buoy Navigation process for mobile flapping-foil buoy.  */
/* Filename : Navigator.c                                                   */
/* Author   : Henthorn                                                      */
/* Project  : Power Buoy                                                    */
/* Version  : 1.0                                                           */
/* Created  : 07/20/2016                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */

/*


typedef struct webnav_data {} WebNavData;
int webnav_init(char* mission_file, WebNavData *wnd);
int webnav_close(WebNavData *wnd);
int webnav_gps_update(WebNavData *wnd);
int webnav_pos_update(WebNavData *wnd, float lat_degrees, float long_degrees);
int webnav_filter(WebNavData *wnd, param1, float param2);
int webnav_get_gps(float *lat_degrees, float *long_degrees);
int webnav_get_position(WebNavData *wnd, float *lat_degrees, float *long_degrees);
int webnav_get_distance(WebNavData *wnd, float *meters);
int webnav_get_bearing(WebNavData *wnd, float *degrees);
int webnav_get_waypoint(WebNavData *wnd, Waypoint *wp);

main()
{
   WebNavData wnd;

   // Initialize mission and GPS comms.
   // Run mission with 5 second GPS updates.
   // 
   if (0 == webnav_init("mission-file", &wnd))
   {
      while (0 == webnav_gps_update(&wnd)) ; //sleep(5);
   }
   webnav_close(WebNavData &wnd);

   // Initialize mission and GPS comms.
   // Run mission with inserted gps positions (interactive test).
   // 
   if (0 <= webnav_init("mission-file", &wnd))
   {
      while (1) {
         // Get lat and long from somewhere
         //
         if (0 != webnav_pos_update(&wnd, lat, lon)) break;
      }FOREVER
   }
   webnav_close(WebNavData &wnd);

   // Initialize NO mission and GPS comms.
   // Station-keep at current position with 5 second GPS updates.
   // 
   if (0 <= webnav_init(NULL, &wnd))
   {
      while (0 == webnav_gps_update(&wnd)) ; //sleep(5);
   }
   webnav_close(WebNavData &wnd);

}

/****************************************************************************/

#include <gps.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <math.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>

#include "NavigationControlLoop.h"

#define  MAX_WAYPOINTS   20
#define  DEFAULT_RADIUS 100   // meters
#define  DEFAULT_REPEAT   1
#define  REPEAT_FOREVER   0

//****************************************************************************/
// Data structures and types
// 

#define TEST_MODE         0x01
#define INTERACTIVE_MODE  0x02

typedef struct _options {
  char   mode;
  int    nupdates;
  double gain;
} Options;

typedef struct _waypoint
{
   unsigned int radius;
   float  latitude;
   float  longitude;
} Waypoint;

typedef struct _mission
{
   unsigned int nwaypoints;
   Waypoint waypoints[MAX_WAYPOINTS];
   unsigned int repeat; 
} Mission;

//****************************************************************************/
// Function prototypes
// 
// Read the mission file and populate the mission structure
// 
int load_mission(const char *filename, Mission *mission);

// Run the mission using the mission structure
// 
int run_mission(Mission *mission, Options *options);

// Use current position rudder commands to navigate to the given waypoint
// 
int goto_waypoint(Waypoint *wp, Options *options);

// GPS daemon interface
//
static struct gps_data_t latest_gps;
int get_gps();

int open_log();
int write_log(float tlat, float tlon, float trad, 
              float clat, float clon, float dist, float head);
FILE *navlog;

float to_radians(float degrees);

//****************************************************************************/
void usage(const char* app)
{
   printf("Usage: %s\n %s -f config-file [-t] [-i] [-g] [-n] \n", app, app);
   printf(" -f  config-file  Required mission configuration file\n");
   printf(" -t               Run in 'test mode' where no commands are sent to rudder controller\n");
   printf(" -i               Run in 'interactive mode' where user is prompted for lat/long coordinates\n");
   printf(" -g               Gain to use in rudder command cals (g >= 0.0)\n");
   printf(" -n               Number of updates before executing rudder command cals (n > 0)\n");
   printf("                  Keep in mind that gpsd runs at approx 4 Hz\n");
}


//****************************************************************************/
// main program
// 
//
int main(int argc, char const *argv[])
{
   Mission mission;
   // Parse command line, set options, etc.
   // 
   char *config = 0;
   Options options;
   options.mode = 0;
   options.nupdates = 90;   // gpsd runs ~4 Hz, so this is ~20 period
   options.gain = 0.5;
   int c;
   while ( (c = getopt(argc, (char *const *)argv, "tif:g:n:")) != -1 )
   {
      switch (c)
      {
         case 't':
            options.mode |= TEST_MODE;
            break;

         case 'i':
            options.mode |= INTERACTIVE_MODE;
            break;

         case 'f':
            config = strdup(optarg);
            break;

         case 'g':
            options.gain = atof(optarg);
            break;

         case 'n':
            options.nupdates = atoi(optarg);
            break;

         default:
            usage(argv[0]);
            break;

      }
   }

   if (!config || options.nupdates <= 0 || options.gain < 0.)
   {
      usage(argv[0]);
      return -1;
   }

   printf("Options: mode=%d gain=%f nupdates=%d\n",
	  options.mode, options.gain, options.nupdates);

   // Read the mission from the configuration file
   // 
   if (load_mission(config, &mission)) return -1;

   // Open interface to gpsd if necessary (not needed in interactive mode)
   //
   if (!(options.mode & INTERACTIVE_MODE))
   {
      int rc;
      if ((rc = gps_open("localhost", "2947", &latest_gps)) == -1)
      {
         printf("gpsd failure - code: %d, reason: %s\n", rc, gps_errstr(rc));
         return EXIT_FAILURE;
      }
      else
      {
         gps_stream(&latest_gps, WATCH_ENABLE | WATCH_JSON, NULL);
         printf("gpsd connected!\n");
      }
   }

   // Open the log file
   //
   if (0 > open_log())
   {
      printf("Failed to open log file!\n");
      return -1;
   }

   // Execute the mission
   // 
   run_mission(&mission, &options);

   return 0;
}

//****************************************************************************/
// Run the mission using the mission structure
// 
int run_mission(Mission *mission, Options *options)
{
   // Outer loop repeats the inner loop of waypoints
   // 
   char forever = (mission->repeat == REPEAT_FOREVER);
   unsigned int repeat, wp;
   for (repeat = 0; repeat < mission->repeat || forever; repeat++)
   {
      for (wp = 0; wp < mission->nwaypoints; wp++)
      {
         goto_waypoint(&mission->waypoints[wp], options);
         printf("Go to %d\n", wp+1);
         sleep(1);
      }
   }

   // All done, continuously station keep around the last waypoint
   // 
   while (1)
   {
      printf("Station-keep at final waypoint\n");
      goto_waypoint(&mission->waypoints[mission->nwaypoints-1], 0);
      sleep(1);
   }
   return 0;
}

//****************************************************************************/
// Use current position rudder commands to navigate to the given waypoint.
// GPS hits trigger activity and update rate.
// 
int goto_waypoint(Waypoint *wp, Options *options)
{
   printf("Going to %f / %f within %d meters\n",
      wp->latitude, wp->longitude, wp->radius);

   double distance;
   static int _last_cmd = -1;
   short hits = -1, cmd = -1;
   do
   {
      // Get the current position. If interactive-test option enabled, query
      // for lat/long, otherwise get it from gpsd
      // 
      float clat, clon;
      if (options->mode & INTERACTIVE_MODE)
      {
         printf("Enter the current decimal latitude >");
         fscanf(stdin, "%f", &clat);
         printf("Enter the current decimal longitude >");
         fscanf(stdin, "%f", &clon);
         printf("Calculating distance and heading to %f / %f\n", clat, clon);
      }
      else
      {
         if (get_gps())
         {
            if (hits == 0)
            {
               printf("Receiving valid gps hits...\n"); fflush(stdout);
               hits = 1;
            }

            // Calculate rudder command using control l00p
            //
            clat = latest_gps.fix.latitude;
            clon = latest_gps.fix.longitude;
         }
         else
         {
            if (hits != 0)
            {
               printf("Not receiving valid gps hits yet, still trying...\n"); fflush(stdout);
               hits = 0;
            }
            continue;
         }
      }
      double wplatlon[2]  = {wp->latitude, wp->longitude};
      double gpslatlon[2] = {clat, clon};
      cmd = ThrustCommandLoop(&distance, gpslatlon, wplatlon,
                              options->gain, options->nupdates);

      if (cmd >= 0 && cmd != _last_cmd)
      {
         printf("Unadjusted cmd is: %d\n", cmd); fflush(stdout);
         _last_cmd = cmd;

         // Adjust to magnetic
         //
         cmd -= 13.;
         if (cmd < 0) cmd += 360.;

         char cmdstr[15];
         sprintf(cmdstr, "rctarget %3d", cmd);
         printf("Distance to waypoint: %.1f m\n", distance); fflush(stdout);
         if (options->mode & TEST_MODE)
         {
            printf("%s (not executing)\n", cmdstr);
            fflush(stdout);
         }
         else
         {
            printf("%s (executing!)\n", cmdstr);
            fflush(stdout);
            system(cmdstr);
         }
      }

      // Calculate distance using the haversine formula
      // 
      float phi1 = to_radians(clat);
      float phi2 = to_radians(wp->latitude);
      //printf("phi1 = %f and phi2 = %f\n", phi1, phi2);

      float delta_phi    = to_radians(wp->latitude)  - to_radians(clat);
      float delta_lambda = to_radians(wp->longitude) - to_radians(clon);
      //printf("delta_phi = %f and delta_lambda = %f\n", delta_phi, delta_lambda);

      float a = sinf(delta_phi/2) * sinf(delta_phi/2) +
                cosf(phi1) * cosf(phi2) * sinf(delta_lambda/2) * sinf(delta_lambda/2);
      float c = 2 * atan2(sqrt(a), sqrt(1-a));
      distance = 6371000L * c;

      // We're done if the distance is within the radius
      //
      printf("Distance is %.3f meters\n", distance);
      if (distance <= wp->radius) 
         break;

      // Otherwise, calculate bearing
      // 
      float y = sinf( to_radians(wp->longitude) - to_radians(clon) ) * cosf(phi2);
      float x = ( cosf(phi1) * sinf(phi2) ) - 
                   sinf(phi1) * cosf(phi2) * cosf( to_radians(wp->longitude) - to_radians(clon) );
      float b = atan2(y, x) * 180 / M_PI;
      b -= 13.;

      float brng = b < 0.0? b + 360 : b;

      write_log(wp->latitude, wp->longitude, wp->radius,
                clat, clon, distance, brng);

      printf("Heading is %.3f degrees\n", brng);
      //if (!(options->mode & INTERACTIVE_MODE)) sleep(5);

   } while (1);

   return 0;
}


//****************************************************************************/
// Read the mission file and populate the mission structure
// 
int load_mission(const char *filename, Mission *mission)
{
   char fbuf[200];

   // Open the config file
   // 
   FILE *cf = fopen(filename, "r");
   if (!cf)
   {
      printf("Error opening file %s \n", filename);
      return -1;
   }


   // Read and parse each line
   // 
   mission->nwaypoints = 0;
   mission->repeat     = DEFAULT_REPEAT;
   int errors = 0, lines = 0;
   float p1, p2, p3, radius = DEFAULT_RADIUS;
   for (lines = 1; fgets(fbuf, sizeof(fbuf), cf); lines++)
   {
      // Strip comments and skip the line if necessary
      // 
      char *pound = index(fbuf, '#');
      if (pound) *pound = '\0';
      if (strlen(fbuf) < 1) continue;

      // Strip commas
      // 
      char *comma;
      while (comma = index(fbuf, ',')) *comma = ' ';

      // Extract the commands
      // 
      char cmd[20];
      cmd[0] = '\0';
      p1 = p2 = p3 = 0;
      int n = sscanf(fbuf, "%s %*s %f %f %f", cmd, &p1, &p2, &p3);

      // Skip blank lines (no items parsed)
      // 
      printf("Parsed %d items\n", n);
      if (n <= 0) continue;

      // Handle the keyword lines individually here
      // 
      //      RADIUS
      //
      if (!strcasecmp(cmd, "radius"))
      {
         if (p1 )
         radius = p1;
         printf("Setting radius to %f\n", p1);
      }

      //      REPEAT
      //
      else if (!strcasecmp(cmd, "repeat"))
      {
         if (strcasestr(fbuf, "forever"))
            mission->repeat = REPEAT_FOREVER;
         else if (p1 <= 0.)
         {
            printf("\nError detected on line %d: bad repeat value %f \n\n",
               lines, p1);
            errors++;
         }
         else
            mission->repeat = p1;
   
         printf("Setting repeat to %d\n", mission->repeat);
      }

      //      WAYPOINT
      //
      else if (!strcasecmp(cmd, "waypoint"))
      {
         if (n < 4) p3 = radius;
         if (mission->nwaypoints == MAX_WAYPOINTS)
         {
            printf("\nError detected on line %d: too many waypoints\n\n", lines);
            errors++;
         }
         else
         {
            printf("Setting waypoint %d to %f, %f, %f\n",
               mission->nwaypoints+1, p1, p2, p3);
            mission->waypoints[mission->nwaypoints].latitude = p1;
            mission->waypoints[mission->nwaypoints].longitude = p2;
            mission->waypoints[mission->nwaypoints].radius = p3;
            mission->nwaypoints++;
         }
      }

      //      UNKNOWN
      //
      else
      {
         printf("\nError detected on line %d: Unknown keyword %s\n\n", lines, cmd);
         printf("%s\n", fbuf);
         errors++;
      }
   }

   return errors;
}

//****************************************************************************/
float to_radians(float degrees)
{
   return (degrees*M_PI) / 180.;
}

int get_gps()
{
   int rc;

   /* wait for 2 seconds to receive data */
   //if (gps_waiting (&latest_gps, 2000000L))
   if (gps_waiting (&latest_gps, 200000L))
   {
      /* read data */
      if ((rc = gps_read(&latest_gps)) == -1)
      {
         printf("error occured reading gps data. code: %d, reason: %s\n", rc, gps_errstr(rc));
      }
      else
      {
         /* Display data from the GPS receiver. */
         if ((latest_gps.status == STATUS_FIX) && 
            (latest_gps.fix.mode == MODE_2D || latest_gps.fix.mode == MODE_3D) &&
            !isnan(latest_gps.fix.latitude) && 
            !isnan(latest_gps.fix.longitude))
         {
            printf("latitude: %f, longitude: %f, speed: %f, timestamp: %ld\n",
               latest_gps.fix.latitude, latest_gps.fix.longitude, latest_gps.fix.speed);
            return 1;
         }
         else
         {
            printf("no GPS data read: fix.mode is %d\n", latest_gps.fix.mode);
         }
      }
   }
   else
   {
     printf("no gps data available.\n");
   }

   return 0;
}

int open_log()
{
   char _pathname[100];
   struct tm now;
   long _file_open_time;
   _file_open_time = time(&_file_open_time);
   localtime_r(&_file_open_time, &now);
   sprintf(_pathname, "%s/Nav-%4d.%02d.%02dT%02d.%02d.%02d.csv", "/logs/nav",
      now.tm_year+1900, now.tm_mon+1, now.tm_mday, now.tm_hour, now.tm_min, now.tm_sec);


   //printf("Attempting to open %s\n", _pathname);
   navlog = fopen(_pathname, "w+");
   if (!navlog)
   {
      perror("pbnav: ");
      return -1;
   }

   fprintf(navlog, "Timestamp,"
          "Target Lat,Target Lon,Target Radius,"
          "Current Lat,Current Lon,Distance to Target,Heading to Target\n");
   //write_header();
   return 0;
}

int write_log(float tlat, float tlon, float trad, 
              float clat, float clon, float dist, float head)
{
   if (navlog)
   {
      struct timeval time;
      gettimeofday( &time, 0 );
      double sec = time.tv_sec + time.tv_usec / 1000000.0;

      fprintf(navlog, "%.3f,%.3f, %.3f, %.1f, %.3f, %.3f, %.1f, %.1f\n",
         sec, tlat, tlon, trad, clat, clon, dist, head);
      fflush(navlog);
   }
}