math/controls/bang_bang.h

#pragma once
 
#include <cmath>
 
namespace core {

class BangBang {
   public:
    BangBang(double tolerance, double setpoint)
        : tolerance_(tolerance), setpoint_(setpoint) {}
 
    BangBang(const BangBang&) = default;
    BangBang(BangBang&&) = default;
    BangBang& operator=(const BangBang&) = default;
    BangBang& operator=(BangBang&&) = default;

    double calculate(double measurement, double setpoint) {
        measurement_ = measurement;
        setpoint_ = setpoint;
 
        return measurement < setpoint ? 1 : 0;
    }

    double calculate(double measurement) {
        return calculate(measurement, setpoint_);
    }

    bool at_setpoint() {
        return std::abs(setpoint_ - measurement_) < tolerance_;
    }
 
    double tolerance() const { return tolerance_; }
    double setpoint() const { return setpoint_; }
 
    void set_tolerance(double tolerance) { tolerance_ = tolerance; }
    void set_setpoint(double setpoint) { setpoint_ = setpoint; }
 
   private:
    double tolerance_;
    double setpoint_;
    double measurement_;
};
 
}  // namespace core