void ReadBinaryFile(float* pointer, int size, const char* filename, void** pointers, int& Npointers, nifti_image** niftiImages, int Nimages)
{
	if (pointer == NULL)
    {
        printf("The provided pointer for file %s is NULL, aborting! \n",filename);
        FreeAllMemory(pointers,Npointers);
		FreeAllNiftiImages(niftiImages,Nimages);
        exit(EXIT_FAILURE);
	}	

	FILE *fp = NULL; 
	fp = fopen(filename,"rb");

    if (fp != NULL)
    {
        fread(pointer,sizeof(float),size,fp);
        fclose(fp);
    }
    else
    {
        printf("Could not open %s , aborting! \n",filename);
        FreeAllMemory(pointers,Npointers);
		FreeAllNiftiImages(niftiImages,Nimages);
        exit(EXIT_FAILURE);
    }
}
void AllocateMemory(float *& pointer, int size, void** pointers, int& Npointers, nifti_image** niftiImages, int Nimages, const char* variable)
{
    pointer = (float*)malloc(size);
    if (pointer != NULL)
    {
        pointers[Npointers] = (void*)pointer;
        Npointers++;
    }
    else
    {
        printf("Could not allocate host memory for variable %s ! \n",variable);        
		FreeAllMemory(pointers, Npointers);
		FreeAllNiftiImages(niftiImages, Nimages);
		exit(EXIT_FAILURE);        
    }
}
Exemple #3
0
int main(int argc, char ** argv)
{
    //-----------------------
    // Input pointers

    float           *h_fMRI_Volumes = NULL;
    float           *h_Certainty = NULL;

    //--------------

    void*           allMemoryPointers[500];
    for (int i = 0; i < 500; i++)
    {
        allMemoryPointers[i] = NULL;
    }

    nifti_image*	allNiftiImages[500];
    for (int i = 0; i < 500; i++)
    {
        allNiftiImages[i] = NULL;
    }

    int             numberOfMemoryPointers = 0;
    int				numberOfNiftiImages = 0;

    size_t			allocatedHostMemory = 0;

    //--------------

    // Default parameters
    int             OPENCL_PLATFORM = 0;
    int             OPENCL_DEVICE = 0;
    bool            DEBUG = false;
    const char*     FILENAME_EXTENSION = "_sm";
    bool            PRINT = true;
    bool			VERBOS = false;

    size_t          DATA_W, DATA_H, DATA_D, DATA_T;
    float           EPI_VOXEL_SIZE_X, EPI_VOXEL_SIZE_Y, EPI_VOXEL_SIZE_Z;

    bool			CHANGE_OUTPUT_FILENAME = false;

    // Settings

    float           EPI_SMOOTHING_AMOUNT = 6.0f;
    bool			MASK = false;
    bool			AUTO_MASK = false;
    const char*		MASK_NAME;

    //-----------------------
    // Output parameters

    const char      *outputFilename;

    //---------------------

    /* Input arguments */
    FILE *fp = NULL;

    // No inputs, so print help text
    if (argc == 1)
    {
        printf("Usage:\n\n");
        printf("Smoothing input.nii [options]\n\n");
        printf("Options:\n\n");
        printf(" -platform        The OpenCL platform to use (default 0) \n");
        printf(" -device          The OpenCL device to use for the specificed platform (default 0) \n");
        printf(" -fwhm            Amount of smoothing to apply (in mm, default 6 mm) \n");
        printf(" -mask            Perform smoothing inside mask (normalized convolution) \n");
        printf(" -automask        Generate a mask and perform smoothing inside mask (normalized convolution) \n");
        printf(" -output          Set output filename (default input_sm.nii) \n");
        printf(" -quiet           Don't print anything to the terminal (default false) \n");
        printf(" -verbose         Print extra stuff (default false) \n");
        printf("\n\n");

        return EXIT_SUCCESS;
    }
    // Try to open file
    else if (argc > 1)
    {
        fp = fopen(argv[1],"r");
        if (fp == NULL)
        {
            printf("Could not open file %s !\n",argv[1]);
            return EXIT_FAILURE;
        }
        fclose(fp);
    }

    // Loop over additional inputs
    int i = 2;
    while (i < argc)
    {
        char *input = argv[i];
        char *p;
        if (strcmp(input,"-platform") == 0)
        {
            if ( (i+1) >= argc  )
            {
                printf("Unable to read value after -platform !\n");
                return EXIT_FAILURE;
            }

            OPENCL_PLATFORM = (int)strtol(argv[i+1], &p, 10);

            if (!isspace(*p) && *p != 0)
            {
                printf("OpenCL platform must be an integer! You provided %s \n",argv[i+1]);
                return EXIT_FAILURE;
            }
            else if (OPENCL_PLATFORM < 0)
            {
                printf("OpenCL platform must be >= 0!\n");
                return EXIT_FAILURE;
            }
            i += 2;
        }
        else if (strcmp(input,"-device") == 0)
        {
            if ( (i+1) >= argc  )
            {
                printf("Unable to read value after -device !\n");
                return EXIT_FAILURE;
            }

            OPENCL_DEVICE = (int)strtol(argv[i+1], &p, 10);

            if (!isspace(*p) && *p != 0)
            {
                printf("OpenCL device must be an integer! You provided %s \n",argv[i+1]);
                return EXIT_FAILURE;
            }
            else if (OPENCL_DEVICE < 0)
            {
                printf("OpenCL device must be >= 0!\n");
                return EXIT_FAILURE;
            }
            i += 2;
        }
        else if (strcmp(input,"-fwhm") == 0)
        {
            if ( (i+1) >= argc  )
            {
                printf("Unable to read value after -fwhm !\n");
                return EXIT_FAILURE;
            }

            EPI_SMOOTHING_AMOUNT = (float)strtod(argv[i+1], &p);

            if (!isspace(*p) && *p != 0)
            {
                printf("Smoothing must be a float! You provided %s \n",argv[i+1]);
                return EXIT_FAILURE;
            }
            else if ( EPI_SMOOTHING_AMOUNT <= 0.0f )
            {
                printf("Smoothing must be > 0.0 !\n");
                return EXIT_FAILURE;
            }
            i += 2;
        }
        else if (strcmp(input,"-mask") == 0)
        {
            if ( (i+1) >= argc  )
            {
                printf("Unable to read name after -mask !\n");
                return EXIT_FAILURE;
            }

            MASK = true;
            MASK_NAME = argv[i+1];
            i += 2;
        }
        else if (strcmp(input,"-automask") == 0)
        {
            AUTO_MASK = true;
            i += 1;
        }
        else if (strcmp(input,"-debug") == 0)
        {
            DEBUG = true;
            i += 1;
        }
        else if (strcmp(input,"-quiet") == 0)
        {
            PRINT = false;
            i += 1;
        }
        else if (strcmp(input,"-verbose") == 0)
        {
            VERBOS = true;
            i += 1;
        }
        else if (strcmp(input,"-output") == 0)
        {
            CHANGE_OUTPUT_FILENAME = true;

            if ( (i+1) >= argc  )
            {
                printf("Unable to read name after -output !\n");
                return EXIT_FAILURE;
            }

            outputFilename = argv[i+1];
            i += 2;
        }
        else
        {
            printf("Unrecognized option! %s \n",argv[i]);
            return EXIT_FAILURE;
        }
    }

    // Check if BROCCOLI_DIR variable is set
    if (getenv("BROCCOLI_DIR") == NULL)
    {
        printf("The environment variable BROCCOLI_DIR is not set!\n");
        return EXIT_FAILURE;
    }

    double startTime = GetWallTime();

    // ---------------------
    // Read data
    // ---------------------
    nifti_image *inputData = nifti_image_read(argv[1],1);

    if (inputData == NULL)
    {
        printf("Could not open nifti file!\n");
        return EXIT_FAILURE;
    }
    allNiftiImages[numberOfNiftiImages] = inputData;
    numberOfNiftiImages++;

    // -----------------------
    // Read mask
    // -----------------------

    nifti_image *inputMask;
    if (MASK)
    {
        inputMask = nifti_image_read(MASK_NAME,1);
        if (inputMask == NULL)
        {
            printf("Could not open mask volume!\n");
            FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
            return EXIT_FAILURE;
        }
        allNiftiImages[numberOfNiftiImages] = inputMask;
        numberOfNiftiImages++;
    }

    double endTime = GetWallTime();

    if (VERBOS)
    {
        printf("It took %f seconds to read the nifti file\n",(float)(endTime - startTime));
    }

    // Get data dimensions
    DATA_W = inputData->nx;
    DATA_H = inputData->ny;
    DATA_D = inputData->nz;
    DATA_T = inputData->nt;

    // Check if mask volume has the same dimensions as the data
    if (MASK)
    {
        size_t TEMP_DATA_W = inputMask->nx;
        size_t TEMP_DATA_H = inputMask->ny;
        size_t TEMP_DATA_D = inputMask->nz;

        if ( (TEMP_DATA_W != DATA_W) || (TEMP_DATA_H != DATA_H) || (TEMP_DATA_D != DATA_D) )
        {
            printf("Input data has the dimensions %zu x %zu x %zu, while the mask volume has the dimensions %zu x %zu x %zu. Aborting! \n",DATA_W,DATA_H,DATA_D,TEMP_DATA_W,TEMP_DATA_H,TEMP_DATA_D);
            FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
            return EXIT_FAILURE;
        }
    }

    // Get voxel sizes
    EPI_VOXEL_SIZE_X = inputData->dx;
    EPI_VOXEL_SIZE_Y = inputData->dy;
    EPI_VOXEL_SIZE_Z = inputData->dz;

    // Calculate size, in bytes
    size_t DATA_SIZE = DATA_W * DATA_H * DATA_D * DATA_T * sizeof(float);
    size_t VOLUME_SIZE = DATA_W * DATA_H * DATA_D * sizeof(float);

    // Print some info
    if (PRINT)
    {
        printf("Authored by K.A. Eklund \n");
        printf("Data size: %zu x %zu x %zu x %zu \n",  DATA_W, DATA_H, DATA_D, DATA_T);
        printf("Voxel size: %f x %f x %f mm \n", EPI_VOXEL_SIZE_X, EPI_VOXEL_SIZE_Y, EPI_VOXEL_SIZE_Z);
        printf("Smoothing filter size: %f mm \n", EPI_SMOOTHING_AMOUNT);
    }

    // ------------------------------------------------

    // Allocate memory on the host

    startTime = GetWallTime();

    // If the data is in float format, we can just copy the pointer
    if ( inputData->datatype != DT_FLOAT )
    {
        AllocateMemory(h_fMRI_Volumes, DATA_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, allocatedHostMemory, "INPUT_DATA");
    }
    else
    {
        allocatedHostMemory += DATA_SIZE;
    }
    AllocateMemory(h_Certainty, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, allocatedHostMemory, "CERTAINTY");

    endTime = GetWallTime();

    if (VERBOS)
    {
        printf("It took %f seconds to allocate memory\n",(float)(endTime - startTime));
    }

    startTime = GetWallTime();

    // Convert data to floats
    if ( inputData->datatype == DT_SIGNED_SHORT )
    {
        short int *p = (short int*)inputData->data;

        for (size_t i = 0; i < DATA_W * DATA_H * DATA_D * DATA_T; i++)
        {
            h_fMRI_Volumes[i] = (float)p[i];
        }
    }
    else if ( inputData->datatype == DT_UINT8 )
    {
        unsigned char *p = (unsigned char*)inputData->data;

        for (size_t i = 0; i < DATA_W * DATA_H * DATA_D * DATA_T; i++)
        {
            h_fMRI_Volumes[i] = (float)p[i];
        }
    }
    else if ( inputData->datatype == DT_UINT16 )
    {
        unsigned short int *p = (unsigned short int*)inputData->data;

        for (size_t i = 0; i < DATA_W * DATA_H * DATA_D * DATA_T; i++)
        {
            h_fMRI_Volumes[i] = (float)p[i];
        }
    }
    // Correct data type, just copy the pointer
    else if ( inputData->datatype == DT_FLOAT )
    {
        h_fMRI_Volumes = (float*)inputData->data;

        // Save the pointer in the pointer list
        allMemoryPointers[numberOfMemoryPointers] = (void*)h_fMRI_Volumes;
        numberOfMemoryPointers++;

        //float *p = (float*)inputData->data;

        //for (size_t i = 0; i < DATA_W * DATA_H * DATA_D * DATA_T; i++)
        //{
        //    h_fMRI_Volumes[i] = p[i];
        //}
    }
    else
    {
        printf("Unknown data type in input data, aborting!\n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
        return EXIT_FAILURE;
    }

    // Free input fMRI data, it has been converted to floats
    if ( inputData->datatype != DT_FLOAT )
    {
        free(inputData->data);
        inputData->data = NULL;
    }
    // Pointer has been copied to h_fMRI_Volumes and pointer list, so set the input data pointer to NULL
    else
    {
        inputData->data = NULL;
    }

    // Mask is provided by user
    if (MASK)
    {
        if ( inputMask->datatype == DT_SIGNED_SHORT )
        {
            short int *p = (short int*)inputMask->data;

            for (size_t i = 0; i < DATA_W * DATA_H * DATA_D; i++)
            {
                h_Certainty[i] = (float)p[i];
            }
        }
        else if ( inputMask->datatype == DT_UINT16 )
        {
            unsigned short int *p = (unsigned short int*)inputMask->data;

            for (size_t i = 0; i < DATA_W * DATA_H * DATA_D; i++)
            {
                h_Certainty[i] = (float)p[i];
            }
        }
        else if ( inputMask->datatype == DT_FLOAT )
        {
            float *p = (float*)inputMask->data;

            for (size_t i = 0; i < DATA_W * DATA_H * DATA_D; i++)
            {
                h_Certainty[i] = p[i];
            }
        }
        else if ( inputMask->datatype == DT_UINT8 )
        {
            unsigned char *p = (unsigned char*)inputMask->data;

            for (size_t i = 0; i < DATA_W * DATA_H * DATA_D; i++)
            {
                h_Certainty[i] = (float)p[i];
            }
        }
        else
        {
            printf("Unknown data type in mask volume, aborting!\n");
            FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
            FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
            return EXIT_FAILURE;
        }
    }
    // Mask is NOT provided by user, set all mask voxels to 1
    else
    {
        for (size_t i = 0; i < DATA_W * DATA_H * DATA_D; i++)
        {
            h_Certainty[i] = 1.0f;
        }
    }

    endTime = GetWallTime();

    if (VERBOS)
    {
        printf("It took %f seconds to convert data to floats\n",(float)(endTime - startTime));
    }

    //------------------------

    startTime = GetWallTime();

    // Initialize BROCCOLI
    BROCCOLI_LIB BROCCOLI(OPENCL_PLATFORM,OPENCL_DEVICE,2,VERBOS); // 2 = Bash wrapper

    endTime = GetWallTime();

    if (VERBOS)
    {
        printf("It took %f seconds to initiate BROCCOLI\n",(float)(endTime - startTime));
    }

    // Print build info to file (always)
    std::vector<std::string> buildInfo = BROCCOLI.GetOpenCLBuildInfo();
    std::vector<std::string> kernelFileNames = BROCCOLI.GetKernelFileNames();

    std::string buildInfoPath;
    buildInfoPath.append(getenv("BROCCOLI_DIR"));
    buildInfoPath.append("compiled/Kernels/");

    for (int k = 0; k < BROCCOLI.GetNumberOfKernelFiles(); k++)
    {
        std::string temp = buildInfoPath;
        temp.append("buildInfo_");
        temp.append(BROCCOLI.GetOpenCLPlatformName());
        temp.append("_");
        temp.append(BROCCOLI.GetOpenCLDeviceName());
        temp.append("_");
        std::string name = kernelFileNames[k];
        // Remove "kernel" and ".cpp" from kernel filename
        name = name.substr(0,name.size()-4);
        name = name.substr(6,name.size());
        temp.append(name);
        temp.append(".txt");
        fp = fopen(temp.c_str(),"w");
        if (fp == NULL)
        {
            printf("Could not open %s for writing ! \n",temp.c_str());
        }
        else
        {
            if (buildInfo[k].c_str() != NULL)
            {
                int error = fputs(buildInfo[k].c_str(),fp);
                if (error == EOF)
                {
                    printf("Could not write to %s ! \n",temp.c_str());
                }
            }
            fclose(fp);
        }
    }

    // Something went wrong...
    if (!BROCCOLI.GetOpenCLInitiated())
    {
        printf("Initialization error is \"%s\" \n",BROCCOLI.GetOpenCLInitializationError().c_str());
        printf("OpenCL error is \"%s\" \n",BROCCOLI.GetOpenCLError());

        // Print create kernel errors
        int* createKernelErrors = BROCCOLI.GetOpenCLCreateKernelErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (createKernelErrors[i] != 0)
            {
                printf("Create kernel error for kernel '%s' is '%s' \n",BROCCOLI.GetOpenCLKernelName(i),BROCCOLI.GetOpenCLErrorMessage(createKernelErrors[i]));
            }
        }

        printf("OpenCL initialization failed, aborting! \nSee buildInfo* for output of OpenCL compilation!\n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
        return EXIT_FAILURE;
    }
    // Initialization OK
    else
    {
        // Set all necessary pointers and values
        BROCCOLI.SetInputfMRIVolumes(h_fMRI_Volumes);
        BROCCOLI.SetAutoMask(AUTO_MASK);
        BROCCOLI.SetInputCertainty(h_Certainty);

        BROCCOLI.SetEPISmoothingAmount(EPI_SMOOTHING_AMOUNT);
        BROCCOLI.SetAllocatedHostMemory(allocatedHostMemory);

        BROCCOLI.SetEPIWidth(DATA_W);
        BROCCOLI.SetEPIHeight(DATA_H);
        BROCCOLI.SetEPIDepth(DATA_D);
        BROCCOLI.SetEPITimepoints(DATA_T);

        BROCCOLI.SetEPIVoxelSizeX(EPI_VOXEL_SIZE_X);
        BROCCOLI.SetEPIVoxelSizeY(EPI_VOXEL_SIZE_Y);
        BROCCOLI.SetEPIVoxelSizeZ(EPI_VOXEL_SIZE_Z);

        // Run the actual slice timing correction
        startTime = GetWallTime();
        BROCCOLI.PerformSmoothingNormalizedHostWrapper();
        endTime = GetWallTime();

        if (VERBOS)
        {
            printf("\nIt took %f seconds to run the smoothing\n",(float)(endTime - startTime));
        }

        // Print create buffer errors
        int* createBufferErrors = BROCCOLI.GetOpenCLCreateBufferErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (createBufferErrors[i] != 0)
            {
                printf("Create buffer error %i is %s \n",i,BROCCOLI.GetOpenCLErrorMessage(createBufferErrors[i]));
            }
        }

        // Print create kernel errors
        int* createKernelErrors = BROCCOLI.GetOpenCLCreateKernelErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (createKernelErrors[i] != 0)
            {
                printf("Create kernel error for kernel '%s' is '%s' \n",BROCCOLI.GetOpenCLKernelName(i),BROCCOLI.GetOpenCLErrorMessage(createKernelErrors[i]));
            }
        }

        // Print run kernel errors
        int* runKernelErrors = BROCCOLI.GetOpenCLRunKernelErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (runKernelErrors[i] != 0)
            {
                printf("Run kernel error for kernel '%s' is '%s' \n",BROCCOLI.GetOpenCLKernelName(i),BROCCOLI.GetOpenCLErrorMessage(runKernelErrors[i]));
            }
        }
    }

    // Write results to file
    startTime = GetWallTime();

    if (!CHANGE_OUTPUT_FILENAME)
    {
        WriteNifti(inputData,h_fMRI_Volumes,FILENAME_EXTENSION,ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
    }
    else
    {
        nifti_set_filenames(inputData, outputFilename, 0, 1);
        WriteNifti(inputData,h_fMRI_Volumes,"",DONT_ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
    }

    if (AUTO_MASK)
    {
        nifti_image *outputNiftifMRISingleVolume = nifti_copy_nim_info(inputData);
        outputNiftifMRISingleVolume->nt = 1;
        outputNiftifMRISingleVolume->dim[0] = 3;
        outputNiftifMRISingleVolume->dim[4] = 1;
        outputNiftifMRISingleVolume->nvox = DATA_W * DATA_H * DATA_D;
        allNiftiImages[numberOfNiftiImages] = outputNiftifMRISingleVolume;
        numberOfNiftiImages++;

        if (!CHANGE_OUTPUT_FILENAME)
        {
            WriteNifti(outputNiftifMRISingleVolume,h_Certainty,"_mask",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        }
        else
        {
            nifti_set_filenames(outputNiftifMRISingleVolume, outputFilename, 0, 1);
            WriteNifti(outputNiftifMRISingleVolume,h_Certainty,"_mask",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        }
    }

    endTime = GetWallTime();

    if (VERBOS)
    {
        printf("It took %f seconds to write the nifti file\n",(float)(endTime - startTime));
    }

    // Free all memory
    FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
    FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);

    return EXIT_SUCCESS;
}
Exemple #4
0
MatchedBlockPair *FindBestSet (MatchedBlockPair *matches, int numMatches, MultipleAlignment *a1, MultipleAlignment *a2, ScoreData *s) {
	Vector min, max = {0.0,0.0,0.0};
	MemoryManagementInfo info = {0,0};
	MatchedBlockPair **list = (MatchedBlockPair **) malloc(numMatches * sizeof(MatchedBlockPair *));
	double displacementRange = s->displacement * 2;
	int best = 0;
	int i;
	int pos;
	OctTreeNode *trees = (OctTreeNode *) malloc(a2->numResidues * sizeof(OctTreeNode));
	double maxBonus = MAX_BONUS;
	double maxPosBonus = MAX_POS_BONUS;

	if (!numMatches) return 0;
	for (i=0; i<a1->numResidues; i++) {
		/* Currently allow missing positions only in single-structure "alignments."
		 * May allow farther on or remove them altogether.  Currently no benefit from
		 * not just removing residues with no alpha carbon coordinates.
		 */
		if (a1->numChains > 1 || a1->residues[0].res[i].exists) {
			max = a1->averages[i++];
			break;
		}
	}
	min = max;
	for (; i<a1->numResidues; i++) {
		/* Safety check. */
		if (a1->numChains == 1 && !a1->residues[0].res[i].exists) continue;

		if (a1->averages[i].x < min.x) {
			min.x = a1->averages[i].x;
		}
		else if (a1->averages[i].x > max.x) {
			max.x = a1->averages[i].x;
		}

		if (a1->averages[i].y < min.y) {
			min.y = a1->averages[i].y;
		}
		else if (a1->averages[i].y > max.y) {
			max.y = a1->averages[i].y;
		}

		if (a1->averages[i].z < min.z) {
			min.z = a1->averages[i].z;
		}
		else if (a1->averages[i].z > max.z) {
			max.z = a1->averages[i].z;
		}
	}

	for (i=0; i<a2->numResidues; i++) {
		InitOctTreeNode(&trees[i], &min, &max);
	}

	for (i=0; i<numMatches; i++) {
		Vector v, first;
		MatchedBlockPair *match = &matches[i];
		match->bestPreviousMatch = -1;
		/* In this case, don't allow it to be added without a previous strand,
		 * unless it's the first strand.
		 */
		if (s->firstStrandLocked) {
			if (i) {
				match->bestAssembledScore = -5000;
			}
			else {
				/* Otherwise, keep previous assembled score.  Means I can
				 * just insert into old alignments without changing scores.
				 */
				match->bestAssembledScore -= match->score;
			}
		}
		else
			match->bestAssembledScore = 0;
		transformVect(&first, &match->m, &a2->averages[match->p2]);
		for (pos=4; pos<match->p2; pos++) {
			int hits, j;
			transformVect(&v, &match->m, &a2->averages[pos]);

			hits = GetEntries((void**)list, &trees[pos], &v, displacementRange);

			for (j=0; j < hits; j++) {
				Vector v1, v2;
				MatchedBlockPair *match2 = list[j];
				double score, invCosHalfTest, angle, displacement;
				if (match2->p1 + match2->len > match->p1) {
					continue;
				}

				if (match2->p2 + match2->len > a2->conflictMap[match->p2][0]) {
					if (match2->p2 + match2->len-1 == a2->conflictMap[match->p2][0] ||
						a2->conflictMap[match->p2][match2->p2 + match2->len-1 - a2->conflictMap[match->p2][0]])
						continue;
				}
				if (match2->p1 + match2->len > a1->conflictMap[match->p1][0]) {
					if (match2->p1 + match2->len-1 == a1->conflictMap[match->p1][0] ||
						a1->conflictMap[match->p1][match2->p1 + match2->len-1 - a1->conflictMap[match->p1][0]])
						continue;
				}

				score = match2->bestAssembledScore;

				if (score + maxBonus < match->bestAssembledScore) continue;

				invCosHalfTest = invHalfCosQuat(&match->q, &match2->q);
				if (invCosHalfTest < s->angleCos) continue;

				angle = 0;
				if (invCosHalfTest<1) angle = 2*acos(invCosHalfTest);
				score += AngleScore(angle);

				if (score + maxPosBonus < match->bestAssembledScore) continue;

				subVect(&v1, &match2->last, &v);
				transformVect(&v2, &match2->m, &a2->averages[match->p2]);
				subVect(&v2, &first,  &v2);

				displacement = (lengthVect(&v1) + lengthVect(&v2))/2;
				if (displacement > s->displacement) continue;
				score += DisplacementScore(displacement);

				if (score < match->bestAssembledScore) continue;

				match->bestAssembledScore = (float) score;
				match->bestPreviousMatch = (int)(match2 - matches);
			}
		}

		if (match->bestAssembledScore < 0) continue;

		match->bestAssembledScore += match->score;
		if (match->bestAssembledScore > matches[best].bestAssembledScore)
			best = i;
		pos = match->p2 + match->len-1;
		AddEntry(&trees[pos], &match->last, &info);
	}

	FreeAllMemory(&info);
	free(trees);
	free(list);

	if (!s->lastStrandLocked)
		return &matches[best];
	else
		return &matches[numMatches-1];
}
int main(int argc, char **argv)
{
    //-----------------------
    // Input pointers
    
    float           *h_Input_Volume;
    float           *h_Displacement_Field_X, *h_Displacement_Field_Y, *h_Displacement_Field_Z;
            
    //-----------------------
    // Output pointers        
        
    float           *h_Interpolated_Volume;

    void            *allMemoryPointers[500];
    int             numberOfMemoryPointers = 0;
    
    // Default parameters
        
    int             OPENCL_PLATFORM = 0;
    int             OPENCL_DEVICE = 0;
    bool            DEBUG = false;
    bool            PRINT = true;
	bool			CHANGE_OUTPUT_NAME = false;    

	const char*		outputFilename;

    // Size parameters
    int             INPUT_DATA_H, INPUT_DATA_W, INPUT_DATA_D;
    int             REFERENCE_DATA_H, REFERENCE_DATA_W, REFERENCE_DATA_D;
           
	float			INPUT_VOXEL_SIZE_X, INPUT_VOXEL_SIZE_Y, INPUT_VOXEL_SIZE_Z;
	float			REFERENCE_VOXEL_SIZE_X, REFERENCE_VOXEL_SIZE_Y, REFERENCE_VOXEL_SIZE_Z;

    //---------------------    
    
    /* Input arguments */
    FILE *fp = NULL; 
    
    // No inputs, so print help text
    if (argc == 1)
    {   
		printf("Transforms a volume using provided displacement fields, which have to be of the same size as the reference volume. The input volume is automagically resized and rescaled to match the input volume. \n\n");     
        printf("Usage:\n\n");
        printf("TransformVolume volume_to_transform.nii reference_volume.nii displacement_field_x.nii displacement_field_y.nii displacement_field_z.nii  [options]\n\n");
        printf("Options:\n\n");
        printf(" -platform                  The OpenCL platform to use (default 0) \n");
        printf(" -device                    The OpenCL device to use for the specificed platform (default 0) \n");
		printf(" -output                    Set output filename (default volume_to_transform_warped.nii) \n");
        printf(" -quiet                     Don't print anything to the terminal (default false) \n");
        printf("\n\n");
        
        return 1;
    }
    else if (argc < 6)
    {
        printf("Need one volume to warp, one reference volume and three displacement field volumes!\n\n");
		return -1;
    }
    // Try to open files
    else if (argc > 1)
    {        
        fp = fopen(argv[1],"r");
        if (fp == NULL)
        {
            printf("Could not open file %s !\n",argv[1]);
            return -1;
        }
        fclose(fp);     
        
        fp = fopen(argv[2],"r");
        if (fp == NULL)
        {
            printf("Could not open file %s !\n",argv[2]);
            return -1;
        }
        fclose(fp);   

        fp = fopen(argv[3],"r");
        if (fp == NULL)
        {
            printf("Could not open file %s !\n",argv[3]);
            return -1;
        }
        fclose(fp);   

        fp = fopen(argv[4],"r");
        if (fp == NULL)
        {
            printf("Could not open file %s !\n",argv[4]);
            return -1;
        }
        fclose(fp);   

        fp = fopen(argv[5],"r");
        if (fp == NULL)
        {
            printf("Could not open file %s !\n",argv[5]);
            return -1;
        }
        fclose(fp);            
    }
    
    // Loop over additional inputs
    int i = 6;
    while (i < argc)
    {
        char *input = argv[i];
        char *p;
        if (strcmp(input,"-platform") == 0)
        {
            OPENCL_PLATFORM = (int)strtol(argv[i+1], &p, 10);
            if (OPENCL_PLATFORM < 0)
            {
                printf("OpenCL platform must be >= 0!\n");
                return -1;
            }
            i += 2;
        }
        else if (strcmp(input,"-device") == 0)
        {
            OPENCL_DEVICE = (int)strtol(argv[i+1], &p, 10);
            if (OPENCL_DEVICE < 0)
            {
                printf("OpenCL device must be >= 0!\n");
                return -1;
            }
            i += 2;
        }      
        else if (strcmp(input,"-quiet") == 0)
        {
            PRINT = false;
            i += 1;
        }
        else if (strcmp(input,"-output") == 0)
        {
			CHANGE_OUTPUT_NAME = true;
            outputFilename = argv[i+1];
            i += 2;
        }
        else
        {
            printf("Unrecognized option! %s \n",argv[i]);
            return -1;
        }                
    }
    
    
    // Read data
    nifti_image *inputVolume = nifti_image_read(argv[1],1);    
    if (inputVolume == NULL)
    {
        printf("Could not open volume to transform!\n");
        return -1;
    }

    nifti_image *referenceVolume = nifti_image_read(argv[2],1);    
    if (referenceVolume == NULL)
    {
        printf("Could not open reference volume!\n");
        return -1;
    }
    
    nifti_image *inputDisplacementX = nifti_image_read(argv[3],1);   
    if (inputDisplacementX == NULL)
    {
        printf("Could not open displacement X volume!\n");
        return -1;
    }

    nifti_image *inputDisplacementY = nifti_image_read(argv[4],1);   
    if (inputDisplacementY == NULL)
    {
        printf("Could not open displacement Y volume!\n");
        return -1;
    }

    nifti_image *inputDisplacementZ = nifti_image_read(argv[5],1);   
    if (inputDisplacementZ == NULL)
    {
        printf("Could not open displacement Z volume!\n");
        return -1;
    }
    
    // Get data dimensions from input data
    INPUT_DATA_W = inputVolume->nx;
    INPUT_DATA_H = inputVolume->ny;
	INPUT_DATA_D = inputVolume->nz;    

	INPUT_VOXEL_SIZE_X = inputVolume->dx;
	INPUT_VOXEL_SIZE_Y = inputVolume->dy;
	INPUT_VOXEL_SIZE_Z = inputVolume->dz;

    REFERENCE_DATA_W = referenceVolume->nx;
    REFERENCE_DATA_H = referenceVolume->ny;
	REFERENCE_DATA_D = referenceVolume->nz;    
    
	REFERENCE_VOXEL_SIZE_X = referenceVolume->dx;
	REFERENCE_VOXEL_SIZE_Y = referenceVolume->dy;
	REFERENCE_VOXEL_SIZE_Z = referenceVolume->dz;

    // Check if the displacement volumes have the same size
	int DISPLACEMENT_DATA_W, DISPLACEMENT_DATA_H, DISPLACEMENT_DATA_D;

	DISPLACEMENT_DATA_W = inputDisplacementX->nx;
	DISPLACEMENT_DATA_H = inputDisplacementX->ny;
	DISPLACEMENT_DATA_D = inputDisplacementX->nz;
	
	if ( (DISPLACEMENT_DATA_W != REFERENCE_DATA_W) || (DISPLACEMENT_DATA_H != REFERENCE_DATA_H) || (DISPLACEMENT_DATA_D != REFERENCE_DATA_D) )
	{
        printf("Dimensions of displacement field x does not match the reference volume!\n");
        return -1;
	}

	DISPLACEMENT_DATA_W = inputDisplacementY->nx;
	DISPLACEMENT_DATA_H = inputDisplacementY->ny;
	DISPLACEMENT_DATA_D = inputDisplacementY->nz;

	if ( (DISPLACEMENT_DATA_W != REFERENCE_DATA_W) || (DISPLACEMENT_DATA_H != REFERENCE_DATA_H) || (DISPLACEMENT_DATA_D != REFERENCE_DATA_D) )
	{
        printf("Dimensions of displacement field y does not match the reference volume!\n");
        return -1;
	}

	DISPLACEMENT_DATA_W = inputDisplacementZ->nx;
	DISPLACEMENT_DATA_H = inputDisplacementZ->ny;
	DISPLACEMENT_DATA_D = inputDisplacementZ->nz;

	if ( (DISPLACEMENT_DATA_W != REFERENCE_DATA_W) || (DISPLACEMENT_DATA_H != REFERENCE_DATA_H) || (DISPLACEMENT_DATA_D != REFERENCE_DATA_D) )
	{
        printf("Dimensions of displacement field z does not match the reference volume!\n");
        return -1;
	}

    // Calculate size, in bytes
    
    int INPUT_VOLUME_SIZE = INPUT_DATA_W * INPUT_DATA_H * INPUT_DATA_D * sizeof(float);
    int REFERENCE_VOLUME_SIZE = REFERENCE_DATA_W * REFERENCE_DATA_H * REFERENCE_DATA_D * sizeof(float);

    // Print some info
    if (PRINT)
    {
        printf("Authored by K.A. Eklund \n");
        printf("Input volume size: %i x %i x %i \n",  INPUT_DATA_W, INPUT_DATA_H, INPUT_DATA_D);
        printf("Input volume voxel size: %f x %f x %f \n",  INPUT_VOXEL_SIZE_X, INPUT_VOXEL_SIZE_Y, INPUT_VOXEL_SIZE_Z);
        printf("Reference volume size: %i x %i x %i \n",  REFERENCE_DATA_W, REFERENCE_DATA_H, REFERENCE_DATA_D);
        printf("Reference volume voxel size: %f x %f x %f \n",  REFERENCE_VOXEL_SIZE_X, REFERENCE_VOXEL_SIZE_Y, REFERENCE_VOXEL_SIZE_Z);
    }

	if (PRINT)
	{
		printf("Input volume size in bytes: %i \n",  INPUT_VOLUME_SIZE);
	}
            
    // ------------------------------------------------
    
    // Allocate memory on the host        
    
    if (!AllocateMemory(h_Input_Volume, INPUT_VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers))
    {
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputVolume);
        nifti_image_free(referenceVolume);
        nifti_image_free(inputDisplacementX);
        nifti_image_free(inputDisplacementY);
        nifti_image_free(inputDisplacementZ);
        return -1;
    }
    
    if (!AllocateMemory(h_Interpolated_Volume, REFERENCE_VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers))
    {
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputVolume);
        nifti_image_free(referenceVolume);
        nifti_image_free(inputDisplacementX);
        nifti_image_free(inputDisplacementY);
        nifti_image_free(inputDisplacementZ);
        return -1;
    }    
    
    
    if (!AllocateMemory(h_Displacement_Field_X, REFERENCE_VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers))
    {
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputVolume);
        nifti_image_free(referenceVolume);
        nifti_image_free(inputDisplacementX);
        nifti_image_free(inputDisplacementY);
        nifti_image_free(inputDisplacementZ);
        return -1;
    }
    if (!AllocateMemory(h_Displacement_Field_Y, REFERENCE_VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers))
    {
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputVolume);
        nifti_image_free(referenceVolume);
        nifti_image_free(inputDisplacementX);
        nifti_image_free(inputDisplacementY);
        nifti_image_free(inputDisplacementZ);
        return -1;
    }
    if (!AllocateMemory(h_Displacement_Field_Z, REFERENCE_VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers))
    {
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputVolume);
        nifti_image_free(referenceVolume);
        nifti_image_free(inputDisplacementX);
        nifti_image_free(inputDisplacementY);
        nifti_image_free(inputDisplacementZ);
        return -1;
    }   
        
    // Convert data to floats
    if ( inputVolume->datatype == DT_SIGNED_SHORT )
    {
        short int *p = (short int*)inputVolume->data;
    
        for (int i = 0; i < INPUT_DATA_W * INPUT_DATA_H * INPUT_DATA_D; i++)
        {
            h_Input_Volume[i] = (float)p[i];
        }
    }
    else if ( inputVolume->datatype == DT_FLOAT )
    {
        float *p = (float*)inputVolume->data;
    
        for (int i = 0; i < INPUT_DATA_W * INPUT_DATA_H * INPUT_DATA_D; i++)
        {
            h_Input_Volume[i] = p[i];
        }
    }
    else
    {
        printf("Unknown data type in volume to transform, aborting!\n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputVolume);
        nifti_image_free(referenceVolume);
        nifti_image_free(inputDisplacementX);
        nifti_image_free(inputDisplacementY);
        nifti_image_free(inputDisplacementZ);
        return -1;
    }
    
    if ( inputDisplacementX->datatype == DT_SIGNED_SHORT )
    {
        short int *p = (short int*)inputDisplacementX->data;
    
        for (int i = 0; i < REFERENCE_DATA_W * REFERENCE_DATA_H * REFERENCE_DATA_D; i++)
        {
            h_Displacement_Field_X[i] = (float)p[i];
        }
    }
    else if ( inputDisplacementX->datatype == DT_FLOAT )
    {
        float *p = (float*)inputDisplacementX->data;
    
        for (int i = 0; i < REFERENCE_DATA_W * REFERENCE_DATA_H * REFERENCE_DATA_D; i++)
        {
            h_Displacement_Field_X[i] = p[i];
        }
    }
    else
    {
        printf("Unknown data type in displacement x volume, aborting!\n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputVolume);
        nifti_image_free(referenceVolume);
        nifti_image_free(inputDisplacementX);
        nifti_image_free(inputDisplacementY);
        nifti_image_free(inputDisplacementZ);
        return -1;
    }

    if ( inputDisplacementY->datatype == DT_SIGNED_SHORT )
    {
        short int *p = (short int*)inputDisplacementY->data;
    
        for (int i = 0; i < REFERENCE_DATA_W * REFERENCE_DATA_H * REFERENCE_DATA_D; i++)
        {
	        h_Displacement_Field_Y[i] = (float)p[i];
        }
    }
    else if ( inputDisplacementY->datatype == DT_FLOAT )
    {
        float *p = (float*)inputDisplacementY->data;
    
        for (int i = 0; i < REFERENCE_DATA_W * REFERENCE_DATA_H * REFERENCE_DATA_D; i++)
        {
            h_Displacement_Field_Y[i] = p[i];			
        }
    }
    else
    {
        printf("Unknown data type in displacement y volume, aborting!\n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputVolume);
        nifti_image_free(referenceVolume);
        nifti_image_free(inputDisplacementX);
        nifti_image_free(inputDisplacementY);
        nifti_image_free(inputDisplacementZ);
        return -1;
    }

    if ( inputDisplacementZ->datatype == DT_SIGNED_SHORT )
    {
        short int *p = (short int*)inputDisplacementZ->data;
    
        for (int i = 0; i < REFERENCE_DATA_W * REFERENCE_DATA_H * REFERENCE_DATA_D; i++)
        {
            h_Displacement_Field_Z[i] = (float)p[i];
        }
    }
    else if ( inputDisplacementZ->datatype == DT_FLOAT )
    {
        float *p = (float*)inputDisplacementZ->data;
    
        for (int i = 0; i < REFERENCE_DATA_W * REFERENCE_DATA_H * REFERENCE_DATA_D; i++)
        {
            h_Displacement_Field_Z[i] = p[i];
        }
    }
    else
    {
        printf("Unknown data type in displacement z volume, aborting!\n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputVolume);
        nifti_image_free(referenceVolume);
        nifti_image_free(inputDisplacementX);
        nifti_image_free(inputDisplacementY);
        nifti_image_free(inputDisplacementZ);
        return -1;
    }
            
    //------------------------
    
    BROCCOLI_LIB BROCCOLI(OPENCL_PLATFORM,OPENCL_DEVICE);
    
    // Something went wrong...
    if (BROCCOLI.GetOpenCLInitiated() == 0)
    {              
        printf("Get platform IDs error is %s \n",BROCCOLI.GetOpenCLErrorMessage(BROCCOLI.GetOpenCLPlatformIDsError()));
        printf("Get device IDs error is %s \n",BROCCOLI.GetOpenCLErrorMessage(BROCCOLI.GetOpenCLDeviceIDsError()));
        printf("Create context error is %s \n",BROCCOLI.GetOpenCLErrorMessage(BROCCOLI.GetOpenCLCreateContextError()));
        printf("Get create context info error is %s \n",BROCCOLI.GetOpenCLErrorMessage(BROCCOLI.GetOpenCLContextInfoError()));
        printf("Create command queue error is %s \n",BROCCOLI.GetOpenCLErrorMessage(BROCCOLI.GetOpenCLCreateCommandQueueError()));
        printf("Create program error is %s \n",BROCCOLI.GetOpenCLErrorMessage(BROCCOLI.GetOpenCLCreateProgramError()));
        printf("Build program error is %s \n",BROCCOLI.GetOpenCLErrorMessage(BROCCOLI.GetOpenCLBuildProgramError()));
        printf("Get program build info error is %s \n",BROCCOLI.GetOpenCLErrorMessage(BROCCOLI.GetOpenCLProgramBuildInfoError()));
    
        // Print create kernel errors
        int* createKernelErrors = BROCCOLI.GetOpenCLCreateKernelErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (createKernelErrors[i] != 0)
            {
                printf("Create kernel error %i is %s \n",i,BROCCOLI.GetOpenCLErrorMessage(createKernelErrors[i]));
            }
        }                
        
        // Print build info to file    
        fp = fopen("buildinfo.txt","w");
        if (fp == NULL)
        {     
            printf("Could not open buildinfo.txt! \n");
        }
        if (BROCCOLI.GetOpenCLBuildInfoChar() != NULL)
        {
            int error = fputs(BROCCOLI.GetOpenCLBuildInfoChar(),fp);
            if (error == EOF)
            {
                printf("Could not write to buildinfo.txt! \n");
            }
        }
        fclose(fp);
        
        printf("OpenCL initialization failed, aborting! \nSee buildinfo.txt for output of OpenCL compilation!\n");      
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputVolume);
        nifti_image_free(referenceVolume);
        nifti_image_free(inputDisplacementX);
        nifti_image_free(inputDisplacementY);
        nifti_image_free(inputDisplacementZ);
        return -1;
    }
    // Initialization OK
    else if (BROCCOLI.GetOpenCLInitiated() == 1)
    {
        // Set all necessary pointers and values
        BROCCOLI.SetInputT1Volume(h_Input_Volume);        
        BROCCOLI.SetT1Width(INPUT_DATA_W);
        BROCCOLI.SetT1Height(INPUT_DATA_H);
        BROCCOLI.SetT1Depth(INPUT_DATA_D);  
		BROCCOLI.SetT1VoxelSizeX(INPUT_VOXEL_SIZE_X);
		BROCCOLI.SetT1VoxelSizeY(INPUT_VOXEL_SIZE_Y);
		BROCCOLI.SetT1VoxelSizeZ(INPUT_VOXEL_SIZE_Z);
             
        BROCCOLI.SetMNIWidth(REFERENCE_DATA_W);
        BROCCOLI.SetMNIHeight(REFERENCE_DATA_H);
        BROCCOLI.SetMNIDepth(REFERENCE_DATA_D);
		BROCCOLI.SetMNIVoxelSizeX(REFERENCE_VOXEL_SIZE_X);
		BROCCOLI.SetMNIVoxelSizeY(REFERENCE_VOXEL_SIZE_Y);
		BROCCOLI.SetMNIVoxelSizeZ(REFERENCE_VOXEL_SIZE_Z);

		BROCCOLI.SetMMT1ZCUT(0);  

        BROCCOLI.SetInterpolationMode(LINEAR);  
		BROCCOLI.SetOutputDisplacementField(h_Displacement_Field_X,h_Displacement_Field_Y,h_Displacement_Field_Z);      
        BROCCOLI.SetOutputInterpolatedT1Volume(h_Interpolated_Volume);
                
        if (DEBUG)
        {
            BROCCOLI.SetDebug(true);            
        }
        
		for (int i = 0; i < numberOfMemoryPointers; i++)
		{
			printf("Pointer i is %i \n",allMemoryPointers[i]);
		}
                    
        // Run the actual transformation
        BROCCOLI.TransformVolumesNonParametricWrapper();

		for (int i = 0; i < numberOfMemoryPointers; i++)
		{
			printf("Pointer i is %i \n",allMemoryPointers[i]);
		}
     
        // Print create buffer errors
        int* createBufferErrors = BROCCOLI.GetOpenCLCreateBufferErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (createBufferErrors[i] != 0)
            {
                printf("Create buffer error %i is %s \n",i,BROCCOLI.GetOpenCLErrorMessage(createBufferErrors[i]));
            }
        }
        
        // Print run kernel errors
        int* runKernelErrors = BROCCOLI.GetOpenCLRunKernelErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (runKernelErrors[i] != 0)
            {
                printf("Run kernel error %i is %s \n",i,BROCCOLI.GetOpenCLErrorMessage(runKernelErrors[i]));
            }
        } 
    }        

    // Create new nifti image
    nifti_image *outputNifti = new nifti_image;
    
    // Copy information from input data
	outputNifti = nifti_copy_nim_info(referenceVolume);    
	

	// Change filename
	if (!CHANGE_OUTPUT_NAME)
	{
    	nifti_set_filenames(outputNifti, inputVolume->fname, 0, 1);

	    // Write transformed data to file
	    WriteNifti(outputNifti,h_Interpolated_Volume,"_warped",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);

		//nifti_set_filenames(outputNifti, "warped.nii", 0, 1);        
		//WriteNifti(outputNifti,h_Interpolated_Volume,"",DONT_ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
	}
	else 
	{
    	//nifti_set_filenames(outputNifti, outputFilename, 0, 1);
    	// Write transformed data to file
	    //WriteNifti(outputNifti,h_Interpolated_Volume,"",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);    
	}
                 
            
    // Free all memory
	FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);	   
	nifti_image_free(inputVolume);
    nifti_image_free(referenceVolume);
    nifti_image_free(inputDisplacementX);
    nifti_image_free(inputDisplacementY);
    nifti_image_free(inputDisplacementZ);
    nifti_image_free(outputNifti);
	

    return 1;
}
int main(int argc, char **argv)
{
    //-----------------------
    // Input pointers
    
    float           *h_Input_Volume;
    float           *h_Displacement_Field_X, *h_Displacement_Field_Y, *h_Displacement_Field_Z;
            
    //-----------------------
    // Output pointers        
        
    float           *h_Interpolated_Volume;

    void            *allMemoryPointers[500];
    int             numberOfMemoryPointers = 0;
	nifti_image*	allNiftiImages[500];
	int				numberOfNiftiImages = 0;		
    
    // Default parameters
        
    int             OPENCL_PLATFORM = 0;
    int             OPENCL_DEVICE = 0;
	int				INTERPOLATION_MODE = 1;
    bool            DEBUG = false;
    bool            PRINT = true;
	bool			CHANGE_OUTPUT_NAME = false;    
	int 			MM_T1_Z_CUT = 0;

	const char*		outputFilename;

	bool			VERBOS = false;

    // Size parameters
    int             INPUT_DATA_H, INPUT_DATA_W, INPUT_DATA_D;
    int             REFERENCE_DATA_H, REFERENCE_DATA_W, REFERENCE_DATA_D;
           
	float			INPUT_VOXEL_SIZE_X, INPUT_VOXEL_SIZE_Y, INPUT_VOXEL_SIZE_Z;
	float			REFERENCE_VOXEL_SIZE_X, REFERENCE_VOXEL_SIZE_Y, REFERENCE_VOXEL_SIZE_Z;

    //---------------------    
    
    /* Input arguments */
    FILE *fp = NULL; 
    
    // No inputs, so print help text
    if (argc == 1)
    {   
		printf("Transforms a volume using provided displacement fields, which have to be of the same size as the reference volume. The input volume is automagically resized and rescaled to match the input volume. \n\n");     
        printf("Usage:\n\n");
        printf("TransformVolume volume_to_transform.nii reference_volume.nii displacement_field_x.nii displacement_field_y.nii displacement_field_z.nii  [options]\n\n");
        printf("Options:\n\n");
        printf(" -platform                  The OpenCL platform to use (default 0) \n");
        printf(" -device                    The OpenCL device to use for the specificed platform (default 0) \n");
		printf(" -interpolation             The interpolation to use, 0 = nearest, 1 = trilinear (default 1) \n");
		printf(" -zcut                      Number of mm to cut from the bottom of the input volume, can be negative (default 0). Should be the same as for the call to RegisterTwoVolumes\n"); 
		printf(" -output                    Set output filename (default volume_to_transform_warped.nii) \n");
        printf(" -quiet                     Don't print anything to the terminal (default false) \n");
        printf("\n\n");
        
        return EXIT_SUCCESS;
    }
    else if (argc < 6)
    {
        printf("Need one volume to warp, one reference volume and three displacement field volumes!\n\n");
		return EXIT_FAILURE;
    }
    // Try to open files
    else if (argc > 1)
    {        
        fp = fopen(argv[1],"r");
        if (fp == NULL)
        {
            printf("Could not open file %s !\n",argv[1]);
            return EXIT_FAILURE;
        }
        fclose(fp);     
        
        fp = fopen(argv[2],"r");
        if (fp == NULL)
        {
            printf("Could not open file %s !\n",argv[2]);
            return EXIT_FAILURE;
        }
        fclose(fp);   

        fp = fopen(argv[3],"r");
        if (fp == NULL)
        {
            printf("Could not open file %s !\n",argv[3]);
            return EXIT_FAILURE;
        }
        fclose(fp);   

        fp = fopen(argv[4],"r");
        if (fp == NULL)
        {
            printf("Could not open file %s !\n",argv[4]);
            return EXIT_FAILURE;
        }
        fclose(fp);   

        fp = fopen(argv[5],"r");
        if (fp == NULL)
        {
            printf("Could not open file %s !\n",argv[5]);
            return EXIT_FAILURE;
        }
        fclose(fp);            
    }
    
    // Loop over additional inputs
    int i = 6;
    while (i < argc)
    {
        char *input = argv[i];
        char *p;
        if (strcmp(input,"-platform") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read value after -platform !\n");
                return EXIT_FAILURE;
			}

            OPENCL_PLATFORM = (int)strtol(argv[i+1], &p, 10);

			if (!isspace(*p) && *p != 0)
		    {
		        printf("OpenCL platform must be an integer! You provided %s \n",argv[i+1]);
				return EXIT_FAILURE;
		    }
            else if (OPENCL_PLATFORM < 0)
            {
                printf("OpenCL platform must be >= 0!\n");
                return EXIT_FAILURE;
            }
            i += 2;
        }
        else if (strcmp(input,"-device") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read value after -device !\n");
                return EXIT_FAILURE;
			}

            OPENCL_DEVICE = (int)strtol(argv[i+1], &p, 10);

			if (!isspace(*p) && *p != 0)
		    {
		        printf("OpenCL device must be an integer! You provided %s \n",argv[i+1]);
				return EXIT_FAILURE;
		    }
            else if (OPENCL_DEVICE < 0)
            {
                printf("OpenCL device must be >= 0!\n");
                return EXIT_FAILURE;
            }
            i += 2;
        }
		else if (strcmp(input,"-interpolation") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read value after -interpolation !\n");
                return EXIT_FAILURE;
			}

            INTERPOLATION_MODE = (int)strtol(argv[i+1], &p, 10);

			if (!isspace(*p) && *p != 0)
		    {
		        printf("Interpolation mode must be an integer! You provided %s \n",argv[i+1]);
				return EXIT_FAILURE;
		    }
			if ( (INTERPOLATION_MODE != 0) && (INTERPOLATION_MODE != 1) )
            {
			    printf("Interpolation mode has to be 0 or 1!\n");
                return EXIT_FAILURE;          	
			}
			i += 2;
        }      
        else if (strcmp(input,"-zcut") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read value after -zcut !\n");
                return EXIT_FAILURE;
			}

            MM_T1_Z_CUT = (int)strtol(argv[i+1], &p, 10);

			if (!isspace(*p) && *p != 0)
		    {
		        printf("zcut must be an integer! You provided %s \n",argv[i+1]);
				return EXIT_FAILURE;
		    }

            i += 2;
        }
        else if (strcmp(input,"-quiet") == 0)
        {
            PRINT = false;
            i += 1;
        }
        else if (strcmp(input,"-output") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read name after -output !\n");
                return EXIT_FAILURE;
			}
			CHANGE_OUTPUT_NAME = true;
            outputFilename = argv[i+1];
            i += 2;
        }
        else
        {
            printf("Unrecognized option! %s \n",argv[i]);
            return EXIT_FAILURE;
        }                
    }
        
	// Check if BROCCOLI_DIR variable is set
	if (getenv("BROCCOLI_DIR") == NULL)
	{
        printf("The environment variable BROCCOLI_DIR is not set!\n");
        return EXIT_FAILURE;
	}

    // Read data
    nifti_image *inputVolume = nifti_image_read(argv[1],1);    
    if (inputVolume == NULL)
    {
        printf("Could not open volume to transform!\n");
        return EXIT_FAILURE;
    }
	allNiftiImages[numberOfNiftiImages] = inputVolume;
	numberOfNiftiImages++;

    nifti_image *referenceVolume = nifti_image_read(argv[2],1);    
    if (referenceVolume == NULL)
    {
        printf("Could not open reference volume!\n");
        return EXIT_FAILURE;
    }
   	allNiftiImages[numberOfNiftiImages] = referenceVolume;
	numberOfNiftiImages++;

    nifti_image *inputDisplacementX = nifti_image_read(argv[3],1);   
    if (inputDisplacementX == NULL)
    {
        printf("Could not open displacement X volume!\n");
        return EXIT_FAILURE;
    }
	allNiftiImages[numberOfNiftiImages] = inputDisplacementX;
	numberOfNiftiImages++;

    nifti_image *inputDisplacementY = nifti_image_read(argv[4],1);   
    if (inputDisplacementY == NULL)
    {
        printf("Could not open displacement Y volume!\n");
        return EXIT_FAILURE;
    }
	allNiftiImages[numberOfNiftiImages] = inputDisplacementY;
	numberOfNiftiImages++;

    nifti_image *inputDisplacementZ = nifti_image_read(argv[5],1);   
    if (inputDisplacementZ == NULL)
    {
        printf("Could not open displacement Z volume!\n");
        return EXIT_FAILURE;
    }
	allNiftiImages[numberOfNiftiImages] = inputDisplacementZ;
	numberOfNiftiImages++;
    
    // Get data dimensions from input data
    INPUT_DATA_W = inputVolume->nx;
    INPUT_DATA_H = inputVolume->ny;
	INPUT_DATA_D = inputVolume->nz;    

	INPUT_VOXEL_SIZE_X = inputVolume->dx;
	INPUT_VOXEL_SIZE_Y = inputVolume->dy;
	INPUT_VOXEL_SIZE_Z = inputVolume->dz;

    REFERENCE_DATA_W = referenceVolume->nx;
    REFERENCE_DATA_H = referenceVolume->ny;
	REFERENCE_DATA_D = referenceVolume->nz;    
    
	REFERENCE_VOXEL_SIZE_X = referenceVolume->dx;
	REFERENCE_VOXEL_SIZE_Y = referenceVolume->dy;
	REFERENCE_VOXEL_SIZE_Z = referenceVolume->dz;

    // Check if the displacement volumes have the same size
	int DISPLACEMENT_DATA_W, DISPLACEMENT_DATA_H, DISPLACEMENT_DATA_D;

	DISPLACEMENT_DATA_W = inputDisplacementX->nx;
	DISPLACEMENT_DATA_H = inputDisplacementX->ny;
	DISPLACEMENT_DATA_D = inputDisplacementX->nz;
	
	if ( (DISPLACEMENT_DATA_W != REFERENCE_DATA_W) || (DISPLACEMENT_DATA_H != REFERENCE_DATA_H) || (DISPLACEMENT_DATA_D != REFERENCE_DATA_D) )
	{
        printf("Dimensions of displacement field X does not match the reference volume!\n");
        return -1;
	}

	DISPLACEMENT_DATA_W = inputDisplacementY->nx;
	DISPLACEMENT_DATA_H = inputDisplacementY->ny;
	DISPLACEMENT_DATA_D = inputDisplacementY->nz;

	if ( (DISPLACEMENT_DATA_W != REFERENCE_DATA_W) || (DISPLACEMENT_DATA_H != REFERENCE_DATA_H) || (DISPLACEMENT_DATA_D != REFERENCE_DATA_D) )
	{
        printf("Dimensions of displacement field Y does not match the reference volume!\n");
        return -1;
	}

	DISPLACEMENT_DATA_W = inputDisplacementZ->nx;
	DISPLACEMENT_DATA_H = inputDisplacementZ->ny;
	DISPLACEMENT_DATA_D = inputDisplacementZ->nz;

	if ( (DISPLACEMENT_DATA_W != REFERENCE_DATA_W) || (DISPLACEMENT_DATA_H != REFERENCE_DATA_H) || (DISPLACEMENT_DATA_D != REFERENCE_DATA_D) )
	{
        printf("Dimensions of displacement field Z does not match the reference volume!\n");
        return -1;
	}

    // Calculate size, in bytes
    
    int INPUT_VOLUME_SIZE = INPUT_DATA_W * INPUT_DATA_H * INPUT_DATA_D * sizeof(float);
    int REFERENCE_VOLUME_SIZE = REFERENCE_DATA_W * REFERENCE_DATA_H * REFERENCE_DATA_D * sizeof(float);

    // Print some info
    if (PRINT)
    {
        printf("Authored by K.A. Eklund \n");
        printf("Input volume size: %i x %i x %i \n",  INPUT_DATA_W, INPUT_DATA_H, INPUT_DATA_D);
        printf("Input volume voxel size: %f x %f x %f \n",  INPUT_VOXEL_SIZE_X, INPUT_VOXEL_SIZE_Y, INPUT_VOXEL_SIZE_Z);
        printf("Reference volume size: %i x %i x %i \n",  REFERENCE_DATA_W, REFERENCE_DATA_H, REFERENCE_DATA_D);
        printf("Reference volume voxel size: %f x %f x %f \n",  REFERENCE_VOXEL_SIZE_X, REFERENCE_VOXEL_SIZE_Y, REFERENCE_VOXEL_SIZE_Z);
    }
           
    // ------------------------------------------------
    
    // Allocate memory on the host        

	AllocateMemory(h_Input_Volume, INPUT_VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "INPUT_VOLUME");
	AllocateMemory(h_Interpolated_Volume, REFERENCE_VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "INTERPOLATED_VOLUME");
	AllocateMemory(h_Displacement_Field_X, REFERENCE_VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "DISPLACEMENT_FIELD_X");
	AllocateMemory(h_Displacement_Field_Y, REFERENCE_VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "DISPLACEMENT_FIELD_Y");
	AllocateMemory(h_Displacement_Field_Z, REFERENCE_VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "DISPLACEMENT_FIELD_Z");
			           
    // Convert data to floats
    if ( inputVolume->datatype == DT_SIGNED_SHORT )
    {
        short int *p = (short int*)inputVolume->data;
    
        for (int i = 0; i < INPUT_DATA_W * INPUT_DATA_H * INPUT_DATA_D; i++)
        {
            h_Input_Volume[i] = (float)p[i];
        }
    }
    else if ( inputVolume->datatype == DT_FLOAT )
    {
        float *p = (float*)inputVolume->data;
    
        for (int i = 0; i < INPUT_DATA_W * INPUT_DATA_H * INPUT_DATA_D; i++)
        {
            h_Input_Volume[i] = p[i];
        }
    }
    else if ( inputVolume->datatype == DT_UINT8 )
    {
        unsigned char *p = (unsigned char*)inputVolume->data;
    
        for (int i = 0; i < INPUT_DATA_W * INPUT_DATA_H * INPUT_DATA_D; i++)
        {
            h_Input_Volume[i] = (float)p[i];
        }
    }
    else
    {
        printf("Unknown data type in volume to transform, aborting!\n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
        return EXIT_FAILURE;
    }
    
    if ( inputDisplacementX->datatype == DT_SIGNED_SHORT )
    {
        short int *p = (short int*)inputDisplacementX->data;
    
        for (int i = 0; i < REFERENCE_DATA_W * REFERENCE_DATA_H * REFERENCE_DATA_D; i++)
        {
            h_Displacement_Field_X[i] = (float)p[i];
        }
    }
    else if ( inputDisplacementX->datatype == DT_FLOAT )
    {
        float *p = (float*)inputDisplacementX->data;
    
        for (int i = 0; i < REFERENCE_DATA_W * REFERENCE_DATA_H * REFERENCE_DATA_D; i++)
        {
            h_Displacement_Field_X[i] = p[i];
        }
    }
    else
    {
        printf("Unknown data type in displacement x volume, aborting!\n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
        return EXIT_FAILURE;
    }

    if ( inputDisplacementY->datatype == DT_SIGNED_SHORT )
    {
        short int *p = (short int*)inputDisplacementY->data;
    
        for (int i = 0; i < REFERENCE_DATA_W * REFERENCE_DATA_H * REFERENCE_DATA_D; i++)
        {
	        h_Displacement_Field_Y[i] = (float)p[i];
        }
    }
    else if ( inputDisplacementY->datatype == DT_FLOAT )
    {
        float *p = (float*)inputDisplacementY->data;
    
        for (int i = 0; i < REFERENCE_DATA_W * REFERENCE_DATA_H * REFERENCE_DATA_D; i++)
        {
            h_Displacement_Field_Y[i] = p[i];			
        }
    }
    else
    {
        printf("Unknown data type in displacement y volume, aborting!\n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
        return EXIT_FAILURE;
    }

    if ( inputDisplacementZ->datatype == DT_SIGNED_SHORT )
    {
        short int *p = (short int*)inputDisplacementZ->data;
    
        for (int i = 0; i < REFERENCE_DATA_W * REFERENCE_DATA_H * REFERENCE_DATA_D; i++)
        {
            h_Displacement_Field_Z[i] = (float)p[i];
        }
    }
    else if ( inputDisplacementZ->datatype == DT_FLOAT )
    {
        float *p = (float*)inputDisplacementZ->data;
    
        for (int i = 0; i < REFERENCE_DATA_W * REFERENCE_DATA_H * REFERENCE_DATA_D; i++)
        {
            h_Displacement_Field_Z[i] = p[i];
        }
    }
    else
    {
        printf("Unknown data type in displacement z volume, aborting!\n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
        return EXIT_FAILURE;
    }
            
    //------------------------
    
    BROCCOLI_LIB BROCCOLI(OPENCL_PLATFORM,OPENCL_DEVICE,2,VERBOS); // 2 = Bash wrapper    

    // Print build info to file (always)
	std::vector<std::string> buildInfo = BROCCOLI.GetOpenCLBuildInfo();
	std::vector<std::string> kernelFileNames = BROCCOLI.GetKernelFileNames();

	for (int k = 0; k < BROCCOLI.GetNumberOfKernelFiles(); k++)
	{
		std::string temp = "buildInfo";
		std::string name = kernelFileNames[k];
		// Remove "kernel" and ".cpp" from kernel filename
		name = name.substr(0,name.size()-4);
		name = name.substr(6,name.size());
		temp.append(name);
		temp.append(".txt");
		fp = fopen(temp.c_str(),"w");
		if (fp == NULL)
		{     
		    printf("Could not open %s for writing ! \n",temp.c_str());
		}
		else
		{	
			if (buildInfo[k].c_str() != NULL)
			{
			    int error = fputs(buildInfo[k].c_str(),fp);
			    if (error == EOF)
			    {
			        printf("Could not write to %s ! \n",temp.c_str());
			    }
			}
			fclose(fp);
		}
	}

    // Something went wrong...
    if ( !BROCCOLI.GetOpenCLInitiated() )
    {              
        printf("Initialization error is \"%s\" \n",BROCCOLI.GetOpenCLInitializationError().c_str());
		printf("OpenCL error is \"%s\" \n",BROCCOLI.GetOpenCLError());

        // Print create kernel errors
        int* createKernelErrors = BROCCOLI.GetOpenCLCreateKernelErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (createKernelErrors[i] != 0)
            {
                printf("Create kernel error for kernel '%s' is '%s' \n",BROCCOLI.GetOpenCLKernelName(i),BROCCOLI.GetOpenCLErrorMessage(createKernelErrors[i]));
            }
        }                
                      
        printf("OpenCL initialization failed, aborting! \nSee buildInfo* for output of OpenCL compilation!\n");      
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
        return EXIT_FAILURE;
    }
    // Initialization OK
    else
    {
        // Set all necessary pointers and values
        BROCCOLI.SetInputT1Volume(h_Input_Volume);        
        BROCCOLI.SetT1Width(INPUT_DATA_W);
        BROCCOLI.SetT1Height(INPUT_DATA_H);
        BROCCOLI.SetT1Depth(INPUT_DATA_D);  
		BROCCOLI.SetT1VoxelSizeX(INPUT_VOXEL_SIZE_X);
		BROCCOLI.SetT1VoxelSizeY(INPUT_VOXEL_SIZE_Y);
		BROCCOLI.SetT1VoxelSizeZ(INPUT_VOXEL_SIZE_Z);
             
        BROCCOLI.SetMNIWidth(REFERENCE_DATA_W);
        BROCCOLI.SetMNIHeight(REFERENCE_DATA_H);
        BROCCOLI.SetMNIDepth(REFERENCE_DATA_D);
		BROCCOLI.SetMNIVoxelSizeX(REFERENCE_VOXEL_SIZE_X);
		BROCCOLI.SetMNIVoxelSizeY(REFERENCE_VOXEL_SIZE_Y);
		BROCCOLI.SetMNIVoxelSizeZ(REFERENCE_VOXEL_SIZE_Z);
		
		BROCCOLI.SetMMT1ZCUT(MM_T1_Z_CUT); 
        BROCCOLI.SetInterpolationMode(INTERPOLATION_MODE);  

		BROCCOLI.SetOutputDisplacementField(h_Displacement_Field_X,h_Displacement_Field_Y,h_Displacement_Field_Z);      
        BROCCOLI.SetOutputInterpolatedT1Volume(h_Interpolated_Volume);
                
        if (DEBUG)
        {
            BROCCOLI.SetDebug(true);            
        }
                          
        // Run the actual transformation
        BROCCOLI.TransformVolumesNonLinearWrapper();
		
        // Print create buffer errors
        int* createBufferErrors = BROCCOLI.GetOpenCLCreateBufferErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (createBufferErrors[i] != 0)
            {
                printf("Create buffer error %i is %s \n",i,BROCCOLI.GetOpenCLErrorMessage(createBufferErrors[i]));
            }
        }
        
        // Print create kernel errors
        int* createKernelErrors = BROCCOLI.GetOpenCLCreateKernelErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (createKernelErrors[i] != 0)
            {
                printf("Create kernel error for kernel '%s' is '%s' \n",BROCCOLI.GetOpenCLKernelName(i),BROCCOLI.GetOpenCLErrorMessage(createKernelErrors[i]));
            }
        } 

        // Print run kernel errors
        int* runKernelErrors = BROCCOLI.GetOpenCLRunKernelErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (runKernelErrors[i] != 0)
            {
                printf("Run kernel error for kernel '%s' is '%s' \n",BROCCOLI.GetOpenCLKernelName(i),BROCCOLI.GetOpenCLErrorMessage(runKernelErrors[i]));
            }
        } 
    }        
   
    // Copy information from input data
	nifti_image* outputNifti = nifti_copy_nim_info(referenceVolume);    	
	allNiftiImages[numberOfNiftiImages] = outputNifti;
	numberOfNiftiImages++;    

	// Change filename and write transformed data to file
	if (!CHANGE_OUTPUT_NAME)
	{
    	nifti_set_filenames(outputNifti, inputVolume->fname, 0, 1);
	    WriteNifti(outputNifti,h_Interpolated_Volume,"_warped",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
	}
	else 
	{
    	nifti_set_filenames(outputNifti, outputFilename, 0, 1);    	
	    WriteNifti(outputNifti,h_Interpolated_Volume,"",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);    
	}
                             
    // Free all memory
	FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);	   
	FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
    
	return EXIT_SUCCESS;
}
Exemple #7
0
int main(int argc, char ** argv)
{
    //-----------------------
    // Input pointers
    
    float           *h_Volume = NULL;

	//--------------

    void*           allMemoryPointers[500];
	for (int i = 0; i < 500; i++)
	{
		allMemoryPointers[i] = NULL;
	}
    
	nifti_image*	allNiftiImages[500];
	for (int i = 0; i < 500; i++)
	{
		allNiftiImages[i] = NULL;
	}

    int             numberOfMemoryPointers = 0;
	int				numberOfNiftiImages = 0;

	size_t			allocatedHostMemory = 0;

	//--------------
  
    // Default parameters
    const char*     FILENAME_EXTENSION = "_roi";
    bool            PRINT = true;
	bool			VERBOS = false;
    
    size_t          DATA_W, DATA_H, DATA_D, DATA_T;
    float           VOXEL_SIZE_X, VOXEL_SIZE_Y, VOXEL_SIZE_Z;

	bool			CHANGE_OUTPUT_FILENAME = false;

	// Settings
	float			RADIUS = 5.0f;	
	float			RADIUSV = 5.0f;	

	float			XCOORDINATE = 0.0f;
	float			YCOORDINATE = 0.0f;
	float			ZCOORDINATE = 0.0f;

	float			XCOORDINATEV = 0.0f;
	float			YCOORDINATEV = 0.0f;
	float			ZCOORDINATEV = 0.0f;
	
	bool			MMRADIUS = false;
	bool			VOXELSRADIUS = false;

	bool			MMCOORDINATE = false;
	bool			VOXELSCOORDINATE = false;

    //-----------------------
    // Output parameters
    
    const char      *outputFilename;
       
    //---------------------
    
    /* Input arguments */
    FILE *fp = NULL; 
    
    // No inputs, so print help text
    if (argc == 1)
    {        
        printf("Usage:\n\n");
        printf("MakeROI input.nii [options]\n\n");
        printf("Options:\n\n");
        printf(" -coordinate      Center of ROI (x,y,z), in mm  \n");
        printf(" -coordinatev     Center of ROI (x,y,z), in voxels  \n");
        printf(" -radius          Radius of ROI, in millimeters \n");
        printf(" -radiusv         Radius of ROI, in voxels \n");
		printf(" -output      	  Set filename of nifti file  \n");
        printf("\n\n");
        
        return EXIT_SUCCESS;
    }
    // Try to open file
    else if (argc > 1)
    {        
        fp = fopen(argv[1],"r");
        if (fp == NULL)
        {            
            printf("Could not open file %s !\n",argv[1]);
            return EXIT_FAILURE;
        }
        fclose(fp);        
    }
    
    // Loop over additional inputs
    int i = 2;
    while (i < argc)
    {
        char *input = argv[i];
        char *p;
        if (strcmp(input,"-coordinate") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read first value after -coordinate !\n");
                return EXIT_FAILURE;
			}

			if ( (i+2) >= argc  )
			{
			    printf("Unable to read second value after -coordinate !\n");
                return EXIT_FAILURE;
			}

			if ( (i+3) >= argc  )
			{
			    printf("Unable to read third value after -coordinate !\n");
                return EXIT_FAILURE;
			}
            
			MMCOORDINATE = true;
            XCOORDINATE = (float)strtod(argv[i+1], &p);
			YCOORDINATE = (float)strtod(argv[i+2], &p);
			ZCOORDINATE = (float)strtod(argv[i+3], &p);
            
            i += 4;
        }        
        else if (strcmp(input,"-coordinatev") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read first value after -coordinatev !\n");
                return EXIT_FAILURE;
			}

			if ( (i+2) >= argc  )
			{
			    printf("Unable to read second value after -coordinatev !\n");
                return EXIT_FAILURE;
			}

			if ( (i+3) >= argc  )
			{
			    printf("Unable to read third value after -coordinatev !\n");
                return EXIT_FAILURE;
			}
            
			VOXELSCOORDINATE = true;
            XCOORDINATEV = (float)strtod(argv[i+1], &p);
			YCOORDINATEV = (float)strtod(argv[i+2], &p);
			ZCOORDINATEV = (float)strtod(argv[i+3], &p);
            
            i += 4;
        }        
        else if (strcmp(input,"-radius") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read value after -radius !\n");
                return EXIT_FAILURE;
			}
            
            RADIUS = (float)strtod(argv[i+1], &p);
			MMRADIUS = true;
            
			if (!isspace(*p) && *p != 0)
		    {
		        printf("Radius must be a float! You provided %s \n",argv[i+1]);
				return EXIT_FAILURE;
		    }
  			else if ( RADIUS <= 0.0f )
            {
                printf("Radius must be > 0.0 !\n");
                return EXIT_FAILURE;
            }
            i += 2;
        } 
        else if (strcmp(input,"-radiusv") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read value after -radiusv !\n");
                return EXIT_FAILURE;
			}
            
            RADIUSV = (float)strtod(argv[i+1], &p);
			VOXELSRADIUS = true;
            
  			if ( RADIUSV <= 0.0f )
            {
                printf("Radius must be > 0 !\n");
                return EXIT_FAILURE;
            }
            i += 2;
        }        
        else if (strcmp(input,"-output") == 0)
        {
			CHANGE_OUTPUT_FILENAME = true;

			if ( (i+1) >= argc  )
			{
			    printf("Unable to read name after -output !\n");
                return EXIT_FAILURE;
			}

            outputFilename = argv[i+1];
            i += 2;
        }
        else
        {
            printf("Unrecognized option! %s \n",argv[i]);
            return EXIT_FAILURE;
        }                
    }
    
    double startTime = GetWallTime();

    if (!MMCOORDINATE && !VOXELSCOORDINATE)
    {
        printf("Have to define center in mm or in voxels!\n");
        return EXIT_FAILURE;
    }
    if (MMCOORDINATE && VOXELSCOORDINATE)
    {
        printf("Cannot define center in both mm and in voxels!\n");
        return EXIT_FAILURE;
    }


    if (!MMRADIUS && !VOXELSRADIUS)
    {
        printf("Have to define radius in mm or in voxels!\n");
        return EXIT_FAILURE;
    }
    if (MMRADIUS && VOXELSRADIUS)
    {
        printf("Cannot define radius in both mm and in voxels!\n");
        return EXIT_FAILURE;
    }

	// ---------------------
    // Read data
	// ---------------------
    nifti_image *inputData = nifti_image_read(argv[1],1);
    
    if (inputData == NULL)
    {
        printf("Could not open nifti file!\n");
        return EXIT_FAILURE;
    }
    allNiftiImages[numberOfNiftiImages] = inputData;
	numberOfNiftiImages++;

	double endTime = GetWallTime();

	if (VERBOS)
 	{
		printf("It took %f seconds to read the nifti file\n",(float)(endTime - startTime));
	}

    // Get data dimensions
    DATA_W = inputData->nx;
    DATA_H = inputData->ny;
    DATA_D = inputData->nz;
    DATA_T = inputData->nt;

	XCOORDINATE = (float)DATA_W - XCOORDINATE - 1.0f;
	YCOORDINATE = (float)DATA_H - YCOORDINATE - 1.0f;

	if (VOXELSCOORDINATE)
	{
		//XCOORDINATEV = (float)DATA_W - XCOORDINATE - 1.0f;
		//YCOORDINATEV = (float)DATA_H - YCOORDINATE - 1.0f;
		//ZCOORDINATE -= 1.0f;
	}

    // Get voxel sizes
    VOXEL_SIZE_X = inputData->dx;
    VOXEL_SIZE_Y = inputData->dy;
    VOXEL_SIZE_Z = inputData->dz;
    	
    // Calculate size, in bytes
    size_t DATA_SIZE = DATA_W * DATA_H * DATA_D * sizeof(float);
    size_t VOLUME_SIZE = DATA_W * DATA_H * DATA_D * sizeof(float);
    
    // Print some info
    if (PRINT)
    {
        printf("Authored by K.A. Eklund \n");
        printf("Data size: %zu x %zu x %zu \n",  DATA_W, DATA_H, DATA_D);
        printf("Voxel size: %f x %f x %f mm \n", VOXEL_SIZE_X, VOXEL_SIZE_Y, VOXEL_SIZE_Z);   
    } 
   
    // ------------------------------------------------
    
    // Allocate memory on the host
    
	startTime = GetWallTime();

	AllocateMemory(h_Volume, DATA_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, allocatedHostMemory, "INPUT_DATA");

	endTime = GetWallTime();
    
	if (VERBOS)
 	{
		printf("It took %f seconds to allocate memory\n",(float)(endTime - startTime));
	}

	startTime = GetWallTime();

    // Convert data to floats
    if ( inputData->datatype == DT_SIGNED_SHORT )
    {
        short int *p = (short int*)inputData->data;
    
        for (size_t i = 0; i < DATA_W * DATA_H * DATA_D; i++)
        {
            h_Volume[i] = (float)p[i];
        }
    }
    else if ( inputData->datatype == DT_UINT8 )
    {
        unsigned char *p = (unsigned char*)inputData->data;
    
        for (size_t i = 0; i < DATA_W * DATA_H * DATA_D; i++)
        {
            h_Volume[i] = (float)p[i];
        }
    }
    else if ( inputData->datatype == DT_UINT16 )
    {
        unsigned short int *p = (unsigned short int*)inputData->data;
    
        for (size_t i = 0; i < DATA_W * DATA_H * DATA_D; i++)
        {
            h_Volume[i] = (float)p[i];
        }
    }
	// Correct data type, just copy the pointer
	else if ( inputData->datatype == DT_FLOAT )
    {
        float *p = (float*)inputData->data;
    
        for (size_t i = 0; i < DATA_W * DATA_H * DATA_D; i++)
        {
            h_Volume[i] = p[i];
        }
    }
    else
    {
        printf("Unknown data type in input data, aborting!\n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
        return EXIT_FAILURE;
    }

	endTime = GetWallTime();

	if (VERBOS)
 	{
		printf("It took %f seconds to convert data to floats\n",(float)(endTime - startTime));
	}

    //------------------------

	if (MMRADIUS)
	{
		for (int x = 0; x < DATA_W; x++)
		{
			for (int y = 0; y < DATA_H; y++)
			{
				for (int z = 0; z < DATA_D; z++)
				{
					float distance = sqrt( ((float) x - XCOORDINATE)*((float) x - XCOORDINATE)*VOXEL_SIZE_X*VOXEL_SIZE_X + ((float) y - YCOORDINATE)*((float) y - YCOORDINATE)*VOXEL_SIZE_Y*VOXEL_SIZE_Y + ((float) z - ZCOORDINATE)*((float) z - ZCOORDINATE)*VOXEL_SIZE_Z*VOXEL_SIZE_Z   );
					if ( distance <= RADIUS )
					{
						h_Volume[x + y * DATA_W + z * DATA_W * DATA_H] = 1.0f;
					}
					else
					{
						h_Volume[x + y * DATA_W + z * DATA_W * DATA_H] = 0.0f;
					}
				}
			}
		}
	}
	else if (VOXELSRADIUS)
	{
		for (int x = 0; x < DATA_W; x++)
		{
			for (int y = 0; y < DATA_H; y++)
			{
				for (int z = 0; z < DATA_D; z++)
				{
					float distance = sqrt( ((float) x - XCOORDINATE)*((float) x - XCOORDINATE) + ((float) y - YCOORDINATE)*((float) y - YCOORDINATE) + ((float) z - ZCOORDINATE)*((float) z - ZCOORDINATE) );
					if ( distance <= RADIUSV )
					{
						h_Volume[x + y * DATA_W + z * DATA_W * DATA_H] = 1.0f;
					}
					else
					{
						h_Volume[x + y * DATA_W + z * DATA_W * DATA_H] = 0.0f;
					}
				}
			}
		}
	}



    //------------------------
         
    // Write results to file            

    nifti_image *outputNifti = nifti_copy_nim_info(inputData);
    outputNifti->nt = 1;	
    outputNifti->dim[0] = 3;
    outputNifti->dim[4] = 1;
    outputNifti->nvox = DATA_W * DATA_H * DATA_D;
    allNiftiImages[numberOfNiftiImages] = outputNifti;
	numberOfNiftiImages++;

    startTime = GetWallTime();

	if (!CHANGE_OUTPUT_FILENAME)
	{
	    WriteNifti(outputNifti,h_Volume,FILENAME_EXTENSION,ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
	}
	else
	{
		nifti_set_filenames(outputNifti, outputFilename, 0, 1);
		WriteNifti(outputNifti,h_Volume,"",DONT_ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
	}

	endTime = GetWallTime();

	if (VERBOS)
 	{
		printf("It took %f seconds to write the nifti file\n",(float)(endTime - startTime));
	}
    
    // Free all memory
    FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);            
    FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
    
    return EXIT_SUCCESS;
}
int main(int argc, char ** argv)
{
    //-----------------------
    // Input pointers
    
    float           *h_fMRI_Volumes = NULL;
    
    void*			allMemoryPointers[500];
    int             numberOfMemoryPointers = 0;

	nifti_image*	allNiftiImages[500];
    int             numberOfNiftiImages = 0;

    // Default parameters
    int             OPENCL_PLATFORM = 0;
    int             OPENCL_DEVICE = 0;
    bool            DEBUG = false;
    const char*     FILENAME_EXTENSION = "_stc";
    bool            PRINT = true;
	bool			VERBOS = false;
    
    int             DATA_W, DATA_H, DATA_D, DATA_T;
    float           EPI_VOXEL_SIZE_X, EPI_VOXEL_SIZE_Y, EPI_VOXEL_SIZE_Z;
	float			TR;
	int				SLICE_ORDER = 0;    

    //-----------------------
    // Output parameters
    
    const char      *outputFilename;
    
    float           *h_Slice_Timing_Corrected_fMRI_Volumes = NULL;
    
    //---------------------
    
    /* Input arguments */
    FILE *fp = NULL; 
    
    // No inputs, so print help text
    if (argc == 1)
    {        
        printf("Usage:\n\n");
        printf("SliceTimingCorrection input.nii [options]\n\n");
        printf("Options:\n\n");
        printf(" -platform        The OpenCL platform to use (default 0) \n");
        printf(" -device          The OpenCL device to use for the specificed platform (default 0) \n");
        printf(" -slicepattern    The sampling pattern used during scanning, 0 = sequential 1-N (bottom-up), 1 = sequential N-1 (top-down), 2 = alternating 1-N, 3 = alternating N-1 (default 0) \n");        
        printf(" -output          Set output filename (default input_stc.nii) \n");
        printf(" -quiet           Don't print anything to the terminal (default false) \n");
        printf(" -verbose         Print extra stuff (default false) \n");
        printf("\n\n");
        
        return EXIT_SUCCESS;
    }
    // Try to open file
    else if (argc > 1)
    {        
        fp = fopen(argv[1],"r");
        if (fp == NULL)
        {            
            printf("Could not open file %s !\n",argv[1]);
            return EXIT_FAILURE;
        }
        fclose(fp);        
    }
    
    // Loop over additional inputs
    int i = 2;
    while (i < argc)
    {
        char *input = argv[i];
        char *p;
        if (strcmp(input,"-platform") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read value after -platform !\n");
                return EXIT_FAILURE;
			}

            OPENCL_PLATFORM = (int)strtol(argv[i+1], &p, 10);

			if (!isspace(*p) && *p != 0)
		    {
		        printf("OpenCL platform must be an integer! You provided %s \n",argv[i+1]);
				return EXIT_FAILURE;
		    }
            else if (OPENCL_PLATFORM < 0)
            {
                printf("OpenCL platform must be >= 0!\n");
                return EXIT_FAILURE;
            }
            i += 2;
        }
        else if (strcmp(input,"-device") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read value after -device !\n");
                return EXIT_FAILURE;
			}

            OPENCL_DEVICE = (int)strtol(argv[i+1], &p, 10);

			if (!isspace(*p) && *p != 0)
		    {
		        printf("OpenCL device must be an integer! You provided %s \n",argv[i+1]);
				return EXIT_FAILURE;
		    }
            else if (OPENCL_DEVICE < 0)
            {
                printf("OpenCL device must be >= 0!\n");
                return EXIT_FAILURE;
            }
            i += 2;
        }
        else if (strcmp(input,"-slicepattern") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read value after -slicepattern !\n");
                return EXIT_FAILURE;
			}

            SLICE_ORDER = (int)strtol(argv[i+1], &p, 10);

			if (!isspace(*p) && *p != 0)
		    {
		        printf("Slice pattern must be an integer! You provided %s \n",argv[i+1]);
				return EXIT_FAILURE;
		    }
            else if (SLICE_ORDER < 0)
            {
                printf("Slice pattern must be a positive number!\n");
                return EXIT_FAILURE;
            }
            else if ( (SLICE_ORDER != 0) && (SLICE_ORDER != 1) && (SLICE_ORDER != 2) && (SLICE_ORDER != 3) )
            {
                printf("Slice pattern must be 0, 1, 2 or 3!\n");
                return EXIT_FAILURE;
            }
            i += 2;
        }
        else if (strcmp(input,"-debug") == 0)
        {
            DEBUG = true;
            i += 1;
        }
        else if (strcmp(input,"-quiet") == 0)
        {
            PRINT = false;
            i += 1;
        }
        else if (strcmp(input,"-verbose") == 0)
        {
            VERBOS = true;
            i += 1;
        }
        else if (strcmp(input,"-output") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read name after -output !\n");
                return EXIT_FAILURE;
			}

            outputFilename = argv[i+1];
            i += 2;
        }
        else
        {
            printf("Unrecognized option! %s \n",argv[i]);
            return EXIT_FAILURE;
        }                
    }
    
    double startTime = GetWallTime();

    // Read data
    nifti_image *inputData = nifti_image_read(argv[1],1);
    
    if (inputData == NULL)
    {
        printf("Could not open nifti file!\n");
        return EXIT_FAILURE;
    }
    allNiftiImages[numberOfNiftiImages] = inputData;
	numberOfNiftiImages++;

	double endTime = GetWallTime();

	if (VERBOS)
 	{
		printf("It took %f seconds to read the nifti file\n",(float)(endTime - startTime));
	}

    // Get data dimensions from input data
    DATA_W = inputData->nx;
    DATA_H = inputData->ny;
    DATA_D = inputData->nz;
    DATA_T = inputData->nt;

    // Get voxel sizes from input data
    EPI_VOXEL_SIZE_X = inputData->dx;
    EPI_VOXEL_SIZE_Y = inputData->dy;
    EPI_VOXEL_SIZE_Z = inputData->dz;
    
    // Get repetition time from input data
    TR = inputData->dt;                           

	

    // Calculate size, in bytes
    int DATA_SIZE = DATA_W * DATA_H * DATA_D * DATA_T * sizeof(float);
    int VOLUME_SIZE = DATA_W * DATA_H * DATA_D * sizeof(float);
    
    // Print some info
    if (PRINT)
    {
        printf("Authored by K.A. Eklund \n");
        printf("Data size: %i x %i x %i x %i \n",  DATA_W, DATA_H, DATA_D, DATA_T);
        printf("Voxel size: %f x %f x %f mm \n", EPI_VOXEL_SIZE_X, EPI_VOXEL_SIZE_Y, EPI_VOXEL_SIZE_Z);    
        printf("TR: %f s \n", TR);
    } 
    
    // ------------------------------------------------
    
    // Allocate memory on the host
    
	startTime = GetWallTime();

	AllocateMemory(h_fMRI_Volumes, DATA_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "INPUT_DATA");
	AllocateMemory(h_Slice_Timing_Corrected_fMRI_Volumes, DATA_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "SLICE_TIMING_CORRECTED_DATA");
    
	endTime = GetWallTime();
    
	if (VERBOS)
 	{
		printf("It took %f seconds to allocate memory\n",(float)(endTime - startTime));
	}

	startTime = GetWallTime();

    // Convert data to floats
    if ( inputData->datatype == DT_SIGNED_SHORT )
    {
        short int *p = (short int*)inputData->data;
    
        for (int i = 0; i < DATA_W * DATA_H * DATA_D * DATA_T; i++)
        {
            h_fMRI_Volumes[i] = (float)p[i];
        }
    }
	else if ( inputData->datatype == DT_FLOAT )
    {
        float *p = (float*)inputData->data;
    
        for (int i = 0; i < DATA_W * DATA_H * DATA_D * DATA_T; i++)
        {
            h_fMRI_Volumes[i] = p[i];
        }
    }
    else
    {
        printf("Unknown data type in input data, aborting!\n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
        return EXIT_FAILURE;
    }
    
	endTime = GetWallTime();

	if (VERBOS)
 	{
		printf("It took %f seconds to convert data to floats\n",(float)(endTime - startTime));
	}

    //------------------------
    
	startTime = GetWallTime();

	// Initialize BROCCOLI
    BROCCOLI_LIB BROCCOLI(OPENCL_PLATFORM,OPENCL_DEVICE,2); // 2 = Bash wrapper

	endTime = GetWallTime();

	if (VERBOS)
 	{
		printf("It took %f seconds to initiate BROCCOLI\n",(float)(endTime - startTime));
	}
    
    // Something went wrong...
    if (!BROCCOLI.GetOpenCLInitiated())
    {              
        printf("Initialization error is \"%s\" \n",BROCCOLI.GetOpenCLInitializationError());
		printf("OpenCL error is \"%s\" \n",BROCCOLI.GetOpenCLError());

        // Print create kernel errors
        int* createKernelErrors = BROCCOLI.GetOpenCLCreateKernelErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (createKernelErrors[i] != 0)
            {
                printf("Create kernel error for kernel '%s' is '%s' \n",BROCCOLI.GetOpenCLKernelName(i),BROCCOLI.GetOpenCLErrorMessage(createKernelErrors[i]));
            }
        }                
        
        // Print build info to file    
        fp = fopen("buildinfo.txt","w");
        if (fp == NULL)
        {     
            printf("Could not open buildinfo.txt! \n");
        }
        if (BROCCOLI.GetOpenCLBuildInfoChar() != NULL)
        {
            int error = fputs(BROCCOLI.GetOpenCLBuildInfoChar(),fp);
            if (error == EOF)
            {
                printf("Could not write to buildinfo.txt! \n");
            }
        }
        fclose(fp);
                
        printf("OpenCL initialization failed, aborting! \nSee buildinfo.txt for output of OpenCL compilation!\n");      
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
        return EXIT_FAILURE;
    }
    // Initialization OK
    else
    {
        // Set all necessary pointers and values
        BROCCOLI.SetInputfMRIVolumes(h_fMRI_Volumes);
        
        BROCCOLI.SetEPIWidth(DATA_W);
        BROCCOLI.SetEPIHeight(DATA_H);
        BROCCOLI.SetEPIDepth(DATA_D);
        BROCCOLI.SetEPITimepoints(DATA_T);  
        BROCCOLI.SetEPITR(TR);  
        BROCCOLI.SetEPISliceOrder(SLICE_ORDER);  
                     
        BROCCOLI.SetOutputSliceTimingCorrectedfMRIVolumes(h_Slice_Timing_Corrected_fMRI_Volumes);      
             
        // Run the actual slice timing correction
		startTime = GetWallTime();        
		BROCCOLI.PerformSliceTimingCorrectionWrapper();        
		endTime = GetWallTime();

		if (VERBOS)
	 	{
			printf("\nIt took %f seconds to run the slice timing correction\n",(float)(endTime - startTime));
		}    

        // Print create buffer errors
        int* createBufferErrors = BROCCOLI.GetOpenCLCreateBufferErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (createBufferErrors[i] != 0)
            {
                printf("Create buffer error %i is %d \n",i,BROCCOLI.GetOpenCLErrorMessage(createBufferErrors[i]));
            }
        }
        
        // Print create kernel errors
        int* createKernelErrors = BROCCOLI.GetOpenCLCreateKernelErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (createKernelErrors[i] != 0)
            {
                printf("Create kernel error for kernel '%s' is '%s' \n",BROCCOLI.GetOpenCLKernelName(i),BROCCOLI.GetOpenCLErrorMessage(createKernelErrors[i]));
            }
        } 

        // Print run kernel errors
        int* runKernelErrors = BROCCOLI.GetOpenCLRunKernelErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (runKernelErrors[i] != 0)
            {
                printf("Run kernel error for kernel '%s' is '%s' \n",BROCCOLI.GetOpenCLKernelName(i),BROCCOLI.GetOpenCLErrorMessage(runKernelErrors[i]));
            }
        } 
    }
        
    // Write slice timing corrected data to file            
    startTime = GetWallTime();

	/*
    // Create new nifti image
	nifti_image *outputNifti = nifti_copy_nim_info(inputData);      
	allNiftiImages[numberOfNiftiImages] = outputNifti;
	numberOfNiftiImages++;   
     
    // Copy information from input data    	
	if (!CHANGE_OUTPUT_NAME)
	{
    	nifti_set_filenames(outputNifti, inputData->fname, 0, 1);    
	}
	else
	{
		nifti_set_filenames(outputNifti, outputFilename, 0, 1);    
	}
	*/

    WriteNifti(inputData,h_Slice_Timing_Corrected_fMRI_Volumes,FILENAME_EXTENSION,ADD_FILENAME,DONT_CHECK_EXISTING_FILE);


	endTime = GetWallTime();

	if (VERBOS)
 	{
		printf("It took %f seconds to write the nifti file\n",(float)(endTime - startTime));
	}
    
    // Free all memory
    FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);            
    FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
    
    return EXIT_SUCCESS;
}
int main(int argc, char **argv)
{
    //-----------------------
    // Input pointers
    
    float           *h_Volume;
      
    void*           allMemoryPointers[500];
    int             numberOfMemoryPointers = 0;

	nifti_image*	allNiftiImages[500];
	int				numberOfNiftiImages = 0;           
    
    int             OPENCL_PLATFORM = 0;
    int             OPENCL_DEVICE = 0;

    // Size parameters

    int             DATA_H, DATA_W, DATA_D;
	float			VOXEL_SIZE_X, VOXEL_SIZE_Y, VOXEL_SIZE_Z;
        
    //---------------------    
    
    /* Input arguments */
    FILE *fp = NULL; 
    
    // No inputs, so print help text
    if (argc == 1)
    {   
		printf("\nThe function renders a volume using direct volume rendering.\n\n");     
        printf("Usage:\n\n");
        printf("RenderVolume volume.nii [options]\n\n");
        printf("Options:\n\n");
        printf(" -platform                  The OpenCL platform to use (default 0) \n");
        printf(" -device                    The OpenCL device to use for the specificed platform (default 0) \n");
        printf(" -verbose                   Print extra stuff (default false) \n");
        printf(" -debug                     Get additional debug information saved as nifti files (default no). Warning: This will use a lot of extra memory! \n");
        printf("\n\n");
        
        return EXIT_SUCCESS;
    }
    // Try to open files
    else if (argc > 1)
    {        
        fp = fopen(argv[1],"r");
        if (fp == NULL)
        {
            printf("Could not open file %s !\n",argv[1]);
            return EXIT_FAILURE;
        }
        fclose(fp);        
    }
    
    // Loop over additional inputs
    int i = 2;
    while (i < argc)
    {
        char *input = argv[i];
        char *p;
        if (strcmp(input,"-platform") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read value after -platform !\n");
                return EXIT_FAILURE;
			}

            OPENCL_PLATFORM = (int)strtol(argv[i+1], &p, 10);
			
			if (!isspace(*p) && *p != 0)
		    {
		        printf("OpenCL platform must be an integer! You provided %s \n",argv[i+1]);
				return EXIT_FAILURE;
		    }
            else if (OPENCL_PLATFORM < 0)
            {
                printf("OpenCL platform must be >= 0!\n");
                return EXIT_FAILURE;
            }
            i += 2;
        }
        else if (strcmp(input,"-device") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read value after -device !\n");
                return EXIT_FAILURE;
			}

            OPENCL_DEVICE = (int)strtol(argv[i+1], &p, 10);

			if (!isspace(*p) && *p != 0)
		    {
		        printf("OpenCL device must be an integer! You provided %s \n",argv[i+1]);
				return EXIT_FAILURE;
		    }
            else if (OPENCL_DEVICE < 0)
            {
                printf("OpenCL device must be >= 0!\n");
                return EXIT_FAILURE;
            }
            i += 2;
        }
        else
        {
            printf("Unrecognized option! %s \n",argv[i]);
            return EXIT_FAILURE;
        }                
    }
    	 
    // Read first volume 
	// -----------------------------------

    nifti_image *inputVolume = nifti_image_read(argv[1],1);
    
    if (inputVolume == NULL)
    {
        printf("Could not open volume to render!\n");
        return EXIT_FAILURE;
    }
	allNiftiImages[numberOfNiftiImages] = inputVolume;
	numberOfNiftiImages++;
  	
	// -----------------------------------

    // Get data dimensions from input data
    DATA_W = inputVolume->nx;
    DATA_H = inputVolume->ny;
    DATA_D = inputVolume->nz;    
    
    // Get voxel sizes from input data
    VOXEL_SIZE_X = inputVolume->dx;
    VOXEL_SIZE_Y = inputVolume->dy;
    VOXEL_SIZE_Z = inputVolume->dz;
                             
    int VOLUME_SIZE = DATA_W * DATA_H * DATA_D * sizeof(float);
    
    // Print some info
    printf("Authored by K.A. Eklund \n");
    printf("Volume size: %i x %i x %i \n",  DATA_W, DATA_H, DATA_D);
    printf("Volume voxel size: %f x %f x %f mm \n", VOXEL_SIZE_X, VOXEL_SIZE_Y, VOXEL_SIZE_Z);    
        
    // ------------------------------------------------
    
    // Allocate memory on the host 
	AllocateMemory(h_Volume, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "INPUT_VOLUME");
        
    // Convert data to floats
    if ( inputVolume->datatype == DT_SIGNED_SHORT )
    {
        short int *p = (short int*)inputVolume->data;
    
        for (int i = 0; i < DATA_W * DATA_H * DATA_D; i++)
        {
            h_Volume[i] = (float)p[i];
        }
    }
    else if ( inputVolume->datatype == DT_UINT8 )
    {
        unsigned char *p = (unsigned char*)inputVolume->data;
    
        for (int i = 0; i < DATA_W * DATA_H * DATA_D; i++)
        {
            h_Volume[i] = (float)p[i];
        }
    }
    else if ( inputVolume->datatype == DT_FLOAT )
    {
        float *p = (float*)inputVolume->data;
    
        for (int i = 0; i < DATA_W * DATA_H * DATA_D; i++)
        {
            h_Volume[i] = p[i];
        }
    }
    else
    {
        printf("Unknown data type in input volume, aborting!\n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
		FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
        return EXIT_FAILURE;
    }
       
    //------------------------
    
    // First initialize OpenGL context, so we can properly setup the OpenGL / OpenCL interop.
    InitGL(&argc, argv); 

    // Create OpenCL context, get device info, select device, select options for image/texture and CL-GL interop
    //createCLContext(argc, (const char**)argv);
	createCLContext(OPENCL_PLATFORM, OPENCL_DEVICE);

	cl_int error;
	

    // create a command-queue
    cqCommandQueue = clCreateCommandQueue(cxGPUContext, cdDevices[uiDeviceUsed], 0, &ciErrNum);
    //oclCheckErrorEX(ciErrNum, CL_SUCCESS, pCleanup);

	//clGetDeviceInfo(cdDevices[uiDeviceUsed], CL_DEVICE_IMAGE_SUPPORT, sizeof(g_bImageSupport), &g_bImageSupport, NULL);
	g_bImageSupport = true;

	// Read the kernel code from file
	std::fstream kernelFile("volumeRender.cl",std::ios::in);

	std::ostringstream oss;
	oss << kernelFile.rdbuf();
	std::string src = oss.str();
	const char *srcstr = src.c_str();


	// Create program and build the code for the selected device
	cpProgram = clCreateProgramWithSource(cxGPUContext, 1, (const char**)&srcstr, NULL, &error);
	printf("Create program with source error is %i \n",error);
    
    // build the program
    std::string buildOpts = "-cl-fast-relaxed-math";
    buildOpts += g_bImageSupport ? " -DIMAGE_SUPPORT" : "";
    ciErrNum = clBuildProgram(cpProgram, 0, NULL, buildOpts.c_str(), NULL, NULL);
	printf("Build program error is %i \n",error);
    if (ciErrNum != CL_SUCCESS)
    {
		printf("Building failed!\n");
        // write out standard error, Build Log and PTX, then cleanup and return error
        //shrlogEx(LOGBOTH | ERRORMSG, ciErrNum, STDERROR);
        //oclLogBuildInfo(cpProgram, oclGetFirstDev(cxGPUContext));
        //oclLogPtx(cpProgram, oclGetFirstDev(cxGPUContext), "oclVolumeRender.ptx");
        Cleanup(EXIT_FAILURE); 
    }

    // create the kernel
    ckKernel = clCreateKernel(cpProgram, "d_render", &error);
	printf("Create kernel error is %i \n",error);
    //oclCheckErrorEX(ciErrNum, CL_SUCCESS, pCleanup);

    // Init OpenCL
    initCLVolume(h_Volume, DATA_W, DATA_H, DATA_D);

    // init timer 1 for fps measurement 
    //shrDeltaT(1);  
    
    // Create buffers and textures, 
    // and then start main GLUT rendering loop for processing and rendering, 
	// or otherwise run No-GL Q/A test sequence
    initPixelBuffer();
    glutMainLoop();

    // Normally unused return path
    Cleanup(EXIT_SUCCESS);

    // Free all memory
    FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
    FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
       
    return EXIT_SUCCESS;
}
CLTALoadOnlyAlloc::~CLTALoadOnlyAlloc()
{
	FreeAllMemory();
}
int main(int argc, char ** argv)
{
    //-----------------------
    // Input pointers
    
    float           *h_fMRI_Volumes = NULL;
    float           *h_Quadrature_Filter_1_Real = NULL;
    float           *h_Quadrature_Filter_2_Real = NULL;
    float           *h_Quadrature_Filter_3_Real = NULL;
    float           *h_Quadrature_Filter_1_Imag = NULL;
    float           *h_Quadrature_Filter_2_Imag = NULL;
    float           *h_Quadrature_Filter_3_Imag = NULL;
    
    void*			allMemoryPointers[500];
    int             numberOfMemoryPointers = 0;

	nifti_image*	allNiftiImages[500];
    int             numberOfNiftiImages = 0;

    // Default parameters
    int             MOTION_CORRECTION_FILTER_SIZE = 7; 
    int             NUMBER_OF_ITERATIONS_FOR_MOTION_CORRECTION = 5;
    int             OPENCL_PLATFORM = 0;
    int             OPENCL_DEVICE = 0;
    int             NUMBER_OF_MOTION_CORRECTION_PARAMETERS = 6;    
    bool            DEBUG = false;
    const char*     FILENAME_EXTENSION = "_mc";
    bool            PRINT = true;
	bool			VERBOS = false;
    
    int             DATA_W, DATA_H, DATA_D, DATA_T;
    float           EPI_VOXEL_SIZE_X, EPI_VOXEL_SIZE_Y, EPI_VOXEL_SIZE_Z;
    
    //-----------------------
    // Output parameters
    
    const char      *outputFilename;
    
    float           *h_Quadrature_Filter_Response_1_Real = NULL;
    float           *h_Quadrature_Filter_Response_1_Imag = NULL;
    float           *h_Quadrature_Filter_Response_2_Real = NULL;
    float           *h_Quadrature_Filter_Response_2_Imag = NULL;
    float           *h_Quadrature_Filter_Response_3_Real = NULL;
    float           *h_Quadrature_Filter_Response_3_Imag = NULL;  
    float           *h_Phase_Differences = NULL;
    float           *h_Phase_Certainties = NULL;
    float           *h_Phase_Gradients = NULL;
    float           *h_Motion_Corrected_fMRI_Volumes = NULL;
    float           *h_Motion_Parameters = NULL;
    
    //---------------------
    
    
    /* Input arguments */
    FILE *fp = NULL; 
    
    // No inputs, so print help text
    if (argc == 1)
    {        
        printf("Usage:\n\n");
        printf("MotionCorrection input.nii [options]\n\n");
        printf("Options:\n\n");
        printf(" -platform   The OpenCL platform to use (default 0) \n");
        printf(" -device     The OpenCL device to use for the specificed platform (default 0) \n");
        printf(" -iterations Number of iterations for the motion correction algorithm (default 5) \n");        
        printf(" -output     Set output filename (default input_mc.nii) \n");
        printf(" -quiet      Don't print anything to the terminal (default false) \n");
        printf(" -verbose    Print extra stuff (default false) \n");
        printf(" -debug      Get additional debug information (default false) \n");
        printf("\n\n");
        
        return EXIT_SUCCESS;
    }
    // Try to open file
    else if (argc > 1)
    {        
        fp = fopen(argv[1],"r");
        if (fp == NULL)
        {            
            printf("Could not open file %s !\n",argv[1]);
            return EXIT_FAILURE;
        }
        fclose(fp);        
    }
    
    // Loop over additional inputs
    int i = 2;
    while (i < argc)
    {
        char *input = argv[i];
        char *p;
        if (strcmp(input,"-platform") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read value after -platform !\n");
                return EXIT_FAILURE;
			}

            OPENCL_PLATFORM = (int)strtol(argv[i+1], &p, 10);

			if (!isspace(*p) && *p != 0)
		    {
		        printf("OpenCL platform must be an integer! You provided %s \n",argv[i+1]);
				return EXIT_FAILURE;
		    }
            else if (OPENCL_PLATFORM < 0)
            {
                printf("OpenCL platform must be >= 0!\n");
                return EXIT_FAILURE;
            }
            i += 2;
        }
        else if (strcmp(input,"-device") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read value after -device !\n");
                return EXIT_FAILURE;
			}

            OPENCL_DEVICE = (int)strtol(argv[i+1], &p, 10);

			if (!isspace(*p) && *p != 0)
		    {
		        printf("OpenCL device must be an integer! You provided %s \n",argv[i+1]);
				return EXIT_FAILURE;
		    }
            else if (OPENCL_DEVICE < 0)
            {
                printf("OpenCL device must be >= 0!\n");
                return EXIT_FAILURE;
            }
            i += 2;
        }
        else if (strcmp(input,"-iterations") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read value after -iterations !\n");
                return EXIT_FAILURE;
			}

            NUMBER_OF_ITERATIONS_FOR_MOTION_CORRECTION = (int)strtol(argv[i+1], &p, 10);

			if (!isspace(*p) && *p != 0)
		    {
		        printf("Number of iterations must be an integer! You provided %s \n",argv[i+1]);
				return EXIT_FAILURE;
		    }
            else if (NUMBER_OF_ITERATIONS_FOR_MOTION_CORRECTION <= 0)
            {
                printf("Number of iterations must be a positive number!\n");
                return EXIT_FAILURE;
            }
            i += 2;
        }
        else if (strcmp(input,"-debug") == 0)
        {
            DEBUG = true;
            i += 1;
        }
        else if (strcmp(input,"-quiet") == 0)
        {
            PRINT = false;
            i += 1;
        }
        else if (strcmp(input,"-verbose") == 0)
        {
            VERBOS = true;
            i += 1;
        }
        else if (strcmp(input,"-output") == 0)
        {
			if ( (i+1) >= argc  )
			{
			    printf("Unable to read name after -output !\n");
                return EXIT_FAILURE;
			}

            outputFilename = argv[i+1];
            i += 2;
        }
        else
        {
            printf("Unrecognized option! %s \n",argv[i]);
            return EXIT_FAILURE;
        }                
    }
    
    double startTime = GetWallTime();

    // Read data
    nifti_image *inputData = nifti_image_read(argv[1],1);
    
    if (inputData == NULL)
    {
        printf("Could not open nifti file!\n");
        return EXIT_FAILURE;
    }
    allNiftiImages[numberOfNiftiImages] = inputData;
	numberOfNiftiImages++;

	double endTime = GetWallTime();

	if (VERBOS)
 	{
		printf("It took %f seconds to read the nifti file\n",(float)(endTime - startTime));
	}

    // Get data dimensions from input data
    DATA_W = inputData->nx;
    DATA_H = inputData->ny;
    DATA_D = inputData->nz;
    DATA_T = inputData->nt;
    
    // Get voxel sizes from input data
    EPI_VOXEL_SIZE_X = inputData->dx;
    EPI_VOXEL_SIZE_Y = inputData->dy;
    EPI_VOXEL_SIZE_Z = inputData->dz;
                               
    // Calculate size, in bytes
    int DATA_SIZE = DATA_W * DATA_H * DATA_D * DATA_T * sizeof(float);
    int MOTION_PARAMETERS_SIZE = NUMBER_OF_MOTION_CORRECTION_PARAMETERS * DATA_T * sizeof(float);
    int FILTER_SIZE = MOTION_CORRECTION_FILTER_SIZE * MOTION_CORRECTION_FILTER_SIZE * MOTION_CORRECTION_FILTER_SIZE * sizeof(float);
    int VOLUME_SIZE = DATA_W * DATA_H * DATA_D * sizeof(float);
    
    // Print some info
    if (PRINT)
    {
        printf("Authored by K.A. Eklund \n");
        printf("Data size: %i x %i x %i x %i \n",  DATA_W, DATA_H, DATA_D, DATA_T);
        printf("Voxel size: %f x %f x %f mm \n", EPI_VOXEL_SIZE_X, EPI_VOXEL_SIZE_Y, EPI_VOXEL_SIZE_Z);    
        printf("Number of iterations for motion correction: %i \n",  NUMBER_OF_ITERATIONS_FOR_MOTION_CORRECTION);
    } 
    
    // ------------------------------------------------
    
    // Allocate memory on the host
    
	startTime = GetWallTime();

	AllocateMemory(h_fMRI_Volumes, DATA_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "INPUT_DATA");
	AllocateMemory(h_Motion_Corrected_fMRI_Volumes, DATA_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "MOTION_CORRECTED_DATA");
	AllocateMemory(h_Quadrature_Filter_1_Real, FILTER_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "QUADRATURE_FILTER_1_REAL");    
  	AllocateMemory(h_Quadrature_Filter_1_Imag, FILTER_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "QUADRATURE_FILTER_1_IMAG");    
	AllocateMemory(h_Quadrature_Filter_2_Real, FILTER_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "QUADRATURE_FILTER_2_REAL");    
  	AllocateMemory(h_Quadrature_Filter_2_Imag, FILTER_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "QUADRATURE_FILTER_2_IMAG");    
	AllocateMemory(h_Quadrature_Filter_3_Real, FILTER_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "QUADRATURE_FILTER_3_REAL");    
  	AllocateMemory(h_Quadrature_Filter_3_Imag, FILTER_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "QUADRATURE_FILTER_3_IMAG");    
	AllocateMemory(h_Motion_Parameters, MOTION_PARAMETERS_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "MOTION_PARAMETERS");       
    
    if (DEBUG)
    {    
		AllocateMemory(h_Quadrature_Filter_Response_1_Real, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "QUADRATURE_FILTER_RESPONSE_1_REAL");
		AllocateMemory(h_Quadrature_Filter_Response_1_Imag, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "QUADRATURE_FILTER_RESPONSE_1_IMAG");        
		AllocateMemory(h_Quadrature_Filter_Response_2_Real, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "QUADRATURE_FILTER_RESPONSE_2_REAL");
		AllocateMemory(h_Quadrature_Filter_Response_2_Imag, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "QUADRATURE_FILTER_RESPONSE_2_IMAG");        
		AllocateMemory(h_Quadrature_Filter_Response_3_Real, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "QUADRATURE_FILTER_RESPONSE_3_REAL");
		AllocateMemory(h_Quadrature_Filter_Response_3_Imag, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "QUADRATURE_FILTER_RESPONSE_3_IMAG");        
		AllocateMemory(h_Phase_Differences, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "PHASE_DIFFERENCES");        
		AllocateMemory(h_Phase_Certainties, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "PHASE_CERTAINTIES");        
		AllocateMemory(h_Phase_Gradients, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers, allNiftiImages, numberOfNiftiImages, "PHASE_GRADIENTS");        
    }
    
	endTime = GetWallTime();
    
	if (VERBOS)
 	{
		printf("It took %f seconds to allocate memory\n",(float)(endTime - startTime));
	}

	startTime = GetWallTime();

    // Convert data to floats
    if ( inputData->datatype == DT_SIGNED_SHORT )
    {
        short int *p = (short int*)inputData->data;
    
        for (int i = 0; i < DATA_W * DATA_H * DATA_D * DATA_T; i++)
        {
            h_fMRI_Volumes[i] = (float)p[i];
        }
    }
	else if ( inputData->datatype == DT_FLOAT )
    {
        float *p = (float*)inputData->data;
    
        for (int i = 0; i < DATA_W * DATA_H * DATA_D * DATA_T; i++)
        {
            h_fMRI_Volumes[i] = p[i];
        }
    }
    else
    {
        printf("Unknown data type in input data, aborting!\n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
        return EXIT_FAILURE;
    }
    
	endTime = GetWallTime();

	if (VERBOS)
 	{
		printf("It took %f seconds to convert data to floats\n",(float)(endTime - startTime));
	}

	startTime = GetWallTime();

    // Read quadrature filters, three real valued and three imaginary valued

	/*
	std::string path = Getexepath();
	path.erase(path.end()-16, path.end()); // 16 is the number of characters in 'MotionCorrection'
	std::string filter1RealName = path;
	std::string filter1ImagName = path;
	std::string filter2RealName = path;
	std::string filter2ImagName = path;
	std::string filter3RealName = path;
	std::string filter3ImagName = path;
	*/

	std::string filter1RealName;
	std::string filter1ImagName;
	std::string filter2RealName;
	std::string filter2ImagName;
	std::string filter3RealName;
	std::string filter3ImagName;

	filter1RealName.append("filter1_real_linear_registration.bin");
	filter1ImagName.append("filter1_imag_linear_registration.bin");
	filter2RealName.append("filter2_real_linear_registration.bin");
	filter2ImagName.append("filter2_imag_linear_registration.bin");
	filter3RealName.append("filter3_real_linear_registration.bin");
	filter3ImagName.append("filter3_imag_linear_registration.bin");

	ReadBinaryFile(h_Quadrature_Filter_1_Real,MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE,filter1RealName.c_str(),allMemoryPointers,numberOfMemoryPointers,allNiftiImages,numberOfNiftiImages); 
	ReadBinaryFile(h_Quadrature_Filter_1_Imag,MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE,filter1ImagName.c_str(),allMemoryPointers,numberOfMemoryPointers,allNiftiImages,numberOfNiftiImages); 
	ReadBinaryFile(h_Quadrature_Filter_2_Real,MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE,filter2RealName.c_str(),allMemoryPointers,numberOfMemoryPointers,allNiftiImages,numberOfNiftiImages); 
	ReadBinaryFile(h_Quadrature_Filter_2_Imag,MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE,filter2ImagName.c_str(),allMemoryPointers,numberOfMemoryPointers,allNiftiImages,numberOfNiftiImages); 
	ReadBinaryFile(h_Quadrature_Filter_3_Real,MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE,filter3RealName.c_str(),allMemoryPointers,numberOfMemoryPointers,allNiftiImages,numberOfNiftiImages); 
	ReadBinaryFile(h_Quadrature_Filter_3_Imag,MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE,filter3ImagName.c_str(),allMemoryPointers,numberOfMemoryPointers,allNiftiImages,numberOfNiftiImages);     
    
	endTime = GetWallTime();

	if (VERBOS)
 	{
		printf("It took %f seconds to read all binary files\n",(float)(endTime - startTime));
	}    

    //------------------------
    
	startTime = GetWallTime();

	// Initialize BROCCOLI
    BROCCOLI_LIB BROCCOLI(OPENCL_PLATFORM,OPENCL_DEVICE,2); // 2 = Bash wrapper

	endTime = GetWallTime();

	if (VERBOS)
 	{
		printf("It took %f seconds to initiate BROCCOLI\n",(float)(endTime - startTime));
	}
    
    // Something went wrong...
    if (!BROCCOLI.GetOpenCLInitiated())
    {              
        printf("Initialization error is \"%s\" \n",BROCCOLI.GetOpenCLInitializationError());
		printf("OpenCL error is \"%s\" \n",BROCCOLI.GetOpenCLError());

        // Print create kernel errors
        int* createKernelErrors = BROCCOLI.GetOpenCLCreateKernelErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (createKernelErrors[i] != 0)
            {
                printf("Create kernel error %i is %d \n",i,BROCCOLI.GetOpenCLErrorMessage(createKernelErrors[i]));
            }
        }                
        
        // Print build info to file    
        fp = fopen("buildinfo.txt","w");
        if (fp == NULL)
        {     
            printf("Could not open buildinfo.txt! \n");
        }
        if (BROCCOLI.GetOpenCLBuildInfoChar() != NULL)
        {
            int error = fputs(BROCCOLI.GetOpenCLBuildInfoChar(),fp);
            if (error == EOF)
            {
                printf("Could not write to buildinfo.txt! \n");
            }
        }
        fclose(fp);
                
        printf("OpenCL initialization failed, aborting! \nSee buildinfo.txt for output of OpenCL compilation!\n");      
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
        return EXIT_FAILURE;
    }
    // Initialization OK
    else
    {
        // Set all necessary pointers and values
        BROCCOLI.SetInputfMRIVolumes(h_fMRI_Volumes);
        
        BROCCOLI.SetEPIWidth(DATA_W);
        BROCCOLI.SetEPIHeight(DATA_H);
        BROCCOLI.SetEPIDepth(DATA_D);
        BROCCOLI.SetEPITimepoints(DATA_T);   
        
        BROCCOLI.SetEPIVoxelSizeX(EPI_VOXEL_SIZE_X);
        BROCCOLI.SetEPIVoxelSizeY(EPI_VOXEL_SIZE_Y);
        BROCCOLI.SetEPIVoxelSizeZ(EPI_VOXEL_SIZE_Z);        
        
        BROCCOLI.SetImageRegistrationFilterSize(MOTION_CORRECTION_FILTER_SIZE);
        BROCCOLI.SetLinearImageRegistrationFilters(h_Quadrature_Filter_1_Real, h_Quadrature_Filter_1_Imag, h_Quadrature_Filter_2_Real, h_Quadrature_Filter_2_Imag, h_Quadrature_Filter_3_Real, h_Quadrature_Filter_3_Imag);
        BROCCOLI.SetNumberOfIterationsForMotionCorrection(NUMBER_OF_ITERATIONS_FOR_MOTION_CORRECTION);
        
        BROCCOLI.SetOutputMotionCorrectedfMRIVolumes(h_Motion_Corrected_fMRI_Volumes);
        BROCCOLI.SetOutputMotionParameters(h_Motion_Parameters);
      
        if (DEBUG)
        {
			BROCCOLI.SetDebug(true);
            //BROCCOLI.SetOutputQuadratureFilterResponses(h_Quadrature_Filter_Response_1_Real, h_Quadrature_Filter_Response_1_Imag, h_Quadrature_Filter_Response_2_Real, h_Quadrature_Filter_Response_2_Imag, h_Quadrature_Filter_Response_3_Real, h_Quadrature_Filter_Response_3_Imag);
            BROCCOLI.SetOutputPhaseDifferences(h_Phase_Differences);
            BROCCOLI.SetOutputPhaseCertainties(h_Phase_Certainties);
            BROCCOLI.SetOutputPhaseGradients(h_Phase_Gradients);
        }
             
        // Run the actual motion correction
		startTime = GetWallTime();        
		BROCCOLI.PerformMotionCorrectionWrapper();        
		endTime = GetWallTime();

		if (VERBOS)
	 	{
			printf("\nIt took %f seconds to run the motion correction\n",(float)(endTime - startTime));
		}    

        // Print create buffer errors
        int* createBufferErrors = BROCCOLI.GetOpenCLCreateBufferErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (createBufferErrors[i] != 0)
            {
                printf("Create buffer error %i is %d \n",i,BROCCOLI.GetOpenCLErrorMessage(createBufferErrors[i]));
            }
        }
        
        // Print run kernel errors
        int* runKernelErrors = BROCCOLI.GetOpenCLRunKernelErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (runKernelErrors[i] != 0)
            {
                printf("Run kernel error %i is %d \n",i,BROCCOLI.GetOpenCLErrorMessage(runKernelErrors[i]));
            }
        } 
    }
    
        
    // Find max displacement
    float maxDisplacement = 0.0f;
    int maxVolume = 0;
    for (int t = 1; t < DATA_T; t++)
    {        
        float displacement = fabs(h_Motion_Parameters[t + 0*DATA_T]) + fabs(h_Motion_Parameters[t + 1*DATA_T]) + fabs(h_Motion_Parameters[t + 2*DATA_T]) + fabs(h_Motion_Parameters[t + 3*DATA_T]) + fabs(h_Motion_Parameters[t + 4*DATA_T]) + fabs(h_Motion_Parameters[t + 5*DATA_T]);
        
        if (displacement > maxDisplacement)
        {
            maxDisplacement = displacement;
            maxVolume = t;
        }
    }

    if (PRINT)
    {
        printf("Max displacement = %f (mm) at volume %i \n",maxDisplacement,maxVolume);
    }
            
    // Print motion parameters to file
    std::ofstream motion;
    motion.open("motion.1D");      
    if ( motion.good() )
    {
        //motion.setf(ios::scientific);
        motion.precision(6);
        for (int t = 0; t < DATA_T; t++)
        {
            //printf("X translation for timepoint %i is %f\n",t+1,h_Motion_Parameters[t + DATA_T]);
            //motion << h_Motion_Parameters[t + 0*DATA_T] << std::setw(2) << " " << h_Motion_Parameters[t + 1*DATA_T] << std::setw(2) << " " << h_Motion_Parameters[t + 2*DATA_T] << std::setw(2) << " " << h_Motion_Parameters[t + 3*DATA_T] << std::setw(2) << " " << h_Motion_Parameters[t + 4*DATA_T] << std::setw(2) << " " << h_Motion_Parameters[t + 5*DATA_T] << std::endl;
            motion << h_Motion_Parameters[t + 4*DATA_T] << std::setw(2) << " " << -h_Motion_Parameters[t + 3*DATA_T] << std::setw(2) << " " << h_Motion_Parameters[t + 5*DATA_T] << std::setw(2) << " " << -h_Motion_Parameters[t + 2*DATA_T] << std::setw(2) << " " << -h_Motion_Parameters[t + 0*DATA_T] << std::setw(2) << " " << -h_Motion_Parameters[t + 1*DATA_T] << std::endl;
        }
        motion.close();
    }
    else
    {
        printf("Could not open motion.1D for writing!\n");
    }
        
    // Write motion corrected data to file            
    startTime = GetWallTime();

    WriteNifti(inputData,h_Motion_Corrected_fMRI_Volumes,FILENAME_EXTENSION,ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
    
    if (DEBUG)
    {
        WriteNifti(inputData,h_Phase_Differences,"_phase_differences",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Phase_Gradients,"_phase_gradients",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Phase_Certainties,"_phase_certainties",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Quadrature_Filter_Response_1_Real,"_quadrature_filter_responses_1_real",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Quadrature_Filter_Response_1_Imag,"_quadrature_filter_responses_1_imag",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Quadrature_Filter_Response_2_Real,"_quadrature_filter_responses_2_real",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Quadrature_Filter_Response_2_Imag,"_quadrature_filter_responses_2_imag",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Quadrature_Filter_Response_3_Real,"_quadrature_filter_responses_3_real",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Quadrature_Filter_Response_3_Imag,"_quadrature_filter_responses_3_imag",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);        
    }

	endTime = GetWallTime();

	if (VERBOS)
 	{
		printf("It took %f seconds to write the nifti file\n",(float)(endTime - startTime));
	}
    
    // Free all memory
    FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);            
    FreeAllNiftiImages(allNiftiImages,numberOfNiftiImages);
    
    return EXIT_SUCCESS;
}
int main(int argc, char ** argv)
{
    //-----------------------
    // Input pointers
    
    float           *h_fMRI_Volumes = NULL;
    float           *h_Quadrature_Filter_1_Real = NULL;
    float           *h_Quadrature_Filter_2_Real = NULL;
    float           *h_Quadrature_Filter_3_Real = NULL;
    float           *h_Quadrature_Filter_1_Imag = NULL;
    float           *h_Quadrature_Filter_2_Imag = NULL;
    float           *h_Quadrature_Filter_3_Imag = NULL;
    
    void            *allMemoryPointers[500];
    int             numberOfMemoryPointers = 0;
    
    // Default parameters
    int             MOTION_CORRECTION_FILTER_SIZE = 7; 
    int             NUMBER_OF_ITERATIONS_FOR_MOTION_CORRECTION = 5;
    int             OPENCL_PLATFORM = 2;
    int             OPENCL_DEVICE = 0;
    int             NUMBER_OF_MOTION_CORRECTION_PARAMETERS = 6;    
    bool            DEBUG = false;
    //bool            DEBUG = true;
    const char*     FILENAME_EXTENSION = "_mc";
    bool            PRINT = true;
    
    int             DATA_W, DATA_H, DATA_D, DATA_T;
    float           EPI_VOXEL_SIZE_X, EPI_VOXEL_SIZE_Y, EPI_VOXEL_SIZE_Z;
    
    //-----------------------
    // Output parameters
    
    const char      *outputFilename;
    
    float           *h_Quadrature_Filter_Response_1_Real = NULL;
    float           *h_Quadrature_Filter_Response_1_Imag = NULL;
    float           *h_Quadrature_Filter_Response_2_Real = NULL;
    float           *h_Quadrature_Filter_Response_2_Imag = NULL;
    float           *h_Quadrature_Filter_Response_3_Real = NULL;
    float           *h_Quadrature_Filter_Response_3_Imag = NULL;  
    float           *h_Phase_Differences = NULL;
    float           *h_Phase_Certainties = NULL;
    float           *h_Phase_Gradients = NULL;
    float           *h_Motion_Corrected_fMRI_Volumes = NULL;
    float           *h_Motion_Parameters = NULL;
    
    //---------------------
    
    
    /* Input arguments */
    FILE *fp = NULL; 
    
    // No inputs, so print help text
    if (argc == 1)
    {        
        printf("Usage:\n\n");
        printf("MotionCorrection input.nii [options]\n\n");
        printf("Options:\n\n");
        printf("-platform   The OpenCL platform to use (default 0) \n");
        printf("-device     The OpenCL device to use for the specificed platform (default 0) \n");
        printf("-iterations Number of iterations for the motion correction algorithm (default 5) \n");        
        printf("-output     Set output filename (default input_mc.nii) \n");
        printf("-quiet      Don't print anything to the terminal (default false) \n");
        printf("-debug      Get additional debug information (default no) \n");
        printf("\n\n");
        
        return 1;
    }
    // Try to open file
    else if (argc > 1)
    {        
        fp = fopen(argv[1],"r");
        if (fp == NULL)
        {            
            printf("Could not open file %s !\n",argv[1]);
            return -1;
        }
        fclose(fp);        
    }
    
    // Loop over additional inputs
    int i = 2;
    while (i < argc)
    {
        char *input = argv[i];
        char *p;
        if (strcmp(input,"-platform") == 0)
        {
            OPENCL_PLATFORM = (int)strtol(argv[i+1], &p, 10);
            if (OPENCL_PLATFORM < 0)
            {
                printf("OpenCL platform must be >= 0!\n");
                return -1;
            }
            i += 2;
        }
        else if (strcmp(input,"-device") == 0)
        {
            OPENCL_DEVICE = (int)strtol(argv[i+1], &p, 10);
            if (OPENCL_DEVICE < 0)
            {
                printf("OpenCL device must be >= 0!\n");
                return -1;
            }
            i += 2;
        }
        else if (strcmp(input,"-iterations") == 0)
        {
            NUMBER_OF_ITERATIONS_FOR_MOTION_CORRECTION = (int)strtol(argv[i+1], &p, 10);
            if (NUMBER_OF_ITERATIONS_FOR_MOTION_CORRECTION <= 0)
            {
                printf("Number of iterations must be a positive number!\n");
                return -1;
            }
            i += 2;
        }
        else if (strcmp(input,"-debug") == 0)
        {
            DEBUG = true;
            i += 1;
        }
        else if (strcmp(input,"-quiet") == 0)
        {
            PRINT = false;
            i += 1;
        }
        else if (strcmp(input,"-output") == 0)
        {
            outputFilename = argv[i+1];
            i += 2;
        }
        else
        {
            printf("Unrecognized option! %s \n",argv[i]);
            return -1;
        }                
    }
    
    
    // Read data
    nifti_image *inputData = nifti_image_read(argv[1],1);
    
    if (inputData == NULL)
    {
        printf("Could not open nifti file!\n");
        return -1;
    }
    
    // Get data dimensions from input data
    DATA_W = inputData->nx;
    DATA_H = inputData->ny;
    DATA_D = inputData->nz;
    DATA_T = inputData->nt;
    
    // Get voxel sizes from input data
    EPI_VOXEL_SIZE_X = inputData->dx;
    EPI_VOXEL_SIZE_Y = inputData->dy;
    EPI_VOXEL_SIZE_Z = inputData->dz;
                               
    // Calculate size, in bytes
    int DATA_SIZE = DATA_W * DATA_H * DATA_D * DATA_T * sizeof(float);
    int MOTION_PARAMETERS_SIZE = NUMBER_OF_MOTION_CORRECTION_PARAMETERS * DATA_T * sizeof(float);
    int FILTER_SIZE = MOTION_CORRECTION_FILTER_SIZE * MOTION_CORRECTION_FILTER_SIZE * MOTION_CORRECTION_FILTER_SIZE * sizeof(float);
    int VOLUME_SIZE = DATA_W * DATA_H * DATA_D * sizeof(float);
    
    // Print some info
    if (PRINT)
    {
        printf("Authored by K.A. Eklund \n");
        printf("OpenCL Platform: %i OpenCL Device %i\n", OPENCL_PLATFORM, OPENCL_DEVICE);
        printf("Data size: %i x %i x %i x %i \n",  DATA_W, DATA_H, DATA_D, DATA_T);
        printf("Voxel size: %f x %f x %f mm \n", EPI_VOXEL_SIZE_X, EPI_VOXEL_SIZE_Y, EPI_VOXEL_SIZE_Z);    
        printf("Number of iterations for motion correction: %i \n",  NUMBER_OF_ITERATIONS_FOR_MOTION_CORRECTION);
    } 
    
    // ------------------------------------------------
    
    // Allocate memory on the host
    
    if (!AllocateMemory(h_fMRI_Volumes, DATA_SIZE, allMemoryPointers, numberOfMemoryPointers))
    {
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    
    if (!AllocateMemory(h_Motion_Corrected_fMRI_Volumes, DATA_SIZE, allMemoryPointers, numberOfMemoryPointers))
    {
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }        
    
    if (!AllocateMemory(h_Quadrature_Filter_1_Real, FILTER_SIZE, allMemoryPointers, numberOfMemoryPointers))
    {
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    
    if (!AllocateMemory(h_Quadrature_Filter_1_Imag, FILTER_SIZE, allMemoryPointers, numberOfMemoryPointers))
    {
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    
    if (!AllocateMemory(h_Quadrature_Filter_2_Real, FILTER_SIZE, allMemoryPointers, numberOfMemoryPointers))
    {
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    
    if (!AllocateMemory(h_Quadrature_Filter_2_Imag, FILTER_SIZE, allMemoryPointers, numberOfMemoryPointers))
    {
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    
    if (!AllocateMemory(h_Quadrature_Filter_3_Real, FILTER_SIZE, allMemoryPointers, numberOfMemoryPointers))
    {
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    
    if (!AllocateMemory(h_Quadrature_Filter_3_Imag, FILTER_SIZE, allMemoryPointers, numberOfMemoryPointers))
    {
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    
    if (!AllocateMemory(h_Motion_Parameters, MOTION_PARAMETERS_SIZE, allMemoryPointers, numberOfMemoryPointers))
    {
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    
    
    if (DEBUG)
    {    
        if (!AllocateMemory(h_Quadrature_Filter_Response_1_Real, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers))
        {
            FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
            nifti_image_free(inputData);
            return -1;
        }
        if (!AllocateMemory(h_Quadrature_Filter_Response_1_Imag, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers))
        {
            FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
            nifti_image_free(inputData);
            return -1;
        }
        if (!AllocateMemory(h_Quadrature_Filter_Response_2_Real, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers))
        {
            FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
            nifti_image_free(inputData);
            return -1;
        }
        if (!AllocateMemory(h_Quadrature_Filter_Response_2_Imag, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers))
        {
            FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
            nifti_image_free(inputData);
            return -1;
        }
        if (!AllocateMemory(h_Quadrature_Filter_Response_3_Real, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers))
        {
            FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
            nifti_image_free(inputData);
            return -1;
        }
        if (!AllocateMemory(h_Quadrature_Filter_Response_3_Imag, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers))
        {
            FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
            nifti_image_free(inputData);
            return -1;
        }
        if (!AllocateMemory(h_Phase_Differences, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers))
        {
            FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
            nifti_image_free(inputData);
            return -1;
        }
        if (!AllocateMemory(h_Phase_Certainties, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers))
        {
            FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
            nifti_image_free(inputData);
            return -1;
        }
        if (!AllocateMemory(h_Phase_Gradients, VOLUME_SIZE, allMemoryPointers, numberOfMemoryPointers))
        {
            FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
            nifti_image_free(inputData);
            return -1;
        }               
    }
    
    // Convert data to floats
    if ( inputData->datatype == DT_SIGNED_SHORT )
    {
        short int *p = (short int*)inputData->data;
    
        for (int i = 0; i < DATA_W * DATA_H * DATA_D * DATA_T; i++)
        {
            h_fMRI_Volumes[i] = (float)p[i];
        }
    }
    else
    {
        printf("Unknown data type in input data, aborting!\n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    
    // Read quadrature filters, three real valued and three imaginary valued
    fp = fopen("filter1_real_parametric_registration.bin","rb");
    if (fp != NULL)
    {
        fread(h_Quadrature_Filter_1_Real,sizeof(float),MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE,fp);
        fclose(fp);
    }
    else
    {
        printf("Could not open filter1_real_parametric_registration.bin \n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    
    fp = fopen("filter1_imag_parametric_registration.bin","rb");
    if (fp != NULL)
    {
        fread(h_Quadrature_Filter_1_Imag,sizeof(float),MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE,fp);        
        fclose(fp);
    }
    else
    {
        printf("Could not open filter1_imag_parametric_registration.bin \n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    
    fp = fopen("filter2_real_parametric_registration.bin","rb");
    if (fp != NULL)
    {
        fread(h_Quadrature_Filter_2_Real,sizeof(float),MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE,fp);
        fclose(fp);
    }
    else
    {
        printf("Could not open filter2_real_parametric_registration.bin \n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    
    fp = fopen("filter2_imag_parametric_registration.bin","rb");
    if (fp != NULL)
    {
        fread(h_Quadrature_Filter_2_Imag,sizeof(float),MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE,fp);
        fclose(fp);
    }
    else
    {
        printf("Could not open filter2_imag_parametric_registration.bin \n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    
    fp = fopen("filter3_real_parametric_registration.bin","rb");
    if (fp != NULL)
    {
        fread(h_Quadrature_Filter_3_Real,sizeof(float),MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE,fp);
        fclose(fp);
    }
    else
    {
        printf("Could not open filter3_real_parametric_registration.bin \n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    
    fp = fopen("filter3_imag_parametric_registration.bin","rb");
    if (fp != NULL)
    {
        fread(h_Quadrature_Filter_3_Imag,sizeof(float),MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE*MOTION_CORRECTION_FILTER_SIZE,fp);
        fclose(fp);
    }
    else
    {
        printf("Could not open filter3_imag_parametric_registration.bin \n");
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    
    //------------------------
    
    BROCCOLI_LIB BROCCOLI(OPENCL_PLATFORM,OPENCL_DEVICE);
    
    // Something went wrong...
    if (BROCCOLI.GetOpenCLInitiated() == 0)
    {              
        printf("Get platform IDs error is %d \n",BROCCOLI.GetOpenCLPlatformIDsError());
        printf("Get device IDs error is %d \n",BROCCOLI.GetOpenCLDeviceIDsError());
        printf("Create context error is %d \n",BROCCOLI.GetOpenCLCreateContextError());
        printf("Get create context info error is %d \n",BROCCOLI.GetOpenCLContextInfoError());
        printf("Create command queue error is %d \n",BROCCOLI.GetOpenCLCreateCommandQueueError());
        printf("Create program error is %d \n",BROCCOLI.GetOpenCLCreateProgramError());
        printf("Build program error is %d \n",BROCCOLI.GetOpenCLBuildProgramError());
        printf("Get program build info error is %d \n",BROCCOLI.GetOpenCLProgramBuildInfoError());
    
        // Print create kernel errors
        int* createKernelErrors = BROCCOLI.GetOpenCLCreateKernelErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (createKernelErrors[i] != 0)
            {
                printf("Create kernel error %i is %d \n",i,createKernelErrors[i]);
            }
        }                
        
        // Print build info to file    
        fp = fopen("buildinfo.txt","w");
        if (fp == NULL)
        {     
            printf("Could not open buildinfo.txt! \n");
        }
        if (BROCCOLI.GetOpenCLBuildInfoChar() != NULL)
        {
            int error = fputs(BROCCOLI.GetOpenCLBuildInfoChar(),fp);
            if (error == EOF)
            {
                printf("Could not write to buildinfo.txt! \n");
            }
        }
        fclose(fp);
        
        
        printf("OpenCL initialization failed, aborting! \nSee buildinfo.txt for output of OpenCL compilation!\n");      
        FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);
        nifti_image_free(inputData);
        return -1;
    }
    // Initialization OK
    else if (BROCCOLI.GetOpenCLInitiated() == 1)
    {
        // Set all necessary pointers and values
        BROCCOLI.SetInputfMRIVolumes(h_fMRI_Volumes);
        
        BROCCOLI.SetEPIWidth(DATA_W);
        BROCCOLI.SetEPIHeight(DATA_H);
        BROCCOLI.SetEPIDepth(DATA_D);
        BROCCOLI.SetEPITimepoints(DATA_T);   
        
        BROCCOLI.SetEPIVoxelSizeX(EPI_VOXEL_SIZE_X);
        BROCCOLI.SetEPIVoxelSizeY(EPI_VOXEL_SIZE_Y);
        BROCCOLI.SetEPIVoxelSizeZ(EPI_VOXEL_SIZE_Z);        
        
        BROCCOLI.SetImageRegistrationFilterSize(MOTION_CORRECTION_FILTER_SIZE);
        BROCCOLI.SetParametricImageRegistrationFilters(h_Quadrature_Filter_1_Real, h_Quadrature_Filter_1_Imag, h_Quadrature_Filter_2_Real, h_Quadrature_Filter_2_Imag, h_Quadrature_Filter_3_Real, h_Quadrature_Filter_3_Imag);
        BROCCOLI.SetNumberOfIterationsForMotionCorrection(NUMBER_OF_ITERATIONS_FOR_MOTION_CORRECTION);
        
        BROCCOLI.SetOutputMotionCorrectedfMRIVolumes(h_Motion_Corrected_fMRI_Volumes);
        BROCCOLI.SetOutputMotionParameters(h_Motion_Parameters);
      
        if (DEBUG)
        {
            //BROCCOLI.SetOutputQuadratureFilterResponses(h_Quadrature_Filter_Response_1_Real, h_Quadrature_Filter_Response_1_Imag, h_Quadrature_Filter_Response_2_Real, h_Quadrature_Filter_Response_2_Imag, h_Quadrature_Filter_Response_3_Real, h_Quadrature_Filter_Response_3_Imag);
            BROCCOLI.SetOutputPhaseDifferences(h_Phase_Differences);
            BROCCOLI.SetOutputPhaseCertainties(h_Phase_Certainties);
            BROCCOLI.SetOutputPhaseGradients(h_Phase_Gradients);
        }
             
        // Run the actual motion correction
        BROCCOLI.PerformMotionCorrectionWrapper();        
    
        // Print create buffer errors
        int* createBufferErrors = BROCCOLI.GetOpenCLCreateBufferErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (createBufferErrors[i] != 0)
            {
                printf("Create buffer error %i is %d \n",i,createBufferErrors[i]);
            }
        }
        
        // Print run kernel errors
        int* runKernelErrors = BROCCOLI.GetOpenCLRunKernelErrors();
        for (int i = 0; i < BROCCOLI.GetNumberOfOpenCLKernels(); i++)
        {
            if (runKernelErrors[i] != 0)
            {
                printf("Run kernel error %i is %d \n",i,runKernelErrors[i]);
            }
        } 
    }
    
        
    // Find max displacement
    float maxDisplacement = 0.0f;
    int maxVolume = 0;
    for (int t = 1; t < DATA_T; t++)
    {        
        float displacement = fabs(h_Motion_Parameters[t + 0*DATA_T]) + fabs(h_Motion_Parameters[t + 1*DATA_T]) + fabs(h_Motion_Parameters[t + 2*DATA_T]) + fabs(h_Motion_Parameters[t + 3*DATA_T]) + fabs(h_Motion_Parameters[t + 4*DATA_T]) + fabs(h_Motion_Parameters[t + 5*DATA_T]);
        
        if (displacement > maxDisplacement)
        {
            maxDisplacement = displacement;
            maxVolume = t;
        }
    }

    if (PRINT)
    {
        printf("Max displacement = %f (mm) at volume %i \n",maxDisplacement,maxVolume);
    }
            
    // Print motion parameters to file
    std::ofstream motion;
    motion.open("motion.1D");      
    if ( motion.good() )
    {
        //motion.setf(ios::scientific);
        motion.precision(6);
        for (int t = 0; t < DATA_T; t++)
        {
            //printf("X translation for timepoint %i is %f\n",t+1,h_Motion_Parameters[t + DATA_T]);
            //motion << h_Motion_Parameters[t + 0*DATA_T] << std::setw(2) << " " << h_Motion_Parameters[t + 1*DATA_T] << std::setw(2) << " " << h_Motion_Parameters[t + 2*DATA_T] << std::setw(2) << " " << h_Motion_Parameters[t + 3*DATA_T] << std::setw(2) << " " << h_Motion_Parameters[t + 4*DATA_T] << std::setw(2) << " " << h_Motion_Parameters[t + 5*DATA_T] << std::endl;
            motion << h_Motion_Parameters[t + 4*DATA_T] << std::setw(2) << " " << -h_Motion_Parameters[t + 3*DATA_T] << std::setw(2) << " " << h_Motion_Parameters[t + 5*DATA_T] << std::setw(2) << " " << -h_Motion_Parameters[t + 2*DATA_T] << std::setw(2) << " " << -h_Motion_Parameters[t + 0*DATA_T] << std::setw(2) << " " << -h_Motion_Parameters[t + 1*DATA_T] << std::endl;
        }
        motion.close();
    }
    else
    {
        printf("Could not open motion.1D for writing!\n");
    }
        
    // Write motion corrected data to file            
    WriteNifti(inputData,h_Motion_Corrected_fMRI_Volumes,FILENAME_EXTENSION,ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
    
    if (DEBUG)
    {
        WriteNifti(inputData,h_Phase_Differences,"_phase_differences",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Phase_Gradients,"_phase_gradients",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Phase_Certainties,"_phase_certainties",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Quadrature_Filter_Response_1_Real,"_quadrature_filter_responses_1_real",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Quadrature_Filter_Response_1_Imag,"_quadrature_filter_responses_1_imag",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Quadrature_Filter_Response_2_Real,"_quadrature_filter_responses_2_real",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Quadrature_Filter_Response_2_Imag,"_quadrature_filter_responses_2_imag",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Quadrature_Filter_Response_3_Real,"_quadrature_filter_responses_3_real",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);
        WriteNifti(inputData,h_Quadrature_Filter_Response_3_Imag,"_quadrature_filter_responses_3_imag",ADD_FILENAME,DONT_CHECK_EXISTING_FILE);        
    }
    
    // Free all memory
    FreeAllMemory(allMemoryPointers,numberOfMemoryPointers);            
    nifti_image_free(inputData);    
    
    return 1;
}