#include "driver/uart.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "string.h"
#include "gps.h"

#define TAG "GPS"
#define BUF_SIZE 1024
#define GPS_LINE_MAX_LEN 128

static QueueHandle_t uart_queue;

// Initializes UART for GPS
void gps_init()
{
    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_CTS_RTS,
        .rx_flow_ctrl_thresh = 122,
    };
    ESP_ERROR_CHECK(uart_param_config(GPS_UART_NUM, &uart_config));
    ESP_ERROR_CHECK(uart_set_pin(GPS_UART_NUM, GPS_TXD, GPS_RXD, GPS_RTS, GPS_CTS));

    const int uart_buffer_size = (1024 * 2);
    ESP_ERROR_CHECK(uart_driver_install(GPS_UART_NUM, uart_buffer_size, uart_buffer_size, 10, &uart_queue, 0));
}


void gps_task(void *arg)
{
    uint8_t byte;
    char line[GPS_LINE_MAX_LEN];
    size_t line_pos = 0;

    gps_init();

    while (1) {
        int len = uart_read_bytes(GPS_UART_NUM, &byte, 1, pdMS_TO_TICKS(1000));
        if (len > 0) {
            if (byte == '\n') {
                line[line_pos] = '\0';

                if (line[0] == '$') {
                    ESP_LOGV(TAG, "Received NMEA: %s", line);

                    if (strstr(line, "$GPGGA") == line) {
                        parse_gpgga(line);
                    } else if (strstr(line, "$GPRMC") == line) {
                        parse_gprmc(line);
                    }
                }

                line_pos = 0;
            } else if (byte != '\r' && line_pos < (GPS_LINE_MAX_LEN - 1)) {
                line[line_pos++] = byte;
            } else if (line_pos >= GPS_LINE_MAX_LEN - 1) {
                ESP_LOGW(TAG, "Line too long, discarded");
                line_pos = 0;
            }
        }
    }
}

void parse_gpgga(const char *nmea)
{
    char *fields[15];
    char temp[GPS_LINE_MAX_LEN];
    strncpy(temp, nmea, GPS_LINE_MAX_LEN);
    temp[GPS_LINE_MAX_LEN - 1] = '\0';

    int i = 0;
    char *token = strtok(temp, ",");
    while (token != NULL && i < 15) {
        fields[i++] = token;
        token = strtok(NULL, ",");
    }

    if (i < 10) return; // Not enough fields

    // Time (hhmmss.sss)
    const char *utc_time = fields[1];

    // Latitude (ddmm.mmmm)
    const char *lat = fields[2];
    const char *lat_dir = fields[3];

    // Longitude (dddmm.mmmm)
    const char *lon = fields[4];
    const char *lon_dir = fields[5];

    // Fix quality (0 = invalid, 1 = GPS fix, 2 = DGPS fix)
    const char *fix_quality = fields[6];

    // Number of satellites
    const char *num_satellites = fields[7];

    // Altitude
    const char *altitude = fields[9];

    ESP_LOGI(TAG, "[GPGGA] Time: %s, Lat: %s %s, Lon: %s %s, Fix: %s, Sats: %s, Alt: %sm",
             utc_time, lat, lat_dir, lon, lon_dir, fix_quality, num_satellites, altitude);
}

void parse_gprmc(const char *nmea)
{
    char *fields[13];
    char temp[GPS_LINE_MAX_LEN];
    strncpy(temp, nmea, GPS_LINE_MAX_LEN);
    temp[GPS_LINE_MAX_LEN - 1] = '\0';

    int i = 0;
    char *token = strtok(temp, ",");
    while (token != NULL && i < 13) {
        fields[i++] = token;
        token = strtok(NULL, ",");
    }

    if (i < 12) return;

    const char *utc_time = fields[1];
    const char *status = fields[2]; // A = active, V = void
    const char *lat = fields[3];
    const char *lat_dir = fields[4];
    const char *lon = fields[5];
    const char *lon_dir = fields[6];
    const char *speed_knots = fields[7];
    const char *date = fields[9];

    ESP_LOGI(TAG, "[GPRMC] Date: %s, Time: %s, Lat: %s %s, Lon: %s %s, Speed: %s knots, Status: %s",
             date, utc_time, lat, lat_dir, lon, lon_dir, speed_knots, status);
}