using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;

/// <summary>独立复核平滑候选的端点拓扑、车辆曲率和连续车体安全性。</summary>
public sealed class SmoothedPathValidator
{
    private const double Tolerance = 1e-6d;
    private readonly FootprintCollisionChecker _collisionChecker;

    /// <summary>创建使用默认连续车体碰撞检查器的平滑路径验证器。</summary>
    public SmoothedPathValidator()
        : this(new FootprintCollisionChecker())
    {
    }

    /// <summary>创建使用指定连续车体碰撞检查器的平滑路径验证器。</summary>
    /// <param name="collisionChecker">以车辆几何中心位姿执行完整车体和扫掠复核的检查器，不能为 <see langword="null"/>。</param>
    public SmoothedPathValidator(FootprintCollisionChecker collisionChecker)
    {
        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
    }

    /// <summary>复核候选平滑路径，并以默认零转弯半径容差重新计算所有净空。</summary>
    /// <param name="candidatePath">待发布候选点序列；位置/弧长单位为 m、航向为 rad、曲率为 1/m。</param>
    /// <param name="candidateSegments">覆盖候选点的前进/倒车方向段集合。</param>
    /// <param name="originalPath">预处理原始路径，候选必须保留其端点与换向拓扑。</param>
    /// <param name="map">已准备的规划地图快照。</param>
    /// <param name="vehicle">车辆尺寸、安全余量和曲率约束。</param>
    /// <param name="maximumCollisionCheckStepMeters">相邻点车体扫掠最大中心步长，单位 m。</param>
    /// <param name="pathWithClearance">成功时为净空由真实复核重算的只读路径；失败时为空集合。</param>
    /// <param name="minimumClearanceMeters">成功时为完整扩大车体的最小保守净空，单位 m；失败时为 0。</param>
    /// <param name="reason">失败原因；成功时为空字符串。</param>
    /// <returns>端点、换向、曲率、点碰撞和扫掠均通过时为 <see langword="true"/>；否则为 <see langword="false"/>，不发布部分路径。</returns>
    public bool TryValidate(
        IReadOnlyList<SmoothedPathPoint> candidatePath,
        IReadOnlyList<SmoothedPathSegment> candidateSegments,
        PreparedPath originalPath,
        PlanningGridMap map,
        VehicleParameters vehicle,
        double maximumCollisionCheckStepMeters,
        out IReadOnlyList<SmoothedPathPoint> pathWithClearance,
        out double minimumClearanceMeters,
        out string reason)
    {
        return TryValidate(
            candidatePath, candidateSegments, originalPath, map, vehicle, maximumCollisionCheckStepMeters, 0d,
            out pathWithClearance, out minimumClearanceMeters, out reason);
    }

    /// <summary>以只用于曲率上限的显式转弯半径容差复核候选路径。</summary>
    /// <param name="candidatePath">待复核候选点序列。</param>
    /// <param name="candidateSegments">候选方向段集合。</param>
    /// <param name="originalPath">必须保持端点和换向拓扑的预处理原始路径。</param>
    /// <param name="map">规划地图快照。</param>
    /// <param name="vehicle">车辆几何和曲率约束。</param>
    /// <param name="maximumCollisionCheckStepMeters">扫掠复核最大中心步长，单位 m。</param>
    /// <param name="curvatureLimitRadiusToleranceMeters">仅曲率限制使用的最小转弯半径容差，单位 m。</param>
    /// <param name="pathWithClearance">成功时为重新计算净空后的完整只读路径；失败时为空集合。</param>
    /// <param name="minimumClearanceMeters">成功时最小保守净空，单位 m；失败时为 0。</param>
    /// <param name="reason">失败诊断；成功时为空字符串。</param>
    /// <returns>候选满足拓扑、严格弧长、曲率、点碰撞和扫掠约束时为 <see langword="true"/>；否则为 <see langword="false"/>。</returns>
    public bool TryValidate(
        IReadOnlyList<SmoothedPathPoint> candidatePath,
        IReadOnlyList<SmoothedPathSegment> candidateSegments,
        PreparedPath originalPath,
        PlanningGridMap map,
        VehicleParameters vehicle,
        double maximumCollisionCheckStepMeters,
        double curvatureLimitRadiusToleranceMeters,
        out IReadOnlyList<SmoothedPathPoint> pathWithClearance,
        out double minimumClearanceMeters,
        out string reason)
    {
        pathWithClearance = EmptyPath();
        minimumClearanceMeters = 0d;
        reason = string.Empty;
        if (candidatePath == null || candidateSegments == null || originalPath == null || map == null || vehicle == null ||
            candidatePath.Count == 0 || candidateSegments.Count == 0 || !NumericGuard.IsPositiveFinite(maximumCollisionCheckStepMeters))
        {
            reason = "平滑候选、原始路径、地图、车辆或碰撞步长无效。";
            return false;
        }

        if (!CurvatureLimitPolicy.TryGetAllowedMaximumVehicleCurvaturePerMeter(
                vehicle, curvatureLimitRadiusToleranceMeters, out double maximumCurvatureMeters))
        {
            reason = "车辆曲率约束无效。";
            return false;
        }

        if (!TryValidateSegmentTopology(candidatePath, candidateSegments, originalPath, out reason)) return false;
        if (Math.Abs(candidatePath[0].ArcLength) > Tolerance)
        {
            reason = "平滑路径首点的全局弧长必须为零。";
            return false;
        }

        if (!TryGetFiniteClearanceCap(map, out double clearanceCapMeters))
        {
            reason = "Planning map bounds cannot produce a finite clearance cap.";
            return false;
        }

        var checkedClearances = new double[candidatePath.Count];
        double minimumClearance = double.PositiveInfinity;
        for (int index = 0; index < candidatePath.Count; index++)
        {
            SmoothedPathPoint current = candidatePath[index];
            if (!IsValidPoint(current) || Math.Abs(current.VehicleCurvature) > maximumCurvatureMeters + Tolerance)
            {
                reason = "平滑路径包含非法数值或超限车辆曲率。";
                return false;
            }

            var currentPose = new Pose2D(current.X, current.Y, current.Heading);
            if (!_collisionChecker.IsPoseCollisionFree(currentPose, map, vehicle, 0d, out double poseClearance))
            {
                reason = "平滑路径点未通过完整车体碰撞或边界复核。";
                return false;
            }

            if (!TryNormalizeClearance(poseClearance, clearanceCapMeters, out poseClearance))
            {
                reason = "Pose collision verification returned an invalid clearance.";
                return false;
            }

            checkedClearances[index] = poseClearance;
            minimumClearance = Math.Min(minimumClearance, poseClearance);
            if (index == 0) continue;

            SmoothedPathPoint previous = candidatePath[index - 1];
            if (!IsUnwrappedHeadingContinuous(previous, current))
            {
                reason = "平滑路径展开航向不连续。";
                return false;
            }

            if (IsDuplicatePoseAndArcLength(previous, current))
            {
                if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
                {
                    reason = "相邻重复点不是合法换向对。";
                    return false;
                }
                continue;
            }

            if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + Tolerance)
            {
                reason = "非换向点必须保持正弧长增量，换向点必须保留重复位姿。";
                return false;
            }

            var previousPose = new Pose2D(previous.X, previous.Y, previous.Heading);
            if (!_collisionChecker.IsSweptMotionCollisionFree(previousPose, currentPose, map, vehicle,
                maximumCollisionCheckStepMeters, out double sweptClearance))
            {
                reason = "平滑路径相邻点之间的完整车体扫掠碰撞复核失败。";
                return false;
            }

            if (!TryNormalizeClearance(sweptClearance, clearanceCapMeters, out sweptClearance))
            {
                reason = "Swept collision verification returned an invalid clearance.";
                return false;
            }

            checkedClearances[index - 1] = Math.Min(checkedClearances[index - 1], sweptClearance);
            checkedClearances[index] = Math.Min(checkedClearances[index], sweptClearance);
            minimumClearance = Math.Min(minimumClearance, sweptClearance);
        }

        var output = new List<SmoothedPathPoint>(candidatePath.Count);
        for (int index = 0; index < candidatePath.Count; index++)
        {
            SmoothedPathPoint point = candidatePath[index];
            output.Add(new SmoothedPathPoint(
                point.X, point.Y, point.Heading, point.UnwrappedHeading, point.ArcLength, point.Direction,
                point.GeometricCurvature, point.VehicleCurvature, point.VehicleCurvatureDerivative,
                checkedClearances[index], point.IsGearSwitchPoint, point.Source));
        }

        pathWithClearance = new ReadOnlyCollection<SmoothedPathPoint>(output);
        minimumClearanceMeters = minimumClearance;
        return true;
    }

    private static bool TryValidateSegmentTopology(
        IReadOnlyList<SmoothedPathPoint> candidatePath,
        IReadOnlyList<SmoothedPathSegment> candidateSegments,
        PreparedPath originalPath,
        out string reason)
    {
        reason = string.Empty;
        if (originalPath.Segments == null || originalPath.Segments.Count != candidateSegments.Count)
        {
            reason = "平滑路径方向段数量必须保持原始换向拓扑。";
            return false;
        }

        int expectedStartIndex = 0;
        for (int segmentIndex = 0; segmentIndex < candidateSegments.Count; segmentIndex++)
        {
            SmoothedPathSegment candidateSegment = candidateSegments[segmentIndex];
            PreparedDirectionSegment originalSegment = originalPath.Segments[segmentIndex];
            if (candidateSegment == null || originalSegment == null || candidateSegment.SegmentIndex != segmentIndex ||
                candidateSegment.StartIndex != expectedStartIndex || candidateSegment.StartIndex < 0 ||
                candidateSegment.EndIndex < candidateSegment.StartIndex || candidateSegment.EndIndex >= candidatePath.Count ||
                candidateSegment.Direction != originalSegment.Direction ||
                candidateSegment.StartsAtGearSwitch != originalSegment.StartsAtGearSwitch ||
                candidateSegment.EndsAtGearSwitch != originalSegment.EndsAtGearSwitch ||
                originalSegment.Points == null || originalSegment.Points.Count == 0)
            {
                reason = "平滑路径方向段索引、方向或换向拓扑无效。";
                return false;
            }

            SmoothedPathPoint candidateStart = candidatePath[candidateSegment.StartIndex];
            SmoothedPathPoint candidateEnd = candidatePath[candidateSegment.EndIndex];
            SmoothingPoint2D originalStart = originalSegment.Points[0];
            SmoothingPoint2D originalEnd = originalSegment.Points[originalSegment.Points.Count - 1];
            if (!SamePose(candidateStart, originalStart) || !SamePose(candidateEnd, originalEnd) ||
                candidateStart.Direction != originalSegment.Direction || candidateEnd.Direction != originalSegment.Direction ||
                candidateStart.IsGearSwitchPoint != originalStart.IsGearSwitchPoint ||
                candidateEnd.IsGearSwitchPoint != originalEnd.IsGearSwitchPoint)
            {
                reason = "平滑路径改变了原始方向段端点或换向点。";
                return false;
            }

            for (int pointIndex = candidateSegment.StartIndex; pointIndex <= candidateSegment.EndIndex; pointIndex++)
            {
                if (candidatePath[pointIndex] == null || candidatePath[pointIndex].Direction != originalSegment.Direction)
                {
                    reason = "平滑路径方向段包含与段方向不一致的点。";
                    return false;
                }
            }

            expectedStartIndex = candidateSegment.EndIndex + 1;
        }

        if (expectedStartIndex != candidatePath.Count)
        {
            reason = "平滑路径方向段未完整覆盖候选点。";
            return false;
        }
        return true;
    }

    private static bool IsValidPoint(SmoothedPathPoint point)
    {
        return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
            NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
            NumericGuard.IsFinite(point.ArcLength) && point.ArcLength >= 0d &&
            NumericGuard.IsFinite(point.GeometricCurvature) && NumericGuard.IsFinite(point.VehicleCurvature) &&
            NumericGuard.IsFinite(point.VehicleCurvatureDerivative) &&
            IsDirection(point.Direction) && Enum.IsDefined(typeof(SmoothedPathPointSource), point.Source) &&
            Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= Tolerance;
    }

    private static bool SamePose(SmoothedPathPoint candidate, SmoothingPoint2D original)
    {
        return candidate != null && original != null && Math.Abs(candidate.X - original.X) <= Tolerance &&
            Math.Abs(candidate.Y - original.Y) <= Tolerance &&
            Math.Abs(AngleMath.ShortestSignedDifference(candidate.Heading, original.Heading)) <= Tolerance;
    }

    private static bool IsUnwrappedHeadingContinuous(SmoothedPathPoint previous, SmoothedPathPoint current)
    {
        double expectedDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
        return NumericGuard.IsFinite(expectedDelta) &&
            Math.Abs((current.UnwrappedHeading - previous.UnwrappedHeading) - expectedDelta) <= Tolerance;
    }

    private static bool IsDuplicatePoseAndArcLength(SmoothedPathPoint previous, SmoothedPathPoint current)
    {
        return Math.Abs(previous.X - current.X) <= Tolerance && Math.Abs(previous.Y - current.Y) <= Tolerance &&
            Math.Abs(AngleMath.ShortestSignedDifference(previous.Heading, current.Heading)) <= Tolerance &&
            Math.Abs(previous.ArcLength - current.ArcLength) <= Tolerance;
    }

    private static bool IsDirection(TravelDirection direction)
    {
        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
    }

    private static bool TryGetFiniteClearanceCap(PlanningGridMap map, out double capMeters)
    {
        capMeters = 0d;
        if (map == null || map.Bounds == null) return false;
        double widthMeters = (map.Bounds.XMax - map.Bounds.XMin) / 1000d;
        double heightMeters = (map.Bounds.YMax - map.Bounds.YMin) / 1000d;
        capMeters = Math.Sqrt(widthMeters * widthMeters + heightMeters * heightMeters);
        return NumericGuard.IsPositiveFinite(capMeters);
    }

    private static bool TryNormalizeClearance(double clearanceMeters, double capMeters, out double normalizedMeters)
    {
        normalizedMeters = 0d;
        if (double.IsPositiveInfinity(clearanceMeters))
        {
            normalizedMeters = capMeters;
            return true;
        }
        if (!NumericGuard.IsFinite(clearanceMeters) || clearanceMeters < 0d)
            return false;
        normalizedMeters = clearanceMeters;
        return true;
    }

    private static IReadOnlyList<SmoothedPathPoint> EmptyPath()
    {
        return new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
    }
}