package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.vingvingrobot.Robot;
import com.vingvingrobot.follower.Follower;
import com.vingvingrobot.geometry.Point;
import com.vingvingrobot.path.BezierLine;

@Autonomous(name = "Simple Auto", group = "VingVing")
public class SimpleAuto extends LinearOpMode {
    Robot robot;
    Follower follower;

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

        telemetry.addLine("Robot initialized");
        telemetry.addLine("Ready to start!");
        telemetry.update();

        waitForStart();

        // Simple path: drive forward 48 inches
        follower.followPath(
            new BezierLine(new Point(48, 0, Point.CARTESIAN))
                .setConstantHeadingInterpolation(0)
        );

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

            // Telemetry
            telemetry.addData("X Position", "%.1f", robot.getLocalizer().getX());
            telemetry.addData("Y Position", "%.1f", robot.getLocalizer().getY());
            telemetry.addData("Progress", "%.0f%%", follower.getPathProgress() * 100);
            telemetry.update();
        }

        telemetry.addLine("✓ Autonomous Complete!");
        telemetry.update();
    }
}

@Autonomous(name = "Multi-Path Auto")
public class MultiPathAuto extends LinearOpMode {
    Robot robot;
    Follower follower;

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

        waitForStart();

        // Path sequence with multiple segments
        follower.followPath(
            // 1. Drive to spike mark
            new BezierLine(new Point(30, 0, Point.CARTESIAN))
                .setConstantHeadingInterpolation(0)
                .setPathEndCallback(() -> {
                    telemetry.addLine("Reached spike mark");
                }),

            // 2. Curve to backstage
            new BezierCurve(
                new Point(30, 0, Point.CARTESIAN),
                new Point(30, 0, Point.CARTESIAN),
                new Point(35, 20, Point.CARTESIAN),
                new Point(20, 40, Point.CARTESIAN),
                new Point(8, 48, Math.toRadians(90), Point.CARTESIAN)
            ).setLinearHeadingInterpolation()
                .addCallback(0.5, () -> {
                    telemetry.addLine("Halfway to backstage");
                }),

            // 3. Final positioning
            new BezierLine(new Point(4, 52, Math.toRadians(90), Point.CARTESIAN))
                .setConstantHeadingInterpolation(Math.toRadians(90))
                .setPathEndCallback(() -> {
                    telemetry.addLine("Ready to score!");
                }),

            // 4. Park
            new BezierLine(new Point(8, 58, Math.toRadians(90), Point.CARTESIAN))
                .setConstantHeadingInterpolation(Math.toRadians(90))
        );

        // Execute all paths
        while (!follower.isPathComplete() && opModeIsActive()) {
            robot.update();
            follower.update();
            robot.setDrivePowers(follower.getMotorPowers());

            telemetry.addData("Path", "%d of 4", follower.getCurrentPathIndex() + 1);
            telemetry.addData("Progress", "%.0f%%", follower.getPathProgress() * 100);
            telemetry.update();
        }
    }
}

@Autonomous(name = "Complete Auto with Mechanisms")
public class CompleteAuto extends LinearOpMode {
    Robot robot;
    Follower follower;
    DcMotor arm;
    Servo claw;

    private static final int ARM_DOWN = 0;
    private static final int ARM_SCORE = 1200;
    private static final double CLAW_OPEN = 0.5;
    private static final double CLAW_CLOSED = 0.0;

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

        // Initialize mechanisms
        arm = hardwareMap.get(DcMotor.class, "arm");
        arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        arm.setTargetPosition(0);
        arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        arm.setPower(0.8);

        claw = hardwareMap.get(Servo.class, "claw");
        claw.setPosition(CLAW_CLOSED);

        waitForStart();

        // === Path 1: To Spike Mark ===
        follower.followPath(
            new BezierLine(new Point(24, 0, Point.CARTESIAN))
                .setConstantHeadingInterpolation(0)
                .addCallback(0.7, () -> {
                    // Open claw near end
                    claw.setPosition(CLAW_OPEN);
                })
        );

        executePath();

        // Brief pause
        sleep(300);

        // === Path 2: To Backstage ===
        follower.followPath(
            new BezierCurve(
                new Point(24, 0, Point.CARTESIAN),
                new Point(24, 0, Point.CARTESIAN),
                new Point(32, 16, Point.CARTESIAN),
                new Point(16, 32, Point.CARTESIAN),
                new Point(8, 48, Point.CARTESIAN)
            ).setLinearHeadingInterpolation(Math.toRadians(90))
                .addCallback(0.3, () -> {
                    // Raise arm while driving
                    arm.setTargetPosition(ARM_SCORE);
                })
                .addCallback(0.7, () -> {
                    // Prepare to score
                    claw.setPosition(CLAW_CLOSED);
                })
        );

        executePath();

        // === Final Actions ===
        // Move to exact scoring position
        follower.followPath(
            new BezierLine(new Point(6, 52, Math.toRadians(90), Point.CARTESIAN))
                .setConstantHeadingInterpolation(Math.toRadians(90))
                .addDeceleration(0.7, 1.0, 0.25)  // Slow approach
        );

        executePath();

        // Score
        sleep(500);
        claw.setPosition(CLAW_OPEN);
        sleep(300);

        // Lower arm and park
        arm.setTargetPosition(ARM_DOWN);
        sleep(500);

        follower.followPath(
            new BezierLine(new Point(8, 58, Math.toRadians(90), Point.CARTESIAN))
                .setConstantHeadingInterpolation(Math.toRadians(90))
        );

        executePath();

        telemetry.addLine("✓ Autonomous Complete!");
        telemetry.update();
    }

    // Helper method to execute path following
    private void executePath() {
        while (!follower.isPathComplete() && opModeIsActive()) {
            robot.update();
            follower.update();
            robot.setDrivePowers(follower.getMotorPowers());

            telemetry.addData("Position", "(%.1f, %.1f)",
                robot.getLocalizer().getX(),
                robot.getLocalizer().getY());
            telemetry.addData("Progress", "%.0f%%", follower.getPathProgress() * 100);
            telemetry.addData("Arm Position", arm.getCurrentPosition());
            telemetry.update();
        }
    }
}

@Autonomous(name = "Robust Auto with Safety")
public class RobustAuto 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 1 with timeout
        follower.followPath(
            new BezierLine(new Point(30, 0, Point.CARTESIAN))
                .setConstantHeadingInterpolation(0)
        );

        if (!executePathWithTimeout(3.0)) {
            telemetry.addLine("⚠ Path 1 timeout - continuing");
        }

        // Path 2 with timeout
        follower.followPath(
            new BezierCurve(
                new Point(30, 0, Point.CARTESIAN),
                new Point(30, 0, Point.CARTESIAN),
                new Point(35, 20, Point.CARTESIAN),
                new Point(20, 40, Point.CARTESIAN),
                new Point(8, 48, Point.CARTESIAN)
            ).setLinearHeadingInterpolation(Math.toRadians(90))
        );

        if (!executePathWithTimeout(4.0)) {
            telemetry.addLine("⚠ Path 2 timeout - continuing");
        }

        telemetry.addLine("Autonomous complete!");
        telemetry.update();
    }

    /**
     * Execute path with timeout safety
     * @param timeoutSeconds Maximum time to wait
     * @return true if completed, false if timed out
     */
    private boolean executePathWithTimeout(double timeoutSeconds) {
        timer.reset();

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

            telemetry.addData("Time", "%.1f / %.1f s", timer.seconds(), timeoutSeconds);
            telemetry.addData("Progress", "%.0f%%", follower.getPathProgress() * 100);
            telemetry.update();
        }

        boolean completed = follower.isPathComplete();

        if (!completed) {
            // Stop robot on timeout
            robot.setDrivePowers(new double[]{0, 0, 0, 0});
        }

        return completed;
    }
}