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

namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;

/// <summary>
/// 将回溯原语装配为调用方可消费的稠密路径和包含式方向分段。
/// 装配器保留原语边界的换向双点，其余相邻重复位姿会被删除。
/// </summary>
public sealed class CoarsePathAssembler
{
    private const double DuplicateTolerance = 1e-8d;
    private readonly FootprintCollisionChecker _collisionChecker;

    /// <summary>创建使用默认连续车体检查器的路径装配器。</summary>
    public CoarsePathAssembler()
        : this(new FootprintCollisionChecker())
    {
    }

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

    /// <summary>
    /// 从已回溯的恒曲率原语构造稠密路径。
    /// 参数：backtrackedPath 提供原语顺序；request 提供地图和车辆；path、segments 为成功时的只读输出。
    /// 返回：首点可通过连续车体检查、每个原语积分点数据一致且方向分段完整覆盖时为 true；否则返回 false 且输出为空。
    /// </summary>
    public bool TryAssemble(BacktrackedPath backtrackedPath, PlanningRequest request,
        out IReadOnlyList<CoarsePathPoint> path, out IReadOnlyList<PathSegment> segments, out string failureReason)
    {
        path = EmptyPath();
        segments = EmptySegments();
        failureReason = string.Empty;
        if (backtrackedPath == null || request == null || request.Map == null || request.Vehicle == null ||
            !IsFinitePose(backtrackedPath.Start) || !IsTravelDirection(backtrackedPath.StartDirection) ||
            !NumericGuard.IsFinite(backtrackedPath.StartCurvaturePerMeter))
        {
            failureReason = "路径装配输入无效。";
            return false;
        }

        if (!_collisionChecker.IsPoseCollisionFree(backtrackedPath.Start, request.Map, request.Vehicle, 0d, out double startClearanceMeters))
        {
            failureReason = "回溯路径起点未通过连续车体检查。";
            return false;
        }

        var points = new List<CoarsePathPoint>();
        TravelDirection currentDirection = backtrackedPath.Primitives.Count > 0
            ? backtrackedPath.Primitives[0].Direction
            : backtrackedPath.StartDirection;
        double currentCurvature = request.StartVehicleCurvature;
        double normalizedStartHeading = AngleMath.NormalizeRadians(backtrackedPath.Start.Heading);
        if (!NumericGuard.IsFinite(normalizedStartHeading))
        {
            failureReason = "回溯路径起点航向无效。";
            return false;
        }

        points.Add(new CoarsePathPoint(backtrackedPath.Start.X, backtrackedPath.Start.Y, normalizedStartHeading,
            backtrackedPath.Start.Heading, 0d, currentDirection, currentCurvature, startClearanceMeters, false,
            CoarsePathPointSource.Start));

        for (int primitiveIndex = 0; primitiveIndex < backtrackedPath.Primitives.Count; primitiveIndex++)
        {
            MotionPrimitive primitive = backtrackedPath.Primitives[primitiveIndex];
            if (!IsValidPrimitive(primitive))
            {
                failureReason = "回溯路径包含无效原语。";
                return false;
            }

            CoarsePathPoint lastPoint = points[points.Count - 1];
            if (primitive.Direction != lastPoint.Direction)
            {
                // 换向处的旧方向终点和新方向起点必须共存，二者位置、航向、弧长完全相同。
                points.Add(new CoarsePathPoint(lastPoint.X, lastPoint.Y, lastPoint.Heading, lastPoint.UnwrappedHeading,
                    lastPoint.ArcLength, primitive.Direction, primitive.CurvaturePerMeter, lastPoint.BodyClearance,
                    true, CoarsePathPointSource.MotionPrimitive));
            }

            Pose2D previousPose = primitive.Start;
            for (int pointIndex = 0; pointIndex < primitive.Points.Count; pointIndex++)
            {
                Pose2D pose = primitive.Points[pointIndex];
                double bodyClearanceMeters = primitive.BodyClearancesMeters[pointIndex];
                if (!IsFinitePose(pose) || !IsValidClearance(bodyClearanceMeters))
                {
                    failureReason = "原语积分点或净空无效。";
                    return false;
                }

                CoarsePathPoint previousPoint = points[points.Count - 1];
                double arcIncrementMeters = CalculateArcIncrement(previousPose, pose, primitive.CurvaturePerMeter);
                if (!NumericGuard.IsFinite(arcIncrementMeters) || arcIncrementMeters <= 0d)
                {
                    failureReason = "原语积分点未产生正弧长。";
                    return false;
                }

                if (IsSamePose(previousPoint, pose))
                {
                    // 非换向情况下不允许重复采样点泄露到对外路径。
                    previousPose = pose;
                    continue;
                }

                double normalizedHeading = AngleMath.NormalizeRadians(pose.Heading);
                double headingDelta = AngleMath.ShortestSignedDifference(previousPoint.Heading, normalizedHeading);
                if (!NumericGuard.IsFinite(normalizedHeading) || !NumericGuard.IsFinite(headingDelta))
                {
                    failureReason = "原语积分点航向无法展开。";
                    return false;
                }

                CoarsePathPointSource source = primitive.IsGoalTruncation && pointIndex == primitive.Points.Count - 1
                    ? CoarsePathPointSource.GoalTruncation
                    : CoarsePathPointSource.MotionPrimitive;
                points.Add(new CoarsePathPoint(pose.X, pose.Y, normalizedHeading,
                    previousPoint.UnwrappedHeading + headingDelta, previousPoint.ArcLength + arcIncrementMeters,
                    primitive.Direction, primitive.CurvaturePerMeter, bodyClearanceMeters, false, source));
                previousPose = pose;
            }
        }

        if (points.Count == 0)
        {
            failureReason = "路径装配未产生起点。";
            return false;
        }

        path = new ReadOnlyCollection<CoarsePathPoint>(points);
        segments = BuildSegments(points);
        return true;
    }

    private static IReadOnlyList<PathSegment> BuildSegments(IReadOnlyList<CoarsePathPoint> points)
    {
        var segments = new List<PathSegment>();
        int startIndex = 0;
        TravelDirection direction = points[0].Direction;
        for (int index = 1; index < points.Count; index++)
        {
            if (points[index].Direction == direction) continue;
            segments.Add(new PathSegment(segments.Count, direction, startIndex, index - 1,
                points[startIndex].IsGearSwitchPoint, true));
            startIndex = index;
            direction = points[index].Direction;
        }

        segments.Add(new PathSegment(segments.Count, direction, startIndex, points.Count - 1,
            points[startIndex].IsGearSwitchPoint, false));
        return new ReadOnlyCollection<PathSegment>(segments);
    }

    private static double CalculateArcIncrement(Pose2D from, Pose2D to, double curvaturePerMeter)
    {
        if (!IsFinitePose(from) || !IsFinitePose(to) || !NumericGuard.IsFinite(curvaturePerMeter)) return double.NaN;
        if (Math.Abs(curvaturePerMeter) < 1e-12d)
        {
            double deltaX = to.X - from.X;
            double deltaY = to.Y - from.Y;
            return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
        }

        double headingDelta = AngleMath.ShortestSignedDifference(from.Heading, to.Heading);
        return Math.Abs(headingDelta / curvaturePerMeter);
    }

    private static bool IsValidPrimitive(MotionPrimitive primitive)
    {
        return primitive != null && primitive.Start != null && primitive.Points != null && primitive.BodyClearancesMeters != null &&
            primitive.Points.Count == primitive.BodyClearancesMeters.Count && primitive.Points.Count > 0 &&
            IsTravelDirection(primitive.Direction) && NumericGuard.IsFinite(primitive.CurvaturePerMeter) &&
            NumericGuard.IsPositiveFinite(primitive.ActualLengthMeters);
    }

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

    private static bool IsFinitePose(Pose2D pose)
    {
        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
    }

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

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

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

    private static IReadOnlyList<PathSegment> EmptySegments()
    {
        return new ReadOnlyCollection<PathSegment>(new List<PathSegment>());
    }
}