%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

%% Path for functions, data and scripts
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;

%% Example of one Stewart platform and associated translational mobility
L_max =  50e-6; % Maximum actuator stroke (+/-) [m]

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 100e-3, 'MO_B', -50e-3);
stewart = generateGeneralConfiguration(stewart, 'FH', 0, 'FR', 100e-3, 'MH', 0, 'MR', 100e-3, ...
                                      'FTh', [-5, 5, 120-5, 120+5, 240-5, 240+5]*(pi/180), ...
                                      'MTh', [-60+5, 60-5, 60+5, 180-5, 180+5, -60-5]*(pi/180));
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1);
stewart = computeJacobian(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 110e-3, 'Mpr', 110e-3);

displayArchitecture(stewart, 'labels', false, 'frames', false, 'F_color', [0,0,0], 'M_color', [0,0,0], 'L_color', [0,0,0]);

thetas = linspace(0, pi, 100);
phis = linspace(0, 2*pi, 100);
rs = zeros(length(thetas), length(phis));

for i = 1:length(thetas)
    for j = 1:length(phis)
        Tx = sin(thetas(i))*cos(phis(j));
        Ty = sin(thetas(i))*sin(phis(j));
        Tz = cos(thetas(i));

        dL = stewart.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction

        rs(i, j) = L_max/max(abs(dL));
    end
end

% Sphere with radius equal to actuator stroke
[Xs,Ys,Zs] = sphere;
Xs = 1e6*L_max*Xs;
Ys = 1e6*L_max*Ys;
Zs = 1e6*L_max*Zs;

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

figure('Renderer', 'painters');
hold on;
surf(X,  Y,  Z,  'FaceColor', 'none', 'LineWidth', 0.1, 'EdgeColor', [0, 0, 0]);
surf(Xs, Ys, Zs, 'FaceColor', colors(2,:), 'EdgeColor', 'none');
quiver3(0, 0, 0, 100, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.9);
quiver3(0, 0, 0, 0, 100, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.9);
quiver3(0, 0, 0, 0, 0, 100, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.9);
hold off;
axis equal;
grid off;
axis off;
view(55, 25);

%% Stewart platform with Vertically oriented struts
L_max =  50e-6; % Maximum actuator stroke (+/-) [m]

stewart_vert = initializeStewartPlatform();
stewart_vert = initializeFramesPositions(stewart_vert, 'H', 40e-3, 'MO_B', -20e-3);
stewart_vert = generateGeneralConfiguration(stewart_vert, 'FH', 0, 'FR', 100e-3, 'MH', 0, 'MR', 100e-3, ...
                                      'FTh', [-25, 25, 120-25, 120+25, 240-25, 240+25]*(pi/180), ...
                                      'MTh', [-60+25, 60-25, 60+25, 180-25, 180+25, -60-25]*(pi/180));
stewart_vert = computeJointsPose(stewart_vert);
stewart_vert = initializeStrutDynamics(stewart_vert, 'k', 1);
stewart_vert = computeJacobian(stewart_vert);
stewart_vert = initializeCylindricalPlatforms(stewart_vert, 'Fpr', 110e-3, 'Mpr', 110e-3);

displayArchitecture(stewart_vert, 'labels', false, 'frames', false, 'F_color', colors(1,:), 'M_color', colors(1,:), 'L_color', colors(1,:));

%% Stewart platform with Horizontally oriented struts
stewart_hori = initializeStewartPlatform();
stewart_hori = initializeFramesPositions(stewart_hori, 'H', 40e-3, 'MO_B', -20e-3);
stewart_hori = generateGeneralConfiguration(stewart_hori, 'FH', 0, 'FR', 100e-3, 'MH', 0, 'MR', 100e-3, ...
                                      'FTh', [-5, 5, 120-5, 120+5, 240-5, 240+5]*(pi/180), ...
                                      'MTh', [-60+5, 60-5, 60+5, 180-5, 180+5, -60-5]*(pi/180));
stewart_hori = computeJointsPose(stewart_hori);
stewart_hori = initializeStrutDynamics(stewart_hori, 'k', 1);
stewart_hori = computeJacobian(stewart_hori);
stewart_hori = initializeCylindricalPlatforms(stewart_hori, 'Fpr', 110e-3, 'Mpr', 110e-3);

displayArchitecture(stewart_hori, 'labels', false, 'frames', false, 'F_color', colors(2,:), 'M_color', colors(2,:), 'L_color', colors(2,:));

%% Translation mobility for two Stewart platform geometry
thetas = linspace(0, pi, 100);
phis = linspace(0, 2*pi, 100);
rs_vert = zeros(length(thetas), length(phis));
rs_hori = zeros(length(thetas), length(phis));

for i = 1:length(thetas)
    for j = 1:length(phis)
        Tx = sin(thetas(i))*cos(phis(j));
        Ty = sin(thetas(i))*sin(phis(j));
        Tz = cos(thetas(i));

        dL = stewart_vert.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction
        rs_vert(i, j) = L_max/max(abs(dL));

        dL = stewart_hori.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction
        rs_hori(i, j) = L_max/max(abs(dL));
    end
end

[phi_grid, theta_grid] = meshgrid(phis, thetas);

X_vert = 1e6 * rs_vert .* sin(theta_grid) .* cos(phi_grid);
Y_vert = 1e6 * rs_vert .* sin(theta_grid) .* sin(phi_grid);
Z_vert = 1e6 * rs_vert .* cos(theta_grid);

X_hori = 1e6 * rs_hori .* sin(theta_grid) .* cos(phi_grid);
Y_hori = 1e6 * rs_hori .* sin(theta_grid) .* sin(phi_grid);
Z_hori = 1e6 * rs_hori .* cos(theta_grid);

[Xs,Ys,Zs] = sphere;
Xs = 1e6*L_max*Xs;
Ys = 1e6*L_max*Ys;
Zs = 1e6*L_max*Zs;

figure;
hold on;
surf(X_vert, Y_vert, Z_vert, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(1,:));
surf(X_hori, Y_hori+400, Z_hori, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(2,:));
quiver3(0, 0, 0, 200, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 200, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 0, 200, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
text(200, 0, 0, '$D_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top'   );
text(0, 200, 0, '$D_y$', 'FontSize', 10, 'HorizontalAlignment', 'right',  'VerticalAlignment', 'bottom');
text(0, 0, 200, '$D_z$', 'FontSize', 10, 'HorizontalAlignment', 'left',   'VerticalAlignment', 'top'   );
quiver3(0, 400, 0, 200, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 400, 0, 0, 200, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 400, 0, 0, 0, 200, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
text(200, 400, 0,   '$D_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top'   );
text(0,   600, 0,   '$D_y$', 'FontSize', 10, 'HorizontalAlignment', 'right',  'VerticalAlignment', 'bottom');
text(0,   400, 200, '$D_z$', 'FontSize', 10, 'HorizontalAlignment', 'left',   'VerticalAlignment', 'top'   );
hold off;
axis equal;
grid off;
axis off;
view(105, 15);

%% Stewart platform with struts close to each other
stewart_close = initializeStewartPlatform();
stewart_close = initializeFramesPositions(stewart_close, 'H', 100e-3, 'MO_B', 0);
stewart_close = generateGeneralConfiguration(stewart_close, 'FH', 0, 'FR', 50e-3, 'MH', 0, 'MR', 50e-3, ...
                                      'FTh', [-6, 6, 120-6, 120+6, 240-6, 240+6]*(pi/180), ...
                                      'MTh', [-60+6, 60-6, 60+6, 180-6, 180+6, -60-6]*(pi/180));
stewart_close = computeJointsPose(stewart_close);
stewart_close = initializeStrutDynamics(stewart_close, 'k', 1);
stewart_close = computeJacobian(stewart_close);
stewart_close = initializeCylindricalPlatforms(stewart_close, 'Fpr', 110e-3, 'Mpr', 110e-3);

displayArchitecture(stewart_close, 'labels', false, 'frames', false, 'F_color', colors(1,:), 'M_color', colors(1,:), 'L_color', colors(1,:));

%% Stewart platform with struts further away from each other
stewart_space = initializeStewartPlatform();
stewart_space = initializeFramesPositions(stewart_space, 'H', 100e-3, 'MO_B', 0);
stewart_space = generateGeneralConfiguration(stewart_space, 'FH', 0, 'FR', 100e-3, 'MH', 0, 'MR', 100e-3, ...
                                      'FTh', [-6, 6, 120-6, 120+6, 240-6, 240+6]*(pi/180), ...
                                      'MTh', [-60+6, 60-6, 60+6, 180-6, 180+6, -60-6]*(pi/180));
stewart_space.platform_F.Fa = stewart_space.platform_M.Mb - (stewart_close.platform_M.Mb - stewart_close.platform_F.Fa);
stewart_space = computeJointsPose(stewart_space);
stewart_space = initializeStrutDynamics(stewart_space, 'k', 1);
stewart_space = computeJacobian(stewart_space);
stewart_space = initializeCylindricalPlatforms(stewart_space, 'Fpr', 110e-3, 'Mpr', 110e-3);

displayArchitecture(stewart_space, 'labels', false, 'frames', false, 'F_color', colors(2,:), 'M_color', colors(2,:), 'L_color', colors(2,:));

%% Translation mobility for two Stewart platform geometry
thetas = linspace(0, pi, 50);
phis = linspace(0, 2*pi, 50);
rs_close = zeros(length(thetas), length(phis));
rs_space = zeros(length(thetas), length(phis));

for i = 1:length(thetas)
    for j = 1:length(phis)
        Rx = sin(thetas(i))*cos(phis(j));
        Ry = sin(thetas(i))*sin(phis(j));
        Rz = cos(thetas(i));

        dL = stewart_close.kinematics.J*[0; 0; 0; Rx; Ry; Rz;];
        rs_close(i, j) = L_max/max(abs(dL));

        dL = stewart_space.kinematics.J*[0; 0; 0; Rx; Ry; Rz;];
        rs_space(i, j) = L_max/max(abs(dL));
    end
end

[phi_grid, theta_grid] = meshgrid(phis, thetas);

X_close = 1e6 * rs_close .* sin(theta_grid) .* cos(phi_grid);
Y_close = 1e6 * rs_close .* sin(theta_grid) .* sin(phi_grid);
Z_close = 1e6 * rs_close .* cos(theta_grid);

X_space = 1e6 * rs_space .* sin(theta_grid) .* cos(phi_grid);
Y_space = 1e6 * rs_space .* sin(theta_grid) .* sin(phi_grid);
Z_space = 1e6 * rs_space .* cos(theta_grid);

figure;
hold on;
surf(X_close, Y_close, Z_close, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(1,:));
surf(X_space, Y_space+6000, Z_space, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(2,:));
quiver3(0, 0, 0, 4000, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 4000, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 0, 4000, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
text(4000, 0, 0, '$R_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top'   );
text(0, 4000, 0, '$R_y$', 'FontSize', 10, 'HorizontalAlignment', 'right',  'VerticalAlignment', 'bottom');
text(0, 0, 4000, '$R_z$', 'FontSize', 10, 'HorizontalAlignment', 'left',   'VerticalAlignment', 'top'   );
quiver3(0, 6000, 0, 4000, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 6000, 0, 0, 4000, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 6000, 0, 0, 0, 4000, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
text(4000, 6000, 0,   '$R_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top');
text(0,    10000, 0,   '$R_y$', 'FontSize', 10, 'HorizontalAlignment', 'right', 'VerticalAlignment', 'bottom');
text(0,    6000, 4000, '$R_z$', 'FontSize', 10, 'HorizontalAlignment', 'left', 'VerticalAlignment', 'top');
hold off;
axis equal;
grid off;
axis off;
view(105, 15);