(* FODO cell *)
(* 6*6 transfer matrix *)
(* Drift space *)
IdentityMatrix[6]

ML[L_]:=MatrixForm[{{1,L,0,0,0,0},{0,1,0,0,0,0},{0,0,1,L,0,0},{0,0,0,1,0,0},{0,0,0,0,1,(L/((γ0)^2))},{0,0,0,0,0,1}}]

(* QF magnet *)

MQF[K_,Lq_]:=MatrixForm[{{Cos[(Sqrt[-K]) Lq],((Sin[(Sqrt[-K]) Lq])/(Sqrt[-K])),0,0,0,0},{-(Sqrt[-K]) Sin[(Sqrt[-K]) Lq],Cos[(Sqrt[-K]) Lq],0,0,0,0},{0,0,Cosh[(Sqrt[-K])Lq],((Sinh[(Sqrt[-K]) Lq])/(Sqrt[-K])),0,0},{0,0,(Sqrt[-K]) Sinh[(Sqrt[-K]) Lq],Cosh[(Sqrt[-K]) Lq],0,0},{0,0,0,0,1,(Lq/(γ0^2))},{0,0,0,0,0,1}}]

(* QD magnet *)

MQD[K_,Lq_]:=MatrixForm[{{Cosh[(Sqrt[-K]) Lq],((Sinh[(Sqrt[-K]) Lq])/(Sqrt[-K])),0,0,0,0},{(Sqrt[-K]) Sinh[(Sqrt[-K]) Lq],Cosh[(Sqrt[-K]) Lq],0,0,0,0},{0,0,Cos[(Sqrt[-K])Lq],((Sin[(Sqrt[-K]) Lq])/(Sqrt[-K])),0,0},{0,0,-(Sqrt[-K]) Sin[(Sqrt[-K]) Lq],Cos[(Sqrt[-K]) Lq],0,0},{0,0,0,0,1,(Lq/(γ0^2))},{0,0,0,0,0,1}}]

(* B magnet *)MB[ρ0_,Lb_]:=MatrixForm[{{Cos[(Lb/(ρ0))],ρ0 Sin[(Lb/(ρ0))],0,0,0,ρ0(1-Cos[(Lb/(ρ0))])},{-((Sin[(Lb/(ρ0))])/(ρ0)),Cos[(Lb/(ρ0))],0,0,0,Sin[(Lb/(ρ0))]},{0,0,1,Lb,0,0},{0,0,0,1,0,0},{-Sin[(Lb/(ρ0))],ρ0(Cos[(Lb/(ρ0))]-1),0,0,1,ρ0 Sin[(Lb/(ρ0))]+(Lb/((γ0)^2))-Lb},{0,0,0,0,0,1}}]

(* FODO normal cell *)

MQF[Kf,Lq/2].ML[L1].MB[ρ0,Lb].ML[L1].MQD[Kd,Lq].ML[L1].MB[ρ0,Lb].ML[L1].MQF[Kf,Lq/2];
%/.{Kf->-K,Kd->-K}

(({{Cos[(((Sqrt[K]) Lq)/2)], ((Sin[(((Sqrt[K]) Lq)/2)])/(Sqrt[K])), 0, 0, 0, 0}, {-(Sqrt[K]) Sin[(((Sqrt[K]) Lq)/2)], Cos[(((Sqrt[K]) Lq)/2)], 0, 0, 0, 0}, {0, 0, Cosh[(Sqrt[K])Lq], ((Sinh[(((Sqrt[K]) Lq)/2)])/(Sqrt[K])), 0, 0}, {0, 0, (Sqrt[K]) Sinh[(((Sqrt[K]) Lq)/2)], Cosh[(((Sqrt[K]) Lq)/2)], 0, 0}, {0, 0, 0, 0, 1, (Lq/(2 ((γ0)^2)))}, {0, 0, 0, 0, 0, 1}})).(({{1, L1, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, L1, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, (L1/((γ0)^2))}, {0, 0, 0, 0, 0, 1}})).(({{Cos[(Lb/(ρ0))], ρ0 Sin[(Lb/(ρ0))], 0, 0, 0, ρ0 (1-Cos[(Lb/(ρ0))])}, {-((Sin[(Lb/(ρ0))])/(ρ0)), Cos[(Lb/(ρ0))], 0, 0, 0, Sin[(Lb/(ρ0))]}, {0, 0, 1, Lb, 0, 0}, {0, 0, 0, 1, 0, 0}, {-Sin[(Lb/(ρ0))], ρ0 (-1+Cos[(Lb/(ρ0))]), 0, 0, 1, -Lb+(Lb/((γ0)^2))+ρ0 Sin[(Lb/(ρ0))]}, {0, 0, 0, 0, 0, 1}})).(({{1, L1, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, L1, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, (L1/((γ0)^2))}, {0, 0, 0, 0, 0, 1}})).(({{Cosh[(Sqrt[K]) Lq], ((Sinh[(Sqrt[K]) Lq])/(Sqrt[K])), 0, 0, 0, 0}, {(Sqrt[K]) Sinh[(Sqrt[K]) Lq], Cosh[(Sqrt[K]) Lq], 0, 0, 0, 0}, {0, 0, Cos[(Sqrt[K])Lq], ((Sin[(Sqrt[K]) Lq])/(Sqrt[K])), 0, 0}, {0, 0, -(Sqrt[K]) Sin[(Sqrt[K]) Lq], Cos[(Sqrt[K]) Lq], 0, 0}, {0, 0, 0, 0, 1, (Lq/((γ0)^2))}, {0, 0, 0, 0, 0, 1}})).(({{1, L1, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, L1, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, (L1/((γ0)^2))}, {0, 0, 0, 0, 0, 1}})).(({{Cos[(Lb/(ρ0))], ρ0 Sin[(Lb/(ρ0))], 0, 0, 0, ρ0 (1-Cos[(Lb/(ρ0))])}, {-((Sin[(Lb/(ρ0))])/(ρ0)), Cos[(Lb/(ρ0))], 0, 0, 0, Sin[(Lb/(ρ0))]}, {0, 0, 1, Lb, 0, 0}, {0, 0, 0, 1, 0, 0}, {-Sin[(Lb/(ρ0))], ρ0 (-1+Cos[(Lb/(ρ0))]), 0, 0, 1, -Lb+(Lb/((γ0)^2))+ρ0 Sin[(Lb/(ρ0))]}, {0, 0, 0, 0, 0, 1}})).(({{1, L1, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, L1, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, (L1/((γ0)^2))}, {0, 0, 0, 0, 0, 1}})).(({{Cos[(((Sqrt[K]) Lq)/2)], ((Sin[(((Sqrt[K]) Lq)/2)])/(Sqrt[K])), 0, 0, 0, 0}, {-(Sqrt[K]) Sin[(((Sqrt[K]) Lq)/2)], Cos[(((Sqrt[K]) Lq)/2)], 0, 0, 0, 0}, {0, 0, Cosh[(Sqrt[K])Lq], ((Sinh[(((Sqrt[K]) Lq)/2)])/(Sqrt[K])), 0, 0}, {0, 0, (Sqrt[K]) Sinh[(((Sqrt[K]) Lq)/2)], Cosh[(((Sqrt[K]) Lq)/2)], 0, 0}, {0, 0, 0, 0, 1, (Lq/(2 ((γ0)^2)))}, {0, 0, 0, 0, 0, 1}}));
%[[5;;6,5;;6]];
FullSimplify[%]

(* Thin lense approximation *)
(* Drift space *)
ML[L_]:=MatrixForm[{{1,L,0,0,0,0},{0,1,0,0,0,0},{0,0,1,L,0,0},{0,0,0,1,0,0},{0,0,0,0,1,(L/((γ0)^2))},{0,0,0,0,0,1}}]
(* QF magnet *)
TQF[kf_]:=MatrixForm[{{1,0,0,0,0,0},{-kf,1,0,0,0,0},{0,0,1,0,0,0},{0,0,kf,1,0,0},{0,0,0,0,1,0},{0,0,0,0,0,1}}]
(* QD magnet *)
TQD[kd_]:=MatrixForm[{{1,0,0,0,0,0},{kd,1,0,0,0,0},{0,0,1,0,0,0},{0,0,-kd,1,0,0},{0,0,0,0,1,0},{0,0,0,0,0,1}}]
(* B magnet *)TB[θ_]:=MatrixForm[{{1,0,0,0,0,0},{0,1,0,0,0,θ},{0,0,1,0,0,0},{0,0,0,1,0,0},{-θ,0,0,0,1,0},{0,0,0,0,0,1}}]

(* FODO normal cell *)
TQF[Kf/2].ML[L1].TB[θ].ML[L1].TQD[Kd].ML[L1].TB[θ].ML[L1].TQF[Kf/2]

(({{1, 0, 0, 0, 0, 0}, {-(Kf/2), 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, 0, (Kf/2), 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}})).(({{1, L1, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, L1, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, (L1/((γ0)^2))}, {0, 0, 0, 0, 0, 1}})).(({{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, θ}, {0, 0, 1, 0, 0, 0}, {0, 0, 0, 1, 0, 0}, {-θ, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}})).(({{1, L1, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, L1, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, (L1/((γ0)^2))}, {0, 0, 0, 0, 0, 1}})).(({{1, 0, 0, 0, 0, 0}, {Kd, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, 0, -Kd, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}})).(({{1, L1, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, L1, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, (L1/((γ0)^2))}, {0, 0, 0, 0, 0, 1}})).(({{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, θ}, {0, 0, 1, 0, 0, 0}, {0, 0, 0, 1, 0, 0}, {-θ, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}})).(({{1, L1, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, L1, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, (L1/((γ0)^2))}, {0, 0, 0, 0, 0, 1}})).(({{1, 0, 0, 0, 0, 0}, {-(Kf/2), 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, 0, (Kf/2), 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}));
FODO=FullSimplify[%]

(* Stability condition *)
(* Horizontal *)
-2<Tr[FODO[[1;;2,1;;2]]]<2;
Simplify[%];
Hstab=Expand[%]
(* Vertical *)
-2<Tr[FODO[[3;;4,3;;4]]]<2;
Simplify[%];
Vstab=Expand[%]

RegionPlot[{Hstab/.L1->1,Vstab/.L1->1},{Kf,0,2},{Kd,0,2}]

(* Tune & β-function *)
Solνx=Solve[Cos[2π νx]==(1/2)Tr[FODO[[1;;2,1;;2]]],νx]
Solve[βx Sin[2π νx]==FODO[[1,2]],{βx}]/.Solνx[[2]];
Simplify[%];Flatten[%]

 %/.{Kd->k,Kf->k}; 
Simplify[%]

Solνy=Solve[Cos[2π νy]==(1/2)Tr[FODO[[3;;4,3;;4]]],νy]
Solve[βy Sin[2π νy]==FODO[[3,4]],{βy}]/.Solνy[[2]];
Simplify[%];Flatten[%]

 %/.{Kd->k,Kf->k}; 
Simplify[%]

(* 3*3 transfer matrix *)
THcell={{FODO[[1,1]], FODO[[1,2]],FODO[[1,6]]}, 
  {FODO[[2,1]],FODO[[2,2]],FODO[[2,6]]},{0,0,1}}

(* Dispersion function *)
Solve[{X,XP,1}==THcell.{X,XP,1},{X,XP}];
Simplify[%];Flatten[%]

 %/.{Kd->k,Kf->k}; 
Simplify[%]