package org.firstinspires.ftc.teamcode;



import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import com.qualcomm.robotcore.hardware.DcMotor;

import com.qualcomm.robotcore.hardware.DcMotorEx;

import com.qualcomm.robotcore.hardware.Gyroscope;

import com.qualcomm.robotcore.hardware.Servo;



import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;





/**



 * Created by Boen on 10-10-19



 */



@TeleOp(name = "Skystone op mode",group = "competition modes")







public class upDownMode extends LinearOpMode {

    private Gyroscope imu;

    private DcMotorEx backLeftWheel, backRightWheel, frontLeftWheel, frontRightWheel;

    private DcMotor linearLift;

    private Servo CLAW;





    @Override



    public void runOpMode() throws InterruptedException {

        imu = hardwareMap.get(Gyroscope.class, "imu");
        backLeftWheel = (DcMotorEx) hardwareMap.get(DcMotor.class, "Back_left_wheel");
        backRightWheel = (DcMotorEx) hardwareMap.get(DcMotor.class, "Back_right_wheel");
        frontLeftWheel = (DcMotorEx) hardwareMap.get(DcMotor.class, "Front_left_wheel");
        frontRightWheel = (DcMotorEx) hardwareMap.get(DcMotor.class, "Front_right_wheel");
        linearLift = (DcMotorEx) hardwareMap.get(DcMotor.class, "linearLift");
        CLAW = hardwareMap.servo.get("CLAW");



        backLeftWheel.setDirection(DcMotor.Direction.REVERSE);

        backRightWheel.setDirection(DcMotor.Direction.REVERSE);

        frontLeftWheel.setDirection(DcMotor.Direction.REVERSE);

        frontRightWheel.setDirection(DcMotor.Direction.REVERSE);

        linearLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        linearLift.setMode(DcMotor.RunMode.RUN_USING_ENCODER);



        telemetry.addData("Status", "Initialized");



        telemetry.update();



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



        waitForStart();





        //run until the end of the match (driver presses STOP)



        double speed;



        double rotation;



        double strafe;





        while (opModeIsActive()) {

            // Drive code

            speed = -this.gamepad1.left_stick_y;

            rotation = this.gamepad1.left_stick_x;

            strafe = -this.gamepad1.right_stick_x;



            backLeftWheel.setPower(speed + strafe - rotation);

            backRightWheel.setPower(-speed + strafe - rotation);

            frontLeftWheel.setPower(speed + strafe + rotation);

            frontRightWheel.setPower(-speed + strafe + rotation);



            // CLAAAAW code

            if (this.gamepad2.a) {

                CLAW.setPosition(0);

            } else if (this.gamepad2.y) {

                CLAW.setPosition(1);

            }

            int currentPosition = linearLift.getCurrentPosition();

            if (currentPosition > 1120){
                linearLift.setPower(-.5);
                sleep(500);
                linearLift.setPower(0);
                while ((gamepad2.left_stick_y >= 0) || (gamepad2.left_stick_y <= 0)){
                    linearLift.setPower(0);
                }
            }else if(currentPosition < 0){
                linearLift.setPower(.5);
                sleep(500);
                linearLift.setPower(0);
                while ((gamepad2.left_stick_y >= 0) || (gamepad2.left_stick_y <= 0)){
                    linearLift.setPower(0);
                }
            }else {
                linearLift.setPower(-gamepad2.left_stick_y);
            }

            //if ((this.gamepad2.left_stick_y < 0) && !(currentPosition > 1120)) {

                //How many degrees per second should it go up

              //  linearLift.setVelocity(90, AngleUnit.DEGREES);

            //}else if ((this.gamepad2.left_stick_y > 0) && !(currentPosition < 0)) {

                // How many degrees per second should it go down

              //  linearLift.setVelocity(-90, AngleUnit.DEGREES);
            //} else {

                // Hold Current Position

               //linearLift.setVelocity(0, AngleUnit.DEGREES);

            //}


            telemetry.addData("CLAAAAAW position", CLAW.getPosition());

            telemetry.addData("Target Power", speed);

            telemetry.addData("Motor Power", backLeftWheel.getPower());

            telemetry.addData("Status", "Running");
            telemetry.addData ("Linear lift height",linearLift.getCurrentPosition());

            telemetry.update();



        }



    }

}