package org.firstinspires.ftc.teamcode;

public abstract class Gamepad extends OpModeWithMovement {
    protected double speed = 0.7;
    protected boolean slowMode = false;
    protected boolean prevX = false;
    protected boolean armUp = false;
    protected boolean prevL3 = false;
    protected double armHeight = 0;

    @Override
    public void init() {
        this.construct();

        api.print("Status", "Initializing, please wait");
        api.pause(1);

//        motors.shoulder1.runToPosition(0);
//        motors.shoulder2.runToPosition(0);
//        motors.shoulder1.setMode(API.MotorMode.RUN_TO_POSITION);
//        motors.shoulder2.setMode(API.MotorMode.RUN_TO_POSITION);
//        motors.shoulder1.start(1);
//        motors.shoulder2.start(1);
        motors.shoulder2.setDirection(API.Direction.REVERSE);

        api.clear();
        api.print("Press play to start");
    }

    @Override
    public void start() {
        api.clear();
    }

    @Override
    public void loop() {
//        double ms = getRuntime() / 1000;

        double currentSpeed = gamepad1.y ? 1 : slowMode ? 0.35 : speed;
        movement.move(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x, currentSpeed, true);

        api.print("Speed", Double.toString(speed));
        api.print("Slow mode", slowMode ? "true" : "false");
        api.print("Arm up", armUp ? "true" : "false");
        api.print("Arm height", Double.toString(armHeight));

        if (gamepad1.x && !prevX) slowMode = !slowMode;
        if (gamepad2.left_stick_button && !prevL3) armUp = !armUp;

        if (gamepad1.right_bumper) speed = Math.min(speed + 0.01, 1);
        else if (gamepad1.left_bumper) speed = Math.max(speed - 0.01, 0.2);

        motors.carousel.controlWithTwoButtons(gamepad2.x, gamepad2.y, 0.9);

//        motors.shoulder1.runToPosition((int)armHeight);
//        motors.shoulder2.runToPosition((int)armHeight);
        motors.shoulder1.start(gamepad2.left_stick_y);
        motors.shoulder2.start(gamepad2.left_stick_y);
        motors.elbow.start(gamepad2.right_stick_y);

//        armHeight += gamepad2.left_stick_y;

//             if (gamepad2.a && !gamepad2.b) posThingy += 0.05; //motors.hand.start(1);
//        else if (!gamepad2.a && gamepad2.b) posThingy -= 0.05; //motors.hand.start(-1);
//        else motors.hand.stop();

//        motors.hand.setPosition(posThingy);

//        System.out.println(posThingy);

        if (armUp) {
            motors.hand.start(1);
        } else {
            motors.hand.start(-1);
        }

        prevX = gamepad1.x;
        prevL3 = gamepad2.left_stick_button;

//        while (getRuntime() / 1000 - ms < 15) ; // no empty while loop here!
    }
}