#include "pid.hpp"
#include <algorithm>
#include <cmath>

PID::PID(float kp, float ki, float kd, float target,
         PIDMode mode, float out_limit, float int_limit)
    : _kp(kp), _ki(ki), _kd(kd), _target(target), _mode(mode)
    , _out_limit(out_limit), _int_limit(int_limit)
    , _output(0), _integral(0), _prev_error(0), _last_measured(0), _first(true) {}

void PID::setPID(float kp, float ki, float kd)
{
    _kp = kp; _ki = ki; _kd = kd;
    reset();
}

void PID::reset()
{
    _integral = 0;
    _prev_error = 0;
    _output = 0;
    _first = true;
}

float PID::update(float measured)
{
    float error = _target - measured;

    if (_mode == PID_POSITIONAL)
    {
        _integral += error;
        _integral = std::clamp(_integral, -_int_limit, _int_limit);

        float derivative = 0;
        if (!_first) derivative = error - _prev_error;

        _output = _kp * error + _ki * _integral + _kd * derivative;
    }
    else // PID_INCREMENTAL
    {
        float inc = _kp * (error - _prev_error) + _ki * error;
        if (!_first)
            inc += _kd * (error - 2.0f * _prev_error + (_prev_error - (_target - _last_measured)));
        _output += inc;
    }

    _output = std::clamp(_output, -_out_limit, _out_limit);
    _prev_error = error;
    _last_measured = measured;
    _first = false;

    return _output;
}