#include "webnav.h"

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

#define NO_MISSION      0x01
#define NO_GPSD         0x02
#define POS_UPDATE      0x04

static int load_mission(const char *filename, Mission *mission);
static int get_gps(struct gps_data_t *gps_data);
static float to_radians(float degrees);
static int increment_waypointer(WebNavData *wnd);

int webnav_init(const char* mission_file, WebNavData *wnd)
{
   int return_val = 0;

   wnd->status = wnd->f_lat = wnd->f_lon = wnd->wp = wnd->reps = \
                 wnd->distance = wnd->bearing = 0;

   printf("webnav_init()...\n");
   if (!wnd)
   {
      printf("webnav_init() - NULL WebNavData reference!\n");
      return -1;
   }

   // Read the mission from the configuration file
   // 
   if (mission_file)
   {
      if (load_mission(mission_file, &wnd->mission)) return -1;
      wnd->reps = wnd->mission.repeat;
      printf("webnav_init() - Mission loaded OK\n");
   }
   else
   {
      return_val |= NO_MISSION;
   }

   // Open interface to gpsd if necessary (not needed in interactive mode)
   //
   int rc;
   if ((rc = gps_open("localhost", "2947", &wnd->gps_data)) == -1)
   {
      printf("webnav_init() - gpsd failure - code: %d, reason: %s\n", rc, gps_errstr(rc));
      return_val |= NO_GPSD;
   }
   else
   {
      gps_stream(&wnd->gps_data, WATCH_ENABLE | WATCH_JSON, NULL);
      printf("webnav_init() - gpsd connected!\n");
   }

   wnd->status |= return_val;

   return return_val;
}

int webnav_close(WebNavData *wnd)
{
   printf("webnav_close()...\n");
   gps_stream(&wnd->gps_data, WATCH_DISABLE, NULL);
   gps_close (&wnd->gps_data);
   printf("webnav_close() - gpsd connection closed\n");

   return 0;
}

int webnav_run(WebNavData *wnd)
{
   if (wnd->status & POS_UPDATE)
   {
      printf("Using inserted lat/long %d\n", wnd->status);
      wnd->status &= ~POS_UPDATE;
      printf("Cleared POS_UPDATE bit %d\n", wnd->status);
   }
   else if (0 != webnav_gps_update(wnd))
   {
      return -1;
   }

   Waypoint *pwp = &wnd->mission.waypoints[wnd->wp];

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

   float delta_phi    = to_radians(pwp->latitude)  - to_radians(wnd->f_lat);
   float delta_lambda = to_radians(pwp->longitude) - to_radians(wnd->f_lon);
   //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));
   wnd->distance = 6371000L * c;

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

   // Otherwise, calculate bearing
   // 
   float y = sinf( to_radians(pwp->longitude) - to_radians(wnd->f_lon) ) * cosf(phi2);
   float x = ( cosf(phi1) * sinf(phi2) ) - 
                sinf(phi1) * cosf(phi2) * cosf( to_radians(pwp->longitude) - to_radians(wnd->f_lon) );
   float b = atan2(y, x) * 180 / M_PI;

   wnd->bearing = b < 0.0? b + 360 : b;

   return 0;
}

int webnav_gps_update(WebNavData *wnd)
{
   printf("webnav_gps_update()...\n");

   if (wnd->status & NO_GPSD)
   {
      printf("webnav_gps_update() - no gpsd connection!\n");
      return -1;
   }

   if (0 != get_gps(&wnd->gps_data))
   {
      return -1;
   }

   wnd->f_lat = wnd->gps_data.fix.latitude;
   wnd->f_lon = wnd->gps_data.fix.longitude;

   return 0;
}

int webnav_pos_update(WebNavData *wnd, float lat_degrees, float long_degrees)
{
   printf("webnav_pos_update()...\n");

   wnd->f_lat = lat_degrees;
   wnd->f_lon = long_degrees;

   wnd->status |= POS_UPDATE;

   return 0;
}

int webnav_filter(WebNavData *wnd, float param1, float param2)
{
   printf("webnav_filter()...\n");
   return 0;
}

int webnav_get_gps(WebNavData *wnd, struct gps_data_t *gps_data)
{
   printf("webnav_get_gps()...\n");

   return get_gps(&wnd->gps_data);
}

int webnav_get_position(WebNavData *wnd, float *lat_degrees, float *long_degrees)
{
   printf("webnav_get_position()...\n");

   *lat_degrees = wnd->f_lat;
   *long_degrees = wnd->f_lon;

   return 0;
}

int webnav_get_distance(WebNavData *wnd, float *meters)
{
   printf("webnav_get_distance()...\n");

   *meters = wnd->distance;

   return 0;
}

int webnav_get_bearing(WebNavData *wnd, float *degrees)
{
   printf("webnav_get_bearing()...\n");

   *degrees = wnd->bearing;

   return 0;
}

int webnav_get_waypoint(WebNavData *wnd, Waypoint *wp)
{
   printf("webnav_get_waypoint()...\n");

   if (wnd->status & NO_MISSION) return -1;

   memcpy(wp, &wnd->mission.waypoints[wnd->wp], sizeof(Waypoint));

   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++;
      }
   }

   fclose(cf);
   return errors;
}

int get_gps(struct gps_data_t *latest_gps)
{
   int rc;

   /* wait for 2 seconds to receive data */
   if (gps_waiting (latest_gps, 2000000))
   {
      /* 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 0;
         }
         else
         {
            printf("no GPS data available: fix.mode is %d\n", latest_gps->fix.mode);
         }
      }
    }
    else
    {
      printf("no gps data.\n");
   }

   return -1;
}

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

int increment_waypointer(WebNavData *wnd)
{
   if ((wnd->wp + 1) < wnd->mission.nwaypoints)
   {
      wnd->wp += 1;
   }

   else if (wnd->reps > 1)
   {
      wnd->reps -= 1;
      wnd->wp = 0;
   }

   else if (wnd->mission.repeat == REPEAT_FOREVER)
   {
      wnd->wp = 0;
   }

   printf("increment_waypointer() set wp to %d\n", wnd->wp);
   return 0;
}