package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;

@TeleOp(name="TeleOp14657POVMode", group="Iterative Opmode")
public class TeleOp14657POVMode extends OpMode {

    private ElapsedTime runtime = new ElapsedTime();
    public DcMotor bRight;
    public DcMotor bLeft;
    public DcMotor fRight;
    public DcMotor fLeft;
    public DcMotor mArm;
    public DcMotor hArm;
    public DcMotor mArm2;
    public CRServo MCollect;
    //    public CRServo MExtend;
    public Servo MExtend;
    //    public CRServo RMineral;
    public Servo RMineral;

    double drivePower;
    double turnPower;
    double mArmPower;
    double hArmPowerOut;
    double hArmPowerIn;
    double RMineralPower;
    double CollectPowerIn;
    double CollectPowerOut;

    @Override
    public void init() {
        // what happens when you press init
        // this is where we put our variables
        bRight = hardwareMap.dcMotor.get("BRightMotors");
        bLeft = hardwareMap.dcMotor.get("BLeftMotor");
        fLeft = hardwareMap.dcMotor.get("FLeftMotor");
        fRight = hardwareMap.dcMotor.get("FRightMotor");
        mArm = hardwareMap.dcMotor.get("MineralArm");
        hArm = hardwareMap.dcMotor.get("HookArm");
        mArm2 = hardwareMap.dcMotor.get("MineralArm2");
        MCollect = hardwareMap.crservo.get("MCollect");
        //MExtend = hardwareMap.crservo.get("MExtend");
        MExtend = hardwareMap.servo.get("MExtend");
//        RMineral = hardwareMap.crservo.get("RMineral");
        RMineral = hardwareMap.servo.get("RMineral");

        hArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        mArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        mArm2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

    }

    @Override
    public void loop() {

        // Setup a variable for each drive wheel to save power level for telemetry
        double leftPower;
        double rightPower;

        double drive = -gamepad1.right_stick_x;
        double turn = -gamepad1.left_stick_y;
        leftPower = 0.5 * Range.clip(drive - turn, -1.0, 1.0);
        rightPower = 0.5 * Range.clip(drive + turn, -1.0, 1.0);

        bLeft.setPower(leftPower);
        bRight.setPower(rightPower);
        fLeft.setPower(leftPower);
        fRight.setPower(rightPower);

        // Show the elapsed game time and wheel power.
        telemetry.addData("Status", "Run Time: " + runtime.toString());
        telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower);
        telemetry.update();
        RMineralPower = (gamepad2.right_stick_y);
//        if (gamepad2.dpad_up) {
//            MExtend.setPower(1);
//        } else if (gamepad2.dpad_down) {
//            MExtend.setPower(-1);
//        } else {
//            MExtend.setPower(0);
//        }

        if (gamepad2.dpad_up) {
            MExtend.setPosition(1);
        } else if (gamepad2.dpad_down) {
            MExtend.setPosition(0);
        } else {
            MExtend.setPosition(0.5);
        }


        // the statement below this is to move the arm that moves our entire mineral collecter
        mArmPower = gamepad2.left_stick_y;
        mArm.setPower(-mArmPower*0.5 );
        mArm2.setPower(-mArmPower*0.5);
        // the statemen[]]]]]]]]t below this is to set the variables for the arm that we latch on to the lander with
        hArmPowerOut = gamepad1.right_trigger;
        hArmPowerIn = gamepad1.left_trigger;
        CollectPowerIn = gamepad2.left_trigger;
        CollectPowerOut = gamepad2.right_trigger;
        // the statement below is to move the arm that we latch on to the lander with move
        // the statement below is so if the right trigger is pressed then it will move the arm
        if (hArmPowerIn > 0.1) {
            hArm.setPower(hArmPowerIn * 1.5);
            //the statement below so if the left trigger is pressed then it will move the arm
        } else if (hArmPowerOut > 0.1) {
            hArm.setPower(-hArmPowerOut * 1.5);
        } else {
            hArm.setPower(0);
        }
//        hArm.setTargetPosition(hArm.getCurrentPosition());
        if (CollectPowerIn > 0.1) {
            MCollect.setPower(-CollectPowerIn * 1.5);
            //the statement below so if the left trigger is pressed then it will move the arm
        } else if (CollectPowerOut > 0.1) {
            MCollect.setPower(CollectPowerOut * 1.5);
        } else {
            MCollect.setPower(0);
        }

        //the statement below is so if the b button is pressed then it will extend
        //the statement below is so when the left bumper is pressed then it will collect the minerals
//        if (gamepad2.left_bumper) {
//            RMineral.setPower(1*1.5);
//        } else if (gamepad2.right_bumper) {//the statement below is so when the right bumper is pressed then it will push out the minerals
//            RMineral.setPower(-1*1.5);
//        } else {//the statement below is so the collecter will stop
//            RMineral.setPower(0);
//        }

        if (gamepad2.left_bumper) {
            RMineral.setPosition(1);
        } else if (gamepad2.right_bumper) {//the statement below is so when the right bumper is pressed then it will push out the minerals
            RMineral.setPosition(0);
        } else {//the statement below is so the collecter will stop
            RMineral.setPosition(0.5);
        }
    }
}