#include <string.h>
#include <stdlib.h>
#include "GpsUtils.h"
#include "MathP.h"
#include "Syslog.h"

char GpsUtils::hemisphere(GpsIF::Hemisphere h)
{
  char errorBuf[100];

  switch (h) {
  case GpsIF::Northern:
    return 'N';
    break;

  case GpsIF::Southern:
    return 'S';
    break;

  case GpsIF::Eastern:
    return 'E';
    break;

  case GpsIF::Western:
    return 'W';
    break;

  default:
    sprintf(errorBuf,
	    "GpsUtils::hemisphere() - bad GpsIF::Hemisphere value: %d", h);

    throw Exception(errorBuf);
  }

  return 0;
}


GpsIF::Hemisphere GpsUtils::hemisphere(char hem)
{
  static GpsIF::Hemisphere h;
  char errorBuf[100];

  switch (hem) {
  case 'N':
    h = GpsIF::Northern;
    break;

  case 'S':
    h = GpsIF::Southern;
    break;

  case 'E':
    h = GpsIF::Eastern;
    break;

  case 'W':
    h = GpsIF::Western;
    break;

  default:
    sprintf(errorBuf,
	    "GpsUtils::hemisphere() - bad hemisphere value: %c", hem);

    throw Exception(errorBuf);
  }

  return h;
}


/*
NOTE: This method implementation is not very smart yet. The following
should probably be added:
  Check for "GGA" in the label
  Check for valid values in each field

Also, the NMEA GGA standard apparently just contains UTC time-of-day, and
does NOT include the date!

T.O'R.
*/
void GpsUtils::parseFix(const char *ggaSentence, GpsIF::Fix *fix)
{
  char errorBuf[100];

  enum {
    InLabel,
    InTime,
    InLatitude,
    InNsHemisphere,
    InLongitude,
    InEwHemisphere,
    InQuality,
    InNSatellites,
    InHdop,
    InAltitude,
    InAltitudeUnits,
    InGeoidHeight,
    InGeoidHeightUnits,
    InDgpsAge,
    InDgpsStationId,
    Finished
  };

  Boolean debug = False;
  dprintf("GpsUtils::parseFix() - %s\n", ggaSentence);

  int state = InLabel;

  // Checksum is at the end
  char *endPtr = strchr(ggaSentence, '*');
  char *checkSumPtr;

  if (endPtr) {
    checkSumPtr = endPtr;
  }
  else {
    // No checksum on this sentence
    checkSumPtr = 0;
    endPtr = (char *)ggaSentence + strlen(ggaSentence);
  }

  char buf[256];

  int nBytes = min(endPtr - ggaSentence, sizeof(buf));
  strncpy(buf, ggaSentence, nBytes);
  buf[nBytes] = '\0';
 
  char *token = (char *)buf;

  while (state != Finished && token != 0) {

    switch (state) {

    case InLabel:
      break;

    case InTime:
      // Don't know how to do this yet!!!
      break;

    case InLatitude:
      fix->latitude = GpsUtils::latitude(token);
      break;

    case InNsHemisphere:
      fix->nsHemisphere = GpsUtils::hemisphere(token[0]);
      break;

    case InLongitude:
      fix->longitude = GpsUtils::longitude(token);
      break;

    case InEwHemisphere:
      fix->ewHemisphere = GpsUtils::hemisphere(token[0]);
      break;

    case InQuality:
      fix->quality = (GpsIF::Quality )atoi(token);
      break;

    case InNSatellites:
      fix->nSatellites = atoi(token);
      break;

    case InHdop:
      fix->hdop = atof(token);
      break;

    case InAltitude:
      fix->altitude = atof(token);
      break;

    case InAltitudeUnits:
      break;

    case InGeoidHeight:
      fix->geoidHeight = atof(token);
      break;

    case InGeoidHeightUnits:
      break;

    case InDgpsAge:
      fix->dgpsDataAge = atoi(token);
      break;

    case InDgpsStationId:
      fix->dgpsStationId = atoi(token);
      break;
    }

    // Advance to next state
    state++;

    // Point to next token
    if ((token = strchr(token, ',')) == 0)
      // No more tokens
      break;

    token++;
  }

  if (state != Finished) {
    sprintf(errorBuf, "GpsUtils::parseFix() - incomplete GGA sentence?\n"
	    "%s",
	    ggaSentence);

    throw Exception(errorBuf);
  }


  if (token) {
    sprintf(errorBuf, "GpsUtils::parseFix() - Extra stuff in GGA sentence?\n"
	    "%s",
	    ggaSentence);

    throw Exception(errorBuf);
  }

  return;
}


double GpsUtils::latitude(const char *token)
{
  char *decimalPtr;
  char errorBuf[100];
  char buf[16];
  double value;

  // First two characters are degrees
  strncpy(buf, token, 2);
  value = atof(buf);

  token += 2;
  value += (atof(token) / 60.);

  // Return value in radians
  return (value * Math::RadsPerDeg);
}



double GpsUtils::longitude(const char *token)
{
  char *decimalPtr;
  char errorBuf[100];
  char buf[16];
  double value;

  // First three characters are degrees
  strncpy(buf, token, 3);
  value = atof(buf);

  token += 3;
  value += (atof(token) / 60.);

  // Return value in radians
  return (value * Math::RadsPerDeg);
}



char *GpsUtils::latitude(double value)
{
  static char string[16];

  // Convert from radians to degrees
  value /= Math::RadsPerDeg;

  double minutes = 60. * (value - int(value));

  sprintf(string, "%02d", (int )value);
  if (minutes < 10.)
    strcat(string, "0");

  sprintf(string + strlen(string), "%2.4f", minutes);

  return string;
}


char *GpsUtils::longitude(double value)
{
  // Convert from radians to degrees
  value /= Math::RadsPerDeg;

  static char string[16];

  double minutes = 60. * (value - int(value));

  sprintf(string, "%03d", (int )value);
  if (minutes < 10.)
    strcat(string, "0");

  sprintf(string + strlen(string), "%2.4f", minutes);

  return string;
}