%% The following pointers will explain how the code works.
%  All required variables that need to be altered are listed in the next
%  header under 'User Inputs'.

%% Algorithm procedure:
% ----------------------------------------------------------------------
% 1. Select the desired measured shaft line torque.
%    a. Determine which case to analyse:
%       Case_No = '1';     % 1 - Case 1, 2 - Case 2, ...
%    b. Ensure the Shaft Torque data for this case is within the same
%       directory as the current algorithm.
% ----------------------------------------------------------------------
% 2. The Unit Response for the selected data is determined.
% ----------------------------------------------------------------------
% 3. Inverse Method is used to determine the ice induced moment on the
%    propeller from shaft line measurements through TSVD, TGSVD and
%    Tikhonov regularization methods.

% The output data is stored under the following file names:
% - IceImpactExt_TSVD.m (Ice induced torque determined through TSVD)
% - IceImpactExt_TGSVD.m (Ice induced torque determined through TGSVD)
% - IceImpactExt_Tikh.m (Ice induced torque determined through Tikhonov)

%% ========================User input==================================
Case_No = '1';     % 1 - Case 1, 2 - Case 2, ...

%% ======================Initialize values=============================
Beta = 0.25;                % Newmark-Beta constants
Gamma = 0.5;                % Newmark-Beta constants

Fs = 600;                   % Sampling frequency
Dt = 1/Fs;                  % Size of time increments
C_a = zeros(13,1);          % Variable for elements
C_b = zeros(13,1);          % Variable for elements
D_Theta = zeros(13,1);      % Change in angular displacement
D_Theta_d = zeros(13,1);    % Change in angular velocity
D_Theta_dd = zeros(13,1);   % Change in angular acceleration

% Load required data from file
Data_File = sprintf('%s%s%s','Case',Case_No,'_ShaftTorque.mat');
Data_Meas = load(Data_File);
Data_Torq = Data_Meas.ShaftTorq_ExHydro;
Data_Time = Data_Meas.Time;
T_final = round(length(Data_Meas.Time)/Fs,1); % Determine length of data

% Allocate shaft torque element measured by strain gauges
Index = 15;      % 1 = T_Prop; 2 = Theta_Prop; 3 = T_shaft1;
% 4 = Theta_shaft1...

%%%%%%%%%%Rolls Royce Data%%%%%%%%%%%
% Mass moment of inertia [kg.m^2]
J_1 = 1.347e4;    	% CPP
J_3 = 5.59e2;     	% Mid propeller shaft
J_5 = 5.12e2;     	% Sleeve coupling
J_7 = 4.87e2;     	% OD box flange
J_9 = 1.41e2;     	% Thrust shaft collar
J_11 = 1.74e2;    	% Electric motor flange
J_13 = 4.415e3;   	% Propulsion motor

% Damping [Nm.s/rad]
C_1 = 0;		% CP Propeller
C_shaft = 0;		% Shaft line damping

% Torsional stiffness [Nm/rad]
K_2 = 5.88e7;     	% Normal steel shaft
K_4 = 5.95e7;     	% Normal steel shaft
K_6 = 1.12e8;     	% Normal steel shaft
K_8 = 6.93e8;     	% Normal steel shaft
K_10 = 5.09e8;    	% Normal steel shaft
K_12 = 1.43e8;    	% Normal steel shaft

J = [J_1; 0;  J_3; 0; J_5; 0; J_7; 0; J_9; 0; J_11; 0; J_13];
C = [C_1; C_shaft; 0; C_shaft; 0; C_shaft; 0; C_shaft; 0; C_shaft;...
    0; C_shaft; 0];
K = [0; K_2; 0; K_4; 0; K_6; 0; K_8; 0; K_10; 0; K_12; 0];
clear var K_2 K_4 K_6 K_8 K_10 K_12
clear var J_1 J_3 J_5 J_7 J_9 J_11 J_13
clear var C_1 C_shaft

%% ======================Initial conditions============================
G = zeros(T_final*Fs+1);      % Impulse response matrix
Z = zeros(28,T_final*Fs+1);   % Torque and angular displacement
Qice = zeros(T_final*Fs+1,1); % Load vector (External moment)
Theta_d = zeros(13,1);        % Change in angular velocity
Theta_dd = zeros(13,1);       % Change in angular acceleration
Theta_d_Pre = zeros(13,1);    % Save previous angular velocity
Theta_dd_Pre = zeros(13,1);   % Save previous angular acceleration
D_T_Ext = zeros(13,1);        % Change in ext. torque
W_sub = zeros(26,4);          % Sub-matrices per element
W_pre = zeros(26,28);         % Sub-matrix of sub-matrices along main diag.
W_first = zeros(1,28);        % First node boundary condition
W_first(1) = 1;
W_last = zeros(1,28);         % Last node boundary condition
W_last(27) = 1;
Time = zeros(1,T_final*Fs+1); % Time vector

for Determine = 1:2 % First determine unit response, then ice induced moments
    for Reg = 1:3   % Determine ice induced moment using 3 regularization methods
        %% ========Forward problem to determine Unit response function=========
        % Only enter loop for full-scale data if ice-induced file has been created
        if exist('IceImpactExt.mat','file') == 2 || Determine == 1
            % Variable in which to store value for loop
            Confirm = 0;	% Initiate variable to zero
            % load data from ice-induced file
            if exist('IceImpactExt.mat','file') == 2
                Test = load('IceImpactExt.mat');
                if length(Test.Qice) == length(Time)
                    Confirm = 1;
                end
            end
            % Confirm that ice-induced data has been obtained from current data
            % through comparison of length of time
            if Confirm == 1 || Determine == 1
                for t = 1:T_final*Fs + 1
                    %============Measured internal-external load=========
                    if Determine == 2
                        Data_MeasExt = load('IceImpactExt.mat');
                        IceMom = Data_MeasExt.Qice(t);
                    else
                        %==================Unit step input======================
                        if t == 1
                            D_T_Ext(1) = 1; % Only apply unit step input to prop.
                        else
                            D_T_Ext(1) = 0;
                        end
                    end
                    %=================Determine change in impulse===============
                    if Determine == 2
                        if t == 1
                            D_T_Ext(1) = IceMom;
                        else
                            D_T_Ext(1) = IceMom - Pre_IceMom;
                        end
                        Pre_IceMom = IceMom;
                    end
                    
                    % Loop through each element
                    for i = 1:13
                        % Test for odd or even
                        if mod(i,2) == 0     % Even (Spring)
                            C_a(i) = K(i) + (C(i)*Gamma)/(Beta*Dt);
                            C_b(i) = C(i)*((Gamma/Beta)*(Theta_d_Pre(i)-Theta_d(i)) + ...
                                ((Gamma/(2*Beta))-1)*Dt*(Theta_dd_Pre(i)-Theta_dd(i)));
                            W_sub((i*2-1):(i*2),:) = [1 C_a(i) 0 -C_a(i); 1 0 -1 0];
                        else                 % Odd (Inertia)
                            C_a(i) = J(i)/(Beta*Dt^2) + (C(i)*Gamma)/(Beta*Dt);
                            C_b(i) = J(i)*((1/(Beta*Dt))*Theta_d(i)+(1/(2*Beta))*...
                                Theta_dd(i)) + C(i)*((Gamma/Beta)*Theta_d(i)+...
                                (Gamma/(2*Beta)-1)*Theta_dd(i)*Dt) + D_T_Ext(i);
                            W_sub((i*2-1):(i*2),:) = [1 C_a(i) -1 0; 0 1 0 -1];
                        end
                        % Create W matrix and determine change in Torque and theta
                        W_pre(2*i-1:2*i,2*i-1:2*i+2) = W_sub((i*2-1):(i*2),:);
                    end
                    W = [W_first; W_pre; W_last];
                    B = [0; C_b(1); 0; C_b(2); 0; C_b(3); 0; C_b(4); 0; C_b(5); 0; ...
                        C_b(6); 0; C_b(7); 0; C_b(8); 0; C_b(9); 0; C_b(10); 0; ...
                        C_b(11); 0; C_b(12); 0; C_b(13); 0; 0];
                    [L,U] = lu(W);
                    y = L\B;
                    D_Z = U\y;
                    
                    clear var L U y W B
                    
                    %% Store angular displacement in matrix
                    for q = 1:13
                        D_Theta(q) = D_Z(2*q);
                    end
                    
                    %% Store torque values in matrix
                    if t == 1
                        Z(:,t) = 0 + D_Z;
                    else
                        Z(:,t) = Z(:,t-1) + D_Z;
                    end
                    g_ind = Z(Index,t)*ones(1,length(G)-(t-1));    % Determine TF between
                    % propeller and shaft line measurement location
                    G = G + diag(g_ind,(1-t));
                    Time(1,t) = (t-1)/Fs;
                    % Update variables for each element
                    for i = 1:13
                        D_Theta_dd(i) = (1/(Beta*Dt^2))*D_Theta(i)-(1/(Beta*Dt))*...
                            Theta_d(i)-(1/(2*Beta))*Theta_dd(i);
                        D_Theta_d(i) = (Gamma/(Beta*Dt))*D_Theta(i)-(Gamma/Beta)*...
                            Theta_d(i)-(Gamma/(2*Beta)-1)*Theta_dd(i)*Dt;
                        % Update variables
                        Theta_d_Pre(i) = Theta_d(i);
                        Theta_dd_Pre(i) = Theta_dd(i);
                        Theta_dd(i) = Theta_dd(i) + D_Theta_dd(i);
                        Theta_d(i) = Theta_d(i) + D_Theta_d(i);
                    end
                end
                
                % Save unit response function
                if Determine == 1
                    save('ImpulseResp','G');
                end
            end
        end
        
        H_struct = load('ImpulseResp.mat'); % impulse response for step input, no dev.
        H = H_struct.G;
        
        %% ===========================Measured data ===========================
        if Determine == 2
            % Load required data from file
            Data_File = sprintf('%s%s%s','Case',Case_No,'_ShaftTorque.mat');
            Data = load(Data_File);
            Data_Meas = Data.ShaftTorq_ExHydro;
            Data_Time = Data.Time;
            
            Resp = Data_Meas(1:T_final*600+1);
            Time = Data_Time(1:T_final*600+1);
        end
        
        %% ===========================Inverse methods==========================
        %===================TSVD========================
        if Reg == 1 && Determine == 2
            [L,~] = get_l(length(Time),1);	% Generate L matrix
            [U,s,V] = csvd(H);			% Determines the compact form of the TSVD
            %reg_corner
            [D_Qice,rho,eta] = tsvd(U,s,V,Resp,120);	% Determines the TSVD
            % solution
            
            % Determine ext. moment from change in ext. moment
            for q = 1:length(D_Qice)
                if q == 1
                    Qice(q) = 0 + D_Qice(q);  % Update external moment on propeller
                else
                    Qice(q) = Qice(q-1) + D_Qice(q);
                end
            end
            figure (5)
            if Determine == 1
                plot(Time,Ref_M,'k');
            end
            hold on;
            Qice = -Qice; %invert data
            plot(Time,Qice,'k');
            hold on;
            xlim([0 max(Time)]);
            ylabel('External moment Q_{ice} [kNm]'); xlabel('Time [s]');
            %     legend('Ref. moment','TSVD')
            if Determine == 2
                % save external moment from measured internal
                save('IceImpactExt.mat','Time','Qice');
                Qice_TSVD = Qice;
                save('IceImpactExt_TSVD.mat','Time','Qice_TSVD');
            end
            
            %===================TGSVD========================
        elseif Reg == 2 && Determine == 2
            [L,~] = get_l(length(Time),1);	% Generate L matrix
            [U,sm,X,V,W] = cgsvd(H,L);		% Determines the compact form of the TGSVD
            [D_Qice,rho,eta] = tgsvd(U,sm,X,Resp,120);
            % Determines the TGSVD solution
            
            % Determine ext. moment from change in ext. moment
            for q = 1:length(D_Qice)
                if q == 1
                    Qice(q) = 0 + D_Qice(q);  % Update external moment on propeller
                else
                    Qice(q) = Qice(q-1) + D_Qice(q);
                end
            end
            figure (5)
            %     plot(Time,Ref_M,'k');
            hold on;
            Qice = -Qice; %invert data
            plot(Time,Qice,'r');
            xlim([0 max(Time)]);
            ylabel('External moment Q_{ice} [kNm]'); xlabel('Time [s]');
            %     legend('Ref. moment','TGSVD')
            % save external moment from measured internal
            save('IceImpactExt.mat','Time','Qice');
            Qice_TGSVD = Qice;
            save('IceImpactExt_TGSVD.mat','Time','Qice_TGSVD');
            
            %=================Tikhonov========================
        elseif Reg == 3 && Determine == 2
            
            [L,~] = get_l(length(Time),1);  % Generate L matrix
            [U,sm,X,V,W] = cgsvd(H,L);  % Determines the compact form of the TGSVD
            [D_Qice,rho,eta] = tikhonov(U,sm,X,Resp,24.57*10^(-2));
            % Determines the Tikhonov regularized solution
            
            % Determine ext. moment from change in ext. moment
            for q = 1:length(D_Qice)
                if q == 1
                    Qice(q) = 0 + D_Qice(q);  % Update external moment on propeller
                else
                    Qice(q) = Qice(q-1) + D_Qice(q);
                end
            end
            
            figure (5)
            %     plot(Time,Ref_M,'k');
            hold on;
            Qice = -Qice; %invert data
            plot(Time,Qice,'b');
            xlim([0 max(Time)]);
            ylabel('Ext. moment Q_{ice} [kNm]'); xlabel('Time [s]');
            
            legend('TSVD','TGSVD','Tikhonov');	% save external moment from measured
            % internal.
            save('IceImpactExt.mat','Qice');
            Qice_Tikh = Qice;
            save('IceImpactExt_Tikh.mat','Time','Qice_Tikh');
            % Save ref. moment variable
            
        end
        if Determine == 1
            break;
        end
    end
end

