#include <Arduino.h>
#include <TinyGPS++.h>
#include <TFT_eSPI.h>   // TFT library for ESP32
#include <SPI.h>

// ------------------- GPS -------------------
TinyGPSPlus gps;
HardwareSerial gpsSerial(1);   // UART1 for GPS (pins defined in begin)

// ------------------- MGA1000 -------------------
HardwareSerial gyroSerial(2);  // UART2 for MGA1000

uint8_t rxBuffer[174] = {0};
int rxIndex = 0;
const uint8_t header[4] = {0x4E, 0x4A, 0x35, 0x94};
bool flag = false;
bool chksum = false;

float heading = 0, roll = 0, pitch = 0, temperature = 0;
float currentheading = 0, previousheading = 0, finalheading = 0, offset = 0;
int count = 0;
uint16_t calculated = 0, received = 0;

// ------------------- TFT -------------------
TFT_eSPI tft = TFT_eSPI();

// ------------------- Helper Functions -------------------
int findHeaderIndex(byte* array, int arrayLength, const byte* header, int headerLength) {
  for (int i = 0; i <= arrayLength - headerLength; i++) {
    bool match = true;
    for (int j = 0; j < headerLength; j++) {
      if (array[i + j] != header[j]) { match = false; break; }
    }
    if (match) return i;
  }
  return -1;
}

void rotateArrayLeft(byte* array, int length, int shift) {
  if (shift <= 0 || shift >= length) return;
  byte temp[shift];
  for (int i = 0; i < shift; i++) temp[i] = array[i];
  for (int i = 0; i < length - shift; i++) array[i] = array[i + shift];
  for (int i = 0; i < shift; i++) array[length - shift + i] = temp[i];
}

float bytesToFloat(byte *data) {
  union { byte b[4]; float f; } u;
  u.b[3] = data[0]; u.b[2] = data[1]; u.b[1] = data[2]; u.b[0] = data[3];
  return u.f;
}

uint16_t calculateChecksum(uint8_t* data) {
  uint32_t sum = 0;
  for (int i = 0; i < 56; i++) sum += data[i];
  return (uint16_t)(sum & 0xFFFF);
}

// ------------------- Setup -------------------
void setup() {
  Serial.begin(115200);

  // GPS on UART1 (RX=16, TX=17)
  gpsSerial.begin(9600, SERIAL_8N1, 16, 17);

  // MGA1000 on UART2 (RX=26, TX=27) -- adjust pins if needed
  gyroSerial.begin(115200, SERIAL_8N1, 26, 27);

  // TFT Init
  tft.init();
  tft.setRotation(3);
  tft.fillScreen(TFT_BLACK);

  tft.setTextColor(TFT_GREEN, TFT_BLACK);
  tft.setTextSize(2);
  tft.setCursor(20, 20);
  tft.println("ESP32 + MGA1000 + GPS");
  delay(1500);
  tft.fillScreen(TFT_BLACK);
}

// ------------------- Loop -------------------
void loop() {
  // ----------- Read MGA1000 -----------
  while (gyroSerial.available() > 0 && rxIndex < 174) {
    rxBuffer[rxIndex++] = gyroSerial.read();
  }

  if (rxIndex == 174) {
    int index = findHeaderIndex(rxBuffer, 174, header, 4);
    if (index >= 0) {
      rotateArrayLeft(rxBuffer, 174, index);
      calculated = calculateChecksum(rxBuffer);
      received = ((uint16_t)rxBuffer[56] << 8) | rxBuffer[57];
      flag = (calculated == received);
      chksum = flag;
    }
    rxIndex = 0; // reset
  }

  if (flag && chksum) {
    temperature = bytesToFloat(&rxBuffer[40]);
    heading     = bytesToFloat(&rxBuffer[44]);
    roll        = bytesToFloat(&rxBuffer[48]);
    pitch       = bytesToFloat(&rxBuffer[52]);

    if ((heading > -360) && (heading < 360)) {
      currentheading = heading;
      if (currentheading != previousheading) {
        count++; previousheading = currentheading;
      } else count = 0;
      if (count >= 3) { finalheading = currentheading; count = 0; }
    }
  }

  // ----------- Display on TFT -----------
  tft.fillScreen(TFT_BLACK);

  tft.setTextColor(TFT_RED, TFT_BLACK);
  tft.setTextSize(2);
  tft.setCursor(20, 40);
  tft.printf("Heading: %.2f", finalheading + offset);

  tft.setCursor(20, 80);
  tft.printf("Roll: %.2f", roll);

  tft.setCursor(20, 120);
  tft.printf("Pitch: %.2f", pitch);

  tft.setCursor(20, 160);
  tft.printf("Temp: %.2f C", temperature);

  // ----------- GPS -----------
  while (gpsSerial.available()) {
    if (gps.encode(gpsSerial.read())) {
      if (gps.location.isValid()) {
        float lat = gps.location.lat();
        float lng = gps.location.lng();

        tft.setTextColor(TFT_CYAN, TFT_BLACK);
        tft.setCursor(20, 200);
        tft.printf("Lat: %.6f", lat);

        tft.setCursor(20, 230);
        tft.printf("Lng: %.6f", lng);
      }
    }
  }

  delay(200);
}