#include "gps.h"

TinyGPSPlus gps;

void Init_GPS()
{
    uint32_t baudrate;
    switch (LubeConfig.GPSBaudRate)
    {
    case BAUD_9600:
        baudrate = 9600;
        break;
    case BAUD_115200:
        baudrate = 115200;
        break;
    default:
        baudrate = 4800;
        break;
    }

    Serial.printf(PSTR("Init GPS with Baud %d\n"), baudrate);
    Serial.begin(baudrate);
}

uint32_t Process_GPS_WheelSpeed()
{
    static uint32_t lastRecTimestamp;
    static uint32_t lastValidSpeedTimestamp;
    uint16_t RearWheelSpeed_kmh;

    while (Serial.available() > 0)
    {
        uint8_t incoming = Serial.read();
        if (gps.encode(incoming))
        {
            RearWheelSpeed_kmh = gps.speed.isValid() ? gps.speed.kmph() : 0;

            if (RearWheelSpeed_kmh > 0)
            {
                uint32_t RWP_millimeter_per_second = ((uint32_t)RearWheelSpeed_kmh * 1000000) / 3600;

                uint32_t timesincelast = millis() - lastValidSpeedTimestamp;
                lastValidSpeedTimestamp = millis();

                uint32_t milimeters_to_add = (RWP_millimeter_per_second * timesincelast) / 1000;

                return milimeters_to_add;
            }
            lastRecTimestamp = millis();
        }
    }

    if (millis() > lastRecTimestamp + 10000)
    {
        if (globals.systemStatus != sysStat_Shutdown)
            globals.systemStatus = sysStat_Error;
        MaintainDTC(DTC_NO_GPS_SERIAL, true);
    }
    else
    {
        if (globals.systemStatus != sysStat_Shutdown)
            globals.systemStatus = globals.resumeStatus;
        MaintainDTC(DTC_NO_GPS_SERIAL, false);
    }

    return 0;
}