function varargout = importrobot(input, varargin )
%

%   Copyright 2016-2023 The MathWorks, Inc.

%#codegen

    narginchk(1,11);

    if isempty(coder.target)
        %Determine type of first input
        [parsedInput, inputType] = processInputs(input, varargin{:});

        %Take different actions depending on the input type
        switch inputType
          case 'Text'
            % Get format for 'Text' input type
            fmt = lower(parsedInput.Results.format);
            switch(fmt)
              case 'urdf'
                % Add name-value pairs for URDF input
                param = {'MeshPath', parsedInput.Results.MeshPath,...
                         'SourceData', 'URDF'};
                importer = robotics.URDFImporter('DataFormat', parsedInput.Results.DataFormat, ...
                                                 'MaxNumBodies', parsedInput.Results.MaxNumBodies, ...
                                                 'CollisionDecomposition', parsedInput.Results.CollisionDecomposition);
                robot = importer.importrobot(parsedInput.Results.input, param{:});
              case 'sdf'
                % Add name-value pairs for SDF input
                param = {'MeshPath', parsedInput.Results.MeshPath,...
                         'SourceData', 'SDF',...
                         'SDFModel',parsedInput.Results.SDFModel};
                importer = robotics.SDFImporter('DataFormat', parsedInput.Results.DataFormat, ...
                                                'MaxNumBodies', parsedInput.Results.MaxNumBodies, ...
                                                'CollisionDecomposition', parsedInput.Results.CollisionDecomposition);
                robot = importer.importrobot(parsedInput.Results.input, param{:});
              case 'xacro'
                % Get URDF string from Xacro file or text input. Also, get
                % the path where Xacro file exist
                [urdfString, pathToDataFile] = robotics.manip.internal.xacroSupport.convertXacroToURDF(parsedInput.Results.input);

                % Add name-value pairs for XACRO input
                param = {'MeshPath', parsedInput.Results.MeshPath,...
                         'SourceData', 'URDF', 'DataPath', pathToDataFile};
                importer = robotics.URDFImporter('DataFormat', parsedInput.Results.DataFormat, ...
                                                 'MaxNumBodies', parsedInput.Results.MaxNumBodies, ...
                                                 'CollisionDecomposition', parsedInput.Results.CollisionDecomposition);

                robot = importer.importrobot(urdfString, param{:});
            end

            %Assign outputs
            varargout{1} = robot;

          case 'Simulink'
            %If the file exist on the path but is not loaded, load it into
            %memory and get the handle. If it is already loaded, remove the
            %extension so only the model name is passed to the importer.
            [~, fileBody] = fileparts(parsedInput.Results.input);
            if ~bdIsLoaded(fileBody)
                sys = load_system(parsedInput.Results.input);
            else
                sys = get_param(fileBody, 'Handle');
            end

            %Create importer object
            importer = robotics.SMImporter('BreakChains', parsedInput.Results.BreakChains, ...
                                           'ConvertJoints', parsedInput.Results.ConvertJoints, ...
                                           'SMConstraints', parsedInput.Results.SMConstraints, ...
                                           'VariableInertias', parsedInput.Results.VariableInertias, ...
                                           'DataFormat', parsedInput.Results.DataFormat, ...
                                           'MaxNumBodies', parsedInput.Results.MaxNumBodies, ...
                                           'CollisionDecomposition', parsedInput.Results.CollisionDecomposition);

            %Import robots and assign outputs
            [robot, importInfo] = importer.importrobot(sys);
            varargout{1} = robot;
            varargout{2} = importInfo;
        end
    else
        %For coder, make an extrinsic call to internal extractStructFromRBT that returns a
        %codegen compatible RBT struct
        rbt = coder.const(@feval, 'robotics.manip.internal.populateRBTStructFromFunction', ...
                          'importrobot', input, varargin{:});

        %Convert RBT struct back to rigidBodyTree object
        robot = rigidBodyTree(robotics.manip.internal.RigidBodyTree(rbt.MaxNumBodies, rbt));

        varargout{1} = robot;
    end


    function [p, idType] = processInputs(argIn, varargin)
        processedInput = convertStringsToChars(argIn);

        %Verify that first input is nonempty
        validateattributes(processedInput, {'char','string','double'},{'nonempty'},'importrobot', '', 1);

        if isnumeric(argIn)
            try %#ok<*EMTC>
                processedInput = get_param(processedInput, 'Name');
            catch
                error(message('robotics:robotmanip:urdfimporter:NumericInputs'));
            end
        end

        %Initialize the output to empty
        idType = [];
        %Initialize the source type to empty
        sourceType = [];

        %Determine the type of conversion based on the main input. To avoid
        %calling Simulink unless it is necessary, try other routes first.
        %There are four valid options:
        %   1) URDF or SDF file (with appropriate extension)
        %   2) Text from a URDF or SDF
        %   3) Simulink model name
        %   4) Name of a subsystem within a larger Simulink model

        %First check if the input is a file with URDF or SDF extension
        [~,~,fileExtension] = fileparts(processedInput);

        % Check 'format' is specified for 'file' or 'text' input
        if nargin > 1
            if strcmpi(varargin{1}, 'sdf')
                % if input FORMAT is 'sdf' then file extension should be
                % any except '.urdf' and '.xacro'
                if strcmpi(fileExtension, '.urdf') || strcmpi(fileExtension, '.xacro')
                    error(message('robotics:robotmanip:urdfimporter:NoMatchingFormat','urdf'));
                else
                    sourceType = 'SDF';
                end
            elseif strcmpi(varargin{1}, 'urdf')
                % if input FORMAT is 'urdf' then file extension should be
                % any except '.sdf' and '.xacro'
                if strcmpi(fileExtension, '.sdf') || strcmpi(fileExtension, '.xacro')
                    error(message('robotics:robotmanip:urdfimporter:NoMatchingFormat','sdf'));
                else
                    sourceType = 'URDF';
                end
            elseif strcmpi(varargin{1}, 'xacro')
                % if input FORMAT is 'xacro' then file extension should be
                % any except '.urdf' and '.sdf'
                if strcmpi(fileExtension, '.urdf') || strcmpi(fileExtension, '.sdf')
                    error(message('robotics:robotmanip:urdfimporter:NoMatchingFormat','xacro'));
                else
                    sourceType = 'Xacro';
                end
            end
        end

        % If 'format' is not specified then select 'idType' as per input
        if isempty(sourceType)
            if strcmpi(fileExtension, '.urdf')
                %Identify URDF files by their file extension
                sourceType = 'URDF';
            elseif strcmpi(fileExtension, '.sdf')
                %Identify SDF files by their file extension
                sourceType = 'SDF';
            elseif strcmpi(fileExtension, '.xacro')
                %Identify Xacro files by their file extension
                sourceType = 'Xacro';
            elseif any(regexpi(processedInput, '<robot', 'once'),'all') && any(regexpi(processedInput, '</robot>', 'once'),'all')
                if contains(processedInput, 'xmlns:xacro="http://')
                    %Identify XacroText, which will include robot
                    %declaration tags as well as xmlns:xacro details
                    sourceType = 'Xacro';
                else
                    %Identify URDFText, which will include robot
                    %declaration tags but don't contains xmlns:xacro
                    %details
                    sourceType = 'URDF';
                end
            elseif any(regexpi(processedInput, '<sdf', 'once'),'all') && any(regexpi(processedInput, '</sdf>', 'once'),'all')
                %Identify SDFText, which will include sdf declaration tags
                sourceType = 'SDF';
            elseif (exist(processedInput, 'file') == 4) || strcmpi(fileExtension, '.mdl') || strcmpi(fileExtension, '.slx')
                %Identify simulink models by file extensions if possible.
                sourceType = 'Simulink';
            else
                %If the string refers to the subsystem of a model, then it must
                %be redirected to the root model in order to be found as a
                %Simulink model
                try %#ok<*EMTC>
                    processedInput = bdroot(processedInput);
                    if exist(processedInput, 'file') == 4
                        sourceType = 'Simulink';
                    end
                catch cause_ME
                    %If none of the above options can be validated, the input
                    %is invalid.
                    base_ME = MException(message('robotics:robotmanip:urdfimporter:InvalidFirstInput'));
                    new_ME = addCause(base_ME, cause_ME);
                    throw(new_ME);
                end
            end
        end

        %Set up input parser and add parameters that work for all input types
        p = inputParser;
        addParameter(p, 'CollisionDecomposition', false);
        addParameter(p, 'DataFormat', 'struct');
        addParameter(p, 'MaxNumBodies', 0);

        % Input-specific name-value pairs
        switch sourceType
          case 'Simulink'
            addRequired(p,'input');
            addParameter(p, 'BreakChains', 'error');
            addParameter(p, 'ConvertJoints', 'error');
            addParameter(p, 'SMConstraints', 'error');
            addParameter(p, 'VariableInertias', 'error');

            idType = 'Simulink';
          case 'URDF'
            addRequired(p,'input');
            addOptional(p,'format', 'urdf', @(x) ischar(validatestring(x, {'urdf'}, 'importrobot', 'format')) );
            addParameter(p,'MeshPath', {});

            if nargin > 1
                % Meshpath can be a cell array of character and/or string inputs
                if nargin > 2 && isa(varargin{end},'cell')
                    cellData = varargin{end};
                    varargin{end} = convertStringsToChars(cellData{:});
                end

                [varargin{:}] = convertStringsToChars(varargin{:});
            end
            idType = 'Text';

          case 'SDF'
            addRequired(p,'input');
            addOptional(p,'format', 'sdf', @(x) ischar(validatestring(x, {'sdf'}, 'importrobot', 'format')) );
            addParameter(p,'MeshPath', {});
            addParameter(p,'SDFModel','', @(x) validateattributes(x,{'char','string'}, {'nonempty'},'importrobot', 'SDFModel') );

            % Meshpath can be a cell array of character and/or string inputs
            % get MeshPath index
            meshIdx = find(strcmp(varargin,'MeshPath') == 1);
            if isempty(meshIdx)
                meshIdx = 1;
            end
            if nargin > 2 && nargin > (meshIdx+1)
                %  mesh should be provided as input, so required to
                %  check meshIdx+1 exist
                if contains('MeshPath',varargin{meshIdx}) &&...
                        isa(varargin{meshIdx+1},'cell')
                    cellData = varargin{meshIdx+1};
                    varargin{meshIdx+1} = convertStringsToChars(cellData{:});
                end
            end
            [varargin{:}] = convertStringsToChars(varargin{:});
            idType = 'Text';

          case 'Xacro'
            addRequired(p,'input');
            addOptional(p,'format', 'xacro', @(x) ischar(validatestring(x, {'xacro'}, 'importrobot', 'format')) );
            addParameter(p,'MeshPath', {});

            if nargin > 1
                % Meshpath can be a cell array of character and/or string inputs
                if nargin > 2 && isa(varargin{end},'cell')
                    cellData = varargin{end};
                    varargin{end} = convertStringsToChars(cellData{:});
                end

                [varargin{:}] = convertStringsToChars(varargin{:});
            end
            idType = 'Text';
        end

        %Parse inputs
        parse(p, processedInput, varargin{:});
    end
end
