using System;
using System.Collections.Generic;
using System.Globalization;
using System.IO;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.Mapping;
using Newtonsoft.Json;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;

/// <summary>显式开发入口：从已成功的 Hybrid A* 粗路径结果生成八个稳定的快速比较夹具。</summary>
public static class SmoothingFixtureGenerator
{
    // 仅用于开发时固化真实规划输出；不会改变业务场景或运行时服务的预算。
    private static readonly TimeSpan FixtureGenerationSearchTimeout = TimeSpan.FromSeconds(120d);

    /// <summary>生成夹具 JSON；目标已存在且未明确允许覆盖时拒绝写入。</summary>
    public static void Generate(string outputPath, bool overwrite)
    {
        if (string.IsNullOrWhiteSpace(outputPath)) throw new ArgumentException("输出路径不能为空。", nameof(outputPath));
        if (File.Exists(outputPath) && !overwrite)
            throw new IOException("夹具目标已存在；必须显式允许覆盖。");

        SmoothingFixtureDocument document = CreateDocument();
        Directory.CreateDirectory(Path.GetDirectoryName(Path.GetFullPath(outputPath)));
        var settings = new JsonSerializerSettings { Culture = CultureInfo.InvariantCulture, Formatting = Formatting.Indented };
        File.WriteAllText(outputPath, JsonConvert.SerializeObject(document, settings), new System.Text.UTF8Encoding(false));
    }

    private static SmoothingFixtureDocument CreateDocument()
    {
        var document = new SmoothingFixtureDocument { SchemaVersion = 1 };
        var planner = new CoarsePathPlanningService();
        IReadOnlyList<FixturePlan> plans = CreatePlans();
        for (int index = 0; index < plans.Count; index++)
        {
            FixturePlan plan = plans[index];
            CoarsePathPlanningJob job = plan.CreateJob();
            job.Configuration.SearchTimeout = FixtureGenerationSearchTimeout;
            CoarsePathPlanningJobResult result = planner.Plan(job, CancellationToken.None);
            document.Scenarios.Add(CreateRecord(plan.Id, job, result));
        }

        for (int index = 0; index < document.Scenarios.Count; index++)
            document.Scenarios[index].ConfigurationFingerprint = SmoothingScenarioFixtureLoader.ComputeFingerprint(document.Scenarios[index]);
        return document;
    }

    private static IReadOnlyList<FixturePlan> CreatePlans()
    {
        return new List<FixturePlan>
        {
            new FixturePlan("straight", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ExplicitEmpty)),
            new FixturePlan("single-turn", () => CoarsePathScenarioFactory.CreateManualGoalDemo(1000d, 1000d, 0d, 3000d, 2000d, 45d)),
            new FixturePlan("s-bend", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ManualAndTwoLeg, 1000d, 1000d, 45d)),
            new FixturePlan("large-heading-change", () => CoarsePathScenarioFactory.CreateManualGoalDemo(1000d, 1000d, 0d, 3000d, 3000d, 90d)),
            new FixturePlan("rectangle-detour", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.RectangleDetour)),
            new FixturePlan("multi-obstacle-detour", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ManualAndTwoLeg)),
            new FixturePlan("narrow-corridor", CreateNarrowCorridor),
            new FixturePlan("forward-reverse-switch", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ReverseGearSwitch)),
        };
    }

    private static CoarsePathPlanningJob CreateNarrowCorridor()
    {
        return CreateFixedObstacleJob("narrow-corridor", new IMapObstacle[]
        {
            new AxisAlignedRectangleObstacle(1500f, 4500f, 500f, 1200f),
            new AxisAlignedRectangleObstacle(1500f, 4500f, 2800f, 3500f),
        });
    }

    private static CoarsePathPlanningJob CreateFixedObstacleJob(string id, IReadOnlyList<IMapObstacle> obstacles)
    {
        var job = new CoarsePathPlanningJob
        {
            MapRequest = new PlanningMapRequest
            {
                Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
                ResolutionMm = 50f,
                AllowExplicitEmptyMap = false,
                ObstacleSources = new IMapObstacleSource[]
                {
                    new ManualObstacleSource("fixture-" + id, 1L, true, obstacles),
                },
            },
            Start = new Pose2D(1d, 2d, 0d),
            Goal = new Pose2D(5d, 2d, 0d),
            Vehicle = new VehicleParameters
            {
                LengthMeters = 0.80d,
                WidthMeters = 0.60d,
                SafetyMarginMeters = 0.05d,
                MaximumCurvaturePerMeter = 1d / 1.20d,
            },
            Configuration = new HybridAStarConfiguration(),
            GoalDirection = GoalDirectionConstraint.Forward,
        };
        job.Configuration.SearchTimeout = TimeSpan.FromSeconds(30d);
        return job;
    }

    private static SmoothingFixtureRecord CreateRecord(string id, CoarsePathPlanningJob job, CoarsePathPlanningJobResult result)
    {
        if (job == null || result == null || result.MapResult == null || !result.MapResult.Succeeded ||
            result.MapResult.Map == null || result.PlanningResult == null || result.PlanningResult.Status != PlanningStatus.Success)
        {
            string status = result == null || result.PlanningResult == null ? "no-result" : result.PlanningResult.Status.ToString();
            string reason = result == null || result.PlanningResult == null || result.PlanningResult.Diagnostics == null
                ? string.Empty
                : result.PlanningResult.Diagnostics.TerminationReason;
            throw new InvalidOperationException("夹具场景未产生成功的粗路径：" + id + "；状态=" + status + "；原因=" + reason);
        }

        PlanningGridMap map = result.MapResult.Map;
        IReadOnlyList<CoarsePathPoint> smoothingPath = SmoothingScenarioFactory.CopyWithFiniteClearance(result.PlanningResult.Path, map);
        var record = new SmoothingFixtureRecord
        {
            Id = id,
            FixtureVersion = 1,
            Map = CreateMapRecord(map, result.MapResult.SourceResults),
            Vehicle = CreateVehicleRecord(job.Vehicle),
            PlanningConfiguration = CreatePlanningConfigurationRecord(job.Configuration),
        };

        for (int index = 0; index < smoothingPath.Count; index++)
            record.Path.Add(CreatePathPointRecord(smoothingPath[index]));
        for (int index = 0; index < result.PlanningResult.Segments.Count; index++)
            record.Segments.Add(CreateSegmentRecord(result.PlanningResult.Segments[index]));
        return record;
    }

    private static SmoothingFixtureMap CreateMapRecord(PlanningGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults)
    {
        var record = new SmoothingFixtureMap
        {
            XMinMm = map.Bounds.XMin,
            XMaxMm = map.Bounds.XMax,
            YMinMm = map.Bounds.YMin,
            YMaxMm = map.Bounds.YMax,
            ResolutionMm = map.ResolutionMm,
        };
        for (int sourceIndex = 0; sourceIndex < sourceResults.Count; sourceIndex++)
        {
            IReadOnlyList<IMapObstacle> obstacles = sourceResults[sourceIndex].Obstacles;
            for (int obstacleIndex = 0; obstacleIndex < obstacles.Count; obstacleIndex++)
                record.Obstacles.Add(CreateObstacleRecord(obstacles[obstacleIndex]));
        }
        return record;
    }

    private static SmoothingFixtureObstacle CreateObstacleRecord(IMapObstacle obstacle)
    {
        if (obstacle is CircleObstacle circle)
        {
            return new SmoothingFixtureObstacle
            {
                Kind = "circle",
                CenterXMm = circle.CenterX,
                CenterYMm = circle.CenterY,
                RadiusMm = circle.RadiusMm,
            };
        }
        if (obstacle is AxisAlignedRectangleObstacle rectangle)
        {
            return new SmoothingFixtureObstacle
            {
                Kind = "axis-aligned-rectangle",
                XMinMm = rectangle.XMin,
                XMaxMm = rectangle.XMax,
                YMinMm = rectangle.YMin,
                YMaxMm = rectangle.YMax,
            };
        }
        throw new InvalidOperationException("夹具仅支持圆形和轴对齐矩形障碍物。" + obstacle?.GetType().FullName);
    }

    private static SmoothingFixtureVehicle CreateVehicleRecord(VehicleParameters vehicle)
    {
        if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
            throw new InvalidOperationException("夹具车辆缺少有效最大曲率限制。");
        return new SmoothingFixtureVehicle
        {
            LengthMeters = vehicle.LengthMeters,
            WidthMeters = vehicle.WidthMeters,
            SafetyMarginMeters = vehicle.SafetyMarginMeters,
            MaximumCurvaturePerMeter = maximumCurvaturePerMeter,
        };
    }

    private static SmoothingFixturePlanningConfiguration CreatePlanningConfigurationRecord(HybridAStarConfiguration configuration)
    {
        return new SmoothingFixturePlanningConfiguration
        {
            PrimitiveLengthMeters = configuration.PrimitiveLengthMeters,
            IntegrationStepMeters = configuration.IntegrationStepMeters,
            MaximumCollisionCheckStepMeters = configuration.MaximumCollisionCheckStepMeters,
            HeadingResolutionRadians = configuration.HeadingResolutionRadians,
            CurvatureLevelCount = configuration.CurvatureLevelCount,
            GoalPositionToleranceMeters = configuration.GoalPositionToleranceMeters,
            GoalHeadingToleranceRadians = configuration.GoalHeadingToleranceRadians,
            MaximumExpandedNodes = configuration.MaximumExpandedNodes,
            SearchTimeoutSeconds = configuration.SearchTimeout.TotalSeconds,
            HeuristicWeight = configuration.HeuristicWeight,
            ReverseCostMultiplier = configuration.ReverseCostMultiplier,
            GearSwitchPenaltyMeters = configuration.GearSwitchPenaltyMeters,
            CurvatureMagnitudeWeight = configuration.CurvatureMagnitudeWeight,
            CurvatureChangePenaltyMetersPerLevel = configuration.CurvatureChangePenaltyMetersPerLevel,
            ClearanceCostWeight = configuration.ClearanceCostWeight,
            ClearanceCostDistanceMeters = configuration.ClearanceCostDistanceMeters,
            AllowReverse = configuration.AllowReverse,
        };
    }

    private static SmoothingFixturePathPoint CreatePathPointRecord(CoarsePathPoint point)
    {
        return new SmoothingFixturePathPoint
        {
            XMeters = point.X,
            YMeters = point.Y,
            HeadingRadians = point.Heading,
            UnwrappedHeadingRadians = point.UnwrappedHeading,
            ArcLengthMeters = point.ArcLength,
            Direction = point.Direction,
            VehicleCurvaturePerMeter = point.VehicleCurvature,
            BodyClearanceMeters = point.BodyClearance,
            IsGearSwitchPoint = point.IsGearSwitchPoint,
            Source = point.Source,
        };
    }

    private static SmoothingFixtureSegment CreateSegmentRecord(PathSegment segment)
    {
        return new SmoothingFixtureSegment
        {
            SegmentIndex = segment.SegmentIndex,
            Direction = segment.Direction,
            StartIndex = segment.StartIndex,
            EndIndex = segment.EndIndex,
            StartsAtGearSwitch = segment.StartsAtGearSwitch,
            EndsAtGearSwitch = segment.EndsAtGearSwitch,
        };
    }

    private sealed class FixturePlan
    {
        public FixturePlan(string id, Func<CoarsePathPlanningJob> createJob)
        {
            Id = id;
            CreateJob = createJob;
        }

        public string Id { get; }
        public Func<CoarsePathPlanningJob> CreateJob { get; }
    }
}