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 = "fullRobotControl")
public class FullRobotControl extends OpMode {
    public double axial;
    public double lateral;
    public double yaw;
    DcMotor frontRight;
    DcMotor backRight;
    DcMotor frontLeft;
    DcMotor backLeft;
    DcMotor armMotor;
    Servo gripper;
    Servo wrist;
    DcMotor arm;
    public ElapsedTime runtime = new ElapsedTime();
    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() {
        frontRight.setPower(0);
        backRight.setPower(0);
        frontLeft.setPower(0);
        backLeft.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();
        frontRight = hardwareMap.dcMotor.get("Drive front rt");
        backRight = hardwareMap.dcMotor.get("Drive back rt");
        frontLeft = hardwareMap.dcMotor.get("Drive front lt");
        backLeft = hardwareMap.dcMotor.get("Drive back lt");
        armMotor = hardwareMap.dcMotor.get("armMotor");
        gripper = hardwareMap.servo.get("gripper");
        gripper.setPosition(1);
        wrist = hardwareMap.servo.get("wrist");
        arm = hardwareMap.dcMotor.get("arm raise");
        wrist.setPosition(1);

    }

    /**
     * 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
     */
    double num = 2;
    String speed = "plaid";
    public void loop() {

        frontLeft.setDirection(DcMotor.Direction.REVERSE);
        backLeft.setDirection(DcMotor.Direction.REVERSE);
        frontRight.setDirection(DcMotor.Direction.FORWARD);
        backRight.setDirection(DcMotor.Direction.REVERSE);

        if(gamepad1.a){
            num = 2.5;
            speed = "medium";
        }
        if(gamepad1.b){
            num = 2;
            speed = "fast";
        }
        if(gamepad1.x){
            num = 1.75;
            speed = "Ludicrous";
        }
        if(gamepad1.y ){
            num = 1.5;
            speed = "plaid";
        }
        axial   = -gamepad1.left_stick_y/num;  // Note: pushing stick forward gives negative value
        lateral =  gamepad1.left_stick_x/num;
        yaw     =  gamepad1.right_stick_x/(num+0.5);



        // Combine the joystick requests for each axis-motion to determine each wheel's power.
        // Set up a variable for each drive wheel to save the power level for telemetry.
        double leftFrontPower  = axial + lateral + yaw;
        double rightFrontPower = axial - lateral - yaw;
        double leftBackPower   = axial - lateral + yaw;
        double rightBackPower  = axial + lateral - yaw;
        double armPower = gamepad1.right_stick_y/3;
        // Normalize the values so no wheel power exceeds 100%
        // This ensures that the robot maintains the desired motion.
        double max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
        max = Math.max(max, Math.abs(leftBackPower));
        max = Math.max(max, Math.abs(rightBackPower));
        if (max > 1.0) {
            leftFrontPower  /= max;
            rightFrontPower /= max;
            leftBackPower   /= max;
            rightBackPower  /= max;
        }
        if(gamepad1.dpad_down){
            wrist.setPosition(1);

        }
        if(gamepad1.dpad_up){

            wrist.setPosition(0.5);
        }
        if(gamepad1.left_bumper){
            gripper.setPosition(0.25);
        }
        if(gamepad1.right_bumper){
            gripper.setPosition(1);
        }

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




        frontLeft.setPower(leftFrontPower);
        frontRight.setPower(rightFrontPower);
        backLeft.setPower(leftBackPower);
        backRight.setPower(rightBackPower);
        armMotor.setPower(armPower);
        // 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)
    {
        armEncoder(.2, degrees*TICKS_TO_DEGREES);

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


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

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

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


        arm.setPower(0);


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

        }
    }

