/*
 * gps_sync.c
 *
 * Copyright (c) 2025 Umber Networks & Robert McMahon
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <sys/time.h>
#include <time.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "esp_err.h"
#include "esp_timer.h"
#include "driver/uart.h"
#include "driver/gpio.h"
#include "gps_sync.h"

static const char *TAG = "GPS_SYNC";

#define GPS_BUF_SIZE 1024

// --- Internal State ---
static gps_sync_config_t s_cfg;
static volatile int64_t s_last_pps_us = 0;
static volatile int64_t s_nmea_epoch_us = 0;
static volatile bool s_nmea_valid = false;
static char s_last_nmea_msg[128] = {0};
static bool s_time_set = false;

// --- PPS Handler ---
static void IRAM_ATTR pps_gpio_isr_handler(void* arg) {
    s_last_pps_us = esp_timer_get_time();
}

// --- Time Helper ---
static void set_system_time(char *time_str, char *date_str) {
    // time_str: HHMMSS.ss (e.g., 123519.00)
    // date_str: DDMMYY    (e.g., 230394)

    struct tm tm_info = {0};

    // Parse Time
    int h, m, s;
    if (sscanf(time_str, "%2d%2d%2d", &h, &m, &s) != 3) return;
    tm_info.tm_hour = h;
    tm_info.tm_min = m;
    tm_info.tm_sec = s;

    // Parse Date
    int day, mon, year;
    if (sscanf(date_str, "%2d%2d%2d", &day, &mon, &year) != 3) return;
    tm_info.tm_mday = day;
    tm_info.tm_mon = mon - 1;       // 0-11
    tm_info.tm_year = year + 100;   // Years since 1900 (2025 -> 125)

    time_t t = mktime(&tm_info);
    if (t == -1) return;

    struct timeval tv = { .tv_sec = t, .tv_usec = 0 };

    // Simple sync: Only set if not set, or if drift is massive (>2s)
    // In a real PTP/GPS app you'd use a PLL here, but this is a shell tool.
    struct timeval now;
    gettimeofday(&now, NULL);

    if (!s_time_set || llabs(now.tv_sec - t) > 2) {
        settimeofday(&tv, NULL);
        s_time_set = true;
        ESP_LOGI(TAG, "System Time Updated to GPS: %s", asctime(&tm_info));
    }
}

// --- NMEA Parser ---
static void parse_nmea_line(char *line) {
    strlcpy(s_last_nmea_msg, line, sizeof(s_last_nmea_msg));

    // Support GPRMC and GNRMC
    if (strncmp(line, "$GPRMC", 6) == 0 || strncmp(line, "$GNRMC", 6) == 0) {
        char *p = line;
        int field = 0;
        char *time_ptr = NULL;
        char *date_ptr = NULL;
        char status = 'V';

        // Walk fields
        // $GPRMC,Time,Status,Lat,NS,Lon,EW,Spd,Trk,Date,...
        // Field 1: Time
        // Field 2: Status
        // Field 9: Date

        while ((p = strchr(p, ',')) != NULL) {
            p++;
            field++;

            if (field == 1) time_ptr = p;
            else if (field == 2) status = *p;
            else if (field == 9) {
                date_ptr = p;
                break; // We have what we need
            }
        }

        s_nmea_valid = (status == 'A');

        if (s_nmea_valid) {
            s_nmea_epoch_us = esp_timer_get_time();

            // Extract substrings for Time/Date (comma terminated)
            if (time_ptr && date_ptr) {
                char t_buf[16] = {0};
                char d_buf[16] = {0};

                char *end = strchr(time_ptr, ',');
                if (end) {
                    int len = end - time_ptr;
                    if (len < sizeof(t_buf)) {
                        memcpy(t_buf, time_ptr, len);
                        t_buf[len] = 0;
                    }
                }

                end = strchr(date_ptr, ',');
                if (end) {
                    int len = end - date_ptr;
                    if (len < sizeof(d_buf)) {
                        memcpy(d_buf, date_ptr, len);
                        d_buf[len] = 0;
                    }
                }

                // Update System Clock
                if (t_buf[0] && d_buf[0]) {
                    set_system_time(t_buf, d_buf);
                }
            }
        }
    }
}

// --- UART Task ---
static void gps_task(void *pvParameters) {
    uint8_t *data = (uint8_t *)malloc(GPS_BUF_SIZE);
    if (!data) {
        ESP_LOGE(TAG, "Failed to allocate GPS buffer");
        vTaskDelete(NULL);
        return;
    }

    char line_buf[128];
    int line_pos = 0;

    while (1) {
        int len = uart_read_bytes(s_cfg.uart_port, data, GPS_BUF_SIZE, 20 / portTICK_PERIOD_MS);
        if (len > 0) {
            for (int i = 0; i < len; i++) {
                char c = (char)data[i];
                if (c == '\n' || c == '\r') {
                    if (line_pos > 0) {
                        line_buf[line_pos] = 0;
                        parse_nmea_line(line_buf);
                        line_pos = 0;
                    }
                } else if (line_pos < sizeof(line_buf) - 1) {
                    line_buf[line_pos++] = c;
                }
            }
        }
    }
    free(data);
    vTaskDelete(NULL);
}

// --- API ---

void gps_sync_init(const gps_sync_config_t *cfg, bool force_enable) {
    if (!cfg) return;
    s_cfg = *cfg;

    uart_config_t uart_config = {
        .baud_rate = 9600,
        .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_err_t err = uart_driver_install(s_cfg.uart_port, GPS_BUF_SIZE * 2, 0, 0, NULL, 0);
    if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
        ESP_LOGE(TAG, "Failed to install UART driver: %s", esp_err_to_name(err));
        return;
    }
    uart_param_config(s_cfg.uart_port, &uart_config);
    err = uart_set_pin(s_cfg.uart_port, s_cfg.tx_pin, s_cfg.rx_pin, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Failed to set UART pins: %s", esp_err_to_name(err));
        return;
    }

    gpio_config_t io_conf = {};
    io_conf.intr_type = GPIO_INTR_POSEDGE;
    io_conf.pin_bit_mask = (1ULL << s_cfg.pps_pin);
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pull_up_en = 1;
    err = gpio_config(&io_conf);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Failed to configure PPS GPIO %d: %s", s_cfg.pps_pin, esp_err_to_name(err));
        return;
    }

    // Install ISR service (ignore error if already installed)
    err = gpio_install_isr_service(0);
    if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
        ESP_LOGE(TAG, "Failed to install GPIO ISR service: %s", esp_err_to_name(err));
        return;
    }

    err = gpio_isr_handler_add(s_cfg.pps_pin, pps_gpio_isr_handler, NULL);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Failed to add PPS GPIO ISR handler: %s", esp_err_to_name(err));
        return;
    }

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

    ESP_LOGI(TAG, "Initialized (UART:%d, PPS:%d)", s_cfg.uart_port, s_cfg.pps_pin);
}

gps_timestamp_t gps_get_timestamp(void) {
    gps_timestamp_t ts = {0};
    int64_t now_boot = esp_timer_get_time(); // Boot time

    // Check Flags
    ts.synced = (now_boot - s_last_pps_us < 1100000);
    ts.valid = s_nmea_valid && (now_boot - s_nmea_epoch_us < 2000000);

    // Return WALL CLOCK time (Epoch), not boot time
    struct timeval tv;
    gettimeofday(&tv, NULL);
    ts.gps_us = (int64_t)tv.tv_sec * 1000000LL + (int64_t)tv.tv_usec;

    return ts;
}

int64_t gps_get_pps_age_ms(void) {
    if (s_last_pps_us == 0) return -1;
    return (esp_timer_get_time() - s_last_pps_us) / 1000;
}

void gps_get_last_nmea(char *buf, size_t buf_len) {
    if (buf && buf_len > 0) {
        strlcpy(buf, s_last_nmea_msg, buf_len);
    }
}