using System;
using System.Collections.Generic;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;

/// <summary>将快速夹具或现有粗路径业务结果转换为独立平滑比较请求。</summary>
public static class SmoothingScenarioFactory
{
    /// <summary>不运行 Hybrid A*，从已验证的 JSON 夹具创建八个快速比较请求。</summary>
    public static IReadOnlyList<PathSmoothingComparisonRequest> CreateFixtureRequests(string fixturePath)
    {
        IReadOnlyList<SmoothingScenarioFixture> fixtures = SmoothingScenarioFixtureLoader.LoadAndVerify(fixturePath);
        var requests = new List<PathSmoothingComparisonRequest>(fixtures.Count);
        for (int index = 0; index < fixtures.Count; index++)
        {
            SmoothingScenarioFixture fixture = fixtures[index];
            requests.Add(new PathSmoothingComparisonRequest(
                new PathSmoothingRequest(
                    fixture.Path,
                    fixture.Segments,
                    SmoothingScenarioFixtureLoader.BuildMap(fixture),
                    SmoothingScenarioFixtureLoader.BuildVehicle(fixture),
                    new PathSmoothingConfiguration())));
        }
        return requests.AsReadOnly();
    }

    /// <summary>把现有 <see cref="CoarsePathScenarioFactory"/> 已规划成功的结果转换为比较请求。</summary>
    public static PathSmoothingComparisonRequest CreateEndToEndRequest(
        CoarsePathPlanningJob job,
        CoarsePathPlanningJobResult result)
    {
        if (job == null || result == null || result.MapResult == null || result.MapResult.Map == null ||
            result.PlanningResult == null || result.PlanningResult.Status != PlanningStatus.Success)
            throw new ArgumentException("只能从包含成功地图和粗路径的业务结果创建平滑比较请求。", nameof(result));

        return new PathSmoothingComparisonRequest(new PathSmoothingRequest(
            CopyWithFiniteClearance(result.PlanningResult.Path, result.MapResult.Map),
            result.PlanningResult.Segments,
            result.MapResult.Map,
            job.Vehicle,
            new PathSmoothingConfiguration()));
    }

    internal static IReadOnlyList<CoarsePathPoint> CopyWithFiniteClearance(
        IReadOnlyList<CoarsePathPoint> source,
        MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap map)
    {
        double widthMeters = (map.Bounds.XMax - map.Bounds.XMin) / 1000d;
        double heightMeters = (map.Bounds.YMax - map.Bounds.YMin) / 1000d;
        double finiteEmptyMapClearance = Math.Sqrt(widthMeters * widthMeters + heightMeters * heightMeters);
        var copy = new List<CoarsePathPoint>(source.Count);
        for (int index = 0; index < source.Count; index++)
        {
            CoarsePathPoint point = source[index];
            double clearance = double.IsPositiveInfinity(point.BodyClearance)
                ? finiteEmptyMapClearance
                : point.BodyClearance;
            copy.Add(new CoarsePathPoint(
                point.X, point.Y, point.Heading, point.UnwrappedHeading, point.ArcLength, point.Direction,
                point.VehicleCurvature, clearance, point.IsGearSwitchPoint, point.Source));
        }
        return copy.AsReadOnly();
    }
}