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

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

colors = colororder;

addpath('./mat/');

%% Load the measurements
apa300ml_1s = {};
for i = 1:7
    apa300ml_1s(i) = {load(['mat/stroke_apa_1stacks_' num2str(i) '.mat'], 't', 'V', 'd')};
end

%% Only take the data between t=2 and t=10 and reset the measured displacement at t=2
for i = 1:7
    t = apa300ml_1s{i}.t;
    apa300ml_1s{i}.d = apa300ml_1s{i}.d - mean(apa300ml_1s{i}.d(t > 1.9 & t < 2.0));
    apa300ml_1s{i}.d = apa300ml_1s{i}.d(t > 2.0 & t < 10.0);
    apa300ml_1s{i}.V = apa300ml_1s{i}.V(t > 2.0 & t < 10.0);
    apa300ml_1s{i}.t = apa300ml_1s{i}.t(t > 2.0 & t < 10.0);
end

%% Applied voltage as a function of time
figure;
plot(apa300ml_1s{1}.t, 20*apa300ml_1s{1}.V)
xlabel('Time [s]'); ylabel('Voltage [V]');
ylim([-20,160]); yticks([-20     0    20    40    60    80   100   120   140   160]);

%% Measured motion for all the APA300ML
figure;
hold on;
for i = 1:7
    plot(apa300ml_1s{i}.t, 1e6*apa300ml_1s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Time [s]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southeast', 'FontSize', 8)

%% Displacement as a function of the applied voltage
figure;
hold on;
for i = 1:7
    plot(20*apa300ml_1s{i}.V, 1e6*apa300ml_1s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Voltage [V]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southwest', 'FontSize', 8)
xlim([-20, 160]); ylim([-140, 0]);

%% Load the measurements
apa300ml_2s = {};
for i = 1:7
    apa300ml_2s(i) = {load(['mat/stroke_apa_2stacks_' num2str(i) '.mat'], 't', 'V', 'd')};
end

%% Only take the data between t=2 and t=10 and reset the measured displacement at t=2
for i = 1:7
    t = apa300ml_2s{i}.t;
    apa300ml_2s{i}.d = apa300ml_2s{i}.d - mean(apa300ml_2s{i}.d(t > 1.9 & t < 2.0));
    apa300ml_2s{i}.d = apa300ml_2s{i}.d(t > 2.0 & t < 10.0);
    apa300ml_2s{i}.V = apa300ml_2s{i}.V(t > 2.0 & t < 10.0);
    apa300ml_2s{i}.t = apa300ml_2s{i}.t(t > 2.0 & t < 10.0);
end

%% Measured motion for all the APA300ML
figure;
hold on;
for i = 1:7
    plot(apa300ml_2s{i}.t, 1e6*apa300ml_2s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Time [s]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southeast', 'FontSize', 8)
ylim([-250, 0]);

%% Displacement as a function of the applied voltage
figure;
hold on;
for i = 1:7
    plot(20*apa300ml_2s{i}.V, 1e6*apa300ml_2s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Voltage [V]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southwest', 'FontSize', 8)
xlim([-20, 160]); ylim([-250, 0]);

%% Motion induced by applying a voltage to the three stack is the sum to the previous two measured displacements
apa300ml_3s = {};
for i = 1:7
    apa300ml_3s(i) = apa300ml_1s(i);
    apa300ml_3s{i}.d = apa300ml_1s{i}.d + apa300ml_2s{i}.d;
end

%% Displacement as a function of the applied voltage
figure;
hold on;
for i = 1:7
    plot(20*apa300ml_3s{i}.V, 1e6*apa300ml_3s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Voltage [V]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southwest', 'FontSize', 8)
xlim([-20, 160]); ylim([-400, 0]);

%% Estimate the maximum stroke
apa300ml_stroke = zeros(1, 7);
for i = 1:7
    apa300ml_stroke(i) = max(apa300ml_3s{i}.d) - min(apa300ml_3s{i}.d);
end