//
// 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";

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


static _Noreturn void Mma_Thread(tMma *env) {

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

    can_rx_message_type frame_data;

    uint32_t step = 0;
    int16_t temp1 = 0;
    int16_t temp2 = 0;

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

//    SystemDelayMs(1000);

    ModelTask_StartThread(&env->ModelTask);

    bool isActuatorWorkBusy = false;

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

    for (;;) {





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

            if (env->linTaskActuator0.busy == 0) {

                for (uint8_t i = 0; i < LIN0_ISSR_ALL; ++i) {

                    env->linTaskActuator0.linCommandActuator[i].POS = env->rtY_local.Out1.POS[i];
                    env->linTaskActuator0.linCommandActuator[i].BUS_ADR = env->rtY_local.Out1.BUS_ADR[i];
                    env->linTaskActuator0.linCommandActuator[i].MODE = env->rtY_local.Out1.MODE[i];

                    if (env->linTaskActuator0.linCommandActuator[i].COM == LIN_ACT_CFR_SUCCESSFUL) {
                        env->rtY_local.Out1.COM[i] = LIN_ACT_CFR_NONE;
                    }

                    env->linTaskActuator0.linCommandActuator[i].COM = env->rtY_local.Out1.COM[i];
                    env->linTaskActuator0.linCommandActuator[i].Stall_SET = env->rtY_local.Out1.Stall_SET[i];
                    env->linTaskActuator0.linCommandActuator[i].Lnoise_SET = env->rtY_local.Out1.Lnoise_SET[i];
                    env->linTaskActuator0.linCommandActuator[i].Autos_SET = env->rtY_local.Out1.Autos_SET[i];
                    env->linTaskActuator0.linCommandActuator[i].Speed_SET = env->rtY_local.Out1.Speed_SET[i];
                    env->linTaskActuator0.linCommandActuator[i].Coils_Stop_SET = env->rtY_local.Out1.Coils_Stop_SET[i];

                    env->rtU_local.in_CPOS_ALL_Ch0[i] = env->linTaskActuator0.linStateActuator[i].CPOS_ALL;
                    env->rtU_local.in_Act_Emrf_Slave_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Emrf_Slave;
                    env->rtU_local.in_Mode_Slave_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Mode_Slave;
                    env->rtU_local.in_Act_Err1_Supply_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Error1_Supply_Slave;
                    env->rtU_local.in_Act_Err2_Communication_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Error2_Communication_Slave;
                    env->rtU_local.in_Act_Err3_Temperature_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Error3_Temperature_Slave;
                    env->rtU_local.in_Act_Err4_Permanent_Electrical_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Error4_Permanent_Electrical_Slave;
                    env->rtU_local.in_Act_Stall_Slave_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Stall_Slave;
                    env->rtU_local.in_Act_Reset_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Reset_Slave;
                }

                isActuatorWorkBusy = setBusy(&env->linTaskActuator0);
                resetStall(&env->linTaskActuator0);

                env->rtU_local.in_Error_Connect_Ch0 = env->linTaskActuator0.error_connect;
                env->rtU_local.in_Busy_Ch0 = env->linTaskActuator0.busy;
            }

            osMutexRelease(env->linTaskActuator0.access);
        }


        if (osMutexAcquire(env->ModelTask.access, 5000) == osOK) {

            if (env->ModelTask.triggerCommand == true) {
                env->rtU_local.in_Busy_Ch0 = 1;
            }

            memcpy(&rtU, &env->rtU_local, sizeof(ExtU));


            if ((env->ModelTask.triggerCommand == true) && (isActuatorWorkBusy == false)) {
                env->ModelTask.triggerCommand = false;
                isActuatorWorkBusy = true;

                for (uint8_t i = 0; i < 9; ++i) {
                    rtY.Out1.COM[i] = env->ModelTask.numCommand[i];
                }

                memcpy(&env->rtY_local.Out1, &rtY.Out1, sizeof(rtY.Out1));
            }

            osMutexRelease(env->ModelTask.access);
        }

*/













/*
        LoggerInfoStatic(&env->slog.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(&env->slog.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(&env->slog.logger, LOG_TASK_ARB, "Temp2 = %d", temp2)
        }
*/


/*
        bool isBusy = true;
        if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
            if (env->linTaskActuator0.linCommandActuator[0].COM == LIN_ACT_CFR_NONE) {
                isBusy = false;
            }
            osMutexRelease(env->linTaskActuator0.access);
        }

        if (isBusy == false) {

            switch (step) {
                case 0: {
                    if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_MOD;
                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
                        env->linTaskActuator0.linCommandActuator[0].MODE = LIN_MODE_STOP;
                        ++step;
                        osMutexRelease(env->linTaskActuator0.access);
                    }
                    break;
                }
                case 1: {
                    if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_INI;
                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
                        env->linTaskActuator0.linCommandActuator[0].POS = 6000;
                        ++step;
                        osMutexRelease(env->linTaskActuator0.access);
                    }
                    break;
                }
                case 2: {
                    if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_MOD;
                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
                        env->linTaskActuator0.linCommandActuator[0].MODE = LIN_MODE_NORMAL;
                        ++step;
                        osMutexRelease(env->linTaskActuator0.access);
                    }
                    break;
                }
                case 3: {
                    if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {

                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_SET;
                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
                        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;

                        resetStall(&env->linTaskActuator0);

                        ++step;
                        osMutexRelease(env->linTaskActuator0.access);
                    }
                    break;
                }

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

                case 5: {
                    if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {

                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_INI;
                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
                        env->linTaskActuator0.linCommandActuator[0].POS = 0;
                        ++step;
                        osMutexRelease(env->linTaskActuator0.access);
                    }
                    break;
                }

                case 6: {
                    if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {

                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_SET;
                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
                        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;

                        resetStall(&env->linTaskActuator0);

                        ++step;
                        osMutexRelease(env->linTaskActuator0.access);
                    }
                    break;
                }

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

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

                case 8: {
                    if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {

                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_SET;
                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
                        env->linTaskActuator0.linCommandActuator[0].POS = 8000;
                        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;

                        resetStall(&env->linTaskActuator0);

                        ++step;
                        osMutexRelease(env->linTaskActuator0.access);
                    }
                    break;
                }

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

                default: {
                }
            }


        }
*/


/*
        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(100);
    }
}


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