/*
  GPS Fake using custom chip
  by Anderson Costa

  GPS NMEA 0183 Messaging Protocol 101
  https://docs.arduino.cc/learn/communication/gps-nmea-data-101


  NMEA Generator
  https://www.nmeagen.org/
*/

#include "NMEA.h"
#include <SoftwareSerial.h>
#include <Adafruit_GPS.h>

#define LEN(arr) ((int)(sizeof(arr) / sizeof(arr)[0]))
#define LED 4
#define R 6371
#define TO_RAD (3.1415926536 / 180)

union {
  char bytes[4];
  float valor;
} velocidadeGPS;

float latitude;
float longitude;

float latitude_fix=35.62751365546846;
float longitude_fix=139.57376234686868;

// Creates a GPS data connection with sentence type GPRMC
NMEA gps(GPRMC);
SoftwareSerial Serial1(2, 3); // RX, TX

void setup() {
  Serial.begin(9600);
  Serial1.begin(4800); // Serial1 is connected to the custom chip
  pinMode(LED, OUTPUT);
  Serial.println("Data received from GPS Fake:");
}

void loop() {
  // Waits for serial port data
  while (Serial1.available()) {
    // Receives data from GPS serial port
    char serialData = Serial1.read();
    Serial.print(serialData);

    // Checks if the GPS sentence is valid
    if (gps.decode(serialData)) {
      // Checks if GPS status is 'A'
      if (gps.gprmc_status() == 'A') {
        // Receives GPS speed in km/h
        velocidadeGPS.valor = gps.gprmc_speed(KMPH);
      } else {
        velocidadeGPS.valor = 0;
      }

      latitude = gps.gprmc_latitude();
      longitude = gps.gprmc_longitude();

      // Add line break
      Serial.println();
      Serial.println();

      // Show Latitude
      Serial.print(" Latitude: ");
      Serial.println(latitude, 8);

      // Show Longitude
      Serial.print("Longitude: ");
      Serial.println(longitude, 8);

     /* Serial.print("Distance between:  ");
      Serial.print(distance(latitude_fix, longitude_fix, latitude, longitude), 3);
      Serial.println("m");*/

      float distancia_total=distance(latitude_fix, longitude_fix, latitude, longitude);
      Serial.print("distancia_total: ");
      Serial.println(distancia_total, 8);
      if(distancia_total<100){
           digitalWrite(LED, HIGH);
           delay(100);
      }
      else{
            digitalWrite(LED, LOW);
            delay(100);
      }
  
    }
  }
}

double distance(double lat1, double lng1, double lat2, double lng2) {
  double dx, dy, dz;
	lng1 -= lng2;
	lng1 *= TO_RAD, lat1 *= TO_RAD, lat2 *= TO_RAD;

	dz = sin(lat1) - sin(lat2);
	dx = cos(lng1) * cos(lat1) - cos(lat2);
	dy = sin(lng1) * cos(lat1);
	return asin(sqrt(dx * dx + dy * dy + dz * dz) / 2) * 2 * R * 1000; // *1000 for metres
}