#ifdef FEATURE_ENABLE_GPS
#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;
    }

    Debug_pushMessage(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();
        }
    }

    MaintainDTC(DTC_NO_GPS_SERIAL,DTC_CRITICAL, (millis() > lastRecTimestamp + 10000));

    return 0;
}

#endif