%% Parameters

d = 1;
n = 4;
h = d/n;
a0 = 1;
b0 = 1;
a = a0/h;
b = b0/h;

%% State space model

A11 = zeros(n,n);
A12 = toeplitz(vertcat(-a,zeros(n-1,1)), horzcat([-a, a], zeros(1,n-2)));
A21 = toeplitz(vertcat(b,zeros(n-1,1)), horzcat([b, -b], zeros(1,n-2)))';
A22 = zeros(n,n);

A = [A11, A12; A21, A22];

B = zeros(2*n,1); B(n,1) = 1;

C = [zeros(n,n),eye(n,n)];

D = zeros(n,1);

G = ss(A,B,C,D);

%% Poles

figure(1)
plot(eig(A),'.')
axis([-1e-3, 1e-3, -30, 30])

%% Simulation

p0q0 = zeros(2*n,1); p0q0(n+n/2) = h/3; p0q0(n+n/2-1) = h/4; p0q0(n+n/2+1) = h/4;

[q,t,x] = initial(G,p0q0,5);

F(length(t)) = struct('cdata',[],'colormap',[]);

figure(2)
for it = 1:length(t)
    plot(0:h:d,[0, q(it,:)],'.-','MarkerSize',10)
    axis([-h,d+h,-h,h])
    xlabel('x')
    ylabel('q(x,t)')
    grid on
    text(0.02,0.02,[' t = ',num2str(t(it))],...
            'VerticalAlignment','middle',...
            'HorizontalAlignment','left',...
            'FontSize',12)
    F(it) = getframe(gcf);
end

%% Playing the animation
% figure(3)
% movie(gcf,F,3,10)

%% True longitudinal deformation
figure(4)

y = cumsum(q,2);

F2(length(t)) = struct('cdata',[],'colormap',[]);

for it = 1:length(t)
    plot([0, y(it,:)+cumsum(h*ones(1,n))],zeros(1,n+1),'.-','MarkerSize',10)
    axis([-h,d+n*h/5,-h,h])
    xlabel('x, y')
    ylabel('-')
    grid on
    text(0.02,0.02,[' t = ',num2str(t(it))],...
            'VerticalAlignment','middle',...
            'HorizontalAlignment','left',...
            'FontSize',12)
    F2(it) = getframe(gcf);
end

%% Playing the animation
figure(5)
movie(gcf,F2,3,10)