%% 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
addpath('./src/'); % Path for functions
addpath('./subsystems/'); % Path for Subsystems Simulink files

%% Data directory
data_dir = './mat/';

% Simulink Model name
mdl = 'nano_hexapod_model';

%% Colors for the figures
colors = colororder;

%% Frequency Vector
freqs = logspace(0, 3, 1000);

% Range validity of the approximate inverse kinematics

% The accuracy of the Jacobian-based forward kinematics solution was estimated through a systematic error analysis.
% For a series of platform positions along the $x\text{-axis}$, the exact strut lengths are computed using the analytical inverse kinematics equation eqref:eq:nhexa_inverse_kinematics.
% These strut lengths are then used with the Jacobian to estimate the platform pose, from which the error between the estimated and true poses can be calculated.

% The estimation errors in the $x$, $y$, and $z$ directions are shown in Figure ref:fig:nhexa_forward_kinematics_approximate_errors.
% The results demonstrate that for displacements up to approximately $1\,\%$ of the hexapod's size (which corresponds to $100\,\mu m$ as the size of the Stewart platform is here $\approx 100\,mm$), the Jacobian approximation provides excellent accuracy.

% This finding has particular significance for the Nano-hexapod application.
% Since the maximum required stroke ($\approx 100\,\mu m$) is three orders of magnitude smaller than the stewart platform size ($\approx 100\,mm$), the Jacobian matrix can be considered constant throughout the workspace.
% It can be computed once at the rest position and used for both forward and inverse kinematics with high accuracy.


%% Estimate the errors associated with approximate forward kinematics using the Jacobian matrix
stewart = initializeSimplifiedNanoHexapod('H', 100e-3, 'MO_B', 0);

Xrs = logspace(-6, -1, 100); % Wanted X translation of the mobile platform [m]

% Compute the strut exact length for each X-position
Ls_exact = zeros(6, length(Xrs));
for i = 1:length(Xrs)
    [~, L_exact(:, i)] = inverseKinematics(stewart, 'AP', [Xrs(i); 0; 0]);
end

% From the strut length, compute the stewart pose using the Jacobian matrix
Xrs_approx = zeros(6, length(Xrs));
for i = 1:length(Xrs)
    Xrs_approx(:, i) = inv(stewart.geometry.J)*L_exact(:, i);
end

%% Errors associated with the use of the Jacobian matrix to solve the forward kinematic problem
figure;
hold on;
plot(1e6*Xrs, 1e9*abs(Xrs - Xrs_approx(1, :)), 'DisplayName', '$\epsilon_x$');
plot(1e6*Xrs, 1e9*abs(Xrs_approx(2, :)), 'DisplayName', '$\epsilon_y$');
plot(1e6*Xrs, 1e9*abs(Xrs_approx(3, :)), 'DisplayName', '$\epsilon_z$');
plot(1e6*Xrs, 1e6*Xrs, 'k--', 'DisplayName', '$0.1\%$ error');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]); ylim([1, 1e4]);
xlabel('$D_x$ motion');
ylabel('Kinematic Errors');
xticks([1, 10, 100, 1000, 10000]);
yticks([1, 10, 100, 1000, 10000]);
xticklabels({'$1\mu m$', '$10\mu m$', '$100\mu m$', '$1mm$', '$10mm$'});
yticklabels({'$1nm$', '$10nm$', '$100nm$', '$1\mu m$', '$10\mu m$'});