! compute transfer matrix and closed orbit according to da/documentation/transfer_matrix_calc/transfer_matrix_calc.pdf
!Input:
! lat_struct: lat
! coord_struct: co - the set of initial phase space vectors
! integer: i_dim  - dimension of phase space (2,4, or 6)
!Output
! real(rp) - M(1:i_dim,1:i_dim) - transfer matrix
! coord_struct: x_co - closed orbit
! If M=0 then then particle is lost for one of trajectories

subroutine compute_ft_matrix(lat, ref_orbit, emitx,emity,emitz,ix_start,ix_end,co,i_dim,full_turn, M,x_co)
use bmad
use nr

implicit none
type (lat_struct) lat
type(coord_struct), allocatable:: co(:), orbit(:), ref_orbit(:)
type(coord_struct) x_co
real(rp), allocatable,save :: X_in(:,:), X_out(:,:), a(:,:),b(:,:),J(:,:), temp(:,:)
real(rp), allocatable,save :: P(:,:), vec(:,:), test(:,:)
real(rp), allocatable  :: M(:,:)

real(rp) emitx, emity, emitz
integer i, i_dim, n
integer k
integer i_off/0/
integer track_state
integer ix_start, ix_end
integer n_use

logical err_flag/.false./
logical full_turn

n=i_dim+1

call reallocate_coord(orbit,lat%n_ele_track)
if(.not. allocated(M))allocate(M(1:i_dim,1:i_dim))
if(.not. allocated(X_in))then
 allocate(X_in(1:n,1:n))
 allocate(X_out(1:n,1:n))
 allocate(a(1:n, 1:n),b(1:n, 1:n))
 allocate(J(1:i_dim,1:i_dim),temp(1:i_dim,1:i_dim))
 allocate(P(1:n,1:n),test(1:n,1:n))
 allocate(vec(1:i_dim,1))
endif

call mat_make_unit(X_in)
call mat_make_unit(X_out)
M(:,:)=0
do i=1, n
    orbit(ix_start)%vec = co(i)%vec + ref_orbit(ix_start)%vec
 X_in(1:i_dim,i) =  co(i)%vec(1+i_off:i_dim+i_off)
 X_in(n,i) = 1

  call track_many (lat, orbit, ix_start, ix_end, direction=1,  track_state= track_state)

     if(track_state /= moving_forward$ .or. err_flag)then
  print *,'lost particle'
 return ! trajectory does not exist so there will be no x_out and no M. Return M=0
endif

 X_out(1:i_dim,i) = orbit(ix_end)%vec(1+i_off:i_dim+i_off) - ref_orbit(ix_end)%vec(1:i_dim)
 X_out(n,i) = 1
end do

 a(:,:) = X_in(:,:)

 n_use=i_dim
 if(full_turn)m=n
 
 call mat_inverse(X_in(1:n_use,1:n_use), a(1:n_use,1:n_use))


P = matmul(X_out(1:n_use,1:n_use),a(1:n_use,1:n_use))
M(1:i_dim,1:i_dim) = P(1:i_dim,1:i_dim)

if(full_turn)then
 call mat_make_unit(J) 
 temp(:,:) =-(M(:,:)-J(:,:)) 

 vec(1:i_dim,1) = P(n,1:i_dim)
 call gaussj(temp,vec)

 vec(1:i_dim,1) = matmul(temp,vec(1:i_dim,1))
 x_co%vec(1:i_dim) = vec(1:i_dim,1)
endif

return
end