%% Helper script to generate plot of available formations

formations = setFormations(1);
clf
figure(2);
hold on;

for j=1:size(formations,2)
    h(j) = subplot(3,2,j);
    hold on;
    initial = formations(j).robot_pos;
    for i=1:size(initial,1) 
        scatter3(initial(i,1), initial(i,2), initial(i,3));
    end
    xlim([-2,2]);
    ylim([-2,2]);
    title(formations(j).name);
    xlabel('X');
    ylabel('Y');
    zlabel('Z');
    view(2);
    if j == size(formations,2)
        view(90,0);
    end;
%     ax = gca;
%     ax.ColorOrderIndex = 1;
end;

pos = get(h,'Position');
new = mean(cellfun(@(v)v(1),pos(1:2)));
set(h(5),'Position',[new,pos{end}(2:end)])