function [best_cost, best_state, best_formation_index] = optimize_formation(start_pos, goal_pos, A, b, robot, formations)
%OPTIMIZE_FORMATION Sets up the optimization problem and outputs the best
%aka lowest-cost formation achieved in the polytope defined by A,b.

%% Optimization

% Algorithm Selection
% Here we can enable the explicit use of Cost (Objective) and Constraint
% gradients or disable it.
options = optimoptions(@fmincon, 'Display', 'none', 'Algorithm', 'sqp','SpecifyObjectiveGradient',true,'SpecifyConstraintGradient',false);
% options = optimoptions(@fmincon, 'Display', 'none', 'Algorithm', 'interior-point','SpecifyObjectiveGradient',true,'SpecifyConstraintGradient',true);


qbar = [1,0,0,0];
goal = setGoal(goal_pos', 1, qbar);

q_init = qbar + 0.1 * rand(1,4);
q_init = q_init/norm(q_init,2);

solutions_x = [];
solutions_fval = [];
weights = [1000, 0.5, 0.5];

for i=1:size(formations,2)
    formation = formations(i);
    
    cost_f = @(x) costFunc(x, goal, weights, formation.cost);
    constr_f = @(x) constraints(x, goal, robot, formation, A, b);
   
    % Setting up starting value for optimization problem
    x_init = [goal.position, 2*max([robot.h, robot.r])/formation.min_distance, q_init];
    %Solving optimization problem for each formation
    [x,fval] = fmincon(cost_f,x_init,[],[],[],[],[],[], constr_f, options);
    
    solutions_x = [solutions_x; x];
    solutions_fval = [solutions_fval; fval];

    %% Optional formation graphing
    % clf
    % figure(2)
    % plotregion(-A(:,1:2),-b,[0,0],[15,15], [0.1,0.1,0.1], 0.05);
    % plotFormation(formation.robot_pos + formation.centroid, x);
    % xlim([0,15]);
    % ylim([0,15]);
    % pause(2);
end

solutions_fval;
[bestFormation_cost,bestFormation_i] = min(solutions_fval);

best_cost = bestFormation_cost;
best_state = solutions_x(bestFormation_i,:);
best_formation_index = bestFormation_i;

end
