#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

gps_binary_struct_t gpsDataOut;
static QueueHandle_t uart_queue;

predicted_binary_position_struct_t predictedPosition;

// Initializes UART for GPS
void gps_init()
{
    memset(&gpsDataOut, 0, sizeof(gpsDataOut));
    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_LOGI(TAG, "Received NMEA: %s", line);

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

                }

                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];

    printf("[GPGGA] Time: %s, Lat: %s %s, Lon: %s %s, Fix: %s, Sats: %s, Alt: %sm\n",
           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];

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


// Function to convert time string (hhmmss.sss) to seconds since start of day
static uint32_t time_to_seconds_struct(const char *time_str) {
    if (time_str == NULL || strlen(time_str) < 6) return 0;
    uint32_t hours = (time_str[0] - '0') * 10 + (time_str[1] - '0');
    uint32_t minutes = (time_str[2] - '0') * 10 + (time_str[3] - '0');
    uint32_t seconds = (time_str[4] - '0') * 10 + (time_str[5] - '0');
    return hours * 3600 + minutes * 60 + seconds;
}

// Function to convert DMS (degrees minutes.decimalminutes) to centi-degrees
static int32_t dms_to_centi_degrees_struct(const char *dms_str, const char *direction) {
    if (dms_str == NULL || direction == NULL || strlen(dms_str) < 7) return 0;
    char data_str[20] = {0};
    char *dataStr = data_str;
    for (int i = 0; i < 20; i++) {
        if (dms_str[i] == 0) {
            break;
        }
        if (dms_str[i] == '.') {
            continue;
        }
        *(dataStr++) = dms_str[i];
    }
    int32_t degrees = atoi(data_str);
    if (direction[0] == 'S' || direction[0] == 'W') {
        degrees *= -1;
    }
    return degrees;
}

// Function to convert altitude string to centi-meters
static int16_t altitude_to_centi_meters_struct(const char *alt_str) {
    if (alt_str == NULL) return 0;
    return (int16_t) (atof(alt_str) * 100);
}

// Function to convert speed from knots to centi-knots
static uint16_t speed_to_centi_knots_struct(const char *speed_str) {
    if (speed_str == NULL) return 0;
    return (uint16_t) (atof(speed_str) * 100);
}

// Function to convert date string (ddmmyy) to yymmdd integer
static uint16_t date_to_yyddmm_struct(const char *date_str) {
    if (date_str == NULL || strlen(date_str) != 6) return 0;
    uint16_t day = (date_str[0] - '0') * 10 + (date_str[1] - '0');
    uint16_t month = (date_str[2] - '0') * 10 + (date_str[3] - '0');
    uint16_t year_short = (date_str[4] - '0') * 10 + (date_str[5] - '0');
    // Assuming year is in the 21st century for simplicity
    return (2000 + year_short) * 10000 + month * 100 + day;
}

// Function to parse GPGGA NMEA string and return the struct
void parse_gpgga_to_struct(const char *nmea, gps_binary_struct_t *data) {
    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) {
        data->time_seconds = time_to_seconds_struct(fields[1]);
        data->latitude_centi_degrees = dms_to_centi_degrees_struct(fields[2], fields[3]);
        data->longitude_centi_degrees = dms_to_centi_degrees_struct(fields[4], fields[5]);
        data->fix_quality = atoi(fields[6]);
        data->num_satellites = atoi(fields[7]);
        data->altitude_centi_meters = altitude_to_centi_meters_struct(fields[9]);
    } else {
        printf("GPGGA: Not enough fields to parse struct");
    }
}

// Function to parse GPRMC NMEA string and return the struct
void parse_gprmc_to_struct(const char *nmea, gps_binary_struct_t *data) {
    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) {
        data->time_seconds = time_to_seconds_struct(fields[1]);
        data->latitude_centi_degrees = dms_to_centi_degrees_struct(fields[3], fields[4]);
        data->longitude_centi_degrees = dms_to_centi_degrees_struct(fields[5], fields[6]);
        data->date_yyddmm = date_to_yyddmm_struct(fields[9]);
        data->speed_centi_knots = speed_to_centi_knots_struct(fields[7]);
        // Fix quality and num_satellites are typically in GPGGA, so they might be 0 here.
    } else {
        printf("GPRMC: Not enough fields to parse struct");
    }
}