//
// 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];
		snprintf(tempstr, 6, "%g", ssr_output);
		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;
}