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