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

%% Example of a typical "cubic" architecture
MO_B = -50e-3;  % Position {B} with respect to {M} [m]

H = 100e-3;     % Height of the Stewart platform [m]

Hc = 100e-3;   % Size of the useful part of the cube [m]

FOc = H + MO_B; % Center of the cube with respect to {F}
                % here positionned at the frame {B}

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1);
stewart = initializeJointDynamics(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 150e-3);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);

displayArchitecture(stewart, 'labels', false, 'frames', false);
plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', false);
view([40, 5]);

%% Example of a typical "cubic" architecture
MO_B = -20e-3;  % Position {B} with respect to {M} [m]

H = 40e-3;     % Height of the Stewart platform [m]

Hc = 100e-3;   % Size of the useful part of the cube [m]

FOc = H + MO_B; % Center of the cube with respect to {F}
                % here positionned at the frame {B}

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1);
stewart = computeJacobian(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 150e-3);

displayArchitecture(stewart, 'labels', false, 'frames', false);
plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', false);
view([40, 5]);

%% Analytical formula for Stiffness matrix of the Cubic Stewart platform
% Define symbolic variables
syms k Hc alpha H

assume(k > 0); % k is positive real
assume(Hc, 'real'); % Hc is real
assume(H, 'real'); % H is real
assume(alpha, 'real'); % alpha is real

% Define si matrix (edges of the cubes)
si = 1/sqrt(3)*[
  [ sqrt(2),    0,        1]; ...
  [-sqrt(2)/2, -sqrt(3/2), 1]; ...
  [-sqrt(2)/2,  sqrt(3/2), 1]; ...
  [ sqrt(2),    0,        1]; ...
  [-sqrt(2)/2, -sqrt(3/2), 1]; ...
  [-sqrt(2)/2,  sqrt(3/2), 1]  ...
];

% Define ci matrix (vertices of the cubes)
ci = Hc * [
  [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2),  sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2),  sqrt(3)/sqrt(2), 0.5]; ...
  [-sqrt(2),   0,               0.5]; ...
  [-sqrt(2),   0,               0.5]  ...
];

% Apply vertical shift to ci
ci = ci + H * [0, 0, 1];

% Calculate bi vectors (Stewart platform top joints)
bi = ci + alpha * si;

% Initialize stiffness matrix
K = sym(zeros(6,6));

% Calculate elements of the stiffness matrix
for i = 1:6
    % Extract vectors for each leg
    s_i = si(i,:)';
    b_i = bi(i,:)';

    % Calculate cross product vector
    cross_bs = cross(b_i, s_i);

    % Build matrix blocks
    K(1:3, 4:6) = K(1:3, 4:6) + s_i * cross_bs';
    K(4:6, 1:3) = K(4:6, 1:3) + cross_bs * s_i';
    K(1:3, 1:3) = K(1:3, 1:3) + s_i * s_i';
    K(4:6, 4:6) = K(4:6, 4:6) + cross_bs * cross_bs';
end

% Scale by stiffness coefficient
K = k * K;

% Simplify the expressions
K = simplify(K);

% Display the analytical stiffness matrix
disp('Analytical Stiffness Matrix:');
pretty(K);

%% Cubic configuration
H = 100e-3;     % Height of the Stewart platform [m]
Hc = 100e-3;   % Size of the useful part of the cube [m]
FOc = 50e-3; % Center of the cube at the Stewart platform center
MO_B = -50e-3;  % Position {B} with respect to {M} [m]
MHb = 0;

stewart_cubic = initializeStewartPlatform();
stewart_cubic = initializeFramesPositions(stewart_cubic, 'H', H, 'MO_B', MO_B);
stewart_cubic = generateCubicConfiguration(stewart_cubic, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', MHb);
stewart_cubic = computeJointsPose(stewart_cubic);
stewart_cubic = initializeStrutDynamics(stewart_cubic, 'k', 1);
stewart_cubic = computeJacobian(stewart_cubic);
stewart_cubic = initializeCylindricalPlatforms(stewart_cubic, 'Fpr', 150e-3, 'Mpr', 150e-3);

% Let's now define the actuator stroke.
L_max =  50e-6; % [m]

%% Mobility of a Stewart platform with Cubic architecture - Translations
thetas = linspace(0, pi, 100);
phis = linspace(0, 2*pi, 200);
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_cubic.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));
        % rs(i, j) = max(abs([dL(dL<0)*L_min; dL(dL>=0)*L_max]));
    end
end

[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;
hold on;
surf(X, Y, Z, 'FaceColor', 'white', 'EdgeColor', colors(1,:));
quiver3(0, 0, 0, 150, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 150, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 0, 150, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
text(150, 0, 0, '$D_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top'   );
text(0, 150, 0, '$D_y$', 'FontSize', 10, 'HorizontalAlignment', 'right',  'VerticalAlignment', 'bottom');
text(0, 0, 150, '$D_z$', 'FontSize', 10, 'HorizontalAlignment', 'left',   'VerticalAlignment', 'top'   );
hold off;
axis equal;
grid off;
axis off;
view(105, 15);

%% Mobility of a Stewart platform with Cubic architecture - Rotations
thetas = linspace(0, pi, 100);
phis = linspace(0, 2*pi, 200);
rs_cubic = 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_cubic.kinematics.J*[0; 0; 0; Rx; Ry; Rz;];
        rs_cubic(i, j) = L_max/max(abs(dL));
    end
end

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

X_cubic = 1e6 * rs_cubic .* sin(theta_grid) .* cos(phi_grid);
Y_cubic = 1e6 * rs_cubic .* sin(theta_grid) .* sin(phi_grid);
Z_cubic = 1e6 * rs_cubic .* cos(theta_grid);

figure;
hold on;
surf(X_cubic, Y_cubic, Z_cubic, 'FaceColor', 'white', 'LineWidth', 0.2, 'EdgeColor', colors(1,:));
quiver3(0, 0, 0, 1500, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 1500, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 0, 1500, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
text(1500, 0, 0, '$R_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top'   );
text(0, 1500, 0, '$R_y$', 'FontSize', 10, 'HorizontalAlignment', 'right',  'VerticalAlignment', 'bottom');
text(0, 0, 1500, '$R_z$', 'FontSize', 10, 'HorizontalAlignment', 'left',   'VerticalAlignment', 'top'   );
hold off;
axis equal;
grid off;
axis off;
view(105, 15);

%% Input/Output definition of the Simscape model
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput');  io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/plant'],      1, 'openoutput'); io_i = io_i + 1; % External metrology [m,rad]

% Prepare simulation
controller = initializeController('type', 'open-loop');
sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'R', 100e-3);

%% Cubic Stewart platform with payload above the top platform - B frame at the CoM
H    = 200e-3; % height of the Stewart platform [m]
MO_B = 50e-3;  % Position {B} with respect to {M} [m]

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart, ...
                                         'Mpm', 1e-6, ... % Massless platform
                                         'Fpm', 1e-6, ... % Massless platform
                                         'Mph', 20e-3, ... % Thin platform
                                         'Fph', 20e-3, ... % Thin platform
                                         'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
                                         'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
stewart = initializeCylindricalStruts(stewart, ...
    'Fsm', 1e-6, ... % Massless strut
    'Msm', 1e-6, ... % Massless strut
    'Fsh', stewart.geometry.l(1)/2, ...
    'Msh', stewart.geometry.l(1)/2 ...
);

% Run the linearization
G_CoM = linearize(mdl, io)*inv(stewart.kinematics.J).';
G_CoM.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_CoM.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};

%% Same geometry but B Frame at cube's center (CoK)
MO_B = -100e-3;  % Position {B} with respect to {M} [m]
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart, ...
                                         'Mpm', 1e-6, ... % Massless platform
                                         'Fpm', 1e-6, ... % Massless platform
                                         'Mph', 20e-3, ... % Thin platform
                                         'Fph', 20e-3, ... % Thin platform
                                         'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
                                         'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
stewart = initializeCylindricalStruts(stewart, ...
    'Fsm', 1e-6, ... % Massless strut
    'Msm', 1e-6, ... % Massless strut
    'Fsh', stewart.geometry.l(1)/2, ...
    'Msh', stewart.geometry.l(1)/2 ...
);

% Run the linearization
G_CoK = linearize(mdl, io)*inv(stewart.kinematics.J.');
G_CoK.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_CoK.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};

%% Coupling in the cartesian frame for a Cubic Stewart platform - Frame {B} is at the center of mass of the payload
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
% for i = 1:5
%     for j = i+1:6
%         plot(freqs, abs(squeeze(freqresp(G_CoM(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
%              'HandleVisibility', 'off');
%     end
% end
plot(freqs, abs(squeeze(freqresp(G_CoM(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$D_x/F_x$');
plot(freqs, abs(squeeze(freqresp(G_CoM(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$D_y/F_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ...
     'DisplayName', '$D_z/F_z$');
plot(freqs, abs(squeeze(freqresp(G_CoM(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ...
     'DisplayName', '$R_x/M_x$');
plot(freqs, abs(squeeze(freqresp(G_CoM(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ...
     'DisplayName', '$R_y/M_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ...
     'DisplayName', '$R_z/M_z$');
plot(freqs, abs(squeeze(freqresp(G_CoM(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$R_x/F_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$R_y/F_x$');
plot(freqs, abs(squeeze(freqresp(G_CoM(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$D_x/M_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$D_y/M_x$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]);
ylim([1e-10, 2e-3])

%% Coupling in the cartesian frame for a Cubic Stewart platform - Frame {B} is at the center of the cube
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
% for i = 1:5
%     for j = i+1:6
%         plot(freqs, abs(squeeze(freqresp(G_CoK(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
%              'HandleVisibility', 'off');
%     end
% end
plot(freqs, abs(squeeze(freqresp(G_CoK(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$D_x/F_x$');
plot(freqs, abs(squeeze(freqresp(G_CoK(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$D_y/F_y$');
plot(freqs, abs(squeeze(freqresp(G_CoK(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ...
     'DisplayName', '$D_z/F_z$');
plot(freqs, abs(squeeze(freqresp(G_CoK(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ...
     'DisplayName', '$R_x/M_x$');
plot(freqs, abs(squeeze(freqresp(G_CoK(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ...
     'DisplayName', '$R_y/M_y$');
plot(freqs, abs(squeeze(freqresp(G_CoK(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ...
     'DisplayName', '$R_z/M_z$');
plot(freqs, abs(squeeze(freqresp(G_CoK(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$R_x/F_y$');
plot(freqs, abs(squeeze(freqresp(G_CoK(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$R_y/F_x$');
plot(freqs, abs(squeeze(freqresp(G_CoK(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$D_x/M_y$');
plot(freqs, abs(squeeze(freqresp(G_CoK(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$D_y/M_x$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]);
ylim([1e-10, 2e-3])

%% Cubic Stewart platform with payload above the top platform
H = 200e-3;
MO_B = -100e-3;  % Position {B} with respect to {M} [m]

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart, ...
                                         'Mpm', 1e-6, ... % Massless platform
                                         'Fpm', 1e-6, ... % Massless platform
                                         'Mph', 20e-3, ... % Thin platform
                                         'Fph', 20e-3, ... % Thin platform
                                         'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
                                         'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
stewart = initializeCylindricalStruts(stewart, ...
    'Fsm', 1e-6, ... % Massless strut
    'Msm', 1e-6, ... % Massless strut
    'Fsh', stewart.geometry.l(1)/2, ...
    'Msh', stewart.geometry.l(1)/2 ...
);

% Sample at the Center of the cube
sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'H_offset', -H/2-50e-3);

% Run the linearization
G_CoM_CoK = linearize(mdl, io)*inv(stewart.kinematics.J.');
G_CoM_CoK.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_CoM_CoK.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};

freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$D_x/F_x$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$D_y/F_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ...
     'DisplayName', '$D_z/F_z$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ...
     'DisplayName', '$R_x/M_x$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ...
     'DisplayName', '$R_y/M_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ...
     'DisplayName', '$R_z/M_z$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$R_x/F_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$R_y/F_x$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$D_x/M_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$D_y/M_x$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]);
ylim([1e-10, 2e-3])

%% Input/Output definition of the Simscape model
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput');  io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/plant'],      2, 'openoutput', [], 'dL'); io_i = io_i + 1; % Displacement sensors [m]
io(io_i) = linio([mdl, '/plant'],      2, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensor [N]

% Prepare simulation : Payload above the top platform
controller = initializeController('type', 'open-loop');
sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'R', 100e-3);

%% Cubic Stewart platform
H    = 200e-3; % height of the Stewart platform [m]
MO_B = 50e-3;  % Position {B} with respect to {M} [m]

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart, ...
                                         'Mpm', 1e-6, ... % Massless platform
                                         'Fpm', 1e-6, ... % Massless platform
                                         'Mph', 20e-3, ... % Thin platform
                                         'Fph', 20e-3, ... % Thin platform
                                         'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
                                         'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
stewart = initializeCylindricalStruts(stewart, ...
    'Fsm', 1e-6, ... % Massless strut
    'Msm', 1e-6, ... % Massless strut
    'Fsh', stewart.geometry.l(1)/2, ...
    'Msh', stewart.geometry.l(1)/2 ...
);

% Run the linearization
G_cubic = linearize(mdl, io);
G_cubic.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_cubic.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
                      'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};

%% Non-Cubic Stewart platform
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = generateGeneralConfiguration(stewart, 'FH', 25e-3, 'FR', 250e-3, 'MH', 25e-3, 'MR', 250e-3, ...
                                      'FTh', [-22, 22, 120-22, 120+22, 240-22, 240+22]*(pi/180), ...
                                      'MTh', [-60+22, 60-22, 60+22, 180-22, 180+22, -60-22]*(pi/180));
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart, ...
                                         'Mpm', 1e-6, ... % Massless platform
                                         'Fpm', 1e-6, ... % Massless platform
                                         'Mph', 20e-3, ... % Thin platform
                                         'Fph', 20e-3, ... % Thin platform
                                         'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
                                         'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
stewart = initializeCylindricalStruts(stewart, ...
    'Fsm', 1e-6, ... % Massless strut
    'Msm', 1e-6, ... % Massless strut
    'Fsh', stewart.geometry.l(1)/2, ...
    'Msh', stewart.geometry.l(1)/2 ...
);

% Run the linearization
G_non_cubic = linearize(mdl, io);
G_non_cubic.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_non_cubic.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
                          'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};

%% Decentralized plant - Actuator force to Strut displacement - Cubic Architecture
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_non_cubic(sprintf('dL%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
             'HandleVisibility', 'off');
    end
end
plot(freqs, abs(squeeze(freqresp(G_non_cubic('dL1', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:)], 'linewidth', 2.5, ...
     'DisplayName', '$l_i/f_i$');
plot(freqs, abs(squeeze(freqresp(G_non_cubic('dL2', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
     'DisplayName', '$l_i/f_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]);
ylim([1e-10, 1e-4])

%% Decentralized plant - Actuator force to Strut displacement - Cubic Architecture
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_cubic(sprintf('dL%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
             'HandleVisibility', 'off');
    end
end
plot(freqs, abs(squeeze(freqresp(G_cubic('dL1', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:)], 'linewidth', 2.5, ...
     'DisplayName', '$l_i/f_i$');
plot(freqs, abs(squeeze(freqresp(G_cubic('dL2', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
     'DisplayName', '$l_i/f_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]);
ylim([1e-10, 1e-4])

%% Decentralized plant - Actuator force to strut force sensor - Cubic Architecture
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_non_cubic(sprintf('fn%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
             'HandleVisibility', 'off');
    end
end
plot(freqs, abs(squeeze(freqresp(G_non_cubic('fn1', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:)], 'linewidth', 2.5, ...
     'DisplayName', '$f_{m,i}/f_i$');
plot(freqs, abs(squeeze(freqresp(G_non_cubic('fn2', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
     'DisplayName', '$f_{m,i}/f_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]); ylim([1e-4, 1e2]);

%% Decentralized plant - Actuator force to strut force sensor - Cubic Architecture
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_cubic(sprintf('fn%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
             'HandleVisibility', 'off');
    end
end
plot(freqs, abs(squeeze(freqresp(G_cubic('fn1', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:)], 'linewidth', 2.5, ...
     'DisplayName', '$f_{m,i}/f_i$');
plot(freqs, abs(squeeze(freqresp(G_cubic('fn2', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
     'DisplayName', '$f_{m,i}/f_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]); ylim([1e-4, 1e2]);

%% Cubic configurations with center of the cube above the top platform
H    = 100e-3; % height of the Stewart platform [m]
MO_B = 20e-3;  % Position {B} with respect to {M} [m]
FOc  = H + MO_B; % Center of the cube with respect to {F}

%% Small cube
Hc   = 2*MO_B;    % Size of the useful part of the cube [m]

stewart_small = initializeStewartPlatform();
stewart_small = initializeFramesPositions(stewart_small, 'H', H, 'MO_B', MO_B);
stewart_small = generateCubicConfiguration(stewart_small, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
stewart_small = computeJointsPose(stewart_small);
stewart_small = initializeStrutDynamics(stewart_small, 'k', 1);
stewart_small = computeJacobian(stewart_small);
stewart_small = initializeCylindricalPlatforms(stewart_small, 'Fpr', 1.1*max(vecnorm(stewart_small.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_small.platform_M.Mb)));

%% ISO View
displayArchitecture(stewart_small, 'labels', false, 'frames', false);
plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');

%% Side view
displayArchitecture(stewart_small, 'labels', false, 'frames', false);
plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');
view([90,0])

%% Top view
displayArchitecture(stewart_small, 'labels', false, 'frames', false);
plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');
view([0,90])

%% Example of a cubic architecture with cube's center above the top platform - Medium cube size
Hc   = H + 2*MO_B;    % Size of the useful part of the cube [m]

stewart_medium = initializeStewartPlatform();
stewart_medium = initializeFramesPositions(stewart_medium, 'H', H, 'MO_B', MO_B);
stewart_medium = generateCubicConfiguration(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
stewart_medium = computeJointsPose(stewart_medium);
stewart_medium = initializeStrutDynamics(stewart_medium, 'k', 1);
stewart_medium = computeJacobian(stewart_medium);
stewart_medium = initializeCylindricalPlatforms(stewart_medium, 'Fpr', 1.1*max(vecnorm(stewart_medium.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_medium.platform_M.Mb)));

%% ISO View
displayArchitecture(stewart_medium, 'labels', false, 'frames', false);
plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');

%% Side view
displayArchitecture(stewart_medium, 'labels', false, 'frames', false);
plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');
view([90,0])

%% Top view
displayArchitecture(stewart_medium, 'labels', false, 'frames', false);
plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');
view([0,90])

%% Example of a cubic architecture with cube's center above the top platform - Large cube size
Hc   = 2*(H + MO_B);    % Size of the useful part of the cube [m]

stewart_large = initializeStewartPlatform();
stewart_large = initializeFramesPositions(stewart_large, 'H', H, 'MO_B', MO_B);
stewart_large = generateCubicConfiguration(stewart_large, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
stewart_large = computeJointsPose(stewart_large);
stewart_large = initializeStrutDynamics(stewart_large, 'k', 1);
stewart_large = computeJacobian(stewart_large);
stewart_large = initializeCylindricalPlatforms(stewart_large, 'Fpr', 1.1*max(vecnorm(stewart_large.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_large.platform_M.Mb)));

%% ISO View
displayArchitecture(stewart_large, 'labels', false, 'frames', false);
plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');

%% Side view
displayArchitecture(stewart_large, 'labels', false, 'frames', false);
plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');
view([90,0])

%% Top view
displayArchitecture(stewart_large, 'labels', false, 'frames', false);
plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');
view([0,90])

%% Get the analytical formula for the location of the top and bottom joints
% Define symbolic variables
syms k Hc Hcom alpha H

assume(k > 0); % k is positive real
assume(Hcom > 0); % k is positive real
assume(Hc > 0); % Hc is real
assume(H > 0); % H is real
assume(alpha, 'real'); % alpha is real

% Define si matrix (edges of the cubes)
si = 1/sqrt(3)*[
  [ sqrt(2),    0,        1]; ...
  [-sqrt(2)/2, -sqrt(3/2), 1]; ...
  [-sqrt(2)/2,  sqrt(3/2), 1]; ...
  [ sqrt(2),    0,        1]; ...
  [-sqrt(2)/2, -sqrt(3/2), 1]; ...
  [-sqrt(2)/2,  sqrt(3/2), 1]  ...
];

% Define ci matrix (vertices of the cubes)
ci = Hc * [
  [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2),  sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2),  sqrt(3)/sqrt(2), 0.5]; ...
  [-sqrt(2),   0,               0.5]; ...
  [-sqrt(2),   0,               0.5]  ...
];

% Apply vertical shift to ci
ci = ci + (H + Hcom) * [0, 0, 1];

% Calculate bi vectors (Stewart platform top joints)
bi = ci + alpha * si;


% Extract the z-component value from the first row of ci
% (all rows have the same z-component)
ci_z = ci(1, 3);

% The z-component of si is 1 for all rows
si_z = si(1, 3);

alpha_for_0 = solve(ci_z + alpha * si_z == 0, alpha);
alpha_for_H = solve(ci_z + alpha * si_z == H, alpha);

% Verify the results
% Substitute alpha values and check the resulting bi_z values
bi_z_0 = ci + alpha_for_0 * si;
disp('Radius for fixed base:');
simplify(sqrt(bi_z_0(1,1).^2 + bi_z_0(1,2).^2))

bi_z_H = ci + alpha_for_H * si;
disp('Radius for mobile platform:');
simplify(sqrt(bi_z_H(1,1).^2 + bi_z_H(1,2).^2))
