//
// Created by zemon on 21.08.24.
//

#include <math.h>
#include "MainModesArbiter_Private.h"
#include "DeviceTesting_TestingMethods.h"
#include "DeviceTesting.h"

#define LOG_SIGN "Главн состояние в UDS."
#define LOGGER &env->slog.logger
eDeviceTestingCode resBatState;
crm_clocks_freq_type crm_clocks_freq_structure = {0};

void Mma_InfoSystemToUDSGetModemState(tModemDataColl *ModemDataCollector, const tDeviceModes *deviceModes, tSystemStateID_0006 *state) {
    switch (*deviceModes) {
        case DEVICE_MODE_UVEOS_MSD_TRANSMIT:
            state->GSM_State = MSD_SENDS;
            break;
        case DEVICE_MODE_UVEOS_IN_CALL:
            state->GSM_State = INCOMING_CALL;
            break;
        case DEVICE_MODE_UVEOS_SYSTEM_STARTUP:
            state->GSM_State = INITIALIZING;
            break;
        case DEVICE_MODE_TESTING:
            state->GSM_State = IDLE;
            break;
        case DEVICE_MODE_UVEOS_ERA_GNSS_READY:
            state->GSM_State = IDLE;
            break;
        case DEVICE_MODE_UVEOS_ERA_WAIT_GNSS:
            state->GSM_State = IDLE;
            break;
        default:
            state->GSM_State = INITIALIZING;
            break;
    }
}

void Mma_InfoSystemToUDSGetMotionState(tPowerManagement *env, tSystemStateID_0006 *state) {
    if (GpioPinGet(&env->powerSusystem->main.ignition)) {
        state->Motion_State = 1;
    } else {
        state->Motion_State = 0;
    }
}

void Mma_InfoSystemToUDSGetBattaryState(tPowerManagement *env, tSystemStateID_0006 *state) {
    if (osMutexAcquire(env->mux_accessPwrMan, 100) == osOK) {
        resBatState = DeviceTesting_AdcBatteryChargeTest(&env->powerSusystem->battery);
        osMutexRelease(env->mux_accessPwrMan);
        switch (resBatState) {
            case DEVICE_TESTING_CODE_ERROR:
                state->Battery_State = NO_CHARGE;
                break;
            case DEVICE_TESTING_CODE_BATTERY_PRE_CHARGE:
                state->Battery_State = TRICLE_CHHARGE;
                break;
            case DEVICE_TESTING_CODE_BATTERY_FAST_CHARGE:
                state->Battery_State = FAST_CHARGE;
                break;
            case DEVICE_TESTING_CODE_BATTERY_CHARGE_DONE:
                state->Battery_State = STABILISATION;
                break;
            default:
                state->Battery_State = UNDEFINDET;
                break;
        }
    }
}

void Mma_InfoSystemToUDSGetInputLinesState(tGpios *gpios, tInputStatesRecord_0001 *env) {
    if (GpioPinGet(&gpios->bip.buttons.emergency)) {
        env->Input_SOS = STATE_RECORD001_ON;
    } else {
        env->Input_SOS = STATE_RECORD001_OFF;
    }

    if (GpioPinGet(&gpios->bip.buttons.additional)) {
        env->Input_Service = STATE_RECORD001_ON;
    } else {
        env->Input_Service = STATE_RECORD001_OFF;
    }

    if (GpioPinGet(&gpios->powerSusystem.main.ignition)) {
        env->Input_KL15 = STATE_RECORD001_ON;
    } else {
        env->Input_KL15 = STATE_RECORD001_OFF;
    }
}

void Mma_InfoSystemToUDSpowerStatus(tGpios *gpios, tPowerManagement *power, tOutputStatesRecord_0002 *env) {
    if (GpioPinGet(&power->powerSusystem->sim7600.powerStatus)) {
        env->OutputMODEM_ON = STATE_RECORD002_ON;
    } else {
        env->OutputMODEM_ON = STATE_RECORD002_OFF;
    }
}

void Mma_InfoSystemToUDSGetModemResetState(tGpios *gpios, tPowerManagement *power, tOutputStatesRecord_0002 *env) {
    if (GpioPinGet(&power->powerSusystem->sim7600.reset)) {
        env->Output_Modem_Reset = STATE_RECORD002_ON;
    } else {
        env->Output_Modem_Reset = STATE_RECORD002_OFF;
    }
}

void Mma_InfoSystemToUDSGetRedLedState(tGpios *gpios, tOutputStatesRecord_0002 *env) {

    if (GpioPinGet(&gpios->bip.led.red)) {
        env->Output_RED_LED = STATE_RECORD002_ON;
    } else {
        env->Output_RED_LED = STATE_RECORD002_OFF;
    }
}

void Mma_InfoSystemToUDSGetRedGreenState(tGpios *gpios, tOutputStatesRecord_0002 *env) {
    if (GpioPinGet(&gpios->bip.led.green)) {
        env->Output_GREEN_LED = STATE_RECORD002_ON;
    } else {
        env->Output_GREEN_LED = STATE_RECORD002_OFF;
    }
}

void Mma_InfoSystemToUDSGetSimProfileAmount(uint8_t *SimProfileAmount) {
    *SimProfileAmount = (uint8_t) 1;
}

void Mma_InfoSystemToUDSGetTCMTimeCalculation(uint8_t *TimeCalculation) {
    *TimeCalculation = (uint8_t) 1;
}

void Mma_InfoSystemToUDSGetBoardVoltage(tPowerManagement *power, uint8_t *BoardVoltage) {
    if (osMutexAcquire(power->mux_accessPwrMan, 100) == osOK) {
        double BoardVoltageTmp = (double )power->powerSusystem->boardPower.currentBoardVoltage / 1000;
        *BoardVoltage = (uint8_t) (BoardVoltageTmp / 0.0942);
        osMutexRelease(power->mux_accessPwrMan);
    }
}

void Mma_InfoSystemToUDSGetBatteryCharge(tPowerManagement *power, uint8_t *ReserveBatteryCharge) {
    if (osMutexAcquire(power->mux_accessPwrMan, 100) == osOK) {

        uint8_t BatteryCharge;

        if (power->powerSusystem->battery.currentBatVoltage < 3300) {
            BatteryCharge = 0;
        } else {

            if (power->powerSusystem->battery.currentBatVoltage > 4200) {
                BatteryCharge = 100;
            } else {
                BatteryCharge = ((power->powerSusystem->battery.currentBatVoltage - 3300) * 100) / (4200 - 3300);
            }
        }

        *ReserveBatteryCharge = (uint8_t) (BatteryCharge / 0.3922);
        osMutexRelease(power->mux_accessPwrMan);
    }
}

void Mma_InfoSystemToUDSGetBatteryCargeState(tGpios *gpios, tOutputStatesRecord_0002 *env) {
    if (GpioPinGet(&gpios->powerSusystem.battery.charge)) {
        env->Output_ON_Charg = STATE_RECORD002_ON;
    } else {
        env->Output_ON_Charg = STATE_RECORD002_OFF;
    }
}

void Mma_InfoSystemToUDSGetMuteState(tGpios *gpios, tPowerManagement *power, tOutputStatesRecord_0002 *env) {
    if (Pwm_GetMuteInCarState(power)) {
        env->Output_Mute = STATE_RECORD002_ON;
    } else {
        env->Output_Mute = STATE_RECORD002_OFF;
    }
}

double cConvertDecToGradus(double dec) {
    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;
    double gradus = deg + min / 60.0 + sec / 3600.0;

    return gradus;
}

void Mma_InfoSystemToUDSGetNavData(tNmeaRmc *nmeaRmc, tVehicleCoordinates *vehicleCoordinates) {
    if (nmeaRmc->status == 'A') {

        double latitude = cConvertDecToGradus(nmeaRmc->location.latitude);
        double longitude = cConvertDecToGradus(nmeaRmc->location.longitude);

        vehicleCoordinates->vehicleLatitude = (uint32_t) ((latitude + 90) * 3600000);
        vehicleCoordinates->vehicleLongitude = (uint32_t) ((longitude + 180) * 3600000);
    } else {
        vehicleCoordinates->vehicleLatitude = 0xFFFFFFFF;
        vehicleCoordinates->vehicleLongitude = 0xFFFFFFFF;
    }
}

void Mma_InfoSystemToUDSGetReliabilityNavData(tNmeaRmc *nmeaRmc, uint8_t *vehicleCoordinatesReliability) {
    if (nmeaRmc->status == 'A') {
        *vehicleCoordinatesReliability = 1;
    } else if (nmeaRmc->status == 'V') {
        *vehicleCoordinatesReliability = 0;
    } else {
        *vehicleCoordinatesReliability = 2;
    }
}

void Mma_InfoSystemToUDSGetRegState(tGsmWithGnss *gsmWithGnss, uint8_t *gsmRegistrationState) {
    if (gsmRegistrationState != NULL) {
        if (gsmWithGnss->regState == AT_NETWORK_REGISTRATION_STATE_NOT_REGISTERED) {
            *gsmRegistrationState = 0;
        }
        if (gsmWithGnss->regState == AT_NETWORK_REGISTRATION_STATE_REGISTERED_HOME) {
            *gsmRegistrationState = 1;
        }
        if (gsmWithGnss->regState == AT_NETWORK_REGISTRATION_STATE_REGISTERED_ROAMING) {
            *gsmRegistrationState = 2;
        }
    }
}

void Mma_InfoSystemToUDSGetErorModemState(tGsmWithGnss *gsmWithGnss, uint8_t *gsmInnerError) {
    if (gsmWithGnss->gsmState == GSM_INITIALIZING) {
        *gsmInnerError = 0;
    }
    if (gsmWithGnss->gsmState == GSM_IDLE) {
        *gsmInnerError = 1;
    }
    if (gsmWithGnss->gsmState == GSM_ERROR) {
        *gsmInnerError = 2;
    }
}

void Mma_InfoSystemToUDSGetSimProfile(const uint8_t *SimProfile) {
     SimProfile = 0;
}

void Mma_InfoSystemGetVehCoorReliabilityMem(tNmeaRmc *nmeaRmc, uint8_t *vehicleCoordinatesReliabilityMem) {
    if (nmeaRmc->status == 'A') {
        *vehicleCoordinatesReliabilityMem = 1;
    } else if (nmeaRmc->status == 'V') {
        *vehicleCoordinatesReliabilityMem = 0;
    } else {
        *vehicleCoordinatesReliabilityMem = 2;
    }
}


void Mma_InfoSystemGetEcallState(tEraGlonassUveos *eraGlonassUveos, uint8_t *EcallOn){
    if (eraGlonassUveos->fl_ecallState == true) {
        *EcallOn = true;
    } else {
        *EcallOn = false;
    }
}

void Mma_InfoSystemToUDSGetTimeStamp(tRtcIO *rtcI0, uint32_t *TimeStamp) {
    time_t timeThis;
    RtcGet(rtcI0, &timeThis);
    if(timeThis > 1727941386){
        *TimeStamp = timeThis;
    } else {
        *TimeStamp = 0xFFFFFFFF;
    }
}

void InfoToUDS_InitSetDefault(tMma *env) {
    env->InputStatesRecord_0001.Input_SOS = STATE_RECORD001_UNDEFINDET;
    env->InputStatesRecord_0001.Input_Service = STATE_RECORD001_UNDEFINDET;
    env->InputStatesRecord_0001.Input_KL15 = STATE_RECORD001_UNDEFINDET;
    env->InputStatesRecord_0001.Input_ACC = STATE_RECORD001_UNDEFINDET;
    env->InputStatesRecord_0001.Reserved = STATE_RECORD001_NOT_USED;
    env->InputStatesRecord_0001.Input_3 = STATE_RECORD001_NOT_USED;
    env->InputStatesRecord_0001.Input_ENS = STATE_RECORD001_NOT_USED;

    env->OutputStatesRecord_0002.NotUsed1 = STATE_RECORD002_NOT_USED;
    env->OutputStatesRecord_0002.Output_SOS_LED = STATE_RECORD002_NOT_USED;
    env->OutputStatesRecord_0002.Output_SERVICE_LED = STATE_RECORD002_NOT_USED;
    env->OutputStatesRecord_0002.NotUsed2 = STATE_RECORD002_NOT_USED;
    env->OutputStatesRecord_0002.NotUsed3 = STATE_RECORD002_NOT_USED;
    env->OutputStatesRecord_0002.OutputMODEM_ON = STATE_RECORD002_UNDEFINDET;
    env->OutputStatesRecord_0002.OutputCheckACC = STATE_RECORD002_NOT_USED;
    env->OutputStatesRecord_0002.OutputEN_CAN = STATE_RECORD002_NOT_USED;
    env->OutputStatesRecord_0002.Output_ON_OFF_Mode = STATE_RECORD002_NOT_USED;
    env->OutputStatesRecord_0002.Output_ON_Charg = STATE_RECORD002_UNDEFINDET;
    env->OutputStatesRecord_0002.Output_RED_LED = STATE_RECORD002_UNDEFINDET;
    env->OutputStatesRecord_0002.Output_GREEN_LED = STATE_RECORD002_UNDEFINDET;
    env->OutputStatesRecord_0002.Output_EN_VMIC = STATE_RECORD002_NOT_USED;
    env->OutputStatesRecord_0002.Output_Modem_Wake = STATE_RECORD002_NOT_USED;
    env->OutputStatesRecord_0002.Output_Modem_Reset = STATE_RECORD002_UNDEFINDET;
    env->OutputStatesRecord_0002.Output_Mute = STATE_RECORD002_UNDEFINDET;

    env->SystemStateID_0006.Comfort_State = 2;
    env->SystemStateID_0006.Airbag_State = 5;
    env->SystemStateID_0006.TCM_State = 1;
    env->SystemStateID_0006.Reserved2 = 2;
    env->SystemStateID_0006.Battery_State = 7;
    env->SystemStateID_0006.Motion_State = 3;
    env->SystemStateID_0006.Reserved3 = 2;
    env->SystemStateID_0006.RolloverAngleStatus = 0;
    env->SystemStateID_0006.GSM_State = INITIALIZING;

    env->state.GsmInnerError = 0;
    env->state.GsmRegistrationState = 0;
    env->state.VehicleCoordinates.vehicleLongitude = 0xFFFFFFFF;
    env->state.VehicleCoordinates.vehicleLatitude  = 0xFFFFFFFF;
    env->state.VehicleCoordinatesReliability = 2;
    env->state.ReserveBatteryTemperature = 80;
}

void Mma_InfoSystemGetGSMSoftwareVersionNumber(tString32 *env, const bool *modemState, char *GSMSoftwareVersionNumber){
    if(*modemState == true) {
        uint8_t length = env->length;
        if(length>20){
            length = 20;
        }
        memcpy(GSMSoftwareVersionNumber, env->data, length);
    }
}

void Mma_InfoSystemToUDSGetGsmRegistrationError(tGsmWithGnss *gsmWithGnss, const bool *modemState, uint8_t *GsmRegistrationError){
    if(*modemState == true) {
        if (GsmRegistrationError != NULL) {
            if (
                    gsmWithGnss->regState == AT_NETWORK_REGISTRATION_STATE_REGISTERED_HOME ||
                    gsmWithGnss->regState == AT_NETWORK_REGISTRATION_STATE_REGISTERED_ROAMING
               ) {
                *GsmRegistrationError = 1;
            } else if (gsmWithGnss->regState == AT_NETWORK_REGISTRATION_STATE_REGISTRATION_DENIED) {
                *GsmRegistrationError = 2;
            } else {
                *GsmRegistrationError = 0;
            }
        }
    }
}

static _Noreturn void Mma_InfoSystemToUDSThread(tMma *env) {
    LoggerInfoStatic(LOGGER, LOG_SIGN, "Поток передачи состояния устройства через UDS протокол запущен")
// GsmRegistartionStateMem
    while (1) {

        env->state.MDS_FormatVersion = 2;
        Mma_InfoSystemToUDSGetSimProfileAmount(&env->state.SimProfileAmount);
        Mma_InfoSystemToUDSGetTCMTimeCalculation(&env->state.TCM_time_calculation);
        Mma_InfoSystemToUDSGetBoardVoltage(&env->power,  &env->state.BoardVoltage);
        Mma_InfoSystemToUDSGetBatteryCharge(&env->power,  &env->state.ReserveBatteryCharge);
        Mma_InfoSystemToUDSGetTimeStamp(&env->rtc->rtcI0, &env->state.TimeStamp);
        Mma_InfoSystemToUDSGetSimProfile(&env->state.SimProfile);
        Mma_InfoSystemToUDSGetErorModemState(&env->gsmWithGnss, &env->state.GsmInnerError);
        Mma_InfoSystemToUDSGetRegState(  &env->gsmWithGnss, &env->state.GsmRegistrationState);
        Mma_InfoSystemToUDSGetNavData(&env->gsmWithGnss.gnss.currentRmc, &env->state.VehicleCoordinates);
        Mma_InfoSystemToUDSGetReliabilityNavData(&env->gsmWithGnss.gnss.currentRmc, &env->state.VehicleCoordinatesReliability);
        Mma_InfoSystemToUDSGetInputLinesState(env->gpios, &env->InputStatesRecord_0001);
        Mma_InfoSystemToUDSGetModemResetState(env->gpios, &env->power, &env->OutputStatesRecord_0002);
        Mma_InfoSystemToUDSGetRedLedState(env->gpios, &env->OutputStatesRecord_0002);
        Mma_InfoSystemToUDSGetRedGreenState(env->gpios, &env->OutputStatesRecord_0002);
        Mma_InfoSystemToUDSGetBatteryCargeState(env->gpios, &env->OutputStatesRecord_0002);
        Mma_InfoSystemToUDSGetMuteState(env->gpios, &env->power, &env->OutputStatesRecord_0002);
        Mma_InfoSystemToUDSpowerStatus(env->gpios, &env->power, &env->OutputStatesRecord_0002);
        Mma_InfoSystemToUDSGetModemState(&env->ModemDataCollector, &env->indication.mode,  &env->SystemStateID_0006);
        Mma_InfoSystemToUDSGetBattaryState(&env->power, &env->SystemStateID_0006);
        Mma_InfoSystemToUDSGetMotionState(&env->power, &env->SystemStateID_0006);

        Mma_InfoSystemToUDSGetGsmRegistrationError(
                &env->gsmWithGnss,
                &env->ModemDataCollector.fl_IsLoad,
                &env->state.GsmRegistrationError
        );

        Mma_InfoSystemGetGSMSoftwareVersionNumber(
                &env->ModemDataCollector.modemVersionRevision.softValue,
                &env->ModemDataCollector.fl_IsLoad,
                env->state.GSMSoftwareVersionNumber
        );
        SystemDelayMs(4);
    }
}


void TMR2_CanMute_Start(tMma *env){
    crm_clocks_freq_get(&crm_clocks_freq_structure);
    crm_periph_clock_enable(CRM_TMR8_PERIPH_CLOCK, TRUE);
    uint32_t tmr = (crm_clocks_freq_structure.apb2_freq * 2 / 10000) - 1;
    tmr = tmr/100;
    tmr_base_init(TMR8, 9990, tmr);
    tmr_cnt_dir_set(TMR8, TMR_COUNT_UP);
    tmr_interrupt_enable(TMR8, TMR_OVF_INT, TRUE);
    nvic_priority_group_config(NVIC_PRIORITY_GROUP_4);
    nvic_irq_enable(TMR8_OVF_TMR13_IRQn, 0x59, 0);
    tmr_counter_enable(TMR8, TRUE);
    uint32_t tmStop =  SystemGetMs() - env->tmStart;
    tmStop = tmStop;
}

//uint32_t tm;
//uint16_t ms[1000];
//volatile  uint16_t i = 0;
//volatile  uint16_t o = 0;
//volatile uint32_t ttt = 0;
//volatile uint32_t aa = 0;
void TMR8_OVF_TMR13_IRQHandler()
{
    if(tmr_interrupt_flag_get(TMR8, TMR_OVF_FLAG) == SET)
    {
        if (EXT_ENV_ADR_TELE.META_EXT_ENV_TELE) {
            EXT_ENV_ADR_TELE.tele_func(NULL, TELE_MODULE_GNSS);

//            if(o == 1000){
//                o=0;
//            }
//            aa = SystemGetMs();
//            if(ttt > aa){
//                ++i;
//            } else {
//                ms[o] = i;
//                i=0;
//                o++;
//                ttt = SystemGetMs()+1000;
//            }
        }
        tmr_flag_clear(TMR8, TMR_OVF_FLAG);
    }
}

void UserInputInfoSystemToUDS(tMma *env) {
    InitThreadAtrStatic(&env->threadToUDS_Info.attr, "Thread_Uds", env->threadToUDS_Info.controlBlock,
                        env->threadToUDS_Info.stack, osPriorityNormal);
    if (!env->threadToUDS_Info.id) {
        env->threadToUDS_Info.id = osThreadNew(
                (osThreadFunc_t) (Mma_InfoSystemToUDSThread),
                (void *) (env),
                &env->threadToUDS_Info.attr
        );
    }
}