function results = run_experiment(get_world_handle, filename, union_poly, obj_avoid_mag)
addpath iris-distro-master/src/matlab/
addpath 'c:/Program Files/Mosek/8/toolbox/r2014a'
addpath  iris-distro-master/src/matlab/iris;
import iris.inflate_region.*;
import iris.drawing.*;
addpath gif_v1.0/gif
import gif.*
addpath optimization_scripts
import optimize_formation.*
addpath optimization_scripts/Drake
import quatRotateVec.*;
import quatProduct.*;
import quatConjugate.*;
import dcross.*;
addpath optimization_scripts/plotregion
import plotregion.*;



%% Get initial (static) stats on world
% Includes obstacles, start/goal positions, robot speeds, etc
[world_A_bounds, world_b_bounds, lb, ub, obstacle_pts, start, goal_points, robots0, robotvel, formationScale, robotScale] = get_world_handle(0, 0);
currentFormationCentroid = start;
goalFormationCentroid = goal_points(:,1);
goal_points(:,1) = [];

%% Get formation information and robot sizes
formations = setFormations(formationScale);
robotSize = setRobot(robotScale);

currentRobotsPosRel = getRobotGoalPosition([0,0,0,1/formationScale,1,0,0,0],formations(1).robot_pos);

%% Set up timing information
iter = 0;
t = 0;
dt = 2.0;
subframes = 20;
ddt = dt / subframes;

%% Time profiling information (under development)
free_region_times = [];
formation_optimization_times = [];

while 1
    iter = iter + 1;
    
    % Generate world information at this time
    [world_A_bounds, world_b_bounds, lb, ub, obstacle_pts, start, g] = get_world_handle(t, 2*dt);
    
    % Generate all polytopes
    goalFormationCentroid;
    try
        t1=datevec(now);
        P_fog = growing_poly(currentRobotsPosRel + currentFormationCentroid, world_A_bounds, world_b_bounds, obstacle_pts, goalFormationCentroid);
        P_og  = growing_poly(currentFormationCentroid, world_A_bounds, world_b_bounds, obstacle_pts, goalFormationCentroid);
        t2 = datevec(now);
        free_region_times = [free_region_times, t2-t1];
        
        P_g   = growing_poly(goalFormationCentroid, world_A_bounds, world_b_bounds, obstacle_pts, goalFormationCentroid);
    catch ME
        ME
    end
    
    % If using combined Pfog+Pog
    if union_poly
        A = [P_fog.A(1:end-6,:); P_og.A];
        b = [P_fog.b(1:end-6,:); P_og.b];
    else
        A = P_fog.A;
        b = P_fog.b;
    end
    
    % Optimize formation in polytope
    t1 = datevec(now);
    [cost, state, formation_index] = optimize_formation(currentFormationCentroid, goalFormationCentroid, A, b, robotSize, formations);
    goalRobotPositions = getRobotGoalPosition( state, formations(formation_index).robot_pos);
    
    % Check validity. If invalid, use Pfog only
    if ~all(A*goalRobotPositions <= b)
        display('Need to plan with P_fog');
        [cost, state, formation_index] = optimize_formation(currentFormationCentroid, goalFormationCentroid, P_fog.A, P_fog.b, robotSize, formations);
        goalRobotPositions = getRobotGoalPosition( state, formations(formation_index).robot_pos);
        form_opt_mode = 'P_{fog}';
    else
        if union_poly
            form_opt_mode = 'P_{fog}\capP_{og}';
        else
            form_opt_mode = 'P_{fog}';
        end
    end
    t2 = datevec(now);
    formation_optimization_times = [formation_optimization_times, t2-t1];
    goalRobotPositions = optimize_goal_assignment(currentFormationCentroid + currentRobotsPosRel, goalRobotPositions);
    
    % Move robots to goal positions    
    for i=1:subframes
        t = t + ddt;
        currentRobotAbsPositions = currentFormationCentroid + currentRobotsPosRel;
        
        total_robot_vel = 0;
        for robotIndex = 1:size(currentRobotsPosRel, 2)
            robot_start_pos = currentRobotAbsPositions(:,robotIndex);
            robot_goal_pos = goalRobotPositions(:,robotIndex);
            
            % Calculate direction towards goal
            v = robotDirectionCommand(robot_start_pos, robot_goal_pos, obstacle_pts, ddt*robotvel, obj_avoid_mag);
            currentRobotAbsPositions(:,robotIndex) = robot_start_pos + v*robotvel*ddt;
            
            total_robot_vel = total_robot_vel + norm(v,2);
        end
        
        currentFormationCentroid = mean(currentRobotAbsPositions,2);
        currentRobotsPosRel = currentRobotAbsPositions - currentFormationCentroid;
        
        fig = plot_world(A, b, P_fog.C, P_fog.d, P_fog.results, lb, ub, goalFormationCentroid, currentRobotAbsPositions, goalRobotPositions,union_poly, t, form_opt_mode);
        frame = getframe(gcf);
        
        % Save to GIF
        if iter == 1 & i == 1
            delete(sprintf('html/%s.gif', filename));
            gif(sprintf('html/%s.gif', filename), 'frame', fig);
        else
            gif();
        end
        
        % Check for goal-reached condition
        err = currentFormationCentroid - goalFormationCentroid;
        if total_robot_vel < 0.1 & err'*err < robotvel*ddt*2
            if isempty(goal_points)
                display('Time Statistics:');
                display(sprintf('IRIS min=%f, mean=%f, max=%f, stddev=%f', min(free_region_times), mean(free_region_times), max(free_region_times), std(free_region_times)));
                display(sprintf('Formation min=%f, mean=%f, max=%f, stddev=%f', min(formation_optimization_times), mean(formation_optimization_times), max(formation_optimization_times), std(formation_optimization_times)));
                return
            else
                goalFormationCentroid = goal_points(:,1);
                goal_points(:,1) = [];
            end
        end
    end
end

end


function dir = robotDirectionCommand(curr, goal, obstacle_pts, max_dist, obj_avoid_mag)
    dir = goal - curr + obj_avoid_mag*passive_obstacle_avoidance(curr, obstacle_pts);
    if norm(dir,2) < 0.55*max_dist
        dir = [0;0;0];
    else
        dir = dir / norm(dir,2);
    end
end


function newGoals = optimize_goal_assignment(curr_robot_positions, goals)
    min_cost = goal_travel_cost(curr_robot_positions, goals);
    newGoals = goals;
    
    n = size(goals,2);
    for i=1:n
        for j=1:i-1
            goals(:,[i,j]) = goals(:,[j,i]);
            cost = goal_travel_cost(curr_robot_positions, goals);
            if cost < min_cost
                min_cost = cost;
                newGoals = goals;
            end
            goals(:,[i,j]) = goals(:,[j,i]);
        end
        for j=i+1:i-n
            goals(:,[i,j]) = goals(:,[j,i]);
            cost = goal_travel_cost(curr_robot_positions, goals);
            if cost < min_cost
                min_cost = cost;
                newGoals = goals;
            end
            goals(:,[i,j]) = goals(:,[j,i]);
        end
    end
    

end

function c = goal_travel_cost(start, endP)
    c = 0;
    for i=1:size(start,2)
       s = start(:,i);
       e = endP(:,i);
       
       c = c + norm(e-s,2)^2;
    end
end