Gamepad.h

#pragma once
 
#include <frc/XboxController.h>
#include <frc/filter/LinearFilter.h>
#include <units/angle.h>
 
#include "valkyrie/sensors/BaseSensor.h"
 
namespace valor {

class Gamepad : public frc::XboxController {
   public:
    class AnalogValue : public sensors::BaseSensor<double> {
       public:
        void SetGetter(std::function<double()> getter);

        inline void Reset() override { filter.Reset(); }

        inline bool Active() const { return Get() != 0; }

        inline AnalogValue& SetDeadband(double deadband) {
            this->deadband = deadband;
            return *this;
        }

        inline AnalogValue& SetRange(double min, double max = 1) {
            minValue = min;
            maxValue = max;
            return *this;
        }

        inline double GetDeadband() const { return deadband; }

        inline double GetMaxValue() const { return maxValue; }

        inline double GetMinValue() const { return minValue; }

        inline AnalogValue& SetPolynomial(int poly) {
            polynomial = poly;
            return *this;
        }

        inline int GetPolynomial() const { return polynomial; }

        inline void Calculate() override { filter.Calculate(Get()); }

        inline double GetFiltered() const { return filter.LastValue(); }

        inline AnalogValue& SetFilterSize(int size) {
            filter = frc::LinearFilter<double>::MovingAverage(size);
            return *this;
        }

        inline AnalogValue& SetFilterTime(units::second_t time) { return SetFilterSize(time / LOOP_PERIOD); }
 
       private:
        double deadband = 0;

        double minValue = 0;

        double maxValue = 1;

        int polynomial = 1;

        frc::LinearFilter<double> filter = frc::LinearFilter<double>::MovingAverage(1);
    };

    explicit Gamepad(int id);

    inline bool DPadUp() const { return GetPOV() == 0; }

    inline bool DPadDown() const { return GetPOV() == 180; }

    inline bool DPadLeft() const { return GetPOV() == 270; }

    inline bool DPadRight() const { return GetPOV() == 90; }

    void SetRumble(bool turnOn, double intensity = .25);

    constexpr AnalogValue& LeftStickX() { return leftStickX; }

    constexpr const AnalogValue& LeftStickX() const { return leftStickX; }

    constexpr AnalogValue& LeftStickY() { return leftStickY; }

    constexpr const AnalogValue& LeftStickY() const { return leftStickY; }

    constexpr AnalogValue& RightStickX() { return rightStickX; }

    constexpr const AnalogValue& RightStickX() const { return rightStickX; }

    constexpr AnalogValue& RightStickY() { return rightStickY; }

    constexpr const AnalogValue& RightStickY() const { return rightStickY; }

    constexpr AnalogValue& LeftTrigger() { return leftTrigger; }

    constexpr const AnalogValue& LeftTrigger() const { return leftTrigger; }

    constexpr AnalogValue& RightTrigger() { return rightTrigger; }

    constexpr const AnalogValue& RightTrigger() const { return rightTrigger; }
 
   private:
    AnalogValue leftStickX;

    AnalogValue leftStickY;

    AnalogValue rightStickX;

    AnalogValue rightStickY;

    AnalogValue leftTrigger;

    AnalogValue rightTrigger;
};
}  // namespace valor