double yeta(float*);
double eta(float*);
double pt(float*);
void vadd(float*, float*, float*);
void l4add(float*, float*, float*);
void l4add3(float*, float*, float*, float*);
double mass(float*);
double m12(float*, float*);
double m123(float*, float*, float*);
double cos12(float*, float*);
double sclprd(float*, float*);
double vlen(float*);
void vunit(float*);
void vecprd(float*, float*, float*);
void loren4(float*, float*, float*);
void mloren4(float*, float*, float*, float);
void generalphi(float*, float*, float*, float*, float*);
double logfact(int);
double likeliAsym(float, int, int);

/**************************************************
      yeta(p) (rapidity)
**************************************************/
double yeta(float* p)
{
  double betaz=p[2]/p[3];
  double eta;
  eta=0.5*log((1.0+betaz)/(1.0-betaz));
  return eta;
}

/**************************************************
      eta(p) (pseudo rapidity)
**************************************************/
double eta(float* p)
{
  double cost=double(p[2])/vlen(p);
  double eta;
  eta=-0.5*log((1.0-cost)/(1.0+cost));

  return eta;
}

/**************************************************
      pt(p)
**************************************************/
double pt(float* p)
{
  return sqrt(double(p[0])*p[0]+double(p[1])*p[1]);
}

/**************************************************
      subroutine vadd (p1, p2, p3)
**************************************************/
void vadd(float* p1, float* p2, float* p3)
{
  for( int i=0; i<3; ++i) {
    p3[i]=double(p1[i])+p2[i];
  }
}

/**************************************************
      subroutine l4add (p1, p2, p3)
**************************************************/
void l4add(float* p1, float* p2, float* p3)
{
  for( int i=0; i<4; ++i) {
    p3[i]=double(p1[i])+p2[i];
  }
}

/**************************************************
      subroutine l4add3 (p1, p2, p3, p4)  p4=p1+p2+p3
**************************************************/
void l4add3(float* p1, float* p2, float* p3, float* p4)
{
  for( int i=0; i<4; ++i) {
    p4[i]=double(p1[i])+p2[i]+p3[i];
  }
}

/* -------------------------------------------------
 *  invariant mass 
 --------------------------------------------------- */
double mass(float* p)
{
  return sqrt(double(p[3])*p[3]-double(p[0])*p[0]-double(p[1])*p[1]
	      -double(p[2])*p[2]);
}

/**************************************************
      real function m12(p1, p2)
C...  pi=(px, py, pz, E)
**************************************************/
double m12(float* p1, float* p2)
{
  double p[4];

  for( int i=0; i<4; ++i) {
    p[i]=double(p1[i])+p2[i];
  }
  return sqrt(p[3]*p[3]-p[0]*p[0]-p[1]*p[1]-p[2]*p[2]);
}

/**************************************************
      real function m123(p1, p2, p3)
**************************************************/
double m123(float* p1, float* p2, float* p3)
{
  double p[4];

  for( int i=0; i<4; ++i) {
    p[i]=double(p1[i])+p2[i]+p3[i];
  }
  return sqrt(p[3]*p[3]-p[0]*p[0]-p[1]*p[1]-p[2]*p[2]);
}

/**************************************************
      real function cos12(p1, p2)
**************************************************/
double cos12(float* p1, float* p2)
{
  return sclprd(p1, p2)/vlen(p1)/vlen(p2);
}

/**************************************************
      real function sclprd(p1, p2)
**************************************************/
double sclprd(float* p1, float* p2)
{
  return double(p1[0])*p2[0]+double(p1[1])*p2[1]+double(p1[2])*p2[2];
}

/**************************************************
      real function vlen(p)
**************************************************/
double vlen(float* p)
{
  return sqrt(double(p[0])*p[0]+double(p[1])*p[1]+double(p[2])*p[2]);
}

/**************************************************
      subroutine vunit(p)
**************************************************/
void vunit(float* p)
{
  double len=vlen(p);
  p[0]=p[0]/len;
  p[1]=p[1]/len;
  p[2]=p[2]/len;
}

/**************************************************
      subroutine vecprd(p1, p2, p3)
**************************************************/
void vecprd(float* p1, float* p2, float* p3)
{
  p3[0]=double(p1[1])*p2[2]-double(p1[2])*p2[1];
  p3[1]=double(p1[2])*p2[0]-double(p1[0])*p2[2];
  p3[2]=double(p1[0])*p2[1]-double(p1[1])*p2[0];
}

/* ------------------------------------------------
 * loren4(S,A,X)
 -------------------------------------------------- */
void loren4(float* s, float* a, float* x)
{
  double ms=mass(s);
  double prod=sclprd(s, a);
  x[3]=(double(a[3])*s[3]-prod)/ms;

  prod=(prod/(ms+double(s[3]))-double(a[3]))/ms;
  for(int i=0; i<3; ++i) {
    x[i]=double(a[i])+s[i]*prod;
  }
}

/* ------------------------------------------------
 * mloren4(S,A,X, mass) : loren4 with mass
 -------------------------------------------------- */
void mloren4(float* s, float* a, float* x, float ms)
{
  //checknan(ms, "loren4:ms!");
  double prod=sclprd(s, a);
  x[3]=(double(a[3])*s[3]-prod)/ms;

  prod=(prod/(ms+double(s[3]))-double(a[3]))/ms;
  for(int i=0; i<3; ++i) {
    x[i]=double(a[i])+s[i]*prod;
  }
}

/* -------------------------------------------------------------------
 * general phi
 * In a coordinates given by vector a1 and a2, that is defined by
 * a1 as z-axis, a1 x a2 as y-axis, and y-axis x z-axis as x-axis,
 * angle phi of vector b will be given as well as cos theta.
 --------------------------------------------------------------------- */
void generalphi(float* a1, float* a2, float * b, float* phi, float* cost)
{
  float x[3], y[3], z[3];

  for(int i=0; i<3; ++i) z[i]=a1[i];
  vunit(z);

  vecprd(a1, a2, y);
  vunit(y);
  vecprd(y, z, x);
  vunit(x);

  double sinp=sclprd(b, y);
  double cosp=sclprd(b, x);
  *phi=atan2(sinp, cosp);
  *cost=cos12(b, z);

  return;
}

/* -----------------------------------------------------
 * calc log(n!)
 ------------------------------------------------------- */
double logfact(int n)
{
  double f=0.0;

  for(int i=1; i<=n; ++i) {f+=log(double(i));}
  return f;
}

/* ---------------------------------------------------------
 * 
 *  likelihood of asymmetry for (np, nn) observed
 *   a=(mp-mn)/(mp+mn)
 * 
 ----------------------------------------------------------- */
double likeliAsym(float a, int np, int nn)
{
  double ll=0.0;

  ll=logfact(np+nn+1)-logfact(np)-logfact(nn);
  ll+=np*log((1.0+a)/2.0)+nn*log((1.0-a)/2.0);
  return 0.5*exp(ll);
}

// EOF ///////