//
// Created by cfif on 05.05.23.
//
#include "MainModesArbiter_Private.h"
#include "stdio.h"
#include "fc7xxx_driver_rgm.h"
#include "Model_actuator.h"

#include "ADC_Temp_Table.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;
}


//uint8_t dataR[1024 * 2];


void BTS5180_120(tMma *env, char *desc, uint16_t adc_value) {
    float kILIS = 550.0f;

    float ERROR_THRESHOLD_V = 4.9f;
    uint16_t ERROR_THRESHOLD_CODE = (uint16_t) (ERROR_THRESHOLD_V * 4095.0f / 5.0f);

    if (adc_value >= ERROR_THRESHOLD_CODE) {
        LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "%s: Error !!!", desc)
    } else {
        // Преобразование в напряжение
        float U = (float) adc_value * 5.0f / 4095.0f;
        float I = U / 1200; // Ток диагностики R = 1200
        float Iout = I * kILIS; // Ток устройства
        LoggerFormatInfo(LOGGER, LOG_TASK_ARB,
                         "%s: U = %f I = %f Iout = %f",
                         desc, U, I, Iout)
    }

}

void VN7008AJ(tMma *env, char *desc, uint16_t adc_value) {

    float RSENSE = 2490.0f;   // Сопротивление датчика, Ом (На схеме)
    float K_TYPICAL = 5890.0f;   // Типичный коэффициент из даташита на микросхему

    float ERROR_THRESHOLD_V = 4.9f;
    uint16_t ERROR_THRESHOLD_CODE = (uint16_t) (ERROR_THRESHOLD_V * 4095.0f / 5.0f);

    // 1. Проверка на ошибку
    if (adc_value >= ERROR_THRESHOLD_CODE) {
        LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "%s: Error !!!", desc)
    } else {

        // 2. Преобразование в напряжение
        float vsense = (float) adc_value * 5.0f / 4095.0f;

        // 3. Вычисление тока
        float Isense = vsense / RSENSE;
        float Iout = Isense * K_TYPICAL;

        LoggerFormatInfo(LOGGER, LOG_TASK_ARB,
                         "%s: U = %f I = %f Iout = %f",
                         desc, vsense, Isense, Iout)
    }

}

void ANALOG_SENSOR(tMma *env, char *desc, uint16_t adc_value) {
    float ERROR_THRESHOLD_V = 4.9f;
    uint16_t ERROR_THRESHOLD_CODE = (uint16_t) (ERROR_THRESHOLD_V * 4095.0f / 5.0f);

    // 1. Проверка на ошибку
    if (adc_value >= ERROR_THRESHOLD_CODE) {
        LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "%s: Error !!!", desc)
    } else {
        // Преобразование в напряжение
        float U = (float) adc_value * 5.0f / 4095.0f;

        LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "%s: U = %f", desc, U)
    }

}

void LoadDataInFromModel(tMma *env) {
    int16_t temp1 = 0;
    float temp2 = 0;

    if (osMutexAcquire(env->adcTask0.access, 1000) == osOK) {

        if (env->adcTask0.ADC_isUpdate) {
            env->adcTask0.ADC_isUpdate = false;


            temp1 = get_temperature_fast(env->adcTask0.ADC0_Data.Sensor_Ambient_Temp);
            temp2 = get_temperature_from_adc(env->adcTask0.ADC0_Data.Sensor_Ambient_Temp, ALG_STEINHART);
            env->rtDW.ADC_Data_Model.Sensor_Incar_Temp_FL =  env->adcTask0.ADC0_Data.Sensor_Ambient_Temp;


            LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "Temp1 = %d; Temp2 = %d;", temp1, (int16_t) (temp2 * 10.0f))
/*
            // R1 = 91000 R2 = 20000 ((5 * (91000 + 20000)) / 20000 = 27.75 В)
            float U_IGN_CHECK = ((float) env->adcTask0.ADC0_Data.IGN_ANS * 27.75f) / 4095.0f;
            LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "U_IGN_CHECK = %f", U_IGN_CHECK)

            BTS5180_120(env, "BTS5120_2EKA_ShutoffValvePowerTXV1",
                        env->adcTask0.ADC0_Data.BTS5120_2EKA_ShutoffValvePowerTXV1);
            BTS5180_120(env, "BTS5120_2EKA_ShutoffValvePowerTXV2",
                        env->adcTask0.ADC0_Data.BTS5120_2EKA_ShutoffValvePowerTXV2);

            BTS5180_120(env, "BTS5180_2EKA_ShutOFFValveFront", env->adcTask0.ADC0_Data.BTS5180_2EKA_ShutOFFValveFront);
            BTS5180_120(env, "BTS5180_2EKA_ShutOFFValveRear", env->adcTask0.ADC0_Data.BTS5180_2EKA_ShutOFFValveRear);

            BTS5180_120(env, "BTS5180_2EKA_TwoWayValve", env->adcTask0.ADC0_Data.BTS5180_2EKA_TwoWayValve);
            BTS5180_120(env, "BTS5180_2EKA_ReservePowerSupply",
                        env->adcTask0.ADC0_Data.BTS5180_2EKA_ReservePowerSupply);

            BTS5180_120(env, "BTS5180_2EKA_FrontIncarMotor", env->adcTask0.ADC0_Data.BTS5180_2EKA_FrontIncarMotor);
            BTS5180_120(env, "BTS5180_2EKA_RearIncarMotor", env->adcTask0.ADC0_Data.BTS5180_2EKA_RearIncarMotor);

            BTS5180_120(env, "BTS5180_2EKA_ChannelPTCPower1", env->adcTask0.ADC0_Data.BTS5180_2EKA_ChannelPTCPower1);
            BTS5180_120(env, "BTS5180_2EKA_ChannelPTCPower2", env->adcTask0.ADC0_Data.BTS5180_2EKA_ChannelPTCPower2);

            ANALOG_SENSOR(env, "Sensor_Ambient_Temp", env->adcTask0.ADC0_Data.Sensor_Ambient_Temp);
            ANALOG_SENSOR(env, "Sensor_AC_Pressure", env->adcTask0.ADC0_Data.Sensor_AC_Pressure);
            ANALOG_SENSOR(env, "Sensor_Incar_Temp_FL", env->adcTask0.ADC0_Data.Sensor_Incar_Temp_FL);
            ANALOG_SENSOR(env, "Sensor_Incar_Temp_RL", env->adcTask0.ADC0_Data.Sensor_Incar_Temp_RL);
            ANALOG_SENSOR(env, "Sensor_Rear_Evap_Temp", env->adcTask0.ADC0_Data.Sensor_Rear_Evap_Temp);
            ANALOG_SENSOR(env, "Sensor_Evap_Temp", env->adcTask0.ADC0_Data.Sensor_Evap_Temp);
            ANALOG_SENSOR(env, "Sensor_Rear_Duct1", env->adcTask0.ADC0_Data.Sensor_Rear_Duct1);
            ANALOG_SENSOR(env, "Sensor_Rear_Duct2", env->adcTask0.ADC0_Data.Sensor_Rear_Duct2);
            ANALOG_SENSOR(env, "Sensor_Front_Duct1", env->adcTask0.ADC0_Data.Sensor_Front_Duct1);
            ANALOG_SENSOR(env, "Sensor_Front_Duct2", env->adcTask0.ADC0_Data.Sensor_Front_Duct2);
            ANALOG_SENSOR(env, "Sensor_Front_Duct3", env->adcTask0.ADC0_Data.Sensor_Front_Duct3);
            ANALOG_SENSOR(env, "Sensor_Front_Duct4", env->adcTask0.ADC0_Data.Sensor_Front_Duct4);
            ANALOG_SENSOR(env, "Sensor_Rear_Duct3", env->adcTask0.ADC0_Data.Sensor_Rear_Duct3);
            ANALOG_SENSOR(env, "Sensor_Rear_Duct4", env->adcTask0.ADC0_Data.Sensor_Rear_Duct4);
            ANALOG_SENSOR(env, "Sensor_Incar_Temp_FR", env->adcTask0.ADC0_Data.Sensor_Incar_Temp_FR);
            ANALOG_SENSOR(env, "Sensor_Incar_Temp_RR", env->adcTask0.ADC0_Data.Sensor_Incar_Temp_RR);
            ANALOG_SENSOR(env, "Sensor_Rear_Duct5", env->adcTask0.ADC0_Data.Sensor_Rear_Duct5);
            ANALOG_SENSOR(env, "Sensor_Rear_Duct6", env->adcTask0.ADC0_Data.Sensor_Rear_Duct6);
            ANALOG_SENSOR(env, "Reserve_Sensor_Duct_Temp_1", env->adcTask0.ADC0_Data.Reserve_Sensor_Duct_Temp_1);
            ANALOG_SENSOR(env, "Sensor_Front_Duct5", env->adcTask0.ADC0_Data.Sensor_Front_Duct5);
            ANALOG_SENSOR(env, "Sensor_Front_Duct6", env->adcTask0.ADC0_Data.Sensor_Front_Duct6);
            ANALOG_SENSOR(env, "Pressure_DIAG", env->adcTask0.ADC0_Data.Pressure_DIAG);
            ANALOG_SENSOR(env, "Reserve_Sensor_Duct_Temp_2", env->adcTask0.ADC0_Data.Reserve_Sensor_Duct_Temp_2);
*/
        }

        osMutexRelease(env->adcTask0.access);

    }

    if (osMutexAcquire(env->adcTask1.access, 1000) == osOK) {

        if (env->adcTask1.ADC_isUpdate) {
            env->adcTask1.ADC_isUpdate = false;
/*
            // R1 = 91000 R2 = 16000 ((5 * (91000 + 16000)) / 16000 = 33.4375 В)
            float U_PBATT_CHECK = ((float) env->adcTask1.ADC1_Data.PBATT_CHECK * 33.4375f) / 4095.0f;
            LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "U_PBATT_CHECK = %f", U_PBATT_CHECK)

            // R1 = 30000 R2 = 12000 ((5 * (30000 + 12000)) / 12000 = 17.5 В)
            float U_VN7008AJ_DIAG_FrontLINActuatorPowerDriverAB =
                    ((float) env->adcTask0.ADC0_Data.VN7008AJ_DIAG_FrontLINActuatorPowerDriverAB * 17.5f) / 4095.0f;
            LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "U_VN7008AJ_DIAG_FrontLINActuatorPowerDriverAB = %f",
                             U_VN7008AJ_DIAG_FrontLINActuatorPowerDriverAB)

            // R1 = 30000 R2 = 12000 ((5 * (30000 + 12000)) / 12000 = 17.5 В)
            float U_VN7008AJ_DIAG_RearLINActuatorPowerDriverC =
                    ((float) env->adcTask0.ADC0_Data.VN7008AJ_DIAG_RearLINActuatorPowerDriverC * 17.5f) / 4095.0f;
            LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "U_VN7008AJ_DIAG_RearLINActuatorPowerDriverC = %f",
                             U_VN7008AJ_DIAG_RearLINActuatorPowerDriverC)

            VN7008AJ(env, "VN7008AJ_FrontLINActuatorPowerDriverAB", env->adcTask1.ADC1_Data.VN7008AJ_FrontLINActuatorPowerDriverAB);
            VN7008AJ(env, "VN7008AJ_RearLINActuatorPowerDriverC", env->adcTask1.ADC1_Data.VN7008AJ_RearLINActuatorPowerDriverC);
*/
        }

        //temp2 = get_temperature_fast(env->adcTask1.ADC_Data[0], fast_lookup_KST45, 512);

        //env->rtDW.controllerDataIncarInput.InIncarFR = env->adcTask1.ADC_Data[0];

        osMutexRelease(env->adcTask1.access);
        //LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "Temp2 = %d", temp2)
    }

    if (osMutexAcquire(env->ModelTask.access, 5000) == osOK) {
        rtDW.t_now = GetSystemTick();
        memcpy(&rtDW, &env->rtDW.ADC_Data_Model, sizeof(rtDW.ADC_Data_Model));

        if (env->ModelTask.isUpdate) {
            env->ModelTask.isUpdate = false;
            set_CCU_Errors(&env->canSpamTransmitter,  (CCU_Errors_t *)&CCU_Errors_Model);
        }

        osMutexRelease(env->ModelTask.access);
    }
}

static _Noreturn void Mma_Thread(tMma *env) {

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

    init_fast_lookup_table(ALG_STEINHART);
//    can_rx_message_type frame_data;

//    uint32_t step = 0;


    if (RGM_SRS_WAKEUP_MASK == (RGM->SRS & RGM_SRS_WAKEUP_MASK)) {
        LoggerInfoStatic(LOGGER, LOG_TASK_ARB, "Wake up from standby")
    }

    SystemDelayMs(1000);

    LoadDataInFromModel(env);

    ModelTask_StartThread(&env->ModelTask);


/*
    for (;;) {
        uint16_t len = SerialPortReceive(&env->serialPorts->SerialPortLog_IO, dataR, 1024, 1000);

        if (len != 0) {
            SerialPortTransmit(&env->serialPorts->SerialPortLog_IO, dataR, len, 1000);
        }
    }
*/


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

    for (;;) {
/*
        env->pwms->pwmIo.setActivePercent(env->pwms->pwmIo.env, 77);
        SystemDelayMs(10);
        uint8_t pwm = env->pwms->pwmCaptureIO.getPwm(env->pwms->pwmCaptureIO.env);
        //LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "PWM = %d", pwm)

        env->pwms->pwmIo.setActivePercent(env->pwms->pwmIo.env, 22);
        SystemDelayMs(10);
        pwm = env->pwms->pwmCaptureIO.getPwm(env->pwms->pwmCaptureIO.env);
        //LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "PWM = %d", pwm)

*/

        LoadDataInFromModel(env);


/*
        LoggerInfoStatic(LOGGER, LOG_TASK_ARB, "Zorro...")
        printf("Test Test Test Test Test Test Test Test Test Test XA XA ...\n");

        GpioPinToggle(&env->gpios->led.LED1);

        SystemDelayMs(500);
*/

/*
        if (RGM_SRS_WAKEUP_MASK == (RGM->SRS & RGM_SRS_WAKEUP_MASK)) {
        } else {
            SMC_SetSystemMode(SMC_MODE_STANBY_3);
        }
*/

/*
        if (osMutexAcquire(env->adcTask0.access, 1000) == osOK) {
            temp1 = get_temperature_fast(env->adcTask0.ADC_Data[0], fast_lookup_Incar, 512);
            osMutexRelease(env->adcTask0.access);
            LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "Temp1 = %d", temp1)
        }


        if (osMutexAcquire(env->adcTask1.access, 1000) == osOK) {
            temp2 = get_temperature_fast(env->adcTask1.ADC_Data[0], fast_lookup_KST45, 512);
            osMutexRelease(env->adcTask1.access);
            LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "Temp2 = %d", temp2)
        }
*/

/*
        if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {

            if ((env->linTaskActuator0.linCommandActuator[0].COM == LIN_ACT_CFR_SUCCESSFUL) ||
                (env->linTaskActuator0.linCommandActuator[0].COM == LIN_ACT_CFR_NONE)) {
                busy = true;
            }

            if (busy == true) {
                busy = false;

                switch (step) {
                    case 0: {

                        resetStall(&env->linTaskActuator0);

                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_MOD;
                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
                        env->linTaskActuator0.linCommandActuator[0].MODE = LIN_MODE_STOP;
                        ++step;

                        break;
                    }
                    case 1: {

                        resetStall(&env->linTaskActuator0);

                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_INI;
                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
                        env->linTaskActuator0.linCommandActuator[0].POS = 6000;
                        ++step;

                        break;
                    }
                    case 2: {

                        resetStall(&env->linTaskActuator0);

                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_MOD;
                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
                        env->linTaskActuator0.linCommandActuator[0].MODE = LIN_MODE_NORMAL;
                        ++step;

                        break;
                    }
                    case 3: {

                        resetStall(&env->linTaskActuator0);

                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_SET;
                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
                        env->linTaskActuator0.linCommandActuator[0].POS = 0;
                        env->linTaskActuator0.linCommandActuator[0].Stall_SET = 1;
                        env->linTaskActuator0.linCommandActuator[0].Lnoise_SET = 0;
                        env->linTaskActuator0.linCommandActuator[0].Autos_SET = 1;
                        env->linTaskActuator0.linCommandActuator[0].Speed_SET = 3;
                        env->linTaskActuator0.linCommandActuator[0].Coils_Stop_SET = 0;

                        ++step;

                        break;
                    }

                    case 4: {
                        SystemDelayMs(10000);
                        asm("nop");
                        ++step;
                        //if (env->linTaskActuator0.linStateActuator[7].CPOS_ALL == 0) {
                        //    ++step;
                        //}
                        break;
                    }

                    case 5: {

                        resetStall(&env->linTaskActuator0);

                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_INI;
                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
                        env->linTaskActuator0.linCommandActuator[0].POS = 0;
                        ++step;

                        break;
                    }

                    case 6: {

                        resetStall(&env->linTaskActuator0);

                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_SET;
                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
                        env->linTaskActuator0.linCommandActuator[0].POS = 6000;
                        env->linTaskActuator0.linCommandActuator[0].Stall_SET = 1;
                        env->linTaskActuator0.linCommandActuator[0].Lnoise_SET = 0;
                        env->linTaskActuator0.linCommandActuator[0].Autos_SET = 1;
                        env->linTaskActuator0.linCommandActuator[0].Speed_SET = 3;
                        env->linTaskActuator0.linCommandActuator[0].Coils_Stop_SET = 0;

                        ++step;

                        break;
                    }

                    case 7: {
                        SystemDelayMs(10000);
                        asm("nop");
                        ++step;

                        //if (env->linTaskActuator0.linStateActuator[7].CPOS_ALL >= 6000) {
                        //    ++step;
                        //}
                        break;
                    }

                    case 8: {
                        if (env->linTaskActuator0.linStateActuator[7].CPOS_ALL >= 8000) {
                            ++step;
                        }
                        break;
                    }

                    default: {
                    }
                }
            }
            osMutexRelease(env->linTaskActuator0.access);
        }

        LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "Step = %d", step)
*/





/*
        uint16_t len = env->canPorts->Can0_IO.receive(env->canPorts->Can0_IO.env, 0, (uint8_t *)&frame_data, 1, 1000);

        if (len > 0) {

            if (frame_data.id_type == FLEXCAN_ID_STD) {
                CanSerialPortFrameSetType(env->canPorts->Can0_IO.env, FLEXCAN_ID_STD);
                CanSerialPortFrameSetId(env->canPorts->Can0_IO.env, frame_data.standard_id);
            } else {
                CanSerialPortFrameSetType(env->canPorts->Can0_IO.env, FLEXCAN_ID_EXT);
                CanSerialPortFrameSetId(env->canPorts->Can0_IO.env, frame_data.extended_id);
            }


            env->canPorts->Can0_IO.transmit(env->canPorts->Can0_IO.env, frame_data.data, frame_data.dlc, 1000);
        }
*/

        SystemDelayMs(10);
    }
}


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