#include <iostream>
#include "TClonesArray.h"
#include "CbmRootManager.h"
#include "POCAtestTask4.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 "PndTpcPoint.h"
#include "poca.h"
#include "StdoutKiller.h"
#include "Track.h"
#include "Kalman.h"
#include "SPhit.h"

using namespace std;

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



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


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

  // Get RootManager
  CbmRootManager* ioman = CbmRootManager::Instance();
  if ( ! ioman ) {
    cout << "-E- POCAtestTask4::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 POCAtestTask4::WriteToFile(){
  TFile f("out.root","RECREATE");

  l1->Write("track");
  m1->Write("points");
  //m2->Write("result");
  //_plane->Write("plane");
  //uline->Write("uline");
  //vline->Write("vline");
  f.Close();
}




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

  Int_t PDGCode= 13;
    

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



  DetPlane start_pl(StartPos,StartMom);

  TVector3 errors(.1,.1,.1);
  std::vector<TVector3> points;


  AbsTrackRep* rep = new GeaneTrackRep(fPro,
									   start_pl,StartMom2,
									   StartPosErr,StartMomErr,
									   fCharge,PDGCode);



  std::cout << "INIT MOMENTUM: " << rep->getMom().Mag() << std::endl;


  AbsTrackRep* rep2 =  new GeaneTrackRep(fPro,
										 start_pl,StartMom,
										 StartPosErr,StartMomErr,
										 fCharge,PDGCode);
  std::cout << "TRACK MOMENTUM: " << rep2->getMom().Mag() << std::endl;

  

  for(int i=0;i<25;++i){
	std::cout << "eeeee" << std::endl;
	rep2->getCov().Print();
	TVector3 pos,mom;
	{//StdoutKiller k;
	pos = rep2->getPos();
	mom = rep2->getMom();
	}
	//rep2->getReferencePlane().Print();
	//pos.Print();
	//mom.Print();
	//mom.SetMag(3.);
	DetPlane d(pos+mom,mom);
	//d.Print();
	//rep2->getState().Print();
	TVector3 posR,momR;
	//	{//StdoutKiller k;
	rep2->getPosMom(d,posR,momR);
	TMatrixT<double> statePred(5,1);
	TMatrixT<double> covPred(5,5);
	TMatrixT<double> jac(5,5);
	{StdoutKiller k; rep2->extrapolate(d,statePred,covPred,jac);}
	rep2->setState(statePred);
	rep2->setCov(covPred);
	rep2->setReferencePlane(d);
	//statePred.Print();
	//	}
	rep2->getCov().Print();
	points.push_back(posR);
	//std::cout << "############" << std::endl;
	std::cout << "fffff" << std::endl;
  }  

  std::vector<AbsRecoHit*> hits;

  m1 = new TPolyMarker3D(points.size(),23);

  for(int i=0;i<(int)points.size();++i){
	//points.at(i).Print();
	AbsRecoHit *aHit = new SPhit(points.at(i),errors);
	//aHit->getRawHitCoord().Print();
	m1->SetPoint(i,(aHit->getRawHitCoord())[0][0],(aHit->getRawHitCoord())[1][0],(aHit->getRawHitCoord())[2][0]);
	m1->SetMarkerColor(kBlack);

	hits.push_back(aHit);
  }

  
  Track *tr = new Track( rep );
  tr->addHitVector(hits);
  Kalman k;
  //std::cout << __FILE__ << __LINE__ << std::endl;
  k.processTrack(tr);
  //std::cout << __FILE__ << __LINE__ << std::endl;
  AbsTrackRep* result =   tr->getCardinalRep();
  //result->getReferencePlane().Print();
  std::cout << "FINAL MOMENTUM: " <<  result->getMom().Mag() << std::endl;
  
  //////////////////

  /*
  TVector3 target(30.,15.,-2.);
  TVector3 result,p;


  DetPlane myPocaPlane;

  TVector3 err(1.,1.,1.);
  findPOCAplane(rep,target,err,myPocaPlane);


  myPocaPlane.getGraphics(2.,10.,&_plane,&uline,&vline);

  std::vector<double> xvals;
  std::vector<double> yvals;
  std::vector<double> zvals;
  */

  l1 = new TPolyLine3D(100);
  for(int i=0;i<100;++i){
	double x=StartPos.X()+i*.5+1.;
	//	DetPlane d;
	TVector3 o(x,x,0.);
	TVector3 n(1.,1.,0.);

	DetPlane d(o,n);
	TVector3 pos,mom;
	{
	  StdoutKiller killa;
	  rep->getPosMom(d,pos,mom);
	}
	l1->SetPoint(i,pos.X(),pos.Y(),pos.Z());
  }
  l1->SetLineColor(kRed);
  l1->SetLineWidth(5);

  

  delete rep;
  
}




ClassImp(POCAtestTask4)