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

#define GPS_UART_NUM     UART_NUM_1
#define GPS_RX_PIN       GPIO_NUM_4
#define GPS_TX_PIN       GPIO_NUM_5
#define PPS_GPIO         GPIO_NUM_1
#define GPS_BAUD_RATE    9600
#define UART_BUF_SIZE    1024

static const char *TAG = "GPS_SYNC";

// GPS sync state
static int64_t monotonic_offset_us = 0;
static volatile int64_t last_pps_monotonic = 0;
static volatile time_t next_pps_gps_second = 0;
static bool gps_has_fix = false;
static bool use_gps_for_logs = false;
static SemaphoreHandle_t sync_mutex;

// For decimal timestamp formatting - stores last timestamp parts
static uint32_t last_timestamp_sec = 0;
static uint16_t last_timestamp_ms = 0;

// PPS interrupt - captures exact monotonic time at second boundary
static void IRAM_ATTR pps_isr_handler(void* arg) {
    last_pps_monotonic = esp_timer_get_time();
}

// Parse GPS time from NMEA sentence
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;
    
    // Time field
    p++;
    int hour, min, sec;
    if (sscanf(p, "%2d%2d%2d", &hour, &min, &sec) != 3) {
        return false;
    }
    
    // Status field (A=valid, V=invalid)
    p = strchr(p, ',');
    if (!p) return false;
    p++;
    *valid = (*p == 'A');
    
    // Skip to date field (8 commas ahead from time)
    for (int i = 0; i < 7; i++) {
        p = strchr(p, ',');
        if (!p) return false;
        p++;
    }
    
    // Date field: ddmmyy
    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;
}

// GPS processing task
static void gps_task(void* arg) {
    char line[128];
    int pos = 0;
    
    while (1) {
        uint8_t data;
        int len = uart_read_bytes(GPS_UART_NUM, &data, 1, 100 / portTICK_PERIOD_MS);
        
        if (len > 0) {
            if (data == '\n') {
                line[pos] = '\0';
                
                struct tm gps_tm;
                bool valid;
                if (parse_gprmc(line, &gps_tm, &valid)) {
                    if (valid) {
                        time_t gps_time = mktime(&gps_tm);
                        
                        xSemaphoreTake(sync_mutex, portMAX_DELAY);
                        next_pps_gps_second = gps_time + 1;
                        xSemaphoreGive(sync_mutex);
                        
                        vTaskDelay(pdMS_TO_TICKS(300));
                        
                        xSemaphoreTake(sync_mutex, portMAX_DELAY);
                        if (last_pps_monotonic > 0) {
                            int64_t gps_us = (int64_t)next_pps_gps_second * 1000000LL;
                            int64_t new_offset = gps_us - last_pps_monotonic;
                            
                            if (monotonic_offset_us == 0) {
                                monotonic_offset_us = new_offset;
                            } else {
                                // Low-pass filter: 90% old + 10% new
                                monotonic_offset_us = (monotonic_offset_us * 9 + new_offset) / 10;
                            }
                            
                            gps_has_fix = true;
                            
                            ESP_LOGI(TAG, "GPS sync: %04d-%02d-%02d %02d:%02d:%02d, offset=%lld us",
                                    gps_tm.tm_year + 1900, gps_tm.tm_mon + 1, gps_tm.tm_mday,
                                    gps_tm.tm_hour, gps_tm.tm_min, gps_tm.tm_sec,
                                    monotonic_offset_us);
                        }
                        xSemaphoreGive(sync_mutex);
                    } else {
                        gps_has_fix = false;
                    }
                }
                
                pos = 0;
            } else if (pos < sizeof(line) - 1) {
                line[pos++] = data;
            }
        }
    }
}

void gps_sync_init(bool use_gps_log_timestamps) {
    ESP_LOGI(TAG, "Initializing GPS sync");
    
    use_gps_for_logs = use_gps_log_timestamps;
    
    if (use_gps_log_timestamps) {
        ESP_LOGI(TAG, "ESP_LOG timestamps: GPS time in seconds.milliseconds format");
        // Override vprintf to add decimal point to timestamps
        esp_log_set_vprintf(gps_log_vprintf);
    }
    
    sync_mutex = xSemaphoreCreateMutex();
    
    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(GPS_UART_NUM, UART_BUF_SIZE, 0, 0, NULL, 0));
    ESP_ERROR_CHECK(uart_param_config(GPS_UART_NUM, &uart_config));
    ESP_ERROR_CHECK(uart_set_pin(GPS_UART_NUM, GPS_TX_PIN, GPS_RX_PIN, 
                                 UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
    
    gpio_config_t io_conf = {
        .intr_type = GPIO_INTR_POSEDGE,
        .mode = GPIO_MODE_INPUT,
        .pin_bit_mask = (1ULL << PPS_GPIO),
        .pull_up_en = GPIO_PULLUP_ENABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
    };
    ESP_ERROR_CHECK(gpio_config(&io_conf));
    
    ESP_ERROR_CHECK(gpio_install_isr_service(0));
    ESP_ERROR_CHECK(gpio_isr_handler_add(PPS_GPIO, pps_isr_handler, NULL));
    
    xTaskCreate(gps_task, "gps_task", 4096, NULL, 5, NULL);
    
    ESP_LOGI(TAG, "GPS sync initialized (RX=GPIO%d, PPS=GPIO%d)", GPS_RX_PIN, PPS_GPIO);
}

gps_timestamp_t gps_get_timestamp(void) {
    gps_timestamp_t ts;
    
    // Using clock_gettime (POSIX standard, portable)
    // ESP32 supports CLOCK_MONOTONIC for monotonic time
    clock_gettime(CLOCK_MONOTONIC, &ts.mono_ts);
    
    xSemaphoreTake(sync_mutex, portMAX_DELAY);
    
    // Convert timespec to microseconds
    ts.monotonic_us = (int64_t)ts.mono_ts.tv_sec * 1000000LL + 
                      ts.mono_ts.tv_nsec / 1000;
    
    // Convert to milliseconds
    ts.monotonic_ms = ts.monotonic_us / 1000;
    
    // Calculate GPS time
    ts.gps_us = ts.monotonic_us + monotonic_offset_us;
    ts.gps_ms = ts.gps_us / 1000;
    
    ts.synced = gps_has_fix;
    xSemaphoreGive(sync_mutex);
    
    return ts;
}

// Alternative: Get just milliseconds using clock_gettime
// Useful for simple logging where you only need millisecond resolution
int64_t gps_get_monotonic_ms(void) {
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    // Convert: seconds to ms + nanoseconds to ms
    return (int64_t)ts.tv_sec * 1000LL + ts.tv_nsec / 1000000;
}

bool gps_is_synced(void) {
    return gps_has_fix;
}

// Custom log timestamp function - returns value formatted as seconds*1000000 + milliseconds*1000
// This allows us to extract both seconds and milliseconds when needed
// When printed directly, shows full milliseconds (we format it with decimal in custom logger)
uint32_t esp_log_timestamp(void) {
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    int64_t monotonic_us = (int64_t)ts.tv_sec * 1000000LL + ts.tv_nsec / 1000;
    
    int64_t time_us;
    if (!use_gps_for_logs || !gps_has_fix) {
        time_us = monotonic_us;
    } else {
        time_us = monotonic_us + monotonic_offset_us;
    }
    
    // Convert to milliseconds and store parts
    uint64_t time_ms = time_us / 1000;
    last_timestamp_sec = time_ms / 1000;
    last_timestamp_ms = time_ms % 1000;
    
    // Return total milliseconds (ESP-IDF will print this)
    // Our custom vprintf will reformat it
    return (uint32_t)time_ms;
}

// Custom vprintf that reformats log timestamps to show decimal point and sync status
// Converts: I (1733424645234) TAG: message
// To:       I (+1733424645.234) TAG: message  (GPS synced)
// Or:       I (*1.234) TAG: message           (not synced - monotonic)
int gps_log_vprintf(const char *fmt, va_list args) {
    static char buffer[512];
    
    // Format the message into our buffer
    int ret = vsnprintf(buffer, sizeof(buffer), fmt, args);
    
    if (use_gps_for_logs) {
        // Look for timestamp pattern: "I (", "W (", "E (", etc.
        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) {
            // Find the closing parenthesis
            char *timestamp_end = strchr(timestamp_start, ')');
            if (timestamp_end) {
                // Extract timestamp value
                uint32_t timestamp_ms = 0;
                if (sscanf(timestamp_start, "%lu", &timestamp_ms) == 1) {
                    uint32_t sec = timestamp_ms / 1000;
                    uint32_t ms = timestamp_ms % 1000;
                    
                    // Choose prefix based on GPS sync status
                    char prefix = gps_has_fix ? '+' : '*';
                    
                    // Rebuild the string with decimal point and prefix
                    char reformatted[512];
                    size_t prefix_len = timestamp_start - buffer;
                    
                    // Copy everything before timestamp
                    memcpy(reformatted, buffer, prefix_len);
                    
                    // Add prefix, formatted timestamp with decimal
                    int decimal_len = snprintf(reformatted + prefix_len, 
                                             sizeof(reformatted) - prefix_len,
                                             "%c%lu.%03u", prefix, sec, ms);
                    
                    // Copy everything after timestamp
                    strcpy(reformatted + prefix_len + decimal_len, timestamp_end);
                    
                    // Print the reformatted string
                    return printf("%s", reformatted);
                }
            }
        }
    }
    
    // If not reformatting or something went wrong, just print original
    return printf("%s", buffer);
}