package org.firstinspires.ftc.teamcode;

public class MotorPowers {
    public final double fl;
    public final double fr;
    public final double bl;
    public final double br;

    public MotorPowers(double fl, double fr, double bl, double br) {
        this.fl = fl;
        this.fr = fr;
        this.bl = bl;
        this.br = br;
    }

    public static MotorPowers calculateUnscaled(double powerX, double powerY, double turn, double speed) {
        double fl = (powerY + powerX + turn) * speed;
        double fr = (powerY - powerX - turn) * speed;
        double bl = (powerY - powerX + turn) * speed;
        double br = (powerY + powerX - turn) * speed;

        return new MotorPowers(fl, fr, bl, br);
    }

    public MotorPowers scaled() {
        double scale = 1;
        scale = Math.max(scale, Math.abs(this.fl));
        scale = Math.max(scale, Math.abs(this.fr));
        scale = Math.max(scale, Math.abs(this.bl));
        scale = Math.max(scale, Math.abs(this.br));

        double fl = this.fl / scale;
        double fr = this.fr / scale;
        double bl = this.bl / scale;
        double br = this.br / scale;

        return new MotorPowers(fl, fr, bl, br);
    }

    public static MotorPowers calculate(double powerX, double powerY, double turn, double speed) {
        return MotorPowers.calculateUnscaled(powerX, powerY, turn, speed).scaled();
    }
}
