using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;

/// <summary>
/// 对准备发布的粗路径执行独立的连续安全与输出契约复核。
/// 复核失败的路径不得被包装为 <see cref="PlanningStatus.Success"/>。
/// </summary>
public sealed class CoarsePathValidator
{
    private const double NumericTolerance = 1e-6d;
    private readonly FootprintCollisionChecker _collisionChecker;

    /// <summary>创建使用默认连续车体检查器的最终路径复核器。</summary>
    public CoarsePathValidator()
        : this(new FootprintCollisionChecker())
    {
    }

    /// <summary>创建使用指定连续车体检查器的最终路径复核器。</summary>
    public CoarsePathValidator(FootprintCollisionChecker collisionChecker)
    {
        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
    }

    /// <summary>
    /// 复核路径数值、曲率、连续扫掠碰撞、终点约束、累计弧长和方向分段。
    /// 参数：path 与 segments 为待发布输出；request 必须是生成该路径的请求；minimumBodyClearanceMeters 返回沿途的保守净空下界。
    /// 返回：所有规则通过时为 true；否则返回 false、写入失败原因，调用方必须丢弃 path 和 segments。
    /// </summary>
    public bool TryValidate(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments,
        PlanningRequest request, out double minimumBodyClearanceMeters, out string failureReason)
    {
        minimumBodyClearanceMeters = 0d;
        failureReason = string.Empty;
        if (path == null || segments == null || request == null || request.Map == null || request.Vehicle == null ||
            request.Configuration == null || path.Count == 0 || segments.Count == 0)
        {
            failureReason = "最终路径或复核请求为空。";
            return false;
        }

        if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double maximumCurvaturePerMeter))
        {
            failureReason = "车辆曲率约束无效。";
            return false;
        }

        CoarsePathPoint first = path[0];
        if (!IsValidPoint(first) || first.Source != CoarsePathPointSource.Start || first.IsGearSwitchPoint ||
            Math.Abs(first.ArcLength) > NumericTolerance || !IsSamePose(first, request.Start))
        {
            failureReason = "最终路径首点不符合起点契约。";
            return false;
        }

        double minimumClearance = double.PositiveInfinity;
        for (int index = 0; index < path.Count; index++)
        {
            CoarsePathPoint current = path[index];
            if (!IsValidPoint(current) || Math.Abs(current.VehicleCurvature) > maximumCurvaturePerMeter + NumericTolerance)
            {
                failureReason = "最终路径包含非法数值或超限曲率。";
                return false;
            }

            var currentPose = new Pose2D(current.X, current.Y, current.Heading);
            if (!_collisionChecker.IsPoseCollisionFree(currentPose, request.Map, request.Vehicle, 0d, out double poseClearanceMeters) ||
                IsClearanceOverclaimed(current.BodyClearance, poseClearanceMeters))
            {
                failureReason = "最终路径点未通过连续车体碰撞复核。";
                return false;
            }

            minimumClearance = Math.Min(minimumClearance, current.BodyClearance);
            if (index == 0) continue;

            CoarsePathPoint previous = path[index - 1];
            if (current.ArcLength + NumericTolerance < previous.ArcLength ||
                !IsUnwrappedHeadingContinuous(previous, current))
            {
                failureReason = "最终路径的弧长或展开航向不连续。";
                return false;
            }

            bool isDuplicate = IsDuplicatePoseAndArcLength(previous, current);
            if (isDuplicate)
            {
                if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
                {
                    failureReason = "相邻重复点不是合法换向对。";
                    return false;
                }
            }
            else
            {
                if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + NumericTolerance ||
                    !IsArcIncrementConsistent(previous, current))
                {
                    failureReason = "非换向路径点的弧长增量不一致。";
                    return false;
                }
            }

            var previousPose = new Pose2D(previous.X, previous.Y, previous.Heading);
            if (!_collisionChecker.IsSweptMotionCollisionFree(previousPose, currentPose, request.Map, request.Vehicle,
                request.Configuration.MaximumCollisionCheckStepMeters, out double sweptClearanceMeters))
            {
                failureReason = "最终路径相邻点之间的车体扫掠碰撞复核失败。";
                return false;
            }

            minimumClearance = Math.Min(minimumClearance, sweptClearanceMeters);
        }

        CoarsePathPoint last = path[path.Count - 1];
        if (!GoalToleranceChecker.IsSatisfied(new Pose2D(last.X, last.Y, last.Heading), request.Goal, request.Configuration,
            last.Direction, request.GoalDirection) || (path.Count > 1 && last.Source != CoarsePathPointSource.GoalTruncation))
        {
            failureReason = "最终路径末点未满足终点容差、方向或来源契约。";
            return false;
        }

        if (!AreSegmentsValid(path, segments, out failureReason)) return false;
        minimumBodyClearanceMeters = minimumClearance;
        return true;
    }

    private static bool AreSegmentsValid(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments, out string failureReason)
    {
        failureReason = string.Empty;
        int expectedStartIndex = 0;
        for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
        {
            PathSegment segment = segments[segmentIndex];
            if (segment == null || segment.SegmentIndex != segmentIndex || segment.StartIndex != expectedStartIndex ||
                segment.StartIndex < 0 || segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count ||
                segment.StartsAtGearSwitch != path[segment.StartIndex].IsGearSwitchPoint)
            {
                failureReason = "方向分段索引或起始换向标记无效。";
                return false;
            }

            for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
            {
                if (path[pointIndex].Direction != segment.Direction)
                {
                    failureReason = "方向分段包含不同方向的路径点。";
                    return false;
                }
            }

            bool hasNextSegment = segmentIndex + 1 < segments.Count;
            bool expectedEndsAtGearSwitch = hasNextSegment && segment.EndIndex + 1 < path.Count &&
                path[segment.EndIndex + 1].IsGearSwitchPoint;
            if (segment.EndsAtGearSwitch != expectedEndsAtGearSwitch)
            {
                failureReason = "方向分段末尾换向标记无效。";
                return false;
            }

            expectedStartIndex = segment.EndIndex + 1;
        }

        if (expectedStartIndex != path.Count)
        {
            failureReason = "方向分段未完整覆盖路径点。";
            return false;
        }

        return true;
    }

    private static bool IsValidPoint(CoarsePathPoint point)
    {
        if (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.VehicleCurvature) || !IsTravelDirection(point.Direction) ||
            !IsValidClearance(point.BodyClearance))
            return false;

        return Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= NumericTolerance;
    }

    private static bool IsSamePose(CoarsePathPoint point, Pose2D pose)
    {
        return point != null && pose != null && Math.Abs(point.X - pose.X) <= NumericTolerance &&
            Math.Abs(point.Y - pose.Y) <= NumericTolerance &&
            Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, pose.Heading)) <= NumericTolerance;
    }

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

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

    private static bool IsArcIncrementConsistent(CoarsePathPoint previous, CoarsePathPoint current)
    {
        double expectedIncrement;
        if (Math.Abs(current.VehicleCurvature) < 1e-12d)
        {
            double deltaX = current.X - previous.X;
            double deltaY = current.Y - previous.Y;
            expectedIncrement = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
        }
        else
        {
            double headingDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
            expectedIncrement = Math.Abs(headingDelta / current.VehicleCurvature);
        }

        return NumericGuard.IsFinite(expectedIncrement) && expectedIncrement > 0d &&
            Math.Abs((current.ArcLength - previous.ArcLength) - expectedIncrement) <= NumericTolerance;
    }

    private static bool IsClearanceOverclaimed(double reportedClearanceMeters, double checkedClearanceMeters)
    {
        if (double.IsPositiveInfinity(reportedClearanceMeters)) return !double.IsPositiveInfinity(checkedClearanceMeters);
        return reportedClearanceMeters > checkedClearanceMeters + NumericTolerance;
    }

    private static bool IsValidClearance(double clearanceMeters)
    {
        return !double.IsNaN(clearanceMeters) && clearanceMeters >= 0d;
    }

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