%% Script Description
% Compare identification from the Simscape model
% with the identification on the real system.

%%
clear; close all; clc;

%% Load the obtained transfer functions
load('./mat/id_micro_station.mat', 'G_ms');

%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');

%% Get Measurement Object
load('2018_01_12.mat', 'm_object');

%% Get Measurements Data
opts = struct('freq_min', 10, 'est_backend', 'idfrd');
meas_sys = getDynamicTFs(m_object, 'marble', 'hexa', {{'tx', 'tx'},{'ty', 'ty'},{'tz', 'tz'}}, opts);

%% Granite to Granite
for dir = 'xyz'
    figure;
    % Amplitude
    ax1 = subplot(2,1,1);
    hold on;
    plot(freqs, abs(squeeze(freqresp(G_ms(['Dg' dir], ['Fg' dir]), freqs, 'Hz'))));
    plot(freqs, abs(squeeze(freqresp(meas_sys(['Dm' dir], ['Fm' dir]), freqs, 'Hz'))), '.');
    set(gca,'xscale','log'); set(gca,'yscale','log');
    ylabel('Amplitude [m/N]');
    set(gca, 'XTickLabel',[]);
    legend({'Model', 'Meas.'});
    hold off;
    % Phase
    ax2 = subplot(2,1,2);
    hold on;
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_ms(['Dg' dir], ['Fg' dir]), freqs, 'Hz'))));
    plot(freqs, 180/pi*angle(squeeze(freqresp(meas_sys(['Dm' dir], ['Fm' dir]), freqs, 'Hz'))), '.');
    set(gca,'xscale','log');
    ylim([-180, 180]);
    yticks([-180, -90, 0, 90, 180]);
    xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
    hold off;
    linkaxes([ax1,ax2],'x');

    if save_fig; exportFig(['comp_meas_g_g_' dir], 'normal-normal', struct('path', 'identification')); end
end

%% Hexapod to Hexapod
for dir = 'xyz'
    figure;
    % Amplitude
    ax1 = subplot(2,1,1);
    hold on;
    plot(freqs, abs(squeeze(freqresp(G_ms(['Dm' dir], ['Fm' dir]), freqs, 'Hz'))));
    plot(freqs, abs(squeeze(freqresp(meas_sys(['Dh' dir], ['Fh' dir]), freqs, 'Hz'))), '.');
    set(gca,'xscale','log'); set(gca,'yscale','log');
    ylabel('Amplitude [m/N]');
    set(gca, 'XTickLabel',[]);
    legend({'Model', 'Meas.'});
    hold off;
    % Phase
    ax2 = subplot(2,1,2);
    hold on;
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_ms(['Dm' dir], ['Fm' dir]), freqs, 'Hz'))));
    plot(freqs, 180/pi*angle(squeeze(freqresp(meas_sys(['Dh' dir], ['Fh' dir]), freqs, 'Hz'))), '.');
    set(gca,'xscale','log');
    ylim([-180, 180]);
    yticks([-180, -90, 0, 90, 180]);
    xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
    hold off;

    linkaxes([ax1,ax2],'x');

    if save_fig; exportFig(['comp_meas_m_m_' dir], 'normal-normal', struct('path', 'identification')); end
end

%% Hexapod to Granite
for dir = 'xyz'
    figure;
    % Amplitude
    ax1 = subplot(2,1,1);
    hold on;
    plot(freqs, abs(squeeze(freqresp(G_ms(['Dm' dir], ['Fg' dir]), freqs, 'Hz'))));
    plot(freqs, abs(squeeze(freqresp(meas_sys(['Dh' dir], ['Fm' dir]), freqs, 'Hz'))), '.');
    set(gca,'xscale','log'); set(gca,'yscale','log');
    ylabel('Amplitude [m/N]');
    set(gca, 'XTickLabel',[]);
    legend({'Model', 'Meas.'});
    hold off;
    % Phase
    ax2 = subplot(2,1,2);
    hold on;
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_ms(['Dm' dir], ['Fg' dir]), freqs, 'Hz'))));
    plot(freqs, 180/pi*angle(squeeze(freqresp(meas_sys(['Dh' dir], ['Fm' dir]), freqs, 'Hz'))), '.');
    set(gca,'xscale','log');
    ylim([-180, 180]);
    yticks([-180, -90, 0, 90, 180]);
    xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
    hold off;

    linkaxes([ax1,ax2],'x');

    if save_fig; exportFig(['comp_meas_m_g_' dir], 'normal-normal', struct('path', 'identification')); end
end