using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Globalization;
using System.Threading;
using System.Threading.Tasks;
using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using FundamentalLib;
using MDCSToolBox.Clumsy.Pilot;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

namespace MultiWheelC;

[MovementTest(name = "EM轨迹规划观察闭环测试")]
public sealed class TrajectoryObservationMovementTest : MovementTest
{
    private const int MaximumManualObstacleCount = 20;
    private static long nextObstacleSnapshotVersion;

    public double GoalXmm = double.NaN;
    public double GoalYmm = double.NaN;
    public double GoalYawDeg = 0d;
    public double MapPaddingMeters = 2d;
    public float MapResolutionMm = 50f;
    public double ReplanPeriodSeconds = 0.20d;
    public double ObserverPeriodSeconds = 0.05d;
    public double SolverTimeoutSeconds = 0.50d;
    public int MaximumOsqpIterations = 12000;
    /// <summary>单次 ST 轨迹覆盖的未来时长，单位 s；默认 6s，不等于观察循环周期。</summary>
    public double TimeHorizonSeconds = 6d;
    /// <summary>Trajectory 相邻 TimeFromStart 时间戳的间隔，单位 s；默认 0.10s，不等于观察循环周期。</summary>
    public double OutputTimeStepSeconds = 0.10d;
    public double VehicleLengthMeters = 0.80d;
    public double VehicleWidthMeters = 0.60d;
    public double SafetyMarginMeters = 0.05d;
    public double MaximumCurvaturePerMeter = 1d / 1.20d;

    public override void Test()
    {
        try
        {
            double goalXMillimeters = GoalXmm;
            double goalYMillimeters = GoalYmm;
            double goalYawDegrees = GoalYawDeg;
            if (!IsFinite(goalXMillimeters) || !IsFinite(goalYMillimeters))
            {
                goalXMillimeters = ReadFiniteInput("EM观察终点 X（世界 mm）");
                goalYMillimeters = ReadFiniteInput("EM观察终点 Y（世界 mm）");
                goalYawDegrees = ReadFiniteInput("EM观察终点航向（世界 deg）");
            }
            EnsureFinite(goalXMillimeters, nameof(GoalXmm));
            EnsureFinite(goalYMillimeters, nameof(GoalYmm));
            EnsureFinite(goalYawDegrees, nameof(GoalYawDeg));

            var settings = new TrajectoryObservationSettings
            {
                MapPaddingMeters = MapPaddingMeters,
                MapResolutionMillimeters = MapResolutionMm,
                ReplanPeriodSeconds = ReplanPeriodSeconds,
                ObserverPeriodSeconds = ObserverPeriodSeconds,
                SolverTimeoutSeconds = SolverTimeoutSeconds,
                MaximumOsqpIterations = MaximumOsqpIterations,
                TimeHorizonSeconds = TimeHorizonSeconds,
                OutputTimeStepSeconds = OutputTimeStepSeconds,
                VehicleLengthMeters = VehicleLengthMeters,
                VehicleWidthMeters = VehicleWidthMeters,
                SafetyMarginMeters = SafetyMarginMeters,
                MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
            };
            settings = settings.CreateValidatedSnapshot();
            TimeSpan observerPeriod = TimeSpan.FromSeconds(settings.ObserverPeriodSeconds);
            IReadOnlyList<TrajectoryObservationObstacle> obstacles = ReadManualObstacles();
            long obstacleSnapshotVersion = obstacles.Count == 0
                ? 0L
                : Interlocked.Increment(ref nextObstacleSnapshotVersion);
            var goal = new Pose2D(goalXMillimeters / 1000d, goalYMillimeters / 1000d,
                goalYawDegrees * Math.PI / 180d);

            TrajectoryObservationMovementTestRunner.Start(
                goal, settings, observerPeriod, obstacles, obstacleSnapshotVersion);
        }
        catch (Exception exception)
        {
            TrajectoryObservationMovementTestRunner.ShowInputFailure(exception);
        }
    }

    public override void TestStop()
    {
        TrajectoryObservationMovementTestRunner.Stop();
    }

    private static IReadOnlyList<TrajectoryObservationObstacle> ReadManualObstacles()
    {
        int count = ReadBoundedIntegerInput("EM观察手动障碍物数量（0-20）", 0, MaximumManualObstacleCount);
        var obstacles = new List<TrajectoryObservationObstacle>(count);
        for (int index = 0; index < count; index++)
        {
            string label = "障碍物 " + (index + 1).ToString(CultureInfo.InvariantCulture);
            int kind = ReadBoundedIntegerInput(label + " 类型（1=圆形，2=轴对齐矩形）", 1, 2);
            double centerXMillimeters = ReadFiniteInput(label + " 中心 X（世界 mm）");
            double centerYMillimeters = ReadFiniteInput(label + " 中心 Y（世界 mm）");
            if (kind == 1)
            {
                double radiusMillimeters = ReadPositiveFiniteInput(label + " 半径（mm）");
                obstacles.Add(TrajectoryObservationObstacle.Circle(
                    centerXMillimeters, centerYMillimeters, radiusMillimeters));
            }
            else
            {
                double lengthXMillimeters = ReadPositiveFiniteInput(label + " X 方向长度（mm）");
                double widthYMillimeters = ReadPositiveFiniteInput(label + " Y 方向宽度（mm）");
                obstacles.Add(TrajectoryObservationObstacle.Rectangle(
                    centerXMillimeters - lengthXMillimeters / 2d,
                    centerXMillimeters + lengthXMillimeters / 2d,
                    centerYMillimeters - widthYMillimeters / 2d,
                    centerYMillimeters + widthYMillimeters / 2d));
            }
        }
        return obstacles;
    }

    private static int ReadBoundedIntegerInput(string prompt, int minimum, int maximum)
    {
        object raw = UI.GetInput(prompt);
        string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
        int value;
        if (!int.TryParse(text, NumberStyles.Integer, CultureInfo.CurrentCulture, out value) &&
            !int.TryParse(text, NumberStyles.Integer, CultureInfo.InvariantCulture, out value))
            throw new ArgumentException("输入必须是整数：" + prompt);
        if (value < minimum || value > maximum)
            throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
        return value;
    }

    private static double ReadPositiveFiniteInput(string prompt)
    {
        double value = ReadFiniteInput(prompt);
        if (value <= 0d)
            throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须是正数：" + prompt);
        return value;
    }

    private static double ReadFiniteInput(string prompt)
    {
        object raw = UI.GetInput(prompt);
        string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
        double value;
        if (!double.TryParse(text, NumberStyles.Float, CultureInfo.CurrentCulture, out value) &&
            !double.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value))
            throw new ArgumentException("输入必须是有限数字：" + prompt);
        EnsureFinite(value, prompt);
        return value;
    }

    private static void EnsureFinite(double value, string name)
    {
        if (!IsFinite(value)) throw new ArgumentException("输入必须是有限数字：" + name);
    }

    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }
}

internal static class TrajectoryObservationMovementTestRunner
{
    private const string StatusChannel = "TrajectoryObserver";
    private const string ObserveOnlyNotice = "OBSERVE_ONLY: no chassis command is sent.";
    private static readonly object SessionSync = new object();
    private static readonly TrajectoryObservationPresentation Presentation =
        new TrajectoryObservationPresentation();

    private static CancellationTokenSource activeCancellation;
    private static Task activeTask;
    private static long nextSessionId;
    private static long activeSessionId;
    private static long stateSequence;

    internal static void Start(Pose2D goal, TrajectoryObservationSettings settings, TimeSpan observerPeriod,
        IReadOnlyList<TrajectoryObservationObstacle> obstacles, long obstacleSnapshotVersion)
    {
        if (goal == null) throw new ArgumentNullException(nameof(goal));
        if (settings == null) throw new ArgumentNullException(nameof(settings));
        if (obstacles == null) throw new ArgumentNullException(nameof(obstacles));

        var cancellation = new CancellationTokenSource();
        CancellationTokenSource previousCancellation;
        long sessionId;
        lock (SessionSync)
        {
            previousCancellation = activeCancellation;
            activeCancellation = cancellation;
            activeTask = null;
            sessionId = ++nextSessionId;
            activeSessionId = sessionId;
            Presentation.ClearAll();
            PrintStatus("Starting frozen observation session " +
                sessionId.ToString(CultureInfo.InvariantCulture) + ".");
        }

        CancelWithoutWaiting(previousCancellation);

        Task task = Task.Run(() => RunSessionAsync(sessionId, goal, settings, observerPeriod,
            obstacles, obstacleSnapshotVersion, cancellation.Token), cancellation.Token);
        lock (SessionSync)
        {
            if (activeSessionId == sessionId) activeTask = task;
        }
        _ = task.ContinueWith(completed => Finish(sessionId, cancellation, completed), TaskScheduler.Default);
    }

    internal static void Stop()
    {
        CancellationTokenSource cancellation;
        lock (SessionSync)
        {
            cancellation = activeCancellation;
            activeCancellation = null;
            activeTask = null;
            activeSessionId = 0L;
            if (TrajectoryObservationSessionLifecycle.ShouldClearLayers(
                TrajectoryObservationSessionEndReason.Cancellation))
                Presentation.ClearAll();
            PrintStatus("Observation stop requested; all observer layers were cleared.");
        }

        CancelWithoutWaiting(cancellation);
    }

    internal static void ShowInputFailure(Exception exception)
    {
        string message = exception == null ? "Unknown input error." : exception.Message;
        PrintStatus("Observation session did not start: " + message);
    }

    private static async Task RunSessionAsync(long sessionId, Pose2D goal,
        TrajectoryObservationSettings settings, TimeSpan observerPeriod,
        IReadOnlyList<TrajectoryObservationObstacle> obstacles, long obstacleSnapshotVersion,
        CancellationToken token)
    {
        token.ThrowIfCancellationRequested();
        VehicleMotionState initialState = ReadVehicleState();
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            initialState.Pose, goal, settings, obstacles, obstacleSnapshotVersion);

        var bootstrapTimer = Stopwatch.StartNew();
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, token);
        bootstrapTimer.Stop();
        if (!bootstrap.Succeeded)
        {
            DrawIfCurrent(sessionId, bootstrap, null, null, null,
                new TrajectoryObservationDiagnostic("bootstrap failed: " + bootstrap.FailureReason));
            LogIfCurrent(sessionId, "Planning bootstrap failed after " +
                bootstrapTimer.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) +
                " ms: " + bootstrap.FailureReason);
            return;
        }

        LogIfCurrent(sessionId, CreateBootstrapStatus(sessionId, bootstrap, settings, obstacles.Count,
            bootstrapTimer.Elapsed));
        var controller = new TrajectoryObservationController(bootstrap, settings,
            new EmPlanningService(new OsqpNativeSolver()),
            "trajectory-observer-" + sessionId.ToString(CultureInfo.InvariantCulture));
        LogIfCurrent(sessionId, controller.CreateConfigurationDiagnostic().Text);
        var observationLoop = new TrajectoryObservationLoop(controller);
        DirectionSegmentView segment = bootstrap.Segments[0];
        bool gearSwitchWaitingPrinted = false;

        while (true)
        {
            await Task.Delay(observerPeriod, token).ConfigureAwait(false);
            VehicleMotionState state = ReadVehicleState();
            DateTimeOffset now = state.CapturedAtUtc;
            TrajectoryObservationLoopTick tick = observationLoop.Tick(now, state, token);
            TrajectoryObservationObservation observation = tick.Observation;
            TrajectoryObservationDiagnostic diagnostic = TrajectoryObservationDiagnostics.Create(
                tick.LatestCycle, tick.LatestPlanningElapsed, tick.PlanningInFlight,
                observation.PublishedTrajectory);
            TrajectoryObservationCharts charts = observation.PublishedTrajectory == null
                ? null
                : TrajectoryObservationCharts.Build(observation.PublishedTrajectory, segment,
                    settings.MapPaddingMeters);
            TrajectoryObservationRuntimeState runtimeState = TrajectoryObservationRuntimeState.Create(
                now, observation.PublishedTrajectory);
            DrawIfCurrent(sessionId, bootstrap, observation, charts, runtimeState, diagnostic);
            if (runtimeState.WaitingAtGearSwitch && !gearSwitchWaitingPrinted)
            {
                LogIfCurrent(sessionId, runtimeState.WorldNotice);
                gearSwitchWaitingPrinted = true;
            }

            if (tick.PlanningStarted)
                LogIfCurrent(sessionId, "planning status=pending");
            if (tick.PlanningCompleted)
                LogIfCurrent(sessionId, diagnostic.Text);
        }
    }

    private static VehicleMotionState ReadVehicleState()
    {
        var location = DetourInterface.getCartLocation();
        if (location == null) throw new InvalidOperationException("Live localization is unavailable.");
        if (BasicPilotBase.Chassis == null) throw new InvalidOperationException("Live chassis read interface is unavailable.");
        var speed = BasicPilotBase.Chassis.GetCarSpeed(true);
        return new VehicleMotionState(
            new Pose2D(location.x / 1000d, location.y / 1000d, location.th * Math.PI / 180d),
            speed.Vx, null, DateTimeOffset.UtcNow, Interlocked.Increment(ref stateSequence));
    }

    private static string CreateBootstrapStatus(long sessionId, TrajectoryObservationBootstrapResult bootstrap,
        TrajectoryObservationSettings settings, int obstacleCount, TimeSpan elapsed)
    {
        float widthMillimeters = bootstrap.Map.Bounds.XMax - bootstrap.Map.Bounds.XMin;
        float heightMillimeters = bootstrap.Map.Bounds.YMax - bootstrap.Map.Bounds.YMin;
        return "Session " + sessionId.ToString(CultureInfo.InvariantCulture) + " bootstrap succeeded in " +
            elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms.\n" +
            "start=(" + Format(bootstrap.Job.Start.X) + ", " + Format(bootstrap.Job.Start.Y) + ", " +
            Format(bootstrap.Job.Start.Heading) + ") m/rad; goal=(" + Format(bootstrap.Job.Goal.X) + ", " +
            Format(bootstrap.Job.Goal.Y) + ", " + Format(bootstrap.Job.Goal.Heading) + ") m/rad.\n" +
            "map=" + Format(widthMillimeters) + " x " + Format(heightMillimeters) + " mm, resolution=" +
            Format(settings.MapResolutionMillimeters) + " mm, padding=" + Format(settings.MapPaddingMeters) +
            " m, obstacles=" + obstacleCount.ToString(CultureInfo.InvariantCulture) + ".\n" +
            "replan=" + Format(settings.ReplanPeriodSeconds) + " s, observer=" +
            Format(settings.ObserverPeriodSeconds) + " s, segment index=0.";
    }

    private static string CreateTickStatus(long sessionId, TrajectoryObservationObservation observation,
        TrajectoryObservationCharts charts, PlanningCycleResult latestCycle, TimeSpan latestPlanningElapsed,
        bool waitingAtGearSwitch)
    {
        string planning = latestCycle == null
            ? "not started"
            : latestCycle.Result.Status + ", published=" + latestCycle.Published +
                ", version=" + latestCycle.Version.ToString(CultureInfo.InvariantCulture);
        string selected = observation.SelectedPoint == null
            ? "selected trajectory point unavailable"
            : "selected t=" + Format(observation.SelectedPoint.TimeFromStart) +
                " s, path-S=" + Format(observation.SelectedPoint.PathS) + " m";
        string loggedCommand = observation.Command == null
            ? "logged command unavailable"
            : "logged command only: signed speed=" + Format(observation.Command.SignedLongitudinalVelocity) +
                " m/s, yaw rate=" + Format(observation.Command.YawRate) + " rad/s, direction=" +
                observation.Command.Direction + ", request direction change=" +
                observation.Command.RequestDirectionChange;
        string terminal = observation.PublishedTrajectory == null
            ? "trajectory unavailable"
            : "trajectory=" + observation.PublishedTrajectory.Metadata.TrajectoryId + ", terminal=" +
                observation.PublishedTrajectory.Metadata.TerminalType + ", segment index=" +
                observation.PublishedTrajectory.Metadata.SegmentIndex.ToString(CultureInfo.InvariantCulture);
        string projectionFailures = charts == null
            ? "unavailable"
            : charts.FailedProjectionCount.ToString(CultureInfo.InvariantCulture);
        string waiting = waitingAtGearSwitch
            ? "\n" + TrajectoryObservationRuntimeState.GearSwitchWaitingNotice
            : string.Empty;

        return "Session " + sessionId.ToString(CultureInfo.InvariantCulture) + ".\n" +
            "pose=(" + Format(observation.VehicleState.Pose.X) + ", " +
            Format(observation.VehicleState.Pose.Y) + ", " + Format(observation.VehicleState.Pose.Heading) +
            ") m/rad, actual longitudinal speed=" +
            Format(observation.VehicleState.SignedLongitudinalSpeedMetersPerSecond) + " m/s.\n" +
            "planning=" + planning + ", latest replan elapsed=" +
            latestPlanningElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms.\n" +
            terminal + ".\n" + selected + ".\n" + loggedCommand + ".\n" +
            "LS projection failures=" + projectionFailures + "." + waiting;
    }

    private static void DrawIfCurrent(long sessionId, TrajectoryObservationBootstrapResult bootstrap,
        TrajectoryObservationObservation observation, TrajectoryObservationCharts charts,
        TrajectoryObservationRuntimeState runtimeState, TrajectoryObservationDiagnostic diagnostic)
    {
        lock (SessionSync)
        {
            if (activeSessionId != sessionId) return;
            string diagnosticText = diagnostic == null ? string.Empty : diagnostic.Text;
            Presentation.DrawWorld(bootstrap, observation, runtimeState, diagnosticText);
            Presentation.DrawLs(charts, diagnosticText);
            Presentation.DrawSt(charts, diagnosticText);
        }
    }

    private static void LogIfCurrent(long sessionId, string message)
    {
        lock (SessionSync)
        {
            if (activeSessionId != sessionId) return;
            PrintStatus(message);
        }
    }

    private static void PrintStatus(string message)
    {
        string text = ObserveOnlyNotice + "\n" + message;
        Hedingben.ToastText(text, StatusChannel);
        Console.WriteLine("[TrajectoryObserver] " + text);
    }

    private static void Finish(long sessionId, CancellationTokenSource cancellation, Task completed)
    {
        try
        {
            completed.GetAwaiter().GetResult();
        }
        catch (OperationCanceledException)
        {
        }
        catch (Exception exception)
        {
            ClearAndLogRuntimeFaultIfCurrent(sessionId, exception);
        }
        finally
        {
            lock (SessionSync)
            {
                if (activeSessionId == sessionId)
                {
                    activeTask = null;
                    activeCancellation = null;
                }
            }
            cancellation.Dispose();
        }
    }

    private static void ClearAndLogRuntimeFaultIfCurrent(long sessionId, Exception exception)
    {
        lock (SessionSync)
        {
            if (activeSessionId != sessionId) return;
            if (TrajectoryObservationSessionLifecycle.ShouldClearLayers(
                TrajectoryObservationSessionEndReason.RuntimeFault))
                Presentation.ClearAll();
            PrintStatus("Observation session failed; all observer layers were cleared: " +
                exception.GetType().Name + ": " + exception.Message);
        }
    }

    private static void CancelWithoutWaiting(CancellationTokenSource cancellation)
    {
        if (cancellation == null) return;
        try
        {
            cancellation.Cancel();
        }
        catch (ObjectDisposedException)
        {
        }
    }

    private static string Format(double value)
    {
        return value.ToString("F3", CultureInfo.InvariantCulture);
    }
}