/***********************************************************************



	File:	volumeRigidWarp.c

	A mex file for Matlab (c)

	The calling syntax is:

			

			  warped_vol = volumeRigidWarp(vol, TParams)

	

	 vol = 3D volume -- fixed volume

	 

	 TParams = 1 x 6 -- [tx, ty, tz, theta, omega, phi]



	 warped_vol = 3D volume - same size of fixed volume

		

	Date:	November 2005



	Copyright (c) 2005 by Mert Rory Sabuncu





************************************************************************/



#include <math.h>

#include <stddef.h>

#include <stdio.h>

#include <stdlib.h>

#include "mex.h"



#define NOT	!

#define AND	&&

#define OR	||

#define EQ	==

#define NE	!=

#define MIN_DISTANCE	1e-5



#define VERBOSE



static float linearInterpolateVolume(const float * volume, const int * size_in, float x, float y, float z);



static float interpAux(const float x, const float y, const float alpha); 



/*

 * The main routine.  Read in graph, compute derivatives, output it.

 */



void

mexFunction(

			int nlhs, 

			mxArray *plhs[], 

			int nrhs, 

			const mxArray *prhs[])

{



	//Declarations

	

	

	const int *		VolSize			=	mxGetDimensions(prhs[0]);

	const int		numOfTParams	=   mxGetN(prhs[1]);

	

	const float *  Volume		= (float *)   mxGetPr(prhs[0]);

	const double *  TParams		=  mxGetPr(prhs[1]);

	float *		output_volume;

	

	

	int		sizeX = VolSize[0];

	int		sizeY = VolSize[1];

	int		sizeZ = VolSize[2];

	int		i,j,k;



	double tx, ty, tz, theta, phi, omega;

	int xxx, yyy, zzz;

	float xxx1, zzz1, yyy1,xxx2, zzz2, yyy2,xxx3, zzz3, yyy3, x_new, y_new, z_new;

	float ct, co, cp, st, sp, so;

	float centerX, centerY, centerZ;

		

	tx		= TParams[0];

	ty		= TParams[1];

	tz		= TParams[2];

	theta	= TParams[3];

	phi		= TParams[4];

	omega	= TParams[5];
  centerX = sizeX/2.0f;
	centerY = sizeY/2.0f;
	centerZ = sizeZ/2.0f;
	
  ct = (float) cos(theta/180.0*3.14);
  st = (float) sin(theta/180.0*3.14);
  cp = (float) cos(phi/180.0*3.14);
  sp = (float) sin(phi/180.0*3.14);         
  co = (float) cos(omega/180.0*3.14);
  so = (float) sin(omega/180.0*3.14);


	plhs[0] = mxCreateNumericArray(mxGetNumberOfDimensions(prhs[0]), mxGetDimensions(prhs[0]),mxSINGLE_CLASS, (mxIsComplex(prhs[0]) ? mxCOMPLEX : mxREAL));
	output_volume = mxGetPr(plhs[0]);

	for (xxx = 0; xxx< sizeX; xxx++){
  	for (yyy = 0; yyy < sizeY; yyy++){
			for (zzz = 0; zzz < sizeZ; zzz++){
				/* map point to new point*/
      xxx1 = (xxx - centerX);
		  yyy1 = -so * (zzz - centerZ) + co * (yyy - centerY);
		  zzz1 = co * (zzz - centerZ) + so * (yyy- centerY);

      zzz2 = ct * zzz1 + st * xxx1;
		  yyy2 = yyy1;
		  xxx2 = -st * zzz1 + ct * xxx1;

		  zzz3 = zzz2;
		  yyy3 = cp * yyy2 + sp * xxx2;
		  xxx3 = -sp * yyy2 + cp * xxx2;

      x_new = xxx3 + centerX + tx;
		  y_new = yyy3 + centerY + ty;
		  z_new = zzz3 + centerZ + tz;
			output_volume[zzz*sizeY*sizeX+yyy*sizeX+xxx] = linearInterpolateVolume(Volume, VolSize, x_new,y_new,z_new);



			}

		}

	};



	return;

};



static float linearInterpolateVolume(const float * volume, const int * size_in, float x, float y, float z){



	int	size_xL = size_in[0];

	int size_yL = size_in[1];

	int size_zL = size_in[2];

	const float * dataL = volume;
  float tol = 0.001;

  int x0 = (int)(floor(x));
  int y0 = (int)(floor(y));
  int z0 = (int)(floor(z));
	

  const float dx = x - x0;
	const float dy = y - y0;
	const float dz = z - z0;
  
  float dxx, dyy, dzz;
  int x00, y00, z00, xr, yr, zr;
  float v0, v1, v2, v3, v4, v5, v6, v7;

	float temp0;

	float temp1;

	float temp2;

	float temp3;

	float temp4;

	float temp5;

	int x1, y1, z1;


	if ((x0 < 0) || (y0 < 0) || (z0 < 0) || (z0 >= (size_zL - 1)) || (y0 >= (size_yL - 1)) || (x0 >= (size_xL - 1))) {

     return (0.0);

	}; 

  x00 = (int)(ceil(x));
  y00 = (int)(ceil(y));
  z00 = (int)(ceil(z));

  dxx = x00 - x;
	dyy = y00 - y;
	dzz = z00 - z;


  xr = dxx < dx ? x00:x0;
  yr = dyy < dy ? y00:y0;
  zr = dzz < dz ? z00:z0;
  
	if ((fabs(x - xr) < tol) && (fabs(y - yr) < tol) && (fabs(z - zr) < tol))

		return (dataL[zr*size_xL*size_yL+yr*size_xL+xr]); 

	

	else {
		v0 = dataL[z0*size_xL*size_yL+y0*size_xL+x0];

		z0++;

		v1 = dataL[z0*size_xL*size_yL+y0*size_xL+x0];

		y0++;

		v2 = dataL[z0*size_xL*size_yL+y0*size_xL+x0];

		z0--;

		v3 = dataL[z0*size_xL*size_yL+y0*size_xL+x0];

		x0++;

		y0--;

		v4 = dataL[z0*size_xL*size_yL+y0*size_xL+x0];

		z0++;

		v5 = dataL[z0*size_xL*size_yL+y0*size_xL+x0];

		y0++;

		v6 = dataL[z0*size_xL*size_yL+y0*size_xL+x0];

		z0--;

		v7 = dataL[z0*size_xL*size_yL+y0*size_xL+x0];



	}

  temp0 = interpAux(v0,v1,dz);

	temp1 = interpAux(v3,v2,dz);

	temp2 = interpAux(v4,v5,dz);

	temp3 = interpAux(v7,v6,dz);

	temp4 = interpAux(temp0,temp1,dy);

	temp5 = interpAux(temp2,temp3,dy);

	return (interpAux(temp4,temp5,dx));

};



static float interpAux(const float x, const float y, const float alpha)

{      

	return (x + alpha * (y - x)); 

};