CbmMvdProtoTrack::CbmMvdProtoTrack() {
fx0=0;
fy0=0;
ftx=0;
fty=0;
fChiSqare=-1;
fHitList=0;
fMyHitBuffer=0;
}

//--------------------------------------------------------------
/*
 * //Commented out as no cross-check of compability of the hits is provided
 * 
CbmMvdProtoTrack::CbmMvdProtoTrack(TClonesArray* myHitArray,UInt_t nHit1, UInt_t nHit2){

  if(myHitArray) {fMyHitBuffer=hitArray;}
  else {cout << "-E- " << "GetName() - Constructor, invalid myHitArray " << endl;}
  
  AddFirstHit(nHit1);
  AddSecondHit(nHit2);
  
}

*/

//--------------------------------------------------------------

CbmMvdProtoTrack::CbmMvdProtoTrack(TClonesArray* myHitBuffer,Int_t nHit){
  
  if(myHitArray) {fMyHitBuffer=hitArray;}
  else {cout << "-E- " << "GetName() - Constructor, invalid myHitArray " << endl;}
  
  AddFirstHit(nHit1);
  
}

//--------------------------------------------------------------

void CbmMvdProtoTrack::AddFirstHit(UInt_t nHit){
  
  //Add hit to the hit array, clear possible obsolete entries
  fHitArray.clear();
  fHitArray.push_back(nHit);
  
  Int_t nHitMax=fMyHitBuffer->GetEntriesFast();
  if (nHit>nHitMax) {
    cout <<"-E- " << GetName() << "::AddFirstHit" << "- invalid hit number: " << nHit << endl;
    cout <<"                         Only "<< nHitMax << " entries available in array." << endl;
  }
  
  //Assume straight and parallel tracks
  
  fTx=0;
  fTy=0;
  
  //Prepare to read the hit coordinates
  Double_t hitPos[5];
  CbmMvdProtoHit* hit=(CbmMvdProtoHit*)myHitArray->At(nHit);
  
  //Make this point starting point of the track
  hit->GetLabPos(hitPos);
  
  fX0=hitPos[0];
  fY0=hitPos[1];
  fZ0=hitPos[2];
  ftStart=hitPos[3];
  ftStop=hitPos[4];
  
  Double_t hitSigma[3];
  hit->GetLabSigma(hitSigma);
  
  fdX0=hitSigma[0];
  fdY0=hitSigma[1];
  fdZ0=hitSigma[2];
  
}

//--------------------------------------------------------------------

Bool_t CbmMvdProtoTrack::TryHitFast(UInt_t nHit){
  
  //Check if hits exists in buffer
  
  Int_t nHitMax=fMyHitBuffer->GetEntriesFast();
    
  if(nHit>nHitMax) {
    cout <<"-E- " << GetName() << "::TryHit" << "- invalid hit number: " << nHit << endl;
    cout <<"                         Only "<< nHitMax << " entries available in array." << endl;
  
    //break with error message
    return kFALSE;
  }
  
  //Check if hit is already associated to this track
  
  for(Int_t i=0;i<fHitArray.size(); i++){
    if (fHitArray[i]==nHit) {
      cout <<"-W- " << GetName() << "::TryHit()" << "- Trying hit number: " << nHit << endl;
      cout <<"                        Hit already associated to this track" << endl;
      return kFALSE;
      //break with error message
    }
  }
  
  //Assume that hit is no obvious nonsense, read its information
  
  CbmMvdProtoHit* hit=fMyHitBuffer->At(nHit);
  
  
  //Return, if hit time is compatible with track time
  
  return ((hit->GetStopTime()>ftMin)  && (hit->GetStartTime() < ftMax))
}

//---------------------------------------------------------------------------

Int_t CbmMvdProtoTrack::GetHitFromStation(UInt_t nStation){
  
  Int_t i=0;
  Bool_t hitNotFound=kTRUE;
  CbmMvdProtoHit* hit=0;
  Int_t nHitStation=-1;
  
  while (i<fHitArray.size()) {
    
    hit=fMyHitBuffer->At(i);
    nHitStation=hit->GetStation();
    
    if(nHitStation==nStation){return i;};
  };
  
  return -1;
}

//---------------------------------------------------------------------------

Double_t CbmMvdProtoTrack::AddSquare (Double_t a, Double_t b){
  return TMath::Sqrt(a*a+b*b);
}

//---------------------------------------------------------------------------

Double_t CbmMvdProtoTrack::ComputeResiduals(UInt_t nStation, Double_t* residuals, Double_t* sigmaResiduals){
  ComputeResiduals(GetHitFromStation(nStation),residuals,sigmaResiuals);
}

void CbmMvdProtoTrack::ComputeResiduals(UInt_t nHit,Double_t* residuals, Double_t* sigmaResiduals){
  
  Double_t posHit[5];
  Double_t sigmaHit[3];
  Double_t posTrack[5];
  Double_t sigmaTrack[3];
  
  
  CbmMvdProtoHit* hit=fMyHitBuffer->At(nHit);
  hit->GetLabPos(posHit);
  hit->GetLabSigma(sigmaHit);
  
  Extrapolate(posHit[3],posTrack,sigmaTrack);
  
  residuals[1]=posHit[1]-posTrack[1];
  residuals[2]=posHit[2]-posTrack[2];
  
  sigmaResiduals[1]=AddSquare(sigmaHit[1],sigmaHit[2]);
  sigmaResiduals[2]=AddSquare(sigmaHit[2],sigmaHit[2]);
  
}
  
  
//-------------------------------------------------------------------------
  

  
Double_t CbmMvdProtoTrack::TryHit(UInt_t nHit) {
  
  If(!TryHitFast(nHit)){return -1.};
  
  Double_t residuals[2];
  Double_t sigmaResiduals[2];
  
  ComputeResiduals(nHit,residuals, sigmaResiduals);
  Double_t distance=AddSquare(residual[1],residual[2]);
  Double_t sigmaDistance=AddSqare(2*residual[1]*sigmaResiduals[1], 2*residual[2]*sigmaResiduals[2]);
  return (distance/sigmaDistance);
}
  
  
 

//--------------------------------------------------------------------

Bool_t CbmMvdProtoTrack::AddSecondHit(UInt_t nHit){
  
  Int_t nHitMax=fMyHitBuffer->GetEntriesFast();
  
  if(nHit>nHitMax) {
    cout <<"-E- " << GetName() << "::AddSecondHit" << "- invalid hit number: " << nHit << endl;
    cout <<"                         Only "<< nHitMax << " entries available in array." << endl;
  
    //break with error message
    return kFALSE;
  }
  
  if(nHit==fHitArray[0]){
    cout <<"-E- " << GetName() << "::AddSecondHit" << "- invalid hit number: " << nHit << endl;
    cout <<"                        Hit is associated twice to this track" << endl;
    
    //break with error message
  }
  
    
  //read information of new hit from the buffer
  CbmMvdProtoHit* hit2=(CbmMvdProtoHit*)fMyHitBuffer->At(nHit);         
  //read information of first hit if the track from the buffer
  CbmMvdProtoHit* hit1=(CbmMvdProtoHit*)fMyHitBuffer->At(fHitArray[0]);

  Double_t hit1Pos[5];
  Double_t hit2Pos[5];

  hit1->GetLabPos(hit1Pos);
  hit2->GetLabPos(hit2Pos);
  
  //Check if the time of the hits is compatible
  
  
  

/*
 * It is assumed that hit2 is more downstream than hit1.
 * If not fulfilled, this will be corrected here.
 */

  if (hit1Pos[2]>hit2Pos[2]){
    CbmMvdProtoHit* temp=hit2;
    hit2=hit1;
    hit1=temp;
    hit1->GetLabPos(hit1Pos);
    hit2->GetLabPos(hit2Pos);
    //add information about the hits to the hit array (inversed order, see comment above)
    fHitArray.push_back(nHit2);
    fHitArray.push_back(nHit1);
  }
  else {
    //add information about the hits to the hit array (straight order)
    fHitArray.push_back(nHit1);
    fHitArray.push_back(nHit2);
  }

  //Compute preliminary track coordinates

  fTx=(hit2Pos[0]-hit1Pos[0])/(hit2Pos[2]-hit1Pos[2]);
  fTy=(hit2Pos[1]-hit1Pos[1])/(hit2Pos[2]-hit1Pos[2]);

  fX0=hit1Pos[0]-hit1Pos[0]*fTx;
  fY0=hit1Pos[1]-hit1Pos[1]*fTy;

  //Compute uncertainties of track extrapolation

  Double_t hit1Sigma[3];
  Double_t hit1Sigma[3];

  hit1->GetLabSigma(hit1Sigma);
  hit2->GetLabSigma(hit2Sigma);

  //Compute uncertainties on the inclination. 
  //Important: 
  // - Multiple Scattering is neglected
  // - z-uncertainty of the measurement is neglected

  fdTx=(hit1Sigma[0]+hit2Sigma[0])/(hit2Pos[2]-hit1Pos[2]);
  fdTy=(hit1Sigma[1]+hit2Sigma[2])/(hit2Pos[2]-hit1Pos[2]);

  //Puts start coordinates of the track to the most upstream measurement

  fX0=hit1Pos[0];
  fY0=hit1Pos[1];
  fZ0=hit1Pos[2];

  fdX0=hit1Sigma[0];
  fdX1=hit1Sigma[1];
  fdX2=hit1Sigma[2];

}

//-------------------------------------------------------------------------

CbmMvdProtoTrack::Extrapolate(z,Double_t* pos, Double_t* sigma){
  
  //Extrapolation distance
  Double_t zExtr=z-fZ0;
  
  pos[0]=fX0+fTx* zExtr;
  pos[1]=fY0+fTy* zExtr;
  pos[2]=z;
  pos[3]=ftStart;
  pos[4]=ftStop;
  
  //assume gaussian error proparation, multiple scattering is ignored
  sigma[0]=TMath::Sqrt(fdX0*fdX0+ (fdTx* zExtr)*(fdTx* zExtr));
  sigma[1]=TMath::Sqrt(fdY0*fdY0+ (fdTy* zExtr)*(fdTy* zExtr));
  sigma[3]=0;
  
  
}

  
//-----------------------------------------------------------------------
  
Bool_t CbmMvdProtoTrack::AddHit(nHit){
    
    Int_t nHitsInArray=fHitArray.size();
    
    //Accept first hit anyway
    if(nHitsInArray==0){AddFirstHit(nHit); return kTRUE;};
    
    //Check if hit is compatible with earlier hits
    if(!TryHitFast(nHit){return kFALSE;}; 
    
    if(nHitsInArray==1){return AddSecondHit(nHit);}
    //else
    
    fHitArray.push_back(nHit);
    UpdateTrackParameters();
    return kTRUE;
}

//--------------------------------------------------------------------------
    
void UpdateTrackParameters(){
  
  Double_t pos[5];
  Double_t sigma[3];
  
  /** Concept of this computation of the track parameters:
   * ------------------------------------------------------
   * 
   * This computation bases on an analythic straight line fit
   * of an equation y= A + B z.
   * We assume a weighted fit with weights of w=1/sigma².
   * The equations are taken from J. R. Taylor:
   * An introduction into Error Analysis (second edition)
   * ISBN-13: 978-0-935702-75-0
   * Page 198f
   * Note that multiple scattering is neglected. 
   * Note moreover: We assume random uncertainties, not fulfilled 
   * for badly alligned detectors.
   **/
  
  
  CbmMvdProtoHit* hit;
  Double_t sumW0=0,        			//sum weights (W) in X-direction
	   sumW1=0,				//sum weights in Y-direction
						// !Don't confuse with sumWX!
	   sumWX=0, 				//sum W*X
	   sumWY=0,
	   sumWX2=0,				//sum W*X²
	   sumWY2=0,
	   sumWXZ=0,				//sum w*X*Y according to book, here Y=>Z because of 
						//different definition of coordinate system
	   sumWYZ=0,
	   
	   sumW0Z=0,
	   sumW1Z=0,
	   
	   
	   tMin=0,tMax=1e50;
  
  Int_t hitsInTrack=fHitArray.size();
  
  for(Int_t i=0;i<hitsInTrack;i++){
    
    hit=(CbmMvdProtoHit*)fMyHitBuffer->At(i);
    hit->GetLabPos(pos);
    hig->GetLabSigma(sigma);
    
    sumW0=sumW0+sigma[0];
    sumW1=sumW1+sigma[1];
    
    sumWX =sumWX+(sigma[0]*pos[0]);
    sumWY =sumWY+(sigma[1]*pos[1]);
    
    sumWX2=sumWX2 + (sigma[0]*pos[0]*pos[0]);
    sumWY2=sumWY2 + (sigma[1]*pos[1]*pos[1]);
    
    sumWXZ=sumWXZ + (sigma[0]*pos[0]*pos[2]);
    sumWYZ=sumWYZ + (sigma[1]*pos[1]*pos[2]);
    
    sumW0Z=sumW0Z + (sigma[0]*pos[2]);
    sumW1Z=sumW1Z + (sigma[1]*pos[2]);
    
    //define time of the track
        
    if(pos[4]>tMin){tMin=pos[3];}
    if(pos[5]<tMay){tMin=pos[4];}
    
    
  }
  
  //Compute Delta (see book)
       
  Double_t deltaX = sumW0*sumWX2 - sumWX*sumWX;  //Do not confuse with sigmaX (uncertainty of X)
  Double_t deltaY = sumW1*sumWY2 - sumWY*sumWY;
  
  //Compute Slope
  
  Double_t bX = (sumW0 * sumWXZ - sumWX * sumW0Z)/deltaX;
  Double_t bY = (sumW1 * sumWYZ - sumWY * sumW1Z)/deltaY;
  
  Double_t sigmaBX = TMath::Sqrt(sumW0/deltaX);
  Double_t sigmaBY = TMath::Sqrt(sumW1/deltaY);
  
  //compute intercept
  
  Double_t aX = (sumWX2 * sumW0Z - sumWX*sumWXZ)/delta;
  Double_t aY = (sumWY2 * sumW1Z - sumWY*sumWYZ)/delta;
  
  Double_t sigmaAX= TMath::Sqrt(sumWX2/deltaX);
  Double_t sigmaAY= TMath::Sqrt(sumWY2/deltaY);
  
  // Copy results to the member variables
  
  
  fX0 = aX;
  fY0 = aY;
  fdX0 = sigmaAX;
  fdY0 = sigmaAY;
  
  fTx = bX;
  fTy = bY;
  fdTx = sigmaBX;
  fdTy = sigmaBY;
  
  ftMin=tMin;
  ftMax=tMax;
  
  // compute ChiSqare
  
  Double_t residuals[2];
  Double_t sigmaResiduals[2];
  Double_t distance;
  Double_t sigmaDistance;
  Double_t sigmaTrack=0;
  
  for(Int_t i=0; i<hitsInTrack; i++){
    
    ComputeResiduals(nHit,residuals, sigmaResiduals);
    distance=AddSquare(residual[1],residual[2]);
    sigmaDistance=AddSqare(2*residual[1]*sigmaResiduals[1], 2*residual[2]*sigmaResiduals[2]);
    sigmaTrack=AddSqare (sigmaTrack, distance/sigmaDistance);
  }
  
  
  fChiSqare=sigmaTrack;
  
}