/* $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_IN plhs[0]
#define Y_OUT plhs[1]

static void calcOut(
			 double	*in,
			 double *out,
			 double *x,
			 double *lup,
			 double **scales,
			 int 	*dims,
			 int	nIn)
			 
{
	int 	*valLow, *valHigh;
	int 	i, j, k, nVertex, factor;
	
	
	/* Allocate the space for the temporary variables */
	valLow = (int*) mxCalloc(nIn,sizeof(int));
	valHigh= (int*) mxCalloc(nIn,sizeof(int));
	
	nVertex = (1<<nIn);
	
	/* mxCalloc throws an exception to Matlab if there is a problem */
	
	// Find in which hypercube the point is
	for (i=0;i<nIn;i++)
	{
		j = 0;
		
		while ((j<dims[i])&&(scales[i][j]<x[i])) j++;
		
		if (j==0)
		{
			valLow[i]=0;
			valHigh[i]=1;	//This could be wrong !!
		}
		else
		{
			if (j==dims[i])
			{
				valHigh[i]=--j;
				valLow[i]=--j;
			}
			else
			{
				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++)
		{
			/*
			All this is based on the fact that binary writing of the label of a vertex
			gives the down(0) high(1) position of it along each dimension
			*/
			in[i*nIn+j] = scales[j][(((i&(1<<j))==0)?valLow[j]:valHigh[j])];
			k += factor * (((i&(1<<j))==0)?valLow[j]:valHigh[j]);
			factor *= dims[j];	
		}
		
		out[i] = *(lup + k);
	}
	
	mxFree(valLow);
	mxFree(valHigh);
	
}

void mexFunction(
								 int nlhs,	mxArray *plhs[],
								 int nrhs, const mxArray *prhs[])
								 
{
	double		*out, *in;
	double		*lup, *x;
	double		**scales;
	int			*dims;
	int			nIn,i,nVertex;
 
					 
	/* Check for proper number of arguments */
	if (nrhs < 3) {
		mexErrMsgTxt("outMex requires three input arguments.");
	}
	
	
	nIn = nrhs-2;
 	
	/* Create a matrix for the return argument */
	nVertex = (1<<nIn);
	
	Y_IN = mxCreateDoubleMatrix(nIn, nVertex, mxREAL);
	Y_OUT = mxCreateDoubleMatrix(1, nVertex, mxREAL);
	
	/* Assign pointers to the various parameters */
	in = mxGetPr(Y_IN);
	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(in,out,x,lup,scales,dims,nIn);
	return;
}


