package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;

/**
 * Integration lesson AFTER all three crews pass individual tests.
 * Not a competition-ready program or an automatic scoring guarantee.
 * Confirm mapping, directions, guards, backstop and rules with a mentor.
 * Pusher remains disabled until real travel endpoints are measured.
 */
@TeleOp(name = "Pathfinder Robot TeleOp", group = "Pathfinder")
public class PathfinderRobotTeleOp extends LinearOpMode {
    private static final double DRIVE_LIMIT = 0.35;
    private static final double INTAKE_POWER = 0.30;
    private static final double FLYWHEEL_POWER = 0.50; // Tune only after guarded trials.
    private static final double FEEDER_POWER = 0.20;
    private static final double SPINUP_SECONDS = 1.5; // Time only; not measured wheel speed.
    private static final double FEED_PULSE_SECONDS = 0.25; // Tune after guarded trials.
    private static final boolean RIGHT_REVERSED = true; // Verify on your supported robot.

    @Override
    public void runOpMode() throws InterruptedException {
        DcMotor left = hardwareMap.get(DcMotor.class, "leftDrive");
        DcMotor right = hardwareMap.get(DcMotor.class, "rightDrive");
        DcMotor intake = hardwareMap.get(DcMotor.class, "intakeMotor");
        DcMotor flywheel = hardwareMap.get(DcMotor.class, "flywheel");
        CRServo feeder = hardwareMap.get(CRServo.class, "crServo");
        Servo pusher = hardwareMap.get(Servo.class, "pollenServo"); // Intentionally not moved.
        right.setDirection(RIGHT_REVERSED ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
        left.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        right.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        flywheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
        left.setPower(0); right.setPower(0); intake.setPower(0);
        flywheel.setPower(0); feeder.setPower(0);
        ElapsedTime spinup = new ElapsedTime();
        ElapsedTime feedPulse = new ElapsedTime();
        boolean previousFeedRequest = false;
        boolean feedActive = false;
        telemetry.addLine("Integrated mode: mentor-approved lane and guards only.");
        telemetry.addLine("Stick drive, RB intake, Y flywheel, Y+LB pulse after spin-up.");
        telemetry.update();
        waitForStart();

        try {
            while (opModeIsActive()) {
                double forward = -gamepad1.left_stick_y;
                double turn = gamepad1.left_stick_x;
                left.setPower(clip(forward + turn) * DRIVE_LIMIT);
                right.setPower(clip(forward - turn) * DRIVE_LIMIT);
                intake.setPower(gamepad1.right_bumper ? INTAKE_POWER : 0);

                if (gamepad1.y) {
                    if (flywheel.getPower() == 0) spinup.reset();
                    flywheel.setPower(FLYWHEEL_POWER);
                } else {
                    flywheel.setPower(0);
                }
                boolean feedRequest = gamepad1.y && gamepad1.left_bumper;
                boolean timeGate = gamepad1.y && spinup.seconds() >= SPINUP_SECONDS;
                // Rising edge: release and press LB again for another short pulse.
                if (feedRequest && !previousFeedRequest && timeGate) {
                    feedPulse.reset();
                    feedActive = true;
                }
                if (!feedRequest || feedPulse.seconds() >= FEED_PULSE_SECONDS) feedActive = false;
                feeder.setPower(feedActive ? FEEDER_POWER : 0);
                previousFeedRequest = feedRequest;
                telemetry.addData("Flywheel on", gamepad1.y);
                telemetry.addData("Time gate (NOT speed sensor)", "%.1f s", gamepad1.y ? spinup.seconds() : 0);
                telemetry.addData("Feed requested / active", "%b / %b", feedRequest, feedActive);
                telemetry.addData("Pusher", "disabled; endpoints not verified");
                telemetry.update();
                idle();
            }
        } finally {
            left.setPower(0); right.setPower(0); intake.setPower(0);
            flywheel.setPower(0); feeder.setPower(0);
        }
    }

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