%% 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 [Hz]
freqs = logspace(0, 3, 1000);

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

Xrs = logspace(-6, -1, 10); % Wanted X translation of the mobile platform [m]
phis = linspace(-pi, pi, 100); % Tested azimutal angles [rad]
thetas = linspace(0, pi, 100); % Tested polar angles [rad]

% Compute the strut exact length for each X-position
Xrs_errors = zeros(1, length(Xrs)); % Maximum distance error [m]
Rrs_errors = zeros(1, length(Xrs)); % Maximum angular error [rad]
for i = 1:length(Xrs)
    Xrs_error_min = 0;
    Rrs_error_min = 0;
    for theta = thetas
        for phi = phis
            ix = [sin(theta)*cos(phi); sin(theta)*sin(phi);cos(theta)]; % Unit vector for the displacement direction
            [~, L_exact] = inverseKinematics(stewart, 'AP', Xrs(i)*ix); % Compute exact strut length for the wanted position
            Xrs_approx = inv(stewart.geometry.J)*L_exact; % Approximate the position using the Jacobian
            Xrs_error = norm(Xrs(i)*ix - Xrs_approx(1:3), 2); % Compute the position estimation error
            Rrs_error = norm(Xrs_approx(4:6), 2); % Compute the angular estimation error
            if Xrs_error > Xrs_error_min
                Xrs_error_min = Xrs_error;
            end
            if Rrs_error > Rrs_error_min
                Rrs_error_min = Rrs_error;
            end
        end
    end
    Xrs_errors(i) = Xrs_error_min;
    Rrs_errors(i) = Rrs_error_min;
end

%% Errors associated with the use of the Jacobian matrix to solve the forward kinematic problem
figure;
yyaxis left
hold on;
plot(1e6*Xrs, 1e9*Xrs_errors, 'DisplayName', '$\epsilon_D$');
plot(1e6*Xrs, 1e6*Xrs, '--', '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('Motion Stroke');
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$'});

yyaxis right
plot(1e6*Xrs, 1e9*Rrs_errors, 'DisplayName', '$\epsilon_R$');
set(gca, 'YScale', 'log');
ylim([1, 1e4]);
yticks([1, 10, 100, 1000, 10000]);
yticklabels({'$1$nrad', '$10$nrad', '$100$nrad', '$1\mu$rad', '$10\mu$rad'});