#include <vector>
#include "hdnum.hh"    // hdnum header

template<typename REAL>
void solveTriDiag(hdnum::DenseMatrix<REAL> &A, std::vector<REAL> &x, std::vector<REAL> &b) {
    int N = b.size();
    x[0] = A[0][0];
    // LU Zerlegung
    REAL l;
    for (int j=1; j<N; j++) {
        // berechne l faktor
        l = A[j][j-1] / x[j-1];
        // modifiziere diagonalelemente und rechte seite
        x[j] = A[j][j] - l * A[j-1][j];
        b[j] = b[j] - l * b[j-1];
    }
    // Loesen durch Rueckwaertseinsetzen
    x[N-1] = b[N-1] / x[N-1];
    for (int j=N-2; j>=0; j--) {
        x[j] = (b[j] - A[j][j+1]*x[j+1])/x[j];
    }
}

template<typename REAL>
class CubicSpline {
    public:
        // erstelle einen kubischen spline mit vorgegebenen stuetzstellen
        // und werten
        CubicSpline(std::vector<REAL> xs, std::vector<REAL> ys) {
            calculateCoefficients(xs, ys);
        }

        // erstelle einen kubischen spline mit vorgegebenen stuetzstellen
        // und einer zu interpolierenden funktion
        CubicSpline(std::vector<REAL> xs, REAL(*f)(REAL)) {
            int N = xs.size();
            std::vector<double> ys(N);
            for (int i=0; i<N; i++) {
                ys[i] = f(xs[i]);
            }
            calculateCoefficients(xs, ys);
        }

        // erstelle einen kubischen spline an aequidistanten stuetzstellen
        // und einer zu interpolierenden funktion
        CubicSpline(REAL a, REAL b, int N, REAL(*f)(REAL)) {
            std::vector<double> xs(N+1);
            std::vector<double> ys(N+1);
            for (int i=0; i<=N; i++) {
                xs[i] = a + (1.0*i)/N*(b-a);
                ys[i] = f(xs[i]);
            }
            calculateCoefficients(xs, ys);
        }

        // werte kubischen spline an vorgegebener stelle aus
        REAL evaluate(REAL x) {
            for (int i=1; i<x_s.size(); i++) {
                if (x > x_s[i] && i < x_s.size() - 1) {
                    continue;
                } else {
                    return a_0[i-1] + a_1[i-1] *(x - x_s[i]) + a_2[i-1]*std::pow(x - x_s[i], 2) + a_3[i-1]*std::pow(x-x_s[i], 3);
                }
            }
            return 0;
        }

        // gebe alle interpolations polynome aus
        void print() {
            for(int i=0; i<x_s.size()-1; i++) {
                printf("p_%d(x) = %4.2f + %4.2f(x - %4.2f) + %4.2f(x - %4.2f)^2 + %4.2f(x - %4.2f)^3\n",
                       i, a_0[i], a_1[i], x_s[i], a_2[i], x_s[i], a_3[i], x_s[i]);
            }
        }

    private:
        // stuetzstellen
        std::vector<REAL> x_s;
        // koeffizienten
        std::vector<REAL> a_0;
        std::vector<REAL> a_1;
        std::vector<REAL> a_2;
        std::vector<REAL> a_3;

        void calculateCoefficients(std::vector<REAL> xs, std::vector<REAL> ys) {
            // stuetzstellen from x_0 ... to x_n
            int n = xs.size()-1;
            // copy stuetzstellen
            x_s = std::vector<double>(n+1);
            x_s = xs;
            // setup (n-1)x(n-1) matrix for a_2
            hdnum::DenseMatrix<REAL> A(n-1,n-1);
            std::vector<REAL> b(n-1);
            std::vector<REAL> x(n-1);
            REAL h; // h_i
            REAL h1; // h_{i+1}
            // setup LGS for a_2
            // A has tridiagonal structure
            for (int i = 1; i<n; i++) {
                h = xs[i] - xs[i-1];
                h1 = xs[i+1] - xs[i];
                b[i-1] = 3 * ((ys[i+1] - ys[i])/h1 - (ys[i] - ys[i-1])/h);
                if (i > 1) {
                    A[i-1][i-2] = h;
                } if (i < n-1) {
                    A[i-1][i] = h1;
                }
                A[i-1][i-1] = 2 * (h + h1);
            }
            // initialize vectors
            a_0 = std::vector<double>(n);
            a_1 = std::vector<double>(n);
            a_2 = std::vector<double>(n);
            a_3 = std::vector<double>(n);
            solveTriDiag(A,a_2,b);
            // natuerliche randbedingung
            a_2[n-1] = 0;
            // berechne restliche koeffizienten
            for (int i = 1; i<=n; i++) {
                h = xs[i] - xs[i-1]; // h_i
                h1 = xs[i+1] - xs[i]; // h_{i+1}
                a_0[i-1] = ys[i];
                if (i == 1) { // a_2[-1] = 0
                    a_1[i-1] = (ys[i] - ys[i-1])/h + (h/3)*(2*a_2[i-1]);
                    a_3[i-1] = (a_2[i-1])/(3*h);
                } else {
                    a_1[i-1] = (ys[i] - ys[i-1])/h + h/3*(2*a_2[i-1] + a_2[i-2]);
                    a_3[i-1] = (a_2[i-1] - a_2[i-2])/(3 * h);
                }
            }
        }
};

double f1(double x) {
    return std::pow(x,3);
}

int main() {
    // std::vector<double> xs = {0, 1, 2};
    // CubicSpline<double> phi(xs, f1);
    // phi.print();

    // saurier fundstellen
    std::vector<double> xs = {3.75, 3.75, 2.25, 1.25, 0.25, 0.75, 4.00, 5.50, 6.25, 9.00, 12.5, 12.75, 12.25};
    std::vector<double> ys = {0.25, 1.50, 3.25, 4.25, 4.65, 4.83, 2.50, 3.25, 3.65, 3.00, 1.25, 2.150, 3.500};

    std::vector<double> ts(13);
    ts[0] = 0;
    for (int i = 1; i<=12; i++) {
        ts[i] = ts[i-1] + std::sqrt(std::pow(xs[i] - xs[i-1], 2) + std::pow(ys[i] - ys[i-1], 2));
    }

    CubicSpline<double> phi(ts, xs);
    CubicSpline<double> psi(ts, ys);

    double xi;
    for (int j = 0; j<=100; j++) {
        xi = j*ts[12] / 100;
        std::cout << phi.evaluate(xi) << " " << psi.evaluate(xi) << std::endl;
    }
}