#include "pid.h"

// PID implementation

void pid_init(pid_state_t* state)
{
	state->i_state = 0;
	state->last_pid_temp = 0;
	state->last_pid_temp_frac = 0;
}

int16_t pid_update(therm_settings_t* set, therm_status_t* status, pid_state_t *state)
{

  // Convert temperature to fixed point number with 1/10th resolution
  int8_t temp_frac = status->temp_frac > 9 ? status->temp_frac / 10 : status->temp_frac;
  temp_frac = status->temp > 0 ? temp_frac : temp_frac * -1;
  int32_t temp = (status->temp * 10) + temp_frac;

  // Calculate instantaneous error
  int16_t error = status->setpoint * 10 - temp; // TODO: Use fixed point fraction

  // Proportional component
  int32_t p_term = set->val.k_p * error;

  // Error accumulator (integrator)
  state->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 * 10) / set->val.k_i;

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

  int32_t i_term = set->val.k_i * state->i_state;

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

  // Save temperature for next iteration
  state->last_pid_temp = temp;

  int16_t result = (p_term + i_term - d_term) / 10;

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

  // Return feedback
  return result;
}