%-------------------------------------------------------------------------%

%    Copyright (c) 2021 Modenese L.                                       %

%    Author:   Luca Modenese,  2021                                       %

%    email:    l.modenese@imperial.ac.uk                                  %

% ----------------------------------------------------------------------- %

% Function replacing the old getJoint() method that we all loved in OpenSim

% 3.3 but currently non available in OpenSim 4.x.

% Returns the joint for which the specified body would be the child.

% ----------------------------------------------------------------------- %

function bodyJoint = getBodyJoint(osimModel, aBodyName, debug_printout)

import org.opensim.modeling.*;

if nargin<3; debug_printout=0; end

% default

bodyJoint = [];

if strcmp(aBodyName, 'ground')

    return

end

% check if body is included in the model

if osimModel.getBodySet().getIndex(aBodyName)<0

    error(['getBodyJoint.m The specified body ', aBodyName,' is not included in the OpenSim model'])

end

% get jointset

jointSet = osimModel.getJointSet();

% loop through jointset

nj = 1;

for n_joint = 0:jointSet.getSize()-1

    % get cur joint

    cur_joint = jointSet.get(n_joint);

    % child frame from joint

    child_frame = cur_joint.getChildFrame();

    % link back to base frame: this could be a body

    body_of_frame = child_frame.findBaseFrame();

    % get base frame name

    possible_body_name = char(body_of_frame.getName());

    % if body with that name exist than the joint is that body's joint

    if osimModel.getBodySet.getIndex(possible_body_name)>=0 && strcmp(aBodyName, possible_body_name)

        % save the joints with the specified body as Child 

        jointName(nj) = {char(cur_joint.getName())};

        if debug_printout

            disp([aBodyName, ' is parent frame on joint: ',jointName{nj}]);

        end

        % update counter

        nj = nj + 1;

        continue

    end

end

% return a string if there is only one body

if numel(jointName)==1

    jointName = jointName{1};

    bodyJoint = osimModel.getJointSet.get(jointName);

else

    error(['getBodyJoint.m More than one joint connected to body of interest', aBodyName,'. This function is design to work as getJoint() in OpenSim 3.3.'])

end

end