001/************************ PROJECT ZIXI ************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved.*/
003/* This work is licensed under the terms of the MIT license.  */
004/**************************************************************/
005package com.stuypulse.robot.util.swerve;
006
007import static org.wpilib.units.Units.RadiansPerSecond;
008import static org.wpilib.units.Units.Seconds;
009
010import com.stuypulse.robot.constants.GlobalSettings;
011
012import org.wpilib.command3.button.CommandGamepad;
013import org.wpilib.math.filter.LinearFilter;
014import org.wpilib.math.util.MathUtil;
015import org.wpilib.units.measure.AngularVelocity;
016
017/**
018 *
019 *
020 * <h2>DriveTurnInputProcessor</h2>
021 *
022 * <p>Class for processing driver turn input for the drivetrain. It's intended to replace StuyLib's
023 * IStream and Filters and use pure WPILib and Java.
024 *
025 * <p>This will be used to be as close as the original StuyLib implementation as possible It will be
026 * used for
027 *
028 * <ul>
029 *   <li>Applying deadbands
030 *   <li>Applying power curves
031 *   <li>Applying max angular velocity
032 *   <li>Applying a low pass filter
033 * </ul>
034 *
035 * <p>To use within a command, create an instance of this class and call the {@link #update()}
036 * method in the start of the Command.
037 *
038 * <p>To get the processed speed, call the {@link #get()} method.
039 *
040 * @author Faizaan (https://github.com/Faizaan-J)
041 */
042public class DriveTurnInputProcessor {
043    private final CommandGamepad controller;
044    private final double deadband;
045    private final double power;
046
047    /** The max angular velocity in radians per second */
048    private final AngularVelocity maxAngularVelocity;
049
050    private final LinearFilter lowPassFilter;
051
052    /** The processed angular velocity after the full processing and filtering. */
053    private double processedAngularVelocity;
054
055    /**
056     *
057     *
058     * <h4>Constructor for the DriveInputProcessor</h4>
059     *
060     * <p>Creates a new DriveInputProcessor with the specified parameters processes the input
061     * periodically.
062     *
063     * @param controller The CommandXboxController to get driver input from
064     * @param deadband The deadband to apply to the input (0-1)
065     * @param power The power to apply to the input
066     * @param maxAngularVelocity The maximum angular velocity to scale the input to (radians/s)
067     * @param rc The time constant for the low pass filter (seconds)
068     */
069    public DriveTurnInputProcessor(
070            CommandGamepad controller,
071            double deadband,
072            double power,
073            AngularVelocity maxAngularVelocity,
074            double rc) {
075        this.controller = controller;
076        this.deadband = deadband;
077        this.power = power;
078        this.maxAngularVelocity = maxAngularVelocity;
079
080        this.lowPassFilter = LinearFilter.singlePoleIIR(rc, GlobalSettings.DT.in(Seconds));
081
082        this.processedAngularVelocity = 0;
083    }
084
085    /**
086     * Read the raw joystick axis value for the right X axis and store it in {@link
087     * #processedAngularVelocity}
088     *
089     * @return This instance of the class
090     */
091    private DriveTurnInputProcessor getRightX() {
092        this.processedAngularVelocity = controller.getRightX();
093        return this;
094    }
095
096    /**
097     * Applies a deadband to the current {@link #processedAngularVelocity} value.
098     *
099     * @return This instance of the class
100     */
101    private DriveTurnInputProcessor applyDeadband() {
102        this.processedAngularVelocity =
103                MathUtil.applyDeadband(this.processedAngularVelocity, deadband);
104        return this;
105    }
106
107    /**
108     * Apply a power curve to the current {@link #processedAngularVelocity} value.
109     *
110     * @return This instance of the class
111     */
112    private DriveTurnInputProcessor applyPowerCurve() {
113        this.processedAngularVelocity =
114                Math.pow(Math.abs(this.processedAngularVelocity), power)
115                        * Math.signum(this.processedAngularVelocity);
116        return this;
117    }
118
119    /**
120     * Scale the {@link #processedAngularVelocity} value to the robot's maximum velocity.
121     *
122     * @return This instance of the class
123     */
124    private DriveTurnInputProcessor applyScalingToMaxAngularVelocity() {
125        this.processedAngularVelocity =
126                this.processedAngularVelocity * maxAngularVelocity.in(RadiansPerSecond);
127        return this;
128    }
129
130    /**
131     * Applies a low pass filter to {@link #processedAngularVelocity} using a {@link LinearFilter}.
132     * This smooths out the input and reduces noise.
133     *
134     * @return This instance of the class
135     */
136    private DriveTurnInputProcessor applyLowPassFilter() {
137        this.processedAngularVelocity = lowPassFilter.calculate(this.processedAngularVelocity);
138        return this;
139    }
140
141    /**
142     * Update method that processes all filters and updates the filtered angular velocity. This
143     * should be called once per cycle in the Command using this processor, before retrieving the
144     * processed angular velociy via {@link #get()}.
145     */
146    public void update() {
147        getRightX()
148                .applyDeadband()
149                .applyPowerCurve()
150                .applyScalingToMaxAngularVelocity()
151                .applyLowPassFilter();
152    }
153
154    /**
155     * Get the processed angular velocity
156     *
157     * @return The processed angular velocity
158     */
159    public AngularVelocity get() {
160        return RadiansPerSecond.of(processedAngularVelocity);
161    }
162}