module field_errors_mod
  USE bmad !, except_dummy =>em_field_custom

  IMPLICIT NONE

contains

SUBROUTINE field_errors(ele,param,s_rel,orb, field_error)

  type(ele_struct), target :: ele
  type(lat_param_struct) param
  type(coord_struct) orb
  type(em_field_struct)  field_error, field
  real(rp), intent(in) :: s_rel
  real(rp) phi, phase, br
  integer i
  integer n
  integer nterms/2/
  real(rp) A(0:2)/68.9569,21.6858,30.4212/, B(0:2)/0,-28.6175,26.6923/
  real(rp) d/68.9569/
  real(rp) r0,psi,r
  real(rp) z
  real(rp) Bfield(3)
  logical twoD/.false./
  character *120 azimuthal_exp_z/'BzFourier20170628_LogID983.dat'/, azimuthal_exp_r/'BrFourier2016.dat'/

!  Br= A(0)r0/r + sum  [A(n)*cos(n phi)+B(n)*sin(n phi)](r0/r)**(n+1)
!  psi= A(0)r0ln(r) - 1/n sum  [A(n)*cos(n phi)+B(n)*sin(n phi)] r0**(n+1)/r**n

  psi=0
  field_error%B=0
  Bfield = 0
  phi = (ele%s_start+s_rel)/param%total_length * twopi
  r0 = ele%value(rho$)
  r = ele%value(rho$) + orb%vec(1)
  z = orb%vec(3)

  if(twoD)then

   psi = A(0)*r0 *log(r)
   Bfield(1) = A(0)*r0/r
   do n=1,nterms
     psi = psi - (1/n)*(A(n)*cos(n*phi)+B(n)*sin(n*phi))*(r0**(n+1)/r**n)
     Bfield(1) = Bfield(1) +   (A(n)*cos(n*phi)+B(n)*sin(n*phi))*(r0/r)**(n+1)  !1/r d psi/d r
     Bfield(3) = Bfield(3) +  (A(n)*sin(n*phi)-B(n)*cos(n*phi))*(r0/r)**(n+1)   !1/r d psi/d phi
   end do
   
   field_error%B(1:3) = Bfield(1:3)*1.45e-6

 endif
 
 call generalize_cyl_exp(azimuthal_exp_z, azimuthal_exp_r, r,phi,z, field)
 field_error%B(1:3) = field%B(1:3)*1.45*1.e-6
 field_error%E=0

!  write(97,'(a,2es12.4,5es12.4)')ele%name, ele%s_start, s_rel,phi,r,field_error%B
  return
 end subroutine
end module