function fig = plot_world(A, b, C, d, results, lb, ub, g, currentRobots, goalRobots, hide_ellipse, time, form_opt_mode)
    addpath iris-distro-master/src/matlab/
    addpath 'c:/Program Files/Mosek/8/toolbox/r2014a'
    import iris.drawing.*;
    fig = figure(1);
    
    % XY Plane
    sp = subplot(2,1,1);
    A_draw = [A(1:end-4,1:2); A(end-2:end-1,1:2)];
    b_draw = [b(1:end-4); b(end-2:end-1)];
    C_draw = C(1:2,1:2);
    d_draw = d(1:2);
%           draw_3d(A,b,C,d,obstacles,lb,ub);
    draw_2d(A_draw,b_draw,(1-hide_ellipse)*C_draw,(1-hide_ellipse)*d_draw,results.obstacles,lb(1:2),ub(1:2), fig);
    plot(results.start(1), results.start(2), 'go', 'MarkerFaceColor', 'g', 'MarkerSize', 8);
    plot(g(1), g(2), 'bo', 'MarkerFaceColor', 'b', 'MarkerSize', 8);
    plot(currentRobots(1,:), currentRobots(2,:), 'bx');
    plot(goalRobots(1,:), goalRobots(2,:), 'rx');
    xlim(sp,[lb(1) ub(1)]);
    ylim(sp,[lb(2) ub(2)]);
    xlabel('X (m)');
    ylabel('Y (m)');
    title(sprintf('World state at t=%03.1fs \t\t Optimized formation in %s', time, form_opt_mode));
    
    

    % XZ Plane
    subp1 = subplot(2,2,3);
    hold on;
    A_draw = A;
    A_draw(:,2) = [];
    A_draw(end-4,:) = [];
    A_draw(end-1,:) = [];
    b_draw = b;
    b_draw(end-4,:) = [];
    b_draw(end-1,:) = [];
    C_draw = C;
    C_draw(:,2) = [];
    C_draw(2,:) = [];
    d_draw = d;
    d_draw(2,:) = [];
    %draw_2d(A_draw,b_draw,C_draw,d_draw,results.obstacles,lb(1:2),ub(1:2), fig);
    plot(results.start(1), results.start(3), 'go', 'MarkerFaceColor', 'g', 'MarkerSize', 8);
    plot(g(1), g(3), 'bo', 'MarkerFaceColor', 'b', 'MarkerSize', 8);
    plot(currentRobots(1,:), currentRobots(3,:), 'bx');
    plot(goalRobots(1,:), goalRobots(3,:), 'rx');
    xlim(subp1,[lb(1) ub(1)]);
    ylim(subp1,[lb(3) ub(3)]);
    xlabel('X (m)');
    ylabel('Z (m)');
    hold off;
    
    xz_pos = get(subp1, 'position');
    
    added_width = xz_pos(3)*0.3;
    xz_pos(3) = xz_pos(3) + added_width;
    set(subp1, 'position', xz_pos);
    
    
    % YZ Plane
    subp2 = subplot(2,2,4);
    hold on;
    A_draw = A;
    A_draw(:,1) = [];
    A_draw(end-5,:) = [];
    A_draw(end-2,:) = [];
    b_draw = b;
    b_draw(end-5,:) = [];
    b_draw(end-2,:) = [];
    C_draw = C;
    C_draw(:,1) = [];
    C_draw(1,:) = [];
    d_draw = d;
    d_draw(1,:) = [];
    %draw_2d(A_draw,b_draw,C_draw,d_draw,results.obstacles,lb(1:2),ub(1:2), fig);
    plot(results.start(2), results.start(3), 'go', 'MarkerFaceColor', 'g', 'MarkerSize', 8);
    plot(g(1), g(3), 'bo', 'MarkerFaceColor', 'b', 'MarkerSize', 8);
    plot(currentRobots(2,:), currentRobots(3,:), 'bx');
    plot(goalRobots(2,:), goalRobots(3,:), 'rx');
    xlim(subp2,[lb(2) ub(2)]);
    ylim(subp2,[lb(3) ub(3)]);
    xlabel('Y (m)');
    ylabel('Z (m)');
    hold off;
    
    yz_pos = get(subp2, 'position');
    xz_scale = ub(1) - lb(1);
    yz_scale = ub(2) - lb(2);
    new_width = xz_pos(3) * yz_scale / xz_scale;
    yz_pos(3) = new_width;
    yz_pos(1) = yz_pos(1) + added_width;
    
    set(subp2, 'position', yz_pos);
end