#include <iostream>
#include "TClonesArray.h"
#include "CbmRootManager.h"
#include "POCAtestTask3.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 "SPhit.h"

using namespace std;

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



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


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

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

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


double findClosePoint(AbsTrackRep* rep,const TVector3& target,TVector3& pos_res,TVector3& mom_res){

  TVector3 x0=rep->getPos();
  TVector3 p0=rep->getMom();

  DetPlane pl;

  //take plane with O=target with N=target-x0point
  pl.setO(target);
  pl.setNormal(target-x0);
  {StdoutKiller k;rep->getPosMom(pl,pos_res,mom_res);}
  double bestDist = (pos_res-target).Mag();


  TVector3 x1,p1;
  
  //take plane with O=target with N=p0
  pl.setO(target);
  pl.setNormal(p0);
  {StdoutKiller k;rep->getPosMom(pl,x1,p1);}
  double dist = (x1-target).Mag();
  if(dist < bestDist){
	pos_res = x1;
	mom_res = p1;
	bestDist = dist;
  }


  //take plane with O=target with N=p0point/|p0point| + (target-x0)/|target-x0|
  pl.setO(target);
  pl.setNormal((p0*(1./p0.Mag()))+((target-x0)*(1./(target-x0).Mag())));
  {StdoutKiller k;rep->getPosMom(pl,x1,p1);}
  dist = (x1-target).Mag();
  if(dist < bestDist){
	pos_res = x1;
	mom_res = p1;
	bestDist = dist;
  }


  
  //take plane with O=target with U=x0point-target V=(x0point-target) cross p0
  pl.set(target,x0-target,p0.Cross(x0-target));
  pl.setO(pl.getO()-0.02*pl.getNormal());
  {StdoutKiller k;rep->getPosMom(pl,x1,p1);}
  dist = (x1-target).Mag();
  if(dist < bestDist){
	pos_res = x1;
	mom_res = p1;
	bestDist = dist;
  }

  return bestDist;
}


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

  Int_t PDGCode= 13;
    

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


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

  AbsTrackRep* rep_clone = rep->clone();
  TVector3 target(60.,15.,0.);
  TVector3 result,p;
  findClosePoint(rep_clone,target,result,p);

  DetPlane myPocaPlane;

  TVector3 err(5.,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);

  m1 = new TPolyMarker3D(1,20);
  m1->SetPoint(0,target.X(),target.Y(),target.Z());
  m1->SetMarkerColor(kBlack);
  m1->SetMarkerSize(2);
  
  m2 = new TPolyMarker3D(1,23);
  m2->SetPoint(0,result.X(),result.Y(),result.Z());
  m2->SetMarkerColor(kGreen);



  //TVector3 target(-40.,27.,-70.);
//   TVector3 target    = TVector3 (0.1,0.2,-90.);


//   DetPlane fromPlane;

//   fromPlane.setO(target);

//   fromPlane.setNormal(0.0,0.0);
//   fromPlane.Print();

//   DetPlane result;
//   findPOCAplane(rep,target,result);
//   std::cout << "##########" << std::endl;
//   exit(1);


//   TVector3 h_err(2.,1.,3.);
//   AbsRecoHit* h = new SPhit(target,h_err);

//   DetPlane virtPlane = h->getDetPlane(rep);
//   TMatrixT<double> resid = h->residualVector(rep,rep->getState());
  
//   TVector3 X = virtPlane.getO() + resid[0][0]*virtPlane.getU() + resid[1][0]*virtPlane.getV();


//   std::cout << "######12" << std::endl;
//   result.Print();
//   std::cout << "######34" << std::endl;
//   virtPlane.Print();
//   std::cout << "######56" << std::endl;
//   target.Print();
//   std::cout << "######78" << std::endl;
//   X.Print();

//   TVector3 V1 = virtPlane.getO(); 
//   TVector3 V2(1.,-1.,0);
//   TVector3 V3(1.,1.,0.);
//   DetPlane covPlane(V1,V2,V3);;
  
//   h->getHitCov(virtPlane).Print();
//   h->getHitCov(covPlane).Print();





  delete rep;

}




ClassImp(POCAtestTask3)