#include <Riostream.h>
#include <TFile.h>
#include <TTree.h>
#include <TString.h>
#include <TROOT.h>
#include <TStopwatch.h>
#include <TSpectrum2.h>
#include <TSpectrum.h>
#include <iostream>
#include <fstream>
#include <cstring>
#include <vector>
#include "TCanvas.h"
#include "TAxis.h"
#include "TStyle.h"
#include "TGraphErrors.h"
#include "TMultiGraph.h"
#include "TLegend.h"
#include "TF1.h"
#include "TH1F.h"
#include "TF2.h"
#include "TH2F.h"
#include "TMath.h"
#include <math.h>
#include "TGeoMatrix.h"
#include "TRandom.h"

#define PI 3.14159265
using namespace std;



void Deconvolution2D_simple()
{


    Int_t i, j, xmin = -50, xmax = 50, ymin = -50, ymax = 50;

    const Int_t nbinsx = 100;
    const Int_t nbinsy = 100;

    Int_t lhx,lhy,i1,i2,j1,j2,k1,k2,lindex,i1min,i1max,i2min,i2max,j1min,j1max,j2min,j2max;

    Int_t positx=0, posity=0;
    Double_t lda,ldb,ldc,area,maximum=0;


    Double_t ** source = new Double_t*[nbinsx];
    Double_t ** response = new Double_t*[nbinsx];
    Double_t ** tempo = new Double_t*[nbinsx];
    Double_t ** tempo_2 = new Double_t*[nbinsx];

    for (i=0;i<nbinsx;i++)
    {
        source [i] = new Double_t [nbinsy];
        response [i] = new Double_t [nbinsy];
        tempo [i] = new Double_t [nbinsy];
        tempo_2 [i] = new Double_t [nbinsy];
    }

    TH2F *h_PSF = new TH2F("h_PSF","h_PSF",nbinsx,xmin,xmax,nbinsy,ymin,ymax);
    TH2F *h_source = new TH2F("h_source","h_source",nbinsx,xmin,xmax,nbinsy,ymin,ymax);
    TH2F *h_decon = new TH2F("h_decon","h_decon",nbinsx,xmin,xmax,nbinsy,ymin,ymax);

    //Remplisage matrice reponse
    ifstream file_s("source.txt");
    for (Int_t i = 0; i < nbinsx; i++)
    {
        for (Int_t j = 0; j < nbinsy; j++)
        {
            file_s >> tempo_2[i][j];
        }
    }
    for (Int_t i = 0; i < nbinsx; i++)
    {
        for (Int_t j = 0; j < nbinsy; j++)
        {
            if (tempo_2[i][j] < 0.01)
            {
                source[i][j] = 0.01;
            }
            else
            {
                source[i][j] = tempo_2[i][j]/11;
            }
        }
    }
    for (i = 0; i < nbinsx; i++)
    {
        for (j = 0; j < nbinsy; j++)
        {
            h_source->SetBinContent(i,j,source[i][j]);
        }
    }

    TCanvas * c1 = new TCanvas("c1", "c1", 800, 800);
    gStyle->SetPalette(55);
    h_source->GetZaxis()->SetRangeUser(0,1);
    h_source->GetXaxis()->SetRangeUser(-10000,10000);
    h_source->GetYaxis()->SetRangeUser(-10000,10000);

    h_source->SetTitle("Image");
    h_source->GetXaxis()->SetTitle("um");
    h_source->GetYaxis()->SetTitle("");
    h_source->Draw("colz");

    //Remplisage matrice reponse
    ifstream file("PSF_100.txt");
    for (Int_t i = 0; i < nbinsx; i++)
    {
        for (Int_t j = 0; j < nbinsy; j++)
        {
            file >> tempo[i][j];
        }
    }
    Int_t decaleur = 0;
    for (Int_t i = 0; i < nbinsx-decaleur; i++)
    {
        for (Int_t j = 0; j < nbinsy-decaleur; j++)
        {
            if (tempo[i+decaleur][j+decaleur] < 0.01)
            {
                response[i][j] = 0.01;
            }
            else
            {
                response[i][j] = tempo[i+decaleur][j+decaleur];
            }
        }
    }
    for (i = 0; i < nbinsx; i++)
    {
        for (Int_t j = 0; j < nbinsy; j++)
        {
            if (response[i][j] < 0.01)
            {
                response[i][j] = 0.01;
            }
            else
            {
                response[i][j] = response[i][j];
            }
        }
    }

    for (i = 0; i < nbinsx; i++)
    {
        for (j = 0; j < nbinsy; j++)
        {
            h_PSF->SetBinContent(i,j,response[i][j]);
        }
    }

    TCanvas * c2 = new TCanvas("c2", "c2", 800, 800);
    gStyle->SetPalette(55);
    h_PSF->GetZaxis()->SetRangeUser(0,1);
    h_PSF->GetXaxis()->SetRangeUser(-10000,10000);
    h_PSF->GetYaxis()->SetRangeUser(-10000,10000);

    h_PSF->SetTitle("PSF");
    h_PSF->GetXaxis()->SetTitle("um");
    h_PSF->GetYaxis()->SetTitle("");
    h_PSF->Draw("colz");

//1st choice with bug
//-----------------------------------------------------//

TSpectrum2 *s = new TSpectrum2();
s->Deconvolution(source,response,nbinsx,nbinsx,10,1,1);

//2nd without bug
//-----------------------------------------------------//

/*int ssizex = nbinsx;
    int ssizey = nbinsy;
    Double_t boost = 1;
    Int_t   repet;
    positx = 0; posity = 0;
    int numberIterations = 10;
    int numberRepetitions = 1;*/

//Original in TSpectrum2.cxx

    /*Double_t **working_space = new Double_t *[ssizex];
    for (i = 0; i < ssizex; i++)
    working_space[i] = new Double_t[5 * ssizey];    */

/*Double_t **working_space = new Double_t *[2*ssizex];
for (i = 0; i < 2*ssizex; i++)
working_space[i] = new Double_t[10 * ssizey];
area = 0;
lhx = -1, lhy = -1;
for (i = 0; i < ssizex; i++) {
for (j = 0; j < ssizey; j++) {
lda = response[i][j];
              if (lda != 0) {
                 if ((i + 1) > lhx)
                   lhx = i + 1;
                if ((j + 1) > lhy)
                   lhy = j + 1;
              }
              working_space[i][j] = lda;
              area = area + lda;
              if (lda > maximum) {
                 maximum = lda;
                 positx = i, posity = j;
              }
           }
        }
        if (lhx == -1 || lhy == -1) {
           delete [] working_space;
           cout<<"Zero response data"<<endl;
        }

     //calculate ht*y and write into p
        for (i2 = 0; i2 < ssizey; i2++) {
           for (i1 = 0; i1 < ssizex; i1++) {
              ldc = 0;
              for (j2 = 0; j2 <= (lhy - 1); j2++) {
                 for (j1 = 0; j1 <= (lhx - 1); j1++) {
                   k2 = i2 + j2, k1 = i1 + j1;
                    if (k2 >= 0 && k2 < ssizey && k1 >= 0 && k1 < ssizex) {
                       lda = working_space[j1][j2];
                       ldb = source[k1][k2];
                       ldc = ldc + lda * ldb;
                    }
                 }
              }
              working_space[i1][i2 + ssizey] = ldc;
           }
        }

     //calculate matrix b=ht*h
        i1min = -(lhx - 1), i1max = lhx - 1;
        i2min = -(lhy - 1), i2max = lhy - 1;
        for (i2 = i2min; i2 <= i2max; i2++) {
           for (i1 = i1min; i1 <= i1max; i1++) {
              ldc = 0;
              j2min = -i2;
              if (j2min < 0)
                 j2min = 0;
              j2max = lhy - 1 - i2;
              if (j2max > lhy - 1)
                 j2max = lhy - 1;
              for (j2 = j2min; j2 <= j2max; j2++) {
                 j1min = -i1;
                 if (j1min < 0)
                    j1min = 0;
                 j1max = lhx - 1 - i1;
                 if (j1max > lhx - 1)
                    j1max = lhx - 1;
                 for (j1 = j1min; j1 <= j1max; j1++) {
                    lda = working_space[j1][j2];
                    if (i1 + j1 < ssizex && i2 + j2 < ssizey)
                       ldb = working_space[i1 + j1][i2 + j2];
                    else
                       ldb = 0;
                    ldc = ldc + lda * ldb;
                 }
              }

              //cout<<"i=  "<<i1 - i1min<<"  j=  "<<i2 - i2min + 2 * ssizey<<  endl;
              //if(i1 - i1min < 100)
              {
                  working_space[i1 - i1min][i2 - i2min + 2 * ssizey ] = ldc;
              }
           }
        }

     //initialization in x1 matrix
        for (i2 = 0; i2 < ssizey; i2++) {
           for (i1 = 0; i1 < ssizex; i1++) {
              working_space[i1][i2 + 3 * ssizey] = 1;
              working_space[i1][i2 + 4 * ssizey] = 0;
           }
        }

        //START OF ITERATIONS
        for (repet = 0; repet < numberRepetitions; repet++) {
           if (repet != 0) {
              for (i = 0; i < ssizex; i++) {
                 for (j = 0; j < ssizey; j++) {
                    working_space[i][j + 3 * ssizey] =
                        TMath::Power(working_space[i][j + 3 * ssizey], boost);
                 }
              }
           }
           for (lindex = 0; lindex < numberIterations; lindex++) {
              for (i2 = 0; i2 < ssizey; i2++) {
                 for (i1 = 0; i1 < ssizex; i1++) {
                    ldb = 0;
                    j2min = i2;
                    if (j2min > lhy - 1)
                       j2min = lhy - 1;
                    j2min = -j2min;
                    j2max = ssizey - i2 - 1;
                    if (j2max > lhy - 1)
                       j2max = lhy - 1;
                    j1min = i1;
                    if (j1min > lhx - 1)
                       j1min = lhx - 1;
                    j1min = -j1min;
                    j1max = ssizex - i1 - 1;
                    if (j1max > lhx - 1)
                       j1max = lhx - 1;
                    for (j2 = j2min; j2 <= j2max; j2++) {
                       for (j1 = j1min; j1 <= j1max; j1++) {
                          ldc =  working_space[j1 - i1min][j2 - i2min + 2 * ssizey];
                          lda = working_space[i1 + j1][i2 + j2 + 3 * ssizey];
                          ldb = ldb + lda * ldc;
                       }
                    }
                    lda = working_space[i1][i2 + 3 * ssizey];
                    ldc = working_space[i1][i2 + 1 * ssizey];
                    if (ldc * lda != 0 && ldb != 0) {
                       lda = lda * ldc / ldb;
                    }

                    else
                       lda = 0;
                    working_space[i1][i2 + 4 * ssizey] = lda;
                 }
              }
              for (i2 = 0; i2 < ssizey; i2++) {
                 for (i1 = 0; i1 < ssizex; i1++)
                    working_space[i1][i2 + 3 * ssizey] =
                        working_space[i1][i2 + 4 * ssizey];
              }
          }
        }
        for (i = 0; i < ssizex; i++) {
          for (j = 0; j < ssizey; j++)
              source[(i + positx) % ssizex][(j + posity) % ssizey] =
                 area * working_space[i][j + 3 * ssizey];
        }
    for( Int_t i = 0; i < nbinsy; i++ )
    {
        delete [] working_space[i];

    }
    delete [] working_space;

*/

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

    for (i = 0; i < nbinsx; i++)
    {
        for (j = 0; j < nbinsy; j++)
        {
            h_decon->SetBinContent(i,j, source[i][j]);
        }
    }



    TCanvas *Deconvol = new TCanvas("Deconvolution","Gold deconvolution",10,10,800,800);
    gStyle->SetPalette(55);
    h_decon->GetZaxis()->SetRangeUser(0,10);
    h_decon->GetXaxis()->SetRangeUser(-10000,10000);
    h_decon->GetYaxis()->SetRangeUser(-10000,10000);
    h_decon->SetTitle("Gold deconvolution");
    h_decon->GetXaxis()->SetTitle("um");
    h_decon->GetYaxis()->SetTitle("");
    h_decon->Draw("colz");

    for( Int_t i = 0; i < nbinsy; i++ )
    {
        delete [] response[i];
        delete [] source[i];
        delete [] tempo[i];
        delete [] tempo_2[i];
    }
    delete [] response;
    delete [] source;
    delete [] tempo;
    delete [] tempo_2;
}