////////////////////////////////////////////////////////////////////////////
// PndFtsGeometricTracker
//
// Class for digitalization for FTS
//
// authors: Pablo Genova - Pavia University
//          Lia Lavezzi  - Pavia University
//
// modified for forward tracking by Isabella Garzia - Ferrara Univertsity
////////////////////////////////////////////////////////////////////////////

#include "PndFtsGeometricTracker.h"

#include "PndFtsHit.h"
#include "PndFtsHitInfo.h"
#include "PndFtsPoint.h"
#include "PndFtsSingleStraw.h"
#include "PndGeoFtsPar.h"
#include "PndFtsTube.h"
#include "PndFtsMapCreator.h"
#include "PndFtsSignalOverlap.h"
#include "PndTrackCand.h"
#include "PndTrack.h"

#include "FairRootManager.h"
#include "FairRunAna.h"
#include "FairRuntimeDb.h"
#include "FairGeoNode.h"
#include "FairGeoTransform.h"
#include "FairGeoRotation.h"
#include "FairGeoVector.h"

#include "TGeoManager.h"
#include "TClonesArray.h"
#include "TVector3.h"
#include "TRandom.h"
#include <iostream>
#include <fstream>
#include <cmath>
#include <cstdlib>
#include <algorithm>

using namespace std;

using std::cout;
using std::endl;
using std::sqrt;

  unsigned PndFtsGeometricTracker::IN_TRK_COUNTER;
  unsigned PndFtsGeometricTracker::OUT_TRK_COUNTER;

// -----   Default constructor   -------------------------------------------
PndFtsGeometricTracker::PndFtsGeometricTracker():
  FairTask("Ideal FTS Hit Producer",0), /*fPointArray(0),*/  fHitArray(0),
  fVolumeArray(0), /*fHitInfoArray(0),*/ fevtn(0), fFtsParameters(new PndGeoFtsPar()),
  fPersistence(kTRUE), fOverlap(kFALSE)
{
}
// -------------------------------------------------------------------------



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



// -----   Public method Init   --------------------------------------------
InitStatus PndFtsGeometricTracker::Init() {
  fevtn=0;

  std::cout<<"#########################################################"<<std::endl;
  std::cout<<"PndFtsGeometricTracker: Init()#######"<<std::endl;
  std::cout<<"#########################################################"<<std::endl;

 // Get RootManager
  FairRootManager* ioman = FairRootManager::Instance();
  if ( ! ioman ) {
    cout << "-E- PndFtsGeometricTracker-wintz::Init: "
	 << "RootManager not instantiated!" << endl;
    return kFATAL;
  }

  //// Get input array
  //fPointArray = (TClonesArray*) ioman->GetObject("FTSPoint");
  //if ( ! fPointArray ) {
    //cout << "-W- PndFtsGeometricTracker::Init: "
	 //<< "No FTSPoint array!" << endl;
    //return kERROR;
  //}

  // Create and register output array: without fOverlap
  //fHitArray = new TClonesArray("PndFtsHit");
  //ioman->Register("FTSHit","FTS",fHitArray, fPersistence);

  ////new part: create and register output array
  //if(!fOverlap){
    //fHitArray = new TClonesArray("PndFtsHit");
    //ioman->Register("FTSHit","FTS",fHitArray, fPersistence);
  //}
  //else{
    ////if overlap on, save the overlapped hits in regular
    ////output TCA (FTSHit) and the "original" hits (non overlapped)
    ////in another TCA (FTSOriginalHit)
    //fOverlapHitArray = new TClonesArray("PndFtsHit");
    //ioman->Register("FTSHit","FTS",fOverlapHitArray,fPersistence);
    //fHitArray = new TClonesArray("PndFtsHit");
    //ioman->Register("FTSOriginalHit","FTS",fHitArray, fPersistence);
  //}

  fHitArray = (TClonesArray*) ioman->GetObject("FTSHit");
  if ( ! fHitArray ) {
    cout << "-W- PndFtsGeometricTracker::Init: "
    << "No FTSHit array!" << endl;
    return kERROR;
  }

	fTrackCandArray = ioman->Register("TrackCand", "PndTrackCand","FTS", kTRUE);
	fTrackArray = ioman->Register("Track", "PndTrack","FTS", kTRUE);

  // Create and register output array
  //fHitInfoArray = new TClonesArray("PndFtsHitInfo");
  //ioman->Register("FTSHitInfo", "FTS", fHitInfoArray, kFALSE);

  fVolumeArray = gGeoManager->GetListOfVolumes();

  cout << "-I- PndFTSHitProducerRealFast: INITIALIZATION SUCCESSFUL" << endl;


  //CHECK added
  PndFtsMapCreator *mapper = new PndFtsMapCreator(fFtsParameters);
  fTubeArray = mapper->FillTubeArray();

 readgeom();

  return kSUCCESS;

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

void PndFtsGeometricTracker::SetParContainers() {

  FairRuntimeDb* rtdb = FairRunAna::Instance()->GetRuntimeDb();
  fFtsParameters = (PndGeoFtsPar*) rtdb->getContainer("PndGeoFtsPar");
}


// -----   Public method Exec   --------------------------------------------
void PndFtsGeometricTracker::Exec(Option_t*) {

  //std::cout<<"PndFtsHitProducer Exec ########"<<std::endl;
  if(fVerbose && fevtn%50==0) cout << "Event Number "<<fevtn<<endl;
  else if(fVerbose >= 3) cout << "Event Number "<<fevtn<<endl;

  fevtn++;

  // Reset output array
  //if ( ! fHitArray ) Fatal("Exec", "No HitArray");

  //fHitArray->Delete();
  //fHitInfoArray->Clear();
  //if(fOverlap) fOverlapHitArray->Delete();

  //Int_t detID = 0;    // detectorID
  TVector3 pos, dpos; // position and error vectors

  // Declare some variables
  PndFtsHit* hit  = NULL;

  // Loop over FtsPoints
  Int_t nHits = fHitArray->GetEntriesFast();

  DataHit.clear();

  //  cout << "------------ " << nPoints << endl;

  ////for (Int_t iPoint = 0; iPoint < nPoints; iPoint++) {
    ////point  = (PndFtsPoint*) fPointArray->At(iPoint);
    ////if (point == NULL) continue;

    ////detID = point->GetDetectorID();

    ////// tubeID  CHECK added
    ////Int_t skew = 0;
    ////Int_t tubeID = point->GetTubeID();
    ////Int_t chamberID=point->GetChamberID();
    ////Int_t layerID=point->GetLayerID();
    ////PndFtsTube *tube = (PndFtsTube*) fTubeArray->At(tubeID);

    //////if skewed tube: skew==1
    ////if(layerID>=3 && layerID<=6){skew=1;} //skewed tudes fts1
    ////if(layerID>=11 && layerID<=14){skew=1;} //skewed tudes fts2
    ////if(layerID>=19 && layerID<=22){skew=1;} //skewed tudes fts3
    ////if(layerID>=27 && layerID<=30){skew=1;} //skewed tudes fts4
    ////if(layerID>=35 && layerID<=38){skew=1;} //skewed tudes fts5
    ////if(layerID>=43 && layerID<=46){skew=1;} //skewed tudes fts6


    ////double InOut[6];
    ////memset(InOut, 0, sizeof(InOut));

    ////InOut[0] = point->GetXInLocal();
    ////InOut[1] = point->GetYInLocal();
    ////InOut[2] = point->GetZInLocal();
    ////InOut[3] = point->GetXOutLocal();
    ////InOut[4] = point->GetYOutLocal();
    ////InOut[5] = point->GetZOutLocal();

    ////// single straw tube simulation -----------------------
    ////PndFtsSingleStraw fts;

    //////setting the single straw tube simulation constants
    ////// 3 options currently available:
    ////// TConst(tube radius (cm), gas pressure (bar), Ar%, CO2%)
    ////// fts.TConst(0.4, 1, 0.9, 0.1);
    //////fts.TConst(0.5, 1, 0.9, 0.1);//1 bar
    ////fts.TConst(0.5, 2, 0.8, 0.2);  //2 bar

    ////// wire positioning->controllare bene
    ////fts.PutWireXYZ(0.,  0., -75., 0., 0., 75.);

    ////// get particle momentum
    ////TVector3 momentum(point->GetPxOut(),point->GetPyOut(),point->GetPzOut()); // GeV/c

    ////Double_t GeV=1.;
    ////// position in cm (already in cm); momentum in GeV (already in GeV); mass in GeV (already in GeV)

    ////// drift time calculation

    ////Double_t pulset =-1;
    //////pulset = fts.PartToTime(point->GetMass()/GeV, momentum.Mag()/GeV, InOut);

    ////// constant initialization
    ////fts.TInit(point->GetMass()/GeV, momentum.Mag()/GeV, InOut);

    ////// true radius (cm)
    ////Double_t true_rad = fts.TrueDist(InOut);

    ////// simulated radius (cm)
    //////Double_t radius = fts.TimnsToDiscm(pulset);
    //////if(radius < 0.) radius = 0.; // CHECK
    //////if(radius <0. ||radius==0.) radius =-999;

    ////// fast simulation
    ////Double_t radius= fts.FastRec(true_rad,1) ; //,0) standard curve ,1) Juelich exp curve
                                               //////Juelich is at 2 bar pressure
    ////// dE calculation
    //////  double depCharge = fts.PartToADC();

     ////// dE calculation ------- check
    ////// charge calculation
    ////Double_t depcharge = fts.FastPartToADC(); // CHECK   arbitrary units!
    ////// dE/dx calculation postponed
    //////Double_t dedx = -999; //[R.K. 01/2017] unused variable?


    ////Double_t closestDistanceError = GetError(radius);//calculates the error according to Juelich experimental curves
    //////cout<<"radius "<<radius<<" error "<<closestDistanceError<<endl;
    //////closestDistanceError = 0.0150; //150 microns check this point!
    //////closestDistanceError =TMath::Sqrt(2.)*radius/TMath::Sqrt(12);

    //////TVector3 position(point->GetX(), point->GetY(), point->GetZ()); // use this for hits having same coordinates as MC points
    ////TVector3 position = tube->GetPosition(); // use this for realistic hit production
    ////pos.SetXYZ(position.X(), position.Y(), position.Z()); // <--- stt1

    //////    dpos.SetXYZ(innerStrawDiameter / 2., innerStrawDiameter / 2., GetLongitudinalResolution(position.Z()));
    ////dpos.SetXYZ(0.5, 0.5, 3.); // per adesso (stessi che in Ideal:
                               ////// innerStrawDiameter/2 = 0.5,
                               ////// longitudinalResolution = 3.)

  for (Int_t iHit = 0; iHit < nHits; iHit++) {
    hit  = (PndFtsHit*) fHitArray->At(iHit);
    if (hit == NULL) continue;
     /* ********************* INFO *******************************************


    - numer eventu WE point->GetEventID()
    - numer toru symulacyjny WE point->GetTrackID()
    - -1 by default (z MC)
    - numer layeru    Int_t layerID=point->GetLayerID();
    - numer slomki     Int_t layerID=point->GetLayerID();
    - radius jako WE radius  (jak wyzej)
    - najblizszy punkt wziety z
       point->GetX();
       point->GetY();
       point->GetZ();

        GEOMETRIA

    - pozycja geometryczna drutu WE
          point  = (PndFtsPoint*)
          Double_t GetXWireDirection() const { return fX_wire_dir; }
          Double_t GetYWireDirection() const { return fY_wire_dir; }
          Double_t GetZWireDirection() const { return fZ_wire_dir; }
    - radius jako WY - z procedury dla danej slomki

            Int_t chamberID=point->GetChamberID();


     // ********************* INFO *******************************************/

    // create hit
    //if ( layerID <= 8 ) {  // but why ???
    //cout << " layerID " << layerID << endl;

    // ***********************************************
    ////AddHit(detID, tubeID, chamberID, layerID, skew, iHit, pos, dpos, pulset, radius, closestDistanceError, depcharge);
    // ***********************************************
    ////AddHitInfo(0, 0, point->GetTrackID(), iHit, 0, kFALSE);
    // ***********************************************
//cout << " tubeID === " << tubeID << " point->GetTrackID() === " << point->GetTrackID() << endl;

    // copy info for tracker
    // TODO: rewrite to use hits directly, not copies.
    //AddDataHit( fevtn, detID, tubeID, chamberID, layerID, skew, iHit, pos, dpos, pulset, radius, closestDistanceError, depcharge, point->GetTrackID(), point->GetPxOut(),point->GetPyOut(),point->GetPzOut() );
    pos.SetXYZ(hit->GetX(),hit->GetY(),hit->GetZ());
    dpos.SetXYZ(hit->GetDx(),hit->GetDy(),hit->GetDz());
    AddDataHit( fevtn, hit->GetDetectorID() , hit->GetTubeID(), hit->GetChamberID(), hit->GetLayerID(), hit->GetSkewed(), hit->GetRefIndex(), pos, dpos, hit->GetPulse(), hit->GetIsochrone(), hit->GetIsochroneError(), hit->GetDepCharge(), iHit, 0,0,0 );

    //}

  }// Loop over MCPoints


  // *** here ALGORITHM

  //cout << " rozmiar input = " << DataHit.size() << endl;
  int trackmults[10] = { 0 }; // I need to check MC mult of each track before I plot IDEAL
  int testmults[10] = { 0 }; // I need to check MC mult of each track before I plot IDEAL
  int in_mults[10] = { 0 }; // I need to check MC mult of each track before I plot IDEAL
  int out_mults[10] = { 0 }; // I need to check MC mult of each track before I plot IDEAL
  double mom_mults_x[10] = { 0 };
  double mom_mults_y[10] = { 0 };
  double mom_mults_z[10] = { 0 };
  for ( auto h : DataHit ) { if (h.trackID<10) ++trackmults[static_cast<unsigned>(h.trackID)]; /*cout << "track id = " << h.trackID << endl;*/ }
  for ( unsigned i=0; i<DataHit.size(); ++i )
      if ( DataHit[i].trackID<10 && trackmults[ static_cast<unsigned>(DataHit[i].trackID) ] < 0 ) DataHit[i].active = false;
  for ( auto h : DataHit ) { if (h.trackID<10 && h.active) ++testmults[static_cast<unsigned>(h.trackID)]; /*cout << "track id = " << h.trackID << endl;*/ }
  //for ( unsigned i{0}; i<10; ++i )
  //    if ( trackmults[i] > 0 ) cout << "track id - " << i << " mult - " << trackmults[i] << " test = " << testmults[i] << endl;

  DataHitIn.clear();
  for (const auto& a : DataHit)
      if ( a.active ) { DataHitIn.push_back( a ); } // ONLY GOOD FOR MC EVALUATION
      //{ DataHitIn.push_back( a ); } // ALL for ALGORITHM INPUT

  DataHitOut.clear();
  if ( !DataHitIn.empty() && DataHitIn.size() < 350. ) {
    DataHitOut = trackFinder( DataHitIn );
    matchEmpty( DataHitOut, DataHitIn );
  //cout << " rozmiar output = " << DataHitOut.size() << endl;
  }
  for ( const auto& h : DataHitIn ) {
           if (h.trackID<10 && h.active) {
             ++in_mults[static_cast<unsigned>(h.trackID)];
             mom_mults_x[static_cast<unsigned>(h.trackID)]=h.mx;
             mom_mults_y[static_cast<unsigned>(h.trackID)]=h.my;
             mom_mults_z[static_cast<unsigned>(h.trackID)]=h.mz;
           }
         }
  std::map<int,std::vector<int> > trackcand;                                      //[R.K.]
  int itrdh=0;                                                                    //[R.K.]
  for ( const auto& h : DataHitOut ) {
    if (h.trackID<10 && h.active) ++out_mults[static_cast<unsigned>(h.trackID)];
    trackcand[h.trackID].push_back(itrdh);                                        //[R.K.]
    itrdh++;                                                                      //[R.K.]
  }                                                                               //[R.K.]
  for ( unsigned i{0}; i<10; ++i ) {
      if ( in_mults[i] > 0 || out_mults[i] > 0 ) cout << "track id - " << i << " IN mult - " << in_mults[i] << " OUT mult = " << out_mults[i] << " --- mom = "
          << mom_mults_x[i] <<" "
          << mom_mults_y[i] <<" "
          << mom_mults_z[i];
      if (in_mults[i]==0 && out_mults[i] > 0) cout << " *ZONK* " << endl; cout << endl;
      if ( in_mults[i] > 0 ) ++IN_TRK_COUNTER;
      if ( out_mults[i] > 0 ) ++OUT_TRK_COUNTER;

  }

  //if ( DataHit.size()>=40. )
  for (auto d : DataHitOut) { // ALGORITHM
  //for (auto d : DataHitIn) {
    //cout << " AddHit --- " << endl;
    //cout <<  "R = " << d.r << " strawId = " << d.tubeID << endl;
    TVector3 pos2; pos2.SetXYZ(d.x, d.y, d.z);
    TVector3 dpos2; dpos2.SetXYZ(d.dx, d.dy, d.dz);
    ////AddHit(d.detID, d.tubeID, d.chamberID, d.layerID, d.skew, d.iHit, pos2, dpos2, d.p, d.r, d.closestDistanceError, d.depcharge);
    ////AddHitInfo(0, d.evtNr, d.trackID, d.iHit, 0, kFALSE);
    //cout << "evt trkid tubeid chaberid layerid " <<  d.evtNr <<","<<d.trackID<<","<<d.tubeID<<","<<d.chamberID<<","<<d.layerID << endl;


    // TODO build track cands and tracks here!                                              //[R.K.]
    //                                                                                      //[R.K.]
    int trknum=0;                                                                           //[R.K.]
    for (auto iter : trackcand) {                                                           //[R.K.]
      // create candidate                                                                   //[R.K.]
      //loop vector                                                                         //[R.K.]
      //PndTrackCand* mycand = new( (*fTrackCandArray)[trknum] ) PndTrackCand();            //[R.K.]
      PndTrackCand mycand;                                                                  //[R.K.]
      int idfirst=iter.second[0];                                                           //[R.K.]
      int idlast=iter.second[0];                                                            //[R.K.]
      for ( auto hititer : iter.second ) {                                                  //[R.K.]
        //add hitrs to candidate                                                            //[R.K.]
        // hititer is pointer to DataHitUnit, use iHit to get actual hit                    //[R.K.]
                                                                                            //[R.K.]
        PndFtsHit* myhit=(PndFtsHit*)fHitArray->At(DataHitOut[hititer].iHit);               //[R.K.]
        if(DataHitOut[hititer].z < DataHitOut[idfirst].z) idfirst=hititer;                  //[R.K.]
        if(DataHitOut[hititer].z > DataHitOut[idlast].z) idlast=hititer;                    //[R.K.]
        mycand.AddHit(myhit->GetEntryNr(),DataHitOut[hititer].z);                           //[R.K.]
          // Modifiers -----------------------                                              //[R.K.]
	        //AddHit(UInt_t detId, UInt_t hitId, Double_t rho);                               //[R.K.]
	        //AddHit(TString branchName, UInt_t hitId, Double_t rho);                         //[R.K.]
  	      //AddHit(FairLink link, Double_t rho);                                            //[R.K.]
      }                                                                                     //[R.K.]
                                                                                            //[R.K.]
      //create track from candidate                                                         //[R.K.]
      TVector3 mom(0.,0.,1.); //(px,py,pz);//TODO set proper seed for kalman filter         //[R.K.]
      // Use the radius R in the circle() function.                                         //[R.K.]
      //Having a struct for tracks (mom,q,vect<ids>) would help a lot!                      //[R.K.]
      TVector3 dmom(1.,1.,1.);//TODO set proper seed for kalman filter                      //[R.K.]
      int q=1; //TODO set proper charge from curve                                          //[R.K.]
                                                                                            //[R.K.]
      TVector3 o(0.,0.,0.), dj(1.,0.,0.), dk(0.,1.,0.);                                     //[R.K.]
                                                                                            //[R.K.]
      TVector3 posfirst(DataHitOut[idfirst].x,DataHitOut[idfirst].y,DataHitOut[idfirst].z); //[R.K.]
      TVector3 dposfirst(1.,1.,1.);//TODO set proper seed for kalman filter                 //[R.K.]
      o.SetZ(posfirst.Z());                                                                 //[R.K.]
      FairTrackParP myfirst(posfirst,mom,dposfirst,dmom,q,o,dj,dk);                         //[R.K.]
                                                                                            //[R.K.]
      TVector3 poslast(DataHitOut[idlast].x,DataHitOut[idlast].y,DataHitOut[idlast].z);     //[R.K.]
      TVector3 dposlast(1.,1.,1.);                                                          //[R.K.]
      o.SetZ(poslast.Z());                                                                  //[R.K.]
      FairTrackParP mylast(poslast,mom,dposlast,dmom,q,o,dj,dk);                            //[R.K.]
                                                                                            //[R.K.]
      new( (*fTrackCandArray)[trknum] ) PndTrackCand(mycand); // copy to output             //[R.K.]
      new( (*fTrackArray)[trknum] ) PndTrack(myfirst,mylast,mycand);  //                    //[R.K.]
      trknum++;                                                                             //[R.K.]
                                                                                            //[R.K.]
    }                                                                                       //[R.K.]

  }

  cout << "IN OUT state = " << IN_TRK_COUNTER << " , " << OUT_TRK_COUNTER << endl;

  //if(fOverlap){
    //PndFtsSignalOverlap *myoverlap=new PndFtsSignalOverlap(fHitArray);
    //myoverlap->OverlapSimultaneousSignals(fOverlapHitArray);

  //}

  // Event summary
  //cout << "-I- PndSttHitProducerRealFast: " << nPoints << " FtsPoints, "
  //<< nPoints << " Hits created." << endl;

}
// -------------------------------------------------------------------------
void PndFtsGeometricTracker::FoldZPosWithResolution(Double_t &zpos, Double_t &zposError,
						    TVector3 , TVector3 ) // localInPos localOutPos //[R.K.03/2017] unused variable(s)
{


  //Double_t
    //zPosInStrawFrame = (localOutPos.Z() - localInPos.Z()) / 2.; //[R.K. 01/2017] unused variable?
 //FIXME We have dummy Error calculation

  //  zposError = gRandom->Gaus(0., GetLongitudinalResolution(zPosInStrawFrame));
  zposError = gRandom->Gaus(0., 3.); // per adesso (stesso che in Ideal:
                                     // longitudinalResolution = 3.)

  zpos += zposError;
}

//----------------------------------------------------------------------------------------------------------------------------------------------------
void PndFtsGeometricTracker::AddDataHit( Int_t evtNr, Int_t detID, Int_t tubeID, Int_t chamberID, Int_t layerID, Int_t skew, Int_t iHit, TVector3& pos, TVector3& dpos, Double_t p, Double_t rsim, Double_t closestDistanceError, Double_t depcharge, Double_t tid, Double_t mmx, Double_t mmy, Double_t mmz ) {

   //cout << "PndFtsGeometricTracker::AddDataHit working here " << mm << endl;
   //if ( rand() % 2 == 0 ) {
      DataHit.push_back( PndFtsGeometricTracker::DataHitUnit( evtNr, detID, tubeID, chamberID, layerID, skew, iHit, pos.X(), pos.Y(), pos.Z(), dpos.X(), dpos.Y(), dpos.Z(), p, rsim, closestDistanceError, depcharge, tid, mmx,mmy,mmz ) );
   //}

/*
   Int_t evtNr;
   Int_t detID;
   Int_t tubeID;
   Int_t chamberID;
   Int_t layerID;
   Int_t skew;
   Int_t iHit;
   Double_t x;
   Double_t y;
   Double_t z;
   Double_t dx;
   Double_t dy;
   Double_t dz;
   Double_t p;
   Double_t r;
   Double_t closestDistanceError;
   Double_t depcharge;

*/
}
//----------------------------------------------------------------------------------------------------------------------------------------------------


//// -----   Private method AddHit   --------------------------------------------
//PndFtsHit* PndFtsGeometricTracker::AddHit(Int_t detID, Int_t tubeID, Int_t chamberID, Int_t layerID, Int_t skew, Int_t iHit, TVector3& pos, TVector3& dpos, Double_t p, Double_t rsim, Double_t closestDistanceError, Double_t depcharge) {


  //// see PndFtsHit for hit description
  //TClonesArray& clref = *fHitArray;
  //Int_t size = clref.GetEntriesFast();

  //PndFtsHit *hitnew = new(clref[size]) PndFtsHit(detID, tubeID, chamberID, layerID, skew, iHit, pos, dpos, p, rsim, closestDistanceError, depcharge);
  //return hitnew;

//}
//// ----

//// -----   Private method AddHitInfo   --------------------------------------------
//PndFtsHitInfo* PndFtsGeometricTracker::AddHitInfo(Int_t fileNumber, Int_t eventNumber, Int_t trackID, Int_t pointID, Int_t nMerged, Bool_t isFake){
  //// see PndFtsHitInfo for hit description


  //TClonesArray& clref = *fHitInfoArray;
  //Int_t size = clref.GetEntriesFast();
  //return new(clref[size])  PndFtsHitInfo(fileNumber, eventNumber, trackID, pointID, nMerged, isFake);
//}


Double_t PndFtsGeometricTracker::GetError(Double_t TrueDcm) {


  // data from julich
  Double_t resmic=-1;
  if(TrueDcm < 0.48){
    resmic =    20. +1.48048e+02
      -3.35951e+02*TrueDcm
      -1.87575e+03*pow(TrueDcm,2)
      +1.92910e+04*pow(TrueDcm,3)
      -6.90036e+04*pow(TrueDcm,4)
      +1.07960e+05*pow(TrueDcm,5)
      -5.90064e+04*pow(TrueDcm,6) ;
  }
  else resmic=65.;

  return resmic*0.0001;
}

//**************************************************************************************************************
//**************************************************************************************************************
//**************************************************************************************************************
//**************************************************************************************************************
//**************************************************************************************************************
//**************************************************************************************************************
//**************************************************************************************************************
/************************************************************************/
vector< PndFtsGeometricTracker::DataHitUnit >  PndFtsGeometricTracker::trackFinder( const vector< PndFtsGeometricTracker::DataHitUnit >& input ){
/************************************************************************/
vector< PndFtsGeometricTracker::DataHitUnit > output;
map<int,float> T;
map<int,float>::iterator it;
int iv12,is12,iv34,is34,iv56,is56;
int i,j,k,lsz,e,n,l,ilslread;
size_t count;
float f;
int nbTr12,nbTr56;
int sumasl=0,sumazn=0;

//cout << endl << "START  trackFinder!" << endl << endl;
//for (int count = 0; count < input.size(); count++)
     //cout << input[count].evtNr << " " << input[count].trackID << " -1 " << input[count].layerID << " " << input[count].tubeID << " " << input[count].r << " " << input[count].x << " " << input[count].y << " " << input[count].z << endl;

T.clear();
lf12v=lf12s=lf34v=lf34s=lf56v=lf56s=0;
for (count = 0; count < input.size(); count++) {
  e=input[count].tubeID;
  f=input[count].r;
  it=T.find(e);
  if(it==T.end()){
        if(e<289) lf12v++;
        else if(e<865) lf12s++;
        else if(e<1441) lf12v++;
        else if(e<2017) lf12s++;
        else if(e<2305) lf12v++;
        else if(e<2689) lf34v++;
        else if(e<3457) lf34s++;
        else if(e<4289) lf34v++;
        else if(e<5185) lf34s++;
        else if(e<5633) lf34v++;
        else if(e<6457) lf56v++;
        else if(e<8105) lf56s++;
        else if(e<9753) lf56v++;
        else if(e<11401) lf56s++;
        else if(e<12225) lf56v++;
        T.insert(pair<int,float>(e,f));
   } else
        if((it->second)>f) (it->second)=f;
}
if(lf12v!=0)  F12v = new  straw[lf12v];
if(lf12s!=0)  F12s = new  straw[lf12s];
if(lf34v!=0)  F34v = new  straw[lf34v];
if(lf34s!=0)  F34s = new  straw[lf34s];
if(lf56v!=0)  F56v = new  straw[lf56v];
if(lf56s!=0)  F56s = new  straw[lf56s];

 iv12=0;is12=0;iv34=0;is34=0;iv56=0;is56=0;
 for (it = T.begin();it != T.end(); ++it)
  {
  n=it->first;
  l=G[n].l;

  if(l==1||l==2||l==7||l==8||l==9||l==10||l==15||l==16)
        { F12v[iv12].nr=n;
          F12v[iv12].l=l;
          F12v[iv12].r=it->second;
          F12v[iv12].x=G[n].x;
          F12v[iv12].y=G[n].y;
          F12v[iv12].z=G[n].z;
          iv12++;
         }
  else if(l<17) {
          F12s[is12].nr=n;
          F12s[is12].l=l;
          F12s[is12].r=it->second;
          F12s[is12].x=G[n].x;
          F12s[is12].y=G[n].y;
          F12s[is12].z=G[n].z;
          is12++;
         }
  else if(l==17||l==18||l==23||l==24||l==25||l==26||l==31||l==32)
        { F34v[iv34].nr=n;
          F34v[iv34].l=l;
          F34v[iv34].r=it->second;
          F34v[iv34].x=G[n].x;
          F34v[iv34].y=G[n].y;
          F34v[iv34].z=G[n].z;
          iv34++;
         }
  else if(l==19||l==20||l==21||l==22||l==27||l==28||l==29||l==30)
        { F34s[is34].nr=n;
          F34s[is34].l=l;
          F34s[is34].r=it->second;
          F34s[is34].x=G[n].x;
          F34s[is34].y=G[n].y;
          F34s[is34].z=G[n].z;
          is34++;
         }
else if(l==33||l==34||l==39||l==40||l==41||l==42||l==47||l==48)
        { F56v[iv56].nr=n;
          F56v[iv56].l=l;
          F56v[iv56].r=it->second;
          F56v[iv56].x=G[n].x;
          F56v[iv56].y=G[n].y;
          F56v[iv56].z=G[n].z;
          iv56++;
       }
  else if(l==35||l==36||l==37||l==38||l==43||l==44||l==45||l==46)
       {  F56s[is56].nr=n;
          F56s[is56].l=l;
          F56s[is56].r=it->second;
          F56s[is56].x=G[n].x;
          F56s[is56].y=G[n].y;
          F56s[is56].z=G[n].z;
          is56++;
         }
  }
if((iv12+is12+iv34+is34+iv56+is56) !=  (lf12v+lf12s+lf34v+lf34s+lf56v+lf56s))
 ilslread=0;    else {
           lf12v=iv12;
           lf12s=is12;
           lf34v=iv34;
           lf34s=is34;
           lf56v=iv56;
           lf56s=is56;
       }


 for(i=0;i<100;i++){
   for(j=0;j<49;j++) SF[i][j]=0;
   for(j=0;j<17;j++) {
       SF12[i][j]=0;
       SF34[i][j]=0;
       SF56[i][j]=0;
       fg[i][j]=-99999;
    }}

 f=12;nbTr12=0;
   if((lf12v*lf12s)!=0)track(f,&nbTr12,F12v,lf12v,F12s,lf12s);
 if(nbTr12>0) {
   f=56;nbTr56=0;
   if((lf56v*lf56s) !=0) track(f,&nbTr56,F56v,lf56v,F56s,lf56s);
   for(i=0;i<nbTr12;i++)
     circle(i,F34v,lf34v,F34s,lf34s,nbTr12,nbTr56);
   for(i=0;i<nbTr12;i++){
     for(j=1;j<17;j++) {SF[i][j]=SF12[i][j];}
     //if(SF34[i][0]>0) for(j=1;j<17;j++) {SF[i][16+j]=SF34[i][j];} else continue;
     for(j=1;j<17;j++) {SF[i][16+j]=SF34[i][j];} // *** WPWP ***
     k=SF12[i][0]; SF[i][0]=k;
     if(k>=0) for(j=1;j<17;j++) {SF[i][32+j]=SF56[k][j];}
for(j=1;j<49;j++)
  if(SF[i][j]!=0)
    {it=T.find(SF[i][j]);
     output.push_back(DataHitUnit( input[0].evtNr, 0, SF[i][j], 0, G[SF[i][j]].l, 0, 0, G[SF[i][j]].x,G[SF[i][j]].y, G[SF[i][j]].z, 0, 0, 0, 0, it->second, 0, 0, i ));
    }}
   //dopasuj(f,nbTr12,&lsz);
 } /* if(ilsladow12>0)*/
 else lsz=0;
 sumasl=sumasl+ilslread;
 sumazn=sumazn+lsz;

cout << "evt in out " << input[0].evtNr << " " << input.size() << " " << output.size() << endl;
//cout << endl << "OUTPUT IN  trackFinder!" << endl << endl;

return output;
}


void PndFtsGeometricTracker::readgeom()
{
      auto tmp = {294.895,295.77,299.39,300.265,304.39,305.265,309.39,
                  310.265,326.895,327.77,331.39,332.265,336.39,337.265,
                  341.39,342.265,393.995,394.87,400.965,401.84,415.49,
                  416.365,422.965,423.84,437.49,438.365,444.965,445.84,
                  459.49,460.365,466.965,467.84,606.995,607.87,611.49,
                  612.365,616.49,617.365,621.49,622.365,638.995,639.87,
                  643.49,644.365,648.49,649.365,653.49,654.365};//!

      copy( tmp.begin(), tmp.end(), zcoord );

//      int nic;
//cout << "FULL STOP" << endl;
//cin >> nic;

const char* env_p = std::getenv("VMCWORKDIR");
TString geopath(env_p);
geopath += "/fts/geom.txt";
ifstream pp(geopath.Data());
//ifstream pp("/lustre/nyx/hades/user/przygoda/PANDAROOT/fts/geom.txt");
if (!pp) { cerr << "ERROR: PndFtsGeometricTracker: geom.txt not found in \""<<geopath.Data()<<"\" !" << endl; exit(1); }
string n;
int a,b,c;
double d,e,f,g,h,j,k,l,m;

       while( !pp.eof() ) {
 pp>>a>>b>>c>>d>>e>>f>>g>>h>>j>>k>>l>>m>>n;
         G[a].l=c;
         G[a].x=d;
         G[a].y=e;
         G[a].z=f;
       }
       pp.close();
//cout << "FULL STOP END" << endl;
//cin >> nic;

}


/*       circle                 */
void PndFtsGeometricTracker::circle(int i,straw *Fv,int lfv,straw *Fs,int lfs, int tr12,int tr56)
{
point P16, P,P1,PS,S;//P17,P33,
int k,jj,SP[17],pom[17]; //yy,j,kk,licz,
float ii,R,a1,a2,b1,b2,wa,wb; //alfa,beta,RR,ff[5],,energ
float d,dmin,dall,dallmin,d1,d2,y1,y2,y; //dd,
float aa,delta,dzox,dzoy;//,dzoxmin,dzoymin dv,ds,
int l,layer,kmin,str_max;

delta=0.5;
//energ=0.55;
//dv=0.5;
//ds=0.5;
P16.x=zcoord[15];
//P17.x=zcoord[16];
for(k=0;k<17;k++)SP[k]=0;
//for(k=0;k<5;k++)ff[k]=0;
//kk=1;licz=0;
dallmin=5.0;str_max=0;
for(jj=lfv-1;jj>=0 && Fv[jj].l>30;jj--){
 P1.x=Fv[jj].z;
 P1.y=Fv[jj].x;
//for(ii=zcoord[15]; ii<P17.x;ii=ii+5)  {P16.x=ii;
   P16.x=368; ii=368;
    P16.y=wsp12[i][0]*P16.x+wsp12[i][1];
    a1=-1/wsp12[i][0];   b1=P16.y-a1*ii;
    line(P16,P1,&wa,&wb);
    PS.x=0.5*(ii+P1.x);  PS.y=0.5*(P16.y+P1.y);
    a2=-1/wa;  b2=PS.y-a2*PS.x;
    S.x=(b2-b1)/(a1-a2);  S.y=a2*S.x+b2;
    R=sqrt(pow(ii-S.x,2)+pow(P16.y-S.y,2));
    l=16;
    for(k=0;k<17;k++)pom[k]=0;
    dall=0;  dmin=-1;
    for(k=0;k<lfv;k++) {
       P.x=Fv[k].z;  P.y=Fv[k].x;
      y1=sqrt((P.x-S.x)*(P.x-S.x) + (P.y-S.y)*(P.y-S.y));
      d=fabs(y1-R);// if(d1<d2) d=d1; else d=d2;
      d=fabs(d-Fv[k].r);//cout<<Fv[k].nr<<"   d="<<d<<endl;
       if(Fv[k].l!=l){if(dmin>=0) dall=dall+dmin;
                      l=Fv[k].l;
                      if(d<delta){ pom[l-16]=Fv[k].nr;} dmin=d;}
           else  if(d<dmin) {if(d<delta) pom[l-16]=Fv[k].nr;dmin=d;}
       }
     if(dmin>=0) dall=dall+dmin; pom[0]=0;
     for(k=1;k<17;k++) if(pom[k]!=0) pom[0]++;
     if(pom[0]>0) dall=dall/pom[0]; else continue;
     if(dall>delta) continue;
     if(dall<dallmin && pom[0]>2 && pom[0]>=str_max) {
            str_max=pom[0];
            dallmin=dall;
            for(k=0;k<17;k++) {SP[k]=pom[k];}
            fg[i][0]=R;fg[i][1]=S.x;fg[i][2]=S.y;
            fg[i][3]=ii;fg[i][4]=wsp12[i][0]*ii+wsp12[i][1];
            fg[i][5]=Fv[jj].z;fg[i][6]=Fv[jj].x;
        }
  }/* for ii*/
   SF34[i][0]=0;SF12[i][0]=-1;
    /* end pionowe */
R=fg[i][0];S.x=fg[i][1];S.y=fg[i][2];
if(tr56>0  && SP[0]>0){
PS.x=560;
 y1=sqrt(R*R-(PS.x-S.x)*(PS.x-S.x))+S.y; y2=-sqrt(R*R-(PS.x-S.x)*(PS.x-S.x))+S.y;
 d1=fabs(y1-fg[i][6]); d2=fabs(y2-fg[i][6]);
 if(d1<d2) PS.y=y1; else PS.y=y2;
 dmin=wsp56[0][0]*PS.x+wsp56[0][1];
 dzox=PS.y-dmin;
 d1=wsp12[i][2]*PS.x+wsp12[i][3]; d2=wsp56[0][2]*PS.x+wsp56[0][3];
 dzoy=d1-d2;
 dmin=fabs(dzox)+fabs(dzoy);
 SF12[i][0]=-1;
 for(k=0;k<tr56;k++){
     y1=wsp56[k][0]*PS.x+wsp56[k][1];
     dzox=PS.y-y1;
     d1=wsp12[i][2]*PS.x+wsp12[i][3];
     d2=wsp56[k][2]*PS.x+wsp56[k][3];
     dzoy=d1-d2;
     d=fabs(dzox)+fabs(dzoy);
     if(d<=dmin){ dmin=d;
                 SF12[i][0]=k;
                 fg[i][7]=dzox;fg[i][8]=dzoy;}
if(fabs(fg[i][7])>4 && fabs(fg[i][8])>8)  SF12[i][0]=-1;
/* styczna do okregu w F56 w pcie PS */
line(PS,S,&wa,&wb);
wa=-1/wa;
wb=PS.y-wa*PS.x;
y=wsp56[k][0]*zcoord[47]+wsp56[k][1];
y1=wa*zcoord[47]+wb;
}
}
   /* ukosne */
P16.x=fg[i][3];
//P33.x=fg[i][5];
if(SP[0]>=2) {
  y1=sqrt(R*R-pow(Fs[0].z-fg[i][1],2))+fg[i][2];
  y2=-sqrt(R*R-pow(Fs[0].z-fg[i][1],2))+fg[i][2];
  d1=fabs(y1-fg[i][4]); d2=fabs(y2-fg[i][4]);
  if(d1<d2) y=y1; else y=y2;
  if(Fs[0].l==19 || Fs[0].l==20 || Fs[0].l==27 || Fs[0].l==28)aa=-11.43;
                else aa=11.43;
  y1=aa*(y-Fs[0].x)+Fs[0].y;
  y2=wsp12[i][2]*Fs[0].z+wsp12[i][3];
  d=fabs(y1-y2); d=fabs(d-Fs[0].r);
  layer=Fs[0].l;dmin=d;kmin=0;
  for(k=0;k<lfs;k++) {
          y1=sqrt(R*R-pow(Fs[k].z-fg[i][1],2))+fg[i][2];
          y2=-sqrt(R*R-pow(Fs[k].z-fg[i][1],2))+fg[i][2];
          d1=fabs(y1-fg[i][4]);
          d2=fabs(y2-fg[i][4]);
          if(d1<d2) y=y1; else y=y2;
 if(Fs[k].l==19 || Fs[k].l==20 || Fs[k].l==27 || Fs[k].l==28)aa=-11.43;
                else aa=11.43;
          y1=aa*(y-Fs[k].x)+Fs[k].y;
          y2=wsp12[i][2]*Fs[k].z+wsp12[i][3];
          d=fabs(y1-y2);
          d=fabs(d-Fs[k].r);
          if(Fs[k].l==layer){ if(d<dmin) { dmin=d; kmin=k;}}
             else {
          SP[layer-16]=Fs[kmin].nr;SP[0]=SP[0]+1;
          dmin=d;kmin=k;
          if(d<dmin) { dmin=d;kmin=k;}
          layer=Fs[k].l;
                 }
  }
 SP[layer-16]=Fs[kmin].nr;SP[0]=SP[0]+1;
}
/* end ukosne  */
if(SP[0]>=4) for(k=0;k<17;k++)  SF34[i][k]=SP[k];
}/*end void circle */


void PndFtsGeometricTracker::porownaj(int vs,int SP[][17],int *size,float *ff,float wsp[][4])
{
int i,j,k,ilsl,licz1,licz2;//licz

ilsl=*size;
int SPOM[ilsl][17];
float ffPOM[ilsl],wspPOM[ilsl][4];

for(i=0;i<ilsl;i++){
   if(SP[i][0]==0) continue;
   for(j=0;j<ilsl;j++){
       if(j==i) continue;
       if(SP[j][0]==0) continue;
       licz1=0;licz2=0;
       if(vs==1) {
         for(k=1;k<17;k++)
             if(k==1||k==2||k==7||k==8||k==9||k==10||k==15||k==16)
              { if(SP[i][k] == SP[j][k]) licz1++;
                if(abs(SP[i][k]-SP[j][k])==1) licz2++; }
         if((licz1==5 && licz2>1) || (licz1==6 && licz2>0) || licz1>6 || (licz1==4 && licz2>2)) {
                        if(SP[i][0]==SP[j][0])
                             { if(ff[i]>ff[j]) SP[i][0]=0;  else SP[j][0]=0;}
                        else if(SP[i][0]>SP[j][0]) SP[j][0]=0; else SP[i][0]=0;
               }
     }/*vs==1*/
     if(vs==2) {
           for(k=1;k<17;k++)
             if(k==3||k==4||k==5||k==6||k==11||k==12||k==13||k==14)
              { if(SP[i][k] == SP[j][k]) licz1++;
                if(abs(SP[i][k]-SP[j][k])==1) licz2++;
              }
            if((licz1==5 && licz2>1) || (licz1==6 && licz2>0) || licz1>6){
               if(SP[i][0]==SP[j][0])
                       { if(ff[i]>ff[j]) SP[i][0]=0;  else SP[j][0]=0;}
               else  {if(SP[i][0]>SP[j][0]) SP[j][0]=0; else SP[i][0]=0;}
             }
     }/*vs==2*/
} }

licz1=0;
for(i=0;i<ilsl;i++)
   if(SP[i][0]!=0) {
     ffPOM[licz1]=ff[i];
     for(k=0;k<4;k++)  wspPOM[licz1][k]=wsp[i][k];
     for(k=0;k<17;k++) {SPOM[licz1][k]=SP[i][k];}
     licz1++;
   }
for(i=0;i<ilsl;i++){
  ff[i]=0;
  for(k=0;k<17;k++) SP[i][k]=0;
  for(k=0;k<4;k++) wsp[i][k]=0;
}
for(i=0;i<licz1;i++){
  ff[i]=ffPOM[i];
  for(k=0;k<4;k++) wsp[i][k]=wspPOM[i][k];
  for(k=0;k<17;k++) {SP[i][k]=SPOM[i][k];}
}
*size=licz1;
}

///*       dopasuj                             */
//void PndFtsGeometricTracker::dopasuj(int f,int tr,int *lsz)
//{
////int j,k,jj,licz,licz1,nb_l;//i,
////int pom[49];
////int liczslznal;
////int liczmax,s12max,s34max,s56max,jmax,sf12,sf34,sf56,s12,s34,s56;
////float R,wa,wb;
////float A,B,C;
////float x1,x2,x3,y1,y2,y3;
////point P1,P2,P3,P4,S;
////liczslznal=0;
//}

/*&&&&&&&&&&&&&&&&& track  &&&&&&&&&&&&&&&&&&&&*/
void PndFtsGeometricTracker::track(int f,int *nbTr,straw *Fv,int lfv,straw *Fs,int lfs)
{
int i,j,licz,tr,SP[300][17];
float ff[300],wsp[300][4];
for(i=0;i<300;i++){
   for(j=0;j<4;j++) wsp[i][j]=0;
   for(j=0;j<17;j++) SP[i][j]=0;}
if(lfv==0 || lfs==0) {*nbTr=0;return;}
  else{
  fv_track(f,SP,Fv,lfv,&tr,wsp,ff);
  porownaj(1,SP,&tr,ff,wsp);
  licz=0;
  for(i=0;i<tr;i++)
  if(SP[i][0]>0){
    fs_track(f,SP,Fs,lfs,tr,wsp,ff);
    porownaj(2,SP,&tr,ff,wsp);
  } else{ tr=0;return;}
      }
licz=0;
if(f==12){
 for(i=0;i<tr;i++) if(SP[i][0]>11){
   for(j=0;j<17;j++) SF12[licz][j]=SP[i][j];
   for(j=0;j<4;j++) wsp12[licz][j]=wsp[i][j];
   licz++;
         }
} else{
   for(i=0;i<tr;i++) if(SP[i][0]>11) {
        for(j=0;j<17;j++) SF56[licz][j]=SP[i][j];
        for(j=0;j<4;j++) wsp56[licz][j]=wsp[i][j];
        licz++;
            }  }
*nbTr=licz;
}

/*&&&&&&&&&&&&&&&&& fs_track  &&&&&&&&&&&&&&&&&&&&*/
void PndFtsGeometricTracker::fs_track(int f,int SP[][17],straw * s,int size,int ilsl,float wsp[][4],float* ff)
{
int i,j,kk,sluk[50];
float d_min,a,b,x,aa,y,cc,wa,wb;//dd,y1,y2,,alfa,beta,beta1,beta2
float z1,z2,znak;
point P1,P2;
straw pom[2*size];

for(i=0;i<ilsl;i++){
 znak=G[SP[i][1]].y+G[SP[i][2]].y;
 if(SP[i][0]==0) continue;
 a=wsp[i][0]; b=wsp[i][1];
 kk=0;
 for(j=0;j<size;j++){
    x=a*s[j].z+b;
    if(f==12){
          if(s[j].l==3 || s[j].l==4 ||s[j].l==11 ||s[j].l==12)aa=-11.43;
          else aa=11.43;
        }
    if(f==56){
          if(s[j].l==35 || s[j].l==36 ||s[j].l==43 ||s[j].l==44)aa=-11.43;
          else aa=11.43;
        }
 y=aa*(x-s[j].x)+s[j].y;
 if(znak!=0) if(y*znak<0) continue;
 cc=s[j].r*sqrt(1+pow(aa,2));

    pom[kk].y=y+cc;
    pom[kk+1].y=y-cc;
    pom[kk].nr=s[j].nr;
    pom[kk+1].nr=s[j].nr;
    pom[kk].l=s[j].l;
    pom[kk+1].l=s[j].l;
    pom[kk].r=s[j].r;
    pom[kk+1].r=s[j].r;
    pom[kk].x=sqrt(x*x+pow(s[j].z+b/a,2));
    pom[kk+1].x=sqrt(x*x+pow(s[j].z+b/a,2));
    kk=kk+2;
}
 prostauk(f,pom,kk,&wa,&wb,sluk,&d_min);
 if((SP[i][0]>=5 && sluk[0]>5) || SP[i][0]+sluk[0]>11){
  SP[i][0]=SP[i][0]+sluk[0];ff[i]=ff[i]+d_min/sluk[0];
  for(kk=1;kk<=sluk[0];kk++) {
    if(f==12){
       if(SP[i][pom[sluk[kk]].l]!=0) SP[i][0]--;
       SP[i][pom[sluk[kk]].l]=pom[sluk[kk]].nr;}
       else {
       if(SP[i][pom[sluk[kk]].l-32]!=0) SP[i][0]--;
       SP[i][pom[sluk[kk]].l-32]=pom[sluk[kk]].nr;
       }
  }
 if(f==12) {z1=zcoord[2]; z2=zcoord[13];}
   else  {z1=zcoord[34]; z2=zcoord[45];}
 x=a*z1+b;
 x=sqrt(x*x+pow(z1+b/a,2));
 y=wa*x+wb; P1.y=y; P1.x=z1;
 x=a*z2+b;
 x=sqrt(x*x+pow(z2+b/a,2));
 y=wa*x+wb; P2.y=y; P2.x=z2;
 line(P1,P2,&wa,&wb);
 wsp[i][2]=wa; wsp[i][3]=wb;
      }else SP[i][0]=0;
}
}

/******************* prostauk ******************/
void PndFtsGeometricTracker::prostauk(int f,straw *s,int size,float *aa,float *bb,int *sluk,float *d_min)
{
int l,pocz,kon,lp1,lp2,lk1,lk2,nb,i,j,ss[50];//w,
float dd,dd_min,dmin,d,alfa,beta,delta;
point P1,P2;
delta=40;
*aa=0;
*bb=0;
for(i=0;i<50;i++) sluk[i]=0;
if(f==12) {lp1=3;lp2=4;lk1=13;lk2=14;}
if(f==34) {lp1=19;lp2=20;lk1=29;lk2=30;}
if(f==56) {lp1=35;lp2=36;lk1=45;lk2=46;}
dd_min=10000;
for(pocz=0;(s[pocz].l)==lp1 || (s[pocz].l)==lp2;pocz++){
    for(kon=size-1;(s[kon].l==lk1) || (s[kon].l==lk2) || (s[kon].l>lk1-1);kon--){
       for(i=0;i<50;i++) ss[i]=0;
       P1.x=(s[pocz].x);  P1.y=(s[pocz].y);
       P2.x=(s[kon].x);   P2.y=(s[kon].y);
       if(P1.y*P2.y<0) continue;
       line(P1,P2,&alfa,&beta);
       nb=0;
       l=s[0].l;
       dmin=fabs(alfa*s[0].x-s[0].y+beta)/sqrt(alfa*alfa+1);
       dd=0;
       for(i=0;i<size;i++){
          if(s[i].l!=l){
                     if(dmin<delta) dd=dd+dmin;
                     if(dmin<delta){
                     ss[0]=ss[0]+1;  ss[ss[0]]=nb;}
                     dmin=fabs(alfa*s[i].x-s[i].y+beta)/sqrt(alfa*alfa+1);
                     nb=i;
                     l=s[i].l;
                              }
              else { d=fabs(alfa*s[i].x-s[i].y+beta)/sqrt(alfa*alfa+1);
                     if(d<=dmin){dmin=d;nb=i;}
                   }
         }
        if(dmin<delta) {
                dd=dd+dmin;
                ss[0]=ss[0]+1;  ss[ss[0]]=nb;
               }
        if(dd<dd_min) {dd_min=dd;
                       for(j=0;j<=ss[0];j++){sluk[j]=ss[j];}
                      *aa=alfa; *bb=beta;
                     }
}}*d_min=dd_min;
}

/*&&&&&&&&&&&&&&&&& fv_track  &&&&&&&&&&&&&&&&&&&&*/
void PndFtsGeometricTracker::fv_track(int f,int SP[][17],straw * s,int size,int *nbTr,float wsp[][4],float *ff)
{
int i,j,k,lp1,lp2,lk1,lk2,kk,pocz,kon,ll,pl,licz,licz1,nr,lsa=0;
float alfa,beta,delta,z,zmin;//,delta2
float OD,d,dmin,tab[4][2];
point P1,P2;
int nowyslad[4][20],pom[size];

delta=0.5;
//delta2=1.4;
if(f==12){lp1=1;lp2=2;lk1=15;lk2=16;}
    else {lp1=33;lp2=34;lk1=47;lk2=48;}
for(pocz=0;(s[pocz].l)==lp1 || (s[pocz].l)==lp2;pocz++){
  for(kon=size-1;(s[kon].l==lk1) || (s[kon].l==lk2) || (s[kon].l>lk1-1);kon--){
    if(s[kon].l>lk2) continue;
    for(i=0;i<size;i++)  pom[i]=0;
    for(i=0;i<4;i++)
         for(j=0;j<20;j++) nowyslad[i][j]=0;
    P1.x=(s[pocz].z);  P1.y=(s[pocz].x);
    P2.x=(s[kon].z);   P2.y=(s[kon].x);
    if(s[pocz].r>s[kon].r) OD=s[pocz].r+0.5; else OD=s[kon].r+0.5;
    if(P1.y==P2.y) {alfa=0;} else line(P1,P2,&alfa,&beta);
    licz=0;
    if(alfa==0)
       for(i=0;i<size;i++){if(fabs(P1.y-s[i].x)<OD) pom[licz++]=i;}
    else for(i=0;i<size;i++){
       if(f==12){
        if((s[pocz].y>0) && (s[kon].y>0) && (s[i].y<0)) continue;
        if((s[pocz].y<0) && (s[kon].y<0) && (s[i].y>0)) continue;
        if((s[pocz].y!=0) && (s[kon].y==0) && (s[pocz].y*s[i].y<0)) continue;
       }
   if(fabs(alfa*s[i].z-s[i].x+beta)/sqrt(alfa*alfa+1)<OD) pom[licz++]=i;
                     }
   if(licz>=5){
     tang(P1,s[pocz].r,P2,s[kon].r,tab);
     kk=-1;zmin=1000;
     for(k=0;k<4;k++){
        ll=0;    pl=0;    dmin=1000; z=0;
        for(i=0;i<licz;i++){
            nr=pom[i];
            d=fabs(tab[k][0]*s[nr].z-s[nr].x+tab[k][1])/sqrt(1+pow(tab[k][0],2));
            d=fabs(d-s[nr].r);
            if(d<delta){
                if(pl==s[nr].l) {if(d<dmin) {
                                z=z-dmin; z=z+d;
                                dmin=d;
                                ll=ll-1;
                                nowyslad[k][++ll]=pom[i];
                                     }
                                 }
                 else {nowyslad[k][++ll]=nr;
                       pl=s[nr].l;
                       z=z+d;
                       dmin=d;
                      }
             }
        }
     nowyslad[k][0]=ll;
     licz1=0; if(nowyslad[k][0]!=0)licz1=1;
     for(j=2;j<=ll;j++){ if(s[nowyslad[k][j]].l != s[nowyslad[k][j-1]].l)
                    licz1++;
                    }
     if(licz1>=5){
            if(z<zmin){
               if(kk!=-1) {
                 if(nowyslad[kk][0]<=nowyslad[k][0])
                            {zmin=z;kk=k;}}
               else {zmin=z;kk=k;}
                }else if(nowyslad[kk][0]<nowyslad[k][0])
                            {zmin=z;kk=k;}
                }
      else { nowyslad[k][0]=0;}
   }
if(kk>=0){
  SP[lsa][0]=nowyslad[kk][0];
  for(i=1;i<=nowyslad[kk][0];i++)
      if(f==12){
       if(SP[lsa][s[nowyslad[kk][i]].l]!=0) SP[lsa][0]--;
       SP[lsa][s[nowyslad[kk][i]].l]=s[nowyslad[kk][i]].nr;
       wsp[lsa][0]=tab[kk][0];  wsp[lsa][1]=tab[kk][1];}
       else {
       if(SP[lsa][s[nowyslad[kk][i]].l-32]!=0) SP[lsa][0]--;
       SP[lsa][s[nowyslad[kk][i]].l-32]=s[nowyslad[kk][i]].nr;
       wsp[lsa][0]=tab[kk][0];  wsp[lsa][1]=tab[kk][1];}
if(nowyslad[kk][0]!=0) ff[lsa]=zmin/nowyslad[kk][0];
lsa++;
}
}
}
}
*nbTr=lsa;
}

/*****&&&&&&&&&&&&&&&&&&&&&&& tang &&&&&&&&&&&&&&&&&&&&******/
void PndFtsGeometricTracker::tang(point A,float rr1,point B,float rr2,float tab[4][2])
{
float s,x1,x2,y1,y2,r1,r2,xx1,xx2,yy1,yy2;//z,
struct point C,P1,P2;
//float b1,b2,OC;
float alfa,beta;//x0,ssin,ccos,

if(rr1>rr2){r1=rr1;x1=A.x;y1=A.y;r2=rr2;x2=B.x;y2=B.y;}
    else {r1=rr2;x1=B.x;y1=B.y;r2=rr1;x2=A.x;y2=A.y;}

C.x=(x2*r1-x1*r2)/(r1-r2);
C.y=(y2*r1-y1*r2)/(r1-r2);

s=pow(C.x-x1,2)+pow(C.y-y1,2);

xx1=(r1*r1*(C.x-x1)+r1*(C.y-y1)*sqrt(s-r1*r1))/s+x1;
xx2=(r1*r1*(C.x-x1)-r1*(C.y-y1)*sqrt(s-r1*r1))/s+x1;
yy1=(r1*r1*(C.y-y1)+r1*(C.x-x1)*sqrt(s-r1*r1))/s+y1;
yy2=(r1*r1*(C.y-y1)-r1*(C.x-x1)*sqrt(s-r1*r1))/s+y1;

s=(y1-yy1)*(C.y-yy1)/((xx1-x1)*(xx1-C.x));
if(s==1){P1.x=xx1;P1.y=yy1; P2.x=xx2;P2.y=yy2; }
       else{P1.x=xx1;P1.y=yy2;P2.x=xx2;P2.y=yy1; }

line(C,P1,&alfa,&beta);
tab[0][0]=alfa;
tab[0][1]=beta;
line(C,P2,&alfa,&beta);
tab[1][0]=alfa;
tab[1][1]=beta;

C.x=(x2*r1+x1*r2)/(r1+r2);
C.y=(y2*r1+y1*r2)/(r1+r2);

s=pow(C.x-x1,2)+pow(C.y-y1,2);

xx1=(r1*r1*(C.x-x1)+r1*(C.y-y1)*sqrt(s-r1*r1))/s+x1;
xx2=(r1*r1*(C.x-x1)-r1*(C.y-y1)*sqrt(s-r1*r1))/s+x1;

yy1=(r1*r1*(C.y-y1)+r1*(C.x-x1)*sqrt(s-r1*r1))/s+y1;
yy2=(r1*r1*(C.y-y1)-r1*(C.x-x1)*sqrt(s-r1*r1))/s+y1;

s=(y1-yy1)*(C.y-yy1)/((xx1-x1)*(xx1-C.x));
if(s==1){P1.x=xx1;P1.y=yy1; P2.x=xx2;P2.y=yy2; }
       else{P1.x=xx1;P1.y=yy2;P2.x=xx2;P2.y=yy1; }

line(C,P1,&alfa,&beta);
tab[2][0]=alfa;
tab[2][1]=beta;
line(C,P2,&alfa,&beta);
tab[3][0]=alfa;
tab[3][1]=beta;
}

/*****&&&&&&&&&&&&&&&&&&&&&&& line &&&&&&&&&&&&&&&&&&&&******/
void PndFtsGeometricTracker::line(point A, point B, float *aa, float *bb)
{
*aa=(B.y-A.y)/(B.x-A.x);
*bb=(A.y*(B.x-A.x)-A.x*(B.y-A.y))/(B.x-A.x);
}

void PndFtsGeometricTracker::matchEmpty( std::vector< PndFtsGeometricTracker::DataHitUnit >& ll, const std::vector< PndFtsGeometricTracker::DataHitUnit >& rr ) {

   for ( auto& l : ll )
       for ( auto& r : rr ) {
           if ( l.evtNr==r.evtNr && l.tubeID==r.tubeID /*&& l.trackID==r.trackID*/ ) {
//cout <<"Match: tubeid " << l.tubeID << " left-right track id " << l.trackID << "-"<<r.trackID<<endl;
   if (l.evtNr==0) l.evtNr = r.evtNr;
   if (l.detID==0) l.detID = r.detID;
   if (l.tubeID==0) l.tubeID = r.tubeID;
   if (l.chamberID==0) l.chamberID = r.chamberID;
   if (l.layerID==0) l.layerID = r.layerID;
   if (l.skew==0) l.skew = r.skew;
   if (l.iHit==0) l.iHit = r.iHit;
   if (l.x==0) l.x = r.x;
   if (l.y==0) l.y = r.y;
   if (l.z==0) l.z = r.z;
   if (l.dx==0) l.dx = r.dx;
   if (l.dy==0) l.dy = r.dy;
   if (l.dz==0) l.dz = r.dz;
   if (l.p==0) l.p = r.p;
   if (l.r==0) l.r = r.r;
   if (l.closestDistanceError==0) l.closestDistanceError = r.closestDistanceError;
   if (l.depcharge==0) l.depcharge = r.depcharge;
   //if (l.trackID==0) l.trackID = r.trackID;
   //l.trackID = r.trackID;
   if (l.mx==0) l.mx = r.mx;
   if (l.my==0) l.my = r.my;
   if (l.mz==0) l.mz = r.mz;

           }
       }

}


ClassImp(PndFtsGeometricTracker)