using System;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;

/// <summary>以车辆几何中心为原点的扩大旋转矩形。</summary>
internal sealed class VehicleFootprint
{
    private VehicleFootprint(Pose2D pose, double halfLengthMeters, double halfWidthMeters)
    {
        CenterX = pose.X;
        CenterY = pose.Y;
        HalfLengthMeters = halfLengthMeters;
        HalfWidthMeters = halfWidthMeters;
        AxisLongitudinalX = Math.Cos(pose.Heading);
        AxisLongitudinalY = Math.Sin(pose.Heading);
        AxisLateralX = -AxisLongitudinalY;
        AxisLateralY = AxisLongitudinalX;
        CircumscribedRadiusMeters = Math.Sqrt(halfLengthMeters * halfLengthMeters + halfWidthMeters * halfWidthMeters);

        double minX = double.PositiveInfinity;
        double maxX = double.NegativeInfinity;
        double minY = double.PositiveInfinity;
        double maxY = double.NegativeInfinity;
        for (int index = 0; index < 4; index++)
        {
            GetCorner(index, out double x, out double y);
            minX = Math.Min(minX, x);
            maxX = Math.Max(maxX, x);
            minY = Math.Min(minY, y);
            maxY = Math.Max(maxY, y);
        }
        MinX = minX;
        MaxX = maxX;
        MinY = minY;
        MaxY = maxY;
    }

    public double CenterX { get; }
    public double CenterY { get; }
    public double HalfLengthMeters { get; }
    public double HalfWidthMeters { get; }
    public double AxisLongitudinalX { get; }
    public double AxisLongitudinalY { get; }
    public double AxisLateralX { get; }
    public double AxisLateralY { get; }
    public double CircumscribedRadiusMeters { get; }
    public double MinX { get; }
    public double MaxX { get; }
    public double MinY { get; }
    public double MaxY { get; }

    /// <summary>创建包含车辆安全余量和临时扫掠余量的矩形。</summary>
    public static bool TryCreate(Pose2D pose, VehicleParameters vehicle, double additionalMarginMeters, out VehicleFootprint footprint)
    {
        footprint = null;
        if (pose == null || vehicle == null || !NumericGuard.IsFinite(pose.X) || !NumericGuard.IsFinite(pose.Y) ||
            !NumericGuard.IsFinite(pose.Heading) || !NumericGuard.IsPositiveFinite(vehicle.LengthMeters) ||
            !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) || !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) ||
            vehicle.SafetyMarginMeters < 0d || !NumericGuard.IsFinite(additionalMarginMeters) || additionalMarginMeters < 0d)
            return false;

        double totalMarginMeters = vehicle.SafetyMarginMeters + additionalMarginMeters;
        if (!NumericGuard.IsFinite(totalMarginMeters)) return false;
        double halfLengthMeters = vehicle.LengthMeters / 2d + totalMarginMeters;
        double halfWidthMeters = vehicle.WidthMeters / 2d + totalMarginMeters;
        if (!NumericGuard.IsPositiveFinite(halfLengthMeters) || !NumericGuard.IsPositiveFinite(halfWidthMeters)) return false;

        footprint = new VehicleFootprint(pose, halfLengthMeters, halfWidthMeters);
        return NumericGuard.IsFinite(footprint.CircumscribedRadiusMeters) && NumericGuard.IsFinite(footprint.MinX) &&
            NumericGuard.IsFinite(footprint.MaxX) && NumericGuard.IsFinite(footprint.MinY) && NumericGuard.IsFinite(footprint.MaxY);
    }

    /// <summary>获取指定角点。索引按逆时针顺序为 0 到 3。</summary>
    public void GetCorner(int index, out double x, out double y)
    {
        double longitudinalSign;
        double lateralSign;
        switch (index)
        {
            case 0: longitudinalSign = 1d; lateralSign = 1d; break;
            case 1: longitudinalSign = -1d; lateralSign = 1d; break;
            case 2: longitudinalSign = -1d; lateralSign = -1d; break;
            case 3: longitudinalSign = 1d; lateralSign = -1d; break;
            default: throw new ArgumentOutOfRangeException(nameof(index));
        }
        x = CenterX + longitudinalSign * HalfLengthMeters * AxisLongitudinalX + lateralSign * HalfWidthMeters * AxisLateralX;
        y = CenterY + longitudinalSign * HalfLengthMeters * AxisLongitudinalY + lateralSign * HalfWidthMeters * AxisLateralY;
    }
}