package org.firstinspires.ftc.teamcode;


import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;



@TeleOp( name = "ArmState")
public class
ArmStates extends OpMode {

    DcMotor arm;
    Servo wrist;
    static final double TICKS_TO_DEGREES = 0.07462686567;
    static final double COUNTS_PER_MOTOR_REV = 537.6;
    static final double DRIVE_GEAR_REDUCTION = 60;
    static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.75 * Math.PI);


    /**
     * this function takes a long milliseconds parameter and sleeps
     *
     * @param millis milliseconds to sleep
     */
    public void sleepmillis(long millis) {
        try {
            Thread.sleep(millis);
        } catch (Exception e) {
        }
    }

    /**
     * stops all drive motors
     */
    public void off() {
        arm.setPower(0);

    }

    /**
     * User defined init method
     * This method will be called once when the INIT button is pressed.
     */

    public void init() {


        telemetry.addData("Status", "In Init()");
        telemetry.update();
        arm = hardwareMap.dcMotor.get("arm raise");
        wrist = hardwareMap.servo.get("wrist");
        arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

    }

    /**
     * User defined init_loop method
     * This method will be called repeatedly when the INIT button is pressed.
     * This method is optional. By default this method takes no action.
     */
    public void init_loop() {
        // Wait for the game to start (driver presses PLAY)
        telemetry.addData("Status", "Initialized");
        telemetry.update();
    }

    /**
     * User defined start method.
     * This method will be called once when the PLAY button is first pressed.
     * This method is optional. By default this method takes not action. Example usage: Starting another thread.
     */
    public void start() {

    }

    /**
     * User defined stop method
     * This method will be called when this op mode is first disabled.
     * The stop method is optional. By default this method takes no action.
     */
    public void stop() {

    }


    /**
     * User defined loop method.
     * This method will be called repeatedly in a loop while this op mode is running
     */
    public int armState = 0;

    public void loop() {
        int currentArmState = armState;

        telemetry.addData("state", armState);
        telemetry.addData("arm pos", arm.getCurrentPosition());
        telemetry.update();
        //Something is wrong, the arm is going the wrong way. Maybe it's just my robot, but you should look at it
        if (gamepad2.x) {
            armState = 1;
        }
        if (gamepad2.y) {
            armState = 2;
        }
        if (gamepad2.b) {
            armState = 3;
        }
        if (gamepad2.a)
        {
            armState = 0;
        }
        if (gamepad2.right_bumper) {
            armState += 1;
        }
        if (gamepad2.left_bumper) {
            armState -= 1;
        }
        if (armState != currentArmState) {
            updateState();
        }
    }


    public void armDriveWithEncoder(double speed,
                                    int ticks) {

        // Determine new target position, and pass to motor controller
        arm.setTargetPosition(ticks);

        // Turn On RUN_TO_POSITION
        arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        // reset the timeout time and start motion.
        arm.setPower(Math.abs(speed));

        while //(opModeIsActive() &&
            //(runtime.seconds() < timeoutS) &&
        (arm.isBusy()) {
            telemetry.addData("Running to", " %7d ", ticks);
            telemetry.addData("Currently at", " %7d", arm.getCurrentPosition());
            telemetry.update();
        }
        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        arm.setPower(0);

        // Turn off RUN_TO_POSITION
        arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

    }


    public void raisearm(int degrees) {
        arm.setDirection(DcMotor.Direction.REVERSE);
        armDriveWithEncoder(.05, degrees);
    }

    public void updateState() {
        if (armState == 0) {
            // arm state 0 means on the ground with gripper down
            wrist.setPosition(.5);
            raisearm(0);
            telemetry.addData("state", 0);
        }
        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        arm.setPower(0);
        if (armState == 1) {
            // arm state 1 is for driving
            raisearm(200);
            telemetry.addData("state", 1);
        }
        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        arm.setPower(0);
        if (armState == 2) {
            raisearm(697);
            wrist.setPosition(0);
            raisearm(697);
            telemetry.addData("state", 2);
        }
        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        arm.setPower(.0);
        if (armState == 3) {
            raisearm(780);
            telemetry.addData("state", 3);
            telemetry.addData("arm pos", arm.getCurrentPosition());
        }
        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        arm.setPower(0);
    }
}
