%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                            Michael Pokojovy                             % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function heateq;
    omega = 0.1;

    T = 60;
    M = 1000;
    
    dt = T/(M - 1);
    
    N = 2;
    
    A = [0 1; -omega^2 0];
    
    B = [0; 1];
    
    t_lattice = linspace(0, T, M);
    
    x0 = [2; -1];
    xT = [-5;  2];
    
    control = optimal_control;
    
    l2norm = sqrt(dt*sum(control*control'))
    
    figure(1);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    set(gcf, 'PaperUnits', 'centimeters');
    xSize = 20; ySize = 10;
    xLeft = (21 - xSize)/2; yTop = (30 - ySize)/2;
    set(gcf,'PaperPosition', [xLeft yTop xSize ySize]);
    set(gcf,'Position',[0 0 xSize*50 ySize*50]);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
    subplot(1, 2, 1);
    hold on;

    t = title(['$L^{2}$-optimale Steuerung $\hat{u}$'], 'interpreter', 'latex');
    set(t, 'FontSize', 16);

    plot(t_lattice, control);

    grid on;

    xlabel('$t$', 'interpreter', 'latex', 'FontSize', 16);
    ylabel('$\hat{u}$', 'interpreter', 'latex', 'FontSize', 16);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
    subplot(1, 2, 2);
    hold on;

    t = title('Zugeh\"origer Zustandsverlauf $y^{x_{0}, \hat{u}}$', 'interpreter', 'latex');
    set(t, 'FontSize', 16);    
     
    state = input_output_operator(control);
    
    plot(x0(1), x0(2), 'r.');
    text(x0(1), x0(2), '$x_{0}$', 'interpreter', 'latex', 'FontSize', 16); 
    plot(xT(1), xT(2), 'r.');
    text(xT(1), xT(2), '$x_{T}$', 'interpreter', 'latex', 'FontSize', 16); 
    plot(state(1, :), state(2, :), 'b');
    
    delta = max(state, [], 2) - min(state, [], 2);
    delta = delta*1.2;
    mid = 0.5*(max(state, [], 2) + min(state, [], 2));
    axis([mid(1) - delta(1)/2, mid(1) + delta(1)/2, mid(2) - delta(2)/2, mid(2) + delta(2)/2]);
    
    grid on;
    
    xlabel('$t$', 'interpreter', 'latex', 'FontSize', 16);
    ylabel('$x$', 'interpreter', 'latex', 'FontSize', 16);
    zlabel('$\theta_{h}$', 'interpreter', 'latex', 'FontSize', 16);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    function output = input_output_operator(u)
        output = zeros(N, M);
        
        output(:, 1) = x0;
        
        for i = 2:M
            output(:, i) = (speye(N, N) - 0.5*dt*A)\((speye(N, N) + 0.5*dt*A)*output(:, i-1) + 0.5*dt*B*u(i) + 0.5*dt*B*u(i - 1));
        end
    end

    function output = gramian
        output = zeros(N, N);
        
        evolution = eye(N, N);
        
        output = output + dt*evolution*B*B'*evolution';
        
        for i = 2:length(t_lattice)
            evolution = (speye(N, N) - 0.5*dt*A)\((speye(N, N) + 0.5*dt*A)*evolution);
            output = output + dt*evolution*B*B'*evolution';
        end
    end

    function output = optimal_control;
        output = zeros(1, M);
        
        Q_T = gramian;
        
        xt = x0;
        for i = 2:length(t_lattice)
            xt = (speye(N, N) - 0.5*dt*A)\((speye(N, N) + 0.5*dt*A)*xt);
        end
        
        evolution = eye(N, N);
        for i = 1:M
            output(M - i + 1) = -B'*evolution'*(Q_T\(xt - xT));
            evolution = (speye(N, N) - 0.5*dt*A)\(speye(N, N) + 0.5*dt*A)*evolution;
        end
    end
end