subroutine opt_inf_angle(lat, co,nbranch, track_state, err_flag, kicker_params)
  use bmad
  use muon_mod
  use parameters_bmad
  use runge_kutta_mod
  use materials_mod
  use quad_scrape_parameters
  use random_mod

  implicit none

  type (lat_struct), target :: lat
  type (coord_struct), allocatable, target :: co(:)
  type (coord_struct), allocatable :: orbit(:)
  type (coord_struct) co_start, co_end, co_end_ref
  type (kicker_params_struct) kicker_params, kicker_params_0
  real(rp) A(2,2), A_inv(2,2)
  real(rp) dx(2), delta_x_initial(2)
  real(rp) det,amp,beta/7.8/
  real(rp) amp_ref,grad(2),const
  integer nbranch, track_state, nturns/10/
  integer n_iterations/20/, i,j
  logical err_flag

  dx(1) = 1.e-4
  dx(2) = 1.e-5
  kicker_params_0%kicker_field(1:3) = kicker_params%kicker_field(1:3)

  
  
  call reallocate_coord(orbit, lat%branch(nbranch)%n_ele_max)
  orbit(0) = co(0)
  co_start = co(0)
  
do j=1,n_iterations

    do i=1,nturns
     call track_all(lat,orbit,nbranch,track_state, err_flag)
     orbit(0) = orbit(lat%branch(nbranch)%n_ele_track)
    end do
    co_end_ref = orbit(lat%branch(nbranch)%n_ele_track)
    amp_ref = co_end_ref%vec(1)**2 + (beta*co_end_ref%vec(2))**2
     
    print '(a,i10,6es14.6)','iterations, angle,kick, co_end_ref%vec(1:2),amp_ref,const',j,co_start%vec(2), kicker_params%kicker_field(1), co_end_ref%vec(1:2), amp_ref, const
    kicker_params%kicker_field(1:3) = kicker_params_0%kicker_field(1:3) + dx(1)
    call set_kicker_params (lat,kicker_params)
    
    orbit(0) = co_start

    do i=1,nturns
     call track_all(lat,orbit,nbranch,track_state, err_flag)
     orbit(0) = orbit(lat%branch(nbranch)%n_ele_track)
    end do
    co_end = orbit(lat%branch(nbranch)%n_ele_track)
    amp = co_end%vec(1)**2 + (beta*co_end%vec(2))**2
    
    A(1,1) = (co_end%vec(1)- co_end_ref%vec(1))/dx(1)
    A(2,1) = (co_end%vec(2)- co_end_ref%vec(2))/dx(1)

    grad(1) = (amp-amp_ref)/dx(1)
    
    kicker_params%kicker_field(1:3) = kicker_params_0%kicker_field(1:3)
    call set_kicker_params (lat,kicker_params)
    orbit(0) = co_start
    orbit(0)%vec(2) = co_start%vec(2) + dx(2)

    do i=1,nturns
     call track_all(lat,orbit,nbranch,track_state, err_flag)
     orbit(0) = orbit(lat%branch(nbranch)%n_ele_track)
    end do
    co_end = orbit(lat%branch(nbranch)%n_ele_track)
    amp = co_end%vec(1)**2 + (beta*co_end%vec(2))**2
    
    A(1,2) = (co_end%vec(1)- co_end_ref%vec(1))/dx(2)
    A(2,2) = (co_end%vec(2)- co_end_ref%vec(2))/dx(2)
    grad(2) = (amp-amp_ref)/dx(2)   
  
    det = A(1,1)*A(2,2)-A(1,2)*A(2,1)

    A_inv(1,1) = A(2,2)/det
    A_inv(2,2) = A(1,1)/det
    A_inv(1,2) = -A(1,2)/det
    A_inv(2,1) = -A(2,1)/det 

    delta_x_initial = -matmul(A_inv,co_end_ref%vec(1:2))/1000.
    const = amp_ref/dot_product(grad,grad)
!    delta_x_initial = -const/100.*grad 
    
    kicker_params_0%kicker_field(1:3) = kicker_params_0%kicker_field(1:3) +delta_x_initial(1)
    kicker_params%kicker_field(1:3) = kicker_params_0%kicker_field(1:3)
    call set_kicker_params (lat,kicker_params)

    co_start%vec(2) = co_start%vec(2) + delta_x_initial(2)
    orbit(0) = co_start
    
 end do !iterations

    do i=1,nturns
     call track_all(lat,orbit,nbranch,track_state, err_flag)
     orbit(0) = orbit(lat%branch(nbranch)%n_ele_track)
    end do
    co_end_ref = orbit(lat%branch(nbranch)%n_ele_track)
    amp = co_end_ref%vec(1)**2 + (beta*co_end_ref%vec(2))**2
    print '(a,3es12.4)','kicker,angle,amp',kicker_params%kicker_field(1), co_start%vec(2), amp
    print '(a,6es12.4)','orbit(0)%vec(1:2), orbit(0)%t, co_end_ref%vec(1:2), co_end_ref%t',co_start%vec(1:2),co_start%t, co_end_ref%vec(1:2),co_end_ref%t

  return
end