/*
  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 <Servo.h>

#define LEN(arr) ((int)(sizeof(arr) / sizeof(arr)[0]))
Servo servo1;                          // create servo object to control a servo
int posn = 0; 

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

float latitude;
float longitude;

// Creates a GPS data connection with sentence type GPRMC
NMEA gps(GPRMC);


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

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);
        for (posn = 0; posn < 180; posn += 1)            // goes from 0 degrees to 180 degrees
 {                                                                       // in steps of 1 degree
    servo1.write (posn);                                                 // tell servo to go to position in variable 'pos'
    delay (10);                                       // waits 10ms for the servo to reach the position
  }
  for (posn = 180; posn>=1; posn-=1)                // goes from 180 degrees to 0 degrees                                                                                 // in steps of 1 degree
{                               
    servo1.write (posn);                                                  // tell servo to go to position in variable 'pos'
    delay (10);                                        // waits 10ms for the servo to reach the position
  }
      } 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);

      // Show Speed ​​in km/h
      Serial.print("    Speed: ");
      Serial.print(velocidadeGPS.valor);
      Serial.println(" Km/h");

      // Converts Geographic Coordinates to Cartesian Plane
      convertCoordinatesToCartesian(latitude, longitude);
    }
  }
}

void convertCoordinatesToCartesian(float latitude, float longitude) {
  // Convert from Degrees to Radians
  float latRadius = latitude * (PI) / 180;
  float lonRadius = longitude * (PI) / 180;

  int earthRadius = 6371; // Radius in km

  float posX = earthRadius * cos(latRadius) * cos(lonRadius);
  float posY = earthRadius * cos(latRadius) * sin(lonRadius);

  Serial.print("        X: ");
  Serial.println(posX);

  Serial.print("        Y: ");
  Serial.println(posY);
}