using System;
using System.Collections.Generic;
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();
            if (!projector.TryProject(request.VehicleState.Pose, segment, 0d, segment.LengthMeters,
                    configuration.Frenet.MaximumProjectionDistanceMeters, 0d, out FrenetProjection startProjection))
            {
                return Failure(EmPlanningStatus.ProjectionFailed, request,
                    "Vehicle pose could not be projected inside 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, 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);
            LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, cancellationToken);
            if (!IsSuccess(lateral.Status))
                return Failure(lateral.Status, request, lateral.FailureReason);
            EmitDebug(request, "LS optimization and validation succeeded");

            var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
                initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration,
                Array.Empty<double>(), Array.Empty<double>());
            EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out string 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);
            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);
            EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(lateral.Path, longitudinal, metadata);
            EmitDebug(request, "trajectory assembly succeeded");

            EmTrajectoryValidationResult publication = new EmTrajectoryValidator().Validate(trajectory, request.Map,
                request.Vehicle, configuration, segment.SegmentIndex, longitudinalInput.TerminalPathS,
                slice.TerminalBoundary.BoundaryType);
            if (!publication.IsValid)
            {
                return Failure(EmPlanningStatus.ValidationFailed, request,
                    publication.Failure + " at point " + publication.PointIndex + ": " + publication.Message);
            }
            EmitDebug(request, "world-space publication validation succeeded");

            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 bool IsSuccess(EmPlanningStatus status)
    {
        return status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
    }

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