using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.Mapping;

namespace TrajectoryOutputDemo;

/// <summary>
/// Trajplanner_output 的唯一演示调参入口。
/// 地图边界与分辨率使用 mm；车辆、起终点位置和安全余量使用 m；航向使用 rad；输出路径为 CSV 文件位置。
/// </summary>
public sealed class TrajectoryOutputDemoConfiguration
{
    /// <summary>演示地图的世界边界；空地图只允许用于算法学习，不能替代真实障碍物来源。</summary>
    public MapBoundsMm MapBounds { get; init; } = new MapBoundsMm(0f, 6000f, 0f, 4000f);

    /// <summary>演示占据栅格分辨率，单位 mm。</summary>
    public float MapResolutionMillimeters { get; init; }

    /// <summary>车辆几何中心起点，位置单位 m、航向单位 rad。</summary>
    public Pose2D Start { get; init; } = new Pose2D(1d, 1d, 0d);

    /// <summary>车辆几何中心目标点，位置单位 m、航向单位 rad。</summary>
    public Pose2D Goal { get; init; } = new Pose2D(3d, 1d, 0d);

    /// <summary>车辆长度，单位 m。</summary>
    public double VehicleLengthMeters { get; init; }

    /// <summary>车辆宽度，单位 m。</summary>
    public double VehicleWidthMeters { get; init; }

    /// <summary>车辆矩形外的附加安全余量，单位 m。</summary>
    public double SafetyMarginMeters { get; init; }

    /// <summary>车辆最大曲率，单位 1/m。</summary>
    public double MaximumCurvaturePerMeter { get; init; }

    /// <summary>轨迹开始时的带符号纵向速度，单位 m/s；前进为正、倒车为负。</summary>
    public double InitialSignedSpeedMetersPerSecond { get; init; }

    /// <summary>输出 CSV 的绝对或相对路径。</summary>
    public string CsvOutputPath { get; init; } = string.Empty;

    /// <summary>创建可直线通行的最小真实 EM 演示配置。</summary>
    public static TrajectoryOutputDemoConfiguration CreateDefault(string csvOutputPath)
    {
        if (string.IsNullOrWhiteSpace(csvOutputPath))
            throw new ArgumentException("CSV 输出路径不能为空。", nameof(csvOutputPath));

        return new TrajectoryOutputDemoConfiguration
        {
            MapBounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
            MapResolutionMillimeters = 50f,
            Start = new Pose2D(1d, 1d, 0d),
            Goal = new Pose2D(3d, 1d, 0d),
            VehicleLengthMeters = 0.80d,
            VehicleWidthMeters = 0.60d,
            SafetyMarginMeters = 0.05d,
            MaximumCurvaturePerMeter = 1d / 1.20d,
            InitialSignedSpeedMetersPerSecond = 0d,
            CsvOutputPath = csvOutputPath,
        };
    }
}