#include "iostream.h"
#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);

Double_t interiorre(Double_t *x, Double_t *par)
{
	Double_t trsigma=0.04;
	Double_t r2 = x[0]*x[0]+x[1]*x[1];

	Double_t r = sqrt(pow((par[0]-x[0]),2.)+pow((par[1]-x[1]),2.)+pow(par[2],2.));
	Double_t lens=1;

	double trans=TMath::Exp(-0.5*r2/trsigma/trsigma);
trans=1;

	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;

	double dist0 = TMath::Sqrt(x[0]*x[0]+x[1]*x[1]+par[2]*par[2]);

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

//	cout << x[0]*1000 << " " << x[1]*1000 << " " << par[0]*1000 << " " << par[1]*1000 << " " << r*1000 << " " << focus*1000 << " " << par[3]*lens*(r+focus) << " " << TMath::Cos(par[3]*lens*(r+focus)) << " " << TMath::Cos(par[3]*lens*(r+focus))*par[2]/r << " "  << par[2] << " " << r+focus << endl;

//	focus=0;

//	Double_t tilt=0.03*x[0];

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

Double_t interiorim(Double_t *x, Double_t *par)
{
	Double_t trsigma=0.04;
	Double_t r2 = x[0]*x[0]+x[1]*x[1];

	Double_t r = sqrt(pow((par[0]-x[0]),2)+pow((par[1]-x[1]),2)+pow(par[2],2));
	Double_t lens=1;

	double trans=TMath::Exp(-0.5*r2/trsigma/trsigma);
	trans=1;

	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;

	double dist0 = TMath::Sqrt(x[0]*x[0]+x[1]*x[1]+par[2]*par[2]);

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


//	focus=0;

//	Double_t tilt=0.03*x[0];

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


// 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);

	Double_t intg=0;

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

	return intg;

	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);

	Double_t intg=0;

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

	return intg;

	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 res = pow(re_int(r, pars),2)+pow(im_int(r,pars),2);//*par[1]/(2*TMath::Pi());
	return res;

//	return re->Eval(x[0], x[1])*re->Eval(x[0], x[1])+im->Eval(x[0], x[1])*im->Eval(x[0], x[1])*par[1]/(2*TMath::Pi());
}


int main(int argc, char **argv)
{
        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;
	
}