//
// Created by cfif on 05.05.23.
//
#include "MainModesArbiter_Private.h"
#include "stdio.h"
#include "fc7xxx_driver_rgm.h"
#include "HVAC_model.h"
#include "LinActuatorWork.h"
#include "StatusError.h"
#include "ADC_Temp.h"


const char LOG_TASK_ARB[] = "Arb";
#define LOGGER &env->slog.logger

void Mma_Init(
        tMma *env,
        tGpios *gpios,
        tAdcs *adcs,
        tSerialPorts *serialPorts,
        tLinPorts *linPorts,
        tCanPorts *canPorts,
        tStorageOnFlash *flash,
        tPwms *pwms,
        tRtcs *rtcs
) {
    env->gpios = gpios;
    env->serialPorts = serialPorts;
    env->linPorts = linPorts;
    env->canPorts = canPorts;
    env->rtcs = rtcs;
    env->adcs = adcs;
    env->flash = flash;
    env->pwms = pwms;

    InitThreadAtrStatic(&env->thread.attr, "Mma", env->thread.controlBlock, env->thread.stack, osPriorityNormal);
    env->thread.id = 0;
}



void GetCanErrors(tMma *env, uint8_t canIndex) {
    FLEXCAN_ErrorInfoType errorInfo;

    // Получить информацию об ошибках
    FLEXCAN_ErrorType result = FLEXCAN_GetErrorInfo(canIndex, &errorInfo);

    if (result == FLEXCAN_ERROR_OK) {
        // Количество ошибок приёма
        uint32_t rxErrors = errorInfo.u32RxErrCnt;
        uint32_t rxErrorsFast = errorInfo.u32RxErrCnt_Fast;

        // Количество ошибок передачи
        uint32_t txErrors = errorInfo.u32TxErrCnt;
        uint32_t txErrorsFast = errorInfo.u32TxErrCnt_Fast;

        LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "RX Errors: %lu (normal), %lu (fast)\n", rxErrors, rxErrorsFast);
        LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "TX Errors: %lu (normal), %lu (fast)\n", txErrors, txErrorsFast);

        // Общее количество ошибок
        uint32_t totalErrors = rxErrors + rxErrorsFast + txErrors + txErrorsFast;
        LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "Total errors: %lu\n", totalErrors);
    }
}




static _Noreturn void Mma_Thread(tMma *env) {

    // Запуск устройства
    Mma_InitStage(env);

    env->pwms->pwmFrontIo.run(env->pwms->pwmFrontIo.env);

    env->pwms->pwmFrontIo.setActivePercent(env->pwms->pwmFrontIo.env, 0);
    env->pwms->pwmRearIo.setActivePercent(env->pwms->pwmRearIo.env, 0);
    env->pwms->pwmFrontReservedIo.setActivePercent(env->pwms->pwmFrontReservedIo.env, 0);
    env->pwms->pwmRearReservedIo.setActivePercent(env->pwms->pwmRearReservedIo.env, 0);

    uint32_t timeoutPowerTrain = SystemGetMs() + 3000;
    bool isNotNull = false;
    while ((!isNotNull) && (timeoutPowerTrain > SystemGetMs())) {
        isNotNull = get_is_NoNull_CanSpam_BCM_PowerTrain_Receiver(&env->canSpamReceiver);
        SystemDelayMs(1);
    }

    ModelTask_StartThread(&env->ModelTask);

    while (!env->ModelTask.isRunning) {
        LoggerInfoStatic(LOGGER, LOG_TASK_ARB, "Waiting for the model to run...")
        SystemDelayMs(1);
    }

    LoggerInfoStatic(LOGGER, LOG_TASK_ARB, "Model running...")

//    SMC_SetSystemMode(SMC_MODE_STANBY_3);

    for (;;) {

//        bool ST_PowerReserve = GpioPinGet(&env->gpios->power.BTS4175SGAXUMA1_PowerReserve.ST_PowerReserve);
//        bool ST_ReservePower = GpioPinGet(&env->gpios->power.BTS4175SGAXUMA1_ReservePowerOutput.ST_ReservePower);


//        GetCanErrors(env, 0);


        SystemDelayMs(25);
        GpioPinToggle(&env->gpios->led.LED_G);
        SystemDelayMs(25);


    }
}


void Mma_StartThread(tMma *env) {
    if (!env->thread.id) {
        env->thread.id = osThreadNew((osThreadFunc_t) (Mma_Thread), (void *) (env), &env->thread.attr);
    } else {
        osThreadResume(env->thread.id);
    }
}