/*
 * pid_int.c
 *
 * PID operation.
 *
 *  Created on: 2023/09/14
 *      Author: BestTechnology
 */

#include <stdint.h>

#include "pid.h"

void PIDReset_int (PPIDParam_int param) {
    param->err[0] = param->err[1] = param->integral = 0;
}

int32_t PIDUpdate_int (PPIDParam_int param, int32_t feedback, int32_t target) {
    int32_t p_term, i_term, d_term, integ, result;
    param->err[0] = param->err[1];
    param->err[1] = feedback - target;
    integ = param->integral + ((param->err[1] + param->err[0]) * param->delta_t) / 2;
    p_term = param->Kp * param->err[1];
    i_term = param->Ki * param->integral;
    d_term = param->Kd * (param->err[1] - param->err[0]) / param->delta_t;
    result = p_term + i_term + d_term;
    // Reset wind-up measure
    if ((param->max == 0) && (param->min == 0)) param->integral = integ;
    else {
        if (result >= param->max) return param->max;
        else if (result <= param->min) return param->min;
        else param->integral = integ;
    }
    return result;
}

void PIDReset_real (PPIDParam_real param) {
    param->err[0] = param->err[1] = param->integral = 0;
}

double PIDUpdate_real (PPIDParam_real param, double feedback, double target) {
    double p_term, i_term, d_term, integ, result;
    param->err[0] = param->err[1];
    param->err[1] = feedback - target;
    integ = param->integral + (param->err[1] + param->err[0]) * 0.5 * param->delta_t;
    p_term = param->Kp * param->err[1];
    i_term = param->Ki * param->integral;
    d_term = param->Kd * (param->err[1] - param->err[0]) / param->delta_t;
    result = p_term + i_term + d_term;
    // Reset wind-up measure
//    if ((param->max == 0) && (param->min == 0)) param->integral = integ;
//    else {
        if (result >= param->max) return param->max;
        else if (result <= param->min) return param->min;
        else param->integral = integ;
//    }
    return result;
}
