%% Bridge relevant numerical example, regularization of Volterra models
%
%
% This file generates data, trains the regularized Volterra-models and plots
% the different plots according to:
% 
%  "Bridge relevant numerical example"
% 
% Please site the following paper when using this code:  
% "Modelling of nonlinear self-excited forces with regularized Volterra
% series models" H.Skyvulstad, Ø.Petersen, T.Argentini, A. Zasso, O. Øiseth
% 
% 
%VolterraRegularization is a free software: you can use, distribute and
%modyfi at will under the terms of the GNU General public license as published
%by the Free Software Foundation, either version 3 of the license, or (at your option)
%any later version. 
%
%VolterraRegularization is distributed because it might be usefull for others,
%but it comes without any form of WARRANTY; without even the implied 
%warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 
%See the GNU General Public License for more details.
%
%A copy of the license is distributed with the code, and can be found here:
%https://www.gnu.org/licenses/.
%
% A regularization process involves choosing a Lambda factor(or two) dependent on the order and type of regularization. 
% This means that one needs a two-step procedure for doing the calculations. 
% 1. Run regularization with an interval of different Lambda Values
%       Note that the single L-curve is plotted automatically in the script.
%       By running the Volterra_model.FindLambda function. But the Double
%       L-curve is plotted lower in the script by using information saved
%       on the Volterra_model properties. 
% 2. Choose Lambda Values based on the different L-curves and double L-curves
% and plot/predicate with the chosen values. 
% 
%
% By, PhD-candidate Henrik Skyvulstad, 2021-03-10
% henrik.skyvulstad@norconsult.com / henrik.skyvulstad@ntnu.no

%%        INPUTS         %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clc 
close all
clear


M = 25;                                                                    %Memory, int
Order = 2;                                                                 %Order, p
ItterateLambda = false;                                                    %Boolean(true or false), decides if we are going to itterate and generate L and double LL curve
LambdaIntervalReg1 = 10.^([-16:1:8]);                                      %Interval to round the L-curve over(is not used for the double L curve). For Tikhonov regularization
LambdaDecayInterval1 = 10.^([-11:1:9]);                                    %Interval axis 1 for the Double L curve for the decay regularization(Vector). 
LambdaDecayInterval2 = 10.^([-11:1:9]);                                    %Interval axis 2 for the Double L curve for the decay regularization(Vector). 
Sym = true;                                                                %Boolean(True or false), if one reduced the regularization due to symmetry
d = 3;                                                                     % d-value for the rational function in time domain. Se cited paper:
V = 1;                                                                     % V-value for the rational function in time domain. Se cited paper:
B = 1;                                                                     % B-value for the rational function in time domain. Se cited paper:
a1 = 1;                                                                    % a1 value for scaling of the linear rational function. Se the cited paper. 
a2 = 0.1;                                                                   % a2 value for scaling of the 2nd-order scaling rational function. Se the cited paper. 
SNR = 0.1;                                                                 % Signal to noise ratio, based on variances on the signals
dt = 0.1;                                                                   % time-increment
TraininSignalLength = 10000;                                                %Training data length (also validation data lengths)
inputType = 'Pink';      %'Pink' or 'White'                                 %Distribution the training data comes from                                                 
validType = 'Pink';        %'Pink' or 'White'                              %Distribution of the validation data
noiseType = 'Pink';        %'Pink' or 'White'                              %Distribution of the noise data



%Lambda values for different models. Reg0 / 1 is Tikhonov regularizations,
%Decay = decay type regularization. Two separate model categories for clean
%and noisy training data. 
LambdaReg0Clean = 1e-14;                                           
LambdaReg1Clean = 1e-14;
LambdaReg0Noisy = 1e+2;
LambdaReg1Noisy = 4e+2;
LambdaReg1NoisyHig = 1e+4;
LambdaReg1NoisyLow = 5e+0;
LambdaDecay1 = 1e+4;                     %Note that decay-regularization uses two inputs Lambda 1st-order kernel regularization
LambdaDecay2 = 1e+3;                     %Decay regularization for the 2nd-order kernel




%%
%Generating the analytical kernels
t2 = 0:dt:dt*(M+1);        
for i = 1:1:M+1
    H1(i) = -d*V/B*exp((-d*V/B)*t2(i));
    for j = 1:1:M+1 
    H2(i,j) = a2*(-d*V/B*exp((-d*V/B)*t2(i))*-d*V/B*exp((-d*V/B)*t2(j)));
    end
end     


%% Generate Noise and Input


if strcmp(inputType,'White')
	Input = wgn(TraininSignalLength,1,0)/2;    
elseif strcmp(inputType,'Pink')
    hcn = dsp.ColoredNoise('InverseFrequencyPower',1,'SamplesPerFrame',TraininSignalLength);
    Input = step(hcn);
    Input = Input * 25/180;
end

if strcmp(validType,'White')
	ValidationInput = wgn(TraininSignalLength,1,0)/2;   
elseif strcmp(validType,'Pink')
    hcn1 = dsp.ColoredNoise('InverseFrequencyPower',1,'SamplesPerFrame',TraininSignalLength);
    ValidationInput = step(hcn1);
    ValidationInput = ValidationInput * 25/180;
end

if strcmp(noiseType,'White')
	Noise = wgn(TraininSignalLength,1,0)/2;   
elseif strcmp(noiseType,'Pink')
    hcn3 = dsp.ColoredNoise('InverseFrequencyPower',1,'SamplesPerFrame',TraininSignalLength);
    Noise = step(hcn3);
    Noise = Noise * 25/180;
end

%Generate training data
Data.RegMatrix = MakeRegressionMatrix(Input,M,1);
F1 = Data.RegMatrix*H1';
F_clean = a1*F1 + a2*F1.^2;


%Scaling the noise according to the variances of the signals
VarOut = var(F_clean);
VarNoise = var(Noise);
F_noisy = F_clean + Noise * sqrt((VarOut/VarNoise))*SNR;

% Generate Validation data
ValidationData.RegMatrix = MakeRegressionMatrix(ValidationInput,M,1);
F1Valid = ValidationData.RegMatrix*H1';
F_Valid = a1*F1Valid + a2*F1Valid.^2;


%% Making the Volterra-models

%Initializing  the Volterra Objects
VReg0Clean = Volterra_model;
VReg0Noisy  = Volterra_model;
VReg1Clean = Volterra_model;
VReg1Noisy = Volterra_model;
VReg1NoisyHigh = Volterra_model;
VReg1NoisyLow = Volterra_model;
VLSQClean = Volterra_model;
VLSQNoisy = Volterra_model;
VDecayNoisy = Volterra_model;



if ItterateLambda         %Function to generate L and double L-curves
 
%FindLambda(Input,Output,Memory,Order,LambdaIntReg,LambdeDecayInt1,LambdeDecayInt2,Sym,type)
%Input = Training input(motion)
%Output = Training output(force)
%Order = model order(1,2,3) ps. max 2 for decay type
%LambdaIntReg = The intervall Lambda is looped over, for Tikhonov regularization(1st and 0th)
%LambdeDecayInt1 = The interval Lambda decay is looped over, axis 1
%LambdeDecayInt2 = The interval Lambda decay is looped over, axis 2
%Sym = true/false. If one uses symmetry reduction (true false). 
%RegType = 'Reg1', 'Reg0' or 'decay' for 1st order Tikhonov, 0th order Tikhonov or decay type regularization

    VReg0Clean.FindLambda(Input,F_clean,M,Order,LambdaIntervalReg1,LambdaDecayInterval1,LambdaDecayInterval2,Sym,'Reg0')
    VReg0Noisy.FindLambda(Input,F_noisy,M,Order,LambdaIntervalReg1,LambdaDecayInterval1,LambdaDecayInterval2,Sym,'Reg0')
    VReg1Clean.FindLambda(Input,F_clean,M,Order,LambdaIntervalReg1,LambdaDecayInterval1,LambdaDecayInterval2,Sym,'Reg1')
    VReg1Noisy.FindLambda(Input,F_noisy,M,Order,LambdaIntervalReg1,LambdaDecayInterval1,LambdaDecayInterval2,Sym,'Reg1')
    VDecayNoisy.FindLambda(Input,F_noisy,M,Order,LambdaIntervalReg1,LambdaDecayInterval1,LambdaDecayInterval2,Sym,'decay')   
end




%Method for training the Volterra-models given correct Lambda values. Able
%to train with all, some or no regularizations. 
 
%TrainVolterra(Input,Output,Memory,Order,Symmetry,LambdaDecay1,LambdaDecay2,LambdaReg1,LambdaReg0)
%Input = Training input(motion)
%Output = Training output(force)
%Order = model order (1,2,3) ps. max 2 for decay type
%Sym = true/false. If one uses symmetry reduction (true false). 
%LambdaDecay1 = Lambda decay 1st value
%LambdaDecay2 = Lambda decay 2nd value
%LambdaReg1 = Lambda Tikhonov 1st
%LambdaReg0 = Lambda Tikhonov 0th
VReg0Clean.TrainVolterra(Input,F_clean,M,Order,Sym,0,0,0,LambdaReg0Clean)
VReg0Noisy.TrainVolterra(Input,F_noisy,M,Order,Sym,0,0,0,LambdaReg0Noisy)
VReg1Clean.TrainVolterra(Input,F_clean,M,Order,Sym,0,0,LambdaReg1Clean,0)
VReg1Noisy.TrainVolterra(Input,F_noisy,M,Order,Sym,0,0,LambdaReg1Noisy,0)
VReg1NoisyHigh.TrainVolterra(Input,F_noisy,M,Order,Sym,0,0,LambdaReg1NoisyHig,0)
VReg1NoisyLow.TrainVolterra(Input,F_noisy,M,Order,Sym,0,0,LambdaReg1NoisyLow,0)
VLSQClean.TrainVolterra(Input,F_clean,M,Order,Sym,0,0,0,0)
VLSQNoisy.TrainVolterra(Input,F_noisy,M,Order,Sym,0,0,0,0)
VDecayNoisy.TrainVolterra(Input,F_noisy,M,Order,Sym,LambdaDecay1,LambdaDecay2,0,0)




%Method that makes time-realization of independent motion series
%ValidateVolterra(InputValid)
%InputValid = Input validation series
%Dumps results in YPred{end}, Note that training a new model does not
%delete the old, but places it at the end. 
VReg0Clean.ValidateVolterra(ValidationInput);
VReg0Noisy.ValidateVolterra(ValidationInput);
VReg1Clean.ValidateVolterra(ValidationInput);
VReg1Noisy.ValidateVolterra(ValidationInput);
VReg1NoisyHigh.ValidateVolterra(ValidationInput);
VReg1NoisyLow.ValidateVolterra(ValidationInput);
VLSQClean.ValidateVolterra(ValidationInput);
VLSQNoisy.ValidateVolterra(ValidationInput);
VDecayNoisy.ValidateVolterra(ValidationInput);



t = dt:dt:dt*length(ValidationInput);

% Print NMSE-values to check the fits
NMSE_VReg0Clean =       goodnessOfFit(F_Valid,VReg0Clean.YPred{end},'NMSE');
NMSE_VReg0Noisy =       goodnessOfFit(F_Valid,VReg0Noisy.YPred{end},'NMSE');
NMSE_VReg1Clean =       goodnessOfFit(F_Valid,VReg1Clean.YPred{end},'NMSE');
NMSE_VReg1Noisy =       goodnessOfFit(F_Valid,VReg1Noisy.YPred{end},'NMSE');
NMSE_VReg1NoisyHigh  =  goodnessOfFit(F_Valid,VReg1NoisyHigh.YPred{end},'NMSE');
NMSE_VReg1NoisyLow =    goodnessOfFit(F_Valid,VReg1NoisyLow.YPred{end},'NMSE');
NMSE_VLSQClean =        goodnessOfFit(F_Valid,VLSQClean.YPred{end},'NMSE');
NMSE_VLSQNoisy =        goodnessOfFit(F_Valid,VLSQNoisy.YPred{end},'NMSE');
NMSE_VDecayNoisy =      goodnessOfFit(F_Valid,VDecayNoisy.YPred{end},'NMSE');

%Print it in a table structure
NMSE_data = [NMSE_VReg0Clean,NMSE_VReg0Noisy,NMSE_VReg1Clean,NMSE_VReg1Noisy,NMSE_VReg1NoisyHigh,NMSE_VReg1NoisyLow,NMSE_VLSQClean,NMSE_VLSQNoisy,NMSE_VDecayNoisy];
ModelNames = {'VReg0Clean','VReg0Noisy','VReg1Clean','VReg1Noisy','VReg1NoisyHigh','VReg1NoisyLow','VLSQClean','VLSQNoisy','VDecayNoisy'};
VarNames = {'Model','NMSE'};
T = table(NMSE_data',ModelNames','VariableNames',VarNames);
disp(T)


%% %%%%%%%%% Start plotting  %%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Plotting the double LL curve

%If we havent generated the Double L curve, do so
if ItterateLambda
    % Hard koded value of the corner point. This value is choosen by using the
    % Data-cursor toole on the figure. 
    XChosen = 0.032768;
    YChosen = 0.02687;
    ZChosen = 44.7193;


    fig = figure;
    plot3(XChosen,YChosen,ZChosen,'xr','markersize',10,'LineWidth',8)
    hold on
    surf(VDecayNoisy.LambdaNormFullDecay1,VDecayNoisy.LambdaNormFullDecay2,VDecayNoisy.Fit_error_normFull)
    set(gca,'XScale','log');
    set(gca,'YScale','log');
    set(gca,'ZScale','log');
    grid on
    zlabel('norm(r*H-F)')
    xlabel('norm(L_{1^{st},decay}*H)')
    ylabel('norm(L_{2^{nd},decay}*H)')
    legend('Corner point','Location','northeast')
    AA = gcf();
    AA.Children(2).View = [137.8036   24.9843];


    %Code that plots the actual Lambda1 and Lambda2 values from the chosen
    %point. 
    XXChose = abs(VDecayNoisy.LambdaNormFullDecay1-XChosen);
    minimum = min(min(XXChose));
    [Xxind,Xyind]=find(XXChose==minimum);

    YYChose = abs(VDecayNoisy.LambdaNormFullDecay2-YChosen);
    minimum = min(min(YYChose));
    [Yxind,Yyind]=find(YYChose==minimum);

    %This code prints what LambdaDecay values the chosen point is from.
    %This to be able to extract the correct value from the double-L curve
    LambdaDecay1Opt = VDecayNoisy.LambdaDecayInt21(Xxind,Xyind)
    LambdaDecay2Opt = VDecayNoisy.LambdaDecayInt22(Yxind,Yyind)

end

%% Plotting L-curve
if ItterateLambda   %Generating the L-curve data if it is not already generated. 
    figure()
    hold on; grid on;
    plot(VReg1Noisy.Fit_err_norm,VReg1Noisy.LH_norm,'x','MarkerIndices',[4],'color',[0 0.5 0]);
    plot(VReg1Noisy.Fit_err_norm,VReg1Noisy.LH_norm,'+','MarkerIndices',[7],'color',[1 0 0]);
    plot(VReg1Noisy.Fit_err_norm,VReg1Noisy.LH_norm,'*','MarkerIndices',[9],'color',[1 0 1]);
    plot(VReg1Noisy.Fit_err_norm,VReg1Noisy.LH_norm,'-ob','MarkerIndices',[1,2,3,5,6,8,10,11,12,13,14]);
    xlabel('norm(r*H-F)')
    ylabel('norm(L*H)')
    set(gca,'XScale','log');
    set(gca,'YScale','log');
    for j=1:length(VReg1Noisy.LambdaInterval)
        if mod(j,2)==0; HorizontalAlignment='Left'; Spaces1='   '; Spaces2='   ';
        else HorizontalAlignment='right'; Spaces1=''; Spaces2='   ';
        end
        text(VReg1Noisy.Fit_err_norm(j),VReg1Noisy.LH_norm(j),[Spaces1 '\lambda^2=' num2str(VReg1Noisy.LambdaInterval(j),'%0.0e') Spaces2],'Rotation',45,'HorizontalAlignment',HorizontalAlignment);
    end

    AA = gcf();
    AA.Children.Children(5).Color = [1 0 1];
    AA.Children.Children(7).Color = [1 0 0];
    AA.Children.Children(10).Color = [0 0.5 0];
    legend('Low','Best','High')
end
%% Plotting 1st-order kernels

figure()
subplot(1,3,1)
hold on
plot(H1,'--','LineWidth',3)
plot(VReg1Clean.H1)
plot(VLSQClean.H1)
plot(VLSQNoisy.H1)
legend('Analytical','R1,SNR=\infty','LSQ,SNR=\infty','LSQ,SNR=10','interpreter','tex','location','southeast','box','off')
grid on
ylabel('h_1')
xlabel('Memory,(M)')

subplot(1,3,2)
hold on
plot(H1,'--','LineWidth',3)
plot(VReg0Noisy.H1)
plot(VReg1Noisy.H1)
plot(VReg1NoisyHigh.H1)

legend('Analytical','R0,SNR=10','R1,SNR=10,best','R1,SNR=10,high','interpreter','tex','location','southeast','box','off')
grid on
ylabel('h_1')
xlabel('Memory,(M)')

subplot(1,3,3)
hold on
plot(H1,'--','LineWidth',3)
plot(VDecayNoisy.H1)
plot(VReg1NoisyLow.H1)
legend('Analytical','Decay,SNR=10','R1,SNR=10,low','interpreter','tex','location','southeast','box','off')
grid on
ylabel('h_1')
xlabel('Memory,(M)')


%% Plotting 2nd-order Kernels

a = 4;
b = 2;

figure()
subplot(4,2,1)
surf(H2,'EdgeColor','interp')
zlabel('h_2 Analytical')
view(73,31)

subplot(4,2,2)
surf(VReg1Clean.HH2,'EdgeColor','interp')
zlabel('h_2 R1,SNR=\infty','interpreter','tex')
view(73,31)

subplot(4,2,3)
surf(VReg1Noisy.HH2,'EdgeColor','interp')
zlabel('h_2 R1,SNR=10,best','interpreter','tex')
view(73,31)


subplot(4,2,4)
surf(VReg1NoisyHigh.HH2,'EdgeColor','interp')
zlabel('h_2 R1,SNR=10,high','interpreter','tex')
view(73,31)

subplot(4,2,5)
surf(VReg1NoisyLow.HH2,'EdgeColor','interp')
zlabel('h_2 R1,SNR=10,low','interpreter','tex')
view(73,31)


subplot(4,2,6)
surf(VLSQClean.HH2,'EdgeColor','interp')
zlabel('h_2 LSQ,SNR=\infty','interpreter','tex')
view(73,31)


subplot(4,2,7)
surf(VLSQNoisy.HH2,'EdgeColor','interp')
zlabel('h_2 LSQ,SNR=10','interpreter','tex')
view(73,31)
xlabel('Memory,(M_1)')
ylabel('Memory,(M_2)')

subplot(4,2,8)
surf(VDecayNoisy.HH2,'EdgeColor','interp')
zlabel('h_2 Decay,SNR=10','interpreter','tex')
view(73,31)
xlabel('Memory,(M_1)')
ylabel('Memory,(M_2)')

%% Plot the diagonals of the 2nd-order kernels
figure()
hold on
plot(diag(H2),'--','LineWidth',3)
plot(diag(VReg1Noisy.HH2))
plot(diag(VReg1NoisyLow.HH2))
plot(diag(VReg1NoisyHigh.HH2))
plot(diag(VLSQNoisy.HH2))
plot(diag(VDecayNoisy.HH2))
legend('Analytical','R1,SNR=10,best','R1,SNR=10,high','R1,SNR=10,low','LSQ,SNR=10','Decay,SNR=10','box','off') %,'Orientation','horizontal'
ylabel('h_2 diagonal')
xlabel('Memory,(M_1=M_2)')
grid on

%% Plot the time-domain realization of the Validation time-series
figure()
plot(t,F_Valid,t,VReg1Noisy.YPred{end},t,VLSQNoisy.YPred{end},t,VDecayNoisy.YPred{end})
legend('Data','R1,SNR=10,best','LSQ,SNR=10','Decay,SNR=10','box','off','Orientation','horizontal', 'interpreter','tex')
xlim([210 250])
xlabel('Time(sec)')
ylabel('F(N)')
grid on
xlim([223 233])
xlabel('Time(sec)')
ylabel('F(N)')
grid on
