! create transfer 6X6 transfer matrix T that propagates phase space vectors from twiss0 to twiss1

subroutine make_transfer(twiss0,twiss1,T)
use precision_def

implicit none

type g2twiss_struct
 real(rp) betax, betay, alphax, alphay, etax, etapx, etay, etapy, phix, phiy, gammax, gammay
end type

real(rp) T(6,6), G(2,2), G_inv(2,2), R(2,2), m(2,2),Mx(2,2),Ny(2,2)
type (g2twiss_struct) twiss0, twiss1
integer i
 
T=0
do i=1,6
 T(i,i)=1.
end do

call makeGR(twiss0%betax,twiss0%alphax,twiss1%phix-twiss0%phix,G, G_inv,R)

Mx = matmul(R,G)
call makeGR(twiss1%betax,twiss1%alphax,twiss1%phix-twiss0%phix,G, G_inv,R)

T(1:2,1:2) = matmul(G_inv, Mx)

call makeGR(twiss0%betay,twiss0%alphay,twiss1%phiy - twiss0%phiy,G, G_inv,R)
Ny = matmul(R,G)
call makeGR(twiss1%betay,twiss1%alphay,twiss1%phiy - twiss0%phiy,G, G_inv,R)
T(3:4,3:4) = matmul(G_inv, Ny)

Mx = T(1:2,1:2)
call make_m(twiss1%etax,twiss0%etax,twiss1%etapx,twiss0%etapx,Mx,m)
T(1:2,5:6) = m

call make_m(twiss1%etay,twiss0%etay,twiss1%etapy,twiss0%etapy,T(3:4,3:4),m)
T(3:4,5:6) = m

return
end


subroutine makeGR(beta,alpha,phi,G,Ginv,R)
 use precision_def

  implicit none
   real(rp) beta,alpha, phi, G(2,2), Ginv(2,2), R(2,2)

G = 0.
Ginv = 0.
   
G(1,1) = 1./sqrt(beta)
G(2,1) = -alpha/sqrt(beta)
G(2,2) = -sqrt(beta)

Ginv(1,1) = sqrt(beta)
Ginv(2,1) = -alpha/sqrt(beta)
Ginv(2,2) = -1./sqrt(beta)

R(1,1) = cos(phi)
R(1,2) = sin(phi)
R(2,1) = -R(1,2)
R(2,2) = R(1,1)

return
end

subroutine make_m(eta1,eta0,etap1,etap0,MM,m)
use precision_def

implicit none

real(rp) eta1, eta0, etap1, etap0
real(rp) eta_b(2,2), MM(2,2), m(2,2), eta_e(2,2)

eta_e = 0
eta_e(1,2) = eta1
eta_e(2,2) = etap1

eta_b = 0
eta_b(1,2) = eta0
eta_b(2,2) = etap0

m=0
m = eta_e + matmul(MM,eta_b)

return
end