#include <memory.h>

#define	INVALID	-1
#define	FCC_FLAG_PACKED	(1<<0)
#define	FCC_FLAG_QUASI	(1<<1)
#define	ABS(x)	((x) > 0?(x):(-(x)))
#ifndef	MULT_MAT_VEC_4
#define	MULT_MAT_VEC_4(M,x,y)											\
	y[0] = M[0][0]*x[0] + M[0][1]*x[1] + M[0][2]*x[2] + M[0][3]*x[3];	\
	y[1] = M[1][0]*x[0] + M[1][1]*x[1] + M[1][2]*x[2] + M[1][3]*x[3];	\
	y[2] = M[2][0]*x[0] + M[2][1]*x[1] + M[2][2]*x[2] + M[2][3]*x[3];	\
	y[3] = M[3][0]*x[0] + M[3][1]*x[1] + M[3][2]*x[2] + M[3][3]*x[3];	
#endif

typedef	double	DTYPE;

// maps 2x2x2 index for FCC lattice coordinates to a 1D array index (0~3)
const int	map_index_FCC[2][2][2] =
{
	{
		{0,			INVALID},
		{INVALID,	1},
	},
	{
		{INVALID,	2},
		{3,			INVALID},
	},
};

// list of multi-indices of degree 3. (`alpha' in Table 3.)
int	multi_index[20][4] =
{
	{3,0,0,0},
	{2,1,0,0},
	{1,2,0,0},
	{0,3,0,0},
	{2,0,1,0},

	{1,1,1,0},
	{0,2,1,0},
	{1,0,2,0},
	{0,1,2,0},
	{0,0,3,0},

	{2,0,0,1},
	{1,1,0,1},
	{0,2,0,1},
	{1,0,1,1},
	{0,1,1,1},

	{0,0,2,1},
	{1,0,0,2},
	{0,1,0,2},
	{0,0,1,2},
	{0,0,0,3}
};

// set of shifts overlapping the input point (`J_{-}' and `J_{+}' in Table 3.)
const int	shifts[2][16][3]=
{
	{
		{ 0, 0, 0},	
		{ 0, 1, 1},
		{ 1, 0, 1},
		{ 1, 1, 0},
		{-1, 0, 1},	//	5
		{-1, 1, 0},
		{ 0,-1, 1},
		{ 0, 1,-1},
		{ 1,-1, 0},
		{ 1, 0,-1},	//	10
		{ 0,-1,-1},
		{-1, 0,-1},
		{-1,-1, 0},
		{ 2, 0, 0},
		{ 0, 2, 0},	//	15
		{ 0, 0, 2}
	},
	{
		{ 0, 0, 0},
		{ 0, 1, 1},
		{ 1, 0, 1},
		{ 1, 1, 0},
		{ 1, 1, 2},	//	5
		{ 1, 2, 1},
		{ 2, 1, 1},
		{ 0, 2, 0},
		{-1, 1, 0},
		{ 0, 1,-1},	//	10
		{ 2, 0, 0},
		{ 1,-1, 0},
		{ 1, 0,-1},
		{ 0, 0, 2},
		{-1, 0, 1},	//	15
		{ 0,-1, 1}
	}
};

// BB-coefficients of the polynomial pieces of the 6-direction box spline
// (Refer to Table 3.)
DTYPE	bb_coeff[2][16][20] =
{
	{
		{12,12, 8, 4,12,12, 8, 8, 8, 4,12,12, 8,12,12, 8, 8, 8, 8, 4},
		{ 1, 1, 0, 0, 2, 2, 0, 4, 4, 4, 2, 2, 0, 4, 4, 8, 4, 4, 8, 4},
		{ 1, 2, 4, 4, 1, 2, 4, 0, 0, 0, 2, 4, 8, 2, 4, 0, 4, 8, 4, 4},
		{ 1, 2, 4, 4, 2, 4, 8, 4, 8, 4, 1, 2, 4, 2, 4, 4, 0, 0, 0, 0},
		{ 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 2, 0, 0, 2, 0, 0, 4, 0, 4, 4},
		{ 1, 0, 0, 0, 2, 0, 0, 4, 0, 4, 1, 0, 0, 2, 0, 4, 0, 0, 0, 0},
		{ 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 2, 2, 0, 0, 0, 0, 4, 4, 0, 4},
		{ 1, 1, 0, 0, 2, 2, 0, 4, 4, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{ 1, 2, 4, 4, 0, 0, 0, 0, 0, 0, 1, 2, 4, 0, 0, 0, 0, 0, 0, 0},
		{ 1, 2, 4, 4, 1, 2, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{ 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{ 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{ 0, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4},
	},
	{
		{ 0, 0, 4, 4, 0, 4, 8, 4, 8, 4, 0, 4, 8, 4,12, 8, 4, 8, 8, 4},
		{ 4, 8, 8, 4, 4, 4, 4, 0, 0, 0, 8,12, 8, 4, 4, 0, 8, 8, 4, 4},
		{ 4, 4, 0, 0, 8, 4, 0, 8, 4, 4, 8, 4, 0,12, 4, 8, 8, 4, 8, 4},
		{ 4, 8, 8, 4, 8,12, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 0, 0, 0, 0},
		{ 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{ 4, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{ 4, 0, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{ 0, 0, 4, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{ 0, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0},
		{ 0, 0, 0, 4, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{ 0, 0, 0, 0, 0, 0, 0, 4, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 0, 0, 0, 0, 0, 4, 0, 0, 0, 0},
		{ 0, 0, 0, 0, 0, 0, 0, 0, 4, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 0, 0, 4},
		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 0, 4},
		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 4},
	}
};

// Inverse matrices of the matrices composed of the vertices of 
// two types of domain tetrahedra. (Refer to (16) )
// Note that the order of the vertices should be consistent with
// the multi-indices of Table 3.
DTYPE	inv_tets[2][4][4] =
{
	{	// type (-)
		// inverse of
		//	                [0 1 0 0]
		//	[v1 v2 v3 v4] = [0 0 1 0]
		//	[ 1  1  1  1]   [0 0 0 1]
		//	                [1 1 1 1]
		{-1,-1,-1, 1},
		{ 1, 0, 0, 0},
		{ 0, 1, 0, 0},
		{ 0, 0, 1, 0},
	},
	{	// type (+)
		// inverse of
		//                 [1 0 1 0]
		// [v1 v2 v3 v4] = [1 1 0 0]
		// [ 1  1  1  1]   [1 0 0 1]
		//                 [1 1 1 1]
		{ 0.5, 0.5, 0.5,-0.5},
		{-0.5, 0.5,-0.5, 0.5},
		{ 0.5,-0.5,-0.5, 0.5},
		{-0.5,-0.5, 0.5, 0.5}
	}
};

typedef struct
{
	int		flag;
	struct
	{
		int	major;
		int	minor;
	} version;
	DTYPE	range[3][2];
	DTYPE	unit[3];
	DTYPE	unit_inv[3];
	int		size[4];
	DTYPE	*data;
} TFCC;

bool	load_fcc(TFCC &fcc, char *filename);
void	report_fcc(TFCC &fcc);
DTYPE	eval_spline_6dir_FCC(TFCC &dataset, DTYPE q[3], DTYPE value_outside);

///////////////////////////////////////////////////////////////////////////////////////////////////

bool load_fcc(TFCC &fcc, char *filename)
{
	FILE	*pfile = fopen(filename, "rb");
	if(!pfile)
	{
		fprintf(stderr, "file not found (%s).\n", filename);
		return false;
	}

	fread(&(fcc.version.major), 1, sizeof(fcc.version.major), pfile);
	fread(&(fcc.version.minor), 1, sizeof(fcc.version.minor), pfile);
	fread(&(fcc.flag), 1, sizeof(fcc.flag), pfile);
	fread(&(fcc.range[0][0]), 2, sizeof(DTYPE), pfile);
	fread(&(fcc.range[1][0]), 2, sizeof(DTYPE), pfile);
	fread(&(fcc.range[2][0]), 2, sizeof(DTYPE), pfile);
	fread(fcc.unit, 3, sizeof(DTYPE), pfile);
	fread(fcc.unit_inv, 3, sizeof(DTYPE), pfile);
	int	sz_total;
	fread(fcc.size, 4, sizeof(int), pfile);
	if(fcc.flag & FCC_FLAG_PACKED)
		sz_total = fcc.size[0]*fcc.size[1]*fcc.size[2]*fcc.size[3];
	else
		sz_total = fcc.size[0]*fcc.size[1]*fcc.size[2];
	fcc.data = new DTYPE[sz_total];
	fread(fcc.data, sz_total, sizeof(DTYPE), pfile);
	fclose(pfile);
	return true;
}

///////////////////////////////////////////////////////////////////////////////////////////////////
void report_fcc(TFCC &fcc)
{
	printf("version: %d.%d", fcc.version.major, fcc.version.minor);
	printf("packed? %s\n", (fcc.flag & FCC_FLAG_PACKED)?"yes":"no");
	printf("quasi-interpolated? %s\n", (fcc.flag & FCC_FLAG_QUASI)?"yes":"no");
	printf("range=[%f,%f]x[%f,%f]x[%f,%f]\n", fcc.range[0][0], fcc.range[0][1],
												fcc.range[1][0], fcc.range[1][1],
												fcc.range[2][0], fcc.range[2][1]);
	printf("unit length=[%f,%f,%f]\n", fcc.unit[0], fcc.unit[1], fcc.unit[2]);
	printf("inverse of unit length=[%f,%f,%f]\n", fcc.unit_inv[0], fcc.unit_inv[1], fcc.unit_inv[2]);

	if(fcc.flag & FCC_FLAG_PACKED)
	{
		printf("size of samples= %d x %d x %d x %d\n", 
				fcc.size[0], fcc.size[1], fcc.size[2], fcc.size[3]);
	}
	else
	{
		printf("size of samples= %d x %d x %d\n", 
				fcc.size[0], fcc.size[1], fcc.size[2]);
	}
}
#define	GET_DATASET_FCC(dataset,x,y,z,k)	\
	*(dataset.data + (((x)*dataset.size[1] + (y))*dataset.size[2] + (z))*dataset.size[3] + (k))

///////////////////////////////////////////////////////////////////////////////////////////////////
//
// eval_spline_6dir_FCC
//
// - evaluates the spline value at the input point `q'
// - returns `value_outside' if the spline is not defined at `q'
//
///////////////////////////////////////////////////////////////////////////////////////////////////

DTYPE eval_spline_6dir_FCC(TFCC &dataset, DTYPE q[3], DTYPE value_outside)
{
	DTYPE	x[3];

	//----------------------------------------------------------------
	// 0. Checks if the spline is defined at `q'.
	if( (q[0] <= dataset.range[0][0]) ||
		(q[1] <= dataset.range[1][0]) ||
		(q[2] <= dataset.range[2][0]) ||
		(q[0] >= dataset.range[0][1]) ||
		(q[1] >= dataset.range[1][1]) ||
		(q[2] >= dataset.range[2][1]))	
	{
		return value_outside;
	}

	//----------------------------------------------------------------
	// 1. Converts to the FCC coordinates by normalization & shift.
	//
	// In FCC coordinates, all the FCC lattice points are located at the non-negative
	// integer point where the sum of its elements is even.
	// Note that 2.0 is added since the size of the support of 
	// the 6-direction box spline is 4x4x4.
	for(int k=0 ; k < 3 ; k++)
		x[k] = (q[k] - dataset.range[k][0])*dataset.unit_inv[k] + 2.0;


	//----------------------------------------------------------------
	// 2. Computes the nearest FCC lattice point. (Refer to Algorithm 1.)
	//
	register int	x_dot[3];				// nearest FCC lattice point from `p'
	DTYPE			x_tilde[4] = {0,0,0,1};	// local coordinates of `p' w.r.t. `x_dot'
	register int	sigma[3] = {1,1,1};	

	for(int k=0 ; k < 3 ; k++)
	{
		x_dot[k] = (int)round(x[k]);
		x_tilde[k] = x[k] - DTYPE(x_dot[k]);
	}

	if((x_dot[0] + x_dot[1] + x_dot[2]) & 0x01)
	{
		// parity is odd...(not an FCC lattice point)
		DTYPE	cur_max = 0;
		int		idx_max;

		// finds the index `idx_max' for the maximum ||x_tilde[k]||_1
		idx_max = 0;
		cur_max = ABS(x_tilde[idx_max]);
		for(int k=1 ; k < 3 ; k++)
		{
			if(ABS(x_tilde[k]) > cur_max)
			{
				idx_max = k;
				cur_max = ABS(x_tilde[idx_max]);
			}
		}
		x_dot[idx_max] += (x_tilde[idx_max]>0?1:-1);

		// Re-computes `x_tilde'.
		x_tilde[idx_max] = x[idx_max] - DTYPE(x_dot[idx_max]);
	}

	// Now, the nearest FCC lattice from `p' is stored in `x_dot' and
	// the local coordinate of `p' w.r.t `x_dot' is stored in `x_tilde'.
	// (Refer to Algorithm 2.)
	
	//----------------------------------------------------------------
	// 3. Transforms the input to the local 1st octant coordinates to consider symmetry.
	//    Also computes the signs of `x_tilde' to be used to transform the `shifts' later.

	for(int k=0 ; k<3 ; k++)
	{
		if(x_tilde[k] < 0)				
		{								
			x_tilde[k] = -x_tilde[k];	
			sigma[k] = -1;				
		}
	}

	//----------------------------------------------------------------
	// 4. Determines the `type' of the polynomial piece 
	//    by the membership test against the plane x+y+z=1.
	//    (||x_tilde||_1 = x_tilde[0]+x_tilde[1]+x_tilde[2])
	int	type;
	if(x_tilde[0] + x_tilde[1] + x_tilde[2] < 1)	type = 0;
	else											type = 1;

	DTYPE	u[4];

	//----------------------------------------------------------------
	// 5. Computes the barycentric coordinates of x_tilde.
//	switch(type)
//	{
//		case 0:
//			u[0] = -x_tilde[0]-x_tilde[1]-x_tilde[2]+1;
//			u[1] = x_tilde[0];
//			u[2] = x_tilde[1];
//			u[3] = x_tilde[2];
//			break;
//		case 1:
//			u[0] = ( x_tilde[0] + x_tilde[1] + x_tilde[2] - 1)*0.5;
//			u[1] = (-x_tilde[0] + x_tilde[1] - x_tilde[2] + 1)*0.5;
//			u[2] = ( x_tilde[0] - x_tilde[1] - x_tilde[2] + 1)*0.5;
//			u[3] = (-x_tilde[0] - x_tilde[1] + x_tilde[2] + 1)*0.5;
//			break;
//	}

	MULT_MAT_VEC_4(inv_tets[type],x_tilde,u);
	//----------------------------------------------------------------
	// 6. Fetches local 16 spline coefficients around `x_dot'.
	

// Computes the j-th FCC coordinate of the i-th shift
// when added to the current FCC point considering symmetry.
#define	COMPUTE_IDX_FCC(i,j)								\
	icoeff[j] = x_dot[j] + sigma[j]*shifts[type][i][j];	\
	iFCC[j] = icoeff[j] >> 1;	// == icoeff[j]/2

#define	FETCH_COEFF(i)																\
	COMPUTE_IDX_FCC(i,0);															\
	COMPUTE_IDX_FCC(i,1);															\
	COMPUTE_IDX_FCC(i,2);															\
	iFCC[3] = map_index_FCC[icoeff[0] & 0x01][icoeff[1] & 0x01][icoeff[2] & 0x01];	\
	coeff[i] = GET_DATASET_FCC(dataset,iFCC[0],iFCC[1],iFCC[2],iFCC[3]);

	DTYPE			coeff[16];

	for(int i = 0 ; i < 16 ; i++)
	{
		int		iFCC[4];
		int		icoeff[3];
		FETCH_COEFF(i);
	}

	//----------------------------------------------------------------
	// 7. Constructs a polynomial piece in BB-form by adding 16 polynomial pieces
	//    weighted by 16 spline coefficients.
	DTYPE	c[4][4][4][4];
	DTYPE	*pc;

	for(int i = 0 ; i < 20 ; i++)
	{
		pc = &(c[multi_index[i][0]][multi_index[i][1]][multi_index[i][2]][multi_index[i][3]]);
		*pc = 0;
		for(int j = 0 ; j < 16 ; j++)
		{
			*pc += bb_coeff[type][j][i]*coeff[j];
		}
	}
	//----------------------------------------------------------------
	// 8. Evaluates the polynomial piece in BB-form by deCasteljau's algorhithm.
	int	d,i0,i1,i2,i3;
	for(d = 2 ; d >= 0 ; d -= 1)
	{
		for(i0 = 0 ; i0 <= d ; i0++)
		{
			for(i1 = 0 ; i1 <= (d-i0) ; i1++)
			{
				for(i2 = 0 ; i2 <= (d-i0-i1) ; i2++)
				{
					i3 = d-i0-i1-i2;
					c[i0][i1][i2][i3]=
						 u[0]*c[i0+1][i1]  [i2]  [i3]
						+u[1]*c[i0]  [i1+1][i2]  [i3]
						+u[2]*c[i0]  [i1]  [i2+1][i3]
						+u[3]*c[i0]  [i1]  [i2]  [i3+1];
				}
			}
		}
	}

	return	c[0][0][0][0]/24.0;
}