MapDrawer.m

classdef MapDrawer
    properties(Constant=true)
        OBSTACLE_COLOR_DICT = dictionary("vase","blue","toy","magenta")
        GOAL_COLOR = "black";
        START_COLOR = "black";
        FONT_SIZE = 16; 
    end
    properties
        legended_targets = [];
        legended_labels = [];
    end
    methods
        function obj = MapDrawer
        end
        function obstacles(obj,environment)
            arguments
                obj MapDrawer
                environment Environment
            end
            theta_s = linspace(0,2*pi);
            cos_s = cos(theta_s); sin_s = sin(theta_s);
            for i=1:length(environment.obstacles)
                obstacle = environment.obstacles(i);
                plot( ...
                    obstacle.x1+obstacle.safety_margin*cos_s, ...
                    obstacle.x2+obstacle.safety_margin*sin_s, ...
                    Color="black", ...
                    LineWidth=1 ...
                );
                text(obstacle.x1,obstacle.x2,0,string(obstacle.type), ...
                    FontSize=MapDrawer.FONT_SIZE, Color="black", ...
                    HorizontalAlignment="center",VerticalAlignment="middle" ...
                );
            end

        end
        function obj = trajectory(obj,X,options)
            arguments
                obj MapDrawer
                X double
                options.Color string = ""
                options.DisplayName char = ''
            end
            trajectory_plot = plot(X(1,:),X(2,:), ...
                Marker="o", ...
                Color=options.Color, ...
                DisplayName=char(options.DisplayName), ...
                LineWidth=1 ...
            );
            obj.legended_targets = [obj.legended_targets trajectory_plot];
            obj.legended_labels = [obj.legended_labels string(options.DisplayName)];
            
        end
        function finish(obj,environment)
            arguments
                obj MapDrawer
                environment Environment
            end
            xlabel("$x_1$",Interpreter="latex",FontSize=MapDrawer.FONT_SIZE)
            ylabel("$x_2$",Interpreter="latex",FontSize=MapDrawer.FONT_SIZE)

            scatter(environment.x0(1),environment.x0(2),200,"filled", ...
                Color=MapDrawer.START_COLOR, ...
                MarkerFaceColor=MapDrawer.START_COLOR, ...
                MarkerEdgeColor="none", ...
                Marker='pentagram' ...
            );
            text(environment.x0(1),environment.x0(2)+0.1,0,"Start", ...
                Color=MapDrawer.START_COLOR, ...
                HorizontalAlignment="center", ...
                VerticalAlignment="bottom", ...
                FontSize=MapDrawer.FONT_SIZE ...
            );

            scatter(0,0,200,"filled", ...
                Color=MapDrawer.GOAL_COLOR, ...
                MarkerFaceColor=MapDrawer.GOAL_COLOR, ...
                MarkerEdgeColor='none',...
                Marker='pentagram' ...
            );
            text(0,0.1,0,"Goal", ...
                Color=MapDrawer.GOAL_COLOR, ...
                HorizontalAlignment="center", ...
                VerticalAlignment="bottom", ...
                FontSize=MapDrawer.FONT_SIZE ...
            );
            
            axis("equal");
            axis("padded")
            legend(obj.legended_targets,obj.legended_labels, ...
                FontSize=MapDrawer.FONT_SIZE ...
            );
        end
    end
end