! Routine to find the inital x,xp at start of injection line that minimize angle and offset at exit of inflecto
subroutine optimize_incident_m5_to_inj(start_orb,lat, opt_orb)
 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 (ele_pointer_struct), allocatable :: eles(:)
 integer track_state
 integer n, iteration
 integer i, ix_end, ix_start, n_loc
 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/

 call reallocate_coord (orb, lat%branch(0)%n_ele_max)
 end_orb%vec=0.
 end_orb%vec(1) = inflector_width - 0.009

 ix_end = lat%branch(0)%n_ele_track
  
! 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, 'BACKLEG_START')/=0) ix_start = i
  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
 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_many(lat,orb,ix_start,ix_end, 1, ix_branch = 0, track_state=track_state)
!  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 '(i10,2(6es12.4))',iteration,x0,y0
 print '(a, 2es12.4)',' end_orb%vec(1:2) = ', end_orb%vec(1:2)
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_many(lat,orb,ix_start,ix_end, 1, ix_branch = 0, track_state=track_state)
!  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_many(lat,orb,ix_start,ix_end, 1,ix_branch = 0, track_state=track_state)
! 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) 
! 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_many(lat,orb,ix_start,ix_end,1, ix_branch = 0, track_state=track_state)
!  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
 
 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 
  lat%ele(1:lat%n_ele_track)%alias= 'No'

return

end subroutine