#include "poca.h"
#include "TMinuit.h"
#include "TROOT.h"

#include <iostream>
#include <cmath>
#include "StdoutKiller.h"
#include "TMath.h"

double poca_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;
}




TVector3 targetPos,targetErr;
DetPlane poca_plane;
TVector3 poca_startO,poca_N;
AbsTrackRep *poca_rep;

double poca_distance(double s){
  poca_plane.setO(poca_startO+s*poca_N);
  TVector3 pos;
  {
	StdoutKiller theKilla;
	pos=poca_rep->getPos(poca_plane);
  }
//   pos.Print();
//   poca_plane.Print();
//   std::cout << "s= " << s << " D=" <<  sqrt (((pos.X()-targetPos.X())*(pos.X()-targetPos.X()) / (targetErr.X()*targetErr.X())) +
// 			   ((pos.Y()-targetPos.Y())*(pos.Y()-targetPos.Y()) / (targetErr.Y()*targetErr.Y())) +
// 			   ((pos.Z()-targetPos.Z())*(pos.Z()-targetPos.Z()) / (targetErr.Z()*targetErr.Z()))
// 											 )<<std::endl;


  return sqrt (((pos.X()-targetPos.X())*(pos.X()-targetPos.X()) / (targetErr.X()*targetErr.X())) +
			   ((pos.Y()-targetPos.Y())*(pos.Y()-targetPos.Y()) / (targetErr.Y()*targetErr.Y())) +
			   ((pos.Z()-targetPos.Z())*(pos.Z()-targetPos.Z()) / (targetErr.Z()*targetErr.Z()))
			   );
}


void poca_fcn(Int_t &npar, Double_t *gin, Double_t &f, Double_t *par, Int_t iflag)
{

  f = poca_distance(par[0]);
	 
  TVector3 pos,mom;
  {
	StdoutKiller specialK;
	poca_rep->getPosMom(poca_plane,pos,mom);
  }
  double angle = (pos-targetPos).Angle(mom)/TMath::Pi()*180.;

  printf("f=%.10f s=%.10f angle=%.10f\n",f,par[0],angle);
}


//#include "gsl/gsl_qrng.h"
bool findPOCAplane(AbsTrackRep* rep,const TVector3& target,const TVector3& target_err,DetPlane& result){

  TVector3 xStart,pStart;
  poca_findClosePoint(rep,target,poca_startO,poca_N);
  poca_plane.setO(poca_startO);
  poca_plane.setNormal(poca_N);

  //set global variables
  poca_rep = rep; 
  targetPos = target;
  targetErr = target_err;

  // for(int i=300; i>=-300;--i){
  //    	double s=i*0.01;
  //    	printf("%.10f %.10f\n",s,poca_distance(s));	
  //   }
  //   return true;


  double minS,maxS,startS;

  static const double dS = .05;
  
  double zeroD = poca_distance(0.);
  double plusD = poca_distance(dS);
  double minusD = poca_distance(-1.*dS);

  if(plusD<zeroD && minusD<zeroD){
	std::cerr << "poca.cxx in trouble: plusD<zeroD && minusD<zeroD -> abort"
			  << std::endl;
	throw;
  }
  if(plusD>zeroD && minusD>zeroD) {
	minS = -1.*dS;
	maxS = dS;
	startS = 0.;
  }
  else{
	if(minusD>zeroD){//
	  double D,D_minus_2dS,D_minus_dS,S;
	  D_minus_2dS = zeroD;
	  D_minus_dS  = plusD;
	  S = 2*dS;
	  D = poca_distance(S);
	  while(D<D_minus_dS){
		S += dS;
		D_minus_2dS = D_minus_dS;
		D_minus_dS = D;
		D = poca_distance(S);
	  }
	  maxS = S;
	  startS = S - dS;
	  minS = S - 2.*dS;
	}
	else{
	  double D,D_plus_2dS,D_plus_dS,S;
	  D_plus_2dS = zeroD;
	  D_plus_dS  = minusD;
	  S = -2.*dS;
	  D = poca_distance(S);
	  while(D<D_plus_dS){
		S -= dS;
		D_plus_2dS = D_plus_dS;
		D_plus_dS = D;
		D = poca_distance(S);
	  }
	  minS = S;
	  startS = S + dS;
	  maxS = S + 2.*dS;
	}
  }

  //printf("###### min=%.10f start=%.10f  max=%.10f\n",minS,startS,maxS);
  /*
  TMinuit *myMinuit = new TMinuit(2);  //initialize TMinuit with a maximum of 1 params
  myMinuit->SetFCN(poca_fcn);
  
  Double_t arglist[10];
  Int_t ierflg = -1;
  
  arglist[0] = 1;
  {
    StdoutKiller killer;
	myMinuit->mnexcm("SET ERR", arglist ,1,ierflg);
  }
  // Set starting values and step sizes for parameters
  

  {
    //StdoutKiller killer;
    myMinuit->mnparm(0, "s", startS,dS/2.,minS-1.,maxS+1.,ierflg);
  }

  // Now ready for minimization step
  arglist[0] = 500;
  arglist[1] = 1.;

  {
    //StdoutKiller killer;
    myMinuit->mnexcm("MIGRAD", arglist ,2,ierflg);
  }
  

  //  if(ierflg!=0) return false;

  double s_final,dummy;
  myMinuit->GetParameter(0,s_final,dummy);
  
  delete myMinuit;  

  //////////////////
  */
  double s_final=startS;

  DetPlane fitresult(poca_startO+s_final*poca_N,poca_N);
  TVector3 pos_res,mom_res;
  {
    StdoutKiller K;
	poca_rep->getPosMom(fitresult,pos_res,mom_res);
  }
  //fitresult.Print();
  //pos_res.Print();
  result.set(pos_res,target-pos_res,mom_res.Cross(target-pos_res));
  //result.Print();
  return true;

}