//
// Created by cfif on 27.09.22.
//

#include <stdlib.h>
#include <sys/select.h>
#include "CmsisRtosThreadUtils.h"
#include "Gnss.h"
#include "SystemDelayInterface.h"
#include "AsciiStringAssmeblingUtils.h"
#include "Nmea0183Parser.h"
#include "string.h"
#include "math.h"

#define LOGGER env->logger
#define LOG_SIGN "GONEC"

void Gnss_GetTime(tGnss *env, uint32_t *timestamp);

void Gnss_Init(tGnss *env, tSerialPortIO *gnssIo) {

    memset(env, 0, sizeof(tGnss));

    vGnssPro04NmeaInit(&env->nmea, gnssIo);
    vAsciiStringInit(env->rxBuffer.data, &env->rxBuffer.len, sizeof(env->rxBuffer.data));
    env->rmcAccess = osMutexNew(NULL);

    //Инициализируем поток
    InitThreadAtrStatic(&env->thread.attr, "Gnss", env->thread.controlBlock, env->thread.stack, osPriorityNormal);
    env->thread.id = 0;

    env->flagSetOneGnss = true;

}

void Gnss_InitDevice(tGnss *env) {
    vGnssPro04NmeaSetRate(&env->nmea, 1);
    vGnssPro04NmeaSetCoordType(&env->nmea, GNNS_PRO04_COORD_W84);
    vGnssPro04NmeaSetStartType(&env->nmea, GNNS_PRO04_STARTUP_HOT);
    vGnssPro04NmeaSetStandard(&env->nmea, GNNS_PRO04_NMEA_STANDARD_2);

    vGnssPro04NmeaSetMessageEnabled(&env->nmea, GNNS_PRO04_MESAGE_DTM, false);
    vGnssPro04NmeaSetMessageEnabled(&env->nmea, GNNS_PRO04_MESAGE_VTG, false);
    vGnssPro04NmeaSetMessageEnabled(&env->nmea, GNNS_PRO04_MESAGE_GSV, false);
    vGnssPro04NmeaSetMessageEnabled(&env->nmea, GNNS_PRO04_MESAGE_GSA, false);
    vGnssPro04NmeaSetMessageEnabled(&env->nmea, GNNS_PRO04_MESAGE_GGA, false);
    vGnssPro04NmeaSetMessageEnabled(&env->nmea, GNNS_PRO04_MESAGE_RMC, true);
}

static _Noreturn void Gnss_Thread(tGnss *env) {

    Gnss_InitDevice(env);

    // Очистка физического буфера UART, вычитываем данные
    SerialPortClearRxBuffer(env->nmea.io);

    tNmeaRmc tmpRmc;
    for (;;) {

        // Очистка входного буфера
        vAsciiStringClean(env->rxBuffer.data, &env->rxBuffer.len);

        // Чтение данных, до тех пор пока не встретится '\n'
        env->rxBuffer.len = SerialPortReceiveTo(
                env->nmea.io,
                (uint8_t *) env->rxBuffer.data,
                sizeof(env->rxBuffer.data),
                '\n',
                SystemWaitForever
        );

        // Поиск строки RMC
        if (bNmea0183IsRmcString(env->rxBuffer.data, env->rxBuffer.len)) {
            if (bNmea0183IsValidString(env->rxBuffer.data, env->rxBuffer.len)) {
                if (osMutexAcquire(env->rmcAccess, 2000) == osOK) {
                    if (bNmea0183ParseRMC(env->rxBuffer.data + 7, env->rxBuffer.len - 7, &tmpRmc)) {

                        if (tmpRmc.status == 'A') {
                            env->lastGoodRmc = tmpRmc;
                        } else if (env->currentRmc.status == 'A') {
                            env->lastGoodRmc = env->currentRmc;
                        } else if (env->lastGoodRmc.status != 'A' && !env->lastGoodRmc.location.nsIndicator) {
                            env->lastGoodRmc = env->currentRmc;
                        }

                        env->currentRmc = tmpRmc;
                        env->lastRmcUpdate = SystemGetMs();
/*
                    if (env->flagSetOneGnss) {

                        time_t set_timestamp = iNmea0183TimestampFromRmc(&env->currentRmc);

                        srand(set_timestamp);

                        if (set_timestamp > (time_t) 1644683158) {`
                            env->flagSetOneGnss = false;
                            set_timestamp += 3600 * env->GMTcorr_v;
                            env->rtcIo->set(&env->rtcIo, &set_timestamp);
                        }

                    }
                    */

                    }

                    osMutexRelease(env->rmcAccess);
                }
            }
        }
    }
}

void Gnss_StartThread(tGnss *env) {
    if (!env->thread.id) {
        env->thread.id = osThreadNew((osThreadFunc_t) (Gnss_Thread), (void *) (env), &env->thread.attr);
    } else {
        osThreadResume(env->thread.id);
    }
}

uint32_t cConvertFrDeg(double frDeg, uint8_t latLonFlag) {

    uint8_t direction = frDeg > 0 ? 1 : 0;

    int8_t fullDeg = (int8_t) (frDeg);
    double frMin = (frDeg - fullDeg) * 60.0;

    int8_t fullMin = (int8_t) frMin;

    int16_t frOfMin = (int16_t) ((frMin - fullMin) * 8192);

    uint8_t absFullDeg = (uint8_t) abs((int) fullDeg);
    uint8_t absFullMin = abs(fullMin);
    uint16_t absFrOfMin = abs(frOfMin);

    uint32_t result =
            (0b01 << 29) |
            ((latLonFlag & 0b1) << 28) |
            ((direction) << 27) |
            ((absFullDeg) << 19) |
            ((absFullMin & 0b111111) << 13) |
            ((absFrOfMin & 0b11111111111111)) << 0;

    return result;
}

// latitude and longitude are in degrees, minutes and fraction of minutes
char cConvertDecToMilliArcSecFromDegMin(int32_t deg, int32_t min, int32_t *mArcs, double *gradus) {
    double sec = 0.0;

    if (gradus != NULL) *gradus = deg + min / 60.0 + sec / 3600.0;

    if (mArcs != NULL) *mArcs = (int) (deg * 3600 + min * 60) * 1000 + (int) (sec * 1000);  // mArcseconds

    return 0;
}

// latitude and longitude are in degrees, minutes and fraction of minutes
char cConvertDecToMilliArcSec(double dec, int32_t *mArcs, double *gradus) {
    int deg = 0, min = 0;
    double sec = 0.0;
    double _dec = dec;

    deg = (int) (_dec / 100);
    min = (int) (_dec) - (deg * 100);

    sec = (double) (_dec - min - 100 * deg) * 60.0;

    if (gradus != NULL) *gradus = deg + min / 60.0 + sec / 3600.0;

    if (mArcs != NULL) *mArcs = (int) (deg * 3600 + min * 60) * 1000 + (int) (sec * 1000);  // mArcseconds

    return 0;
}


// Получение данных с GNSS о местоположении и времени
void Gnss_GetNavDataTracert(tGnss *env, tTracertNavData *location) {

    if (osMutexAcquire(env->rmcAccess, 2000) == osOK) {

        uint32_t timestamp = iNmea0183TimestampFromRmc(&env->currentRmc);

        uint32_t lastGoodTimestamp = iNmea0183TimestampFromRmc(&env->lastGoodRmc);

        tNmeaRmc *rmc = &env->currentRmc;

        if (env->currentRmc.status != 'A') {

//            LoggerInfoStatic(LOGGER, LOG_SIGN, "Координаты не достоверны (tracert)");

            rmc = &env->lastGoodRmc;
        }




//        if (env->currentRmc.status != 'A' &&
//            (!env->currentRmc.location.nsIndicator) &&
//            (env->currentRmc.location.nsIndicator) &&
//            ((timestamp - lastGoodTimestamp) < 10000)) {
//            rmc = &env->lastGoodRmc;
//        }

        cConvertDecToMilliArcSec(rmc->location.latitude, NULL, &location->gnss_latitude);
        if (rmc->location.nsIndicator == 'S')
            location->gnss_latitude *= -1;

        cConvertDecToMilliArcSec(rmc->location.longitude, NULL, &location->gnss_longitude);
        if (rmc->location.ewIndicator == 'W')
            location->gnss_longitude *= -1;

        location->valid = (rmc->status == 'A') ? true : false;

        location->curse = lround(rmc->headingAngle);
        location->speed = lround(rmc->knotVelocity * 1.852);


        osMutexRelease(env->rmcAccess);
    }

/*
    if (osMutexAcquire(env->rmcAccess, 2000) == osOK) {

        tModemNavData loc;
        Gnss_GetNavDataNmeaRmc(env, &loc);

        location->gnss_latitude = loc.gnss_latitude;
        location->gnss_longitude = loc.gnss_longitude;

        location->curse = lround(env->currentRmc.headingAngle);
        location->speed = lround(env->currentRmc.kmhVelocity);

        location->valid = loc.valid;

        osMutexRelease(env->rmcAccess);
    }
    */
}


// Получение данных с GNSS о местоположении
void Gnss_GetNavDataNmeaRmc(tGnss *env, tModemNavData *location) {
    if (osMutexAcquire(env->rmcAccess, 2000) == osOK) {

        uint32_t timestamp = iNmea0183TimestampFromRmc(&env->currentRmc);

        uint32_t lastGoodTimestamp = iNmea0183TimestampFromRmc(&env->lastGoodRmc);

        tNmeaRmc *rmc = &env->currentRmc;

        if (env->currentRmc.status != 'A') {
//            LoggerInfoStatic(LOGGER, LOG_SIGN, "Координаты не достоверны (modem)");

            rmc = &env->lastGoodRmc;
        }

//        if (env->currentRmc.status != 'A' &&
//            (!env->currentRmc.location.nsIndicator) &&
//            (env->currentRmc.location.nsIndicator) &&
//            ((timestamp - lastGoodTimestamp) < 10000)) {
//            rmc = &env->lastGoodRmc;
//        }

        cConvertDecToMilliArcSec(rmc->location.latitude, NULL, &location->gnss_latitude);
        if (rmc->location.nsIndicator == 'S')
            location->gnss_latitude *= -1;

        cConvertDecToMilliArcSec(rmc->location.longitude, NULL, &location->gnss_longitude);
        if (rmc->location.ewIndicator == 'W')
            location->gnss_longitude *= -1;

        location->valid = (rmc->status == 'A') ? true : false;

        osMutexRelease(env->rmcAccess);
    }
}


// Получение данных с GNSS о местоположении
void Gnss_GetNavDataMsdNmeaRmc(tGnss *env, tNavDataMsd *location) {
    if (osMutexAcquire(env->rmcAccess, 2000) == osOK) {

        uint32_t timestamp = iNmea0183TimestampFromRmc(&env->currentRmc);

        uint32_t lastGoodTimestamp = iNmea0183TimestampFromRmc(&env->lastGoodRmc);

        tNmeaRmc *rmc = &env->currentRmc;

        if (env->currentRmc.status != 'A') {
            rmc = &env->lastGoodRmc;
        }

        cConvertDecToMilliArcSec(rmc->location.latitude, &location->latitude, NULL);
        cConvertDecToMilliArcSec(rmc->location.longitude, &location->longitude, NULL);

        location->direction = (uint16_t) rmc->headingAngle;

        location->valid = (rmc->status == 'A') ? true : false;

        osMutexRelease(env->rmcAccess);

    }
}

// Получение времени
void Gnss_GetTime(tGnss *env, uint32_t *timestamp) {
    if (osMutexAcquire(env->rmcAccess, 2000) == osOK) {

        *timestamp = iNmea0183TimestampFromRmc(&env->currentRmc);

        osMutexRelease(env->rmcAccess);
    }
}