% problem 2f
clc, clear

% define A, B, T, D, and T^{-1}
A = [-2,4,3,0;
    0,-1,0,4;
    0,0,-3,0;
    0,0,0,-8];

B = [1,0;
    0,0;
    0,1;
    0,0];

T = [4,1,-3,8/21;
    1,0,0,-4/7;
    0,0,1,0;
    0,0,0,1];

D = diag([-1,-2,-3,-8]);

T_inv = [0,1,0,4/7;
    1,-4,3,-8/3;
    0,0,1,0;
    0,0,0,1];

% define initial condition, time step, and time
x0 = [1;-1;2;0];
h = 0.01;
time = 0:h:2;

% instantiate empty matrix for values of x(t) at each time step
x_numerical = zeros(4, length(time));

% main loop over all time steps
for i = 1:length(time)
    ti = time(i);
    
    % create tau values based on time step h
    tau = 0:h:ti;
    nTau = length(tau);
    % instantiate empty matrix of integrand values
    integrand_values = zeros(4, nTau);
    
    % compute the values of the integrand for each tau_k
    for k = 1:nTau
        tau_k = tau(k);

        % define u(tau_k)
        u_tau = [1-exp(2.*tau_k); 2.*cos(tau_k)];

        % compute e^{A(t-tau)}
        eAtau = T * diag(exp([-1,-2,-3,-8]*(ti - tau_k))) * T_inv;

        % compute and store integrand at tau_k
        integrand_values(:,k) = eAtau * B * u_tau;
    end

    % numerically integrate using trapezoidal rule
    x_integral_term = trapz(tau, integrand_values, 2);

    % compute first term
    eAt = T * diag(exp([-1,-2,-3,-8]*ti)) * T_inv;
    x_first_term = eAt * x0;

    % add terms for full solution at time t_i and store it
    x_numerical(:,i) = x_first_term + x_integral_term;
end

% plot
f1 = figure();
set(f1, 'Position', [100, 100, 1000, 800]);

subplot(2,2,1);
plot(time, x_numerical(1,:), 'LineWidth', 3, 'Color', 'r');
set(gca, "FontSize", 12);
grid;
title("\textbf{State $x_1(t)$ (Numerical)}", "FontSize", 16, "Interpreter", "latex");
xlabel("time $t$ [s]", "FontSize", 16, "Interpreter", "latex");
ylabel("$x_1(t)$", "FontSize", 16, "Interpreter", "latex");

subplot(2,2,2);
plot(time, x_numerical(2,:), 'LineWidth', 3, 'Color', 'c');
set(gca, "FontSize", 12);
grid;
title("\textbf{State $x_2(t)$ (Numerical)}", "FontSize", 16, "Interpreter", "latex");
xlabel("time $t$ [s]", "FontSize", 16, "Interpreter", "latex");
ylabel("$x_2(t)$", "FontSize", 16, "Interpreter", "latex");

subplot(2,2,3);
plot(time, x_numerical(3,:), 'LineWidth', 3, 'Color', 'g');
set(gca, "FontSize", 12);
grid;
title("\textbf{State $x_3(t)$ (Numerical)}", "FontSize", 16, "Interpreter", "latex");
xlabel("time $t$ [s]", "FontSize", 16, "Interpreter", "latex");
ylabel("$x_3(t)$", "FontSize", 16, "Interpreter", "latex");

subplot(2,2,4);
plot(time, x_numerical(4,:), 'LineWidth', 3, 'Color', 'm');
set(gca, "FontSize", 12);
grid;
title("\textbf{State $x_4(t)$ (Numerical)}", "FontSize", 16, "Interpreter", "latex");
xlabel("time $t$ [s]", "FontSize", 16, "Interpreter", "latex");
ylabel("$x_4(t)$", "FontSize", 16, "Interpreter", "latex");

sgtitle("\textbf{Numerical Solution Plots for All States}",...
    "Interpreter", "latex", "FontSize", 18);