using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.Mapping;

/// <summary>将检测局部坐标中的两条腿转换为世界 mm 坐标圆形障碍物。</summary>
public static class TwoLegObstacleProjector
{
    /// <summary>
    /// 投影一份 TwoLeg 快照。
    ///
    /// 参数：input 为检测时刻采集的纯数据快照，可为 null。
    /// 返回：null 为 Unavailable；未检测到目标为 Empty；数值无效为 Invalid；成功时为含两个圆形障碍物的 Applied。
    /// 注意：本方法只读取 input，不访问传感器、定位、UI 或时钟。
    /// </summary>
    public static ObstacleProjectionResult Project(TwoLegProjectionInput input)
    {
        if (input == null) return ObstacleProjectionResult.Unavailable("TwoLeg snapshot is unavailable.");
        if (!input.HasDetection) return ObstacleProjectionResult.Empty(input.Diagnostic);
        if (!input.IsValid) return ObstacleProjectionResult.Invalid("TwoLeg snapshot contains invalid values.");
        CoordinateTransform.LocalToWorld(input.DetectionWorldX, input.DetectionWorldY, input.DetectionHeadingRadians,
            input.FirstLocalX, input.FirstLocalY, out double firstX, out double firstY);
        CoordinateTransform.LocalToWorld(input.DetectionWorldX, input.DetectionWorldY, input.DetectionHeadingRadians,
            input.SecondLocalX, input.SecondLocalY, out double secondX, out double secondY);
        var obstacles = new List<IMapObstacle>
        {
            new CircleObstacle((float)firstX, (float)firstY, input.RadiusMm),
            new CircleObstacle((float)secondX, (float)secondY, input.RadiusMm),
        };
        return ObstacleProjectionResult.Applied(obstacles, input.Diagnostic);
    }
}