#include <iostream>
#include "TClonesArray.h"
#include "CbmRootManager.h"
#include "POCAtestTask.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   -------------------------------------------
POCAtestTask::POCAtestTask() :
  CbmTask("Test") { }
// -------------------------------------------------------------------------



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


// -----   Public method Init   --------------------------------------------
InitStatus POCAtestTask::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 POCAtestTask::WriteToFile(){
  TFile f("out.root","RECREATE");
  g1->Write();
  g2->Write();
  g3->Write();
  g4->Write();
  g5->Write();
  g6->Write();
  g7->Write();
  g8->Write();

  l1->Write("track");
  m1->Write("target");
  m2->Write("result");

  f.Close();
}

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

  Int_t PDGCode= 13;
    

  TVector3 StartPos    = TVector3 (0.1,0.2,-100.);
  TVector3 StartPosErr = TVector3(0,0,0);
  TVector3 StartMom    = TVector3 (-1.,1.,1.);
  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(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);

  

  std::vector<double> xvals;
  std::vector<double> yvals;
  std::vector<double> zvals;
  l1 = new TPolyLine3D(100);
  for(int i=0;i<100;++i){
	double z=StartPos.Z()+i*1.+1.;
	DetPlane d;
	TVector3 o(0.,0.,z);
	d.setO(o);
	TVector3 pos,mom;
	{
	  StdoutKiller killa;
	  rep->getPosMom(d,pos,mom);
	}
	pos.Print();
	xvals.push_back(pos.X());
	yvals.push_back(pos.Y());
	zvals.push_back(pos.Z());
	l1->SetPoint(i,pos.X(),pos.Y(),pos.Z());
  }
  l1->SetLineColor(kRed);
  l1->SetLineWidth(5);

  double _xvals[xvals.size()];
  double _yvals[yvals.size()];
  double _zvals[zvals.size()];
  for(int i=0;i<xvals.size();++i){
	_xvals[i]=xvals.at(i);
	_yvals[i]=yvals.at(i);
	_zvals[i]=zvals.at(i);
  }
  g1 = new TGraph(xvals.size(),_xvals,_yvals);
  g2 = new TGraph(xvals.size(),_zvals,_yvals);
  g1->SetMarkerStyle(20);
  g1->SetMarkerSize(1);
  g1->SetMarkerColor(kRed);
  g1->SetName("YvsX");
  g2->SetName("YvsZ");
  g2->SetMarkerStyle(20);
  g2->SetMarkerSize(1);
  g2->SetMarkerColor(kRed);



  std::vector<double> xvals1;
  std::vector<double> yvals1;
  std::vector<double> zvals1;

  for(int i=0;i<100;++i){
	double z=StartPos.Z()+i*1.+1.;
	DetPlane d;
	TVector3 o(0.,0.,z);
	d.setO(o);
	double xx = gRandom->Uniform()-.5;
	double yy = gRandom->Uniform()-.5;
	TVector3 n(xx,yy,1.);
	d.setNormal(n);
	TVector3 pos,mom;
	{
	  StdoutKiller killa;
	  rep->getPosMom(d,pos,mom);
	}
	pos.Print();
	xvals1.push_back(pos.X());
	yvals1.push_back(pos.Y());
	zvals1.push_back(pos.Z());
  }
  double _xvals1[xvals1.size()];
  double _yvals1[yvals1.size()];
  double _zvals1[zvals1.size()];
  for(int i=0;i<xvals.size();++i){
	_xvals1[i]=xvals1.at(i);
	_yvals1[i]=yvals1.at(i);
	_zvals1[i]=zvals1.at(i);
  }
  g3 = new TGraph(xvals1.size(),_xvals1,_yvals1);
  g4 = new TGraph(xvals1.size(),_zvals1,_yvals1);
  g3->SetName("YvsX_1");
  g4->SetName("YvsZ_1");
  g3->SetMarkerStyle(24);
  g3->SetMarkerSize(2);
  g3->SetMarkerColor(kBlue);
  g4->SetMarkerStyle(24);
  g4->SetMarkerSize(2);
  g4->SetMarkerColor(kBlue);




  //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();


  double x2[1];
  double y2[1];
  double z2[1];

  x2[0]=result.getO().X();
  y2[0]=result.getO().Y();
  z2[0]=result.getO().Z();
  g5 = new TGraph(1,x2,y2);
  g6 = new TGraph(1,z2,y2);
  g5->SetMarkerStyle(23);
  g5->SetMarkerSize(1);
  g5->SetMarkerColor(kGreen);
  g6->SetMarkerStyle(23);
  g6->SetMarkerSize(1);
  g6->SetMarkerColor(kGreen);
  g5->SetName("XYresult");
  g6->SetName("ZYresult");

  double x3[1];
  double y3[1];
  double z3[1];

  x3[0]=target.X();
  y3[0]=target.Y();
  z3[0]=target.Z();
  g7 = new TGraph(1,x3,y3);
  g8 = new TGraph(1,z3,y3);
  g7->SetMarkerStyle(23);
  g7->SetMarkerSize(1);
  g7->SetMarkerColor(kBlack);
  g8->SetMarkerStyle(23);
  g8->SetMarkerSize(1);
  g8->SetMarkerColor(kBlack);
  g7->SetName("XYtarget");
  g8->SetName("ZYtarget");
  
  m1 = new TPolyMarker3D(1,23);
  m1->SetPoint(0,target.X(),target.Y(),target.Z());
  m1->SetMarkerColor(kBlack);

  m2 = new TPolyMarker3D(1,23);
  m2->SetPoint(0,result.getO().X(),result.getO().Y(),result.getO().Z());
  m2->SetMarkerColor(kGreen);

/*

    CbmTrackParH *fStart = new  CbmTrackParH(StartPos, StartMom, StartPosErr, StartMomErr, fCharge);
 
    CbmTrackParH *fRes = new CbmTrackParH();
 
   
    // ----- propagation: I use propagate to closest ------
    TVector3 v0 = TVector3(0., 0., 0.);
    fPro->SetPoint(v0);
    TVector3 wire1 = TVector3(0, 0, 0);
    TVector3 wire2 = TVector3(0, 0, 0);
    fPro->SetWire(wire1, wire2);
    fPro->PropagateToPCA(1);   // 1 if point; 2 if wire
    Bool_t rc = fPro->Propagate(fStart, fRes, PDGCode); 
	printf("%.10f,%.10f,%.10f\n",fRes->GetX(),fRes->GetY(),fRes->GetZ());
	//  }

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




ClassImp(POCAtestTask)