//
// Created by cfif on 15.10.2024.
//
#include <stdlib.h>
#include "CanMain.h"
#include "SystemDelayInterface.h"
#include "ext_telematica.h"
#include "string.h"
#include "Rtc.h"
#include "EgtsTimestamp.h"
#include "math.h"

#define LOG_SIGN "UDS_MUTE"
#define LOGGER &env->slog->logger

extern eDeviceTestingCode deviceTestingCodeCan;

void CanMainMute_Init(
        tCanMainMute *env,
        tSerialPortFrameIO *CanIO,
        SystemMutexCmsis txAccessQueue,
        tGsmWithGnss *gsm,
        tLoggerToSerialPort *slog
) {
    env->CanIO = CanIO;
    env->txAccessQueue = txAccessQueue;
    env->gsm = gsm;
    env->slog = slog;

    memset(env->GNSS_NMEA1, 0, sizeof(env->GNSS_NMEA1));
    memset(env->GNSS_NMEA2, 0, sizeof(env->GNSS_NMEA2));
    memset(env->GNSS_NMEA3, 0, sizeof(env->GNSS_NMEA3));
    memset(env->GNSS_NMEA4, 0, sizeof(env->GNSS_NMEA4));
    memset(env->GNSS_NMEA5, 0, sizeof(env->GNSS_NMEA5));
    memset(env->GNSS_NMEA6, 0, sizeof(env->GNSS_NMEA6));

    InitThreadBlock(env->T_can_MainMute, "CanMainMute", osPriorityNormal);
    InitThreadBlock(env->T_can_MainNav, "CanMainNav", osPriorityNormal);
}

static double nmeaLocationToDeg(double dec) {
    double _dec = dec;
    int deg = (int) (_dec / 100);
    int min = (int) (_dec) - (deg * 100);

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

    return deg + min / 60.0 + sec / 3600.0;
}

void CanMainTaskNav(tCanMainMute *env) {
    uint16_t sent = 0;
    uint8_t countCycle = 0;
    uint8_t lcCounter = 0;

    for (;;) {

        if (!EXT_ENV_TELE.store.device->factoryMode) {
            if ((EXT_ENV_TELE.DataToCan != NULL) && (EXT_ENV_TELE.DataToCan->nav_data_ToCan == true)) {

                deviceTestingCodeCan = DEVICE_TESTING_CODE_NOT_ACTIVE;

                //начало-----------------------------------НАВИГАЦИЯ----------------------------------------------------
                tGNSS_NMEA1 *GNSS_NMEA1 = (tGNSS_NMEA1 *) env->GNSS_NMEA1;
                tGNSS_NMEA2 *GNSS_NMEA2 = (tGNSS_NMEA2 *) env->GNSS_NMEA2;
                tGNSS_NMEA3 *GNSS_NMEA3 = (tGNSS_NMEA3 *) env->GNSS_NMEA3;

                time_t timestamp = 0;
                RtcGet(env->gsm->Rtc, &timestamp);
                tNmeaRmc nmeaRmc;
                Gnss_GetFullNavData(env->gsm, &nmeaRmc);
                int32_t alt = GnssGgaGetAlt(env->gsm);
                uint8_t countSats = GnssGgaGetSat(env->gsm);

                GNSS_NMEA1->NTM = Egts_EpochTimestampToEgtsTime(timestamp);
                double lat = (fabs(nmeaLocationToDeg(nmeaRmc.location.latitude)) / 90) * 0xFFFFFFFF;
                GNSS_NMEA1->LAT = (uint32_t) lat;

                double lon = (fabs(nmeaLocationToDeg(nmeaRmc.location.longitude)) / 90) * 0xFFFFFFFF;
                GNSS_NMEA2->LONG = (uint32_t) lon;
                GNSS_NMEA2->FLG.VLD = (nmeaRmc.status == 'A') ? 1 : 0;
                GNSS_NMEA2->FLG.LAHS = (nmeaRmc.location.nsIndicator == 'S') ? 1 : 0;
                GNSS_NMEA2->FLG.LOHS = (nmeaRmc.location.ewIndicator == 'W') ? 1 : 0;
                GNSS_NMEA2->FLG.ALTH = 1;
                GNSS_NMEA2->FLG.SATH = 1;

                uint16_t speed = (uint16_t) (nmeaRmc.knotVelocity * 1.82 * 10);
                GNSS_NMEA2->SPD.SPD = speed & 0b0011111111111111;

                if (alt >= 0) {
                    GNSS_NMEA2->SPD.ALTS = 0;
                } else {
                    GNSS_NMEA2->SPD.ALTS = 1;
                }

                uint16_t dir = (uint16_t) nmeaRmc.headingAngle;
                GNSS_NMEA2->SPD.DIRH = dir >> 8;

                GNSS_NMEA2->DIR = dir;

                GNSS_NMEA3->ALT = abs(alt);
                GNSS_NMEA3->SAT = countSats;


                if (osMutexAcquire(env->txAccessQueue, 3000) == osOK) {

                    CanSerialPortFrameSetId(env->CanIO->env, 0x5A4);
                    sent = env->CanIO->transmit(env->CanIO->env, env->GNSS_NMEA1, sizeof(env->GNSS_NMEA1),
                                                WAIT_FRAME_WRITE);

                    CanSerialPortFrameSetId(env->CanIO->env, 0x5A6);
                    sent = env->CanIO->transmit(env->CanIO->env, env->GNSS_NMEA2, sizeof(env->GNSS_NMEA2),
                                                WAIT_FRAME_WRITE);

                    CanSerialPortFrameSetId(env->CanIO->env, 0x5A8);
                    sent = env->CanIO->transmit(env->CanIO->env, env->GNSS_NMEA3, sizeof(env->GNSS_NMEA3),
                                                WAIT_FRAME_WRITE);

                    CanSerialPortFrameSetId(env->CanIO->env, 0x5AA);
                    sent = env->CanIO->transmit(env->CanIO->env, env->GNSS_NMEA4, sizeof(env->GNSS_NMEA4),
                                                WAIT_FRAME_WRITE);

                    CanSerialPortFrameSetId(env->CanIO->env, 0x5AC);
                    sent = env->CanIO->transmit(env->CanIO->env, env->GNSS_NMEA5, sizeof(env->GNSS_NMEA5),
                                                WAIT_FRAME_WRITE);

                    CanSerialPortFrameSetId(env->CanIO->env, 0x5AE);
                    sent = env->CanIO->transmit(env->CanIO->env, env->GNSS_NMEA6, sizeof(env->GNSS_NMEA6),
                                                WAIT_FRAME_WRITE);

                    osMutexRelease(env->txAccessQueue);
                } else {
                    LoggerInfoStatic(LOGGER, LOG_SIGN, "Ошибка доступа CanMainTaskNav (1)")
                }

                //конец------------------------------------НАВИГАЦИЯ----------------------------------------------------

                SystemDelayMs(50);
                ++countCycle;
            }
        }

    }

}

void CanMainTaskMute(tCanMainMute *env) {
    uint16_t sent = 0;
    uint8_t countCycle = 0;
    uint8_t lcCounter = 0;
    for (;;) {
        if (!EXT_ENV_TELE.store.device->factoryMode) {
//            if( (EXT_ENV_TELE.DataToCan != NULL) && (EXT_ENV_TELE.DataToCan->gnss_general_ToCan == true) ) {
            //начало-----------------------------------ТИШИНА-------------------------------------------------------
            if (osMutexAcquire(env->txAccessQueue, 3000) == osOK) {

                CanSerialPortFrameSetId(env->CanIO->env, 0x640);

                if (lcCounter >= 15) {
                    lcCounter = 0;
                } else {
                    ++lcCounter;
                }
                EXT_ENV_TELE.GNSS_General->GNSS_LC = lcCounter;

                tGNSS_General GNSS_General;
                memcpy(&GNSS_General, EXT_ENV_TELE.GNSS_General, sizeof(tGNSS_General));

                if (EXT_ENV_TELE.GNSS_General->GNSS_Call_Auto == CAN_GNSS_ROLLOVER_DETECT) {
                    GNSS_General.GNSS_Call_By_button = CAN_GNSS_CALL;
                }

//                if (EXT_ENV_TELE.GNSS_General->GNSS_Call_Test == CAN_GNSS_CALL_TEST_ACTIVE) {
//                    GNSS_General.GNSS_Call_By_button = CAN_GNSS_CALL;
//                }

//                GNSS_General.GNSS_Call_Test = 0;
                GNSS_General.GNSS_Call_Auto = 0;
                GNSS_General.Reserved1 = 0;
                GNSS_General.Reserved2 = 0;

                sent = env->CanIO->transmit(env->CanIO->env, (uint8_t *) &GNSS_General, 2,
                                            WAIT_FRAME_WRITE);

                osMutexRelease(env->txAccessQueue);
            } else {
                LoggerInfoStatic(LOGGER, LOG_SIGN, "Ошибка доступа CanMainTaskMute (1)")
            }
            //конец-----------------------------------ТИШИНА-------------------------------------------------------

            SystemDelayMs(100);
            ++countCycle;
//            }
        } else {
            env->isPassedTestCan = false;

            if (osMutexAcquire(env->txAccessQueue, 1000) == osOK) {

                CanSerialPortFrameSetId(env->CanIO->env, 0x88);
                sent = env->CanIO->transmit(env->CanIO->env, (uint8_t *) canTestData, 8,
                                            WAIT_FRAME_WRITE);

                osMutexRelease(env->txAccessQueue);
            } else {
                LoggerInfoStatic(LOGGER, LOG_SIGN, "Ошибка доступа CanMainTaskMute (2)")
            }

            SystemDelayMs(100);

            if (env->isPassedTestCan) {
                deviceTestingCodeCan = DEVICE_TESTING_CODE_PASSED;
            } else {
                deviceTestingCodeCan = DEVICE_TESTING_CODE_ERROR;
            }

        }
    }
}


void CanMainMute_Start(tCanMainMute *env) {
    ThreadBlock_Start(env->T_can_MainMute, env, CanMainTaskMute);
    ThreadBlock_Start(env->T_can_MainNav, env, CanMainTaskNav);
}