#include "main.h"
#include "pid_controller.h"
#include "control_loop_hal.h"

/* Rename these handles to match the CubeMX-generated project. */
extern TIM_HandleTypeDef htim_control;
extern TIM_HandleTypeDef htim_pwm;
extern TIM_HandleTypeDef htim_encoder;

static PID_Controller_t speed_pid;
static volatile uint8_t control_tick_pending;

void ControlLoop_HAL_Init(void)
{
    control_tick_pending = 0U;

    PID_Controller_Init(&speed_pid,
                        0.30f,
                        0.30f,
                        0.00f,
                        0.040f,
                        -100.0f,
                        100.0f,
                        100.0f);

    PID_Controller_Reset(&speed_pid, 0.0f);
    speed_pid.conditional_integration = 1U;

    HAL_TIM_Base_Start_IT(&htim_control);
    HAL_TIM_PWM_Start(&htim_pwm, TIM_CHANNEL_1);
    HAL_TIM_Encoder_Start(&htim_encoder, TIM_CHANNEL_ALL);
}

/* Keep the callback short; do not run the PID calculation here. */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    static uint8_t divider;

    if (htim == &htim_control)
    {
        divider++;
        if (divider >= 40U)
        {
            divider = 0U;
            if (control_tick_pending < 2U)
            {
                control_tick_pending++;
            }
        }
    }
}

static int16_t Encoder_ReadDelta(void)
{
    static int16_t previous_count;
    int16_t current_count = (int16_t)__HAL_TIM_GET_COUNTER(&htim_encoder);
    int16_t delta = (int16_t)(current_count - previous_count);

    previous_count = current_count;
    return delta;
}

static void Motor_SetSignedPwm(float command)
{
    float magnitude = command;

    if (magnitude >= 0.0f)
    {
        HAL_GPIO_WritePin(MOTOR_IN1_GPIO_Port, MOTOR_IN1_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(MOTOR_IN2_GPIO_Port, MOTOR_IN2_Pin, GPIO_PIN_SET);
    }
    else
    {
        magnitude = -magnitude;
        HAL_GPIO_WritePin(MOTOR_IN1_GPIO_Port, MOTOR_IN1_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(MOTOR_IN2_GPIO_Port, MOTOR_IN2_Pin, GPIO_PIN_RESET);
    }

    if (magnitude > 100.0f)
    {
        magnitude = 100.0f;
    }
    __HAL_TIM_SET_COMPARE(&htim_pwm, TIM_CHANNEL_1, (uint32_t)magnitude);
}

void ControlLoop_HAL_Run(void)
{
    int16_t speed_feedback;
    float output;

    if (control_tick_pending == 0U)
    {
        return;
    }

    __disable_irq();
    control_tick_pending--;
    __enable_irq();

    speed_feedback = Encoder_ReadDelta();
    output = PID_Controller_Update(&speed_pid,
                                   speed_pid.target,
                                   (float)speed_feedback);
    Motor_SetSignedPwm(output);
}

void ControlLoop_HAL_SetTarget(float target)
{
    speed_pid.target = target;
}

void ControlLoop_HAL_Stop(void)
{
    control_tick_pending = 0U;
    PID_Controller_Reset(&speed_pid, 0.0f);
    Motor_SetSignedPwm(0.0f);
}