#pragma once
#include "types.hpp"

class PID {
public:
    PID(float kp = 0, float ki = 0, float kd = 0, float target = 0,
        PIDMode mode = PID_INCREMENTAL, float out_limit = 100, float int_limit = 100);

    float update(float measured);
    void  setTarget(float t)    { _target = t; }
    void  setPID(float kp, float ki, float kd);
    void  setMode(PIDMode m)    { _mode = m; }
    void  reset();

    float output() const { return _output; }

private:
    float   _kp, _ki, _kd;
    float   _target;
    PIDMode _mode;
    float   _out_limit, _int_limit;

    float   _output;
    float   _integral;
    float   _prev_error;
    float   _last_measured;
    bool    _first;
};