%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                            Michael Pokojovy                             % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function heateq;
    c_rho = 0.47;
    rho = 7.85;
    kappa = 54;
    
    L = 0.5;
    N = 50;
    
    T = 60;
    %T = 1000;
    
    M = 5000;
    
    h = 1/(N + 1);
    dt = T/(M - 1);
    
    e = ones(N, 1);
    A = spdiags([e -2*e e], -1:1, N, N);
    A(1, 1) = -1;
    A(N, N) = -1;
    A = kappa/(rho*c_rho)*A/h^2;
    
    B = zeros(N, 1);
    B(1) = -1/h;
    
    t_lattice = linspace(0, T, M);
    x_lattice = linspace(0, L, N + 2);
   
    %x0 = 30 + cos(4*pi/L*x_lattice(2:end-1))';
    %xT = 500 + cos(2*pi/L*x_lattice(2:end-1))';
    
    x0 = 300*ones(N, 1);
    xT = zeros(N, 1);
    
    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);
    
    [T_grid, X_grid] = meshgrid(t_lattice, x_lattice(2:end-1));
    Y_grid = zeros(size(T_grid));
    
    for i = 1:size(T_grid, 1)
    for j = 1:size(T_grid, 2)
        Y_grid(i, j) = state(i, j);
    end
    end
    
    surf(T_grid, X_grid, Y_grid, 'EdgeColor', 'none');
    view([18 20]);
    
    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
        
        delta = (Q_T\(xt - xT));
        
        evolution = eye(N, N);
        for i = 1:M
           output(M - i + 1) = -B'*evolution'*delta;
           evolution = (speye(N, N) - 0.5*dt*A)\(speye(N, N) + 0.5*dt*A)*evolution;
        end
    end
end