%% matlab cleaning
close all  
clear all
clc

%% Data
a = 0.5;
b = 2;
c = 0.2;
N = 200;
lambda = 0.3;

%% real model
Theta0 = [a b c]';
E = rand(N,1)*sqrt(lambda);
U = zeros(N,1);
U([31:60 131:160]) = 1;
U([61:100 161:200]) = 2;
xo = [0;0;0];
Y = Simula(U,E,Theta0,xo);

% draw chart
figure(1),
    plot(1:N,U,'+',1:N,Y,'o');
    title('true system')
    legend({'Input','Output'});
    xlabel('sample time')

%% Cross validation
disp(['Cross Validation'])
% data split
Ni = 100; 
Zid = [U(1:Ni) Y(1:Ni)];
Zval = [U((Ni+1):end) Y((Ni+1):end)];
xo1 = [0 0 0]';
xo1gen = [xo ; 0];

% identification phase
theta1 = fminsearch(@(t) Varx1(t,Zid(:,1), Zid(:,2), xo1), zeros(3,1));
theta1gen = [theta1; 0];
Yp1_id = ArxPred( Zid(:,1), Zid(:,2),theta1gen ,xo1gen);
SSR1_id = sum((Yp1_id-Zid(:,2)).^2);

theta2 = fminsearch(@(t) Varx2(t,Zid(:,1), Zid(:,2),xo1), zeros(3,1));
theta2gen = [theta2(1); 0; theta2(2:3)];
xo2gen = [xo(1); 0 ; xo(2:3)];
Yp2_id = ArxPred( Zid(:,1), Zid(:,2), theta2gen,xo2gen);
SSR2_id = sum((Yp2_id-Zid(:,2)).^2);

% validation phase
Yp1_val = ArxPred( Zval(:,1), Zval(:,2), theta1gen ,xo1gen);
SSR1_val = sum((Yp1_val-Zval(:,2)).^2);

Yp2_val = ArxPred( Zval(:,1), Zval(:,2), theta2gen ,xo2gen);
SSR2_val = sum((Yp2_val-Zval(:,2)).^2);

% output

disp(['Model 1: Theta = ',mat2str(theta1gen,4)]);
disp(['Model 2: Theta = ',mat2str(theta2gen,4)]);
disp(['Model 1: SSR_id = ',num2str(SSR1_id),' SSR_val = ',num2str(SSR1_val)]);
disp(['Model 2: SSR_id = ',num2str(SSR2_id),' SSR_val = ',num2str(SSR2_val)]);

% draw charts
figure(2),
    subplot(2,2,1),
    plot(1:Ni,Zid(:,2),'*',1:Ni,Yp1_id(:),'o')
    title(['Model 1: Identification Data SSR = ',num2str(SSR1_id)])
    legend({'Real','Predicted'});
    xlabel('sample time')
    ylabel('output')
    
    subplot(2,2,2),
    plot(1:Ni,Zval(:,2),'*',1:Ni,Yp1_val(:),'o')
    title(['Model 1: Validation Data SSR = ',num2str(SSR1_val)])
    legend({'Real','Predicted'});
    xlabel('sample time')
    ylabel('output')    
    
    
    subplot(2,2,3),
    plot(1:Ni,Zid(:,2),'*',1:Ni,Yp2_id(:),'o')
    title(['Model 2: Identification Data SSR = ',num2str(SSR2_id)])
    legend({'Real','Predicted'});
    xlabel('sample time')
    ylabel('output')
    
    subplot(2,2,4),
    plot(1:Ni,Zval(:,2),'*',1:Ni,Yp2_val(:),'o')
    title(['Model 2: Validation Data SSR = ',num2str(SSR2_val)])
    legend({'Real','Predicted'});
    xlabel('sample time')
    ylabel('output')        
    
%% Whiteness Test
disp('Whiteness Test');
% identification phase
theta1_w = fminsearch(@(t) Varx1(t,U, Y, xo1), zeros(3,1));
theta1_wGen = [theta1_w; 0];
Yp1_w = ArxPred( U, Y, theta1_wGen, xo1gen);
ep1_w = Yp1_w-Y;
SSR1_w = sum(ep1_w.^2);

theta2_w = fminsearch(@(t) Varx2(t,U, Y, xo1), zeros(3,1));
theta2_wGen = [theta2_w(1); 0; theta2_w(2:3)];
Yp2_w = ArxPred( U, Y, theta2_wGen,xo2gen);
ep2_w = Yp2_w-Y;
SSR2_w = sum(ep2_w.^2);

% test phase
rho1 = 0;
for i = 1:(N-1)
    if (ep1_w(i)*ep1_w(i+1) <= 0)
        rho1 = rho1 + 1; % sign change
    end
end
z1 = (rho1 - N/2)/sqrt(N/4);
pvalue1 = cdf('Normal',z1,0,1);

rho2 =0;
for i = 1:(N-1)
    if (ep2_w(i)*ep2_w(i+1) <= 0)
        rho2 = rho2 + 1; % sign change
    end
end
z2 = (rho2 - N/2)/sqrt(N/4);
pvalue2 = cdf('Normal',z2,0,1);

% output
disp(['Model 1: Theta = ',mat2str(theta1_wGen,4)]);
disp(['Model 2: Theta = ',mat2str(theta2_wGen,4)]);

boxT1 = ['Model 1: SSR = ',num2str(SSR1_w),...
         ' estimator value = ',num2str(z1) ,...
         ' p-value = ',num2str(pvalue1)];
disp(boxT1);

boxT2 = ['Model 2: SSR = ',num2str(SSR2_w),...
         ' estimator value = ',num2str(z2) ,...
         ' p-value = ',num2str(pvalue2)];
disp(boxT2);

% draw chart
figure(3),
    subplot(2,1,1),
    plot(1:N,Y,'*',1:N,Yp1_w,'o')
    title(boxT1);
    legend({'Real','Predicted'});
    xlabel('sample time')
    ylabel('output')
    
    subplot(2,1,2),
    plot(1:N,Y,'*',1:N,Yp2_w,'o')
    title(boxT2);
    legend({'Real','Predicted'});
    xlabel('sample time')
    ylabel('output')    