#include <Wire.h>
#include <rgb_lcd.h>
#include <LGPS.h>

gpsSentenceInfoStruct info;
rgb_lcd lcd;

const int colorR = 255;
const int colorG = 0;
const int colorB = 0;

char buff[256];

static unsigned char getComma(unsigned char num,const char *str)
{
  unsigned char i,j = 0;
  int len=strlen(str);
  for(i = 0;i < len;i ++)
  {
     if(str[i] == ',')
      j++;
     if(j == num)
      return i + 1; 
  }
  return 0; 
}

static double getDoubleNumber(const char *s)
{
  char buf[10];
  unsigned char i;
  double rev;
  
  i=getComma(1, s);
  i = i - 1;
  strncpy(buf, s, i);
  buf[i] = 0;
  rev=atof(buf);
  return rev; 
}

static double getIntNumber(const char *s)
{
  char buf[10];
  unsigned char i;
  double rev;
  
  i=getComma(1, s);
  i = i - 1;
  strncpy(buf, s, i);
  buf[i] = 0;
  rev=atoi(buf);
  return rev; 
}

double convertDMStoD(double degree)
{
  double pos;
  
  double deg = (int)degree / 100;
  double m = ((int)degree - (deg * 100)) / 60;
  double s = (degree - (int)degree) / 60;
  pos = deg + m + s;
  return(pos);
}

void parseGPGGA(const char* GPGGAstr)
{
  /* Refer to http://www.gpsinformation.org/dale/nmea.htm#GGA
   * Sample data: $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
   * Where:
   *  GGA          Global Positioning System Fix Data
   *  123519       Fix taken at 12:35:19 UTC
   *  4807.038,N   Latitude 48 deg 07.038' N
   *  01131.000,E  Longitude 11 deg 31.000' E
   *  1            Fix quality: 0 = invalid
   *                            1 = GPS fix (SPS)
   *                            2 = DGPS fix
   *                            3 = PPS fix
   *                            4 = Real Time Kinematic
   *                            5 = Float RTK
   *                            6 = estimated (dead reckoning) (2.3 feature)
   *                            7 = Manual input mode
   *                            8 = Simulation mode
   *  08           Number of satellites being tracked
   *  0.9          Horizontal dilution of position
   *  545.4,M      Altitude, Meters, above mean sea level
   *  46.9,M       Height of geoid (mean sea level) above WGS84
   *                   ellipsoid
   *  (empty field) time in seconds since last DGPS update
   *  (empty field) DGPS station ID number
   *  *47          the checksum data, always begins with *
   */
  double latitude;
  double longitude;
  double tmpPos;
  int tmp, hour, minute, second, num ;
  if(GPGGAstr[0] == '$')
  {
    tmp = getComma(1, GPGGAstr);
    hour     = (GPGGAstr[tmp + 0] - '0') * 10 + (GPGGAstr[tmp + 1] - '0');
    minute   = (GPGGAstr[tmp + 2] - '0') * 10 + (GPGGAstr[tmp + 3] - '0');
    second    = (GPGGAstr[tmp + 4] - '0') * 10 + (GPGGAstr[tmp + 5] - '0');
    
    sprintf(buff, "%02d%02d%02d", hour, minute, second);
    lcd.setCursor(0, 0);
    lcd.print(buff);
    
    tmp = getComma(2, GPGGAstr);
    tmpPos = getDoubleNumber(&GPGGAstr[tmp]);
    latitude = convertDMStoD(tmpPos);
    tmp = getComma(4, GPGGAstr);
    tmpPos = getDoubleNumber(&GPGGAstr[tmp]);
    longitude = convertDMStoD(tmpPos);
    sprintf(buff, "LAT%10.6f%cST", latitude, GPGGAstr[getComma(3, GPGGAstr)]);
    lcd.setCursor(0, 0);
    lcd.print(buff);
    sprintf(buff, "LON%10.6f%c   ", longitude,  GPGGAstr[getComma(5, GPGGAstr)]);
    lcd.setCursor(0, 1);
    lcd.print(buff);
        
    tmp = getComma(7, GPGGAstr);
    num = getIntNumber(&GPGGAstr[tmp]);
    sprintf(buff, "%d   ", num);
    lcd.setCursor(14, 1);
    lcd.print(buff);
    if(num == 0)
      lcd.setRGB(255, 0, 0);
    else
      lcd.setRGB(256 / (num * 16), num * 16, 0);
  }
  else
  {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("No data"); 
  }
}

void setup() {
  // put your setup code here, to run once:
  lcd.begin(16, 2);
  lcd.setRGB(colorR, colorG, colorB);
  lcd.print(" Gregory Fenton");
  LGPS.powerOn();
  lcd.setCursor(0, 1);
  lcd.print("GPS on, waiting"); 
  delay(3000);
}

void loop() {
  // put your main code here, to run repeatedly:
  LGPS.getData(&info);
  parseGPGGA((const char*)info.GPGGA);
  delay(2000);
}