using System;
using System.Collections.Generic;
using System.Globalization;
using System.IO;
using System.Security.Cryptography;
using System.Text;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
using Newtonsoft.Json;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;

/// <summary>加载并验证版本化 JSON 快速夹具；此类不运行 Hybrid A*。</summary>
public static class SmoothingScenarioFixtureLoader
{
    /// <summary>读取、校验并冻结指定的快速夹具文件。</summary>
    public static IReadOnlyList<SmoothingScenarioFixture> LoadAndVerify(string fixturePath)
    {
        if (string.IsNullOrWhiteSpace(fixturePath)) throw new ArgumentException("夹具路径不能为空。", nameof(fixturePath));
        if (!File.Exists(fixturePath)) throw new FileNotFoundException("找不到路径平滑夹具文件。", fixturePath);

        SmoothingFixtureDocument document = JsonConvert.DeserializeObject<SmoothingFixtureDocument>(
            File.ReadAllText(fixturePath, Encoding.UTF8));
        if (document == null || document.SchemaVersion != 1 || document.Scenarios == null)
            throw new InvalidDataException("路径平滑夹具架构版本无效。");

        var fixtures = new List<SmoothingScenarioFixture>(document.Scenarios.Count);
        var ids = new HashSet<string>(StringComparer.Ordinal);
        for (int index = 0; index < document.Scenarios.Count; index++)
        {
            SmoothingFixtureRecord record = document.Scenarios[index];
            Validate(record, ids);
            string fingerprint = ComputeFingerprint(record);
            if (!string.Equals(record.ConfigurationFingerprint, fingerprint, StringComparison.Ordinal))
                throw new InvalidDataException("路径平滑夹具指纹已过期：" + record.Id + "。");
            fixtures.Add(new SmoothingScenarioFixture(record, true));
        }
        return fixtures.AsReadOnly();
    }

    internal static string ComputeFingerprint(SmoothingFixtureRecord record)
    {
        string material = BuildFingerprintMaterial(record);
        using (SHA256 sha256 = SHA256.Create())
        {
            byte[] hash = sha256.ComputeHash(Encoding.UTF8.GetBytes(material));
            var builder = new StringBuilder(hash.Length * 2 + 7);
            builder.Append("sha256:");
            for (int index = 0; index < hash.Length; index++) builder.Append(hash[index].ToString("x2", CultureInfo.InvariantCulture));
            return builder.ToString();
        }
    }

    internal static PlanningGridMap BuildMap(SmoothingScenarioFixture fixture)
    {
        SmoothingFixtureRecord record = fixture.Record;
        var obstacles = new List<IMapObstacle>(record.Map.Obstacles.Count);
        for (int index = 0; index < record.Map.Obstacles.Count; index++)
        {
            SmoothingFixtureObstacle obstacle = record.Map.Obstacles[index];
            if (string.Equals(obstacle.Kind, "circle", StringComparison.Ordinal))
                obstacles.Add(new CircleObstacle(obstacle.CenterXMm, obstacle.CenterYMm, obstacle.RadiusMm));
            else if (string.Equals(obstacle.Kind, "axis-aligned-rectangle", StringComparison.Ordinal))
                obstacles.Add(new AxisAlignedRectangleObstacle(obstacle.XMinMm, obstacle.XMaxMm, obstacle.YMinMm, obstacle.YMaxMm));
            else
                throw new InvalidDataException("夹具包含未知障碍物类型。");
        }

        var mapRequest = new PlanningMapRequest
        {
            Bounds = new MapBoundsMm(record.Map.XMinMm, record.Map.XMaxMm, record.Map.YMinMm, record.Map.YMaxMm),
            ResolutionMm = record.Map.ResolutionMm,
            AllowExplicitEmptyMap = obstacles.Count == 0,
            ObstacleSources = obstacles.Count == 0
                ? Array.Empty<IMapObstacleSource>()
                : new IMapObstacleSource[] { new ManualObstacleSource("fixture-" + record.Id, record.FixtureVersion, true, obstacles) },
        };
        PlanningMapBuildResult build = new PlanningMapFactory().Create(mapRequest);
        if (!build.Succeeded || build.Map == null) throw new InvalidDataException("夹具地图无法重建：" + record.Id + "。");
        return build.Map;
    }

    internal static VehicleParameters BuildVehicle(SmoothingScenarioFixture fixture)
    {
        SmoothingFixtureVehicle vehicle = fixture.Record.Vehicle;
        return new VehicleParameters
        {
            LengthMeters = vehicle.LengthMeters,
            WidthMeters = vehicle.WidthMeters,
            SafetyMarginMeters = vehicle.SafetyMarginMeters,
            MaximumCurvaturePerMeter = vehicle.MaximumCurvaturePerMeter,
        };
    }

    private static void Validate(SmoothingFixtureRecord record, ISet<string> ids)
    {
        if (record == null || string.IsNullOrWhiteSpace(record.Id) || record.FixtureVersion <= 0 ||
            record.Map == null || record.Vehicle == null || record.PlanningConfiguration == null || record.Path == null || record.Path.Count < 2 ||
            record.Segments == null || record.Segments.Count == 0 || !ids.Add(record.Id))
            throw new InvalidDataException("路径平滑夹具缺少必需字段、版本或唯一 ID。");

        ValidateMap(record);
        ValidateVehicle(record);
        ValidatePlanningConfiguration(record);
        ValidatePathContract(record);
    }

    private static void ValidateMap(SmoothingFixtureRecord record)
    {
        SmoothingFixtureMap map = record.Map;
        if (!IsFinite(map.XMinMm) || !IsFinite(map.XMaxMm) || !IsFinite(map.YMinMm) || !IsFinite(map.YMaxMm) ||
            !IsFinite(map.ResolutionMm) || map.XMaxMm <= map.XMinMm || map.YMaxMm <= map.YMinMm || map.ResolutionMm <= 0f ||
            map.Obstacles == null)
        {
            throw new InvalidDataException("Fixture map contract is invalid: " + record.Id + ".");
        }

        for (int index = 0; index < map.Obstacles.Count; index++)
        {
            SmoothingFixtureObstacle obstacle = map.Obstacles[index];
            bool circle = obstacle != null && string.Equals(obstacle.Kind, "circle", StringComparison.Ordinal) &&
                IsFinite(obstacle.CenterXMm) && IsFinite(obstacle.CenterYMm) && IsFinite(obstacle.RadiusMm) && obstacle.RadiusMm >= 0f;
            bool rectangle = obstacle != null && string.Equals(obstacle.Kind, "axis-aligned-rectangle", StringComparison.Ordinal) &&
                IsFinite(obstacle.XMinMm) && IsFinite(obstacle.XMaxMm) && IsFinite(obstacle.YMinMm) && IsFinite(obstacle.YMaxMm) &&
                obstacle.XMaxMm >= obstacle.XMinMm && obstacle.YMaxMm >= obstacle.YMinMm;
            if (!circle && !rectangle)
                throw new InvalidDataException("Fixture map contract is invalid: " + record.Id + ".");
        }
    }

    private static void ValidateVehicle(SmoothingFixtureRecord record)
    {
        SmoothingFixtureVehicle vehicle = record.Vehicle;
        if (!IsFinite(vehicle.LengthMeters) || !IsFinite(vehicle.WidthMeters) || !IsFinite(vehicle.SafetyMarginMeters) ||
            !IsFinite(vehicle.MaximumCurvaturePerMeter) || vehicle.LengthMeters <= 0d || vehicle.WidthMeters <= 0d ||
            vehicle.SafetyMarginMeters < 0d || vehicle.MaximumCurvaturePerMeter <= 0d)
        {
            throw new InvalidDataException("Fixture vehicle contract is invalid: " + record.Id + ".");
        }
    }

    private static void ValidatePlanningConfiguration(SmoothingFixtureRecord record)
    {
        SmoothingFixturePlanningConfiguration configuration = record.PlanningConfiguration;
        if (!IsPositiveFinite(configuration.PrimitiveLengthMeters) || !IsPositiveFinite(configuration.IntegrationStepMeters) ||
            !IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) || !IsPositiveFinite(configuration.HeadingResolutionRadians) ||
            configuration.CurvatureLevelCount < 2 || !IsPositiveFinite(configuration.GoalPositionToleranceMeters) ||
            !IsPositiveFinite(configuration.GoalHeadingToleranceRadians) || configuration.MaximumExpandedNodes <= 0 ||
            !IsPositiveFinite(configuration.SearchTimeoutSeconds) || !IsFinite(configuration.HeuristicWeight) ||
            !IsFinite(configuration.ReverseCostMultiplier) || !IsFinite(configuration.GearSwitchPenaltyMeters) ||
            !IsFinite(configuration.CurvatureMagnitudeWeight) || !IsFinite(configuration.CurvatureChangePenaltyMetersPerLevel) ||
            !IsFinite(configuration.ClearanceCostWeight) || !IsPositiveFinite(configuration.ClearanceCostDistanceMeters))
        {
            throw new InvalidDataException("Fixture planning configuration contract is invalid: " + record.Id + ".");
        }
    }

    private static void ValidatePathContract(SmoothingFixtureRecord record)
    {
        var fixture = new SmoothingScenarioFixture(record, true);
        try
        {
            var request = new PathSmoothingRequest(
                fixture.Path,
                fixture.Segments,
                BuildMap(fixture),
                BuildVehicle(fixture),
                new PathSmoothingConfiguration());
            if (!new PathSmoothingPreprocessor().TryPrepare(request, out _, out string reason))
                throw new InvalidDataException("Fixture path contract is invalid: " + record.Id + "; " + reason);
        }
        catch (InvalidDataException)
        {
            throw;
        }
        catch (Exception exception)
        {
            throw new InvalidDataException("Fixture path contract is invalid: " + record.Id + "; " + exception.Message, exception);
        }
    }

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

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

    private static bool IsPositiveFinite(double value)
    {
        return IsFinite(value) && value > 0d;
    }

    private static string BuildFingerprintMaterial(SmoothingFixtureRecord record)
    {
        var builder = new StringBuilder();
        Append(builder, record.Id); Append(builder, record.FixtureVersion);
        Append(builder, record.Map.XMinMm); Append(builder, record.Map.XMaxMm); Append(builder, record.Map.YMinMm);
        Append(builder, record.Map.YMaxMm); Append(builder, record.Map.ResolutionMm);
        for (int index = 0; index < record.Map.Obstacles.Count; index++)
        {
            SmoothingFixtureObstacle obstacle = record.Map.Obstacles[index];
            Append(builder, obstacle.Kind); Append(builder, obstacle.XMinMm); Append(builder, obstacle.XMaxMm);
            Append(builder, obstacle.YMinMm); Append(builder, obstacle.YMaxMm); Append(builder, obstacle.CenterXMm);
            Append(builder, obstacle.CenterYMm); Append(builder, obstacle.RadiusMm);
        }
        Append(builder, record.Vehicle.LengthMeters); Append(builder, record.Vehicle.WidthMeters);
        Append(builder, record.Vehicle.SafetyMarginMeters); Append(builder, record.Vehicle.MaximumCurvaturePerMeter);
        SmoothingFixturePlanningConfiguration configuration = record.PlanningConfiguration;
        Append(builder, configuration.PrimitiveLengthMeters); Append(builder, configuration.IntegrationStepMeters);
        Append(builder, configuration.MaximumCollisionCheckStepMeters); Append(builder, configuration.HeadingResolutionRadians);
        Append(builder, configuration.CurvatureLevelCount); Append(builder, configuration.GoalPositionToleranceMeters);
        Append(builder, configuration.GoalHeadingToleranceRadians); Append(builder, configuration.MaximumExpandedNodes);
        Append(builder, configuration.SearchTimeoutSeconds); Append(builder, configuration.HeuristicWeight);
        Append(builder, configuration.ReverseCostMultiplier); Append(builder, configuration.GearSwitchPenaltyMeters);
        Append(builder, configuration.CurvatureMagnitudeWeight); Append(builder, configuration.CurvatureChangePenaltyMetersPerLevel);
        Append(builder, configuration.ClearanceCostWeight); Append(builder, configuration.ClearanceCostDistanceMeters);
        Append(builder, configuration.AllowReverse ? 1 : 0);
        for (int index = 0; index < record.Path.Count; index++)
        {
            SmoothingFixturePathPoint point = record.Path[index];
            Append(builder, point.XMeters); Append(builder, point.YMeters); Append(builder, point.HeadingRadians);
            Append(builder, point.UnwrappedHeadingRadians); Append(builder, point.ArcLengthMeters); Append(builder, (int)point.Direction);
            Append(builder, point.VehicleCurvaturePerMeter); Append(builder, point.BodyClearanceMeters);
            Append(builder, point.IsGearSwitchPoint ? 1 : 0); Append(builder, (int)point.Source);
        }
        for (int index = 0; index < record.Segments.Count; index++)
        {
            SmoothingFixtureSegment segment = record.Segments[index];
            Append(builder, segment.SegmentIndex); Append(builder, (int)segment.Direction); Append(builder, segment.StartIndex);
            Append(builder, segment.EndIndex); Append(builder, segment.StartsAtGearSwitch ? 1 : 0); Append(builder, segment.EndsAtGearSwitch ? 1 : 0);
        }
        return builder.ToString();
    }

    private static void Append(StringBuilder builder, string value) { builder.Append(value ?? string.Empty).Append('|'); }
    private static void Append(StringBuilder builder, int value) { builder.Append(value.ToString(CultureInfo.InvariantCulture)).Append('|'); }
    private static void Append(StringBuilder builder, float value) { builder.Append(value.ToString("R", CultureInfo.InvariantCulture)).Append('|'); }
    private static void Append(StringBuilder builder, double value) { builder.Append(value.ToString("R", CultureInfo.InvariantCulture)).Append('|'); }
}