#include "gps_sync.h"
#include "driver/gpio.h"
#include "driver/uart.h"
#include "esp_timer.h"
#include "esp_log.h"
#include "esp_rom_sys.h"
#include <string.h>
#include <time.h>
#include <stdarg.h>
#include <stdio.h>
#include <assert.h>
#include <inttypes.h>

static const char *TAG = "GPS_SYNC";

#define GPS_BAUD_RATE    9600
#define UART_BUF_SIZE    1024

// --- GLOBAL STATE ---
static uart_port_t gps_uart_num = UART_NUM_1;
static int64_t monotonic_offset_us = 0;
static volatile int64_t last_pps_monotonic = 0;
static bool gps_has_fix = false;
static bool use_gps_for_logs = false;
static SemaphoreHandle_t sync_mutex;
static volatile bool force_sync_update = true;

// Debug Buffer
static char s_last_nmea_line[128] = "<WAITING FOR DATA>";

// PPS interrupt: Captures the exact monotonic time of the rising edge
static void IRAM_ATTR pps_isr_handler(void* arg) {
    int64_t now = esp_timer_get_time();
    last_pps_monotonic = now;
}

// Parse GPS time from NMEA (GPRMC or GNRMC)
static bool parse_gprmc(const char* nmea, struct tm* tm_out, bool* valid) {
    if (strncmp(nmea, "$GPRMC", 6) != 0 && strncmp(nmea, "$GNRMC", 6) != 0) return false;

    char *p = strchr(nmea, ',');
    if (!p) return false;
    p++; // Move past comma

    int hour, min, sec;
    if (sscanf(p, "%2d%2d%2d", &hour, &min, &sec) != 3) return false;

    p = strchr(p, ',');
    if (!p) return false;
    p++;
    *valid = (*p == 'A');

    for (int i = 0; i < 7; i++) {
        p = strchr(p, ',');
        if (!p) return false;
        p++;
    }

    int day, month, year;
    if (sscanf(p, "%2d%2d%2d", &day, &month, &year) != 3) return false;

    year += (year < 80) ? 2000 : 1900;

    tm_out->tm_sec = sec;
    tm_out->tm_min = min;
    tm_out->tm_hour = hour;
    tm_out->tm_mday = day;
    tm_out->tm_mon = month - 1;
    tm_out->tm_year = year - 1900;
    tm_out->tm_isdst = 0;

    return true;
}

void gps_force_next_update(void) {
    force_sync_update = true;
    ESP_LOGW(TAG, "Requesting forced GPS sync update");
}

static time_t timegm_impl(struct tm *tm) {
    time_t t = mktime(tm);
    return t;
}

static void gps_task(void* arg) {
    uint8_t d_buf[64];
    char line[128];
    int pos = 0;
    static int log_counter = 0;

    setenv("TZ", "UTC", 1);
    tzset();

    while (1) {
        int len = uart_read_bytes(gps_uart_num, d_buf, sizeof(d_buf), pdMS_TO_TICKS(100));

        if (len > 0) {
            for (int i = 0; i < len; i++) {
                uint8_t data = d_buf[i];

                if (data == '\n' || data == '\r') {
                    if (pos > 0) {
                        line[pos] = '\0';

                        // Copy to debug buffer
                        xSemaphoreTake(sync_mutex, portMAX_DELAY);
                        strncpy(s_last_nmea_line, line, sizeof(s_last_nmea_line) - 1);
                        s_last_nmea_line[sizeof(s_last_nmea_line) - 1] = '\0';
                        xSemaphoreGive(sync_mutex);

                        struct tm gps_tm;
                        bool valid_fix;

                        if (parse_gprmc(line, &gps_tm, &valid_fix)) {
                            if (valid_fix) {
                                time_t gps_time_sec = timegm_impl(&gps_tm);
                                int64_t last_pps_snap = last_pps_monotonic;
                                int64_t now = esp_timer_get_time();
                                int64_t age_us = now - last_pps_snap;

                                if (last_pps_snap > 0 && age_us < 900000) {

                                    int64_t gps_time_us = (int64_t)gps_time_sec * 1000000LL;
                                    int64_t new_offset = gps_time_us - last_pps_snap;

                                    xSemaphoreTake(sync_mutex, portMAX_DELAY);
                                    if (monotonic_offset_us == 0 || force_sync_update) {
                                        monotonic_offset_us = new_offset;
                                        if (force_sync_update) {
                                            ESP_LOGW(TAG, "GPS SNAP: Offset forced to %" PRIi64 " us", monotonic_offset_us);
                                            force_sync_update = false;
                                            log_counter = 0;
                                        }
                                    } else {
                                        monotonic_offset_us = (monotonic_offset_us * 9 + new_offset) / 10;
                                    }
                                    gps_has_fix = true;
                                    xSemaphoreGive(sync_mutex);

                                    // Periodic Logging
                                    if (log_counter <= 0) {
                                        // CHANGED FROM ESP_LOGI TO ESP_LOGD (Hidden by default)
                                        ESP_LOGD(TAG, "GPS Sync: %02d:%02d:%02d | Offset: %" PRIi64 " us | PPS Age: %" PRIi64 " ms",
                                                 gps_tm.tm_hour, gps_tm.tm_min, gps_tm.tm_sec,
                                                 monotonic_offset_us, age_us / 1000);
                                        log_counter = 10;
                                    }
                                    log_counter--;

                                } else {
                                    // Keep Warnings visible
                                    if (log_counter <= 0) {
                                        ESP_LOGW(TAG, "GPS valid but PPS missing/old (Age: %" PRIi64 " ms)", age_us / 1000);
                                        log_counter = 10;
                                    }
                                    log_counter--;
                                }
                            } else {
                                gps_has_fix = false;
                            }
                        }
                    }
                    pos = 0;
                } else {
                    if (pos < sizeof(line) - 1) {
                        line[pos++] = data;
                    }
                }
            }
        }
    }
}

void gps_get_last_nmea(char *buf, size_t max_len) {
    if (!buf || max_len == 0) return;
    xSemaphoreTake(sync_mutex, portMAX_DELAY);
    strncpy(buf, s_last_nmea_line, max_len);
    buf[max_len - 1] = '\0';
    xSemaphoreGive(sync_mutex);
}

void gps_sync_init(const gps_sync_config_t *config, bool use_gps_log_timestamps) {
    ESP_LOGI(TAG, "Initializing GPS Sync (UART %d, PPS GPIO %d)", config->uart_port, config->pps_pin);

    gpio_config_t pps_poll_conf = {
        .pin_bit_mask = (1ULL << config->pps_pin),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_DISABLE
    };
    ESP_ERROR_CHECK(gpio_config(&pps_poll_conf));

    bool pps_detected = false;
    int start_level = gpio_get_level(config->pps_pin);
    for (int i = 0; i < 2000; i++) {
        if (gpio_get_level(config->pps_pin) != start_level) {
            pps_detected = true;
            break;
        }
        vTaskDelay(pdMS_TO_TICKS(1));
    }

    if (!pps_detected) {
        ESP_LOGW(TAG, "No PPS signal detected on GPIO %d during boot check.", config->pps_pin);
    } else {
        ESP_LOGI(TAG, "PPS signal activity detected.");
    }

    gps_uart_num = config->uart_port;
    use_gps_for_logs = use_gps_log_timestamps;
    gps_force_next_update();
    sync_mutex = xSemaphoreCreateMutex();

    if (use_gps_log_timestamps) {
        esp_log_set_vprintf(gps_log_vprintf);
    }

    uart_config_t uart_config = {
        .baud_rate = GPS_BAUD_RATE,
        .data_bits = UART_DATA_8_BITS,
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
        .source_clk = UART_SCLK_DEFAULT,
    };

    ESP_ERROR_CHECK(uart_driver_install(config->uart_port, UART_BUF_SIZE, 0, 0, NULL, 0));
    ESP_ERROR_CHECK(uart_param_config(config->uart_port, &uart_config));
    ESP_ERROR_CHECK(uart_set_pin(config->uart_port, config->tx_pin, config->rx_pin, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));

    gpio_config_t pps_intr_conf = {
        .intr_type = GPIO_INTR_POSEDGE,
        .mode = GPIO_MODE_INPUT,
        .pin_bit_mask = (1ULL << config->pps_pin),
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
    };
    ESP_ERROR_CHECK(gpio_config(&pps_intr_conf));

    gpio_install_isr_service(0);
    ESP_ERROR_CHECK(gpio_isr_handler_add(config->pps_pin, pps_isr_handler, NULL));

    xTaskCreate(gps_task, "gps_task", 4096, NULL, 5, NULL);
}

gps_timestamp_t gps_get_timestamp(void) {
    gps_timestamp_t ts;
    clock_gettime(CLOCK_MONOTONIC, &ts.mono_ts);

    ts.monotonic_us = (int64_t)ts.mono_ts.tv_sec * 1000000LL + ts.mono_ts.tv_nsec / 1000;
    ts.monotonic_ms = ts.monotonic_us / 1000;

    xSemaphoreTake(sync_mutex, portMAX_DELAY);
    ts.gps_us = ts.monotonic_us + monotonic_offset_us;
    ts.synced = gps_has_fix;
    xSemaphoreGive(sync_mutex);

    ts.gps_ms = ts.gps_us / 1000;
    return ts;
}

int64_t gps_get_monotonic_ms(void) {
    return esp_timer_get_time() / 1000;
}

bool gps_is_synced(void) {
    return gps_has_fix;
}

// --- NEW: PPS Age Getter ---
int64_t gps_get_pps_age_ms(void) {
    if (last_pps_monotonic == 0) return -1;
    int64_t now = esp_timer_get_time();
    return (now - last_pps_monotonic) / 1000;
}

// ---------------- LOGGING SYSTEM INTERCEPTION ----------------

uint32_t gps_log_timestamp(void) {
    return (uint32_t)(esp_timer_get_time() / 1000ULL);
}

int gps_log_vprintf(const char *fmt, va_list args) {
    static char buffer[512];
    int ret = vsnprintf(buffer, sizeof(buffer), fmt, args);
    assert(ret >= 0);

    if (use_gps_for_logs) {
        char *timestamp_start = NULL;
        for (int i = 0; buffer[i] != '\0' && i < sizeof(buffer) - 20; i++) {
            if ((buffer[i] == 'I' || buffer[i] == 'W' || buffer[i] == 'E' ||
                 buffer[i] == 'D' || buffer[i] == 'V') &&
                buffer[i+1] == ' ' && buffer[i+2] == '(') {
                timestamp_start = &buffer[i+3];
                break;
            }
        }

        if (timestamp_start) {
            char *timestamp_end = strchr(timestamp_start, ')');
            if (timestamp_end) {
                uint32_t monotonic_log_ms = 0;
                if (sscanf(timestamp_start, "%lu", &monotonic_log_ms) == 1) {
                    char reformatted[512];
                    size_t prefix_len = timestamp_start - buffer;

                    if (prefix_len > sizeof(reformatted)) prefix_len = sizeof(reformatted);
                    memcpy(reformatted, buffer, prefix_len);

                    int decimal_len = 0;

                    if (gps_has_fix) {
                        int64_t log_mono_us = (int64_t)monotonic_log_ms * 1000;
                        int64_t log_gps_us = log_mono_us + monotonic_offset_us;

                        uint64_t gps_sec = log_gps_us / 1000000;
                        uint32_t gps_ms = (log_gps_us % 1000000) / 1000;

                        decimal_len = snprintf(reformatted + prefix_len,
                                             sizeof(reformatted) - prefix_len,
                                               "+%" PRIu64 ".%03lu", gps_sec, gps_ms);
                    } else {
                        uint32_t sec = monotonic_log_ms / 1000;
                        uint32_t ms = monotonic_log_ms % 1000;
                        decimal_len = snprintf(reformatted + prefix_len,
                                             sizeof(reformatted) - prefix_len,
                                               "*%lu.%03lu", (unsigned long)sec, (unsigned long)ms);
                    }
                    strcpy(reformatted + prefix_len + decimal_len, timestamp_end);
                    return printf("%s", reformatted);
                }
            }
        }
    }
    return printf("%s", buffer);
}