function x_opt = plot(varargin)
%PLOT  Plots the feasible region of a set of constraints
%
% p = plot(C,x,c,n,options)
%
% C:  SET object
% x:  Plot variables [At most three variables]
% c:  color [double] ([r g b] format) or char from 'rymcgbk'
% n:  #vertices [double ]
% options: options structure from sdpsettings


% Author Johan Löfberg
% $Id: plot.m,v 1.18 2010-03-31 14:27:39 joloef Exp $

% Get the onstraints
if nargin<1
    return
end

F = varargin{1};

if length(F)==0
    return;
end

if nargin < 5
    opts = sdpsettings('verbose',0);
else
    opts = varargin{5};
    if isempty(opts)
        opts = sdpsettings('verbose',0);
    end
end

if any(is(F,'uncertain'))
    F = robustify(F,[],opts);
end

if nargin < 3
    color=['rymcgbk']';
else
    color = varargin{3};
    color = color(:)';
    if isempty(color)
        color=['rymcgbk']';
    end
end

% Plot onto this projection (at most in 3D)
if nargin < 2
    x = [];
else
    x = varargin{2};
    if ~isempty(x)
        x = x(:);
        x = x(1:min(3,length(x)));
    end
end

if isempty(F)
    return
end

if any(is(F,'sos'))
    % Use image representation, feels safer (I'm not sure about the logic
    % in the code at the moment. Can the dualization mess up something
    % otherwise...)
    if ~(opts.sos.model == 1)
        opts.sos.model = 2;
    end
    % Assume the variables we are plotting are the parametric
    F = compilesos(F,[],opts,x);
end

% Create a model in YALMIPs low level format
% All we change later is the cost vector
%sol = solvesdp(F,sum(x),opts);
[model,recoverdata,diagnostic,internalmodel] = export(F,[],opts,[],[],0);
if isempty(internalmodel) | (~isempty(diagnostic) & diagnostic.problem)
    error('Could not create model. Can you actually solve problems with this model?')
end
internalmodel.options.saveduals = 0;
internalmodel.getsolvertime = 0;
internalmodel.options.dimacs = 0;

% Try to find a suitable set to plot
if isempty(x)
    if isempty(internalmodel.extended_variables) & isempty(internalmodel.aux_variables)
        x = depends(F);
        x = x(1:min(3,length(x)));
        localindex = 1;
        localindex = find(ismember(recoverdata.used_variables,x));
    else
        % not extended variables
        candidates = setdiff(1:length(internalmodel.c),[ internalmodel.aux_variables(:)' internalmodel.extended_variables(:)']);
        % Not nonlinear variables
        candidates = candidates(find(internalmodel.variabletype(candidates)==0));
        % Settle with this guess
        localindex = candidates(1:min(3,length(candidates)));
        x = localindex;
    end
else
    localindex = [];
    for i = 1:length(x)
        localindex = [localindex find(ismember(recoverdata.used_variables,getvariables(x(i))))];
    end
end

if nargin < 4
    if length(x)==3
        n = 100;
    else
        n = 25;
    end
else
    n = varargin{4};
    if isempty(n)
        if length(x)==3
            n = 100;
        else
            n = 25;
        end
    end
    if ~isa(n,'double')
        error('4th argument should be an integer>0');
    end
end

if ~isempty(internalmodel.integer_variables)
    error('PLOT can currently not display sets involving integer variables');
end

if isempty(internalmodel.binary_variables)
    [x_opt{1},errorstatus] = generateBoundary(internalmodel,x,n,localindex);
else
    if strcmp(internalmodel.solver.tag,'BNB')
        internalmodel.solver = internalmodel.solver.lower;
    end
    nBin = length(internalmodel.binary_variables);
    p = extractLP(internalmodel);
    p = extractOnly(p,internalmodel.binary_variables);
    
    internalmodel.F_struc = [zeros(nBin,size(internalmodel.F_struc,2));internalmodel.F_struc];
    I = eye(nBin);
    internalmodel.F_struc(1:nBin,1+internalmodel.binary_variables) = I;
    internalmodel.K.f = internalmodel.K.f + length(internalmodel.binary_variables);
    errorstatus = 1;
    x_opt = {};
    errorstatus = zeros(1,2^nBin);
    for i = 0:2^nBin-1;
        comb = dec2decbin(i,nBin);
        if checkfeasiblefast(p,comb(:),1e-6)
            internalmodel.F_struc(1:nBin,1) = -comb(:);
            [x_temp,wrong] = generateBoundary(internalmodel,x,n,localindex);
            if ~wrong
                errorstatus(i+1) = 0;
                x_opt{end+1} = x_temp;
            end
        else
            errorstatus(i+1)=0;
        end
    end
end

if all(errorstatus)
    if nargout==0
        plot(0);
    end
elseif nargout == 0
    for i = 1:length(x_opt)
        try
            plotSet(x_opt{i},color(1+rem(i-1,size(color,1)),:),opts);
        catch
        end
    end
end


function [xout,errorstatus] = solvefordirection(c,internalmodel,uv)
internalmodel.c = 0*internalmodel.c;
internalmodel.c(uv) = c;
sol  = feval(internalmodel.solver.call,internalmodel);
xout = sol.Primal;
xout = xout(uv(:));
errorstatus = sol.problem;


function plotSet(x_opt,color,options)
if size(x_opt,1)==2
    patch(x_opt(1,:),x_opt(2,:),color);
else
    K = convhulln(x_opt');
    %p = patch('Vertices', x_opt', 'Faces', K, 'FaceVertexCData', color, 'FaceColor', color);%, 'FaceAlpha', 0.5);
    p = patch('Vertices', x_opt', 'Faces', K,'FaceColor', color);
    set(p,'LineStyle',options.plot.wirestyle);
    set(p,'LineWidth',options.plot.linewidth);
    set(p,'EdgeColor',options.plot.edgecolor);
    set(p,'Facealpha',options.plot.shade);     
    lighting gouraud;
    view(3);
    camlight('headlight','infinite');
    camlight('headlight','infinite');
    camlight('right','local');
    camlight('left','local');
   
%     if ~(isequal(options.plot.wirestyle,'') | isequal(options.plot.wirestyle,'none'))
%         for i = 1:size(K,1) 
%             p1 = K(i,1);p2 = K(i,2);p3 = K(i,3);
%             x1 = x_opt(:,p1);
%             x2 = x_opt(:,p2);
%             x3 = x_opt(:,p3);
%             n = cross(x2-x1,x3-x1);n = n/norm(n);
%             normals(:,i)=n;
%         end
%         for i = 1:size(K,1)    
%             p1 = K(i,1);p2 = K(i,2);p3 = K(i,3);
%             x1 = x_opt(:,p1);
%             x2 = x_opt(:,p2);
%             x3 = x_opt(:,p3);
%             n = cross(x2-x1,x3-x1);n = n/norm(n);
%             cand1 =  setdiff(find(sum((sort(K')' == p1) | (sort(K')' == p2),2)==2),i);
%             cand2 =  setdiff(find(sum((sort(K')' == p1) | (sort(K')' == p3),2)==2),i);
%             cand3 =  setdiff(find(sum((sort(K')' == p2) | (sort(K')' == p3),2)==2),i);
%             cand = [cand1(:);cand2(:);cand3(:)];
%             doplot = 1;
%             if length(cand)>0
%                for j = 1:length(cand)
%                    if norm(n-normals(:,cand(j)))<1e-8
%                        doplot = 0;
%                    end
%                end
%             end
%             if doplot
%            l = line(x_opt(1,K(i,:)),x_opt(2,K(i,:)),x_opt(3,K(i,:)));
%            set(l,'LineStyle',options.plot.wirestyle);
%            set(l,'LineWidth',options.plot.linewidth);
%            set(l,'Color',options.plot.wirecolor);
%             end
%         end
%     end
end




function [x_opt,errorstatus] = generateBoundary(internalmodel,x,n,localindex);

x_opt = [];
phi = [];
errorstatus = 0;
try % Try to ensure that we close h
    if length(x)==2
        mu = 0.5;
    else
        mu=1;
    end
    n_ = n;
    n = ceil(mu*n);
    h = waitbar(0,['Please wait, solving ' num2str(n_) ' problems using ' internalmodel.solver.tag]);
    angles = (0:(n))*2*pi/n;
    if length(x)==2
        c = [cos(angles);sin(angles)];
    else
        c = randn(3,n);
    end
    i=1;
    while i<=n & errorstatus ~=1
        [xi,errorstatus] = solvefordirection(c(:,i),internalmodel,localindex(:));
        if errorstatus == 2
            disp('Discovered unbounded direction. You should add bounds on variables')
        end
        x_opt = [x_opt xi];
        waitbar(i/n_,h)
        i=i+1;
    end
    
    if errorstatus==0 & length(x)==2
        % Close the set
        x_opt = [x_opt x_opt(:,1)];
        
        % Add points adaptively
        pick = 1;
        n = floor((1-mu)*n_);
        for i = 1:1:n
            for j= 1:(size(x_opt,2)-1)
                d = x_opt(:,j)-x_opt(:,j+1);
                distance(j,1) = d'*d;
            end
            [dist,pos]=sort(-distance);
            % Select insertion point
            phii=(angles(pos(pick))+angles(pos(pick)+1))/2;
            xi = solvefordirection([cos(phii);sin(phii)],internalmodel,localindex);
            d1=xi-x_opt(:,pos(pick));
            d2=xi-x_opt(:,pos(pick)+1);
            if d1'*d1<1e-3 | d2'*d2<1e-3
                pick = pick+1;
            else
                angles = [angles(1:pos(pick)) phii  angles((pos(pick))+1:end)];
                x_opt = [x_opt(:,1:pos(pick)) xi  x_opt(:,(pos(pick))+1:end)];
            end
            waitbar((ceil(n_*mu)+i)/n_,h);
        end
    end
    close(h);
catch
    close(h);
end

function pLP = extractLP(p);
pLP = p;
pLP.F_struc = pLP.F_struc(1:p.K.f+p.K.l,:);
pLP.K.q = 0;
pLP.K.s = 0;

function pRed = extractOnly(p,these);
pRed = p;
p.F_struc(:,1+these) = 0;
removeEQ = find(any(p.F_struc(1:pRed.K.f,2:end),2));
removeLP = find(any(p.F_struc(1+pRed.K.f:end,2:end),2));
pRed.F_struc(pRed.K.f+removeLP,:)=[];
pRed.F_struc(removeEQ,:)=[];
pRed.K.f = pRed.K.f - length(removeEQ);
pRed.K.l = pRed.K.l - length(removeLP);
pRed.F_struc = pRed.F_struc(:,[1 1+these]);
pRed.lb = pRed.lb(these);
pRed.ub = pRed.ub(these);