#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "pico/stdlib.h"

typedef struct {
    int   valid;       // 1 = good fix parsed, 0 = rejected
    float lat;         // decimal degrees, +N / -S
    float lon;         // decimal degrees, +E / -W
    int   satellites;
} gps_fix_t;

// XOR checksum of chars between '$' and '*'
static int checksum_ok(const char *s) {
    if (s[0] != '$') return 0;
    unsigned char cs = 0;
    int i = 1;
    while (s[i] && s[i] != '*') { cs ^= s[i]; i++; }
    if (s[i] != '*') return 0;                    // no checksum present
    unsigned int given;
    if (sscanf(&s[i + 1], "%2x", &given) != 1) return 0;
    if (cs != (unsigned char)given)
        printf("  [checksum: computed %02X, sentence says %02X]\n", cs, given);
    return cs == (unsigned char)given;
}

// "4807.038" + 'N' -> 48.1173 decimal degrees
static float nmea_to_degrees(const char *field, char hemisphere) {
    float raw = atof(field);                      // 4807.038
    int   deg = (int)(raw / 100);                 // 48
    float min = raw - deg * 100;                  // 7.038
    float dec = deg + min / 60.0f;
    if (hemisphere == 'S' || hemisphere == 'W') dec = -dec;
    return dec;
}

// Parse a $GPGGA sentence into a fix. Returns valid=0 on any problem.
gps_fix_t parse_gga(const char *sentence) {
    gps_fix_t fix = {0};

    if (!checksum_ok(sentence)) return fix;
    if (strncmp(sentence, "$GPGGA", 6) != 0) return fix;

    // split on commas into fields (copy first: strtok modifies the string)
    char buf[128];
    strncpy(buf, sentence, sizeof(buf) - 1);
    buf[sizeof(buf) - 1] = '\0';

    char *fields[16] = {0};
    int   n = 0;
    char *tok = strtok(buf, ",");
    while (tok && n < 16) { fields[n++] = tok; tok = strtok(NULL, ","); }

    // fields: [0]$GPGGA [1]time [2]lat [3]N/S [4]lon [5]E/W [6]quality [7]sats
    if (n < 8) return fix;
    if (atoi(fields[6]) == 0) return fix;         // no fix yet - reject

    fix.lat        = nmea_to_degrees(fields[2], fields[3][0]);
    fix.lon        = nmea_to_degrees(fields[4], fields[5][0]);
    fix.satellites = atoi(fields[7]);
    fix.valid      = 1;
    return fix;
}

// ---- test vectors: known sentences, known expected results ----
int main() {
    stdio_init_all();
    sleep_ms(3000);

    const char *tests[] = {
        // good fix, Munich: expect lat=48.1173, lon=11.5167, sats=8
        "$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47",
        // good fix, southern hemisphere: expect lat=-37.8608, lon=145.1227
        "$GPGGA,081836,3751.65,S,14507.36,E,1,05,1.5,280.2,M,-34.0,M,,*79",
        // quality=0 (no fix yet): expect REJECTED on quality, not checksum
        "$GPGGA,002153,0000.000,N,00000.000,E,0,00,99.9,0.0,M,0.0,M,,*4F",
        // corrupted byte (checksum mismatch): expect REJECTED with debug line
        "$GPGGA,123519,4907.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47",
    };
    printf("=== NMEA parser tests ===\n");
    for (int i = 0; i < 4; i++) {
        gps_fix_t f = parse_gga(tests[i]);
        if (f.valid)
            printf("Test %d: VALID  lat=%.4f lon=%.4f sats=%d\n",
                   i, f.lat, f.lon, f.satellites);
        else
            printf("Test %d: REJECTED\n", i);
    }
    while (true) sleep_ms(1000);
}