PIDController.h

#pragma once
#include <frc/controller/PIDController.h>
#include <valkyrie/Robot.h>
#include <valkyrie/controllers/PIDF.h>
 
namespace valor {

template <typename INPUT, typename OUTPUT>
class PIDController : frc::PIDController {
   public:
    using ErrorDerivative = units::unit_t<units::compound_unit<INPUT, units::inverse<units::second>>>;

    constexpr PIDController(const PIDF<INPUT, OUTPUT>& pidf, units::second_t period = 20_ms)
          : frc::PIDController{pidf.P.value(), pidf.I.value(), pidf.D.value(), period}, pidf{pidf} {
        SetTolerance(pidf.error, GetErrorDerivativeTolerance());
    }

    constexpr void SetPID(const PIDF<INPUT, OUTPUT>& pidf) {
        this->pidf = pidf;
        frc::PIDController::SetPID(pidf.P.value(), pidf.I.value(), pidf.D.value());
        SetTolerance(pidf.error, GetErrorDerivativeTolerance());
    }

    constexpr void SetIZone(PIDF<INPUT, OUTPUT>::Output_t iZone) { frc::PIDController::SetIZone(iZone.value()); }

    constexpr PIDF<INPUT, OUTPUT>::Output_t GetIZone() const {
        return typename PIDF<INPUT, OUTPUT>::Output_t{frc::PIDController::GetIZone()};
    }

    constexpr PIDF<INPUT, OUTPUT>::Input_t GetErrorTolerance() const {
        return typename PIDF<INPUT, OUTPUT>::Input_t{frc::PIDController::GetErrorTolerance()};
    }

    constexpr ErrorDerivative GetErrorDerivativeTolerance() const {
        return ErrorDerivative{frc::PIDController::GetErrorDerivativeTolerance()};
    }

    constexpr PIDF<INPUT, OUTPUT>::Input_t GetAccumulatedError() const {
        return typename PIDF<INPUT, OUTPUT>::Input_t{frc::PIDController::GetAccumulatedError()};
    }

    constexpr void SetSetpoint(PIDF<INPUT, OUTPUT>::Input_t setpoint) { frc::PIDController::SetSetpoint(setpoint.value()); }

    constexpr PIDF<INPUT, OUTPUT>::Input_t GetSetpoint() const {
        return typename PIDF<INPUT, OUTPUT>::Input_t{frc::PIDController::GetSetpoint()};
    }

    constexpr void EnableContinuousInput(PIDF<INPUT, OUTPUT>::Input_t minimumInput, PIDF<INPUT, OUTPUT>::Input_t maximumInput) {
        frc::PIDController::EnableContinuousInput(minimumInput.value(), maximumInput.value());
    }

    constexpr void SetIntegratorRange(PIDF<INPUT, OUTPUT>::Output_t minimumIntegral, PIDF<INPUT, OUTPUT>::Output_t maximumIntegral) {
        frc::PIDController::SetIntegratorRange(minimumIntegral.value(), maximumIntegral.value());
    }

    constexpr void SetTolerance(PIDF<INPUT, OUTPUT>::Input_t errorTolerance,
                                ErrorDerivative errorDerivativeTolerance = ErrorDerivative{std::numeric_limits<double>::infinity()}) {
        pidf.error = errorTolerance;
        frc::PIDController::SetTolerance(errorTolerance.value(), errorDerivativeTolerance.value());
    }

    constexpr PIDF<INPUT, OUTPUT>::Input_t GetError() const {
        return typename PIDF<INPUT, OUTPUT>::Input_t{frc::PIDController::GetError()};
    }

    constexpr ErrorDerivative GetErrorDerivative() const { return ErrorDerivative{frc::PIDController::GetErrorDerivative()}; }

    constexpr const valor::PIDF<INPUT, OUTPUT>& GetPID() const { return pidf; }

    constexpr PIDF<INPUT, OUTPUT>::Output_t Calculate(PIDF<INPUT, OUTPUT>::Input_t measurement) {
        typename PIDF<INPUT, OUTPUT>::Output_t output{frc::PIDController::Calculate(measurement.value())};
        if constexpr (std::is_same_v<decltype(measurement), typename PIDF<INPUT, OUTPUT>::Velocity_t>)
            if (pidf.kV)
                output += *pidf.kV * GetSetpoint();
        output += pidf.kS;
        if (pidf.GType == GravityFFType::LINEAR)
            output += pidf.kG;
        else if constexpr (std::is_convertible_v<decltype(measurement), units::turn_t>)
            if (pidf.GType == GravityFFType::CIRCULAR)
                output += pidf.kG * units::math::cos(measurement + pidf.GTarget);
        return output;
    }
 
    using frc::PIDController::AtSetpoint;
    using frc::PIDController::DisableContinuousInput;
    using frc::PIDController::GetPeriod;
    using frc::PIDController::IsContinuousInputEnabled;
    using frc::PIDController::Reset;
 
   private:
    PIDF<INPUT, OUTPUT> pidf;
};
 
}  // namespace valor