%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   FBSSIS
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;
close all;

%% Inputs
n=1000;
M=20;

T=20;
x0=.1;

R0=5;
A=.4;
w1=3;
w2=.1;

%% Temporal domain
t=linspace(0,T,n);
dt=t(2)-t(1);



%% Forward Backward Sweep Method


x=x0*ones(1,n);
lambda=zeros*ones(1,n);

%% Initial guess
u=zeros(1,n);
figure('units','normalized','outerposition',[0 0 1 1]);
for k=1:M
    
    for j=1:n-1
        xdot=R0*(1-x(j))*x(j)-x(j)-A*x(j)-u(j)*x(j);
        x(j+1)=x(j)+xdot*dt;
    end
    
    
    for j=n:-1:2
        lambdadot=-w1-lambda(j)*(R0*(1-2*x(j))-1-A-u(j));
        lambda(j-1)=lambda(j)-lambdadot*dt;
    end
    
    
    %% Plotting numerically computed solution
    
    subplot(2,2,1)
    hold on;
    plot(t,x,'linewidth',2)
    xlabel('$$t$$','interpreter','latex')
    ylabel('$$x(t)$$','interpreter','latex')
    axis([0,T,0,1]);
    ax=gca;
    ax.FontSize = 40;
    
    subplot(2,2,2)
    hold on;
    plot(t,lambda,'linewidth',2)
    xlabel('$$t$$','interpreter','latex')
    ylabel('$$\lambda(t)$$','interpreter','latex')
    ax=gca;
    ax.FontSize = 40;
    
    subplot(2,2,3)
    hold on;
    plot(t,u,'linewidth',2)
    xlabel('$$t$$','interpreter','latex')
    ylabel('$$u(t)$$','interpreter','latex')
    ax=gca;
    ax.FontSize = 40;
    
    subplot(2,2,4)
    hold on;
    
    L=trapz(t,(w1*x+w2*u.^2));
    plot(k,L,'o','linewidth',2,'markersize',8);
    xlabel('$$N$$','interpreter','latex')
    ylabel('$$C[u(t)]$$','interpreter','latex')
    ax=gca
    ax.FontSize = 40;
    
    drawnow;
    %% Finding new optimal u
    u=lambda.*x/(2*w2);
end