/* $Revision: 1.2 $ */
/*
 *
 * YPRIME.C Sample .MEX file corresponding to YPRIME.M
 *					Solves simple 3 body orbit problem 
 *
 * The calling syntax is:
 *
 *		[yp] = yprime(t, y)
 *
 * Copyright (c) 1984-1998 by The MathWorks, Inc.
 * All Rights Reserved.
 */

#include <math.h>
#include "mex.h"
/* Input Arguments */

#define X_IN	prhs[0]
#define LKUP_IN	prhs[1]

/* Output Arguments */
#define Y_OUT plhs[0]

static double mean(
			 double	*idxs[],
			 double levels[],
			 int	level)
{
	if (level == 0)
	{
		return 	*(idxs[0]);
	}
	else
	{
		return (1-levels[level-1]) * mean(idxs, levels, level-1) + levels[level-1] * mean(idxs + (1<<(level-1)),levels , level-1);
	}
}

static void calcOut(
			 double *out,
			 double *x,
			 double *lup,
			 double **scales,
			 int 	*dims,
			 int	nIn,
			 int	nPoints)
			 
{
	double 	*levels;
	int		*valLow, *valHigh;
	double	**idxs;
	int 	thePoint, xOff, i, j, k, nVertex, factor;
	
	
	/* Allocate the space for the temporary variables */
	levels = (double*) mxCalloc(nIn,sizeof(double));
	valLow = (int*) mxCalloc(nIn,sizeof(int));
	valHigh= (int*) mxCalloc(nIn,sizeof(int));
	
	nVertex = (1<<nIn);
	idxs = (double**) mxCalloc(nVertex,sizeof(double*));
	
	/* mxCalloc throws an exception to Matlab if there is a problem */
	
	for (thePoint=0,xOff=0;thePoint<nPoints;thePoint++,xOff+=nIn)
	{
		// Find in which hypercube the point is
		for (i=0;i<nIn;i++)
		{
			j = 0;
			
			while ((j<dims[i])&&(scales[i][j]<x[xOff+i])) j++;
			
			if (j==0)
			{
				levels[i]=1;
				valLow[i]=0;
				valHigh[i]=0;
			}
			else
			{
				if (j==dims[i])
				{
					levels[i]=0;
					valHigh[i]=--j;
					valLow[i]=j;
				}
				else
				{
					levels[i]=(x[xOff+i] - scales[i][j-1])/(scales[i][j] - scales[i][j-1]);
					valHigh[i]=j--;
					valLow[i]=j;
				}
			}
		}
		
		//Get the idxs
		for (i=0;i<nVertex;i++)
		{
			k=0;
			factor=1;
			for (j=0;j<nIn;j++)
			{
				k += factor * (((i&(1<<j))==0)?valLow[j]:valHigh[j]);
				factor *= dims[j];
			}
			idxs[i] = lup + k;
		}
		
		out[thePoint] = (1-levels[nIn-1]) * mean(idxs, levels, nIn-1) + levels[nIn-1] * mean(idxs + (1<<(nIn-1)),levels , nIn-1);
		
	}
	
	mxFree(levels);
	mxFree(valLow);
	mxFree(valHigh);
	mxFree(idxs);
	
}

void mexFunction(
								 int nlhs,	mxArray *plhs[],
								 int nrhs, const mxArray *prhs[])
								 
{
	double		*out;
	double		*lup, *x;
	double		**scales;
	int			*dims;
	int			nIn,nPoints,i;
 
					 
	/* Check for proper number of arguments */
	
	if (nrhs < 3) {
		mexErrMsgTxt("outMex requires three input arguments.");
	}
	
	
	nIn = nrhs-2;
 	nPoints = mxGetN(X_IN);
	
	/* Create a matrix for the return argument */
	Y_OUT = mxCreateDoubleMatrix(1, nPoints, mxREAL);
	
	/* Assign pointers to the various parameters */
	out = mxGetPr(Y_OUT);
	
	x = mxGetPr(X_IN);
	lup = mxGetPr(LKUP_IN);
	
	//Allocate space for the tables
	scales = (double**)mxCalloc(nrhs-2,sizeof(double*));
	dims = (int*)mxCalloc(nrhs-2,sizeof(int));
	
	for (i=2;i<nrhs;i++)
	{
		scales[i-2] = mxGetPr(prhs[i]);
		dims[i-2] = mxGetN(prhs[i]);
	}
	
	/* Do the actual computations in a subroutine */
	calcOut(out,x,lup,scales,dims,nIn,nPoints);
	return;
}


