interp_FOfields_aorsa Subroutine

subroutine interp_FOfields_aorsa(prtcls, F, params)
  TYPE(KORC_PARAMS), INTENT(IN)                              :: params
  TYPE(PARTICLES), INTENT(INOUT) :: prtcls
  TYPE(FIELDS), INTENT(IN)       :: F
  REAL(rp),DIMENSION(1)   :: Y_R,Y_PHI,Y_Z
  REAL(rp),DIMENSION(1)   :: B0_R,B0_PHI,B0_Z
  REAL(rp),DIMENSION(1)   :: B0_X,B0_Y
  REAL(rp),DIMENSION(1)   :: B1_X,B1_Y,B1_Z
  REAL(rp),DIMENSION(1)   :: B1Re_X,B1Re_Y,B1Re_Z
  REAL(rp),DIMENSION(1)   :: B1Im_X,B1Im_Y,B1Im_Z
  REAL(rp),DIMENSION(1)   :: E1Re_X,E1Re_Y,E1Re_Z
  REAL(rp),DIMENSION(1)   :: E1Im_X,E1Im_Y,E1Im_Z
  REAL(rp),DIMENSION(1)   :: cP,sP,cnP,snP
  REAL(rp), DIMENSION(1,3)  :: A
  !  INTEGER(ip) :: ezerr
  INTEGER                                      :: pp,ss
  !! Particle chunk iterator.
  REAL(rp) :: psip_conv
  REAL(rp) :: amp,nmode

  if (size(prtcls%Y,1).eq.1) then
     ss = size(prtcls%Y,1)
  else
     if (prtcls%Y(2,1).eq.0) then
        ss=1_idef
     else
        ss = size(prtcls%Y,1)
     end if
  endif

  psip_conv=F%psip_conv
  amp=F%AMP
  nmode=F%AORSA_nmode


  !$OMP PARALLEL DO &
  !$OMP& PRIVATE(pp,ezerr,A,B1Re_X,B1Re_Y,B1Re_Z,B1Im_X,B1Im_Y,B1Im_Z, &
  !$OMP& E1Re_X,E1Re_Y,E1Re_Z,E1Im_X,E1Im_Y,E1Im_Z, &
  !$OMP& B0_R,B0_PHI,B0_X,B0_Y,B0_Z,B1_X,B1_Y,B1_Z,cP,sP,cnP,snP,Y_R,Y_Z,Y_PHI)&
  !$OMP& FIRSTPRIVATE(psip_conv,amp,nmode) &
  !$OMP& SHARED(prtcls,F,params)
  do pp = 1,ss

     Y_R(1)=prtcls%Y(pp,1)
     Y_PHI(1)=prtcls%Y(pp,2)
     Y_Z(1)=prtcls%Y(pp,3)
     
     call EZspline_interp(bfield_2d%A,&
          b1Refield_2dx%X,b1Refield_2dx%Y,b1Refield_2dx%Z, &
          b1Imfield_2dx%X,b1Imfield_2dx%Y,b1Imfield_2dx%Z, &
          e1Refield_2dx%X,e1Refield_2dx%Y,e1Refield_2dx%Z, &
          e1Imfield_2dx%X,e1Imfield_2dx%Y,e1Imfield_2dx%Z, &
          1,Y_R,Y_Z,A,B1Re_X,B1Re_Y,B1Re_Z,B1Im_X,B1Im_Y,B1Im_Z, &
          E1Re_X,E1Re_Y,E1Re_Z,E1Im_X,E1Im_Y,E1Im_Z,ezerr)
     call EZspline_error(ezerr)
     
     prtcls%PSI_P=A(1,1)

     B0_R = psip_conv*A(1,3)/prtcls%Y(pp,1)
     B0_PHI = -F%Bo*F%Ro/prtcls%Y(pp,1)
     B0_Z = -psip_conv*A(1,2)/prtcls%Y(pp,1)

     cnP=cos(nmode*Y_PHI)
     snP=sin(nmode*Y_PHI)

     cP=cos(Y_PHI)
     sP=sin(Y_PHI)
     
     B1_X = amp*(B1Re_X*cnP-B1Im_X*snP)
     B1_Y = amp*(B1Re_Y*cnP-B1Im_Y*snP)
     B1_Z = amp*(B1Re_Z*cnP-B1Im_Z*snP)

     prtcls%E(pp,1) = amp*(E1Re_X(1)*cnP(1)-E1Im_X(1)*snP(1))
     prtcls%E(pp,2) = amp*(E1Re_Y(1)*cnP(1)-E1Im_Y(1)*snP(1))
     prtcls%E(pp,3) = amp*(E1Re_Z(1)*cnP(1)-E1Im_Z(1)*snP(1))

     B0_X = B0_R*cP - B0_PHI*sP
     B0_Y = B0_R*sP + B0_PHI*cP
     prtcls%B(pp,3) = B0_Z(1)+B1_Z(1)
     
     prtcls%B(pp,1) = B0_X(1)+B1_X(1)
     prtcls%B(pp,2) = B0_Y(1)+B1_Y(1)

     !write(6,*) '(R,PHI,Z)',Y_R*params%cpp%length,Y_PHI, &
     !     Y_Z*params%cpp%length
     !write(6,*) 'amp',amp,'cP,sP',cP,sP,'cnP,snP',cnP,snP
     !write(6,*) 'psi',PSIp*params%cpp%Bo*params%cpp%length**2
     !write(6,*) 'dpsidR',A(:,2)*params%cpp%Bo*params%cpp%length
     !write(6,*) 'dpsidZ',A(:,3)*params%cpp%Bo*params%cpp%length
     !write(6,*) 'B0',B0_R*params%cpp%Bo,B0_PHI*params%cpp%Bo,B0_Z*params%cpp%Bo
     !write(6,*) 'AMP',amp
     !write(6,*) 'B1Re',B1Re_X*params%cpp%Bo,B1Re_Y*params%cpp%Bo,B1Re_Z*params%cpp%Bo
     !write(6,*) 'B1Im',B1Im_X*params%cpp%Bo,B1Im_Y*params%cpp%Bo,B1Im_Z*params%cpp%Bo
     !write(6,*) 'B1',B1_X*params%cpp%Bo,B1_Y*params%cpp%Bo,B1_Z*params%cpp%Bo
     !write(6,*) 'B',B_X*params%cpp%Bo,B_Y*params%cpp%Bo,B_Z*params%cpp%Bo
     
  end do
  !$OMP END PARALLEL DO


  
end subroutine interp_FOfields_aorsa