function [stewart] = generateDeltaRobot(stewart, args)
% generateDeltaRobot - Generate a Delta Robot Configuration
%
% Syntax: [stewart] = generateDeltaRobot(args)
%
% Inputs:
%    - args - Can have the following fields:
%        - d [1x1] - Size of the cube [m]
%        - b [1x1] - Distance between top joints bi and cube's vertices [m]
%        - L [1x1] - Length of the struts (i.e. distances between ai and bi) [m]

arguments
    stewart
    args.d (1,1) double {mustBeNumeric, mustBePositive}    = 70e-3
    args.b (1,1) double {mustBeNumeric, mustBeNonnegative} = 50e-3
    args.L (1,1) double {mustBeNumeric, mustBePositive}    = 50e-3
    args.m_top (1,1) double {mustBeNumeric, mustBePositive} = 0.3 % Mass of the top platform
end

% We define the useful vertices of the cube with respect to the Cube's center
% ${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}

sx = [ 2; -1; -1];
sy = [ 0;  1; -1];
sz = [ 1;  1;  1];

R = [sx, sy, sz]./vecnorm([sx, sy, sz]);

% Coordinates of the cube's vertices
Cc = R'*[[args.d/2;-args.d/2;-args.d/2],[args.d/2;args.d/2;-args.d/2],[-args.d/2;args.d/2;-args.d/2],[-args.d/2;args.d/2;args.d/2],[-args.d/2;-args.d/2;args.d/2],[args.d/2;-args.d/2;args.d/2]];

% Coordinates of the cube's vertices corresponding to the six struts, expresssed with respect to the cube's center
CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg
CCm = [Cc(:,6), Cc(:,4), Cc(:,2), Cc(:,6), Cc(:,4), Cc(:,2)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg

% We can compute the vector of each leg si
CSi = (CCm - CCf)./vecnorm(CCm - CCf);

% We now compute the position of the joints ai and bi with respect to the cube's center
Ca = CCf - (args.b + args.L).*CSi;
Cb = CCf - args.b.*CSi;

%% Computes relative position of frames
FO_M = [0; 0; args.L*CSi(3,1)]; % Position of {M} with respect to {F} [m]

MO_B = [0; 0; (args.b+args.d/2)*CSi(3,1)]; % Position of {B} with respect to {M} [m]

FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]

stewart.geometry.d      = args.d; % Size of the cube
stewart.geometry.H      = args.L*CSi(3,1); % Height between {M} and {F} frames
stewart.geometry.FO_M   = FO_M;
stewart.platform_M.MO_B = MO_B;
stewart.platform_F.FO_A = FO_A;

%% Now ai and bi can be expressed with respect to the {F} and {M} frames
Fa = Ca + FO_A;
Mb = Cb + MO_B;

%%
stewart.platform_F.Ca = Ca; % Position of bottom joints ai with respect to {F}
stewart.platform_F.Fa = Fa; % Position of bottom joints ai with respect to {F}
stewart.platform_M.Mb = Mb; % Position of top joints bi with respect to {M}
stewart.platform_M.Cb = Cb; % Position of top joints bi with respect to {M}

stewart.platform_M.m = args.m_top;