clear all
close all
clc

% Compare FRFs (direct and cross FRFs)
% in Receptance format in [m/N]

% Huynh Hoai Nam
% hoainam.huynh@umons.ac.be

%%%%%%%%%%%%%%
% User input %
%%%%%%%%%%%%%%

% Base FileName
FilenameExp = 'FRF_Exp_Receptance';
FilenameSimu = 'FRF_Simu_Inertance';
% Complete filename is appended as
% ex: FRF_Exp_Receptance_X_Y

% Bandwidth to plot
MinFreq = 5; % [Hz]
MaxFreq = 30; % [Hz]

% Flags
Exp_Divide_Gravity = false; % Signal is divided by 'g'
Exp_Multiply_Gravity = false; % Signal is multiplied by 'g'
Exp_Inertance = false; % Switch to receptance ?

Simu_Divide_Gravity = false; % Signal is divided by 'g'
Simu_Multiply_Gravity =  true; % Signal is multiplied by 'g' from g/N to m/sē/N
Simu_Inertance = true; % Switch to receptance ?

% Plot FRFs in db
Flag_dB = true;

%%%%%%%%%%%
% Program %
%%%%%%%%%%%

% Gravity constant
g = 9.81;

% Load experimental data (9 FRFs)
fmt = repmat('%f',1,3);
fid = fopen(strcat([FilenameExp,'_X_X.txt']), 'rt');
datacell = textscan(fid, fmt, 'Delimiter', ',', 'HeaderLines', 54, 'CollectOutput', 1);
fclose(fid);
ExpRawData = datacell{1};
clear datacell
ExpFrequency = ExpRawData(:,1);
ExpOmega = ExpRawData(:,1) * 2.0 * pi;

MinFreqIndexExp = max(find(ExpFrequency(:,1)<=MinFreq));
MaxFreqIndexExp = min(find(ExpFrequency(:,1)>=MaxFreq));

H_exp = zeros(size(ExpFrequency,1),9);
string_iFRF = '';
string_jFRF = '';
for iFRF = 1 : 3
    
    switch iFRF
        case 1
            string_iFRF = 'X';
        case 2
            string_iFRF = 'Y';
        case 3
            string_iFRF = 'Z';
    end
    
    for jFRF = 1 : 3
        
        switch jFRF
            case 1
                string_jFRF = 'X';
            case 2
                string_jFRF = 'Y';
            case 3
                string_jFRF = 'Z';
        end
        
        CurrentFilename = strcat([FilenameExp,'_',string_iFRF,'_',string_jFRF,'.txt']);
        
        fmt = repmat('%f',1,3);
        fid = fopen(CurrentFilename, 'rt');
        datacell = textscan(fid, fmt, 'Delimiter', ',', 'HeaderLines', 54, 'CollectOutput', 1);
        fclose(fid);
        ExpRawData = datacell{1};
        clear datacell
        
        % Load FRF
        H_exp(:, (iFRF-1)*3 + jFRF ) = complex(ExpRawData(:,2),ExpRawData(:,3));
        
        if(Exp_Divide_Gravity == true)
            H_exp(:, (iFRF-1)*3 + jFRF ) = H_exp(:, (iFRF-1)*3 + jFRF )./g;
        end
        
        if(Exp_Multiply_Gravity == true)
            H_exp(:, (iFRF-1)*3 + jFRF ) = H_exp(:, (iFRF-1)*3 + jFRF ).*g;
        end
        
        if(Exp_Inertance == true)
            H_exp(:, (iFRF-1)*3 + jFRF ) = H_exp(:, (iFRF-1)*3 + jFRF ) ./ (ExpOmega).^2;
        end
        
    end
    
end

% Load simulated data (9 FRFs)
fmt = repmat('%f',1,3);
fid = fopen(strcat([FilenameSimu,'_X_X.txt']), 'rt');
datacell = textscan(fid, fmt, 'Delimiter', ',', 'HeaderLines', 54, 'CollectOutput', 1);
fclose(fid);
SimuRawData = datacell{1};
clear datacell
SimuFrequency = SimuRawData(:,1);
SimuOmega = SimuRawData(:,1) * 2.0 * pi;

MinFreqIndexSimu = max(find(SimuFrequency(:,1)<=MinFreq));
MaxFreqIndexSimu = min(find(SimuFrequency(:,1)>=MaxFreq));

H_simu = zeros(size(SimuFrequency,1),9);

for iFRF = 1 : 3
    
    switch iFRF
        case 1
            string_iFRF = 'X';
        case 2
            string_iFRF = 'Y';
        case 3
            string_iFRF = 'Z';
    end
    
    for jFRF = 1 : 3
        
        switch jFRF
            case 1
                string_jFRF = 'X';
            case 2
                string_jFRF = 'Y';
            case 3
                string_jFRF = 'Z';
        end
        
        CurrentFilename = strcat([FilenameSimu,'_',string_iFRF,'_',string_jFRF,'.txt']);
        
        fmt = repmat('%f',1,3);
        fid = fopen(CurrentFilename, 'rt');
        datacell = textscan(fid, fmt, 'Delimiter', ',', 'HeaderLines', 54, 'CollectOutput', 1);
        fclose(fid);
        SimuRawData = datacell{1};
        clear datacell
        
        % Load FRF
        H_simu(:, (iFRF-1)*3 + jFRF ) = complex(SimuRawData(:,2),SimuRawData(:,3));
        
        if(Simu_Divide_Gravity == true)
            H_simu(:, (iFRF-1)*3 + jFRF ) = H_simu(:, (iFRF-1)*3 + jFRF )./g;
        end
        
        if(Simu_Multiply_Gravity == true)
            H_simu(:, (iFRF-1)*3 + jFRF ) = H_simu(:, (iFRF-1)*3 + jFRF ).*g;
        end
        
        if(Simu_Inertance == true)
            H_simu(:, (iFRF-1)*3 + jFRF ) = H_simu(:, (iFRF-1)*3 + jFRF ) ./ (SimuOmega).^2;
        end
        
    end
    
end

% Display all FRFs
h=figure(1);
set(h, 'Position', [100, 100, 800, 800]); 
for iFRF = 1 : 3
    
    for jFRF = 1 : 3
        subplot(3,3, (iFRF-1)*3 + jFRF )
        plot(ExpFrequency(MinFreqIndexExp:MaxFreqIndexExp,1),abs(H_exp(MinFreqIndexExp:MaxFreqIndexExp,(iFRF-1)*3 + jFRF)),'k',...
             SimuFrequency(MinFreqIndexSimu:MaxFreqIndexSimu,1),abs(H_simu(MinFreqIndexSimu:MaxFreqIndexSimu,(iFRF-1)*3 + jFRF)),'--k')
        
        % Measured only
        % plot(ExpFrequency(MinFreqIndexExp:MaxFreqIndexExp,1),abs(H_exp(MinFreqIndexExp:MaxFreqIndexExp,(iFRF-1)*3 + jFRF)),'k') 
         
        % Simulated only
        % plot(SimuFrequency(MinFreqIndexSimu:MaxFreqIndexSimu,1),abs(H_simu(MinFreqIndexSimu:MaxFreqIndexSimu,(iFRF-1)*3 + jFRF)),'--k')
        
        ylabel('Amplitude [m/N]')
        xlabel('Frequency [Hz]')
        xlim([MinFreq MaxFreq])
        
        % Set same limits for cross FRFs
        if( ((iFRF == 1) && (jFRF == 2)) || ((iFRF == 2) && (jFRF == 1)) )
            ylim([0 3.1e-5])
        end
        
        if( ((iFRF == 1) && (jFRF == 3)) || ((iFRF == 3) && (jFRF == 1)) )
            ylim([0 3.1e-5])
        end
        
        if( ((iFRF == 2) && (jFRF == 3)) || ((iFRF == 3) && (jFRF == 2)) )
            ylim([0 3.1e-5])
        end
        
        if( ((iFRF == 1) && (jFRF == 1)) )
            ylim([0 3.1e-5])
        end
        
        if( ((iFRF == 2) && (jFRF == 2)) )
            ylim([0 6.1e-5])
        end
        
        if( ((iFRF == 3) && (jFRF == 3)) )
            ylim([0 4.1e-5])
        end
        
        %ylim([0 1.3e-4])
    end
    
end

if(Flag_dB==true)
    
    % Display all FRFs
    h=figure(2);
    set(h, 'Position', [100, 100, 800, 800]); 
    for iFRF = 1 : 3

        for jFRF = 1 : 3
            subplot(3,3, (iFRF-1)*3 + jFRF )
            plot(ExpFrequency(MinFreqIndexExp:MaxFreqIndexExp,1),20*log10(abs(H_exp(MinFreqIndexExp:MaxFreqIndexExp,(iFRF-1)*3 + jFRF))),'k',...
                 SimuFrequency(MinFreqIndexSimu:MaxFreqIndexSimu,1),20*log10(abs(H_simu(MinFreqIndexSimu:MaxFreqIndexSimu,(iFRF-1)*3 + jFRF))),'--k')


            ylabel('Amplitude [m/N]')
            xlabel('Frequency [Hz]')
            xlim([MinFreq MaxFreq])

%             % Set same limits for cross FRFs
%             if( ((iFRF == 1) && (jFRF == 2)) || ((iFRF == 2) && (jFRF == 1)) )
%                 ylim([0 0.8e-5])
%             end
% 
%             if( ((iFRF == 1) && (jFRF == 3)) || ((iFRF == 3) && (jFRF == 1)) )
%                 ylim([0 5e-5])
%             end
% 
%             if( ((iFRF == 2) && (jFRF == 3)) || ((iFRF == 3) && (jFRF == 2)) )
%                 ylim([0 1.2e-5])
%             end

            %ylim([0 1.3e-4])
        end

    end
    
end
