//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class GenfitTester
//      see GenfitTester.hh for details
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Sebastian Neubert    TUM            (original author)
//
//
//-----------------------------------------------------------

// Panda Headers ----------------------

// This Class' Header ------------------
#include "GenfitTester.h"

// C/C++ Headers ----------------------
#include <iostream>

#include "TVector3.h"
#include "TPolyMarker3D.h"
#include "TFile.h"

// Collaborating Class Headers --------
#include "LSLTrackRep.h"
#include "TestRecoHit.h"
#include "Track.h"
#include "DetPlane.h"
#include "ExtrapolateToPCA.h"
// Class Member definitions -----------

GenfitTester::GenfitTester()
{}

GenfitTester::~GenfitTester()
{}


void
GenfitTester::test_RecoHit(){
  TestRecoHit hit(2,2,10);
}

void
GenfitTester::test_locTrkRep(){
  std::cout<<"*** test local track rep"<<std::endl;
  
  LSLTrackRep trkRep(0,1,1,0.1,0,2,0,0,0,0,0,NULL);
  trkRep.Print();
  TMatrixT<double> bla(5,1);
  DetPlane p(TVector3(2,3,150),TVector3(1,0,0),TVector3(0,1,0));
  trkRep.extrapolate(p,bla);
  bla.Print();
  
  TestRecoHit hit(2,2,10);

  std::cout<<"hit in coordinate system of track:"<<std::endl;
  
  hit.getHitCoord(p).Print();

  // Calculate Residual
  TMatrixT<double> res=hit.residualVector(&trkRep,bla);
  res.Print();
    
}

//-------------------------------------------------
void
GenfitTester::test_DetPlane(){
 std::cout<<"*** test DetPlane"<<std::endl;
 TVector3 o(0,0,0);
 TVector3 u(1,0,0);
 TVector3 v(1,1,0);
 DetPlane myplane(o,u,v);
 
 TVector3 bla(2.3,8,3);
 myplane.project(bla).Print();
  
}


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

void
GenfitTester::test_LSLTrkRep(double accuracy, bool adaptive){
  std::cout<<"*** test LSL track rep"<<std::endl;
  double z0=200;
  LSLTrackRep* rep=new LSLTrackRep(z0,1,1,0.001,1,5,0,0,0,0,0,NULL);
  rep->setRungeKuttaAcc(accuracy);
  rep->setRungeKuttaAdaptive(adaptive);
  Track* trk=new Track(rep);
  std::cout<<"Pos before extrapolation:"<<std::endl;
  trk->getPos().Print();
  TMatrixT<double> state1=trk->getTrackRep(0)->getState();
  state1.Print();
  
  TPolyMarker3D* mark=new TPolyMarker3D();

  std::vector<bool> success;

  for(double z=10; z<300; z+=20){
    trk->getTrackRep(0)->setState(state1);
    std::cout<<"Extrapolate to z="<<z<<std::endl;
    trk->gotoPoint(TVector3(20,20,z));
  
    std::cout<<"Pos after extrapolation:"<<std::endl;
    TVector3 p=trk->getPos();
    p.Print();
    mark->SetNextPoint(p.X(),p.Y(),p.Z());
    
    trk->gotoPoint(TVector3(20,20,z0));
    TMatrixT<double> state2=trk->getTrackRep(0)->getState();
    TVector3 pos2=trk->getPos();

    TMatrixT<double> dif=state2-state1;
    bool out=false;
    for(int i=0;i<2;++i){
      double d=dif[i][0];
      if(d>accuracy)out=true;
    }
    if(out){
      std::cout<<"Out of bounds!"<<std::endl;
      state2.Print();
      pos2.Print();
      trk->getTrackRep(0)->getReferencePlane().Print();
      //break;
    }
    success.push_back(!out);
    
  }

  
  TVector3 pos3=trk->getPos();
  trk->stepalong(10);
  TVector3 pos4=trk->getPos();
  std::cout<<"doing a step of 10 cm. before/after"<<std::endl;
  pos3.Print();
  pos4.Print();

  
  std::cout<<"done "<<success.size()<<" extrapolations"<<std::endl;

  for(int i=0;i<success.size();++i){
    std::cout<<"Extrapolation#"<<i<<" was "<<success[i]<<std::endl;
  }


  TFile* f=new TFile("lsltrkrep.root","RECREATE");
  mark->Write("trk");
  f->Close();

}

void
GenfitTester::test_PCA(){
  std::cout<<"*** test Extrapolate to PCA ***"<<std::endl;
  double z0=20;
  Track* trk1=new Track(new LSLTrackRep(z0,1,1,1,0.001,2,0,0,0,0,0,NULL));
  Track* trk2=new Track(new LSLTrackRep(z0,1,2,-1,0.001,2,0,0,0,0,0,NULL));
  // go away from vertex
  trk1->stepalong(70);
  trk2->stepalong(80);
  std::cout<<"Start positions"<<std::endl;
  trk1->getPos().Print();
  trk2->getPos().Print();
  TVector3 dif1=trk1->getPos()-trk2->getPos();
  std::cout<<"d="<<dif1.Mag()<<std::endl;;
  
  ExtrapolateToPCA(trk1,trk2);
  
  std::cout<<"Positions after Extrapolation"<<std::endl;
  trk1->getPos().Print();
  trk2->getPos().Print();
  TVector3 dif2=trk1->getPos()-trk2->getPos();
  std::cout<<"d="<<dif2.Mag()<<std::endl;
}


ClassImp(GenfitTester)