//
// PID: proportional/integral/derivative controller
//

#include "pid.h"
#include "flash.h"

// PID implementation

static pid_state_t state;


void pid_init()
{
	state.i_state = 0;
	state.last_pid_temp = 0;
	state.last_pid_temp_frac = 0;
}

float pid_process(void)
{

//            #ifdef MAX31865_RTD_SENSOR
//            max31865_readtemp(spi_get(), &set, &status);
//			#else
//			max31855_readtemp(spi_get(), &set, &status); // Read MAX31855
//			#endif

		float ssr_output = 0;


		if(runtime_status()->pid_enabled)
		{
			// Get ssr output for next time
			int16_t power_percent = pid_update();

			if(flash_getsettings()->val.plant_type == PLANT_HEATER)
				power_percent *= -1;

			//power-percent is 0-1000?
			ssr_output = power_percent; //(((uint32_t)SSR_PERIOD * (uint32_t)10 * (uint32_t)100) * power_percent) / (uint32_t)1000000;

			// put ssr output on display
			ssd1306_drawstring("      ", 0, 90); //fixme: this is bad, but I can't get the old digits to clear otherwise
			char tempstr[6];
			itoa(ssr_output, tempstr, 10);
			ssd1306_drawstring(tempstr, 0, 90);
		}
		else
		{
			ssr_output = 0.0;
		}

		return ssr_output;
}

int16_t pid_update(void)
{
	therm_status_t* status = runtime_status();
	therm_settings_t* set = flash_getsettings();

  // Convert temperature to fixed point number with 1/10th resolution
  float temp = status->temp;

  // 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;
}