using System;
using System.Threading;
using System.Threading.Tasks;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>
/// 围绕纯单次 <see cref="IEmPlanningService"/> 的 latest-wins 滚动规划协调器。
/// 它只使用调用方提供的状态快照和时钟值；不读取定位、硬件、UI 或系统时钟，并且只原子发布当前身份的完整成功轨迹。
/// </summary>
public sealed class EmPlanningCoordinator
{
    private readonly IEmPlanningService planningService;
    private readonly IEmPlanningCycleSink sink;
    private readonly TrajectoryHandoffSelector handoffSelector = new TrajectoryHandoffSelector();
    private readonly object publicationGate = new object();
    private long latestCycleVersion;
    private PlanningCycleIdentity latestIdentity;
    private CancellationTokenSource latestCancellation;
    private EmTrajectory publishedTrajectory;
    private DateTimeOffset? lastCycleStartedAtUtc;
    private double replanPeriodSeconds = 0.20d;

    /// <summary>创建滚动规划协调器。</summary>
    /// <param name="planningService">同步纯规划服务；每个周期在后台任务中调用其 <c>Plan</c> 方法。</param>
    /// <param name="sink">可选周期完成观察器；观察器异常只追加诊断，不能回滚发布结果。</param>
    public EmPlanningCoordinator(IEmPlanningService planningService, IEmPlanningCycleSink sink = null)
    {
        this.planningService = planningService ?? throw new ArgumentNullException(nameof(planningService));
        this.sink = sink;
    }

    /// <summary>最后一条已原子发布的完整不可变轨迹；尚无成功周期或失败周期未替换旧轨迹时可为 <see langword="null"/>。</summary>
    public EmTrajectory PublishedTrajectory
    {
        get
        {
            lock (publicationGate)
                return publishedTrajectory;
        }
    }

    /// <summary>判断调用方提供的时刻是否到达下一次滚动规划周期。</summary>
    /// <param name="now">调用方冻结的当前时间；协调器不读取墙上时钟。</param>
    /// <returns>尚未启动周期或距上次开始已达到配置周期时为 <see langword="true"/>；否则为 <see langword="false"/>。</returns>
    public bool ShouldStartCycle(DateTimeOffset now)
    {
        lock (publicationGate)
        {
            return !lastCycleStartedAtUtc.HasValue ||
                now - lastCycleStartedAtUtc.Value >= TimeSpan.FromSeconds(replanPeriodSeconds);
        }
    }

    /// <summary>异步启动一个 latest-wins 滚动周期，并取消此前仍在执行的周期。</summary>
    /// <param name="input">调用方冻结的规划请求、完整周期身份和 caller-clocked 时刻；其地图、路径和状态版本必须一致。</param>
    /// <param name="cancellationToken">调用方取消令牌；会与本周期内部令牌链接。</param>
    /// <returns>完成后给出不可变 <see cref="PlanningCycleResult"/>；只有版本和身份仍当前且结果成功完整时其 <c>Published</c> 为 <see langword="true"/>。</returns>
    public Task<PlanningCycleResult> PlanLatestAsync(PlanningCycleInput input, CancellationToken cancellationToken)
    {
        if (input == null)
            throw new ArgumentNullException(nameof(input));

        long version;
        CancellationTokenSource cycleCancellation;
        lock (publicationGate)
        {
            latestCancellation?.Cancel();
            latestCancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
            cycleCancellation = latestCancellation;
            version = ++latestCycleVersion;
            latestIdentity = input.Identity;
            lastCycleStartedAtUtc = input.Now;
            replanPeriodSeconds = input.ReplanPeriodSeconds;
        }

        return Task.Run(() => CompleteCycle(version, input, cycleCancellation));
    }

    /// <summary>从协调器当前已发布的不可变轨迹构建下一周期的安全交接决策。</summary>
    /// <param name="input">下一周期的请求、状态快照和时刻。</param>
    /// <param name="expectedDirection">下一周期期望的实际行驶方向。</param>
    /// <returns>同段、同方向、年龄和追踪误差均安全时返回未来轨迹样本；否则返回调用方测量状态和拒绝原因。</returns>
    public TrajectoryHandoffSelection SelectHandoff(PlanningCycleInput input, TravelDirection expectedDirection)
    {
        if (input == null)
            throw new ArgumentNullException(nameof(input));

        EmTrajectory trajectory;
        lock (publicationGate)
            trajectory = publishedTrajectory;
        return handoffSelector.Select(trajectory, input.Request.VehicleState, input.Identity.SegmentIndex, expectedDirection,
            input.Now, input.Request.Configuration);
    }

    /// <summary>执行一次后台纯规划并在锁内比较版本和身份后决定是否发布。</summary>
    /// <param name="version">启动时分配的单调递增周期版本。</param>
    /// <param name="input">该周期的不可变请求、身份和 caller-clocked 时刻。</param>
    /// <param name="cycleCancellation">本周期与调用方取消状态链接的令牌源。</param>
    /// <returns>包含原始或 Superseded 规划结果、发布标记和观察器诊断的周期记录。</returns>
    private PlanningCycleResult CompleteCycle(long version, PlanningCycleInput input,
        CancellationTokenSource cycleCancellation)
    {
        EmPlanningResult planned;
        try
        {
            planned = planningService.Plan(input.Request, cycleCancellation.Token);
            if (planned == null)
                planned = new EmPlanningResult(EmPlanningStatus.Failed, null, "planning service returned no result");
        }
        catch (OperationCanceledException)
        {
            planned = new EmPlanningResult(EmPlanningStatus.Cancelled, null, "planning cycle cancelled");
        }
        catch (Exception exception)
        {
            planned = new EmPlanningResult(EmPlanningStatus.Failed, null, "planning service exception: " + exception.Message);
        }

        PlanningCycleResult result;
        lock (publicationGate)
        {
            bool current = version == latestCycleVersion && input.Identity.Equals(latestIdentity);
            if (!current)
            {
                var superseded = new EmPlanningResult(EmPlanningStatus.Superseded, null,
                    "rolling cycle superseded before publication");
                result = new PlanningCycleResult(version, input.Identity, superseded, false, superseded.FailureReason);
            }
            else
            {
                bool publishable = (planned.Status == EmPlanningStatus.Success || planned.Status == EmPlanningStatus.SuccessWithFallback) &&
                    planned.Trajectory != null;
                if (publishable)
                    publishedTrajectory = planned.Trajectory;
                result = new PlanningCycleResult(version, input.Identity, planned, publishable, planned.FailureReason);
            }
        }

        if (sink == null)
            return result;

        try
        {
            sink.OnCycleCompleted(result);
            return result;
        }
        catch (Exception exception)
        {
            string diagnostic = string.IsNullOrEmpty(result.Diagnostic)
                ? "cycle sink exception: " + exception.Message
                : result.Diagnostic + "; cycle sink exception: " + exception.Message;
            return result.WithDiagnostic(diagnostic);
        }
    }
}
