using System;
using System.Diagnostics;
using System.Globalization;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.CoarsePath;

/// <summary>
/// 已建 <see cref="PlanningGridMap"/> 上 Hybrid A* 粗路径规划的下层门面。
/// 本类不建图、不读取 UI 或传感器；只有路径经回溯、装配和最终连续复核后才发布成功结果。
/// </summary>
public sealed class HybridAStarPlanner
{
    private readonly HybridAStarSearch _search;
    private readonly PathBacktracker _backtracker;
    private readonly CoarsePathAssembler _assembler;
    private readonly CoarsePathValidator _validator;
    private readonly FootprintCollisionChecker _collisionChecker;

    /// <summary>创建使用默认搜索、回溯、装配和最终复核组件的规划器。</summary>
    public HybridAStarPlanner()
        : this(new HybridAStarSearch(), new PathBacktracker(), new CoarsePathAssembler(), new CoarsePathValidator(),
            new FootprintCollisionChecker())
    {
    }

    internal HybridAStarPlanner(HybridAStarSearch search, PathBacktracker backtracker, CoarsePathAssembler assembler,
        CoarsePathValidator validator, FootprintCollisionChecker collisionChecker)
    {
        _search = search ?? throw new ArgumentNullException(nameof(search));
        _backtracker = backtracker ?? throw new ArgumentNullException(nameof(backtracker));
        _assembler = assembler ?? throw new ArgumentNullException(nameof(assembler));
        _validator = validator ?? throw new ArgumentNullException(nameof(validator));
        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
    }

    /// <summary>
    /// 在请求提供的不可变规划地图上执行一次 Hybrid A* 粗路径规划。
    /// 参数：request 的位置单位为 m、航向单位为 rad、曲率单位为 1/m；cancellationToken 会在搜索扩展检查点取消。
    /// 返回：输入、边界、碰撞、搜索、回溯或最终复核失败均返回空路径；只有 <see cref="PlanningStatus.Success"/> 携带完整路径和分段。
    /// </summary>
    public PlanningResult Plan(PlanningRequest request, CancellationToken cancellationToken = default(CancellationToken))
    {
        PlanningOperationBudget budget = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
            ? new PlanningOperationBudget(cancellationToken, request.Configuration.SearchTimeout)
            : PlanningOperationBudget.Unlimited(cancellationToken);
        return Plan(request, budget);
    }

    /// <summary>使用门面传入的共享预算执行预检、搜索和最终路径复核。</summary>
    internal PlanningResult Plan(PlanningRequest request, PlanningOperationBudget budget)
    {
        Stopwatch pathSearchStopwatch = null;
        try
        {
            if (budget == null) throw new ArgumentNullException(nameof(budget));
            PlanningOperationStopReason stopReason = budget.GetStopReason();
            if (stopReason != PlanningOperationStopReason.None)
                return Failure(ToPlanningStatus(stopReason), budget, "规划在开始前已停止。", null);

            PlanningStatus preflightStatus = ValidatePreflight(request, out string preflightReason);
            if (preflightStatus != PlanningStatus.Success)
                return Failure(preflightStatus, budget, preflightReason, null);

            if (!IsFootprintInsideMap(request.Start, request.Map, request.Vehicle))
                return Failure(PlanningStatus.StartOutsideMap, budget, "起始扩大车体不完全位于地图边界内。", null);
            if (!_collisionChecker.IsPoseCollisionFree(request.Start, request.Map, request.Vehicle, 0d, out _))
                return Failure(PlanningStatus.StartInCollision, budget, "起始扩大车体与障碍物相交或擦边。", null);
            if (!IsFootprintInsideMap(request.Goal, request.Map, request.Vehicle))
                return Failure(PlanningStatus.GoalOutsideMap, budget, "目标扩大车体不完全位于地图边界内。", null);
            if (!_collisionChecker.IsPoseCollisionFree(request.Goal, request.Map, request.Vehicle, 0d, out _))
                return Failure(PlanningStatus.GoalInCollision, budget, "目标扩大车体与障碍物相交或擦边。", null);

            stopReason = budget.GetStopReason();
            if (stopReason != PlanningOperationStopReason.None)
                return Failure(ToPlanningStatus(stopReason), budget, "规划在搜索前已停止。", null);
            pathSearchStopwatch = Stopwatch.StartNew();
            HybridAStarSearchResult searchResult = _search.Search(request, budget);
            if (searchResult == null)
                return Failure(PlanningStatus.InternalError, budget, "搜索器未返回结果。", null, pathSearchStopwatch);
            if (searchResult.Status != PlanningStatus.Success)
                return Failure(searchResult.Status, budget,
                    BuildSearchFailureReason(searchResult, request.Configuration), searchResult, pathSearchStopwatch);

            if (!_backtracker.TryBacktrack(searchResult, request, out BacktrackedPath backtrackedPath, out string backtrackingReason))
                return Failure(PlanningStatus.BacktrackingFailed, budget, backtrackingReason, searchResult, pathSearchStopwatch);
            if (!_assembler.TryAssemble(backtrackedPath, request, out var path, out var segments, out string assemblyReason))
                return Failure(PlanningStatus.FinalValidationFailed, budget, assemblyReason, searchResult, pathSearchStopwatch);
            if (!_validator.TryValidate(path, segments, request, out double minimumClearanceMeters, out string validationReason))
                return Failure(PlanningStatus.FinalValidationFailed, budget, validationReason, searchResult, pathSearchStopwatch);

            double pathLengthMeters = path[path.Count - 1].ArcLength;
            return PlanningResult.Success(path, segments, CreateDiagnostics(searchResult, budget.Elapsed, pathLengthMeters,
                minimumClearanceMeters, string.Empty, pathSearchStopwatch.Elapsed));
        }
        catch (Exception exception)
        {
            string reason = "规划内部错误：" + exception.GetType().Name +
                (string.IsNullOrEmpty(exception.Message) ? "。" : "。" + exception.Message);
            return Failure(PlanningStatus.InternalError, budget ?? PlanningOperationBudget.Unlimited(CancellationToken.None),
                reason, null, pathSearchStopwatch);
        }
    }

    private static PlanningStatus ValidatePreflight(PlanningRequest request, out string failureReason)
    {
        failureReason = string.Empty;
        if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
            !IsFinitePose(request.Start) || !IsFinitePose(request.Goal) || !NumericGuard.IsFinite(request.StartVehicleCurvature) ||
            !IsGoalDirection(request.GoalDirection) || (request.StartDirection.HasValue && !IsTravelDirection(request.StartDirection.Value)))
        {
            failureReason = "规划请求缺少必要对象或包含非法数值。";
            return PlanningStatus.InvalidRequest;
        }

        PlanningGridMap map = request.Map;
        if (map.Bounds == null || map.Rows <= 0 || map.Cols <= 0 || !NumericGuard.IsPositiveFinite(map.ResolutionMeters))
        {
            failureReason = "规划地图结构无效。";
            return PlanningStatus.InvalidMap;
        }
        if (!map.PlanningReady)
        {
            failureReason = string.IsNullOrEmpty(map.PlanningBlockReason) ? "规划地图尚未就绪。" : map.PlanningBlockReason;
            return PlanningStatus.MapNotReady;
        }

        VehicleParameters vehicle = request.Vehicle;
        if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) || !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
            !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
        {
            failureReason = "车辆尺寸、安全余量或曲率限制无效。";
            return PlanningStatus.InvalidVehicleParameters;
        }

        HybridAStarConfiguration configuration = request.Configuration;
        if (!IsValidConfiguration(configuration) || Math.Abs(request.StartVehicleCurvature) > maximumCurvaturePerMeter ||
            (request.StartDirection == TravelDirection.Reverse && !configuration.AllowReverse))
        {
            failureReason = "Hybrid A* 曲率、离散、代价或资源配置无效。";
            return PlanningStatus.InvalidCurvatureConfiguration;
        }

        return PlanningStatus.Success;
    }

    private static bool IsValidConfiguration(HybridAStarConfiguration configuration)
    {
        return NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) &&
            NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) &&
            NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) &&
            NumericGuard.IsPositiveFinite(configuration.HeadingResolutionRadians) &&
            configuration.HeadingResolutionRadians <= 2d * Math.PI && configuration.CurvatureLevelCount >= 3 &&
            configuration.CurvatureLevelCount % 2 == 1 && NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) &&
            configuration.GoalPositionToleranceMeters >= 0d && NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) &&
            configuration.GoalHeadingToleranceRadians >= 0d && configuration.MaximumExpandedNodes >= 0 &&
            configuration.SearchTimeout >= TimeSpan.Zero && NumericGuard.IsFinite(configuration.HeuristicWeight) &&
            configuration.HeuristicWeight >= 0d && NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier) &&
            NumericGuard.IsFinite(configuration.GearSwitchPenaltyMeters) && configuration.GearSwitchPenaltyMeters >= 0d &&
            NumericGuard.IsFinite(configuration.CurvatureMagnitudeWeight) && configuration.CurvatureMagnitudeWeight >= 0d &&
            NumericGuard.IsFinite(configuration.CurvatureChangePenaltyMetersPerLevel) &&
            configuration.CurvatureChangePenaltyMetersPerLevel >= 0d && NumericGuard.IsFinite(configuration.ClearanceCostWeight) &&
            configuration.ClearanceCostWeight >= 0d && NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters);
    }

    private static bool IsFootprintInsideMap(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle)
    {
        if (!map.TryWorldToGrid(pose.X, pose.Y, out _, out _)) return false;

        double halfLengthMeters = vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters;
        double halfWidthMeters = vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters;
        double longitudinalX = Math.Cos(pose.Heading);
        double longitudinalY = Math.Sin(pose.Heading);
        double lateralX = -longitudinalY;
        double lateralY = longitudinalX;
        for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
        for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
        {
            double cornerX = pose.X + longitudinalSign * halfLengthMeters * longitudinalX + lateralSign * halfWidthMeters * lateralX;
            double cornerY = pose.Y + longitudinalSign * halfLengthMeters * longitudinalY + lateralSign * halfWidthMeters * lateralY;
            if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
        }

        return true;
    }

    private static string BuildSearchFailureReason(HybridAStarSearchResult searchResult,
        HybridAStarConfiguration configuration)
    {
        string reason = string.IsNullOrEmpty(searchResult.TerminationReason)
            ? "Hybrid A* 搜索以 " + searchResult.Status + " 状态终止。"
            : searchResult.TerminationReason;
        string resourceLimit = string.Empty;
        if (searchResult.Status == PlanningStatus.SearchTimeout)
        {
            resourceLimit = "总预算=" + configuration.SearchTimeout.TotalSeconds.ToString(
                "F3", CultureInfo.InvariantCulture) + "s；";
        }
        else if (searchResult.Status == PlanningStatus.SearchNodeLimitExceeded)
        {
            resourceLimit = "节点上限=" + configuration.MaximumExpandedNodes.ToString(
                CultureInfo.InvariantCulture) + "；";
        }

        return reason + resourceLimit +
            "扩展=" + searchResult.ExpandedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
            "生成=" + searchResult.GeneratedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
            "重开=" + searchResult.ReopenedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
            "陈旧条目=" + searchResult.StaleOpenListEntryCount.ToString(CultureInfo.InvariantCulture) + "，" +
            "Open List峰值=" + searchResult.PeakOpenListCount.ToString(CultureInfo.InvariantCulture) + "。";
    }

    private static PlanningResult Failure(PlanningStatus status, PlanningOperationBudget budget, string reason,
        HybridAStarSearchResult searchResult, Stopwatch pathSearchStopwatch = null)
    {
        TimeSpan pathSearchElapsed = pathSearchStopwatch == null ? TimeSpan.Zero : pathSearchStopwatch.Elapsed;
        return PlanningResult.Failure(status, CreateDiagnostics(searchResult, budget.Elapsed, 0d, 0d,
            reason, pathSearchElapsed));
    }

    private static PlanningDiagnostics CreateDiagnostics(HybridAStarSearchResult searchResult, TimeSpan elapsed,
        double pathLengthMeters, double minimumClearanceMeters, string reason, TimeSpan pathSearchElapsed)
    {
        return new PlanningDiagnostics(
            searchResult == null ? 0 : searchResult.ExpandedNodeCount,
            searchResult == null ? 0 : searchResult.GeneratedNodeCount,
            searchResult == null ? 0 : searchResult.ReopenedNodeCount,
            searchResult == null ? 0 : searchResult.StaleOpenListEntryCount,
            searchResult == null ? 0 : searchResult.PeakOpenListCount,
            pathLengthMeters,
            minimumClearanceMeters,
            elapsed,
            reason,
            pathSearchElapsed);
    }

    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 IsGoalDirection(GoalDirectionConstraint direction)
    {
        return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
    }

    private static PlanningStatus ToPlanningStatus(PlanningOperationStopReason stopReason)
    {
        if (stopReason == PlanningOperationStopReason.Cancelled) return PlanningStatus.Cancelled;
        if (stopReason == PlanningOperationStopReason.TimedOut) return PlanningStatus.SearchTimeout;
        throw new ArgumentOutOfRangeException(nameof(stopReason));
    }
}