clear all; clc;
t0 = 0;
y0 = [1;1];
T = 5;

A = [-2 1; 1 -2];
f = @(t,y) A * y;

h = 1e-2;
t = t0:h:T;
y_true = zeros(2,length(t));
y_true(:,1) = y0;
for i = 2:length(t)
    y_true(:,i) = expm(t(i)*A) * y0;
end

[t,y] = simpson(h, f, t0, y0, T, @(t) A);

figure()
subplot(2,2,1)
plot(t, y_true(1,:), '-k','LineWidth',2);
xlabel('t','FontSize',14);
ylabel('x_1','FontSize',14);
title('True Solution','FontSize',14)
set(gca,'FontSize',14)

subplot(2,2,2)
plot(t, y_true(1,:), '-k','LineWidth',2);
xlabel('t','FontSize',14);
ylabel('x_2','FontSize',14);
title('True Solution','FontSize',14)
set(gca,'FontSize',14)

subplot(2,2,3)
plot(t, y(1,:), '-r','LineWidth',2);
xlabel('t','FontSize',14);
ylabel('x_1','FontSize',14);
title('Approximation by Simpson','FontSize',14)
set(gca,'FontSize',14)

subplot(2,2,4)
plot(t, y(2,:), '-r','LineWidth',2);
xlabel('t','FontSize',14);
ylabel('x_2','FontSize',14);
title('Approximation by Simpson','FontSize',14)
set(gca,'FontSize',14)

% % Obtain spectrum of A
% s = eig(A)
% 
% % Define stability polynomial
% for lambda = s'
% 	p = [1-h*lambda/3, -4/3*h*lambda, -1 - h*lambda/3];
% 	r = roots(p)
% end
