function [Visualizer,Objects]=prepareVisualization(KinDynModel,meshFilePrefix,varargin)
% Creates the figures, loads the meshes, handles some visual effects, and creates the transform objects
% - Inputs:
%   - `KinDynModel` : iDyntreewrappers main variable. Contains the model.
%   - `meshFilePrefix` : Path in which we can find the meshes. As an example the path to the mesh in a iCub urdf is `'package://iCub/meshes/simmechanics/sim_sea_2-5_root_link_prt-binary.stl'
%   `. `meshFilePrefix` should replace package to allow to find the rest of the path. If the value is "", the standard iDynTree workflow of locating the mesh via the ExternalMesh.getFileLocationOnLocalFileSystem method is used.
% - Optional Inputs:
%     - `view` : Selects the angle in which the figure is seen.
%     - `material` : Selects effect with which the patch is rendered. Options are : 'dull','metal','shiny';
%     - `debug` : Enables having extra information for debugging purposes.
%     - `groundOn` : Enables showing a plane as the ground.
%     - `groundColor` : Selects the color of the ground.
%     - `groundTransparency` : Selects the transparency of the ground.
%     - `groundFrame` : Selects the frame in which the ground is attached.
%     - `name` : The name of the figure 
%     - `reuseFigure` : Enable the reuse of an already open figure. It can be the following values:
%         - 'name': Reuse the figure with the same name (the figure is cleared before reusing it). The name must be set.
%         - 'gcf': Reuse the figure returned by gcf.
%         - 'none': Do not reuse the figure (Default).
%     Note: all extra variables are sent to `plotMeshInWorld`
%   - Outputs:
%       - `Visualizer` : Struct containing the following fields
%           - `transforms` : Is the transform objects array. There is one transform for each link
%           - `linkNames`  : The name of the links in the same order of the transforms
%           - `linkNames_idyn`  : The idyntree string vector equivalent to `linkNames`
%           - `NOBJ` : Contains the number of visual objects in the visualizer
%           - `meshHandles` : Has the handles of the mesh objects.
%           - `parent`     : Contains the axes object of the parent figure.
%           - `mainHandler`: Is the handle of the overall figure.
%           - `DEBUG` : Flag stating if the debug mode was set or not.
%           Fields included if debug mode is on:
%           - `map`       : Cell array having both the names of the meshes and the associated link
%           - `linkMeshInfo` : Contains the link name and a struct (meshInfo) that contains the name of file or if is a simple geometry, the triangulation ( edges and vertices of the mesh ) and the link to geometry transform.% Author : Francisco Andrade (franciscojavier.andradechavez@iit.it)
%
% Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
% This software may be modified and distributed under the terms of the
% GNU Lesser General Public License v2.1 or any later version.
%% input parser section
p = inputParser;
p.StructExpand = false;
p.KeepUnmatched= true;
% Default values
default_view=[128.0181 16.8000];
default_material='dull';
default_debug=false;
default_groundOn=false;
default_groundColor=[0 0.5 0.5];
default_groundTransparency=0.8;
default_groundFrame='none';
default_name='iDynTreeVisualizer';
default_reuseFigure='none';
% accepted values
expected_materials={'dull','metal','shiny'};
expected_reuseFigure={'name', 'gcf', 'none'};
% add parameters and validity funcitons
addRequired(p,'structInput');
addRequired(p,'meshFilePrefix',@(x) isstring(x) || ischar(x));
addParameter(p,'view',default_view,@isnumeric);
addParameter(p,'material',default_material,@(x) any(validatestring(x,expected_materials)));
addParameter(p,'debug',default_debug,@(x) islogical(x));
addParameter(p,'groundOn',default_groundOn,@(x) islogical(x));
addParameter(p,'groundColor',default_groundColor,@(x)validateattributes(x,{'numeric'},{'numel', 3}));
addParameter(p,'groundTransparency',default_groundTransparency,@(x) isnumeric(x) && isscalar(x));
addParameter(p,'groundFrame',default_groundFrame,@(x) isstring(x) || ischar(x));
addParameter(p,'name',default_name,@(x) isstring(x) || ischar(x));
addParameter(p,'reuseFigure',default_reuseFigure,@(x) any(validatestring(x,expected_reuseFigure)));

% parse inputs
parse(p,KinDynModel,meshFilePrefix,varargin{:});
options=p.Results;
isNameSet=~any(strcmp(p.UsingDefaults, 'name'));
%% Get meshes from the model variable
model=KinDynModel.kinDynComp.model;
[linkMeshInfo,map]=iDynTreeWrappers.getMeshes(model,meshFilePrefix);
numberOfLinks=length(linkMeshInfo);
linkNames=cell(numberOfLinks,1);
switch options.reuseFigure
    case 'gcf'
        mainHandler=gcf;
        cla(mainHandler.Children);
    case 'name'
        figHandles = findobj('Type', 'figure', 'Name', options.name);
        if isNameSet && ~isempty(figHandles)
            mainHandler=figHandles(1,1);
            cla(mainHandler.Children);
        else
            mainHandler=figure;
        end
    otherwise
        mainHandler=figure;
end
if isNameSet
    set(mainHandler,'Name', options.name,'numbertitle','off')
end
set(0, 'CurrentFigure', mainHandler) %Set the figure as current figure such that gca works
parent=gca;
hold on
linkNames_idyn=iDynTree.StringVector();

for it=1:numberOfLinks
    w_H_link=iDynTreeWrappers.getWorldTransform(KinDynModel,linkMeshInfo(it).linkName);
    [meshHandles(it,:),transforms(it)]=iDynTreeWrappers.plotMeshInWorld(linkMeshInfo(it),w_H_link,varargin{:});
    linkNames{it}=linkMeshInfo(it).linkName;
    linkNames_idyn.push_back(linkNames{it});
end

% Add a camera light, and tone down the specular highlighting
camlight('headlight');
material(options.material);

% Make axis pretty and add grid for visual reference
axis equal;
axis tight;
grid on;

% Apply views
view(parent,options.view);

% Add world reference frame
hold on;
currentAxisValues=axis;
axisRange=currentAxisValues([2,4,6])-currentAxisValues([1,3,5]);
frameAxisSize = min(axisRange)/4;
% linewidth value 1 = to 0.35mm. It is better if we increase the linewidth
% in proportion to the axisSize of the frame.c
linewidthSize=frameAxisSize*50;
iDynTreeWrappers.plotFrame(eye(4), frameAxisSize, linewidthSize);

% Axis labels
title('Robot Visualizer ');
xlabel('{x}');
ylabel('{y}');
zlabel('{z}');

% Draw the ground
if options.groundOn
    % Create plane on x y axis
    normal_vector=[0,0,1];
    point_on_plane=[0,0,0];
    d=normal_vector*point_on_plane';
    x = [1 -1 -1 1]*max(axisRange(1:2)); % Generate data for x vertices
    y = [1 1 -1 -1]*max(axisRange(1:2)); % Generate data for y vertices
    z = 1/normal_vector(3)*(normal_vector(1)*x + normal_vector(2)*y + d); % Solve for z vertices data
    groundHandle=patch('XData',x,'YData',y,'ZData',z,'FaceColor', options.groundColor);
    alpha(groundHandle,options.groundTransparency);
    % apply transform
    groundTransform = hgtransform('Parent',parent);
    set(groundHandle,'Parent',groundTransform);
    if any(ismember(p.UsingDefaults,'groundFrame'))
        w_H_plane=eye(4,4);
        options.groundFrame=linkNames{1};
    else
        w_H_plane_idyn=KinDynModel.kinDynComp.getWorldTransform(options.groundFrame);
        w_H_plane= w_H_plane_idyn.asHomogeneousTransform.toMatlab();
        w_H_plane(1:2,4)=0;
    end
    set(groundTransform,'Matrix',w_H_plane);

    objectFrames_idyn=iDynTree.StringVector();
    objectFrames_idyn.push_back(options.groundFrame);

    % Build Object struct
    Objects.frames_idyn=objectFrames_idyn;
    Objects.transform=groundTransform;
    Objects.frames=options.groundFrame;
    Objects.types={'plane'};
    Objects.names={'ground'};
    Objects.handle=groundHandle;

    if options.debug
        Objects.map=[Objects.names',Objects.frames'];
    end
else
    Objects=[];
end

% Build Output struct
Visualizer.transforms=transforms;
Visualizer.linkNames_idyn=linkNames_idyn;
Visualizer.linkNames=linkNames;
Visualizer.NOBJ=length(transforms);
Visualizer.meshHandles=meshHandles;
Visualizer.mainHandler=mainHandler;
Visualizer.parent=parent;
Visualizer.DEBUG=options.debug;

if options.debug
    Visualizer.map=map;
    Visualizer.linkMeshInfo=linkMeshInfo;
end
