// In RobotConfig - set how close is "close enough"
public static FollowerConstants followerConstants = new FollowerConstants()
    .completionTolerances(
        2.0,                     // X tolerance: within 2 inches
        2.0,                     // Y tolerance: within 2 inches
        Math.toRadians(5)        // Heading tolerance: within 5°
    );

// Path completes when robot is within ALL three tolerances

// Standard path following loop
while (!follower.isPathComplete() && opModeIsActive()) {
    robot.update();
    follower.update();
    robot.setDrivePowers(follower.getMotorPowers());

    // Optional: monitor progress
    telemetry.addData("Path Progress", "%.1f%%", follower.getPathProgress() * 100);
    telemetry.addData("Distance to End", follower.getDistanceToEnd());
    telemetry.update();
}

// When loop exits, path is complete!
telemetry.addLine("✓ Path Complete");

Path pathWithEndAction = new BezierLine(new Point(48, 0, Point.CARTESIAN))
    .setConstantHeadingInterpolation(0)
    .setPathEndCallback(() -> {
        // Automatically called when path completes
        telemetry.addLine("Arrived at destination!");
        intake.setPower(0);
        arm.setTargetPosition(SCORE_POSITION);
    });

// No need to manually check completion for callbacks

// Add timeout safety to prevent getting stuck
ElapsedTime timer = new ElapsedTime();
double timeoutSeconds = 5.0;

follower.followPath(/* ... */);

while (!follower.isPathComplete() && opModeIsActive() && timer.seconds() < timeoutSeconds) {
    robot.update();
    follower.update();
    robot.setDrivePowers(follower.getMotorPowers());
}

if (timer.seconds() >= timeoutSeconds) {
    telemetry.addLine("⚠ Path timeout - continuing anyway");
    // Continue with autonomous (degraded but functional)
} else {
    telemetry.addLine("✓ Path completed successfully");
}

follower.followPath(
    path1,  // Completes when within tolerances
    path2,  // Then automatically starts
    path3   // Then this one
);

// Single loop waits for ALL paths to complete
while (!follower.isPathComplete() && opModeIsActive()) {
    robot.update();
    follower.update();
    robot.setDrivePowers(follower.getMotorPowers());

    // Check which segment is running
    int currentSegment = follower.getCurrentPathIndex();
    telemetry.addData("Path Segment", "%d / 3", currentSegment + 1);
}

// All three paths are now complete

// Stop path if condition met
while (!follower.isPathComplete() && opModeIsActive()) {
    robot.update();
    follower.update();

    // Emergency stop condition
    if (gamepad1.b || visionDetectsObstacle()) {
        follower.stopPath();  // Immediately halt
        break;
    }

    robot.setDrivePowers(follower.getMotorPowers());
}

// Resume with new path or handle abort
if (follower.isPathComplete()) {
    // Normal completion
} else {
    // Early termination - recovery logic
    telemetry.addLine("Path aborted - taking evasive action");
}

@Autonomous
public class RobustAutonomous extends LinearOpMode {
    Robot robot;
    Follower follower;
    ElapsedTime timer = new ElapsedTime();

    @Override
    public void runOpMode() {
        robot = new Robot(hardwareMap, RobotConfig.class);
        robot.initializeLocalizer(hardwareMap);
        follower = new Follower(hardwareMap);

        waitForStart();

        // Path with completion callback
        follower.followPath(
            new BezierLine(new Point(30, 0, Point.CARTESIAN))
                .setConstantHeadingInterpolation(0)
                .setPathEndCallback(() -> {
                    telemetry.addLine("✓ Reached spike mark");
                    releasePixel();
                })
        );

        // Execute with timeout safety
        timer.reset();
        while (!follower.isPathComplete() && opModeIsActive() && timer.seconds() < 3.0) {
            robot.update();
            follower.update();
            robot.setDrivePowers(follower.getMotorPowers());

            // Real-time monitoring
            telemetry.addData("Progress", "%.0f%%", follower.getPathProgress() * 100);
            telemetry.addData("Distance Remaining", "%.1f in", follower.getDistanceToEnd());
            telemetry.addData("Time Elapsed", "%.1f s", timer.seconds());
            telemetry.update();
        }

        // Verify completion
        if (follower.isPathComplete()) {
            telemetry.addLine("✓ Path 1 Complete");
        } else {
            telemetry.addLine("⚠ Path 1 Timeout");
        }

        // Continue with autonomous...
    }
}