% Matlab Init                                              :noexport:ignore:

%% dcm_identification.m
% Extraction of system dynamics using Simscape model

%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

%% Path for functions, data and scripts
addpath('./mat/'); % Path for data

%% Simscape Model - Nano Hexapod
addpath('./STEPS/')

%% Initialize Parameters for Simscape model
controller.type = 0; % Open Loop Control

%% Options for Linearization
options = linearizeOptions;
options.SampleTime = 0;

%% Open Simulink Model
mdl = 'simscape_dcm';

open(mdl)

%% Colors for the figures
colors = colororder;

%% Frequency Vector
freqs = logspace(1, 3, 1000);



% #+name: fig:schematic_system_inputs_outputs
% #+caption: Dynamical system with inputs and outputs
% #+RESULTS:
% [[file:figs/schematic_system_inputs_outputs.png]]

% The system is identified from the Simscape model.


%% Input/Output definition
clear io; io_i = 1;

%% Inputs
% Control Input {3x1} [N]
io(io_i) = linio([mdl, '/control_system'], 1, 'openinput');  io_i = io_i + 1;

%% Outputs
% Interferometers {3x1} [m]
io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1;

%% Extraction of the dynamics
G = linearize(mdl, io);

%% Input and Output names
G.InputName  = {'u_ur',  'u_uh',  'u_d'};
G.OutputName = {'int_111_1', 'int_111_2', 'int_111_3'};

% Plant in the frame of the fastjacks

load('mat/dcm_kinematics.mat');



% #+name: fig:schematic_jacobian_frame_fastjack
% #+caption: Use of Jacobian matrices to obtain the system in the frame of the fastjacks
% #+RESULTS:
% [[file:figs/schematic_jacobian_frame_fastjack.png]]



%% Compute the system in the frame of the fastjacks
G_pz = J_a_111*inv(J_s_111)*G;



% #+name: tab:dc_gain_plan_fj
% #+caption: DC gain of the plant in the frame of the fast jacks $\bm{G}_{\text{fj}}$
% #+attr_latex: :environment tabularx :width 0.5\linewidth :align ccc
% #+attr_latex: :center t :booktabs t
% #+RESULTS:
% | 4.4407e-09 | 2.7656e-12 | 1.0132e-12 |
% | 2.7656e-12 | 4.4407e-09 | 1.0132e-12 |
% | 1.0109e-12 | 1.0109e-12 | 4.4424e-09 |

% The bode plot of $\bm{G}_{\text{fj}}(s)$ is shown in Figure [[fig:bode_plot_plant_fj]].


%% Bode plot for the plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_pz(1,1), freqs, 'Hz'))), ...
     'DisplayName', 'd');
plot(freqs, abs(squeeze(freqresp(G_pz(2,2), freqs, 'Hz'))), ...
     'DisplayName', 'uh');
plot(freqs, abs(squeeze(freqresp(G_pz(3,3), freqs, 'Hz'))), ...
     'DisplayName', 'ur');
for i = 1:2
    for j = i+1:3
        plot(freqs, abs(squeeze(freqresp(G_pz(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
ylim([1e-13, 1e-6]);

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(1,1), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(2,2), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(3,3), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 180]);

linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);



% #+name: fig:schematic_jacobian_frame_crystal
% #+caption: Use of Jacobian matrices to obtain the system in the frame of the crystal
% #+RESULTS:
% [[file:figs/schematic_jacobian_frame_crystal.png]]


G_mr = inv(J_s_111)*G*inv(J_a_111');



% #+RESULTS:
% | 1.9978e-09 |  3.9657e-09 |  7.7944e-09 |
% | 3.9656e-09 |  8.4979e-08 | -1.5135e-17 |
% | 7.7944e-09 | -3.9252e-17 |   1.834e-07 |


%% Bode plot for the plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_mr(1,1), freqs, 'Hz'))), ...
     'DisplayName', 'd');
plot(freqs, abs(squeeze(freqresp(G_mr(2,2), freqs, 'Hz'))), ...
     'DisplayName', 'uh');
plot(freqs, abs(squeeze(freqresp(G_mr(3,3), freqs, 'Hz'))), ...
     'DisplayName', 'ur');
for i = 1:2
    for j = i+1:3
        plot(freqs, abs(squeeze(freqresp(G_mr(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(1,1), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(2,2), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(3,3), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 180]);

linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);

%% Bode plot for the plant
fig = figure;
tiledlayout(3, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

for i_out = 1:3
    for i_in = 1:3
        ax = nexttile;
        plot(freqs, abs(squeeze(freqresp(G_mr(i_out, i_in), freqs, 'Hz'))));
        set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
    end
end

linkaxes(findall(fig, 'type', 'axes'),'xy');
xlim([freqs(1), freqs(end)]);