#include <Adafruit_LIS2MDL.h>       // Magnetometer (Compass) library
#include <Adafruit_Sensor.h>         // Unified sensor library
#include <Wire.h>                    // I2C communication
#include <Adafruit_GFX.h>            // Core graphics library for the display
#include <Adafruit_ST7735.h>         // Specific library for the ST7735 display
#include <Adafruit_ST7789.h>         // Specific library for the ST7789 display
#include <SPI.h>                     // SPI communication for the display
#include <FastLED.h>                 // Library to control the LED ring (WS2812)
#include <TinyGPS++.h>               // Library to handle GPS data
#include <SoftwareSerial.h>          // Software serial for GPS communication
#include <RH_RF95.h>                // 


// MAGNETOMETER (LIS2MDL) CONFIGURATION
Adafruit_LIS2MDL lis2mdl = Adafruit_LIS2MDL(12345);  // Create magnetometer object

// DISPLAY CONFIGURATION (ST7789)
#define TFT_CS 5                     // Chip select pin for the display
#define TFT_RST 9                    // Reset pin for the display
#define TFT_DC 6                     // Data/Command pin for the display
Adafruit_ST7789 tft = Adafruit_ST7789(TFT_CS, TFT_DC, TFT_RST);  // Initialize display

// LED RING CONFIGURATION (WS2812)
#define NUM_LEDS 24                  // Number of LEDs in the ring
#define DATA_PIN 12                  // Pin for LED data
CRGB leds[NUM_LEDS];                 // Array to hold LED colors

// GPS CONFIGURATION (BN-220)
#define GPS_TX_PIN 10                // GPS TX pin connected to Arduino RX
#define GPS_RX_PIN 11                // GPS RX pin connected to Arduino TX
SoftwareSerial gpsSerial(GPS_RX_PIN, GPS_TX_PIN);  // Software serial for GPS
TinyGPSPlus gps;                     // TinyGPS++ object to process GPS data

// Radio defines 
#define RFM95_CS    8
#define RFM95_INT   7
#define RFM95_RST   4
#define RF95_FREQ 915.0
#define RF95_MAX_DB 23
#define RF95_MIN_DB 5
RH_RF95 rf95(RFM95_CS, RMF95_INT);  // Radio instance.


typedef struct NodeId{
    uint8_t id;
    String name;
}NodeId


// Variables
static int prevHeading = -1;         // Store previous heading to avoid frequent updates
unsigned long lastUpdate = 0;        // Time tracking for updates
const int updateInterval = 500;      // Update interval in milliseconds (500ms = 0.5s)


void radio_setup(void) {
    pinMode(RFM95_RST, OUTPUT);
    digitalWrite(RFM95_RST, HIGH);

    while(!rf95.init()){}
    Serial.println("radio: Initialized");
    
    if(rf95.setFrequency(RF95_FREQ)) {
        Serial.println("radio: Error could not set frequency");
        while(true){} //Loop forever
    }
    Serial.print("radio: Frequency = ");
    Serial.println(RF95_FREQ);

    rf95.setTxPower(RF95_MIN_DB, false);
}

void radio_reset(void) {
  digitalWrite(RFM95_RST, LOW);
  delay(10);
  digitalWrite(RFM95_RST, HIGH);
  delay(10);
}

void setup(void) {
  // Initialize Serial Communication for debugging
  Serial.begin(115200);

  // Display Setup
  tft.init(240, 280);                // Initialize the display with a resolution of 240x280 pixels
  tft.fillScreen(ST77XX_BLACK);      // Set the screen background to black

  // Magnetometer Setup
  if (!lis2mdl.begin()) {            // Initialize the magnetometer
    Serial.println("Error: LIS2MDL not detected. Check your wiring!");
    while (1) delay(10);             // Stop the program if the sensor isn't detected
  }

  // LED Ring Setup
  FastLED.addLeds<NEOPIXEL, DATA_PIN>(leds, NUM_LEDS);  // Initialize the LED ring

  // GPS Setup
  gpsSerial.begin(9600);             // Start GPS communication at 9600 baud rate
}

void loop() {
  unsigned long currentTime = millis();  // Get the current time

  // GPS Data Handling
  if (gpsSerial.available() > 0) {       // Check if data is available from the GPS module
    gps.encode(gpsSerial.read());        // Decode the GPS data
    if (gps.location.isValid()) {        // Check if GPS location data is valid
      double latitude = gps.location.lat();  // Get latitude
      double longitude = gps.location.lng(); // Get longitude
      Serial.print("Lat: ");                 // Output latitude for debugging
      Serial.println(latitude, 6);           // Print latitude with 6 decimal places
      Serial.print("Lon: ");                 // Output longitude for debugging
      Serial.println(longitude, 6);          // Print longitude with 6 decimal places
    } else {
      Serial.println("Waiting for valid GPS data...");  // GPS signal lost or invalid
    }
  }

  // Magnetometer Readings
  sensors_event_t event;               // Create an event to store magnetometer readings
  lis2mdl.getEvent(&event);            // Get magnetic field data
  int correctedX = event.magnetic.x + 18;  // Apply corrections to X-axis data
  int correctedY = event.magnetic.y + 59;  // Apply corrections to Y-axis data

  // Calculate the heading (bearing) from the magnetometer data

  //NEED TO ADD TILT COMPENSATION

  int heading = (atan2(correctedX, correctedY) * 180) / PI;  // Calculate heading in degrees
  heading = heading - 90;             // Adjust to align with compass directions
  if (heading < 0) heading += 360;    // Ensure the heading is within the 0-360 range

  // Update LED ring and display every 500ms to avoid flickering
  if (currentTime - lastUpdate > updateInterval) {
    updateLEDs(heading);              // Update the LED ring based on the current heading
    updateDisplay();                  // Placeholder: Update the central TFT display with GPS data
    lastUpdate = currentTime;         // Reset the last update time
  }
}

// Function to update the LED ring based on heading direction
void updateLEDs(int heading) {
  if (abs(heading - prevHeading) >= 10) {  // Only update if the heading changes by 10 degrees or more
    prevHeading = heading;                 // Save the current heading as the previous heading
    int ledIndex = map(heading, 0, 360, 0, NUM_LEDS - 1);  // Map the heading to an LED index (0-23)

    // Update the LEDs in the ring
    for (int i = 0; i < NUM_LEDS; i++) {
      leds[i] = (i == ledIndex) ? CRGB(255, 0, 0) : CRGB::Black;  // Turn on the correct LED for the heading
    }
    FastLED.show();  // Display the updated LED colors
  }
}

// Placeholder for updating the TFT display with GPS data
void updateDisplay() {
  // Add logic to display the distance between devices on the screen
  // You can add code here to:
  // - Display the current distance between devices using the GPS data
  // - Display any other relevant information, such as GPS coordinates or signal strength
  // Example:
  // tft.setCursor(10, 50);  // Set cursor position
  // tft.setTextColor(ST77XX_WHITE);  // Set text color
  // tft.setTextSize(2);  // Set text size
  // tft.print("Distance: ");
  // tft.print(calculatedDistance);
}

// Future function to handle LoRa transmission of GPS data - See adafruit LORA code examples 
void sendGPSData() {
  // Add logic here to transmit the device's GPS coordinates using LoRa
  // You will need to:
  // - Get the current GPS coordinates
  // - Format the data for transmission
  // - Use the LoRa library to send the data to the paired device
  // Example: LoRa.beginPacket(); LoRa.print(latitude); LoRa.print(longitude); LoRa.endPacket();
}

// Future function to receive GPS data from the other device via LoRa - See adafruit LORA code examples
void receiveGPSData() {
  // Add logic here to receive the paired device's GPS coordinates using LoRa
  // You will need to:
  // - Listen for incoming LoRa packets
  // - Parse the received data
  // - Update the display and LED ring based on the other device's location
  // Example: if (LoRa.parsePacket()) { double otherLat = LoRa.read(); double otherLon = LoRa.read(); }
}

// Future function to handle edge cases (e.g., GPS signal loss, close proximity)
void handleEdgeCases() {
  // Add logic here to handle cases where:
  // - GPS signal is lost: Display a message on the screen or flash the LED ring
  // - Devices are too close: Display a warning if the GPS data is unreliable due to proximity
  // - LoRa communication is lost: Display a notification or error on the screen
}