/**
 * \file TrkLevel2.cpp
 * \author Elena Vannuccini
 */
#include <TrkLevel2.h>
#include <iostream>
using namespace std;
//......................................
// F77 routines
//......................................
extern "C" {    
    void dotrack_(int*, double*, double*, double*, double*, int*);
    void dotrack2_(int*, double*, double*, double*, double*,double*, double*, double*,int*);
    int  readb_(const char*); 
}
//--------------------------------------
//
//
//--------------------------------------
TrkTrack::TrkTrack(){
    seqno = -1; 
    image = -1; 
    chi2  = 0;
    for(int it1=0;it1<5;it1++){
	al[it1] = 0;
	for(int it2=0;it2<5;it2++)
	    coval[it1][it2] = 0;
    };
    for(int ip=0;ip<6;ip++){
	xgood[ip]  = 0;
	ygood[ip]  = 0;
	xm[ip]     = 0;
	ym[ip]     = 0;
	zm[ip]     = 0;
	resx[ip]   = 0;
	resy[ip]   = 0;
	xv[ip]     = 0;
	yv[ip]     = 0;
	zv[ip]     = 0;
	axv[ip]    = 0;
	ayv[ip]    = 0;
	dedx_x[ip] = 0;
	dedx_y[ip] = 0;
    };    
};
//--------------------------------------
//
//
//--------------------------------------
TrkTrack::TrkTrack(const TrkTrack& t){
    seqno = t.seqno; 
    image = t.image; 
    chi2  = t.chi2;
    for(int it1=0;it1<5;it1++){
	al[it1] = t.al[it1];
	for(int it2=0;it2<5;it2++)
	    coval[it1][it2] = t.coval[it1][it2];
    };
    for(int ip=0;ip<6;ip++){
	xgood[ip]  = t.xgood[ip];
	ygood[ip]  = t.ygood[ip];
	xm[ip]     = t.xm[ip];
	ym[ip]     = t.ym[ip];
	zm[ip]     = t.zm[ip];
	resx[ip]   = t.resx[ip];
	resy[ip]   = t.resy[ip];
	xv[ip]     = t.xv[ip];
	yv[ip]     = t.yv[ip];
	zv[ip]     = t.zv[ip];
	axv[ip]    = t.axv[ip];
	ayv[ip]    = t.ayv[ip];
	dedx_x[ip] = t.dedx_x[ip];
	dedx_y[ip] = t.dedx_y[ip];
    };    
};
//--------------------------------------
//
//
//--------------------------------------
/**
 * Evaluates the trajectory in the apparatus associated to the track. 
 * It integrates the equations of motion in the magnetic field. The magnetic field should be previously loaded ( by calling  TrkLevel2::LoadField() ), otherwise an error message is returned.  
 * @param t pointer to an object of the class Trajectory, 
 * which z coordinates should be previously initialized by calling the proper constructor ( Trajectory::Trajectory(int n, float* zin) ).
 * @return error flag.
 */
int TrkTrack::DoTrack(Trajectory* t){

    double *dxout = new double[t->npoint];
    double *dyout = new double[t->npoint];
    double *dzin = new double[t->npoint];
    double dal[5];

    int ifail = 0;

    for (int i=0; i<5; i++)         dal[i]  = (double)al[i];
    for (int i=0; i<t->npoint; i++) dzin[i] = (double)t->z[i];

    dotrack_(&(t->npoint),dzin,dxout,dyout,dal,&ifail);
    
    for (int i=0; i<t->npoint; i++){
	t->x[i] = (float)*dxout++;
	t->y[i] = (float)*dyout++;
    }

//    delete [] dxout;
//    delete [] dyout;
//    delete [] dzin;

    return ifail;
};
//--------------------------------------
//
//
//--------------------------------------
/**
 * Evaluates the trajectory in the apparatus associated to the track. 
 * It integrates the equations of motion in the magnetic field. The magnetic field should be previously loaded ( by calling  TrkLevel2::LoadField() ), otherwise an error message is returned.  
 * @param t pointer to an object of the class Trajectory, 
 * which z coordinates should be previously initialized by calling the proper constructor ( Trajectory::Trajectory(int n, float* zin) ).
 * @return error flag.
 */
int TrkTrack::DoTrack2(Trajectory* t){

    double *dxout   = new double[t->npoint];
    double *dyout   = new double[t->npoint];
    double *dthxout = new double[t->npoint];
    double *dthyout = new double[t->npoint];
    double *dtlout  = new double[t->npoint];
    double *dzin    = new double[t->npoint];
    double dal[5];

    int ifail = 0;

    for (int i=0; i<5; i++)         dal[i]  = (double)al[i];
    for (int i=0; i<t->npoint; i++) dzin[i] = (double)t->z[i];

    dotrack2_(&(t->npoint),dzin,dxout,dyout,dthxout,dthyout,dtlout,dal,&ifail);
    
    for (int i=0; i<t->npoint; i++){
	t->x[i]   = (float)*dxout++;
	t->y[i]   = (float)*dyout++;
	t->thx[i] = (float)*dthxout++;
	t->thy[i] = (float)*dthyout++;
	t->tl[i]  = (float)*dtlout++;
    }

//    delete [] dxout;
//    delete [] dyout;
//    delete [] dzin;

    return ifail;
};
//--------------------------------------
//
//
//--------------------------------------
//float TrkTrack::BdL(){
//};
//--------------------------------------
//
//
//--------------------------------------
Float_t TrkTrack::GetRigidity(){ 
	Float_t rig=0;
	if(chi2>0)rig=1./al[4];
	if(rig<0) rig=-rig;
	return rig;
};
//
Float_t TrkTrack::GetDeflection(){
	Float_t def=0;
	if(chi2>0)def=al[4];
	return def;
};
//
Float_t TrkTrack::GetDEDX(){
	Float_t dedx=0; 
	for(Int_t i=0; i<6; i++)dedx+=dedx_x[i]*xgood[i]+dedx_y[i]*ygood[i]; 
	dedx = dedx/(this->GetNX()+this->GetNY());
	return dedx;
};

//--------------------------------------
//
//
//--------------------------------------
void TrkTrack::Dump(){
    cout << endl << "========== Track " ;
    cout << endl << "al       : "; for(int i=0; i<5; i++)cout << al[i] << " ";
    cout << endl << "chi^2    : "<< chi2;
    cout << endl << "xgood    : "; for(int i=0; i<6; i++)cout << xgood[i] ;
    cout << endl << "ygood    : "; for(int i=0; i<6; i++)cout << ygood[i] ;
    cout << endl << "xm       : "; for(int i=0; i<6; i++)cout << xm[i] << " ";
    cout << endl << "ym       : "; for(int i=0; i<6; i++)cout << ym[i] << " ";
    cout << endl << "zm       : "; for(int i=0; i<6; i++)cout << zm[i] << " ";
    cout << endl << "dedx_x   : "; for(int i=0; i<6; i++)cout << dedx_x[i] << " ";
    cout << endl << "dedx_y   : "; for(int i=0; i<6; i++)cout << dedx_y[i] << " ";
}
//--------------------------------------
//
//
//--------------------------------------
TrkSinglet::TrkSinglet(){
    plane    = 0;
    coord[0] = 0; 
    coord[1] = 0;
    sgnl     = 0;
};
//--------------------------------------
//
//
//--------------------------------------
TrkSinglet::TrkSinglet(const TrkSinglet& s){
    plane    = s.plane;
    coord[0] = s.coord[0]; 
    coord[1] = s.coord[1];
    sgnl     = s.sgnl;
};
//--------------------------------------
//
//
//--------------------------------------
void TrkSinglet::Dump(){
    int i=0;
    cout << endl << "========== Singlet " ;
    cout << endl << "plane    : " << plane;
    cout << endl << "coord[2] : "; while( i<2 && cout << coord[i] << " ") i++;
    cout << endl << "sgnl     : " << sgnl;
}
//--------------------------------------
//
//
//--------------------------------------
TrkLevel2::TrkLevel2(){
    good2    = -1;
    for(Int_t i=0; i<12 ; i++){
	crc[i] = -1;
    };
    Track    = new TClonesArray("TrkTrack");
    SingletX = new TClonesArray("TrkSinglet");
    SingletY = new TClonesArray("TrkSinglet");
//    Track    = 0;
//    Singlet = 0;
//    SingletY = 0;
}
//--------------------------------------
//
//
//--------------------------------------
void TrkLevel2::Dump(){
    TClonesArray &t  = *Track;
    TClonesArray &sx = *SingletX;
    TClonesArray &sy = *SingletY;

    cout << endl << endl << "=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-";
    cout << endl << "good2    : " << good2;
    cout << endl << "crc      : "; for(int i=0; i<12; i++) cout << crc[i];
    cout << endl << "ntrk()   : " << this->ntrk() ;
    cout << endl << "nclsx()  : " << this->nclsx();
    cout << endl << "nclsy()  : " << this->nclsy();
    for(int i=0; i<this->ntrk(); i++)     ((TrkTrack *)t[i])->Dump();
    for(int i=0; i<this->nclsx(); i++) ((TrkSinglet *)sx[i])->Dump();
    for(int i=0; i<this->nclsy(); i++) ((TrkSinglet *)sy[i])->Dump();
}
//--------------------------------------
//
//
//--------------------------------------
/**
 * Fills a TrkLevel2 object with values from a struct cTrkLevel2 (to get data from F77 common).
 */
void TrkLevel2::FillCommonVar(cTrkLevel2 *l2){
    //
//    Track    = new TClonesArray("TrkTrack");
//    SingletX = new TClonesArray("TrkSinglet");
//    SingletY = new TClonesArray("TrkSinglet");
//  temporary objects:
    TrkSinglet* t_singlet = new TrkSinglet();
    TrkTrack*   t_track   = new TrkTrack();
//  general variables
    good2 = l2->good2;
    for(Int_t i=0; i<12 ; i++){
	crc[i] = l2->crc[i];
    };
//  *** TRACKS ***
    TClonesArray &t = *Track;
    for(int i=0; i<l2->ntrk; i++){
	t_track->seqno = i;
	t_track->image = l2->image[i]-1;
//	cout << "track "<<i<<t_track->seqno << t_track->image<<endl;
	t_track->chi2  = l2->chi2_nt[i];
	for(int it1=0;it1<5;it1++){
	    t_track->al[it1] = l2->al_nt[i][it1];
	    for(int it2=0;it2<5;it2++)
		t_track->coval[it1][it2] = l2->coval[i][it2][it1];
	};
	for(int ip=0;ip<6;ip++){
	    t_track->xgood[ip]  = l2->xgood_nt[i][ip];
	    t_track->ygood[ip]  = l2->ygood_nt[i][ip];
	    t_track->xm[ip]     = l2->xm_nt[i][ip];
	    t_track->ym[ip]     = l2->ym_nt[i][ip];
	    t_track->zm[ip]     = l2->zm_nt[i][ip];
	    t_track->resx[ip]   = l2->resx_nt[i][ip];
	    t_track->resy[ip]   = l2->resy_nt[i][ip];
	    t_track->xv[ip]     = l2->xv_nt[i][ip];
	    t_track->yv[ip]     = l2->yv_nt[i][ip];
	    t_track->zv[ip]     = l2->zv_nt[i][ip];
	    t_track->axv[ip]    = l2->axv_nt[i][ip];
	    t_track->ayv[ip]    = l2->ayv_nt[i][ip];
	    t_track->dedx_x[ip] = l2->dedx_x[i][ip];
	    t_track->dedx_y[ip] = l2->dedx_y[i][ip];
	};
	new(t[i]) TrkTrack(*t_track);
	t_track->Clear();
    };
//  *** SINGLETS ***
    TClonesArray &sx = *SingletX;
    for(int i=0; i<l2->nclsx; i++){
	t_singlet->plane    = l2->planex[i];
	t_singlet->coord[0] = l2->xs[i][0];
	t_singlet->coord[1] = l2->xs[i][1];
	t_singlet->sgnl     = l2->signlxs[i];
	new(sx[i]) TrkSinglet(*t_singlet);
	t_singlet->Clear();
    }
    TClonesArray &sy = *SingletY;
    for(int i=0; i<l2->nclsy; i++){
	t_singlet->plane    = l2->planey[i];
	t_singlet->coord[0] = l2->ys[i][0];
	t_singlet->coord[1] = l2->ys[i][1];
	t_singlet->sgnl     = l2->signlys[i];
	new(sy[i]) TrkSinglet(*t_singlet);
	t_singlet->Clear();
	};
	
	delete t_track;
	delete t_singlet;
}
//--------------------------------------
//
//
//--------------------------------------
void TrkLevel2::Clear(){
    good2    = -1;
    for(Int_t i=0; i<12 ; i++){
	crc[i] = -1;
    };
/*    Track->RemoveAll();
    SingletX->RemoveAll();
    SingletY->RemoveAll();*/
	// modify to avoid memory leakage
	Track->Clear();
	SingletX->Clear();
	SingletY->Clear();
}
//--------------------------------------
//
//
//--------------------------------------
/**
 * Sort physical tracks and stores them in a TObjectArray, ordering by increasing chi**2 value (in case of track image, it selects the one with lower chi**2). The total number of physical tracks is given by GetNTracks() and the it-th physical track can be retrieved by means of the method GetTrack(int it).
 * This method is overridden by PamLevel2::GetTracks(), where calorimeter and TOF information is used.
 */
TClonesArray *TrkLevel2::GetTracks(){
    TClonesArray *sorted = GetTracks_NFitSorted();
    return sorted;
};

TClonesArray *TrkLevel2::GetTracks_Chi2Sorted(){

    TClonesArray *sorted = new TClonesArray("TrkTrack");
    TClonesArray &t = *Track;
    TClonesArray &ts = *sorted;
    int N=this->ntrk();
    vector<int> m(N); for(int i=0; i<N; i++)m[i]=1;

    int indo=0;
    int indi=0;
    while(N != 0){
	float chi2ref=1000000;
	for(int i=0; i<this->ntrk(); i++){
	    if(((TrkTrack *)t[i])->chi2 < chi2ref && m[i]==1){
		chi2ref = ((TrkTrack *)t[i])->chi2;
		indi = i;
	    }
	}
	if( ((TrkTrack *)t[indi])->image != -1 ){
	    m[((TrkTrack *)t[indi])->image] = 0;
	    N--;
	}
	new(ts[indo]) TrkTrack(*(TrkTrack*)t[indi]);
	m[indi] = 0;
	N--;	
	indo++;	
    }
    return sorted;
}
TClonesArray *TrkLevel2::GetTracks_NFitSorted(){

    TClonesArray *sorted = new TClonesArray("TrkTrack");
    TClonesArray &t = *Track;
    TClonesArray &ts = *sorted;
    int N=this->ntrk();
    vector<int> m(N); for(int i=0; i<N; i++)m[i]=1;

    int indo=0;
    int indi=0;
    while(N != 0){
	int nfit =0;
	float chi2ref=1000000;
	// first loop to search maximum num. of fit points
	for(int i=0; i<this->ntrk(); i++){
	    if( ((TrkTrack *)t[i])->GetNtot() >= nfit && m[i]==1){
		nfit =    ((TrkTrack *)t[i])->GetNtot();
//		cout << "1** "<<i<< " " << nfit<<endl;
	    }
	}
	//second loop to search minimum chi2 among selected
	for(int i=0; i<this->ntrk(); i++){
	    if(    ((TrkTrack *)t[i])->chi2 < chi2ref 
		&& ((TrkTrack *)t[i])->GetNtot()== nfit 
		&& m[i]==1){
		chi2ref = ((TrkTrack *)t[i])->chi2;
		indi = i;
//		cout << "2** "<<i<< " " << nfit <<" "<<chi2ref<<endl;
	    }
	}
	if( ((TrkTrack *)t[indi])->HasImage() ){
	    m[((TrkTrack *)t[indi])->image] = 0;
	    N--;

//	    Int_t nfiti=((TrkTrack *)t[((TrkTrack *)t[indi])->image  ])->GetNtot();
//	    Float_t chi2i=((TrkTrack *)t[((TrkTrack *)t[indi])->image  ])->chi2;
		
//	    cout << "i** "<< ((TrkTrack *)t[indi])->image << " " << nfiti <<" "<<chi2i<<endl;
	}
	new(ts[indo]) TrkTrack(*(TrkTrack*)t[indi]);
	m[indi] = 0;
	N--;	
	indo++;	
    }
    return sorted;
}
//--------------------------------------
//
//
//--------------------------------------
/**
 * Retrieves the is-th stored track. 
 * @param it Track number, ranging from 0 to ntrk().
 * Fitted tracks ( images included ) are stored in a TObjectArray ( TrkLevel2::Track ) in the same order they are returned by the F77 fitting routine. 
 */
TrkTrack *TrkLevel2::GetStoredTrack(int is){

    if(is >= this->ntrk()){
	cout << "** TrkLevel2 ** Track "<< is << "doen not exits! " << endl; 
	cout << "                Stored tracks ntrk() = "<< this->ntrk() << endl;
	return 0;
    }
    TClonesArray &t = *(Track);
    TrkTrack *track = (TrkTrack*)t[is];
    return track;
}
//--------------------------------------
//
//
//--------------------------------------
/**
 * Retrieves the it-th "physical" track, sorted by the method GetNTracks(). 
 * @param it Track number, ranging from 0 to GetNTracks().
 */
TrkTrack *TrkLevel2::GetTrack(int it){
    
    if(it >= this->GetNTracks()){
	cout << "** TrkLevel2 ** Track "<< it << "does not exits! " << endl; 
	cout << "                Physical tracks GetNTracks() = "<< this->ntrk() << endl;
	return 0;
    }
    TrkTrack *track = (TrkTrack*)(*(this->GetTracks()))[it];
	GetTracks()->Delete();////TEMPORANEO
    return track;
}
Int_t TrkLevel2::GetNTracks(){
	Int_t ntot=0;
	ntot = GetTracks()->GetEntries();
	GetTracks()->Delete();////TEMPORANEO
	return ntot;
};
//--------------------------------------
//
//
//--------------------------------------
/**
 * Retrieves (if present) the image of the it-th "physical" track, sorted by the method GetNTracks(). 
 * @param it Track number, ranging from 0 to GetNTracks().
 */
TrkTrack *TrkLevel2::GetTrackImage(int it){

    if(it >= this->GetNTracks()){
	cout << "** TrkLevel2 ** Track "<< it << "does not exits! " << endl; 
	cout << "                Physical tracks GetNTracks() = "<< this->ntrk() << endl;
	return 0;
    }
    TrkTrack *track = (TrkTrack*)(*(this->GetTracks()))[it];
    if(!track->HasImage()){
	cout << "** TrkLevel2 ** Track "<< it << "does not have image! " << endl; 
	return 0;
    }
    TrkTrack *image = (TrkTrack*)(*Track)[track->image];
    return image;
    
}
//--------------------------------------
//
//
//--------------------------------------
/**
 * Loads the magnetic field. 
 * @param s Path of the magnetic-field files.
 */
void TrkLevel2::LoadField(TString s){
    readb_(s.Data());
};
//--------------------------------------
//
//
//--------------------------------------
/**
 * Trajectory default constructor. 
 * (By default is created with z-coordinates inside the tracking volume)
  */
Trajectory::Trajectory(){
    npoint = 10;
    x = new float[npoint];
    y = new float[npoint];
    z = new float[npoint];
    thx = new float[npoint];
    thy = new float[npoint];
    tl = new float[npoint];
    float dz = ((ZTRKUP)-(ZTRKDW))/(npoint-1);
    for(int i=0; i<npoint; i++){
	x[i] = 0;
	y[i] = 0;
	z[i] = (ZTRKUP) - i*dz;
	thx[i] = 0;
	thy[i] = 0;
	tl[i] = 0;
    }
}
//--------------------------------------
//
//
//--------------------------------------
/**
 * Trajectory constructor.
 * (By default is created with z-coordinates inside the tracking volume)
 * \param n Number of points
 */
Trajectory::Trajectory(int n){
    if(n<=0){
	cout << "NB! Trajectory must have at least 1 point >>> created with 10 points" << endl;
	n=10;
    }
    npoint = n;
    x = new float[npoint];
    y = new float[npoint];
    z = new float[npoint];
    thx = new float[npoint];
    thy = new float[npoint];
    tl = new float[npoint];
    float dz = ((ZTRKUP)-(ZTRKDW))/(npoint-1);
    for(int i=0; i<npoint; i++){
    	x[i] = 0;
	y[i] = 0;
	z[i] = (ZTRKUP) - i*dz;
	thx[i] = 0;
	thy[i] = 0;
	tl[i] = 0;
    }
}
//--------------------------------------
//
//
//--------------------------------------
/**
 * Trajectory constructor.
 * \param n Number of points
 * \param pz Pointer to float array, defining z coordinates
 */
Trajectory::Trajectory(int n, float* zin){
    npoint = 10;
    if(n>0)npoint = n;
    x = new float[npoint];
    y = new float[npoint];
    z = new float[npoint];
    thx = new float[npoint];
    thy = new float[npoint];
    tl = new float[npoint];
    int i=0;
    do{
	x[i] = 0;
	y[i] = 0;
	z[i] = zin[i];
	thx[i] = 0;
	thy[i] = 0;
	tl[i] = 0;
	i++;		
    }while(zin[i-1] > zin[i] && i < npoint);
    npoint=i;
    if(npoint != n)cout << "NB! Trajectory created with "<<npoint<<" points"<<endl;
}
//--------------------------------------
//
//
//--------------------------------------
/**
 * Dump the trajectory coordinates.
 */
void Trajectory::Dump(){
    cout <<endl<< "Trajectory ========== "<<endl;
    for (int i=0; i<npoint; i++){
	cout << i <<" >> " << x[i] <<" "<< y[i] <<" "<< z[i] ;
	cout <<" -- " << thx[i] <<" "<< thy[i] ;
	cout <<" -- " << tl[i] << endl;
    };
}
//--------------------------------------
//
//
//--------------------------------------
/**
 * Get trajectory length between two points
 * @param ifirst first point (default 0)
 * @param ilast last point (default npoint)
 */
float Trajectory::GetLength(int ifirst, int ilast){
    if( ifirst<0    ) ifirst = 0;
    if( ilast>=npoint) ilast  = npoint-1;
    float l=0;
    for(int i=ifirst;i<=ilast;i++){
	l=l+tl[i]; 
    };
    if(z[ilast] > ZINI)l=l-tl[ilast];
    if(z[ifirst] < ZINI)   l=l-tl[ifirst];
    
    return l;

}
ClassImp(TrkLevel2);
ClassImp(TrkSinglet);
ClassImp(TrkTrack);
ClassImp(Trajectory);