#include <TFT_eSPI.h>
#include <TinyGPSPlus.h>
#include <HardwareSerial.h>
#include <Arduino.h>
#include <SPI.h>

#include "Free_Fonts.h"

#define RX2_PIN 16
#define TX2_PIN 17
#define GPS_BAUD 9600
#define LED 2

#define KEY_X 0 // start X
#define KEY_Y 0
#define KEY_W 240       // Key width
#define KEY_H 40        // Key height
#define KEY_SPACING_X 1 // X gap
#define KEY_SPACING_Y 1 // Y gap
#define KEY_TEXTSIZE 1  // Font size multiplier
#define BUTTON_X_DELTA 22
#define NUM_KEYS 3
#define QUEUE_SIZE 5

TFT_eSPI tft = TFT_eSPI();
TinyGPSPlus gps;
HardwareSerial serialGPS(1);
TaskHandle_t taskBlinkHandle = NULL; // for task control: suspend / resume
TaskHandle_t taskGPSHandle = NULL;
TFT_eSPI_Button key[NUM_KEYS];
SemaphoreHandle_t spiMutex;
QueueHandle_t gpsQueue = NULL;

char label[] = "label";
String btnLabel[3] = {"GPS DATA", "led state", "Other"};

// uint16_t calData[5] = { 293, 3474, 445, 3470, 0 };
// uint16_t calData[5] = { 314, 3512, 424, 3489, 0 };
// uint16_t calData[5] = { 233, 3558, 390, 3514, 0 };
// uint16_t calData[5] = { 283, 3553, 416, 3514, 0 };
// Initial Free Heap: 274996 bytes

struct GpsData
{
  float lat;
  float lng;
  float speed;
  float altitude;
  uint8_t satellites;
  bool isValid;

  uint8_t hour;
  uint8_t minute;
  uint8_t second;

  uint16_t year;
  uint8_t month;
  uint8_t day;
};

// struct GPSData1 {
//     float lat = 0.0f;
//     float lon = 0.0f;
//     float alt = 0.0f;
//     uint8_t satellites = 0;
//     float speed = 0.0f;
//     float course = 0.0f;
//     bool valid = false;
// };
// packet.lat = gps.location.isValid() ? gps.location.lat() : 0.0f;

void smartDelay(unsigned long ms);
void initDisplay();
void drawButtons();
void taskBlink(void *pvParameters);
void taskGPS(void *pvParameters);
void taskTFT(void *pvParameters);
void drawString(const char *title, uint32_t x, uint32_t y);

//------------------------------------------------------------------------------------------
void setup()
{
  Serial.begin(115200);
  spiMutex = xSemaphoreCreateMutex();
  Serial.printf("Starting FreeRTOS: Memory Usage\nInitial Free Heap: %u bytes\n", xPortGetFreeHeapSize());

  gpsQueue = xQueueCreate(QUEUE_SIZE, sizeof(GpsData));
  spiMutex = xSemaphoreCreateMutex();

  if (gpsQueue == NULL && spiMutex == NULL)
  {
    Serial.println("[Error] Failed to create queue or mutex");
    while (1)
    {
      delay(100);
    }
  }

  xTaskCreatePinnedToCore(taskBlink, "taskBlink", 2000, NULL, 1, &taskBlinkHandle, 0);
  xTaskCreatePinnedToCore(taskGPS, "taskGPS", 2000, NULL, 1, &taskGPSHandle, 0);
  xTaskCreatePinnedToCore(taskTFT, "taskTFT", 32000, NULL, 1, NULL, 1);
}

void loop()
{
  static uint32_t lastCheck = 0;
  if (millis() - lastCheck > 5000)
  {
    Serial.printf("Free Heap: %u bytes\n", xPortGetFreeHeapSize());
    lastCheck = millis();
  }
}

void taskGPS(void *pvParameters)
{
  serialGPS.begin(GPS_BAUD, SERIAL_8N1, RX2_PIN, TX2_PIN);
  Serial.print("Task GPS running on core ");
  Serial.println(xPortGetCoreID());

  GpsData data;
  unsigned long lastSendTime = 0;

  while (true)
  {
    while (serialGPS.available() > 0)
    {
      gps.encode(serialGPS.read());
    }

    if (gps.location.isUpdated() && (millis() - lastSendTime > 1000))
    {
      data.lat = gps.location.lat();
      data.lng = gps.location.lng();
      data.speed = gps.speed.kmph();
      data.altitude = gps.altitude.meters();
      data.satellites = gps.satellites.value();
      data.isValid = gps.location.isValid();

      data.year = gps.date.year();
      data.month = gps.date.month();
      data.day = gps.date.day();
      data.hour = gps.time.hour();
      data.minute = gps.time.minute();
      data.second = gps.time.second();

      // Serial.printf("Lat: %.6f, Lon: %.6f, Speed: %.1f km/h, Alt: %.1f m, Satellites: %d\n", data.lat, data.lng, data.speed, data.alt, data.sat);
      // Serial.printf("Date: %04d-%02d-%02d Time: %02d:%02d:%02d\n", data.year, data.month, data.day, data.hour, data.minute, data.second);

      if (xQueueSend(gpsQueue, &data, pdMS_TO_TICKS(100)) != pdTRUE)
      {
        Serial.println("[Gps] queue is full!");
      }
      else
      {
        lastSendTime = millis();
      }
    }

    vTaskDelay(100 / portTICK_PERIOD_MS);
    // Serial.printf("TaskGPS Stack Free: %u bytes\n", uxTaskGetStackHighWaterMark(NULL));
  }
}

void taskBlink(void *pvParameters)
{
  pinMode(LED, OUTPUT);

  for (;;)
  { // Infinite loop
    digitalWrite(LED, HIGH);
    // Serial.println("BlinkTask: LED ON");
    vTaskDelay(1000 / portTICK_PERIOD_MS); // 1000ms
    digitalWrite(LED, LOW);
    // Serial.println("BlinkTask: LED OFF");
    vTaskDelay(1000 / portTICK_PERIOD_MS);
    // Serial.print("Task Blink running on core ");
    // Serial.println(xPortGetCoreID());
    // Serial.printf("TaskBlink Stack Free: %u bytes\n", uxTaskGetStackHighWaterMark(NULL));
  }
}

void taskTFT(void *pvParameters)
{
  initDisplay();
  // drawTitle("Hello world!");
  // drawButtons();

  TFT_eSprite spr = TFT_eSprite(&tft);
  spr.createSprite(tft.width(), tft.height()/2);

  GpsData data;
  bool hasNewData = false;
  char latBuffer[20], lngBuffer[20], altBuffer[20], timeBuffer[20], dateBuffer[20];
  char satelitesBuffer[20];

  while (true)
  {
    // Serial.printf("taskTFT Stack Free: %u bytes\n", uxTaskGetStackHighWaterMark(NULL));

    if (xQueueReceive(gpsQueue, &data, 0) == pdTRUE)
    {
      hasNewData = true;
      Serial.println("[UI] get new data from gps");
    }

    if (hasNewData)
    {

      // Serial.printf("Lat: %.6f, Lon: %.6f, Speed: %.1f km/h, Alt: %.1f m, Satellites: %d\n", data.lat, data.lng, data.speed, data.alt, data.sat);
      // Serial.printf("Date: %04d-%02d-%02d Time: %02d:%02d:%02d\n", data.year, data.month, data.day, data.hour, data.minute, data.second);

      if (xSemaphoreTake(spiMutex, pdMS_TO_TICKS(100)) == pdTRUE)
      {

        snprintf(latBuffer, sizeof(latBuffer), "LAT: %.6f", data.lat);
        snprintf(lngBuffer, sizeof(lngBuffer), "LNG: %.6f", data.lng);

        snprintf(altBuffer, sizeof(altBuffer), "ALTITUDE: %.1f m", data.altitude);
        snprintf(satelitesBuffer, sizeof(satelitesBuffer), "Satellites: %d", data.satellites);

        snprintf(timeBuffer, sizeof(timeBuffer), "TIME: %02d:%02d:%02d", data.hour, data.minute, data.second);
        snprintf(dateBuffer, sizeof(dateBuffer), "DATE: %04d-%02d-%02d", data.year, data.month, data.day);

        // tft.fillScreen(TFT_BLACK);
        // tft.setCursor(0, 20);
        // tft.printf("Date: %04d-%02d-%02d", data.year, data.month, data.day);

        // tft.setCursor(0, 40);
        // tft.printf("Time: %02d:%02d:%02d", data.hour, data.minute, data.second);

        // tft.setCursor(0, 60);
        // tft.setTextPadding(240);
        // tft.print(latBuffer);

        // tft.setCursor(0, 80);
        // tft.printf("Lon: %.6f", data.lng);

        // tft.setCursor(0, 100);
        // tft.printf("Speed: %.1f km/h", data.speed);

        // tft.setCursor(0, 120);
        // tft.printf("Satellites: %d", data.satellites);

        // spr.printf("Alt: %.1f m", data.altitude);

        // drawString("ABCDEFGHIJKLMNOPQRST", 0,0);
        // drawString(timeBuffer, 0, 100);
        // drawString(altBuffer, 0, 150);

        spr.fillSprite(TFT_BLUE);
        spr.setTextDatum(TL_DATUM);

        spr.drawString(dateBuffer, 0, 0, FONT4);
        spr.drawString(timeBuffer, 0, 20, FONT4);

        spr.drawString(latBuffer, 0, 40, FONT4);
        spr.drawString(lngBuffer, 0, 60, FONT4);

        spr.drawString(satelitesBuffer, 0, 80, FONT4);

        spr.pushSprite(0, 0);

        uint16_t x = 0, y = 0;
        if (tft.getTouch(&x, &y))
        {
          tft.fillCircle(x, y, 3, TFT_BLUE);
        }

        xSemaphoreGive(spiMutex);
        hasNewData = false;
      }
    }
    vTaskDelay(50 / portTICK_PERIOD_MS);
    Serial.printf("TaskTFT Stack Free: %u bytes\n", uxTaskGetStackHighWaterMark(NULL));
  }
}

void loop_1()
{
  uint16_t t_x = 0, t_y = 0; // To store the touch coordinates

  // Get current touch state and coordinates
  bool pressed = tft.getTouch(&t_x, &t_y);
  if (pressed)
  {
    Serial.println("pressed");
    digitalWrite(LED, HIGH);
  }
  else
  {
    digitalWrite(LED, LOW);
  }

  // Adjust press state of each key appropriately
  for (uint8_t b = 0; b < NUM_KEYS; b++)
  {
    if (pressed && key[b].contains(t_x, t_y))
    {
      key[b].press(true);
    }
    else
    {
      key[b].press(false);
    }
  }

  // Check if any key has changed state
  for (uint8_t b = 0; b < NUM_KEYS; b++)
  {
    // If button was just pressed, redraw inverted button
    if (key[b].justPressed())
    {
      Serial.println("Button " + (String)b + " pressed");
      tft.setFreeFont(FF5);
      key[b].drawButton(true, btnLabel[b]);
    }

    // If button was just released, redraw normal color button
    if (key[b].justReleased())
    {
      Serial.println("Button " + (String)b + " released");
      tft.setFreeFont(FF5);
      key[b].drawButton(false, btnLabel[b]);
    }
  }

  smartDelay(100);
}

void drawButtons()
{
  tft.setFreeFont(FF1);
  for (int i = 0; i < NUM_KEYS; i++)
  {
    key[i].initButtonUL(&tft,
                        KEY_X,
                        KEY_Y + i * (KEY_H + KEY_SPACING_Y),
                        KEY_W,
                        KEY_H,
                        TFT_WHITE,
                        TFT_BLACK,
                        TFT_WHITE,
                        label,
                        KEY_TEXTSIZE);

    key[i].setLabelDatum(0, 2, MC_DATUM);
    key[i].drawButton(false, btnLabel[i]);
  }
}

void initDisplay()
{
  tft.init();
  tft.setRotation(2);

  uint16_t calData[5] = {283, 3553, 416, 3514, 0};
  tft.setTouch(calData);

#ifndef TFT_BL
  Serial.println("No TFT backlight pin defined");
#else
  pinMode(TFT_BL, OUTPUT);
  digitalWrite(TFT_BL, HIGH);
#endif

  tft.setFreeFont(FF1);
  tft.setTextColor(TFT_WHITE, TFT_BLACK);
  tft.fillScreen(TFT_BLACK);
}

void drawString(const char *title, uint32_t x, uint32_t y)
{
  tft.setFreeFont(FF1);
  tft.setTextColor(TFT_WHITE, TFT_BLACK);
  tft.drawString(title, x, y, GFXFF);
}

void smartDelay(unsigned long ms)
{
  unsigned long start = millis();
  do
  {
    while (serialGPS.available())
      gps.encode(serialGPS.read());
  } while (millis() - start < ms);
}