#define left1 32
#define left2 26
#define right2 30
#define right1 28
#define eLeft A8
#define eRight A9
//********************************************************************************************************************************************************************************************
#include <LiquidCrystal.h>
int lcd_key     = 0;
int adc_key_in  = 0;
// select the pins used on the LCD panel
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);
//********************************************************************************************************************************************************************************************
String data="";
int mark = 0;
boolean Mark_Start=false;
boolean valid=false;
String GGAUTCtime,GGAlatitude,GGAlongitude,GPStatus,SatelliteNum,HDOPfactor,Height,
PositionValid,RMCUTCtime,RMClatitude,RMClongitude,Speed,Direction,Date,Declination,Mode;
//********************************************************************************************************************************************************************************************
int longitude, latitude;
//********************************************************************************************************************************************************************************************
void setup() {
  pinMode(right1, OUTPUT);
  pinMode(right2, OUTPUT);
  pinMode(left1, OUTPUT);
  pinMode(left2, OUTPUT);
  pinMode(eLeft, OUTPUT);
  pinMode(eRight, OUTPUT);  
  digitalWrite(eLeft, HIGH);
  digitalWrite(eRight, HIGH);
//********************************************************************************************************************************************************************************************
  Serial.begin(9600);
  Serial1.begin(9600);
  Serial.println(0);
  delay(1000);
  pinMode(21, OUTPUT);
  digitalWrite (21, HIGH);
  pinMode(20, OUTPUT);
  digitalWrite (20, LOW);
  lcd.begin(16, 2);              // start the library
  lcd.setCursor(0,0);
  lcd.print("Initializing GPS"); // print a simple message
}
//********************************************************************************************************************************************************************************************
void loop() {
  gpsGet();
}
//********************************************************************************************************************************************************************************************
void turnLeft(int speed){
  analogWrite(left2, speed);
  digitalWrite(left1, LOW);
  analogWrite(right1, speed);
  digitalWrite(right2, LOW);
}
//******************************
void turnRight(int speed){
  analogWrite(left1, speed);
  digitalWrite(left2, LOW);
  analogWrite(right2, speed);
  digitalWrite(right1, LOW);
}
//******************************
void moveForwards(int speed){
analogWrite(right1, speed);
digitalWrite(right2, LOW);
analogWrite(left1, speed);
digitalWrite(left2, LOW);
}
//******************************
void moveBackwards(int speed){
analogWrite(right2, speed);
digitalWrite(right1, LOW);
analogWrite(left2, speed);
digitalWrite(left1, LOW);
}
//******************************
void turn90right(){
  turnRight(150);
  delay(985);
}
void turn90left(){
  turnLeft(150);
  delay(985);
}
//******************************
void haltyoudemon(){
digitalWrite(right2, LOW);
digitalWrite(right1, LOW);
digitalWrite(left2, LOW);
digitalWrite(left1, LOW);
}
//******************************
void gpsGet(){
    while (Serial1.available()> 0){
    if(Mark_Start){
      data=reader();
      Serial.println(data);
      if(data.equals("GPGGA")){
        //Serial.println(1);
        GGAUTCtime=reader();
        GGAlatitude=reader();
        GGAlatitude+=reader();
        GGAlongitude=reader();
        GGAlongitude+=reader();
        GPStatus=reader();
        SatelliteNum=reader();
        HDOPfactor=reader();
        Height=reader();
        Mark_Start=false;
        valid=true;
        data="";
 
      }
      else if(data.equals("GPGSA")){
        Serial.println(2);
        Mark_Start=false;
        data="";
      }
      else if(data.equals("GPGSV")){
        Serial.println(3);
        Mark_Start=false;
        data="";
      }
      else if(data.equals("GPRMC")){
        //Serial.println(4);
        RMCUTCtime=reader();
        PositionValid=reader();
        RMClatitude=reader();
        RMClatitude+=reader();
        RMClongitude=reader();
        RMClongitude+=reader();
        Speed=reader();
        Direction=reader();
        Date=reader();
        Declination=reader();
        Declination+=reader();
        Mode=reader();
        valid=true;
        Mark_Start=false;
        data="";
      }
      else if(data.equals("GPVTG")){
        Serial.println(5);
        Mark_Start=false;
        data="";
      }
      else{
        Serial.println(6);
        Mark_Start=false;
        data="";
      }
    }
    if(valid){
      if(PositionValid=="A"){
        Serial.println("Position Valid");
      }
      else{
        Serial.println("Your position is not valid.");
      }
      lcd.clear();
      CoordNum();
      Serial.print("Date:");
      Serial.println(Date);
      Serial.print("UTCtime:");
      Serial.print(RMCUTCtime);
      Serial.print("   ");
      Serial.println(GGAUTCtime);
      Serial.print("Latitude:");
      Serial.print(RMClatitude);
      lcd.setCursor(0,0);
      lcd.print(RMClatitude);
      Serial.print("   ");
      Serial.println(GGAlatitude);
      Serial.print("Longitude:");
      Serial.print(RMClongitude);
      lcd.setCursor(0,1);
      lcd.print(RMClongitude);
      Serial.print("   ");
      Serial.println(GGAlongitude);
      Serial.print("GPStatus:");
      Serial.println(GPStatus);
      Serial.print("SatelliteNum:");
      Serial.println(SatelliteNum);
      Serial.print("HDOPfactor:");
      Serial.println(HDOPfactor);
      Serial.print("Height:");
      Serial.println(Height);
      Serial.print("Speed:");
      Serial.println(Speed);
      Serial.print("Direction:");
      Serial.println(Direction);
      Serial.print("Declination:");
      Serial.println(Declination);
      Serial.print("Mode:");
      Serial.println(Mode);     
      valid=false;
    }
    if(Serial1.find("$")){
      Serial.println("capture");
      Mark_Start=true;
    }
  }
}

String reader(){
  String value="";
  int temp;
startover:
  while (Serial1.available() > 0){
    delay(2);
    temp=Serial1.read();
    if((temp==',')||(temp=='*')){
      if(value.length()){
        //Serial.println("meaningful message");
        return value;
      }
      else {
        //Serial.println("empty");
        return "";
      }     
    }
    else if(temp=='$'){
      //Serial.println("failure");
      Mark_Start=false;
    }
    else{
      //Serial.println("add");
      value+=char(temp);
    }
  }
  while (!(Serial1.available()>0)){
  }
  goto startover;
}
//******************************
void CoordNum(){
  RMClatitude.remove(RMClatitude.length()-1);
  RMClongitude.remove(RMClongitude.length()-1);
  latitude = RMClatitude.toInt();
  longitude = RMClongitude.toInt();
}