/*********************************************************************************
*
* Purpose:  interpolate interpolates for the position and velocity components
*           at a specific time within time ordered arrays of position and 
*           velocity data.  A hermite polynomial, which considers both 
*           position and velocity components, is used for the interpolation.
*
* Input:    posArray  2D Array of Position Components
*           velArray  2D Array of Velocity Components
*           xn        Starting Point Index for Interpolation
*           cnum      Number of Coordinates Being Interpolated
*           asize     Number of Elements in Array[cnum][asize]
*           tstep     Time Difference between Points in Array
*           intpts    Number of Interpolation Points
*           tdif      Time Difference xn - xi = tdif
* Output:   newPos    Interpolated Position Vector for time xi
*           newVel    Interpolated Velocity Vector for time xi
*
* Converted from FORTRAN INTRP0 (J.Toth) on 10/21/2002 by C.W.Brown 
*
**********************************************************************************/
#include <math.h>
#include "HInterpolate.h"

//using namespace std;

//void interpolate(double posArray[][MAX_ARR_SIZE], double velArray[][MAX_ARR_SIZE], 
void interpolate(double *posArray, double *velArray, int xn, int cnum, int asize, 
                        double tstep, int intpts, double tdif, double newPos[], double newVel[])
{
	int i, j;
	int ic, jc, xnmod;             //Coefficient Counters
	int npoint;
	double nsteps;
	double ll, a, b;
	double close;
	//double posArray[3][MAX_ARR_SIZE];
	//double velArray[3][MAX_ARR_SIZE];

	double hp[2][MAX_INTPTS];
	double hv[2][MAX_INTPTS];

/*	for(i=0; i<asize; i++)	
		for(j=0; j<3; j++){
			cout << "PosArray"  << j+3*i << ":  " << posArray[j+3*i] << "Vel:  "  << velArray[j+3*i] << endl;
		}
	//return;
*/

	//Compute Number of Timesteps Between Base and Interpolation Time
	nsteps = tdif / tstep;
        //cout << "NSteps:  "  << nsteps << endl;
	//Check to see if interpolation is needed (i.e. not close to another point)

	close = fabs(fmod(tdif, tstep));
	if(close<1.0e-10)
	{
		for(int i=0; i<cnum; i++)
		{
			npoint = int(fabs(nsteps)+0.1e0);
			//newPos[i]=posArray[i][xn+npoint];
			newPos[i]=posArray[i+3*(xn+npoint)];
			//newVel[i]=velArray[i][xn+npoint];
			newVel[i]=velArray[i+3*(xn+npoint)];
		}
		return;
	}
	
	jc = 1;
	while(jc <= intpts)                              // until jc is larger than the interpolation order
	{
		initialize_hc(ic, ll, a, b);
		while(ic <= intpts)                              // until ic is larger than the interpolation order
		{	
			if(ic != jc)                             // calculate hermitian coefficients
				calculate_hc(ll, a, b, nsteps, ic, jc);
			ic++;
		}

		//Compute Hermitian Values for hp[][] and hv[][]
		compute_hermi(jc, nsteps, ll, a, b, tstep, hp, hv);
		jc++;
	}

	for(int i=0; i<cnum; i++)
	{  
		newPos[i] = 0.0e0;
		newVel[i] = 0.0e0;
		for(int j=1; j<=intpts; j++)
		{
			xnmod = ((xn-j+1) % asize);
			//cout << "XN: " << xnmod << endl;
			//newPos[i] += hp[0][j]*posArray[i][xnmod] + hp[1][j]*velArray[i][xnmod];
			newPos[i] += hp[0][j]*posArray[i+3*xnmod] + hp[1][j]*velArray[i+3*xnmod];
			//newVel[i] += hv[0][j]*posArray[i][xnmod] + hv[1][j]*velArray[i][xnmod];
			newVel[i] += hv[0][j]*posArray[i+3*xnmod] + hv[1][j]*velArray[i+3*xnmod];
		}
		
//		cout << "PosArray"  << i+3*xnmod << ":  " << posArray[i+xnmod] << "Vel:  "  << velArray[i+xnmod] << endl;
//		cout << "Pos"  << i << ":  " << newPos[i] << "Vel:  "  << newPos[i] << endl;
	}

	return;
}

void compute_hermi(int a, double v, double w, double x, double y, double z, double xArr[2][MAX_INTPTS], double yArr[2][MAX_INTPTS])
{
	double phi0, phi1;
	double g0, g1;
	double step = v + a - 1;

	phi0 = 1 - 2 * step * y;
	phi1 = z * step;
	xArr[0][a] = phi0 * w * w;
	xArr[1][a] = phi1 * w * w;
	g0 = (2/z) * (x - 2 * x * y * step - y);
	g1 = 1 + 2 * x * step;
	yArr[0][a] = g0 * w * w;
	yArr[1][a] = g1 * w * w;

	return;
}
		

void calculate_hc(double &w, double &x, double &y, double z, int a, int b)
{
	w = w * (z + a - 1)/(a - b);
	x = x + 1 / (z + a - 1);
	y = y + 1.0e0 / (a - b);
	return;
}

void initialize_hc(int &a, double &x, double &y, double &z)
{
	a = 1;
	x = 1.0e0;
	y = 0.0e0;
	z = 0.0e0;
	return;
}