#include "gps_handler.h"
#include <string.h>
#include <stdlib.h>

static char buffer[BUFFER_SIZE];
static int gps_index = 0;

static float convert_to_decimal(char *coord, char dir) {
    float raw = atof(coord);
    int degrees = (int)(raw / 100);
    float minutes = raw - (degrees * 100);
    float decimal = degrees + (minutes / 60.0f);
    if (dir == 'S' || dir == 'W') decimal *= -1.0f;
    return decimal;
}

void gps_init(void) {
    uart_init(UART_ID, BAUD_RATE);
    gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
    gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);

    uart_set_hw_flow(UART_ID, false, false);
    uart_set_format(UART_ID, 8, 1, UART_PARITY_NONE);
}

void gps_update(GPS_Data_t *data) {
    while (uart_is_readable(UART_ID)) {
        char c = uart_getc(UART_ID);
        if (c == '\n') {
            buffer[gps_index] = '\0';
            if (strstr(buffer, "$GPRMC")) {
                char *token = strtok(buffer, ",");
                int field = 0;
                char lat[16] = {0}, lon[16] = {0}, lat_dir = 0, lon_dir = 0;
                while (token != NULL) {
                    field++;
                    if (field == 4) strcpy(lat, token);
                    if (field == 5) lat_dir = token[0];
                    if (field == 6) strcpy(lon, token);
                    if (field == 7) lon_dir = token[0];
                    token = strtok(NULL, ",");
                }
                if (lat[0] != 0 && lon[0] != 0) {
                    data->latitude = convert_to_decimal(lat, lat_dir);
                    data->longitude = convert_to_decimal(lon, lon_dir);
                    data->has_fix = true;
                }
            }
            gps_index = 0;
        } else if (gps_index < BUFFER_SIZE - 1) {
            buffer[gps_index++] = c;
        }
    }
}