namespace Flyshot.ControllerClientCompat;

/// <summary>
/// 对飞拍稠密关节轨迹的首尾采样点做速度整形，降低启动和结束时的单步角度变化。
/// </summary>
internal static class FlyshotTrajectoryEdgeShaper
{
    /// <summary>
    /// 首尾整形默认覆盖的采样点数（含锚点）。
    /// </summary>
    internal const int DefaultEdgePointCount = 10;

    /// <summary>
    /// 对稠密关节轨迹做首尾整形，时间列保持不变，首段采用 ease-in，尾段采用 ease-out。
    /// </summary>
    /// <param name="denseJointTrajectory">输入稠密关节轨迹，每行格式为 [time, j1..jN]，关节单位为弧度。</param>
    /// <param name="maxEdgeStepDegrees">保留旧签名兼容调用方；当前实现不再按角度阈值扩窗。</param>
    /// <param name="maxWindowPoints">单侧整形覆盖的采样点数（含锚点），默认首尾各 10 点。</param>
    /// <returns>经过首尾整形后的新轨迹；若不满足整形条件则返回原轨迹副本。</returns>
    internal static IReadOnlyList<IReadOnlyList<double>> ShapeDenseJointTrajectory(
        IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
        double maxEdgeStepDegrees = 0.0,
        int maxWindowPoints = DefaultEdgePointCount)
    {
        ArgumentNullException.ThrowIfNull(denseJointTrajectory);
        if (denseJointTrajectory.Count == 0)
        {
            return Array.Empty<IReadOnlyList<double>>();
        }

        var copiedRows = denseJointTrajectory
            .Select(static row => row.ToArray())
            .ToArray();
        if (copiedRows.Length < 5 || maxWindowPoints < 2)
        {
            return copiedRows;
        }

        var lastIndex = copiedRows.Length - 1;
        var window = Math.Min(maxWindowPoints, lastIndex / 2);
        if (window < 2)
        {
            return copiedRows;
        }

        // 以原始轨迹为参考估计窗口边界的速度，并在位移累计量上做单段单调整形，
        // 目标是让首尾 10 点表现为更平滑的加减速，而不是硬匹配高阶导数导致振荡。
        var originalRows = copiedRows
            .Select(static row => row.ToArray())
            .ToArray();

        ApplyLeadingHermiteBlend(copiedRows, originalRows, window);
        ApplyTrailingHermiteBlend(copiedRows, originalRows, window);

        return copiedRows;
    }

    /// <summary>
    /// 对首段做单段 Hermite 累计位移整形：起点速度为 0，窗口末端按原轨迹边界速度接回中段。
    /// </summary>
    private static void ApplyLeadingHermiteBlend(double[][] rows, double[][] originalRows, int window)
    {
        var startRow = originalRows[0];
        var endRow = originalRows[window];
        var totalDuration = endRow[0] - startRow[0];
        if (totalDuration <= 0.0)
        {
            return;
        }

        for (var jointIndex = 1; jointIndex < startRow.Length; jointIndex++)
        {
            var delta = endRow[jointIndex] - startRow[jointIndex];
            if (Math.Abs(delta) <= 1e-12)
            {
                continue;
            }

            var endVelocity = EstimateVelocity(originalRows, window, jointIndex);
            var normalizedEndSlope = ClampNormalizedSlope((endVelocity * totalDuration) / delta);

            for (var index = 1; index < window; index++)
            {
                var normalizedTime = (rows[index][0] - startRow[0]) / totalDuration;
                var shapedValue = startRow[jointIndex]
                    + (delta * EvaluateHermiteProgress(normalizedTime, startSlope: 0.0, endSlope: normalizedEndSlope));
                var blendWeight = Math.Pow(1.0 - normalizedTime, 2.0);
                rows[index][jointIndex] = Lerp(originalRows[index][jointIndex], shapedValue, blendWeight);
            }
        }
    }

    /// <summary>
    /// 对尾段做单段 Hermite 累计位移整形：窗口起点按原轨迹边界速度接入，终点速度减到 0。
    /// </summary>
    private static void ApplyTrailingHermiteBlend(double[][] rows, double[][] originalRows, int window)
    {
        var startIndex = rows.Length - 1 - window;
        var startRow = originalRows[startIndex];
        var endRow = originalRows[^1];
        var totalDuration = endRow[0] - startRow[0];
        if (totalDuration <= 0.0)
        {
            return;
        }

        for (var jointIndex = 1; jointIndex < startRow.Length; jointIndex++)
        {
            var delta = endRow[jointIndex] - startRow[jointIndex];
            if (Math.Abs(delta) <= 1e-12)
            {
                continue;
            }

            var startVelocity = EstimateVelocity(originalRows, startIndex, jointIndex);
            var normalizedStartSlope = ClampNormalizedSlope((startVelocity * totalDuration) / delta);

            for (var index = 1; index < window; index++)
            {
                var normalizedTime = (rows[startIndex + index][0] - startRow[0]) / totalDuration;
                var shapedValue = startRow[jointIndex]
                    + (delta * EvaluateHermiteProgress(normalizedTime, startSlope: normalizedStartSlope, endSlope: 0.0));
                var blendWeight = Math.Pow(normalizedTime, 2.0);
                rows[startIndex + index][jointIndex] = Lerp(originalRows[startIndex + index][jointIndex], shapedValue, blendWeight);
            }
        }
    }

    /// <summary>
    /// 估算给定行在原始轨迹上的一阶导，首尾退化为单边差分。
    /// </summary>
    private static double EstimateVelocity(double[][] rows, int index, int jointIndex)
    {
        if (index <= 0)
        {
            var dt = rows[1][0] - rows[0][0];
            return dt <= 0.0 ? 0.0 : (rows[1][jointIndex] - rows[0][jointIndex]) / dt;
        }

        if (index >= rows.Length - 1)
        {
            var dt = rows[^1][0] - rows[^2][0];
            return dt <= 0.0 ? 0.0 : (rows[^1][jointIndex] - rows[^2][jointIndex]) / dt;
        }

        var previousDt = rows[index][0] - rows[index - 1][0];
        var nextDt = rows[index + 1][0] - rows[index][0];
        if (previousDt <= 0.0 || nextDt <= 0.0)
        {
            return 0.0;
        }

        var backward = (rows[index][jointIndex] - rows[index - 1][jointIndex]) / previousDt;
        var forward = (rows[index + 1][jointIndex] - rows[index][jointIndex]) / nextDt;
        return (backward + forward) / 2.0;
    }

    /// <summary>
    /// 估算给定行在原始轨迹上的二阶导，端点退化为 0 以避免放大边界噪声。
    /// </summary>
    private static double EstimateAcceleration(double[][] rows, int index, int jointIndex)
    {
        if (index <= 0 || index >= rows.Length - 1)
        {
            return 0.0;
        }

        var previousDt = rows[index][0] - rows[index - 1][0];
        var nextDt = rows[index + 1][0] - rows[index][0];
        if (previousDt <= 0.0 || nextDt <= 0.0)
        {
            return 0.0;
        }

        var backward = (rows[index][jointIndex] - rows[index - 1][jointIndex]) / previousDt;
        var forward = (rows[index + 1][jointIndex] - rows[index][jointIndex]) / nextDt;
        var averageDt = (previousDt + nextDt) / 2.0;
        return averageDt <= 0.0 ? 0.0 : (forward - backward) / averageDt;
    }

    /// <summary>
    /// 计算 Hermite 累计位移曲线在 0..1 归一化时间上的进度值。
    /// </summary>
    private static double EvaluateHermiteProgress(double normalizedTime, double startSlope, double endSlope)
    {
        var u = Math.Clamp(normalizedTime, 0.0, 1.0);
        var u2 = u * u;
        var u3 = u2 * u;
        var h00 = (2.0 * u3) - (3.0 * u2) + 1.0;
        var h10 = u3 - (2.0 * u2) + u;
        var h01 = (-2.0 * u3) + (3.0 * u2);
        var h11 = u3 - u2;
        return (h00 * 0.0) + (h10 * startSlope) + (h01 * 1.0) + (h11 * endSlope);
    }

    /// <summary>
    /// 把归一化边界斜率限制在单调 Hermite 常见的稳定区间内，避免过冲和窗口内振荡。
    /// </summary>
    private static double ClampNormalizedSlope(double normalizedSlope)
    {
        if (double.IsNaN(normalizedSlope) || double.IsInfinity(normalizedSlope))
        {
            return 0.0;
        }

        return Math.Clamp(normalizedSlope, 0.0, 3.0);
    }

    /// <summary>
    /// 在线性插值基础上做温和混合，避免首尾窗口为了追赶锚点而产生过大的局部跃度。
    /// </summary>
    private static double Lerp(double originalValue, double shapedValue, double weight)
    {
        var clampedWeight = Math.Clamp(weight, 0.0, 1.0);
        return originalValue + ((shapedValue - originalValue) * clampedWeight);
    }
}