classdef Importer < robotics.manip.internal.InternalAccess
%Base class for importer objects that reference URDFshared models

%   Copyright 2018-2023 The MathWorks, Inc.

    properties (Access=private)
        %DecompSettings Struct containing the settings for the decomposition
        DecompSettingsStruct

        %DecompSource String indicating the source for decomposition, if applicable
        DecompSource
    end

    properties (Abstract)

        %ParseInertial Indicates whether to parse inertial parameters
        ParseInertial

        %DataFormat Defines data format for the RigidBodyTree object
        DataFormat

        %CollisionDecomposition Decomposition details to post-process collision import
        CollisionDecomposition

        %MaxNumBodies Maximum number of bodies on the RigidBodyTree object
        %   This property is used only during codegen.
        MaxNumBodies
    end

    methods (Access = ?robotics.manip.internal.InternalAccess)
        function robot = importURDFShared(obj, model, varargin)
        %importURDFShared Import rigidBodyTree object from matlabshared.multibody.internal.urdf.Model
        %   VARARGIN holds the following arguments:
        %   MESHPATH    - Mesh path provided by user
        %   PATHTOURDF  - Directory containing the URDF
        %   SOURCEDATA  - Description file type, either URDF or SDF
        %   FILEINPUT   - Name of the input description file

            if nargin > 2
                meshPath = varargin{1};
                pathToURDF = varargin{2};
                if nargin > 4
                    sourceData = varargin{3};
                    fileInput = varargin{4};
                else
                    sourceData = 'URDF';
                    fileInput = '';
                end
            else
                meshPath = {};
                pathToURDF = '';
                sourceData = 'URDF';
                fileInput = '';
            end

            linkNames = keys(model.Links);
            bodies = containers.Map;

            % Store decomposition settings based on importer inputs
            obj.populateDecompositionSettings(model);

            for i = 1:model.Links.Count
                urdflink = model.Links(linkNames{i});
                body = rigidBody(urdflink.Name);

                % inertial Information
                if obj.ParseInertial
                    if ~isempty(urdflink.Inertial)
                        % URDF contains inertial information
                        xyz = urdflink.Inertial.Origin.xyz;
                        rpy = urdflink.Inertial.Origin.rpy;

                        mass = urdflink.Inertial.Mass;
                        % in URDF, the origin of the inertia frame collocates with CoM
                        ixx = urdflink.Inertial.Inertia.Ixx;
                        iyy = urdflink.Inertial.Inertia.Iyy;
                        izz = urdflink.Inertial.Inertia.Izz;
                        ixy = urdflink.Inertial.Inertia.Ixy;
                        ixz = urdflink.Inertial.Inertia.Ixz;
                        iyz = urdflink.Inertial.Inertia.Iyz;

                        ypr = [rpy(3) rpy(2) rpy(1)];
                        R = eul2rotm(ypr, 'ZYX');  % orientation of inertia frame rel body frame

                        com = xyz;
                        I = [ixx, ixy, ixz; ixy, iyy, iyz; ixz iyz izz];
                        Io = robotics.manip.internal.inertiaTransform(I, mass, R, com);

                        body.Mass = mass;
                        body.CenterOfMass = com;

                        body.Inertia = robotics.manip.internal.flattenInertia(Io);
                    else
                        % URDF does not contain inertial information.
                        % Assign default values (zeros)
                        body.Mass = 0;
                        body.CenterOfMass = [0 0 0];
                        body.Inertia = [0 0 0 0 0 0];
                    end
                end

                % Initialize the collisions if applicable
                bodyInternal = getInternalBody(obj, body);
                if ~isempty(urdflink.Collision) || obj.DecompSource == robotics.core.internal.VHACDConstants.VISUALGEOMSTRING
                    bodyInternal.CollisionsInternal =...
                        robotics.manip.internal.CollisionSet(1);
                end

                if ~isempty(urdflink.Visual)
                    for k = 1:length(urdflink.Visual)
                        xyz = urdflink.Visual(k).Origin.xyz;
                        rpy = urdflink.Visual(k).Origin.rpy;
                        ypr = [rpy(3) rpy(2) rpy(1)];
                        R = eul2rotm(ypr, 'ZYX');
                        T = [R xyz(:); [0 0 0 1]];

                        type = urdflink.Visual(k).Geometry.Type;
                        color = [];
                        if ~isempty(urdflink.Visual(k).Material)
                            color = urdflink.Visual(k).Material.Color;
                        end

                        switch type
                          case 'mesh'
                            parameter = cell(1,2);
                            parameter{2} = urdflink.Visual(k).Geometry.Scale; % scale
                            fn = urdflink.Visual(k).Geometry.FileName; % mesh filename
                            parameter{1} = robotics.manip.internal.RobotDataImporter.findMeshFilePath(fn, pathToURDF, meshPath, sourceData, fileInput);
                          case 'box'
                            parameter = urdflink.Visual(k).Geometry.Size; % [xl, yl, zl]
                          case 'cylinder'
                            parameter = [urdflink.Visual(k).Geometry.Radius, urdflink.Visual(k).Geometry.Length]; % [radius, length]
                          case 'sphere'
                            parameter = urdflink.Visual(k).Geometry.Radius; % radius
                        end

                        type = validatestring(type, {'Mesh', 'Box', 'Cylinder', 'Sphere'});

                        % Note: if the type is mesh, we will ignore the normals read from stl
                        errCode = addVisualInternal(getInternalBody(obj, body), type, parameter, T, color );
                        if errCode == -2
                            %Throw warning when DAE meshes are not readable
                            warning(message('robotics:robothoops:hoopsconvert:CorruptedInputCADFile', parameter{1}));
                        end

                        % If the visuals are intended to be decomposed into
                        % collisions, do that while the parameters from the
                        % visuals are all still known
                        if obj.DecompSource==robotics.core.internal.VHACDConstants.VISUALGEOMSTRING
                            obj.decomposeGeometryAndAddToBody( ...
                                bodyInternal, ...
                                type, ...
                                parameter, ...
                                T, ...
                                obj.DecompSettingsStruct(i));
                        end
                    end
                end

                % If the link has <collision> data associated with it, parse it
                % for the parameters of the collision geometry.
                if ~isempty(urdflink.Collision) && obj.DecompSource~=robotics.core.internal.VHACDConstants.VISUALGEOMSTRING
                    for k = 1:length(urdflink.Collision)
                        collisionParser =...
                            robotics.manip.internal.URDFCollisionParameterParser;

                        [type, parameter, T] = ...
                            collisionParser.extractCollisionParameters(...
                            urdflink.Collision(k), ...
                            meshPath, ...
                            pathToURDF, ...
                            sourceData, ...
                            fileInput);

                        if obj.DecompSource==robotics.core.internal.VHACDConstants.COLLISIONGEOMSTRING
                            % If the decomposition is from the collisions,
                            % decompose and add to the collision objects on
                            % the body
                            obj.decomposeGeometryAndAddToBody( ...
                                bodyInternal, ...
                                type, ...
                                parameter, ...
                                T, ...
                                obj.DecompSettingsStruct(i));
                        else
                            %Add the collision geometry to the RigidBody
                            %bodyInternal based on the parsed type and parameters.
                            addCollisionFromImporter(...
                                bodyInternal, ...
                                type, ...
                                parameter, ...
                                T);
                        end
                    end
                end


                bodies(urdflink.Name) = body;
            end

            baseName = [];
            parents = containers.Map;
            childrenSets = containers.Map;
            for i = 1:model.Links.Count
                childrenSets(linkNames{i}) = {};
            end

            % processing urdf joints
            % shared_multibody makes sure all joint names are unique
            jointNames = keys(model.Joints);
            for i=1:model.Joints.Count
                urdfjoint = model.Joints(jointNames{i});

                % joint name and type
                jtype = urdfjoint.Type;
                if strcmp(jtype, 'continuous')
                    jtype = 'revolute';
                end
                jnt = rigidBodyJoint(urdfjoint.Name, jtype);

                % fixed transform
                if ~isempty(urdfjoint.Origin)
                    xyz = urdfjoint.Origin.xyz;
                    rpy = urdfjoint.Origin.rpy;

                    ypr = [rpy(3) rpy(2) rpy(1)];
                    R = eul2rotm(ypr, 'ZYX');
                    jnt.setFixedTransform([R, reshape(xyz, 3,1);[0 0 0], 1]);
                end

                jointTypesWithoutLimitsAndHomePosition=["fixed","floating"];
                if ~any(strcmp(jnt.Type,jointTypesWithoutLimitsAndHomePosition))
                    % Set the home position. This is a property of the
                    % urdfshared model. For imports from URDF, the value is
                    % always zero since the property does not exist in URDF,
                    % but rather is used for conversion between RBT and
                    % Simscape Multibody.

                    % Joint limits and home positions sometimes have
                    % conflicting values. If a home position is set and
                    % joint limits are applied that no longer allow that
                    % home position, a warning is thrown and the home
                    % position is moved. However, if joint limits are set
                    % and a home position is applied outside these bounds,
                    % an error is thrown. To avoid the error case, this
                    % function first changes the joint limits to an
                    % unbounded interval, making sure to store the default
                    % values set in the rigidBodyJoint constructor. Then,
                    % once the home position has been set, the joint limits
                    % are reapplied, ensuring that the robot is imported
                    % and the home position values are shifted (and a
                    % warning is thrown) if they are not compatible with
                    % the joint limits.
                    defaultPositionLimits = jnt.PositionLimits;
                    jnt.PositionLimits = [-inf, inf];
                    jnt.HomePosition = urdfjoint.HomePosition;

                    % Apply joint limits. If the joint
                    % is continuous, or if the URDFshared joint has
                    % specified joint limits that may differ from the
                    % defaults, the limits must be updated.
                    if strcmp(urdfjoint.Type, 'continuous')
                        % Continuous joints have infinite joint limits.
                        % Setting the values is redundant but is left here
                        % to ensure a clear overview of the different cases
                        jnt.PositionLimits = [-inf, inf];

                    elseif ~isempty(urdfjoint.Limit)
                        % If the source URDFshared joint has joint limits,
                        % update the joint to have those limits
                        lb = urdfjoint.Limit.Lower;
                        ub = urdfjoint.Limit.Upper;

                        % ROS parser supports joint limits irrespective
                        % of lower and upper limit value. Thus, we are
                        % sorting limit values.
                        jnt.PositionLimits = sort([lb, ub]);
                    else
                        % If the source URDFshared joint doesn't have any
                        % joint limits, use the default values
                        jnt.PositionLimits = defaultPositionLimits;
                    end
                end

                % joint axis
                if ismember(jnt.Type, {'revolute', 'prismatic'})
                    if ~isempty(urdfjoint.Axis)
                        jnt.JointAxis = urdfjoint.Axis.xyz;
                    end
                end

                % identify parents, childrenSet, robot base name
                childName = urdfjoint.ChildLink;
                child = bodies(childName);
                child.Joint = jnt;

                parentName = urdfjoint.ParentLink;
                parent = bodies(parentName);
                parentLink = model.Links(parentName);
                if strcmp(parentLink.ParentJoint, '')
                    % shared_multibody makes sure there is only one
                    % root link (base frame) in the model
                    baseName = parentName;
                end

                parents(childName) = parent;

                if isKey(childrenSets, {parentName})
                    tmp = childrenSets(parentName);
                    tmp = [tmp, {child}]; %#ok<AGROW>
                    childrenSets(parentName) = tmp;
                else
                    childrenSets(parentName) = {child};
                end
            end

            % Assemble robot
            robot = rigidBodyTree('MaxNumBodies', obj.MaxNumBodies);
            if isempty(jointNames) % when the model contains only one link and no joint
                                   % Override base to match internal representation with
                                   % visual information.
                overrideBase(obj, robot, bodies(linkNames{1}));
            else
                overrideBase(obj, robot, bodies(baseName));
                bodiesToAdd = childrenSets(baseName);
                while ~isempty(bodiesToAdd)
                    body = bodiesToAdd{1};

                    parent = parents(body.Name);
                    robot.addBody(body, parent.Name);
                    bodiesToAdd = bodiesToAdd(2:end);
                    bodiesToAdd = [childrenSets(body.Name), bodiesToAdd]; %#ok<AGROW>

                end
            end

            % Assign gravity
            if ~isempty(model.Gravity)
                robot.Gravity = model.Gravity;
            end

        end
    end

    methods (Access = {?robotics.manip.internal.InternalAccess})
        function overrideBase(~, robot, body)
        %overrideBase
            robot.TreeInternal.Base = copy(body.BodyInternal);
            robot.TreeInternal.Base.Index = 0;
        end

        function bodyIn = getInternalBody(~, body)
        %getInternalBody
            bodyIn = body.BodyInternal;
        end
    end

    methods (Access = ?matlab.unittest.TestCase)
        function populateDecompositionSettings(obj, model)
            %populateDecompositionSettings Populate decomposition settings struct
            %   Based on the inputs to the importer, define the
            %   decomposition settings struct, which defines whether and
            %   how collision decomposition into convex collisions will be
            %   handled by the importer.

            % By default, decomposition is disabled
            obj.DecompSettingsStruct = robotics.core.internal.defaultVHACDOpts;
            obj.DecompSource = ""; % 1x1 empty string ensures safe usage in comparators

            % Check if the collision decomposition input is a vhacdOptions
            % object or a logical. In the first case, there is more info to
            % extract. 
            if isa(obj.CollisionDecomposition, 'vhacdOptions')
                optionsObj = obj.CollisionDecomposition;
                [obj.DecompSettingsStruct, obj.DecompSource] = robotics.core.internal.VHACDHelpers.readRBTStructArrayFromVHACDOptions(optionsObj, model.Links.Count);
            elseif obj.CollisionDecomposition
                % If it's not an object, it has to be a logical. When this
                % is TRUE, default to collision mesh decomposition.
                optionsObj = vhacdOptions("RigidBodyTree");
                obj.DecompSettingsStruct = robotics.core.internal.VHACDHelpers.readRBTStructArrayFromVHACDOptions(optionsObj, model.Links.Count);
                obj.DecompSource = robotics.core.internal.VHACDConstants.COLLISIONGEOMSTRING;
            end
        end
    end

    methods (Static, Access=?matlab.unittest.TestCase)
        function decomposeGeometryAndAddToBody(bodyInternal, type, parameter, T, decompOptionsStruct)
        %decomposeGeometryAndAddToBody Decompose the input geometry and add it to the rigid body

            if ~strcmpi(type, "mesh")
                % Decomposition is unnecessary; the shape is already convex
                addCollisionFromImporter(...
                    bodyInternal, ...
                    type, ...
                    parameter, ...
                    T);
            else
                result = robotics.manip.internal.readCADFile(parameter{1});
                scale = parameter{2};
                if result.IsSuccess
                    F = result.Faces;
                    V = [scale(1)*result.Vertices(:,1), scale(2)*result.Vertices(:,2), scale(3)*result.Vertices(:,3)];
                    F = robotics.core.internal.PrimitiveMeshGenerator.flipFace(F);
                    [hullArray, numHulls] = robotics.core.internal.computeVHACD(decompOptionsStruct, V, F);
                    for hullIdx = 1:numHulls
                        addCollisionInternal(bodyInternal, type, hullArray(hullIdx).Hull, T);
                    end
                end
            end
        end
    end
end
