package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction;
import com.qualcomm.robotcore.hardware.HardwareMap;

public class MovementAPI {
    private final DcMotor fl;
    private final DcMotor fr;
    private final DcMotor bl;
    private final DcMotor br;

    public DcMotor getFL() { return this.fl; }
    public DcMotor getFR() { return this.fr; }
    public DcMotor getBL() { return this.bl; }
    public DcMotor getBR() { return this.br; }

    public MovementAPI(Hardware hardware) {
        this(
                hardware.fl,
                hardware.fr,
                hardware.bl,
                hardware.br
        );
    }

    public MovementAPI(DcMotor fl, DcMotor fr, DcMotor bl, DcMotor br) {
        this.fl = fl;
        this.fr = fr;
        this.bl = bl;
        this.br = br;
    }

    public void move(double powerX, double powerY, double turn, double speed) {
        double flPower = (powerY + powerX + turn) * speed;
        double frPower = (powerY - powerX - turn) * speed;
        double blPower = (powerY - powerX + turn) * speed;
        double brPower = (powerY + powerX - turn) * speed;

        double scale = Math.max(1, (Math.abs(powerY) + Math.abs(turn) + Math.abs(powerX)) * Math.abs(speed)); // shortcut for max(abs([fl,fr,bl,br]))
        flPower /= scale;
        frPower /= scale;
        blPower /= scale;
        brPower /= scale;

        fl.setPower(flPower);
        fr.setPower(frPower);
        bl.setPower(blPower);
        br.setPower(brPower);
    }

    public void stop() {
        fl.setPower(0);
        fr.setPower(0);
        bl.setPower(0);
        br.setPower(0);
    }
}