/// Fit of straight, linear tracks based on the Kalman Filter
  ///
  /// @author I.Kulakov
  /// @e-mail I.Kulakov@gsi.de
  /// 
  /// use "root -l KFLineFitter.C+" or "bash run" to run
 

//#define DISPLAY // if def DISPLAY - show track fit display. otherwise - show pulls
//#define DRAW_MC_ERRORS // define to draw hit errors around MC track

#include <iostream>
#include <cmath>
#include <vector>

using namespace std;

#include "TRandom3.h"
#include "TH1.h"
#include "TF1.h"
#include "TMath.h"
#include "TCanvas.h"
#include "TFrame.h"
#include "TStyle.h"

#include "TPolyLine.h"
#include "TPolyMarker.h"
#include "TLine.h"

enum LFDistributionType {
  kGaus,
  kUniform
};

const int NAN0 = -100000;


const float InfX = 1000;
#ifdef DISPLAY
const float InfTx  = 1;
#else
const float InfTx  = 1000;
#endif

  // standard parameters. (Are chenged in the main function!)
const int   DNStations = 6; // number of stations
const float DDZ = 10;     // distance between stations in standard geometry
const float DScatter = 0.01; // sigma of multiple scattering angle distribution
const float DSigma = 0.5; // sigma of hit distribution
const LFDistributionType DDistType = kGaus;

struct LFPoint {
  LFPoint():x(NAN0),z(NAN0){};
  LFPoint( float x_, float z_ ): x(x_),z(z_) {};
  
  float x; // x-position of the hit
  float z; // coordinate of station
};

struct LFTrackParam {
  LFTrackParam():z(0){p[0] = 0; p[1] = 0;};
  LFTrackParam( float x_, float tx_, float z_ ):z(z_){p[0] = x_; p[1] = tx_;};
  float X( float z_ ) const { return p[1]*(z_-z)+p[0]; };
  
  const float &X() const { return p[0]; };
  const float &Tx() const { return p[1]; };
  const float &Z() const { return z; };

  
  float &X() { return p[0]; };
  float &Tx() { return p[1]; };
  float &Z() { return z; };
  
  float p[2]; // x, tx.
  float z;

  void Print() { cout << z << " " << p[0] << " " << p[1] << endl;};
};

struct LFTrackCovMatrix {
  float &C00() { return c[0]; };
  float &C10() { return c[1]; };
  float &C11() { return c[2]; };
  
  float c[3]; // C00, C10, C11

  void Print() { cout << c[0] << " " << c[1] << " " << c[2] << endl;};
};

struct LFTrack {
  vector<LFPoint> hits;
  
  LFTrackParam rParam; // reconstructed by the fitter track parameters
  LFTrackCovMatrix rCovMatrix;
  float chi2;
  int ndf;

  vector<LFTrackParam> mcPoints; // simulated track parameters
};

struct LFGeometry {
  // LFGeometry():zs(){ zs.clear(); };
  // ~LFGeometry(){};
  
  int GetNStations() const { return zs.size(); }
  vector<float> zs; // z of a station
};

  /// ----------- SIMULATOR -------------------



  // return simulated track with given parameters
class LFSimulator {
 public:
  LFSimulator():fGeometry(), fScatter(DScatter), fSigma(DSigma), fDistType(DDistType), fRandom() {
    fGeometry.zs.resize(DNStations);
    for( int i = 0; i < DNStations; ++i ) fGeometry.zs[i] = ( DDZ*static_cast<float>(i) );
    fRandom.SetSeed(2);
  };

  ~LFSimulator(){};
  
  inline void Simulate( const LFTrackParam& param, LFTrack& track ) ;
  inline void Simulate( const LFTrackParam& param, LFTrack* track, int N ) ;


  void SetGeometry( const LFGeometry &g ) { fGeometry = g;  }
  void SetScatter ( const float      &s ) { fScatter = s;  }
  void SetSigma   ( const float      &s ) { fSigma = s;  }
  void SetDistType( const LFDistributionType &t ) { fDistType = t;  }
  
 private:
  void Scatter( LFTrackParam &tp );
  LFGeometry fGeometry;
  float fScatter; // multiply scatter parameter ( currently it is sigma of teta distribution )
  float fSigma;

  LFDistributionType fDistType;
  TRandom3 fRandom;
};

inline void LFSimulator::Scatter( LFTrackParam &tp )  
{
  const float teta = fRandom.Gaus( 0, fScatter );
  //tp.Tx() += teta * ( 1 + tp.Tx()*tp.Tx() ); // approximate (corresponds to fit)
  tp.Tx() = tan( atan(tp.Tx()) + teta );
}

inline void LFSimulator::Simulate( const LFTrackParam& param, LFTrack& track )  
{
  track.mcPoints.clear();
  LFTrackParam curMCPoint = param;
  for ( int i = 0; i < fGeometry.GetNStations(); ++i ) {
    const float z = fGeometry.zs[i];
      // propagate to next station
    curMCPoint.X() = curMCPoint.X(z);
    curMCPoint.Z() = z;
    Scatter( curMCPoint );
      // save
    track.mcPoints.push_back( curMCPoint );
  }

  track.hits.clear();
  for ( int i = 0; i < fGeometry.GetNStations(); ++i ) {
    const float z = track.mcPoints[i].z;
    float x = NAN0;
    switch ( fDistType ) {
      case kGaus:
        x = fRandom.Gaus( track.mcPoints[i].X(), fSigma );
        break;
      case kUniform:
        const float halfWidth = fSigma*sqrt(3); // sqrt(12)/2
        x = fRandom.Uniform( track.mcPoints[i].X() - halfWidth, track.mcPoints[i].X() + halfWidth );
    }
    
    track.hits.push_back( LFPoint( x, z ) );
  }
}

inline void LFSimulator::Simulate( const LFTrackParam& param, LFTrack* track, int N )  
{
  for ( int i = 0; i < N; ++i ) {
    Simulate( param, track[i] );
  }
}


  /// ------------ FITTER ------------

class LFFitter {
 public:
  LFFitter(): fScatter(DScatter), fSigma(DSigma) {
  };
  
  inline void Fit( LFTrack& track ) const;
  
  void FitUpToZ( LFTrack& track, float zEnd ) const; // fit until zEnd is reached - not all hits can be taken into account. It is used to display the fitting procedure.
  
  void SetScatter ( const float      &s ) { fScatter = s;  }
  void SetSigma( const float &s ) { fSigma = s; };
  
 private:

  inline void Initialize( LFTrack& track ) const;
  inline void Initialize( LFTrack& track, const LFTrackParam &param ) const;
  inline void Extrapolate( LFTrack& track, float z ) const;
  inline void AddMaterial( LFTrack& track ) const;
  inline void Filter( LFTrack& track, float x ) const;

  float fScatter; // multiply scatter parameter ( currently it is sigma of teta distribution )
  float fSigma;
};

 // fit until zEnd is reached - not all hits can be taken into account. It is used to display the fitting procedure.
void LFFitter::FitUpToZ( LFTrack& track, float zEnd ) const
{
  int i = 0;

#if 1 // Initialize by zeros and add the first measurement 
  Initialize( track );
  if ( track.hits[i].z <= zEnd ) {
    Extrapolate( track, track.hits[i].z );
    Filter( track, track.hits[i].x );
  }
#else // Initialize and add the first measurement simultaniously
  if ( track.hits[i].z <= zEnd ) {
    LFTrackParam param;
    param.Z() = track.hits[0].z;
    param.X() = track.hits[0].x;
    param.Tx() = (track.hits[1].x - track.hits[0].x)/(track.hits[1].z - track.hits[0].z);
    Initialize( track, param );
  }
#endif // 0

  i++;
  
  const int NHits = track.hits.size();
  for ( ; (i < NHits) && (track.hits[i].z <= zEnd); ++i ) {

    Extrapolate( track, track.hits[i].z );
    AddMaterial( track ); // CHECKME
    // float* s = const_cast<float*>(&fSigma);
    // static TRandom3 r;
    // if ( r.Uniform(0,1) > 0.99998 ) *s = 0.5*0.7;
    // if ( r.Uniform(0,1) > 0.9998 ) *s = 0.5/0.9;

    Filter( track, track.hits[i].x );
  }

  if (track.hits[i-1].z < zEnd)
    Extrapolate( track, zEnd );
}

void LFFitter::Fit( LFTrack& track ) const
{
  FitUpToZ( track, track.hits.back().z );

  Extrapolate( track, track.mcPoints.back().z ); // just for pulls
}

void LFFitter::Initialize( LFTrack& track ) const
{
  track.rParam.Z() = 0;
  track.rParam.X() = 0;
  track.rParam.Tx() = 0;
  track.chi2 = 0;
  track.ndf = -2;

  track.rCovMatrix.C00() = InfX;
  track.rCovMatrix.C10() = 0;
  track.rCovMatrix.C11() = InfTx;
}

void LFFitter::Initialize( LFTrack& track, const LFTrackParam &param ) const
{
  track.rParam = param;
  track.chi2 = 0;
  track.ndf = -1;
  
  track.rCovMatrix.C00() = fSigma*fSigma;
  track.rCovMatrix.C10() = 0;
  track.rCovMatrix.C11() = InfTx;
}

void LFFitter::Extrapolate( LFTrack& track, float z_ ) const
{
  float &z = track.rParam.Z();
  float &x = track.rParam.X();
  float &tx = track.rParam.Tx();
  float &C00 = track.rCovMatrix.C00();
  float &C10 = track.rCovMatrix.C10();
  float &C11 = track.rCovMatrix.C11();
      
  const float dz = z_ - z;

  x += dz * tx;
  z = z_;

  // F = 1  dz
  //     0  1 

  const float C10_in = C10;
  C10 += dz * C11;
  C00 += dz * ( C10 + C10_in );
}

void LFFitter::AddMaterial( LFTrack& track ) const
{
  float &tx = track.rParam.Tx();
  float &C11 = track.rCovMatrix.C11();
  
  const float Q11 = (1 + tx*tx)*(1 + tx*tx)*fScatter*fScatter; // TODO CHECKME why doesn't work for big angles (~1 rad)
  C11 += Q11;
}

void LFFitter::Filter( LFTrack& track, float x_ ) const
{
 
  float &x = track.rParam.X();
  float &tx = track.rParam.Tx();
  float &C00 = track.rCovMatrix.C00();
  float &C10 = track.rCovMatrix.C10();
  float &C11 = track.rCovMatrix.C11();

    // H = { 1, 0 }
    // zeta = Hr - r // P.S. not "r - Hr" here becase later will be rather "r = r - K * zeta" then  "r = r + K * zeta"
  float zeta = x - x_;

  // F = C*H'
  float F0 = C00;
  float F1 = C10;
  
    // H*C*H'
  float HCH = F0;

    // S = 1. * ( V + H*C*H' )^-1
  float wi = 1./( fSigma*fSigma + HCH );
  float zetawi = zeta * wi;
  
  track.chi2 += zeta * zetawi ;
  track.ndf  += 1;

    // K = C*H'*S = F*S
  float K0 = F0*wi;
  float K1 = F1*wi;

    // r = r - K * zeta
  x  -= K0*zeta;
  tx -= K1*zeta;

    // C = C - K*H*C = C - K*F
  C00 -= K0*F0;
  C10 -= K1*F0;
  C11 -= K1*F1;

  // cout << " Filter " << endl;
  // track.rParam.Print();
  // track.rCovMatrix.Print();
}

/// ------------ DISPLAY --------------

class LFDisplay {
 public:
  LFDisplay():fNSigma(3),fHitSigma(DSigma), fFit(),fTrack(),fZStart(0),fZEnd(0),fVerbose(1),fAsk(true),fCanvas(0){
    fCanvas = new TCanvas ("ZX", "ZX Side View", -1, 0, 700*1.0, 250*1.0);
  };

  void Print() const;
  void Draw();

  void Ask();

  void SetNSigma( float nSigma ) { fNSigma = nSigma; };
  void SetHitSigma( float sigma ) { fHitSigma = sigma; };
  void SetFit( const LFFitter& fit ) { fFit = fit; }
  void SetTrack( const LFTrack& track ) {
    fTrack = track;
    const float maxX = std::max( fTrack.mcPoints.front().X(), fTrack.mcPoints.back().X() );
    const float minX = std::min( fTrack.mcPoints.front().X(), fTrack.mcPoints.back().X() );
    const float maxZ = fTrack.mcPoints.back().Z();
    const float minZ = fTrack.mcPoints.front().Z();
    const float dX = maxX - minX;
    const float dZ = maxZ - minZ;
    fCanvas->Range( minZ - dZ*0.1, minX - dX*0.9, maxZ + dZ*0.1, maxX + dX*0.6);

    fZStart = minZ - dZ*0.1;
    fZEnd   = maxZ + dZ*0.1;
  }

  
 private:
  void GetXdX( float z, float& x, float& dx ) const;
  float GetX( float z ) const;
  float GetXMin( float z ) const;
  float GetXMax( float z ) const;

  void DrawHits() const;
  void DrawTrackFit() const;
  void DrawMCTrack() const;
  void DrawGeo() const;

  float fNSigma; // fNSigma of error to show
  float fHitSigma; // Sigma for hits
  
  LFFitter fFit;
  LFTrack fTrack;

  float fZStart; // range to draw
  float fZEnd;
  
  int fVerbose;
  bool fAsk;

  TCanvas *fCanvas;
};

void LFDisplay::Draw()
{
  DrawGeo();
  DrawMCTrack();
  DrawTrackFit();
  DrawHits();
  fCanvas->Update();
  fCanvas->SaveAs("LFDisplay.pdf");
  Ask();
}

void LFDisplay::DrawHits() const
{

  const int NPoints = fTrack.hits.size();
  vector<double> lz, lx;
  lz.resize(NPoints);
  lx.resize(NPoints);

  for ( int iP = 0; iP < NPoints; iP++ ) {
    lx[iP] = fTrack.hits[iP].x;
    lz[iP] = fTrack.hits[iP].z;
  }
  
  TPolyMarker *maker = new TPolyMarker(NPoints, &(lz[0]), &(lx[0]));
  maker->SetMarkerColor(kRed);
  maker->SetMarkerStyle(29);
  maker->SetMarkerSize(1.5);
  maker->Draw();
  
    // hits errors
  TLine line;
  line.SetLineWidth(1);
  line.SetLineColor(kRed);
  const float dX = fNSigma*fHitSigma;
  const float dZ = (fTrack.hits[0].z - fTrack.hits[1].z)*0.05;
  for ( int iP = 0; iP < NPoints; iP++ ) {
    const float z = fTrack.hits[iP].z;
    const float x = fTrack.hits[iP].x;
    line.SetLineWidth(2);
    line.DrawLine( z, x - dX, z, x + dX );
    line.SetLineWidth(2);
    line.DrawLine( z - dZ, x - dX, z + dZ, x - dX );
    line.DrawLine( z - dZ, x + dX, z + dZ, x + dX );
  }
}

void LFDisplay::DrawTrackFit() const
{
  const float Dz = 0.01;

  float z = fZStart;
  const int NPoints = static_cast<int>( (fZEnd-fZStart)/Dz ) + 1;
  vector<double> lz, lx, lxMin, lxMax;
  lz.resize(NPoints);
  lx.resize(NPoints);
  lxMin.resize(NPoints);
  lxMax.resize(NPoints);
  for ( int iP = 0; iP < NPoints; z += Dz, iP++ ) {
    
    lz[iP] = z;
    lx[iP] = GetX( z );
    lxMin[iP] = GetXMin( z );
    lxMax[iP] = GetXMax( z );
  }
  
  TPolyLine pline;
  pline.SetLineColor(kBlue);
  pline.SetLineWidth(2);
  pline.DrawPolyLine( NPoints, &(lz[0]), &(lx[0]) );
  pline.SetLineColor(kBlue);
  pline.SetLineWidth(1);
  pline.DrawPolyLine( NPoints, &(lz[0]), &(lxMin[0]) );
  pline.DrawPolyLine( NPoints, &(lz[0]), &(lxMax[0]) );

  
//     TPad *newpad= new TPad("newpad","Transparent pad",0,0,1,1);
//     Double_t a1,a2,a3,a4;
//     fCanvas->GetRange(a1,a2,a3,a4);
//     newpad->Range(a1,a2,a3,a4);
//     newpad->SetFillStyle(4050);
//     newpad->Draw();
//     newpad->cd();
//   
  
  vector<double> lxAll = lxMin, lzAll = lz;
  for ( int iP = 0; iP < NPoints; iP++ ) {
    lxAll.push_back(lxMax[NPoints-1-iP]);
    lzAll.push_back(lz[NPoints-1-iP]);
  }
  lxAll.push_back(lxAll[0]);
  lzAll.push_back(lzAll[0]);
  pline.SetLineColor(kBlue);
  pline.SetFillColor(kBlue);
  pline.SetFillStyle(3003); // 3003
  pline.DrawPolyLine( lxAll.size(), &(lzAll[0]), &(lxAll[0]), "f" );
}

void LFDisplay::Print() const
{
  float x, dx;
  for ( float z = 0; z < 110; z += 1 ) {
    GetXdX( z, x, dx );
    cout << z << ": " << x - dx << " " << x << " " << x + dx << endl;
  }
}

void LFDisplay::DrawMCTrack() const
{
  const float Dz = 0.1;
  const int NPoints = static_cast<int>( (fZEnd-fZStart)/Dz ) + 1;

  unsigned int iMCP = 0;
  float z = fZStart;

  vector<double> lz, lx;
  lz.resize(NPoints);
  lx.resize(NPoints);
#ifdef DRAW_MC_ERRORS
  vector<double> lxMin, lxMax;
  lxMin.resize(NPoints);
  lxMax.resize(NPoints);
#endif
  for ( int iP = 0; iP < NPoints; z += Dz, iP++ ) {
    if( (iMCP+1 < fTrack.mcPoints.size()) && (fTrack.mcPoints[iMCP+1].Z() <= z) ) iMCP++;
    lz[iP] = z;
    lx[iP] = fTrack.mcPoints[iMCP].X(z);
#ifdef DRAW_MC_ERRORS
    lxMin[iP] = lx[iP] - fNSigma*fHitSigma;
    lxMax[iP] = lx[iP] + fNSigma*fHitSigma;
#endif
  }

  TPolyLine pline;
  pline.SetLineColor(kRed);
  pline.SetLineWidth(2);
  pline.SetLineStyle(9);
  pline.DrawPolyLine( NPoints, &(lz[0]), &(lx[0]) );
#ifdef DRAW_MC_ERRORS
  pline.SetLineColor(kRed);
  pline.SetLineWidth(1);
  pline.DrawPolyLine( NPoints, &(lz[0]), &(lxMin[0]) );
  pline.DrawPolyLine( NPoints, &(lz[0]), &(lxMax[0]) );

  vector<double> lxAll = lxMin, lzAll = lz;
  for ( int iP = 0; iP < NPoints; iP++ ) {
    lxAll.push_back(lxMax[NPoints-1-iP]);
    lzAll.push_back(lz[NPoints-1-iP]);
  }
  lxAll.push_back(lxAll[0]);
  lzAll.push_back(lzAll[0]);
  pline.SetLineColor(kRed);

  pline.SetFillColor(kRed);
  pline.SetFillStyle(3018);
  pline.DrawPolyLine( lxAll.size(), &(lzAll[0]), &(lxAll[0]), "f" );
#endif
}

void LFDisplay::DrawGeo() const
{
  const int NPoints = fTrack.mcPoints.size();
  TLine line;
  line.SetLineWidth(3);
  line.SetLineColor(kGray);
  for ( int iP = 0; iP < NPoints; iP++ ) {
    const float z = fTrack.mcPoints[iP].Z();
    line.DrawLine(z,-100,z,100);
  }
}

void LFDisplay::Ask()
{
  char symbol;
  if (fAsk){
    std::cout << "ask>";
    do{
      std::cin.get(symbol);
      if (symbol == 'r')
        fAsk = false;
      if (symbol == 'q')
        exit(1);
    } while (symbol != '\n');
    std::cout << endl;
  }
}


void LFDisplay::GetXdX( float z, float& x, float& dx ) const
{
  LFTrack t = fTrack;
  fFit.FitUpToZ( t, z );
  x = t.rParam.X();
  dx = sqrt(t.rCovMatrix.C00());
}

float LFDisplay::GetX( float z ) const
{
  float x, dx;
  GetXdX( z, x, dx );
  return x;
}

float LFDisplay::GetXMax( float z ) const
{
  float x, dx;
  GetXdX( z, x, dx );
  return x + fNSigma*dx;
}

float LFDisplay::GetXMin( float z ) const
{
  float x, dx;
  GetXdX( z, x, dx );
  return x - fNSigma*dx;
}

  


/// ------------ PERFORMANCE ------------


void FitHistoGaus(TH1* hist, float* sigma = 0)
{
  if (hist && (hist->GetEntries() != 0)){
    TF1  *fit = new TF1("fit","gaus");
    fit->SetLineColor(2);
    fit->SetLineWidth(3);
    hist->Fit(fit,"","",hist->GetXaxis()->GetXmin(),hist->GetXaxis()->GetXmax()); // FIXME sometimes fails w\o a message here!
    if (sigma) *sigma = fit->GetParameter(2);
  }
  else{
    std::cout << " E: Read hists error! " << std::endl;
  }
}

void MakeUpHisto(TH1* hist, TString title)
{
  const int textFont = 22; // TNewRoman
  if (hist && (hist->GetEntries() != 0)){

    hist->GetXaxis()->SetLabelFont(textFont);
    hist->GetXaxis()->SetTitleFont(textFont);
    hist->GetYaxis()->SetLabelFont(textFont);
    hist->GetYaxis()->SetTitleFont(textFont);
    
    hist->GetXaxis()->SetTitle(title);
    hist->GetXaxis()->SetTitleOffset(1);
    hist->GetYaxis()->SetTitle("Entries");
    hist->GetYaxis()->SetTitleOffset(1.05); // good then entries per bit <= 9999
  }
  else{
    std::cout << " E: Read hists error! " << std::endl;
  }
}

TStyle *GetStyle() {
    
  TStyle *histoStyle = new TStyle("histoStyle","Plain Style(no colors/fill areas)");
//   TStyle *histoStyle = gStyle;
  
  histoStyle->SetTextFont(22);
  histoStyle->SetPadColor(0);
  histoStyle->SetCanvasColor(0);
  histoStyle->SetTitleColor(0);
  histoStyle->SetStatColor(0);
  
  histoStyle->SetOptTitle(0); // without main up title
  histoStyle->SetOptStat(1000111011);
//   The parameter mode can be = ksiourmen  (default = 000001111)
//   k = 1;  kurtosis printed
//   k = 2;  kurtosis and kurtosis error printed
//   s = 1;  skewness printed
//   s = 2;  skewness and skewness error printed
//   i = 1;  integral of bins printed
//   o = 1;  number of overflows printed
//   u = 1;  number of underflows printed
//   r = 1;  rms printed
//   r = 2;  rms and rms error printed
//   m = 1;  mean value printed
//   m = 2;  mean and mean error values printed
//   e = 1;  number of entries printed
//   n = 1;  name of histogram is printed
  
  histoStyle->SetOptFit(10001);
//   The parameter mode can be = pcev  (default = 0111)
//   p = 1;  print Probability
//   c = 1;  print Chisquare/Number of degress of freedom
//   e = 1;  print errors (if e=1, v must be 1)
//   v = 1;  print name/values of parameters
  
  histoStyle->SetStatW(0.175);
  histoStyle->SetStatH(0.02);
  histoStyle->SetStatX(0.95);
  histoStyle->SetStatY(0.97);
  histoStyle->SetStatFontSize(0.05);

  histoStyle->SetStatFont(22);
  return histoStyle;
}

/// ------------ RUNNER ------------


void KFLineFitter(){
  std::cout << " Begin " << std::endl;

      ///  ---- CREATE SIMULATOR & RECONSTRUCTOR -----

  const int   NStations = 8; // number of stations
  const float DZ = 10;     // distance between stations in standard geometry
  const float Scatter = 0.000; // sigma of multiple scattering angle distribution
  const float Sigma = 0.2; // sigma of hit distribution
  const LFDistributionType DistType = kGaus;//kUniform;//kGaus;
  
  LFGeometry geo;
  geo.zs.resize(NStations);
  for( int i = 0; i < NStations; ++i ) geo.zs[i] = ( DZ * static_cast<float>(i) );

  LFSimulator sim;

  sim.SetGeometry(geo);
  sim.SetScatter(Scatter);
  sim.SetSigma(Sigma);
  sim.SetDistType(DistType);

  LFFitter fit;

  fit.SetScatter( Scatter );
  fit.SetSigma( Sigma );

    ///  ---- GET FITTED TRACKS -----

#ifdef DISPLAY // DISPLAY
  {
    LFTrack track;
    LFTrackParam par;
    par.Z() = -1;
    par.X()  = 0;
    par.Tx() = 0.02;
    sim.Simulate( par, track );

    LFDisplay display;
    display.SetFit( fit );
    display.SetTrack( track );
    display.SetHitSigma( Sigma );
    // display.Print();
    display.Draw();
  }   
#else // PULLS

  
  const int NTracks = 100000;
  LFTrack tracks[NTracks];

  TRandom3 rand;
  for ( int i = 0; i < NTracks; ++i ) {
    LFTrack &track = tracks[i];
    
    LFTrackParam par;
    par.Z() = 0;
    par.X()  = rand.Uniform(-5,5);
    par.Tx() = rand.Uniform(-0.5,0.5);
    
    sim.Simulate( par, track );
    fit.Fit( track );
  }


  
  ///  ---- SHOW RESULTS -----
  
  const int NBins = 110;
  TH1F *h_prob = new TH1F("prob", "Prob", NBins, -0.05, 1.05);
  TH1F *h_chi2 = new TH1F("chi2", "Chi2", NBins, -0.5, 10.5);
  
  TH1F *h_x_pull = new TH1F("x_pull", "X pull", NBins, -5.5, 5.5);
  TH1F *h_tx_pull = new TH1F("tx_pull", "Tx pull", NBins, -5.5, 5.5);
  
  for ( int i = 0; i < NTracks; ++i ) {
    LFTrack &track = tracks[i];

    if (track.rCovMatrix.C00() > 0) h_x_pull->Fill(  (track.rParam.X()  - track.mcPoints.back().X()) /sqrt(track.rCovMatrix.C00()) );
    if (track.rCovMatrix.C11() > 0) h_tx_pull->Fill( (track.rParam.Tx() - track.mcPoints.back().Tx())/sqrt(track.rCovMatrix.C11()) );
    
    h_chi2->Fill( track.chi2 );
    h_prob->Fill( TMath::Prob( static_cast<Double_t>(track.chi2), track.ndf ) );
  }


  GetStyle()->cd();
  MakeUpHisto(h_x_pull, "X Pull");
  MakeUpHisto(h_tx_pull, "Tx Pull");
  MakeUpHisto(h_chi2, "Chi2");
  MakeUpHisto(h_prob, "Prob");
  FitHistoGaus(h_x_pull);
  FitHistoGaus(h_tx_pull);
  TCanvas *c = new TCanvas("c1","c1",10,10,1010,610);

  c->Divide(2,2);
  c->Update();
  c->cd(1);
  h_chi2->Draw();
  c->cd(2);
  h_prob->Draw();
  c->cd(3);
  h_x_pull->Draw();
  c->cd(4);
  h_tx_pull->Draw();
  c->cd(0);

    // c->GetFrame()->SetBorderMode(0);
    // c->GetFrame()->SetBorderSize(0);
  c->Update();
#endif // PULLS
  
  std::cout << " End " << std::endl;
}