import numpy as np

rho = 0.2
theta = 90*np.pi/180
gap = 40*10**-3
L2 = 0.34

h = 1/rho
kx = np.sqrt(h**2)
ky = 0
L = rho*theta
beta_relat = 0.046132147
gamma_relat = 1.0010658


### DIPOLE

T11 = round(np.cos(kx*L),1)
T12 = round(1/kx*np.sin(kx*L),1)
T16 = round(1/(rho*kx*kx)*(1-np.cos(kx*L)),1)
T21 = round(-kx*np.sin(kx*L),1)
T22 = round(np.cos(kx*L),1)
T26 = round(1/(rho*kx)*np.sin(kx*L),1)

if ky != 0 :
    T33 = round(np.cos(ky*L),1)
    T34 = round(1/ky*np.sin(ky*L),1)
    T43 = round(-ky*np.sin(ky*L),1)
    T44 = round(np.cos(ky*L),1)
else :
    T33 = 1
    T34 = round(L,1)
    T43 = 0
    T44 = 1

T51 = - T26
T52 = - T16
T55 = 1
T56 = round(-1/(rho*rho*kx*kx*kx)*(kx*L*beta_relat*beta_relat-np.sin(kx*L)) + L/(gamma_relat/gamma_relat)*(1-1/(rho*rho*kx*kx)),1)
T66 = 1

T13, T14, T15 = 0, 0, 0
T23, T24, T25 = 0, 0, 0
T31, T32, T35, T36 = 0, 0, 0, 0
T41, T42, T45, T46 = 0, 0, 0, 0
T53, T54 = 0, 0
T61, T62, T63, T64, T65 = 0, 0, 0, 0, 0

M_dip = np.array([[T11, T12, T13, T14, T15, T16],
                [T21, T22, T23, T24, T25, T26],
                [T31, T32, T33, T34, T35, T36],
                [T41, T42, T43, T44, T45, T46],
                [T51, T52, T53, T54, T55, T56],
                [T61, T62, T63, T64, T65, T66]])

### EDGE


M11, M22, M33, M44, M55, M66 = 1, 1, 1, 1, 1, 1
k1 = 0.45
k2 = 2.8
beta = 22.5*np.pi/180
psi = k1*gap/rho*((1+np.sin(beta)*np.sin(beta))/np.cos(beta))*(1-k1*k2*gap/rho*np.tan(beta))
M21 = np.round(h * np.tan(beta),1)
M43 = np.round(-h * np.tan(beta-psi),1)

M_edge = np.array([[M11, 0, 0, 0, 0, 0],
                [M21, M22, 0, 0, 0, 0],
                [0, 0, M33, 0, 0, 0],
                [0, 0, M43, M44, 0, 0],
                [0, 0, 0, 0, M55, 0],
                [0, 0, 0, 0, 0, M66]])

### DRIFT

M_L2 = np.array([[1, L2, 0, 0, 0, 0],
                [0, 1, 0, 0, 0, 0],
                [0, 0, 1, L2, 0, 0],
                [0, 0, 0, 1, 0, 0],
                [0, 0, 0, 0, 1, L2/(gamma_relat*gamma_relat)],
                [0, 0, 0, 0, 0, 1]])

### Transport

M = M_L2 @ M_edge @ M_dip @ M_edge