#include <SeqButton.h> //AGGIUNTO
#include <EEPROM.h>
#include <TinyGPS++.h>
#include <SoftwareSerial.h>
TinyGPSPlus gps;
static const int RXPin = 4, TXPin = 3;
SoftwareSerial ss(RXPin, TXPin);
float distanceToDest[50];
float courseToDest[50];
byte a = 22;
int posto = 0;
float piu_vicino = 999923.54;
struct LatLon
{
  float Lat;
  float Lon;
};

struct LatLon LatLonList[50] =
{
  //{0,0},
  //{0,0}, // United Arab Emirates
  //{0,0}, // mio44
  // {0,0}, // Albania
  // {0,0}, // Armenia
  //  {0,0}, // Armenia
  //
};
int LatLonCount = sizeof LatLonList /  sizeof LatLonList[0];
SeqButton pulsanteScritturaCoordinate;  //AGGIUNTO
#define PULSANTE_8 8                    //AGGIUNTO

unsigned long last = 0UL;
int ledPin = 13;
int Posizione = 0;  //posizione memoria eeprom

void writeCoords() {
  int address = 0;
  //for (int i = 0; i < LatLonCount; i++) {
    EEPROM.put(address, Posizione);
    address += sizeof(Posizione);
    EEPROM.put(address, Posizione);
    address += sizeof(Posizione);
    EEPROM.put(address, Posizione);
    address += sizeof(Posizione);
  //}
}
void readCoords() {
  int address = 0;
  //for (int i = 0; i < LatLonCount; i++) {
    EEPROM.get(address, Posizione);
    address += sizeof(Posizione);
    EEPROM.get(address, Posizione);
    address += sizeof(Posizione);
    EEPROM.get(address, Posizione);
    address += sizeof(Posizione);
  //}
}
void setup() {
  readCoords();
  Serial.begin(9600);
  ss.begin(9600);
  pinMode(ledPin, OUTPUT);
  pinMode(7, INPUT_PULLUP);
  pinMode(8, INPUT_PULLUP);
  pinMode(9, INPUT_PULLUP);

  pulsanteScritturaCoordinate.init(PULSANTE_8, NULL, &f_ButtonRelease, false, LOW, 100); //AGGIUNTO
}

void loop() {

  pulsanteScritturaCoordinate.handler(); //AGGIUNTO

  if (digitalRead(9) == LOW) {
    Serial.print(F(" cancella tutte le posizioni"));
    //per azzerare la posizione di salvataggio
    //rimetto la variabile --posizione--  a 0
    Posizione = 0;
    writeCoords();

  }
  float currentLat, currentLon;

  // Dispatch incoming characters
  while (ss.available() > 0) {
    gps.encode(ss.read());
  }
  if (gps.location.isUpdated() && millis() - last > 5000)
  {
    last = millis();
    currentLat = gps.location.lat();
    currentLon = gps.location.lng();
    Serial.print(F("  Lat="));
    Serial.print(currentLat, 6);
    Serial.print(F(" Long="));
    Serial.println(currentLon, 6);
    Serial.print(F(" course="));
    Serial.println(gps.course.deg());

    if (gps.location.isValid())
    {
      Serial.println("Fix is valid.");
    }
    else
    {
      Serial.println("Fix is not yet valid.");
      return;
    }


    for (int x = 0; x < Posizione; x++) {//-----MODIFICATO------
      //for (int x = 0; x < LatLonCount; x++) {
      distanceToDest[x] =
        TinyGPSPlus::distanceBetween(
          currentLat,
          currentLon,
          LatLonList[x].Lat,
          LatLonList[x].Lon);

      courseToDest[x] =
        TinyGPSPlus::courseTo(
          currentLat,
          currentLon,
          LatLonList[x].Lat,
          LatLonList[x].Lon);

      Serial.print(F("Distance:"));
      Serial.print(x); Serial.print(" Km ");
      Serial.print(distanceToDest[x] / 1000, 3);     // *Prints distance to destination
      Serial.print(" angolazione : ");
      Serial.print(courseToDest[x]);
      Serial.println(F(" gradi"));
      Serial.print(F("direzione cardinale ["));
      Serial.print(TinyGPSPlus::cardinal(courseToDest[x]));
      Serial.println(F("]"));
      if (distanceToDest[x] < piu_vicino) {//creo valore
        posto = x;                        // point
        piu_vicino = distanceToDest[x] / 1000; //+ vicino
      }
      if (distanceToDest[x] / 1000 < 6.0500) { //
        a = x;   // imposto soglia limite distanza
      }
    }
    Serial.print(piu_vicino);
    Serial.print(F(" Km piu' vicino ")); Serial.println(posto);
    if (a < 7) {
      Serial.print("We have arrived.");
      Serial.println(a);
      digitalWrite(ledPin, HIGH);
      a = 8;
    }
    else
    {
      digitalWrite(ledPin, LOW);
      a = 8;
    }
  }
  if (gps.charsProcessed() < 10)
    Serial.println(F("WARNING: No GPS data.  Check wiring."));
}


//AGGIUNTO
//ogni volta che viene premuto e rilasciato il pulsante viene salvata una nuova posizione GPS
void f_ButtonRelease()
{
  //static int Posizione=0;

  if (gps.location.isValid())
  {
    LatLonList[Posizione].Lat = gps.location.lat();
    LatLonList[Posizione].Lon = gps.location.lng();
    Posizione = (Posizione + 1) % LatLonCount;
    Serial.print(F(" scrivi   nuova posizione posizione: "));
    Serial.println(Posizione);
    writeCoords();
  }
  else
  {
    Serial.println(F("Modulo GPS non pronto - memorizzazione posizione fallita"));
  }
}