//
#include <string>
#include <sstream>
 
#include "CbmSttHelixTrackFitter.h"
#include "CbmSttTrackMatch.h"
#include "CbmRootManager.h"
#include "CbmTask.h"
#include "CbmSttHoughDefines.h"

#include "TArc.h"
#include "TH2.h"
#include "TClonesArray.h"
// #include "TH3.h"
// #include "TLine.h"
#include "TMatrixD.h" // stt1
#include "TMarker.h"
#include "TLine.h"
#include "TPolyLine.h"
#include "TMinuit.h"
#include "CbmSttGeomPoint.h"

// #define rootoutput kFALSE

Int_t h = 0; // negative charge = 1; positive charge = -1
TH2F *h3, *h4;
TCanvas *eventCanvas3;
TCanvas *eventCanvas4, *eventCanvas5;
TCanvas *hougCanvas;
TH2F *houg; 
Double_t vote[201][1001];
TArrayD marray(100);

CbmSttHelixTrackFitter::CbmSttHelixTrackFitter()
{
  fEventCounter = 0;
  //  fHotArray = new TClonesArray("CbmSttHOT");
}

CbmSttHelixTrackFitter::CbmSttHelixTrackFitter(Int_t verbose)
{
  fVerbose = verbose;
  if (verbose < 3)
    rootoutput = kFALSE;
  else
    rootoutput = kTRUE;
  fEventCounter = 0; 
  // fHotArray = new TClonesArray("CbmSttHOT");
}

CbmSttHelixTrackFitter::~CbmSttHelixTrackFitter()
{ 
  //  if (fHotArray) 
  //     {
  // 	fHotArray->Delete(); 
  // 	delete fHotArray;
  //     }
}

void CbmSttHelixTrackFitter::Init()
{
  fEventCounter = 0;

  // Get and check CbmRootManager
  CbmRootManager
    *ioman = CbmRootManager::Instance();

  if (! ioman) 
    {
      cout << "-E- CbmSttHelixTrackFitter::Init: "
	   << "RootManager not instantised!" << endl;
      //      return kFATAL;
    }
  
  // Get hit Array
  fHitArray = (TClonesArray*) ioman->GetObject("STTHit");
  if ( ! fHitArray) 
    {
      cout << "-W- CbmSttHelixTrackFitter::Init: No Hit array!"
	   << endl;
    }

  // Get point Array
  fPointArray = (TClonesArray*) ioman->GetObject("STTPoint");
  if ( ! fPointArray) 
    {
      cout << "-W- CbmSttHelixTrackFitter::Init: No Point array!"
	   << endl;
    }

  //  // Create fHotArray
  //   fHotArray = new TClonesArray("CbmSttHOT",1000);


  if(rootoutput) {
    h1 = new TH2F("h1","event display - transverse plane",100,-50,50,100,-50,50);
    h2 = new TH2F("h2","z fit",100, -40, 40, 100, -75, 75); // -4,100,-50,50);  
    h3 = new TH2F("h3","conformal plane",100,-1.5, 1.5, 100, -1.5, 1.5);
    h4 = new TH2F("h4","tubes",100,-45, 45, 100, -80, 80);
    houg = new TH2F("houg","houg", 100,-1.001,1.001,100,-150.001,151.001);
    eventCanvas = new TCanvas("eventcanvas", "eventcanvas", 600, 600); 
    eventCanvas2 = new TCanvas("eventcanvas2", "eventcanvas2", 650, 0 ,600, 600); 
    eventCanvas3 = new TCanvas("eventcanvas3", "eventcanvas3", 650, 0 ,600, 600); 
    eventCanvas4 = new TCanvas("eventcanvas4", "eventcanvas4", 650, 0 ,600, 600); 
    eventCanvas5 = new TCanvas("eventcanvas5", "eventcanvas5", 650, 0 ,600, 600); 
    hougCanvas = new TCanvas("hougCanvas", "hougCanvas", 650, 0 ,600, 600); 
 }
 
}



Int_t CbmSttHelixTrackFitter::DoFit(CbmSttTrack* pTrack, Int_t pidHypo)
{

  fEventCounter++;
 
  if(!pTrack) return 0;
  fTrack = pTrack;
  
  //  fHotArray->Clear();
  if(rootoutput) {
    char goOnChar;
    cout << "press any key to continue: " << endl;
    cin >> goOnChar;
    eventCanvas->cd();
    h1->Draw();
    eventCanvas2->cd();
    h2->Draw();
    cout << "EVENT: " << fEventCounter << endl;
    eventCanvas3->cd();
    h3->Draw();
    eventCanvas4->cd();
    h4->Draw();
    eventCanvas5->cd();
    h4->Draw();
    hougCanvas->cd();
    houg->Draw();
  }
  
  Int_t fit = 0;
 
  fit = Fit4b(pTrack, 1);
  
  if(fit == 0 || pTrack->GetParamLast()->GetTx() == 0 || !(pTrack->GetParamLast()->GetTx()) || pTrack->GetParamLast()->GetTx() > 3000) {
    cout << "-E- pre prefit FAILED " << fit << " " << pTrack->GetParamLast()->GetTx() << endl;
    pTrack->GetParamLast()->SetTx(-999);
    return 0;
  }
  
  // cout << "prefit-------------------------------" << endl;
  fit = 0;
  fit = MinuitFit(pTrack, 1);
 
  // if the fit fails
  if(fit == 0 || pTrack->GetParamLast()->GetTx() == 0 || !(pTrack->GetParamLast()->GetTx()) || pTrack->GetParamLast()->GetTx() > 3000) {
    pTrack->GetParamLast()->SetTx(-999); 
    cout << "-E- prefit FAILED" << endl;
    return 0;
  }
  else {
    pTrack->SetFlag(1); // prefit done 
    Bool_t Rint = IntersectionFinder(pTrack, pTrack->GetParamLast());
    //    cout << "refit" << endl;                                       
    // if refit is OK
    if(Rint == kTRUE) fit = Fit4b(pTrack, 2);  
    else return 0;
    if(fit == 1 && pTrack->GetParamLast()->GetTx() != 0 && (pTrack->GetParamLast()->GetTx()) != NULL && pTrack->GetParamLast()->GetTx() < 3000) { 
     
      pTrack->SetFlag(2); // refit done 
      //      Bool_t zint = ZFinder(pTrack, 1); // old
      Bool_t zint = ZFinderbb3(pTrack, 1); // last working version
      //      if(zint == kFALSE)  zint = ZFinder6b(pTrack, 1); 

      if(zint == kTRUE) {
	// Int_t zfit = Zfit(pTrack, 1); // old
	Int_t zfit = Zfitbb2(pTrack, 1); // last working version
	if(zfit == 1) {
	  pTrack->SetFlag(3); // z fit done 
	} 
      }
      else cout << "-E- zfinder FAILED" << endl;
    }
  }
  
  cout << "param last x: "  << pTrack->GetParamLast()->GetX() << endl;
  cout << "param last y: "  << pTrack->GetParamLast()->GetY() << endl;
  cout << "param last tx: " << pTrack->GetParamLast()->GetTx() << endl;
  cout << "param last ty: " << pTrack->GetParamLast()->GetTy() << endl;
  cout << "param last qp: " << pTrack->GetParamLast()->GetQp() << endl;

  

  if(rootoutput) {
    eventCanvas->Update();
    eventCanvas->Modified();
    eventCanvas2->Update();
    eventCanvas2->Modified();
    eventCanvas3->Update();
    eventCanvas3->Modified();
    eventCanvas4->Update();
    eventCanvas4->Modified();
    eventCanvas5->Update();
    eventCanvas5->Modified();
    hougCanvas->Update();
    hougCanvas->Modified();

   }
  
  return 0;
}

// Int_t CbmSttHelixTrackFitter::AddHitOnTrack(CbmSttTrack *pTrack) { 
//   if(!pTrack) return 0;
//   Int_t hitcounter = pTrack->GetNofHits();
//   for (Int_t k = 0; k < hitcounter; k++) {
//     Int_t iHit = pTrack->GetHitIndex(k);
//     CbmSttHit *currenthit = (CbmSttHit*) fHitArray->At(iHit);
//     if(currenthit->GetXint() == -999 || currenthit->GetXint() == -999) continue;
//     Int_t refindex = currenthit->GetRefIndex(); 
//     // get point
//     CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
//     TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());

//  //     cout << "x: " << currenthit->GetXint() << endl;
//  //     cout << "y: " << currenthit->GetYint() << endl;
//  //     cout << "z: " << currenthit->GetZint() << endl;

//     // axial tubes
//     if(wiredirection != TVector3(0.,0.,1.)) {
//       AddHOT(currenthit->GetXint(), currenthit->GetYint(), -999, iHit, refindex, 0);
//     }
//     // skewd tubes
//     else { 
//       AddHOT(currenthit->GetXint(), currenthit->GetYint(), currenthit->GetZint(), iHit, refindex, 0);
//     }
//   }

//   // pTrack->SetHOT(fHotArray);
  
//   //  fHotArray->Clear();
//   return 0;
// }



Int_t CbmSttHelixTrackFitter::Fit4(CbmSttTrack* pTrack, Int_t pidHypo) {
 
  if(!pTrack) return 0;

  Int_t hitcounter = pTrack->GetNofHits();
  Bool_t first = kFALSE;
  if(hitcounter == 0) return 0;
  if(hitcounter > 50 || hitcounter < 5) {
    cout << "Bad No of hits in STT " << hitcounter << endl;
    return 0;
  }
  cout << "FIT 4 ********************" << endl;
 
  // traslation and rotation -----------
  // traslation near the first point
  Double_t s = 0.001;
  Double_t trasl[2];
  // rotation
  Double_t alpha;
  cout << "hitcounter: " << hitcounter << endl;
  for(Int_t k = 0; k <hitcounter; k++) {
    Int_t iHit = pTrack->GetHitIndex(k);
    CbmSttHit *currenthit = (CbmSttHit*) fHitArray->At(iHit);
    if(!currenthit) continue;

    if(currenthit->GetXint() == -999 || currenthit->GetYint() == -999) continue;
    Int_t refindex = currenthit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    CbmSttHit *hitfirst, *hitlast;

    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());
    if(wiredirection != TVector3(0.,0.,1.)) continue;
    else if(first == kFALSE)
      {
	hitfirst = (CbmSttHit*) fHitArray->At(iHit); 
	first = kTRUE;
	trasl[0] = hitfirst->GetXint();
	trasl[1] = hitfirst->GetYint();
      }
    else{
      hitlast = (CbmSttHit*) fHitArray->At(iHit);  
      alpha = TMath::ATan2(hitlast->GetYint() -  hitfirst->GetYint(), hitlast->GetXint() - hitfirst->GetXint());
    }
  }  

  // error <--> resolution
  Double_t sigr, sigxy, sigx, sigy; // = 0.14;
    
  // FITTING IN X-Y PLANE:
  // v = a + bu + cu^2
    
 
  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.;
  
  Int_t digicounter = 0;
  TArrayD uarray(hitcounter);
  TArrayD varray(hitcounter);
  TArrayD sigv2array(hitcounter);
  TArrayD sigu2array(hitcounter);

  for(Int_t i=0; i < hitcounter; i++){
    uarray.AddAt(-999, i);
    varray.AddAt(-999, i);
    sigv2array.AddAt(-999, i);
    sigu2array.AddAt(-999, i);
  }
  for(Int_t i=0; i < hitcounter; i++){
      
    Int_t iHit = pTrack->GetHitIndex(i);
    CbmSttHit *currenthit = (CbmSttHit*) fHitArray->At(iHit);
    if(!currenthit) continue;
    if(currenthit->GetXint() == -999 || currenthit->GetYint() == -999) continue;
    Int_t refindex = currenthit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    
    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());
    if(wiredirection != TVector3(0.,0.,1.)) continue;
    

    if(pidHypo == 2) {
      Double_t resx = iPoint->GetXtot() - currenthit->GetXint();
      Double_t resy = iPoint->GetYtot() - currenthit->GetYint();
      Double_t resdist = TMath::Sqrt((iPoint->GetYtot() - currenthit->GetYint())*(iPoint->GetYtot() - currenthit->GetYint()) + (iPoint->GetXtot() - currenthit->GetXint())*(iPoint->GetXtot() - currenthit->GetXint()));
      
    }
 
    if(rootoutput) { 
      //     if(pidHypo == 2) {
      eventCanvas->cd();
      TMarker *cir1 = new TMarker(currenthit->GetXint(), currenthit->GetYint(), 6);
      cir1->SetMarkerColor(2);
      if(pidHypo == 2) cir1->Draw("SAME");
      TMarker *cir2 = new TMarker(currenthit->GetX(), currenthit->GetY(), 6);
      if(pidHypo == 1) cir2->Draw("SAME");
      //     }
    }
 
    Double_t xtrasl, ytrasl;
    // traslation
    xtrasl = currenthit->GetXint() - trasl[0];
    ytrasl = currenthit->GetYint() - trasl[1];

    Double_t xrot, yrot;
    // rotation 
    xrot = TMath::Cos(alpha)*xtrasl + TMath::Sin(alpha)*ytrasl;
    yrot = -TMath::Sin(alpha)*xtrasl + TMath::Cos(alpha)*ytrasl;

    // re-traslation
    xtrasl = xrot + s;
    ytrasl = yrot;
     
    if(rootoutput) { 
      eventCanvas->cd();
      TMarker *pt = new TMarker(xrot, yrot, 6);
      pt->SetMarkerColor(3);
      //  if(pidHypo == 2) 
      // pt->Draw("SAME");
    }

    // change coordinate
    Double_t u, v, sigv2, sigu2;
    u = xtrasl / (xtrasl*xtrasl + ytrasl*ytrasl);
    v = ytrasl / (xtrasl*xtrasl + ytrasl*ytrasl);

    if(rootoutput){// && pidHypo == 2) { 
      eventCanvas3->cd();
      TMarker *uv = new TMarker(u, v, 6);
      uv->SetMarkerColor(3);
      if(pidHypo == 2) uv->Draw("SAME");
      eventCanvas3->Update();
      eventCanvas3->Modified();
    }
    
    if(pidHypo == 1) {
      sigr = sqrt(2.) * currenthit->GetIsochrone()/sqrt(12.);
      sigx = sigr;
      sigy = sigr;
    }
    else {
      sigr = 0.0150;
      sigx = fabs(sigr * TMath::Cos(TMath::ATan(marray.At(i))));
      sigy = fabs(sigr * TMath::Sin(TMath::ATan(marray.At(i))));
      sigxy = sigr * TMath::Sqrt(fabs(TMath::Cos(TMath::ATan(marray.At(i))) * TMath::Sin(TMath::ATan(marray.At(i)))));;
    }

    Double_t dvdx = (-2 * xtrasl * ytrasl)/pow((xtrasl*xtrasl + ytrasl*ytrasl),2);
    Double_t dvdy = (xtrasl*xtrasl - ytrasl*ytrasl) / pow((xtrasl*xtrasl + ytrasl*ytrasl),2);
    Double_t dudx = (ytrasl*ytrasl - xtrasl*xtrasl) / pow((xtrasl*xtrasl + ytrasl*ytrasl),2);
    Double_t dudy = (-2 * xtrasl * ytrasl)/pow((xtrasl*xtrasl + ytrasl*ytrasl),2);

    sigu2 = dudx * dudx * sigx * sigx + dudy * dudy * sigy * sigy + 2 * dudx * dudy * sigx * sigy; 
    sigv2 = dvdx * dvdx * sigx * sigx + dvdy * dvdy * sigy * sigy + 2 * dvdx * dvdy * sigx * sigy; 

    uarray.AddAt(u, digicounter);
    varray.AddAt(v, digicounter);
    sigu2array.AddAt(sigu2, digicounter);
    sigv2array.AddAt(sigv2, digicounter);
    
    Su = Su + (u/sigv2);
    Sv = Sv + (v/sigv2);
    
    Suv = Suv + ((u*v)/sigv2);
    Suu = Suu + ((u*u)/sigv2);
    
    Suuu = Suuu + ((u*u*u)/sigv2);
    Suuv = Suuv + ((u*u*v)/sigv2);  
    
    Suuuu = Suuuu + ((u*u*u*u)/sigv2);  
      
    S1 = S1 + 1/sigv2;

    digicounter++;
  }

  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 0;
    cout << "DET 0" << endl;
  }
  
  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];
  
  //     std::cout << "1) parabolic parameters:\n";
  //     std::cout << "a = " << column2[0][0] <<  "\n";
  //     std::cout << "b = " << column2[1][0] <<  "\n";
  //     std::cout << "c = " << column2[2][0] <<  "\n";
  
  Double_t chi2;
  chi2 = 0.;
  for(Int_t i=0; i < digicounter; i++){
    if(uarray.At(i) == -999 || varray.At(i) == -999 || sigv2array.At(i) == -999) continue;
    chi2 = chi2 + pow(((varray.At(i) -  (a + b*uarray.At(i) + c*uarray.At(i)*uarray.At(i))) /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);
  Double_t sigE2;
  for(Int_t i=0; i< digicounter; i++){
    if(uarray.At(i) == -999 || varray.At(i) == -999 || sigv2array.At(i) == -999) continue;
    sigE2 = sigv2array.At(i) + sigu2array.At(i) * pow((b + 2*c*uarray.At(i)),2);
    sigE2array.AddAt(sigE2, i);
   
    Su = Su + (uarray.At(i)/sigE2);
    Sv = Sv + (varray.At(i)/sigE2);
    
    Suv = Suv + ((uarray.At(i)*varray.At(i))/sigE2);
    Suu = Suu + ((uarray.At(i)*uarray.At(i))/sigE2);
    
    Suuu = Suuu + ((uarray.At(i)*uarray.At(i)*uarray.At(i))/sigE2);
    Suuv = Suuv + ((uarray.At(i)*uarray.At(i)*varray.At(i))/sigE2);  
    
    Suuuu = Suuuu + ((uarray.At(i)*uarray.At(i)*uarray.At(i)*uarray.At(i))/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 0;
    cout << "DET 0" << endl;
  }

  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];
   
  //  std::cout << "*************\n";
  //     std::cout << "2) parabolic parameters:\n";
  //     std::cout << "a = " << column2b[0][0] <<  "\n";
  //     std::cout << "b = " << column2b[1][0] <<  "\n";
  //     std::cout << "c = " << column2b[2][0] <<  "\n";
  
  //v = a + bu + cu^2
  chi2 = 0.;
  for(Int_t i=0; i<digicounter; i++){ 
    if(uarray.At(i) == -999 || varray.At(i) == -999 || sigv2array.At(i) == -999) continue;
    chi2 = chi2 + pow(((varray.At(i) - (a + b*uarray.At(i) + c*uarray.At(i)*uarray.At(i)))/sqrt(sigE2array.At(i))), 2);
  }
  
  if(rootoutput){// && pidHypo == 2) { 
    eventCanvas3->cd();
    Double_t uu[100];
    Double_t vv[100];
    for(Int_t p = 0; p<100; p++){
      uu[p] = -1.5 + p*3*0.01;
      vv[p] = a + b*uu[p] + c*uu[p]*uu[p];
    }
    TPolyLine *uvline = new TPolyLine(100, uu, vv);
    if(pidHypo == 2) uvline->Draw("SAME");
    eventCanvas3->Update();
    eventCanvas3->Modified();
  }

  // center and radius
  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 -------------------
  //     // 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<fTrackNumSttHits[fitTrack]; 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
  // translation
  xc = xc - s;
  // rotation    
  xc = TMath::Cos(alpha)*xcrot - TMath::Sin(alpha)*ycrot;
  yc = TMath::Sin(alpha)*xcrot + TMath::Cos(alpha)*ycrot;
  // traslation
  xc = xc + trasl[0];
  yc = yc + trasl[1];
  //     cout << "FIT2: " << xc << " " << yc << endl;
  Double_t phi = TMath::ATan2(yc, xc); // CHECK
  Double_t d;
  d = ((xc + yc) - R*(TMath::Cos(phi) + TMath::Sin(phi)))/(TMath::Cos(phi) + TMath::Sin(phi)); // CHECK
  
  pTrack->GetParamLast()->SetX(d);                
  pTrack->GetParamLast()->SetY(phi);
  //    Double_t newZ = -999.; // CHECK da cambiare
  //    pTrack->GetParamLast()->SetZ(newZ);                
  pTrack->GetParamLast()->SetTx(R);
  //    Double_t newTheta = -999.; // CHECK da cambiare
  //    pTrack->GetParamLast()->SetTy(newTheta);                
  pTrack->GetParamLast()->SetQp(0.); 

  //   cout << "FIT: " << xc << " " << yc << endl;
  //   cout << "RAGGIO: " << R << endl;

  if(rootoutput) {
    eventCanvas->cd();
    TArc *fitarc = new TArc(((pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * cos(pTrack->GetParamLast()->GetY())), ((pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * sin(pTrack->GetParamLast()->GetY())), pTrack->GetParamLast()->GetTx());
    if(pidHypo == 2)  fitarc->SetLineColor(2);
    fitarc->Draw("SAME");
    eventCanvas->Update();
    eventCanvas->Modified();
  }
  return 1;
}
Int_t CbmSttHelixTrackFitter::Fit4b(CbmSttTrack* pTrack, Int_t pidHypo) {
 
  if(!pTrack) return 0;

  Int_t hitcounter = pTrack->GetNofHits();
  Bool_t first = kFALSE;
  if(hitcounter == 0) return 0;
  if(hitcounter > 50 || hitcounter < 5) {
    cout << "Bad No of hits in STT " << hitcounter << endl;
    return 0;
  }
  cout << "FIT 4b ********************" << endl;
 
  // traslation and rotation -----------
  // traslation near the first point
  Double_t s = 0.001;
  Double_t trasl[2];
  // rotation
  Double_t alpha;
  cout << "hitcounter: " << hitcounter << endl;
  for(Int_t k = 0; k <hitcounter; k++) {
    Int_t iHit = pTrack->GetHitIndex(k);
    CbmSttHit *currenthit = (CbmSttHit*) fHitArray->At(iHit);
    if(!currenthit) continue;

    if(currenthit->GetXint() == -999 || currenthit->GetYint() == -999) continue;
    Int_t refindex = currenthit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    CbmSttHit *hitfirst, *hitlast;

    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());
    if(wiredirection != TVector3(0.,0.,1.)) continue;
    else if(first == kFALSE)
      {
	hitfirst = (CbmSttHit*) fHitArray->At(iHit); 
	first = kTRUE;
	trasl[0] = hitfirst->GetXint();
	trasl[1] = hitfirst->GetYint();
      }
    else{
      hitlast = (CbmSttHit*) fHitArray->At(iHit);  
      alpha = TMath::ATan2(hitlast->GetYint() -  hitfirst->GetYint(), hitlast->GetXint() - hitfirst->GetXint());
    }
  }  

  // error <--> resolution
  Double_t sigr, sigxy, sigx, sigy; // = 0.14;
    
  // FITTING IN X-Y PLANE:
  // v = a + bu + cu^2
    
 
  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.;
  
  Int_t digicounter = 0;
  TArrayD uarray(hitcounter);
  TArrayD varray(hitcounter);
  TArrayD sigv2array(hitcounter);
  TArrayD sigu2array(hitcounter);

  for(Int_t i=0; i < hitcounter; i++){
    uarray.AddAt(-999, i);
    varray.AddAt(-999, i);
    sigv2array.AddAt(-999, i);
    sigu2array.AddAt(-999, i);
  }
  for(Int_t i=0; i < hitcounter; i++){
   
    Int_t iHit = pTrack->GetHitIndex(i);
    CbmSttHit *currenthit = (CbmSttHit*) fHitArray->At(iHit);
    if(!currenthit) continue;
    if(currenthit->GetXint() == -999 || currenthit->GetYint() == -999) continue;
    Int_t refindex = currenthit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    
    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());
    if(wiredirection != TVector3(0.,0.,1.)) continue;
    

    if(pidHypo == 2) {
      Double_t resx = iPoint->GetXtot() - currenthit->GetXint();
      Double_t resy = iPoint->GetYtot() - currenthit->GetYint();
      Double_t resdist = TMath::Sqrt((iPoint->GetYtot() - currenthit->GetYint())*(iPoint->GetYtot() - currenthit->GetYint()) + (iPoint->GetXtot() - currenthit->GetXint())*(iPoint->GetXtot() - currenthit->GetXint()));
      
    }
 
    if(rootoutput) { 
      //     if(pidHypo == 2) {
      eventCanvas->cd(); 
      if(pidHypo == 1) {
	// draw MC hits
	TMarker *cir0 = new TMarker(iPoint->GetXtot(), iPoint->GetYtot(), 6);
	cir0->SetMarkerStyle(6);
	cir0->SetMarkerColor(4);
	//	cir0->Draw("SAME");
	//	cout << "MC: " << iPoint->GetXtot() << " " << iPoint->GetYtot() << endl;
      }
      TMarker *cir1 = new TMarker(currenthit->GetXint(), currenthit->GetYint(), 6);
      cir1->SetMarkerColor(2);
      if(pidHypo == 2) cir1->Draw("SAME");
      TMarker *cir2 = new TMarker(currenthit->GetX(), currenthit->GetY(), 6);
      if(pidHypo == 1) cir2->Draw("SAME");
      //     }
    }
 
    Double_t xtrasl, ytrasl;
    // traslation
    xtrasl = currenthit->GetXint() - trasl[0];
    ytrasl = currenthit->GetYint() - trasl[1];

    Double_t xrot, yrot;
    // rotation 
    xrot = TMath::Cos(alpha)*xtrasl + TMath::Sin(alpha)*ytrasl;
    yrot = -TMath::Sin(alpha)*xtrasl + TMath::Cos(alpha)*ytrasl;
   
    // re-traslation
    xtrasl = xrot + s;
    ytrasl = yrot;

    if(rootoutput) { 
      eventCanvas->cd();
      TMarker *pt = new TMarker(xrot, yrot, 6);
      pt->SetMarkerColor(3);
      //  if(pidHypo == 2) 
      // pt->Draw("SAME");
    }

    // change coordinate
    Double_t u, v, sigv2, sigu2;
    u = xtrasl / (xtrasl*xtrasl + ytrasl*ytrasl);
    v = ytrasl / (xtrasl*xtrasl + ytrasl*ytrasl);

    if(rootoutput){// && pidHypo == 2) { 
      eventCanvas3->cd();
      TMarker *uv = new TMarker(u, v, 6);
      uv->SetMarkerColor(3);
      if(pidHypo == 2) uv->Draw("SAME");
      eventCanvas3->Update();
      eventCanvas3->Modified();
    }

    if(pidHypo == 1) {
      if(currenthit->GetIsochrone() == 0) sigr = sqrt(2.)/sqrt(12.);
      else sigr = sqrt(2.) * currenthit->GetIsochrone()/sqrt(12.);
      sigx = sigr;
      sigy = sigr;
    }
    else {
      if(currenthit->GetIsochrone() == 0) {
	sigr = sqrt(2.)/sqrt(12.);
	sigx = sigr;
	sigy = sigr;
      }
      else {
	sigr = 0.0150;
	sigx = fabs(sigr * TMath::Cos(TMath::ATan(marray.At(i))));
	sigy = fabs(sigr * TMath::Sin(TMath::ATan(marray.At(i))));
	sigxy = sigr * TMath::Sqrt(fabs(TMath::Cos(TMath::ATan(marray.At(i))) * TMath::Sin(TMath::ATan(marray.At(i)))));;
      }
    }

    Double_t dvdx = (-2 * xtrasl * ytrasl)/pow((xtrasl*xtrasl + ytrasl*ytrasl),2);
    Double_t dvdy = (xtrasl*xtrasl - ytrasl*ytrasl) / pow((xtrasl*xtrasl + ytrasl*ytrasl),2);
    Double_t dudx = (ytrasl*ytrasl - xtrasl*xtrasl) / pow((xtrasl*xtrasl + ytrasl*ytrasl),2);
    Double_t dudy = (-2 * xtrasl * ytrasl)/pow((xtrasl*xtrasl + ytrasl*ytrasl),2);

    sigu2 = dudx * dudx * sigx * sigx + dudy * dudy * sigy * sigy + 2 * dudx * dudy * sigx * sigy; 
    sigv2 = dvdx * dvdx * sigx * sigx + dvdy * dvdy * sigy * sigy + 2 * dvdx * dvdy * sigx * sigy; 

    uarray.AddAt(u, digicounter);
    varray.AddAt(v, digicounter);
    sigu2array.AddAt(sigu2, digicounter);
    sigv2array.AddAt(sigv2, digicounter); 

    Su = Su + (u/sigv2);
    Sv = Sv + (v/sigv2);
    
    Suv = Suv + ((u*v)/sigv2);
    Suu = Suu + ((u*u)/sigv2);
    
    Suuu = Suuu + ((u*u*u)/sigv2);
    Suuv = Suuv + ((u*u*v)/sigv2);  
    
    Suuuu = Suuuu + ((u*u*u*u)/sigv2);  
      
    S1 = S1 + 1/sigv2;

    digicounter++;
  }

  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 0;
    cout << "DET 0" << endl;
  }
  
  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];
  
  //     std::cout << "1) parabolic parameters:\n";
  //     std::cout << "a = " << column2[0][0] <<  "\n";
  //     std::cout << "b = " << column2[1][0] <<  "\n";
  //     std::cout << "c = " << column2[2][0] <<  "\n";
  
  Double_t chi2;
  chi2 = 0.;
  for(Int_t i=0; i < digicounter; i++){
    if(uarray.At(i) == -999 || varray.At(i) == -999 || sigv2array.At(i) == -999) continue;
    chi2 = chi2 + pow(((varray.At(i) -  (a + b*uarray.At(i) + c*uarray.At(i)*uarray.At(i))) /sqrt(sigv2array.At(i))),2) ;
  }

  cout << "digicounter: " << digicounter << endl;

  //------------------ with right errors
  Su = 0.;
  Sv = 0.;
  Suu = 0.;
  Suv = 0.;
  Suuu = 0.;
  S1 = 0.;
  Suuv = 0.;
  Suuuu = 0.;
 
  TArrayD sigE2array(digicounter);
  Double_t sigE2;
  for(Int_t i=0; i< digicounter; i++){
    if(uarray.At(i) == -999 || varray.At(i) == -999 || sigv2array.At(i) == -999) continue;
    sigE2 = sigv2array.At(i) + sigu2array.At(i) * pow((b + 2*c*uarray.At(i)),2);
    sigE2array.AddAt(sigE2, i);
   
    Su = Su + (uarray.At(i)/sigE2);
    Sv = Sv + (varray.At(i)/sigE2);
    
    Suv = Suv + ((uarray.At(i)*varray.At(i))/sigE2);
    Suu = Suu + ((uarray.At(i)*uarray.At(i))/sigE2);
    
    Suuu = Suuu + ((uarray.At(i)*uarray.At(i)*uarray.At(i))/sigE2);
    Suuv = Suuv + ((uarray.At(i)*uarray.At(i)*varray.At(i))/sigE2);  
    
    Suuuu = Suuuu + ((uarray.At(i)*uarray.At(i)*uarray.At(i)*uarray.At(i))/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 0;
    cout << "DET 0" << endl;
  }

  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];
   
  //  std::cout << "*************\n";
  //     std::cout << "2) parabolic parameters:\n";
  //     std::cout << "a = " << column2b[0][0] <<  "\n";
  //     std::cout << "b = " << column2b[1][0] <<  "\n";
  //     std::cout << "c = " << column2b[2][0] <<  "\n";

  //v = a + bu + cu^2
  chi2 = 0.;
  for(Int_t i=0; i<digicounter; i++){ 
    if(uarray.At(i) == -999 || varray.At(i) == -999 || sigv2array.At(i) == -999) continue;
    chi2 = chi2 + pow(((varray.At(i) - (a + b*uarray.At(i) + c*uarray.At(i)*uarray.At(i)))/sqrt(sigE2array.At(i))), 2);
  }
  
  if(rootoutput){// && pidHypo == 2) { 
    eventCanvas3->cd();
    Double_t uu[100];
    Double_t vv[100];
    for(Int_t p = 0; p<100; p++){
      uu[p] = -1.5 + p*3*0.01;
      vv[p] = a + b*uu[p] + c*uu[p]*uu[p];
    }
    TPolyLine *uvline = new TPolyLine(100, uu, vv);
    if(pidHypo == 2) uvline->Draw("SAME");
    eventCanvas3->Update();
    eventCanvas3->Modified();
  }

  // center and radius
  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 -------------------
  //     // 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<fTrackNumSttHits[fitTrack]; 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
  // translation
  xc = xc - s;
  // rotation    
  xc = TMath::Cos(alpha)*xcrot - TMath::Sin(alpha)*ycrot;
  yc = TMath::Sin(alpha)*xcrot + TMath::Cos(alpha)*ycrot;
  // traslation
  xc = xc + trasl[0];
  yc = yc + trasl[1];
  Double_t phi = TMath::ATan2(yc, xc); // CHECK
  Double_t d;
  d = ((xc + yc) - R*(TMath::Cos(phi) + TMath::Sin(phi)))/(TMath::Cos(phi) + TMath::Sin(phi)); // CHECK
  
  pTrack->GetParamLast()->SetX(d);                
  pTrack->GetParamLast()->SetY(phi);
  //    Double_t newZ = -999.; // CHECK da cambiare
  //    pTrack->GetParamLast()->SetZ(newZ);                
  pTrack->GetParamLast()->SetTx(R);
  //    Double_t newTheta = -999.; // CHECK da cambiare
  //    pTrack->GetParamLast()->SetTy(newTheta);                
  h = -GetCharge(d, phi, R);
  pTrack->GetParamLast()->SetQp(-h); 

  //   cout << "FIT: " << xc << " " << yc << endl;
  //   cout << "RAGGIO: " << R << endl;

  if(rootoutput) {
    eventCanvas->cd();
    TArc *fitarc = new TArc(((pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * cos(pTrack->GetParamLast()->GetY())), ((pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * sin(pTrack->GetParamLast()->GetY())), pTrack->GetParamLast()->GetTx());
    if(pidHypo == 2)  fitarc->SetLineColor(2);
    fitarc->Draw("SAME");
    eventCanvas->Update();
    eventCanvas->Modified();
  }
  return 1;
}


// -------------- IntersectionFinder  --------------------------------------
Bool_t CbmSttHelixTrackFitter::IntersectionFinder(CbmSttTrack *pTrack, CbmTrackParam *par)
{

  ResetMArray();

  // calculation of the curvature from the helix prefit
  if(pTrack->GetParamLast()->GetTx() == 0) return kFALSE;

  TVector2 vec((pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * cos(pTrack->GetParamLast()->GetY()), (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * sin(pTrack->GetParamLast()->GetY()));

  //==========
  // POINT ----------------------------------------------------
  // 1. find the cooordinates of the point fired wire of the track
  TVector2 point; // point
  Double_t radius;

  Int_t counter = 0;
  // loop over input points
  Int_t hitcounter = pTrack->GetNofHits();
  for(Int_t k = 0; k < hitcounter; k++){
    // get hit
    Int_t iHit = pTrack->GetHitIndex(k);
    CbmSttHit *pMhit = (CbmSttHit*) fHitArray->At(iHit);
    if (!pMhit ) continue;

    Int_t refindex = pMhit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());
    
    if(wiredirection != TVector3(0.,0.,1.)) continue;

    // [xp, yp] point = coordinates xy of the centre of the firing tube
    point.Set(pMhit->GetX(), pMhit->GetY());
    radius = pMhit->GetIsochrone();

    // 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();

    if(rootoutput) { 
      eventCanvas->cd();
      TArc *archetto = new TArc(point.X(), point.Y(), radius);
      archetto->Draw("SAME");
      TLine *line = new TLine(-50, -50*m + q, 50, 50*m + q);
      line->SetLineColor(4);
      // line->Draw("SAME"); 
      eventCanvas->Update();
      eventCanvas->Modified();
    }
   
    // cut on radius CHECK
    // if the simulated radius is too small, the pMhit
    // is not used for the fit 
    if(radius < 0.1) {
      marray.AddAt(-999, k);
      pMhit->SetXint(-999);
      pMhit->SetYint(-999);
      continue;  // CHECK cosi' butto l' hit
    }
    // 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() - radius*radius))) / (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() - radius*radius))) / (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() - (pTrack->GetParamLast()->GetTx()) *(pTrack->GetParamLast()->GetTx()) ))) / (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() - (pTrack->GetParamLast()->GetTx()) *(pTrack->GetParamLast()->GetTx())))) / (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())) - radius > 0.000001) { 
    //      cout << "ATTENZIONE: " << "differenza = " << TMath::Sqrt((xy->X() - point.X())*(xy->X() - point.X()) + (xy->Y() - point.Y())*(xy->Y() - point.Y())) - radius << endl;
    //    }

    //    cout << "x hit prima: " << pMhit->GetX() << " " << pMhit->GetXint() << endl;
    //    cout << prima: " << pMhit->GetXint() << " " << pMhit->GetYint() << " " << pMhit->GetZint() << endl;

    marray.AddAt(m, k);
    pMhit->SetXint(xy->X());
    pMhit->SetYint(xy->Y());
    
    if(rootoutput) { 
      eventCanvas->cd();
      TMarker *np = new TMarker(pMhit->GetXint(), pMhit->GetYint(), 6);
      np->SetMarkerColor(4);
      //      np->Draw("SAME");
      eventCanvas->Update();
      eventCanvas->Modified();
    }

    counter++;
  }

  if(counter==0) return kFALSE;
  else return kTRUE;
}

// -------------- IntersectionFinder  --------------------------------------
Bool_t CbmSttHelixTrackFitter::IntersectionFinder4b(CbmSttTrack *pTrack, CbmTrackParam *par)
// if the drift radius is too small the center of the tube is used
{

  ResetMArray();

  // calculation of the curvature from the helix prefit
  if(pTrack->GetParamLast()->GetTx() == 0) return kFALSE;

  TVector2 vec((pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * cos(pTrack->GetParamLast()->GetY()), (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * sin(pTrack->GetParamLast()->GetY()));

  //==========
  // POINT ----------------------------------------------------
  // 1. find the cooordinates of the point fired wire of the track
  TVector2 point; // point
  Double_t radius;

  Int_t counter = 0;
  // loop over input points
  Int_t hitcounter = pTrack->GetNofHits();
  for(Int_t k = 0; k < hitcounter; k++){
    // get hit
    Int_t iHit = pTrack->GetHitIndex(k);
    CbmSttHit *pMhit = (CbmSttHit*) fHitArray->At(iHit);
    if (!pMhit ) continue;

    Int_t refindex = pMhit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());
    
    if(wiredirection != TVector3(0.,0.,1.)) continue;


    // [xp, yp] point = coordinates xy of the centre of the firing tube
    point.Set(pMhit->GetX(), pMhit->GetY());
    radius = pMhit->GetIsochrone();

    // 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();

    if(rootoutput) { 
      eventCanvas->cd();
      TArc *archetto = new TArc(point.X(), point.Y(), radius);
      archetto->Draw("SAME");
      TLine *line = new TLine(-50, -50*m + q, 50, 50*m + q);
      line->SetLineColor(4);
      // line->Draw("SAME"); 
      eventCanvas->Update();
      eventCanvas->Modified();
    }
   
    // cut on radius CHECK
    // if the simulated radius is too small, the pMhit
    // is not used for the fit 
    if(radius < 0.1) {
      marray.AddAt(-999, k);
      pMhit->SetXint(pMhit->GetX());
      pMhit->SetYint(pMhit->GetY());
      continue;  // CHECK cosi' butto l' hit
    }
    // 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() - radius*radius))) / (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() - radius*radius))) / (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() - (pTrack->GetParamLast()->GetTx()) *(pTrack->GetParamLast()->GetTx()) ))) / (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() - (pTrack->GetParamLast()->GetTx()) *(pTrack->GetParamLast()->GetTx())))) / (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())) - radius > 0.000001) { 
    //      cout << "ATTENZIONE: " << "differenza = " << TMath::Sqrt((xy->X() - point.X())*(xy->X() - point.X()) + (xy->Y() - point.Y())*(xy->Y() - point.Y())) - radius << endl;
    //    }

    //    cout << "x hit prima: " << pMhit->GetX() << " " << pMhit->GetXint() << endl;
    //    cout << prima: " << pMhit->GetXint() << " " << pMhit->GetYint() << " " << pMhit->GetZint() << endl;

    marray.AddAt(m, k);
    pMhit->SetXint(xy->X());
    pMhit->SetYint(xy->Y());

    if(rootoutput) { 
      eventCanvas->cd();
      TMarker *np = new TMarker(pMhit->GetXint(), pMhit->GetYint(), 6);
      np->SetMarkerColor(4);
      //      np->Draw("SAME");
      eventCanvas->Update();
      eventCanvas->Modified();
    }
   
    counter++;
  }

  if(counter==0) return kFALSE;
  else return kTRUE;
}



// -------- ZFinder --------------------------------------------
Bool_t CbmSttHelixTrackFitter::ZFinder(CbmSttTrack* pTrack, Int_t pidHypo) {

  if(!pTrack) return 0;

  Int_t hitcounter = pTrack->GetNofHits();
 
  // cut on number of hits
  if(hitcounter > 30) {
    //    cout << "more than 30 hits" << endl;
    //  pTrack->GetParamLast()->SetX(-999);                
    //     pTrack->GetParamLast()->SetY(-999);
    //     //    Double_t newZ = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetZ(newZ);                
    //     pTrack->GetParamLast()->SetTx(-999);
    //     //    Double_t newTheta = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetTy(newTheta);                
    //     pTrack->GetParamLast()->SetQp(0.);
    return 0;
  }
  if(hitcounter < 5)  {
    //    cout << "less than 5 hits" << endl;
    //     pTrack->GetParamLast()->SetX(-999);                
    //     pTrack->GetParamLast()->SetY(-999);
    //     //    Double_t newZ = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetZ(newZ);                
    //     pTrack->GetParamLast()->SetTx(-999);
    //     //    Double_t newTheta = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetTy(newTheta);                
    //     pTrack->GetParamLast()->SetQp(0.);
    return 0;
  }
  
  ZPointsArray = new TObjArray(2*hitcounter);

  // SCOSL ======
  // phi0
  TVector3 *v0;
  Double_t Phi0;
  // ============

  // ZFIT =======
  if(hitcounter == 0) return kFALSE;
  
  Double_t Sxx, Sx, Sz, Sxz, S1z;
  Double_t Detz = 0.;
  Double_t fitm, fitp;
  Double_t sigz = 1.;  // CHECK
  
  Sx = 0.;
  Sz = 0.;
  Sxx = 0.;
  Sxz = 0.;
  S1z = 0.;
  // ============

  TVector3 *tofit, *tofit2;
 
  // centre of curvature
  Double_t x_0 = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * cos(pTrack->GetParamLast()->GetY());
  Double_t y_0 = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * sin(pTrack->GetParamLast()->GetY());
  // radius of curvature
  Double_t R  = pTrack->GetParamLast()->GetTx();
  Int_t wireOk = 0;
  for (Int_t i = 0; i < hitcounter; i++) {
   
    // get index of hit
    Int_t iHit = pTrack->GetHitIndex(i);

    // get hit
    CbmSttHit *pMhit = (CbmSttHit*) fHitArray->At(iHit);

    if(pMhit == NULL) continue;

    Int_t refindex = pMhit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());
    TVector3 wiredirection2;

    wiredirection2 = 75. * wiredirection;
    TVector3 cenposition(pMhit->GetX(), pMhit->GetY(), 0.);  // CHECK! z = 35!!


    TVector3 min, max;
    min = cenposition - wiredirection2;
    max = cenposition + wiredirection2;
    
    // first extremity
    Double_t x_1= min.X(); 
    Double_t y_1= min.Y(); 
    Double_t z_1= min.Z(); 
    
    // second extremity
    Double_t x_2= max.X();
    Double_t y_2= max.Y();
    Double_t z_2= max.Z();
    
    Double_t rcur = pMhit->GetIsochrone();
    
    Double_t x1 = -9999.;
    Double_t y1 = -9999.;
    Double_t x2 = -9999.;
    Double_t y2 = -9999.;

    if(wiredirection != TVector3(0.,0.,1.)) 
      {
	wireOk++;
	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-cenposition.X())*(x1-cenposition.X())+
				(y1-cenposition.Y())*(y1-cenposition.Y()));
	Double_t d2=TMath::Sqrt((x2-cenposition.X())*(x2-cenposition.X())+
				(y2-cenposition.Y())*(y2-cenposition.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-cenposition.X())*(x1-cenposition.X())+(y1-cenposition.Y())*(y1-cenposition.Y()));
	d2=TMath::Sqrt((x2-cenposition.X())*(x2-cenposition.X())+(y2-cenposition.Y())*(y2-cenposition.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 kFALSE;
	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 

	tofit = new TVector3(xcen0,ycen0,z_);
	ZPointsArray->Add(tofit);
	tofit2 = new TVector3(xcen1,ycen1,z_bis);
	ZPointsArray->Add(tofit2);
   
	// FIRST CHOICE
	//====================  
	// scosl
	Double_t x0;
	Double_t y0;
	if(wireOk == 1) {
	  if(tofit != NULL) {
	    // phi0
	    v0 = new TVector3(tofit->X(), tofit->Y(), tofit->Z());
	  }
	  else if(tofit2 != NULL) {
	    v0 = new TVector3(tofit2->X(), tofit2->Y(), tofit2->Z());
	  }
	   
	  Phi0 = TMath::ATan2((v0->Y() - y_0),(v0->X() - x_0));
	  
	  // CHECK
	  // we are using the first digi for the arc length calculation
	  // but this may be wrong
	  x0 = v0->X();
	  y0 = v0->Y();
	}
	
	TVector3 vi(tofit->X(), tofit->Y(), tofit->Z());
	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));
	//====================  
	if(rootoutput) {
	  eventCanvas2->cd();
	  TMarker *mrk = new TMarker(scos, tofit->Z(), 6);
	  mrk->Draw("SAME");
	}
	//====================  
	// zfit
	//
	Sx = Sx + (scos/(sigz * sigz));
	Sz = Sz + (tofit->Z()/(sigz * sigz));
	Sxz = Sxz + ((scos * tofit->Z())/(sigz * sigz));
	Sxx = Sxx + ((scos * scos)/(sigz * sigz));
	S1z = S1z + 1/(sigz * sigz);
	//====================

	// SECOND CHOICE
	//====================  
	// scosl
	vi.SetXYZ(tofit2->X(), tofit2->Y(), tofit2->Z());
	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));
	//====================  
	
	//====================  
	// zfit
	//
	Sx = Sx + (scos/(sigz * sigz));
	Sz = Sz + (tofit2->Z()/(sigz * sigz));
	Sxz = Sxz + ((scos * tofit2->Z())/(sigz * sigz));
	Sxx = Sxx + ((scos * scos)/(sigz * sigz));
	S1z = S1z + 1/(sigz * sigz);
	//====================
	if(rootoutput) {     
	  eventCanvas2->cd();
	  TMarker *mrk2 = new TMarker(scos, tofit2->Z(), 6);
	  mrk2->Draw("SAME");
	}
      }
    
  }
  cout << "skewed: " << wireOk << endl;
  if(wireOk < 2) return kFALSE;
 
  Detz = S1z*Sxx - Sx*Sx;
  if(Detz == 0) {
    return kFALSE;
  }
 
  fitp = (1/Detz)*(Sxx*Sz - Sx*Sxz);
  fitm = (1/Detz)*(S1z*Sxz - Sx*Sz);
  //   fiterrp2 = (1/Detz)*Sxx;
  //   fiterrm2 = (1/Detz)*S1z;

  if(rootoutput) {  
    eventCanvas2->cd();
    TLine *line = new TLine(-20, -20*fitm + fitp, 20, (20*fitm + fitp));
    line->Draw("SAME");
  }
 
 
  //  Double_t chi2z;
  //  chi2z = 0.;
  //  for(Int_t i=0; i < hitcounter; 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);
  Int_t counter = 0;

  if(fitm == 0. && fitp == 0.) return kFALSE;
 
  if( wireOk < 2) return kFALSE;

  wireOk = 0;
  Int_t okcounter = 0;
  for(Int_t i = 0; i < (2*hitcounter); i+=2) {
    // get index of hit
    //   cout << i << " counter: " << counter << endl;
    Int_t iHit = pTrack->GetHitIndex(counter);
    counter++;
   
    // get hit
    CbmSttHit *pMhit = (CbmSttHit*) fHitArray->At(iHit);
  
    Int_t refindex = pMhit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());

    if(wiredirection == TVector3(0.,0.,1.)) continue;
    wireOk++;
   
    Double_t sigz = 1.; // CHECK
   
    Double_t x0;
    Double_t y0;
    if(wireOk==1) {
      TVector3 *vi = (TVector3*) ZPointsArray->At(okcounter);
      // phi0
      v0->SetXYZ(vi->X(), vi->Y(), vi->Z());
      Phi0 = TMath::ATan2((v0->Y() - y_0),(v0->X() - x_0));
     
      // CHECK
      // we are using the first digi for the arc length calculation
      // but this may be wrong
      x0 = v0->X();
      y0 = v0->Y();
    }
   
    // FIRST CHOICE
    TVector3 *vi = (TVector3*) ZPointsArray->At(okcounter);
 
    if(vi == NULL) continue;
    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));
 
    Bool_t fitdone = kFALSE;
    Double_t distfirst = pow(((vi->Z() - (outz.Y() + outz.X() * scos))/sigz), 2);
    if(distfirst < 10) {

      pMhit->SetXint(vi->X());
      pMhit->SetYint(vi->Y());
      pMhit->SetZint(vi->Z());
      fitdone = kTRUE;
    }
    // SECOND CHOICE
    vi = (TVector3*) ZPointsArray->At(okcounter+1); 
    if(vi == NULL) continue;
    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));
    Double_t distsecond = pow(((vi->Z() - (outz.Y() + outz.X() * scos))/sigz), 2);

    if(fitdone == kTRUE){
      if(distsecond < 10 && distsecond < distfirst) {
	pMhit->SetXint(vi->X());
	pMhit->SetYint(vi->Y());
	pMhit->SetZint(vi->Z());
      }
    }
    else {
      if (distsecond < 10) {
	pMhit->SetXint(vi->X());
	pMhit->SetYint(vi->Y());
	pMhit->SetZint(vi->Z());
	fitdone = kTRUE;
      }
      else {
	pMhit->SetXint(-999);
	pMhit->SetYint(-999);
	pMhit->SetZint(-999);
      }
    }

    if(rootoutput) {
      if(pMhit->GetZint()!= -999) {
	eventCanvas2->cd();
	TMarker *mrk3 = new TMarker(scos, pMhit->GetZint(), 6);
	mrk3->SetMarkerColor(2);    
	mrk3->Draw("SAME");
      }
    }
   
    okcounter = okcounter + 2;
  }
 
  return kTRUE;
  //   fiterrp = sqrt(fiterrp2);
  //   fiterrm = sqrt(fiterrm2);
 
} 
 
// -------- ZFinder2 --------------------------------------------
Bool_t CbmSttHelixTrackFitter::ZFinder2(CbmSttTrack* pTrack, Int_t pidHypo) {
  // CHANGED Scosl

  if(!pTrack) return 0;

  Int_t hitcounter = pTrack->GetNofHits();
 
  // cut on number of hits
  if(hitcounter > 30) {
    //    cout << "more than 30 hits" << endl;
    //  pTrack->GetParamLast()->SetX(-999);                
    //     pTrack->GetParamLast()->SetY(-999);
    //     //    Double_t newZ = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetZ(newZ);                
    //     pTrack->GetParamLast()->SetTx(-999);
    //     //    Double_t newTheta = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetTy(newTheta);                
    //     pTrack->GetParamLast()->SetQp(0.);
    return 0;
  }
  if(hitcounter < 5)  {
    //    cout << "less than 5 hits" << endl;
    //     pTrack->GetParamLast()->SetX(-999);                
    //     pTrack->GetParamLast()->SetY(-999);
    //     //    Double_t newZ = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetZ(newZ);                
    //     pTrack->GetParamLast()->SetTx(-999);
    //     //    Double_t newTheta = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetTy(newTheta);                
    //     pTrack->GetParamLast()->SetQp(0.);
    return 0;
  }
  
  ZPointsArray = new TObjArray(2*hitcounter);

  // SCOSL ======
  // phi0
  TVector3 *v0;
  Double_t Phi0;
  // ============

  // ZFIT =======
  if(hitcounter == 0) return kFALSE;
  
  Double_t Sxx, Sx, Sz, Sxz, S1z;
  Double_t Detz = 0.;
  Double_t fitm, fitp;
  Double_t sigz = 1.;  // CHECK
  
  Sx = 0.;
  Sz = 0.;
  Sxx = 0.;
  Sxz = 0.;
  S1z = 0.;
  // ============

  TVector3 *tofit, *tofit2;
 
  // centre of curvature
  Double_t x_0 = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * cos(pTrack->GetParamLast()->GetY());
  Double_t y_0 = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * sin(pTrack->GetParamLast()->GetY());
  // radius of curvature
  Double_t R  = pTrack->GetParamLast()->GetTx();
  Int_t wireOk = 0;
  for (Int_t i = 0; i < hitcounter; i++) {
   
    // get index of hit
    Int_t iHit = pTrack->GetHitIndex(i);

    // get hit
    CbmSttHit *pMhit = (CbmSttHit*) fHitArray->At(iHit);

    if(pMhit == NULL) continue;

    Int_t refindex = pMhit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());
    TVector3 wiredirection2;

    wiredirection2 = 75. * wiredirection;
    TVector3 cenposition(pMhit->GetX(), pMhit->GetY(), 0.);  // CHECK! z = 35!!


    TVector3 min, max;
    min = cenposition - wiredirection2;
    max = cenposition + wiredirection2;
    
    // first extremity
    Double_t x_1= min.X(); 
    Double_t y_1= min.Y(); 
    Double_t z_1= min.Z(); 
    
    // second extremity
    Double_t x_2= max.X();
    Double_t y_2= max.Y();
    Double_t z_2= max.Z();
    
    Double_t rcur = pMhit->GetIsochrone();
    
    Double_t x1 = -9999.;
    Double_t y1 = -9999.;
    Double_t x2 = -9999.;
    Double_t y2 = -9999.;

    if(wiredirection != TVector3(0.,0.,1.)) 
      {
	wireOk++;
	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-cenposition.X())*(x1-cenposition.X())+
				(y1-cenposition.Y())*(y1-cenposition.Y()));
	Double_t d2=TMath::Sqrt((x2-cenposition.X())*(x2-cenposition.X())+
				(y2-cenposition.Y())*(y2-cenposition.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-cenposition.X())*(x1-cenposition.X())+(y1-cenposition.Y())*(y1-cenposition.Y()));
	d2=TMath::Sqrt((x2-cenposition.X())*(x2-cenposition.X())+(y2-cenposition.Y())*(y2-cenposition.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 kFALSE;
	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 

	tofit = new TVector3(xcen0,ycen0,z_);
	ZPointsArray->Add(tofit);
	tofit2 = new TVector3(xcen1,ycen1,z_bis);
	ZPointsArray->Add(tofit2);
   
	// FIRST CHOICE
	//====================  
	// scosl
	Double_t x0;
	Double_t y0;
	if(wireOk == 1) {
	  if(tofit != NULL) {
	    // phi0
	    v0 = new TVector3(tofit->X(), tofit->Y(), tofit->Z());
	  }
	  else if(tofit2 != NULL) {
	    v0 = new TVector3(tofit2->X(), tofit2->Y(), tofit2->Z());
	  }
	   
	  Phi0 = TMath::ATan2((v0->Y() - y_0),(v0->X() - x_0));
	  
	  // CHECK
	  // we are using the first digi for the arc length calculation
	  // but this may be wrong
	  x0 = v0->X();
	  y0 = v0->Y();


	}
	
	TVector3 vi(tofit->X(), tofit->Y(), tofit->Z());
	//	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));
	Double_t scos = R*(TMath::ATan2(R*TMath::Sin(Phi0) - (vi.Y() - y0), R * TMath::Cos(Phi0) - (vi.X() - x0)) - Phi0);
	//====================  
	if(rootoutput) {
	  eventCanvas2->cd();
	  TMarker *mrk = new TMarker(scos, tofit->Z(), 6);
	  mrk->Draw("SAME");
	}
	//====================  
	// zfit
	//
	Sx = Sx + (scos/(sigz * sigz));
	Sz = Sz + (tofit->Z()/(sigz * sigz));
	Sxz = Sxz + ((scos * tofit->Z())/(sigz * sigz));
	Sxx = Sxx + ((scos * scos)/(sigz * sigz));
	S1z = S1z + 1/(sigz * sigz);
	//====================

	// SECOND CHOICE
	//====================  
	// scosl
	vi.SetXYZ(tofit2->X(), tofit2->Y(), tofit2->Z());
	//	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));	
	scos = R*(TMath::ATan2(R*TMath::Sin(Phi0) - (vi.Y() - y0), R * TMath::Cos(Phi0) - (vi.X() - x0)) - Phi0);

	//====================  
	
	//====================  
	// zfit
	//
	Sx = Sx + (scos/(sigz * sigz));
	Sz = Sz + (tofit2->Z()/(sigz * sigz));
	Sxz = Sxz + ((scos * tofit2->Z())/(sigz * sigz));
	Sxx = Sxx + ((scos * scos)/(sigz * sigz));
	S1z = S1z + 1/(sigz * sigz);
	//====================
	if(rootoutput) {     
	  eventCanvas2->cd();
	  TMarker *mrk2 = new TMarker(scos, tofit2->Z(), 6);
	  mrk2->Draw("SAME");
	}
      }
    
  }
  cout << "skewed: " << wireOk << endl;
  if(wireOk < 2) return kFALSE;
 
  Detz = S1z*Sxx - Sx*Sx;
  if(Detz == 0) {
    return kFALSE;
  }
 
  fitp = (1/Detz)*(Sxx*Sz - Sx*Sxz);
  fitm = (1/Detz)*(S1z*Sxz - Sx*Sz);
  //   fiterrp2 = (1/Detz)*Sxx;
  //   fiterrm2 = (1/Detz)*S1z;

  if(rootoutput) {  
    eventCanvas2->cd();
    TLine *line = new TLine(-20, -20*fitm + fitp, 20, (20*fitm + fitp));
    line->Draw("SAME");
  }
 
 
  //  Double_t chi2z;
  //  chi2z = 0.;
  //  for(Int_t i=0; i < hitcounter; 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);
  Int_t counter = 0;

  if(fitm == 0. && fitp == 0.) return kFALSE;
 
  if( wireOk < 2) return kFALSE;

  wireOk = 0;
  Int_t okcounter = 0;
  for(Int_t i = 0; i < (2*hitcounter); i+=2) {
    // get index of hit
    //   cout << i << " counter: " << counter << endl;
    Int_t iHit = pTrack->GetHitIndex(counter);
    counter++;
   
    // get hit
    CbmSttHit *pMhit = (CbmSttHit*) fHitArray->At(iHit);
  
    Int_t refindex = pMhit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());
  
    if(wiredirection == TVector3(0.,0.,1.)) continue;
  
    wireOk++;
   
    Double_t sigz = 1.; // CHECK
   
    Double_t x0;
    Double_t y0;
    if(wireOk==1) {
      TVector3 *vi = (TVector3*) ZPointsArray->At(okcounter);
      // phi0
      v0->SetXYZ(vi->X(), vi->Y(), vi->Z());
      Phi0 = TMath::ATan2((v0->Y() - y_0),(v0->X() - x_0));
     
      // CHECK
      // we are using the first digi for the arc length calculation
      // but this may be wrong
      x0 = v0->X();
      y0 = v0->Y();
    
    }
   
    // FIRST CHOICE
    TVector3 *vi = (TVector3*) ZPointsArray->At(okcounter);
 

    if(vi == NULL) continue;
    Double_t scos = R*(TMath::ATan2(R*TMath::Sin(Phi0) - (vi->Y() - y0), R * TMath::Cos(Phi0) - (vi->X() - x0)) - Phi0);

    Bool_t fitdone = kFALSE;
    Double_t distfirst = pow(((vi->Z() - (outz.Y() + outz.X() * scos))/sigz), 2);
    if(distfirst < 10) {

      pMhit->SetXint(vi->X());
      pMhit->SetYint(vi->Y());
      pMhit->SetZint(vi->Z());
      fitdone = kTRUE;
    }
    // SECOND CHOICE
    vi = (TVector3*) ZPointsArray->At(okcounter+1); 
    if(vi == NULL) continue;
    scos = R*(TMath::ATan2(R*TMath::Sin(Phi0) - (vi->Y() - y0), R * TMath::Cos(Phi0) - (vi->X() - x0)) - Phi0);
     
    Double_t distsecond = pow(((vi->Z() - (outz.Y() + outz.X() * scos))/sigz), 2);
   
    if(fitdone == kTRUE){
      if(distsecond < 10 && distsecond < distfirst) {
	pMhit->SetXint(vi->X());
	pMhit->SetYint(vi->Y());
	pMhit->SetZint(vi->Z());
      }
    }
    else {
      if (distsecond < 10) {
	pMhit->SetXint(vi->X());
	pMhit->SetYint(vi->Y());
	pMhit->SetZint(vi->Z());
	fitdone = kTRUE;
      }
      else {
	pMhit->SetXint(-999);
	pMhit->SetYint(-999);
	pMhit->SetZint(-999);
      }
    }

  
    if(rootoutput) {
      if(pMhit->GetZint()!= -999) {
	eventCanvas2->cd();
	TMarker *mrk3 = new TMarker(scos, pMhit->GetZint(), 6);
	mrk3->SetMarkerColor(2);    
	mrk3->Draw("SAME");
      }
    }
 
    okcounter = okcounter + 2;
  }
 
 
  return kTRUE;
  //   fiterrp = sqrt(fiterrp2);
  //   fiterrm = sqrt(fiterrm2);
 
} 

// -------- ZFinder --------------------------------------------
Bool_t CbmSttHelixTrackFitter::ZFinderbb3(CbmSttTrack* pTrack, Int_t pidHypo) {
  // the z finding procedure uses the hough transform to find the line
  // in the plane z - track length on which the correct points lie.

  cout << "ZFINDERbb3" << endl;

  if(!pTrack) return 0;

  Int_t hitcounter = pTrack->GetNofHits();
 
  // cut on number of hits
  if(hitcounter > 50) {
    cout << "more than 50 hits " << hitcounter << endl;
    //  pTrack->GetParamLast()->SetX(-999);                
    //     pTrack->GetParamLast()->SetY(-999);
    //     //    Double_t newZ = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetZ(newZ);                
    //     pTrack->GetParamLast()->SetTx(-999);
    //     //    Double_t newTheta = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetTy(newTheta);                
    //     pTrack->GetParamLast()->SetQp(0.);
    return 0;
  }
  if(hitcounter < 5)  {
    cout << "less than 5 hits" << endl;
    //     pTrack->GetParamLast()->SetX(-999);                
    //     pTrack->GetParamLast()->SetY(-999);
    //     //    Double_t newZ = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetZ(newZ);                
    //     pTrack->GetParamLast()->SetTx(-999);
    //     //    Double_t newTheta = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetTy(newTheta);                
    //     pTrack->GetParamLast()->SetQp(0.);
    return 0;
  }
  
  ZPointsArray = new TObjArray(2*hitcounter);

  // ZFIT =======
  if(hitcounter == 0) return kFALSE;
  
  Double_t Sxx, Sx, Sz, Sxz, S1z;
  Double_t Detz = 0.;
  Double_t fitm, fitp;
  Double_t sigz = 1.;  // CHECK
  
  Sx = 0.;
  Sz = 0.;
  Sxx = 0.;
  Sxz = 0.;
  S1z = 0.;
  // ============

  TVector3 *tofit, *tofit2;
 
  // centre of curvature
  Double_t x_0 = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * cos(pTrack->GetParamLast()->GetY());
  Double_t y_0 = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * sin(pTrack->GetParamLast()->GetY());

  // radius of curvature
  Double_t R = pTrack->GetParamLast()->GetTx();
  Int_t wireOk = 0;
  Bool_t first = kTRUE;

  for (Int_t i = 0; i < hitcounter; i++) {
   
    // get index of hit
    Int_t iHit = pTrack->GetHitIndex(i);
   
    // get hit
    CbmSttHit *pMhit = (CbmSttHit*) fHitArray->At(iHit);
   
  if(pMhit == NULL) continue;
 

    Int_t refindex = pMhit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());
    TVector3 wiredirection2;

    wiredirection2 = 75. * wiredirection;
    TVector3 cenposition(pMhit->GetX(), pMhit->GetY(), pMhit->GetZ());  // CHECK! z = 35!!
    //    if(pMhit->GetZ() != 0) continue; // to throw away short tubes // CHECK

    TVector3 min, max;
    min = cenposition - wiredirection2;
    max = cenposition + wiredirection2;
    
    // first extremity
    Double_t x_1= min.X(); 
    Double_t y_1= min.Y(); 
    Double_t z_1= min.Z(); 
    
    // second extremity
    Double_t x_2= max.X();
    Double_t y_2= max.Y();
    Double_t z_2= max.Z();
    
    Double_t rcur = pMhit->GetIsochrone();

    if(rootoutput) {
      eventCanvas->cd();
      TArc *drftarc = new TArc(cenposition.X(), cenposition.Y(), rcur); 
      drftarc->SetLineColor(5);
      drftarc->Draw("SAME");
    }

    if(rootoutput  && wiredirection != TVector3(0.,0.,1.)) {
      eventCanvas4->cd();

      // first wire extremity
      TMarker *pt1z = new TMarker(x_1, z_1, 6); 
      pt1z->SetMarkerColor(6);
      pt1z->Draw("SAME");
      // last wire extremity
      TMarker *pt2z = new TMarker(x_2, z_2, 6);
      pt2z->SetMarkerColor(6);
      pt2z->Draw("SAME");
      // tube line
      TLine *ztubeline = new TLine(x_1, z_1, x_2, z_2); // wire position
      ztubeline->Draw("SAME");
      eventCanvas5->cd();

      // first wire extremity
      TMarker *pt1z2 = new TMarker(y_1, z_1, 6);
      pt1z2->SetMarkerColor(6);
      pt1z2->Draw("SAME");
      // last wire extremity
      TMarker *pt2z2 = new TMarker(y_2, z_2, 6);
      pt2z2->SetMarkerColor(6);
      pt2z2->Draw("SAME");
      // tube line
      TLine *ztubeline2 = new TLine(y_1, z_1, y_2, z_2); // wire position
      ztubeline2->SetLineColor(wireOk);
      ztubeline2->Draw("SAME");
    }
  
    Double_t x1 = -9999.;
    Double_t y1 = -9999.;
    Double_t x2 = -9999.;
    Double_t y2 = -9999.;

    // from xy plane fit
    Double_t phi0 = pTrack->GetParamLast()->GetY();
    Double_t d0 = pTrack->GetParamLast()->GetX();
    Double_t x0 = d0*TMath::Cos(phi0);
    Double_t y0 = d0*TMath::Sin(phi0);
    // in xy plabe: angle of the PCA to the origin
    // with respect to the curvature center
    Double_t Phi0 = TMath::ATan2((y0 - y_0),(x0 - x_0));

    if(wiredirection != TVector3(0.,0.,1.)) 
      {
	wireOk++;
	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;
	  }
	}
	else {
	  cout << "-E- intersection point not found" << endl;
	  continue;
	}
	
	if(rootoutput) {
	  eventCanvas->cd();
	  TMarker *pt1 = new TMarker(x_1, y_1, 6);
	  pt1->SetMarkerColor(6);
	  //  pt1->Draw("SAME");
	  TMarker *pt2 = new TMarker(x_2, y_2, 6);
	  pt2->SetMarkerColor(6);
	  //  pt2->Draw("SAME");
	  //	  TLine *tubeline = new TLine(x_1, y_1, x_2, y_2);
	  //  tubeline->Draw("SAME");

	  // intersection lines
	  TLine *intline = new TLine(-50, -50*a + b, 50, 50*a + b);
	  intline->SetLineColor(5);
	  intline->Draw("SAME"); 
	}
	
	//x1 and x2 are the 2 intersection points
	Double_t d1=TMath::Sqrt((x1-cenposition.X())*(x1-cenposition.X())+
				(y1-cenposition.Y())*(y1-cenposition.Y()));
	Double_t d2=TMath::Sqrt((x2-cenposition.X())*(x2-cenposition.X())+
				(y2-cenposition.Y())*(y2-cenposition.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;}    
	
	if(rootoutput) {
	  eventCanvas->cd();
	  TMarker *pt = new TMarker(x_, y_, 6);
	  pt->SetMarkerColor(6);
	  pt->Draw("SAME");
	}
    
	// 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;
	}
 	else if((B*B-A*C)==0){          // CHECK forse da scommentare
 	  x1= B/A;
	  x2 = x1;
	  y1=a*x1+b;
 	  y2=a*x2+b;
 	}
 	else {
 	  cout << "NO WAY2" << endl;
 	  continue;
 	}	

	if(rootoutput) {
	  eventCanvas->cd();
	  TArc *drftarc = new TArc(x1, y1, rcur); 
	  drftarc->SetLineColor(3);
	  drftarc->Draw("SAME");
	  TArc *drftarc2 = new TArc(x2, y2, rcur); 
	  drftarc2->SetLineColor(3);
	  drftarc2->Draw("SAME");
	}

	d1=TMath::Sqrt((x1-cenposition.X())*(x1-cenposition.X())+(y1-cenposition.Y())*(y1-cenposition.Y()));
	d2=TMath::Sqrt((x2-cenposition.X())*(x2-cenposition.X())+(y2-cenposition.Y())*(y2-cenposition.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 kFALSE;
	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 


	//	tofit = new TVector3(xcen0,ycen0,z_);
	tofit = new TVector3(x_,y_,z_);
	ZPointsArray->Add(tofit);
	//	tofit2 = new TVector3(xcen1,ycen1,z_bis);
	tofit2 = new TVector3(x_,y_,z_bis);
	ZPointsArray->Add(tofit2);
   
	if(rootoutput){
	  eventCanvas4->cd();

	  // xz plane
	  // found point 1
	  TMarker *mrkt2 = new TMarker(x_, z_, 6);
	  mrkt2->SetMarkerColor(wireOk);
	  mrkt2->Draw("SAME");
	  // found point2
	  TMarker *mrkt2bis = new TMarker(x_, z_bis, 6);
	  mrkt2bis->SetMarkerColor(wireOk);
	  mrkt2bis->Draw("SAME");
	  // MC point
	  TMarker *mcmrkt2bis = new TMarker(iPoint->GetXtot(), iPoint->GetZtot(), 6);
	  mcmrkt2bis->SetMarkerColor(4);
	  mcmrkt2bis->Draw("SAME");	 

	  eventCanvas5->cd();
	  // yz plane
	  // found point 1
	  TMarker *mrkt2b = new TMarker(y_, z_, 6);
	  mrkt2b->SetMarkerColor(wireOk);
	  mrkt2b->Draw("SAME");
	  // found point 2 
	  TMarker *mrkt2bbis = new TMarker(y_, z_bis, 6);
	  mrkt2bbis->SetMarkerColor(wireOk);
	  mrkt2bbis->Draw("SAME");
	  // MC point
	  TMarker *mcmrkt2bbis = new TMarker(iPoint->GetYtot(), iPoint->GetZtot(), 6);
	  mcmrkt2bbis->SetMarkerColor(4);
	  mcmrkt2bbis->Draw("SAME");

	  eventCanvas2->cd();
	  // MC point in z - track length plane
	  TMarker *MCmrk = new TMarker(h* R*TMath::ATan2((iPoint->GetYtot() - y0)*TMath::Cos(Phi0) - (iPoint->GetXtot() - x0)*TMath::Sin(Phi0) , R + (iPoint->GetXtot() - x0) * TMath::Cos(Phi0) + (iPoint->GetYtot()-y0) * TMath::Sin(Phi0)), iPoint->GetZtot(), 6);
	  MCmrk->SetMarkerColor(4);
	  MCmrk->Draw("SAME");

	}
	
 	// ------- HOUGH TRANSFORM ------------
	// FIRST CHOICE
	if(tofit) Hough(tofit, Phi0, x0, y0, R);
	
	// SECOND CHOICE
	if(tofit2) Hough(tofit2, Phi0, x0, y0, R);

      }
    
  }

  //  cout << "skewed: " << wireOk << endl;


  TVector3 outz = GetHoughResponse();

  if(rootoutput) {  
    eventCanvas2->cd();

    // found line in z track length plane
    TLine *line = new TLine(-40, -40*outz.X() + outz.Y(), 40, (40*outz.X() + outz.Y()));
    line->Draw("SAME");
  }
  
  pTrack->GetParamFirst()->SetX(outz.Z());
  
  // if votes < 6 consider the fit failed CHECK
  //  if(outz.Z() < 6) return kFALSE;
  
  Int_t counter = 0;
  
  if(outz.X() == 0. && outz.Y() == 0.) return kFALSE;
  
  if( wireOk < 2){
    return kFALSE;
  }

  first = kTRUE;
  wireOk = 0;
  Int_t okcounter = 0;
  for(Int_t i = 0; i < (2*hitcounter); i+=2) {
    //  for(Int_t i = 0; i < hitcounter; i+=2) {   // CHECK Controlla
    
    // get index of hit
    Int_t iHit = pTrack->GetHitIndex(counter);
    counter++;
    
    // get hit
    CbmSttHit *pMhit = (CbmSttHit*) fHitArray->At(iHit);
     
    Int_t refindex = pMhit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());

  
    if(wiredirection == TVector3(0.,0.,1.)) continue;
    wireOk++;
   
    Double_t sigz = 1.; // CHECK
    
    // FIRST CHOICE .................
    TVector3 *vi = (TVector3*) ZPointsArray->At(okcounter);
    
    if(vi == NULL) continue;
    Double_t scos = CalculateScosl(h, pTrack->GetParamLast()->GetX(), pTrack->GetParamLast()->GetY(), R, vi->X(), vi->Y());
       
    if(rootoutput) {
      eventCanvas2->cd();
      // first point z track lenght plane
      TMarker *mrk = new TMarker(scos, vi->Z(), 6);
      mrk->Draw("SAME");
    }
    
    Bool_t fitdone = kFALSE;

    // distance between the found z and the predicted from the found line one
    Double_t distfirst = pow(((vi->Z() - (outz.Y() + outz.X() * scos))/sigz), 2);
    if(distfirst < 10) {
      pMhit->SetXint(vi->X());
      pMhit->SetYint(vi->Y());
      pMhit->SetZint(vi->Z());
      fitdone = kTRUE;
    }


    // SECOND CHOICE ...................
    vi = (TVector3*) ZPointsArray->At(okcounter+1); 
    if(vi == NULL) continue;
    scos = CalculateScosl(h, pTrack->GetParamLast()->GetX(), pTrack->GetParamLast()->GetY(), R, vi->X(), vi->Y());

    Double_t distsecond = pow(((vi->Z() - (outz.Y() + outz.X() * scos))/sigz), 2);
    
    if(rootoutput) {
      eventCanvas2->cd();
      TMarker *mrk2 = new TMarker(scos, vi->Z(), 6);
      mrk2->Draw("SAME");
    }

    // Xint Yin Zint set
    if(fitdone == kTRUE){
      if(distsecond < 10 && distsecond < distfirst) {
	pMhit->SetXint(vi->X());
	pMhit->SetYint(vi->Y());
	pMhit->SetZint(vi->Z());
      }
    }
    else {
      if (distsecond < 10) {
	pMhit->SetXint(vi->X());
	pMhit->SetYint(vi->Y());
	pMhit->SetZint(vi->Z());
	fitdone = kTRUE;
      }
      else {
	pMhit->SetXint(-999);
	pMhit->SetYint(-999);
	pMhit->SetZint(-999);
      }
    }

    if(rootoutput) {
      if(pMhit->GetZint()!= -999) {
	eventCanvas2->cd();
	TMarker *mrk3 = new TMarker(scos, pMhit->GetZint(), 6);
	mrk3->SetMarkerColor(2);    
	mrk3->Draw("SAME");
      }
    }
   
    okcounter = okcounter + 2;
  }
  
  return kTRUE;
  //   fiterrp = sqrt(fiterrp2);
  //   fiterrm = sqrt(fiterrm2);
}

// -------- ZFinder5 --------------------------------------------
Bool_t CbmSttHelixTrackFitter::ZFinder6b(CbmSttTrack* pTrack, Int_t pidHypo) {
  cout << "ZFINDER6b" << endl;

  if(!pTrack) return kFALSE;

  Int_t hitcounter = pTrack->GetNofHits();
 
  // cut on number of hits
  if(hitcounter > 50) {
    //    cout << "more than 50 hits" << endl;
    //  pTrack->GetParamLast()->SetX(-999);                
    //     pTrack->GetParamLast()->SetY(-999);
    //     //    Double_t newZ = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetZ(newZ);                
    //     pTrack->GetParamLast()->SetTx(-999);
    //     //    Double_t newTheta = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetTy(newTheta);                
    //     pTrack->GetParamLast()->SetQp(0.);
    cout << "TOO MANY HITS" << endl;
    return kFALSE;
  }
  if(hitcounter < 5)  {
    //    cout << "less than 5 hits" << endl;
    //     pTrack->GetParamLast()->SetX(-999);                
    //     pTrack->GetParamLast()->SetY(-999);
    //     //    Double_t newZ = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetZ(newZ);                
    //     pTrack->GetParamLast()->SetTx(-999);
    //     //    Double_t newTheta = -999.; // CHECK da cambiare
    //     //    pTrack->GetParamLast()->SetTy(newTheta);                
    //     pTrack->GetParamLast()->SetQp(0.);
    cout << "TOO FEW HITS" << endl;
    return kFALSE;
  }
  
  ZPointsArray = new TObjArray(2*hitcounter);

  // ZFIT =======
  if(hitcounter == 0) {
    cout << "HITCOUNTER = 0" << endl;
    return kFALSE;
  }
  
  Double_t Sxx, Sx, Sz, Sxz, S1z;
  Double_t Detz = 0.;
  Double_t fitm, fitp;
  Double_t sigz = 1.;  // CHECK
  
  Sx = 0.;
  Sz = 0.;
  Sxx = 0.;
  Sxz = 0.;
  S1z = 0.;
  // ============

  TVector3 *tofit, *tofit2;
 
  // centre of curvature
  Double_t x_0 = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * cos(pTrack->GetParamLast()->GetY());
  Double_t y_0 = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * sin(pTrack->GetParamLast()->GetY());
  // radius of curvature
  Double_t R = pTrack->GetParamLast()->GetTx();
  Int_t wireOk = 0;
  Bool_t first = kTRUE;

  for (Int_t i = 0; i < hitcounter; i++) {
   
    // get index of hit
    Int_t iHit = pTrack->GetHitIndex(i);

    // get hit
    CbmSttHit *pMhit = (CbmSttHit*) fHitArray->At(iHit);

    if(pMhit == NULL) continue;

    Int_t refindex = pMhit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    //   TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());
 
    TVector3 wiredirection = pMhit->GetWireDirection();
    
   
    if(wiredirection == TVector3(0.,0.,1.)) continue;
  
    TVector3 wiredirection2;
    
    wiredirection2 = 75. * wiredirection;
  
    TVector3 cenposition(pMhit->GetX(), pMhit->GetY(), pMhit->GetZ());  // CHECK! z = 35!!
    //       if(pMhit->GetZ() != 0) continue; // for the moment I throw away short tubes // CHECK

    TVector3 min, max;
    min = cenposition - wiredirection2;
    max = cenposition + wiredirection2;
    
    // first extremity
    Double_t x_1= min.X(); 
    Double_t y_1= min.Y(); 
    Double_t z_1= min.Z(); 
    
    // second extremity
    Double_t x_2= max.X();
    Double_t y_2= max.Y();
    Double_t z_2= max.Z();
    
    Double_t rcur = pMhit->GetIsochrone();

    if(rootoutput && wiredirection != TVector3(0.,0.,1.)) {
      eventCanvas4->cd();
      TMarker *pt1z = new TMarker(x_1, z_1, 6);
      pt1z->SetMarkerColor(6);
      pt1z->Draw("SAME");
      TMarker *pt2z = new TMarker(x_2, z_2, 6);
      pt2z->SetMarkerColor(6);
      pt2z->Draw("SAME");
      TLine *ztubeline = new TLine(x_1, z_1, x_2, z_2);
      ztubeline->SetLineColor(wireOk);
      ztubeline->Draw("SAME");

      eventCanvas5->cd();
      TMarker *pt1z2 = new TMarker(y_1, z_1, 6);
      pt1z2->SetMarkerColor(6);
      pt1z2->Draw("SAME");
      TMarker *pt2z2 = new TMarker(y_2, z_2, 6);
      pt2z2->SetMarkerColor(6);
      pt2z2->Draw("SAME");
      TLine *ztubeline2 = new TLine(y_1, z_1, y_2, z_2);
      ztubeline2->SetLineColor(wireOk);
      ztubeline2->Draw("SAME");

      //     // mio // calcolato con checkWire.C da cancellare ....
      //       Double_t mio1[3] = {33.6418, 4.31172, 15.9381};
      //       Double_t mio2[3] = {32.8066, 5.75846, -15.9381};
      //       eventCanvas5->cd();
      //       TLine *miotubeline = new TLine(mio1[1], mio1[2], mio2[1], mio2[2]); // wire position
      //       miotubeline->SetLineColor(3);
      //       miotubeline->Draw("SAME");
      //       eventCanvas4->cd();
      //       TLine *miotubeline2 = new TLine(mio1[0], mio1[2], mio2[0], mio2[2]); // wire position
      //       miotubeline2->SetLineColor(3);
      //       miotubeline2->Draw("SAME");
      //       //....................................................
      
    }


    if(rootoutput) {
      eventCanvas->cd();
      TArc *drftarc = new TArc(cenposition.X(), cenposition.Y(), rcur); 
      drftarc->SetLineColor(5);
      drftarc->Draw("SAME");
    }

    Double_t x1 = -9999.;
    Double_t y1 = -9999.;
    Double_t x2 = -9999.;
    Double_t y2 = -9999.;

    if(wiredirection != TVector3(0.,0.,1.)) 
      {
	wireOk++;
	Double_t a = -999;
	Double_t b = -999;
      
	// intersection point between the reconstructed 
	// circumference and the line of the projected tube
	if(fabs(x_2-x_1) != 0) {              // OK
	  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) {             // OK
	  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;
	  }
	}
	else {
	  cout << "NO WAY1" << endl;
	  continue;
	}
	
	if(rootoutput) {
	  eventCanvas->cd();
	  TMarker *pt1 = new TMarker(x_1, y_1, 6);
	  pt1->SetMarkerColor(6);
	  //  pt1->Draw("SAME");
	  TMarker *pt2 = new TMarker(x_2, y_2, 6);
	  pt2->SetMarkerColor(6);
	  //  pt2->Draw("SAME");
	  //	  TLine *tubeline = new TLine(x_1, y_1, x_2, y_2);
	  //  tubeline->Draw("SAME");
	  TLine *intline = new TLine(-50, -50*a + b, 50, 50*a + b);
	  intline->SetLineColor(5);
	  intline->Draw("SAME"); 
	}
	
	// 	cout << "ex: " << x_1 << " " << y_1 << " " << x_2 << " " << y_2 << endl;
	// 	cout << "ce: " << cenposition.X() << " " << cenposition.Y() << endl;

	//x1 and x2 are the 2 intersection points
	Double_t d1=TMath::Sqrt((x1-cenposition.X())*(x1-cenposition.X())+
				(y1-cenposition.Y())*(y1-cenposition.Y()));
	Double_t d2=TMath::Sqrt((x2-cenposition.X())*(x2-cenposition.X())+
				(y2-cenposition.Y())*(y2-cenposition.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;}    

	if(rootoutput) {
	  eventCanvas->cd();
	  TMarker *pt = new TMarker(x_, y_, 6);
	  pt->SetMarkerColor(6);
	  pt->Draw("SAME");
	}
	
	//	cout << "x_: " << x_ << " " << y_ << endl;


	// 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.
	
	x1 = -999.;
	x2 = -999.;
	y1 = -999.;
	y2 = -999.;

	//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;

	//	cout << "A: " << A << " B: " << B << " C: " << C << endl;

	if((B*B-A*C)>0) {            // OK
	  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((B*B-A*C)==0){
	  x1= B/A;
	  x2 = x1;
	  y1=a*x1+b;
	  y2=a*x2+b;
	}
	else {
	  cout << "NO WAY2" << endl;
	  continue;
	}
	

	//	cout << "tg: " << x1 << " " << y1 << " " << x2 << " " << y2 << endl;
	
	if(rootoutput) {
	  eventCanvas->cd();
	  TArc *drftarc = new TArc(x1, y1, rcur); 
	  drftarc->SetLineColor(3);
	  drftarc->Draw("SAME");
	  TArc *drftarc2 = new TArc(x2, y2, rcur); 
	  drftarc2->SetLineColor(3);
	  drftarc2->Draw("SAME");
	}

	d1=TMath::Sqrt((x1-cenposition.X())*(x1-cenposition.X())+(y1-cenposition.Y())*(y1-cenposition.Y()));
	d2=TMath::Sqrt((x2-cenposition.X())*(x2-cenposition.X())+(y2-cenposition.Y())*(y2-cenposition.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.) return kFALSE;
	// 	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 
	
	//----------------------------------------
	Double_t t1, t2, p1, p2;
	Double_t zcen0, zcen1;
	if((x_2-x_1) != 0.) {
	  t1 = (max.Z() - min.Z())/(max.X() - min.X());
	  p1 = min.Z() - t1 * min.X();
	  zcen0 = t1 * xcen0 + p1;
	  zcen1 = t1 * xcen1 + p1;
	}
	else if((y_2-y_1) != 0.) {
	  t2 = (max.Z() - min.Z())/(max.Y() - min.Y());
	  p2 = min.Z() - t2 * min.Y();
	  zcen0 = t2 * ycen0 + p2;
	  zcen1 = t2 * ycen1 + p2;
	}
	else {
	  cout << "HELP" << endl;
	  continue;
	}
	
	Double_t z_  = zcen0;
	Double_t z_bis = zcen1;
	//----------------------------------------



	//	tofit = new TVector3(xcen0,ycen0,z_);
	tofit = new TVector3(x_,y_,z_);
	ZPointsArray->Add(tofit);
	//	tofit2 = new TVector3(xcen1,ycen1,z_bis);
	tofit2 = new TVector3(x_,y_,z_bis);
	ZPointsArray->Add(tofit2);
   
	if(rootoutput){
	  eventCanvas4->cd();
	  TMarker *mrkt2 = new TMarker(x_, z_, 6);
	  mrkt2->SetMarkerColor(wireOk-1);
	  mrkt2->Draw("SAME");
	  TMarker *mrkt2bis = new TMarker(x_, z_bis, 6);
	  mrkt2bis->SetMarkerColor(wireOk-1);
	  mrkt2bis->Draw("SAME");

	  // MC point
	  TMarker *mcmrkt2bis = new TMarker(iPoint->GetXtot(), iPoint->GetZtot(), 6);
	  mcmrkt2bis->SetMarkerColor(4);
	  mcmrkt2bis->Draw("SAME");	 
	  
	  eventCanvas5->cd();
	  TMarker *mrkt2b = new TMarker(y_, z_, 6);
	  mrkt2b->SetMarkerColor(wireOk-1);
	  mrkt2b->Draw("SAME");
	  TMarker *mrkt2bbis = new TMarker(y_, z_bis, 6);
	  mrkt2bbis->SetMarkerColor(wireOk-1);
	  mrkt2bbis->Draw("SAME");
    		  
	  // MC point
	  TMarker *mcmrkt2bbis = new TMarker(iPoint->GetYtot(), iPoint->GetZtot(), 6);
	  mcmrkt2bbis->SetMarkerColor(4);
	  mcmrkt2bbis->Draw("SAME");

 	  cout << "WIRE: " << cenposition.X() << " " << cenposition.Y() << " " << cenposition.Z() << endl;
	  cout << "WIREDIR: " << wiredirection.X() << " " << wiredirection.Y() << " " << wiredirection.Z() << endl;
	  // 	  cout << "WIRE1: " << x_1 << " " << y_1 << " " << z_1 << endl;
	  // 	  cout << "WIRE2: " << x_2 << " " << y_2 << " " << z_2 << endl;
	  // 	  cout << "POINT: " << iPoint->GetXtot() << " " << iPoint->GetYtot() << " " << iPoint->GetZtot() << endl;
	  // 	  cout << "TOFIT1: " << x_ << " " << y_ << " " << z_ << endl;
	  // 	  cout << "TOFIT2: " << x_ << " " << y_ << " " << z_bis << endl;

	}



	// FIRST CHOICE
	//====================  
	// scosl
	TVector3 vi(tofit->X(), tofit->Y(), tofit->Z());

	Double_t scos = CalculateScosl(h, pTrack->GetParamLast()->GetX(), pTrack->GetParamLast()->GetY(), R, vi.X(), vi.Y());

	//====================  
	if(rootoutput) {
	  eventCanvas2->cd();
	  TMarker *mrk = new TMarker(scos, tofit->Z(), 6);
	  mrk->Draw("SAME");
	}
	
	//	cout << "1: " << scos << " " << tofit->Z() << endl;
	if(pMhit->GetIsochrone() != 0.) sigz = pMhit->GetIsochrone()*10;   // CHECK DA CAMBIARE
	else continue;
	//	else sigz = 0.1;
	//====================  
	// zfit
	//
	if(tofit->Z() >= -75 || tofit->Z() <= 75 ){
	  Sx = Sx + (scos/(sigz * sigz));
	  Sz = Sz + (tofit->Z()/(sigz * sigz));
	  Sxz = Sxz + ((scos * tofit->Z())/(sigz * sigz));
	  Sxx = Sxx + ((scos * scos)/(sigz * sigz));
	  S1z = S1z + 1/(sigz * sigz);
	}
	//====================

	// SECOND CHOICE
	//====================  
	// scosl
	vi.SetXYZ(tofit2->X(), tofit2->Y(), tofit2->Z());
	scos = CalculateScosl(h, pTrack->GetParamLast()->GetX(), pTrack->GetParamLast()->GetY(), R, vi.X(), vi.Y());

	//====================  

	//		cout << "2: " << scos << " " << tofit2->Z() << endl;

	//====================  
	// zfit
	//
	if(tofit2->Z() >= -75 || tofit2->Z() <= 75 ){
	  Sx = Sx + (scos/(sigz * sigz));
	  Sz = Sz + (tofit2->Z()/(sigz * sigz));
	  Sxz = Sxz + ((scos * tofit2->Z())/(sigz * sigz));
	  Sxx = Sxx + ((scos * scos)/(sigz * sigz));
	  S1z = S1z + 1/(sigz * sigz);
	}
	//====================
	if(rootoutput) {     
	  eventCanvas2->cd();
	  TMarker *mrk2 = new TMarker(scos, tofit2->Z(), 6);
	  mrk2->Draw("SAME");
	}

	if(rootoutput) {     
	  eventCanvas2->cd();
	  TMarker *mcmrk2 = new TMarker(scos, iPoint->GetZtot(), 6);
	  mcmrk2->SetMarkerColor(4);
	  mcmrk2->Draw("SAME");
	}

	//	cout << "M: " << scos << " " << iPoint->GetZtot() << endl;

      }
    
  }
  //  cout << "skewed: " << wireOk << endl;
  if(wireOk < 2) return kFALSE;
 
  Detz = S1z*Sxx - Sx*Sx;
  if(Detz == 0) {
    cout << "DET 0" << endl;
    return kFALSE;
  }

  fitp = (1/Detz)*(Sxx*Sz - Sx*Sxz);
  fitm = (1/Detz)*(S1z*Sxz - Sx*Sz);
  //   fiterrp2 = (1/Detz)*Sxx;
  //   fiterrm2 = (1/Detz)*S1z;

  if(rootoutput) {  
    eventCanvas2->cd();
    TLine *line = new TLine(-40, -40*fitm + fitp, 40, (40*fitm + fitp));
    line->Draw("SAME");
  }
 
 
  //  Double_t chi2z;
  //  chi2z = 0.;
  //  for(Int_t i=0; i < hitcounter; 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);
  Int_t counter = 0;

  if(fitm == 0. && fitp == 0.) {
    cout << "FITM || FITP" << endl;
    return kFALSE;
  }
 
  if( wireOk < 2) {
    cout << "WIREOK < 2" << endl;
    return kFALSE;
  }
  first = kTRUE;

  wireOk = 0;
  Int_t okcounter = 0;
  for(Int_t i = 0; i < (2*hitcounter); i+=2) {
    // get index of hit
    //   cout << i << " counter: " << counter << endl;
    Int_t iHit = pTrack->GetHitIndex(counter);
    counter++;
   
    // get hit
    CbmSttHit *pMhit = (CbmSttHit*) fHitArray->At(iHit);
  
    Int_t refindex = pMhit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());
    
    if(wiredirection == TVector3(0.,0.,1.)) continue;
    wireOk++;
   
    Double_t sigz = 1.; // CHECK
   
    //  Double_t x0;
    //     Double_t y0;
    //     if(wireOk==1) {
    //       TVector3 *vi = (TVector3*) ZPointsArray->At(okcounter);
    //       // phi0
    //       v0->SetXYZ(vi->X(), vi->Y(), vi->Z());
    //       Phi0 = TMath::ATan2((v0->Y() - y_0),(v0->X() - x_0));
     
    //       // CHECK
    //       // we are using the first digi for the arc length calculation
    //       // but this may be wrong
    //       x0 = v0->X();
    //       y0 = v0->Y();
    //     }
   
    // FIRST CHOICE
    TVector3 *vi = (TVector3*) ZPointsArray->At(okcounter);
 
    if(vi == NULL) continue;
    Double_t scos = CalculateScosl(h, pTrack->GetParamLast()->GetX(), pTrack->GetParamLast()->GetY(), R, vi->X(), vi->Y());
    Bool_t fitdone = kFALSE;
    Double_t distfirst = pow(((vi->Z() - (outz.Y() + outz.X() * scos))/sigz), 2);
    if(distfirst < 10) {

      pMhit->SetXint(vi->X());
      pMhit->SetYint(vi->Y());
      pMhit->SetZint(vi->Z());
      fitdone = kTRUE;
    }
    // SECOND CHOICE
    vi = (TVector3*) ZPointsArray->At(okcounter+1); 
    if(vi == NULL) continue;
    scos = CalculateScosl(h, pTrack->GetParamLast()->GetX(), pTrack->GetParamLast()->GetY(), R, vi->X(), vi->Y());
    Double_t distsecond = pow(((vi->Z() - (outz.Y() + outz.X() * scos))/sigz), 2);

    if(fitdone == kTRUE){
      if(distsecond < 10 && distsecond < distfirst) {
	pMhit->SetXint(vi->X());
	pMhit->SetYint(vi->Y());
	pMhit->SetZint(vi->Z());
      }
    }
    else {
      if (distsecond < 10) {
	pMhit->SetXint(vi->X());
	pMhit->SetYint(vi->Y());
	pMhit->SetZint(vi->Z());
	fitdone = kTRUE;
      }
      else {
	pMhit->SetXint(-999);
	pMhit->SetYint(-999);
	pMhit->SetZint(-999);
      }
    }

    if(rootoutput) {
      if(pMhit->GetZint()!= -999) {
	eventCanvas2->cd();
	TMarker *mrk3 = new TMarker(scos, pMhit->GetZint(), 6);
	mrk3->SetMarkerColor(2);    
	mrk3->Draw("SAME");
      }
    }
   
    okcounter = okcounter + 2;
  }
 
  cout << "fitm: " << fitm << " fitp: " << fitp << endl;

  return kTRUE;
  //   fiterrp = sqrt(fiterrp2);
  //   fiterrm = sqrt(fiterrm2);
 
} 




// ----- Zfit  ----------------------------------------
Int_t CbmSttHelixTrackFitter::Zfit(CbmSttTrack* pTrack, Int_t pidHypo) {

  // fEventCounter++;

  if(!pTrack) return 0;
  
  //  TVector2 *position = NULL;
  Int_t hitcounter = pTrack->GetNofHits();
 
  // cut on number of hits
  //  if(hitcounter > 30) return 0;
  if(hitcounter < 5) return 0;
  
  // CHECK: ATTENTION
  // refit for error correction missing
  
  
  // SCOSL ======
  // get 1st hit
  CbmSttHit *fMhit = (CbmSttHit*) fHitArray->At(pTrack->GetHitIndex(0));
  // phi0
  TVector3 * v0;
  Double_t Phi0;
  // ============


  Double_t Sxx, Sx, Sz, Sxz, S1z;
  Double_t Detz = 0.;
  Double_t fitm, fitp;
  Double_t sigz = 1.;  // CHECK
  
  Sx = 0.;
  Sz = 0.;
  Sxx = 0.;
  Sxz = 0.;
  S1z = 0.;
  
  // centre of curvature
  Double_t x_0 = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * cos(pTrack->GetParamLast()->GetY());
  Double_t y_0 = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * sin(pTrack->GetParamLast()->GetY());
  // radius of curvature
  Double_t R  = pTrack->GetParamLast()->GetTx();
  Int_t counter = 0;
  Int_t wireOk = 0;
  for (Int_t i = 0; i < hitcounter; i++) {
   
    // get index of hit
    Int_t iHit = pTrack->GetHitIndex(i);

    // get hit
    CbmSttHit *pMhit = (CbmSttHit*) fHitArray->At(iHit);
    if ( ! pMhit ) continue;

    if(pMhit->GetXint() == -999 || pMhit->GetYint() == -999 || pMhit->GetZint() == -999) continue; // CHECK

    Int_t refindex = pMhit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());
    
    if(wiredirection == TVector3(0.,0.,1.)) continue;
    counter++;
    // SCOSL
    Double_t x0;
    Double_t y0;
    wireOk++;
    if(wireOk == 1) {
      TVector3 *vi = new TVector3(pMhit->GetXint(), pMhit->GetYint(), pMhit->GetZint());
      
      // phi0
      v0 = new TVector3(vi->X(), vi->Y(), vi->Z());
      Phi0 = TMath::ATan2((v0->Y() - y_0),(v0->X() - x_0));
      
      // CHECK
      // we are using the first digi for the arc length calculation
      // but this may be wrong
      x0 = v0->X();
      y0 = v0->Y();
    }
    
    TVector3 *vi = new TVector3(pMhit->GetXint(), pMhit->GetYint(), pMhit->GetZint());
    Double_t scos = R*TMath::ATan2((pMhit->GetYint() - y0)*TMath::Cos(Phi0) - (pMhit->GetXint() - x0)*TMath::Sin(Phi0) , R + (pMhit->GetXint() - x0) * TMath::Cos(Phi0) + (pMhit->GetYint()-y0) * TMath::Sin(Phi0));
   
    Sx = Sx + (scos /(sigz * sigz));
    Sz = Sz + (vi->Z()/(sigz * sigz));
    Sxz = Sxz + ((scos *vi->Z())/(sigz * sigz));
    Sxx = Sxx + ((scos * scos)/(sigz * sigz));
    S1z = S1z + 1/(sigz * sigz);
  }

  if(counter == 0) return 0;
    
  Detz = S1z*Sxx - Sx*Sx;
  if(Detz == 0) {
    return 0;
  }

  fitp = (1/Detz)*(Sxx*Sz - Sx*Sxz);
  fitm = (1/Detz)*(S1z*Sxz - Sx*Sz);

  //    cout <<  "fit: " << fitp << " " << fitm << " " << Detz << endl;


  //   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);
  
  pTrack->GetParamLast()->SetZ(fitp); // z (adesso fitp) CHECK
  // pTrack->GetParamLast()->SetTy((TMath::Pi()/2.) - TMath::ATan(fitm)); // theta
  pTrack->GetParamLast()->SetTy(fitm); // CHECK da cambiare con theta

  if(rootoutput) {
    eventCanvas2->cd();
    TLine *line2 = new TLine(-20, -20*fitm + fitp, 20, (20*fitm + fitp));
    line2->SetLineColor(3);
    line2->Draw("SAME");
  } 
  return 1;
  //   fiterrp = sqrt(fiterrp2);
  //   fiterrm = sqrt(fiterrm2);
}
// -------------------------------------------------------------------------------


// ----- Zfit2  ----------------------------------------
Int_t CbmSttHelixTrackFitter::Zfit2(CbmSttTrack* pTrack, Int_t pidHypo) {

  // fEventCounter++;
  if(!pTrack) return 0;
  
  //  TVector2 *position = NULL;
  Int_t hitcounter = pTrack->GetNofHits();
 
  // cut on number of hits
  //  if(hitcounter > 30) return 0;
  if(hitcounter < 5) return 0;
  
  // 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.);
  
  // SCOSL ======
  // get 1st hit
  CbmSttHit *fMhit = (CbmSttHit*) fHitArray->At(pTrack->GetHitIndex(0));
  // phi0
  TVector3 * v0;
  Double_t Phi0;
  // ============


  Double_t Sxx, Sx, Sz, Sxz, S1z;
  Double_t Detz = 0.;
  Double_t fitm, fitp;
  Double_t sigz = 1.;  // CHECK
  
  Sx = 0.;
  Sz = 0.;
  Sxx = 0.;
  Sxz = 0.;
  S1z = 0.;
  
  // centre of curvature
  Double_t x_0 = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * cos(pTrack->GetParamLast()->GetY());
  Double_t y_0 = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * sin(pTrack->GetParamLast()->GetY());
  // radius of curvature
  Double_t R  = pTrack->GetParamLast()->GetTx();
  Int_t counter = 0;
  Int_t wireOk = 0;
  for (Int_t i = 0; i < hitcounter; i++) {
   
    // get index of hit
    Int_t iHit = pTrack->GetHitIndex(i);

    // get hit
    CbmSttHit *pMhit = (CbmSttHit*) fHitArray->At(iHit);
    if ( ! pMhit ) continue;

    if(pMhit->GetXint() == -999 || pMhit->GetYint() == -999 || pMhit->GetZint() == -999) continue; // CHECK

    Int_t refindex = pMhit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());
    
    if(wiredirection == TVector3(0.,0.,1.)) continue;
    counter++;
    // SCOSL
    Double_t x0;
    Double_t y0;
    wireOk++;
    if(wireOk == 1) {
      TVector3 *vi = new TVector3(pMhit->GetXint(), pMhit->GetYint(), pMhit->GetZint());
      
      // phi0
      v0 = new TVector3(vi->X(), vi->Y(), vi->Z());
      Phi0 = TMath::ATan2((v0->Y() - y_0),(v0->X() - x_0));
      
      // CHECK
      // we are using the first digi for the arc length calculation
      // but this may be wrong
      x0 = v0->X();
      y0 = v0->Y();
    }
    
    TVector3 *vi = new TVector3(pMhit->GetXint(), pMhit->GetYint(), pMhit->GetZint());
    //    Double_t scos = R*TMath::ATan2((pMhit->GetYint() - y0)*TMath::Cos(Phi0) - (pMhit->GetXint() - x0)*TMath::Sin(Phi0) , R + (pMhit->GetXint() - x0) * TMath::Cos(Phi0) + (pMhit->GetYint()-y0) * TMath::Sin(Phi0));
    Double_t scos = R*(TMath::ATan2(R*TMath::Sin(Phi0) - (pMhit->GetYint() - y0), R * TMath::Cos(Phi0) - (pMhit->GetXint() - x0)) - Phi0);

    Sx = Sx + (scos /(sigz * sigz));
    Sz = Sz + (vi->Z()/(sigz * sigz));
    Sxz = Sxz + ((scos *vi->Z())/(sigz * sigz));
    Sxx = Sxx + ((scos * scos)/(sigz * sigz));
    S1z = S1z + 1/(sigz * sigz);
  }

  if(counter == 0) return 0;
    
  Detz = S1z*Sxx - Sx*Sx;
  if(Detz == 0) {
    return 0;
  }

  fitp = (1/Detz)*(Sxx*Sz - Sx*Sxz);
  fitm = (1/Detz)*(S1z*Sxz - Sx*Sz);

  //    cout <<  "fit: " << fitp << " " << fitm << " " << Detz << endl;


  //   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);
  
  pTrack->GetParamLast()->SetZ(fitp); // z (adesso fitp) CHECK
  // pTrack->GetParamLast()->SetTy((TMath::Pi()/2.) - TMath::ATan(fitm)); // theta
  pTrack->GetParamLast()->SetTy(fitm); // CHECK da cambiare con theta

  if(rootoutput) {
    eventCanvas2->cd();
    TLine *line2 = new TLine(-20, -20*fitm + fitp, 20, (20*fitm + fitp));
    line2->SetLineColor(3);
    line2->Draw("SAME");
  } 
  return 1;
  //   fiterrp = sqrt(fiterrp2);
  //   fiterrm = sqrt(fiterrm2);
}
// ----
 
// ----- Zfit  ----------------------------------------
Int_t CbmSttHelixTrackFitter::Zfitbb2(CbmSttTrack* pTrack, Int_t pidHypo) {

  cout << "ZFITbb2" << endl;

  if(!pTrack) return 0;
  
  Int_t hitcounter = pTrack->GetNofHits();
 
  // cut on number of hits
  if(hitcounter > 50) return 0;
  if(hitcounter < 5) return 0;
  
  // CHECK: ATTENTION
  // refit for error correction missing
  
  
  // SCOSL ======
  // get 1st hit
  CbmSttHit *fMhit = (CbmSttHit*) fHitArray->At(pTrack->GetHitIndex(0));

  Double_t Sxx, Sx, Sz, Sxz, S1z;
  Double_t Detz = 0.;
  Double_t fitm, fitp;
  Double_t sigz = 1.;  // CHECK
  
  Sx = 0.;
  Sz = 0.;
  Sxx = 0.;
  Sxz = 0.;
  S1z = 0.;
  
  // centre of curvature
  Double_t x_0 = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * cos(pTrack->GetParamLast()->GetY());
  Double_t y_0 = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * sin(pTrack->GetParamLast()->GetY());
  // radius of curvature
  Double_t R  = pTrack->GetParamLast()->GetTx();
  Int_t counter = 0;
  Int_t wireOk = 0;
  Bool_t first = kTRUE;
  for (Int_t i = 0; i < hitcounter; i++) {
   
    // get index of hit
    Int_t iHit = pTrack->GetHitIndex(i);

    // get hit
    CbmSttHit *pMhit = (CbmSttHit*) fHitArray->At(iHit);
    if ( ! pMhit ) continue;

    if(pMhit->GetXint() == -999 || pMhit->GetYint() == -999 || pMhit->GetZint() == -999) continue; // CHECK

    Int_t refindex = pMhit->GetRefIndex(); 
    // get point
    CbmSttPoint *iPoint = (CbmSttPoint*) fPointArray->At(refindex);
    TVector3 wiredirection(iPoint->GetXWireDirection(), iPoint->GetYWireDirection(), iPoint->GetZWireDirection());
    
    if(wiredirection == TVector3(0.,0.,1.)) continue;
    //    if(pMhit->GetZ() != 0) continue; // for the moment I throw away short tubes // CHECK

    counter++;
    wireOk++;
    
    TVector3 *vi = new TVector3(pMhit->GetXint(), pMhit->GetYint(), pMhit->GetZint());

    // if the found z is > 75 cm or < -75 cm continue: this has to be fixed
    if(pMhit->GetZint() < -75. || pMhit->GetZint() > 75.) continue; // CHECK 

    Double_t scos = CalculateScosl(h, pTrack->GetParamLast()->GetX(),  pTrack->GetParamLast()->GetY(), R, pMhit->GetXint(), pMhit->GetYint());
    
    Sx = Sx + (scos /(sigz * sigz));
    Sz = Sz + (vi->Z()/(sigz * sigz));
    Sxz = Sxz + ((scos *vi->Z())/(sigz * sigz));
    Sxx = Sxx + ((scos * scos)/(sigz * sigz));
    S1z = S1z + 1/(sigz * sigz);
  }

  if(counter == 0) return 0;
    
  Detz = S1z*Sxx - Sx*Sx;
  if(Detz == 0) {
    return 0;
  }

  fitp = (1/Detz)*(Sxx*Sz - Sx*Sxz);
  fitm = (1/Detz)*(S1z*Sxz - Sx*Sz);
    
  // y = m*x + p
  TVector2 outz(fitm, fitp);

  // fill in parameters
  pTrack->GetParamLast()->SetTy(fitm); // tan(lambda)
  pTrack->GetParamLast()->SetZ(fitp);  // z0

  if(rootoutput) {
    eventCanvas2->cd();

    // fit line
    TLine *line2 = new TLine(-40, -40*fitm + fitp, 40, (40*fitm + fitp));
    line2->SetLineColor(3);
    line2->Draw("SAME");
  } 
  return 1;
  //   fiterrp = sqrt(fiterrp2);
  //   fiterrm = sqrt(fiterrm2);
}
// -------------------------------------------------------------------------------

void CbmSttHelixTrackFitter::Hough(TVector3* choice, Double_t Phi0, Double_t x0, Double_t y0, Double_t R)
{

  // since the points stay on a line in z track length plane
  // I can use the hough transform to transform the points in
  // z track length plane to lines in the parameters plane ab
  // (scos = a * z + b) and find their intersections.
  //
  // grid:
  // a [-pi/2 , pi/2]; 200 steps
  // b [-150, 150];    1000 steps  // CHECK deve essere -75, 75

  Double_t y = choice->Z(); 
  Double_t x = h* R*TMath::ATan2((choice->Y() - y0)*TMath::Cos(Phi0) - (choice->X() - x0)*TMath::Sin(Phi0) , R + (choice->X() - x0) * TMath::Cos(Phi0) + (choice->Y()-y0) * TMath::Sin(Phi0));
 

  // Hough  -------------------------- angle [-90, 90]
  Double_t a = -TMath::Pi()/2.;
  Double_t tga;
  Double_t b = -150;
  Double_t ycalc;
  
  // 200 step per -pi/2 < a < pi/2
  for(Int_t i = 0; i < 200; i++) {
    b = -150;
    tga = TMath::Tan(a);
    // 1000 step per -150 < b < 150
    for(Int_t j = 0; j <= 1000; j++) {
      Double_t ycalc = x * tga + b;
    
      //  if(fabs(ycalc - y) < (0.3 + TMath::Tan(TMath::Pi()/200.) * x)) {
      if(fabs(ycalc - y) < 0.2) {
	vote[i][j] += 1;
      }

      b += 0.3;
    } 
    a += (TMath::Pi()/200.);
  }
}

TVector3 CbmSttHelixTrackFitter::GetHoughResponse()
{
  Double_t a = -TMath::Pi()/2.;
  Double_t tga;
  Double_t b = -150;
  TMarker *mrk2;
  
  Double_t aref, bref, vref = 0;
  Double_t aref2, bref2, vref2 = 0, cref2 = 0;
  for(Int_t i=0; i <= 200; i++)
    {
      tga = TMath::Tan(a);
      b = -150;
      for(Int_t j = 0; j <= 1000; j++) {
      	if(vote[i][j] != 0)
      	  { 
	    if(rootoutput) {
      	      hougCanvas->cd();
      	      mrk2 = new TMarker(tga, b, 6);
      	      mrk2->SetMarkerColor(3);
      	      mrk2->Draw("SAME");
      	    }
	    
	    if(vote[i][j] >= vref) {
	      aref = tga;
	      bref = b;
	      vref = vote[i][j];
	      
	      // 	      if(vote[i][j] > vref) {
	      // 		aref2 = tga;
	      // 		bref2 = b;
	      // 		vref2 = vote[i][j];
	      // 		cref2 = 1;
	      // 	      }
	      // 	      else if(vote[i][j] > vref) {
	      // 		aref2 += tga;
	      // 		bref2 += b;
	      // 		cref2++;
	      // 	      }
	    } 
	  } 
      	b += 0.3; 
            }
            a += (TMath::Pi()/200.);
          }
  //   if(vref < vref2) 
  // 	{
  // 	  aref = aref2/cref2;
  // 	  bref = bref2/cref2;
  //      vref = vref2;
  //    }
  
  TVector3 hough(aref, bref, vref);
  
  // reset votes
  for(Int_t i=0; i <= 200; i++) for(Int_t j = 0; j <= 1000; j++) vote[i][j] = 0;
  return hough;

}


// ------ Extrapolate ------------------------------------------------------------
void CbmSttHelixTrackFitter::Extrapolate(CbmSttTrack* track, Double_t r, CbmTrackParam *param )
{
  cout << "-W- CbmSttMinuitTrackFitter::Extrapolate: Not yet implemented, sorry!"
       << endl;
}

// // -----   Private method AddHOT   -----------------------------------------
// CbmSttHOT* CbmSttHelixTrackFitter::AddHOT(Double_t x, Double_t y, Double_t z, Int_t hitindex, Int_t pointindex, Int_t trackindex)
// {
//   TClonesArray& 
//       clref = *fHotArray;

//   Int_t 
//       size = clref.GetEntriesFast();

//   //  cout << "filling HOT" << endl;


//   return new(clref[size]) CbmSttHOT(x, y, z, hitindex, pointindex, trackindex);

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

void CbmSttHelixTrackFitter::ResetMArray() {
  for(Int_t i = 0; i < 100; i++) marray.AddAt(-999, i);
}


Int_t CbmSttHelixTrackFitter::MinuitFit(CbmSttTrack* pTrack, Int_t pidHypo)
{    
  cout << "MINUIT FIT " << pTrack->GetNofHits() << endl;
 
  fEventCounter++;
  Double_t hitcounter = pTrack->GetNofHits();
     
  Double_t rstart = pTrack->GetParamLast()->GetTx();
  Double_t xcstart = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * cos(pTrack->GetParamLast()->GetY());
  Double_t ycstart = (pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * sin(pTrack->GetParamLast()->GetY());
  
  
  TMinuit minimizer(3);
  
  cout << "*******MINUIT********" << endl;
  cout << "D   SEED: " << pTrack->GetParamLast()->GetX()  << endl;
  cout << "PHI SEED: " << pTrack->GetParamLast()->GetY()  << endl;
  cout << "R   SEED: " << pTrack->GetParamLast()->GetTx() << endl;
  cout << "********************" << endl;



  minimizer.SetFCN(fcnHelix);
  //  minimizer.SetErrorDef(1);  // ???
  
  minimizer.DefineParameter(0, "xc", xcstart, 0.1, -3000., 3000.); // ???
  minimizer.DefineParameter(1, "yc", ycstart, 0.1, -3000., 3000.); // ??? LIMITS ???
  minimizer.DefineParameter(2, "r", rstart, 0.1, 0., 3000.);   // ???
  cout << "xcstart: " << xcstart << " ycxtart: " << ycstart << " rstart: " << rstart << endl;
  minimizer.SetObjectFit(this);
 
  minimizer.SetPrintLevel(-1); 

  minimizer.SetMaxIterations(500);

  minimizer.Migrad();

  Double_t chisquare, resultsRadial[3], errorsRadial[3]; 

  minimizer.GetParameter(0, resultsRadial[0], errorsRadial[0]);
  minimizer.GetParameter(1, resultsRadial[1], errorsRadial[1]);
  minimizer.GetParameter(2, resultsRadial[2], errorsRadial[2]);
  
  //  minimizer.Eval(3, NULL, chisquare, resultsRadial, 0); // ???
  
  cout << "xc: " << resultsRadial[0] << endl; 
  cout << "yc: " << resultsRadial[1] << endl; 
  cout << "R:  " << resultsRadial[2] << endl;
  
  Double_t phi = TMath::ATan2(resultsRadial[1], resultsRadial[0]); // CHECK
  Double_t d;
  d = ((resultsRadial[0] + resultsRadial[1]) - resultsRadial[2] *(TMath::Cos(phi) + TMath::Sin(phi)))/(TMath::Cos(phi) + TMath::Sin(phi)); // CHECK
  
  pTrack->SetChi2Rad(chisquare);
  pTrack->SetNDF(fTrack->GetNofHits());
  pTrack->GetParamLast()->SetX(d);                
  pTrack->GetParamLast()->SetY(phi);
  //   pTrack->GetParamLast()->SetZ(z0);                
  pTrack->GetParamLast()->SetTx(resultsRadial[2]);
  //   pTrack->GetParamLast()->SetTy(alpha);                
  pTrack->GetParamLast()->SetQp(0.);
  
  if(rootoutput) {
    eventCanvas->cd();
    TArc *fitarc = new TArc(((pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * cos(pTrack->GetParamLast()->GetY())), ((pTrack->GetParamLast()->GetX() + pTrack->GetParamLast()->GetTx()) * sin(pTrack->GetParamLast()->GetY())), pTrack->GetParamLast()->GetTx());
    fitarc->SetLineColor(3);
    fitarc->Draw("SAME");
    eventCanvas->Update();
    eventCanvas->Modified();
  }

  return 1;
}

void fcnHelix(Int_t &npar, Double_t *gin, Double_t &f, Double_t *par, Int_t iflag)
{

  const CbmSttHelixTrackFitter *mama = (CbmSttHelixTrackFitter *)gMinuit->GetObjectFit();

  CbmSttTrack *fTrack = mama->GetTrack(); 

  CbmSttHit *currenthit  = NULL;

  Double_t chisq = 0;
  Double_t delta = 0;
  Int_t hitcounter = fTrack->GetNofHits();

  Double_t xcur=0;
  Double_t ycur=0;

  for (Int_t i = 0; i < hitcounter; i++)
    { 

      // get index of hit
      Int_t iHit = fTrack->GetHitIndex(i);

      // get hit
      currenthit = mama->GetHitFromCollections(iHit);

      if(!currenthit) continue;

      if(currenthit->GetXint() == -999 || currenthit->GetYint() == -999) continue;
      TVector3 wiredirection = currenthit->GetWireDirection();

      if(wiredirection != TVector3(0.,0.,1.)) continue;

      xcur = currenthit->GetXint();
      ycur = currenthit->GetYint();
      // cout <<"MINUIT: " << currenthit->GetXint() << " " << currenthit->GetYint() << endl;
    

      delta =sqrt((xcur-par[0])*(xcur-par[0])+(ycur-par[1])*(ycur-par[1])) -par[2] ;
      if(currenthit->GetIsochrone() == 0) chisq += (delta * delta * 12.);
      else chisq += (delta*delta)/(currenthit->GetIsochrone() * currenthit->GetIsochrone() / 12.);
      
    }
 
  f = chisq;
}



CbmSttHit* CbmSttHelixTrackFitter::GetHitFromCollections(Int_t hitCounter) const
{
  CbmSttHit
    *retval = NULL;
 
  Int_t
    relativeCounter = hitCounter;

  for (Int_t collectionCounter = 0; collectionCounter < fHitCollectionList.GetEntries(); collectionCounter++)
    {
      Int_t
	size = ((TClonesArray *)fHitCollectionList.At(collectionCounter))->GetEntriesFast();

      if (relativeCounter < size)
	{
	  retval = (CbmSttHit*) ((TClonesArray *)fHitCollectionList.At(collectionCounter))->At(relativeCounter);
	  break;
	}
      else
	{
	  relativeCounter -= size;
	}
    }

  return retval;
}


Double_t CbmSttHelixTrackFitter::CalculateScosl(Double_t h, Double_t d0,  Double_t phi0, Double_t R, Double_t x, Double_t y)
{

  Double_t x_0 = (d0 + R) * TMath::Cos(phi0);
  Double_t y_0 = (d0 + R) * TMath::Sin(phi0);

  Double_t x0 = d0 * TMath::Cos(phi0);
  Double_t y0 = d0 * TMath::Sin(phi0);

  Double_t Phi0 = TMath::ATan2((y0 - y_0),(x0 - x_0));

  //  cout << "calculate Phi0: " << Phi0 << " v0: " << x0 << " " << y0 << endl;
  //  cout << "phi: " << phi0 << " Phi0: " << Phi0 << endl;

  Double_t scos = h * R * TMath::ATan2((y - y0) * TMath::Cos(Phi0) - (x - x0) * TMath::Sin(Phi0) , R + (x - x0) * TMath::Cos(Phi0) + (y - y0) * TMath::Sin(Phi0));

  return scos;

}

Double_t CbmSttHelixTrackFitter::GetHitAngle(Int_t hitNo, Double_t dCenter, 
					      Double_t phiCenter, Double_t radius)
{
    CbmSttHit
	*pMhit = NULL;

    pMhit = GetHitFromCollections(hitNo);
	
    Double_t
      xCenter = (dCenter + radius) * cos(phiCenter),
      yCenter = (dCenter + radius) * sin(phiCenter);
    
    // get XY of wire
    Double_t
      deltaX = pMhit->GetX() - xCenter,
      deltaY = pMhit->GetY() - yCenter;
    
    Double_t
      angle = atan(deltaY / deltaX);
    
    // bring angle into the usual frame of reference
    if (deltaX < 0.)
      {
	if (deltaY < 0.)
	  angle -= dPi;
	else
	  angle += dPi;
      }
    
    return angle;
}

void CbmSttHelixTrackFitter::OrderHitsByR(map<Double_t, Int_t> &hitMap)
{
  // sort the hits by r position
    CbmSttHit
      *pMhit = NULL;
  
    cout << "n of hits: " << fTrack->GetNofHits() << endl;
  
    for (Int_t i = 0; i < fTrack->GetNofHits(); i++)
      {
	// get index of hit
	Int_t
	  iHit = fTrack->GetHitIndex(i);
	
	// get hit
	pMhit = GetHitFromCollections(iHit);
	if ( ! pMhit ) 
	  continue;
	
	hitMap[sqrt(pMhit->GetX() * pMhit->GetX() + pMhit->GetY() * pMhit->GetY())] = iHit;
      }
}

Int_t CbmSttHelixTrackFitter::GetCharge(Double_t dCenter, Double_t phiCenter, Double_t radius)
{
  map<Double_t, Int_t>  hitMap;
  Int_t charge = 0;

  OrderHitsByR(hitMap);
  
  map<Double_t, Int_t>::iterator
    hitMapStart = hitMap.begin(),
    hitMapEnd = hitMap.end();
  
  hitMapEnd--;
  
  CbmSttHit
    *startHit = GetHitFromCollections(hitMapStart->second),
    *endHit = GetHitFromCollections(hitMapEnd->second);
  
  CbmSttGeomPoint
    vertex(0., 0., 0.),
    firstPoint(startHit->GetX(), startHit->GetY(), startHit->GetZ()), 
    lastPoint(endHit->GetX(), endHit->GetY(), endHit->GetZ());
  
  Bool_t
    retval = kTRUE;
  
  Double_t
    angle,
    oldAngle,
    difAngle;
  
    Int_t 
      votesForPositive = 0,
      votesForNegative = 0,
      hitNo = 0;
    
    map<Double_t, Int_t>::iterator
      hitMapIter = hitMap.begin();
    
    while (hitMapIter != hitMap.end())
      {
	// get hit's angle
	angle = GetHitAngle(hitMapIter->second, dCenter, phiCenter, radius);
	
	// store the first hit's angle as a reference (hit with smallest z coordinate)
	if (hitNo == 0)
	{
	    refAngle = angle;
	}

	// calculate the angle difference between the reference hit and this hit
	difAngle = angle - refAngle;

	while (difAngle < 0)
	{
	    difAngle += 2 * dPi;
	}

	// cast votes for criterium 1
	if (hitNo > 0)
	{
	    if (difAngle > oldAngle)
		votesForPositive++;
	    else
		votesForNegative++;
	}

	oldAngle = difAngle;

	hitNo++;
	hitMapIter++;
    }

    // 2/3 majority is enough to decide on track
    if (votesForPositive > 2 * votesForNegative)
    {

      // when swimming the track from -z to z we get a positive rotation
      if (firstPoint.DistanceTo(vertex) < lastPoint.DistanceTo(vertex))
	{
	  charge = -1;
	}
      else
	{
	  charge = 1;
	}
    }
    else if (votesForNegative > 2 * votesForPositive)
    {
      // when swimming the track from -z to z we get a positive rotation
      if (firstPoint.DistanceTo(vertex) < lastPoint.DistanceTo(vertex))
	{
	  charge = 1;
	}
      else
	{
	  charge = -1;
	}
    }
    else
      {
	charge = 0;
      }
    
    return charge;
}

ClassImp(CbmSttTrackFitter)