#pragma once

#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

#include "robot.h"
#include "ins_task.h"
#include "motor_task.h"
#include "daemon.h"
#include "buzzer.h"

#include "bsp_log.h"

osThreadId_t insTaskHandle;
osThreadId_t robotTaskHandle;
osThreadId_t motorTaskHandle;
osThreadId_t daemonTaskHandle;

void StartINSTASK(void *argument);
void StartMOTORTASK(void *argument);
void StartDAEMONTASK(void *argument);
void StartROBOTTASK(void *argument);

void OSTaskInit()
{
    const osThreadAttr_t insTask_attr = {
        .name = "instask",
        .priority = osPriorityAboveNormal,
        .stack_size = 1024,
    };
    insTaskHandle = osThreadNew(StartINSTASK, NULL, &insTask_attr);

    const osThreadAttr_t motorTask_attr = {
        .name = "motortask",
        .priority = osPriorityNormal,
        .stack_size = 1024,
    };
    motorTaskHandle = osThreadNew(StartMOTORTASK, NULL, &motorTask_attr);

    const osThreadAttr_t daemonTask_attr = {
        .name = "daemontask",
        .priority = osPriorityNormal,
        .stack_size = 512,
    };
    daemonTaskHandle = osThreadNew(StartDAEMONTASK, NULL, &daemonTask_attr);

    const osThreadAttr_t robotTask_attr = {
        .name = "robottask",
        .priority = osPriorityNormal,
        .stack_size = 1024,
    };
    robotTaskHandle = osThreadNew(StartROBOTTASK, NULL, &robotTask_attr);
}

__attribute__((noreturn)) void StartINSTASK(void *argument)
{
    static float ins_start, ins_dt;
    INS_Init();
    LOGINFO("[freeRTOS] INS Task Start");
    for (;;)
    {
        ins_start = DWT_GetTimeline_ms();
        INS_Task();
        ins_dt = DWT_GetTimeline_ms() - ins_start;
        if (ins_dt > 1)
            LOGERROR("[freeRTOS] INS Task DELAY dt=%.3f", ins_dt);
        osDelay(1);
    }
}

__attribute__((noreturn)) void StartMOTORTASK(void *argument)
{
    static float motor_start, motor_dt;
    LOGINFO("[freeRTOS] MOTOR Task Start");
    for (;;)
    {
        motor_start = DWT_GetTimeline_ms();
        MotorControlTask();
        motor_dt = DWT_GetTimeline_ms() - motor_start;
        if (motor_dt > 1)
            LOGERROR("[freeRTOS] MOTOR Task DELAY dt=%.3f", motor_dt);
        osDelay(1);
    }
}

__attribute__((noreturn)) void StartDAEMONTASK(void *argument)
{
    static float daemon_start, daemon_dt;
    BuzzerInit();
    LOGINFO("[freeRTOS] Daemon Task Start");
    for (;;)
    {
        daemon_start = DWT_GetTimeline_ms();
        DaemonTask();
        BuzzerTask();
        daemon_dt = DWT_GetTimeline_ms() - daemon_start;
        if (daemon_dt > 10)
            LOGERROR("[freeRTOS] Daemon Task DELAY dt=%.3f", daemon_dt);
        osDelay(10);
    }
}

__attribute__((noreturn)) void StartROBOTTASK(void *argument)
{
    static float robot_start, robot_dt;
    LOGINFO("[freeRTOS] ROBOT core Task Start");
    for (;;)
    {
        robot_start = DWT_GetTimeline_ms();
        RobotTask();
        robot_dt = DWT_GetTimeline_ms() - robot_start;
        if (robot_dt > 5)
            LOGERROR("[freeRTOS] ROBOT core Task DELAY dt=%.3f", robot_dt);
        osDelay(ROBOT_CMD_DELAY);
    }
}