text: #include "RecoVertex/KinematicFit/interface/SmartPointingConstraint.h"
  #include "RecoVertex/VertexPrimitives/interface/VertexException.h"
  
  
  std::pair<AlgebraicVector, AlgebraicVector> SmartPointingConstraint::value(const AlgebraicVector& exPoint) const
  {
   if(exPoint.num_row() ==0 ) throw VertexException("PointingKinematicConstraint::value requested for zero Linearization point");
  
  //security check for extended cartesian parametrization 
   int inSize = exPoint.num_row(); 
   if((inSize%7) !=0) throw VertexException("PointingKinematicConstraint::linearization point has a wrong dimension");
   int nStates = inSize/7;
   if(nStates != 1) throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
   
   AlgebraicVector lPar = exPoint;
   AlgebraicVector vl(2,0);
   
  //vector of values 1x2  for given particle
   AlgebraicVector lValue = makeValue(lPar).first;
   vl(1) =lValue(1);
   vl(2) =lValue(2);
   return std::pair<AlgebraicVector,AlgebraicVector>(vl,lPar); 
  }
  
  std::pair<AlgebraicMatrix, AlgebraicVector> SmartPointingConstraint::derivative(const AlgebraicVector& exPoint) const
  {
   if(exPoint.num_row() ==0 ) throw VertexException("PointingKinematicConstraint::value requested for zero Linearization point");
  
  //security check for extended cartesian parametrization 
   int inSize = exPoint.num_row(); 
   if((inSize%7) !=0) throw VertexException("PointingKinematicConstraint::linearization point has a wrong dimension");
   int nStates = inSize/7;
   if(nStates != 1) throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
   AlgebraicVector lPar = exPoint;
  
  //2x7 derivative matrix for given particle
   AlgebraicMatrix lDeriv = makeDerivative(lPar).first;
   AlgebraicMatrix dr(2,7,0);
   dr.sub(1,1,lDeriv);
   return std::pair<AlgebraicMatrix,AlgebraicVector>(dr,lPar);
  }
  
  std::pair<AlgebraicMatrix, AlgebraicVector> SmartPointingConstraint::derivative(const std::vector<RefCountedKinematicParticle> &par) const
  {
   int nStates = par.size();
   if(nStates == 0) throw VertexException("PointingKinematicConstraint::Empty vector of particles passed");
   if(nStates != 1) throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
   
   AlgebraicMatrix dr(2,7,0);
   AlgebraicVector lPoint = asHepVector<7>(par.front()->currentState().kinematicParameters().vector());
  
  //2x7 derivative matrix for given state  
   AlgebraicMatrix lDeriv = makeDerivative(lPoint).first;
   dr.sub(1,1,lDeriv);
  // cout<<"Derivative returned: "<<dr<<endl;
  // cout<<"For the value: "<<lPoint<<endl;
   return std::pair<AlgebraicMatrix,AlgebraicVector>(dr,lPoint);
  }
  
  std::pair<AlgebraicVector, AlgebraicVector> SmartPointingConstraint::value(const std::vector<RefCountedKinematicParticle> &par) const
  { 
   int nStates = par.size();
   if(nStates == 0) throw VertexException("PointingKinematicConstraint::Empty vector of particles passed");
   if(nStates != 1) throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
   AlgebraicVector vl(2,0);
   AlgebraicVector lPoint = asHepVector<7>(par.front()->currentState().kinematicParameters().vector());
   vl(1) = makeValue(lPoint).first(1);
   vl(2) = makeValue(lPoint).first(2);
  // cout<<"Value returned: "<<vl<<endl;
  // cout<<"For the point: "<<lPoint<<endl;
   
   return std::pair<AlgebraicVector,AlgebraicVector>(vl,lPoint);
  }
   
  AlgebraicVector SmartPointingConstraint::deviations(int nStates) const
  {return AlgebraicVector(7*nStates,0);}
   
  int SmartPointingConstraint::numberOfEquations() const
  {return 2;}
   
  std::pair<AlgebraicVector,AlgebraicVector> SmartPointingConstraint::makeValue(const AlgebraicVector& exPoint)const 
  { 
  // cout<<"Make value called"<<endl;
   AlgebraicVector vl(2,0);
   AlgebraicVector point = exPoint;
   double dx = point(1) - refPoint.x();
   double dy = point(2) - refPoint.y();
   double dz = point(3) - refPoint.z();
   double px = point(4);
   double py = point(5); 
   double pz = point(6);
  
  
  //full angle solution: sin(alpha - betha) = 0
  //sign swap allowed
   double cos_phi_p = px/sqrt(px*px + py*py);
   double sin_phi_p = py/sqrt(px*px + py*py);
   double cos_phi_x = dx/sqrt(dx*dx + dy*dy);
   double sin_phi_x = dy/sqrt(dx*dx + dy*dy);
   
   double sin_theta_p = pz/sqrt(px*px + py*py + pz*pz); 
   double sin_theta_x = dz/sqrt(dx*dx + dy*dy + dz*dz);
   
   double cos_theta_p = sqrt(px*px + py*py)/sqrt(px*px + py*py + pz*pz); 
   double cos_theta_x = sqrt(dx*dx + dy*dy)/sqrt(dx*dx + dy*dy + dz*dz);
   
   float feq = sin_phi_p*cos_phi_x - cos_phi_p*sin_phi_x;
   float seq = sin_theta_p* cos_theta_x - cos_theta_p * sin_theta_x;
   
   vl(1) = feq;
   vl(2) = seq;
  
   return std::pair<AlgebraicVector,AlgebraicVector>(vl,point);
  }
  
  std::pair<AlgebraicMatrix, AlgebraicVector> SmartPointingConstraint::makeDerivative(const AlgebraicVector& exPoint) const
  { 
   AlgebraicMatrix dr(2,7,0);
   AlgebraicVector point = exPoint;
   double dx = point(1) - refPoint.x();
   double dy = point(2) - refPoint.y();
   double dz = point(3) - refPoint.z();
   double px = point(4);
   double py = point(5); 
   double pz = point(6);
   
  //angular functuions:
  
  //half angle solution
  //d/dx_i
   dr(1,1) = (dy*(dx*px + dy*py))/(pow(pow(dx,2) + pow(dy,2),1.5)*sqrt(pow(px,2) + pow(py,2))) ;
  	
   dr(1,2) = -((dx*(dx*px + dy*py))/(pow(pow(dx,2) + pow(dy,2),1.5)*sqrt(pow(px,2) + pow(py,2)))) ;
   
   dr(1,3) = 0;
   
  //d/dp_i  
  //debug: x->p index xhange in denominator
   dr(1,4) = -((py*(dx*px + dy*py))/(sqrt(pow(dx,2) + pow(dy,2))*pow(pow(px,2) + pow(py,2),1.5)));
  			   
   dr(1,5) = (px*(dx*px + dy*py))/(sqrt(pow(dx,2) + pow(dy,2))*pow(pow(px,2) + pow(py,2),1.5));
   
   dr(1,6) = 0;
   dr(1,7) = 0; 
  
  //2nd equation
  //d/dx_i
  
   dr(2,1) = (dx*dz*(sqrt(pow(dx,2) + pow(dy,2))*sqrt(pow(px,2) + pow(py,2)) + dz*pz))/
             (sqrt(pow(dx,2) + pow(dy,2))*pow(pow(dx,2) + pow(dy,2) + pow(dz,2),1.5)*
              sqrt(pow(px,2) + pow(py,2) + pow(pz,2)));
             
   dr(2,2) = (dy*dz*(sqrt(pow(dx,2) + pow(dy,2))*sqrt(pow(px,2) + pow(py,2)) + dz*pz))/
             (sqrt(pow(dx,2) + pow(dy,2))*pow(pow(dx,2) + pow(dy,2) + pow(dz,2),1.5)*
              sqrt(pow(px,2) + pow(py,2) + pow(pz,2)));
             
  	
   dr(2,3) = (-((pow(dx,2) + pow(dy,2))*sqrt(pow(px,2) + pow(py,2))) - sqrt(pow(dx,2) + pow(dy,2))*dz*pz)/
             (pow(pow(dx,2) + pow(dy,2) + pow(dz,2),1.5)*sqrt(pow(px,2) + pow(py,2) + pow(pz,2)));
             
  
   
  //d/dp_i 
  //debug: x->p index xhange in denominator
  
   dr(2,4) = -((px*pz*(sqrt(pow(dx,2) + pow(dy,2))*sqrt(pow(px,2) + pow(py,2)) + dz*pz))/
              (sqrt(pow(dx,2) + pow(dy,2) + pow(dz,2))*sqrt(pow(px,2) + pow(py,2))*
               pow(pow(px,2) + pow(py,2) + pow(pz,2),1.5)));
   
   dr(2,5) = -((py*pz*(sqrt(pow(dx,2) + pow(dy,2))*sqrt(pow(px,2) + pow(py,2)) + dz*pz))/
              (sqrt(pow(dx,2) + pow(dy,2) + pow(dz,2))*sqrt(pow(px,2) + pow(py,2))*
              pow(pow(px,2) + pow(py,2) + pow(pz,2),1.5))) ;
   
   dr(2,6) = (sqrt(pow(dx,2) + pow(dy,2))*(pow(px,2) + pow(py,2)) + dz*sqrt(pow(px,2) + pow(py,2))*pz)/
             (sqrt(pow(dx,2) + pow(dy,2) + pow(dz,2))*pow(pow(px,2) + pow(py,2) + pow(pz,2),1.5)) ;
   
   dr(2,7) = 0;
   
  // cout<<"derivative matrix "<<dr<<endl;
   return std::pair<AlgebraicMatrix,AlgebraicVector>(dr,point); 
  }
  