#ifdef _OPENMP
#include <omp.h>
#endif
#include "w-stacking.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>

#ifdef __CUDACC__
#endif

void phase_correction(double* gridss, double* image_real, double* image_imag, int xaxis, int yaxis, int num_w_planes, int xaxistot, int yaxistot)
{
	double dnum_w_planes = (double)num_w_planes;
	double dxaxistot = (double)xaxistot;
	double dyaxistot = (double)yaxistot;

	for (int iw=0; iw<num_w_planes; iw++)
	{
            double wterm = (double)iw/dnum_w_planes;

	    for (int iv=0; iv<yaxis; iv++)
            for (int iu=0; iu<xaxis; iu++)
            {

		    long index = 2*(iu+iv*xaxis+xaxis*yaxis*iw);
		    long img_index = iu+iv*xaxis;
#ifdef PHASE_ON
                    double xcoord = (double)(iu-xaxistot/2);
                    if(xcoord < 0.0)xcoord = (double)(iu+xaxistot/2);
                    double ycoord = (double)(iv-yaxistot/2);
                    if(ycoord < 0.0)ycoord = (double)(iv+yaxistot/2);
		    xcoord = xcoord/dxaxistot;
		    ycoord = ycoord/dyaxistot;

		    double efact;
		    double preal, pimag;
		    double radius2 = (xcoord*xcoord+ycoord*ycoord);
		    if(xcoord <= 1.0)
		    {
			    preal = cos(2.0*PI*wterm*(sqrt(1-radius2)-1.0));
			    pimag = sin(2.0*PI*wterm*(sqrt(1-radius2)-1.0));
		    } else {
			    preal = cos(-2.0*PI*wterm*(sqrt(radius2-1.0)-1));
			    pimag = 0.0;
		    } 


		    double p,q,r,s;
		    p = gridss[index];
		    q = gridss[index+1];
		    r = preal;
		    s = pimag;

		    //printf("%d %d %d %ld %ld\n",iu,iv,iw,index,img_index);
		    image_real[img_index] += p*r-q*s;
		    image_imag[img_index] += p*s+q*r;
#else
		    image_real[img_index] += gridss[index];
		    image_imag[img_index] += gridss[index+1];
#endif

            }  
	}


}