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

#include "pid.h"

// Private variables
static pid_state_t state;


// Initialize PID loop
void pid_init()
{
    state.i_state = 0;
    state.last_pid_temp = 0;
    state.last_pid_temp_frac = 0;
}


// Apply PID output values
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_COOLER)
            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
        //TODO: move to display
        ssd1306_drawstring("      ", 0, 90); //fixme: this is bad, but I can't get the old digits to clear otherwise
        char tempstr[8];
        snprintf(tempstr, 8, "%4.1f%%", ssr_output/10.0);
        ssd1306_drawstring(tempstr, 0, 85);
    }
    else
    {
        ssr_output = 0.0;
    }

    return ssr_output; //ssr_output;=== START OF INFORMATION SECTION ===

}


// Calculate new PID values
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
    // EMZ FIXME: was regulating to 1 degree below the setpoint! +1 accomadates for this
    int16_t error = (status->setpoint+1) - temp;

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

    // Discard I if we've achieved the setpoint (TODO: add setting disable/enable)
    if(error <= 0)
        state.i_state = 0;

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