subroutine tester
 use bmad
 implicit none
 real(rp) x(3), B(3),z, b1(3,3),b2(3,3),y
 real(rp) xdum 
 character*120 file, file_inf
 integer iz, iy,ix
   real(rp) xoffset/0.085/, yoffset/0.015/ 
 logical out_of_range

! file = "/Users/dlr10/lepp/g-2/magneticfield/injec_fld.dat"
! file_inf = "/Users/dlr10/lepp/g-2/magneticfield/inf_field_alone.dat"
 file = "/home/dlr/development9_linux/g2_magneticfield/injec_fld.dat"
 file_inf = "/home/dlr/development9_linux/g2_magneticfield/inf_field_alone.dat"



!write(11,'(/,a,i4,/)')'#' , iy
!  call random_number(xdum)
! turns out that range in x where fields make sense is 7.5 to 9.5
! and range in y is 0.25 to 2.75
do  iy=-10,10 
write(11,'(/,a,i4,/)')'#' , ix
do iz = 1, 400000
!print *
  z = iz*0.1 + 0.
  x(1:3)=0
  x(1) = 8.5 ! ix*0.5 +xoffset*100
  x(2) = 1.5
  x(3)=z 

!  print *,file, x,B

  call get_g2_fields(file,file_inf, x, B, b1,b2, out_of_range)
  if(out_of_range)then
!   print *,' Outside of field volume '
   cycle
  else
   write(11, '(3es12.4,6x,3es12.4)')x,B
!   print '(3es12.4,6x,3es12.4)',x,B
  endif
 end do
end do
 return
 end subroutine

  subroutine em_field_custom(ele, param, s_rel,t_rel, orb, local_ref,field,calcd, err_flag)

use bmad_struct
use bmad_interface, except_dummy => em_field_custom

   implicit none
   type(ele_struct) :: ele
   type(lat_param_struct) param
   type(coord_struct) orb
   type(em_field_struct) field
   real(rp), intent(in) :: s_rel, t_rel
   real(rp) xx(3), BB(3),zz, b1(3,3),b2(3,3)
   real(rp) xoffset/0.085/, yoffset/0.015/ !   real(rp) xoffset/0.05/, yoffset/0.0125/
   logical  out_of_range,local_ref
   logical, optional :: calcd, err_flag
   character(32) :: r_name = 'em_field_custom'
   character*120 file_inj, file_inf
!   file_inj = "/Users/dlr10/lepp/g-2/magneticfield/injec_fld.dat"
!   file_inf = "/Users/dlr10/lepp/g-2/magneticfield/inf_field_alone.dat"
 file_inj = "/home/dlr/development9_linux/g2_magneticfield/injec_fld.dat"
 file_inf = "/home/dlr/development9_linux/g2_magneticfield/inf_field_alone.dat"

!   if(present(err_flag))err_flag = .false.
!   if(present(calcd))calcd = .false.
 ! meters to cm
   xx(1) = (orb%vec(1)+xoffset)*100. 
   xx(2) = (orb%vec(3)+yoffset)*100. 
   xx(3) = s_rel*100.                
   call get_g2_fields(file_inj, file_inf,xx, BB,b1,b2,out_of_range)
   field%B(1:3) = BB(1:3) * 1.e-4  ! gauss to tesla
!  print '(2(a,3(es12.4)))',' xx ', xx, ' field%B', field%B
  write(12, '(2(a,3(ES14.6)))')' xx ', xx, ' field%B', field%B
!   if(out_of_range) err_flag = .true.
   return
   end subroutine
   
   
  subroutine interpolate_field(x,B,b1,b2)
   use bmad
   implicit none
   real(rp) x(3), B(3),z, b1(3,3),b2(3,3)
   real(rp) xgrid(3), dx(3)
   integer ind(3),i

   ind(1:3) = (x(1:3))*2+1
   xgrid(1:3) = (ind(1:3)-1)*0.5
   dx(1:3) = x(1:3) - xgrid(1:3)
!  print '(a,6es12.4)',' dx, B ', dx,B
   forall(i=1:3) B(i) = B(i) + dot_product(b1(i,:),dx(:))    
!  print '(a,3es12.4)', 'B(1:3) ', B(1:3)
   return
  end subroutine


! READ magnetic field file

 subroutine get_g2_fields(field_file_inj, field_file_inf, &
                                         x,B,b1,b2, out_of_range)
   use magfield
   use magfield_interface
   implicit none


 logical first/.true./
 logical out_of_range
 character(*) field_file_inj, field_file_inf

   type(magfield_struct), allocatable,save :: map_inj(:,:,:),map_inf(:,:,:)
   type(magfield_struct), allocatable,save :: map(:,:,:)
   type(magfield_struct) xmin_inj, xmin_inf, xmin

   real(rp) x(3), B(3) ,b1(3,3),b2(3,3)
   integer ind(3)

 if(first)then
   call read_field(field_file_inj,map_inj,xmin_inj)
   call read_field(field_file_inf,map_inf,xmin_inf)
   call sum_fields(map_inj,map_inf, xmin_inj, xmin_inf, map, xmin)
   call compute_derivatives(map)
   first = .false.
 endif

 ind(1:3) = (x(1:3))*2+1
 if(all(ind(1:3) > 0) .and. ind(1) .le. size(map,1) .and. ind(2) .le. size(map,2) .and. ind(3) .le. size(map,3))then
   B(1:3) = map(ind(1),ind(2),ind(3))%B(1:3)
   b1(1:3,1:3) = map(ind(1),ind(2),ind(3))%dB(1:3,1:3)
   b2(1:3,1:3) = map(ind(1),ind(2),ind(3))%d2B(1:3,1:3)
   out_of_range = .false.
!  print '(a,3i10, 6es12.4)',' Before interpolate ind(1:3), x(1:3) B(1:3)= ', ind(1:3),x(1:3), B(1:3)

  call interpolate_field(x,B,b1,b2)
!  print '(a,3i10, 6es12.4)','  After interpolate ind(1:3), x(1:3) B(1:3)= ', ind(1:3),x(1:3), B(1:3)
 else
   out_of_range  =.true.
 endif

return
end subroutine

subroutine read_field(field_file, map,xmin)

   use magfield
   use magfield_interface

   implicit none
   type(magfield_struct), allocatable :: position(:), map(:,:,:)
   type(magfield_struct) x0, xmax, xmin, dxyz(-1:1)
   integer iu, istat/0/, nlines, ind(3),i,j,k,l, ntot(3), ix,jx
   integer k1, k2
   integer delta
   character(*)  field_file
   character*140 string
   real(rp)  b1(3), b2(3)

   iu = lunget()
   open(unit=iu, file = field_file, status = 'old', action='read')
   nlines = 0
    xmin%x=1000.
   xmax%x=-1000.
   do while(.true.)
      string(1:140) = '                                                                                                                                                                       '
      read(iu, '(a140)', IOSTAT = istat ) string
      if(istat < 0) exit
     if(index(string,'#') /= 0)cycle
     if(string(1:10) == '          ')cycle
     if(index(field_file,'inj')/=0)read(string, *)x0%x(1:3), x0%B(1:3), x0%H
     if(index(field_file,'inj')==0)read(string, *)x0%x(1:3), x0%B(1:3)
     xmin%x(1:3) = min(xmin%x(1:3),x0%x(1:3))
     xmax%x(1:3) = max(xmax%x(1:3),x0%x(1:3))

     nlines = nlines + 1
   end do

!   print *, ' nlines = ', nlines
!   print *,' xmax ',xmax%x, 'xmin', xmin%x
   ntot(1:3) = (xmax%x(1:3)-xmin%x(1:3))*2 + 1
!   print *, ntot
   allocate(map(ntot(1),ntot(2),ntot(3)))
   rewind(unit=iu)

   do while(.true.)
      string(1:140) = '                                                                                                                                                                       '
     read(iu,'(a140)',IOSTAT = istat) string
     if(istat < 0)exit
     if(index(string,'#') /= 0)cycle
     if(string(1:10) == '          ')cycle
     if(index(field_file,'inj')/=0)read(string, *)x0%x(1:3), x0%B(1:3), x0%H
     if(index(field_file,'inj')==0)read(string, *)x0%x(1:3), x0%B(1:3)

    ind(1:3) = (x0%x(1:3) - xmin%x(1:3))*2 + 1
    map(ind(1),ind(2),ind(3))%B(1:3) = x0%B(1:3)
    map(ind(1),ind(2),ind(3))%x(1:3) = x0%x(1:3)
  end do
! correct the discontinuity at z=259.5. Set the field at this point to be average of 259 and 260

  k1 = 259.5*2+1
  k2 = 430.5*2+1
  do i = 1,ntot(1)
   do j = 1,ntot(2)
!    print *,'map(i,j,k)%x(3)',map(i,j,k1)%x(3), map(i,j,k2)%x(3)
    map(i,j,k1)%B(1:3) = (map(i,j,k1-1)%B(1:3) + map(i,j,k1+1)%B(1:3))*0.5
    map(i,j,k2)%B(1:3) = (map(i,j,k2-1)%B(1:3) + map(i,j,k2+1)%B(1:3))*0.5
   delta = 10
   do k = -delta,delta
    map(i,j,k1+k)%B(1) = (map(i,j,k1-delta)%B(1) + map(i,j,k1+delta)%B(1))*0.5
    map(i,j,k2+k)%B(1) = (map(i,j,k2-delta)%B(1) + map(i,j,k2+delta)%B(1))*0.5
   end do
   end do
 end do

return
end

subroutine compute_derivatives(map)

   use magfield
   use magfield_interface

   implicit none
   type(magfield_struct), allocatable :: map(:,:,:)
   type(magfield_struct) x0, xmax, xmin, dxyz(-1:1)
   integer i,j,k,l, ntot(3), ix,jx
   real(rp)  b1(3), b2(3)


! next compute dB(1:3)/dxyz(1:3)

!print *
ntot(1) = size(map,1)
ntot(2) = size(map,2)
ntot(3) = size(map,3)

print '(a,3i4)',' ntot ', ntot(1:3)
do j=1,ntot(1)
  do k=1,ntot(2)
    do l=1,ntot(3)
     do ix=1,3
      do jx=-1,1,1

        if(ix == 1 .and. j+jx >= 1 .and. j+jx <= ntot(ix))then
          dxyz(jx)%B(1:3) = map(j+jx,k,l)%B(1:3)
         elseif(ix == 2 .and. k+jx >= 1 .and. k+jx <= ntot(ix))then
          dxyz(jx)%B(1:3) = map(j,k+jx,l)%B(1:3)
         elseif(ix == 3 .and. l+jx >= 1 .and. l+jx <= ntot(ix))then
          dxyz(jx)%B(1:3) = map(j,k,l+jx)%B(1:3)
         else
          cycle
        endif
      end do


!      print '(a,i,3es12.4)','2  ix, dxyz', ix,  dxyz(-1:1)%B(2)
      call quadratic_fit(dxyz, b1,b2)
!      print '(a,3es12.4)','b1(1:3) ', b1
      map(j,k,l)%dB(1:3,ix) = b1(1:3) !first derivative at dB_1:3/dx_1:3
      map(j,k,l)%d2B(1:3,ix) = b2(1:3) !second derivative at d2B_1:3/d2x_1:3 /2
     end do
!      write(11,'(6F12.4)') map(j,k,l)%x(1:3),map(j,k,l)%B(1:3)
   end do
  end do
end do

return
end subroutine

   subroutine sum_fields(map_inj,map_inf, xmin_inj, xmin_inf,map, xmin)
    use magfield
    implicit none
    type(magfield_struct), allocatable :: map_inj(:,:,:), map_inf(:,:,:)
    type(magfield_struct), allocatable :: map(:,:,:)
    type(magfield_struct) x0, xmax, xmin, dxyz(-1:1), xmin_inj, xmin_inf, diffx
    integer ntot(3), ntot_inf(3), idif(3)
    integer i,j,k,l

    forall(i=1:3)ntot(i) = size(map_inj,i)
    forall(i=1:3)ntot_inf(i) = size(map_inf,i)

         idif(1:3) = 2*(xmin_inf%x(1:3) - xmin_inj%x(1:3))
    allocate(map(1:ntot(1),1:ntot(2),1:ntot(3)))
    map(:,:,:) = map_inj(:,:,:)
    xmin%x = xmin_inj%x

    do i=1+idif(1),ntot(1)
!     write(13,'(/,a,/)')'#'
     do j=1+idif(2),ntot(2)
!     write(13,'(/,a,/)')'#'
      do k=1+idif(3),ntot(3)

       map(i,j,k)%B(1:3) = map_inj(i,j,k)%B(1:3) +  map_inf(i-idif(1),j-idif(2),k-idif(3))%B(1:3)

    diffx%x(1:3) = map_inj(i,j,k)%x(1:3) - map_inf(i-idif(1),j-idif(2),k-idif(3))%x(1:3)
    if(any(abs(diffx%x(1:3)) > 0.00001))then
       
!       print '(4(3es12.4))', map(i,j,k)%B(2), map_inj(i,j,k)%B(2), map_inf(i-idif(1),j-idif(2),k-idif(3))%B(2), &
!              map_inf(i-idif(1),j-idif(2),k-idif(3))%x(1:3),map_inj(i,j,k)%x(1:3),diffx%x
       
       write(13,'(4(3es12.4))')map(i,j,k)%B(2), map_inj(i,j,k)%B(2), map_inf(i-idif(1),j-idif(2),k-idif(3))%B(2), &
              map_inf(i-idif(1),j-idif(2),k-idif(3))%x(1:3),map_inj(i,j,k)%x(1:3),diffx%x
    endif
      end do
     end do
    end do
!      do k=1,ntot(3)

!      end do
    return
   end

      subroutine quadratic_fit(dxyz,b1,b2)
       use magfield
       use nr
       implicit none
       real(rp) dx/0.5/
       type(magfield_struct) dxyz(-1:1)
       real(rp) b0(3), b1(3), b2(3)
!       real(rp)  A(3,3), V(3,3)
       integer ix, i 
!print '(3(4x,3e12.4))', dxyz(-1)%B,dxyz(0)%B,dxyz(1)%B
!pause
!         A(1,1:3)= 1.
!         A(2,1:3) = (/-0.5,0.,0.5/)
!         A(3,1:3) = (/0.25, 0., 0.25/)
!   print '(a,3es12.4)',' dxyz(-1:1)%B(1) - Bx ', dxyz(-1:1)%B(1)
!   print '(a,3es12.4)',' dxyz(-1:1)%B(2) - By ', dxyz(-1:1)%B(2)
!   print '(a,3es12.4)',' dxyz(-1:1)%B(3) - Bz ', dxyz(-1:1)%B(3)
!do i=1,3
!     print '(a,i3,3es12.4)',' A row = ', i, A(1:3,i)  
!end do
!     print '(/,a,3es12.4)',' V before ' , V(1,1:3)

!         do i=1,3
!          V(1,1:3) = dxyz(-1:1)%B(i)
!          call gaussj(A,V)
!          B1(i) = V(1,2)
!          B2(i) = V(1,3)
!         end do 
!     print '(/,a,3es12.4)',' V after ' , V(1,1:3)

         b0(1:3) = dxyz(0)%B(1:3)
         b1(1:3) = (dxyz(1)%B(1:3) - dxyz(-1)%B(1:3))/2/dx ! derivative of B_ix wr x,y,z
         b1(1:3) = (dxyz(1)%B(1:3) - dxyz(0)%B(1:3))/dx ! derivative of B_ix wr x,y,z
         b2(1:3) = (dxyz(1)%B(1:3) + dxyz(-1)%B(1:3) - 2*b0(1:3))/2/dx**2 ! derivative of B_ix wr x,y,z


!          print '(a,i3,6e12.4)',' ix, b0,b1,b2 ',ix,b0(1:3), b1(1:3)
      return
      end subroutine quadratic_fit