using System;

namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;

/// <summary>旋转矩形与轴对齐栅格的分离轴相交判定。</summary>
internal static class OrientedRectangleCellIntersection
{
    /// <summary>任一投影轴没有严格分离时返回 true；擦边按相交处理。</summary>
    public static bool Intersects(VehicleFootprint rectangle, double cellMinX, double cellMaxX, double cellMinY, double cellMaxY)
    {
        if (rectangle == null || cellMaxX < cellMinX || cellMaxY < cellMinY) return false;
        double cellCenterX = (cellMinX + cellMaxX) / 2d;
        double cellCenterY = (cellMinY + cellMaxY) / 2d;
        double cellHalfX = (cellMaxX - cellMinX) / 2d;
        double cellHalfY = (cellMaxY - cellMinY) / 2d;

        return !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLongitudinalX, rectangle.AxisLongitudinalY) &&
            !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLateralX, rectangle.AxisLateralY) &&
            !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 1d, 0d) &&
            !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 0d, 1d);
    }

    private static bool HasStrictSeparation(VehicleFootprint rectangle, double cellCenterX, double cellCenterY, double cellHalfX, double cellHalfY,
        double axisX, double axisY)
    {
        double rectangleCenter = rectangle.CenterX * axisX + rectangle.CenterY * axisY;
        double cellCenter = cellCenterX * axisX + cellCenterY * axisY;
        double rectangleRadius = rectangle.HalfLengthMeters * Math.Abs(rectangle.AxisLongitudinalX * axisX + rectangle.AxisLongitudinalY * axisY) +
            rectangle.HalfWidthMeters * Math.Abs(rectangle.AxisLateralX * axisX + rectangle.AxisLateralY * axisY);
        double cellRadius = cellHalfX * Math.Abs(axisX) + cellHalfY * Math.Abs(axisY);
        return Math.Abs(rectangleCenter - cellCenter) > rectangleRadius + cellRadius;
    }
}