using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.Mapping;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;

/// <summary>
/// 一次路径平滑所需的不可变输入快照。
/// 粗路径、方向段、车辆和配置均在构造时复制；地图保留为调用方已冻结的 <see cref="PlanningGridMap"/> 快照。
/// </summary>
public sealed class PathSmoothingRequest
{
    private readonly VehicleParameters _vehicle;
    private readonly PathSmoothingConfiguration _configuration;

    /// <summary>
    /// 创建路径平滑请求，并防御性复制可变的路径、分段、车辆和配置输入。
    /// </summary>
    /// <param name="coarsePath">按起点到终点顺序排列的粗路径点集合；位置与弧长单位为 m，航向单位为 rad。</param>
    /// <param name="segments">与 <paramref name="coarsePath"/> 对应的前进/倒车包含式方向分段集合。</param>
    /// <param name="map">用于完整车体碰撞与净空复核的已准备不可变规划地图快照。</param>
    /// <param name="vehicle">车辆长宽、安全余量和曲率约束；长度单位为 m，曲率单位为 1/m。</param>
    /// <param name="configuration">本次平滑的采样、净空、曲率和 Local G2 配置；构造后不会与调用方共享可变实例。</param>
    public PathSmoothingRequest(
        IReadOnlyList<CoarsePathPoint> coarsePath,
        IReadOnlyList<PathSegment> segments,
        PlanningGridMap map,
        VehicleParameters vehicle,
        PathSmoothingConfiguration configuration)
    {
        CoarsePath = CopyReadOnly(coarsePath);
        Segments = CopyReadOnly(segments);
        Map = map;
        _vehicle = CopyVehicle(vehicle);
        _configuration = CopyConfiguration(configuration);
    }

    /// <summary>原始粗路径的只读快照，按起点到终点顺序排列；位置与弧长单位为 m，航向单位为 rad。</summary>
    public IReadOnlyList<CoarsePathPoint> CoarsePath { get; }

    /// <summary>原始粗路径方向分段的只读快照；每段标识连续前进或倒车区间。</summary>
    public IReadOnlyList<PathSegment> Segments { get; }

    /// <summary>用于平滑后完整车体碰撞和净空复核的已冻结规划栅格地图快照。</summary>
    public PlanningGridMap Map { get; }

    /// <summary>车辆几何与最大曲率约束的不可变快照副本。</summary>
    public VehicleParameters Vehicle => CopyVehicle(_vehicle);

    /// <summary>本次平滑配置的不可变快照副本。</summary>
    public PathSmoothingConfiguration Configuration => CopyConfiguration(_configuration);

    private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
    {
        var copy = new List<T>(source == null ? 0 : source.Count);
        if (source != null)
        {
            for (int index = 0; index < source.Count; index++)
                copy.Add(source[index]);
        }
        return new ReadOnlyCollection<T>(copy);
    }

    private static VehicleParameters CopyVehicle(VehicleParameters source)
    {
        if (source == null) return null;
        return new VehicleParameters
        {
            LengthMeters = source.LengthMeters,
            WidthMeters = source.WidthMeters,
            SafetyMarginMeters = source.SafetyMarginMeters,
            MaximumCurvaturePerMeter = source.MaximumCurvaturePerMeter,
            MinimumTurningRadiusMeters = source.MinimumTurningRadiusMeters,
        };
    }

    private static PathSmoothingConfiguration CopyConfiguration(PathSmoothingConfiguration source)
    {
        if (source == null) return null;
        var copy = new PathSmoothingConfiguration
        {
            OutputSpacingMeters = source.OutputSpacingMeters,
            MaximumCollisionCheckStepMeters = source.MaximumCollisionCheckStepMeters,
            MinimumClearanceReserveMeters = source.MinimumClearanceReserveMeters,
            CurvatureLimitRadiusToleranceMeters = source.CurvatureLimitRadiusToleranceMeters,
        };
        copy.LocalG2Quintic.MinimumWindowLengthMeters = source.LocalG2Quintic.MinimumWindowLengthMeters;
        copy.LocalG2Quintic.PreferredWindowLengthMeters = source.LocalG2Quintic.PreferredWindowLengthMeters;
        copy.LocalG2Quintic.MaximumWindowLengthMeters = source.LocalG2Quintic.MaximumWindowLengthMeters;
        copy.LocalG2Quintic.MaximumDeviationMeters = source.LocalG2Quintic.MaximumDeviationMeters;
        copy.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter = source.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter;
        copy.LocalG2Quintic.CurvatureJumpRatioOfMaximum = source.LocalG2Quintic.CurvatureJumpRatioOfMaximum;
        copy.LocalG2Quintic.MinimumPeakGradientImprovementRatio = source.LocalG2Quintic.MinimumPeakGradientImprovementRatio;
        copy.LocalG2Quintic.MaximumVariationCostRegressionRatio = source.LocalG2Quintic.MaximumVariationCostRegressionRatio;
        copy.LocalG2Quintic.MaximumCandidatesPerRegion = source.LocalG2Quintic.MaximumCandidatesPerRegion;
        return copy;
    }
}