#include <stdio.h>
#include "main.h"
#include "FreeRTOS.h"
#include "task.h"
#include "app_temp.h"
#include "app_pid.h"
#include "app_heater.h"
#include "cmsis_os2.h"
#include "tim.h"
#include "queue.h"
#include "app_gui.h"
#include "app_safety.h"

extern osMutexId_t heaterMutexHandle;

static void heater_pwm_init()
{
    LL_TIM_OC_SetCompareCH1(TIM_HEATER, 0); // Off
    LL_TIM_EnableCounter(TIM_HEATER);
    LL_TIM_CC_EnableChannel(TIM_HEATER, LL_TIM_CHANNEL_CH1);
}

static void heater_pwm_set_perc(float perc)
{
    uint16_t perc_u = (uint16_t) perc;
    if (perc_u > 100) {
        perc_u = 100;
    }

    // (TIM3->ARR / 100)
    LL_TIM_OC_SetCompareCH1(TIM_HEATER, 640 * perc_u);
//    TIM3->CCR1 = 640 * perc_u;
}

static struct {
  float oven_temp;
  float soc_temp;
  // these will be loaded from flash and stored back
  float tuning_p;
  float tuning_i;
  float tuning_d;
  // PID state
  struct PID pid;
} state = {
        .tuning_p = 10.0f,
        .tuning_i = 0.052f,
        .tuning_d = 100.0f,
        .pid = {
                .SampleTimeTicks = pdMS_TO_TICKS(1000),
                .outMax = 100.0f,
                .outMin = 0.0f,
                .ctlMode = PID_MANUAL,
                .controllerDirection = PID_DIRECT,
        },
};

static inline void heaterEnterCritical() {
    osMutexAcquire(heaterMutexHandle, portMAX_DELAY);
}

static inline void heaterExitCritical() {
    osMutexRelease(heaterMutexHandle);
}

void app_heater_set_tuning(float p, float i, float d) {
    heaterEnterCritical();
    PID_SetTunings(&state.pid, p, i, d);
    heaterExitCritical();
}

void app_heater_enable(bool enable) {
    PRINTF("Set heater enabled = %d\r\n", (int) enable);
    heaterEnterCritical();
    PID_SetCtlMode(&state.pid, enable ? PID_AUTOMATIC : PID_MANUAL);
    heaterExitCritical();
}

void app_heater_set_target(float target) {
    PRINTF("Set heater target = %d\r\n", (int) target);
    heaterEnterCritical();
    PID_SetSetpoint(&state.pid, target);
    heaterExitCritical();
}

// emergency shutdown, this must not block use RTOS since it can be called from fault handlers or interrupt
void app_heater_emergency_shutdown() {
    // Stop pwm
    LL_TIM_OC_SetCompareCH1(TIM_HEATER, 0);
    LL_TIM_CC_DisableChannel(TIM_HEATER, LL_TIM_CHANNEL_CH1);
    LL_TIM_DisableCounter(TIM_HEATER);

    // Also kill the GPIO PWM output
    LL_GPIO_InitTypeDef GPIO_InitStruct = {};
    GPIO_InitStruct.Pin = PWM_HEATER_Pin;
    GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
    GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
    GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
    LL_GPIO_Init(PWM_HEATER_GPIO_Port, &GPIO_InitStruct);

    // Output zero
    LL_GPIO_ResetOutputPin(PWM_HEATER_GPIO_Port, PWM_HEATER_Pin);
}

void app_task_heater(void *argument)
{
    // Wait until inited
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
    PUTS("Heater task starts\r\n");

    heater_pwm_init();

    heaterEnterCritical();
    // TODO load from flash
    PID_SetTunings(&state.pid, state.tuning_p, state.tuning_i, state.tuning_d);
    PID_Initialize(&state.pid);
    heaterExitCritical();

    uint32_t wake_time = xTaskGetTickCount();
    while (1) {
        app_temp_sample();

        state.oven_temp = app_temp_read_oven();
        state.soc_temp = app_temp_read_soc();

        enum GuiNotify ev = GUI_NOTIFY_TEMP_CHANGE;
        xQueueSend(guiEventQueHandle, &ev, pdMS_TO_TICKS(100));

        heaterEnterCritical();
        app_safety_pass_reg_loop_running();
        PID_Compute(&state.pid, state.oven_temp);
        if (state.pid.ctlMode == PID_AUTOMATIC) {
            PRINTF("temp %d, output %d\r\n", (int) state.oven_temp, (int) state.pid.Output);
            heater_pwm_set_perc(state.pid.Output);
        } else {
            // turn it off
            heater_pwm_set_perc(0);
        }
        heaterExitCritical();

        // TODO notify UI thread of the new temperature and heating percent

        vTaskDelayUntil(&wake_time, pdMS_TO_TICKS(100));
    }
}