/***************************************************************************** 
 * Project: RooFit                                                           * 
 *                                                                           * 
 * This code was autogenerated by RooClassFactory                            * 
 *****************************************************************************/ 

// Your description goes here... 

#include "Riostream.h" 

#include "RooClusterPdf.h" 
#include "RooAbsReal.h" 
#include "RooAbsCategory.h" 
#include <math.h> 
#include "TMath.h" 
#include "RooListProxy.h"
#include "RooRealProxy.h"
#include "TMatrixDSym.h"
#include "TMatrixD.h"
#include "TVectorD.h"

ClassImp(RooClusterPdf) 

RooClusterPdf::RooClusterPdf(const char *name, const char *title, 
			   const RooArgList& xvec,
			   RooAbsReal& _frac,
			   const RooArgList& muvec,
			   const TMatrixDSym& covMatrix,
			   const TMatrixDSym& covMatrixPad ) :
			   RooAbsPdf(name,title),
			   x("x","Observables",this,kTRUE,kFALSE),
			   frac("frac","fraction",this,_frac),
			   mu("mu","Offset vector",this,kTRUE,kFALSE),
			   cov(covMatrix),
			   covI(covMatrix),
			   covPad(covMatrixPad),
			   covIPad(covMatrixPad)
{ 
	 x.add(xvec) ;
	 mu.add(muvec);
	 det    = covMatrix.Determinant() ;
	 detPad = covMatrixPad.Determinant() ;

	 // Invert covariance matrix
	 covI.Invert() ;
	 covIPad.Invert();
	 syncMuVec();
} 


RooClusterPdf::RooClusterPdf(const RooClusterPdf& other, const char* name) :  
RooAbsPdf(other,name), 
  x("x",this,other.x),
  mu("muY",this,other.mu),
  det(other.det),
  detPad(other.detPad),
  cov(other.cov),
  covI(other.covI),
  covPad(other.covPad),
  covIPad(other.covIPad),
  frac("frac",this,other.frac)
{ 
	syncMuVec();
} 


//Int_t RooG2DimGauss::getAnalyticalIntegral(RooArgSet& allVars, RooArgSet& analVars, const char* /*rangeName*/) const
 /*
{
  if (matchArgs(allVars,analVars,x)) return 1 ;
  return 0 ;
}


Double_t RooClusterPdf::analyticalIntegral(Int_t code, const char* rangeName) const 
{
  assert(code==1) ;

}
 */

Double_t RooClusterPdf::evaluate() const 
{ 

  double retval=0;

  // Represent observables as vector
  TVectorD xVec(x.getSize()) ;
  for (int i=0 ; i<x.getSize() ; i++)
  {
    xVec[i] = ((RooAbsReal*)x.at(i))->getVal() ;
  }
  syncMuVec();

  TVectorD x_to_mu(xVec - muVec);
  TVectorD x_to_mu_norm(x_to_mu);
  //std::cout<<"x_to_mu_norm("<<x_to_mu_norm[0]<<","<<x_to_mu_norm[1]<<")\n";
  x_to_mu_norm*=1./TMath::Sqrt(x_to_mu_norm.Norm2Sqr());
  //std::cout<<"x_to_mu_norm("<<x_to_mu_norm[0]<<","<<x_to_mu_norm[1]<<")\n";
  //calculate intersection point of x_to_mu with covariance matrix
  x_to_mu_norm*=cutCov(covI,x_to_mu) ;
  //std::cout<<"x_to_mu_norm("<<x_to_mu_norm[0]<<","<<x_to_mu_norm[1]<<")\n";
  TVectorD x_to_mu_intersect(muVec);
  x_to_mu_intersect+= x_to_mu_norm;

  /*
  if(x_to_mu[0]>0)
  {
	  std::cout<<"bigger than\n";
  }
  else{std::cout<<"smaller than\n";}
  cov.Print();
  muVec.Print();
*/
  //TVectorD x_to_mu_anti(x_to_mu);
  //x_to_mu_anti*=-1;
  //std::cout<<cutCov(covI,x_to_mu)<<";"<<cutCov(covI,x_to_mu_anti)<<"\n";
  //if(fabs(xVec[0]-0.08)<0.001)syncMuVec();
  /*{
	  std::cout<<"xVec("<<xVec[0]<<","<<xVec[1]<<") muVec("<<muVec[0]<<","<<muVec[1]<<")\n";
	  std::cout<<"x_to_mu("<<x_to_mu[0]<<","<<x_to_mu[1]<<") cutCov("<<cutCov(covI,x_to_mu)<<")\n";
	  std::cout<<"x_to_mu_intersect("<<x_to_mu_intersect[0]<<","<<x_to_mu_intersect[1]<<")\n";
	  std::cout<<"x_to_mu_norm("<<x_to_mu_norm[0]<<","<<x_to_mu_norm[1]<<")\n";
	  std::cout<<"len x_to_mu: "<<TMath::Sqrt(x_to_mu.Norm2Sqr())<<" len intersect: "<<TMath::Sqrt(x_to_mu_intersect.Norm2Sqr())<<"\n";
  }*/

  Double_t sig_int= TMath::Sqrt(x_to_mu_norm.Norm2Sqr());
  Double_t arg_int= TMath::Sqrt((x_to_mu).Norm2Sqr());
  Double_t g1=exp(-0.5*arg_int*arg_int/(sig_int*sig_int));
  //Double_t height =exp(-0.5) ;

  //if(TMath::Sqrt(x_to_mu.Norm2Sqr())<= TMath::Sqrt(x_to_mu_norm.Norm2Sqr()))
  {

	  //return exp(-0.5*arg_int*arg_int/(sig_int*sig_int));
  }



  //calculate sigma of pad gauss in direction of x_to_mu
  Double_t sig=cutCov(covIPad,x_to_mu);

  Double_t arg= TMath::Sqrt((xVec - x_to_mu_intersect).Norm2Sqr());
  Double_t g2 =exp(-0.5*arg*arg/(sig*sig)) ;
  //std::cout<<"returning 0\n";
  //std::cout<<"\n";
  //std::cout<<"returning: "<<ret<<" at: "<<"xVec("<<xVec[0]<<","<<xVec[1]<<")\n";
  retval=frac*g1+(1-frac)*g2;
  return retval;
} 

void RooClusterPdf::syncMuVec() const
{
	muVec.ResizeTo(mu.getSize()) ;
	for (Int_t i=0 ; i<mu.getSize() ; i++) {
		muVec[i] = ((RooAbsReal*)mu.at(i))->getVal() ;
	}
}

double RooClusterPdf::cutCov(TMatrixDSym acov, TVectorD avec) const
{
	double cutVal=0;

	if(TMath::Sqrt(avec.Norm2Sqr())!=0)
	{
		avec*=1./TMath::Sqrt(avec.Norm2Sqr());
	}
	else
	{
		std::cout<<"error in cutCov: length of avec==0\n";
		return cutVal;
	}
	cutVal=acov.Similarity(avec);

	if(cutVal!=0)
	{
		cutVal=1./TMath::Sqrt(fabs(cutVal));
	}
	else
	{
		std::cout<<"error in cutCov: cut with cov ==0\n";
		cutVal=0;
	}
	return cutVal;
}