#include "stm32f0xx_hal.h"
#include "states.h"

// PID implementation
// TODO: Make struct that has the last_temp and i_state in it, pass by ref. Make struct that has other input values maybe.
static int16_t last_pid_temp = 0;
static uint8_t last_pid_temp_frac = 0;
static int32_t i_state = 0;

int16_t pid_update(uint16_t k_p, uint16_t k_i, uint16_t k_d, int16_t temp, uint8_t temp_frac, int16_t setpoint, therm_settings_t* set, therm_status_t* status) 
{
  // Calculate instantaneous error
  int16_t error = setpoint - temp; // TODO: Use fixed point fraction

  // Proportional component
  int32_t p_term = k_p * error;

  // Error accumulator (integrator)
  i_state += error;

  // to prevent the iTerm getting huge from lots of 
  //  error, we use a "windup guard" 
  // (this happens when the machine is first turned on and
  // it cant help be cold despite its best efforts)
  // not necessary, but this makes windup guard values 
  // relative to the current iGain
  int32_t windup_guard_res = set->val.windup_guard / k_i;  

  // Calculate integral term with windup guard 
  if (i_state > windup_guard_res) 
    i_state = windup_guard_res;
  else if (i_state < -windup_guard_res) 
    i_state = -windup_guard_res;

  int32_t i_term = k_i * i_state;

  // Calculate differential term (slope since last iteration)
  int32_t d_term = (k_d * (status->temp - last_pid_temp));

  // Save temperature for next iteration
  last_pid_temp = status->temp;
  last_pid_temp_frac = status->temp_frac;

  int16_t result = p_term + i_term - d_term;

  // Put out tenths of percent, 0-1000. 
  if(result > 1000)
    result = 1000;
  else if(result < -1000)
    result = -1000;

  // Return feedback
  return result;
}