/*
 * Copyright (c) 2026 Martin Javier Klöckner <mklockner@fi.uba.ar>
 *
 * See file `LICENSE` for full details
 */

#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>

#include "main.h"
#include "app.h"
#include "task_temp_ctrl.h"
#include "logger.h"

#define G_TASK_TEMP_CTRL_TICK_CNT_INI  0ul

#define G_TASK_TEMP_CTRL_PID_KP 40.0f
#define G_TASK_TEMP_CTRL_PID_KI 0.75f
#define G_TASK_TEMP_CTRL_PID_KD 0.01f
#define G_TASK_TEMP_CTRL_PID_DT 250 // 100 ms

// Private data definition
typedef struct {
    TIM_HandleTypeDef *htim;
    uint32_t tim_channel;
} task_temp_ctrl_cfg_t;

typedef struct {
    float pid_kp, pid_ki, pid_kd;
    float pid_p, pid_i, pid_d;
    float error, prev_error;
    uint32_t tick;
} task_temp_ctrl_pid_t;

static const task_temp_ctrl_cfg_t task_temp_ctrl_cfg = {&htim1, TIM_CHANNEL_1};
static task_temp_ctrl_pid_t task_temp_ctrl_pid = {
    G_TASK_TEMP_CTRL_PID_KP, G_TASK_TEMP_CTRL_PID_KI, G_TASK_TEMP_CTRL_PID_KD 
};

// Private functions declaration
static inline int16_t clampf(float val, float min, float max);
static void task_temp_ctrl_set_duty(TIM_HandleTypeDef *htim, uint32_t ch, uint8_t duty);

// Public data definition
volatile uint32_t g_task_temp_ctrl_tick_cnt; // Incremented by HAL_SysTick callback

// Public functions definition
void task_temp_ctrl_init(void *parameters)
{
    LOGGER_INFO("Initializing `task_temp_ctrl`...");

    const task_temp_ctrl_cfg_t *p_task_cfg = &task_temp_ctrl_cfg;
    shared_data_type *p_shared_data = (shared_data_type *)parameters;

    task_temp_ctrl_set_duty(p_task_cfg->htim,
            p_task_cfg->tim_channel,
            p_shared_data->temp_ctrl_pwm_dc);

    // __HAL_TIM_SET_COMPARE(p_task_cfg->htim,
    //         p_task_cfg->tim_channel,
    //         p_shared_data->temp_ctrl_pwm_dc);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);

    LOGGER_INFO("Done initializing `task_temp_ctrl`");
}

void task_temp_ctrl_update(void *parameters)
{
    bool b_time_update_required;
    shared_data_type *p_shared_data = (shared_data_type *)parameters;
    const task_temp_ctrl_cfg_t *p_task_cfg = &task_temp_ctrl_cfg;
    task_temp_ctrl_pid_t *p_pid = &task_temp_ctrl_pid;

    __asm("CPSID i");
    if (G_TASK_TEMP_CTRL_TICK_CNT_INI < g_task_temp_ctrl_tick_cnt)
    {
        g_task_temp_ctrl_tick_cnt--;
        b_time_update_required = true;
    }
    __asm("CPSIE i");

    while (b_time_update_required)
    {
        __asm("CPSID i");
        if (G_TASK_TEMP_CTRL_TICK_CNT_INI < g_task_temp_ctrl_tick_cnt)
        {
            g_task_temp_ctrl_tick_cnt--;
            b_time_update_required = true;
        }
        else
        {
            b_time_update_required = false;
        }
        __asm("CPSIE i");

        if (false == p_shared_data->temp_ctrl_enabled)
        {
            if (0 != __HAL_TIM_GET_COMPARE(p_task_cfg->htim, p_task_cfg->tim_channel))
            {
                task_temp_ctrl_set_duty(p_task_cfg->htim, p_task_cfg->tim_channel, 0);
            }
        }
        else
        {
            if (0 < p_pid->tick)
            {
                p_pid->tick--;
            }
            else
            {
                p_pid->tick = G_TASK_TEMP_CTRL_PID_DT;
                float dt = (p_pid->tick / 1000.0f);

                p_pid->prev_error = p_pid->error;
                p_pid->error = p_shared_data->temp_set_point - p_shared_data->temp_sensor;

                p_pid->pid_p = p_pid->error * p_pid->pid_kp;
                p_pid->pid_i = (p_pid->pid_i + (p_pid->error*dt)) * p_pid->pid_ki;
                p_pid->pid_d = ((p_pid->error - p_pid->prev_error)/dt) * p_pid->pid_kd;

                p_shared_data->temp_ctrl_pwm_dc = clampf(
                        p_pid->pid_p + p_pid->pid_i + p_pid->pid_d,
                        0.0f,
                        100.0f);

                task_temp_ctrl_set_duty(p_task_cfg->htim,
                        p_task_cfg->tim_channel,
                        p_shared_data->temp_ctrl_pwm_dc);
            }
        }
    }
}

// Public functions declaration
static inline int16_t clampf(float val, float min, float max)
{
    return (val > max) ? max : ((val < min) ? min : val);
}

static void task_temp_ctrl_set_duty(TIM_HandleTypeDef *htim, uint32_t ch, uint8_t duty)
{
    uint32_t ARR = __HAL_TIM_GET_AUTORELOAD(htim);     // Auto Reload Reg
    uint32_t CCR = (uint32_t)((ARR + 1) * duty / 100); // Capture/Compare Reg
    __HAL_TIM_SET_COMPARE(htim, ch, CCR);
}