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>
    public SmoothedPathValidator(FootprintCollisionChecker collisionChecker)
    {
        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
    }

    /// <summary>
    /// 复核候选平滑路径。每个候选方向段必须保持原始段的端点和换向拓扑；
    /// 输出中的净空均由本次实际车体检查重新计算，绝不沿用候选声明值。
    /// </summary>
    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)
    {
        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 (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, 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;
        }

        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;
            }

            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;
            }

            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 IReadOnlyList<SmoothedPathPoint> EmptyPath()
    {
        return new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
    }
}