/perf/fdk

#include <stdio.h>
#include <stdlib.h>

#include <math.h>
#include <pthread.h>
#include <stdint.h>
#include <string.h>

#include <limits.h>

#include <mex.h>

#include "/opt/intel/ipp/include/ippi.h"
#include "/opt/intel/ipp/include/ipps.h"
#include "/opt/intel/ipp/include/ippcore.h"


/* structure to pass arguments to thread */
struct thread_info /* Used as argument to thread_start() */
{
    pthread_t	thread_id;          /* ID returned by pthread_create() */
    int         thread_num;         /* Application-defined thread # */
    int        n_elements;
    int         n_proj;
    Ipp32f      cor;
    Ipp32f      d;
    Ipp32f      detector_size; 
    Ipp32f      pixel_size;
    Ipp32f      cor_offset;
    Ipp32f      detector_offset_u;
    Ipp32f      detector_offset_v;
    Ipp32f      detector_offset_z;
    Ipp32f      *source;
    Ipp32f      *u_detector; 
    Ipp32f      *v_detector; 
    Ipp32f      *n_detector;
    Ipp32f      *projections;
    Ipp32f      *slice_x;
    Ipp32f      *slice_y; 
    Ipp32f      *slice_coord_z;
    Ipp32f      *out_volume;
};

/* global variables */
pthread_mutex_t mutex;
int counter = 0;


void statusinfo(IppStatus st) 
{
    if ((int)st != 0)
    {
        printf("%d : %s\n", st, ippGetStatusString(st));
    }
}  


bool is_aligned(void *p, int N)
{
    return (uintptr_t)p % N == 0;
}


/* canonical multiplication of square matrices */
int mult(Ipp32f *a, Ipp32f *b, Ipp32f *c) 
{
    int i, j, k;
	
    for (k = 0; k < 4; k++) 
    {
        for (i = 0; i < 4; i++) 
        {
            for (j = 0; j < 4; j++) 
            {
                c[i * 4 + j] += a[i * 4 + k] * b[k * 4 + j];
            }
        }
    }

    return 0;
} /* mult */


void *process (void *args) 
{
    struct thread_info *t_info = (thread_info*) args;
    
    Ipp32f *px_map = NULL, *py_map = NULL;
    Ipp32f *tmp_2 = NULL;
    Ipp32f *current_slice = NULL;
    
    Ipp32f angle, z, w_x, w_y, w_z;
    
    IppiSize im_size;
    IppiRect im_roi_size;
    
    int imStepBytes, ippStepBytes;
    
    int i_angle, slice_number;
    
    long idx;
    
    /* image size */
    im_size  = {t_info->n_elements, t_info->n_elements};
        
    /* region of interest = radisograph and slice size */
    im_roi_size = {0, 0, t_info->n_elements, t_info->n_elements};
    
    /* step in bytes */
    imStepBytes = t_info->n_elements * sizeof(float);

    /* allocate temporal array */
    tmp_2 = ippiMalloc_32f_C1(t_info->n_elements, t_info->n_elements, &ippStepBytes);
    if (tmp_2 == NULL) 
    {
        printf("Cannot allocate tmp_2");
	exit(-1);
    }

    current_slice = ippiMalloc_32f_C1(t_info->n_elements, t_info->n_elements, &ippStepBytes);
    if (current_slice == NULL) 
    {
        printf("Cannot allocate current_slice");
	exit(-1);
    }
    
    /* allocate interpolation maps */
    px_map = ippiMalloc_32f_C1(t_info->n_elements, t_info->n_elements, &ippStepBytes);
    if (px_map == NULL) 
    {
        printf("Cannot allocate px_map");
	exit(-1);
    }
    
    py_map = ippiMalloc_32f_C1(t_info->n_elements, t_info->n_elements, &ippStepBytes);
    if (py_map == NULL) 
    {
        printf("Cannot allocate py_map");
	exit(-1);
    }
        
    
    while (1)
    {
        /* counter increment */
retry: 
        int err = pthread_mutex_lock(&mutex);
        if (err) 
        {
    	    printf("Retrying\n");
    	    goto retry;
    	}

        slice_number = counter++;
        //if (counter > t_info->n_elements) 
        if (counter > 2) 
        {
            pthread_mutex_unlock(&mutex);
            break;
        }
        
        pthread_mutex_unlock(&mutex);
        
        /* z coordinate of current slice */
        z = t_info->slice_coord_z[slice_number];
    
        /* set current slice to zero */ 
        statusinfo(ippiSet_32f_C1R((Ipp32f)0, current_slice, ippStepBytes, im_size));
        
        for (i_angle = 0; i_angle < t_info->n_proj; i_angle++)
        {    
            /* set temporal variable to zero */
            statusinfo(ippiSet_32f_C1R((Ipp32f)0, tmp_2, ippStepBytes, im_size));
            
            /* current rotation angle */
            angle = 2*M_PI/t_info->n_proj*i_angle;
            
            /* set some matrices to calculate forward projection matrix operator in homogeneous coordinates*/
            Ipp32f P1[4 * 4] = {(t_info->d + t_info->detector_offset_z) / t_info->pixel_size, 0, (t_info->detector_size / 2 - t_info->detector_offset_u) / t_info->pixel_size, 0,
                0, -(t_info->d + t_info->detector_offset_z) / t_info->pixel_size, (t_info->detector_size / 2 - t_info->detector_offset_v) / t_info->pixel_size, 0,
                0, 0, 1, 0,
                0, 0, 0, 1};
        
            Ipp32f P2[4 * 4] = {t_info->u_detector[0], t_info->u_detector[1], t_info->u_detector[2], 0,
                t_info->v_detector[0], t_info->v_detector[1], t_info->v_detector[2], 0,
                t_info->n_detector[0], t_info->n_detector[1], t_info->n_detector[2], 0,
                0, 0, 0, 1};
        
            Ipp32f P3[4 * 4] = {1, 0, 0, -t_info->source[0],
                0, 1, 0, -t_info->source[1],
                0, 0, 1, -t_info->source[2],
                0, 0, 0, 1};
        
            Ipp32f P4[4 * 4] = {1, 0, 0, 0,
                0, 1, 0, 0,
                0, 0, 1, t_info->cor + t_info->cor_offset,
                0, 0, 0, 1};
        
            Ipp32f P5[4 * 4] = {(Ipp32f)(cosf(angle)), 0, (Ipp32f)(-sinf(angle)), 0,
                0, 1, 0, 0,
                (Ipp32f)(sinf(angle)), 0, (Ipp32f)(cosf(angle)), 0,
                0, 0, 0, 1};
            
            /* set to zero temporal arrays */
            Ipp32f P_tmp_1[4 * 4] = {0};
            Ipp32f P_tmp_2[4 * 4] = {0};
            Ipp32f P_tmp_3[4 * 4] = {0};
            Ipp32f P[4 * 4] = {0};
            
            /* forward projection operator */
            mult(P1, P2, P_tmp_1);
            mult(P_tmp_1, P3, P_tmp_2);
            mult(P_tmp_2, P4, P_tmp_3);
            mult(P_tmp_3, P5, P);
            
            for (int i = 0; i < t_info->n_elements; i++)
            {
                for (int j = 0; j < t_info->n_elements; j++)
                {
                    idx = i * t_info->n_elements + j;
                    
                    w_x = P[0] * t_info->slice_x[idx] + P[1] * t_info->slice_y[idx] + P[2] * z + P[3];
                    w_y = P[4] * t_info->slice_x[idx] + P[5] * t_info->slice_y[idx] + P[6] * z + P[7];
                    w_z = P[8] * t_info->slice_x[idx] + P[9] * t_info->slice_y[idx] + P[10] * z + P[11];
                    
                    px_map[idx] =  w_x / w_z;
                    py_map[idx] =  w_y / w_z;
                }
            }
            
            /* interpolate */
            statusinfo(ippiRemap_32f_C1R(t_info->projections + (ippStepBytes / sizeof(float) * (long)t_info->n_elements * i_angle), im_size, ippStepBytes, im_roi_size, px_map, ippStepBytes, py_map, ippStepBytes, tmp_2, ippStepBytes, im_size, IPPI_INTER_LINEAR));

            /* accumulate */
            /* in-place addition of two matrices */
            statusinfo(ippiAdd_32f_C1IR(tmp_2, ippStepBytes, current_slice, ippStepBytes, im_size));
        }
        
        /* write current slice to final array */
        //memcpy(t_info->out_volume + (t_info->n_elements * t_info->n_elements * slice_number), current_slice, t_info->n_elements * t_info->n_elements * sizeof(float));
        statusinfo(ippiCopy_32f_C1R(current_slice, ippStepBytes, t_info->out_volume + ((long)t_info->n_elements * t_info->n_elements * slice_number), imStepBytes, im_size));
    }
    
    /* free memory */
    ippiFree(px_map);
    ippiFree(py_map);
    ippiFree(tmp_2);
    ippiFree(current_slice); 
} /* process */



void mexFunction( int nlhs, mxArray *plhs[],
        int nrhs, const mxArray *prhs[])
{
    /* prhs[0]      int   n_voxels */
    /* prhs[1]      int   n_proj */
    /* prhs[2]      float   cor */
    /* prhs[3]      float   d */
    /* prhs[4]      float   detector_size */
    /* prhs[5]      float   pixel_size */
    /* prhs[6]      float   cor_offset */
    /* prhs[7]      float   detector_offset_u */
    /* prhs[8]      float   detector_offset_v */
    /* prhs[9]      float   detector_offset_z */
    /* prhs[10]      float   s */
    /* prhs[11]      float   u_detector */
    /* prhs[12]      float   v_detector */
    /* prhs[13]      float   n_detector_r */
    /* prhs[14]      float   projections */
    /* prhs[15]      float   slice_x */
    /* prhs[16]      float   slice_y */
    /* prhs[17]      float   slice_coord_z */
    /* prhs[18]      int     n_threads */
    
    int n_proj, n_threads;
    int n_elements;
    Ipp32f cor, d, detector_size, pixel_size, cor_offset, detector_offset_u, detector_offset_v, detector_offset_z;
    Ipp32f *source = NULL, *u_detector = NULL, *v_detector = NULL, *n_detector = NULL;
    Ipp32f *projections = NULL;
    Ipp32f *slice_x = NULL, *slice_y = NULL, *slice_coord_z = NULL;
    Ipp32f *out_volume = NULL;
    
    int tnum, m;
    struct thread_info *t_info;
    void *res;
    
    int alignment = 32;
    
    //////////// PARSE INPUT //////////////////
    
    /* radiograph and slice size */
    /* check n_voxels data type */
    if (mxGetNumberOfElements(prhs[0])!=1 || !mxIsInt32(prhs[0]))
    {
        mexErrMsgTxt("mex: Input n_voxels is not scalar or int32.");
    }
     
    n_elements = (int)mxGetScalar(prhs[0]);
    
    if (n_elements % 16 != 0)
    {
        printf("n_elements has to be multiple of 16 since IPPI uses 64 bytes alignment");
        exit(-1);
    }

    
    /* number of projections */
    /* check n_proj data type */
    if (mxGetNumberOfElements(prhs[1])!=1 || !mxIsInt32(prhs[1]))
    {
        mexErrMsgTxt("mex: Input n_proj is not scalar or int32.");
    }
    
    n_proj = (int)mxGetScalar(prhs[1]);
    
    
    /* source to rotation distance */
    /* check input cor data type */
    if (!mxIsSingle(prhs[2]) || mxIsComplex(prhs[2]) || mxGetNumberOfElements(prhs[2])!=1)
    {
        mexErrMsgTxt("mex: Input cor must be scalar and have type single.");
    }
    
    cor = (Ipp32f)mxGetScalar(prhs[2]);
    
    /* source to detector distance */
    /* check input d data type */
    if (!mxIsSingle(prhs[3]) || mxIsComplex(prhs[3]) || mxGetNumberOfElements(prhs[3])!=1)
    {
        mexErrMsgTxt("mex: Input d must be scalar and have type single.");
    }
    
    d = (Ipp32f)mxGetScalar(prhs[3]);
    
    
    /* physical size of detector (assumed to be square) */
    /* check input detector_size data type */
    if (!mxIsSingle(prhs[4]) || mxIsComplex(prhs[4]) || mxGetNumberOfElements(prhs[4])!=1)
    {
        mexErrMsgTxt("mex: Input detector_size must be scalar and have type single.");
    }
    
    detector_size = (Ipp32f)mxGetScalar(prhs[4]);
    
    
    /* physical pixel size */
    /* check input pixel_size data type */
    if (!mxIsSingle(prhs[5]) || mxIsComplex(prhs[5]) || mxGetNumberOfElements(prhs[5])!=1)
    {
        mexErrMsgTxt("mex: Input pixel_size must be scalar and have type single.");
    }
    
    pixel_size = (Ipp32f)mxGetScalar(prhs[5]);
   
    
    /* offset in cor */
    /* check input cor_offset data type */
    if (!mxIsSingle(prhs[6]) || mxIsComplex(prhs[6]) || mxGetNumberOfElements(prhs[6])!=1)
    {
        mexErrMsgTxt("mex: Input cor_offset must be scalar and have type single.");
    }
    
    cor_offset = (Ipp32f)mxGetScalar(prhs[6]);
    
    
    /* detector offset in U direction */
    /* check input detector_offset_u data type */
    if (!mxIsSingle(prhs[7]) || mxIsComplex(prhs[7]) || mxGetNumberOfElements(prhs[7])!=1)
    {
        mexErrMsgTxt("mex: Input detector_offset_u must be scalar and have type single.");
    }
    
    detector_offset_u = (Ipp32f)mxGetScalar(prhs[7]);
    
    
    /* detector offset in V direction */
    /* check input detector_offset_v data type */
    if (!mxIsSingle(prhs[8]) || mxIsComplex(prhs[8]) || mxGetNumberOfElements(prhs[8])!=1)
    {
        mexErrMsgTxt("mex: Input detector_offset_v must be scalar and have type single.");
    }
    
    detector_offset_v = (Ipp32f)mxGetScalar(prhs[8]);
    
    
    /* detector offset along magnification line */
    /* check input detector_offset_z data type */
    if (!mxIsSingle(prhs[9]) || mxIsComplex(prhs[9]) || mxGetNumberOfElements(prhs[9])!=1)
    {
        mexErrMsgTxt("mex: Input detector_offset_z must be scalar and have type single.");
    }
    
    detector_offset_z = (Ipp32f)mxGetScalar(prhs[9]);
    
    
    /* source position */
    /* check input source data type */
    if (!mxIsSingle(prhs[10]) || mxIsComplex(prhs[10]) || mxGetNumberOfElements(prhs[10])!=3)
    {
        mexErrMsgTxt("mex: Input source must be type single and contain 3 elements.");
    }
    
    source = (Ipp32f *)mxGetData(prhs[10]);
    
    if (source == NULL) 
    {
        printf("Cannot get source");
	exit(-1);
    }
    
    
    /* detector coordinate frame */
    /* check input u_detector data type */
    if( !mxIsSingle(prhs[11]) || mxIsComplex(prhs[11]) || mxGetNumberOfElements(prhs[11])!=3)
    {
        mexErrMsgTxt("mex: Input u_detector must be type single and contain 3 elements.");
    }
    
    /* check input v_detector data type */
    if( !mxIsSingle(prhs[12]) || mxIsComplex(prhs[12]) || mxGetNumberOfElements(prhs[12])!=3)
    {
        mexErrMsgTxt("mex: Input v_detector must be type single and contain 3 elements.");
    }
    
    /* check input n_detector data type */
    if( !mxIsSingle(prhs[13]) || mxIsComplex(prhs[13]) || mxGetNumberOfElements(prhs[13])!=3)
    {
        mexErrMsgTxt("mex: Input n_detector must be type single and contain 3 elements.");
    }
    
    u_detector = (Ipp32f *)mxGetData(prhs[11]);
    
    if (u_detector == NULL) 
    {
        printf("Cannot get u_detector");
	exit(-1);
    }
    
    v_detector = (Ipp32f *)mxGetData(prhs[12]);
    
    if (u_detector == NULL) 
    {
        printf("Cannot get v_detector");
	exit(-1);
    }
    
    n_detector = (Ipp32f *)mxGetData(prhs[13]);
    
    if (n_detector == NULL) 
    {
        printf("Cannot get n_detector");
	exit(-1);
    }
    
    
    /* volume with projections, x-y-angle */
    /* check input projections data type */
    if (!mxIsSingle(prhs[14]) || mxIsComplex(prhs[14]))
    {
        mexErrMsgTxt("mex: Input projections array must be type single.");
    }
    
    /* check that number of elements in projections is equal to n_voxels x n_voxels x n_proj */
    if ((mxGetNumberOfElements(prhs[14]) != (long)n_elements * n_elements * n_proj))
    {
        mexErrMsgTxt("mex: Input projections must be a n_elements x n_elements x n_proj array.");
    }
    
    projections = (Ipp32f *)mxGetData(prhs[14]);
    
    if (projections == NULL) {
        printf("Can not get projections");
	exit(-1);
    }
    
    /* check if dataset is aligned by 32 bytes */
    if (is_aligned(projections, (int)alignment) == 0)
    {
        printf("Projections have to be aligned by 32 bytes\n");
        exit(-1);
    }
    
    
    /* grids */
    /* check input slice_x data type */
    if ( !mxIsSingle(prhs[15]) || mxIsComplex(prhs[15]))
    {
        mexErrMsgTxt("mex: Input slice_x array must be type single.");
    }
    
    /* check that number of rows  and columns in slice_x is equal to n_voxels */
    if(mxGetM(prhs[15]) != n_elements || mxGetN(prhs[15]) != n_elements)
    {
        mexErrMsgTxt("mex: Input slice_x must be a n_voxels x n_elements array.");
    }
    
    /* check input slice_y data type */
    if( !mxIsSingle(prhs[16]) || mxIsComplex(prhs[16]))
    {
        mexErrMsgTxt("mex: Input slice_y array must be type single.");
    }
    
    /* check that number of rows  and columns in slice_y is equal to n_voxels */
    if(mxGetM(prhs[16]) != n_elements || mxGetN(prhs[16]) != n_elements)
    {
        mexErrMsgTxt("mex: Input slice_y must be a n_voxels x n_elements array.");
    }
    
    /* check input slice_coord_z data type */
    if( !mxIsSingle(prhs[17]) || mxIsComplex(prhs[17]))
    {
        mexErrMsgTxt("mex: Input slice_coord_z array must be type single.");
    }
    
    /* check that number of elements in slice_coord_z is equal to n_voxels */
    if(mxGetNumberOfElements(prhs[17]) != n_elements)
    {
        mexErrMsgTxt("mex: Input slice_coord_z must be a n_voxels array.");
    }
    
    slice_x = (Ipp32f *)mxGetData(prhs[15]);
    
    if (slice_x == NULL) 
    {
        printf("Cannot get slice_x");
	exit(-1);
    }
    
    /* check if slice_x is aligned by 32 bytes */
    if (is_aligned(slice_x, (int)alignment) == 0)
    {
        printf("slice_x have to be aligned by 32 bytes\n");
        exit(-1);
    }
    
    slice_y = (Ipp32f *)mxGetData(prhs[16]);
    
    if (slice_y == NULL) 
    {
        printf("Cannot get slice_y");
	exit(-1);
    }
    
    /* check if slice_y is aligned by 32 bytes */
    if (is_aligned(slice_y, (int)alignment) == 0)
    {
        printf("slice_y have to be aligned by 32 bytes\n");
        exit(-1);
    }
    
    slice_coord_z = (Ipp32f *)mxGetData(prhs[17]);
    
    if (slice_coord_z == NULL) {
        printf("Cannot get slice_coord_z");
	exit(-1);
    }
    
    
    /* n_threads */
    /* check input n_threads data type */
    if(mxGetNumberOfElements(prhs[18])!=1 || !mxIsInt32(prhs[18]))
    {
        mexErrMsgTxt("mex: Input n_threads is not scalar or int32.");
    }
    
    n_threads = (int)mxGetScalar(prhs[18]);
    
    
    /* create the output matrix im_size x im_size*/
    mwSize dims[3];
    dims[0] = n_elements;
    dims[1] = n_elements;
    dims[2] = n_elements;
    plhs[0] = mxCreateNumericArray((mwSize)3,dims,mxSINGLE_CLASS,mxREAL);
    
    /* get a pointer to the data in the output matrix */
    out_volume = (Ipp32f *)mxGetData(plhs[0]);
    
    if (out_volume == NULL) {
        printf("Cannot get out_volume");
	exit(-1);
    }
    
    ///////// INITIALIZE THREADS //////////////
    
    /* set global variable counter to zero */
    counter = 0;
    
    /* initialize mutex */
    m = pthread_mutex_init(&mutex, NULL);
    
    if (m != 0)
    {
        printf("Mutex initialization failed with error %i\n", m);
        exit(-1);
    }
    
    /* allocate memory for threads */
    t_info = (thread_info*) calloc(n_threads, sizeof(struct thread_info));
    
    if (t_info == NULL)
    {
        printf("Structure t_info memory allocation error\n");
        exit(-1);
    }
    
    /* fill tread_info structure and create threads */
    for (tnum = 0; tnum < n_threads; tnum++)
    {
        t_info[tnum].thread_num = tnum;                      /* Application-defined thread # */
        t_info[tnum].n_elements = n_elements;
        t_info[tnum].n_proj = n_proj;
        t_info[tnum].cor = cor;
        t_info[tnum].d = d;
        t_info[tnum].detector_size = detector_size; 
        t_info[tnum].pixel_size = pixel_size;
        t_info[tnum].cor_offset = cor_offset;
        t_info[tnum].detector_offset_u = detector_offset_u;
        t_info[tnum].detector_offset_v = detector_offset_v;
        t_info[tnum].detector_offset_z = detector_offset_z;
        t_info[tnum].source = source;
        t_info[tnum].u_detector = u_detector; 
        t_info[tnum].v_detector = v_detector; 
        t_info[tnum].n_detector = n_detector;
        t_info[tnum].projections = projections;
        t_info[tnum].slice_x = slice_x;
        t_info[tnum].slice_y = slice_y; 
        t_info[tnum].slice_coord_z = slice_coord_z;
        t_info[tnum].out_volume = out_volume;
        
        m = pthread_create(&t_info[tnum].thread_id, NULL, &process, &t_info[tnum]);
        
        if (m != 0)
        {
            printf("Cannot create thread with error %i\n", m);
            exit(-1);
        }
    }
    
    /* join threads */
    for (tnum = 0; tnum < n_threads; tnum++)
    {
        m = pthread_join(t_info[tnum].thread_id, &res);
        
        if (m != 0)
        {
            printf("Cannot join threads with error %i\n", m);
            exit(-1);
        }
    }
    
    /* free memory */
    pthread_mutex_destroy(&mutex);
    
    free(t_info);
    
} /* mexFunction */