#include <stdio.h>
#include <math.h>
#ifndef NOUNISTD_H
#include <unistd.h>
#endif
#include "fudgit.h"
#include "dalloca.h"
#include "head.h"

static double *Xs, *Ys, *Y2s;
static int SplData = 0;
static int initspline(double *x, double *y, int n);


int Ft_spline(double *x, double *y, double yp1, double ypn, int n)
{
	int i, k;
	double p, qn, sig, un, *u;
	extern double *Y2s;

	/* initialize vectors Xs, Ys and Y2s */
	if (initspline(x, y, n) == ERRR) {
		fputs("spline: Allocation error.\n", stderr);
		return(ERRR);
	}
	ADVECTOR(u, 1, n-1, "spline", return);
	if (yp1 > 0.99e30) {
		Y2s[1] = u[1] = 0.0;
	}
	else {
		Y2s[1] = -0.5;
		u[1] = (3.0/(x[2]-x[1])) * ((y[2]-y[1])/(x[2]-x[1]) - yp1);
	}
	for (i=2; i<=n-1; i++) {
		sig = (x[i]-x[i-1]) / (x[i+1]-x[i-1]);
		p = sig*Y2s[i-1]+2.0;
		Y2s[i] = (sig-1.0)/p;
		u[i] = (y[i+1]-y[i])/(x[i+1]-x[i]) - (y[i]-y[i-1])/(x[i]-x[i-1]);
		u[i] = (6.0*u[i]/(x[i+1]-x[i-1])-sig*u[i-1]) / p;
	}
	if (ypn > 0.99e30) {
		qn = un = 0.0;
	}
	else {
		qn = 0.5;
		un = (3.0/(x[n] - x[n-1])) * (ypn - (y[n] - y[n-1])/(x[n]-x[n-1]));
	}
	Y2s[n] = (un - qn * u[n-1]) / (qn * Y2s[n-1] + 1.0);
	for (k=n-1; k>=1; k--) {
		Y2s[k] = Y2s[k] * Y2s[k+1] + u[k];
	}
	SplData = n;
	return(0);
}

/* routine called from C-calculator mode */

double Ft_interp(double x)
{
	int klo, khi, k;
	double h, b, a, y;
	extern double *Xs, *Ys, *Y2s;
	extern int SplData;

	if (!SplData) {
		fputs("Math error: interp: Routine not initialized.\n", stderr);
		Ft_catcher(ERRR);
	}
	klo = 1;
	khi = SplData;
	while (khi-klo > 1) {
		k = (khi+klo) >> 1;
		if (Xs[k] > x)
			khi = k;
		else
			klo = k;
	}
	h = Xs[khi] - Xs[klo];
	if (h == 0.0) {
		fputs("Math error: interp: Routine not properly initialized.\n",
		stderr);
		Ft_catcher(ERRR);
	}
	a = (Xs[khi]-x)/h;
	b = (x-Xs[klo])/h;
	y = a * Ys[klo] + b * Ys[khi]
	+ (a*(a*a-1.0) * Y2s[klo] + b*(b*b-1.0) * Y2s[khi]) * (h*h) / 6.0;
	return(y);
}

static int initspline(double *x, double *y, int n)
{
	int i;
	extern double *Xs, *Ys, *Y2s;
	extern void Ft_free_dvector(double *v, int nl, int nh);
	extern double *Ft_dvector(int nl, int nh);

	if (n != SplData) {
		if (Xs)
			Ft_free_dvector(Xs, 1, n);
		if ((Xs = Ft_dvector(1, n)) == NULL)
			return(ERRR);
		if (Ys)
			Ft_free_dvector(Ys, 1, n);
		if ((Ys = Ft_dvector(1, n)) == NULL)
			return(ERRR);
		if (Y2s)
			Ft_free_dvector(Y2s, 1, n);
		if ((Y2s = Ft_dvector(1, n)) == NULL)
			return(ERRR);
	}
	for (i=1;i<=n;i++) {
		Xs[i] = x[i];
		Ys[i] = y[i];
	}
	return(0);
}