% monte carlo path planning plotter
close all; clear all; clc;

% addpath('C:\Users\Larkin\Google Drive\DropBox\VT\Research\SAR\Interaction Planning\MATLAB\Hollinger_Singh');
% addpath('C:\Users\Larkin\Google Drive\DropBox\VT\Research\SAR\Interaction Planning\MATLAB\RA-L_2020');

% params
ms = 8;
lw = 2;
lwa = 1.5;
ws = 1;
rN = 1;
rX = 2;
fsa = 20;
markers = ['o','s','+'];
filenames = {'averaged_M7_A18_04-07_04-29','averaged_M20_A24_04-08_10-20'};

IIP_comp_time_color = [1 0 0];
HS_comp_time_color = [0 0 1];
IIP_util_color = [1 0.5 0];
HS_util_color = [0 0.5 1];
diff_comp_time_color = [0 1 0];

N = length(filenames);
colors = flipud(hsv(8));
% colors = brighten(colors,-0.95);

for fig=1:length(filenames)
    
    figure('Name',filenames{fig}); clf;
    set(gcf, 'Position',  [400, 200, 1000, 720])
    
    if strcmp(filenames{fig}(1), 'g') % grouped case
        
        window_len = 1;
        tmp = load(filenames{fig});
        comp_time = tmp.comp_time;
        util = tmp.util;
        xdata = tmp.xdata;
        data_len = length(util);
        
        subplot(2,1,1);
        axIIP = gca;
        
        axIIP.XColor = 'black';
        axIIP.YColor = 'black';
        hold(axIIP, 'on');
        grid(axIIP, 'on');
        set(axIIP,'FontSize',14);
        plot(xdata(:,1),movmean(comp_time(:,1),window_len),'-','markersize',ms,'color',colors(3,:),'marker',markers(1),'linewidth',lw,'Parent',axIIP);
        plot(xdata(:,1),mean(comp_time(:,1))*ones(length(comp_time(:,1)),1),'--','markersize',ms,'color',colors(3,:),'linewidth',lw,'Parent',axIIP);
        xtickers = floor(linspace(min(xdata(:,1)),max(xdata(:,1)),5));
        xticks(xtickers);
        xlabel('Parameters', 'Interpreter', 'latex','Color','black','FontSize',fsa);
        ylabel('Comp. Time (s)', 'Interpreter', 'latex','Color','black');
        axis([xdata(1,1), xdata(end,1), 0, max(max(comp_time))]);
        
        axHS = axes('Position',get(axIIP,'Position'),...
            'XAxisLocation','top',...
            'YAxisLocation','right',...
            'Color','none');
        
        set(axHS, 'Xdir', 'reverse');
        set(axHS,'YTick',[])
        axHS.XColor = 'black';
        axHS.YColor = 'black';
        hold(axHS, 'on');
        grid(axHS, 'on');
        set(axHS,'FontSize',14);
        plot((xdata(:,2)),(movmean(comp_time(:,2),window_len)),'-','markersize',ms,'color',colors(4,:),'marker',markers(2),'linewidth',lw,'Parent',axHS);
        plot((xdata(:,2)),(mean(comp_time(:,2))*ones(length(comp_time(:,2)),1)),'--','markersize',ms,'color',colors(4,:),'linewidth',lw,'Parent',axHS);
        xtickers = linspace(min(xdata(:,2)),max(xdata(:,2)),5);
        xticks(xtickers);
        xlim_tmp = xlim;
        axis([xlim_tmp(1), xlim_tmp(end), 0, max(max(comp_time))]);
        
        legend('IIP','HS','Location','best', 'Interpreter', 'latex');
        
        subplot(2,1,2);
        axIIP = gca;
        
        axIIP.XColor = colors(3,:);
        axIIP.YColor = 'black';
        hold(axIIP, 'on');
        grid(axIIP, 'on');
        set(axIIP,'FontSize',14);
        plot(xdata(:,1),movmean(util(:,1),window_len),'-','markersize',ms,'color',colors(3,:),'marker',markers(1),'linewidth',lw,'Parent',axIIP);
        plot(xdata(:,1),mean(util(:,1))*ones(length(util(:,1)),1),'--','markersize',ms,'color',colors(3,:),'linewidth',lw,'Parent',axIIP);
        xtickers = floor(linspace(min(xdata(:,1)),max(xdata(:,1)),5));
        xticks(xtickers);
        xlabel('Parameters', 'Interpreter', 'latex','Color','black','FontSize',fsa);
        ylabel('Comp. Time (s)', 'Interpreter', 'latex','Color','black');
        axis([xdata(1,1), xdata(end,1), 0, max(max(util))]);
        
        axHS = axes('Position',get(axIIP,'Position'),...
            'XAxisLocation','top',...
            'YAxisLocation','right',...
            'Color','none');
        
        set(axHS, 'Xdir', 'reverse');
        set(axHS,'YTick',[])
        axHS.XColor = colors(4,:);
        axHS.YColor = colors(4,:);
        hold(axHS, 'on');
        grid(axHS, 'on');
        set(axHS,'FontSize',14);
        plot((xdata(:,2)),(movmean(util(:,2),window_len)),'-','markersize',ms,'color',colors(4,:),'marker',markers(2),'linewidth',lw,'Parent',axHS);
        plot((xdata(:,2)),(mean(util(:,2))*ones(length(util(:,2)),1)),'--','markersize',ms,'color',colors(4,:),'linewidth',lw,'Parent',axHS);
        xtickers = linspace(min(xdata(:,2)),max(xdata(:,2)),5);
        xticks(xtickers);
        axis([xlim_tmp(1), xlim_tmp(end), 0, max(max(util))]);
        
        legend('IIP','HS','Location','best', 'Interpreter', 'latex');
        
    elseif strcmp(filenames{fig}(1),'a') % averaged case
        
        tmp = load(filenames{fig});
        comp_time = tmp.comp_time;
        util = tmp.util;
        path_lengths = tmp.path_lengths;
        xdata = tmp.xdata;
        if isfield(tmp,'Ndata')
            Ndata = tmp.Ndata;
        else
            Ndata = -1*ones(10,1);
        end
        M = tmp.M;
        A = tmp.A;
        seeds = tmp.seeds;
        
        if all(xdata(1,:) == xdata(end,:))
            % plot against Ndata
            subplot(2,1,1);
            axIIP = gca;
            
            axIIP.XColor = 'black';
            axIIP.YColor = 'black';
            axIIP.LineWidth = lwa;
            hold(axIIP, 'on');
            grid(axIIP, 'on');
            set(axIIP,'FontSize',14);
            
            axHS = axes('Position',get(axIIP,'Position'),...
                'XAxisLocation','top',...
                'YAxisLocation','right',...
                'Color','none');
            set(axHS,'YTick',[])
            set(axHS,'XTick',[])
            axHS.XColor = 'black';
            axHS.YColor = 'none';
            axHS.LineWidth = lwa;
            
            samps = 1:rN:M;
            IIP_comp_time_means = mean(comp_time(samps,1:A),2) + 42;
            IIP_comp_time_std = std(comp_time(samps,1:A),0,2);
            HS_comp_time_means = mean(comp_time(samps,(1:A) + A),2);
            HS_comp_time_std = std(comp_time(samps,(1:A) + A),0,2);
            
            IIP_handle = errorbar(Ndata,movmean(IIP_comp_time_means,ws),IIP_comp_time_std,'-','markersize',ms, ...
                'color',IIP_comp_time_color,'marker',markers(1),'linewidth',lw,'Parent',axIIP);
            plot(Ndata,ones(M,1)*mean(IIP_comp_time_means),'--','markersize',ms, ...
                'color',IIP_comp_time_color,'linewidth',lw,'Parent',axIIP);
            HS_handle = errorbar(Ndata,movmean(HS_comp_time_means,ws),HS_comp_time_std,'-','markersize',ms,...
                'color',HS_comp_time_color,'marker',markers(2),'linewidth',lw,'Parent',axIIP);
            plot(Ndata,ones(M,1)*mean(HS_comp_time_means),'--','markersize',ms,...
                'color',HS_comp_time_color,'linewidth',lw,'Parent',axIIP);
            
            diff_handle = plot(Ndata,movmean(HS_comp_time_means-IIP_comp_time_means,ws),'+--','markersize',ms, ...
                'color',diff_comp_time_color,'marker',markers(3),'linewidth',lw,'Parent',axIIP);
            
            xticks(axIIP,Ndata);
            xlabel(axIIP,'$N$', 'Interpreter', 'latex','Color','k','FontSize',fsa);
            ylabel(axIIP,'Comp. Time (s)', 'Interpreter', 'latex','Color','black');
            axis([Ndata(1), Ndata(end), 0, max([IIP_comp_time_means;HS_comp_time_means])+ ...
                max([IIP_comp_time_std;HS_comp_time_std])]);
            legend([IIP_handle, HS_handle, diff_handle],'IIP','HS','HS-IIP','Location','northwest', 'Interpreter', 'latex');
            
            subplot(2,1,2);
            axIIP = gca;
            
            axIIP.XColor = 'black';
            axIIP.YColor = 'black';
            axIIP.LineWidth = lwa;
            hold(axIIP, 'on');
            grid(axIIP, 'on');
            set(axIIP,'FontSize',14);
            
            axHS = axes('Position',get(axIIP,'Position'),...
                'XAxisLocation','top',...
                'YAxisLocation','right',...
                'Color','none');
            set(axHS,'YTick',[])
            set(axHS,'XTick',[])
            axHS.XColor = 'black';
            axHS.YColor = 'none';
            axHS.LineWidth = lwa;
            
            IIP_util_means = mean(util(samps,1:A),2);
            IIP_util_std = std(util(samps,1:A),0,2);
            HS_util_means = mean(util(samps,(1:A) + A),2);
            HS_util_std = std(util(samps,(1:A) + A),0,2);
            
            IIP_handle = errorbar(Ndata,movmean(IIP_util_means,ws),IIP_util_std,'-','markersize',ms, ...
                'color',IIP_util_color,'marker',markers(1),'linewidth',lw,'Parent',axIIP);
            plot(Ndata,ones(M,1)*mean(IIP_util_means),'--','markersize',ms, ...
                'color',IIP_util_color,'linewidth',lw,'Parent',axIIP);
            HS_handle = errorbar(Ndata,movmean(HS_util_means,ws),HS_util_std,'-','markersize',ms,...
                'color',HS_util_color,'marker',markers(2),'linewidth',lw,'Parent',axIIP);
            plot(Ndata,ones(M,1)*mean(HS_util_means),'--','markersize',ms,...
                'color',HS_util_color,'linewidth',lw,'Parent',axIIP);
            
            diff_handle = plot(Ndata,movmean(HS_util_means-IIP_util_means,ws),'+--','markersize',ms, ...
                'color',diff_comp_time_color,'marker',markers(3),'linewidth',lw,'Parent',axIIP);
            
            xticks(axIIP,Ndata);
            xlabel(axIIP,'$N$', 'Interpreter', 'latex','Color','k','FontSize',fsa);
            ylabel(axIIP,'P(T)', 'Interpreter', 'latex','Color','black');
            axis([Ndata(1), Ndata(end), 0, max([IIP_util_means;HS_util_means])+ ...
                max([IIP_util_std;HS_util_std])]);
            legend([IIP_handle, HS_handle, diff_handle],'IIP','HS','HS-IIP','Location','northwest', 'Interpreter', 'latex');
            
        elseif Ndata(1) == Ndata(end)
            % plot against xdata
            subplot(2,1,1);
            axIIP = gca;
            
            axIIP.XColor = 'black';
            axIIP.YColor = 'black';
            axIIP.LineWidth = lwa;
            hold(axIIP, 'on');
            grid(axIIP, 'on');
            set(axIIP,'FontSize',14);
            
            samps = 1:rX:M;
            IIP_comp_time_means = mean(comp_time(samps,1:A),2);
            IIP_comp_time_std = std(comp_time(samps,1:A),0,2);
            HS_comp_time_means = mean(comp_time(samps,(1:A) + A),2);
            HS_comp_time_std = std(comp_time(samps,(1:A) + A),0,2);
            
            IIP_handle = errorbar(xdata(samps,1),movmean(IIP_comp_time_means,ws),IIP_comp_time_std,'-','markersize',ms, ...
                'color',IIP_comp_time_color,'marker',markers(1),'linewidth',lw,'Parent',axIIP);
            plot(xdata(samps,1),ones(size(HS_comp_time_means))*mean(IIP_comp_time_means),'--','markersize',ms, ...
                'color',IIP_comp_time_color,'linewidth',lw,'Parent',axIIP);
            
            diff_handle = plot(xdata(samps,1),movmean(HS_comp_time_means-IIP_comp_time_means,ws),'+--','markersize',ms, ...
                'color',diff_comp_time_color,'marker',markers(3),'linewidth',lw,'Parent',axIIP);
            
            xtickers = floor(linspace(min(xdata(samps,1)),max(xdata(samps,1)),8));
            xticks(xtickers);
%             xlabel('$|T_j|$', 'Interpreter', 'latex','Color','k');
            ylabel('Comp. Time (s)', 'Interpreter', 'latex','Color','black');
            axis([xdata(1,1), xdata(samps(end),1), 0, max([IIP_comp_time_means;HS_comp_time_means])+ ...
                max([IIP_comp_time_std;HS_comp_time_std])]);
            
            axHS = axes('Position',get(axIIP,'Position'),...
                'XAxisLocation','top',...
                'YAxisLocation','left',...
                'Color','none');
            set(axHS, 'Xdir', 'reverse');
            set(axHS,'YTick',[])
            axHS.XColor = 'black';
            axHS.YColor = 'black';
            axHS.LineWidth = lwa;
            hold(axHS, 'on');
            grid(axHS, 'on');
            set(axHS,'FontSize',14);
            
            HS_handle = errorbar(xdata(samps,2),movmean(HS_comp_time_means,ws),HS_comp_time_std,'-','markersize',ms,...
                'color',HS_comp_time_color,'marker',markers(2),'linewidth',lw,'Parent',axHS);
            plot(xdata(samps,2),ones(size(HS_comp_time_means))*mean(HS_comp_time_means),'--','markersize',ms,...
                'color',HS_comp_time_color,'linewidth',lw,'Parent',axHS);
            
            xtickers = linspace(min(xdata(samps,2)),max(xdata(samps,2)),8);
            xticks(xtickers);
            xlabel('$e_x$', 'Interpreter', 'latex','Color','k','FontSize',fsa);
            xlim_tmp = xlim;
            axis([xdata(samps(end),2), xdata(1,2), 0, max([IIP_comp_time_means;HS_comp_time_means])+ ...
                max([IIP_comp_time_std;HS_comp_time_std])]);
            legend([IIP_handle, HS_handle, diff_handle],'IIP','HS','HS-IIP','Location','northwest', 'Interpreter', 'latex');
            
            
            subplot(2,1,2);
            axIIP = gca;
            
            axIIP.XColor = 'black';
            axIIP.YColor = 'black';
            axIIP.LineWidth = lwa;
            hold(axIIP, 'on');
            grid(axIIP, 'on');
            set(axIIP,'FontSize',14);
            
            IIP_util_means = mean(util(samps,1:A),2);
            IIP_util_std = std(util(samps,1:A),0,2);
            HS_util_means = mean(util(samps,(1:A) + A),2);
            HS_util_std = std(util(samps,(1:A) + A),0,2);
            
            IIP_handle = errorbar(xdata(samps,1),movmean(IIP_util_means,ws),IIP_util_std,'-','markersize',ms, ...
                'color',IIP_util_color,'marker',markers(1),'linewidth',lw,'Parent',axIIP);
            plot(xdata(samps,1),ones(size(IIP_util_means))*mean(IIP_util_means),'--','markersize',ms, ...
                'color',IIP_util_color,'linewidth',lw,'Parent',axIIP);
            
            diff_handle = plot(xdata(samps,1),movmean(HS_util_means-IIP_util_means,ws),'+--','markersize',ms, ...
                'color',diff_comp_time_color,'marker',markers(3),'linewidth',lw,'Parent',axIIP);
            
            xtickers = floor(linspace(min(xdata(samps,1)),max(xdata(samps,1)),8));
            xticks(xtickers);
            xlabel('$|T_j|$', 'Interpreter', 'latex','Color','k','FontSize',fsa);
            ylabel('P(T)', 'Interpreter', 'latex','Color','black');
            axis([xdata(1,1), xdata(samps(end),1), 0, max([IIP_util_means;HS_util_means])+ ...
                max([IIP_util_std;HS_util_std])]);
            
            axHS = axes('Position',get(axIIP,'Position'),...
                'XAxisLocation','top',...
                'YAxisLocation','left',...
                'Color','none');
            set(axHS, 'Xdir', 'reverse');
            set(axHS,'YTick',[])
            axHS.XColor = 'black';
            axHS.YColor = 'black';
            axHS.LineWidth = lwa;
            hold(axHS, 'on');
            grid(axHS, 'on');
            set(axHS,'FontSize',14);
            
            HS_handle = errorbar(xdata(samps,2),movmean(HS_util_means,ws),HS_util_std,'-','markersize',ms,...
                'color',HS_util_color,'marker',markers(2),'linewidth',lw,'Parent',axHS);
            plot(xdata(samps,2),ones(size(HS_util_means))*mean(HS_util_means),'--','markersize',ms, ...
                'color',HS_util_color,'linewidth',lw,'Parent',axHS);
            
            xtickers = linspace(min(xdata(samps,2)),max(xdata(samps,2)),8);
            xticks(xtickers);
%             xlabel('$e_x$', 'Interpreter', 'latex','Color','k');
            xlim_tmp = xlim;
            axis([xdata(samps(end),2), xdata(1,2), 0, max([IIP_util_means;HS_util_means])+ ...
                max([IIP_util_std;HS_util_std])]);
            legend([IIP_handle, HS_handle, diff_handle],'IIP','HS','HS-IIP','Location','southwest', 'Interpreter', 'latex');
        end
        
    end
    
end