%--------------------------------------------------------------------------
%
% Kuramoto Sivashinsky equation
%
% P-tau compensator.
%
% A simple proportional + time-delay controller is considered. The system 
% is excited by a white Gaussian noise d(t). The control gain has been 
% obtained by a trial and error procedure.
% Actuator saturation and displacemnt are also implemented: to activate
% uncomment the respective code lines.
%
% Reference figure(s): Fig. 22, Fig. 23
% 
% Nicolo' Fabbiane, March 2014
% KTH Mechanics
% nicolo@mech.kth.se
%
%--------------------------------------------------------------------------
clc, clear all
fprintf('\nProportional controller.\n\n')




%% Initialization of the plant --------------------------------------------

% LTI system matrix A and x grid
nq = 400; % number of grid points

[A,x,I] = KS_init(nq);


% Inputs matrix B

% disturbance d (Gaussian shape at x_d with sigma_d variance)
x_d = 35; sigma_d = 4; 
Bd  = exp(-((x-x_d)/sigma_d).^2)/sigma_d;

% actuator u (Gaussian shape at x_u with sigma_u variance)
x_u = 400; sigma_u = 4; 
Bu  = exp(-((x-x_u)/sigma_u).^2)/sigma_u;


% Outputs matrix C

% measurement y (Gaussian shape at x_y with sigma_y variance)
x_y = 300; sigma_y = 4; 
Cy  = (exp(-((x-x_y)/sigma_y).^2)/sigma_y).' * I;

% output z (Gaussian shape at x_z with sigma_z variance)
x_z = 700; sigma_z = 4; 
Cz  = (exp(-((x-x_z)/sigma_z).^2)/sigma_z).' * I;




%% Proportional controller ------------------------------------------------
P   =  -0.5432; % gain
tau = 250;      %time delay




%% Actuator displacement (uncomment to acrivate) --------------------------
% x_u = x_u + 5; 
% Bu = exp(-((x-x_u)/sigma_u).^2)/sigma_u;



        
%% Time integration -------------------------------------------------------

% Parameters
dt   = 1;
Tend = 50000;

Ton = 4000;

t  = 0:dt:Tend;
nt = length(t);


% Time-stepper matrices (Crank-Nicholson)
CNi = (speye(nq) - dt/2*A);
CNe = (speye(nq) + dt/2*A);


% Noises inizialization
d = randn(1,nt);     % unitary variance Gaussian white noise
d = d - mean(d,2);   % enforce zero-mean
d = d / std(d,[],2); % enforce unitary variance

n = randn(1,nt);     % unitary variance Gaussian white noise
n = n - mean(n,2);   % enforce zero-mean
n = n./ std(n,[],2) * 1e-1;


% Variables initialization
v = zeros(nq,nt); % velocity
y = zeros(1 ,nt); % measurement signal
z = zeros(1 ,nt); % output signal
u = zeros(1 ,nt); % control signal

y_buf   = zeros(1,round(tau/dt)); % measurement signal buffer


% Time-loop
for i = 1:nt-1
    
    % outputs
    y(:,i) = Cy*v(:,i) + n(:,i);
    z(:,i) = Cz*v(:,i);
    
    % control forcing (Eqn. 106)
    if t(i) >= Ton
        u(:,i) =P * y_buf(end);
    end
    
    % saturation (uncomment to activate)
    % u(:,i) = min([max([-2,u(:,i)]),2]);
    
    % update measurement buffer
    y_buf  = [y(:,i) y_buf(:,1:end-1)];
    
    % timestep
    v(:,i+1) = CNi\(CNe*v(:,i) + dt * Bd*d(:,i) + dt * Bu*u(:,i));
    
    % runtime output
    runtime_output

end




%% Velocity statistics ----------------------------------------------------
Tsta = 20000;
ista = (Tend-Tsta <= t) & (t <= Tend);

% Root-Mean-Square
v_rms = sqrt( mean(v(:,ista).^2,2) - mean(v(:,ista),2).^2 );

% visualization
rms_style  = '-g';
rms_legend = 'P-\tau';

plot_rms
