function varargout = importrobot(input, varargin )
%IMPORTROBOT Import rigid body tree model from URDF file, text, or Simscape Multibody model
%   ROBOT = IMPORTROBOT(FILENAME) returns a rigidBodyTree object
%   by parsing the Unified Robot Description Format (URDF) robot description
%   from file FILENAME.
%
%   ROBOT = IMPORTROBOT(URDFTEXT) returns a rigidBodyTree object
%   by parsing the URDF robot description given as a string URDFTEXT.
%
%   ROBOT = IMPORTROBOT(FILENAME, FORMAT) allows the user to specify the
%   type of the robot description file. The only supported FORMAT is
%   'urdf'.
%
%   [ROBOT] = IMPORTROBOT(___, Name, Value) provides additional options
%   specified by Name-Value pair arguments. Available parameter name:
%
%      'MeshPath'     - Search directories for mesh files,
%                       specified as a char vector or string,
%                       or a cell array of char vectors or strings.
%
%   [ROBOT, IMPORTINFO] = IMPORTROBOT(MODEL) returns a
%   rigidBodyTree object imported from the Simscape Multibody
%   components in the Simulink model specified by MODEL. MODEL may be the
%   name of a loaded model or model on the path, or the handle to a
%   Simulink model.
%
%   [ROBOT, IMPORTINFO] = IMPORTROBOT(___, Name, Value) provides additional
%   options specified by Name-Value pair arguments. Available parameter
%   names:
%
%      'BreakChains'        - Indicates whether to throw an error or break 
%                             closed chains specified in the Simulink
%                             model. The imported rigid body tree has the
%                             chain closure joint removed.
%                             Options: 'error' (default) | 'remove-joints'
%
%      'ConvertJoints'      - Indicates whether to convert Simscape
%                             Multibody joints to fixed joints if there is
%                             no rigid body tree equivalent.
%                             Options: 'error' (default) | 'convert-to-fixed'
%
%      'SMConstraints'      - Indicates whether to remove constraint blocks
%                             from the imported rigid body tree.
%                             Options: 'error' (default) | 'remove'
%
%      'VariableInertias'	- Indicates whether to remove variable inertia 
%                             blocks from the imported rigid body tree.
%                             Options: 'error' (default) | 'remove'
%
%   Notes on meshes when the first input is a URDF file or string:
%
%   Assume the raw mesh path retrieved from URDF is denoted as FN. If FN
%   contains ROS-style "package:\\<pkg_name>" prefix, that part of path
%   is removed. Then the mesh file is searched in the following way:
%   1) If FN is an absolute path, it is checked directly with
%      no modification. If nothing is found, no further attempt is made.
%   2) If FN is a relative path, the following folders to which FN might
%      be relative to are tested in order:
%      - User-specified MeshPath
%      - Current directory
%      - MATLAB path
%      - The folder that has the URDF file
%      - One level above the URDF folder
%    3) If 1) and 2) both return nothing, the parser further
%    strips FN to only contain the file name and extension, and
%    appends it directly after each provided MeshPath.
%
%   If the mesh file is still not found, the parser ignores the mesh file
%   path and returns a rigidBody object with no mesh visuals.
%
%   Example when the first input is a URDF file or string:
%      % Import an LBR iiwa manipulator model from URDF file
%      lbr = importrobot('iiwa14.urdf')
%      show(lbr)
%
%      % It's also possible to specify a search directory for the mesh
%      % files indicated in URDF
%      lbr = importrobot('iiwa14.urdf', 'MeshPath', '..\meshes')
%
%      % Import from a minimalistic URDF string
%      s = '<?xml version="1.0" ?><robot name="min"><link name="L0"/></robot>';
%      mini = importrobot(s)
%
%   See also robotics.URDFImporter
%
%   Example when the first input is a Simulink model:
%      % A simscape Multibody license is required to execute these
%      % functions and the following examples
%
%      % Open a simulink model containing Simscape Multibody
%      open_system('sm_import_humanoid_urdf.slx')
%
%      % Generate a rigidBodyTree and an import information
%      % object from the Simulink model
%      [robot, importInfo] = importrobot(gcs)
%
%      % See import details
%      showdetails(importInfo)
%
%   See also robotics.SMImporter, rigidBodyTreeImportInfo

%   Copyright 2016-2019 The MathWorks, Inc.

narginchk(1,9);

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

%Take different actions depending on the input type
switch inputType
    case 'URDF'
        %Input is either a string XML file, or references a URDF file
        param = {'MeshPath', parsedInput.Results.MeshPath};
        fmt = parsedInput.Results.format;
        
        switch(fmt)
            case 'urdf'
                importer = robotics.URDFImporter();
                robot = importer.importrobot(parsedInput.Results.input, 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);
        
        %Import robots and assign outputs
        [robot, importInfo] = importer.importrobot(sys);
        varargout{1} = robot;
        varargout{2} = importInfo;
end



    function [p, idType] = processInputs(argIn, varargin)
        processedInput = convertStringsToChars(argIn);
        
        %Verify that first input is nonempty
        validateattributes(processedInput, {'char','string','double'},{'nonempty'},1);
        
        if isnumeric(argIn)
            try
                processedInput = get_param(processedInput, 'Name');
            catch
                error(message('robotics:robotmanip:urdfimporter:NumericInputs'));
            end
        end
        
        %Initialize the output to empty
        idType = [];
        
        %Determine the type of conversion based on the main input. To avoid
        %calling Simulink unless it necessary, try other routes first.
        %There are four valid options:
        %   1) URDF file (with appropriate extension)
        %   2) Text from a URDF
        %   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 XML extension
        [~,~,fileExtension] = fileparts(processedInput);
        if strcmpi(fileExtension, '.urdf')
            %Identify URDF files by their file extension
            idType = 'URDF';
        elseif ~isempty(regexpi(processedInput, '<robot'))
            %Identify URDFText, which will include robot declaration tags
            idType = 'URDF';
        elseif (exist(processedInput, 'file') == 4) || strcmpi(fileExtension, '.mdl') || strcmpi(fileExtension, '.slx')
            %Identify simulink models by file extensions if possible.
            idType = '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                
                processedInput = bdroot(processedInput);
                if exist(processedInput, 'file') == 4
                    idType = '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
        
        %Set up input parser based on input type
        p = inputParser;
        switch idType
            case 'Simulink'
                addRequired(p,'input');
                addParameter(p, 'BreakChains', 'error');
                addParameter(p, 'ConvertJoints', 'error');
                addParameter(p, 'SMConstraints', 'error');
                addParameter(p, 'VariableInertias', 'error');
            
            case 'URDF'
                p = inputParser;
                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
        end
        
        %Parse inputs
        parse(p, processedInput, varargin{:});
    end
end

