/**
 * Eugene Marinelli
 * 4/16/08
 *
 * Ghetto NMEA GPS parser.
 */

#include <assert.h>
#include <healthnet_gumstix.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

typedef enum {GGA, GSA, GSV, RMC, VTG, GLL} MessageType;

#define NMEA_NUM_DATA_ENTRIES 20
#define MAX_DATA_LEN 40
#define STACK_SIZE 80

typedef struct {
  MessageType msg_type;
  char data[NMEA_NUM_DATA_ENTRIES][MAX_DATA_LEN];
  int index;
  int invalid; // set to 1 if it is determined that packet is invalid.
} GpsPacket;

typedef struct {
  int top;
  char stack[STACK_SIZE];
} CharStack;

static void stack_clear(CharStack* s);

static GpsData current_data;
static int location_available, time_available;

static GpsPacket cur_packet;
static CharStack parse_stack;

/**
 * @return 1 if useful data received, 0 if no useful data received, -1 if invalid packet.
 */
static int interpret_packet(GpsData* gpsdata, GpsPacket* pkt) {
  // TODO - verify checksum.

  MessageType mt = pkt->msg_type;
  char valid;
  char* timeofday;
  int hh, mm;
  float ss;

  switch (mt) {
  case RMC: // Recommended Minimum Specific GNSS Data.
    location_available = 0;

    // fprintf(stderr, "** GOT RMC **\n");
    timeofday = pkt->data[0];
    sscanf(timeofday, "%2d%2d%f", &hh, &mm, &ss);

    gpsdata->time.seconds = 60 * (hh * 60 + mm) + ss;

    // Change time format.
    char yy[3], mm[3], dd[3];
    sscanf(pkt->data[8], "%2s%2s%2s", dd, mm, yy);
    sprintf(gpsdata->time.date, "%s%s%s", yy, mm, dd);

    time_available = 1;

    valid = pkt->data[1][0];
    if (valid == 'V') {
      printf("RMC location is not valid (V)\n");
      return 1; // At least we have the time.
    } else if (valid != 'A') {
      return -1;
    }

    float deg, min;
    sscanf(pkt->data[2], "%2f%f", &deg, &min);
    gpsdata->location.latitude = deg + min / 60.0;

    printf("lat: %f\n", gpsdata->location.latitude);

    char lat_dir = (pkt->data[3])[0];
    if (lat_dir == 'N') {
      gpsdata->location.latitude_dir = NORTH;
      printf("north\n");
    } else if (lat_dir == 'S') {
      gpsdata->location.latitude_dir = SOUTH;
      printf("south\n");
    } else {
      return -1;
    }

    sscanf(pkt->data[4], "%3f%f", &deg, &min);
    gpsdata->location.longitude = deg + min / 60.0;

    printf("long: %f\n", gpsdata->location.longitude);

    char lon_dir = (pkt->data[5])[0];
    if (lon_dir == 'E') {
      gpsdata->location.longitude_dir = EAST;
      printf("east\n");
    } else if (lon_dir == 'W') {
      gpsdata->location.longitude_dir = WEST;
      printf("west\n");
    } else {
      return -1;
    }

    location_available = 1;
    return 1;

  case VTG: // Course Over Ground and Ground Speed
  case GSA:
  case GGA:
  case GSV:
  case GLL:
    return 0;

  default:
    return -1;
  }
}

void parser_init() {
  stack_clear(&parse_stack);
  cur_packet.index = 0;
  location_available = 0;
  time_available = 0;
}

/**
 * @return -1 if msgtype invalid, -2 if header invalid, 0 if valid.
 */
static int reduce_stack_to_msgtype(MessageType* result_buf, CharStack* s) {
  char* p = s->stack;
  MessageType mt;

  if (*p != '$') {
//    printf("first mistake=(%c=%d)\n", *p, *p);
    stack_clear(s);
    return -2;
  }

  p++;

  if (*p != 'G') {
    printf("second\n");
    stack_clear(s);
    return -2;
  }

  p++;

  if (*p != 'P') {
    printf("third\n");
    stack_clear(s);
    return -2;
  }

  p++;

/*  if ((*p++ != '$') || (*p++ != 'G') || (*p++ != 'P')) {
    fprintf(stderr, "invalid header:%s", s->stack);
    stack_clear(s);
    return -2;
    }*/

  if (*p == 'G') {
    p++;

    if (*p == 'G') {
      mt = GGA;
    } else if (*p == 'S') {
      p++;

      if (*p == 'A') {
        mt = GSA;
      } else {
        mt = GSV;
      }
    } else if (*p == 'L') {
      mt = GLL;
    }
  } else if (*p == 'R') {
    mt = RMC;
  } else if (*p == 'V') {
    mt = VTG;
  } else {
    //fprintf(stderr, "invalid message type:%s\n", s->stack);

    stack_clear(s);
    return -1;
  }

  stack_clear(s);
  *result_buf = mt;
  return 0;
}

static void packet_clear(GpsPacket* p) {
  memset(p, 0, sizeof(GpsPacket));
}

static void stack_push(CharStack* s, char c) {
  //assert(s->top < STACK_SIZE);
  if (s->top >= STACK_SIZE) {
    fprintf(stderr, "WARNING: STACK OVERFLOW. Resetting stack.");
    stack_clear(s);
  } else {
    s->stack[s->top++] = c;
  }
}

static void stack_clear(CharStack* s) {
  memset(s->stack, 0, STACK_SIZE);
  s->top = 0;
}

static void reduce_stack_to_data(char* target_buf, CharStack* s) {
  assert(s->top >= 0);
  if (s->top != 0) {
    memcpy(target_buf, s->stack, s->top);
  }

  stack_clear(s);
}

/**
 * Feed data to parser.
 * @return 0 on success, -1 on failure.
 */
int parser_feed(char* data) {
  char* itr = data;

  char c;
  while ((c = *itr++)) { // For each character...
    if (cur_packet.invalid) {
//      fprintf(stderr, "invalid\n");
      if (c == '\n') {
        // Now try to match the next line.
        packet_clear(&cur_packet);
      } else if (c == '$') {
        packet_clear(&cur_packet);
        stack_push(&parse_stack, '$');
      }
    } else {
      if (c == ',' || c == '\n') {
        // Hit a delimiter
        if (cur_packet.index == 0) {
          MessageType mt;
          int ret = reduce_stack_to_msgtype(&mt, &parse_stack);
          if (ret == 0) {
            cur_packet.msg_type = mt;
            cur_packet.index++;
          } else if (ret == -1) {
            fprintf(stderr, "%s: Received invalid message type.\n", __FUNCTION__);
            cur_packet.invalid = 1;
          } else if (ret == -2) {
            //fprintf(stderr, "%s: Invalid message header.\n", __FUNCTION__);
            cur_packet.invalid = 1;
          }
        } else {
          reduce_stack_to_data(cur_packet.data[cur_packet.index - 1], &parse_stack);
          cur_packet.index++;

          if (c == '\n') {
            int ret = interpret_packet(&current_data, &cur_packet);
            if (ret == -1) {
              fprintf(stderr, "%s: invalid packet received\n", __FUNCTION__);
            }

            packet_clear(&cur_packet);
          }
        }
      } else {
        stack_push(&parse_stack, c);
      }
    }
  }

  return 0;
}

/**
 * If available, sets current latitude and longitude.
 * @return 0 if location and time data available, 1 if just location, 2 if just time, -1 if unavailable.
 */
int parser_get_data(GpsData* gpsdata) {
  if (location_available || time_available) {
    *gpsdata = current_data;

    if (location_available && time_available) {
      return 0;
    } else if (location_available) {
      return 1;
    } else {
      return 2;
    }
  } else {
    return -1;
  }
}