#include <stdio.h>
#include <string.h>
#include <math.h>
#include "TSS_Pid.h"

/**
  * @brief  TSS pid初始化
  * @param  q   传入pid结构体
  * @note   无
  */
void TSS_PidInit(TSS_PidDef *q)
{
    // Configure settings
    q->AntiWindup = ENABLED;
    q->Bumpless = ENABLED;

    // Set mode to auotmatic
    // (otherwise it will be in manual mode)
    q->Mode = AUTOMATIC;

    // 设置输出范围
    q->OutputMax = 1.0;
    q->OutputMin = -1.0;

    // 设置1秒内调节几次
    q->Ts = 0.005;

    // 设置调节比例
    q->b = 1.0;
    q->c = 1.0;

    // 设置调整常量
    q->K = 0.5;       //比例
    q->Ti = 1 / 0.02; //积分
    q->Td = 0.0;      //微分
    q->Nd = 4.0;      //微分器

    q->ctx.Ui_old = 0.0;
    q->ctx.Ud_old = 0.0;
    q->ctx.PV_old = 0.0;
    q->ctx.SP_old = 0.0;
}

/**
  * @brief  pid处理函数
  * @param  q      传入pid结构体
  * @param  Input  输入参数
  * @param  PV     传感器参数
  * @param  terms  保留参数,用于debug
  * @retval 输出参数
  * @note   可调用TSS_PidProcess的宏,简化调用
  */
float TSS_PidProcess_(TSS_PidDef *q, float Input, float PV, float terms[])
{
    // For local use
    float ControllerOutput;
    float Up, Ui, Ud;
    float Kp, Ki, Kd_a, Kd_b, Kd_c;

    if (fabsf(q->Ti) < EPSILON)
    {
        q->Ti = EPSILON;
    }
    if (fabsf(q->Nd) < EPSILON)
    {
        q->Nd = EPSILON;
    }
    Kp = q->K;
    Ki = ((q->K) * (q->Ts)) / (q->Ti);
    Kd_a = q->Td / (q->Td + q->Nd * q->Ts);
    Kd_b = (q->K * q->Td * q->Nd) / (q->Td + q->Nd * q->Ts);
    Kd_c = (q->c * q->K * q->Td * q->Nd) / (q->Td + q->Nd * q->Ts);

    // Proportional gain
    Up = Kp * (((q->b) * (Input)) - PV);

    // Deriative gain with filter
    Ud = Kd_a * (q->ctx.Ud_old) - Kd_b * (PV - q->ctx.PV_old) + Kd_c * (Input - q->ctx.SP_old);

    // Get last integral
    Ui = q->ctx.Ui_old;

    // Calculate controler output for Automatic or manual mode
    switch (q->Mode)
    {
    case MANUAL:
        ControllerOutput = Input;
        if (q->Bumpless == ENABLED)
        {
            q->ctx.Ui_old = PV;
        }
        break;
    case AUTOMATIC:
        ControllerOutput = Up + Ui + Ud;
        if (ControllerOutput > q->OutputMax)
        {
            ControllerOutput = q->OutputMax;
        }
        else if (ControllerOutput < q->OutputMin)
        {
            ControllerOutput = q->OutputMin;
        }
        break;
    case RELAY:
        if ((Input - PV) >= 0)
        {
            ControllerOutput = q->OutputMax;
        }
        else
        {
            ControllerOutput = q->OutputMin;
        }
        break;
    case OFF:
        ControllerOutput = 0;
        break;
    default:
        // ERROR
        ControllerOutput = NAN;
        break;
    }

    // Output parameters for debug
    if (terms != NULL)
    {
        terms[0] = Up;
        terms[1] = Ui;
        terms[2] = Ud;
    }

    // Anti Windup
    if ((q->AntiWindup == ENABLED) && (q->Mode == AUTOMATIC))
    {
        //if (Ui >= q->OutputMax) {
        if (ControllerOutput >= q->OutputMax)
        {
            // do not integrate anymore
        }
        else if (ControllerOutput <= q->OutputMin)
        {
            // do not integrate anymore
        }
        else
        {
            Ui = q->ctx.Ui_old + Ki * ((Input)-PV);
        }
    }
    else
    {
        // Calc de integral for the next step in any other case
        Ui = q->ctx.Ui_old + Ki * ((Input)-PV);
    }

    // Save context for next step.
    q->ctx.Ui_old = Ui;
    q->ctx.Ud_old = Ud;
    q->ctx.PV_old = PV;
    q->ctx.SP_old = Input;

    return ControllerOutput;
}