using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Threading;
using System.Threading.Tasks;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;

namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

public sealed class TrajectoryObservationBootstrapResult
{
    private readonly VehicleParameters vehicle;

    private TrajectoryObservationBootstrapResult(CoarsePathPlanningJob job, CoarsePathPlanningJobResult coarse,
        PathSmoothingResult smoothedPath, IReadOnlyList<DirectionSegmentView> segments, string failureReason)
    {
        Job = job ?? throw new ArgumentNullException(nameof(job));
        vehicle = CopyVehicle(job.Vehicle);
        CoarseResult = coarse;
        SmoothedPath = smoothedPath;
        Segments = CopySegments(segments);
        FailureReason = failureReason ?? string.Empty;
        Succeeded = coarse != null && smoothedPath != null && Segments.Count > 0 && FailureReason.Length == 0;
    }

    public bool Succeeded { get; }

    public CoarsePathPlanningJob Job { get; }

    public VehicleParameters Vehicle => CopyVehicle(vehicle);

    public CoarsePathPlanningJobResult CoarseResult { get; }

    public PathSmoothingResult SmoothedPath { get; }

    public IReadOnlyList<DirectionSegmentView> Segments { get; }

    public string FailureReason { get; }

    public PlanningGridMap Map => CoarseResult?.MapResult?.Map;

    public static TrajectoryObservationBootstrapResult FromFailure(CoarsePathPlanningJob job,
        CoarsePathPlanningJobResult coarse, PathSmoothingResult smoothedPath, string failureReason)
    {
        return new TrajectoryObservationBootstrapResult(job, coarse, smoothedPath,
            Array.Empty<DirectionSegmentView>(), string.IsNullOrEmpty(failureReason) ? "Planning bootstrap failed." : failureReason);
    }

    public static TrajectoryObservationBootstrapResult Success(CoarsePathPlanningJob job,
        CoarsePathPlanningJobResult coarse, PathSmoothingResult smoothedPath,
        IReadOnlyList<DirectionSegmentView> segments)
    {
        if (coarse == null) throw new ArgumentNullException(nameof(coarse));
        if (smoothedPath == null) throw new ArgumentNullException(nameof(smoothedPath));
        if (segments == null || segments.Count == 0)
            throw new ArgumentException("A successful bootstrap requires reference segments.", nameof(segments));
        return new TrajectoryObservationBootstrapResult(job, coarse, smoothedPath, segments, string.Empty);
    }

    private static IReadOnlyList<DirectionSegmentView> CopySegments(IReadOnlyList<DirectionSegmentView> source)
    {
        var copy = new List<DirectionSegmentView>(source == null ? 0 : source.Count);
        if (source != null)
        {
            for (int index = 0; index < source.Count; index++)
                copy.Add(source[index]);
        }
        return new ReadOnlyCollection<DirectionSegmentView>(copy);
    }

    private static VehicleParameters CopyVehicle(VehicleParameters source)
    {
        if (source == null) return null;
        return new VehicleParameters
        {
            LengthMeters = source.LengthMeters,
            WidthMeters = source.WidthMeters,
            SafetyMarginMeters = source.SafetyMarginMeters,
            MaximumCurvaturePerMeter = source.MaximumCurvaturePerMeter,
            MinimumTurningRadiusMeters = source.MinimumTurningRadiusMeters,
        };
    }
}

public sealed class TrajectoryObservationBootstrapper
{
    private readonly CoarsePathPlanningService coarseService;
    private readonly PathSmoothingService smoothingService;

    public TrajectoryObservationBootstrapper()
        : this(new CoarsePathPlanningService(), new PathSmoothingService())
    {
    }

    public TrajectoryObservationBootstrapper(CoarsePathPlanningService coarseService,
        PathSmoothingService smoothingService)
    {
        this.coarseService = coarseService ?? throw new ArgumentNullException(nameof(coarseService));
        this.smoothingService = smoothingService ?? throw new ArgumentNullException(nameof(smoothingService));
    }

    public TrajectoryObservationBootstrapResult Bootstrap(CoarsePathPlanningJob job,
        CancellationToken cancellationToken = default)
    {
        if (job == null) throw new ArgumentNullException(nameof(job));

        CoarsePathPlanningJobResult coarse = coarseService.Plan(job, cancellationToken);
        if (coarse.PlanningResult.Status != PlanningStatus.Success)
            return TrajectoryObservationBootstrapResult.FromFailure(
                job, coarse, null, "Coarse planning status: " + coarse.PlanningResult.Status);

        var smoothingRequest = new PathSmoothingRequest(
            CopyFiniteClearance(coarse.PlanningResult.Path, coarse.MapResult.Map),
            coarse.PlanningResult.Segments, coarse.MapResult.Map, job.Vehicle,
            new PathSmoothingConfiguration());
        PathSmoothingResult smooth = smoothingService.Smooth(smoothingRequest, cancellationToken);
        if (!IsPublishedSmoothingStatus(smooth.Status))
            return TrajectoryObservationBootstrapResult.FromFailure(
                job, coarse, smooth, smooth.Diagnostics.TerminationReason);
        return TrajectoryObservationBootstrapResult.Success(job, coarse, smooth, ReferencePathSegmenter.Create(smooth));
    }

    private static IReadOnlyList<CoarsePathPoint> CopyFiniteClearance(IReadOnlyList<CoarsePathPoint> path,
        PlanningGridMap map)
    {
        if (path == null) throw new ArgumentNullException(nameof(path));
        if (map == null) throw new ArgumentNullException(nameof(map));

        double widthMeters = (map.Bounds.XMax - map.Bounds.XMin) / 1000d;
        double heightMeters = (map.Bounds.YMax - map.Bounds.YMin) / 1000d;
        double mapDiagonalMeters = Math.Sqrt(widthMeters * widthMeters + heightMeters * heightMeters);
        var copy = new List<CoarsePathPoint>(path.Count);
        for (int index = 0; index < path.Count; index++)
        {
            CoarsePathPoint point = path[index] ?? throw new ArgumentException(
                "The coarse path cannot contain null points.", nameof(path));
            double clearance = double.IsPositiveInfinity(point.BodyClearance)
                ? mapDiagonalMeters
                : point.BodyClearance;
            copy.Add(new CoarsePathPoint(point.X, point.Y, point.Heading, point.UnwrappedHeading,
                point.ArcLength, point.Direction, point.VehicleCurvature, clearance,
                point.IsGearSwitchPoint, point.Source));
        }
        return new ReadOnlyCollection<CoarsePathPoint>(copy);
    }

    private static bool IsPublishedSmoothingStatus(PathSmoothingStatus status)
    {
        return status == PathSmoothingStatus.Complete ||
            status == PathSmoothingStatus.PartialImprovement ||
            status == PathSmoothingStatus.NotNeeded ||
            status == PathSmoothingStatus.Unchanged;
    }
}

public sealed class TrajectoryObservationObservation
{
    internal TrajectoryObservationObservation(DateTimeOffset observedAtUtc, VehicleMotionState vehicleState,
        EmTrajectory publishedTrajectory, EmTrajectoryPoint selectedPoint, TrajectoryControlCommand command,
        TrajectoryExecutionState executorState)
    {
        ObservedAtUtc = observedAtUtc;
        VehicleState = vehicleState ?? throw new ArgumentNullException(nameof(vehicleState));
        PublishedTrajectory = publishedTrajectory;
        SelectedPoint = selectedPoint;
        Command = command;
        ExecutorState = executorState;
    }

    public DateTimeOffset ObservedAtUtc { get; }

    public VehicleMotionState VehicleState { get; }

    public EmTrajectory PublishedTrajectory { get; }

    public EmTrajectoryPoint SelectedPoint { get; }

    public TrajectoryControlCommand Command { get; }

    public TrajectoryExecutionState ExecutorState { get; }
}

public sealed class TrajectoryObservationController
{
    private readonly TrajectoryObservationBootstrapResult bootstrap;
    private readonly EmPlannerConfiguration configuration;
    private readonly EmPlanningCoordinator coordinator;
    private readonly TrajectoryExecutor executor;
    private readonly string sessionId;
    private long cycleId;

    public TrajectoryObservationController(TrajectoryObservationBootstrapResult bootstrap,
        TrajectoryObservationSettings settings, IEmPlanningService planningService, string sessionId)
    {
        this.bootstrap = bootstrap ?? throw new ArgumentNullException(nameof(bootstrap));
        if (!bootstrap.Succeeded)
            throw new ArgumentException("A successful planning bootstrap is required.", nameof(bootstrap));
        if (settings == null) throw new ArgumentNullException(nameof(settings));
        if (planningService == null) throw new ArgumentNullException(nameof(planningService));
        if (string.IsNullOrWhiteSpace(sessionId)) throw new ArgumentException("A session ID is required.", nameof(sessionId));

        settings.Validate();
        configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.ReplanPeriodSeconds = settings.ReplanPeriodSeconds;
        configuration.Scheduling.SolverTimeoutSeconds = settings.SolverTimeoutSeconds;
        configuration.Solver.MaximumOsqpIterations = settings.MaximumOsqpIterations;
        configuration.Scheduling.TimeHorizonSeconds = settings.TimeHorizonSeconds;
        configuration.Scheduling.OutputTimeStepSeconds = settings.OutputTimeStepSeconds;
        coordinator = new EmPlanningCoordinator(planningService);
        executor = new TrajectoryExecutor(configuration);
        this.sessionId = sessionId;
    }

    public EmTrajectory PublishedTrajectory => coordinator.PublishedTrajectory;

    public TrajectoryObservationDiagnostic CreateConfigurationDiagnostic()
    {
        return TrajectoryObservationDiagnostics.CreateConfiguration(configuration);
    }

    public bool ShouldStartCycle(DateTimeOffset now)
    {
        return coordinator.ShouldStartCycle(now);
    }

    public Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
        CancellationToken cancellationToken)
    {
        if (state == null) throw new ArgumentNullException(nameof(state));

        const int segmentIndex = 0;
        long currentCycleId = Interlocked.Increment(ref cycleId);
        EmTrajectory previousTrajectory = coordinator.PublishedTrajectory;
        var request = new EmPlanningRequest(
            bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state, configuration,
            segmentIndex, previousTrajectory, now, now,
            sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference",
            previousTrajectory?.Metadata.TrajectoryId ?? string.Empty,
            EmMotionModel.NonholonomicForwardReverse);
        return coordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken);
    }

    public TrajectoryObservationObservation Observe(DateTimeOffset now, VehicleMotionState state)
    {
        if (state == null) throw new ArgumentNullException(nameof(state));

        EmTrajectory trajectory = coordinator.PublishedTrajectory;
        if (trajectory == null)
            return new TrajectoryObservationObservation(now, state, null, null, null, null);

        TrajectoryControlCommand command = executor.UpdateCommand(now, state, trajectory,
            trajectory.Metadata.Direction, trajectory.Metadata.Direction, true);
        TrajectoryExecutionState executorState = executor.State;
        return new TrajectoryObservationObservation(now, state, trajectory, executorState.SelectedPoint,
            command, executorState);
    }
}

public sealed class TrajectoryObservationLoopTick
{
    internal TrajectoryObservationLoopTick(TrajectoryObservationObservation observation,
        PlanningCycleResult latestCycle, TimeSpan latestPlanningElapsed, bool planningInFlight,
        bool planningStarted, bool planningCompleted)
    {
        Observation = observation ?? throw new ArgumentNullException(nameof(observation));
        LatestCycle = latestCycle;
        LatestPlanningElapsed = latestPlanningElapsed;
        PlanningInFlight = planningInFlight;
        PlanningStarted = planningStarted;
        PlanningCompleted = planningCompleted;
    }

    public TrajectoryObservationObservation Observation { get; }

    public PlanningCycleResult LatestCycle { get; }

    public TimeSpan LatestPlanningElapsed { get; }

    public bool PlanningInFlight { get; }

    public bool PlanningStarted { get; }

    public bool PlanningCompleted { get; }

    public bool ShouldLog => true;
}

public sealed class TrajectoryObservationLoop
{
    private readonly TrajectoryObservationController controller;
    private Task<PlanningCycleResult> planningTask;
    private DateTimeOffset planningStartedAtUtc;
    private PlanningCycleResult latestCycle;
    private TimeSpan latestPlanningElapsed;

    public TrajectoryObservationLoop(TrajectoryObservationController controller)
    {
        this.controller = controller ?? throw new ArgumentNullException(nameof(controller));
    }

    public TrajectoryObservationLoopTick Tick(DateTimeOffset now, VehicleMotionState state,
        CancellationToken cancellationToken)
    {
        if (state == null) throw new ArgumentNullException(nameof(state));
        cancellationToken.ThrowIfCancellationRequested();

        bool planningCompleted = ConsumeCompletedPlanning(now);
        bool planningStarted = false;
        if (planningTask == null && controller.ShouldStartCycle(now))
        {
            planningStarted = true;
            planningStartedAtUtc = now;
            planningTask = controller.StartCycle(now, state, cancellationToken);
            planningCompleted |= ConsumeCompletedPlanning(now);
        }

        TrajectoryObservationObservation observation = controller.Observe(now, state);
        return new TrajectoryObservationLoopTick(observation, latestCycle, latestPlanningElapsed,
            planningTask != null, planningStarted, planningCompleted);
    }

    private bool ConsumeCompletedPlanning(DateTimeOffset observedAtUtc)
    {
        Task<PlanningCycleResult> completed = planningTask;
        if (completed == null || !completed.IsCompleted) return false;

        latestCycle = completed.GetAwaiter().GetResult();
        TimeSpan elapsed = observedAtUtc - planningStartedAtUtc;
        latestPlanningElapsed = elapsed < TimeSpan.Zero ? TimeSpan.Zero : elapsed;
        planningTask = null;
        return true;
    }
}

public enum TrajectoryObservationSessionEndReason
{
    BootstrapFailure,
    RuntimeFault,
    Cancellation,
}

public static class TrajectoryObservationSessionLifecycle
{
    public static bool ShouldClearLayers(TrajectoryObservationSessionEndReason reason)
    {
        switch (reason)
        {
            case TrajectoryObservationSessionEndReason.BootstrapFailure:
                return false;
            case TrajectoryObservationSessionEndReason.RuntimeFault:
            case TrajectoryObservationSessionEndReason.Cancellation:
                return true;
            default:
                throw new ArgumentOutOfRangeException(nameof(reason));
        }
    }
}

public sealed class TrajectoryObservationRuntimeState
{
    public const string GearSwitchWaitingNotice =
        "等待真实档位/方向确认；观察模式不会推进下一方向段";

    private TrajectoryObservationRuntimeState(bool waitingAtGearSwitch)
    {
        WaitingAtGearSwitch = waitingAtGearSwitch;
        WorldNotice = waitingAtGearSwitch ? GearSwitchWaitingNotice : string.Empty;
    }

    public bool WaitingAtGearSwitch { get; }

    public string WorldNotice { get; }

    public static TrajectoryObservationRuntimeState Create(DateTimeOffset now, EmTrajectory trajectory)
    {
        if (trajectory == null || trajectory.Metadata.TerminalType != EmTerminalType.GearSwitch)
            return new TrajectoryObservationRuntimeState(false);

        EmTrajectoryPoint finalPoint = trajectory.Points[trajectory.Points.Count - 1];
        bool waiting = (now - trajectory.Metadata.EffectiveAtUtc).TotalSeconds >= finalPoint.TimeFromStart;
        return new TrajectoryObservationRuntimeState(waiting);
    }
}

public sealed class TrajectoryObservationLsSample
{
    public TrajectoryObservationLsSample(double pathS, double lateralOffset)
    {
        PathS = pathS;
        LateralOffset = lateralOffset;
    }

    public double PathS { get; }

    public double LateralOffset { get; }
}

public sealed class TrajectoryObservationStSample
{
    public TrajectoryObservationStSample(double timeFromStart, double pathS)
    {
        TimeFromStart = timeFromStart;
        PathS = pathS;
    }

    public double TimeFromStart { get; }

    public double PathS { get; }
}

public sealed class TrajectoryObservationSpeedSample
{
    public TrajectoryObservationSpeedSample(double timeFromStart, double signedLongitudinalVelocity)
    {
        TimeFromStart = timeFromStart;
        SignedLongitudinalVelocity = signedLongitudinalVelocity;
    }

    public double TimeFromStart { get; }

    public double SignedLongitudinalVelocity { get; }
}

public sealed class TrajectoryObservationCharts
{
    private TrajectoryObservationCharts(IReadOnlyList<TrajectoryObservationLsSample> lsSamples,
        IReadOnlyList<TrajectoryObservationStSample> stSamples,
        IReadOnlyList<TrajectoryObservationSpeedSample> speedSamples, int failedProjectionCount)
    {
        LsSamples = new ReadOnlyCollection<TrajectoryObservationLsSample>(
            new List<TrajectoryObservationLsSample>(lsSamples));
        StSamples = new ReadOnlyCollection<TrajectoryObservationStSample>(
            new List<TrajectoryObservationStSample>(stSamples));
        SpeedSamples = new ReadOnlyCollection<TrajectoryObservationSpeedSample>(
            new List<TrajectoryObservationSpeedSample>(speedSamples));
        FailedProjectionCount = failedProjectionCount;
    }

    public IReadOnlyList<TrajectoryObservationLsSample> LsSamples { get; }

    public IReadOnlyList<TrajectoryObservationStSample> StSamples { get; }

    public IReadOnlyList<TrajectoryObservationSpeedSample> SpeedSamples { get; }

    public int FailedProjectionCount { get; }

    public static TrajectoryObservationCharts Build(EmTrajectory trajectory, DirectionSegmentView segment,
        double maximumProjectionDistanceMeters)
    {
        if (trajectory == null) throw new ArgumentNullException(nameof(trajectory));
        if (segment == null) throw new ArgumentNullException(nameof(segment));
        if (double.IsNaN(maximumProjectionDistanceMeters) || double.IsInfinity(maximumProjectionDistanceMeters) ||
            maximumProjectionDistanceMeters < 0d)
            throw new ArgumentOutOfRangeException(nameof(maximumProjectionDistanceMeters));

        var ls = new List<TrajectoryObservationLsSample>(trajectory.Points.Count);
        var st = new List<TrajectoryObservationStSample>(trajectory.Points.Count);
        var speed = new List<TrajectoryObservationSpeedSample>(trajectory.Points.Count);
        var projector = new FrenetProjector();
        double seedReferenceS = 0d;
        int failedProjectionCount = 0;
        for (int index = 0; index < trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint point = trajectory.Points[index];
            var pose = new Pose2D(point.X, point.Y, point.Yaw);
            if (projector.TryProject(pose, segment, 0d, segment.LengthMeters,
                maximumProjectionDistanceMeters, seedReferenceS, out FrenetProjection projection))
            {
                ls.Add(new TrajectoryObservationLsSample(
                    segment.SourceStartArcLength + projection.ReferenceS, projection.LateralOffset));
                seedReferenceS = projection.ReferenceS;
            }
            else
            {
                failedProjectionCount++;
            }

            st.Add(new TrajectoryObservationStSample(point.TimeFromStart, point.PathS));
            speed.Add(new TrajectoryObservationSpeedSample(point.TimeFromStart, point.SignedLongitudinalVelocity));
        }

        return new TrajectoryObservationCharts(ls, st, speed, failedProjectionCount);
    }
}