% Matlab Init                                              :noexport:ignore:

%% dcm_active_damping_strain_gauges.m
% Active Damping using relative motion sensors (strain gauges)

%% 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);

% Identification

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

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

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

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

G_sg.InputName  = {'u_ur',  'u_uh',  'u_d'};
G_sg.OutputName = {'sg_ur', 'sg_uh', 'sg_d'};



% #+RESULTS:
% | -1.4113e-13 |  1.0339e-13 |   3.774e-14 |
% |  1.0339e-13 | -1.4113e-13 |   3.774e-14 |
% |  3.7792e-14 |  3.7792e-14 | -7.5585e-14 |


%% 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_sg(1,1), freqs, 'Hz'))), ...
     'DisplayName', 'd');
plot(freqs, abs(squeeze(freqresp(G_sg(2,2), freqs, 'Hz'))), ...
     'DisplayName', 'uh');
plot(freqs, abs(squeeze(freqresp(G_sg(3,3), freqs, 'Hz'))), ...
     'DisplayName', 'ur');
for i = 1:2
    for j = i+1:3
        plot(freqs, abs(squeeze(freqresp(G_sg(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_sg(1,1), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_sg(2,2), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_sg(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)]);