#include <SoftwareSerial.h>
#include <math.h>
#define NMEA_SIZE 256
#define SIZE 120
#define SIZEA 120

SoftwareSerial GPS = SoftwareSerial(6, 1);

byte NMEA[NMEA_SIZE];
double lat_values[SIZE];
double lon_values[SIZEA];


double dis,lo_dif,la_dif,lat_1_radian1,lat_1_radian2,lon_1_radian1,lon_1_radian2,lat2_dif,lon2_dif,displacement,sum,distance,b,lon1_dif,lon_1_radian,lon1_m,lon1_d,lat1_dif,lat1_memory,lat1_mdeneme,lat_1_radiandeneme,lat_1_radian,lat1_m,lat1_d,lat1_degree,lat1_degree_1, lat1_degree_2,d,d2;
int lon1_m1,lon1_m2,lon1_m3,lon1_m4,lon1_m5,lon1_m6,lon1_d1,lon1_d2,s,m,n,e,z,f,g,h,lat1_d1,lat1_d2,lat1_m1,lat1_m2,lat1_m3,lat1_m4,lat1_m5,lat1_m6;


void getData() {

  byte character;
  int index = 0;

  do {
    if (GPS.available()) {
      character = GPS.read();
      NMEA[index] = character;
      index++;
    }
  } 
  while(index < NMEA_SIZE && character != '$');

  NMEA[index - 2] = '\0';
}

void setup() {

  GPS.begin(4800);
  Serial.begin(9600);
  n=0;
  m=0;
  sum=0;
  

  for(s =0 ; s<100000 ; s++){

    getData();

    if(NMEA[17] == 'V')
    {
      Serial.print("NO DATA\n");
    }

    i = 0; NMEA[i] != '\0'; i++) { Serial.write(NMEA[i]);  }

    else if(NMEA[2] == 'R' && NMEA[3] == 'M' && NMEA[4] == 'C') {
      int i = 0;
    
      Serial.print("\n");

      Serial.print("Latitude: ");
      for(i = 19; i<21; i++) { 
        Serial.write(NMEA[i]);  
      }
      Serial.print(".");
      for(i = 21; i<23; i++) { 
        Serial.write(NMEA[i]);  
      }
      for(i = 24; i<28; i++) { 
        Serial.write(NMEA[i]);  
      }
      Serial.print(" N");
      Serial.print("\n");

      Serial.print("Longitude: ");
      for(i = 32; i<34; i++) { 
        Serial.write(NMEA[i]);  
      }
      Serial.print(".");
      for(i = 34; i<36; i++) { 
        Serial.write(NMEA[i]);  
      }
      for(i = 37; i<41; i++) { 
        Serial.write(NMEA[i]);  
      }
      Serial.print(" E");



      lat1_d1=NMEA[19]- '0';
      lat1_d2=NMEA[20]- '0';

      lat1_m1=NMEA[21]- '0';
      lat1_m2=NMEA[22]- '0';
      lat1_m3=NMEA[24]- '0';
      lat1_m4=NMEA[25]- '0';
      lat1_m5=NMEA[26]- '0';
      lat1_m6=NMEA[27]- '0';

      lon1_d1=NMEA[32]- '0';
      lon1_d2=NMEA[33]- '0';

      lon1_m1=NMEA[34]- '0';
      lon1_m2=NMEA[35]- '0';
      lon1_m3=NMEA[37]- '0';
      lon1_m4=NMEA[38]- '0';
      lon1_m5=NMEA[39]- '0';
      lon1_m6=NMEA[40]- '0';

      lat1_d= (double)lat1_d1*10+lat1_d2;
      lat1_m= (double)lat1_m1*10+lat1_m2+lat1_m3*0.1+lat1_m4*0.01+lat1_m5*0.001+lat1_m6*0.0001;


      lat_1_radian=(double)(((22*lat1_d)/(180))/7)+((((((22*lat1_m)/180)/60)/7)));

      lon1_d= (double)lon1_d1*10+lon1_d2;
      lon1_m= (double)lon1_m1*10+lon1_m2+lon1_m3*0.1+lon1_m4*0.01+lon1_m5*0.001+lon1_m6*0.0001;


      lon_1_radian=(double)(((22*lon1_d)/(180))/7)+((((((22*lon1_m)/180)/60)/7)));


      if(m<120){
        lat_values[m]=lat_1_radian;
        if(m>0){
          lat1_dif=lat_values[m]-lat_values[m-1];
          lat2_dif=lat_values[m]-lat_values[1];
        }

      } 

      if(n<120){
        lon_values[n]=lon_1_radian;
        if(n>0){
          lon1_dif=lon_values[n]-lon_values[n-1];
          lon2_dif=lon_values[n]-lon_values[1];
        }  


      }

      distance=(double)(2*(6364.49)*(1000))*(asin((sqrt(((sin((lat1_dif)/2)))*(sin((lat1_dif)/2))+(cos(lat_values[m-1])*cos(lat_values[m])*(((sin((lon1_dif)/2)))*(sin((lon1_dif)/2))))))));


      sum=sum+distance;
      displacement=(double)(2*(6364.49)*(1000))*(asin((sqrt(((sin((lat2_dif)/2)))*(sin((lat2_dif)/2))+(cos(lat_values[1])*cos(lat_values[m])*(((sin((lon2_dif)/2)))*(sin((lon2_dif)/2))))))));


      n++;
      m++;

     

      Serial.print("\n");



      Serial.print("Toplam Yol: ");
      Serial.println(sum,4);
      Serial.print("Yerdegistirme: ");
      Serial.println(displacement,4);

     delay(3000);

      
    }
    


  }

}


void loop() {  
}