@TeleOp(name = "RobojoustTeleOp")

public class RobojoustTeleOp extends LinearOpMode {

    public void setup() {

    }

    public void runOpMode() throws InterruptedException{

        MecanumAuto drive = new MecanumAuto(hardwareMap);
        Intake intake = new Intake();
        Lift lift = new Lift();
        Foundation foundation = new Foundation();
        Scorer scorer = new Scorer();


        intake.init(hardwareMap);
        lift.init(hardwareMap);
        foundation.init(hardwareMap);

        waitForStart();

        while(opModeIsActive()) {
            //Driver
            //driving omni directional movement: this works now as of 12/4/19
            if(Math.abs(gamepad1.left_stick_x) > 0.25 || Math.abs(gamepad1.left_stick_y) > 0.25 || Math.abs(gamepad1.right_stick_x) > 0.25){
                double FrontLeftVal =  gamepad1.left_stick_y - (gamepad1.left_stick_x)  + -gamepad1.right_stick_x;
                double FrontRightVal =  gamepad1.left_stick_y  + (gamepad1.left_stick_x) - -gamepad1.right_stick_x;
                double BackLeftVal = gamepad1.left_stick_y  + (gamepad1.left_stick_x)  + -gamepad1.right_stick_x;
                double BackRightVal = gamepad1.left_stick_y - (gamepad1.left_stick_x) - -gamepad1.right_stick_x;

                //Move range to between 0 and +1, if not already
                double[] wheelPowers = {FrontRightVal, FrontLeftVal, BackLeftVal, BackRightVal};
                Arrays.sort(wheelPowers);
                if (wheelPowers[3] > 1) {
                    FrontLeftVal /= wheelPowers[3];
                    FrontRightVal /= wheelPowers[3];
                    BackLeftVal /= wheelPowers[3];
                    BackRightVal /= wheelPowers[3];
                }
                drive.frontLeft.setPower(FrontLeftVal);
                drive.frontRight.setPower(FrontRightVal);
                drive.backLeft.setPower(BackLeftVal);
                drive.backRight.setPower(BackRightVal);
            }
            else{
                drive.frontLeft.setPower(0);
                drive.frontRight.setPower(0);
                drive.backLeft.setPower(0);
                drive.backRight.setPower(0);
            }
            //arm
            if(gamepad1.right_trigger > 0.15){
               lift.Arm(gamepad1.right_trigger, true);

            }
            else if(gamepad1.left_trigger > 0.15){
                lift.Arm(-gamepad1.left_trigger, true);
            }
            else{
                lift.Arm(0, false);
            }
            //Arm servos
            if(gamepad1.y){
                scorer.gripper.setPosition(1);//gripper up
                telemetry.addData("Gripper Position: ", scorer.gripper.getPosition());
                telemetry.update();
            }
            else if(gamepad1.a) {
                scorer.gripper.setPosition(0);//gripper gripped
                telemetry.addData("Gripper Position: ", scorer.gripper.getPosition());
                telemetry.update();
            }
            //pivot
            if(gamepad1.b){//out
                scorer.pivot.setPosition(1);
                telemetry.addData("Pivot Position: ", scorer.gripper.getPosition());
                telemetry.update();
            }
            else if(gamepad1.x){//in
                scorer.pivot.setPosition(0.05);
                telemetry.addData("Pivot Position: ", scorer.gripper.getPosition());
                telemetry.update();
            }

            //Gunner
            //foundation latch
            if(gamepad2.a){
                foundation.latchL.setPosition(0);
                foundation.latchR.setPosition(1);
            }
            else if(gamepad2.x){//mid
                foundation.latchL.setPosition(0.5);
                foundation.latchR.setPosition(0.5);
            }
            else if(gamepad2.y){//up
                foundation.latchL.setPosition(1);
                foundation.latchR.setPosition(0);
            }
            //hitter
            if(gamepad2.left_bumper){
                intake.hitter.setPosition(0.4);
            }
            else if(gamepad2.right_bumper){
                intake.hitter.setPosition(1);
            }
            //collection
            if(gamepad2.left_stick_y > 0.15){
                intake.collect(gamepad2.left_stick_y);
            }
            else if(gamepad2.left_stick_y < -0.15 ) {
                intake.collect(gamepad2.left_stick_y);
            }
            else{
                intake.collect(0.0);
            }

        }
    }
}