//test.cpp
#include "mex.h"
#include "matrix.h"//mxGetDimensionsで必要
#include<math.h>

void trilinear(short *cube,const int *cubesize,double *p,double *imgaddress);

void mexFunction(int nlhs,mxArray *plhs[],int nrhs, const mxArray *prhs[]){
/* V=hns_interp3(cube,kx,ky,kz,check)*/
	const int *cubesize;    // size of cube
	short     *cube,*V;     // 3D-array
	double    *kx,*ky,*kz;  // result of linspace(1,128,512)
	int   x,y,z,n;          // for loop
	int nx,ny,nz;           // number of kx,ky,kz
	int nn[3];
	double value,
		   p[3] ;           // to save calculation task
    cube=(short *)mxGetData(prhs[0]);
	cubesize=mxGetDimensions(prhs[0]);
	kx=mxGetPr(prhs[1]);    nx=mxGetNumberOfElements(prhs[1]);
	ky=mxGetPr(prhs[2]);    ny=mxGetNumberOfElements(prhs[2]);
	kz=mxGetPr(prhs[3]);    nz=mxGetNumberOfElements(prhs[3]);
	nn[0]=nx;
	nn[1]=ny;
	nn[2]=nz;
    plhs[0]=mxCreateNumericArray(3,nn,mxINT16_CLASS,mxREAL);
	V=(short *)mxGetData(plhs[0]);
	value=0;
    n=nx*ny;
    for(z=0;z<nz;++z){
		p[2]=*(kz+z);
        if(p[2]<1 || p[2]>*(cubesize+2)){
			for(y=0;y<ny;++y){
				for(x=0;x<nx;++x){
					*(V+x+y*nx+z*n)=0;
				}
			}
		}
		else{
			for(y=0;y<ny;++y){
				p[1]=*(ky+y);
				if(p[1]<1 || p[1]>*(cubesize+1)){
					for(x=0;x<nx;++x){
						*(V+x+y*nx+z*n)=0;
					}
				}
				else{
					for(x=0;x<nx;++x){
						p[0]=*(kx+x);
						if(p[0]<1 || p[0]>*cubesize){
							*(V+x+y*nx+z*n)=0;
						}
						else{
							trilinear(cube,cubesize,p,&value);
							*(V+x+y*nx+z*n)=(short)value;
						}
					}
				}
			}
		}
	}
}

void trilinear(short *cube,const int *cubesize,double *p,double *imgaddress){
	int i,j=1,z=0,xy,n;
	double dx,dy,dz,xd,yd,zd;
	int pint[3];
	double V[8];
	*pint=(int)floor(*p);
	*(pint+1)=(int)floor(*(p+1));
	*(pint+2)=(int)floor(*(p+2));	
    if(pint[0]>*cubesize){pint[0]=*cubesize;}
	if(pint[1]>*(cubesize+1)){pint[1]=*(cubesize+1);}
	if(pint[2]>*(cubesize+2)){pint[2]=*(cubesize+2);}
	dx=*p-(double)*pint;
	dy=*(p+1)-(double)*(pint+1);
	dz=*(p+2)-(double)*(pint+2);
	xd=1-dx;yd=1-dy;zd=1-dz;
	xy=(*(cubesize))*(*(cubesize+1));
	for(i=0;i<3;++i){z+=(*(pint+i)-1)*j;j=j*(*(cubesize+i));}
    if(pint[0]<*cubesize){
		if(pint[1]<*(cubesize+1)){
			if(pint[2]<*(cubesize+2)){
                V[0]=xd*yd*zd*(double)(*(cube+z));
                V[1]=dx*yd*zd*(double)(*(cube+z+1));//x方向
	            V[2]=xd*dy*zd*(double)(*(cube+z+(*cubesize)));//y方向
	            V[3]=dx*dy*zd*(double)(*(cube+z+(*cubesize)+1));//xとy方向
	            V[4]=xd*yd*dz*(double)(*(cube+z+xy));//z方向
	            V[5]=dx*yd*dz*(double)(*(cube+z+xy+1));//xとz方向
	            V[6]=xd*dy*dz*(double)(*(cube+z+xy+(*cubesize)));//yとz方向
	            V[7]=dx*dy*dz*(double)(*(cube+z+xy+(*cubesize)+1));//xとyとz方向
	            *imgaddress=V[0]+V[1]+V[2]+V[3]+V[4]+V[5]+V[6]+V[7];
			}else{ //z方向
                V[0]=xd*yd*(double)(*(cube+z));
                V[1]=dx*yd*(double)(*(cube+z+1));//x方向
	            V[2]=xd*dy*(double)(*(cube+z+(*cubesize)));//y方向
	            V[3]=dx*dy*(double)(*(cube+z+(*cubesize)+1));//xとy方向
				*imgaddress=V[0]+V[1]+V[2]+V[3];
			}
		}else{
			if(pint[2]<*(cubesize+2)){//y方向
                V[0]=xd*zd*(double)(*(cube+z));
                V[1]=dx*zd*(double)(*(cube+z+1));//x方向
	            V[4]=xd*dz*(double)(*(cube+z+xy));//z方向
	            V[5]=dx*dz*(double)(*(cube+z+xy+1));//xとz方向
	            *imgaddress=V[0]+V[1]+V[4]+V[5];
			}else{ //yとz方向
				V[0]=xd*(double)(*(cube+z));
                V[1]=dx*(double)(*(cube+z+1));//x方向
				*imgaddress=V[0]+V[1];
			}
		}
	}else{
		if(pint[1]<*(cubesize+1)){
			if(pint[2]<*(cubesize+2)){//x方向
                V[0]=yd*zd*(double)(*(cube+z));
	            V[2]=dy*zd*(double)(*(cube+z+(*cubesize)));//y方向
	            V[4]=yd*dz*(double)(*(cube+z+xy));//z方向
	            V[6]=dy*dz*(double)(*(cube+z+xy+(*cubesize)));//yとz方向
	            *imgaddress=V[0]+V[2]+V[4]+V[6];
			}else{  // xとz方向
                V[0]=yd*(double)(*(cube+z));
	            V[2]=dy*(double)(*(cube+z+(*cubesize)));//y方向
	            *imgaddress=V[0]+V[2];	
			}
		}else{
			if(pint[2]<*(cubesize+2)){  //xとy方向
                V[0]=zd*(double)(*(cube+z));
	            V[4]=dz*(double)(*(cube+z+xy));//z方向
	            *imgaddress=V[0]+V[4];	
			}else{
				*imgaddress=(double)*(cube+z);
			}
		}
	}
}