// The TinyGPSPlus object

#include <TinyGPSPlus.h>
#include <math.h>
#include <EEPROM-Storage.h>

TinyGPSPlus gps;
// For stats that happen every 5 seconds
unsigned long last = 0UL;

//eprom
struct MyObject {
  float field1;
  float field2;
};

struct MyObjecttwo {
  float field1;
  float field2;
};

int eprom = 0;
int epromHome = 0;

double gotoLat1, gotoLon1;
double fromLat = 50.3914984386008, fromLon = 18.451405668007972;  // start point        = 50.391495534730375, 18.451403017902898
double gotoLat = 50.393805368279146,  gotoLon = 18.45412793159416;  // destination point  = 29405163, 18.453361030420297 
double distance;                                  // Distance between start and destination (orthodrome, hypothetical sphere of radius 6372795 meters)
double dtk;                                       // Desired Track in degrees (forward azimuth)
//double presentLat = 50.391554101825974,  presentLon = 18.451568244623196; //LINIA po lewej 5,41m
//double presentLat = 50.39166472369322, presentLon = 18.451429842479637; //Linia po PRAWEJ -10m


double presentLat = 50.39433196609621,   presentLon = 18.45449445745729 ; //Linia po PRAWEJ -93m


//double presentLat = 50.39154763917555, presentLon = 18.451452422565087; // 555298685, 18.452674384934742double distance;                                  // Distance between start and destination (orthodrome, hypothetical sphere of radius 6372795 meters)
//double presentLat = 50.391311549839365, presentLon = 18.451350059502605; // z tyłu HOME w lewo
//double presentLat = 50.391452627752884,  presentLon = 18.450919532540077; // z tyłu HOME w prawo


double cmg;
double error;
int errorInt, kp1 = 15, kp2 = 45 , bias = 0, bias1 = 0, bias2 = 0;

double crossTrackDistanceFromTo(double lat3, double lon3, double lat1, double lon1, double lat2, double lon2) {

  const double R     = 6372795.0;
  const double ad13  = gps.distanceBetween(lat1, lon1, lat3, lon3) / R;
  const double rtk13 = radians(gps.courseTo(lat1, lon1, lat3, lon3));
  const double rtk12 = radians(gps.courseTo(lat1, lon1, lat2, lon2));
  const double dxt   = asin(sin(ad13) * sin(rtk13 - rtk12));

  return dxt * R;
}


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  zapis(20, fromLat, fromLon);
  delay(200);
  zapis(40, gotoLat, gotoLon);
  delay(200);
  zapis(60, presentLat, presentLon);
  delay(200);
}

void loop() {
  // put your main code here, to run repeatedly:


  odczyt(20);
  
   fromLat = gotoLat1;
   fromLon = gotoLon1;
 
  odczyt(40);
  
   gotoLat = gotoLat1;
   gotoLon = gotoLon1;

  odczyt(60);
  
   presentLat = gotoLat1;
   presentLon = gotoLon1;
  


     cmg = gps.courseTo(fromLat, fromLon, presentLat, presentLon);// Course Made Good in degrees
     dtk = gps.courseTo(fromLat, fromLon, gotoLat, gotoLon); // Desired Track in degrees (forward azimuth)
     distance = gps.distanceBetween(presentLat, presentLon, gotoLat, gotoLon); // Distance between present location and destination (orthodrome, hypothetical sphere of radius 6372795 meters)
     double xte = crossTrackDistanceFromTo(presentLat, presentLon, fromLat, fromLon, gotoLat, gotoLon);


        if (presentLat > gotoLat)
        {
          Serial.println("Powrót do HOME ! " +  String(presentLat, 6));
          Serial.println("DTK: " + String(dtk, 6)); 
          Serial.println("CMG: " + String(cmg, 6)); 
          error = (cmg-180-dtk);
          errorInt = int(error);
          errorInt = ((errorInt + 540)%360-180);
          Serial.println("ERROR INT DTK - CMG : " + String(errorInt)); 

        }
        else
        {
          Serial.println("DTK: " + String(dtk, 6)); 
          Serial.println("CMG: " + String(cmg, 6)); 
          error = (cmg-dtk);
          errorInt = int(error);
          errorInt = ((errorInt + 540)%360-180);
          Serial.println("ERROR INT DTK - CMG : " + String(errorInt)); 
        }


    // Serial.println("DTK - CMG: " + String(error)); 
      bias1 = (kp1*errorInt)/10;
       Serial.println("BIAS1: " + String(bias1)); 
       Serial.println("DYSTANS: " + String(distance)); 
       Serial.print(F("Cross track distance = ")); Serial.print(xte); Serial.print(F(" m, "));
        Serial.println(xte > 0 ? F("to the LEFT") : F("to the RIGHT"));
      bias2 = (kp2*xte)/10;
       Serial.println("BIAS2: " + String(bias2)); 

       bias = bias1+bias2;
       Serial.println("BIAS: " + String(bias)); 

delay(1000);

delay(500);


}



void zapis(int eeprom, float field1, float field2) 
{
 MyObject customVar = {
    field1,
    field2
  };


  EEPROM.put(eeprom, customVar);

}



void odczyt(int eeprom) {

  
  MyObjecttwo customVar; //Variable to store custom object read from EEPROM.


  EEPROM.get(eeprom, customVar);

  gotoLat1 = customVar.field1;
  gotoLon1 = customVar.field2;

}