/*
* Modem_GPS.c
*
* Implementacja funkcji GNSS - patrz Modem_GPS.h.
*/

/* Includes ------------------------------------------------------------------*/
#include "Modem_GPS.h"
#include <stdlib.h> // atoi, atof
#include <string.h>

/* Private user code -----------------------------------------------------------*/

/*
 * Aktywuje silnik GNSS w module (AT+CGNSPWR=1). Bez tego AT+CGNSINF
 * w Modem_GPS_Read bedzie zwracac puste dane (run=0).
 */
uint8_t Modem_GPS_Activate(sim7000e_t *sim)
{
    return Modem_Send_Command(sim, "AT+CGNSPWR=1\r\n", "OK", 3000);
}

// Pomocniczy wskaźnik do N-tego pola za przecinkami
static const char* get_field_ptr(const char* str, uint8_t target_field) {
    uint8_t current_field = 0;
    if (target_field == 0) return str;

    while (*str != '\0') {
        if (*str == ',') {
            current_field++;
            if (current_field == target_field) {
                return str + 1;
            }
        }
        str++;
    }
    return NULL;
}

/*
 * Odpytuje modul o biezaca pozycje komenda AT+CGNSINF i parsuje
 * odpowiedz, wypelniajac pola sim->lat, sim->lon, sim->fix.
 *
 * Format odpowiedzi:
 * +CGNSINF: <run>,<fix>,<data/czas UTC>,<lat>,<lon>,<wys>,...
 *
 * W kazdym przypadku bledu (brak odpowiedzi, brak fixa, blad parsowania)
 * pola struktury sa jawnie nadpisywane wartosciami sygnalizujacymi
 * brak danych (fix=0, lat=9999.9f, lon=9999.9f) - dzieki temu program
 * glowny nigdy nie odczyta "starych", nieaktualnych wspolrzednych z
 * poprzedniego, udanego odczytu.
 *
 * return -> 1 jesli uzyskano wazny fix, 0 w przeciwnym razie
 */
uint8_t Modem_GPS_Read(sim7000e_t *sim) {
    // 1. Zapytanie o status NMEA
    if (!Modem_Send_Command(sim, "AT+CGNSINF\r\n", "+CGNSINF:", 3000)) {
        Debug_Print("Brak odpowiedzi na AT+CGNSINF\r\n");
        sim->fix = 0;
        sim->lat = 9999.9f;
        sim->lon = 9999.9f;
        return 0;
    }

    // 2. Wyszukanie ciągu początkowego
    const char *p = strstr(sim->bufin, "+CGNSINF:");
    if (p == NULL) {
        sim->fix = 0;
        sim->lat = 9999.9f;
        sim->lon = 9999.9f;
        return 0;
    }

    p += strlen("+CGNSINF:");
    while (*p == ' ') p++;

    // 3. Sprawdzenie statusu FIX (Pole 1)
    const char *fix_ptr = get_field_ptr(p, 1);
    if (fix_ptr == NULL || *fix_ptr != '1') {
        sim->fix = 0;
        sim->lat = 9999.9f;
        sim->lon = 9999.9f;
        return 0;
    }

    // 4. Pobranie Latitude (Pole 3) i Longitude (Pole 4)
    const char *lat_ptr = get_field_ptr(p, 3);
    const char *lon_ptr = get_field_ptr(p, 4);

    if (lat_ptr != NULL && lon_ptr != NULL && *lat_ptr != ',' && *lon_ptr != ',') {
        sim->lat = strtod(lat_ptr, NULL); // Konwersja do double
        sim->lon = strtod(lon_ptr, NULL);
        sim->fix = 1;
        return 1; // FIX JEST I SPARSYWANY!
    }

    sim->fix = 0;
    sim->lat = 9999.9f;
    sim->lon = 9999.9f;
    return 0;
}

// Odczyt szerokosci geograficznej z ostatniego udanego pomiaru
double Modem_GPS_Get_Lat(sim7000e_t *sim)
{
    return sim->lat;
}

// Odczyt dlugosci geograficznej z ostatniego udanego pomiaru
double Modem_GPS_Get_Lon(sim7000e_t *sim)
{
    return sim->lon;
}
