#include <iostream>
#include "math.h"
#include "TApplication.h"
#include "TF2.h"
#include "TMath.h"
#include "TTimer.h"
#include "TCanvas.h"
#include "TStyle.h"

TF2 *reint, *imint, *re, *im;
Double_t wnorm=1./(0.085*0.085);
bool man_intg=false;

Double_t interiorre(Double_t *x, Double_t *par)
{
	Double_t r = sqrt(pow((par[0]-x[0]),2.)+pow((par[1]-x[1]),2.)+pow(par[2],2.));
	Double_t focus=sqrt(0.085*0.085+0.035*0.035)-sqrt(x[0]*x[0]+x[1]*x[1]+0.085*0.085);//+(par[0]*x[0]+par[1]*x[1])*wnorm;

	int ishift = (int)((focus+r)/(500*1e-9));
	focus -= ishift*(500*1e-9);

	return TMath::Cos(par[3]*(r+focus))*par[2]/r;
}

Double_t interiorim(Double_t *x, Double_t *par)
{
	Double_t r = sqrt(pow((par[0]-x[0]),2)+pow((par[1]-x[1]),2)+pow(par[2],2));
	Double_t focus=sqrt(0.085*0.085+0.035*0.035)-sqrt(x[0]*x[0]+x[1]*x[1]+0.085*0.085);//+(par[0]*x[0]+par[1]*x[1])*wnorm;

	int ishift = (int)((r+focus)/(500*1e-9));
	focus -= ishift*(500*1e-9);

	return TMath::Sin(par[3]*(r+focus))*par[2]/r;
}


// Function returning real part integral for given image x
Double_t re_int(Double_t *x, Double_t *par)
{
	// i coordinates - coordinates of image
	Double_t xi=x[0], yi=x[1], zi=par[0], k=par[1];

	reint->SetParameters(xi, yi, zi, k);

	// Manual integration
	if(man_intg==true)
	{
		Double_t intg=0;

		for(Double_t i=-par[2]; i<=par[2]; i+=(par[2]/500))
			for(Double_t j=-par[3]; j<=par[3]; j+=(par[3]/500))
				intg+=(reint->Eval(i, j)/500);

		return intg*(2*par[2]*2*par[3]);
	}
	else // ROOT integration
		return reint->Integral(-par[2], par[2], -par[3], par[3]);
}

// Function returning imaginary part integral for given image x
Double_t im_int(Double_t *x, Double_t *par)
{
	// i coordinates - coordinates of image
	Double_t xi=x[0], yi=x[1], zi=par[0], k=par[1];

	imint->SetParameters(xi, yi, zi, k);

	// Manual integration
	if(man_intg==true)
	{
		Double_t intg=0;

		for(Double_t i=-par[2]; i<=par[2]; i+=(par[2]/500))
			for(Double_t j=-par[3]; j<=par[3]; j+=(par[3]/500))
				intg+=(imint->Eval(i, j)/500);

		return intg*(2*par[2]*2*par[3]);
	}
	else //ROOT integration
		return imint->Integral(-par[2], par[2], -par[3], par[3]);
}

Double_t psf_calc(Double_t *x, Double_t *par)
{
//	re->SetParameters(par[0], par[1], par[2], par[3]);
//	im->SetParameters(par[0], par[1], par[2], par[3]);

//	Double_t r=sqrt(x[0]*x[0]+x[1]*x[1]);

	Double_t r[]={x[0], x[1]};
	Double_t pars[]={par[0], par[1], par[2], par[3]};

	Double_t rre = re_int(r, pars);
        Double_t rim = im_int(r,pars);
	Double_t res = rre*rre+rim*rim;
	return res;
}


//int main(int argc, char **argv)
int psf2d(bool pint) 
{
	man_intg=pint;

        //TApplication theApp("zernike", &argc, argv);

	gStyle->SetPalette(1, 0);

	// How many pixels to display
	Double_t img_size_pix=5;
	Double_t pix_size=15*1e-6;
	Double_t img_radius=pix_size*img_size_pix;

	reint = new TF2("reint", interiorre, -img_radius, img_radius, -img_radius, img_radius, 4);
	imint = new TF2("imint", interiorim, -img_radius, img_radius, -img_radius, img_radius, 4);
	re = new TF2("re", re_int, -img_radius, img_radius, -img_radius, img_radius, 4);
	im = new TF2("im", im_int, -img_radius, img_radius, -img_radius, img_radius, 4);


	TF2 *psf = new TF2("psf", psf_calc, -img_radius, img_radius, -img_radius, img_radius, 4);
	psf->SetParameters(0.085, 2*TMath::Pi()/(500*1e-9), 0.035, 0.035);

	psf->SetNpx(10);
	psf->SetNpy(10);

	psf->Draw("colz");
/*
        cout << "Press any key to quit" << endl;
        TTimer *timer =  new TTimer("gSystem->ProcessEvents();", 50, kFALSE);
        timer->TurnOn();
        timer->Reset();

        TString a;
        cin >> a;
        if(strstr(a, "no")!=NULL)
        {
                timer->TurnOff();
                return 0;
        }
        timer->TurnOff();
*/	
	return 0;
	
}