using System;
using System.Collections.Generic;
using System.IO;
using System.Text;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

namespace EMPlannerVerificationHost;

internal static class TrajectoryObservationChecks
{
    public static void Run()
    {
        VerifiesObservationSourceHasNoActuatorCalls();
        VerifiesObservationSourceUsesRequiredOperatorText();
        VerifiesStartGoalBoundsUseOnlyConfiguredPadding();
        RejectsObstacleOutsideConfiguredBounds();
        VerifiesLsAndStUsePublishedTrajectoryData();
        VerifiesPresentationTextDescribesObservationWithoutSendingCommand();
        VerifiesLsPresentationUsesPathSOnHorizontalAxis();
        VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot();
        FreezesBootstrapVehicleForRollingRequests();
        VerifiesEmptyMapBootstrapProducesPublishableReference();
    }

    private static void VerifiesObservationSourceHasNoActuatorCalls()
    {
        string sourceDirectory = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
            "ParkrobTrajplanner", "tarjplanner_movementtest");
        string movementTestSource = Path.Combine(sourceDirectory, "MovementTest.TrajectoryObservationTest.cs");
        Verification.True(File.Exists(movementTestSource), "observation MovementTest source exists");

        string[] forbiddenTokens =
        {
            ".SendXYThSpeed(", ".SendMotion(", ".SendTh(", ".AccumulateSpeed(",
            ".ComputeWheelsGeometrically(", ".DriveStop(", ".PredefinedDriveStop("
        };
        string[] runtimeSources = Directory.GetFiles(sourceDirectory, "*.cs", SearchOption.TopDirectoryOnly);
        for (int sourceIndex = 0; sourceIndex < runtimeSources.Length; sourceIndex++)
        {
            string source = File.ReadAllText(runtimeSources[sourceIndex]);
            for (int tokenIndex = 0; tokenIndex < forbiddenTokens.Length; tokenIndex++)
            {
                Verification.True(source.IndexOf(forbiddenTokens[tokenIndex], StringComparison.Ordinal) < 0,
                    "observation runtime source excludes actuator token " + forbiddenTokens[tokenIndex]);
            }
        }
    }

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

        Verification.True(source.Contains("[MovementTest(name = \"EM轨迹规划观察闭环测试\")]"),
            "observation MovementTest uses required Chinese display name");
        Verification.True(source.Contains("\"等待真实档位/方向确认；观察模式不会推进下一方向段\""),
            "observation MovementTest uses required gear-switch notice");
    }

    private static void VerifiesStartGoalBoundsUseOnlyConfiguredPadding()
    {
        var settings = new TrajectoryObservationSettings
        {
            MapPaddingMeters = 2d,
            MapResolutionMillimeters = 50f,
        };
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(10d, -5d, 0d), new Pose2D(13d, -1d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 17L);

        Verification.NearlyEqual(8000d, job.MapRequest.Bounds.XMin, "observer map x min");
        Verification.NearlyEqual(15000d, job.MapRequest.Bounds.XMax, "observer map x max");
        Verification.NearlyEqual(-7000d, job.MapRequest.Bounds.YMin, "observer map y min");
        Verification.NearlyEqual(1000d, job.MapRequest.Bounds.YMax, "observer map y max");
        Verification.NearlyEqual(50d, job.MapRequest.ResolutionMm, "observer map resolution");
    }

    private static void RejectsObstacleOutsideConfiguredBounds()
    {
        Verification.True(Throws(() => TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 1d, 0d), new TrajectoryObservationSettings(),
            new[] { TrajectoryObservationObstacle.Rectangle(-2100d, -2000d, 0d, 100d) }, 1L)),
            "observer obstacle outside configured bounds");
    }

    private static void VerifiesLsAndStUsePublishedTrajectoryData()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero);
        DirectionSegmentView segment = CreateStraightSegment();
        EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt);

        TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(trajectory, segment, 0.5d);
        Verification.Equal(2, charts.StSamples.Count, "observer ST sample count");
        Verification.NearlyEqual(0d, charts.StSamples[0].TimeFromStart, "observer first ST time");
        Verification.NearlyEqual(4d, charts.StSamples[0].PathS, "observer first ST path S");
        Verification.NearlyEqual(1d, charts.StSamples[1].TimeFromStart, "observer second ST time");
        Verification.NearlyEqual(5d, charts.StSamples[1].PathS, "observer second ST path S");
        Verification.Equal(2, charts.SpeedSamples.Count, "observer speed sample count");
        Verification.NearlyEqual(0.20d, charts.SpeedSamples[0].SignedLongitudinalVelocity,
            "observer first signed speed");
        Verification.NearlyEqual(0.40d, charts.SpeedSamples[1].SignedLongitudinalVelocity,
            "observer second signed speed");
        Verification.Equal(2, charts.LsSamples.Count, "observer LS projection count");
        Verification.Equal(0, charts.FailedProjectionCount, "observer LS projection failure count");
        Verification.NearlyEqual(10.25d, charts.LsSamples[0].PathS, "observer first LS path S");
        Verification.NearlyEqual(0.10d, charts.LsSamples[0].LateralOffset, "observer first LS offset");

        var settings = new TrajectoryObservationSettings();
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper(
            new CoarsePathPlanningService(), new PathSmoothingService())
            .Bootstrap(job, CancellationToken.None);
        Verification.True(bootstrap.Succeeded, "observer bootstrap succeeds");

        var planningService = new FixedTrajectoryPlanningService(trajectory);
        var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "observer-check");
        var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 1L);
        PlanningCycleResult firstCycle = controller.StartCycle(effectiveAt, state, CancellationToken.None)
            .GetAwaiter().GetResult();
        Verification.True(firstCycle.Published, "observer first cycle publishes");
        Verification.Equal(0, planningService.Requests[0].SegmentIndex, "observer starts at segment zero");
        Verification.Equal("observer-check-trajectory-1", planningService.Requests[0].OutputTrajectoryId,
            "observer first trajectory identity");
        Verification.NearlyEqual(settings.ReplanPeriodSeconds,
            planningService.Requests[0].Configuration.Scheduling.ReplanPeriodSeconds,
            "observer configured replan period");

        TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state);
        Verification.NearlyEqual(0.5d, observation.SelectedPoint.TimeFromStart,
            "observer executor interpolates from published effective time");
    }

    private static void VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 1, 0, 0, TimeSpan.Zero);
        var settings = new TrajectoryObservationSettings();
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        Verification.True(bootstrap.Succeeded, "observer rolling snapshot bootstrap succeeds");

        EmTrajectory published = CreatePublishedTrajectory(effectiveAt);
        var planningService = new FixedTrajectoryPlanningService(published);
        var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "snapshot-check");
        var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 2L);
        controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult();
        controller.StartCycle(effectiveAt.AddSeconds(settings.ReplanPeriodSeconds), state, CancellationToken.None)
            .GetAwaiter().GetResult();

        EmPlanningRequest rollingRequest = planningService.Requests[1];
        Verification.True(ReferenceEquals(published, rollingRequest.PreviousTrajectory),
            "observer rolling request uses the controlled published trajectory object");
        Verification.Equal(rollingRequest.PreviousTrajectory.Metadata.TrajectoryId,
            rollingRequest.PreviousTrajectoryId,
            "observer rolling request trajectory object and ID use one publication snapshot");
    }

    private static void VerifiesPresentationTextDescribesObservationWithoutSendingCommand()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero);
        var settings = new TrajectoryObservationSettings();
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        var controller = new TrajectoryObservationController(bootstrap, settings,
            new FixedTrajectoryPlanningService(CreatePublishedTrajectory(effectiveAt)), "presentation-check");
        var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 4L);
        controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult();
        TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state);
        TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(
            observation.PublishedTrajectory, CreateStraightSegment(), 0.5d);

        string text = TrajectoryObservationPresentationText.Create(observation, charts);

        Verification.True(text.Contains("OBSERVE_ONLY: no chassis command is sent."),
            "observer presentation observe-only notice");
        Verification.True(text.Contains("selected t=0.50 s, path-S=4.50 m"),
            "observer presentation selected point time and path-S");
        Verification.True(text.Contains("signed speed=0.30 m/s"),
            "observer presentation signed speed");
        Verification.True(text.Contains("yaw rate=0.00 rad/s"),
            "observer presentation yaw rate");
        Verification.True(text.Contains("LS projection failures=0"),
            "observer presentation LS projection failures");
    }

    private static void VerifiesLsPresentationUsesPathSOnHorizontalAxis()
    {
        TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(
            CreatePublishedTrajectory(new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero)),
            CreateStraightSegment(), 0.5d);

        TrajectoryObservationLsPresentationModel model = TrajectoryObservationLsPresentationModel.Create(charts);

        Verification.Equal("path-S (m)", model.HorizontalAxisLabel, "observer LS horizontal axis label");
        Verification.Equal("lateral offset (m)", model.VerticalAxisLabel, "observer LS vertical axis label");
        Verification.NearlyEqual(10.25d, model.Samples[0].HorizontalPathS,
            "observer LS path-S is horizontal");
        Verification.NearlyEqual(0.10d, model.Samples[0].VerticalLateralOffset,
            "observer LS lateral offset is vertical");
    }

    private static void FreezesBootstrapVehicleForRollingRequests()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 2, 0, 0, TimeSpan.Zero);
        var settings = new TrajectoryObservationSettings();
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        VehicleParameters originalVehicle = job.Vehicle;
        double expectedLength = originalVehicle.LengthMeters;
        double expectedWidth = originalVehicle.WidthMeters;
        double expectedMargin = originalVehicle.SafetyMarginMeters;
        double? expectedMaximumCurvature = originalVehicle.MaximumCurvaturePerMeter;
        double? expectedMinimumRadius = originalVehicle.MinimumTurningRadiusMeters;
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);
        Verification.True(bootstrap.Succeeded, "observer vehicle snapshot bootstrap succeeds");

        originalVehicle.LengthMeters = 91d;
        originalVehicle.WidthMeters = 92d;
        originalVehicle.SafetyMarginMeters = 93d;
        originalVehicle.MaximumCurvaturePerMeter = 94d;
        originalVehicle.MinimumTurningRadiusMeters = 95d;

        EmTrajectory published = CreatePublishedTrajectory(effectiveAt);
        var planningService = new FixedTrajectoryPlanningService(published);
        var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "vehicle-check");
        var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 3L);
        controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult();
        AssertVehicleSnapshot(planningService.Requests[0].Vehicle, expectedLength, expectedWidth, expectedMargin,
            expectedMaximumCurvature, expectedMinimumRadius, "mutated bootstrap job vehicle");
        Verification.True(!ReferenceEquals(originalVehicle, planningService.Requests[0].Vehicle),
            "observer request does not retain mutable bootstrap vehicle object");

        job.Vehicle = new VehicleParameters
        {
            LengthMeters = 101d,
            WidthMeters = 102d,
            SafetyMarginMeters = 103d,
            MaximumCurvaturePerMeter = 104d,
            MinimumTurningRadiusMeters = 105d,
        };
        controller.StartCycle(effectiveAt.AddSeconds(settings.ReplanPeriodSeconds), state, CancellationToken.None)
            .GetAwaiter().GetResult();
        AssertVehicleSnapshot(planningService.Requests[1].Vehicle, expectedLength, expectedWidth, expectedMargin,
            expectedMaximumCurvature, expectedMinimumRadius, "replaced bootstrap job vehicle");
    }

    private static void VerifiesEmptyMapBootstrapProducesPublishableReference()
    {
        var settings = new TrajectoryObservationSettings();
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0.5d, 0.5d, 0d), new Pose2D(3.5d, 0.5d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);

        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
            .Bootstrap(job, CancellationToken.None);

        Verification.True(bootstrap.Succeeded, "observer empty-map bootstrap succeeds");
        Verification.True(bootstrap.CoarseResult.MapResult.Succeeded,
            "observer empty-map bootstrap builds a successful map");
        Verification.True(bootstrap.Map != null, "observer empty-map bootstrap returns a map");
        Verification.Equal(PlanningStatus.Success, bootstrap.CoarseResult.PlanningResult.Status,
            "observer empty-map coarse planning succeeds");
        Verification.True(bootstrap.SmoothedPath != null && IsPublishableSmoothingStatus(bootstrap.SmoothedPath.Status),
            "observer empty-map bootstrap returns a publishable smoothing result");
        Verification.True(bootstrap.Segments.Count > 0,
            "observer empty-map bootstrap returns at least one direction segment");
    }

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

    private static void AssertVehicleSnapshot(VehicleParameters actual, double expectedLength, double expectedWidth,
        double expectedMargin, double? expectedMaximumCurvature, double? expectedMinimumRadius, string name)
    {
        Verification.NearlyEqual(expectedLength, actual.LengthMeters, name + " length");
        Verification.NearlyEqual(expectedWidth, actual.WidthMeters, name + " width");
        Verification.NearlyEqual(expectedMargin, actual.SafetyMarginMeters, name + " safety margin");
        Verification.Equal(expectedMaximumCurvature, actual.MaximumCurvaturePerMeter,
            name + " maximum curvature");
        Verification.Equal(expectedMinimumRadius, actual.MinimumTurningRadiusMeters,
            name + " minimum turning radius");
    }

    private static DirectionSegmentView CreateStraightSegment()
    {
        var points = new List<SmoothedPathPoint>
        {
            new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward,
                0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
            new SmoothedPathPoint(2d, 0d, 0d, 0d, 2d, TravelDirection.Forward,
                0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
        };
        return new DirectionSegmentView(0, TravelDirection.Forward, points,
            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 10d),
            new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 12d), 10d);
    }

    private static EmTrajectory CreatePublishedTrajectory(DateTimeOffset effectiveAt)
    {
        var metadata = new EmTrajectoryMetadata("observer-published", effectiveAt, effectiveAt, 1L,
            "observer-reference", 1L, string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(0.25d, 0.10d, 0d, 0.20d, 0d, 0d, 0, 0.25d, 4d,
                TravelDirection.Forward, EmBoundaryType.None, 0d, 0d),
            new EmTrajectoryPoint(1.25d, -0.20d, 0d, 0.40d, 1d, 0d, 0, 1.25d, 5d,
                TravelDirection.Forward, EmBoundaryType.None, 0d, 0d),
        });
    }

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

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

        public List<EmPlanningRequest> Requests { get; } = new List<EmPlanningRequest>();

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

    private static bool Throws(Action action)
    {
        try { action(); return false; }
        catch (ArgumentOutOfRangeException) { return true; }
    }
}