#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "nmea.h"

/*
 * GPGGA-Record untersuchen und auswerten
 */
void nmea_parse_gpgga(char *nmea, struct gpgga *loc)
  {
  char *p = nmea;

  p = strchr(p, ',')+1;
  p = strchr(p, ',')+1;
  loc->latitude = atof(p);

  p = strchr(p, ',')+1;
  switch (p[0])
    {
    case 'N':
      loc->lat = 'N';
      break;
    case 'S':
      loc->lat = 'S';
      break;
    case ',':
      loc->lat = '\0';
      break;
    }
  p = strchr(p, ',')+1;
  loc->longitude = atof(p);
  p = strchr(p, ',')+1;
  switch (p[0])
    {
    case 'W':
      loc->lon = 'W';
      break;
    case 'E':
      loc->lon = 'E';
      break;
    case ',':
      loc->lon = '\0';
      break;
    }
  p = strchr(p, ',')+1;
  loc->quality = ( char)atoi(p);
  p = strchr(p, ',')+1;
  loc->satellites = ( char)atoi(p);
  p = strchr(p, ',')+1;
  p = strchr(p, ',')+1;
  loc->altitude = atof(p);
  }

/*
 * GPRMC-Record untersuchen und auswerten
 */
void nmea_parse_gprmc( char *nmea, struct gprmc *loc)
  {
  char *p = nmea;

  p = strchr(p, ',')+1; /* store Time as double */
  loc->time = atof(p);
  p = strchr(p, ',')+1; /* skip status */
  p = strchr(p, ',')+1;
  loc->latitude = atof(p);
  p = strchr(p, ',')+1;
  switch (p[0])
    {
    case 'N':
      loc->lat = 'N';
      break;
    case 'S':
      loc->lat = 'S';
      break;
    case ',':
      loc->lat = '\0';
      break;
    }
  p = strchr(p, ',')+1;
  loc->longitude = atof(p);
  p = strchr(p, ',')+1;
  switch (p[0])
    {
    case 'W':
      loc->lon = 'W';
      break;
    case 'E':
      loc->lon = 'E';
      break;
    case ',':
      loc->lon = '\0';
      break;
    }
  p = strchr(p, ',')+1;
  loc->speed = atof(p);
  p = strchr(p, ',')+1;
  loc->course = atof(p);
  p = strchr(p, ',')+1;
  loc->date = atoi(p); /* store Date as long int */
  }

/*
 * Messagetyp ermitteln (GPGGA, GPRMC, etc..)
 * Ausserdem: Filtern fehlerhafter Pakete (falsche Checksum)
 * Als Parameter message den Messagetyp uebergeben
 * Return-Wert: Der Messagetyp sofern gueltig
 */
char nmea_get_message_type( char *message)
  {
   char checksum = 0;
  if ((checksum = nmea_valid_checksum(message)) != N_EMPTY)
    {
    return checksum;
    }

    if (strstr(message, N_GPGGA_STR) != NULL)
      {
      return N_GPGGA;
      }

    if (strstr(message, N_GPRMC_STR) != NULL)
      {
      return N_GPRMC;
      }
  return N_UNKNOWN;
  }

 /*
  * Checksum eines Records pruefen
  */
 char nmea_valid_checksum( char *message)
  {
  char checksum = ( char)strtol(strchr(message, '*')+1, NULL, 16);
  char p;
  char sum = 0;
  ++message;
  while ((p = *message++) != '*')
    {
    sum ^= p;
    }

    if (sum != checksum)
      {
      return N_CHECKSUM_ERR;
      }
  return N_EMPTY;
  }

/*
 * Laengen- oder Breitenangabe von Grad in Dezimalnotation umrechnen
 */
double gps_deg_dec(double deg_point)
  {
  double ddeg;
  double sec = modf(deg_point, &ddeg)*60;
  int deg = (int)(ddeg/100);
  int min = (int)(deg_point-(deg*100));
  double absdlat = round(deg * 1000000.0);
  double absmlat = round(min * 1000000.0);
  double absslat = round(sec * 1000000.0);
  return round(absdlat + (absmlat/60) + (absslat/3600)) /1000000.0;
  }

/*
 * Laenge und Breite von Grad in Dezimalnotation umrechnen
 */
void gps_deg(double *latitude, char ns, double *longitude, char we)
  {
  double lat = (ns == 'N') ? *latitude : -1 * (*latitude);
  double lon = (we == 'E') ? *longitude : -1 * (*longitude);

  *latitude = gps_deg_dec(lat);
  *longitude = gps_deg_dec(lon);
  }