--- DarthVader/TrackerLevel2/src/TrkLevel2.cpp	2006/10/24 07:28:38	1.12
+++ DarthVader/TrackerLevel2/src/TrkLevel2.cpp	2006/11/21 14:00:40	1.20
@@ -4,6 +4,7 @@
  */
 #include <TrkLevel2.h>
 #include <iostream>
+#include <math.h>
 using namespace std;
 //......................................
 // F77 routines
@@ -11,7 +12,10 @@
 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*);
+//    int  readb_(const char*);
+    int  readb_();
+    void mini2_(int*,int*,int*); 
+    void guess_();
 }
 //--------------------------------------
 //
@@ -21,10 +25,10 @@
     seqno = -1; 
     image = -1; 
     chi2  = 0;
-	nstep = 0;
-	for(int it1=0;it1<5;it1++){
-		al[it1] = 0;
-		for(int it2=0;it2<5;it2++)coval[it1][it2] = 0;
+    nstep = 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;
@@ -55,10 +59,10 @@
     seqno = t.seqno; 
     image = t.image; 
     chi2  = t.chi2;
-	nstep = t.nstep;
-	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];
+    nstep = t.nstep;
+    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];
@@ -196,19 +200,204 @@
 //--------------------------------------
 void TrkTrack::Dump(){
     cout << endl << "========== Track " ;
-	cout << endl << "seq.  n. : "<< seqno;
-	cout << endl << "image n. : "<< image;
-	cout << endl << "al       : "; for(int i=0; i<5; i++)cout << al[i] << " ";
+    cout << endl << "seq.  n. : "<< seqno;
+    cout << endl << "image n. : "<< image;
+    cout << endl << "al       : "; for(int i=0; i<5; i++)cout << al[i] << " ";
     cout << endl << "chi^2    : "<< chi2;
-	cout << endl << "n.step   : "<< nstep;
-	cout << endl << "xgood    : "; for(int i=0; i<6; i++)cout << xgood[i] ;
+    cout << endl << "n.step   : "<< nstep;
+    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 << "xv       : "; for(int i=0; i<6; i++)cout << xv[i] << " ";
+    cout << endl << "yv       : "; for(int i=0; i<6; i++)cout << yv[i] << " ";
+    cout << endl << "zv       : "; for(int i=0; i<6; i++)cout << zv[i] << " ";
+    cout << endl << "resx     : "; for(int i=0; i<6; i++)cout << resx[i] << " ";
+    cout << endl << "resy     : "; for(int i=0; i<6; i++)cout << resy[i] << " ";
+    cout << endl << "coval    : "; for(int i=0; i<5; i++)cout << coval[0][i]<<" ";
+    cout << endl << "           "; for(int i=0; i<5; i++)cout << coval[1][i]<<" ";
+    cout << endl << "           "; for(int i=0; i<5; i++)cout << coval[2][i]<<" ";
+    cout << endl << "           "; for(int i=0; i<5; i++)cout << coval[3][i]<<" ";
+    cout << endl << "           "; for(int i=0; i<5; i++)cout << coval[4][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] << " ";
+    cout << endl;
+}
+/**
+ * Set the TrkTrack position measurements 
+ */
+void TrkTrack::SetMeasure(double *xmeas, double *ymeas, double *zmeas){
+    for(int i=0; i<6; i++) xm[i]=*xmeas++;
+    for(int i=0; i<6; i++) ym[i]=*ymeas++;
+    for(int i=0; i<6; i++) zm[i]=*zmeas++;
+}
+/**
+ * Set the TrkTrack position resolution 
+ */
+void TrkTrack::SetResolution(double *rx, double *ry){
+    for(int i=0; i<6; i++) resx[i]=*rx++;
+    for(int i=0; i<6; i++) resy[i]=*ry++;
+}
+/**
+ * Set the TrkTrack good measurement 
+ */
+void TrkTrack::SetGood(int *xg, int *yg){
+    for(int i=0; i<6; i++) xgood[i]=*xg++;
+    for(int i=0; i<6; i++) ygood[i]=*yg++;
+}
+
+/**
+ * Load the magnetic field
+ */
+void TrkTrack::LoadField(TString path){
+    
+    strcpy(path_.path,path.Data());
+    path_.pathlen = path.Length();
+    path_.error   = 0;
+    readb_();
+
+};
+
+/**
+ * Method to fill minimization-routine common
+ */
+void TrkTrack::FillMiniStruct(cMini2track& track){
+
+    for(int i=0; i<6; i++){ 
+
+	track.xgood[i]=xgood[i];
+	track.ygood[i]=ygood[i];
+	
+	track.xm[i]=xm[i];
+	track.ym[i]=ym[i];
+	track.zm[i]=zm[i];
+	
+//      --- temporaneo ----------------------------
+//      andrebbe inserita la dimensione del sensore
+	float segment = 100.;
+	track.xm_a[i]=xm[i];
+	track.xm_b[i]=xm[i];
+	track.ym_a[i]=ym[i];
+	track.ym_b[i]=ym[i];
+	if(       xgood[i] && !ygood[i] ){
+	    track.ym_a[i] = track.ym_a[i]+segment;
+	    track.ym_b[i] = track.ym_b[i]-segment;
+	}else if( !xgood[i] && ygood[i]){
+	    track.xm_a[i] = track.xm_a[i]+segment;
+	    track.xm_b[i] = track.xm_b[i]-segment;
+	}
+//      --- temporaneo ----------------------------
+	
+	track.resx[i]=resx[i];
+	track.resy[i]=resy[i];
+    }
+
+    for(int i=0; i<5; i++) track.al[i]=al[i]; 
+    track.zini = 23.5; 
+// ZINI = 23.5 !!! it should be the same parameter in all codes
+    
+}
+/**
+ * Method to set values from  minimization-routine common
+ */
+void TrkTrack::SetFromMiniStruct(cMini2track *track){
+
+    for(int i=0; i<5; i++) {
+	al[i]=track->al[i];
+	for(int j=0; j<5; j++) coval[i][j]=track->cov[i][j];
+    }
+    chi2  = track->chi2;
+    nstep = track->nstep;
+    for(int i=0; i<6; i++){ 
+	xv[i]  = track->xv[i];
+	yv[i]  = track->yv[i];
+	zv[i]  = track->zv[i];
+	xm[i]  = track->xm[i];
+	ym[i]  = track->ym[i];
+	zm[i]  = track->zm[i];
+	axv[i] = track->axv[i];
+	ayv[i] = track->ayv[i];	
+    }
+
+    
 }
+/**
+ * Tracking method. It calls F77 mini routine.
+ */
+void TrkTrack::Fit(double pfixed, int& fail, int iprint){
+
+    float al_ini[] = {0.,0.,0.,0.,0.};
+
+    extern cMini2track track_;
+    fail = 0;
+    FillMiniStruct(track_);
+
+    // if fit variables have been reset, evaluate the initial guess
+    if(al[0]==-9999.&&al[1]==-9999.&&al[2]==-9999.&&al[3]==-9999.&&al[4]==-9999.)guess_();
+
+    // --------------------- free momentum 
+    if(pfixed==0.) {
+	track_.pfixed=0.; 
+    }
+    // --------------------- fixed momentum 
+    if(pfixed!=0.) {
+	al[4]=1./pfixed;     
+	track_.pfixed=pfixed;
+    }
+
+    //  store temporarily the initial guess
+    for(int i=0; i<5; i++) al_ini[i]=track_.al[i]; 
+
+    //  ------------------------------------------
+    //  call mini routine
+    int istep=0;
+    int ifail=0;
+    mini2_(&istep,&ifail, &iprint);
+    if(ifail!=0) {
+	if(iprint)cout << "ERROR: ifail= " << ifail << endl;
+	fail = 1;
+    }
+    //  ------------------------------------------
+    
+    SetFromMiniStruct(&track_);
+//    cout << endl << "eta ===> " << track_.al[4] << endl;
+
+//     for(int i=0; i<5; i++) al[i]=track_.al[i];
+//     chi2=track_.chi2;
+//     nstep=track_.nstep;
+//     for(int i=0; i<6; i++) xv[i]=track_.xv[i];
+//     for(int i=0; i<6; i++) yv[i]=track_.yv[i];
+//     for(int i=0; i<6; i++) zv[i]=track_.zv[i];
+//     for(int i=0; i<6; i++) axv[i]=track_.axv[i];
+//     for(int i=0; i<6; i++) ayv[i]=track_.ayv[i];
+//     for(int i=0; i<5; i++) {
+// 	for(int j=0; j<5; j++) coval[i][j]=track_.cov[i][j];
+//     }
+
+    if(fail){
+ 	if(iprint)cout << " >>>> fit failed >>>> drawing initial par"<<endl;
+ 	for(int i=0; i<5; i++) al[i]=al_ini[i]; 
+    }
+
+};
+/*
+ * Reset the fit parameters 
+ */
+void TrkTrack::FitReset(){
+    for(int i=0; i<5; i++) al[i]=-9999.;
+    chi2=0.;
+    nstep=0;
+    for(int i=0; i<6; i++) xv[i]=0.;
+    for(int i=0; i<6; i++) yv[i]=0.;
+    for(int i=0; i<6; i++) zv[i]=0.;
+    for(int i=0; i<6; i++) axv[i]=0.;
+    for(int i=0; i<6; i++) ayv[i]=0.;
+    for(int i=0; i<5; i++) {
+	for(int j=0; j<5; j++) coval[i][j]=0.;
+    }
+}
+
 //--------------------------------------
 //
 //
@@ -777,8 +966,13 @@
  * Loads the magnetic field. 
  * @param s Path of the magnetic-field files.
  */
-void TrkLevel2::LoadField(TString s){
-    readb_(s.Data());
+void TrkLevel2::LoadField(TString path){
+//
+    strcpy(path_.path,path.Data());
+    path_.pathlen = path.Length();
+    path_.error   = 0;
+    readb_();
+//
 };
 //--------------------------------------
 //
@@ -925,6 +1119,40 @@
 
 }
 
+/**
+ * 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 Trajectory::DoTrack2(float* al){
+
+    double *dxout   = new double[npoint];
+    double *dyout   = new double[npoint];
+    double *dthxout = new double[npoint];
+    double *dthyout = new double[npoint];
+    double *dtlout  = new double[npoint];
+    double *dzin    = new double[npoint];
+    double dal[5];
+
+    int ifail = 0;
+
+    for (int i=0; i<5; i++)      dal[i]  = (double)al[i];
+    for (int i=0; i<npoint; i++) dzin[i] = (double)z[i];
+
+    dotrack2_(&(npoint),dzin,dxout,dyout,dthxout,dthyout,dtlout,dal,&ifail);
+    
+    for (int i=0; i<npoint; i++){
+	x[i]   = (float)*dxout++;
+	y[i]   = (float)*dyout++;
+	thx[i] = (float)*dthxout++;
+	thy[i] = (float)*dthyout++;
+	tl[i]  = (float)*dtlout++;
+    }
+
+    return ifail;
+};
 
 ClassImp(TrkLevel2);
 ClassImp(TrkSinglet);