/**
 * \file CaloAxis.cpp
 * \author Elena Vannuccini
 */
#if !defined(__CINT__) || defined(__MAKECINT__)

#include <CaloAxis.h>

#endif
//===============================================================================
//
//
//
//
//===============================================================================
/**
 * Add a calorimeter hit
 * @param iis strip 0-95 
 * @param iip plane 0-21
 * @param fq signal
 * @param fz z-coordinate
 * @param fxy x or y-coordinate
 */
void CaloEvent::Add(int iis, int iip, float fq, float fz, float fxy){
//    cout << " CaloEvent::Add()"<<endl; 
    ids.push_back(iis);
    idp.push_back(iip);
    q.push_back(fq);
    zcoord.push_back(fz);
    xycoord.push_back(fxy);	
};
/**
 * Reset
 */
void CaloEvent::Reset(){
//    cout << " CaloEvent::Reset()"<<endl; 
    ids.clear();
    idp.clear();
    q.clear();
    zcoord.clear();
    xycoord.clear();	
};
/**
 * Set a CaloEvent from CaloLevel1 object
 */
void CaloEvent::Set(CaloLevel1* calo, int view, Bool_t usemechanicalalignement){
	
    Reset();
//    cout << " CaloEvent::Set()"<<endl; 
    CaloStrip cstrip;
    cstrip = CaloStrip(calo,usemechanicalalignement);  

    for(int ih=0;ih<calo->istrip;ih++){
	int iv=-1;
	int ip=-1;
	int is=-1;
	calo->DecodeEstrip(ih,iv,ip,is);
	if(iv==view){
	    cstrip.Set(view,ip,is);
	    float fxy = 0.;
	    if( !view ) fxy = cstrip.GetX();
	    else        fxy = cstrip.GetY();
	    float fz = cstrip.GetZ();
	    float fq = cstrip.GetE();	    
	    if(fq>0)Add(is,ip,fq,fz,fxy);   
	}	
    }
	
};
//===============================================================================
//
//
//
//
//===============================================================================
/**
 * Initialization
 */
void CaloAxis::Init(){
    for(Int_t i=0; i<2;i++){
	par[i]=0;
	for(Int_t j=0; j<2;j++)covpar[i][j]=0;
    };
    sel  = 0;
    axis = 0;
    for(Int_t i=0; i<22; i++){
	cog[i]=0;
	qpl[i]=0;
    }
}

/**
 * Reset 
 */    
void CaloAxis::Reset(){
    x.clear();
    y.clear();
    w.clear();
    q.clear();
    if(sel)sel->Delete();
    if(axis)axis->Delete();
    Init();
}
/**
 * 
 */
void CaloAxis::Add(float xin, float yin){
    x.push_back(xin);
    y.push_back(yin);
    w.push_back(1.);
    q.push_back(0.);
};
void CaloAxis::Add(float xin, float yin, float qin){
    x.push_back(xin);
    y.push_back(yin);
    q.push_back(qin);
    w.push_back(1.);
};
void CaloAxis::Add(float xin, float yin, float qin, float win){
    x.push_back(xin);
    y.push_back(yin);
    w.push_back(win);
    q.push_back(qin);
};
/**
 * Fit a straight line thtrough the stored hits
 */
int CaloAxis::Fit(){

    Float_t SSS = 0;
    Float_t SSX = 0;
    Float_t SXX = 0;
	
    Float_t SSY = 0;
    Float_t SXY = 0;

    Int_t np=0;
    for(Int_t i=0; i<(int)(x.size()); i++){
	SSS += w[i]*w[i];
	SSX += x[i]*w[i]*w[i];
	SXX += x[i]*x[i]*w[i]*w[i];
	SSY += y[i]*w[i]*w[i];
	SXY += x[i]*y[i]*w[i]*w[i];
	if(w[i])np++;
    }
    Float_t det = SSS*SXX-SSX*SSX;
    if(det==0)return 0;
    if(np<3)return 0;
    par[0] = (SSY*SXX-SXY*SSX)/det;
    par[1] = (SXY*SSS-SSY*SSX)/det;
    return 1;
};

/**
 * Print select hits
 */
void CaloAxis::Print(){
    for(Int_t i=0; i<(int)(x.size()); i++)
	if(w[i])cout << x[i] << " - "<<y[i]<<endl;
}


float* CaloAxis::GetX(){
    float *xout = new float[(int)(x.size())];
    for(Int_t i=0; i<(int)(x.size()); i++)xout[i]=x[i];
    return xout;
}

float* CaloAxis::GetY(){
    float *yout = new float[(int)(y.size())];
    for(Int_t i=0; i<(int)(y.size()); i++) yout[i]=y[i];
    return yout;
}

float* CaloAxis::GetQ(){
    float *qout = new float[(int)(q.size())];
    for(int i=0; i<(int)(q.size()); i++) qout[i]=q[i];
    return qout;
}

float CaloAxis::GetChi2(){
    float chi2=0;
    int   nchi=0;
    for(Int_t i=0; i<(int)(y.size()); i++){
	chi2 += w[i]*w[i]*(y[i]-par[0]-par[1]*x[i])*(y[i]-par[0]-par[1]*x[i]);
	nchi += (int)w[i];
    };
    if(nchi<3)return -999;
    chi2=chi2/(nchi-2);
    return chi2;
}

/**
 * Return the total signal of the hits included to evaluate the axis
 */
float CaloAxis::GetQaxis(){
    float qaxis=0;
    for(Int_t i=0; i<(int)(q.size()); i++) qaxis +=q[i];
    return qaxis;
}
/**
 * Signal of the hits included to evaluate the axis, 
 * within a distance toll from the axis
 */
float CaloAxis::GetQaxis(float toll){
    float qaxis=0;
    for(int ih=0;ih<cevent->GetN(); ih++){
	float x = cevent->xycoord[ih];
	float z = cevent->zcoord[ih];
	float q = cevent->q[ih];
//	int ip = cevent->idp[ih];
	float d = fabs(x-par[0]-par[1]*z)/sqrt(par[1]*par[1]+1);
	if( d < toll )qaxis+=q;
    }
    return qaxis;
}

/**
 * Average signal per strip
 */
float CaloAxis::GetQstrip(){
	
    float qaxis=0;
    for(Int_t i=0; i<(int)(q.size()); i++) qaxis +=q[i];
    if(q.size()==0) return 0.;
    return qaxis/q.size();
}
/**
 * Coordinate of the last hit included in the axis
 */
float CaloAxis::GetXmin(){
    float zmin=9999;
    for(Int_t i=0; i<(int)(x.size()); i++) if(x[i]<zmin) zmin = x[i];
    return zmin;
}
/**
 * Coordinate of the first hit included in the axis
 */
float CaloAxis::GetXmax(){
    float zmax=-9999;
    for(Int_t i=0; i<(int)(x.size()); i++) if(x[i]>zmax) zmax = x[i];
    return zmax;
	
}

/**
 * Draw the axis (in PAMELA reference plane)
 */

void CaloAxis::Draw(int col){
	
//	cout << " CaloAxis::Draw()"<<endl;
    if(GetN()<3)return;

    sel = new TPolyMarker(GetN(),GetY(),GetX());
    sel->SetMarkerSize(0.4);
    sel->SetMarkerColor(col);

    axis = new TLine( par[0]+GetXmin()*par[1], GetXmin(), par[0]+GetXmax()*par[1], GetXmax() );
    axis->SetLineWidth(1);
    axis->SetLineColor(col);

//	cout << sel << endl;
//	cout << axis << endl;

    sel->Draw();
    axis->Draw();
};

/**
 * Fit an axis through the calorimeter pattern. 
 * NB!! This method is optimized for non-interacting particles.
 */
int CaloAxis::FitAxis( CaloLevel1* calo , Int_t view , Float_t toll , Bool_t usemechanicalalignement ){

//	cout << "CaloAxis::FitAxis(...)"<<endl;

    Set(calo,view,usemechanicalalignement);

    // ------------------------------
    // first : all hits included, w=1
    // ------------------------------
    for(int ih=0;ih<cevent->GetN(); ih++){
	float x = cevent->xycoord[ih];
	float z = cevent->zcoord[ih];
	float q = cevent->q[ih];
	Add(z,x,q);
    }

    if(GetN()<3)return 0;
    Fit();
//	cout << " n. hits :"<<GetN()<<endl;
//	cout << " P0 P1   :"<<par[0]<< " "<<par[1]<<endl;
//	cout << " Chi2    :"<<GetChi2()<<endl;
//	cout << " Q axis  :"<<GetQaxis()<<endl;
//	cout << " Q strip :"<<GetQstrip()<<endl;

    float P0;
    float P1;
    int   dof;

    // ---------------------------
    // second : iteration
    // ---------------------------
    int niter=0;                      // number of iterations
    int pok[22];
	
    // prova
    // pesi lineari
    float W22=1.;
    float W1=10.;
    float b=(W22-W1)/(22.-1.);// (w22 - w1)/(22-1) 
    float a=W1-b;

    for(int i=0; i<22; i++)pok[i]=0;
    do{
	niter++;
	float ttoll = toll;           //tolerance (cm)
	if(niter==1)ttoll = 10.*toll;
	dof = GetN();           //previous fit
	P0  = par[0];
	P1  = par[1];
	Reset();                //fit reset
	for(int ih=0;ih<cevent->GetN(); ih++){
	    float x = cevent->xycoord[ih];
	    float z = cevent->zcoord[ih];
	    float q = cevent->q[ih];
	    int ip = cevent->idp[ih];
	    float d = fabs(x-P0-P1*z)/sqrt(P1*P1+1);
//		cout<<d<<endl;
	    if( d < ttoll && (niter==1 || (niter>1 && pok[ip]==1)) ){ 
//		if( d < ttoll ){
		float W=a+b*(ip+1);
//		    cout << ip << " "<<W<<endl;
		Add(z,x,q,W);
		pok[ip]=1;
	    }
	}
// break the track if more than 3 planes are missing
	int   nmissing = 0;
	for(int ip=0; ip<22; ip++){
	    if(pok[ip]==0){
		nmissing++;
	    }else{
		nmissing = 0;
	    }
	    if(nmissing==3){
		for(int ipp=ip+1; ipp<22; ipp++)pok[ipp]=0;
		break;
	    }
	}


	if(niter==100)break;
	if(GetN()<3)break;
	Fit();
    }while(niter==1 || GetN() != dof);
//	cout << " >>> "<<GetN()<<endl;
    if(GetN()<3)return 0;
    Fit();
//	cout << " n. hits :"<<GetN()<<endl;
//	cout << " P0 P1   :"<<par[0]<< " "<<par[1]<<endl;
//	cout << " Chi2    :"<<GetChi2()<<endl;
//	cout << " Q axis  :"<<GetQaxis()<<endl;
//	cout << " Q strip :"<<GetQstrip()<<endl;

    // ---------------------------------------------
    // third : selecting only closest strip-clusters
    // and fit baricenters
    // ---------------------------------------------
    P0  = par[0];
    P1  = par[1];
    Reset();
 
    float dmin[22];
    int closest[22];
    for(int ip=0; ip<22; ip++){
	dmin[ip]=999;
	closest[ip]=-1;
    }
    for(int ih=0;ih<cevent->GetN(); ih++){
	float x = cevent->xycoord[ih];
	float z = cevent->zcoord[ih];
//	float q = cevent->q[ih];
	int ip = cevent->idp[ih];
//	int is = cevent->ids[ih];
	float d = fabs(x-P0-P1*z)/sqrt(P1*P1+1);
	if( d < toll && d<dmin[ip] && pok[ip]==1 ){
//		Add(z,x,q,q);
	    closest[ip]=ih;
//		cog[ip] += x*q;
//		qpl[ip] += q;
	}
    }
    for(Int_t ip=0; ip<22; ip++){
	if(closest[ip]!=-1){
	    float x = cevent->xycoord[closest[ip]];
	    float z = cevent->zcoord[closest[ip]];
	    float q = cevent->q[closest[ip]];
//	    int is = cevent->ids[closest[ip]];
	    Add(z,x,q,q);
	    cog[ip] += x*q;
	    qpl[ip] += q;
//		cout << ip << " -o- "<<is<<endl;
	}
    }
    // add +/- one strip
    for(int ih=0;ih<cevent->GetN(); ih++){
	float x = cevent->xycoord[ih];
	float z = cevent->zcoord[ih];
	float q = cevent->q[ih];
	int ip = cevent->idp[ih];
	int is = cevent->ids[ih];
	if( closest[ip]!=-1 ){
	    int isc = cevent->ids[closest[ip]];
	    if( is == isc+1 || is == isc-1 ){
		Add(z,x,q,q);
		cog[ip] += x*q;
		qpl[ip] += q;
//		    cout << ip << " -+- "<<is<<endl;
	    }
	}
    }
    Fit();

    if(GetN()<3)return 0;
    for(int ip=0; ip<22; ip++){
	if(qpl[ip]!=0){
	    cog[ip]=cog[ip]/qpl[ip];
//	    Add(z,cog[ip],cog[ip],0.7);
	}
    }
	
// 	cout << " n. hits :"<<GetN()<<endl;
//	cout << " P0 P1   :"<<par[0]<< " "<<par[1]<<endl;
// 	cout << " Chi2    :"<<GetChi2()<<endl; 
//	cout << " Q axis  :"<<GetQaxis()<<endl; 
// 	cout << " Q strip :"<<GetQstrip()<<endl; 
//	cout << " ---------------------------"<<endl;
    return 1;
};
    
/**
 * Fit an axis through the calorimeter pattern. 
 * NB!! This method is optimized for non-interacting particles.
 */

int CaloAxis::FitShower( CaloLevel1* calo , Int_t view , Float_t toll , Bool_t usemechanicalalignement ){

//	cout << "CaloAxis::FitAxis(...)"<<endl;

    Set(calo,view,usemechanicalalignement);

    // ------------------------------
    // first :
    // ------------------------------
    for(int ih=0;ih<cevent->GetN(); ih++){
	float x = cevent->xycoord[ih];
//	float z = cevent->zcoord[ih];
	float q = cevent->q[ih];
	int ip  = cevent->idp[ih];
	cog[ip] += x*q;
	qpl[ip] += q;
//	    Add(z,x,q);
    }
    for(int ip=0; ip<22; ip++){
	if(qpl[ip]!=0)cog[ip]=cog[ip]/qpl[ip];
//	    cout << ip << " > "<<qpl[ip]<<" "<<cog[ip]<<endl;
    }
    // ----------------
    // plane of maximum
    // ----------------
    float qplmax=1;
    int ipmax = -1;
    float dmin[22];
    int closest[22];
    for(int ip=0; ip<22; ip++)if(qpl[ip]>qplmax){
	ipmax=ip;
	qplmax=qpl[ip];
	dmin[ip]=1000.;//init
	closest[ip]=-1;//init
    }
    for(int ih=0;ih<cevent->GetN(); ih++){
	float x = cevent->xycoord[ih];
	float z = cevent->zcoord[ih];
	float q = cevent->q[ih];
	int ip  = cevent->idp[ih];
//	    if( (ipmax>3 && ip<=ipmax) || ipmax<=3 )Add(z,x,q);
	if( ip<=ipmax || ip<=3 ){
	    Add(z,x,q);
	}
    }
    if(GetN()<3)return 0;
    Fit();
//	cout << " n. hits :"<<GetN()<<endl;
//	cout << " P0 P1   :"<<par[0]<< " "<<par[1]<<endl;
//	cout << " Chi2    :"<<GetChi2()<<endl;
//	cout << " Q axis  :"<<GetQaxis()<<endl;
//	cout << " Q strip :"<<GetQstrip()<<endl;

    float P0;
    float P1;
    int   dof;

    // ---------------------------
    // second : iteration
    // ---------------------------
    int niter=0;                      // number of iterations
    int pok[22];
    // prova
    // pesi lineari
    float W22=1.;
    float W1=10.;
    float b=(W22-W1)/(22.-1.);// (w22 - w1)/(22-1)
    float a=W1-b;

    for(int i=0; i<22; i++){
	pok[i]=0;	
	cog[i]=0;
	qpl[i]=0;
    }
    do{
	niter++;
	float ttoll = toll;           //tolerance (cm)
	if(niter==1)ttoll = 10.*toll;
	dof = GetN();           //previous fit
	P0  = par[0];
	P1  = par[1];
	Reset();                //fit reset
	for(int i=0; i<22; i++){
	    cog[i]=0;
	    qpl[i]=0;
	}
	for(int ih=0;ih<cevent->GetN(); ih++){
	    float x = cevent->xycoord[ih];
	    float z = cevent->zcoord[ih];
	    float q = cevent->q[ih];
	    int ip = cevent->idp[ih];
	    float d = fabs(x-P0-P1*z)/sqrt(P1*P1+1);
//		cout<<d<<endl;
	    if( 
		(niter==1 || (niter>1 && pok[ip]==1)) && 
//		    (ip<=ipmax || ip<=3) &&
		(ip<ipmax || ip<=3) &&
//		    ((ipmax>3 && ip<=ipmax) || ipmax<=3) &&
		true){
		    
		if(niter==1 && d<dmin[ip]){
		    dmin[ip]=d;
		    closest[ip]=ih;
		}

		float W=a+b*(ip+1);
//		    cout << ip << " "<<W<<endl;
		if( d < ttoll || ( niter==2 && ih==closest[ip] )){
/*  			if(ip==ipmax && ipmax>1)Add(z,x,q,W*q);  */
/*  			else Add(z,x,q,W);  */
		    cog[ip] += x*q;
		    qpl[ip] += q;
		    Add(z,x,q,W);
		    pok[ip]=1;
		}
	    }
	}
// break the track if more than 3 planes are missing
	int   nmissing = 0;
	for(int ip=0; ip<22; ip++){
	    if(pok[ip]==0){
		nmissing++;
	    }else{
		nmissing = 0;
	    }
	    if(nmissing==6){
		for(int ipp=ip+1; ipp<22; ipp++)pok[ipp]=0;
		break;
	    }
	}
	    
	for(int ip=0; ip<22; ip++)if(qpl[ip]!=0)cog[ip]=cog[ip]/qpl[ip];
	    
	if(niter==100)break;
	if(GetN()<3)break;

	Fit();

    }while(niter==1 || GetN() != dof);
//	cout << " >>> "<<GetN()<<endl;
    if(GetN()<3)return 0;
    Fit();
	
// 	cout << " n. hits :"<<GetN()<<endl;
//	cout << " P0 P1   :"<<par[0]<< " "<<par[1]<<endl;
// 	cout << " Chi2    :"<<GetChi2()<<endl;
//	cout << " Q axis  :"<<GetQaxis()<<endl;
// 	cout << " Q strip :"<<GetQstrip()<<endl;
//	cout << " ---------------------------"<<endl;
    return 1;
};
    
ClassImp(CaloEvent);
ClassImp(CaloAxis);