using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Globalization;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Runs one deterministic EM LS/ST planning pipeline and publishes only independently validated trajectories.</summary>
public sealed class EmPlanningService : IEmPlanningService
{
    private static readonly TimeSpan MaximumCycleDeadlineRemaining =
        TimeSpan.FromMilliseconds(int.MaxValue);
    private readonly IQpSolver qpSolver;
    private readonly IEmPlannerDebugSink defaultDebugSink;
    private readonly Func<TimeSpan> cycleElapsedForTesting;

    public EmPlanningService(IQpSolver qpSolver, IEmPlannerDebugSink defaultDebugSink = null)
        : this(qpSolver, defaultDebugSink, null)
    {
    }

    internal EmPlanningService(IQpSolver qpSolver, IEmPlannerDebugSink defaultDebugSink,
        Func<TimeSpan> cycleElapsedForTesting)
    {
        this.qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
        this.defaultDebugSink = defaultDebugSink;
        this.cycleElapsedForTesting = cycleElapsedForTesting;
    }

    public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
    {
        Stopwatch cycleStopwatch = cycleElapsedForTesting == null ? Stopwatch.StartNew() : null;
        Func<TimeSpan> cycleElapsed = cycleElapsedForTesting ?? (() => cycleStopwatch.Elapsed);
        if (CallerCancellationRequested(request, cancellationToken))
            return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before request validation.");
        if (request?.CycleDeadlineRemaining is TimeSpan suppliedRemaining &&
            (suppliedRemaining < TimeSpan.Zero || suppliedRemaining > MaximumCycleDeadlineRemaining))
        {
            return Failure(EmPlanningStatus.InvalidInput, request,
                "CycleDeadlineRemaining must be between zero and " +
                MaximumCycleDeadlineRemaining.TotalMilliseconds + " milliseconds.");
        }
        if (DeadlineExpired(request, cycleElapsed))
            return CycleDeadlineFailure(request, "request", CycleRemaining(request, cycleElapsed));

        EmPlanningRequestValidationResult requestValidation = EmPlanningRequestValidator.Validate(request);
        if (!requestValidation.IsValid)
            return Failure(requestValidation.Status, request, requestValidation.FailureReason);
        EmPlannerConfiguration configuration = requestValidation.Snapshot.Configuration;
        EmitDebug(request, "request/config validation succeeded");
        if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
                "request-validation", out EmPlanningResult validationFailure))
        {
            return validationFailure;
        }
        TimeSpan expirationRemaining = CycleRemaining(request, cycleElapsed);
        using var cycleExpiration = request.CycleDeadlineRemaining.HasValue
            ? new CancellationTokenSource(expirationRemaining)
            : null;
        using var linkedCancellation = CancellationTokenSource.CreateLinkedTokenSource(
            cancellationToken, request.CallerCancellationToken, request.CycleDeadlineToken,
            cycleExpiration?.Token ?? CancellationToken.None);
        CancellationToken planningCancellationToken = linkedCancellation.Token;

        try
        {
            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
                    "projection", out EmPlanningResult deadlineFailure))
                return deadlineFailure;
            IReadOnlyList<DirectionSegmentView> segments = ReferencePathSegmenter.Create(request.ReferencePath);
            if (request.SegmentIndex < 0 || request.SegmentIndex >= segments.Count)
                return Failure(EmPlanningStatus.InvalidReferencePath, request, "The requested direction segment is unavailable.");
            DirectionSegmentView segment = segments[request.SegmentIndex];
            if (!HasCompatibleStateDirection(request.VehicleState, segment.Direction,
                    configuration.Longitudinal.StopSpeedToleranceMetersPerSecond))
            {
                return Failure(EmPlanningStatus.StateDirectionMismatch, request,
                    "Vehicle signed speed contradicts the selected direction segment.");
            }
            EmitDebug(request, "direction-segment selection succeeded");

            var projector = new FrenetProjector();
            double startProjectionUpperS = request.PlanningScope == EmPlanningScope.FullDirectionSegment
                ? Math.Min(segment.LengthMeters, configuration.Frenet.MaximumProjectionDistanceMeters)
                : segment.LengthMeters;
            if (!projector.TryProject(request.VehicleState.Pose, segment, 0d, startProjectionUpperS,
                    configuration.Frenet.MaximumProjectionDistanceMeters, 0d, out FrenetProjection startProjection))
            {
                return Failure(EmPlanningStatus.ProjectionFailed, request,
                    "Vehicle pose could not be projected at an admissible start of the selected direction segment.");
            }
            if (Math.Abs(startProjection.HeadingError) >= Math.PI / 2d)
            {
                return Failure(EmPlanningStatus.ProjectionFailed, request,
                    "Vehicle travel heading differs by at least 90 degrees from the selected direction segment.");
            }
            EmitDebug(request, "bounded ego projection succeeded");
            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "projection", out deadlineFailure))
                return deadlineFailure;

            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "corridor", out deadlineFailure))
                return deadlineFailure;
            double initialProgressSpeed = Math.Abs(request.VehicleState.SignedLongitudinalSpeedMetersPerSecond);
            double initialAcceleration = request.VehicleState.LongitudinalAccelerationMetersPerSecondSquared ?? 0d;
            var horizonSelector = new PlanningHorizonSelector();
            EmPlanningStatus horizonStatus = horizonSelector.Select(segment, startProjection.ReferenceS, initialProgressSpeed,
                initialAcceleration, request.PlanningScope, configuration, out PlanningHorizonSelection horizon,
                out string horizonReason);
            if (horizonStatus != EmPlanningStatus.Success)
                return Failure(horizonStatus, request, horizonReason);
            ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.WindowEndReferenceS);
            EmitDebug(request, "exact horizon and terminal selection succeeded");

            IReadOnlyList<FrenetProjection> previousSeed = ProjectPreviousTrajectorySeed(request.PreviousTrajectory, segment,
                startProjection.ReferenceS, horizon.WindowEndReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters);
            var corridorSeed = new List<FrenetProjection>(previousSeed.Count + 1) { startProjection };
            for (int index = 0; index < previousSeed.Count; index++) corridorSeed.Add(previousSeed[index]);
            EmitDebug(request, "previous-trajectory seed projection completed");

            var corridorBuilder = new StaticCorridorBuilder();
            if (!corridorBuilder.TryBuild(segment, startProjection.ReferenceS, slice.TerminalBoundary.SegmentLocalS, corridorSeed,
                    request.Map, request.Vehicle, configuration.Corridor, out StaticCorridor corridor, out string corridorReason))
            {
                return Failure(EmPlanningStatus.CorridorInfeasible, request, corridorReason);
            }
            EmitDebug(request, "static connected corridor succeeded");
            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "corridor", out deadlineFailure))
                return deadlineFailure;

            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "lateral", out deadlineFailure))
                return deadlineFailure;
            TimeSpan lateralBudget = SmallerBudget(
                TimeSpan.FromSeconds(configuration.Scheduling.SolverTimeoutSeconds),
                CycleRemaining(request, cycleElapsed));
            EmPlannerConfiguration lateralConfiguration = configuration.Copy();
            lateralConfiguration.Scheduling.SolverTimeoutSeconds = lateralBudget.TotalSeconds;
            var lateralInput = new LateralPlanningInput(segment, corridor, startProjection, horizon.TerminalType,
                request.Vehicle, lateralConfiguration, previousSeed);
            TimeSpan totalSolveBudget = lateralBudget;
            var solveBudgetStopwatch = Stopwatch.StartNew();
            LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, planningCancellationToken);
            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "lateral", out deadlineFailure))
                return deadlineFailure;
            if (!IsSuccess(lateral.Status))
                return Failure(lateral.Status, request, lateral.FailureReason);
            if (cancellationToken.IsCancellationRequested)
                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after LS optimization.");
            EmitDebug(request, "LS optimization and validation succeeded");

            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "envelope", out deadlineFailure))
                return deadlineFailure;
            EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction,
                initialProgressSpeed, initialAcceleration, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit,
                out string envelopeReason);
            if (envelopeStatus != EmPlanningStatus.Success)
                return Failure(envelopeStatus, request, envelopeReason);
            LongitudinalKnotSchedule knotSchedule;
            if (request.PlanningScope == EmPlanningScope.FullDirectionSegment)
            {
                EmPlanningStatus scheduleStatus = new FullDirectionSegmentScheduleBuilder().TryBuild(lateral.Path, speedLimit,
                    initialProgressSpeed, initialAcceleration, DesiredSpeed(configuration, segment.Direction), configuration,
                    out knotSchedule, out string scheduleReason);
                if (scheduleStatus != EmPlanningStatus.Success)
                    return Failure(scheduleStatus, request, scheduleReason);
            }
            else
            {
                knotSchedule = LongitudinalKnotSchedule.CreateRolling(configuration.Scheduling.TimeHorizonSeconds,
                    configuration.Scheduling.OutputTimeStepSeconds);
            }
            LongitudinalPreviousTrajectorySeed previousLongitudinalSeed =
                new LongitudinalPreviousTrajectorySeedBuilder().Build(
                    request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotSchedule,
                    segment.SegmentIndex, segment.Direction);
            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "envelope", out deadlineFailure))
                return deadlineFailure;
            TimeSpan remainingSolveBudget = totalSolveBudget - solveBudgetStopwatch.Elapsed;
            remainingSolveBudget = SmallerBudget(remainingSolveBudget, CycleRemaining(request, cycleElapsed));
            if (remainingSolveBudget <= TimeSpan.Zero)
            {
                if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
                        "longitudinal", out deadlineFailure))
                    return deadlineFailure;
                return Failure(EmPlanningStatus.SolverTimedOut, request,
                    "LS/ST optimization exhausted the shared solve budget before ST optimization.");
            }
            EmPlannerConfiguration longitudinalConfiguration = configuration.Copy();
            longitudinalConfiguration.Scheduling.SolverTimeoutSeconds = remainingSolveBudget.TotalSeconds;
            var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
                initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, longitudinalConfiguration,
                request.PlanningScope, knotSchedule,
                previousLongitudinalSeed.PathS, previousLongitudinalSeed.ProgressSpeedMetersPerSecond);
            envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out envelopeReason);
            if (envelopeStatus != EmPlanningStatus.Success)
                return Failure(envelopeStatus, request, envelopeReason);
            EmitDebug(request, "PathS speed envelope succeeded");

            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
                    "longitudinal", out deadlineFailure))
                return deadlineFailure;
            LongitudinalPlanningResult longitudinal = new LongitudinalPlanner(qpSolver).Plan(
                longitudinalInput, planningCancellationToken);
            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
                    "longitudinal", out deadlineFailure))
                return deadlineFailure;
            if (!IsSuccess(longitudinal.Status))
                return Failure(longitudinal.Status, request, longitudinal.FailureReason);
            if (cancellationToken.IsCancellationRequested)
                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after ST optimization.");
            EmitDebug(request, "ST optimization and validation succeeded");

            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "assembly", out deadlineFailure))
                return deadlineFailure;
            var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
                request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId,
                segment.SegmentIndex, segment.Direction, horizon.TerminalType, horizon.LongitudinalMode,
                request.PlanningScope);
            EmPlanningStatus assemblyStatus = new EmTrajectoryAssembler(configuration).TryAssemble(lateral.Path,
                longitudinal, metadata, out EmTrajectory trajectory, out string assemblyFailure);
            if (assemblyStatus != EmPlanningStatus.Success)
                return Failure(assemblyStatus, request, assemblyFailure);
            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "assembly", out deadlineFailure))
                return deadlineFailure;
            if (cancellationToken.IsCancellationRequested)
                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after trajectory assembly.");
            EmitDebug(request, "trajectory assembly succeeded");

            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
                    "world-validation", out deadlineFailure))
                return deadlineFailure;
            EmBoundaryType terminalBoundary = slice.TerminalBoundary.BoundaryType;
            Pose2D terminalPose = IsRealTerminalBoundary(terminalBoundary)
                ? TerminalPose(lateral.Path)
                : null;
            EmTrajectoryValidationResult publication = new EmTrajectoryValidator().Validate(trajectory, request.Map,
                request.Vehicle, configuration, segment.SegmentIndex, segment.LengthMeters, longitudinalInput.PathUpperBoundS,
                terminalPose, terminalBoundary);
            if (!publication.IsValid)
            {
                return Failure(MapPublicationFailure(publication.Failure), request,
                    publication.Failure + " at point " + publication.PointIndex + ": " + publication.Message);
            }
            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
                    "world-validation", out deadlineFailure))
                return deadlineFailure;
            EmitDebug(request, "world-space publication validation succeeded");

            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "publication", out deadlineFailure))
                return deadlineFailure;
            if (cancellationToken.IsCancellationRequested)
                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before trajectory publication.");

            EmPlanningStatus finalStatus = lateral.Status == EmPlanningStatus.SuccessWithFallback ||
                longitudinal.Status == EmPlanningStatus.SuccessWithFallback
                ? EmPlanningStatus.SuccessWithFallback
                : EmPlanningStatus.Success;
            string publicationDiagnostic = DiagnosticsPrefix(request) + ";terminal=" + horizon.TerminalType +
                ";publication=validated";
            if (lateral.Status == EmPlanningStatus.SuccessWithFallback)
                publicationDiagnostic += ";lateralFallback=" + lateral.FailureReason;
            if (!string.IsNullOrWhiteSpace(longitudinal.FailureReason))
            {
                publicationDiagnostic += longitudinal.Status == EmPlanningStatus.SuccessWithFallback
                    ? ";longitudinalFallback=" + longitudinal.FailureReason
                    : ";longitudinal=" + longitudinal.FailureReason;
            }
            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "publication", out deadlineFailure))
                return deadlineFailure;
            return new EmPlanningResult(finalStatus, trajectory, publicationDiagnostic);
        }
        catch (OperationCanceledException)
        {
            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
                    "operation", out EmPlanningResult deadlineFailure))
                return deadlineFailure;
            return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled.");
        }
        catch (ArgumentException exception)
        {
            return Failure(EmPlanningStatus.Failed, request, exception.Message);
        }
        catch (InvalidOperationException exception)
        {
            return Failure(EmPlanningStatus.Failed, request, exception.Message);
        }
    }

    private static bool TryTerminalFailure(EmPlanningRequest request, CancellationToken cancellationToken,
        Func<TimeSpan> elapsed, string phase, out EmPlanningResult failure)
    {
        if (CallerCancellationRequested(request, cancellationToken))
        {
            failure = Failure(EmPlanningStatus.Cancelled, request,
                "callerCancellation=true;phase=" + phase);
            return true;
        }
        TimeSpan remaining = CycleRemaining(request, elapsed);
        if (!DeadlineExpired(request, elapsed))
        {
            failure = null;
            return false;
        }
        failure = CycleDeadlineFailure(request, phase, remaining);
        return true;
    }

    private static bool CallerCancellationRequested(EmPlanningRequest request,
        CancellationToken cancellationToken)
    {
        if (request?.CallerCancellationToken.IsCancellationRequested == true)
            return true;
        if (!cancellationToken.IsCancellationRequested)
            return false;
        return request == null || !request.CycleDeadlineToken.IsCancellationRequested ||
            !request.CallerCancellationToken.CanBeCanceled;
    }

    private static bool DeadlineExpired(EmPlanningRequest request, Func<TimeSpan> elapsed)
    {
        return request != null && (request.CycleDeadlineToken.IsCancellationRequested ||
            request.IsCycleDeadlineExpired() ||
            request.CycleDeadlineRemaining.HasValue && CycleRemaining(request, elapsed) <= TimeSpan.Zero);
    }

    private static TimeSpan CycleRemaining(EmPlanningRequest request, Func<TimeSpan> elapsed)
    {
        TimeSpan snapshotRemaining = TimeSpan.MaxValue;
        if (request.CycleDeadlineRemaining.HasValue)
        {
            snapshotRemaining = request.CycleDeadlineRemaining.Value - elapsed();
            if (snapshotRemaining < TimeSpan.Zero)
                snapshotRemaining = TimeSpan.Zero;
        }
        TimeSpan? dynamicRemaining = request.DynamicCycleDeadlineRemaining();
        if (!dynamicRemaining.HasValue)
            return snapshotRemaining;
        return dynamicRemaining.Value < snapshotRemaining ? dynamicRemaining.Value : snapshotRemaining;
    }

    private static TimeSpan SmallerBudget(TimeSpan configured, TimeSpan cycleRemaining)
    {
        return configured < cycleRemaining ? configured : cycleRemaining;
    }

    private static EmPlanningResult CycleDeadlineFailure(EmPlanningRequest request, string phase,
        TimeSpan remaining)
    {
        return Failure(EmPlanningStatus.CycleDeadlineExpired, request,
            "cycleDeadlineExpired=true;phase=" + phase + ";remainingMs=" +
            remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
    }

    private void EmitDebug(EmPlanningRequest request, string message)
    {
        if (defaultDebugSink == null || request == null || request.Configuration == null || request.Configuration.Solver == null ||
            !request.Configuration.Solver.NativeVerbose)
        {
            return;
        }
        try
        {
            defaultDebugSink.Write(message ?? string.Empty);
        }
        catch
        {
            // Debug output is deliberately isolated from pure planning results.
        }
    }

    private static IReadOnlyList<FrenetProjection> ProjectPreviousTrajectorySeed(EmTrajectory previousTrajectory,
        DirectionSegmentView segment, double minimumReferenceS, double maximumReferenceS, double maximumDistanceMeters)
    {
        var projected = new List<FrenetProjection>();
        if (previousTrajectory == null || previousTrajectory.Metadata.SegmentIndex != segment.SegmentIndex ||
            previousTrajectory.Metadata.Direction != segment.Direction)
        {
            return projected;
        }

        var projector = new FrenetProjector();
        double seedReferenceS = minimumReferenceS;
        for (int index = 0; index < previousTrajectory.Points.Count; index++)
        {
            EmTrajectoryPoint point = previousTrajectory.Points[index];
            if (point == null || point.TimeFromStart <= 0d || point.Direction != segment.Direction)
                continue;
            if (projector.TryProject(new Pose2D(point.X, point.Y, point.Yaw), segment, minimumReferenceS,
                    maximumReferenceS, maximumDistanceMeters, seedReferenceS, out FrenetProjection projection))
            {
                projected.Add(projection);
                seedReferenceS = projection.ReferenceS;
            }
        }
        return projected;
    }

    private static bool HasCompatibleStateDirection(VehicleMotionState state, TravelDirection direction, double stopTolerance)
    {
        if (state == null || double.IsNaN(stopTolerance) || double.IsInfinity(stopTolerance) || stopTolerance < 0d)
            return false;
        if (Math.Abs(state.SignedLongitudinalSpeedMetersPerSecond) <= stopTolerance)
            return true;
        return direction == TravelDirection.Forward
            ? state.SignedLongitudinalSpeedMetersPerSecond > 0d
            : state.SignedLongitudinalSpeedMetersPerSecond < 0d;
    }

    private static double DesiredSpeed(EmPlannerConfiguration configuration, TravelDirection direction)
    {
        return direction == TravelDirection.Forward
            ? configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond
            : configuration.Longitudinal.DesiredReverseSpeedMetersPerSecond;
    }

    private static bool IsSuccess(EmPlanningStatus status)
    {
        return status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
    }

    internal static EmPlanningStatus MapPublicationFailure(EmTrajectoryValidationFailure failure)
    {
        return failure == EmTrajectoryValidationFailure.TerminalPoseMismatch
            ? EmPlanningStatus.TerminalPoseMismatch
            : EmPlanningStatus.ValidationFailed;
    }

    private static bool IsRealTerminalBoundary(EmBoundaryType boundaryType)
    {
        return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
    }

    private static Pose2D TerminalPose(LateralPath path)
    {
        LateralPathPoint terminal = path.Points[path.Points.Count - 1];
        return new Pose2D(terminal.X, terminal.Y, terminal.VehicleYaw);
    }

    private static EmPlanningResult Failure(EmPlanningStatus status, EmPlanningRequest request, string reason)
    {
        return new EmPlanningResult(status, null, DiagnosticsPrefix(request) + ";reason=" + (reason ?? string.Empty));
    }

    private static string DiagnosticsPrefix(EmPlanningRequest request)
    {
        if (request == null)
            return "map=;reference=;state=;previous=;segment=";
        return "map=" + (request.Map == null ? string.Empty : request.Map.SnapshotId.ToString()) +
            ";reference=" + (request.ReferencePathId ?? string.Empty) +
            ";state=" + (request.VehicleState == null ? string.Empty : request.VehicleState.SequenceId.ToString()) +
            ";previous=" + (request.PreviousTrajectoryId ?? string.Empty) +
            ";segment=" + request.SegmentIndex + ";scope=" + request.PlanningScope;
    }
}