! Routine to find the inital x,xp at start of injection line that minimize angle and offset at exit of inflecto
subroutine optimize_incident(start_orb,lat, inflector_end_target, opt_orb, inflector_angle)
 use bmad
 use parameters_bmad
 implicit none
 type (lat_struct) lat
 type (coord_struct) start_orb, opt_orb, end_orb
 type (coord_struct), allocatable :: orb(:)
  type (initial_offsets_struct) inflector_end_target
 integer track_state
 integer n, iteration
 integer i
 integer lun
 logical err_flag, err
 real(rp), dimension(2,2) :: M, dydx
 real(rp) dx(2)/1.0_rp,0.0_rp/, dpx(2)/0.0_rp,1.0_rp/, y0(2), y1(2), x0(2), x1(2)
 real(rp) Delta(2)/0.0_rp,0.0_rp/
 real(rp) d0/0.000001_rp/
 real(rp) det
 real(rp) tol/1.e-8/
 real(rp) inflector_angle

 lun=lunget()
 open(unit=lun,file=trim(directory)//'/optimize_incident_out.dat')

 if(inflector_end_target%pxmean >= 10.)then    !fit to ibms data
    call fit_to_ibms(start_orb, lat, inflector_end_target, opt_orb, inflector_angle)
    
 else  !fit to target values at end of inflector
    call reallocate_coord (orb, lat%branch(0)%n_ele_max)
    
 end_orb%vec=0.
 end_orb%vec(1) = inflector_width - 0.009  + inflector_end_target%x_mean
 end_orb%vec(2) = inflector_end_target%pxmean

! n = lat%branch(0)%n_ele_track-3 !Mark inflector ds
 n=-1
 do i=1,lat%branch(0)%n_ele_track
  if (index(lat%branch(0)%ele(i)%name, 'MARK_INFLECTOR_DS')/=0)n=i
 end do
 if(n>0)then
    print *,' element at end of injection line = ', lat%branch(0)%ele(n)%name
    write(lun, *)' element at end of injection line = ', lat%branch(0)%ele(n)%name
 else
  print *,' Optimize incident: end element not found '
 endif

  orb(0) = start_orb
  lat%ele(1:lat%n_ele_track)%alias= 'Record'  !write magnetic field and position along trajectory to unit 12 in em_field_custom_inj.f90

! compute jacobian
  x0(1:2) = start_orb%vec(1:2)  !input 2-vector
  call track_all(lat,orb, ix_branch = 0, track_state = track_state, err_flag = err_flag)  !compute trajectory through injection line
    print '(a,1x,l,1x,a,1x,i10)',' Optimize_Incident: err_flag = ', err_flag, ' track_state = ',track_state
  y0(1:2) = orb(n)%vec(1:2)  !output 2-vector
  iteration = 1
  print '(a10,1x, 6a12)', 'iteration','x_init', 'xp_init','x_final','xp_final'
  print '(i10,2(6es12.4))',iteration,x0,y0
  write(lun,'(a10,1x, 6a12)')'iteration','x_init', 'xp_init','x_final','xp_final'
  write(lun, '(i10,2(6es12.4))') iteration,x0,y0

  print '(a, 2es12.4)',' end_orb%vec(1:2) = ', end_orb%vec(1:2)
  write(lun, '(a, 2es12.4)')' end_orb%vec(1:2) = ', end_orb%vec(1:2)
  write(lun, '(a10,6a12)')'iteration','x_init','xp_init','x_final','xp_final', 'Delta x','Delta xp'
do while(abs(y0(1)-end_orb%vec(1))> tol .or. abs(y0(2)-end_orb%vec(2)) > tol)
  orb(0)%vec(1:2) = x0(1:2) + dx(1:2)*d0 !change x component of input vector
  call track_all(lat,orb, ix_branch = 0, track_state = track_state, err_flag = err_flag)  !compute trajectory through injection line
  y1(1:2) = orb(n)%vec(1:2) !new output
  dydx(1:2,1) = (y1-y0)/d0  !derivative of output with respect to input where only x change in input
  orb(0)%vec(1:2) = x0(1:2) + dpx(1:2) * d0 !change xp component of input vector
  call track_all(lat,orb, ix_branch = 0, track_state = track_state, err_flag = err_flag)  !compute trajectory through injection line
  y1(1:2) = orb(n)%vec(1:2) !new output 
  dydx(1:2,2) = (y1-y0)/d0 !derivative of output with respect to input where only xp change in input
!  print '(2es12.4)',dydx(1,1),dydx(1,2)
!  print '(2es12.4)',dydx(2,1),dydx(2,2)

  det = dydx(1,1)*dydx(2,2)-dydx(1,2)*dydx(2,1) 
 if(det == 0.) then
    print '(a)',' optimize_incident: No inverse for transfer matrix'
    stop
endif
! M is inverse of Jacobian
  M(1,1) = dydx(2,2)/det
  M(2,2) = dydx(1,1)/det
  M(1,2) = -dydx(1,2)/det
  M(2,1) = -dydx(2,1)/det
 
  Delta = -matmul(M,(y0-end_orb%vec(1:2)))
  x0 = x0 + Delta
  iteration = iteration + 1
  orb(0)%vec(1:2) = x0(1:2)
  call track_all(lat,orb, ix_branch = 0, track_state = track_state, err_flag = err_flag)  !compute trajectory through injection line
  if(err_flag .or. track_state >= 0)print '(a)',' WARNING from OPTIMIZE_INCIDENT: Error in track_all '
  y0(1:2) = orb(n)%vec(1:2)

  print '(i10,2(1x,6es12.4),1x, 2es12.4)',iteration,x0,y0, Delta
  write(lun, '(i10,2(1x,6es12.4),1x, 2es12.4)')iteration,x0,y0, Delta
 
 end do
  opt_orb = start_orb
  opt_orb%vec(1:2) = x0(1:2)
 
  print '(i10,2(1x,6es12.4),1x, 2es12.4)',iteration,x0,y0, Delta
  write(lun, '(i10,2(1x,6es12.4),1x, 2es12.4)')iteration,x0,y0, Delta 
  lat%ele(1:lat%n_ele_track)%alias= 'No'

  write(lun,'(/,a17,es12.4,a4,a4,es12.4,a4,a4)')'initial_offsets =',x0(1),' 0.0',' 0.0',x0(2),' 0.0',' 0.0'
  write(lun,'(a21,es12.4,a4,a4,es12.4,a4,a4)')'initial_offsets_ref =',x0(1),'0.0','0.0',x0(2),'0.0','0.0'
endif  !fit to target values at end of inflector
close(unit=lun)
return

end subroutine

   subroutine fit_to_ibms(start_orb, lat, inflector_end_target, opt_orb, inflector_angle)
      use bmad
      use parameters_bmad
      use nr
      implicit none
      type (lat_struct) lat
      type (coord_struct) start_orb, opt_orb
      type (coord_struct) orb, temp_orb
      type (initial_offsets_struct) inflector_end_target
      real(rp) dx(2)/1.0_rp,0.0_rp/, dpx(2)/0.0_rp,1.0_rp/
      real(rp) d0/0.000001_rp/
      real(rp) db0/0.0001/
      real(rp) dfdx(3,3), V(3,1)
      real(rp) inflector_field_0, inflector_angle, inflector_field_ref
      real(rp) x_ibms_0(3), x_ibms_x(3), x_ibms_xp(3), x_ibms_db(3), x_ibms_target(3)
      real(rp) chis, c, dchis(1:2)
      real(rp) r
      real(rp) chis_latest
      integer i, iteration, n

      do i=1,lat%branch(0)%n_ele_track
         if (index(lat%branch(0)%ele(i)%name, 'IBMS')/=0)n=i
      end do
      if(n>0)then
         print *,' IBMS 2 = ', lat%branch(0)%ele(n)%name
      else
         print *,' Optimize incident: end element not found '
      endif
      x_ibms_target(1) = inflector_end_target%x_mean
      x_ibms_target(2) = inflector_end_target%y_mean
      x_ibms_target(3) = inflector_end_target%tmean
      iteration=0
      opt_orb = start_orb
      temp_orb = start_orb
      inflector_field_ref = inflector_field
      inflector_field_0 = inflector_field

      inflector_field = inflector_field_0
      do while(iteration < 5 )
         orb=opt_orb
         inflector_field_0 = inflector_field
         call set_inflector_params(lat, 0,inflector_angle)
         call evaluate_x_ibms(lat, orb, x_ibms_0)
         chis = sum((x_ibms_0(1:3)-x_ibms_target(1:3))**2)

         print '(a,i10, a,2es12.4,a,es12.4)', ' iteration =',iteration,' opt_orb%vec(1:2) =',opt_orb%vec(1:2),' inflector_field = ',inflector_field
         print '(a,es12.4,a,3es12.4,a,3es12.4)',' chis = ', chis, ' target = ', x_ibms_target(1:3), ' fit = ', x_ibms_0(1:3) 

         orb%vec(1:2) = opt_orb%vec(1:2)+dx(1:2)*d0
         call evaluate_x_ibms(lat, orb, x_ibms_x)
         dchis(1) = (sum((x_ibms_x(1:3)-x_ibms_target(1:3))**2)-chis)/d0
         orb%vec(1:2) = opt_orb%vec(1:2)+dpx(1:2)*d0
         call evaluate_x_ibms(lat, orb, x_ibms_xp)
         dchis(2) = (sum((x_ibms_xp(1:3)-x_ibms_target(1:3))**2)-chis)/d0
         orb%vec(1:2) = opt_orb%vec(1:2)
         inflector_field = inflector_field_0 + db0
         call set_inflector_params(lat, 0,inflector_angle)
         call evaluate_x_ibms(lat, orb, x_ibms_db)

         dfdx(1:3,1) = (x_ibms_0(1:3) -x_ibms_x(1:3))/d0
         dfdx(1:3,2) = (x_ibms_0(1:3) -x_ibms_xp(1:3))/d0
         dfdx(1:3,3) = (x_ibms_0(1:3) -x_ibms_db(1:3))/db0
         V(1:3,1) = x_ibms_target(1:3) -x_ibms_0(1:3)

         c = chis/sum(dchis(1:2)**2)
         V(1:2,1) = c*dchis(1:2)
         V(3,1)=0

!         call gaussj(dfdx,V)

         temp_orb%vec(1:2) = orb%vec(1:2) - V(1:2,1)
         inflector_field = inflector_field_0 - V(3,1)
         call evaluate_x_ibms(lat, temp_orb, x_ibms_0)
         chis_latest = sum((x_ibms_0(1:3)-x_ibms_target(1:3))**2)
         r=1
         do while(chis_latest > chis .and. 1./r > 0.02)
            r= 2*r
            temp_orb%vec(1:2) = orb%vec(1:2) - V(1:2,1)/r  
            call evaluate_x_ibms(lat, temp_orb, x_ibms_0)
            chis_latest = sum((x_ibms_0(1:3)-x_ibms_target(1:3))**2)
         end do
         if(chis_latest < chis)opt_orb = temp_orb
         if(chis_latest >= chis)exit
         
         iteration = iteration + 1
      end do
      
      call set_inflector_params(lat, 0,inflector_angle)
      call evaluate_x_ibms(lat, opt_orb, x_ibms_0)

      print '(a,i10, a,2es12.4,a,es12.4)', ' iteration =',iteration,' opt_orb%vec(1:2) =',opt_orb%vec(1:2),' inflector_field = ',inflector_field
      print '(a,3es12.4,a,3es12.4)',' target = ', x_ibms_target(1:3), ' fit = ', x_ibms_0(1:3)

      inflector_field = inflector_field_ref
      call set_inflector_params(lat, 0,inflector_angle)
      return
    end subroutine fit_to_ibms
    
      subroutine evaluate_x_ibms(lat, start_orb, x_ibms)
        use bmad
        implicit none
        type (lat_struct) lat
        type (coord_struct) start_orb, opt_orb
        type (coord_struct), allocatable :: from_orbit(:), to_orbit(:)
        real(rp) x_ibms(3)
        real(rp) ibms_vec(3,3), dl
        integer ie_from, i, n
        ! evaluate x_ibms(:)
!  !MacCoy
      ibms_vec(1,1)=-0.045
      ibms_vec(2,1)= 0.0109
      ibms_vec(3,1)= 0.0681
      ibms_vec(1,2)=0.
      ibms_vec(2,2)= 0.0021
      ibms_vec(3,2)=0.0021
      ibms_vec(1,3)=-4.397  !this is 0.097m upstream of start
      dl = ibms_vec(1,3) + 4.3
      ibms_vec(2,3)= -1.9505
      ibms_vec(3,3)= 1.016

        call reallocate_coord (from_orbit, lat%branch(0)%n_ele_max)
        call reallocate_coord (to_orbit, lat%branch(1)%n_ele_max)
        
        ie_from = lat%branch(1)%ix_from_ele
        from_orbit(0)=start_orb
        x_ibms(1) = -(from_orbit(0)%vec(1) +dl*from_orbit(0)%vec(2) -ibms_vec(1,1))
      
      do i=1,lat%branch(0)%n_ele_track
         call track1(from_orbit(i-1),lat%branch(0)%ele(i), lat%param, from_orbit(i))
         if(index(lat%branch(0)%ele(i)%name,'IBMS2')/=0) x_ibms(2) = -(from_orbit(i)%vec(1) - ibms_vec(2,1))
      end do

      to_orbit(0) = from_orbit(ie_from)
      do i=1,lat%branch(1)%n_ele_track
         call track1(to_orbit(i-1),lat%branch(1)%ele(i), lat%param, to_orbit(i))
         if(index(lat%branch(1)%ele(i)%name,'IBMS3')/=0)then
            x_ibms(3) = -(to_orbit(i)%vec(1) - ibms_vec(3,1))
            exit
         endif
      end do
      return
    end subroutine evaluate_x_ibms