namespace Flyshot.Core.Planning;

/// <summary>
///  clamps 到零速度边界条件的三次样条插值器，专用于 ICSP 规划核心。
///
/// 为什么自己实现而不是用现成库？
/// ---
/// 逆向分析已经锁定原系统使用 scipy.interpolate.CubicSpline 的 clamped-zero 边界，
/// 且需要精确控制每段系数以便解析求导峰值。第三方库的边界条件语义和系数存储格式
/// 可能与 scipy 不完全对齐，自己实现可以把数学公式与 Python 参考代码逐行对照，
/// 确保后续 golden-sample 对拍时误差来源只集中在算法本身，而不是插值器差异。
/// </summary>
public sealed class CubicSplineInterpolator
{
    private readonly double[] _times;
    private readonly double[][] _values;
    private readonly int _n;      // 节点数
    private readonly int _nseg;   // 段数 = n - 1
    private readonly int _dof;    // 自由度（关节数）

    // 每段每个关节的系数：S_i(t) = a*t^3 + b*t^2 + c*t + d，t 相对于段起点归一化到 [0, h]
    // 数组维度 [_nseg][_dof]
    private readonly double[][] _a;
    private readonly double[][] _b;
    private readonly double[][] _c;
    private readonly double[][] _d;
    private readonly double[] _h; // 每段时长

    /// <summary>
    /// 用 clamped-zero 边界构造三次样条。
    /// </summary>
    /// <param name="times">严格递增的时间节点，长度至少为 2。</param>
    /// <param name="values">与 times 一一对应的路点值，每个路点是一个 double[dof]。</param>
    /// <exception cref="ArgumentException">输入维度不一致或时间节点非递增时抛出。</exception>
    public CubicSplineInterpolator(double[] times, double[][] values)
    {
        if (times.Length < 2)
        {
            throw new ArgumentException("时间节点至少需要 2 个。", nameof(times));
        }

        if (values.Length != times.Length)
        {
            throw new ArgumentException("路点数量必须与时间节点数量一致。", nameof(values));
        }

        _n = times.Length;
        _nseg = _n - 1;
        _dof = values[0].Length;
        _times = new double[_n];
        Array.Copy(times, _times, _n);
        _values = new double[_n][];
        for (int i = 0; i < _n; i++)
        {
            if (values[i].Length != _dof)
            {
                throw new ArgumentException($"第 {i} 个路点的维度与第一个路点不一致。");
            }

            _values[i] = new double[_dof];
            Array.Copy(values[i], _values[i], _dof);
        }

        _h = new double[_nseg];
        for (int i = 0; i < _nseg; i++)
        {
            _h[i] = _times[i + 1] - _times[i];
            if (_h[i] <= 0.0)
            {
                throw new ArgumentException($"时间节点必须严格递增，但在索引 {i} 处发现 h={_h[i]}。");
            }
        }

        _a = new double[_nseg][];
        _b = new double[_nseg][];
        _c = new double[_nseg][];
        _d = new double[_nseg][];
        for (int seg = 0; seg < _nseg; seg++)
        {
            _a[seg] = new double[_dof];
            _b[seg] = new double[_dof];
            _c[seg] = new double[_dof];
            _d[seg] = new double[_dof];
        }

        BuildCoefficients();
    }

    /// <summary>
    /// 获取路点数量。
    /// </summary>
    public int WaypointCount => _n;

    /// <summary>
    /// 获取关节自由度。
    /// </summary>
    public int DegreesOfFreedom => _dof;

    /// <summary>
    /// 获取时间节点数组的副本。
    /// </summary>
    public double[] Times
    {
        get
        {
            var copy = new double[_n];
            Array.Copy(_times, copy, _n);
            return copy;
        }
    }

    /// <summary>
    /// 在指定时刻求样条值（位置）。
    /// </summary>
    public double[] Evaluate(double t)
    {
        int seg = FindSegment(t);
        double local = t - _times[seg];
        double h = _h[seg];
        var result = new double[_dof];
        for (int d = 0; d < _dof; d++)
        {
            result[d] = _a[seg][d] * local * local * local
                      + _b[seg][d] * local * local
                      + _c[seg][d] * local
                      + _d[seg][d];
        }

        return result;
    }

    /// <summary>
    /// 在指定时刻求指定阶导数（order=1 速度，2 加速度，3 jerk）。
    /// </summary>
    public double[] EvaluateDerivative(double t, int order)
    {
        if (order < 1 || order > 3)
        {
            throw new ArgumentOutOfRangeException(nameof(order), "仅支持 1~3 阶导数。");
        }

        int seg = FindSegment(t);
        double local = t - _times[seg];
        var result = new double[_dof];
        for (int d = 0; d < _dof; d++)
        {
            result[d] = order switch
            {
                1 => 3.0 * _a[seg][d] * local * local + 2.0 * _b[seg][d] * local + _c[seg][d],
                2 => 6.0 * _a[seg][d] * local + 2.0 * _b[seg][d],
                3 => 6.0 * _a[seg][d],
                _ => 0.0
            };
        }

        return result;
    }

    /// <summary>
    /// 解析计算每段每个关节的 1/2/3 阶导绝对值峰值。
    ///
    /// 为什么用解析法而不是逐点采样？
    /// ---
    /// 三次样条的一阶导是二次函数、二阶导是线性函数、三阶导是常数，它们的极值点
    /// 可以通过解析公式精确计算。逐点采样会引入"漏检"风险，解析法能保证找到真正的最大值。
    /// </summary>
    /// <returns>
    /// 三元组：(max_dq[_nseg, _dof], max_ddq[_nseg, _dof], max_dddq[_nseg, _dof])。
    /// </returns>
    public (double[,] maxDq, double[,] maxDdq, double[,] maxDddq) SegmentMaxAbsDerivatives()
    {
        var maxDq = new double[_nseg, _dof];
        var maxDdq = new double[_nseg, _dof];
        var maxDddq = new double[_nseg, _dof];

        for (int seg = 0; seg < _nseg; seg++)
        {
            double h = _h[seg];
            for (int d = 0; d < _dof; d++)
            {
                double aa = _a[seg][d];
                double bb = _b[seg][d];
                double cc = _c[seg][d];

                // 一阶导是二次函数 3a*t^2 + 2b*t + c，峰值在端点或顶点。
                var candidates = new List<double>(3)
                {
                    Math.Abs(cc),
                    Math.Abs(3.0 * aa * h * h + 2.0 * bb * h + cc),
                };

                if (Math.Abs(aa) > 1e-12)
                {
                    // 二次函数顶点：导数的导数 = 6a*t + 2b = 0 => t = -b/(3a)
                    double critical = -bb / (3.0 * aa);
                    if (critical > 0.0 && critical < h)
                    {
                        candidates.Add(Math.Abs(3.0 * aa * critical * critical + 2.0 * bb * critical + cc));
                    }
                }

                maxDq[seg, d] = candidates.Max();

                // 二阶导是线性函数 6a*t + 2b，最大值在端点。
                maxDdq[seg, d] = Math.Max(Math.Abs(2.0 * bb), Math.Abs(6.0 * aa * h + 2.0 * bb));

                // 三阶导是常数 6a。
                maxDddq[seg, d] = Math.Abs(6.0 * aa);
            }
        }

        return (maxDq, maxDdq, maxDddq);
    }

    /// <summary>
    /// 定位给定时刻所属段索引，处理越界情况（ clamp 到首尾段）。
    /// </summary>
    private int FindSegment(double t)
    {
        if (t <= _times[0])
        {
            return 0;
        }

        if (t >= _times[_n - 1])
        {
            return _nseg - 1;
        }

        // 线性扫描，因为路点数量通常很小（几十到几百个）。
        for (int i = 0; i < _nseg; i++)
        {
            if (t >= _times[i] && t < _times[i + 1])
            {
                return i;
            }
        }

        return _nseg - 1;
    }

    /// <summary>
    /// 对每个关节独立建立 clamped-zero 边界的三次样条系数。
    /// </summary>
    private void BuildCoefficients()
    {
        for (int d = 0; d < _dof; d++)
        {
            // 解三对角方程组求二阶导数 M_i。
            // 方程：h_{i-1}*M_{i-1} + 2(h_{i-1}+h_i)*M_i + h_i*M_{i+1} = 6*((y_{i+1}-y_i)/h_i - (y_i-y_{i-1})/h_{i-1})
            // 边界：2h_0*M_0 + h_0*M_1 = 6*((y_1-y_0)/h_0 - y'_0)，y'_0 = 0
            //       h_{n-2}*M_{n-2} + 2h_{n-2}*M_{n-1} = 6*(y'_n - (y_n-y_{n-1})/h_{n-2})，y'_n = 0
            var alpha = new double[_n];
            alpha[0] = 6.0 * ((_values[1][d] - _values[0][d]) / _h[0] - 0.0);
            alpha[_n - 1] = 6.0 * (0.0 - (_values[_n - 1][d] - _values[_n - 2][d]) / _h[_n - 2]);
            for (int i = 1; i < _n - 1; i++)
            {
                alpha[i] = 6.0 * ((_values[i + 1][d] - _values[i][d]) / _h[i] - (_values[i][d] - _values[i - 1][d]) / _h[i - 1]);
            }

            var l = new double[_n];
            var mu = new double[_n];
            var z = new double[_n];
            l[0] = 2.0 * _h[0];
            mu[0] = 0.5;
            z[0] = alpha[0] / l[0];

            for (int i = 1; i < _n - 1; i++)
            {
                l[i] = 2.0 * (_h[i - 1] + _h[i]) - _h[i - 1] * mu[i - 1];
                mu[i] = _h[i] / l[i];
                z[i] = (alpha[i] - _h[i - 1] * z[i - 1]) / l[i];
            }

            l[_n - 1] = 2.0 * _h[_n - 2] - _h[_n - 2] * mu[_n - 2];
            z[_n - 1] = (alpha[_n - 1] - _h[_n - 2] * z[_n - 2]) / l[_n - 1];

            var m = new double[_n];
            m[_n - 1] = z[_n - 1];
            for (int j = _n - 2; j >= 0; j--)
            {
                m[j] = z[j] - mu[j] * m[j + 1];
            }

            // 计算每段系数
            for (int seg = 0; seg < _nseg; seg++)
            {
                double h = _h[seg];
                double yj = _values[seg][d];
                double yj1 = _values[seg + 1][d];
                _a[seg][d] = (m[seg + 1] - m[seg]) / (6.0 * h);
                _b[seg][d] = m[seg] / 2.0;
                _c[seg][d] = (yj1 - yj) / h - h * (2.0 * m[seg] + m[seg + 1]) / 6.0;
                _d[seg][d] = yj;
            }
        }
    }
}
