!+
! Subroutine bmad_set_twiss_at_beginning (ix_lat, s_pos, twiss_a, twiss_b)
!
! Routine to set the twiss parameters at the beginning of the lattice based
! upon the twiss parameters specified at some given s-position.
! Call this routine after bmad_create_finalize.
!
! Input:
!   ix_lat    -- Integer: Index of lattice to create.
!   s_pos     -- Real(rp): Position where input Twiss parameters are given.
!   twiss_a   -- C_twiss: a-mode Twiss parameters at s = s_pos.
!   twiss_b   -- C_twiss: b-mode Twiss parameters at s = s_pos.
!-

subroutine bmad_set_twiss_at_beginning (ix_lat, s_pos, twiss_a, twiss_b)

use bmad_common_mod
use bookkeeper_mod
use transfer_map_mod

implicit none

type (c_ptr), value :: twiss_a, twiss_b
type (lat_struct), pointer :: lat
type (ele_struct), target :: ele

real(rp) s_pos, mat6(6,6), vec0(6), eta(4), v_mat(4,4), v_inv_mat(4,4)

integer ix_lat

logical ok

character(32), parameter :: r_name = 'bmad_set_twiss_at_beginning'

!

if (lat_status(ix_lat) /= init_done$) then
  call out_io (s_error$, r_name, "LATTICE \i0\ NOT INITIALIZED!", ix_lat)
  return
end if

lat => bd_com(ix_lat)%lat

!

call init_ele (ele)
call twiss_to_f (twiss_a, c_loc(ele%a))
call twiss_to_f (twiss_b, c_loc(ele%b))

eta = [ele%a%eta, ele%a%etap, ele%b%eta, ele%b%etap]
call make_v_mats (ele, v_mat, v_inv_mat)
eta = matmul(v_mat, eta)
ele%x%eta  = eta(1)
ele%x%etap = eta(2)
ele%y%eta  = eta(3)
ele%y%etap = eta(4)

call mat6_from_s_to_s (lat, mat6, vec0, lat%ele(0)%s, s_pos)
call mat_inverse (mat6, lat%ele(0)%mat6, ok)
if (.not. ok) then
  call out_io (s_error$, r_name, "CANNOT INVERT MATRIX IN LATTICE \i0\ ", ix_lat)
  return
end if

call twiss_propagate1(ele, lat%ele(0))
call mat_make_unit(lat%ele(0)%mat6)

call twiss_propagate_all (lat)

end subroutine