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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>
/// Latest-wins rolling coordinator around the pure one-shot planning service.
/// It owns no localization, hardware, UI, or wall-clock source; callers supply each cycle's state and time.
/// </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;

    public EmPlanningCoordinator(IEmPlanningService planningService, IEmPlanningCycleSink sink = null)
    {
        this.planningService = planningService ?? throw new ArgumentNullException(nameof(planningService));
        this.sink = sink;
    }

    public EmTrajectory PublishedTrajectory
    {
        get
        {
            lock (publicationGate)
                return publishedTrajectory;
        }
    }

    /// <summary>Returns whether a caller-supplied time is due for another rolling cycle.</summary>
    public bool ShouldStartCycle(DateTimeOffset now)
    {
        lock (publicationGate)
        {
            return !lastCycleStartedAtUtc.HasValue ||
                now - lastCycleStartedAtUtc.Value >= TimeSpan.FromSeconds(replanPeriodSeconds);
        }
    }

    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>Builds a handoff decision from the coordinator's immutable published trajectory snapshot.</summary>
    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);
    }

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