#include "Utilities/Configuration/interface/Architecture.h"
#include "TrackerReco/LowPtTracking/interface/V0Finder.h"
#include "TrackerReco/TkMSParametrization/interface/PixelRecoUtilities.h"

inline double sqr(double x) { return x*x; }
static int Debug = 1;

#include <iostream>

/*****************************************************************************/
V0Finder::V0Finder(vector<PixelRecTrack *> tracks)
{
  for(vector<PixelRecTrack *>::const_iterator it  = tracks.begin();
                                              it != tracks.end(); it++)
  {
    helix_t h;
    prepareHelix(**it,&h);
    helix.push_back(h);
  }
}

/*****************************************************************************/
V0Finder::~V0Finder() {}

/*****************************************************************************/
void V0Finder::infoHelix(helix_t h1,helix_t h2)
{
 FILE *file;
 file = fopen("out/helix.par","w");

 fprintf(file,"# Helix 1\n");
 fprintf(file,
  "X1 = %.3e; Y1=%.3e; R1=%.3e; q1=%d; psi1=%.3e; chi1=%.3e; bz1=%.3e; c1=%.3e\n",
              h1.X,h1.Y,h1.R, h1.q, h1.psi,h1.chi, h1.bz, h1.c);

 fprintf(file,"# Helix 2\n");
 fprintf(file,
  "X2 = %.3e; Y2=%.3e; R2=%.3e; q2=%d; psi2=%.3e; chi2=%.3e; bz2=%.3e; c2=%.3e\n",
              h2.X,h2.Y,h2.R, h2.q, h2.psi,h2.chi, h2.bz, h2.c);

 fclose(file);
}

/*****************************************************************************/
void V0Finder::prepareHelix(PixelRecTrack t, helix_t *h)
{
  h->q   = t.charge();

  h->R   = getRadius(t.pT().value());
  h->br  = t.charge() * t.transverseImpactParameterSignedByTrajCenter().value();
  h->bz  = t.zImpactParameter().value();
  h->c   = h->R * t.cotTheta().value();
  h->chi = t.phi().value() + t.charge() * M_PI_2;
  h->X   = -(h->R + h->br) * cos(h->chi);
  h->Y   = -(h->R + h->br) * sin(h->chi);

  h->pT       = t.pT().value();
  h->cotTheta = t.cotTheta().value();
}

/*****************************************************************************/
#include "MagneticField/BaseMagneticField/interface/MagneticField.h"
#include "ClassReuse/GeomVector/interface/GlobalPoint.h"

float V0Finder::getRadius(double pt)
{
 float fieldInInvGeV =
    1./fabs(MagneticField::inInverseGeV(GlobalPoint(0,0,0)).z());

 return pt*fieldInInvGeV; 
}

/*****************************************************************************/
bool V0Finder::getZ(helix_t h, float *z)
{
  float dpsi = h.psi - h.chi;

  while(dpsi < -M_PI) dpsi += 2*M_PI;
  while(dpsi >  M_PI) dpsi -= 2*M_PI;

  // Check if dpsi is in the proper direction
  if(h.q * dpsi < 0)
  {
    *z = h.bz - h.c * h.q * dpsi;
    return true;
  }
  else
  {
    return false;
  }
}

/*****************************************************************************/
#include "ClassReuse/GeomVector/interface/GlobalVector.h"

void V0Finder::checkIntersection(helix_t h1,helix_t h2,float deltaR)
{
  float z1,z2;

  if(getZ(h1,&z1) && getZ(h2,&z2))
  {
    // Production vertex
    GlobalVector r1(h1.X + h1.R * cos(h1.psi), h1.Y + h1.R * sin(h1.psi), z1);
    GlobalVector r2(h2.X + h2.R * cos(h2.psi), h2.Y + h2.R * sin(h2.psi), z2);
    GlobalVector r = 0.5*(r1 + r2);

    if(r.mag() < 5.)
    {
      float deltaZ = fabs(z1-z2);

      // Production momentum
      GlobalVector p1 ( h1.pT * h1.q*sin(h1.psi),
                       -h1.pT * h1.q*cos(h1.psi), h1.pT * h1.cotTheta);
      GlobalVector p2 ( h2.pT * h2.q*sin(h2.psi),
                       -h2.pT * h2.q*cos(h2.psi), h2.pT * h2.cotTheta);
      GlobalVector p = p1 + p2;
      float P = sqrt(p*p);
  
      GlobalVector b = r - (r*p)*p / sqr(P);
      float B = sqrt(b*b);

      FILE *file;
      file = fopen("out/mother.par","w");
      fprintf(file,"# Mother\n");
      fprintf(file,"rx=%.3e; ry=%.3e; rz=%.3e\n", r.x(),r.y(),r.z());
      fprintf(file,"bx=%.3e; by=%.3e; bz=%.3e\n", b.x(),b.y(),b.z());
      fclose(file);
  
      // Podolanski-Armenteros
      float Pt    = (p1.cross(p2)).mag() / P;
      float Alpha = h1.q * (p1*p1 - p2*p2)/sqr(P);

      float m_el = 0.51099892e-3;
      float m_pi = 0.13957018;
      float m_pr = 0.938272029;

      float m_k0 = sqrt(sqr(sqrt(p1*p1 + sqr(m_pi)) +
                            sqrt(p2*p2 + sqr(m_pi))) - sqr(P));

      float m_la;
      if(h1.q > 0)
        m_la = sqrt(sqr(sqrt(p1*p1 + sqr(m_pr)) + 
                        sqrt(p2*p2 + sqr(m_pi))) - sqr(P));
      else
        m_la = sqrt(sqr(sqrt(p2*p2 + sqr(m_pr)) + 
                        sqrt(p1*p1 + sqr(m_pi))) - sqr(P));

      float m_ph = sqrt(sqr(sqrt(p1*p1 + sqr(m_el)) +
                            sqrt(p2*p2 + sqr(m_el))) - sqr(P));

      fprintf(stderr,"   dr = %.3f | dz = %.3f | r = %.3f |",
                     deltaR,deltaZ,r.mag());
      fprintf(stderr," b = %.3f | p = %.3f | pt = %.3f | alpha = %.3f |",
                     B,P,Pt,Alpha);
      fprintf(stderr," m_k0 = %f | m_la = %f | m_ph = %f | rt = %f\n",
                     m_k0,m_la,m_ph, r.perp());

      infoHelix(h1,h2);
//      while(getchar()==0);
    }
    else fprintf(stderr,"   did not pass r.mag()\n");
  }
  else fprintf(stderr,"   did not pass getZ\n");
}

/*****************************************************************************/
void V0Finder::getAzimuth(helix_t h1,helix_t h2)
{
 float deltaR;

 // Distance and relative direction of the centers
 float dx = h2.X - h1.X;
 float dy = h2.Y - h1.Y;

 float R12  = sqrt(sqr(dx) + sqr(dy));
 float psi0 = atan2(dy, dx);

 // Check triangle inequalities
 if(R12 < h1.R + h2.R && R12 > fabs(h1.R - h2.R))
 { // intersection
  if(Debug) cerr << "  circles intersect" << endl;
  float gamm = acos((sqr(h1.R) - sqr(h2.R) + sqr(R12))/(2*h1.R*R12));

  for(int j=0; j<2; j++)
  {
   int sign = 2*j-1;
   h1.psi = psi0 + sign*gamm;
   h2.psi = atan2(h1.Y - h2.Y + h1.R*sin(h1.psi),
                  h1.X - h2.X + h1.R*cos(h1.psi));

   deltaR = 0.;

   checkIntersection(h1,h2,deltaR);
  }
 }
 else
 {
  if(R12 > h1.R + h2.R)
  { // outside, transverse distance R21-(R1+R2)
   if(Debug) cerr << "  circles disjoint" << endl;
   h1.psi = psi0;
   h2.psi = psi0 + M_PI;

   deltaR = R12 - (h1.R + h2.R);
  }
  else
  { // inside, transverse distance |R1-R2|-R12
   if(Debug) cerr << "  circles contains" << endl;
   if(h2.R < h1.R) h1.psi = psi0;
              else h1.psi = psi0 + M_PI;
   h2.psi = h1.psi;

   deltaR = fabs(h1.R - h2.R) - R12;
  }

  checkIntersection(h1,h2,deltaR);
 }
}

/*****************************************************************************/
void V0Finder::doIt()
{
 for(vector<helix_t>::const_iterator h1  = helix.begin();
                                     h1 != helix.end()-1; h1++)
 for(vector<helix_t>::const_iterator h2 = h1+1;
                                     h2 != helix.end(); h2++)
   if((*h1).q != (*h2).q)
     getAzimuth(*h1,*h2);
}