#include <WiFi.h>
#include <ArduinoHttpClient.h>
#include "NMEA.h"

#define LEN(arr) ((int)(sizeof(arr) / sizeof(arr)[0]))

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

float latitude;
float longitude;

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

// Connect WiFi
const char* ssid = "Wokwi-GUEST";
const char* password = "";

// WebSocket path
char serverAddress[] = "archocell.obaa.cloud";
int port = 8181; // 443;           // standard HTTPS port
char endpoint[] = "/echo";

// initialize the webSocket client
WiFiClient wifi;
// WiFiSSLClient wifiSec;
WebSocketClient client = WebSocketClient(wifi, serverAddress, port);
// message sending interval, in ms:
int interval = 5000;
// last time a message was sent, in ms:
long lastSend = 0;

// JSON string to store the location data
String jsonMessage;  

void setup() {
  Serial.begin(115200);
  Serial2.begin(9600);  // Serial2 is connected to the custom chip

  // Connect to Wi-Fi
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Connecting to WiFi...");
  }
  Serial.println("Connected to WiFi");

  // Connect to WS server
  client.begin(endpoint);

  Serial.println("Ready to receive data from GPS!");
  Serial.println();
  Serial.println();
}

void loop() {
  while (Serial2.available()) {
    char serialData = Serial2.read();
    Serial.print(serialData);

    if (gps.decode(serialData)) {
      if (gps.gprmc_status() == 'A') {
        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);

      // 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);

      // Create JSON message
      jsonMessage = "{\"latitude\":" + String(latitude, 8) +
                    ",\"longitude\":" + String(longitude, 8) +
                    ",\"speed\":" + String(velocidadeGPS.valor) + "}";

      // Send JSON message to WebSocket server
      client.beginMessage(TYPE_TEXT);
      client.print(jsonMessage);
      client.endMessage();
      Serial.print("Sending: ");
      Serial.println(jsonMessage);


      // Check if it's time to send the message
      if (millis() - lastSend > interval) {
        
        // Update last send time
        lastSend = millis();
      }
    }
  }
}



void convertCoordinatesToCartesian(float latitude, float longitude) {
  float latRadius = latitude * (PI) / 180;
  float lonRadius = longitude * (PI) / 180;

  int earthRadius = 6371;

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

  Serial.println("Cartesian Plane");
  Serial.print("        X: ");
  Serial.println(posX);
  Serial.print("        Y: ");
  Serial.println(posY);
}