clear
close all

%% Selected file
path_series = '.';
series_name = '1C_5C_WavesDir_goodgains_U8';
testnumbers = [302, 303];

% Plot full scale or model scale
plotfullscale = true;
if plotfullscale
    scale = 40;
else
    scale = 1;
end

%% NOTE:
% -----
% The measured data is given at model scale in the TestData. The time vector is model scale.
% The simulated data (data family "OpenFAST") is given at full scale).
% By convention, the time vector in TestData is the same for all families,
% despite the real-time simulation being run at full-scale in the SIL setup.

%% Read data and apply Froude scaling law
Res = {};
leg = {};
for irun = 1:length(testnumbers)
    filename = fullfile(path_series, series_name, sprintf('FLOATECH_C3_%03d.mat', testnumbers(irun)));
    Res{irun} = load(filename, 'TestData');
    leg{irun} = ['Test no. ' num2str(testnumbers(irun))];
    
    % Apply Froude scaling law
    if plotfullscale
        Res{irun}.TestData.time = Res{irun}.TestData.time * sqrt(scale);
        for ichannel = 1:length(Res{irun}.TestData.chanNames)
            if ~strcmp(Res{irun}.TestData.family{ichannel}, 'FAST') % FAST data is already at full scale
                switch Res{irun}.TestData.chanUnits{ichannel}
                    case 'm/s2'
                        scalingfactor = scale^0;
                    case 'm/s'
                        scalingfactor = scale^0.5;
                    case 'm'
                        scalingfactor = scale^1;
                    case 'deg'
                        scalingfactor = scale^0;
                    case 'N'
                        scalingfactor = scale^3;
                    case 'N.m'
                        scalingfactor = scale^4;
                    case 'deg/s'
                        scalingfactor = scale^-0.5;
                    otherwise
                        scalingfactor = 1;
                end
                Res{irun}.TestData.data(:,ichannel) = Res{irun}.TestData.data(:,ichannel) * scalingfactor;
            end
        end
    end
end

%% Plot
% axis limits
% tmin_plt = 0;
% tmax_plt = 1000;


% Wave elevation
figure
ax(1) = gca; hold all
%
for irun = 1:length(testnumbers)
    plot(ax(1), Res{irun}.TestData.time, Res{irun}.TestData.data(:, Res{irun}.TestData.II.WG03))
end
%
sgtitle('Wave elevation')
ylabel(ax(1), [strrep(Res{1}.TestData.chanNames{Res{1}.TestData.II.BG2_Fx}, '_', ' ') ' ('  Res{1}.TestData.chanUnits{Res{1}.TestData.II.BG2_Fx} ')'])
xlabel(ax(1), 'Time (s)')
% xlim(ax(1),[tmin_plt tmax_plt])
lgd = legend(leg);
% lgd.Layout.Tile = 'south';
lgd.NumColumns = 3;
for i=1:1
    grid(ax(i), 'on'); box(ax(i), 'on');
end




% Platform positions
figure('Position', [440,100,600,650])
tiledlayout(3,2)
for n=1:6
    %     ax(n) = subplot(2,1,n); hold all
    ax(n) = nexttile; hold all
end
%
for irun = 1:length(testnumbers)
    plot(ax(1), Res{irun}.TestData.time, Res{irun}.TestData.data(:, Res{irun}.TestData.II.PlatformX))
    %
    plot(ax(3), Res{irun}.TestData.time, Res{irun}.TestData.data(:, Res{irun}.TestData.II.PlatformPitch))
    %
    plot(ax(5), Res{irun}.TestData.time, Res{irun}.TestData.data(:, Res{irun}.TestData.II.PlatformPitch))
    %
    plot(ax(2), Res{irun}.TestData.time, Res{irun}.TestData.data(:, Res{irun}.TestData.II.PlatformPitch))
    %
    plot(ax(4), Res{irun}.TestData.time, Res{irun}.TestData.data(:, Res{irun}.TestData.II.PlatformPitch))
    %
    plot(ax(6), Res{irun}.TestData.time, Res{irun}.TestData.data(:, Res{irun}.TestData.II.PlatformPitch))
end
%
sgtitle('Platform motions')
ylabel(ax(1), [strrep(Res{1}.TestData.chanNames{Res{1}.TestData.II.PlatformX}, '_', ' ') ' ('  Res{1}.TestData.chanUnits{Res{1}.TestData.II.PlatformX} ')'])
ylabel(ax(3), [strrep(Res{1}.TestData.chanNames{Res{1}.TestData.II.PlatformY}, '_', ' ') ' ('  Res{1}.TestData.chanUnits{Res{1}.TestData.II.PlatformY} ')'])
ylabel(ax(5), [strrep(Res{1}.TestData.chanNames{Res{1}.TestData.II.PlatformZ}, '_', ' ') ' ('  Res{1}.TestData.chanUnits{Res{1}.TestData.II.PlatformZ} ')'])
ylabel(ax(2), [strrep(Res{1}.TestData.chanNames{Res{1}.TestData.II.PlatformRoll}, '_', ' ') ' ('  Res{1}.TestData.chanUnits{Res{1}.TestData.II.PlatformRoll} ')'])
ylabel(ax(4), [strrep(Res{1}.TestData.chanNames{Res{1}.TestData.II.PlatformPitch}, '_', ' ') ' ('  Res{1}.TestData.chanUnits{Res{1}.TestData.II.PlatformPitch} ')'])
ylabel(ax(6), [strrep(Res{1}.TestData.chanNames{Res{1}.TestData.II.PlatformYaw}, '_', ' ') ' ('  Res{1}.TestData.chanUnits{Res{1}.TestData.II.PlatformYaw} ')'])
xlabel(ax(5), 'Time (s)')
xlabel(ax(6), 'Time (s)')
linkaxes(ax(1:6), 'x')
% xlim(ax(1),[tmin_plt tmax_plt])
lgd = legend(leg);
lgd.Layout.Tile = 'south';
lgd.NumColumns = 3;
for i=1:6
    grid(ax(i), 'on'); box(ax(i), 'on');
end


% Tower base loads
figure('Position', [440,100,600,650])
tiledlayout(3,2)
for n=1:6
    %     ax(n) = subplot(2,1,n); hold all
    ax(n) = nexttile; hold all
end
%
for irun = 1:length(testnumbers)
    plot(ax(1), Res{irun}.TestData.time, Res{irun}.TestData.data(:, Res{irun}.TestData.II.BG2_Fx))
    %
    plot(ax(3), Res{irun}.TestData.time, Res{irun}.TestData.data(:, Res{irun}.TestData.II.BG2_Fy))
    %
    plot(ax(5), Res{irun}.TestData.time, Res{irun}.TestData.data(:, Res{irun}.TestData.II.BG2_Fz))
    %
    plot(ax(2), Res{irun}.TestData.time, Res{irun}.TestData.data(:, Res{irun}.TestData.II.BG2_Mx))
    %
    plot(ax(4), Res{irun}.TestData.time, Res{irun}.TestData.data(:, Res{irun}.TestData.II.BG2_My))
    %
    plot(ax(6), Res{irun}.TestData.time, Res{irun}.TestData.data(:, Res{irun}.TestData.II.BG2_Mz))
end
%
sgtitle('Tower base loads')
ylabel(ax(1), [strrep(Res{1}.TestData.chanNames{Res{1}.TestData.II.BG2_Fx}, '_', ' ') ' ('  Res{1}.TestData.chanUnits{Res{1}.TestData.II.BG2_Fx} ')'])
ylabel(ax(3), [strrep(Res{1}.TestData.chanNames{Res{1}.TestData.II.BG2_Fy}, '_', ' ') ' ('  Res{1}.TestData.chanUnits{Res{1}.TestData.II.BG2_Fy} ')'])
ylabel(ax(5), [strrep(Res{1}.TestData.chanNames{Res{1}.TestData.II.BG2_Fz}, '_', ' ') ' ('  Res{1}.TestData.chanUnits{Res{1}.TestData.II.BG2_Fz} ')'])
ylabel(ax(2), [strrep(Res{1}.TestData.chanNames{Res{1}.TestData.II.BG2_Mx}, '_', ' ') ' ('  Res{1}.TestData.chanUnits{Res{1}.TestData.II.BG2_Mx} ')'])
ylabel(ax(4), [strrep(Res{1}.TestData.chanNames{Res{1}.TestData.II.BG2_My}, '_', ' ') ' ('  Res{1}.TestData.chanUnits{Res{1}.TestData.II.BG2_My} ')'])
ylabel(ax(6), [strrep(Res{1}.TestData.chanNames{Res{1}.TestData.II.BG2_Mz}, '_', ' ') ' ('  Res{1}.TestData.chanUnits{Res{1}.TestData.II.BG2_Mz} ')'])
xlabel(ax(5), 'Time (s)')
xlabel(ax(6), 'Time (s)')
linkaxes(ax(1:6), 'x')
% xlim(ax(1),[tmin_plt tmax_plt])
lgd = legend(leg);
lgd.Layout.Tile = 'south';
lgd.NumColumns = 3;
for i=1:6
    grid(ax(i), 'on'); box(ax(i), 'on');
end


% OpenFAST RT simulation
clear ax
figure('Position', [440,0,600,750]);
tiledlayout(5, 1)
for n=1:5
%     ax(n) = subplot(2,1,n); hold all
    ax(n) = nexttile; hold all
end
% leg = {};
for irun = 1:length(testnumbers)
    plot(ax(1), Res{irun}.TestData.time, Res{irun}.TestData.data(:,Res{irun}.TestData.II.OF_Wind_Speed_x))
    %
    plot(ax(2), Res{irun}.TestData.time, Res{irun}.TestData.data(:,Res{irun}.TestData.II.OF_Generator_Power))
    %
    plot(ax(3), Res{irun}.TestData.time, Res{irun}.TestData.data(:,Res{irun}.TestData.II.OF_Rotor_Speed))
    %
    plot(ax(4), Res{irun}.TestData.time, Res{irun}.TestData.data(:,Res{irun}.TestData.II.OF_Collective_Blade_Pitch))
    %
    plot(ax(5), Res{irun}.TestData.time, Res{irun}.TestData.data(:,Res{irun}.TestData.II.OF_Fx))
end
sgtitle('OpenFAST real-time simulation')
ylabel(ax(1), 'Wind speed (m/s)')
ylabel(ax(2), 'Gen power (W)')
ylabel(ax(3), 'RPM')
ylabel(ax(4), 'Blade pitch (deg)')
ylabel(ax(5), 'Axial thrust (N)')

% set(ax(1), 'YScale', 'log')
% set(ax(2), 'YScale', 'log')
% set(ax(3), 'YScale', 'log')
% set(ax(4), 'YScale', 'log')
% set(ax(5), 'YScale', 'log')

xlabel(ax(5), 'time (s)');

lgd = legend(leg);
lgd.Layout.Tile = 'south';

for i=1:5
    grid(ax(i), 'on'); box(ax(i), 'on');
end

linkaxes(ax(1:5), 'x')
% xlim(ax(1),[tmin_plt tmax_plt])