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

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

addpath('strut_encoder/');

open('strut_encoder.slx');

% Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates
% We first extract the stiffness and mass matrices.

K = readmatrix('strut_encoder_mat_K.CSV');
M = readmatrix('strut_encoder_mat_M.CSV');



% #+caption: First 10x10 elements of the Mass matrix
% #+RESULTS:
% |   0.04 |  -0.005 |   0.007 |   2e-06 | 0.0001 | -5e-07 | -1e-05 |  -9e-07 |  8e-05 | -5e-10 |
% | -0.005 |    0.03 |    0.02 | -0.0001 |  1e-06 | -3e-07 |  3e-05 | -0.0001 |  8e-05 | -3e-08 |
% |  0.007 |    0.02 |    0.08 |  -6e-06 | -5e-06 | -7e-07 |  4e-05 | -0.0001 | 0.0005 | -3e-08 |
% |  2e-06 | -0.0001 |  -6e-06 |   2e-06 | -4e-10 |  2e-11 | -8e-09 |   3e-08 | -2e-08 |  6e-12 |
% | 0.0001 |   1e-06 |  -5e-06 |  -4e-10 |  3e-06 |  2e-10 | -3e-09 |   3e-09 | -7e-09 |  6e-13 |
% | -5e-07 |  -3e-07 |  -7e-07 |   2e-11 |  2e-10 |  5e-07 | -2e-08 |   5e-09 | -5e-09 |  1e-12 |
% | -1e-05 |   3e-05 |   4e-05 |  -8e-09 | -3e-09 | -2e-08 |   0.04 |   0.004 |  0.003 |  1e-06 |
% | -9e-07 | -0.0001 | -0.0001 |   3e-08 |  3e-09 |  5e-09 |  0.004 |    0.02 |  -0.02 | 0.0001 |
% |  8e-05 |   8e-05 |  0.0005 |  -2e-08 | -7e-09 | -5e-09 |  0.003 |   -0.02 |   0.08 | -5e-06 |
% | -5e-10 |  -3e-08 |  -3e-08 |   6e-12 |  6e-13 |  1e-12 |  1e-06 |  0.0001 | -5e-06 |  2e-06 |


% Then, we extract the coordinates of the interface nodes.

[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('strut_encoder_out_nodes_3D.txt');

% Piezoelectric parameters
% Parameters for the APA300ML:


d33 = 3e-10; % Strain constant [m/V]
n = 80; % Number of layers per stack
eT = 1.6e-8; % Permittivity under constant stress [F/m]
sD = 2e-11; % Elastic compliance under constant electric displacement [m2/N]
ka = 235e6; % Stack stiffness [N/m]
C = 5e-6; % Stack capactiance [F]

na = 2; % Number of stacks used as actuator
ns = 1; % Number of stacks used as force sensor

% Identification of the Dynamics

m = 0.01;



% The dynamics is identified from the applied force to the measured relative displacement.

%% Name of the Simulink File
mdl = 'strut_encoder';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fa'], 1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/L'],  1, 'openoutput'); io_i = io_i + 1;

Gh = -linearize(mdl, io);



% The same dynamics is identified for a payload mass of 10Kg.

m = 10;

Ghm = -linearize(mdl, io);

freqs = logspace(0, 4, 5000);

figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Gh, freqs, 'Hz'))), '-');
plot(freqs, abs(squeeze(freqresp(Ghm, freqs, 'Hz'))), '-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
axis padded 'auto x'
hold off;

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gh, freqs, 'Hz'))), '-', ...
     'DisplayName', '$m = 0kg$');
plot(freqs, 180/pi*angle(squeeze(freqresp(Ghm, freqs, 'Hz'))), '-', ...
     'DisplayName', '$m = 10kg$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
yticks(-360:90:360);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
axis padded 'auto x'