HarpoKalmanNew.cxx

Go to the documentation of this file.

// 
// File HarpoKalman.cxx
//
#include "HarpoKalmanNew.h"
#include "HarpoConfig.h"
#include "HarpoDetSet.h"
#include "HarpoDebug.h"
#include "HarpoDccEvent.h"
#include "Pmm2Event.h"
#include "HarpoEvent.h"
#include "HarpoRecoEvent.h"
#include "MakeNiceHisto.h"

#include "TFile.h"
#include "TStyle.h"
#include "TCanvas.h"
#include "TLatex.h"
#include "TGraph.h"
#include "TF1.h"
#include "TArrow.h"
#include "TEllipse.h"
#include "TLinearFitter.h"
#include "TMath.h"
#include "TSystem.h"

#include <cstdlib>
#include <cstring>
#include <cassert>
#include <fstream>
#include <iostream>


ClassImp(HarpoKalmanNew);

void   HarpoKalmanNew::print()
 {
   unsigned int nd; // number of detectors
   HarpoEventHeader *hdr = fEvt->GetHeader();

   assert(hdr != NULL);
   hdr->print();
   
   for (nd = 0; nd < gkNDetectors; nd++) {
     //     if (fEvt->isdataExist(nd)) {
       HarpoDccMap *plane = fEvt->GetDccMap(nd);
       if (plane != NULL )plane->print();
   }
 }

void   HarpoKalmanNew::process()
{
  //  Bool_t drawEvent = kFALSE;
  nEvents++;
  if(gHarpoDebug>0)
    Info("process","Event %d",nEvents);

  // for(Int_t i = 0; i< NTRACK; i++)
  //   fNclTrack[i] = 0;
  if(!fEvt->GetRecoEvent())  // Reconstructed data (clusters, tracks, matched tracks)
    return;
  HarpoRecoEvent* reco = fEvt->GetRecoEvent();
  reco->SetTrackType(KALMAN);
  // Clusters in the event (both X and Y directions)
  TClonesArray* clArray = reco->GetClustersArray();
  Int_t nCl = clArray->GetEntries();
  TClonesArray* vArray = reco->GetVertexArray();
  if(!vArray) return;
  Int_t nV = vArray->GetEntries();
  if(nV<=0) return;

  //Info("process","A");

  //  TMatrixD Xorig(4,1);  
  Double_t inc = fResKalman; // Space resolution
  Double_t theta = fScatKalman; // Multiple scattering
  TMatrixD Corig(4,4);
  for(int a = 0;a<4;a++){
    for(int b =0;b<4;b++){
      if(a==b){
        if(a<2)
          Corig[a][b]=inc*inc;                                      //initialisation de C
        else
          Corig[a][b]=(2*inc)*(2*inc)+theta*theta;
      }
      else
        Corig[a][b]=0;
    }
  }

  //Info("process","nV = %d",nV);
  for(Int_t plane = 0; plane<2; plane++){
    //    Info("process","______________PLANE %d _________________________________",plane);
    InitPlane(clArray, plane);
    //Info("process","C %d",plane);
    Int_t iTr = 0;
    TArrayI* arr[NTRACK];
    Double_t xStart[NTRACK][4];
    Double_t xEnd[NTRACK][4];
    Int_t ncl[NTRACK];
    //    Int_t track[nV][2];
    Int_t vert[NTRACK][2];
    for(Int_t iV = 0; iV<nV; iV++){
      HarpoRecoVertex* vertex = (HarpoRecoVertex*)vArray->At(iV);
      if(vertex->GetPlane() != plane) continue;
      //Info("process","D %d",iV);
      Double_t x = vertex->GetXvertex();
      Double_t z = vertex->GetZvertex();
      Int_t color = kRed, fill = 0;//3001;
      //TArrayI * arr = new TArrayI(nCl);
      //Double_t angle = 0;
      TMatrixD Xorig(4,1);
      Xorig[0][0] = x;
      Xorig[1][0] = z;
      Xorig[2][0] = vertex->GetPxVertex();
      Xorig[3][0] = vertex->GetPzVertex();
      //     Info("process","%g %g   %g %g",Xorig[0][0],Xorig[1][0],Xorig[2][0],Xorig[3][0]);
      Int_t nTr = 2;
      if(vertex->GetThetaVertex()==0) nTr = 1;
      Double_t cos = TMath::Cos(vertex->GetThetaVertex()*0.5);
      Double_t sin = TMath::Sin(vertex->GetThetaVertex()*0.5);
      // track[iV][0] = -1;
      // track[iV][1] = -1;
      for(Int_t j = 0; j<nTr; j++){
        color = iTr+2;
        Xorig[2][0] = cos*vertex->GetPxVertex()-sin*vertex->GetPzVertex();
        Xorig[3][0] = cos*vertex->GetPzVertex()+sin*vertex->GetPxVertex();
        if(vertex->GetThetaVertex()>0){
          Xorig[0][0] = x+20*Xorig[2][0];
          Xorig[1][0] = z+20*Xorig[3][0];
        }
        sin *= -1;
        if(fCanvas[plane]){
          TArrow* arrow = new TArrow();
          arrow->SetLineColor(kGray);
          arrow->SetLineWidth(2);
          arrow->SetFillColor(color);
          fCanvas[plane]->cd();
          arrow->DrawArrow(z,x,z+30*Xorig[3][0],x+30*Xorig[2][0],0.01);
          // fHist[plane]->GetYaxis()->SetRangeUser(Xorig[0][0] - 10,Xorig[0][0] + 10);
          // fHist[plane]->GetXaxis()->SetRangeUser(Xorig[1][0] - 10,Xorig[1][0] + 10);
          fCanvas[plane]->Update();
        }
        //Info("process","_________________________________________________________");
        for(Int_t i = 0; i<4; i++) xStart[iTr][i] = Xorig[i][0];

        arr[iTr] = new TArrayI(nCl);
        for(Int_t i = 0; i<nCl; i++) arr[iTr]->AddAt(-1,i);
        //      FindNextClosest(Xorig,Corig,angle,0,iTr,clArray,arr[iTr],0,plane,color,fill,0,0);
        color = iTr+2;
        std::vector<TMatrixD> output = NextStep(Xorig,Corig,iTr,clArray,arr[iTr],0,plane,color,fill,0,0,kTRUE);
        TMatrixD Xend = output[1];
        for(Int_t i = 0; i<2; i++) xEnd[iTr][i] = Xend[i][0];
        for(Int_t i = 2; i<4; i++) xEnd[iTr][i] = -Xend[i][0];
        //      track[iV][j] = iTr;
        vert[iTr][0] = iV;
        vert[iTr][1] = j;
        ncl[iTr] = 0;
        while(arr[iTr]->At(ncl[iTr])>=0) ncl[iTr]++;
        //if(ncl[iTr]<5) continue;
        // HarpoRecoKalmanTracks* tr = new HarpoRecoKalmanTracks(iTr,100000,0,0,0,0,0,100,plane,0,0,0,1,0,0,0,-1);
        // //    Info("SpliceTracks","Add graph %p",fGraphs[plane][itr]);
        // fEvt->GetRecoEvent()->AddTracks(tr);
        iTr++;
      }
    }

    Int_t nTracks = 0;
    Int_t usedTr[iTr];
    for(Int_t i1 = 0; i1<iTr; i1++) usedTr[i1] = 0;
    for(Int_t i1 = 0; i1<iTr; i1++){
      if(!arr[i1]) continue;
      if(usedTr[i1]) continue;
      Int_t ncl1 = ncl[i1];
      Int_t dFirstMin = 1000;//, dFirstMax = -1000, dLastMin = 1000, dLastMax = -1000;
      Int_t iFirstMin = -1;//, iFirstMax = -1, iLastMin = -1, iLastMax = -1;
      Bool_t endMin = kFALSE;
      Int_t nMerge = 1;
      Int_t merge[iTr];
      for(Int_t i = 1; i<iTr; i++) merge[i] = -1;
      merge[0] = i1;
      for(Int_t i2 = i1+1; i2<iTr; i2++){
        if(!arr[i2]) continue;
        Int_t ncl2 = ncl[i2];
        Bool_t first = kTRUE;
        Int_t nComm = 0; // number of common clusters
        Int_t j1first = -1, j2first = -1;
        //      Int_t j1last = -1;
        Int_t j2last = -1;
        for(Int_t j1 = 0; j1<ncl1; j1++){
          if(arr[i1]->At(j1)<0){
            Info("process","???????????????");
            ncl1 = j1;
            break;
          }
          for(Int_t j2 = 0; j2<ncl2; j2++){
            if(arr[i2]->At(j2)<0){
              ncl2 = j2;
              break;
            }
            if(arr[i2]->At(j2)==arr[i1]->At(j1)){
              if(first){j2first = j2;   j1first = j1; first = kFALSE;}
              j2last = j2;
              //j1last = j1;
              nComm++;
              //Info("process","track %d, %d %d %d %d",i2,j1first,j2first,j1last,j2last);
              break;
            }
          }
        }
        //      Info("process","** ncl[%d] = %d, ncl[%d] = %d => nComm = %d",i1,ncl1,i2,ncl2,nComm);
        if(nComm>=8 || nComm>0.3*ncl1 || nComm>0.3*ncl2){
          // Merge tracks i1 and i2
          usedTr[i2] = 1; // remove track i2
          Int_t dFirst = -1; //, dLast = -1;
          //Info("process","** track %d (%d), %d %d %d %d",i2,nComm,j1first,j2first,j1last,j2last);
          if(j2first<j2last){ // both tracks in same direction
            dFirst = j1first - j2first;
            // dLast = (ncl1-j1last) - (ncl2-j2last);         
            //Info("process","track %d, dFirst = %d, (%g %g)",i2,dFirst,xStart[i2][1],xStart[i2][0]);
          }else{ // opposite directions
            dFirst = j1first - (ncl2-j2first);
            // dLast = (ncl1-j1last) - j2last;        
            //Info("process","track %d, dFirst = %d, (%g %g)",i2,dFirst,xEnd[i2][1],xEnd[i2][0]);
          }
          if(dFirst<dFirstMin){ dFirstMin = dFirst; iFirstMin = i2; endMin = (j2first<j2last);}
          //      if(dFirst>dFirstMax){ dFirstMax = dFirst; iFirstMax = i2; endMin = (j2first<j2last)}
          // if(dLast<dLastMin){ dLastMin = dLast; iLastMin = i2;}
          // if(dLast>dLastMax){ dLastMax = dLast; iLastMax = i2;}
          merge[nMerge] = i2; nMerge++;
        }
      }
      //      Info("process","Merge tracks with %d => %d",i1,nTracks);
      for(Int_t i = 0; i<nMerge; i++){
        Int_t iTrack = merge[i];
        //Info("process","      track %d (%d)",iTrack,ncl[iTrack]);
        for(Int_t j = 0; j<ncl[iTrack]; j++){
          if(arr[iTrack]->At(j)<0) break;
          //Info("process","       %d",arr[iTrack]->At(j));
          HarpoRecoClusters* cluster = (HarpoRecoClusters*)clArray->At(arr[iTrack]->At(j));
          cluster->AddIdClusterTrack(nTracks);
        }
      }
      
      TMatrixD Xorig(4,1);
      if(dFirstMin>=0 || nMerge==1){
        //Info("process","track %d, dFirst = %d",i1,dFirstMin);
        for(Int_t i = 0; i<4; i++) Xorig[i][0] = xStart[i1][i];
      }else{
        //Info("process","track %d, dFirst = %d => %d %d",i1,dFirstMin,iFirstMin,endMin);
        if(endMin)
          for(Int_t i = 0; i<4; i++) Xorig[i][0] = xStart[iFirstMin][i];
        else
          for(Int_t i = 0; i<4; i++) Xorig[i][0] = xEnd[iFirstMin][i];
      }
      //      Info("process","Start: (%g %g   %g %g)",Xorig[0][0],Xorig[1][0],Xorig[2][0],Xorig[3][0]);
      Int_t color = 2+nTracks, fill = 3001+nTracks;
      for(Int_t icl = 0; icl<nCl; icl++)
        used[icl] = -1000;
      for(Int_t i = 0; i<2; i++) Xorig[i][0] = Xorig[i][0]-10*Xorig[2+i][0];
      fill = 3002;


        if(fCanvas[plane]){
          TArrow* arrow = new TArrow();
          arrow->SetLineColor(kGray+2);
          arrow->SetLineWidth(4);
          arrow->SetLineStyle(2);
          arrow->SetFillColor(color);
          fCanvas[plane]->cd();
          arrow->DrawArrow(Xorig[1][0],Xorig[0][0],Xorig[1][0]+30*Xorig[3][0],Xorig[0][0]+30*Xorig[2][0],0.01);
          fCanvas[plane]->Update();
        }


      std::vector<TMatrixD> output = NextStep(Xorig,Corig,nTracks,clArray,0,0,plane,color,fill,1,0,kFALSE);

      //      Info("process","size = %ld",output.size());
      if(output.size()<5) continue;
      TMatrixD extra = output[4];
      Int_t nCl = Int_t(extra[0][0]);
      if(nCl<5) continue;

      Double_t angle = extra[1][0]/nCl;

      Double_t rms[8];
      for(Int_t i = 0; i<8; i++){
        if(nCl-i < 1) break;
        //      Double_t angle = extra[3*i+1][0]/(nCl-i);
        Double_t dangle = extra[3*i+2][0]/(nCl-i);
        Double_t dangleSq = extra[3*i+3][0]/(nCl-i);
        if(dangleSq-dangle*dangle<1e-6) rms[i] = -1;
        else rms[i] = TMath::Sqrt(dangleSq-dangle*dangle);
        //Info("process","%g %g %g   %g",angle,dangle,dangleSq, dangleSq-dangle*dangle);
      }
      //Info("process","%g %g %g %g %g %g %g %g",rms[0],rms[1],rms[2],rms[3],rms[4],rms[5],rms[6],rms[7]);
      //      Info("process","Angles: %g %g %g",angle,rmsAngle,rmsAngle2);
      Double_t rmsAngle = rms[0];
      Double_t rmsAngle2 = rms[1];

      Double_t qTot = 0;
      //HarpoRecoKalmanTracks* tr = new HarpoRecoKalmanTracks(nTracks,qTot,angle,rmsAngle,rmsAngle2,0,0,nCl,plane,0,0,0,1,0,0,0,-1);
      HarpoRecoKalmanTracks* tr = new HarpoRecoKalmanTracks(nTracks,qTot,angle,rmsAngle,rmsAngle2,0,0,nCl,plane,rms[0],rms[1],rms[2],rms[3],rms[4],rms[5],rms[6],rms[7]);
      fEvt->GetRecoEvent()->AddTracks(tr);


      for(Int_t i = 0; i<nMerge; i++){
        Int_t iTrack = merge[i];
        Int_t iV = vert[iTrack][0];
        Int_t j = vert[iTrack][1];
        HarpoRecoVertex* vertex = (HarpoRecoVertex*)vArray->At(iV);
        if(j==0) vertex->SetTr1Vertex(nTracks);
        if(j==1) vertex->SetTr2Vertex(nTracks);
      }
      nTracks++;
      
      
    }

    if(fCanvas[plane]){
      fHist[plane]->GetXaxis()->SetRangeUser(0,NADC);
      fHist[plane]->GetYaxis()->SetRangeUser(0,NCHAN);
      fCanvas[plane]->Update();
    }
  }
  //  Info("process","DONE");
}

Int_t  HarpoKalmanNew::InitPlane(TClonesArray* clArray, Int_t plane)
{

  for(Int_t i = 0; i<NTRACK; i++){
    if(fGraph[plane][i]) fGraph[plane][i]->Delete();
    if(fGraph2[plane][i]) fGraph2[plane][i]->Delete();
    fGraph[plane][i] = new TGraph();
    fGraph2[plane][i] = new TGraph();
  }

  fNtr[plane] = 0;
  Int_t nCl = clArray->GetEntries();
  if(nCl>=4000){
    Warning("InitPlane","Too many clusters %d",nCl);
    return 0;
  }
  TGraph* gCl = 0;
  if(fCanvas[plane]){
    gCl = new TGraph();
    gCl->SetMarkerStyle(7);
  }
  // Organise Clusters in layers
  for(Int_t icl = 0; icl<nCl; icl++){
    reused[icl] = 0;
    used[icl] = -1000;
    fTrack[icl] = -1000;
    HarpoRecoClusters* cluster = (HarpoRecoClusters*)clArray->At(icl);
    //    Int_t index = cluster->GetIndex();
    if(cluster->GetPlane() != plane) continue;
    if(cluster->GetQ() < fQmin) continue;
    if(cluster->GetType() != BLOCCLUSTER) continue;
    if(fCanvas[plane])
      gCl->SetPoint(gCl->GetN(),cluster->GetZ(),cluster->GetX());
  }
  if(fCanvas[plane]){
    fCanvas[plane]->cd();
    gCl->Draw("Psame");
  }

  return 1;
}


Int_t HarpoKalmanNew::GetNclCommon(TClonesArray* clArray, Int_t plane, Int_t iTr1, Int_t iTr2)
{

  Int_t nComm = 0;
  Int_t nCl = clArray->GetEntries();
  for(Int_t icl = 0; icl<nCl; icl++){ // For all clusters
    HarpoRecoClusters* cluster = (HarpoRecoClusters*)clArray->At(icl);
    if(cluster->GetPlane() != plane) continue;
    if(!cluster->CheckIdClusterTrack(iTr1)) continue;
    if(!cluster->CheckIdClusterTrack(iTr2)) continue;
    nComm++;
  }
  return nComm;
}




std::vector<TMatrixD> HarpoKalmanNew::NextStep(TMatrixD X, TMatrixD C, Int_t iTr, TClonesArray* clArray, TArrayI* arr, Int_t ncl, Int_t plane, Int_t color, Int_t fill, Int_t smooth, Int_t qold, Bool_t finder)
{
  if(gHarpoDebug>2) Info("NextStep","SMOOTH = %d, FINDER = %d",smooth,finder);
  std::vector<TMatrixD> output;
  //  Double_t deltaRes = fResFinder;
  //  Double_t deltaScat = fScatFinder;
  Double_t theta = fScatKalman; // Multiple scattering

  //  Int_t ntr = 0;

  Double_t norm = TMath::Sqrt(X[2][0]*X[2][0]+X[3][0]*X[3][0]);
  X[2][0] /= norm;
  X[3][0] /= norm;

  if(fCanvas[plane]){
    TArrow* arrow = new TArrow();
    arrow->SetLineColor(kBlack);
    arrow->SetFillColor(color);
    fCanvas[plane]->cd();
    arrow->DrawArrow(X[1][0],X[0][0],X[1][0]+5*X[3][0],X[0][0]+5*X[2][0],0.01);
    // fHist[plane]->GetYaxis()->SetRangeUser(X[0][0] - 10,X[0][0] + 10);
    // fHist[plane]->GetXaxis()->SetRangeUser(X[1][0] - 10,X[1][0] + 10);
    fCanvas[plane]->Update();
  }


  Int_t iclMin = FindClosestNeighbour(X,iTr,clArray, plane, color, fill, finder);
  if(iclMin==-1){ // END OF TRACK
    output.push_back(C);
    output.push_back(X);
    return output;
  }
  //Info("NextStep","arr->AddAt(%d,%d)",iclMin,ncl);
  if(arr) arr->AddAt(iclMin,ncl);
  HarpoRecoClusters* cluster = (HarpoRecoClusters*)clArray->At(iclMin);
  Double_t xMin = cluster->GetX();
  Double_t zMin = cluster->GetZ();
  Int_t qMin = cluster->GetQ();
  Double_t resMin = GetResolution(cluster);
 
  // Position of the cluster (x,z) along the line of propagation
  Double_t lambdaMin = (xMin-X[0][0])*X[2][0]+(zMin-X[1][0])*X[3][0];

  
  if(gHarpoDebug>2)
    Info("FindNextClosest","Add New Cluster");

  TMatrixD F(4,4);      
  for(int i = 0;i<4;i++){
    for(int j =0;j<4;j++){
      F[i][j]=0;
      if(i==j) F[i][j]=1;                               
    }
  }
  TMatrixD Xproj(4,1);
  TMatrixD H(2,4);
  TMatrixD Ht(2,4);
  TMatrixD Q(4,4);
  TMatrixD G(2,2);
  TMatrixD V(2,2);
  
  Double_t inc = resMin;//fResKalman; // Space resolution
  if(inc==0) inc = 0.000001;
  Double_t v = X[2][0]*X[2][0] + X[3][0]*X[3][0];
  for(int i = 0;i<4;i++){
    for(int j =0;j<4;j++){
      if(i==j){
        if(i>1)
          Q[i][j]=theta*theta*(1-X[i][0]*X[i][0]/v);                //initialisation de Q
        else
          Q[i][j]=0;
      }
    }
  }
  for(int i = 0;i<2;i++){
    for(int j =0;j<2;j++){
      if(i==j){
        H[i][j]=1;
        V[i][j]=inc*inc;
        G[i][j]=1./inc*inc;
      }else{
        H[i][j]=0;
        V[i][j]=0;
        G[i][j]=0;
      }
    }
  }
  Ht=H;
  Ht.T();
  // G=V;
  // G.Invert();
  
  
  TMatrixD Ft(4,4);
  TMatrixD Cinv(4,4);
  TMatrixD M(2,1);
  TMatrixD Cproj(4,4);
  TMatrixD Xfilter(4,1);
  TMatrixD Cfilter(4,4);
  TMatrixD Cprojinv(4,4);  

  F[0][2]=lambdaMin;
  F[1][3]=lambdaMin;
  Xproj=F*X;
  // Kalman matrices calculations
  Ft = F;
  Ft.T();
  M[0][0] = xMin;
  M[1][0] = zMin;
  Cproj=F*C*Ft+Q;
  Cprojinv=Cproj;
  Cprojinv.Invert();
  Cinv=Cprojinv+Ht*G*H;
  Cfilter=Cinv;
  Cfilter.Invert();
  Xfilter=Cfilter*((Cprojinv*Xproj)+(Ht*G*M));

  used[iclMin] = iTr;

  if(gHarpoDebug>2)
    Info("FindNextClosest","Add cluster %d, (%d  %p) %d",iclMin,iTr,arr,fStartIndex[iTr]%10);

  //ncl++;
  if(gHarpoDebug>2)
    Info("FindNextClosest","Test %d %p %p %d %d %d %d %d", iTr, clArray, arr, ncl, plane, color, fill,smooth);
  //  Xproj.Print();
  // Info("NextStep","Xproj[%d] = (%g %g %g %g)",ncl,Xproj[0][0],Xproj[1][0],Xproj[2][0],Xproj[3][0]);
  //Info("NextStep","Xfilter[%d] = (%g %g %g %g)",ncl,Xfilter[0][0],Xfilter[1][0],Xfilter[2][0],Xfilter[3][0]);

  std::vector<TMatrixD> matrices = 
    NextStep(Xfilter, Cfilter, iTr, clArray, arr, ncl+1, plane, color, fill,smooth,qMin+qold,finder);

  if(smooth==0) fGraph2[plane][iTr]->SetPoint(ncl,Xfilter[1][0],Xfilter[0][0]);
  if(smooth==0){
    cluster->SetXfit(0,Xfilter[0][0]);
    cluster->SetZfit(0,Xfilter[1][0]);
    return matrices; // no smoothing
  }

  TMatrixD Xsmooth(4,1);
  TMatrixD Csmooth(4,4);
  Int_t nExtra = 1+3*8;
  TMatrixD extraold(nExtra,1);
  if(matrices.size()<5){
    //Info("NextStep","******* Initialize smoothing (%ld) ********",matrices.size());
    Xsmooth = Xfilter;
    Csmooth = Cfilter;
    extraold[0][0] = 0;
    for(Int_t i = 0; i<8; i++){
      extraold[3*i+1][0] = -1000;
      extraold[3*i+2][0] = 0; // mean deflection angle
      extraold[3*i+3][0] = 0; // rms deflection angle
    }
    extraold[1][0] = TMath::ATan2(Xfilter[2][0],Xfilter[3][0]); // angle[k]
  }else{
    TMatrixD Cprojnew = matrices[0];
    TMatrixD Xprojnew = matrices[1];
    TMatrixD Csmoothold = matrices[2];
    TMatrixD Xsmoothold = matrices[3];
    extraold = matrices[4];
    
    TMatrixD A(4,4);
    //Info("NextStep","ncl = %d (%d,%d)*(%d,%d)*(%d,%d)",ncl,Cfilter.GetNcols(),Cfilter.GetNrows(),Ft.GetNcols(),Ft.GetNrows(),(Cprojnew).GetNcols(),(Cprojnew).GetNrows());
    TMatrixD Cprojnewinv(4,4);
    Cprojnewinv = Cprojnew;
    Cprojnewinv.Invert();
    A = Cfilter*Ft*Cprojnewinv;
    TMatrixD At(4,4);
    At = A;
    At.T();
    Xsmooth = Xfilter + A*(Xsmoothold-Xprojnew);
    Csmooth = Cfilter + A*(Csmoothold-Cprojnew)*At;  
    // Info("NextStep","Xsmoothold[%d] = (%g %g %g %g)",ncl,Xsmoothold[0][0],Xsmoothold[1][0],Xsmoothold[2][0],Xsmoothold[3][0]);
    // Info("NextStep","Xprojnew[%d] = (%g %g %g %g)",ncl,Xprojnew[0][0],Xprojnew[1][0],Xprojnew[2][0],Xprojnew[3][0]);
  }
  // Info("NextStep","Xfilter[%d] = (%g %g %g %g)",ncl,Xfilter[0][0],Xfilter[1][0],Xfilter[2][0],Xfilter[3][0]);
  //  Info("NextStep","Xsmooth[%d] = (%g %g %g %g)",ncl,Xsmooth[0][0],Xsmooth[1][0],Xsmooth[2][0],Xsmooth[3][0]);

  output.push_back(Cproj);
  output.push_back(Xproj);
  output.push_back(Csmooth);
  output.push_back(Xsmooth);

  // TMatrixD residual(2,1);
  // TMatrixD residualt(2,1);
  // TMatrixD Residual(2,2);
  // residual = M - H*Xsmooth;
  // residualt = residual;
  // residualt.T();
  // Residual = V - H*Csmooth*Ht;
  // Residual.Invert();
  // TMatrixD chi2m(1,1);
  // chi2m = residualt*(Residual*residual);
  // Double_t chi2 = chi2m[0][0];
  // hChi2->Fill(chi2);

  //  HarpoRecoClusters* cluster = (HarpoRecoClusters*)clArray->At(iclMin);
  //  cluster->AddIdClusterTrack(iTr);
  cluster->SetXfit(0,Xsmooth[0][0]);
  cluster->SetZfit(0,Xsmooth[1][0]);

  fGraph[plane][iTr]->SetPoint(ncl,Xsmooth[1][0],Xsmooth[0][0]);

  // Double_t angleTmp = TMath::ATan2(Xsmooth[2][0], Xsmooth[3][0]); 
  // Double_t angleOld = extraold[1][0];
  // Double_t angleOld2 = extraold[4][0];
  // Double_t dangle = angleTmp - angleOld; 
  // Double_t dangle2 = 0;
  // if(angleOld2>-100) dangle2 = angleTmp - angleOld2;
  // if(dangle>TMath::Pi()) dangle -= TMath::Pi();
  // if(dangle<-TMath::Pi()) dangle += TMath::Pi();
  // if(dangle2>TMath::Pi()) dangle2 -= TMath::Pi();
  // if(dangle2<-TMath::Pi()) dangle2 += TMath::Pi();
  // Info("NextStep","(%g %g %g) => %g %g / %g %g",angleTmp,angleOld,angleOld2,dangle,dangle2,extraold[3][0],extraold[6][0]);
  // TMatrixD extra(nExtra,1);
  // extra[0][0] = extraold[0][0]+1;
  // extra[1][0] = angleTmp;
  // extra[2][0] = extraold[2][0] + dangle;
  // extra[3][0] = extraold[3][0] + dangle*dangle;
  // extra[4][0] = angleOld;
  // extra[5][0] = extraold[5][0] + dangle2;
  // extra[6][0] = extraold[6][0] + dangle2*dangle2;
  // output.push_back(extra);
  // Double_t angleOld = TMath::ATan2(Xsmoothold[2][0], Xsmoothold[3][0]);     
  //  Double_t angleTmp = TMath::ATan2(Xsmooth[0][0] - Xsmoothold[0][0], Xsmooth[1][0] - Xsmoothold[1][0]); 

  Double_t angleTmp = TMath::ATan2(Xsmooth[2][0], Xsmooth[3][0]); 
  TMatrixD extra(nExtra,1);
  extra[0][0] = extraold[0][0]+1;
  extra[1][0] = angleTmp;
  for(Int_t i = 0; i<8; i++){
    Double_t angleOld = extraold[3*i+1][0];
    Double_t dangle = 0;
    if(angleOld>-100) dangle = angleTmp - angleOld; 
    if(dangle>TMath::Pi()) dangle -= TMath::Pi();
    if(dangle<-TMath::Pi()) dangle += TMath::Pi();
    extra[3*i+2][0] = extraold[3*i+2][0] + dangle;
    extra[3*i+3][0] = extraold[3][0] + dangle*dangle;
    if(i>0) extra[3*i+1][0] = extra[3*i-2][0];
  }
  output.push_back(extra);
  return output;
}


Int_t HarpoKalmanNew::FindClosestNeighbour(TMatrixD X, Int_t iTr, TClonesArray* clArray, Int_t plane, Int_t color, Int_t fill, Bool_t finder)
{

  if(gHarpoDebug>2) Info("FindNextClosest","FINDER = %d",finder);

  Double_t fRangeMax = 15;
  Double_t deltaRes = fResFinder;
  Double_t deltaScat = fScatFinder;
  Double_t theta = fScatKalman; // Multiple scattering

  //Double_t distMin = 1000, dMin = 100000, xMin = -10000, zMin = -10000, rangeMin = 0, angleMin = -1000, resMin = -1, lambdaMin = 10000;
  //  Double_t distMin = 1000;
  Double_t dMin = 100000, xMin = -10000, zMin = -10000, rangeMin = 0, lambdaMin = 10000;
  //Int_t iclMin = -1, typeMin = -1, iMin  = -1, kMin = -1, qMin = -1;
  Int_t iclMin = -1; //, qMin = -1;

  Int_t nCl = clArray->GetEntries();
  for(Int_t i = 0; i<nCl; i++){ // For all clusters
    Int_t icl = i;
    if(used[icl]==iTr) continue;
    HarpoRecoClusters* cluster = (HarpoRecoClusters*)clArray->At(icl);
    if(cluster->GetPlane() != plane) continue;
    if(cluster->GetType() != BLOCCLUSTER) continue;
    if(!finder && !cluster->CheckIdClusterTrack(iTr)) continue;
    Double_t x = cluster->GetX();
    Double_t z = cluster->GetZ();
    //    Int_t q = cluster->GetQ();
    
    // Position of the cluster (x,z) along the line of propagation
    Double_t lambda = (x-X[0][0])*X[2][0]+(z-X[1][0])*X[3][0];
    if(lambda<fLambdaMin) continue;
    //if(finder && lambda>50) continue;
    if(lambda>fLambdaMax) continue;
    // Distance from new point (x,z) to the line of propagation
    Double_t dist = TMath::Abs((x-X[0][0])*X[3][0]-(z-X[1][0])*X[2][0]);

    //Double_t d = lambda*lambda+dist*dist;
    Double_t d = lambda*lambda*dist;
        
    Double_t res = GetResolution(cluster);
    Double_t range = deltaRes*res + deltaScat*theta*lambda;
    if(range>fRangeMax) range = fRangeMax;
    
    if(finder && TMath::Abs(dist)>range) continue;
    if(d < dMin){
      dMin = d;
      //      distMin = dist;
      iclMin = icl;
      lambdaMin = lambda;
      rangeMin = range;
      xMin = x;
      zMin = z;
      //      qMin = q;
    }
  }
  
  if(fCanvas[plane]){ // Display for Reco monitor
    // Float_t z[3] = {X[1][0],X[1][0]+lambdaMin*X[3][0]-rangeMin*X[2][0],X[1][0]+lambdaMin*X[3][0]+rangeMin*X[2][0]};
    // Float_t x[3] = {X[0][0],X[0][0]+lambdaMin*X[2][0]+rangeMin*X[3][0],X[0][0]+lambdaMin*X[2][0]-rangeMin*X[3][0]};
    Double_t w1 = rangeMin; //, w2 = rangeMin;
    if(finder) w1 *= 5./lambdaMin;
    Float_t z[4] = {(Float_t) (X[1][0]+5*X[3][0]+w1*X[2][0]),
                    (Float_t) (X[1][0]+5*X[3][0]-w1*X[2][0]),
                    (Float_t) (X[1][0]+lambdaMin*X[3][0]-rangeMin*X[2][0]),
                    (Float_t) (X[1][0]+lambdaMin*X[3][0]+rangeMin*X[2][0])};
    Float_t x[4] = {(Float_t) (X[0][0]+5*X[2][0]-w1*X[3][0]),
                    (Float_t) (X[0][0]+5*X[2][0]+w1*X[3][0]),
                    (Float_t) (X[0][0]+lambdaMin*X[2][0]+rangeMin*X[3][0]),
                    (Float_t) (X[0][0]+lambdaMin*X[2][0]-rangeMin*X[3][0])};
    if(gHarpoDebug>2)
      Info("FindNextClosest","%g %g     %g %g    %g %g",x[0],z[0],x[1],z[1],x[2],z[2]);
    TPolyLine* triangle;
    triangle = new TPolyLine(4,z,x,"FL");
    triangle->SetLineColor(kBlack);
    triangle->SetFillColor(color);
    triangle->SetFillStyle(fill);
    fCanvas[plane]->cd();
    triangle->Draw("FL");

    fHist[plane]->GetXaxis()->SetRangeUser(zMin-20,zMin+20);
    fHist[plane]->GetYaxis()->SetRangeUser(xMin-20,xMin+20);
    fCanvas[plane]->Update();
    gSystem->Sleep(100);
  }

  return iclMin;

}



Double_t HarpoKalmanNew::GetResolution(HarpoRecoClusters* cl)
{

  return 5;

  if((cl->GetQuality() & 3) == 3) return 2;
  if(cl->GetQuality() & 4) return 1;
  if(cl->GetQuality() & 3) return 1;//0.5;


  if(cl->GetType() == CCLUSTER) return 1;
  if(cl->GetType() == TCLUSTER) return 0.2;

  return 10;
}


 void   HarpoKalmanNew::Init()
 {

   nEvents = 0;

   // Initialise histograms here
   
   fCanvas[0] = NULL;
   fCanvas[1] = NULL;
   fHist[0] = NULL;
   fHist[1] = NULL;

   fLambdaMin = 5;
   fLambdaMax = 50;
   fNclMin = 10;
   fNclMin2 = 15;
   fMaxSlopeType = 1.5;
   fResKalman = 0.2;
   fScatKalman = 0.1;
   fResFinder = 1;
   fScatFinder = 3;//2;

   fQmin = 200;
   fWidthMinT = 2;
   fWidthMinC = 5;
   fWidthMax = 30;

   for(Int_t plane = 0; plane<2; plane++){
     for(Int_t i = 0; i<NTRACK; i++){
       fGraph[plane][i] = 0;
       fGraph2[plane][i] = 0;
     }
   }


        //   fNtr = 0;
   hNcl = new TH1F("hNcl","hNcl",500,0,500);

  const Int_t nbinsDist = 500;
  Double_t xminDist = 1e-6;
  Double_t xmaxDist = 1e5;
  Double_t logxminDist = TMath::Log(xminDist);
  Double_t logxmaxDist = TMath::Log(xmaxDist);
  Double_t binwidthDist = (logxmaxDist-logxminDist)/nbinsDist;
  Double_t xbinsDist[nbinsDist+1];
  xbinsDist[0] = xminDist;
  for (Int_t i=1;i<=nbinsDist;i++)
    xbinsDist[i] = TMath::Exp(logxminDist+i*binwidthDist);
  hChi2 = new TH1F("hChi2","hChi2",nbinsDist,xbinsDist);


  const Int_t nbinsTheta = 100;
  Double_t xminTheta = 1e-5;
  Double_t xmaxTheta = 2;
  Double_t logxminTheta = TMath::Log(xminTheta);
  Double_t logxmaxTheta = TMath::Log(xmaxTheta);
  Double_t binwidthTheta = (logxmaxTheta-logxminTheta)/nbinsTheta;
  Double_t xbinsTheta[nbinsTheta+1];
  xbinsTheta[0] = xminTheta;
  for (Int_t i=1;i<=nbinsTheta;i++)
    xbinsTheta[i] = TMath::Exp(logxminTheta+i*binwidthTheta);

   hDistTracks = new TH1F("hDistTracks","",nbinsDist,xbinsDist);
   hDistMinTracks = new TH1F("hDistMinTracks","",nbinsDist,xbinsDist);
   hThetaTracks = new TH1F("hThetaTracks","",nbinsTheta,xbinsTheta);//,100,-1,1);
   //   hThetaTracksNtr = new TH2F("hThetaTracksNtr","",NTRACK,0,NTRACK,100,-1,1);
   hThetaTracksNtr = new TH2F("hThetaTracksNtr","",nbinsDist,xbinsDist,nbinsTheta,xbinsTheta);
   for(Int_t i = 0; i<10; i++)
     hThetaDist[i] = new TH2F(Form("hThetaDist%d",i),"",nbinsDist,xbinsDist,nbinsTheta,xbinsTheta);

 
  const Int_t nbinsQ = 100;
  Double_t xminQ = 1e-3;
  Double_t xmaxQ = 1e3;
  Double_t logxminQ = TMath::Log(xminQ);
  Double_t logxmaxQ = TMath::Log(xmaxQ);
  Double_t binwidthQ = (logxmaxQ-logxminQ)/nbinsQ;
  Double_t xbinsQ[nbinsQ+1];
  xbinsQ[0] = xminQ;
  for (Int_t i=1;i<=nbinsQ;i++)
    xbinsQ[i] = TMath::Exp(logxminQ+i*binwidthQ);
  fHistQQtestVsDist = new TH2F("fHistQQtestVsDist","",nbinsQ,xbinsQ,nbinsDist,xbinsDist);
   fHistQQmin = new TH2F("fHistQQmin","",nbinsQ,xbinsQ,nbinsDist,xbinsDist);

 }

void   HarpoKalmanNew::Save(char * /* mode */)
 {

   // TString * hstFile = gHConfig->GetHistFile();
   // if ( hstFile == NULL ) {
   //   std::cout << gHConfig->GetProgramName() << " " <<
   //     "No Hist File name given, use default" << std::endl;
   //   //    const char* dir = gSystem->ExpandPathName("$HARPO_ANA_DIR/reco");
   //  const char* dir = "$HARPO_ANA_DIR/reco";
   //  if(gSystem->AccessPathName(dir))
   //    hstFile = new TString(Form("trackingKalman%lli.root",gHConfig->GetRunNo()));
   //  else
   //    hstFile = new TString(Form("%s/trackingKalman%lli.root",dir,gHConfig->GetRunNo()));
   //  //    hstFile = new TString(Form("outputs/anaph%lli.root",gHConfig->GetRunNo()));
   // }

   // TFile *hf = new TFile(hstFile->Data(),"RECREATE");
   OpenHistFile("trackingKalman");
   // Save histograms here
   hDistTracks->Write();
   hDistMinTracks->Write();
   hThetaTracks->Write();
   hThetaTracksNtr->Write();
   for(Int_t i = 0; i<10; i++){
     if(hThetaDist[i])
       hThetaDist[i]->Write();
   }
   fHistQQtestVsDist->Write();
   fHistQQmin->Write();
   NormaliseBins(hChi2);
   hChi2->Write();

   //   hf->Close();
   //   printf("fNewFile %s closed\n", hstFile->Data() );
 
 }



void HarpoKalmanNew::DisplayAnalysis(TRootEmbeddedCanvas* ecTab, TGListBox* /* infobox */ )
{

  TCanvas* cTab = ecTab->GetCanvas();
  HarpoRecoEvent* reco = fEvt->GetRecoEvent();
  TClonesArray* clArray = reco->GetClustersArray();
  Int_t nCl = clArray->GetEntries();

  TGraph* g[2];
  g[0] = new TGraph();
  g[0]->SetMarkerStyle(7);
  g[1] = new TGraph();
  g[1]->SetMarkerStyle(7);
  for(Int_t icl = 0; icl<nCl; icl++){
    HarpoRecoClusters* cluster = (HarpoRecoClusters*)clArray->At(icl);
    Int_t plane = cluster->GetPlane();
    Double_t x = cluster->GetX();
    Double_t z = cluster->GetZ();
    g[plane]->SetPoint(g[plane]->GetN(),z,x);
  }

  for(Int_t plane = 0; plane<2; plane++){
    cTab->cd(plane+1);
    if(g[plane]->GetN()>0) g[plane]->Draw("Psame");
    for(Int_t i = 0; i<NTRACK; i++){
      if(!fGraph[plane][i]) continue;
      fGraph[plane][i]->SetMarkerStyle(24);
      fGraph[plane][i]->SetMarkerColor(i+2);
      fGraph[plane][i]->SetLineColor(i+2);
      if(fGraph[plane][i]->GetN()>0) fGraph[plane][i]->Draw("LPsame");
      fGraph2[plane][i]->SetMarkerStyle(6);
      fGraph2[plane][i]->SetMarkerColor(i+2);
      fGraph2[plane][i]->SetLineColor(i+2);
      if(fGraph2[plane][i]->GetN()>5) fGraph2[plane][i]->Draw("LPsame");
    }
  }
  cTab->Update();
}