#include "TinyGPS.h"
#include <SoftwareSerial.h>
#include <math.h>

static const uint32_t GPSBaud = 9600;
const double R = 6371.0; // Radius of the Earth in kilometers

// The TinyGPSPlus object
TinyGPSPlus gps;

// Variables to store the coordinates
double lat1 = 0.0;
double lng1 = 0.0;
double lat2 = 0.0;
double lng2 = 0.0;
double totalDistance = 0.0; // Variable to track the cumulative distance
bool firstUpdate = true; // Flag to check if it's the first update

const int resetButtonPin = 7; // Pin for the reset button

void setup()
{
  Serial.begin(115200);
  Serial1.begin(GPSBaud);

  pinMode(resetButtonPin, INPUT_PULLUP); // Set reset button pin as input with internal pull-up resistor

  Serial.println(F("DeviceExample.ino"));
  Serial.println(F("A simple demonstration of TinyGPSPlus with an attached GPS module"));
  Serial.print(F("Testing TinyGPSPlus library v. ")); Serial.println(TinyGPSPlus::libraryVersion());
  Serial.println(F("by Mikal Hart"));
  Serial.println();
}

void loop()
{
  // This sketch displays information every time a new sentence is correctly encoded.
  while (Serial1.available() > 0)
    if (gps.encode(Serial1.read()))
      displayInfo();

  if (millis() > 5000 && gps.charsProcessed() < 10)
  {
    Serial.println(F("No GPS detected: check wiring."));
    while (true);
  }

  // Check if the reset button is pressed
  if (digitalRead(resetButtonPin) == LOW)
  {
    totalDistance = 0.0; // Reset the total distance
    Serial.println(F("Jarka direset"));
  }
}

void displayInfo()
{
  if (gps.location.isValid())
  {
    if (firstUpdate)
    {
      // Set initial coordinates
      lat1 = gps.location.lat();
      lng1 = gps.location.lng();
      firstUpdate = false; // Set flag to false after the first update

      Serial.print(F("Initial Location: "));
      Serial.print(lat1, 8);
      Serial.print(F(","));
      Serial.print(lng1, 8);
      Serial.println();
    }
    else
    {
      // Update coordinates for location 2 with new values
      lat2 = gps.location.lat();
      lng2 = gps.location.lng();

      Serial.print(F("Location 1: "));
      Serial.print(lat1, 8);
      Serial.print(F(","));
      Serial.print(lng1, 8);
      Serial.println();

      // Display location 2
      Serial.print(F("Location 2: "));
      Serial.print(lat2, 8);
      Serial.print(F(","));
      Serial.print(lng2, 8);
      Serial.println();

      // Calculate the new distance
      double newDistance = calculateDistance(lat1, lng1, lat2, lng2);

      // Add the new distance to the total distance
      totalDistance += newDistance;

      // Display the total distance
      Serial.print(F("Total Distance: "));
      Serial.print(totalDistance, 8);
      Serial.print(F(" km"));
      Serial.println();

      // Update location 1 to the new location
      lat1 = lat2;
      lng1 = lng2;

      // Check if the total distance exceeds 1 meter (0.001 km)
      if (totalDistance > 0.060)
      {
        Serial.println(F("Berhenti"));
      }
    }
  }
  else
  {
    Serial.print(F("Location: INVALID"));
    Serial.println();
  }
}

double calculateDistance(double lat1, double lng1, double lat2, double lng2)
{
  // Convert degrees to radians
  double lat1Rad = radians(lat1);
  double lng1Rad = radians(lng1);
  double lat2Rad = radians(lat2);
  double lng2Rad = radians(lng2);

  // Haversine formula
  double dlat = lat2Rad - lat1Rad;
  double dlng = lng2Rad - lng1Rad;
  double a = sin(dlat / 2) * sin(dlat / 2) + cos(lat1Rad) * cos(lat2Rad) * sin(dlng / 2) * sin(dlng / 2);
  double c = 2 * atan2(sqrt(a), sqrt(1 - a));
  double distance = R * c; // Distance in kilometers

  return distance;
}