//testing script for the spline stuff - needs to be compiled

//THIS MACRO HAS GROWN MUCH TOO BIG!!! 
//there need to be a couple of smaller ones. 
//Felix

#include "tpc/spacecharge/BiCubSpline.h"
#include "tpc/spacecharge/BSpline.h"
#include "tpc/spacecharge/BiCubSplineFitter.h"
#include "tpc/spacecharge/SplineTF2Interface.h"
#include "PndTpcLaserStat.h" 
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <cmath>
#include "TF2.h"
#include "TH2D.h"
#include "TGraph2D.h"
#include "TArrayD.h"
#include "TMatrixD.h"
#include "TString.h"
#include "TVectorD.h"
#include "TCanvas.h"
#include "TRandom.h"
#include "TClonesArray.h"
#include "TTree.h"
#include "TPolyMarker3D.h"
#include "TStopwatch.h"
#include "TDirectory.h"
#include "TFile.h"
#include "TList.h"


void runLaserRecoFit() {


TStopwatch timer;
timer.Start();

//TODO: URGENT!!! UNDERDSTAND INFLUENCE OF KNOTS ON FITTER STABILITY!!!
//INCREASING the # of knots kills the fitter, with or without solution error!
//see comment on stepx: this may have been the reason
//Update: knot area has to be safely bigger than data area...

int NknotsX = 5;
int NknotsY = 3; 
int NlamdaX = NknotsX+8;
int NlamdaY = NknotsY+8;
std::vector<double>* _kx = new std::vector<double>;
std::vector<double>* _ky = new std::vector<double>;


std::vector<std::vector<double>*>* coeff=new std::vector<std::vector<double>*>;


double lLx = -42;
double uLx = 150;
double lLy = 11;
double uLy = 45;

double stepx = (uLx - lLx + 1)/NknotsX;  //"+1": make sure last valid knot is slightly 
double stepy = (uLy - lLy + 1)/NknotsY;  //larger than uLx or uLy

double kx,ky;

for(int i=0; i<NlamdaX; ++i){
  kx = lLx + (i-3)*stepx;
  _kx->push_back(kx); 
}
for(int j=0; j<NlamdaY; ++j){
  ky = lLy + (j-3)*stepy;
  _ky->push_back(ky); 
}

// std::cout<<"\nThe knots in x:"<<std::endl;
// for(unsigned int k=0; k<_kx->size(); k++)
//   std::cout<<_kx->at(k)<<"  ";

// std::cout<<"\nThe knots in y:"<<std::endl;
// for(unsigned int k=0; k<_ky->size(); k++)
//   std::cout<<_ky->at(k)<<"  ";
// std::cout<<std::endl;

for(int i=0; i<NlamdaY-4; i++) {
  coeff->push_back(new std::vector<double>(NlamdaX-4, 1.));
}

std::vector<std::vector<double>*>* coeff2=new std::vector<std::vector<double>*>(*coeff);

// Cub-Spline testing ------------------------------------------
BiCubSpline* csp = new BiCubSpline(_kx, _ky, coeff);
BiCubSpline* csp2 = new BiCubSpline(_kx, _ky, coeff2);


SplineTF2Interface* func = new SplineTF2Interface(csp);   
SplineTF2Interface* func2 = new SplineTF2Interface(csp2);   


//construct the TF2-----------------------------------------------------

TF2* f2 =new TF2("f2",func,&SplineTF2Interface::eval,-40,110,15.5,40,
		 (NlamdaX-4)*(NlamdaY-4),"Function", "eval");
TF2* f3 =new TF2("f3",func2,&SplineTF2Interface::eval,-40,110,15.5,40,
		 (NlamdaX-4)*(NlamdaY-4),"Function", "eval");


//get Devmap-Data: ------------------------------------------------------
double minR;
double maxR;
int rBins;
double minZ;
double maxZ;
int zBins;

double zWidth;
double rWidth;


//read file
std::string filename = "DevMap_corrected_E_and_B_30-05-08.dat";

std::ifstream infile(filename.c_str(), std::fstream::in);

TFile* laserfile = new TFile("laserdevmap.root");

//read geometry
infile>>rBins>>zBins>>minR>>maxR>>minZ>>maxZ;

zWidth = (maxZ - minZ) / zBins;
rWidth = (maxR - minR) / rBins;

TGraph2D* lasergraph = (TGraph2D*) laserfile->Get("devmap");
TH2D* laserdens = (TH2D*)laserfile->Get("densmap");
TH2D* laserhist = (TH2D*)((TGraph2D*)laserfile->Get("devmap"))->GetHistogram();
TPolyMarker3D* laserpoly = (TPolyMarker3D*)laserfile->Get("raw_Poly");

std::vector<std::vector<double>*> devRdata;       //standard deviation data

//needed later for comparison
int lbinsz = laserhist->GetNbinsX();
int lbinsr = laserhist->GetNbinsY();
double lzmin = laserhist->GetXaxis()->GetXmin();
double lzmax = laserhist->GetXaxis()->GetXmax();
double lrmin = laserhist->GetYaxis()->GetXmin();
double lrmax = laserhist->GetYaxis()->GetXmax();
double zstep = (lzmax-lzmin)/lbinsz;
double rstep = (lrmax-lrmin)/lbinsr;


//for(int j=0; j<laserpoly->GetN(); j++) {
//  //if(j%10==0) {	
//  double x, y, z;
//  laserpoly->GetPoint(j,x,y,z);
//  laserpolydata.push_back(new TVector3(x,y,z));
//  //}
//}

//build data from original devmap ---------------------------------------------------
for (int nr=0; nr<rBins; nr++)
{
  for (int nz=0; nz<zBins; nz++)
  {
    double xDev,yDev,time,way,temp;
    infile>>xDev>>yDev>>time>>temp>>way;
    std::vector<double>* vec = new std::vector<double>(4,0);
    vec->at(0) = minZ + nz*zWidth;
    vec->at(1) = minR + nr*rWidth;
    vec->at(2) = xDev;
    devRdata.push_back(vec);
  }
}
infile.close();



//build data from reco file directly -------------------------------------------------

std::vector<std::vector<double>*> laserpolydata;  //raw reconstructed residual points
std::vector<std::vector<double>*> laserpolydata_noErr;  //no errors

TFile* laserrecofile = new TFile("eumel.laser.reco.root");
TTree* t=(TTree*)laserrecofile->Get("cbmsim");
TClonesArray* statA=new TClonesArray("PndTpcLaserStat");
t->SetBranchAddress("PndTpcLaserStat",&statA);
t->GetEntry(0);

int nr=statA->GetEntriesFast();
std::cout<<nr<<" residuals found."<<std::endl;
for(int i=0;i<nr;++i){   //loop over residuals
  PndTpcLaserStat* stat=(PndTpcLaserStat*)statA->At(i);
  if(stat==NULL){std::cout<<"Invalid stat object!"<<std::endl;continue;}
  
  TVector3 proj=stat->getProjection();
  TVector3 res=stat->getRes();
  TVector3 cl_pos=stat->getPos();
  TVector3 sig=stat->getResSig();
  TVector3 sigpP=stat->getProjectionSig();
          
  double r=proj.Perp();
  double z=proj.Z();
  double clr=cl_pos.Perp();
   
  TVector3 runit=proj;
  runit.SetZ(0);
  TVector3 resXY=res;
  resXY.SetZ(0);
  runit*=(1./runit.Mag());
  TVector3 sigRU = sigpP * (1./runit.Mag()); 
  sigRU.SetZ(0);
  double dr=res*runit;
  double sig_r=sqrt(pow(res.X()*sigRU.X(),2) + pow(res.Y()*sigRU.Y(),2) + pow(runit.X()*sig.X(),2) + pow(runit.Y()*sig.Y(),2));
  
    //if(z<105)
  std::vector<double>* vec = new std::vector<double>(4,0);
  std::vector<double>* vec2 = new std::vector<double>(4,0);
  vec->at(0)=z; vec->at(1)=r; vec->at(2)=dr; vec->at(3)=sig_r;
  vec2->at(0)=z; vec2->at(1)=r; vec2->at(2)=dr;
  laserpolydata.push_back(vec);
  laserpolydata_noErr.push_back(vec2);
}



//try to build a BiCubSplineFitter ----------------------------------------------------

std::vector<std::vector<double>*> devRdata2 = std::vector<std::vector<double>*>(devRdata);

//TODO: understand why using the SAME set of data for both fits gives DIFFERENT results!
//apparantly 2 times of sorting CHANGES the data set!
//-> make data const and copy data in the fitter!!

BiCubSplineFitter* fitter = new BiCubSplineFitter(csp, &laserpolydata);
BiCubSplineFitter* fitter2 = new BiCubSplineFitter(csp2, &devRdata);



//solve the problem and draw -----------------------------------------------------------
fitter->decompose();
TVectorD solution = fitter->solve();
fitter2->decompose();
TVectorD solution2 = fitter2->solve();

//extract the right number of elements
TArrayD result = TArrayD((NlamdaX-4)*(NlamdaY-4));
for(int i=0; i<(NlamdaX-4)*(NlamdaY-4); i++)
  result[i] = solution[i];

TArrayD result2 = TArrayD((NlamdaX-4)*(NlamdaY-4));
for(int i=0; i<(NlamdaX-4)*(NlamdaY-4); i++)
  result2[i] = solution2[i];


double* param = result.GetArray();
double* param2 = result2.GetArray();

f2->SetParameters(param);// THIS DOES NOT AFFECT CSP'S _COEFF VECTOR !!!
                         // and f2 does NOT take CSP'S coeff vector in the
                         // first place!!!! 

f2->SetNpx(50);
f2->SetNpy(50);

f3->SetParameters(param2);

f3->SetNpx(50);
f3->SetNpy(50);

TH2D* diff = new TH2D("diff",
		      "difference of original and reconstructed devmap",
		      lbinsz,lzmin,lzmax,lbinsr,lrmin,lrmax);

TH1D* diff_hist = new TH1D("blub", "blob", 100, -0.1, 0.1);


for(int r=0; r<50; r++)
  for(int z=0; z<50; z++) {
  	if(z*zstep+lzmin>110 || r*rstep+lrmin> 42) continue;
    double d = f2->Eval(z*zstep+lzmin,r*rstep+lrmin)-f3->Eval(z*zstep+lzmin,r*rstep+lrmin);
  	diff->SetBinContent(z+1, r+1,d);
  	diff_hist->Fill(d);  
  }
     

std::cout<<"\n**** fit complete ****"<<std::endl;

TCanvas* c1 = new TCanvas();

c1->Divide(2,2);
//c1->cd(1);
//f2->Draw("SURF1");

c1->cd(1);
//hist->Fit("f2","R");
f2->Draw("SURF1"); 

c1->cd(2);
f3->Draw("SURF1");

c1->cd(3);
diff->Draw("COLZ");

c1->cd(4);
diff_hist->Draw();

timer.Stop();
Double_t rtime = timer.RealTime();
Double_t ctime = timer.CpuTime();
printf("RealTime=%f seconds, CpuTime=%f seconds\n",rtime,ctime);

 TString outfile = "runLaserRecoFit_OUT.root";

TList* Hlist=gDirectory->GetList();
//Hlist->Remove(tree);

//filename.ReplaceAll(".eps",".root");
TFile* file=TFile::Open(outfile,"RECREATE");
c1->Write();

int nobj=Hlist->GetEntries();
std::cout<<"Found "<<nobj<<" Objects in HList"<<std::endl;
Hlist->Print();
Hlist->Write();

file->Close();


}