QF = OptElem(0.2, Quadrupole(-1.2, 0.2));
QD = OptElem(0.4, Quadrupole(1.2, 0.4));
B  = OptElem(1.5, Dipole(3.8197, 1.5));
EB = OptElem(0.0, Edge(3.8197, 0.1964));
D  = OptElem(0.55, Drift(0.55));

C=QF*D*EB*B*EB*D*QD*D*EB*B*EB*D*QF;
C2=EB*B*EB*D*QF;
C3=D*QD*D;
ACC=C*C*C*C;

xz = [0.001; 0.001; -0.002; -0.001; 0.0];
xz2 = C2*xz;
xz3 = C3*xz2;
x = xz(1);
xp = xz(2);
x2 = xz2(1);
x2p = xz2(2);
x3 = xz3(1);
x3p = xz3(2);
z = xz(3);
zp = xz(4);
z2 = xz2(3);
z2p = xz2(4);
z3 = xz3(3);
z3p = xz3(4);

for i=1:100
	xz = ACC*xz;
	xz2 = C2*xz;
    xz3 = C3*xz2;

	x = [x xz(1)];
	xp = [xp xz(2)];
	x2 = [x2 xz2(1)];
	x2p = [x2p xz2(2)];
	x3 = [x3 xz3(1)];
	x3p = [x3p xz3(2)];


	z = [z xz(3)];
	zp = [zp xz(4)];
	z2 = [z2 xz2(3)];
	z2p = [z2p xz2(4)];
	z3 = [z3 xz3(3)];
	z3p = [z3p xz3(4)];
end


xset("window",1);
clf();
xgrid(2);
xtitle("Horizontal phase space", "x [m]", "x'' [rad]");
plot2d(x, xp, style=-1)
plot2d(x2, x2p, style=-2)
plot2d(x3, x3p, style=-3)

xset("window",2);
clf();
xgrid(2);
xtitle("Vertical phase space", "x [m]", "x'' [rad]");
plot2d(z, zp, style=-1)
plot2d(z2, z2p, style=-2)
plot2d(z3, z3p, style=-3)

