using System;
using System.Collections.Generic;
using System.Diagnostics;
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 readonly IQpSolver qpSolver;
    private readonly IEmPlannerDebugSink defaultDebugSink;

    public EmPlanningService(IQpSolver qpSolver, IEmPlannerDebugSink defaultDebugSink = null)
    {
        this.qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
        this.defaultDebugSink = defaultDebugSink;
    }

    public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
    {
        if (cancellationToken.IsCancellationRequested)
            return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before request validation.");

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

        try
        {
            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");

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

            var lateralInput = new LateralPlanningInput(segment, corridor, startProjection, horizon.TerminalType,
                request.Vehicle, configuration, previousSeed);
            TimeSpan totalSolveBudget = TimeSpan.FromSeconds(configuration.Scheduling.SolverTimeoutSeconds);
            var solveBudgetStopwatch = Stopwatch.StartNew();
            LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, cancellationToken);
            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");

            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);
            TimeSpan remainingSolveBudget = totalSolveBudget - solveBudgetStopwatch.Elapsed;
            if (remainingSolveBudget <= TimeSpan.Zero)
            {
                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");

            LongitudinalPlanningResult longitudinal = new LongitudinalPlanner(qpSolver).Plan(longitudinalInput, cancellationToken);
            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");

            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 (cancellationToken.IsCancellationRequested)
                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after trajectory assembly.");
            EmitDebug(request, "trajectory assembly succeeded");

            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);
            }
            EmitDebug(request, "world-space publication validation succeeded");

            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;
            return new EmPlanningResult(finalStatus, trajectory,
                DiagnosticsPrefix(request) + ";terminal=" + horizon.TerminalType + ";publication=validated");
        }
        catch (OperationCanceledException)
        {
            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 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;
    }
}
