// compilation:
// g++ `root-config --cflags` CAonline.cpp `root-config --glibs` -std=c++11
// run:
// /a.out

#include <TCanvas.h>
#include <TLegend.h>
#include <TF1.h>
#include <TH1.h>
#include <TH2.h>
#include <TRandom.h>
#include <TRandom3.h>
#include <TFile.h>
#include <cmath>
#include <TVirtualFFT.h>
#include <TApplication.h>
#include <TLorentzVector.h>
#include <iostream>
#include <TGeoMatrix.h>
#include <map>
#include <cctype>
#include <utility>
#include <sstream>
#include <TGraphErrors.h>
#include <TGraphAsymmErrors.h>
#include <TTree.h>
#include <TMarker.h>
#include <TLine.h>
#include <TAxis.h>
#include <vector>
#include <TGraph2DErrors.h>
#include <TVirtualFitter.h>
#include <TMatrixDSym.h>
#include <TVector2.h>
#include <TFitterMinuit.h>

using namespace std;

const double t_min = 1e-6;
const double t_max = 1e-3;

const double energy = 1.5;

const double mass = 0.938;

const int n_steps = 100;

const int delta_t = (t_max - t_min)/(double)n_steps;

double last_percent(-1.);

class THit{
public:

  int col;
  int row;
  double x;
  double y;
  double z;
  int time;
  int event;
  int plane;
  int MCtrk;
  int id;
  THit():col(0),row(0), x(0), y(0), z(0), time(0), event(0), plane(0), MCtrk(-1), id(-1){
		;
	}
};

typedef vector<THit> THits;

THits generate_hits();

class TCell{ // stores index for a hit upstream and downstream
public:
	int ihit_upstream;
	int ihit_downstream;
	// double dxdz; // direction of the cell in xz plane
	// double dydz; // direction of the cell in yz plane
        int pv; //cell position in evolution
};

typedef vector<TCell> TCells;

class TBreakingAngle{
public:
	int icell_upstream;
	int icell_downstream;
	double angle_dxdz;
	double angle_dydz;
};

typedef vector<TBreakingAngle> TBreakingAngles;

class TTrack{
public:

  THits hits; // hits assigned to the track
  TCells cells; // cells indexing the hits above
  TVector3 point; //starting point
  TVector3 dir; //direction
  double covmtx[6][6];
  double chi2;
  vector<double> kinkAx;// breaking angles in x
  vector<double> kinkAy;// breaking angles in y

  void SetCovMtx(double fcovmtx[6][6]){
    for(int i=0;i<6;i++){
      for(int j=0;j<6;j++){
	covmtx[i][j] = fcovmtx[i][j];
    }
    }
  }
  // void SetChi2(double fchi){chi2 = fchi;}

// calculate distance from hit to this trk
std::pair <double,double> distance_hit_trk(const THit& hit) { 
double dX = hit.x - point.X()+dir.X()*(-hit.z+point.Z());
double dY = hit.y -  point.Y()+dir.Y()*(-hit.z+point.Z());
pair <double,double> dist;
dist.first = dX;                 
dist.second = dY; 
return dist;
}
};

typedef vector<TTrack> TTracks;

//bool BreakingAngle(const TCell& icellup, const TCell& icelldo, const THits& hits){
bool Neighbours(const THit& hit0, const THit& hit1, const THit& hit2){ //hit0 = common hit between cells
  // double Ax = hits[icellup.ihit_downstream].x - hits[icellup.ihit_upstream].x;  
  // double Ay = hits[icellup.ihit_downstream].y - hits[icellup.ihit_upstream].y;  
  // double Az = hits[icellup.ihit_downstream].z - hits[icellup.ihit_upstream].z;
  // double Bx = hits[icelldo.ihit_downstream].x - hits[icelldo.ihit_upstream].x;  
  // double By = hits[icelldo.ihit_downstream].y - hits[icelldo.ihit_upstream].y;  
  // double Bz = hits[icelldo.ihit_downstream].z - hits[icelldo.ihit_upstream].z;
  double Ax = hit0.x - hit1.x;  
  double Ay = hit0.y - hit1.y;  
  double Az = hit0.z - hit1.z;  
  double Bx = hit2.x - hit0.x;  
  double By = hit2.y - hit0.y;  
  double Bz = hit2.z - hit0.z;  
  
  //	      cout<<"Az ="<<Az<<endl;
  double AB = Ax*Bx+Ay*By+Az*Bz;
  double Amag = sqrt(Ax*Ax+Ay*Ay+Az*Az);
  double Bmag = sqrt(Bx*Bx+By*By+Bz*Bz);
  double cosPsi = 1-fabs(AB)/(Amag*Bmag);
  // cout<<cosPsi<<endl;
  bool stline=false;
  if(cosPsi<8e-5) 
  //  if(cosPsi<7e-5) 
  // if(cosPsi<1.3e-4) 
    stline = true; // 1e-5 value from the test with 1 GeV/c tracks 
  return stline;
}

double BreakingAngle(const TTrack& trk){ //hit0 = common hit between cells
  //assumed trk has 4 hits
  const THits trkhit = trk.hits;
  double cosPsi = 0;
  //  cout<<"----------- "<<trk.hits.size()<<endl;

  for(int ihit=0;ihit<trk.hits.size()-1;ihit++){
    // cout<<trkhit[ihit].z<<" "<<trkhit[ihit+1].z<<" "<<trkhit[ihit+2].z<<endl;
    double Ax = trkhit[ihit+1].x - trkhit[ihit].x;  
    double Ay = trkhit[ihit+1].y - trkhit[ihit].y;  
    double Az = trkhit[ihit+1].z - trkhit[ihit].z;  
    double Bx = trkhit[ihit+2].x - trkhit[ihit+1].x;  
    double By = trkhit[ihit+2].y - trkhit[ihit+1].y;  
    double Bz = trkhit[ihit+2].z - trkhit[ihit+1].z;    

    double AB = Ax*Bx+Ay*By+Az*Bz;
    double Amag = sqrt(Ax*Ax+Ay*Ay+Az*Az);
    double Bmag = sqrt(Bx*Bx+By*By+Bz*Bz);
    cosPsi += 1-fabs(AB)/(Amag*Bmag);
    // cout<<ihit<<" "<<ihit+1<<" "<<ihit+2<<" cos="<<cosPsi<<endl;
  }
  return cosPsi;

}



// search track candidates 
// assuming full efficiency of all planes
TTracks SearchTracks(const THits& hits){
	TTracks result;

	// build cells between all possible combinations of hits in different planes
	TCells cells;
	for (int ihit0 = 0; ihit0 < hits.size(); ihit0++){
	  for (int ihit1 = ihit0+1; ihit1 < hits.size(); ihit1++){
	    int hit_up = ihit0;
	    int hit_do = ihit1;
	    // use the correct order
	    if (hits[hit_up].plane < hits[hit_do].plane){
	      hit_up = ihit1;
	      hit_do = ihit0;
	    }
	    // cut neighboring planes only
	    // (no missing planes => != 1)
	    // missing planes => <= 0
	    // in both cases:
	    // no hits in the same plane
	    if ((hits[hit_up].plane - hits[hit_do].plane) <= 0){
	    //    if ((hits[hit_up].plane - hits[hit_do].plane) != 1){ // no missing planes!
	      continue;
	    }
	    //cout << hits[hit_do].plane << " " << hits[hit_up].plane << endl;
	    // build cells
	    TCell cell;
	    cell.ihit_downstream = hit_do;
	    cell.ihit_upstream = hit_up;
	    cell.pv = -1;
	    cells.push_back(cell);
	    //cout << hits[cells[cells.size()-1].ihit_downstream].plane << " " << hits[cells[cells.size()-1].ihit_upstream].plane << endl << endl;
	  }
	}
	// for (int icell0 = 0; icell0 < cells.size(); icell0++){
	//   cout<<"cell#"<<icell0<<" with hits "<<cells[icell0].ihit_downstream<<" and "<<cells[icell0].ihit_upstream<<endl; 
	// }
	//evolution steps
	// 1) forward evolution when the automaton iteratively updates the states of all cells having neighbors; 
	// (connect cells to each other if breaking angle is reasonable)
	bool evl_stop;
	for(int iiter=0; iiter<100; iiter++){ //should be infinite loop! due to small number of layers 100 is close to infinity ;)
	  evl_stop=true;
	//	for(int iiter=0; iiter<2; iiter++){ //TEST
	  vector<int> pstate;
	  //	  if(cells.size()>0) cout<<"BEFORE: "<<endl;
	    for (int icell0 = 0; icell0 < cells.size(); icell0++){
	      cout<<"cell#"<<icell0<<" "<<hits[cells[icell0].ihit_upstream].z<<" has state="<<cells[icell0].pv<<endl; 
	      pstate.push_back(cells[icell0].pv);
	    }

	  for (int icell0 = 0; icell0 < cells.size(); icell0++){
	    for (int icell1 = icell0+1; icell1 < cells.size(); icell1++){
	      int icellup = 0;
	      int icelldo = 0;
	      // use the correct order and
	    // connect only cells sharing hits
	    //TODO: check if it is possible to skip this step!
	    //cout << hits[cells[icell0].ihit_downstream].plane << " " << hits[cells[icell1].ihit_upstream].plane << endl;
	    if (cells[icell0].ihit_downstream == cells[icell1].ihit_upstream){
	      icellup = icell0;	// 
	      icelldo = icell1;
	    } else {
	      if (cells[icell0].ihit_upstream == cells[icell1].ihit_downstream){
	  	icellup = icell1;
	  	icelldo = icell0;
	      } else {
	  	continue;
	      }
	    }
	    //  cout<<"do, up: "<<icelldo<<" "<<icellup<<endl;
	    bool stline = false;
	    if(cells[icelldo].ihit_upstream == cells[icellup].ihit_downstream)
	      stline = Neighbours(hits[cells[icellup].ihit_downstream], hits[cells[icelldo].ihit_downstream], 
				  hits[cells[icellup].ihit_upstream]); //hit0 = common hit between cells
	      if(stline){
		cout<<"cells "<<icellup<<" ["<<cells[icellup].pv<<"] "<<icelldo<<" ["<<cells[icelldo].pv<<"] are connected!"<<endl;
		if(pstate[icelldo]<0) pstate[icelldo]=0; //1st itter
		if(pstate[icellup]<0) pstate[icellup]=0; //1st itter
		if(cells[icelldo].pv==cells[icellup].pv) { 
		  pstate[icellup]=cells[icellup].pv+1;
		  evl_stop=false;
		}
	      }
	    }
	  }
	  //update states of cells
	  for (int icell0 = 0; icell0 < cells.size(); icell0++)
	    cells[icell0].pv = pstate[icell0]; 
	  if(evl_stop) cout<<"Only "<<iiter<<" iter were done!"<<endl;
	  if(evl_stop) break; //if no more changes in evolution, stop
	}


	//test:
	if(cells.size()>0) cout<<"--------------------"<<endl; 
	int pmax =0;
	for (int icell0 = 0; icell0 < cells.size(); icell0++){
	  if(pmax<cells[icell0].pv) pmax = cells[icell0].pv;
	  //	  cout<<"cell#"<<icell0<<" "<<hits[cells[icell0].ihit_upstream].z<<" has state="<<cells[icell0].pv<<endl; 
	  cout<<"cell#"<<icell0<<" has state="<<cells[icell0].pv<<endl; 
	}

	// 2) backward pass when the automaton collects optimal sequences starting from cells with the highest states.
	//	vector<double> trk_cand_brang;//breaking angles for trk-candidates
	//double brang_min=1e2;
	TTracks trk_cand;
	// vector<double> trk_cand_brang;//breaking angles for trk-candidates
	// double brang_min=1e2;
	// TTracks trk_cand;
	     
	for(int ipmax=pmax;ipmax>0;ipmax--){// loop over trk-seeds
	  //	  cout<<"current pmax = "<<ipmax<<endl;
	  //	  cout<<"Number of cand = "<<trk_cand.size()<<endl;
	for (int icell0 = 0; icell0 < cells.size(); icell0++){	//4hits tracks
	  if(cells[icell0].pv==ipmax){ //from "upstream" to "downstream"
	    //save tracklets in tmp, then pick up the best one
	    //[filter for tracks with 2 common hits]
	    for (int icell1 = 0; icell1 < cells.size(); icell1++){
	      //   cout<<cells[icell0].ihit_upstream<<", "<<cells[icell1].ihit_downstream<<endl;
	      if(cells[icell0].ihit_downstream == cells[icell1].ihit_upstream){ 
		bool stline = Neighbours(hits[cells[icell1].ihit_upstream], hits[cells[icell1].ihit_downstream], 
					 hits[cells[icell0].ihit_upstream]); //hit0 = common hit between cells
		//		if(stline) 
		if(stline && (cells[icell0].pv-cells[icell1].pv)==1){
		//	if(stline && (cells[icell0].pv-cells[icell1].pv)>0){
		  cout<<cells[icell0].pv<<" "<<cells[icell1].pv<<endl;
		  TCells trackcells;
		  THits trackhits;
		  TTrack track;
		  trackhits.push_back(hits[cells[icell0].ihit_upstream]);
		  trackhits.push_back(hits[cells[icell0].ihit_downstream]);
		  trackhits.push_back(hits[cells[icell1].ihit_downstream]);
		  cout<<"Hits: "<<hits[cells[icell0].ihit_upstream].id<<" ["<<hits[cells[icell0].ihit_upstream].plane<<"] "
		      <<hits[cells[icell0].ihit_downstream].id<<" ["<<hits[cells[icell0].ihit_downstream].plane<<"] "
		      <<hits[cells[icell1].ihit_downstream].id<<" ["<<hits[cells[icell1].ihit_downstream].plane<<"]"<<endl;
		
		  trackcells.push_back(cells[icell0]);
		  trackcells.push_back(cells[icell1]);
		  //	  cout<<"in trk: "<<icell0<<", "<<icell1;
		  // bool twocell=false;//enable multiple cells participation in trk
		  if(cells[icell1].pv>0){ //search for the last connected cell
		    for (int icell2 = 0; icell2 < cells.size(); icell2++){
		      if(cells[icell1].ihit_downstream == cells[icell2].ihit_upstream){
			bool stline = Neighbours(hits[cells[icell2].ihit_upstream], hits[cells[icell2].ihit_downstream], 
						 hits[cells[icell1].ihit_upstream]); //hit0 = common hit between cells
			//	if(stline) 
			if(stline && (cells[icell1].pv-cells[icell2].pv)==1){
			//	if(stline && (cells[icell1].pv-cells[icell2].pv)>0){
			  //	  cout<<cells[icell1].pv<<" "<<cells[icell2].pv<<endl;
			  trackcells.push_back(cells[icell2]);
			  trackhits.push_back(hits[cells[icell2].ihit_downstream]);
			  cout<<" + one more hit: "<<hits[cells[icell2].ihit_downstream].id<<" ["<<hits[cells[icell2].ihit_downstream].plane<<"]"<<endl;
			  track.cells = trackcells;
			  track.hits = trackhits;
			  trk_cand.push_back(track);
			  //			  double ang = BreakingAngle(track);
			  //	  if(brang_min>ang) brang_min=ang;
			  //			  trk_cand_brang.push_back(ang);
			  cells[icell0].pv = cells[icell0].pv-1;
			  cells[icell1].pv = cells[icell1].pv-1;		  
			  cells[icell2].pv = cells[icell2].pv-1;
			  //    cout<<", "<<icell2<<endl;
			  // if(twocell){
			  // clean only last cell and hit
			  trackcells.pop_back();
			  trackhits.pop_back();
			  // }
			  //  break;
			}
		      }
		     
		  }
		  }//[END] search for the last connected cell
		  else{
		    track.cells = trackcells;
		    track.hits = trackhits;
		    trk_cand.push_back(track);
		    // double ang = BreakingAngle(track);
		    // //if(brang_min>ang) brang_min=ang;
		    // trk_cand_brang.push_back(ang);
		    cells[icell0].pv = cells[icell0].pv-1;
		    cells[icell1].pv = cells[icell1].pv-1;		  
		  }
		}
	      }
	    }
	    // trackcells.clear();
	    // trackhits.clear();
	    // }
	    //pick up the best candidate from tmp 
	    
		// for (int itrk=0; itrk<trk_cand_brang.size(); itrk++){
		//   result.push_back(trk_cand[itrk]);	
		//   //	  cout<<"trk_cand_brang[itrk] = "<<trk_cand_brang[itrk]<<" min="<<brang_min<<endl;
		//   if(trk_cand_brang[itrk]>brang_min){}
		//   else{
		//     // cout<<"!!!"<<endl;
		//     result.push_back(trk_cand[itrk]);	
		//     break;
		//   }
		// }
	  }
	}//[END] 4hits tracks
	}// loop over trk-seeds

	cout<<"Before Filter "<<trk_cand.size()<<" trks"<<endl;
	if(trk_cand.size()==1) result.push_back(trk_cand[0]);
	else{
	//Filter
	// if tracks have 3 hits in common:
	// equal number of hits in tracks -> remove one with the largest breaking angles sum
	// not equal number of hits in tracks  -> remove one with smallest number of hits
	//   bool ch_smth=true;
	  for(int iiter=0;iiter<2*trk_cand.size();iiter++){
	  //	  if(!ch_smth) break;
	  //	  cout<<"=== Iter "<<iiter<<" with "<<trk_cand.size()<<" trks"<<endl;
	  //	  bool game_over=false:
	for (int itrk0=0; itrk0<trk_cand.size(); itrk0++){
	  THits hits_trk0 = trk_cand[itrk0].hits;
	  //  bool filt=false;
	  bool erase_trk0=false;
	  for (int itrk1=0; itrk1<trk_cand.size(); itrk1++){
	    if(itrk0==itrk1) continue;
	    bool erase_trk1=false;
	    THits hits_trk1 = trk_cand[itrk1].hits;
	    cout<<"check trks with "<<hits_trk0.size()<<" and "<<hits_trk1.size()<<" hits"<<endl;
	    int count_cm_hit=0;
	    for(int ihit0=0;ihit0<hits_trk0.size();ihit0++){
	      for(int ihit1=0;ihit1<hits_trk1.size();ihit1++){
		cout<<"check hit id: "<<hits_trk0[ihit0].id<<" @plane"<<hits_trk0[ihit0].plane
		    <<" "<<hits_trk1[ihit1].id<<" @plane"<<hits_trk1[ihit1].plane<<" "<<endl;
		if(hits_trk0[ihit0].id!=hits_trk1[ihit1].id){
		}
		else{
		  //		if(hits_trk0[ihit0].id==hits_trk1[ihit1].id){
		  count_cm_hit++;
		  break;
		}
	      }
	    }
	    cout<<"count_cm_hit = "<<count_cm_hit<<endl;
	    //  if(count_cm_hit<2) ch_smth=false;
	    //    if(count_cm_hit<3) 
	    if(count_cm_hit>=2){
	    if(hits_trk0.size()>hits_trk1.size()){// take trk with max length
	      // filt=true;
	      // result.push_back(trk_cand[itrk0]);
	      //	      trk_cand.erase(trk_cand.begin()+itrk1);
	      erase_trk1=true;
	      cout<<"add trk with "<<hits_trk0.size()<<" hits"<<endl;
	      cout<<"remove trk with "<<hits_trk1.size()<<" hits"<<endl;
	      //	      ch_smth=true;
	    }
	    if(hits_trk0.size()<hits_trk1.size()){// take trk with max length
	      // filt=true;
	      // result.push_back(trk_cand[itrk0]);
	      //	      trk_cand.erase(trk_cand.begin()+itrk1);
	      erase_trk0=true;
	      cout<<"add trk with "<<hits_trk1.size()<<" hits"<<endl;
	      cout<<"remove trk with "<<hits_trk0.size()<<" hits"<<endl;
	      //	      ch_smth=true;
	    }
	    if(hits_trk0.size()==hits_trk1.size()){// check breaking angles
		//	filt=true;
		double ang0 =  BreakingAngle(trk_cand[itrk0]);
		double ang1 =  BreakingAngle(trk_cand[itrk1]);
		if(ang0<ang1){
		  //	  result.push_back(trk_cand[itrk0]);
		  //		  trk_cand.erase(trk_cand.begin()+itrk1);
		  erase_trk1=true;
		  cout<<"add trk with "<<hits_trk0.size()<<" hits"<<endl;
		  cout<<"remove trk with "<<hits_trk1.size()<<" hits"<<endl;
		}
		else{
		  //	  result.push_back(trk_cand[itrk1]);
		  //  trk_cand.erase(trk_cand.begin()+itrk0);
		  erase_trk0=true;
		  cout<<"add trk with "<<hits_trk1.size()<<" hits"<<endl;
		  cout<<"remove trk with "<<hits_trk0.size()<<" hits"<<endl;
		  //	  ch_smth=true;
		  break;//go to next trk
		}
	      }
	      // 	trk_cand.erase(trk_cand.begin()+itrk0);
	    
	    }
	    // if(count_cm_hit<3 && hits_trk0.size()<hits_trk1.size()){
	    //   result.push_back(trk_cand[itrk0]);
	 
	    // }
	    //if(count_cm_hit<count_cm_hit_cur) 
	    //    count_cm_hit_cur=count_cm_hit;
	    if(erase_trk1){
	      trk_cand.erase(trk_cand.begin()+itrk1);
	      break;
	    }
	    
	  }
	  if(erase_trk0){
	      trk_cand.erase(trk_cand.begin()+itrk0);
	      break;
	    }

	  // if(!filt){
	  //   cout<<"Nothing bad with this trk, add it!"<<endl;
	  //   result.push_back(trk_cand[itrk0]);
	  // }
	}
	//	cout<<"end of "<<iiter<<" filtering"<<endl;
	  }
	for (int itrk0=0; itrk0<trk_cand.size(); itrk0++){
	  result.push_back(trk_cand[itrk0]);
	}
	}
	//   if(trk_cand[itrk]->hits.size()=3){
// 	    vector<int> trk_h;
// 	    for(int ihit=0;ihit<trk_cand[itrk]->hits.size();ihit++){
// trk_h
// 	    }
// 	  }
	  //for (int itrk=0; itrk<trk_cand_brang.size(); itrk++){
// 	  result.push_back(trk_cand[itrk]);	
// }
	cout<<"Finished with output of "<<result.size()<<" trks"<<endl;
return result;
}

double ScatteredAngle(){
  //Calculation of ThetaMS -------------------------------------
  double momentum = 1;
  const double beta = 1/sqrt(1+pow(0.511/momentum,2));
  //cout << "the beta factor for " << momentum << " GeV/c electrons is " << beta << endl;
  const double x_over_x0 = 2.329/21.82*350/1000/10;
  //cout << "x/X0 is" << x_over_x0 << endl;
  const double theta_0_slicon = 13.6/beta/(momentum*1e3)*sqrt(x_over_x0)*(1+0.038*log(x_over_x0));
  Double_t thetaMS = theta_0_slicon;
  // //Charge & mass of particle
  // double fPbeam = 1;// the worst case 1 GeV/c
  // Double_t fMass = 0.938272046;
  // Double_t Ebeam = TMath::Hypot(fPbeam,fMass);
  // TLorentzVector LorMom(0, 0, fPbeam, Ebeam);
  // Double_t beta = LorMom.Beta();
  // Double_t X_to_X0 =  radLen;
  // Double_t thetaMS = 13.6*1e-3*TMath::Sqrt(X_to_X0)*(1+0.038*TMath::Log(X_to_X0))/(beta*fPbeam);
  // //cout<<"for Pbeam = "<<fPbeam<<" thetaMS="<<thetaMS<<endl;
 // cout<<" thetaMS="<<thetaMS<<endl;
  return thetaMS;
}



// calculate distance from point to trk-cand
double distance_MS(double x,double y,double z, double errx,double erry,double errz, double *p, double *zpr) { 
  double THfunc[4] = {1,1,1,1};
 // Double_t t_min = p[1]*(x-p[0])+p[3]*(y-p[2])+p[5]*(z-p[4]);
 Double_t t_min = (z-p[4]);//telescope isn't rotated
 for(int iz=0;iz<4;iz++){
    if(((p[4]+t_min)-zpr[iz])<=0) THfunc[iz]=0;
    //   cout<<"t_min = "<<t_min<<" p[4]="<<p[4]<<" zpr[iz]="<<zpr[iz]<<" THfunc[iz] = "<<THfunc[iz]<<endl;
 }
  double funcX = p[0]+p[1]*t_min
    +(-p[6])*((p[4]+t_min)-zpr[0])*THfunc[0]
    +(-p[7])*((p[4]+t_min)-zpr[1])*THfunc[1]
    +(-p[8])*((p[4]+t_min)-zpr[2])*THfunc[2]
    +(-p[9])*((p[4]+t_min)-zpr[3])*THfunc[3];
  double funcY = p[2]+p[3]*t_min
    +(-p[10])*((p[4]+t_min)-zpr[0])*THfunc[0]
    +(-p[11])*((p[4]+t_min)-zpr[1])*THfunc[1]
    +(-p[12])*((p[4]+t_min)-zpr[2])*THfunc[2]
    +(-p[13])*((p[4]+t_min)-zpr[3])*THfunc[3];

  double fdx = TMath::Power((x-funcX)/errx,2);
  double fdy = TMath::Power((y-funcY)/erry,2);
  double fdz = 0;
  double fchi2 = fdx + fdy +fdz;
  //  cout<<"current chi2 = "<<fchi2<<" funcX = "<<funcX<<" funcY = "<<funcY<<endl;
  return fchi2; 
}



// function to be minimized with kink angle due to multiple scattering
void LocalFCN_MS(int &, double *, double & sum, double * par, int ) { 
  TGraph2DErrors * gr = dynamic_cast<TGraph2DErrors*>( (TVirtualFitter::GetFitter())->GetObjectFit());
  // assert(gr != 0);
  double * x = gr->GetX();
  double * y = gr->GetY();
  double * z = gr->GetZ();
  double * errx = gr->GetEX();
  double * erry = gr->GetEY();
  double * errz = gr->GetEZ();
  int npoints = gr->GetN();
  sum = 0;
  // double totRadLen  = 0.00306;//standart rad.length of whole plane
  double errTot = ScatteredAngle();
  double zpr[4]={0.9,1.0,1.1,1.2};//ToDo: read it from geometry!
  // int izcur = 0;
    // for(int izpl = 0;izpl<4;izpl++){
    //   zpr[izcur] = z[izpl];
    //   // izcur++;
    //   // zpr[izcur] = z[izpl];
    //   // izcur++;
    // }
    for (int i  = 0; i < npoints; ++i) {  
      double chi2 = distance_MS(x[i],y[i],z[i],errx[i],erry[i],errz[i],par,zpr); 
      sum += chi2;
    }
  double fdal = 0;
  for(int jms=0;jms<4;jms++){
    fdal += TMath::Power(par[6+jms]/errTot,2);
    fdal += TMath::Power(par[10+jms]/errTot,2);
  }
  sum +=fdal;
}
//chi2 with kink angles fit [G.Lutz, NIM A273 (1988)]
double line3DfitMS(Int_t nd, TGraph2DErrors* gr, TVector3 posSeed, TVector3 dirSeed, Double_t* fitpar, TMatrixDSym *covmatrix)
{

   const int nparams = 14;
  TVirtualFitter::SetDefaultFitter("Minuit2");
   TVirtualFitter *fmin = TVirtualFitter::Fitter(0,nparams);
   fmin->Clear();
   //  if(fVerbose>2) 
   cout<<"line3Dfit with SEED is used (multiple scattering taking into account by kinks)"<<endl;
  Int_t Npoint = gr->GetN();
  Double_t ErrX1 = gr->GetErrorX(0);
  Double_t ErrY1 = gr->GetErrorY(0);
  Double_t ErrZ1 = gr->GetErrorZ(0);
  TVector3 ErrPosSeed(ErrX1,ErrY1,ErrZ1);
  Double_t ErrX2 = gr->GetErrorX(2);
  Double_t ErrY2 = gr->GetErrorY(2);
  Double_t ErrZ2 = gr->GetErrorY(2);
  
  Double_t errRx = 1*TMath::Hypot(ErrX1,ErrX2);
  Double_t errRy = 1*TMath::Hypot(ErrY1,ErrY2);
  Double_t errRz = 1*TMath::Hypot(ErrZ1,ErrZ2);
  
  fmin->SetObjectFit(gr);
  fmin->SetFCN(*LocalFCN_MS);
  Double_t arglist[100];
  arglist[0] = 1;
  //  fmin->ExecuteCommand("SET PRINT",arglist,10);//output
  //  fmin->ExecuteCommand("SET PRINT",arglist,0);//no output
  ((TFitterMinuit*)fmin->GetFitter())->SetPrintLevel(0);
  //if(fVerbose>5){
    cout<<"Number of hits = "<<Npoint<<endl;
    cout<<"posSeed:"<<endl;
    posSeed.Print();
    cout<<"ErrposSeed:"<<endl;
    ErrPosSeed.Print();
    cout<<"dirSeed:"<<endl;
    dirSeed.Print();
    //  }
  double l = 1/dirSeed.Mag();
  double pStart[nparams] = {posSeed.X(),l*dirSeed.X(),posSeed.Y(),l*dirSeed.Y(),posSeed.Z(),l*dirSeed.Z(),0,0,0,0,0,0,0,0};
  double fsigmaMS = ScatteredAngle();
  double errfsigmaMS = 1e-2*fsigmaMS;
  double pStartErr[nparams] = {ErrX1,errRx,ErrY1,errRy,ErrZ1,errRz,errfsigmaMS,errfsigmaMS,errfsigmaMS,errfsigmaMS ,errfsigmaMS,errfsigmaMS,errfsigmaMS,errfsigmaMS};
 
  fmin->SetParameter(0,"x0",pStart[0],pStartErr[0],0,0);
  fmin->SetParameter(1,"Ax",pStart[1],pStartErr[1],0,0);
  fmin->SetParameter(2,"y0",pStart[2],pStartErr[2],0,0);
  fmin->SetParameter(3,"Ay",pStart[3],pStartErr[3],0,0);
  fmin->SetParameter(4,"z0",pStart[4],0,0,0);
  fmin->SetParameter(5,"Az",pStart[5],0,0,0);
  
  fmin->SetParameter(6,"al0x",pStart[6],1e-4*fsigmaMS,0,0);
  fmin->SetParameter(7,"al1x",pStart[7],1e-4*fsigmaMS,0,0);
  fmin->SetParameter(8,"al2x",pStart[8],1e-4*fsigmaMS,0,0);
  fmin->SetParameter(9,"al3x",pStart[9],1e-4*fsigmaMS,0,0);
  fmin->SetParameter(10,"al0y",pStart[10],1e-4*fsigmaMS,0,0);
  fmin->SetParameter(11,"al1y",pStart[11],1e-4*fsigmaMS,0,0);
  fmin->SetParameter(12,"al2y",pStart[12],1e-4*fsigmaMS,0,0);
  fmin->SetParameter(13,"al3y",pStart[13],1e-4*fsigmaMS,0,0);

// fmin->FixParameter(6);
// fmin->FixParameter(10);
 fmin->FixParameter(9);
    fmin->FixParameter(13);

 //  if(Npoint<4){
 //    fmin->FixParameter(8);
 //    fmin->FixParameter(12);
 //  }

//   //TEST
// fmin->FixParameter(7); fmin->FixParameter(8); fmin->FixParameter(9); 
// fmin->FixParameter(11); fmin->FixParameter(12); fmin->FixParameter(13); 

double recpres = 1e-7;

  // Now ready for minimization step
   arglist[0] = 5000;
  //  arglist[0] = 2;
  arglist[1] = recpres;
  fmin->ExecuteCommand("MIGRAD", arglist,3);
  fmin->ExecuteCommand("SET PRI", arglist,5);
  ///Get results ---------------------------------------------
  int nvpar,nparx; 
  double amin,edm, errdef;
  fmin->GetStats(amin,edm,errdef,nvpar,nparx);
  if(edm>1e2*recpres) return 1e6; 
  // if(fVerbose>1){
    cout<<"------------- Final result ---------------- "<<endl;
    //    fmin->PrintResults(fVerbose,amin);
    fmin->PrintResults(0,amin);
    // }
  // if(fVerbose>1){
  //   cout<<"------------- Final result ---------------- "<<endl;
  //   fmin->PrintResults(1,amin);
  // }
  
   Double_t fitparerr[nparams];
   // get fit parameters
   for (int i = 0; i <nparams; ++i){
      fitpar[i] = fmin->GetParameter(i); 
      fitparerr[i] = fmin->GetParError(i);
   }
  
  
   for(size_t i=0;i<6;i++){
     for(size_t j=0;j<6;j++){
       (*covmatrix)(i,j)= fmin->GetCovarianceMatrixElement(i,j);
     }
   }

   if((fitpar[1]*fitpar[1]+fitpar[3]*fitpar[3])<1.){
   fitpar[5]=sqrt(1-fitpar[1]*fitpar[1]-fitpar[3]*fitpar[3]);
   }
   else{
     fitpar[5]=0;
     amin=1e9;
   }
  
   //!!!!!!!!!!!!!!!!!! z0, dz weren't fitted
   (*covmatrix)(4,4) =  (gr->GetErrorZ(0)*gr->GetErrorZ(0))/12.;
   double dp5_dp1 = fitpar[1]/fitpar[5];
   double dp5_dp3 = fitpar[3]/fitpar[5];
   double errdz2 = pow(dp5_dp1,2)*(*covmatrix)(1,1) + pow(dp5_dp3,2)*(*covmatrix)(3,3) + 
     2*fabs(dp5_dp1*dp5_dp3*(*covmatrix)(1,3));
   (*covmatrix)(5,5) = errdz2;
   //!!!!!!!!!!!!!!!!!!

  

   Double_t chi2 = amin/(2.*Npoint-4);
   ///-------------------------------------------------------------
   // if(fVerbose>2){
   //   falx0a = fitpar[6];
   //   ferralx0a = fitparerr[6];
   //   falx1a = fitpar[8];
   //   ferralx1a = fitparerr[8];
   //   falx2a = fitpar[10];
   //   ferralx2a = fitparerr[10];
   //   falx3a = fitpar[12];
   //   ferralx3a = fitparerr[12];
   //   faly0a = fitpar[14];
   //   ferraly0a = fitparerr[14];
   //   faly1a = fitpar[16];
   //   ferraly1a = fitparerr[16];
   //   faly2a = fitpar[18];
   //   ferraly2a = fitparerr[18];
   //   faly3a = fitpar[20];
   //   ferraly3a = fitparerr[20];
   //   falx0b = fitpar[7];
   //   ferralx0b = fitparerr[7];
   //   falx1b = fitpar[9];
   //   ferralx1b = fitparerr[9];
   //   falx2b = fitpar[11];
   //   ferralx2b = fitparerr[11];
   //   falx3b = fitpar[13];
   //   ferralx3b = fitparerr[13];
   //   faly0b = fitpar[15];
   //   ferraly0b = fitparerr[15];
   //   faly1b = fitpar[17];
   //   ferraly1b = fitparerr[17];
   //   faly2b = fitpar[19];
   //   ferraly2b = fitparerr[19];
   //   faly3b = fitpar[21]; 
   //   ferraly3b = fitparerr[21];

   //   fnpoints = Npoint;
   //   fchi2 =  chi2;
   //   fzhit0 = (gr->GetZ())[0];
   //   ttal->Fill();
   // }

   fmin->Clear();
   //delete min;
   return chi2; 
}

void FitTracks(TTracks &tracks){

  //for (auto itrack = tracks.begin(); itrack != tracks.end(); itrack++){
 // THits hits_trk = itrack->hits;
  	for (int itrk=0; itrk<tracks.size(); itrk++){ //loop over all trks
  	  THits hits_trk = tracks[itrk].hits;
 
	  const int numPts = hits_trk.size();
	  TGraph2DErrors fitme(numPts); //new graph for fitting
	  int gl_h=0;
	  for (int ihit=numPts-1; ihit>=0; ihit--){ //loop over hits in trk, hits saved starting from the last plane
	    //   TVector3 addPos(hits_trk[ihit].x,hits_trk[ihit].y,hits_trk[ihit].z);
	    fitme.SetPoint(gl_h, hits_trk[ihit].x, hits_trk[ihit].y, hits_trk[ihit].z);
	    fitme.SetPointError(gl_h,2.3e-5,2.3e-5,1.5e-4);//80/sqrt(12), 80/sqrt(12), sensor thickness 150 mkm
	    gl_h++;
	    cout<<"Add hit with "<<hits_trk[ihit].z<<endl;
	  }//[END] loop over hits in trk

	  TVector3 dirSeed(hits_trk[numPts-2].x-hits_trk[numPts-1].x,hits_trk[numPts-2].y-hits_trk[numPts-1].y,hits_trk[numPts-2].z-hits_trk[numPts-1].z);
	  dirSeed *=1./dirSeed.Mag();
	  TVector3 posSeed(hits_trk[numPts-1].x,hits_trk[numPts-1].y,hits_trk[numPts-1].z);
	  Double_t parFit[22]; //fit-parameter
	  TMatrixDSym *COVmatrix = new TMatrixDSym(6);
	  Double_t accuracy = line3DfitMS(numPts, &fitme, posSeed, dirSeed, parFit, COVmatrix);//with kink angles

	  //save result
	  TVector3 FitPoint(parFit[0], parFit[2], parFit[4]);
	  TVector3 FitDir(parFit[1], parFit[3], parFit[5]);
	  // TVector3 FitMom = FitDir*fPbeam;
	  Double_t COVmatrixPosMom[6][6];
	  
	  for(int ij=0;ij<6;ij++){
	    for(int ji=0;ji<6;ji++)
	      COVmatrixPosMom[ij][ji] = 0;
	  }  

	  int iconver[6]={3,0,4,1,5,2};
	  for(int ij=0;ij<6;ij++){
	    for(int ji=0;ji<6;ji++){
	      // if(ij==1 || ij==3 || ij==5) (*COVmatrix)(ij,ji) *=fPbeam;
	      // if(ji==1 || ji==3 || ji==5) (*COVmatrix)(ij,ji) *=fPbeam;
	      int km =  iconver[ij];
	      int mk = iconver[ji];
	      COVmatrixPosMom[km][mk] = (*COVmatrix)(ij,ji);
	    }
	  }
	  //	  itrack->point = FitPoint;

	  tracks[itrk].point = FitPoint;
	  tracks[itrk].dir = FitDir;
	  tracks[itrk].SetCovMtx(COVmatrixPosMom);
	  tracks[itrk].chi2 = accuracy;

// fmin->SetParameter(6,"al0x",pStart[6],1e-4*fsigmaMS,0,0);
//   fmin->SetParameter(7,"al1x",pStart[8],1e-4*fsigmaMS,0,0);
//   fmin->SetParameter(8,"al2x",pStart[10],1e-4*fsigmaMS,0,0);
//   fmin->SetParameter(9,"al3x",pStart[12],1e-4*fsigmaMS,0,0);
//   fmin->SetParameter(10,"al0y",pStart[14],1e-4*fsigmaMS,0,0);
//   fmin->SetParameter(11,"al1y",pStart[16],1e-4*fsigmaMS,0,0);
//   fmin->SetParameter(12,"al2y",pStart[18],1e-4*fsigmaMS,0,0);
//   fmin->SetParameter(13,"al3y",pStart[20],1e-4*fsigmaMS,0,0);
	  for(int ip=6;ip<10;ip++)
	    tracks[itrk].kinkAx.push_back(parFit[ip]);
	  for(int ip=10;ip<14;ip++)
	    tracks[itrk].kinkAy.push_back(parFit[ip]);




	  // cout<<"chi2 = "<<accuracy<<endl;
	  //	  tracks[itrk].SetChi2(accuracy);
	}//[END] loop over all trks
}

void DrawProgressBar(int len, double percent) {
	if ((int) (last_percent * 100) == (int) (percent * 100))
		return;
	//cout << " drawing " << endl;
	cout << "\x1B[2K"; // Erase the entire current line.
	cout << "\x1B[0E"; // Move to the beginning of the current line.
	string progress;
	for (int i = 0; i < len; ++i) {
		if (i < static_cast<int> (len * percent)) {
			progress += "=";
		} else {
			progress += " ";
		}
	}
	cout << "[" << progress << "] " << (static_cast<int> (100 * percent))
			<< "%";
	flush( cout); // Required.
	last_percent = percent;
}

void track_finder(){
	TCanvas canvas_distributions("canvas_distributions", "distributions", 800, 800);
	TH2F* hist_XY[4];
	hist_XY[0] = new TH2F("hist_XY_plane_0", "xy distribution at plane 0", 49, -2e-3, 2e-3, 49, -2e-3, 2e-3);
	hist_XY[1] = new TH2F("hist_XY_plane_1", "xy distribution at plane 1", 49, -2e-3, 2e-3, 49, -2e-3, 2e-3);
	hist_XY[2] = new TH2F("hist_XY_plane_2", "xy distribution at plane 2", 49, -2e-3, 2e-3, 49, -2e-3, 2e-3);
	hist_XY[3] = new TH2F("hist_XY_plane_3", "xy distribution at plane 3", 49, -2e-3, 2e-3, 49, -2e-3, 2e-3);

	TH2F* hist_dxdz[2]; // breaking angles between planes
	hist_dxdz[0] = new TH2F("hist_dxdz_plane_012", "dx/dz and dy/dz distribution between plane 0,1 and 2", 49, -2e-2, 2e-2, 49, -2e-2, 2e-2);
	hist_dxdz[1] = new TH2F("hist_dxdz_plane_123", "dx/dz and dy/dz distribution between plane 1,2 and 3", 49, -2e-2, 2e-2, 49, -2e-2, 2e-2);
//	hist_dxdz[2] = new TH2F("hist_dxdz_plane_23", "dxdz distribution between plane 2 and 3", 49, -2e-2, 2e-2, 49, -2e-2, 2e-2);

	TH2F* hist_dxdz_track; // breaking angles between planes
	hist_dxdz_track = new TH2F("hist_dxdz_track", "dx/dz and dy/dz direction distribution", 49, -0.02, 0.02, 49, -0.02, 0.02);
	TH1F *hist_breakang_trk = new TH1F("hist_breakang_trk",";breaking angle, rad",1e2,0,2e-4);
	//TH2I *hist_trk_mult = new TH2I("hist_trk_mult",";MC trks(>2hits)/ev; REC.trks/ev",20,0,20,20,0,20);
	TH2I *hist_trk_mult = new TH2I("hist_trk_mult",";MC trks/ev; REC.trks/ev",15,0,15,15,0,15);
	TH1F *hist_hits_trk = new TH1F("hist_hits_trk","; hits/REC.trk",6,0,6);
	TH1F *hist_hits_mctrk = new TH1F("hist_hits_mctrk","; hits/trk",6,0,6);
	TH1I *hist_trks_pure = new TH1I("hist_trks_pure","; number of MCids/REC.trk",10,0,10);
	TH1F *hist_trks_chi2 = new TH1F("hist_trks_chi2","; #chi^{2}",1e2,0,1e1);

	TH2F* hist_kink[4];
	hist_kink[0] = new TH2F("hist_kink_0", "kink angles on plane 0; #alpha_{x}, #murad; #alpha_{y}, #murad",1e2,-3e3,3e3,1e2,-3e3,3e3);
	hist_kink[1] = new TH2F("hist_kink_1", "kink angles on plane 1; #alpha_{x}, #murad; #alpha_{y}, #murad",1e2,-3e3,3e3,1e2,-3e3,3e3);
	hist_kink[2] = new TH2F("hist_kink_2", "kink angles on plane 2; #alpha_{x}, #murad; #alpha_{y}, #murad",1e2,-3e3,3e3,1e2,-3e3,3e3);
	hist_kink[3] = new TH2F("hist_kink_3", "kink angles on plane 3; #alpha_{x}, #murad; #alpha_{y}, #murad",1e2,-3e3,3e3,1e2,-3e3,3e3);

	TH2F* hist_hit_residuals[4];
	hist_hit_residuals[0] = new TH2F("hist_hit_residuals_0", "residuals between hit and track on plane 0; #delta_{x}, #mum; #delta_{y}, #mum",1e2,-1e3,1e3,1e2,-1e3,1e3);
	hist_hit_residuals[1] = new TH2F("hist_hit_residuals_1", "residuals between hit and track on plane 1; #delta_{x}, #mum; #delta_{y}, #mum",1e2,-1e3,1e3,1e2,-1e3,1e3);
	hist_hit_residuals[2] = new TH2F("hist_hit_residuals_2", "residuals between hit and track on plane 2; #delta_{x}, #mum; #delta_{y}, #mum",1e2,-1e3,1e3,1e2,-1e3,1e3);
	hist_hit_residuals[3] = new TH2F("hist_hit_residuals_3", "residuals between hit and track on plane 3; #delta_{x}, #mum; #delta_{y}, #mum",1e2,-1e3,1e3,1e2,-1e3,1e3);


	int mean_hits(0);
	int mean_tracks(0);

	//	int nevents = 1000000;
	int nevents = 10000;
	for (int ievent = 0; ievent < nevents; ievent++){
	  cout<<"------ Event "<< ievent<<" ------- "<<endl;
	  //	  DrawProgressBar(50, (ievent + 1) / ((double) nevents));
	  THits hits = generate_hits();
		for (auto ihit = hits.begin(); ihit != hits.end(); ihit++){
			hist_XY[ihit->plane]->Fill(ihit->x, ihit->y);
		}
		mean_hits += hits.size();
		int MCMCtrks=0;//number of MC trks left hits in LMD
		if((hits.size())>1){
		  cout<<"MC MC ids: "<<hits[0].MCtrk;
		  //		int MCMCid=-1;
		  vector<int> MCMCids;
		  MCMCids.push_back(hits[0].MCtrk);
		  int MCcount=0;
		  for (auto ihit = hits.begin()+1; ihit != hits.end(); ihit++){
		    int MCtrk_cur = ihit->MCtrk;
		    cout<<" "<<MCtrk_cur;
		    bool ch_hit=false;
		    for(int iid=0;iid<MCMCids.size();iid++){
		      if(MCtrk_cur!=MCMCids[iid]){
			MCcount=0;
			MCMCids.push_back(MCtrk_cur);
			ch_hit=true;
			break;
		      }
		      else
			ch_hit=false;
		    }
		    if(!ch_hit) MCcount++;
		    // if(MCMCid!=MCtrk_cur){
		    //   MCMCid=MCtrk_cur;
		    //   MCMCtrks++;
		    // }
		    // cout<<" "<<MCtrk_cur;
		    if(MCcount>2){
		      //     MCMCtrks++;
		      MCcount=0;//reset counter
		    }
		  }//loop over hits
		  cout<<endl;
		  //count number of hits in MC trk
		  int MCpos=hits[0].MCtrk;
		  int counthit=1;
		  //bool ch_mctrk=false;
		  for (auto ihit = hits.begin()+1; ihit != hits.end(); ihit++){
		    //		    cout<<"MCpos "<<MCpos<<" "<<ihit->MCtrk<<endl;
		    if(MCpos!=ihit->MCtrk){
		      MCpos=ihit->MCtrk;
		      cout<<"MC trk with "<<counthit<<" hits"<<endl;
		      MCMCtrks++;
		      hist_hits_mctrk->Fill(counthit);
		      counthit=1;
		      //      ch_mctrk=true;
		    }
		    else{
		      counthit++;
		    }
		  }
		  cout<<"[last] MC trk with "<<counthit<<" hits"<<endl;
		  hist_hits_mctrk->Fill(counthit);//pick up the last MCtrk
		  //		  if(counthit>2) 
		    MCMCtrks++;
		    //MCMCtrks = MCMCids.size();
		}


		TTracks tracks = SearchTracks(hits);
		FitTracks(tracks);//TEST FIT


		int ntrks = tracks.size();
		mean_tracks += ntrks; 
		if(ntrks>0) 
		  hist_trk_mult->Fill(MCMCtrks,ntrks);
		if(MCMCtrks==1 && ntrks<1 && hits.size()>2) cout<<"WTF?! MC="<<MCMCtrks<<" REC="<<ntrks<<endl;
		for (auto itrack = tracks.begin(); itrack != tracks.end(); itrack++){
		  hist_hits_trk->Fill(itrack->hits.size());
		  hist_trks_chi2->Fill(itrack->chi2);
		  vector<double> kAx = itrack->kinkAx; 	vector<double> kAy = itrack->kinkAy;
		  for(int ip=0;ip<4;ip++)
		    hist_kink[ip]->Fill(1e6*kAx[ip],1e6*kAy[ip]);
		  for(int ihit=0;ihit<itrack->hits.size();ihit++){
		    cout<<"calculate residuals for hit "<<ihit<<" out of"<<itrack->hits.size()<<endl;
		    std::pair <double,double> dist = itrack->distance_hit_trk(itrack->hits[ihit]);
		    hist_hit_residuals[itrack->hits[ihit].plane]->Fill(1e6*dist.first,1e6*dist.second);
		  }


			double dxdz_mean(0);
			double dydz_mean(0);
			for (auto icell = itrack->cells.begin(); icell != itrack->cells.end(); icell++){
			  int hit_do = icell->ihit_downstream;
			  int hit_up = icell->ihit_upstream;
			  double dz = (hits[hit_do].z - hits[hit_up].z);
			  double dxdz = (hits[hit_do].x - hits[hit_up].x) / dz;
			  double dydz = (hits[hit_do].y - hits[hit_up].y) / dz;
			  dxdz_mean += dxdz;
			  dydz_mean += dydz;
			}
			dxdz_mean /= itrack->cells.size();
			dydz_mean /= itrack->cells.size();
			hist_dxdz_track->Fill(dxdz_mean, dydz_mean);
			double breaking_ang = BreakingAngle(*itrack);
			hist_breakang_trk->Fill(breaking_ang);

			//check tracks
			THits trkhits = itrack->hits;
			vector<int> MCids;
			cout<<"MC ids: "<<trkhits[0].MCtrk;
			MCids.push_back(trkhits[0].MCtrk);
			for (auto ihit = trkhits.begin()+1; ihit != trkhits.end(); ihit++){
			  int MCtrk_cur = ihit->MCtrk;
			  cout<<" "<<MCtrk_cur;
			  bool mcneu=false;
			  for(int imc=0;imc<MCids.size();imc++){
			    if(MCtrk_cur == MCids[imc]){
			      mcneu=false;
			      break;
			    }
			    else{
			      mcneu=true;
			    }
			  }
			  if(mcneu) MCids.push_back(MCtrk_cur);
			}
			// for(int imc=0;imc<MCids.size();imc++){
			//   cout<<" ! "<<MCids[imc]<<"";
			// }
			cout<<" Number of MCinREC trks="<<MCids.size()<<endl;
			hist_trks_pure->Fill(MCids.size());
		}
	}

  	cout << endl << " some statistics for " << nevents << " events " << endl;
	cout << " total number of hits processed: " << mean_hits << endl;
	cout << " total number of tracks found: " << mean_tracks << endl;
	cout << " ratio tracks / hits " << ((double) mean_tracks)/((double) mean_hits) << endl;
	cout << " mean number of hits " << ((double) mean_hits)/((double) nevents) << endl;
	cout << " mean number of tracks " << ((double) mean_tracks)/((double) nevents) << endl;

	canvas_distributions.Divide(2,2);
	for (int iplane = 0; iplane < 4; iplane++){
		canvas_distributions.cd(iplane+1);
		hist_XY[iplane]->Draw("colz");
	}
	canvas_distributions.Print("hit_ditros.pdf(");
	canvas_distributions.cd(2);
	hist_dxdz_track->Draw("colz");
	canvas_distributions.cd(3);
	//	hist_trk_mult->Draw("colz");
	//	hist_breakang_trk->Draw();
	gPad->Clear();
	canvas_distributions.cd(4);
	//	hist_breakang_trk->Draw();
	gPad->Clear();
	canvas_distributions.cd(1);
	gPad->SetLogy();
	hist_breakang_trk->Draw();
	canvas_distributions.Print("hit_ditros.pdf(");
	canvas_distributions.cd(1);
	//	gPad->SetLogy();
	hist_hits_mctrk->SetLineWidth(2);
	hist_hits_mctrk->Draw();
	hist_hits_trk->SetLineColor(2);
	hist_hits_trk->SetLineWidth(2);
	hist_hits_trk->Draw("same");
	TLegend* leg = new TLegend(0.15, 0.8, .3, .99);
	leg->AddEntry(hist_hits_mctrk, "MC", "l");
	leg->AddEntry(hist_hits_trk, "REC", "l");
	leg->Draw();
	//	gPad->Clear();
	canvas_distributions.cd(2);
	//	gPad->SetLogy();
	TH1F *hist_h_trk=(TH1F*)hist_hits_trk->Clone();
	hist_h_trk->SetName("hist_h_trk");
	hist_h_trk->Divide(hist_hits_mctrk);
	hist_h_trk->GetYaxis()->SetTitle("REC/MC");
	//	hist_h_trk->GetYaxis()->SetRangeUser(0.8,1.5);
	hist_h_trk->Draw();

	canvas_distributions.cd(4);
	gPad->SetLogy();
	hist_trks_pure->Draw();
	//	gPad->Clear();
	canvas_distributions.cd(3);
	hist_trk_mult->Draw("colz");
	//	gPad->Clear();
	canvas_distributions.Print("hit_ditros.pdf(");
	canvas_distributions.cd(1);
	gPad->SetLogy(0);
	hist_kink[1]->Draw("LEGO2Z");
	canvas_distributions.cd(2);
	hist_kink[2]->Draw("LEGO2Z");
	canvas_distributions.cd(3);
	hist_trks_chi2->Draw();
	canvas_distributions.cd(4);
	gPad->Clear();
//hist_kink[3]->Draw("LEGO");
	canvas_distributions.Print("hit_ditros.pdf(");
	for (int iplane = 0; iplane < 4; iplane++){
		canvas_distributions.cd(iplane+1);
		gPad->SetLogy(0);
		hist_hit_residuals[iplane]->Draw("colz");
	}


	canvas_distributions.Print("hit_ditros.pdf)");

	TFile *f = new TFile("hit_ditros.root","RECREATE");
	hist_trk_mult->Write();
	hist_h_trk->Write();
	hist_hits_trk->Write();
	hist_hits_mctrk->Write();
	leg->Write();
	hist_breakang_trk->Write();
	hist_trks_pure->Write();
hist_trks_chi2->Write();
 for(int ip=0;ip<4;ip++){
  hist_kink[ip]->Write();
  hist_hit_residuals[ip]->Write();
 }
	f->Close();

	//print values----------------------
	//Nmc = number of MC trks with >= 3 hits
	//Nperfect = number of REC tracks with 100% hits of one MC trk
	//Nrec = total number of REC tracks
	//Nperfect/Nmc = efficiency; (Nrec-Nperfect)/Nmc = fake; (Nmc-Nperfect)/Nmc = missed
	double Nmc = hist_hits_mctrk->GetBinContent(4)+hist_hits_mctrk->GetBinContent(5);
	double Nperfect = hist_trks_pure->GetBinContent(2);
	double Nrec = hist_trks_pure->GetEntries();
	cout<<"Nmc = "<<Nmc<<" Nrec = "<<Nrec<<" Nperfect = "<<Nperfect<<endl;
	cout<<"Eff = "<<Nperfect/Nmc<<" missed = "<<(Nmc-Nperfect)/Nmc<<" fake = "<<(Nrec-Nperfect)/Nmc<<endl;
	//[END]print values--------------
}

double stepfunction(const double* x, const double* pixelsize){
	/*double result = x[0];
	result *= 1e6;
lö	result /= 80;
	result = floor(result);
	result *= 80.;
	result *= 1e-6;
	return result;*/
	return (floor((x[0]*1e6+pixelsize[0]/2)/pixelsize[0])*pixelsize[0])*1e-6;
}

// simulate electrons going through 4 HV-MAPS mupix6 prototypes
// behind the tagger (setup of march 2015 at MAMI)
// simulate ntracks which hits are distributed over nevents
THits generate_hits(){
  // const int ntracks_mean = 2;
  // const int ntracks_mean = 20;
  // int ntracks = gRandom->Poisson(ntracks_mean); 
  const int ntracks_mean = 1;
   int ntracks = ntracks_mean;
  THits result;
	const int nevents = 1; // not used yet
	//	const double momentum = 0.9*1.5; // GeV/c
	const double momentum = 1; // GeV/c
	//	const double momentum = 15; // GeV/c
	// direction from the focal plane. 0 is perpendicular to the detector plane
	// unit is rad
	const double dxdz_mean = -0.005;//-0.01;//-0.1;
	const double dydz_mean = +0.003;

	const double dxdz_sigma = 0.001;
	const double dydz_sigma = 0.001;
	const double posx = 0.1; // location of the starting point +/- [m]
	const double posy = 0.01; // location of the starting point +/- [m]
	
	const double pixelsize = 80; // um
	// z positions of planes in m
	double z[5] = {0.0, 0.9, 1.0, 1.1, 1.2};
	//double z[4] = {0.0, 0.095, 0.21, 0.3};
		// acceptance in the plane
	const double xmin = -1.5e-3;
	const double xmax = 1.5e-3;

	const double beta = 1/sqrt(1+pow(0.511/momentum,2));
	//cout << "the beta factor for " << momentum << " GeV/c electrons is " << beta << endl;
	const double x_over_x0 = 2.329/21.82*350/1000/10;
	//cout << "x/X0 is" << x_over_x0 << endl;
	const double theta_0_slicon = 13.6/beta/(momentum*1e3)*sqrt(x_over_x0)*(1+0.038*log(x_over_x0));
	//cout << "scattering angle of 350 mum silicon is " << theta_0_slicon << endl;

	//	while(result.size()<((ntracks-1)*4+3)){
	//	while(result.size()<((ntracks-1)*3+2)){
	while(result.size()<(ntracks*4)){

	double dxdz =  gRandom->Gaus(dxdz_mean, dxdz_sigma);
	double dydz =  gRandom->Gaus(dydz_mean, dydz_sigma);

	TVector3 dir(0., 0., momentum);
	dir.SetXYZ(dxdz, dydz, momentum);

	TVector3 pos(gRandom->Uniform(-posx, posx), gRandom->Uniform(-posy, posy), 0.); // m
	//	cout<<"POSITION ("<<pos.X()<<", "<<pos.Y()<<")"<<endl;
	int id_gl=0;
	//	cout<<"Generagted hit ID: ";
	// propagate to the first plane
	for (int itrack = 0; itrack < ntracks; itrack++)
	for (int iplane = 0; iplane < 4; iplane++){
		pos += dir.Unit()*(z[iplane+1]-z[iplane]);
		TVector3 pos_plane_0 = pos;
		double X = pos.X();
		double Y = pos.Y();

		// //TEST for missed planes
		// if(iplane==0) continue;
		// simulate a granularity of the detector in theta of XX mum
		//		if (xmin < X && X < xmax && xmin < Y && Y < xmax)
		{
		  TVector3 pos_reco_plane(stepfunction(&X,&pixelsize),
					  stepfunction(&Y,&pixelsize),
					  z[iplane+1]);
			THit hit;
			hit.id = id_gl; 
			//		cout<<id_gl<<" ";
			id_gl++;
			hit.x = pos_reco_plane.X();
			hit.y = pos_reco_plane.Y();
			hit.z = pos_reco_plane.Z();
			hit.plane = iplane;
			hit.MCtrk = itrack;
			/*
			// missalign
			if (iplane == 1){
				double _x = hit.x;
				double _y = hit.y;
				hit.x = _x *  cos(0.06) + _y * sin(0.06) + 0.00016;
				hit.y = _x * -sin(0.06) + _y * cos(0.06) - 0.00004;
			}

			if (iplane == 2){
				double _x = hit.x;
				double _y = hit.y;
				hit.x = _x *  cos(0.1) + _y * -sin(0.1) - 0.00020;
				hit.y = _x *  sin(0.1) + _y *  cos(0.1) - 0.00012;
			}

			if (iplane == 3){
				double _x = hit.x;
				double _y = hit.y;
				hit.x = _x *  cos(0.04) + _y * -sin(0.04) + 0.00012;
				hit.y = _x *  sin(0.04) + _y *  cos(0.04) + 0.00020;
			}
			
			*/
			//if (iplane != 3)
			if (xmin < X && X < xmax && xmin < Y && Y < xmax){
				result.push_back(hit);
			}
			

			// smear angle due to silicon in the xz plane
			double tan_theta_new = tan(atan(dir.X()/dir.Z())+gRandom->Gaus(0,theta_0_slicon));
			double l_x = dir.X()*dir.X()+dir.Z()*dir.Z();
			double x_new = sqrt(l_x / (1/(tan_theta_new*tan_theta_new)+1));
			// recover the lost sign
			if (tan_theta_new < 0 && x_new > 0) x_new = -x_new;
			double z_new = sqrt(l_x - x_new*x_new);
			dir.SetX(x_new);
			dir.SetZ(z_new);
			// same smearing procedure in the yz plane
			tan_theta_new = tan(atan(dir.Y()/dir.Z())+gRandom->Gaus(0,theta_0_slicon));
			double l_y = dir.Y()*dir.Y()+dir.Z()*dir.Z();
			double y_new = sqrt(l_y / (1/(tan_theta_new*tan_theta_new)+1));
			// recover the lost sign
			if (tan_theta_new < 0 && y_new > 0) y_new = -y_new;
			z_new = sqrt(l_y - y_new*y_new);
			dir.SetY(y_new);
			dir.SetZ(z_new);
		} //else {
		//	break;
		//}
	}
	//	cout<<" "<<endl;
	//	if(result.size()<((ntracks-1)*4+3)){
	if(result.size()<(ntracks*4)){
	//	if(result.size()<((ntracks-1)*3+2)){
 	  //	  cout<<"result.size() = "<<result.size()<<endl;
 	  result.clear();
 	}
 	//	else 
 	  //  cout<<" return result!"<<endl;
	}
	return result;
}





#include <cstdint>
#include <omp.h>

int main(void)
{
	track_finder();
	return 0;
}