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;
    DcMotor armThroughBoreEncoder;
    static final double TICKS_TO_DEGREES = 0.07462686567;
    static final double COUNTS_PER_MOTOR_REV = 537.6;
    static final double DRIVE_GEAR_REDUCTION = 1.0;
    static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * 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");
        armThroughBoreEncoder = hardwareMap.dcMotor.get("armThroughBoreEncoder");

    }

    /**
     * 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 void loop() {
if (gamepad1.right_bumper)
{
    raisearm(200);
}


//        int armState = 0;
//        int armPos = 0;
//        if(gamepad2.x)
//        {
//            armState = 1;
//          raisearm(0 - armPos);
//          armPos = 0;
//        }
//        if(gamepad2.y)
//        {
//             armState = 2;
//            raisearm(1000);
//            armPos = 30;
//        }
//        if(gamepad2.b)
//        {
//             armState = 2;
//            raisearm(200 - armPos);
//            armPos = 200;
//        }
//        if(gamepad2.right_bumper)
//        {
//            armState =+ 1;
//        }
//        if(gamepad2.left_bumper)
//        {
//            armState =- 1;
//        }
//        if(armState == 1)
//        {
//            raisearm(0 - armPos);
//            armPos = 0;
//        }
//        if(armState == 2)
//        {
//            raisearm(30 - armPos);
//            armPos = 30;
//        }
//        if(armState == 3)
//        {
//            raisearm(200 - armPos);
//            armPos = 200;
//
//        }





//         Show the elapsed game time and wheel power
//        telemetry.addData("Status", "Run Time: " + runtime.toString());
//        telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
//        telemetry.addData("Back  left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
//        telemetry.addData("Speed", speed);


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

    }
    public void armDriveWithEncoder(double speed,
                                    double Inches) {
        int newarmTarget;


        // Determine new target position, and pass to motor controller
        newarmTarget = armThroughBoreEncoder.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
        armThroughBoreEncoder.setTargetPosition(newarmTarget);

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

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


        arm.setPower(0);


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

    }
}

