package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;

/**
 * REV 2026-27 Starter Bot drive-only bench test. Mentor supervision required.
 * Lift the wheels for the first test. LEFT BUMPER is the hold-to-run switch.
 * Check motor names, wiring and RIGHT_REVERSED against YOUR robot before PLAY.
 */
@TeleOp(name = "Pathfinder Drive Test", group = "Pathfinder")
public class PathfinderDriveTest extends LinearOpMode {
    private static final double MAX_POWER = 0.25;
    private static final boolean RIGHT_REVERSED = true; // Verify on supported robot.

    @Override
    public void runOpMode() throws InterruptedException {
        DcMotor left = hardwareMap.get(DcMotor.class, "leftDrive");
        DcMotor right = hardwareMap.get(DcMotor.class, "rightDrive");
        left.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        right.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        right.setDirection(RIGHT_REVERSED ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
        left.setPower(0);
        right.setPower(0);
        telemetry.addLine("Wheels raised? Verify names/direction. Hold LB only when clear.");
        telemetry.update();
        waitForStart();

        try {
            while (opModeIsActive()) {
                double forward = -gamepad1.left_stick_y;
                double turn = gamepad1.left_stick_x;
                double leftPower = clip(forward + turn) * MAX_POWER;
                double rightPower = clip(forward - turn) * MAX_POWER;
                if (!gamepad1.left_bumper) { leftPower = 0; rightPower = 0; }
                left.setPower(leftPower);
                right.setPower(rightPower);
                telemetry.addData("Enabled (LB held)", gamepad1.left_bumper);
                telemetry.addData("Left / Right", "%.2f / %.2f", leftPower, rightPower);
                telemetry.update();
                idle();
            }
        } finally {
            left.setPower(0);
            right.setPower(0);
        }
    }

    private double clip(double value) { return Math.max(-1.0, Math.min(1.0, value)); }
}