package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.firstinspires.ftc.robotcontroller.external.samples.HardwarePushbot;


@Autonomous (name = "autoTest" , group = "pushbot")
public class Drive extends LinearOpMode
{
    /* Declare OpMode members. */
    HardWareBushBot robot   = new HardWareBushBot();   // Use a Pushbot's hardware
    private ElapsedTime runtime = new ElapsedTime();

    static final double     COUNTS_PER_MOTOR_REV    = 1440 ;    // eg: TETRIX Motor Encoder
    static final double     DRIVE_GEAR_REDUCTION    = 2.0 ;     // This is < 1.0 if geared UP
    static final double     WHEEL_DIAMETER_INCHES   = 4.0 ;     // For figuring circumference
    static final double     COUNTS_PER_INCH         = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
            (WHEEL_DIAMETER_INCHES * 3.1415);


    public void encoderDrive(double speed,
                             double leftInches, double rightInches,
                             double timeoutS) {
        int newLeftTarget;
        int newRightTarget;

        // Ensure that the opmode is still active
        if (opModeIsActive()) {

            // Determine new target position, and pass to motor controller
            newLeftTarget = robot.BDL.getCurrentPosition() + (int)(leftInches * COUNTS_PER_INCH);
            newRightTarget = robot.BDR.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH);
            robot.BDL.setTargetPosition(newLeftTarget);
            robot.BDR.setTargetPosition(newRightTarget);

            // Turn On RUN_TO_POSITION
            robot.BDL.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            robot.BDR.setMode(DcMotor.RunMode.RUN_TO_POSITION);

            // reset the timeout time and start motion.
            runtime.reset();
            robot.BDL.setPower(Math.abs(speed));
            robot.BDR.setPower(Math.abs(speed));

            // keep looping while we are still active, and there is time left, and both motors are running.
            // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
            // its target position, the motion will stop.  This is "safer" in the event that the robot will
            // always end the motion as soon as possible.
            // However, if you require that BOTH motors have finished their moves before the robot continues
            // onto the next step, use (isBusy() || isBusy()) in the loop test.
            while (opModeIsActive() &&
                    (runtime.seconds() < timeoutS) &&
                    (robot.BDL.isBusy() && robot.BDR.isBusy())) {

                // Display it for the driver.
                telemetry.addData("Path1",  "Running to %7d :%7d", newLeftTarget,  newRightTarget);
                telemetry.addData("Path2",  "Running at %7d :%7d",
                        robot.BDL.getCurrentPosition(),
                        robot.BDR.getCurrentPosition());
                telemetry.update();
            }

            // Stop all motion;
            robot.BDL.setPower(0);
            robot.BDR.setPower(0);

            // Turn off RUN_TO_POSITION
            robot.BDL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            robot.BDR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

            //  sleep(250);   // optional pause after each move
        }
    }
    public void runOpMode()
    {
        robot.init(hardwareMap);
        robot.BDR.setDirection(DcMotor.Direction.REVERSE);
        telemetry.addData("Status", "Resetting Encoders");    //
        telemetry.update();

        robot.BDL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        robot.BDR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        robot.BDL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        robot.BDR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

        // Send telemetry message to indicate successful Encoder reset
        telemetry.addData("Path0",  "Starting at %7d :%7d",
                robot.BDL.getCurrentPosition(),
                robot.BDR.getCurrentPosition());
        telemetry.update();

        // Wait for the game to start (driver presses PLAY)
        waitForStart();
    }
}