//
// Created by cfif on 19.12.2025.
//
#include "Model_Task.h"
#include <SystemDelayInterface.h>
#include <CmsisRtosThreadUtils.h>
#include "Model_actuator.h"

#define LOG_SIGN "Model"
#define LOGGER env->logger

void ModelTask_Init(
        tModelTask *env,
        tLoggerInterface *logger
) {
    Model_actuator_initialize();
    env->access = osMutexNew(NULL);
    env->logger = logger;
    InitThreadAtrStatic(&env->thread.attr, "ModelTask", env->thread.controlBlock, env->thread.stack, osPriorityNormal);
    env->thread.id = 0;
}

static bool setActuatorBusy(tModelTask *env) {

    for (uint8_t i = 0; i < 9; ++i) {
        if (rtY.Out1.COM[i] != 0) {
#if (LOG_LIN_ACTUATOR == 1)
            LoggerInfoStatic(LOGGER, LOG_SIGN, "DETECT COMMAND")
#endif
            env->triggerCommand = true;

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

            rtU.in_Busy_Ch0 = 1;
            return true;
        }
    }

    return false;
}

static _Noreturn void ModelTask_Thread(tModelTask *env) {
    for (;;) {
        if (osMutexAcquire(env->access, 1000) == osOK) {

            //LoggerFormatInfo(LOGGER, LOG_SIGN, "in_Busy_Ch0 = %d in_Act_Stall_Slave_Ch0 = %d", rtU.in_Busy_Ch0, rtU.in_Act_Stall_Slave_Ch0[7])

            Model_actuator_step();
            setActuatorBusy(env);
            osMutexRelease(env->access);
        }
        SystemDelayMs(250);
    }
}

void ModelTask_StartThread(tModelTask *env) {
    if (!env->thread.id) {
        env->thread.id = osThreadNew((osThreadFunc_t) (ModelTask_Thread), (void *) (env), &env->thread.attr);
    } else {
        osThreadResume(env->thread.id);
    }
}

void ModelTask_StopThread(tModelTask *env) {
    if (env->thread.id) {
        osThreadSuspend(env->thread.id);
    }
}