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

%% Colors for the figures
colors = colororder;

% Analysis of one measurement

% In this section is shown how the data are analysis in order to measured:
% - the bending stiffness
% - the bending stroke
% - the stiffness once the mechanical stops are in contact


% The height from the flexible joint's center and the point of application force $h$ is defined below:

h = 25e-3; % [m]

%% Load Data
load('meas_stiff_flex_1_x.mat', 't', 'F', 'd');

%% Zero the force
F = F - mean(F(t > 0.1 & t < 0.3));

%% Start measurement at t = 0.2 s
d = d(t > 0.2);
F = F(t > 0.2);
t = t(t > 0.2); t = t - t(1);



% The obtained time domain measurements are shown in Figure ref:fig:test_joints_meas_bend_time.


%% Time Domain plots
figure;
tiledlayout(2, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile;
plot(t, F);
ylabel('Force [N]'); set(gca, 'XTickLabel',[]);

ax2 = nexttile;
plot(t, 1e3*d);
hold off;
xlabel('Time [s]'); ylabel('Displacement [mm]');

linkaxes([ax1,ax2],'x');
xlim([0,5]);



% #+name: fig:test_joints_meas_bend_time
% #+caption: Typical time domain measurements
% #+RESULTS:
% [[file:figs/test_joints_meas_bend_time.png]]

% The displacement as a function of the force is then shown in Figure ref:fig:test_joints_meas_F_d.


figure;
plot(F, 1e3*d);
xlabel('Force [N]'); ylabel('Displacement [mm]');
xlim([0,6]); ylim([0,1]);



% #+name: fig:test_joints_meas_F_d
% #+caption: Typical measurement of the diplacement as a function of the applied force
% #+RESULTS:
% [[file:figs/test_joints_meas_F_d.png]]

% The bending stiffness can be estimated by computing the slope of the curve in Figure ref:fig:test_joints_meas_F_d.
% The bending stroke and the stiffness when touching the mechanical stop can also be estimated from the same figure.


%% Determine the linear region and region when touching the mechanical stop
% Find when the force sensor touches the flexible joint
i_l_start = find(F > 0.3, 1, 'first');
% Reset the measured diplacement at that point
d = d - d(i_l_start);
% Find then the maximum force is applied
[~, i_s_stop] = max(F);
% Linear region stops ~ when 90% of the stroke is reached
i_l_stop = find(d > 0.9*d(i_s_stop), 1, 'first');
% "Stop" region start ~1N before maximum force is applied
i_s_start = find(F > max(F)-1, 1, 'first');

%% Define variables for the two regions
F_l = F(i_l_start:i_l_stop);
d_l = d(i_l_start:i_l_stop);

F_s = F(i_s_start:i_s_stop);
d_s = d(i_s_start:i_s_stop);

%% Fit the best straight line for the two regions
fit_l = polyfit(F_l, d_l, 1);
fit_s = polyfit(F_s, d_s, 1);

%% Reset displacement based on fit
d = d - fit_l(2);
fit_s(2) = fit_s(2) - fit_l(2);
fit_l(2) = 0;



% The raw data as well as the fit corresponding to the two stiffnesses are shown in Figure ref:fig:test_joints_meas_F_d_lin_fit.


figure;
hold on;
plot(F(1:i_s_stop), 1e3*d(1:i_s_stop), '.k')
plot(F_l, 1e3*(F_l*fit_l(1) + fit_l(2)))
plot(F_s, 1e3*(F_s*fit_s(1) + fit_s(2)))
hold off;
xlabel('Force [N]'); ylabel('Displacement [mm]');
xlim([0,6]);



% #+name: fig:test_joints_meas_F_d_lin_fit
% #+caption: Typical measurement of the diplacement as a function of the applied force with estimated linear fits
% #+RESULTS:
% [[file:figs/test_joints_meas_F_d_lin_fit.png]]

% Then, the bending stroke is estimated as crossing point between the two fitted lines:

d_max = fit_l(1)*fit_s(2)/(fit_l(1) - fit_s(1));

% Bending stiffness and bending stroke of all the flexible joints

% Now, let's estimate the bending stiffness and stroke for all the flexible joints.


%% Initialize variables
kRx = zeros(1,16);
kSx = zeros(1,16);
Rmx = zeros(1,16);

for i = 1:16
    %% Load the data
    load(['meas_stiff_flex_' num2str(i) '_x.mat'], 't', 'F', 'd');

    %% Automatic Zero of the force
    F = F - mean(F(t > 0.1 & t < 0.3));

    %% Start measurement at t = 0.2 s
    d = d(t > 0.2);
    F = F(t > 0.2);
    t = t(t > 0.2); t = t - t(1);

    %% Estimate linear region and "stop" region
    i_l_start = find(F > 0.3, 1, 'first');
    d = d - d(i_l_start);
    [~, i_s_stop] = max(F);
    i_l_stop = find(d > 0.9*d(i_s_stop), 1, 'first');
    i_s_start = find(F > max(F)-1, 1, 'first');

    F_l = F(i_l_start:i_l_stop);
    d_l = d(i_l_start:i_l_stop);

    F_s = F(i_s_start:i_s_stop);
    d_s = d(i_s_start:i_s_stop);

    %% Straight line fit
    fit_l = polyfit(F_l, d_l, 1);
    fit_s = polyfit(F_s, d_s, 1);

    %% Reset displacement based on fit
    d = d - fit_l(2);
    fit_s(2) = fit_s(2) - fit_l(2);
    fit_l(2) = 0;

    %% Estimated Stroke
    d_max = fit_l(1)*fit_s(2)/(fit_l(1) - fit_s(1));

    %% Save stiffnesses and stroke
    kRx(i) = (h)^2/fit_l(1);
    kSx(i) = (h)^2/fit_s(1);
    Rmx(i) = atan2(d_max,h);

end

%% Initialize variables
kRy = zeros(1,16);
kSy = zeros(1,16);
Rmy = zeros(1,16);

for i = 1:16
    %% Load the data
    load(['meas_stiff_flex_' num2str(i) '_y.mat'], 't', 'F', 'd');

    %% Automatic Zero of the force
    F = F - mean(F(t > 0.1 & t < 0.3));

    %% Start measurement at t = 0.2 s
    d = d(t > 0.2);
    F = F(t > 0.2);
    t = t(t > 0.2); t = t - t(1);

    %% Estimate linear region and "stop" region
    i_l_start = find(F > 0.3, 1, 'first');
    d = d - d(i_l_start);
    [~, i_s_stop] = max(F);
    i_l_stop = find(d > 0.9*d(i_s_stop), 1, 'first');
    i_s_start = find(F > max(F)-1, 1, 'first');

    F_l = F(i_l_start:i_l_stop);
    d_l = d(i_l_start:i_l_stop);

    F_s = F(i_s_start:i_s_stop);
    d_s = d(i_s_start:i_s_stop);

    %% Straight line fit
    fit_l = polyfit(F_l, d_l, 1);
    fit_s = polyfit(F_s, d_s, 1);

    %% Reset displacement based on fit
    d = d - fit_l(2);
    fit_s(2) = fit_s(2) - fit_l(2);
    fit_l(2) = 0;

    %% Estimated Stroke
    d_max = fit_l(1)*fit_s(2)/(fit_l(1) - fit_s(1));

    %% Save stiffnesses and stroke
    kRy(i) = (h)^2/fit_l(1);
    kSy(i) = (h)^2/fit_s(1);
    Rmy(i) = atan2(d_max,h);

end

% Analysis
% The dispersion of the measured bending stiffness is shown in Figure ref:fig:test_joints_bend_stiff_hist and of the bending stroke in Figure ref:fig:test_joints_bend_stroke_hist.


figure;
hold on;
histogram(kRx, 'DisplayName', '$k_{R_x}$')
histogram(kRy, 'DisplayName', '$k_{R_y}$')
hold off;
xlabel('Bending Stiffness [Nm/rad]')
legend();



% #+name: fig:test_joints_bend_stiff_hist
% #+caption: Histogram of the measured bending stiffness
% #+RESULTS:
% [[file:figs/test_joints_bend_stiff_hist.png]]


figure;
hold on;
histogram(1e3*Rmx, 'DisplayName', '$k_{R_x}$')
histogram(1e3*Rmy, 'DisplayName', '$k_{R_y}$')
hold off;
xlabel('Bending Stroke [mrad]')
legend();



% #+name: fig:test_joints_bend_stroke_hist
% #+caption: Histogram of the measured bending stroke
% #+RESULTS:
% [[file:figs/test_joints_bend_stroke_hist.png]]

% The relation between the measured beam thickness and the measured bending stiffness is shown in Figure ref:fig:test_joints_thickness_stiffness.


load('flex_meas_dim.mat', 'meas_flex');
figure;
hold on;
plot((meas_flex(:,1)+meas_flex(:,2))/2, kRx, 'o', 'DisplayName', '$x$')
plot((meas_flex(:,3)+meas_flex(:,4))/2, kRy, 'o', 'DisplayName', '$y$')
hold off;
xlabel('Flexible Beam Thickness [$\mu m$]');
ylabel('Bending Stiffness [Nm/rad]');
legend('location', 'southeast');