#include "stm32f10x.h"
#include "Encoder.h"
#include "Motor.h"
#include "PID.h"
#include "ControlLoop_SPL.h"

static PID_t speed_pid;
static volatile uint8_t control_tick_pending;
static int16_t previous_location;

void ControlLoop_Init(void)
{
    control_tick_pending = 0U;
    previous_location = Encoder_GetLocation(1);

    /* TIM1 is configured for a 1 ms update interrupt in Timer.c. */
    PID_Init(&speed_pid,
             0.30f,
             0.30f,
             0.00f,
             0.040f,
             -100.0f,
             100.0f,
             100.0f);
    PID_Reset(&speed_pid, 0.0f);
    speed_pid.conditional_integration = 1U;
}

/* Call this from TIM1_UP_IRQHandler after clearing the update flag. */
void ControlLoop_On1msTick(void)
{
    static uint8_t divider;

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

/* Call this repeatedly from the main loop. */
void ControlLoop_Run(void)
{
    int16_t current_location;
    int16_t location_delta;
    float output;

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

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

    current_location = Encoder_GetLocation(1);
    location_delta = (int16_t)(current_location - previous_location);
    previous_location = current_location;

    /* The reference project uses encoder-count delta as its speed feedback. */
    output = PID_Update(&speed_pid, speed_pid.target, (float)location_delta);
    Motor_SetPWM(1, (int16_t)output);
}

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

void ControlLoop_Stop(void)
{
    control_tick_pending = 0U;
    previous_location = Encoder_GetLocation(1);
    PID_Reset(&speed_pid, 0.0f);
    Motor_SetPWM(1, 0);
}