namespace Hawkeye.VisionBuilder.UI.Sources.Hawkeye;

public class SplineInterpolator
{
    private readonly Dictionary<double, double> _nodes;
    private readonly double[] _keys;

    private readonly double[] _values;

    private readonly double[] _h;

    private readonly double[] _a;

    /// <summary>
    /// Class constructor.
    /// </summary>
    /// <param name="nodes">Collection of known points for further interpolation.
    /// Should contain at least two items.</param>
    public SplineInterpolator(Dictionary<double, double> nodes)
    {
        if (nodes == null)
        {
            throw new ArgumentNullException("nodes");
        }

        _nodes = nodes;

        var n = nodes.Count;

        if (n < 2)
        {
            throw new ArgumentException("At least two point required for interpolation.");
        }

        _keys = nodes.Keys.ToArray();
        _values = nodes.Values.ToArray();
        _a = new double[n];
        _h = new double[n];

        for (int i = 1; i < n; i++)
        {
            _h[i] = _keys[i] - _keys[i - 1];
        }

        if (n > 2)
        {
            var sub = new double[n - 1];
            var diag = new double[n - 1];
            var sup = new double[n - 1];

            for (int i = 1; i <= n - 2; i++)
            {
                diag[i] = (_h[i] + _h[i + 1]) / 3;
                sup[i] = _h[i + 1] / 6;
                sub[i] = _h[i] / 6;
                _a[i] = (_values[i + 1] - _values[i]) / _h[i + 1] - (_values[i] - _values[i - 1]) / _h[i];
            }

            SolveTridiag(sub, diag, sup, ref _a, n - 2);
        }
    }

    public double[] Keys => _keys;

    public double[] Values => _values;

    public Dictionary<double, double> Nodes => _nodes;

    /// <summary>
    /// Gets interpolated value for specified argument.
    /// </summary>
    /// <param name="key">Argument value for interpolation. Must be within 
    /// the interval bounded by lowest ang highest <see cref="_keys"/> values.</param>
    public double GetValue(double key)
    {
        int gap = 0;
        var previous = double.MinValue;


        if (key > _keys.Max()) key = _keys.Max();
        if (key < _keys.Min()) key = _keys.Min();

        for (int i = 0; i < _keys.Length; i++)
        {
            if (Math.Abs(_keys[i] - key) < 0.001)
            {
                return _values[i];
            }
        }



        // At the end of this iteration, "gap" will contain the index of the interval
        // between two known values, which contains the unknown z, and "previous" will
        // contain the biggest z value among the known samples, left of the unknown z
        for (int i = 0; i < _keys.Length; i++)
        {
            if (_keys[i] < key && _keys[i] > previous)
            {
                previous = _keys[i];
                gap = i + 1;
            }
        }

        var x1 = key - previous;
        var x2 = _h[gap] - x1;

        var res = ((-_a[gap - 1] / 6 * (x2 + _h[gap]) * x1 + _values[gap - 1]) * x2 +
            (-_a[gap] / 6 * (x1 + _h[gap]) * x2 + _values[gap]) * x1) / _h[gap];
        if (res > 255) res = 255;
        if (res < 0) res = 0;
        return res;
    }


    /// <summary>
    /// Solve linear system with tridiagonal n*n matrix "a"
    /// using Gaussian elimination without pivoting.
    /// </summary>
    private static void SolveTridiag(double[] sub, double[] diag, double[] sup, ref double[] b, int n)
    {
        int i;

        for (i = 2; i <= n; i++)
        {
            sub[i] = sub[i] / diag[i - 1];
            diag[i] = diag[i] - sub[i] * sup[i - 1];
            b[i] = b[i] - sub[i] * b[i - 1];
        }

        b[n] = b[n] / diag[n];

        for (i = n - 1; i >= 1; i--)
        {
            b[i] = (b[i] - sup[i] * b[i + 1]) / diag[i];
        }
    }
}