#include <iostream>
#include "TClonesArray.h"
#include "CbmRootManager.h"
#include "POCAtestTask2.h"
#include "TGeant3TGeo.h"
#include "TGeant3.h"
#include "TVector3.h"
#include "TCanvas.h"
#include "TRandom.h"
#include "TH1.h"
#include "TFile.h"
#include "TTree.h"
#include "TDatabasePDG.h"
#include "CbmTrackParH.h"

#include "DetPlane.h"
#include "GeaneTrackRep.h"
#include "Track.h"
#include "Kalman.h"
#include "AbsRecoHit.h"
#include "SPhit.h"
#include "PndTpcPoint.h"
#include "poca.h"
#include "StdoutKiller.h"

#include "SPhit.h"

using namespace std;

// -----   Default constructor   -------------------------------------------
POCAtestTask2::POCAtestTask2() :
  CbmTask("Test") { }
// -------------------------------------------------------------------------



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


// -----   Public method Init   --------------------------------------------
InitStatus POCAtestTask2::Init() {

  // Get RootManager
  CbmRootManager* ioman = CbmRootManager::Instance();
  if ( ! ioman ) {
    cout << "-E- POCAtestTask::Init: "
	 << "RootManager not instantised!" << endl;
    return kFATAL;
  }
  
  // Get input array

  fPointArray = (TClonesArray*) ioman->GetObject("PndTpcPoint");
  // Create and register output array

  fPro = new CbmGeanePro();

  return kSUCCESS;

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

void POCAtestTask2::WriteToFile(){
  TFile f("out.root","RECREATE");

  f.Close();
}

// -----   Public method Exec   --------------------------------------------
void POCAtestTask2::Exec(Option_t* opt) {
  // 	cout << "POCAtestTask::Exec" << endl;
  

  Int_t PDGCode= 13;
    

  TVector3 StartPos    = TVector3 (0.,0.,0.);
  TVector3 StartPosErr = TVector3(0.1,0.1,0.1);
  TVector3 StartMom    = TVector3 (1.,1.,1.);
  StartMom.SetMag(1.);
  TVector3 StartMomErr = TVector3(0.01,0.01,0.01);
  
  
  TDatabasePDG *fdbPDG= TDatabasePDG::Instance();
  TParticlePDG *fParticle= fdbPDG->GetParticle(PDGCode);
  Double_t  fCharge= fParticle->Charge();


  TVector3 U(1.,0.,0.);
  TVector3 V(0.,1.,0.);
  DetPlane start_pl(StartPos,U,V);
  
  AbsTrackRep* rep = new GeaneTrackRep(fPro,
									   start_pl,StartMom,
									   StartPosErr,StartMomErr,
									   fCharge);

  TVector3 errors(0.3,0.3,1.);


  std::vector<TVector3> points;

  for(int i=1;i<=100;++i){

	TVector3 Z(0.,0.,i);
	DetPlane d(Z,U,V);
	TVector3 pos,mom;
	rep->getPosMom(d,pos,mom);
	points.push_back(pos);
  }  

  std::vector<AbsRecoHit*> hits;

  for(int i=0;i<(int)points.size();++i){

	AbsRecoHit *aHit = new SPhit(points.at(i),errors);
	hits.push_back(aHit);
  }

  Track *tr = new Track( new GeaneTrackRep(fPro,
										   start_pl,StartMom,
										   StartPosErr,StartMomErr,
										   fCharge)
						 );
  tr->addHitVector(hits);
  Kalman k;
  k.processTrack(tr);
  AbsTrackRep* result =   tr->getCardinalRep();
  result->getReferencePlane().Print();
  std::cout <<  result->getMom().Mag() << std::endl;
  

  delete rep;
  static int count(0);
  std::cout << "dreggn " << count++ << std::endl;
}




ClassImp(POCAtestTask2)