package org.firstinspires.ftc.teamcode;

public class MovementAPI {
    private final API api;

    private final API.Motor fl;
    private final API.Motor fr;
    private final API.Motor bl;
    private final API.Motor br;

    private boolean flipped = false;

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

    /**
     * Initializes the API
     *
     * @param _fl the front-left wheel
     * @param _fr the front-right wheel
     * @param _bl the back-left wheel
     * @param _br the back-right wheel
     */
    public MovementAPI(API api, API.Motor _fl, API.Motor _fr, API.Motor _bl, API.Motor _br) {
        this.api = api;

        api.print("Initializing MovementApi...");

        fl = _fl;
        fr = _fr;
        bl = _bl;
        br = _br;

        fl.setDirection(API.Direction.REVERSE);
        bl.setDirection(API.Direction.REVERSE);
        fr.setDirection(API.Direction.FORWARD);
        br.setDirection(API.Direction.FORWARD);
    }

    /**
     * Initializes the API with the motors in the GameMotor enum
     */
    public MovementAPI(API api, GameMotors motors) {
        this(api, motors.fl, motors.fr, motors.bl, motors.br);
    }

    public void setFlipped(boolean flipped) {
        this.flipped = flipped;
    }

    /**
     * Moves the robot given the speed to move forward/back and left/right
     *
     * @param powerY  the speed to move forward/back, -1 to 1, positive being forward
     * @param powerX  the speed to move left/right, -1 to 1,  positive being to the right
     * @param turn    the speed to turn at, -1 to 1, positive being clockwise
     * @param speed   a multiplier on the final speed
     * @param verbose whether or not to log extra data to telemetry
     */
    public void move(double powerY, double powerX, double turn, double speed, boolean verbose) {
        if (flipped) {
            powerX *= -1;
            turn *= -1;
        }

        double flPower = (powerY + turn + powerX) * speed;
        double frPower = (powerY - turn - powerX) * speed;
        double blPower = (powerY + turn - powerX) * speed;
        double brPower = (powerY - turn + powerX) * 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.start(flPower);
        fr.start(frPower);
        bl.start(blPower);
        br.start(brPower);

        if (verbose) api.print(
            "Front Left: " + flPower + System.lineSeparator() +
            "Back Left: " + blPower + System.lineSeparator() +
            "Front Right: " + frPower + System.lineSeparator() +
            "Back Right: " + brPower + System.lineSeparator()
        );
    }

    /**
     * Moves the robot given the speed to move forward/back and left/right
     * @param powerY the speed to move forward/back, -1 to 1, positive being forward
     * @param powerX the speed to move left/right, -1 to 1,  positive being to the right
     * @param turn   the speed to turn at, -1 to 1, positive being clockwise
     * @param speed  a multiplier on the final speed
     *
     */
    public void move(double powerY, double powerX, double turn, double speed) {
        move(powerY, powerX, turn, speed, false);
    }

    /**
     * Moves the robot in a given direction with a given speed
     *
     * @param direction the direction to move in, in degrees, with positive being left of forward
     * @param speed     the speed to move at
     * @param verbose   whether or not to log extra data to telemetry
     */
    public void move(double direction, double speed, boolean verbose) {
        move(Math.cos(Math.toRadians(direction)), Math.sin(Math.toRadians(-direction)), 0, speed, verbose);
    }

    /**
     * Moves the robot in a given direction with a given speed
     *
     * @param direction the direction to move in, in degrees, with positive being left of forward
     * @param speed     the speed to move at
     */
    public void move(double direction, double speed) {
        move(direction, speed, false);
    }

    /**
     * Stops the robot
     */
    public void stop() {
        fl.stop();
        fr.stop();
        bl.stop();
        br.stop();
    }
}