using System;
using System.Collections.Generic;
using System.Globalization;
using System.IO;
using System.Linq;
using System.Reflection;
using System.Text;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
using TrajectoryPlanningVisualization;

namespace EMPlannerVerificationHost;

internal static class TrajectoryObservationVisualizationChecks
{
    public static void Run()
    {
        VerifiesStaticSnapshotExportsFrozenConfigurationAndGeometry();
        VerifiesKinematicChartsKeepKnotAndIntervalSemantics();
        VerifiesHandoffUsesSharedReferenceSAndSurvivesProjectionFailure();
        VerifiesDynamicSnapshotContainsObservationEvidenceWithoutCyclePoints();
        VerifiesFullModeSnapshotPublishesSingleCurrentTrajectoryAndBoundaryMarkers();
        VerifiesPainterBoundarySemanticsMatchWeb();
        VerifiesWebPublisherFusesFaultWithoutStoppingObserverTicks();
        VerifiesWebPublisherGatesAtConfiguredCadenceAndSkipsDisabledOutput();
    }

    private static void VerifiesStaticSnapshotExportsFrozenConfigurationAndGeometry()
    {
        var settings = new TrajectoryObservationSettings();
        TrajectoryObservationBootstrapResult bootstrap = Bootstrap(settings);
        var controller = new TrajectoryObservationController(bootstrap, settings,
            new FixedPlanningService(CreateTrajectory("static", DateTimeOffset.UnixEpoch, 2, EmTerminalType.Goal,
                EmLongitudinalMode.RollingContinuation, TravelDirection.Forward)), "static-configuration");
        EmPlannerConfiguration effective = ReadEffectiveConfiguration(controller);
        settings.TimeHorizonSeconds = 99d;

        PlanningVisualizationStaticSnapshot snapshot = new TrajectoryObservationStaticSnapshotBuilder()
            .Build(bootstrap, effective, settings.CreateValidatedSnapshot(), 17L);

        Verification.Equal(10, snapshot.ConfigurationGroups.Count, "static configuration group count");
        Verification.Equal("调度", snapshot.ConfigurationGroups[0].ChineseTitle, "static configuration group 0");
        Verification.Equal("OSQP", snapshot.ConfigurationGroups[1].ChineseTitle, "static configuration group 1");
        Verification.Equal("车辆与安全", snapshot.ConfigurationGroups[2].ChineseTitle, "static configuration group 2");
        Verification.Equal("纵向限制", snapshot.ConfigurationGroups[3].ChineseTitle, "static configuration group 3");
        Verification.Equal("ST 权重", snapshot.ConfigurationGroups[4].ChineseTitle, "static configuration group 4");
        Verification.Equal("横向限制", snapshot.ConfigurationGroups[5].ChineseTitle, "static configuration group 5");
        Verification.Equal("LS 权重", snapshot.ConfigurationGroups[6].ChineseTitle, "static configuration group 6");
        Verification.Equal("走廊与投影", snapshot.ConfigurationGroups[7].ChineseTitle, "static configuration group 7");
        Verification.Equal("地图", snapshot.ConfigurationGroups[8].ChineseTitle, "static configuration group 8");
        Verification.Equal("可视化与换向确认", snapshot.ConfigurationGroups[9].ChineseTitle, "static configuration group 9");

        IReadOnlyDictionary<string, VisualizationValue> values = Flatten(snapshot);
        AssertValue(values, "TimeHorizonSeconds", "2", "effective configuration is frozen before editable settings change");
        AssertValue(values, "MaximumOsqpIterations", "100000", "effective OSQP iteration limit");
        AssertEffectiveValues(values, effective, settings, bootstrap);
        foreach (string rawName in new[]
        {
            "AbsoluteTolerance", "RelativeTolerance", "StrictResidualTolerance", "WarmStart", "Polish", "NativeVerbose",
            "LengthMeters", "WidthMeters", "SafetyMarginMeters", "MaximumCurvaturePerMeter",
            "MaximumForwardSpeedMetersPerSecond", "MaximumReverseSpeedMetersPerSecond",
            "MaximumAccelerationMetersPerSecondSquared", "MaximumDecelerationMetersPerSecondSquared",
            "MaximumJerkMetersPerSecondCubed", "MaximumLateralAccelerationMetersPerSecondSquared",
            "MaximumCurvatureRatePerMeterPerSecond", "StopSpeedToleranceMetersPerSecond", "ZeroSpeedHoldSeconds",
            "ReferenceSpeed", "Acceleration", "Jerk", "PreviousTrajectory", "TerminalAcceleration",
            "ReferenceOffset", "HeadingDeviation", "SecondDerivative", "ThirdDerivative", "Curvature",
            "CurvatureVariation", "RollingTerminal", "LongitudinalSampleSpacingMeters",
            "LateralSampleSpacingMeters", "MaximumLateralOffsetMeters", "AdditionalClearanceReserveMeters",
            "MaximumCollisionCheckStepMeters", "MaximumProjectionDistanceMeters", "MinimumFrenetDenominator",
            "BoundaryAnchorToleranceMeters", "SpatialToleranceMeters", "KinematicTolerance", "ObstacleSnapshotVersion",
            "ResolutionMm", "Bounds.XMin", "Bounds.XMax", "Bounds.YMin", "Bounds.YMax", "Rows", "Cols",
            "EnableWebVisualization", "AutoOpenWebVisualization", "WebVisualizationPort", "WebRefreshRateHz",
            "VisualizationHistoryCycleLimit", "EnableNativePainterVisualization",
            "DirectionConfirmationSpeedMetersPerSecond", "DirectionConfirmationSamples",
            "GearSwitchProjectionToleranceMeters", "GearSwitchStopHoldSeconds",
        })
            Verification.True(values.ContainsKey(rawName), "static configuration exports " + rawName);

        Verification.Equal(bootstrap.Segments.Count, snapshot.DirectionSegments.Count, "all direction segments exported");
        Verification.True(snapshot.StaticPolylines.Any(x => x.Id == "coarse-path"), "coarse path exported");
        Verification.True(snapshot.StaticPolylines.Any(x => x.Id == "local-g2-path"), "full Local G2 path exported");
        VisualizationMarker smoothStart = snapshot.StaticMarkers.Single(x => x.Kind == "smooth-start");
        Verification.Equal("平滑路径起点", smoothStart.LabelChinese,
            "static snapshot labels the smoothed-path start");
        Verification.NearlyEqual(bootstrap.SmoothedPath.Path[0].X, smoothStart.Position.X,
            "smoothed-path start marker X matches the first smoothed point");
        Verification.NearlyEqual(bootstrap.SmoothedPath.Path[0].Y, smoothStart.Position.Y,
            "smoothed-path start marker Y matches the first smoothed point");
        byte[] occupancy = Convert.FromBase64String(snapshot.OccupancyGrid.OccupancyBitsBase64);
        Verification.Equal((bootstrap.Map.Rows * bootstrap.Map.Cols + 7) / 8, occupancy.Length,
            "occupancy bitset compact length");
        bool verifiedOccupiedCell = false;
        for (int row = 0; row < bootstrap.Map.Rows; row++)
            for (int col = 0; col < bootstrap.Map.Cols; col++)
                if (bootstrap.Map.IsOccupied(row, col))
                {
                    int bit = row * bootstrap.Map.Cols + col;
                    Verification.True((occupancy[bit >> 3] & (1 << (bit & 7))) != 0,
                        "occupancy uses row-major LSB-first bitset");
                    verifiedOccupiedCell = true;
                    break;
                }
        Verification.True(verifiedOccupiedCell, "static fixture has an occupied cell");

        EmPlannerConfiguration defaults = EmPlannerConfiguration.CreateDefault();
        TrajectoryObservationBootstrapResult gearBootstrap = TrajectoryObservationBootstrapResult.Success(
            bootstrap.Job, bootstrap.CoarseResult, bootstrap.SmoothedPath,
            new[] { CreateGearSwitchSegment(0, 0d) });
        PlanningVisualizationStaticSnapshot gearStatic = new TrajectoryObservationStaticSnapshotBuilder()
            .Build(gearBootstrap, defaults, settings.CreateValidatedSnapshot(), 0L);
        Verification.True(gearStatic.StaticMarkers.Any(x => x.Kind == "gear-switch-end"),
            "static snapshot marks gear-switch s_end");
        Verification.True(snapshot.StaticMarkers.Any(x => x.Kind == "final-goal"),
            "static snapshot marks final goal s_end");
    }

    private static void VerifiesKinematicChartsKeepKnotAndIntervalSemantics()
    {
        DirectionSegmentView segment = CreateStraightSegment(0, 50d);
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        EmTrajectory trajectory = CreateTrajectory("rolling", DateTimeOffset.UnixEpoch, 21,
            EmTerminalType.Goal, EmLongitudinalMode.RollingContinuation, TravelDirection.Forward);

        var vehicle = new VehicleParameters
        {
            LengthMeters = 0.8d,
            WidthMeters = 0.6d,
            SafetyMarginMeters = 0.05d,
            MaximumCurvaturePerMeter = 0.4d,
            MinimumTurningRadiusMeters = 2.5d,
        };
        IReadOnlyList<VisualizationChart> charts = new TrajectoryObservationKinematicChartBuilder()
            .Build(trajectory, segment, vehicle, configuration);

        Verification.Equal(8, charts.Count, "all eight kinematic charts exported");
        Verification.Equal(20, FindChart(charts, "jerk-t").Series[0].Points.Count, "jerk has N-1 intervals");
        Verification.True(FindChart(charts, "jerk-t").NoteChinese.Contains("末点后无时间区间"),
            "jerk explains no terminal successor interval");
        Verification.Equal(21, FindChart(charts, "velocity-t").Series[0].Points.Count, "velocity remains knot based");
        Verification.Equal("ReferenceS (m)", FindChart(charts, "ls").XAxisLabel, "LS uses shared reference station");
        Verification.Equal("PathS (m)", FindChart(charts, "st").YAxisLabel, "ST uses plan-local PathS");
        foreach (string id in new[] { "curvature-s", "curvature-t", "velocity-t", "acceleration-t", "jerk-t" })
            Verification.True(FindChart(charts, id).Series.Any(x => x.LineStyle == VisualizationLineStyle.Limit),
                id + " has an effective limit line");
        Verification.True(!FindChart(charts, "yaw-rate-t").Series.Any(x => x.LineStyle == VisualizationLineStyle.Limit),
            "yaw rate has no invented limit");
        configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1.25d;
        charts = new TrajectoryObservationKinematicChartBuilder().Build(trajectory, segment, vehicle, configuration);
        Verification.Equal("ω (rad/s)", FindChart(charts, "yaw-rate-t").YAxisLabel,
            "yaw-rate axis uses omega unit");
        Verification.NearlyEqual(0.4d, FindChart(charts, "curvature-s")
            .Series.Single(x => x.Id == "curvature-s-positive-limit").Points[0].Y,
            "curvature hard limit uses vehicle curvature");
        Verification.True(FindChart(charts, "curvature-s").Series[0].Points.Any(p => Math.Abs(p.Y) > 1e-6),
            "curvature-distance uses vehicle curvature data");
        PropertyInfo? annotationsProperty = typeof(VisualizationChart).GetProperty("Annotations");
        Verification.True(annotationsProperty != null, "chart exposes annotation collection");
        if (annotationsProperty != null)
        {
            object annotationList = annotationsProperty!.GetValue(FindChart(charts, "st"))!;
            Verification.True(annotationList is System.Collections.IEnumerable,
                "ST annotations are enumerable");
            var annotations = new List<object>();
            foreach (object annotation in (System.Collections.IEnumerable)annotationList!)
            {
                annotations.Add(annotation);
            }
            Verification.True(annotations.Count > 0, "ST chart marks s_end");
            Verification.True(annotations.Any(annotation =>
                (string)annotation.GetType().GetProperty("Kind")!.GetValue(annotation)! == "s-end"),
                "ST s_end annotation kind is explicit");
            object lsAnnotationList = annotationsProperty!.GetValue(FindChart(charts, "ls"))!;
            var lsAnnotations = new List<object>();
            foreach (object annotation in (System.Collections.IEnumerable)lsAnnotationList!)
            {
                lsAnnotations.Add(annotation);
            }
            Verification.True(lsAnnotations.Count > 0, "LS chart marks s_end");
            Verification.True(lsAnnotations.Any(annotation =>
                (string)annotation.GetType().GetProperty("Kind")!.GetValue(annotation)! == "s-end"),
                "LS s_end annotation kind is explicit");
        }
        Verification.True(typeof(TrajectoryObservationDynamicSnapshotBuilder).GetMethods().Any(method =>
                method.Name == "Build" &&
                method.GetParameters().Any(parameter => parameter.ParameterType == typeof(VehicleParameters))),
            "dynamic builder accepts vehicle parameters");
        EmTrajectory unprojectable = CreateTrajectory("unprojectable", DateTimeOffset.UnixEpoch, 21,
            EmTerminalType.Goal, EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 100d);
        Verification.True(FindChart(new TrajectoryObservationKinematicChartBuilder().Build(unprojectable, segment, vehicle, configuration), "ls")
            .NoteChinese.Contains("投影失败=21"), "LS chart reports every failed full-segment projection");
    }

    private static void VerifiesHandoffUsesSharedReferenceSAndSurvivesProjectionFailure()
    {
        DirectionSegmentView segment = CreateStraightSegment(0, 100d);
        DateTimeOffset t0 = DateTimeOffset.UnixEpoch;
        EmTrajectory previous = CreateTrajectory("previous", t0, 11, EmTerminalType.Goal,
            EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 2d, 0d);
        EmTrajectory current = CreateTrajectory("current", t0.AddSeconds(1d), 2, EmTerminalType.Goal,
            EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 4d, 0d);
        TrajectoryObservationHandoffMetrics metrics = new TrajectoryObservationHandoffAnalyzer().Analyze(current, previous, segment);

        Verification.True(metrics.Available, "handoff samples absolute-time aligned trajectories");
        Verification.NearlyEqual(1d, metrics.DeltaPositionMeters!.Value, "handoff world delta");
        Verification.NearlyEqual(1d, metrics.DeltaReferenceSMeters!.Value, "handoff compares shared ReferenceS");
        Verification.True(metrics.DeltaReferenceSMeters.Value != current.Points[0].PathS - previous.Points[0].PathS,
            "handoff does not compare local PathS origins");

        EmTrajectory offSegment = CreateTrajectory("off-segment", t0.AddSeconds(1d), 2, EmTerminalType.Goal,
            EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 300d, 0d);
        TrajectoryObservationHandoffMetrics unavailable = new TrajectoryObservationHandoffAnalyzer().Analyze(offSegment, previous, segment);
        Verification.True(unavailable.Available, "handoff keeps non-projection deltas available");
        Verification.True(!unavailable.DeltaReferenceSMeters.HasValue, "projection failure makes only ReferenceS unavailable");
        Verification.True(unavailable.DeltaPositionMeters.HasValue && unavailable.DeltaVelocityMetersPerSecond.HasValue &&
            unavailable.DeltaAccelerationMetersPerSecondSquared.HasValue, "projection failure preserves world and kinematic deltas");
    }

    private static void VerifiesDynamicSnapshotContainsObservationEvidenceWithoutCyclePoints()
    {
        var settings = new TrajectoryObservationSettings();
        TrajectoryObservationBootstrapResult bootstrap = Bootstrap(settings);
        DateTimeOffset now = DateTimeOffset.UnixEpoch.AddMinutes(1);
        EmTrajectory current = CreateTrajectory("dynamic", now, 21, EmTerminalType.Goal,
            EmLongitudinalMode.RollingContinuation, TravelDirection.Forward);
        var controller = new TrajectoryObservationController(bootstrap, settings, new FixedPlanningService(current), "dynamic");
        var loop = new TrajectoryObservationLoop(controller);
        var state = new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0.1d, null, now, 1L);
        controller.StartCycle(now, state, CancellationToken.None).GetAwaiter().GetResult();
        TrajectoryObservationLoopTick tick = loop.Tick(now.AddSeconds(settings.ObserverPeriodSeconds), state, CancellationToken.None);
        EmPlannerConfiguration configuration = ReadEffectiveConfiguration(controller);
        EmTrajectory previous = CreateTrajectory("dynamic-previous", now.AddSeconds(-1d), 21, EmTerminalType.Goal,
            EmLongitudinalMode.RollingContinuation, TravelDirection.Forward);

        PlanningVisualizationDynamicSnapshot snapshot = new TrajectoryObservationDynamicSnapshotBuilder()
            .Build(5L, tick, controller.ActiveSegment, previous, bootstrap.Vehicle, configuration);

        Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "current"), "dynamic snapshot contains current trajectory");
        Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "previous"), "dynamic snapshot contains previous trajectory");
        Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "active-segment"), "dynamic snapshot contains active segment highlight");
        Verification.Equal(1, snapshot.DynamicPolylines.Count(x => x.Id == "current"),
            "same current trajectory is published once");
        Verification.True(!snapshot.DynamicPolylines.Any(x => x.Id == "current-horizon"),
            "identical current trajectory is not duplicated as current horizon");
        foreach (string rawName in new[] { "ActiveSegmentIndex", "ActiveDirection", "PlanningStatus", "LongitudinalMode",
            "TerminalType", "PlanningElapsedMilliseconds", "TrajectoryAgeSeconds", "LsProjectionFailures",
            "RollingTerminalConstraint", "TerminalVelocity", "TerminalAcceleration", "DeltaPositionMeters",
            "DeltaReferenceSMeters", "DeltaVelocityMetersPerSecond", "DeltaAccelerationMetersPerSecondSquared",
            "GearConfirmation" })
            Verification.True(snapshot.StatusValues.Any(x => x.RawName == rawName), "dynamic status includes " + rawName);
        Verification.True(snapshot.StatusValues.Single(x => x.RawName == "RollingTerminalConstraint").Value.Contains("滚动末端停车硬约束：未启用"),
            "rolling status describes disabled exact-stop constraint");
        Verification.NearlyEqual(current.Points[current.Points.Count - 1].SignedLongitudinalVelocity,
            snapshot.CycleSummary.TerminalVelocity!.Value, "cycle summary uses actual terminal velocity");
        Verification.NearlyEqual(ReadAcceleration(current.Points[current.Points.Count - 1]),
            snapshot.CycleSummary.TerminalAcceleration!.Value, "cycle summary uses actual terminal acceleration");
        Verification.True(!typeof(VisualizationCycleSummary).GetProperties().Any(x =>
            x.PropertyType == typeof(EmTrajectory) || x.PropertyType == typeof(EmTrajectoryPoint) ||
            (x.PropertyType.IsGenericType && x.PropertyType.GetGenericArguments().Any(t => t == typeof(EmTrajectoryPoint)))),
            "cycle summary does not carry complete point arrays");

        EmTrajectory exactStop = CreateTrajectory("exact-stop", now, 21, EmTerminalType.GearSwitch,
            EmLongitudinalMode.ExactStopAtBoundary, TravelDirection.Forward);
        var exactController = new TrajectoryObservationController(bootstrap, settings,
            new FixedPlanningService(exactStop), "exact-stop");
        exactController.StartCycle(now, state, CancellationToken.None).GetAwaiter().GetResult();
        TrajectoryObservationLoopTick exactTick = new TrajectoryObservationLoop(exactController)
            .Tick(now.AddSeconds(settings.ObserverPeriodSeconds), state, CancellationToken.None);
        PlanningVisualizationDynamicSnapshot exactSnapshot = new TrajectoryObservationDynamicSnapshotBuilder().Build(6L,
            exactTick, exactController.ActiveSegment, null, bootstrap.Vehicle,
            ReadEffectiveConfiguration(exactController));
        Verification.True(exactSnapshot.StatusValues.Single(x => x.RawName == "RollingTerminalConstraint").Value.Contains("精确停车锚点"),
            "exact-stop status describes the true stop anchor");
        Verification.NearlyEqual(exactStop.Points[exactStop.Points.Count - 1].SignedLongitudinalVelocity,
            exactSnapshot.CycleSummary.TerminalVelocity!.Value, "exact-stop terminal velocity is real final knot");
        Verification.NearlyEqual(ReadAcceleration(exactStop.Points[exactStop.Points.Count - 1]),
            exactSnapshot.CycleSummary.TerminalAcceleration!.Value, "exact-stop terminal acceleration is real final knot");

        EmTrajectory offSegment = CreateTrajectory("dynamic-off-segment", now, 21, EmTerminalType.Goal,
            EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 100d);
        var offSegmentController = new TrajectoryObservationController(bootstrap, settings,
            new FixedPlanningService(offSegment), "dynamic-off-segment");
        offSegmentController.StartCycle(now, state, CancellationToken.None).GetAwaiter().GetResult();
        TrajectoryObservationLoopTick offSegmentTick = new TrajectoryObservationLoop(offSegmentController)
            .Tick(now.AddSeconds(settings.ObserverPeriodSeconds), state, CancellationToken.None);
        PlanningVisualizationDynamicSnapshot recovered = new TrajectoryObservationDynamicSnapshotBuilder().Build(7L,
            offSegmentTick, offSegmentController.ActiveSegment, null, bootstrap.Vehicle,
            ReadEffectiveConfiguration(offSegmentController));
        Verification.Equal("21", recovered.StatusValues.Single(x => x.RawName == "LsProjectionFailures").Value,
            "projection failure is reported while dynamic snapshot construction remains available");
    }

    private static void VerifiesFullModeSnapshotPublishesSingleCurrentTrajectoryAndBoundaryMarkers()
    {
        var settings = new TrajectoryObservationSettings();
        TrajectoryObservationBootstrapResult goalBootstrap = Bootstrap(settings);
        TrajectoryObservationBootstrapResult gearBootstrap = TrajectoryObservationBootstrapResult.Success(
            goalBootstrap.Job, goalBootstrap.CoarseResult, goalBootstrap.SmoothedPath,
            new[] { CreateGearSwitchSegment(0, 0d) });
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();

        PlanningVisualizationDynamicSnapshot gearSnapshot = BuildDynamicSnapshot(gearBootstrap, settings, configuration,
            CreateTrajectory("full-gear", DateTimeOffset.UnixEpoch.AddSeconds(1d), 21, EmTerminalType.GearSwitch,
                EmLongitudinalMode.ExactStopAtBoundary, TravelDirection.Forward, 0d, 0d,
                EmPlanningScope.FullDirectionSegment));
        Verification.Equal(1, gearSnapshot.DynamicPolylines.Count(x => x.Kind == "current"),
            "full mode publishes one current trajectory");
        Verification.True(!gearSnapshot.DynamicPolylines.Any(x => x.Id == "current-horizon"),
            "full mode does not duplicate current horizon");
        Verification.True(gearSnapshot.DynamicMarkers.Any(x => x.Kind == "vehicle"),
            "dynamic snapshot marks the vehicle");
        Verification.True(gearSnapshot.DynamicMarkers.Any(x => x.Kind == "plan-start"),
            "dynamic snapshot marks the plan start");
        VisualizationMarker gearEnd = gearSnapshot.DynamicMarkers.Single(x => x.Kind == "gear-switch-end");
        Verification.True(gearEnd.LabelChinese.Contains("换向点") && gearEnd.LabelChinese.Contains("s_end"),
            "gear switch boundary uses the explicit s_end label");

        PlanningVisualizationDynamicSnapshot goalSnapshot = BuildDynamicSnapshot(goalBootstrap, settings, configuration,
            CreateTrajectory("full-goal", DateTimeOffset.UnixEpoch.AddSeconds(2d), 21, EmTerminalType.Goal,
                EmLongitudinalMode.ExactStopAtBoundary, TravelDirection.Forward, 0d, 0d,
                EmPlanningScope.FullDirectionSegment));
        Verification.Equal(1, goalSnapshot.DynamicPolylines.Count(x => x.Kind == "current"),
            "goal full mode publishes one current trajectory");
        Verification.True(!goalSnapshot.DynamicPolylines.Any(x => x.Id == "current-horizon"),
            "goal full mode does not duplicate current horizon");
        VisualizationMarker finalGoal = goalSnapshot.DynamicMarkers.Single(x => x.Kind == "final-goal");
        Verification.True(finalGoal.LabelChinese.Contains("终点") && finalGoal.LabelChinese.Contains("s_end"),
            "final goal boundary uses the explicit s_end label");
    }

    private static void VerifiesWebPublisherFusesFaultWithoutStoppingObserverTicks()
    {
        var settings = new TrajectoryObservationSettings { EnableWebVisualization = true };
        var sink = new RecordingVisualizationSink { ThrowOnPublish = true };
        var logs = new List<string>();
        var publisher = new TrajectoryObservationVisualizationPublisher(settings, sink, logs.Add);
        DateTimeOffset now = DateTimeOffset.UnixEpoch;

        publisher.Start(CreateStaticSnapshot(settings));
        Verification.True(!publisher.TryPublish(now, () => CreateDynamicSnapshot(now)),
            "publisher reports a failed web publish");
        Verification.True(!publisher.TryPublish(now.AddSeconds(1d), () => throw new InvalidOperationException("must not build after fuse")),
            "publisher permanently disables web output after the first fault");
        Verification.Equal(1, sink.PublishCalls, "publisher invokes the throwing sink once");
        Verification.Equal(1, sink.StopCalls, "publisher stops the sink once after its fault");
        Verification.Equal(1, logs.Count, "publisher logs the web fault once");
        Verification.True(publisher.FaultReason.Contains("网页"), "publisher records an immutable Chinese web fault reason");

        TrajectoryObservationBootstrapResult bootstrap = Bootstrap(settings);
        DateTimeOffset tickAt = now.AddSeconds(1d);
        var controller = new TrajectoryObservationController(bootstrap, settings,
            new FixedPlanningService(CreateTrajectory("publisher-loop", tickAt, 2, EmTerminalType.Goal,
                EmLongitudinalMode.RollingContinuation, TravelDirection.Forward)), "publisher-loop");
        var loop = new TrajectoryObservationLoop(controller);
        var state = new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, null, tickAt, 1L);
        TrajectoryObservationLoopTick firstTick = loop.Tick(tickAt, state, CancellationToken.None);
        TrajectoryObservationLoopTick secondTick = loop.Tick(tickAt.AddSeconds(settings.ObserverPeriodSeconds), state,
            CancellationToken.None);
        Verification.True(firstTick.PlanningStarted || secondTick.PlanningStarted || secondTick.PlanningCompleted,
            "observer loop continues producing ticks after a web publisher fault");
    }

    private static void VerifiesWebPublisherGatesAtConfiguredCadenceAndSkipsDisabledOutput()
    {
        var settings = new TrajectoryObservationSettings
        {
            EnableWebVisualization = true,
            WebRefreshRateHz = 10d,
        };
        var sink = new RecordingVisualizationSink();
        var publisher = new TrajectoryObservationVisualizationPublisher(settings, sink, _ => { });
        DateTimeOffset start = DateTimeOffset.UnixEpoch;
        publisher.Start(CreateStaticSnapshot(settings));
        for (int index = 0; index <= 20; index++)
            publisher.TryPublish(start.AddMilliseconds(index * 50), () => CreateDynamicSnapshot(start));
        Verification.True(sink.PublishCalls <= 11, "20 Hz observer ticks publish no more than 11 snapshots at 10 Hz over one second");
        Verification.Equal(11, sink.PublishCalls, "publisher includes the initial 10 Hz snapshot");
        publisher.Stop();
        publisher.Stop();
        Verification.Equal(1, sink.StopCalls, "publisher stop is idempotent");

        var disabledSettings = new TrajectoryObservationSettings { EnableWebVisualization = false };
        var disabledSink = new RecordingVisualizationSink();
        var disabledPublisher = new TrajectoryObservationVisualizationPublisher(disabledSettings, disabledSink, _ => { });
        int factoryCalls = 0;
        disabledPublisher.Start(CreateStaticSnapshot(disabledSettings));
        disabledPublisher.TryPublish(start, () =>
        {
            factoryCalls++;
            return CreateDynamicSnapshot(start);
        });
        Verification.Equal(0, disabledSink.StartCalls, "disabled web visualization does not start a service");
        Verification.Equal(0, factoryCalls, "disabled web visualization does not build dynamic snapshots");
    }

    private static void VerifiesPainterBoundarySemanticsMatchWeb()
    {
        string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
            "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs");
        string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath));

        Verification.True(source.Contains("DrawBoundaryMarkers"),
            "observer painter draws shared boundary markers");
        foreach (string expected in new[]
        {
            "计划起点", "车辆", "换向点 ", " / s_end", "终点 / s_end",
        })
        {
            Verification.True(source.Contains(expected),
                "observer painter boundary semantics include " + expected);
        }
    }

    private static PlanningVisualizationStaticSnapshot CreateStaticSnapshot(TrajectoryObservationSettings settings)
    {
        TrajectoryObservationBootstrapResult bootstrap = Bootstrap(settings);
        var controller = new TrajectoryObservationController(bootstrap, settings,
            new FixedPlanningService(CreateTrajectory("publisher-static", DateTimeOffset.UnixEpoch, 2,
                EmTerminalType.Goal, EmLongitudinalMode.RollingContinuation, TravelDirection.Forward)), "publisher-static");
        return new TrajectoryObservationStaticSnapshotBuilder().Build(bootstrap,
            ReadEffectiveConfiguration(controller), settings.CreateValidatedSnapshot(), 0L);
    }

    private static PlanningVisualizationDynamicSnapshot CreateDynamicSnapshot(DateTimeOffset now)
    {
        return new PlanningVisualizationDynamicSnapshot(1L, now, "OBSERVE_ONLY", 0, "Forward",
            new VisualizationPose(0d, 0d, 0d), Array.Empty<VisualizationPolyline>(),
            Array.Empty<VisualizationMarker>(), Array.Empty<VisualizationChart>(), Array.Empty<VisualizationValue>(),
            new VisualizationCycleSummary(1L, now, "Success", true, 0d, 0, "Forward", "RollingContinuation",
                "Goal", 0d, 0d, string.Empty));
    }

    private sealed class RecordingVisualizationSink : ITrajectoryObservationVisualizationSink
    {
        public bool ThrowOnPublish { get; set; }
        public int StartCalls { get; private set; }
        public int PublishCalls { get; private set; }
        public int StopCalls { get; private set; }

        public PlanningVisualizationSessionInfo Start(PlanningVisualizationOptions options,
            PlanningVisualizationStaticSnapshot snapshot)
        {
            StartCalls++;
            return new PlanningVisualizationSessionInfo(new Uri("http://127.0.0.1:12345/?token=test"), "test");
        }

        public void Publish(PlanningVisualizationDynamicSnapshot snapshot)
        {
            PublishCalls++;
            if (ThrowOnPublish) throw new InvalidOperationException("synthetic publish failure");
        }

        public void Stop()
        {
            StopCalls++;
        }
    }

    private static TrajectoryObservationBootstrapResult Bootstrap(TrajectoryObservationSettings settings)
    {
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            new[] { TrajectoryObservationObstacle.Circle(1500d, 1500d, 100d) }, 17L);
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper().Bootstrap(job, CancellationToken.None);
        Verification.True(bootstrap.Succeeded, "visualization bootstrap succeeds");
        return bootstrap;
    }

    private static EmPlannerConfiguration ReadEffectiveConfiguration(TrajectoryObservationController controller)
    {
        MethodInfo method = typeof(TrajectoryObservationController).GetMethod("CreateEffectiveConfigurationSnapshot",
            BindingFlags.Instance | BindingFlags.NonPublic)!;
        return (EmPlannerConfiguration)method.Invoke(controller, Array.Empty<object>())!;
    }

    private static IReadOnlyDictionary<string, VisualizationValue> Flatten(PlanningVisualizationStaticSnapshot snapshot)
    {
        return snapshot.ConfigurationGroups.SelectMany(x => x.Entries)
            .GroupBy(x => x.RawName).ToDictionary(x => x.Key, x => x.Last());
    }

    private static double ReadAcceleration(EmTrajectoryPoint point)
    {
        PropertyInfo property = typeof(EmTrajectoryPoint).GetProperty("LongitudinalAcceleration",
            BindingFlags.Instance | BindingFlags.NonPublic)!;
        return (double)property.GetValue(point)!;
    }

    private static void AssertValue(IReadOnlyDictionary<string, VisualizationValue> values, string rawName, string expected, string name)
    {
        Verification.Equal(expected, values[rawName].Value, name);
    }

    private static void AssertEffectiveValues(IReadOnlyDictionary<string, VisualizationValue> values,
        EmPlannerConfiguration configuration, TrajectoryObservationSettings settings,
        TrajectoryObservationBootstrapResult bootstrap)
    {
        string F(double value) => value.ToString("R", CultureInfo.InvariantCulture);
        string I(int value) => value.ToString(CultureInfo.InvariantCulture);
        string B(bool value) => value ? "true" : "false";
        foreach ((string RawName, string Value) expected in new[]
        {
            ("ReplanPeriodSeconds", F(configuration.Scheduling.ReplanPeriodSeconds)),
            ("TimeHorizonSeconds", F(configuration.Scheduling.TimeHorizonSeconds)),
            ("DistanceHorizonMeters", F(configuration.Scheduling.DistanceHorizonMeters)),
            ("OutputTimeStepSeconds", F(configuration.Scheduling.OutputTimeStepSeconds)),
            ("SolverTimeoutSeconds", F(configuration.Scheduling.SolverTimeoutSeconds)),
            ("HandoffLookaheadSeconds", F(configuration.Scheduling.HandoffLookaheadSeconds)),
            ("MaximumVehicleStateAgeSeconds", F(configuration.Scheduling.MaximumVehicleStateAgeSeconds)),
            ("MaximumOuterIterations", I(configuration.Solver.MaximumOuterIterations)),
            ("MaximumOsqpIterations", I(configuration.Solver.MaximumOsqpIterations)),
            ("AbsoluteTolerance", F(configuration.Solver.AbsoluteTolerance)),
            ("RelativeTolerance", F(configuration.Solver.RelativeTolerance)),
            ("StrictResidualTolerance", F(configuration.Solver.StrictResidualTolerance)),
            ("WarmStart", B(configuration.Solver.WarmStart)), ("Polish", B(configuration.Solver.Polish)),
            ("NativeVerbose", B(configuration.Solver.NativeVerbose)),
            ("MaximumForwardSpeedMetersPerSecond", F(configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond)),
            ("MaximumReverseSpeedMetersPerSecond", F(configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond)),
            ("MaximumAccelerationMetersPerSecondSquared", F(configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared)),
            ("MaximumDecelerationMetersPerSecondSquared", F(configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared)),
            ("MaximumJerkMetersPerSecondCubed", F(configuration.Longitudinal.MaximumJerkMetersPerSecondCubed)),
            ("MaximumLateralAccelerationMetersPerSecondSquared", F(configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared)),
            ("MaximumCurvatureRatePerMeterPerSecond", F(configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond)),
            ("StopSpeedToleranceMetersPerSecond", F(configuration.Longitudinal.StopSpeedToleranceMetersPerSecond)),
            ("ZeroSpeedHoldSeconds", F(configuration.Longitudinal.ZeroSpeedHoldSeconds)),
            ("ReferenceSpeed", F(configuration.Longitudinal.Weights.ReferenceSpeed)),
            ("Acceleration", F(configuration.Longitudinal.Weights.Acceleration)),
            ("Jerk", F(configuration.Longitudinal.Weights.Jerk)),
            ("TerminalAcceleration", F(configuration.Longitudinal.Weights.TerminalAcceleration)),
            ("ReferenceOffset", F(configuration.Lateral.Weights.ReferenceOffset)),
            ("HeadingDeviation", F(configuration.Lateral.Weights.HeadingDeviation)),
            ("SecondDerivative", F(configuration.Lateral.Weights.SecondDerivative)),
            ("ThirdDerivative", F(configuration.Lateral.Weights.ThirdDerivative)),
            ("Curvature", F(configuration.Lateral.Weights.Curvature)),
            ("CurvatureVariation", F(configuration.Lateral.Weights.CurvatureVariation)),
            ("RollingTerminal", F(configuration.Lateral.Weights.RollingTerminal)),
            ("MaximumLateralStepPerIterationMeters", F(configuration.Lateral.MaximumLateralStepPerIterationMeters)),
            ("MaximumLateralSlope", F(configuration.Lateral.MaximumLateralSlope)),
            ("MaximumLateralSecondDerivativePerMeter", F(configuration.Lateral.MaximumLateralSecondDerivativePerMeter)),
            ("MaximumLateralThirdDerivativePerSquareMeter", F(configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter)),
            ("LongitudinalSampleSpacingMeters", F(configuration.Corridor.LongitudinalSampleSpacingMeters)),
            ("LateralSampleSpacingMeters", F(configuration.Corridor.LateralSampleSpacingMeters)),
            ("MaximumLateralOffsetMeters", F(configuration.Corridor.MaximumLateralOffsetMeters)),
            ("AdditionalClearanceReserveMeters", F(configuration.Corridor.AdditionalClearanceReserveMeters)),
            ("MaximumCollisionCheckStepMeters", F(configuration.Corridor.MaximumCollisionCheckStepMeters)),
            ("MaximumProjectionDistanceMeters", F(configuration.Frenet.MaximumProjectionDistanceMeters)),
            ("MinimumFrenetDenominator", F(configuration.Frenet.MinimumFrenetDenominator)),
            ("BoundaryAnchorToleranceMeters", F(configuration.Frenet.BoundaryAnchorToleranceMeters)),
            ("SpatialToleranceMeters", F(configuration.Validation.SpatialToleranceMeters)),
            ("KinematicTolerance", F(configuration.Validation.KinematicTolerance)),
            ("LengthMeters", F(bootstrap.Vehicle.LengthMeters)), ("WidthMeters", F(bootstrap.Vehicle.WidthMeters)),
            ("SafetyMarginMeters", F(bootstrap.Vehicle.SafetyMarginMeters)),
            ("MaximumCurvaturePerMeter", F(bootstrap.Vehicle.MaximumCurvaturePerMeter!.Value)),
            ("ObstacleSnapshotVersion", "17"), ("ResolutionMm", F(bootstrap.Map.ResolutionMm)),
            ("Bounds.XMin", F(bootstrap.Map.Bounds.XMin)), ("Bounds.XMax", F(bootstrap.Map.Bounds.XMax)),
            ("Bounds.YMin", F(bootstrap.Map.Bounds.YMin)), ("Bounds.YMax", F(bootstrap.Map.Bounds.YMax)),
            ("Rows", I(bootstrap.Map.Rows)), ("Cols", I(bootstrap.Map.Cols)),
            ("EnableWebVisualization", B(settings.EnableWebVisualization)), ("AutoOpenWebVisualization", B(settings.AutoOpenWebVisualization)),
            ("WebVisualizationPort", I(settings.WebVisualizationPort)), ("WebRefreshRateHz", F(settings.WebRefreshRateHz)),
            ("VisualizationHistoryCycleLimit", I(settings.VisualizationHistoryCycleLimit)),
            ("EnableNativePainterVisualization", B(settings.EnableNativePainterVisualization)),
            ("DirectionConfirmationSpeedMetersPerSecond", F(settings.DirectionConfirmationSpeedMetersPerSecond)),
            ("DirectionConfirmationSamples", I(settings.DirectionConfirmationSamples)),
            ("GearSwitchProjectionToleranceMeters", F(settings.GearSwitchProjectionToleranceMeters)),
            ("GearSwitchStopHoldSeconds", F(settings.GearSwitchStopHoldSeconds)),
        })
            AssertValue(values, expected.RawName, expected.Value, "effective configuration value " + expected.RawName);
    }

    private static VisualizationChart FindChart(IReadOnlyList<VisualizationChart> charts, string id)
    {
        return charts.Single(x => x.Id == id);
    }

    private static PlanningVisualizationDynamicSnapshot BuildDynamicSnapshot(
        TrajectoryObservationBootstrapResult bootstrap, TrajectoryObservationSettings settings,
        EmPlannerConfiguration configuration, EmTrajectory trajectory)
    {
        DateTimeOffset now = DateTimeOffset.UnixEpoch.AddMinutes(1);
        var controller = new TrajectoryObservationController(bootstrap, settings,
            new FixedPlanningService(trajectory), "snapshot-" + trajectory.Metadata.TrajectoryId);
        var loop = new TrajectoryObservationLoop(controller);
        var state = new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0.1d, null, now, 1L);
        controller.StartCycle(now, state, CancellationToken.None).GetAwaiter().GetResult();
        TrajectoryObservationLoopTick tick = loop.Tick(now.AddSeconds(settings.ObserverPeriodSeconds),
            state, CancellationToken.None);
        return new TrajectoryObservationDynamicSnapshotBuilder().Build(
            1L, tick, controller.ActiveSegment, null, bootstrap.Vehicle, configuration);
    }

    private static DirectionSegmentView CreateStraightSegment(int index, double sourceStart)
    {
        var points = new List<MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint>
        {
            new MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint(0d, 0d, 0d, 0d, 0d,
                TravelDirection.Forward, 0d, 0d, 1d, false, MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPointSource.Anchor),
            new MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint(10d, 0d, 0d, 0d, 10d,
                TravelDirection.Forward, 0d, 0d, 1d, false, MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPointSource.Anchor),
        };
        return new DirectionSegmentView(index, TravelDirection.Forward, points,
            new ReferenceBoundary(index, 0d, EmBoundaryType.None, sourceStart),
            new ReferenceBoundary(index, 10d, EmBoundaryType.Goal, sourceStart + 10d), sourceStart);
    }

    private static DirectionSegmentView CreateGearSwitchSegment(int index, double sourceStart)
    {
        var points = new List<MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint>
        {
            new MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint(0d, 0d, 0d, 0d, 0d,
                TravelDirection.Forward, 0d, 0d, 1d, false, MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPointSource.Anchor),
            new MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint(10d, 0d, 0d, 0d, 10d,
                TravelDirection.Forward, 0d, 0d, 1d, false, MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPointSource.Anchor),
        };
        return new DirectionSegmentView(index, TravelDirection.Forward, points,
            new ReferenceBoundary(index, 0d, EmBoundaryType.None, sourceStart),
            new ReferenceBoundary(index, 10d, EmBoundaryType.GearSwitchApproach, sourceStart + 10d), sourceStart);
    }

    private static EmTrajectory CreateTrajectory(string id, DateTimeOffset effectiveAt, int count, EmTerminalType terminal,
        EmLongitudinalMode mode, TravelDirection direction, double startX = 0d, double startPathS = 1000d,
        EmPlanningScope planningScope = EmPlanningScope.RollingHorizon)
    {
        var points = new List<EmTrajectoryPoint>();
        for (int index = 0; index < count; index++)
        {
            double t = index * 0.1d;
            points.Add(new EmTrajectoryPoint(startX + t, 0d, 0d, 0.1d + t, t, 0.01d, 0,
                startX + t, startPathS + t, direction, index == count - 1 ? EmBoundaryType.Goal : EmBoundaryType.None,
                0.2d + t, 0.3d + t));
        }
        return new EmTrajectory(new EmTrajectoryMetadata(id, effectiveAt, effectiveAt, 1L, "visualization-reference", 1L,
            string.Empty, 0, direction, terminal, mode, planningScope), points);
    }

    private sealed class FixedPlanningService : IEmPlanningService
    {
        private readonly EmTrajectory trajectory;

        public FixedPlanningService(EmTrajectory trajectory) { this.trajectory = trajectory; }

        public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
        {
            return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
        }
    }
}
