#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>                // RadioHead Library(LoRa)

#include "inc/board_config.h"

//GLOBAL VARS/OBJS

// MAGNETOMETER (LIS2MDL) CONFIGURATION
Adafruit_LIS2MDL lis2mdl = Adafruit_LIS2MDL(12345);  // Create magnetometer object
Adafruit_ST7789 tft = Adafruit_ST7789(TFT_CS, TFT_DC, TFT_RST);  // Initialize display
CRGB leds[NUM_LEDS];                 // Array to hold LED colors
SoftwareSerial gpsSerial(GPS_RX_PIN, GPS_TX_PIN);  // Software serial for GPS
TinyGPSPlus gps;                     // TinyGPS++ object to process GPS data
RH_RF95 rf95(RFM9X_CS, RFM9X_INT);  // Radio instance.


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

const uint8_t MAX_BATT_PERCENTAGE = 100;
const uint8_t MIN_BATT_PERCENTAGE = 0;


typedef struct MSG{
  uint8_t id;
  double latitude;
  double longitude;
}MSG;


enum result {
  Ok = 0,
  Err,
  ReceiveFailed,
  NoReply,
};

uint8_t db; //Used to hold the current setting for the TX power.
uint8_t buffer[sizeof(MSG)];

MSG msg_out;
MSG msg_in;
//msg_out.id = 0;   //Put the system ID here


void MSG_Print(MSG *msg) {
  Serial.print("ID: ");
  Serial.println(msg->id);

  Serial.print("Latitude: ");
  Serial.println(msg->latitude);

  Serial.print("Longitude: ");
  Serial.println(msg->longitude);
}

void Radio_Reset(void) {
  digitalWrite(RFM9X_RST, LOW);
  delay(10);
  digitalWrite(RFM9X_RST, HIGH);
  delay(10);
}

uint8_t Radio_Setup(void) {
  Serial.println("Radio_Setup()");

  pinMode(RFM9X_RST, OUTPUT);
  digitalWrite(RFM9X_RST, HIGH);

  Radio_Reset();

  while (!rf95.init()) {
    Serial.println("Radio_Setup(): Failed to initialize");
    Serial.println("Check SPI connections!");
    return Err;
  }
  Serial.println("LoRa radio init OK!");
  return Ok;
}

uint8_t Radio_Configure(void) {
  // Defaults after init are 434.0MHz, modulation GFSK_Rb250Fd250, +13dbM
  if(!rf95.setFrequency(RF95_FREQ)) {
    Serial.println("Radio_Configure(): failed to set frequency");
    return Err;
  }
  Serial.print("Set Freq to: "); Serial.println(RF9X_FREQ);

  rf95.setTxPower(RF9X_MIN_DB, false);

  return Ok;
}

uint8_t Radio_SendMsg(MSG *msg) {
  Serial.println("Radio_SendMsg()");

  Serial.println("Sending Message: ");
  Serial.print("Message size: ");
  Serial.println(sizeof(MSG));

  MSG_Print(msg);

  rf95.send((uint8_t *)msg, sizeof(MSG));

  Serial.println("waiting for packet sent...");
  delay(10);
  rf95.waitPacketSent();

  return Ok;
}

uint8_t Radio_CheckForMsg(MSG *msg) {
  Serial.println("Radio_CheckForMsg()");

  //Now we wait for an packet.
  if (rf95.waitAvailableTimeout(1000)) {
    // Should be a reply message for us now
    if (rf95.recv(buffer, sizeof(MSG))) {
      Serial.print("Received reply: ");

      msg->id = (uint8_t) buffer[0];
      msg->latitude = *(double *) &buffer[1];
      msg->longitude = *(double *) &buffer[5];

      MSG_Print(msg);

      Serial.print("RSSI: ");
      Serial.println(rf95.lastRssi(), DEC);
    } else {
      Serial.println("Receive failed");
      return ReceiveFailed;
    }
  } else {
    Serial.println("No reply");
    return NoReply;
  }
  return Ok;
}

//The algo below allows us to minimize the needed TX power.
void Radio_Main(MSG *msg_out, MSG *msg_in) {
  for(db = RF9X_MIN_DB; db <= RF9X_MAX_DB; db++) {
    Radio_SendMsg(msg_out);
    if(Radio_CheckForMsg(msg_in) == Ok) {
      break;
    }
    else{
      Serial.print("TX Power set to:");
      Serial.println(db);
    }
  }

  //We "delay" for 1 seconds by calling the radio check twice.
  if(Radio_CheckForMsg(msg_in) == Ok) {
    Radio_SendMsg(msg_out);
  }

}

//Returns the result type and updates the global values(coordinates).
uint8_t GPS_GetCoordinates() {
    Serial.println("GPS_GetCoordinates()");
  if (!gpsSerial.available()) {
    Serial.println("INFO: GPS not availble.");
    return NoReply;
  }
    gps.encode(gpsSerial.read());        // Decode the GPS data
    if (gps.location.isValid()) {        // Check if GPS location data is valid
      msg_out.latitude = gps.location.lat();
      msg_out.longitude = gps.location.lat();
    }

    return Ok;
}

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

  TFT_Init();

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

  // GPS Setup
  gpsSerial.begin(GPS_BUADRATE); 

  // Setup the Radio module.

  //Retry until sucsess.
  for(uint8_t i = 0; i < RETRIES; i++) {
    delay(100);
    if(Radio_Setup() == Ok){
      break;
    }
  }

  //Retry configuration until success
  for(uint8_t i = 0; i < RETRIES; i++) {
    delay(100);
    if(Radio_Configure() == Ok){
      break;
    }
  }
}

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

  // 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
  }

    //Handle the Radio data.
    sendGPSData();
    receiveGPSData();

    //Preform display update.
    updateDisplay();
}

// 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
  }
}

// Updates the distance part of the display
void TFT_UpdateDistance(uint16_t meters) {
  Serial.println("TFT_UpdateDistance()");
  tft.setTextSize(4);
  tft.setCursor(0, 20);
  tft.setTextColor(ST77XX_WHITE);
  tft.setTextWrap(true);
  tft.println(" Distance");
  tft.print("   ");
  tft.print(distance);
  tft.print("m");
}

// Updates the direction part of the display
// Mostly optional if you want to add degrees for navigation in addtion to
// the led ring.
void TFT_UpdateDirection(uint16_t degrees) {
    tft.setTextWrap(false);
    tft.setCursor(0, 150);
    tft.setTextColor(ST77XX_WHITE);
    tft.setTextSize(3);
    tft.print(meters);
    tft.println(" Degrees");
}

// Updates the batter/voltage state.
uint8_t TFT_UpdateBatteryState(uint8_t percentage) {
    if(percentage > MAX_BATT_PERCENTAGE || percentage < MIN_BATT_PERCENTAGE){
        return Err;
    }
    tft.setTextSize(4);
    tft.setCursor(0, 100);
    if(percentage >= 80){
    tft.setTextColor(ST77XX_GREEN);
    }
    else if(percentage < 80 && percentage > 50) {
    tft.setTextColor(ST77XX_ORANGE);
    }
    else if(percentage <= 50) {
    tft.setTextColor(ST77XX_RED);
    }
    tft.setTextWrap(true);
    tft.print("Batt: ");
    tft.print(percentage);
    tft.print("%");
    return Ok;
}

// Displays the specifications of the node.
void TFT_DisplaySpecs(void) {
    Serial.println("TFT_DisplaySpecs(): No data");
}


// Resets the the display
uint8_t TFT_Reset() {
  Serial.println("TFT_Reset()");
  tft.fillScreen(ST77XX_BLACK);     // Set the screen background to black
    //Can add default startup logo/screen here for later.
}

// Initalizes the Screen with default startup screen.
uint8_t TFT_Init(void) {
  Serial.println("TFT_Init()");
  // Display Setup
  tft.init(TFT_X, TFT_Y);           // Initialize the display with a resolution of 240x280 pixels
  tft.fillScreen(ST77XX_BLACK);     // Set the screen background to black
  tft.setRotation(0);                // Set's the rotation of the screen.

  TFT_UpdateDistance(0);
  TFT_UpdateDirection(0);
  TFT_UpdateBatteryState(100);
}


// Placeholder for updating the TFT display with GPS data
void updateDisplay() {
    //Print out our debugging statment.
    Serial.println("updateDisplay()");

    // Disable interrupts while using the dispaly.
    cli();

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

  tft.fillScreen(ST77XX_BLACK);     // Set the screen background to black
  TFT_UpdateDistance(0);
  TFT_UpdateDirection(0);
  TFT_UpdateBatteryState(100);

    //Re-Enable interrupts globally.
    sei(); 
}

// Future function to handle LoRa transmission of GPS data - See adafruit LORA code examples 
void sendGPSData() {
  Serial.println("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(); 

    //Don't waste cpu cycles if no new gps data.
    if(GPS_GetCoordinates() == NoReply){
        return;
    }

    for(db = RF9X_MIN_DB; db <= RF9X_MAX_DB; db++) {
    Radio_SendMsg(&msg_out);
    if(Radio_CheckForMsg(&msg_in) == Ok) {
      break;
    }
    else{
      Serial.print("TX Power set to:");
      Serial.println(db);
    }
  } 
}

// Future function to receive GPS data from the other device via LoRa - See adafruit LORA code examples
void receiveGPSData() {
  Serial.println("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() {
  Serial.println("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

}