#include <stdlib.h>

/* solve tridiagonal system, a lower, b diagonal, c upper, d right hand side */
void tridiagonal_solve(double * a, double * b, double * c, double *d, int n, double * r)
{
    int i;
    c[0] = c[0] / b[0];
    d[0] = d[0] / b[0];
    for (i = 1; i < n; i++) {
        double divisor = (b[i] - a[i] * c[i-1]);
        c[i] = c[i] / divisor;
        d[i] = (d[i] - a[i] * d[i-1]) / divisor;
    }

    /* backward substitution */
    r[n - 1] = d[n - 1];
    for (i = n - 2; i >= 0; i--) {
        r[i] = d[i] - c[i] * r[i + 1];
    }
}


/* natural spline parametesr wity y'' = 0 boundary condition
 * to be used by nat_spline_int */
void nat_spline(const float * x, const float *y, int n, double * p)
{
    double * a = malloc(n * sizeof(*a));
    double * b = malloc(n * sizeof(*b));
    double * c = malloc(n * sizeof(*c));
    double * d = malloc(n * sizeof(*d));
    int i;

    double xf = x[1] - x[0];
    double xb = x[n-1] - x[n-2];
    /* build rhs with natural boundary condition (y'' = 0) */
    d[0] = 3*((y[1] - y[0]) / (xf * xf));
    for (i = 1; i < n - 1; i++) {
        double h = (x[i] - x[i - 1]);
        double h2 = (x[i+1] - x[i]);
        d[i] = 3 * ((y[i] - y[i-1])/(h*h) + (y[i+1]-y[i]) / (h2*h2));
    }
    d[n - 1] = 3*(y[n-1] - y[n-2]) / (xb*xb);

    /* build tridiagonal matrix a lower, b diag, c upper*/
    b[0] = 2 / xf;
    c[0] = 1 / xf;
    for (i = 1; i < n - 1; i++) {
        double h = (x[i] - x[i - 1]);
        double h2 = (x[i+1] - x[i]);
        a[i] = 1/ h;
        b[i] = (2/h + 2/h2);
        c[i] = 1/ h2;
    }
    a[n-1] = 1 / xb;
    b[n-1] = 2 / xb;

    tridiagonal_solve(a, b, c, d, n, p);

    free(a);
    free(b);
    free(c);
    free(d);

    return;
}

/* spline interpolation at point xv, p parameters from nat_spline
 * x must be ascending
 * last evaluated spline position storage, must be NULL if xv is smaller than
 * that of the last call */
double nat_spline_int(const float * x, const float * y, double * p, int n, float xv,
                      int * last)
{
    int jl = 0;
    int jh = 1;
    int j;
    int l = last ? *last : 0;
    for (j = l; j < n - 1; j++) {
        if (xv >= x[j]) {
            jl = j;
            jh = j + 1;
        }
        if (xv < x[j]) {
            break;
        }
    }
    if (last) {
        *last = jl;
    }
    {
        double t = (xv - x[jl]) / (x[jh] - x[jl]);
        double pa =  p[jl] * (x[jh]-x[jl]) - (y[jh] - y[jl]);
        double pb = -p[jh] * (x[jh]-x[jl]) + (y[jh] - y[jl]);
        return (1-t)*y[jl] + t*y[jh] + t*(1-t)*(pa*(1-t)+pb*t);
    }
}

/* prepare 2d splines, fortran indexing (z and p2d ragged) */
void nat_spline2d(const float *y, float **z, int nx, int ny, double ** p2d)
{
    int i;
    for (i = 0; i < nx; i++) {
        nat_spline(&y[1], &(z[i + 1][1]), ny, &(p2d[i + 1][1]));
    }
}

/* interpolate 2d data along ninterpolate points of xpos
 * fortran indexing (z and p2d ragged)
 * xpos and interpolated 0 indexing*/
void nat_spline2d_int(const float * x, const float *y, float **z,
                      double **p2d,
                      int nx, int ny,
                      int ninterpolate, float * xpos, float ypos, float * interpolated)
{
    float * yytmp = malloc(nx * sizeof(*yytmp));
    double * p = malloc(ny * sizeof(*p));
    int i;
    int last = 0;

    for (i = 0; i < nx; i++) {
        yytmp[i] = nat_spline_int(&y[1], &(z[i + 1][1]), &(p2d[i + 1][1]), ny, ypos, &last);
    }
    nat_spline(x, yytmp, nx, p);

    last = 0;
    /* interpolate points */
    for (i = 0; i < ninterpolate; i++) {
        interpolated[i] = nat_spline_int(&x[1], yytmp, p, nx, xpos[i], &last);
    }
    free(p);
    free(yytmp);
}

/***************************************************************
 *
 * hsplint(): spline interpolation based on Hermite polynomials.
 *
 ***************************************************************/
double hsplint( double xp, double *x, float *y, int n, int *istart )
/* double xp;	 x-value to interpolate */
/* double *x;	 x-array [1..n] */
/* float  *y;	 y-array [1..n] */
/* int *istart; initial index   */
{
    double yp1, yp2, yp;
    double xpi, xpi1, l1, l2, lp1, lp2;
    int i;

    if ( xp < x[1] || xp > x[n] )
	return(0.0);

    for ( i = *istart; i <= n && xp >= x[i]; i++ )
	;

    *istart = i;
    i--;

    lp1 = 1.0 / (x[i] - x[i+1]);
    lp2 = -lp1;

    if ( i == 1 )
	yp1 = (y[2] - y[1]) / (x[2] - x[1]);
    else
	yp1 = (y[i+1] - y[i-1]) / (x[i+1] - x[i-1]);

    if ( i >= n - 1 )
	yp2 = (y[n] - y[n-1]) / (x[n] - x[n-1]);
    else
	yp2 = (y[i+2] - y[i]) / (x[i+2] - x[i]);

    xpi1 = xp - x[i+1];
    xpi  = xp - x[i];
    l1   = xpi1*lp1;
    l2   = xpi*lp2;

    yp = y[i]*(1 - 2.0*lp1*xpi)*l1*l1 +
         y[i+1]*(1 - 2.0*lp2*xpi1)*l2*l2 +
         yp1*xpi*l1*l1 + yp2*xpi1*l2*l2;

    return(yp);
}
