#include "pid.h"
#include "board.h"
#include "print.h"

PID::PID(float *_input, float *_feedback, float _kp, float _ki, float _kd, unsigned short _max_output)
    : kp(_kp), ki(_ki), kd(_kd), max_output(_max_output * 1.0), input(_input), feedback(_feedback)
{
    Clear();
}

void PID::Clear()
{
    err = integra = derivative = preErr = 0;
}

void PID::Update(float _kp, float _ki, float _kd, unsigned short _maxOutput)
{
    kp = _kp;
    ki = _ki;
    kd = _kd;
    max_output = _maxOutput;
}

short PID::Compute(float interval)
{
    // 计算输入值和反馈值的差值
    err = *input - *feedback;
    // 计算积分误差
    integra = integra + err * interval;
    // 计算微分参数
    derivative = (err - preErr) / interval;
    // 更新误差
    preErr = err;

    if (ki != 0)
    {
#if PID_DEBUG_OUTPUT
        log("integra=%ld max_output=%ld %ld", long(integra * 1000), long(-(max_output / ki * 1000)), long(max_output / ki * 1000));
#endif
        if (integra < -(max_output / ki))
        {
#if PID_DEBUG_OUTPUT
            log("integra clear-");
#endif
            integra = -(max_output / ki);
        }
        if (integra > max_output / ki)
        {
#if PID_DEBUG_OUTPUT
            log("integra clear+");
#endif
            integra = max_output / ki;
        }
    }

    // 计算PID值
    float val = err * kp + integra * ki + derivative * kd;

    // 输出范围约束
    if (val < -max_output)
        val = -max_output + 1;
    else if (val > max_output)
        val = max_output - 1;

#if PID_DEBUG_OUTPUT
    log("error=%ld integra=%ld derivative=%ld val=%ld", long(error * 1000), long(integra * 1000), long(derivative * 1000), long(val * 1000));
#endif

    return val;
}