/*
   Subroutine: abc_mag.c

      This subroutine swims particles through the magnetic field of given
   object (i1).  Currently, only a dipole field type is defined (constant,
   uniform field along z-axis of object).  Initial and final step points
   are transformed to object coordinates, where perpendicular and parallel
   momentum components to the field become separated.  The radius and angle
   of curvature in the field are then calculated, and the final point position
   and momentum along the curve determined, with a final rotation back to lab
   frame values.
*/

#include "abc.h"

void abc_mag(int it,int i1,int i2)
{
   int i,ip,is;
   char bt;
   double bm,mp,qp,rm,pm,rc,rd,ac,a1,a2;
   double r1[4],r2[4],p1[4],p2[4];
   double xw,yw,zw,x0,y0,z0,ea,eb,ec;
   double r1x,r1y,r1z,p1x,p1y,p1z;
   double r2x,r2y,r2z,p2x,p2y,p2z;

/* Fill particle data and point arrays */
   bm = objbmg[i1]; if(bm == 0.0) return;
   bt = objbty[i1]; if(bt == '0') return; if(bt != 'd')
   {
      fprintf(stderr,"abc_mag: undefined field type \n"); return;
   }
   ip = trkprt[it]; mp = prtmas[ip]; qp = prtchr[ip];
   is = trkpnt[it] - 2;
   r1[0] = trktpo[it][is]; p1[0] = trkemo[it][is];
   r1[1] = trkxpo[it][is]; p1[1] = trkxmo[it][is];
   r1[2] = trkypo[it][is]; p1[2] = trkymo[it][is];
   r1[3] = trkzpo[it][is]; p1[3] = trkzmo[it][is];
   is = trkpnt[it] - 1;
   r2[0] = trktpo[it][is]; p2[0] = trkemo[it][is];
   r2[1] = trkxpo[it][is]; p2[1] = trkxmo[it][is];
   r2[2] = trkypo[it][is]; p2[2] = trkymo[it][is];
   r2[3] = trkzpo[it][is]; p2[3] = trkzmo[it][is];

/* Dipole field */
   if(bt == 'd')
   {
   /* Rotate to field frame */
      x0 = objdim[i1][1]; xw = 0.5*objdim[i1][4]; ea = objdim[i1][7];
      y0 = objdim[i1][2]; yw = 0.5*objdim[i1][5]; eb = objdim[i1][8];
      z0 = objdim[i1][3]; zw = 0.5*objdim[i1][6]; ec = objdim[i1][9];
      uti_rot(+1,r1[1],r1[2],r1[3],x0,y0,z0,ea,eb,ec,&r1x,&r1y,&r1z);
      uti_rot(+1,r2[1],r2[2],r2[3],x0,y0,z0,ea,eb,ec,&r2x,&r2y,&r2z);
      uti_rot(+1,p1[1],p1[2],p1[3],0.,0.,0.,ea,eb,ec,&p1x,&p1y,&p1z);
      uti_rot(+1,p2[1],p2[2],p2[3],0.,0.,0.,ea,eb,ec,&p2x,&p2y,&p2z);
   /* Radius,angle of curvature */
      rm = sqrt((r2x-r1x)*(r2x-r1x) + (r2y-r1y)*(r2y-r1y));    /* arc length */
      pm = sqrt(p2x*p2x + p2y*p2y);                        /* trans momentum */
      rc = pm/(0.3*qp*bm);                                /* radius of curve */
      ac = rm/rc;                                          /* angle of curve */
   /* Calculate new momentum */
      a1 = atan2(p2y,p2x);                                 /* starting angle */
      a2 = a1 - ac;                                           /* final angle */
      p2x = pm*cos(a2);
      p2y = pm*sin(a2);
   /* Calculate new position */
      rd = 2.0*rc*sin(0.5*ac);                               /* chord length */
      a2 = a1 - 0.5*ac;                                       /* chord angle */
      r2x = r1x + rd*cos(a2);
      r2y = r1y + rd*sin(a2);
   /* Rotate back to lab frame */
      uti_rot(-1,r2x,r2y,r2z,x0,y0,z0,ea,eb,ec,&r2[1],&r2[2],&r2[3]);
      uti_rot(-1,p2x,p2y,p2z,0.,0.,0.,ea,eb,ec,&p2[1],&p2[2],&p2[3]);
   }

/* Adjust final point position,momentum */
   is = trkpnt[it] - 1;
   if(i1 == i2)
   {
      trkxpo[it][is] = r2[1];
      trkypo[it][is] = r2[2];
      trkzpo[it][is] = r2[3];
   }
   trkxmo[it][is] = p2[1];
   trkymo[it][is] = p2[2];
   trkzmo[it][is] = p2[3];
}