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.commands.swerve.driveAligned;
007
008import static edu.wpi.first.units.Units.*;
009
010import java.util.function.BooleanSupplier;
011import java.util.function.Supplier;
012
013import com.ctre.phoenix6.swerve.SwerveRequest;
014import com.stuypulse.robot.constants.Gains.Swerve.Alignment;
015import com.stuypulse.robot.constants.Settings;
016import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
017import com.stuypulse.robot.util.swerve.AlignmentUtil;
018
019import edu.wpi.first.math.filter.Debouncer;
020import edu.wpi.first.math.filter.Debouncer.DebounceType;
021import edu.wpi.first.math.geometry.Pose2d;
022import edu.wpi.first.math.geometry.Rotation2d;
023import edu.wpi.first.wpilibj2.command.Command;
024
025public class SwerveDriveSetAlignment extends Command {
026
027    protected static final CommandSwerveDrivetrain swerve;
028
029    protected final BooleanSupplier isAligned;
030    protected final Debouncer alignmentDebouncer;
031
032    private Supplier<Pose2d> pose;
033
034    protected SwerveDriveSetAlignment(Supplier<Pose2d> pose) {
035        this.isAligned = () -> Math.abs(swerve.getPose().getRotation().minus(getTargetAngle())
036                .getDegrees()) < Settings.Swerve.Alignment.Tolerances.THETA_TOLERANCE.getDegrees();
037        this.alignmentDebouncer = new Debouncer(Settings.Swerve.Alignment.Tolerances.ALIGNMENT_DEBOUNCE.in(Seconds), DebounceType.kBoth);
038        this.pose = pose;
039        addRequirements(swerve);
040    }
041
042    static {
043        swerve = CommandSwerveDrivetrain.getInstance();
044    }
045
046    public Rotation2d getTargetAngle() {
047        return AlignmentUtil.getTargetAlignmentAngle(swerve.getPose(), pose.get());
048    }
049
050    @Override
051    public boolean isFinished() {
052        return alignmentDebouncer.calculate(isAligned.getAsBoolean());
053    }
054
055    @Override
056    public void execute() {
057        SwerveRequest request = new SwerveRequest.FieldCentricFacingAngle()
058                .withTargetDirection(getTargetAngle())
059                .withVelocityX(0)
060                .withVelocityY(0)
061                .withHeadingPID(Alignment.akP, Alignment.akI, Alignment.akD);
062        swerve.setControl(request);
063    }
064}