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

%% Force Sensor Calibration
% Load measurement data
load('calibration_force_sensor.mat', 't', 'F', 'Fc')

% Remove any offset such that they are both measuring no force when not in contact.
F  = F  - mean(F( t > 0.5 & t < 1.0)); % FC2231 [N]
Fc = Fc - mean(Fc(t > 0.5 & t < 1.0)); % XFL212R [N]

% Only get useful stroke
F  = F( t > 2 & t < 2.2);
Fc = Fc(t > 2 & t < 2.2);
t  = t( t > 2 & t < 2.2);

% Make a line fit
fit_F = polyfit(Fc, F, 1);

% Estimate the gain mismatch
F_gain_mismatch = 100*(1 - fit_F(1)); % in %

% Estimate non-linearity of the sensors
F_non_linearity = 100*(F - (Fc*fit_F(1) + fit_F(2)))./Fc; % in %

%% Measured two forces and linear fit
figure;
yyaxis left
hold on;
plot(Fc, F, '.', 'DisplayName', 'Raw Data');
plot([0, 6], [0,6]*fit_F(1) + fit_F(2), '-', 'color', [colors(3,:), 0.5], 'DisplayName', 'Line Fit');
hold off;
xlabel('XFL212R [N]'); ylabel('FC2231 [N]');
xlim([0,6]); ylim([0,6]);
xticks([0:6])
yticks([0:6])

yyaxis right
plot(Fc, movmean(F_non_linearity, 200), '-', 'DisplayName', 'Non linearity');
ylim([-0.15, 0.15]);
ylabel('Non Linearity [$\%$]')

leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Estimaetd load cell stiffness
% Load measurement data
load('force_sensor_stiffness_meas.mat', 't', 'F', 'd')

% Remove offset
F  = F - mean(F(t > 0.5 & t < 1.0));

% Select important part of data
F  = F( t > 4.55 & t < 7.24);
d  = d( t > 4.55 & t < 7.24); d = d - d(1);
t  = t( t > 4.55 & t < 7.24);

% Linear fit
fit_k = polyfit(F(F<10), d(F<10), 1);

%% Displacement as a function of the measured force
figure;
hold on;
plot(F, 1e6*d, 'k-', 'DisplayName', 'Raw Data');
plot(F([1,end]), 1e6*(F([1,end])*fit_k(1) + fit_k(2)), '--', 'DisplayName', sprintf('Fit, $k_F \\approx %.2f N/\\mu m$', 1e-6/fit_k(1)));
hold off;
xlabel('Force [$N$]'); ylabel('Displacement [$\mu m$]');
xlim([0,45]); ylim([0,60]);
legend('location', 'southeast', 'FontSize', 8);

%% Estimate the bending stiffness and stroke from the measurement - First Flexible joint
% Load Measured Data for first flexible joint
load('meas_stiff_flex_1_x.mat', 't', 'F', 'd');

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

% Zero the force
F = F - mean(F(t < 0.2));

% Find when the force sensor touches the flexible joint
i_l_start = find(F > 0.3, 1, 'first');

% Compute torque and angular displacement
h = 22.5e-3; % Height [m]
Tx = h * F; % Applied torque in [Nm]
thetax = atan2(d - d(i_l_start), h); % Measured angle in [rad]

% Find then the maximum torque is applied
[~, i_s_stop] = max(Tx);
% Linear region stops ~ when 90% of the stroke is reached
i_l_stop = find(thetax > 0.9*thetax(i_s_stop), 1, 'first');
% Mechanical "Stop" region start ~20Nmm before maximum torque is applied
i_s_start = find(Tx > max(Tx)-20e-3, 1, 'first');

% Linear fit in the "linear" region
fit_l = polyfit(Tx(i_l_start:i_l_stop), thetax(i_l_start:i_l_stop), 1);

% Linear fit in the "mechanical stop" region
fit_s = polyfit(Tx(i_s_start:i_s_stop), thetax(i_s_start:i_s_stop), 1);

% Reset displacement more precisely based on fit
thetax = thetax - fit_l(2);
fit_s(2) = fit_s(2) - fit_l(2);
fit_l(2) = 0;

%% Estimation of the bending stiffness
kRx_l = 1/fit_l(1); % Bending Stiffness [Nm/rad]
kRx_s = 1/fit_s(1); % Mechanical "Stop" Stiffness [Nm/rad]

%% Estimation of the bending stroke
% This is done by finding the intersection of the two linear fits
theta_max = fit_l(1)*fit_s(2)/(fit_l(1) - fit_s(1)); % Maximum angular stroke [rad]
Tx_at_theta_max = (fit_s(2) - fit_l(2))/(fit_l(1) - fit_s(1));

%% Measured force and displacement as a function of time
figure;
yyaxis left
hold on;
plot(t, F);
plot(0.8*cos(0:0.01:2*pi)+0.8, ...
     2.8*sin(0:0.01:2*pi)-1, 'k--', 'HandleVisibility', 'off');
text(1.8, -1.2, sprintf('Not in\ncontact'), 'horizontalalignment', 'left');
plot(0.4*cos(0:0.01:2*pi)+3, ...
     1.1*sin(0:0.01:2*pi)+4.8, 'k--');
text(3.5, 4.8, sprintf('Mechanical\nStop'), 'horizontalalignment', 'left');
hold off;
ylabel('Force $F_y$ [N]');
ylim([-4, 6])
ylimr = get(gca,'Ylim');

yyaxis right
plot(t, 1e3*d);
xlabel('Time [s]');
ylabel('Displacement $d_y$ [mm]');
xlim([0,5]);
% Make the force and displacement superimpose
ylim([0.364 - 4*(0.8315-0.364)/4.095, 0.364 + 6*(0.8315-0.364)/4.095])

% Regions where to plot the fitted data
Tx_l_fit = [0, Tx_at_theta_max];
Tx_s_fit = [Tx_at_theta_max, Tx(i_s_stop)];

figure;
hold on;
plot(Tx(1:i_s_stop), 1e3*thetax(1:i_s_stop), '-k', 'DisplayName', 'Raw data')
plot(Tx_l_fit, 1e3*(Tx_l_fit*fit_l(1) + fit_l(2)), '--', 'DisplayName', sprintf('$k_{R_x} = %.1f$ Nm/rad', kRx_l))
plot(Tx_s_fit, 1e3*(Tx_s_fit*fit_s(1) + fit_s(2)), '--', 'DisplayName', sprintf('$k_{R_x,stop} = %.0f$ Nm/rad', kRx_s))
plot([0, Tx_at_theta_max], [1e3*theta_max, 1e3*theta_max], 'k--', 'HandleVisibility', 'off')
plot([0, Tx_at_theta_max], [0, 0], 'k--', 'HandleVisibility', 'off')
anArrow = annotation('doublearrow', 'LineWidth', 0.5);
anArrow.Parent = gca;
anArrow.Position = [0.05, 0, 0, 1e3*fit_l(1)*fit_s(2)/(fit_l(1) - fit_s(1))];
text(0.052, 0.4*1e3*fit_l(1)*fit_s(2)/(fit_l(1) - fit_s(1)), sprintf('$\\theta_{x,\\max} = %.1f$ mrad', 1e3*theta_max), 'horizontalalignment', 'left');
hold off;
xlabel('Torque $T_x$ [Nm]');
ylabel('Angle $\theta_x$ [mrad]');
xlim([0, 0.15]);
ylim([-5,25]);
leg = legend('location', 'southeast', 'FontSize', 8);
leg.ItemTokenSize(1) = 15;

%% Measure the bending stiffness and Stroke for all the flexible joints
figure;
hold on;

%% Start with the X-bending
% Initialize variables
Rx = zeros(1,16); % Bending stiffnesses [Nm/rad]
kSx = zeros(1,16); % Bending stiffnesses at "stop" [Nm/rad]
Rmx = zeros(1,16); % Bending stroke [rad]

for i = 1:16
    % Load the data
    load(sprintf('meas_stiff_flex_%i_x.mat', i), 't', 'F', 'd');

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

    % Zero the force
    F = F - mean(F(t < 0.2));

    % Find when the force sensor touches the flexible joint
    i_l_start = find(F > 0.3, 1, 'first');

    % Zero the displacement when it comes in contact
    d = d - d(i_l_start);

    % Compute torque and angular displacement
    h = 22.5e-3; % Height [m]
    Tx = h * F; % Applied torque in [Nm]
    thetax = atan2(d, h); % Measured angle in [rad]

    % Find then the maximum torque is applied
    [~, i_s_stop] = max(Tx);
    % Linear region stops ~ when 90% of the stroke is reached
    i_l_stop = find(thetax > 0.9*thetax(i_s_stop), 1, 'first');
    % Mechanical "Stop" region start ~20Nmm before maximum torque is applied
    i_s_start = find(Tx > max(Tx)-20e-3, 1, 'first');

    % Linear fit in the "linear" region
    fit_l = polyfit(Tx(i_l_start:i_l_stop), thetax(i_l_start:i_l_stop), 1);

    % Linear fit in the "mechanical stop" region
    fit_s = polyfit(Tx(i_s_start:i_s_stop), thetax(i_s_start:i_s_stop), 1);

    % Reset displacement more precisely based on fit
    thetax = thetax - fit_l(2);
    fit_s(2) = fit_s(2) - fit_l(2);
    fit_l(2) = 0;

    % Estimation of the bending stiffness and bending stroke
    kRx(i) = 1/fit_l(1); % Bending Stiffness [Nm/rad]
    kSx(i) = 1/fit_s(1); % Mechanical "Stop" Stiffness [Nm/rad]
    Rmx(i) = fit_l(1)*fit_s(2)/(fit_l(1) - fit_s(1)); % Maximum angular stroke [rad]

    if i == 1
        plot(Tx(1:10:i_s_stop), 1e3*thetax(1:10:i_s_stop), '-', 'color', [colors(1,:), 0.4], ...
            'DisplayName', '$k_{R_x}$')
    else
        plot(Tx(1:10:i_s_stop), 1e3*thetax(1:10:i_s_stop), '-', 'color', [colors(1,:), 0.4], ...
            'HandleVisibility', 'off')
    end
end

%% Continue with the Y-bending
% Initialize variables
kRy = zeros(1,16); % Bending stiffnesses [Nm/rad]
kSy = zeros(1,16); % Bending stiffnesses at "stop" [Nm/rad]
Rmy = zeros(1,16); % Bending stroke [rad]

for i = 1:16
    % Load the data
    load(sprintf('meas_stiff_flex_%i_y.mat', i), 't', 'F', 'd');

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

    % Zero the force
    F = F - mean(F(t < 0.2));

    % Find when the force sensor touches the flexible joint
    i_l_start = find(F > 0.3, 1, 'first');

    % Zero the displacement when it comes in contact
    d = d - d(i_l_start);

    % Compute torque and angular displacement
    h = 22.5e-3; % Height [m]
    Ty = h * F; % Applied torque in [Nm]
    thetay = atan2(d, h); % Measured angle in [rad]

    % Find then the maximum torque is applied
    [~, i_s_stop] = max(Ty);
    % Linear region stops ~ when 90% of the stroke is reached
    i_l_stop = find(thetay > 0.9*thetay(i_s_stop), 1, 'first');
    % Mechanical "Stop" region start ~20Nmm before maximum torque is applied
    i_s_start = find(Ty > max(Ty)-20e-3, 1, 'first');

    % Linear fit in the "linear" region
    fit_l = polyfit(Ty(i_l_start:i_l_stop), thetay(i_l_start:i_l_stop), 1);

    % Linear fit in the "mechanical stop" region
    fit_s = polyfit(Ty(i_s_start:i_s_stop), thetay(i_s_start:i_s_stop), 1);

    % Reset displacement more precisely based on fit
    thetay = thetay - fit_l(2);
    fit_s(2) = fit_s(2) - fit_l(2);
    fit_l(2) = 0;

    % Estimation of the bending stiffness and bending stroke
    kRy(i) = 1/fit_l(1); % Bending Stiffness [Nm/rad]
    kSy(i) = 1/fit_s(1); % Mechanical "Stop" Stiffness [Nm/rad]
    Rmy(i) = fit_l(1)*fit_s(2)/(fit_l(1) - fit_s(1)); % Maximum angular stroke [rad]

    if i == 1
        plot(Ty(1:10:i_s_stop), 1e3*thetay(1:10:i_s_stop), '-', 'color', [colors(2,:), 0.4], ...
            'DisplayName', '$k_{R_y}$')
    else
        plot(Ty(1:10:i_s_stop), 1e3*thetay(1:10:i_s_stop), '-', 'color', [colors(2,:), 0.4], ...
            'HandleVisibility', 'off')
    end
end

xlabel('Torque $T$ [Nm]');
ylabel('Angle $\theta$ [mrad]');
xlim([0, 0.15]);
ylim([-5,25]);
legend('location', 'southeast', 'FontSize', 8);

figure;
histogram([kRx, kRy], [4:0.2:5])
xlabel('Bending stiffness [Nm/rad]')
xticks([4:0.2:5])