using Flyshot.Core.Domain;
using Flyshot.Core.Planning;

namespace Flyshot.Core.Triggering;

/// <summary>
/// 把补中点后的规划轨迹映射回原始示教点的时间戳。
///
/// 为什么需要单独拆一层？
/// ---
/// self-adapt-icsp 会在原始示教点之间插入中点，导致 PlannedWaypoints 的数量和顺序
/// 与 OriginalProgram.Waypoints 不一致。但触发时序（ShotEvent）必须绑定到原始示教点，
/// 而不是中点。这一层负责在补中点后的轨迹中“找回”每个原始示教点对应的精确时刻。
/// </summary>
public sealed class WaypointTimestampResolver
{
    /// <summary>
    /// 判定两个路点是否为同一示教点的关节空间距离容差。
    /// </summary>
    public const double DefaultPositionTolerance = 1e-4;

    /// <summary>
    /// 为原始示教点列表找回在规划轨迹中的对应时间戳。
    /// </summary>
    /// <param name="trajectory">规划后的轨迹（可能已补中点）。</param>
    /// <returns>与原始示教点一一对应的时间戳数组（单位：秒）。</returns>
    /// <exception cref="InvalidOperationException">当某个原始示教点无法在当前路点列表中匹配时抛出。</exception>
    public double[] Resolve(PlannedTrajectory trajectory)
    {
        ArgumentNullException.ThrowIfNull(trajectory);

        var originalWaypoints = trajectory.OriginalProgram.Waypoints;
        var plannedWaypoints = trajectory.PlannedWaypoints;
        var waypointTimes = trajectory.WaypointTimes;

        var timestamps = new double[originalWaypoints.Count];
        int plannedIndex = 0;

        for (int originalIndex = 0; originalIndex < originalWaypoints.Count; originalIndex++)
        {
            bool matched = false;
            for (; plannedIndex < plannedWaypoints.Count; plannedIndex++)
            {
                if (WaypointDistance(originalWaypoints[originalIndex], plannedWaypoints[plannedIndex]) <= DefaultPositionTolerance)
                {
                    timestamps[originalIndex] = waypointTimes[plannedIndex];
                    matched = true;
                    plannedIndex++; // 下一次匹配从当前位置之后开始，保证顺序
                    break;
                }
            }

            if (!matched)
            {
                throw new InvalidOperationException(
                    $"无法在补中点后的轨迹中为原始示教点 {originalIndex} 找回对应时间戳。");
            }
        }

        return timestamps;
    }

    /// <summary>
    /// 计算两个路点在关节空间的欧氏距离。
    /// </summary>
    private static double WaypointDistance(JointWaypoint a, JointWaypoint b)
    {
        double sumSq = 0.0;
        int count = Math.Min(a.Positions.Count, b.Positions.Count);
        for (int i = 0; i < count; i++)
        {
            double diff = a.Positions[i] - b.Positions[i];
            sumSq += diff * diff;
        }

        return Math.Sqrt(sumSq);
    }
}
