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

% Simulink Model name
mdl = 'nano_hexapod_model';

%% Colors for the figures
colors = colororder;

%% Obtained Nano Hexapod Design
nano_hexapod = initializeStewartPlatform();
nano_hexapod = initializeFramesPositions(nano_hexapod, ...
                                    'H', 95e-3, ...
                                    'MO_B', 150e-3);

nano_hexapod = generateGeneralConfiguration(nano_hexapod, ...
                                       'FH', 15e-3, ...
                                       'FR', 120e-3, ...
                                       'FTh', [220, 320, 340, 80, 100, 200]*(pi/180), ...
                                       'MH', 15e-3, ...
                                       'MR', 110e-3, ...
                                       'MTh', [255, 285, 15, 45, 135, 165]*(pi/180));
nano_hexapod = computeJointsPose(nano_hexapod);
nano_hexapod = initializeStrutDynamics(nano_hexapod, 'k', 1);
nano_hexapod = computeJacobian(nano_hexapod);
nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpr', 125e-3, 'Mpr', 115e-3);

%% Obtained architecture for the Nano Hexapod
displayArchitecture(nano_hexapod, 'labels', true, 'frames', true);
% Bottom circle
h = 15e-3;
r = 120e-3;
theta = linspace(0, 2*pi, 100);
x = r * cos(theta);
y = r * sin(theta);
z = h * ones(size(theta)); % All points at same height
plot3(x, y, z, '--', 'color', [colors(1,:)], 'LineWidth', 0.5);
for i = 1:6
    plot3([0, nano_hexapod.platform_F.Fa(1,i)], [0, nano_hexapod.platform_F.Fa(2,i)], [h,h], '--', 'color', [colors(1,:)], 'LineWidth', 0.5);
end
% Top circle
h = 95e-3 - 15e-3;
r = 110e-3;
theta = linspace(0, 2*pi, 100);
x = r * cos(theta);
y = r * sin(theta);
z = h * ones(size(theta)); % All points at same height
plot3(x, y, z, '--', 'color', [colors(2,:)], 'LineWidth', 0.5);
for i = 1:6
    plot3([0, nano_hexapod.platform_M.Mb(1,i)], [0, nano_hexapod.platform_M.Mb(2,i)], [h,h], '--', 'color', [colors(2,:)], 'LineWidth', 0.5);
end

%% Estimate required actuator stroke for the wanted mobility
max_translation = 50e-6; % Wanted translation mobility [m]
max_rotation = 50e-6; % Wanted rotation mobility [rad]

Dxs = linspace(-max_translation, max_translation, 3);
Dys = linspace(-max_translation, max_translation, 3);
Dzs = linspace(-max_translation, max_translation, 3);
Rxs = linspace(-max_rotation, max_rotation, 3);
Rys = linspace(-max_rotation, max_rotation, 3);

L_min = 0; % Required actuator negative stroke [m]
L_max = 0; % Required actuator negative stroke [m]

for Dx = Dxs
    for Dy = Dys
        for Dz = Dzs
            for Rx = Rxs
                for Ry = Rys
                    ARB = [ cos(Ry) 0        sin(Ry);
                            0       1        0;
                           -sin(Ry) 0        cos(Ry)] * ...
                          [ 1       0        0;
                            0       cos(Rx) -sin(Rx);
                            0       sin(Rx)  cos(Rx)];

                    [~, Ls] = inverseKinematics(nano_hexapod, 'AP', [Dx;Dy;Dz], 'ARB', ARB);

                    if min(Ls) < L_min
                        L_min = min(Ls);
                    end
                    if max(Ls) > L_max
                        L_max = max(Ls);
                    end
                end
            end
        end
    end
end

sprintf('Actuator stroke should be from %.0f um to %.0f um', 1e6*L_min, 1e6*L_max)

%% Compute mobility in translation with combined angular motion
% Direction of motion (spherical coordinates)
thetas = linspace(0, pi, 100);
phis = linspace(0, 2*pi, 100);

% Considered Rotations
Rxs = linspace(-max_rotation, max_rotation, 3);
Rys = linspace(-max_rotation, max_rotation, 3);

% Maximum distance that can be reached in the direction of motion
% Considering combined angular motion and limited actuator stroke
rs = zeros(length(thetas), length(phis));
worst_rx_ry = zeros(length(thetas), length(phis), 2);

for i = 1:length(thetas)
    for j = 1:length(phis)
        % Compute unitary motion in the considered direction
        Tx = sin(thetas(i))*cos(phis(j));
        Ty = sin(thetas(i))*sin(phis(j));
        Tz = cos(thetas(i));
        % Start without considering rotations
        dL_lin = nano_hexapod.kinematics.J*[Tx; Ty; Tz; 0; 0; 0];
        % Strut motion for maximum displacement in the considered direction
        dL_lin_max = L_max*dL_lin/max(abs(dL_lin));

        % Find rotation that gives worst case stroke
        dL_worst = max(abs(dL_lin_max)); % This should be equal to L_max
        dL_rot_max = zeros(6,1);
        % Perform (small) rotations, and find the (worst) case requiring maximum strut motion
        for Rx = Rxs
            for Ry = Rys
                dL_rot = nano_hexapod.kinematics.J*[0; 0; 0; Rx; Ry; 0];
                if max(abs(dL_lin_max + dL_rot)) > dL_worst
                    dL_worst = max(abs(dL_lin_max + dL_rot));
                    dL_rot_max = dL_rot;
                    worst_rx_ry(i,j,:) = [Rx, Ry];
                end
            end
        end

        stroke_ratio = min(abs([( L_max - dL_rot_max) ./ dL_lin_max; (-L_max - dL_rot_max) ./ dL_lin_max]));
        dL_real = dL_lin_max*stroke_ratio + dL_rot_max;

        % % Obtained maximum displacement in the considered direction with angular motion
        rs(i, j) = stroke_ratio*L_max/max(abs(dL_lin));
    end
end

%% Wanted translation mobility of the Nano-Hexapod and computed Mobility
[phi_grid, theta_grid] = meshgrid(phis, thetas);
X = 1e6 * rs .* sin(theta_grid) .* cos(phi_grid);
Y = 1e6 * rs .* sin(theta_grid) .* sin(phi_grid);
Z = 1e6 * rs .* cos(theta_grid);

vertices = 1e6*max_translation*[
    -1 -1 -1;  % vertex 1
     1 -1 -1;  % vertex 2
     1  1 -1;  % vertex 3
    -1  1 -1;  % vertex 4
    -1 -1  1;  % vertex 5
     1 -1  1;  % vertex 6
     1  1  1;  % vertex 7
    -1  1  1   % vertex 8
];

% Define the faces using the vertex indices
faces = [
    1 2 3 4;  % bottom face
    5 6 7 8;  % top face
    1 2 6 5;  % front face
    2 3 7 6;  % right face
    3 4 8 7;  % back face
    4 1 5 8   % left face
];

figure;
hold on;
s = surf(X, Y, Z, 'FaceColor', 'none');
s.EdgeColor = colors(2, :);
patch('Vertices', vertices, 'Faces', faces, ...
      'FaceColor', [0.7 0.7 0.7], ...
      'EdgeColor', 'black', ...
      'FaceAlpha', 1);
hold off;
axis equal;
grid on;
xlabel('X Translation [$\mu$m]'); ylabel('Y Translation [$\mu$m]'); zlabel('Z Translation [$\mu$m]');
xlim([-150 150]); ylim([-150, 150]); zlim([-120, 120]);
view(-160, 20)

%% Estimate required actuator stroke for the wanted mobility
max_translation = 50e-6; % Wanted translation mobility [m]
max_rotation = 50e-6; % Wanted rotation mobility [rad]

Dxs = linspace(-max_translation, max_translation, 3);
Dys = linspace(-max_translation, max_translation, 3);
Dzs = linspace(-max_translation, max_translation, 3);
Rxs = linspace(-max_rotation, max_rotation, 3);
Rys = linspace(-max_rotation, max_rotation, 3);

max_angles_F = zeros(1,6); % Maximum bending angle - Fixed joints [rad]
max_angles_M = zeros(1,6); % Maximum bending angle - Mobile joints [rad]

% Compute initial strut orientation
nano_hexapod = computeJointsPose(nano_hexapod, 'AP', zeros(3,1), 'ARB', eye(3));
As = nano_hexapod.geometry.As; % Fixed joints
Bs = nano_hexapod.geometry.Bs; % Mobile joints

for Dx = Dxs
    for Dy = Dys
        for Dz = Dzs
            for Rx = Rxs
                for Ry = Rys
                    ARB = [ cos(Ry) 0        sin(Ry);
                            0       1        0;
                           -sin(Ry) 0        cos(Ry)] * ...
                          [ 1       0        0;
                            0       cos(Rx) -sin(Rx);
                            0       sin(Rx)  cos(Rx)];

                    nano_hexapod = computeJointsPose(nano_hexapod, 'AP', [Dx;Dy;Dz], 'ARB', ARB);

                    angles_M = acos(dot(Bs, nano_hexapod.geometry.Bs));
                    max_angles_M(angles_M > max_angles_M) = angles_M(angles_M > max_angles_M);

                    angles_F = acos(dot(Bs, nano_hexapod.geometry.As));
                    max_angles_F(angles_F > max_angles_F) = angles_F(angles_F > max_angles_F);
                end
            end
        end
    end
end

sprintf('Fixed joint stroke should be %.1f mrad', 1e3*max(max_angles_F))
sprintf('Mobile joint stroke should be %.1f mrad', 1e3*max(max_angles_M))
