/////////////////////////////////////////////////////////////
// CbmSttReco
//
// Class for track reconstruction for STT1
//
// authors: Pablo Genova - Pavia University
//          Lia Lavezzi  - Pavia University
//
/////////////////////////////////////////////////////////////
#include "TClonesArray.h"

#include "CbmRootManager.h"
#include "CbmSttReco.h"
#include "CbmSttDigi.h"
#include "CbmSttRecoTrk.h"
#include "TMatrixD.h"
#include "TVector3.h"
#include "TVector2.h"
#include "TObjArray.h"
#include "TClonesArray.h"

// -----   Default constructor   -------------------------------------------
CbmSttReco::CbmSttReco() :
  CbmTask("Ideal STT Reco Producer") { }
// -------------------------------------------------------------------------



// -----   Destructor   ----------------------------------------------------
CbmSttReco::~CbmSttReco() { }
// -------------------------------------------------------------------------



// -----   Public method Init   --------------------------------------------
InitStatus CbmSttReco::Init() {
 
  // Get RootManager
  CbmRootManager* ioman = CbmRootManager::Instance();
  if ( ! ioman ) {
    cout << "-E- CbmSttReco::Init: "
	 << "RootManager not instantiated!" << endl;
    return kFATAL;
  }
  
  // Get input array
  fDigiArray = (TClonesArray*) ioman->GetObject("SttDigi");
  if ( ! fDigiArray ) {
    cout << "-W- CbmSttReco::Init: "
	 << "No SttDigi array!" << endl;
    return kERROR;
  }

  // Create and register output array
  fRecoArray = new TClonesArray("CbmSttRecoTrk");
 
  ioman->Register("SttReco","Stt",fRecoArray,kTRUE);

  cout << "-I- CbmSttReco: Initialization successfull" << endl;

  return kSUCCESS;

}
// -------------------------------------------------------------------------
// -----   Public method Exec  ---------------------------------------------
void CbmSttReco::Exec(Option_t* opt) {

  // Reset output array
  if ( ! fRecoArray ) Fatal("Exec", "No RecoArray");
  
  fRecoArray->Clear();

  CbmSttDigi *digi = NULL;
  digi  = (CbmSttDigi*) fDigiArray->At(0); // first digi
  if(digi!= NULL) {
    fXYArray = new TObjArray(1000); // CHECK
    fTrkArray = new TArrayI(1000); // CHECK
    fLengthArray = new TArrayD(1000); // CHECK

    // make the array containing the trackID: 
    // ideal pattern recognition
    TArrayI *trks = MCTrkIndex(fDigiArray);
   
    // TO BE SET AS DEBUG
    // test di MCTrkIndex ---------
    //   for(Int_t o = 0; o<fDigiArray->GetEntries(); o++){
    //     cout << "evt: " << ((CbmSttDigi*) fDigiArray->At(o))->GetEventID() << " trk: " << ((CbmSttDigi*) fDigiArray->At(o))->GetTrackID() << endl;
    //   }
    //   for(Int_t u = 0; u < trks->GetSize() ; u++){
    //     if(u != 0 && trks->At(u)==0) break;
    //     cout << "index: " << trks->At(u) << endl;
    //   }
    //-----------------------------
    
    Int_t eventID = digi->GetEventID();

    // if(eventID%100 == 0) cout << "evt: " << eventID << endl;
         
    Int_t trackID;

    // loop over tracks
    for(Int_t u = 0; u < trks->GetSize() ; u++){

      if(trks->At(u) == -999) break;
        
      // fitstatus initialization: +1 every 
      // time a successful fit is completed
      fitstatus = 0;

      trackID = trks->At(u);
    
      // =====================================
      //  xy fitting procedure =======
      // =============================

      // putting into digitoarr TVector2's of the
      // centres of the unskewed hit tubes:
      // digitoarr points to fXYArray
      TObjArray *digitoarr = DigiToArray(fDigiArray, trackID);
    
      TVector3 xy;
    
      Int_t digitoarrsize = 0;
      if(digitoarr == NULL) {
	xy.SetXYZ(0.,0.,0.);
      }
      else {
	digitoarrsize = digitoarr->GetEntries();

	// TO BE SET AS DEBUG
	//	if(digitoarrsize > fDigiArray->GetEntries()) cout << "ATTENZIONE: digitoarr > fDigiArray" << endl;

	xy = Fit(digitoarr); // fitstatus = 1 if successful

      }
      fXYArray->Clear();
      TObjArray *intarr = NULL;
      if(xy.X() == 0 && xy.Y() == 0) {
	; // TO BE SET AS DEBUG
	// cout << "xy vuoto" << endl;
      }
      else {
	intarr = IntersectionFinder(xy, fDigiArray, trackID);
      }

      if(intarr == NULL) {
	; // TO BE SET AS DEBUG
	// cout << "NULL" << endl;
      }
      else { 
	// TO BE SET AS DEBUG
	// if(intarr->GetEntries() > digitoarrsize) cout << "ATTENZIONE: intarr > digitoarr" << endl;

	// if the prefit gives some result, 
	// but the refit fails, 
	// let' s keep the prefit response
	TVector3 xytry = Fit(intarr);  // fitstatus = 2 if successful
	if(xytry.X() != 0 && xytry.Y() != 0 && xytry.Z() != 0) xy = xytry;
      }

      // =====================================
      //  z fitting procedure =======
      // ============================
     
      // defining a TObjArray to be filled with 
      // TVector3's containing the x,y,z of the 
      // centres of the tubes. 
      // |->....  fZPointsArray
      fZPointsArray = new TObjArray(1000);
      fRightZPointsArray = new TObjArray(1000);
      TObjArray *ZArray = NULL;

      // function calculating the TObjArray, 
      // the z is determined through the geometrical intersection 
      // in x-y plane using the x-y fit already obtained. 
      // An auxiliary function for calculating the 
      // solutions of 2nd order equations is used
      // |->..... ZArray =ZedFinder(fDigiArray) 2ndSolve()
      ZArray = ZFinder(xy, fDigiArray, trackID); 

      // zed fit function (called twice to discard the wrong z's)
      // ptot Zfit(ZArray) returning transverse momentum total momentum z_0   
      TVector2 mp;
      if(ZArray != NULL) {
	mp = Zfit(xy,ZArray);  // fitstatus = 3 if successfull
      }
      else {
	mp.Set(0., 0.);
      }
      // =====================================

      // Momentum of the track
      Double_t ptot = Momentum(trackID,eventID,xy,mp, fitstatus);

      if(mp.X() != 0. && mp.Y() != 0.) AddRecoTrk(trackID, eventID, xy, mp, fitstatus, ptot); // CHECK if
  
      fXYArray->Delete();
      
    }
  }
}
// -------------------------------------------------------------------------

// -----   Private method AddRecoTrk   -------------------------------------
CbmSttRecoTrk* CbmSttReco::AddRecoTrk(Int_t trackID, Int_t eventID, TVector3 xy_par, TVector2 z_par, Int_t fitstatus, Double_t ptot){
  // see CbmSttRecoTrack for track description
  TClonesArray& clref = *fRecoArray;
  Int_t size = clref.GetEntriesFast();
  // cout << "-I- CbmSttReco: Adding Reco: event " << eventID << " track " << trackID << endl;
  return new(clref[size]) CbmSttRecoTrk(trackID,eventID,xy_par,z_par, fitstatus, ptot);
}
// -------------------------------------------------------------------------

// -----   Fit   --------------------------------------
TVector3 CbmSttReco::Fit(TObjArray * inputarray) {

  TVector2 *position = NULL;
  Int_t hitcounter = inputarray->GetEntriesFast();

  // cut on number of hits
  if(hitcounter > 25) return TVector3(0.,0.,0);
  if(hitcounter <=3 || hitcounter > 25) return TVector3(0.,0.,0);

  // error on xy
  Double_t sigxy= 0.1 ;// 150 micron (resolution)  // CHECK
  
  // FITTING IN X-Y PLANE:
  // v = a + bu + cu^2
  
  // translation and rotation -----------
  // translation near the first point
  Double_t s = 0.01;  // <--- CHECK!!!!
  Double_t trasl[2];

  TVector2 *positionfirst = (TVector2 *) inputarray->At(0); // first point
  trasl[0] = positionfirst->X() - s;
  trasl[1] = positionfirst->Y() - s;
  
  TVector2 *positionlast = (TVector2 *) inputarray->At(hitcounter-1); // last point
 
  // rotation
  Double_t alpha;
  position = (TVector2 *) inputarray->At(0); // first point
  alpha = TMath::ATan2(positionlast->Y() - positionfirst->Y(), positionlast->X() - positionfirst->X());

  TObjArray traslrot;
 
  Double_t Suu, Su, Sv, Suv, S1, Suuu, Suuv, Suuuu;
  Su = 0.;
  Sv = 0.;
  Suu = 0.;
  Suv = 0.;
  Suuu = 0.;
  S1 = 0.;
  Suuv = 0.;
  Suuuu = 0.;
  TObjArray uvarray(hitcounter);
  TArrayD sigv2array(hitcounter);
  Int_t digicounter = 0;

  for(Int_t j = 0; j < hitcounter; j++) {
    position = (TVector2 *) inputarray->At(j);
    
    // translation
    Double_t xtrasl, ytrasl;
    xtrasl = position->X() - trasl[0];
    ytrasl = position->Y() - trasl[1];
    // rotation 
    Double_t xrot, yrot;
    xrot = TMath::Cos(alpha)*xtrasl + TMath::Sin(alpha)*ytrasl;
    yrot = -TMath::Sin(alpha)*xtrasl + TMath::Cos(alpha)*ytrasl;
    
    // change coordinate
    TVector2 *uv = new TVector2(xrot / (xrot*xrot + yrot*yrot), yrot / (xrot*xrot + yrot*yrot));
    Double_t sigv2 = pow((sigxy/(xrot*xrot + yrot*yrot)),2);
    
    uvarray.AddAt(uv, digicounter);
    sigv2array.AddAt(sigv2, digicounter);
    digicounter++;
    
    Su = Su + (uv->X()/sigv2);
    Sv = Sv + (uv->Y()/sigv2);
    
    Suv = Suv + ((uv->X()*uv->Y())/sigv2);
    Suu = Suu + ((uv->X()*uv->X())/sigv2);
    
    Suuu = Suuu + ((uv->X()*uv->X()*uv->X())/sigv2);
    Suuv = Suuv + ((uv->X()*uv->X()*uv->Y())/sigv2);  
    
    Suuuu = Suuuu + ((uv->X()*uv->X()*uv->X()*uv->X())/sigv2);  
    
    S1 = S1 + 1/sigv2;
    
  }

  TMatrixD matrix(3,3);
  matrix[0][0] = S1;
  matrix[0][1] = Su;
  matrix[0][2] = Suu;
  
  matrix[1][0] = Su;
  matrix[1][1] = Suu;
  matrix[1][2] = Suuu;
  
  matrix[2][0] = Suu;
  matrix[2][1] = Suuu;
  matrix[2][2] = Suuuu;
  
  Double_t determ;
  
  determ = matrix.Determinant();
  if (determ != 0) {
    matrix.Invert();

  }
  else {
    return TVector3(0.,0.,0.);
  }

  TMatrixD column(3,1);
  column[0][0] = Sv;
  column[1][0] = Suv;
  column[2][0] = Suuv;

  TMatrixD column2(3,1);
  column2.Mult(matrix, column);

  Double_t a, b, c;
  a = column2[0][0];
  b = column2[1][0];
  c = column2[2][0];
 
  Double_t chi2 = 0.;
  
  for(Int_t i=0; i<digicounter; i++){
    TVector2 *uvrecall = (TVector2*) uvarray.At(i);
    chi2 = chi2 + pow(((uvrecall->Y() -  (a + b*uvrecall->X() + c*uvrecall->X()*uvrecall->X())) /sqrt(sigv2array.At(i))),2);
  }
  
  //------------------ with right errors
  Su = 0.;
  Sv = 0.;
  Suu = 0.;
  Suv = 0.;
  Suuu = 0.;
  S1 = 0.;
  Suuv = 0.;
  Suuuu = 0.;
  
  TArrayD sigE2array(digicounter);
  
  for(Int_t i=0; i<digicounter; i++){
    TVector2 *uvrecall = (TVector2*) uvarray.At(i);
    Double_t sigE2 = sigv2array.At(i) * (1 + pow((b + 2*c*uvrecall->X()),2));
    sigE2array.AddAt(sigE2, i);      
    
    Su = Su + (uvrecall->X()/sigE2);
    Sv = Sv + (uvrecall->Y()/sigE2);
    
    Suv = Suv + ((uvrecall->X()*uvrecall->Y())/sigE2);
    Suu = Suu + ((uvrecall->X()*uvrecall->X())/sigE2);
    
    Suuu = Suuu + ((uvrecall->X()*uvrecall->X()*uvrecall->X())/sigE2);
    Suuv = Suuv + ((uvrecall->X()*uvrecall->X()*uvrecall->Y())/sigE2);  
    
    Suuuu = Suuuu + ((uvrecall->X()*uvrecall->X()*uvrecall->X()*uvrecall->X())/sigE2);  
    
    S1 = S1 + 1/sigE2;
  }
  
  TMatrixD matrixb(3,3);
  matrixb[0][0] = S1;
  matrixb[0][1] = Su;
  matrixb[0][2] = Suu;
  
  matrixb[1][0] = Su;
  matrixb[1][1] = Suu;
  matrixb[1][2] = Suuu;
  
  matrixb[2][0] = Suu;
  matrixb[2][1] = Suuu;
  matrixb[2][2] = Suuuu;
  
  determ = matrixb.Determinant();
  if (determ != 0) {
    matrixb.Invert();
  }
  else {
    return TVector3(0.,0.,0.);
  }
  
  TMatrixD columnb(3,1);
  columnb[0][0] = Sv;
  columnb[1][0] = Suv;
  columnb[2][0] = Suuv;
  
  TMatrixD column2b(3,1);
  column2b.Mult(matrixb, columnb);
  
  a = column2b[0][0];
  b = column2b[1][0];
  c = column2b[2][0];
  
  //v = a + bu + cu^2
  chi2 = 1.;
  
  for(Int_t i=0; i<digicounter; i++) {
    TVector2 *uvrecall = (TVector2*) uvarray.At(i);
    chi2 = chi2 + pow(((uvrecall->Y() - (a + b*uvrecall->X() + c*uvrecall->X()*uvrecall->X())) /sqrt(fabs(sigE2array.At(i)))), 2);
  }
  
  // cut on chi2
  if((chi2/digicounter)>30.) return TVector3(0.,0.,0.);
  
  Double_t xcrot, ycrot, xc, yc, epsilon, R;
  ycrot = 1/(2*a);
  xcrot = -b/(2*a);
  epsilon = -c*pow((1+(b*b)), -3/2);
  R = epsilon + sqrt((xcrot*xcrot)+(ycrot*ycrot));
  

  //  // errors on parameters ------------------- CHECK
  //   // a, b, c
  
  //   Double_t Da[MAXNUMSTTHITS], Db[MAXNUMSTTHITS], Dc[MAXNUMSTTHITS];
  //   Double_t p1[MAXNUMSTTHITS], pu[MAXNUMSTTHITS], puu[MAXNUMSTTHITS];
  //   Double_t erra2, errb2, errc2;
  //   erra2 = 0.;
  //   errb2 = 0.;
  //   errc2 = 0.; 
  
  //   for(Int_t i=0; i<hitcounter; i++){
  //   p1[i] = 1/sigE2[i]; 
  //   pu[i] = u[i]*p1[i];
  //   puu[i] = u[i]*pu[i];
  
  //   Da[i] = matrixb[0][0] * p1[i] + matrixb[0][1] * pu[i] + matrixb[0][2] * puu[i];
  //   Db[i] = matrixb[1][0] * p1[i] + matrixb[1][1] * pu[i] + matrixb[1][2] * puu[i];
  //   Dc[i] = matrixb[2][0] * p1[i] + matrixb[2][1] * pu[i] + matrixb[2][2] * puu[i];
  
  //   erra2 = erra2 + (Da[i]*Da[i]*sigE2[i]);
  //   errb2 = errb2 + (Db[i]*Db[i]*sigE2[i]);
  //   errc2 = errc2 + (Dc[i]*Dc[i]*sigE2[i]);
  //   }
  
  //   //  std::cout << "**************\n";
  //   //   std::cout << "erra = " <<  sqrt(erra2) << "\n";
  //   //   std::cout << "errb = " <<  sqrt(errb2)<< "\n";
  //   //   std::cout << "errc = " <<  sqrt(errc2)<< "\n";
  
  //   // errors on xc, yc, R
  //   Double_t errxcrot2, errycrot2, errR2;
  //   errxcrot2 = (1./(4*pow(a,4))) * (b*b*erra2 + a*a*errb2 - 2*a*b*sqrt(erra2*errb2)) ;
  //   errycrot2 = (1./4)*(erra2/pow(a,4)) ;
  //   errR2 = (1./(R*R)) * (ycrot*ycrot*errxcrot2 + xcrot*xcrot*errycrot2 + 2*xcrot*ycrot*sqrt(errxcrot2*errycrot2));
  
  // re-rotation and re-traslation of xc and yc
  // rotation    
  xc = TMath::Cos(alpha)*xcrot - TMath::Sin(alpha)*ycrot;
  yc = TMath::Sin(alpha)*xcrot + TMath::Cos(alpha)*ycrot;
  // translation
  xc = xc + trasl[0];
  yc = yc + trasl[1];
  
  // TO BE SET AS DEBUG
  //   std::cout << "*************\n";
  //   std::cout << "circle center&radius:\n";
  //   std::cout << "xc = " <<  xc << "\n";
  //   std::cout << "yc = " <<  yc << "\n";
  //   std::cout << "R = " <<  R << "\n";
  //========================================================

  TVector3 out(xc,yc,R);
  fitstatus++;
  return out;
}

// ----- Curvature ---------------------------------------------------------
Double_t CbmSttReco::Curvature(Double_t radius){
  // calculation of the curvature from the helix fit
  if(radius != 0) return 1./radius;
  else return 999;
}
// -------------------------------------------------------------------------

// -------------- IntersectionFinder  --------------------------------------
TObjArray * CbmSttReco::IntersectionFinder(TVector3 vec, TClonesArray * DigiArray, Int_t trkID){

  // calculation of the curvature from the helix prefit
  if(vec.Z() == 0) return (TObjArray *) NULL;

  //==========
  // POINT ----------------------------------------------------
  // 1. find the cooordinates of the point fired wire of the track
  TVector2 point; // point
  Double_t rsim;
  Double_t rtrue;
  //  cout << "Int Finder" << endl;
  Int_t counter = 0;
  // loop over input points
  for(Int_t k = 0; k < DigiArray->GetEntries(); k++){
    CbmSttDigi *digit = (CbmSttDigi*) DigiArray->At(k); 
    TString nam = digit->getNam();
    if(nam.Contains("stt02") || nam.Contains("stt04") || nam.Contains("stt06") || nam.Contains("stt08") || nam.Contains("stt10")) continue;
    if(digit->GetTrackID() != trkID) continue;

    // [xp, yp] point = coordinates xy of the centre of the firing tube
    point.Set(digit->getcenter()->X(), digit->getcenter()->Y());
    rsim = digit->rSim();
    rtrue = digit->rTrue();
  
    // SetRMode(0) --> reconstruction with simulated radius (TStraw)
    // SetRMode(1) --> reconstruction with true radius
    if(frmode==kTRUE) rsim = rtrue; 
    
    // the coordinates of the point are taken from the intersection
    // between the circumference from the drift time and the R radius of
    // curvature. -------------------------------------------------------
    // 2. find the intersection between the little circle and the line // R
    TVector2 first;
    TVector2 second;
    // 2.a
    // find the line passing throught [xc, yc] (centre of curvature) and [xp, yp] (first wire)
    // y = mx + q
    Double_t m = (point.Y() - vec.Y())/(point.X() - vec.X());
    Double_t q = point.Y() - m*point.X();
    
    // cut on rsim CHECK
    // if the simulated radius is too small, the digit
    // is not used for the fit 
    if(frmode == kFALSE && rsim < 0.1) continue;

    // 2.b
    // intersection little circle and line --> [x1, y1]
    // + and - refer to the 2 possible intersections
    // +
    Double_t x1 = (-(m*(q - point.Y()) - point.X()) + sqrt((m*(q - point.Y()) - point.X())*(m*(q - point.Y()) - point.X()) - (m*m + 1)*((q - point.Y())*(q - point.Y()) + point.X()*point.X() - rsim*rsim))) / (m*m + 1);
    Double_t y1 = m*x1 + q;
    first.Set(x1, y1);
    // - 
    Double_t x2 = (-(m*(q - point.Y()) - point.X()) - sqrt((m*(q - point.Y()) - point.X())*(m*(q - point.Y()) - point.X()) - (m*m + 1)*((q - point.Y())*(q - point.Y()) + point.X()*point.X() - rsim*rsim))) / (m*m + 1);
    Double_t y2 = m*x2 + q;
    second.Set(x2, y2);
    
    // 2.c intersection between line and circle
    // +
    Double_t xb1 = (-(m*(q - vec.Y()) - vec.X()) + sqrt((m*(q - vec.Y()) - vec.X())*(m*(q - vec.Y()) - vec.X()) - (m*m + 1)*((q - vec.Y())*(q - vec.Y()) + vec.X()*vec.X() - vec.Z()*vec.Z()))) / (m*m + 1);
    Double_t yb1 = m*xb1 + q;
    // -
    Double_t xb2 = (-(m*(q - vec.Y()) - vec.X()) - sqrt((m*(q - vec.Y()) - vec.X())*(m*(q - vec.Y()) - vec.X()) - (m*m + 1)*((q - vec.Y())*(q - vec.Y()) + vec.X()*vec.X() - vec.Z()*vec.Z()))) / (m*m + 1);
    Double_t yb2 = m*xb2 + q;
    
    // calculation of the distance between [xb, yb] and [xp, yp]
    Double_t distb1 = sqrt((yb1 - point.Y())*(yb1 - point.Y()) + (xb1 - point.X())*(xb1 - point.X()));
    Double_t distb2 = sqrt((yb2 - point.Y())*(yb2 - point.Y()) + (xb2 - point.X())*(xb2 - point.X()));
    
    // choice of [xb, yb]
    TVector2 xyb;
    if(distb1 > distb2) xyb.Set(xb2, yb2); 
    else xyb.Set(xb1, yb1); 
    
    // calculation of the distance between [x, y] and [xb. yb]
    Double_t dist1 = sqrt((xyb.Y() - y1)*(xyb.Y() - y1) + (xyb.X() - x1)*(xyb.X() - x1));
    Double_t dist2 = sqrt((xyb.Y() - y2)*(xyb.Y() - y2) + (xyb.X() - x2)*(xyb.X() - x2));
    
    // choice of [x, y]
    TVector2 *xy;
    if(dist1 > dist2) xy = new TVector2(x2, y2);
    else xy = new TVector2(x1, y1);   // <========= THIS IS THE NEW POINT to be used for the fit

    // SET AS DEBUG
    //    if (TMath::Sqrt((xy->X() - point.X())*(xy->X() - point.X()) + (xy->Y() - point.Y())*(xy->Y() - point.Y())) - rsim > 0.000001) { 
    //      cout << "ATTENZIONE: " << "differenza = " << TMath::Sqrt((xy->X() - point.X())*(xy->X() - point.X()) + (xy->Y() - point.Y())*(xy->Y() - point.Y())) - rsim << endl;
    //    }

    fXYArray->Add(xy);
    counter++;
  }

  if(counter==0) return (TObjArray *) NULL;
  else return fXYArray;
}

//------- DigiToArray  -------------------------------
// copies the fDigiArray to a TObjArray
TObjArray * CbmSttReco::DigiToArray(TClonesArray * DigiArray, Int_t trkID){
  Int_t counter = 0;
  for(Int_t k = 0; k < DigiArray->GetEntries(); k++){
    CbmSttDigi *digit = (CbmSttDigi*) DigiArray->At(k); 
    TString nam = digit->getNam();
    if(nam.Contains("stt02") || nam.Contains("stt04") || nam.Contains("stt06") || nam.Contains("stt08") || nam.Contains("stt10")) continue;
    if(digit->GetTrackID() != trkID) continue;

    TVector2 *cen;
    cen = new TVector2(digit->getcenter()->X(), digit->getcenter()->Y());
    fXYArray->Add(cen);
    counter++;
  }
  
  if(counter==0) return (TObjArray *) NULL;
  else  return fXYArray;
}
// -----------------------------------------------------------

// ------- MCTrkIndex ----------------------------------------
// creates an array of integers containing the trackIDs
TArrayI * CbmSttReco::MCTrkIndex(TClonesArray * DigiArray){
  Int_t trkref = -999;
  Int_t trkid = -999;
  Int_t counter = 0;
  for(Int_t y = 0; y < DigiArray->GetEntries(); y++ ){
    if(y == 0) trkref = ((CbmSttDigi *) DigiArray->At(y))->GetTrackID();
    trkid = ((CbmSttDigi *) DigiArray->At(y))->GetTrackID();
    if(trkid != trkref || y == (DigiArray->GetEntries() - 1)) {
      fTrkArray->AddAt(trkref, counter);
      trkref = trkid;
      counter++;
    }
    if(y == (DigiArray->GetEntries() - 1)) {
      fTrkArray->AddAt(-999, counter);
    }
  }
  return fTrkArray;
}
// -------------------------------------------------------------

// -------- SetRMode -------------------------------------------
// rmode = 0 --> rsim is used
// rmode = 1 --> rtrue is used
void CbmSttReco::SetRMode(Bool_t rmode){
  frmode = rmode;
}
// -------------------------------------------------------------

// -------- GetRMode -------------------------------------------
// rmode = 0 --> rsim is used
// rmode = 1 --> rtrue is used
Bool_t CbmSttReco::GetRMode(){
  return frmode;
}
// -------------------------------------------------------------

// -------- ZFinder --------------------------------------------
TObjArray * CbmSttReco::ZFinder(TVector3 xy, TClonesArray * DigiArray, Int_t trackID) {

  if(DigiArray == NULL || (xy.X() == 0 && xy.Y() == 0 && xy.Z() == 0)) return (TObjArray *) NULL;

  Int_t counter = 0;

  for(Int_t k = 0; k < DigiArray->GetEntries(); k++) {
    CbmSttDigi *digit = (CbmSttDigi*) DigiArray->At(k); 

    if(digit->GetTrackID() != trackID) continue; // CHECK

    TString nam = digit->getNam();
    
    Double_t x_0= xy.X();
    Double_t y_0= xy.Y();
    Double_t R  = xy.Z();//radius
    Double_t x_1= digit->getmin()->X();//two extremities of the tube central line
    Double_t y_1= digit->getmin()->Y();//remember ->getcenter() gives the coordinates  
    Double_t z_1= digit->getmin()->Z();//of the geometrical centre of the tube
    
    Double_t x_2= digit->getmax()->X();
    Double_t y_2= digit->getmax()->Y();
    Double_t z_2= digit->getmax()->Z();

   
    Double_t rcur = (frmode==kTRUE)? digit->rTrue():digit->rSim();
    
    Double_t x1 = -9999.;
    Double_t y1 = -9999.;
    Double_t x2 = -9999.;
    Double_t y2 = -9999.;

    if(nam.Contains("stt02") || nam.Contains("stt04") || nam.Contains("stt06") || nam.Contains("stt08") || nam.Contains("stt10")) {
      
      Double_t a = -999;
      Double_t b = -999;
      
      // intersection point between the reconstructed 
      // circumference and the line joining the centres
      // of the reconstructed circle and the i_th drift circle
      if(fabs(x_2-x_1)>0.0001) {
	a =(y_2-y_1)/(x_2-x_1);
	b =(y_1-a*x_1);
	Double_t A = a*a+1;
	Double_t B = x_0+a*y_0-a*b;
	Double_t C = x_0*x_0+y_0*y_0+b*b-R*R-2*y_0*b;
	if((B*B-A*C)>0) {
	  x1= (B+TMath::Sqrt(B*B-A*C))/A;
	  x2= (B-TMath::Sqrt(B*B-A*C))/A;
	  y1=a*x1+b;
	  y2=a*x2+b;
	}
      }
      else if(fabs(y_2-y_1)>0.0001) {
	Double_t A = 1;
	Double_t B = y_0;
	Double_t C = y_0*y_0 +(x_1-x_0)*(x_1-x_0) -R*R;

	if((B*B-A*C)>0) {
	  y1= (B+TMath::Sqrt(B*B-A*C))/A;
	  y2= (B-TMath::Sqrt(B*B-A*C))/A;
	  x1=x2=x_1;
	}
	
      }

      //x1 and x2 are the 2 intersection points
      Double_t d1=TMath::Sqrt((x1-digit->getcenter()->X())*(x1-digit->getcenter()->X())+
			     (y1-digit->getcenter()->Y())*(y1-digit->getcenter()->Y()));
      Double_t d2=TMath::Sqrt((x2-digit->getcenter()->X())*(x2-digit->getcenter()->X())+
			     (y2-digit->getcenter()->Y())*(y2-digit->getcenter()->Y()));

      Double_t x_ = x1;
      Double_t y_ = y1;
     
      // the intersection point nearest to the drift circle's centre is taken
      if(d2<d1) {x_=x2;y_=y2;}    

      // now we need to find the actual centre of the drift circle,
      // by translating the drift circle until it becomes tangent
      // to the reconstructed circle. 
      // Two solutions are possible (left rigth abiguity), 
      // they are both kept, only the following zed fit will discard the wrong ones.
      // Using the parametric equation of the 3d-straigth line and taking the
      // x points just obtained, the zed coordinate of the skewed tube centre is calculated.
      
      //solving the equation to find out the centre of the tangent circle
      Double_t A = a*a+1;
      Double_t B = -(a*b-a*y_-x_);
      Double_t C = x_*x_+ y_*y_+b*b-2*b*y_-rcur*rcur;
      if((B*B-A*C)>0) {
	x1= (B+TMath::Sqrt(B*B-A*C))/A;
	x2= (B-TMath::Sqrt(B*B-A*C))/A;
	y1=a*x1+b;
	y2=a*x2+b;
      }
      
      d1=TMath::Sqrt((x1-digit->getcenter()->X())*(x1-digit->getcenter()->X())+(y1-digit->getcenter()->Y())*(y1-digit->getcenter()->Y()));
      d2=TMath::Sqrt((x2-digit->getcenter()->X())*(x2-digit->getcenter()->X())+(y2-digit->getcenter()->Y())*(y2-digit->getcenter()->Y()));
      
      Double_t xcen0=x1;
      Double_t xcen1=x2;
      Double_t ycen0=y1;
      Double_t ycen1=y2;
 
      if(d2<d1) {//z2 contains the points nearest (in x-y) to the initial centre of the skewed tube
	xcen0=x2;
	xcen1=x1;
	ycen0=y2;
	ycen1=y1;
	
      }
      
      //zed association
      if(fabs(x_2-x_1)<0.001) return (TObjArray*) NULL;
      Double_t t_      =(xcen0-x_1)/(x_2-x_1); // x= a_x*t + x_1 [t=1 x=x_2]
      Double_t z_      =(z_2-z_1)*t_ +z_1;     // z= a_z*t + z_1 [t=1 z=z_2]

      Double_t t_bis   =(xcen1-x_1)/(x_2-x_1); //from x_'s (the 2 solutions of the 2nd order equation)
      Double_t z_bis   =(z_2-z_1)*t_bis +z_1;  //and the 2 parametric equations the z coord. are obtained 

      TVector3 *tofit = new TVector3(xcen0,ycen0,z_);
      fZPointsArray->Add(tofit);
      counter++;
      TVector3 *tofit2 = new TVector3(xcen1,ycen1,z_bis);
      fZPointsArray->Add(tofit2);
      counter++;
    }  
  }
  
  if(counter==0) return (TObjArray *) NULL;
  //  return fZPointsArray;

  // call ZFit to solve ambiguities --> fRightZPointsArray -----
  TVector2 dipz0 = Zfit(xy, fZPointsArray); 

  if(dipz0.X() != 0 && dipz0.Y() != 0)  fitstatus--;
  TArrayD *trklcosl = SCosL(xy, fZPointsArray);

  for(Int_t i=0; i < fZPointsArray->GetEntries(); i++) {
    TVector3 * vi= (TVector3 *) fZPointsArray->At(i);
    Double_t sigz = 1.; // CHECK
    if(pow(((vi->Z() - (dipz0.Y() + dipz0.X() * trklcosl->At(i)))/sigz), 2) > 10) continue;
    TVector3 *tofitright = new TVector3(vi->X(), vi->Y(), vi->Z());
    fRightZPointsArray->Add(tofitright);
  }
  // ------------------------------------------------------------

  return fRightZPointsArray;

}
// ----------------------------------------------------

// ----- Zfit  ----------------------------------------
TVector2 CbmSttReco::Zfit(TVector3 xy, TObjArray * ZArray) {//z fit 

  // CHECK: ATTENTION
  // refit for error correction missing
  
  if(ZArray==NULL || (xy.X() == 0 && xy.Y() == 0 && xy.Z() == 0)) return TVector2(0.,0.);
  if(ZArray->GetEntries() <= 1) return TVector2(0.,0.);
  
  TArrayD *scosl = SCosL(xy, ZArray);
  
  Int_t hitcounter = ZArray->GetEntries();
  if(hitcounter == 0) return TVector2(0.,0.);
  
  Int_t zcounter = 0;
  Double_t Sxx, Sx, Sz, Sxz, S1z;
  Double_t Detz;
  Double_t fitm, fitp;
  Double_t sigz = 1.;  // CHECK
  
  Sx = 0.;
  Sz = 0.;
  Sxx = 0.;
  Sxz = 0.;
  S1z = 0.;
  
  for(Int_t i=0; i < hitcounter; i++) { 
    
    TVector3 * vi= (TVector3 *) ZArray->At(i);
    
      Sx = Sx + (scosl->At(i)/(sigz * sigz));
      Sz = Sz + (vi->Z()/(sigz * sigz));
      Sxz = Sxz + ((scosl->At(i)*vi->Z())/(sigz * sigz));
      Sxx = Sxx + ((scosl->At(i)*scosl->At(i))/(sigz * sigz));
      S1z = S1z + 1/(sigz * sigz);
    }
    
    Detz = S1z*Sxx - Sx*Sx;
    if(Detz == 0) {
      return TVector2(0.,0.);
    }
    fitp = (1/Detz)*(Sxx*Sz - Sx*Sxz);
    fitm = (1/Detz)*(S1z*Sxz - Sx*Sz);
    //   fiterrp2 = (1/Detz)*Sxx;
    //   fiterrm2 = (1/Detz)*S1z;
    
    Double_t chi2z;
    chi2z = 0.;
    for(Int_t i=0; i < zcounter; i++) {
      TVector3 * vi= (TVector3 *) ZArray->At(i);
      chi2z = chi2z + pow(((vi->Z()-(fitp+fitm*scosl->At(i)))/sigz), 2);
      // RESIDUALS
      //    if(pow(((vi->Z()-(fitp+fitm*scosl[i]))/sigz), 2)>10) continue;
      //   bestz[ctbest]=vi->Z();
      //   bests[ctbest]=scosl[i];
      //   ctbest++;
    }
    
    // y = m*x + p
    TVector2 outz(fitm, fitp);
    fitstatus++;
    return outz;
    //   fiterrp = sqrt(fiterrp2);
    //   fiterrm = sqrt(fiterrm2);
  }
// -------------------------------------------------------------------------------

// ----------- SCosL() -----------------------------------------------------------
// arc length calculation
TArrayD * CbmSttReco::SCosL(TVector3 xy, TObjArray * ZArray) {

  if(ZArray == NULL || (xy.X() == 0 && xy.Y() == 0 && xy.Z() == 0)) return (TArrayD *) NULL;

  // parameters from xy fit
  Double_t xc = xy.X();
  Double_t yc = xy.Y();
  Double_t R  = xy.Z();

  // phi0
  TVector3 * v0 = (TVector3 *) ZArray->At(0);
  Double_t Phi0 = TMath::ATan2((v0->Y() - yc),(v0->X() - xc));

  // CHECK
  // we are using the first digi for the arc length calculation
  // but this may be wrong
  Double_t x0 = v0->X();
  Double_t y0 = v0->Y();

  // track length*cos(lambda) ----------
  // calculation
  Int_t zcounter = 0;
  for(Int_t i=0; i < ZArray->GetEntries(); i++) {
    TVector3 * vi= (TVector3 *) ZArray->At(i);
    Double_t scos = R*TMath::ATan2((vi->Y() - y0)*TMath::Cos(Phi0) - (vi->X() - x0)*TMath::Sin(Phi0) , R + (vi->X() - x0) * TMath::Cos(Phi0) + (vi->Y()-y0) * TMath::Sin(Phi0));
  fLengthArray->AddAt(scos, zcounter);
  zcounter++;
  }
  // -----------------------------------
  
  return fLengthArray;
}
// ------------------------------------------------------------------------

// ------ Momentum --------------------------------------------------------
Double_t CbmSttReco::Momentum(Int_t trackID, Int_t eventID, TVector3 xy, TVector2 mp, Int_t fitstatus) {

  // pt = 0.3 * R * B * 0.01 --> GeV/c
  Double_t pt = 0.3 * 2. * 0.01 * xy.Z();

  // pl = pt * tg(lambda) = pt * fitm
  Double_t pl = pt * mp.X(); 

  // ptot = sqrt(pt^2 + pl^2)
  Double_t momentum = TMath::Sqrt(pt*pt + pl*pl);
  return momentum;
}

ClassImp(CbmSttReco)