function [Li, dLi] = inverseKinematics(stewart, args)
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
%
% Syntax: [stewart] = inverseKinematics(stewart)
%
% Inputs:
%    - stewart - A structure with the following fields
%        - geometry.Aa   [3x6] - The positions ai expressed in {A}
%        - geometry.Bb   [3x6] - The positions bi expressed in {B}
%        - geometry.l    [6x1] - Length of each strut
%    - args - Can have the following fields:
%        - AP   [3x1] - The wanted position of {B} with respect to {A}
%        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
%
% Outputs:
%    - Li   [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
%    - dLi  [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}

arguments
    stewart
    args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
    args.ARB (3,3) double {mustBeNumeric} = eye(3)
end

assert(isfield(stewart.geometry, 'Aa'),   'stewart.geometry should have attribute Aa')
Aa = stewart.geometry.Aa;

assert(isfield(stewart.geometry, 'Bb'),   'stewart.geometry should have attribute Bb')
Bb = stewart.geometry.Bb;

assert(isfield(stewart.geometry, 'l'),   'stewart.geometry should have attribute l')
l = stewart.geometry.l;

Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));

dLi = Li-l;