 #include "float.h"
 
 
-// Converts a double to a float and stores the result back in the dst pointer.
+// Converts a double to a float and stores the result back in the dest pointer.
 // The float is downcast, discarding the least significant bits in the double.
 //
 // This is useful because CUDA prefers floats for its computations, while
 // Matlab defaults to doubles.
-void double_to_float(float* dst, double* src, const int n) {
+void double_to_float(float* dest, double* source, const int n) {
 	for (int i = 0; i < n; ++i)
-		dst[i] = (float) src[i];
-}
-
+		dest[i] = (float) source[i];
+}
 
 
 // Convert an unsigned char (or a uint8 in Matlab's type name scheme) to a
-// float and store the result to dst. The char is converted to the nearest
+// float and store the result to dest. The char is converted to the nearest
 // float. For example, 8 becomes 8.0. 
-void uint8_to_float(float* dst, unsigned char* src, const int n) {
+void uint8_to_float(float* dest, unsigned char* source, const int n) {
 	for (int i = 0; i < n; ++i)
-		dst[i] = (float) src[i];
-}
-
-
+		dest[i] = (float) source[i];
+}
 
 
 // Performs integer division, but rounds up if the division is not exact.


// Returns the magnitude of the complex-valued float, a.
// This device kernel only runs on a CUDA device.
__device__ float complexMagnitude(float2 a)
{
	return sqrtf( a.x*a.x + a.y*a.y );
}



	//minimum intensity
	for (int i=0; i<nz; i++){
		//copy the fft spectrum to a variable that's temporary for each plane
		cudaMemcpy(reconstruction_gpu, chologram_gpu, sizeof(float2)*M*N, cudaMemcpyDeviceToDevice);

		//set the propagation parameters
		setPropagationParams( (float)(z[i]), (float)wavelength, (float)pixel_size, M, N, 
			&adudu, &cutoff2, &offsetPhaseExp, &fftscale, false);

		//apply the propagation kernel (full... could speed this up by ignoring cutoff)
		fresnelKernel<<<mygrid, myblock>>>(reconstruction_gpu, M, N, adudu, 
			cutoff2, offsetPhaseExp, fftscale);

		//take the inverse transform: get an image at the plane.
		cufftExecC2C(plan, reconstruction_gpu, reconstruction_gpu, CUFFT_INVERSE);

		//compute the magnitude
		computeMagKernel<<<mygrid, myblock>>>(reconstruction_gpu, M, N);

		//compute the steerable gradient responses in the x and y directions
		cudaMemcpy( sx, reconstruction_gpu, sizeof(float2) * M * N, cudaMemcpyDeviceToDevice); //copy the magnitude info
		cufftExecC2C(plan, sx, sx, CUFFT_FORWARD); //the spectrum of the magnitude
		cudaMemcpy( sy, sx, sizeof(float2) * M * N, cudaMemcpyDeviceToDevice); //copy the spectrum information
		//apply the sfilt kernels in the x and y directions
		ComplexMultiplicationKernel<<<mygrid, myblock>>>(sx, sx, gradient_filter_in_x, M, N);
		ComplexMultiplicationKernel<<<mygrid, myblock>>>(sy, sy, gradient_filter_in_y, M, N);
		cufftExecC2C(plan, sx, sx, CUFFT_INVERSE);
		cufftExecC2C(plan, sy, sy, CUFFT_INVERSE);
		//finally: compute the magnitude and orientation
		GradientMagnitudeKernel<<<mygrid,myblock>>>(gradient_magnitude_gpu, sx, sy, M, N);
		scaleFloatKernel<<<mygrid,myblock>>>(gradient_magnitude_gpu, 1.0f / ((float) (M * M) * (float) (N * N)), M, N);

		//find the local min intensity (for focus_metric)
		//localMinKernel<<<mygrid, myblock>>>(image_magnitude, reconstruction_gpu, M, N);
		copyRealKernel<<<mygrid, myblock>>>(image_magnitude, reconstruction_gpu, M, N);


		//compute the SIM metric
		computeSIMkernel<<<mygrid, myblock>>>(sim, gradient_magnitude_gpu, image_magnitude, maximum_intensity, M, N);
		

		if (rintmode){
			//compare the current intensity to the previous minimum
			if (i == 0) 
				copyRealKernel<<<mygrid,myblock>>>(min_intensity_gpu, reconstruction_gpu, M, N);
			else 
				compareReal2Kernel<<<mygrid, myblock>>>(min_intensity_gpu, index_of_min_intensity_gpu, reconstruction_gpu, i, M, N);
		}

		//compare the current SIM to the previous SIMs
		if (i == 0) {
			cudaMemcpy(simmaxdev, sim, sizeof(float)*M*N, cudaMemcpyDeviceToDevice);
			if (record_intensity_at_best_focus)
				cudaMemcpy(intensity_at_best_focus_gpu, min_intensity_gpu, sizeof(float)*M*N, cudaMemcpyDeviceToDevice);
		}
		else {
			if (!record_intensity_at_best_focus)
				compareRealKernel<<<mygrid, myblock>>>(
                    simmaxdev, 
                    index_of_max_focus_metric_gpu,
                    sim,
                    i, 
                    M, N);
			else //record_intensity_at_best_focus == true
				compareRealKernel<<<mygrid, myblock>>>(
                    simmaxdev,
                    index_of_max_focus_metric_gpu, 
                    sim, 
                    intensity_at_best_focus_gpu, 
                    reconstruction_gpu, 
                    i, 
                    M, N);
		}


	}

	// C-style indexing uses 0 as its first element, while Matlab uses 1 for its
    // first element. Add a 1 to all the slice indices recorded so that the
    // returned indices are directly usable in Matlab.
	if (rintmode)
		AddShortsKernel<<<mygrid, myblock>>>(index_of_min_intensity_gpu, 1, M, N);
	AddShortsKernel<<<mygrid, myblock>>>(
        index_of_max_focus_metric_gpu, 1, M, N);

    // Copy results from the GPU back to the CPU memory.
	mexPrintf("Copying results back to host\n");
	cudaMemcpy(focus_metric, simmaxdev, sizeof(float) * M * N, cudaMemcpyDeviceToHost);
	cudaMemcpy(index_of_max_focus_metric, index_of_max_focus_metric_gpu, sizeof(short) * M * N, cudaMemcpyDeviceToHost);
	if (rintmode) {
		cudaMemcpy(min_intensity_cpu, min_intensity_gpu, sizeof(float) * M * N, cudaMemcpyDeviceToHost);
		cudaMemcpy(index_of_min_intensity, index_of_min_intensity_gpu, sizeof(short) * M * N, cudaMemcpyDeviceToHost);
	}
	if (record_intensity_at_best_focus)
		cudaMemcpy(intensity_at_best_focus_cpu, intensity_at_best_focus_gpu, sizeof(float) * M * N, cudaMemcpyDeviceToHost);

	mexPrintf("Cleaning up memory and plans\n");

	//clean up!
	cufftDestroy(plan);
	cudaFree(chologram_gpu);
	cudaFree(reconstruction_gpu);
	if (rintmode){
		cudaFree(min_intensity_gpu);
		cudaFree(index_of_min_intensity_gpu);
	}
	cudaFree(gradient_filter_in_x);
	cudaFree(gradient_filter_in_y);
	cudaFree(sx);
	cudaFree(sy);
	cudaFree(gradient_magnitude_gpu);
	cudaFree(sim);
	cudaFree(image_magnitude);
	cudaFree(index_of_max_focus_metric_gpu);
	cudaFree(simmaxdev);
	if (record_intensity_at_best_focus)
		cudaFree(intensity_at_best_focus_gpu);
}



// [focus_metric, index_of_max_focus_metric, min_intensity_cpu, index_of_min_intensity, R_at_SIM] = maxSIMfm(img, z, wavelength, pixel_size, ...
//           powerKernOrder, powerKernOffset, sigma, maximum_intensity);
void mexFunction( int nlhs, mxArray* plhs[],
                  int nrhs, const mxArray* prhs[]) {

    // Pointer for the hologram data.
	float* hologram_cpu;
    // Minimum reconstruction intensity.
    float* min_intensity_cpu;
    // Stores a metric for how well each hologram pixel can be focused.
    float* focus_metric;
    float* intensity_at_best_focus_cpu = NULL;
    // Pointer for source hologram data, if it is passed in as a double.
	double* dsrc;
    // Pointer for source hologram data, if it is passed in as an unsigned char.
	unsigned char* isrc;
	short* index_of_min_intensity;
    short* index_of_max_focus_metric;
	double* z;
    // Wavelength of the illumination light in meters.
    double wavelength = 658e-9;
    // Size of a camera pixel in meters.
    double pixel_size = 9e-6;
	int M, N, nz;
	float power_filter_order = -1, power_filter_offset = .001, sigma = 2, maximum_intensity = 1000;
	bool rintmode, record_intensity_at_best_focus;

	if (nrhs<2)
		mexErrMsgTxt("maxSIMfm(image, z, [wavelength], [pixel_size]) requires at least two arguments.");

	//get the size of the hologram
	M = mxGetN(prhs[0]); //number of y-direction pixels
