#include <TinyGPSPlus.h>


void termialgps (void);
int readGpsData(uint8_t TrialNbr, unsigned long rdinterval);
double GPSdistanceBetween(double lat1, double long1, double lat2, double long2);

#define RXD2 16
#define TXD2 17

//Reference Long, Latt: Entrée INSAT
float LongitudeRef = 10.197458;
float LattitudeRef = 36.842979;

//Data variables filled by GPS_Function (ReadGPSLine Function).
float gpsLattitude, gpsLongitude;
int Distance;
int Day, Month, Year;
int Hour, Minute;

TinyGPSPlus gps;


/* ------------  SETUP  ------------- */// The TinyGPSPlus object
void setup() {
  
  // put your setup code here, to run once:
  Serial.begin(9600);

  Serial2.begin(9600, SERIAL_8N1, RXD2, TXD2);
  Serial2.setTimeout(2000);

  Serial.println("-- Application launched ---- ");
  delay(2000);
}





void loop() {

    //Reading The NMEA DATA from GPS ublox  
  termialgps();

   delay(3000);
  
    //Launching GPS Encode/read
       Serial.println( ); Serial.println( );
      Serial.println("Decoding GPS  NMEA DATA");
      Serial.println();
      
      if (readGpsData(5, 2000)){
                                if (gps.date.isValid() && gps.time.isValid())
                                {
                                //Reading DATE and time
                                Day = gps.date.day();
                                Month = gps.date.month();
                                Year = gps.date.year();
                                
                                      if (Day!=0){
                                      Serial.println(F("  readGpsline: Date/Time OK "));
                                      Serial.print(Day); Serial.print("/"); 
                                      Serial.print(Month); Serial.print ("/");
                                      Serial.print(Year);    Serial.println (".");
        
                                      Hour = gps.time.hour();
                                      Minute = gps.time.minute();
                                      Serial.print(Hour);  Serial.print(":");  Serial.print(Minute);  Serial.println(".");
                                      }else Serial.println("INVALID DATE, TIME");
                                }
                                else{
                                Serial.print(F("INVALID DATE and or TIME + LOCATION"));
                                }
                   
                   
                   //Reading GPS Location 
                              if (gps.location.isValid())
                              {
                              gpsLattitude = gps.location.lat();
                              gpsLongitude = gps.location.lng();
                              Distance = GPSdistanceBetween(gpsLattitude, gpsLongitude,LattitudeRef,LongitudeRef);
                              Serial.print ("Longitude : "); Serial.println (gpsLongitude,6); 
                              Serial.print ("Lattitude : "); Serial.println (gpsLattitude,6); 
                              Serial.print ("The Distance to reference point [Meter] : "); Serial.println (Distance);

                              }else{
                              Serial.print(F("INVALID LOCATION "));
                              }
          
         delay(5000);
   }else{
    Serial.println ("NO DATA AVAILBALE FROM SERIAL COM");
   }
 
}


/* =========OTHER FUNCTIONS FOR DEBUGGING =========== */

void termialgps (void)
{
   Serial.println( ); Serial.println( );
  Serial.println ("Recieving Raw NMEA DATA From UBLOX");
   Serial.println( );



  while ( Serial2.available()==0);
 
 while (Serial2.available()>0){
  Serial.print (char (Serial2.read()) ); 
 }  

 Serial.println( );
 Serial.println ("Serial Brfdge finoished !! ");

}


//Read GPS Data 
//returns 1 if data availble
//returns 0 else

int readGpsData(uint8_t TrialNbr, unsigned long rdinterval)
{
  uint8_t ValidState = 0;  
  uint8_t encodenbr = 0;

  if (encodenbr==0){
  //Flushing Serial before reading
  while (Serial2.available() == 0);
  while (Serial2.available()){
    char Cflush=Serial2.read();
  }
  }

  while ( ValidState ==0 && TrialNbr-- >0 ){
  //read gps data (encode for 2 sec + 
 
  
  unsigned long start = millis();
    do {
        while (Serial2.available())   gps.encode(Serial2.read());
       } while (millis() - start < 2000);
   
        if (gps.charsProcessed() < 10) {
          ValidState= 0;
       }
       else
       
       
       ValidState = 1;
    }
  return ValidState;
}

double GPSdistanceBetween(double lat1, double long1, double lat2, double 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
  
  double delta = radians(long1-long2);
  double sdlong = sin(delta);
  double cdlong = cos(delta);
  lat1 = radians(lat1);
  lat2 = radians(lat2);
  double slat1 = sin(lat1);
  double clat1 = cos(lat1);
  double slat2 = sin(lat2);
  double clat2 = cos(lat2);
  delta = (clat1 * slat2) - (slat1 * clat2 * cdlong);
  delta = sq(delta);
  delta += sq(clat2 * sdlong);
  delta = sqrt(delta);
  double denom = (slat1 * slat2) + (clat1 * clat2 * cdlong);
  delta = atan2(delta, denom);
  return delta * 6372795;
}