/* 
  TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
  Based on work by and "distance_to" courtesy of Maarten Lamers.
  Copyright (C) 2008-2011 Mikal Hart
  All rights reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/




#include "gps.h"

#define _GPRMC_TERM   "GPRMC"
#define _GPGGA_TERM   "GPGGA"
#define _GPVTG_TERM   "GPVTG"

/*TinyGPS::TinyGPS()
:  _time(GPS_INVALID_TIME)
,  _date(GPS_INVALID_DATE)
,  _latitude(GPS_INVALID_ANGLE)
,  _longitude(GPS_INVALID_ANGLE)
,  _altitude(GPS_INVALID_ALTITUDE)
,  _speed(GPS_INVALID_SPEED)
,  _course(GPS_INVALID_ANGLE)
,  _last_time_fix(GPS_INVALID_FIX_TIME)
,  _last_position_fix(GPS_INVALID_FIX_TIME)
,  _parity(0)
,  _is_checksum_term(false)
,  _sentence_type(_GPS_SENTENCE_OTHER)
,  _term_number(0)
,  _term_offset(0)
,  _gps_data_good(false)
#ifndef _GPS_NO_STATS
,  _encoded_characters(0)
,  _good_sentences(0)
,  _failed_checksum(0)
#endif
{
  _term[0] = '\0';
}
 */
 //   unsigned long _time, _new_time;
//    unsigned long _date, _new_date;
	char _date[7], _new_date[7];
	char _time[7], _new_time[7];
 //   long _latitude, _new_latitude;
 //   long _longitude, _new_longitude;
char _latitude[10], _new_latitude[10];
char _longitude[11], _new_longitude[11];
    long _altitude, _new_altitude;
    unsigned long  _speed, _new_speed;
    unsigned long  _course, _new_course;
	byte _sats, _new_sats;

    unsigned long _last_time_fix, _new_time_fix;
    unsigned long _last_position_fix, _new_position_fix;

	    byte _parity;
    bool _is_checksum_term;
    char _term[15];
    byte _sentence_type;
    byte _term_number;
    byte _term_offset;
    bool _gps_data_good;



uint8_t parse(char c)
{
  bool valid_sentence = false;

 
  switch(c)
  {
  case ',': // term terminators
    _parity ^= c;
  case '\r':
  case '\n':
  case '*':
    if (_term_offset < sizeof(_term))
    {
      _term[_term_offset] = 0;
      valid_sentence = term_complete();
    }
    ++_term_number;
    _term_offset = 0;
    _is_checksum_term = c == '*';
    return valid_sentence;

  case '$': // sentence begin
    _term_number = _term_offset = 0;
    _parity = 0;
    _sentence_type = _GPS_SENTENCE_OTHER;
    _is_checksum_term = false;
    _gps_data_good = false;
    return valid_sentence;
  }

  // ordinary characters
  if (_term_offset < sizeof(_term) - 1)
    _term[_term_offset++] = c;
  if (!_is_checksum_term)
    _parity ^= c;

  return valid_sentence;
}



//
// internal utilities
//
int from_hex(char a) 
{
  if (a >= 'A' && a <= 'F')
    return a - 'A' + 10;
  else if (a >= 'a' && a <= 'f')
    return a - 'a' + 10;
  else
    return a - '0';
}


/*
#ifndef _GPS_NO_STATS
void TinyGPS::stats(unsigned long *chars, unsigned short *sentences, unsigned short *failed_cs)
{
  if (chars) *chars = _encoded_characters;
  if (sentences) *sentences = _good_sentences;
  if (failed_cs) *failed_cs = _failed_checksum;
}
#endif

//
// internal utilities
//
*/
unsigned long parse_decimal()
{ unsigned long ret;
  char *p = _term;
  bool isneg = *p == '-';
  if (isneg) ++p;
  ret = 100UL * gpsatol(p);
  while (gpsisdigit(*p)) ++p;
  if (*p == '.')
  {
    if (gpsisdigit(p[1]))
    {
      ret += 10 * (p[1] - '0');
      if (gpsisdigit(p[2]))
        ret += p[2] - '0';
    }
  }
  return isneg ? -ret : ret;
}

byte parse_sat()
{ unsigned long ret;
  char *p = _term;
  ret = gpsatol(p);

  return (byte)ret;
}

unsigned long parse_degrees()
{
  char *p;
  unsigned long left = gpsatol(_term);
  unsigned long tenk_minutes = (left % 100UL) * 10000UL;
  for (p=_term; gpsisdigit(*p); ++p);
  if (*p == '.')
  {
    unsigned long mult = 1000;
    while (gpsisdigit(*++p))
    {
      tenk_minutes += mult * (*p - '0');
      mult /= 10;
    }
  }
  return (left / 100) * 100000 + tenk_minutes / 6;
}	  

/*
// Processes a just-completed term
// Returns true if new sentence has just passed checksum test and is validated
*/
bool term_complete(void)
{ uint8_t pom,r;
  if (_is_checksum_term)
  {
    byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]);
    if (checksum == _parity)
    {
      if (_gps_data_good)
      {

        _last_time_fix = _new_time_fix;
        _last_position_fix = _new_position_fix;

        switch(_sentence_type)
        {
        case _GPS_SENTENCE_GPRMC:
   //       _time      = _new_time;
   //       _date      = _new_date;
		  for(r=0;r<7;r++) _date[r]=_new_date[r];
		  for(r=0;r<7;r++) _time[r]=_new_time[r];
  //        _latitude  = _new_latitude;
  //        _longitude = _new_longitude;
		  for(r=0;r<9;r++) _latitude[r]=_new_latitude[r];
		  for(r=0;r<10;r++) _longitude[r]=_new_longitude[r];
          _speed     = _new_speed;
          _course    = _new_course;
          break;
        case _GPS_SENTENCE_GPGGA:
		  _sats  = _new_sats;
          _altitude  = _new_altitude;
  //        _time      = _new_time;
//          _latitude  = _new_latitude;
 //         _longitude = _new_longitude;
          break;
        }

        return true;
      }
    }


    return false;
  }

  // the first term determines the sentence type
  if (_term_number == 0)
  {
    if (!gpsstrcmp(_term, _GPRMC_TERM))
      _sentence_type = _GPS_SENTENCE_GPRMC;
    else if (!gpsstrcmp(_term, _GPGGA_TERM))
      _sentence_type = _GPS_SENTENCE_GPGGA;
    else if (!gpsstrcmp(_term, _GPVTG_TERM))
      _sentence_type = _GPS_SENTENCE_GPVTG;
    else
      _sentence_type = _GPS_SENTENCE_OTHER;
    return false;
  }

  if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]){
  if(_sentence_type == _GPS_SENTENCE_GPGGA) pom=200;
  else if(_sentence_type == _GPS_SENTENCE_GPVTG) pom=230;
  else pom=100;
  pom=pom+_term_number;
//  switch((_sentence_type == _GPS_SENTENCE_GPGGA ? 200 : 100) + _term_number)
	switch(pom)
  {
    case 101: // Time in both sentences
    case 201:
  //    _new_time = parse_decimal();
      _new_time_fix = 10;
    		  for(r=0;r<7;r++) _new_time[r]=_term[r];
      break;
    case 102: // GPRMC validity
      _gps_data_good = _term[0] == 'A';
      break;
    case 103: // Latitude
    case 202:
 //     _new_latitude = parse_degrees();
 	 for(r=0;r<10;r++) _new_latitude[r]=_term[r];
      _new_position_fix = 10;
      break;
    case 104: // N/S
    case 203:
 //     if (_term[0] == 'S')
 //       _new_latitude = -_new_latitude;
  	 _new_latitude[8]=_term[0];
      break;
    case 105: // Longitude
    case 204:
   //   _new_longitude = parse_degrees();
  	 for(r=0;r<11;r++) _new_longitude[r]=_term[r];  
      break;
    case 106: // E/W
    case 205:
   //   if (_term[0] == 'W')
   //     _new_longitude = -_new_longitude;
        	 _new_longitude[9]=_term[0];
	  break;
//    case 107: // Speed (GPRMC)
	  case 237:	// Speed (GPVTG)
      _new_speed = parse_decimal();
      break;
    case 108: // Course (GPRMC)
      _new_course = parse_decimal();
      break;
    case 109: // Date (GPRMC)
     // _new_date = gpsatol(_term);
     		  for(r=0;r<7;r++) _new_date[r]=_term[r];
	  break;
    case 206: // 6.term Fix data (GPGGA)
      _gps_data_good = _term[0] > '0';
      break;
    case 207: // 7.term Num sats (GPGGA)
      _new_sats = parse_sat();
      break;
    case 209: // 9.term Altitude (GPGGA)
      _new_altitude = parse_decimal();
      break;
  }
}
  return false;
}

long gpsatol(const char *str)
{
  long ret = 0;
  while (gpsisdigit(*str))
    ret = 10 * ret + *str++ - '0';
  return ret;
}

int gpsstrcmp(const char *str1, const char *str2)
{
  while (*str1 && *str1 == *str2)
    ++str1, ++str2;
  return *str1;
}

bool gpsisdigit(char c) { return c >= '0' && c <= '9'; }


void get_position(void)
   {
     u8 r;
   extern char lat[10],lon[11];
		 for(r=0;r<9;r++) lat[r]=_latitude[r];
   		 for(r=0;r<10;r++) lon[r]=_longitude[r];
 	}
   void get_date(void)
   {
   u8 r;
   extern char datum[7];
		 for(r=0;r<6;r++) datum[r]=_date[r];
   }
   void get_time(void)
   {
   u8 r;
   extern char cas[7];
		 for(r=0;r<6;r++) cas[r]=_time[r];
   }
     // signed altitude in centimeters (from GPGGA sentence)
  long altitude(void) { return _altitude; }

     // course in last full GPRMC sentence in 100th of a degree
   unsigned long course(void) { return _course; }

   // speed in last full GPRMC sentence in 100ths of a knot
       unsigned long speed(void) { return _speed; }

    byte numsats(void) { return _sats; }
/*
float TinyGPS::distance_between (float lat1, float long1, float lat2, float long2) 
{
  // returns distance in meters between two positions, both specified 
  // as signed decimal-degrees latitude and longitude. Uses great-circle 
  // distance computation for hypothetical sphere of radius 6372795 meters.
  // Because Earth is no exact sphere, rounding errors may be up to 0.5%.
  // Courtesy of Maarten Lamers
  float delta = radians(long1-long2);
  float sdlong = sin(delta);
  float cdlong = cos(delta);
  lat1 = radians(lat1);
  lat2 = radians(lat2);
  float slat1 = sin(lat1);
  float clat1 = cos(lat1);
  float slat2 = sin(lat2);
  float clat2 = cos(lat2);
  delta = (clat1 * slat2) - (slat1 * clat2 * cdlong); 
  delta = sq(delta); 
  delta += sq(clat2 * sdlong); 
  delta = sqrt(delta); 
  float denom = (slat1 * slat2) + (clat1 * clat2 * cdlong); 
  delta = atan2(delta, denom); 
  return delta * 6372795; 
}
 */

