package frc.robot;

import java.util.Optional;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rectangle2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc.robot.Constants.FieldMiscellaneous;

public class util {
    /**
     * lerp but fancier long edition
    */
    public static long map(long x, long inmin, long inmax, long outmin, long outmax)
    {
        return (x - inmin) * (outmax - outmin) / (inmax - inmin) + outmin;
    }
    /**
     * lerp but fancier double edition
    */
    public static double map(double x, double inmin, double inmax, double outmin, double outmax) // double version
    {
        return (x - inmin) * (outmax - outmin) / (inmax - inmin) + outmin;
    }
    /**
     * ai generated function that offsets the perimeter of each Rectangle2d in an array of rectangle2ds by some amount
     * @param rectangles Rectangle2d array to offset
     * @param expansion offset amount (radius)
     * @return Rectangle2d array offset by expansion
     */
    public static Rectangle2d[] offsetRectangle2dArray(Rectangle2d[] rectangles, double expansion) {
        Rectangle2d[] expanded = new Rectangle2d[rectangles.length];

        for (int i = 0; i < rectangles.length; i++) {
            Rectangle2d rect = rectangles[i];

            Pose2d center = rect.getCenter();
            double newWidth = rect.getYWidth() + 2.0 * expansion;
            double newHeight = rect.getXWidth() + 2.0 * expansion;

            expanded[i] = new Rectangle2d(center, newHeight, newWidth);
        }

        return expanded;
    }
    /**
     * 
     * @param rectangles Rectangle2d array
     * @param point Translation2d point
     * @return true if point inside any Rectangle2d inside array
     */
    public static boolean pointInsideRectangle2dArray(
            Rectangle2d[] rectangles,
            Translation2d point
    ) {
        if (rectangles == null || point == null) {
            return false;
        }

        for (Rectangle2d rect : rectangles) {
            if (rect != null && rect.contains(point)) {
                return true;
            }
        }

        return false;
    }
    public static Translation2d getAllianceHub() {
        final Translation2d bluHub2d = FieldMiscellaneous.gBLUALLIANCEHUB.toTranslation2d();
        final Translation2d redHub2d = FieldMiscellaneous.gREDALLIANCEHUB.toTranslation2d();
        Optional<Alliance> ally = DriverStation.getAlliance();
        
        if (ally.isPresent()) {
            if (ally.get() == Alliance.Red) {
                return redHub2d;
            }
            if (ally.get() == Alliance.Blue) {
                return bluHub2d;
            }
        }
        return new Translation2d();
    }
    public static Translation2d getRobotToHubNormalized(Supplier<Pose2d> poseSupplier) {
    Translation2d hub = getAllianceHub();
    Translation2d robot = poseSupplier.get().getTranslation(); // field-relative robot position
    Translation2d robotToHub = hub.minus(robot); // vector from robot to hub

    double maxMagnitude = Math.max(Math.abs(robotToHub.getX()), Math.abs(robotToHub.getY()));

    if (maxMagnitude > 1) {
        return robotToHub.div(maxMagnitude);
    }
    return robotToHub;
}
}