package org.firstinspires.ftc.teamcode;

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

@TeleOp(name = "BBBBBBBBB")
public class Gamepad extends OpMode {
    Hardware hardware;
    MovementAPI movement;
    double speed = 0.6;

    @Override
    public void init() {
        hardware = new Hardware(this);
        movement = new MovementAPI(hardware);

        hardware.claw.setPosition(1);
        hardware.wrist.setPosition(0.5);
    }

    @Override
    public void loop() {
        if (gamepad1.left_bumper) {
            speed -= 0.005;
        }

        if (gamepad1.right_bumper) {
            speed += 0.005;
        }

        speed = Math.max(0, Math.min(speed, 1));

        hardware.spool.setPower(gamepad2.left_stick_y);
        hardware.claw.setPosition(1 - gamepad2.left_trigger);
        hardware.wrist.setPosition((-gamepad2.right_stick_y + 1) / 2);

        movement.move(
                gamepad1.left_stick_x,
                -gamepad1.left_stick_y,
                gamepad1.right_stick_x,
                speed
        );

        telemetry.addLine(speed + "");
    }
}