/*
   Subroutine: abc_uti.c

      This subroutine contains various utilities used by the ABC Simulator
   program:

         uti_bst  -->  performs Lorentz boost on 4-vector
         uti_int  -->  determines intersection point of line and object
         uti_loc  -->  determines location of point relative to object
         uti_rnu  -->  returns uniform deviate
         uti_rnn  -->  returns normal deviate
         uti_rot  -->  performs translation and rotation of coordinates
*/

#include "abc.h"

/*
   Utility: uti_bst

      Performs a Lorentz boost on the 4-vector X (in reference frame Kx) to a
   frame Ky moving with velocity vector B with respect to Kx, returning the Ky
   4-vector Y [see Jackson, Classical Electrodynamics, (11.19)].
*/
void uti_bst(bx,by,bz,x,y)
double bx,by,bz,x[4],y[4];
{
   int i;
   double bm,bd,gm;
/* Calculate beta magnitude, dot product, gamma */
   bm = sqrt(bx*bx + by*by + bz*bz);
   if(bm > 1.0)
   {
      fprintf(stderr,"uti_bst: beta > 1 \n"); return;
   }
   if(bm == 0.0)
   {
      for(i=0;i<4;i++) y[i] = x[i]; return;
   }
   bd = bx*x[1] + by*x[2] + bz*x[3]; gm = 1.0/sqrt(1.0-bm*bm);
/* Perform boost */
   y[0] = gm*(x[0]-bd);
   y[1] = x[1] + (bx*(gm-1.0)*bd)/(bm*bm) - gm*bx*x[0];
   y[2] = x[2] + (by*(gm-1.0)*bd)/(bm*bm) - gm*by*x[0];
   y[3] = x[3] + (bz*(gm-1.0)*bd)/(bm*bm) - gm*bz*x[0];
}

/*
   Utility: uti_int

      Determines the intersection point of an object between two points, given
   a preference of nearest point (is = 1,2), with the following return codes:

         ir = +2  -->  line crosses object (both points outside)
         ir = +1  -->  line enters object (point 2 inside)
         ir = +0  -->  no intersection (both points outside, or undefined)
         ir = -1  -->  line exits object (point 1 inside)
         ir = -2  -->  no intersection (both points inside)
*/
int uti_int(io,is,x1,y1,z1,x2,y2,z2,xo,yo,zo)
int io,is;
double x1,y1,z1,x2,y2,z2,*xo,*yo,*zo;
{
   double x0,y0,z0,xw,yw,zw,iw,ow,ea,eb,ec,x1p,y1p,z1p,x2p,y2p,z2p,r1p,r2p,rbp;
   double xi,yi,zi,xip,yip,zip,dd,dx,dy,dz,cx,cy,cz,t1p,t2p,tdp;
   double xa,ya,za,xb,yb,zb,xc,yc,zc,xn,yn,zn,xf,yf,zf,ra,rb,rc;
   double mx,bx,my,by,be,ga,xt,yt,zt,dt,rt,rn,rf;
   int i,ir,i1,i2,ia,ib,ic,in,ti,to;

/* Initialize object data, rotate to object frame */
   x0 = objdim[io][1]; ea = objdim[io][7]; *xo = x2;
   y0 = objdim[io][2]; eb = objdim[io][8]; *yo = y2;
   z0 = objdim[io][3]; ec = objdim[io][9]; *zo = z2;
   i1 = uti_loc(io,x1,y1,z1); i2 = uti_loc(io,x2,y2,z2);
   uti_rot(+1,x1,y1,z1,x0,y0,z0,ea,eb,ec,&x1p,&y1p,&z1p);
   uti_rot(+1,x2,y2,z2,x0,y0,z0,ea,eb,ec,&x2p,&y2p,&z2p);

/* Calculate line quantities (distance, direction cosine) */
   dx = x2p - x1p; dy = y2p - y1p; dz = z2p - z1p;
   dd = sqrt(dx*dx + dy*dy + dz*dz); if(dd == 0.0) return(0);
   cx = dx/dd; cy = dy/dd; cz = dz/dd;

/* Box */
   if(strcmp(objshp[io],"box") == 0)
   {
   /* Initialize object size, check if points outside, pick preference */
      if(i1 < 1 && i2 < 1) return(-2);
      xw = 0.5*objdim[io][4]; ia = 1; xa = 0.0;
      yw = 0.5*objdim[io][5]; ib = 1; yb = 0.0;
      zw = 0.5*objdim[io][6]; ic = 1; zc = 0.0;
      if((x1p<-xw && x2p<-xw)||(x1p>+xw && x2p>+xw)) return(0);
      if((y1p<-yw && y2p<-yw)||(y1p>+yw && y2p>+yw)) return(0);
      if((z1p<-zw && z2p<-zw)||(z1p>+zw && z2p>+zw)) return(0);
      if(is == 1) { xn=x1p; yn=y1p; zn=z1p; xf=x2p; yf=y2p; zf=z2p; }
      else        { xn=x2p; yn=y2p; zn=z2p; xf=x1p; yf=y1p; zf=z1p; }
   /* Solution 1 (x-border) */
      if     (xn<-xw && -xw<xf) xa = -xw;
      else if(xf<+xw && +xw<xn) xa = +xw;
      else if(xf<-xw && -xw<xn) xa = -xw;
      else if(xn<+xw && +xw<xf) xa = +xw;
      ya = (xa-x1p)*(cy/cx) + y1p;
      za = (xa-x1p)*(cz/cx) + z1p;
      if(ya < -yw || ya > +yw || za < -zw || za > +zw || xa == 0.0) ia = 0;
      else ra = (xa-xn)*(xa-xn) + (ya-yn)*(ya-yn) + (za-zn)*(za-zn);
   /* Solution 2 (y-border) */
      if     (yn<-yw && -yw<yf) yb = -yw;
      else if(yf<+yw && +yw<yn) yb = +yw;
      else if(yf<-yw && -yw<yn) yb = -yw;
      else if(yn<+yw && +yw<yf) yb = +yw;
      xb = (yb-y1p)*(cx/cy) + x1p;
      zb = (yb-y1p)*(cz/cy) + z1p;
      if(xb < -xw || xb > +xw || zb < -zw || zb > +zw || yb == 0.0) ib = 0;
      else rb = (xb-xn)*(xb-xn) + (yb-yn)*(yb-yn) + (zb-zn)*(zb-zn);
   /* Solution 3 (z-border) */
      if     (zn<-zw && -zw<zf) zc = -zw;
      else if(zf<+zw && +zw<zn) zc = +zw;
      else if(zf<-zw && -zw<zn) zc = -zw;
      else if(zn<+zw && +zw<zf) zc = +zw;
      xc = (zc-z1p)*(cx/cz) + x1p;
      yc = (zc-z1p)*(cy/cz) + y1p;
      if(xc < -xw || xc > +xw || yc < -yw || yc > +yw || zc == 0.0) ic = 0;
      else rc = (xc-xn)*(xc-xn) + (yc-yn)*(yc-yn) + (zc-zn)*(zc-zn);
   /* Choose solution */
      if(ia == 0 && ib == 0 && ic == 0) return(-2); in = 0;
      if(ia == 1 && ib == 0 && ic == 0) in = 1;
      if(ia == 0 && ib == 1 && ic == 0) in = 2;
      if(ia == 0 && ib == 0 && ic == 1) in = 3;
      if(ia == 1 && ib == 1 && ic == 0)
      { if(ra > 0.0) in = 1; if(rb > 0.0 && rb < ra) in=2; }
      if(ia == 1 && ib == 0 && ic == 1)
      { if(ra > 0.0) in = 1; if(rc > 0.0 && rc < ra) in=3; }
      if(ia == 0 && ib == 1 && ic == 1)
      { if(rb > 0.0) in = 2; if(rc > 0.0 && rc < rb) in=3; }
      if(in == 0) { fprintf(stderr,"uti_int: weirdo box \n"); return(-2); }
      if(in == 1) { xi = xa; yi = ya; zi = za; }
      if(in == 2) { xi = xb; yi = yb; zi = zb; }
      if(in == 3) { xi = xc; yi = yc; zi = zc; }
   /* Determine return code */
      xip = xi; yip = yi; zip = zi;
      uti_rot(-1,xip,yip,zip,x0,y0,z0,ea,eb,ec,&xi,&yi,&zi);
      *xo = xi; *yo = yi; *zo = zi;
      if(i1 < 1 && i2 == 1) return(-1);                              /* exit */
      if(i2 < 1 && i1 == 1) return(+1);                             /* enter */
      return(+2);                                                   /* cross */
   }

/* Cylinder */
   if(strcmp(objshp[io],"cyl") == 0)
   {
   /* Initialize object size, check if points outside, pick preference */
      iw = 0.5*objdim[io][4];
      ow = 0.5*objdim[io][5];
      zw = 0.5*objdim[io][6];
      if((x1p<-ow && x2p<-ow)||(x1p>+ow && x2p>+ow)) return(0);  /* xy check */
      if((y1p<-ow && y2p<-ow)||(y1p>+ow && y2p>+ow)) return(0);
      if((z1p<-zw && z2p<-zw)||(z1p>+zw && z2p>+zw)) return(0);
      r1p = sqrt(x1p*x1p + y1p*y1p); t1p = atan2(y1p,x1p);      /* rad check */
      r2p = sqrt(x2p*x2p + y2p*y2p); t2p = atan2(y2p,x2p);
      if(iw > 0.0 && r1p < iw && r2p < iw) return(0);
      tdp = fabs(t1p-t2p); if(tdp > pi) tdp = 2.0*pi - tdp;  /* impact check */
      rbp = (r1p*r2p/dd)*sin(tdp);
      if(rbp > ow) return(0);
      if(rbp > iw && i1 < 1 && i2 < 1) return(0);
      if(is == 1) { xn=x1p; yn=y1p; zn=z1p; xf=x2p; yf=y2p; zf=z2p; } /* prf */
      else        { xn=x2p; yn=y2p; zn=z2p; xf=x1p; yf=y1p; zf=z1p; }
      rn = sqrt(xn*xn + yn*yn); ti = 1;
      rf = sqrt(xf*xf + yf*yf); to = 1;
      if     (rn > ow) ti = 0;                           /* filter solutions */
      else if(rn < iw) to = 0;
      else if(rf < ow) to = 0;
      if(iw <= 0.0) ti = 0;
   /* Solution 1 (xy-border) */
      ia = 0; if(cx != 0.0)
      {
         ra = dd; mx = cy/cx; bx = yn - mx*xn; be = (2.0*mx*bx)/(mx*mx+1.0);
         for(i=1;i<=4;i++)
         {
            ga = 2.0*be*be;
            if(ti == 1 && i <= 2) ga = 4.0*(bx*bx - iw*iw)/(mx*mx+1.0);
            if(to == 1 && i >= 3) ga = 4.0*(bx*bx - ow*ow)/(mx*mx+1.0);
            if(ga <= be*be)
            {
               if(i == 1 || i == 3) xt = -0.5*be + 0.5*sqrt(be*be-ga);
               if(i == 2 || i == 4) xt = -0.5*be - 0.5*sqrt(be*be-ga);
               yt = (xt-xn)*(cy/cx) + yn;
               zt = (xt-xn)*(cz/cx) + zn;
               rt = (xt-xn)*(xt-xn) + (yt-yn)*(yt-yn) + (zt-zn)*(zt-zn);
               if(   ((xn <= xt && xt <= xf) || (xn >= xt && xt >= xf))
                  && ((yn <= yt && yt <= yf) || (yn >= yt && yt >= yf))
                  && ((zn <= zt && zt <= zf) || (zn >= zt && zt >= zf))
                  && zt > -zw && zt < +zw && rt < ra )
               { ia = 1; ra = rt; xa = xt; ya = yt; za = zt; }
            }
         }
      }
   /* Solution 2 (xy-border) */
      ib = 0; if(cx == 0.0)
      {
         rb = dd; my = cx/cy; by = xn - my*yn; be = (2.0*my*by)/(my*my+1.0);
         for(i=1;i<=4;i++)
         {
            ga = 2.0*be*be;
            if(ti == 1 && i <= 2) ga = 4.0*(by*by - iw*iw)/(my*my+1.0);
            if(to == 1 && i >= 3) ga = 4.0*(by*by - ow*ow)/(my*my+1.0);
            if(ga <= be*be)
            {
               if(i == 1 || i == 3) yt = -0.5*be + 0.5*sqrt(be*be-ga);
               if(i == 2 || i == 4) yt = -0.5*be - 0.5*sqrt(be*be-ga);
               xt = (yt-yn)*(cx/cy) + xn;
               zt = (yt-yn)*(cz/cy) + zn;
               rt = (xt-xn)*(xt-xn) + (yt-yn)*(yt-yn) + (zt-zn)*(zt-zn);
               if(   ((xn <= xt && xt <= xf) || (xn >= xt && xt >= xf))
                  && ((yn <= yt && yt <= yf) || (yn >= yt && yt >= yf))
                  && ((zn <= zt && zt <= zf) || (zn >= zt && zt >= zf))
                  && zt > -zw && zt < +zw && rt < rb )
               { ib = 1; rb = rt; xb = xt; yb = yt; zb = zt; }
            }
         }
      }
   /* Solution 3 (z-border) */
      ic = 1; zc = 0.0;
      if     (zn<-zw && -zw<zf) zc = -zw;
      else if(zf<+zw && +zw<zn) zc = +zw;
      else if(zf<-zw && -zw<zn) zc = -zw;
      else if(zn<+zw && +zw<zf) zc = +zw;
      xc = (zc-zn)*(cx/cz) + xn;
      yc = (zc-zn)*(cy/cz) + yn;
      dt = sqrt(xc*xc + yc*yc);
      if(dt < +iw || dt > +ow || zc == 0.0) ic = 0;
      else rc = (xc-xn)*(xc-xn) + (yc-yn)*(yc-yn) + (zc-zn)*(zc-zn);
   /* Choose solution */
      if(ia == 0 && ib == 0 && ic == 0) return(-2); in = 0;
      if(ia == 1 && ib == 0 && ic == 0) in = 1;
      if(ia == 0 && ib == 1 && ic == 0) in = 2;
      if(ia == 0 && ib == 0 && ic == 1) in = 3;
      if(ia == 1 && ib == 1 && ic == 0)
      { if(ra > 0.0) in = 1; if(rb > 0.0 && rb < ra) in=2; }
      if(ia == 1 && ib == 0 && ic == 1)
      { if(ra > 0.0) in = 1; if(rc > 0.0 && rc < ra) in=3; }
      if(ia == 0 && ib == 1 && ic == 1)
      { if(rb > 0.0) in = 2; if(rc > 0.0 && rc < rb) in=3; }
      if(in == 0) { fprintf(stderr,"uti_int: weirdo cyl \n"); return(-2); }
      if(in == 1) { xi = xa; yi = ya; zi = za; }
      if(in == 2) { xi = xb; yi = yb; zi = zb; }
      if(in == 3) { xi = xc; yi = yc; zi = zc; }
   /* Determine return code */
      xip = xi; yip = yi; zip = zi;
      uti_rot(-1,xip,yip,zip,x0,y0,z0,ea,eb,ec,&xi,&yi,&zi);
      *xo = xi; *yo = yi; *zo = zi;
      if(i1 <  1 && i2 == 1) return(-1);                             /* exit */
      if(i2 <  1 && i1 == 1) return(+1);                            /* enter */
      if(i1 == 1 && i2 == 1) return(+2);                            /* cross */
      if(i1 <  1 && i2 <  1 && (in==1 || in==2)) return(-1);  /* exit inside */
      return(-2);
   }
   return(0);
}

/*
   Utility: uti_loc

      Determines the location of a point relative to an object, with the
   following return codes:

         ir = +1  -->  outside (default for undefined objects)
         ir = +0  -->  on object edge (within tolerance mintol)
         ir = -1  -->  inside
*/
int uti_loc(io,xi,yi,zi)
int io;
double xi,yi,zi;
{
   double et,x0,y0,z0,iw,ow,xw,yw,zw,ea,eb,ec,xo,yo,zo,ro;
   int ir=0,ix=0,iy=0,iz=0;
   et = mintol;
   x0 = objdim[io][1]; ea = objdim[io][7];
   y0 = objdim[io][2]; eb = objdim[io][8];
   z0 = objdim[io][3]; ec = objdim[io][9];
   if(strcmp(objshp[io],"box") == 0)
   {
      xw = 0.5*objdim[io][4];
      yw = 0.5*objdim[io][5];
      zw = 0.5*objdim[io][6];
      uti_rot(+1,xi,yi,zi,x0,y0,z0,ea,eb,ec,&xo,&yo,&zo);
      if(xo < -(xw+et) || xo > +(xw+et)) ix = +1;
      if(yo < -(yw+et) || yo > +(yw+et)) iy = +1;
      if(zo < -(zw+et) || zo > +(zw+et)) iz = +1;
      if(xo > -(xw-et) && xo < +(xw-et)) ix = -1;
      if(yo > -(yw-et) && yo < +(yw-et)) iy = -1;
      if(zo > -(zw-et) && zo < +(zw-et)) iz = -1;
      if       (ix == +1 || iy == +1 || iz == +1) ir = +1;
      else { if(ix == -1 && iy == -1 && iz == -1) ir = -1; }
      return(ir);
   }
   if(strcmp(objshp[io],"cyl") == 0)
   {
      iw = 0.5*objdim[io][4];
      ow = 0.5*objdim[io][5];
      zw = 0.5*objdim[io][6];
      uti_rot(+1,xi,yi,zi,x0,y0,z0,ea,eb,ec,&xo,&yo,&zo);
      ro = sqrt(xo*xo + yo*yo);
      if(ro < +(iw-et) || ro > +(ow+et)) ix = +1;
      if(zo < -(zw+et) || zo > +(zw+et)) iz = +1;
      if(ro > +(iw+et) && ro < +(ow-et)) ix = -1;
      if(zo > -(zw-et) && zo < +(zw-et)) iz = -1;
      if       (ix == +1 || iz == +1) ir = +1;
      else { if(ix == -1 && iz == -1) ir = -1; }

      if(ir < 1 && trctag == 10 && io == 1) fprintf(stderr,"loc: in 1 \n");
      return(ir);
   }
   return(+1);
}

/*
   Utility: uti_rnu

      Returns a random number with uniform distribution between (0,1).
*/
double uti_rnu()
{
   return(drand48());
}

/*
   Utility: uti_rnn

      Returns a random number having a normal distribution, with mean ravg and
   standard deviation rsig, using the Box-Muller method [see Numerical Recipes,
   (7.2.10)].
*/
double uti_rnn(ravg,rsig)
double ravg,rsig;
{
   double r1,r2,rn;
   choose:
   r1 = uti_rnu(); r2 = uti_rnu(); if(r2 <= 0.0) goto choose;
   rn = sin(2.0*pi*r1)*sqrt(-2.0*log(r2));
   rn = ravg + rsig*rn;
   return(rn);
}

/*
   Utility: uti_rot

      Performs a translation and rotation of vector (xi,yi,zi), in reference
   frame K, to (xo,yo,zo), in a reference frame K' with origin (x0,y0,z0) and 
   Euler angles (ea,eb,ec) [see Arfken, Math. Meth. for Phys., (4.83-87)].
   The flag (ir=+1) performs (K --> K'), while (ir=-1) performs (K' --> K).
*/
void uti_rot(ir,xi,yi,zi,x0,y0,z0,ea,eb,ec,xo,yo,zo)
int ir;
double xi,yi,zi,x0,y0,z0,ea,eb,ec,*xo,*yo,*zo;
{
   double r11,r12,r13,r21,r22,r23,r31,r32,r33,xt,yt,zt;
   ea = ea*pi/180.0; eb = eb*pi/180.0; ec = ec*pi/180.0;
   r11 = +cos(ec)*cos(eb)*cos(ea) - sin(ec)*sin(ea);
   r12 = +cos(ec)*cos(eb)*sin(ea) + sin(ec)*cos(ea);
   r13 = -cos(ec)*sin(eb);
   r21 = -sin(ec)*cos(eb)*cos(ea) - cos(ec)*sin(ea);
   r22 = -sin(ec)*cos(eb)*sin(ea) + cos(ec)*cos(ea);
   r23 = +sin(ec)*sin(eb);
   r31 = +sin(eb)*cos(ea);
   r32 = +sin(eb)*sin(ea);
   r33 = +cos(eb);
   if(ir == 1)
   {
      xt = xi - x0; yt = yi - y0; zt = zi - z0;
      *xo = r11*xt + r12*yt + r13*zt;
      *yo = r21*xt + r22*yt + r23*zt;
      *zo = r31*xt + r32*yt + r33*zt;
   }
   else
   {
      xt = xi; yt = yi; zt = zi;
      *xo = r11*xt + r21*yt + r31*zt + x0;
      *yo = r12*xt + r22*yt + r32*zt + y0;
      *zo = r13*xt + r23*yt + r33*zt + z0;
   }
}

double uti_atn(y,x)
double y,x;
{
   double atn;
   if(x == 0.0) { atn = -0.5*pi; if(y > 0) atn = +0.5*pi; }
   else
   {
      atn = atan(y/x); if(x < 0.0)
      {
         if(y > 0) atn = atn + pi;
         if(y < 0) atn = atn - pi;
      }
   }
   return(atn);
}