The FollowPathCommand

package org.firstinspires.ftc.teamcode.commands;
 
import android.util.Log;
import com.arcrobotics.ftclib.command.CommandBase;
import com.pedropathing.follower.Follower;
import com.pedropathing.paths.PathChain;
import com.qualcomm.robotcore.util.ElapsedTime;
 
public class FollowPathCommand extends CommandBase {
    private final Follower follower;
    private final PathChain path;
    private final boolean holdEnd;
 
    // Stall detection variables
    private ElapsedTime stallTimer = new ElapsedTime();
    private final ElapsedTime initializationTimer = new ElapsedTime();
    private static final double STALL_VELOCITY_THRESHOLD = 0.5; // inches per second
    private static final double STALL_TIMEOUT = 500; // milliseconds before giving up
    private static final double END_TOLERANCE = 3.0; // Finish when 3 inches away
    private static final double MINIMUM_RUN_TIME = 500.0; // Ensure at least 500ms of run time
 
    private boolean initialized = false;
 
    public FollowPathCommand(Follower follower, PathChain path, boolean holdEnd) {
        this.follower = follower;
        this.path = path;
        this.holdEnd = holdEnd;
        this.initialized = false;
    }
 
    @Override
    public void initialize() {
        // 1. Tell Pedro to start driving!
        follower.followPath(path, holdEnd);
 
        // 2. Force an update immediately so the state locks in
        follower.update();
    }
 
    @Override
    public void execute() {
        if (!initialized) {
            initializationTimer.reset();
            stallTimer.reset();
            initialized = true;
        }
 
        double currentVelocity = follower.getVelocity().getMagnitude();
 
        // 3. Stall Detection: If we are moving faster than the threshold, reset the timer
        if (currentVelocity > STALL_VELOCITY_THRESHOLD) {
            stallTimer.reset();
        }
    }
 
    @Override
    public boolean isFinished() {
        // SAFETY: Do not allow the command to finish if it hasn't been running for 500ms.
        // This prevents race conditions where isBusy() is checked before the path latches.
        if (initializationTimer.milliseconds() < MINIMUM_RUN_TIME) {
            return false;
        }
 
        // 4. Finish Logic
        double distanceRemaining = follower.getCurrentPath().getDistanceRemaining();
        boolean closeEnough = distanceRemaining < END_TOLERANCE;
 
        // Finish if Pedro says done, OR we are close enough, OR we stalled (hit a wall)
        return !follower.isBusy() || closeEnough || stallTimer.milliseconds() > STALL_TIMEOUT;
    }
 
    @Override
    public void end(boolean interrupted) {
        if (stallTimer.milliseconds() > STALL_TIMEOUT) {
            Log.w("Follow Path Command", "Path Stalled! Moving to next command.");
            follower.breakFollowing(); // Stop the motors immediately
        }
        initialized = false;
    }
}