% The polygon inflation algorithm (and the inverse polygon inflation
% algorithm).
% Created for the bachelors thesis "On the adaptive polygon inflation
% algorithm and its implementation" with the submission date 20.06.2018 at
% the University of Rostock.
% This is an experimental version. All results should be checked with
% FAC-PACK.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This algorithm is written as a script. It uses functions that are defined
% after the script.
% To use the polygon inflation algorithm, choose par(6) = 0 below. If an AFS
% with one segment is detected, then it will switch to the inverse polygon
% inflation algorithm automatically.
% To calculate the outer boundary, use par(6) = 1
% To use the inverse polygon inflation algorithm (outer and inner boundary)
% use par(6) = 2

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Data input

% Currently there are two possibilities to input a data matrix. 
% It can be simply loaded onto the workspace under the name D.
% Alternatively, the generation of a data matrix from example2.mat can be
% used, it is implemented under the function generate_data:

 % [D,C,A] = generate_data(1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The parameters

% The default values are given in comments
par = ...
    [1e-12 ... % par(1)  = epsilon     = 1e-12 
    	% Accepted negativity in the matrices C and A.
    1e-3 ...   % par(2)  = epsilon_b   = 1e-3 
    	% Terminating parameter of the bisection method.
    1e-10 ...  % par(3)  = epsilon_tol = 1e-10 
        % Negates rounding errors when deciding if a point is feasible.
    1e-3 ...   % par(4)  = delta       = 1e-3 
        % Terminating parameter of the main iteration.
    0.1 ...    % par(5)                = 0.1
        % Multiplier for the pre-bisection step size.
    0 ...      % par(6)                = 0
        % Cases: 0 Polygon inflation, 1 FIRPOL, 2 inverse iteration, 
        % 3 triangle, 4 rectangle, 5 circle, 9 two circles.
    1 ...      % par(7)                = 1
        % Output: 0 surpress the output, 1 see the outputs.
    100 ...    % par(8)                = 100
        % Maximum number of steps in pre-bisection stage.
    190 ...    % par(9)                = 190
        % Vertices with larger interior angles not accepted/removed 
        % in polygon inflation.
    45 ...     % par(10) = theta       = 45
        % Rotation angle (deg) if the initial direction was unsuccessful
    0 ...      % par(11) = alpha_min   = 0
        % Vertices with smaller interior angles not accepted/removed 
        % in inverse inflation. This parameter does not influence anything
    300 ...    % par(12) = alpha_max   = 200
        % Vertices with larger interior angles not accepted/removed 
        % in inverse inflation.
    ];
% Initial direction for the first vertex
v_init = [-1 ; 0];
v = v_init;
% Initial step size for the pre-bisection stages of the first two vertices
init_step = 1; 
% Initial point for cases with triangle, rectangle etc.
P_0.loc = [0.345 0.225];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Main variables

% During the iteration, vertices are saved in the structure P:
% P(i).loc is a 1x2 vector, containing the location (alpha and beta) value
% for a vertex
% P(i).S is a 2x2 matrix S, used for next approximations
% P(i).div is a scalar, containing Delta_i for the edge i, i+1 
% (gain-of-height)

% When the iteration is finished, the P(i).loc are saved in AFS_cell{it}
AFS_cell = cell(1,3);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Other variables

% Variable that determines which vertex will be removed if a subdivision 
% fails. It changes after each removal.
removePreviousOrNext = 1;

% Variable that changes the starting location for the search of the new
% vertex. There are five implemented cases. Case 1 starts the search
% from the midpoint.
caseDivideEdge = 1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Statistics

stats = struct;
% Some data about the iteration it are saved in the structure stats
stats.count =  zeros(8,4);
% stats.count(1,it) number of times other starting matrix S2 is needed
% stats.count(2,it) number of times ga is needed
% stats.count(3,it) number of times when an infinite loop inwards occurs
% stats.count(4,it) number of times when an infinite loop outwards occurs
% stats.count(5,it) number of times when a point was not added by the angle
% stats.count(6,it) number of times when a point was removed by the angle
% stats.count(7,it) number of times when a point was removed after 5
% unsuccessful subdivisions
% stats.count(8,it) total number of vertices for the segment
% stats.count(:,4) is reserved for the sum of the values of each segment
stats.steps = cell(1,3);
% The number of steps in the pre-bisection and bisection stages for each
% vertex are saved in the first and second row. 
stats.Delta = cell(1,3);
% The minimal Delta_i (gain-of-height) for each subdivision

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test for irreducibility

% [row_size,column_size]=size(D);
% D1=(eye(row_size)+D*D')^(row_size-1);
% D2=(eye(column_size)+D'*D)^(column_size-1);
% if min(D1(:))<=0 || min(D2(:))<=0
%     disp('Irreducibility not satisfied')
%     return
% end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialization
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if par(6)==0 || par(6)==1 || par(6)==2
    % Singular value decomposition of D
    [U,Sigma,V] = svd(D);
    % Creating a positive first singular vector and fixing the orientation
    % of the AFS
    for i = 1:3
        if max(V(:,i))>=max(-V(:,i))
            p(i) = 1;
        else
            p(i) = -1;
        end
    end
    U_hat = U(:,1:3)*diag([p(1) p(2) p(3)]);
    Sigma_hat = Sigma(1:3,1:3);
    V_hat = V(:,1:3)*diag([p(1) p(2) p(3)]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Finding the initial point (only when A is known)

%     T=A*V_hat;
%     T(1,:)=T(1,:)./T(1,1);
%     T(2,:)=T(2,:)./T(2,1);
%     T(3,:)=T(3,:)./T(3,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Finding the initial point (when A is unknown) with nnmf initialization

%     test_par=[0 0 0 0];
%     rng('default')    
%     while test_par(1)*test_par(2)*test_par(3)==0 && test_par(4)<1000
%         test_par(4)=test_par(4)+1;      
%         [C,A]=nnmf(D,3,'algorithm','mult');
%          fprintf('nnmf initialization for the %d. time. ',...
%             'norm(D-C*A)=%d\n',test_par(4),norm(D-C*A))
%         sumy(test_par(4))=norm(D-C*A);
%         T=A*V_hat;
%         T(1,:)=T(1,:)./T(1,1);
%         T(2,:)=T(2,:)./T(2,1);
%         T(3,:)=T(3,:)./T(3,1);
%         for it=1:3
%             P_0.loc = T(it,2:3);
%             %Initial matrix for lsqnonlin
%             %init_matrix=eye(2,2);
% 
%             %???? Saprast kas te notiek
%             P_0.S=T(:,2:3);
%             P_0.S(it,:)=[];
%             [n,S]=F(P_0.loc,P_0.S,U_hat,Sigma_hat,V_hat,par);
% 
%             if n>=par(3)
%                 if par(7)==1
%                     disp('Initial point point is ouside the set.')
%                 end
%                 test_par(it)=0;
%             else
%                 if par(7)==1
%                     disp('Initial point is inside the set.')
%                 end
%                 test_par(it)=1;
%             end
%         end
%     end
%     T=A*V_hat;
%     T(1,:)=T(1,:)./T(1,1);
%     T(2,:)=T(2,:)./T(2,1);
%     T(3,:)=T(3,:)./T(3,1);  
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Finding the initial point (when A is unknown) with ga-lsqnonlin
% initialization
    
    % Initialization parameters
    test_par = [0 0 0 0];
    % test_par(1:3) are set to 1 if a feasible starting point has been
    % found for each of the three segments
    % test_par(4) counts the number of searches
    
    % Default random number generator is used for the first search
    rng('default')    
	while test_par(1)*test_par(2)*test_par(3)==0 && test_par(4)<1000
    	test_par(4) = test_par(4)+1; % Number of searches
        % Searching for matrix T
        optionsga.Display = 'none';
        [mat,~] = ga(@(S) fun_ga_init(S,U_hat,Sigma_hat,V_hat,par),6,[],...
            [],[],[],[],[],[],[],optionsga);
        options = optimoptions('lsqnonlin','Display','none',...
            'FunctionTolerance',1e-12,'OptimalityTolerance',1e-12);
        [mat,n] = lsqnonlin(@(S) fun_init(S,U_hat,Sigma_hat,V_hat,par),...
            mat,[],[],options);
        T = [1 mat(1) mat(2);1 mat(3) mat(4);1 mat(5) mat(6)];
        if par(7) == 1
            fprintf('ga-lsqnonlin initialization for the %d. time.\n',...
                test_par(4))
        end
        % Testing the initial points for three segments
        for it=1:3
            P_0.loc = T(it,2:3);
            P_0.S=T(:,2:3);
            P_0.S(it,:)=[];
            [n,S]=F(P_0.loc,P_0.S,U_hat,Sigma_hat,V_hat,par);
            if n>=par(3)
                 if par(7)==1
                     fprintf('Initial point %d is not feasible.\n',it)
                 end
                 test_par(it)=0;
            else
                if par(7)==1
                    fprintf('Initial point %d is feasible.\n',it)
                end
                test_par(it)=1;
            end
        end
	end 
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The Iteration for the segments of the AFS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for it=1:3 % For a three-component system
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
% If the user wants to start with the inverse iteration, a different case
% than simply FIRPOL is needed. This lets the programm know that the user
% wants FIRPOL and the inner boundary. Otherwise the programm would stop 
% after FIRPOL. First iteration is simply skipped.

    if par(6) == 2 && it == 1
        par(6) = 1;
        if par(7) == 1
            disp('Calculating FIRPOL.')
        end
        continue
    end
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Defining the structure P for saving vertices
    
    P = struct; 
    % One does not need to clear after each iteration
    P(1).loc = [0 0];
    P(3).loc = [0 0];
    P(1).S = eye(2);
    P(1).div = 1;
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initial point

    if par(6) == 0
        P_0.loc = T(it,2:3);
        % S is a 2 by 2 matrix and consists of the remaining rows of the
        % matrix T        
        P_0.S=T(:,2:3);
        P_0.S(it,:)=[]; 
    elseif par(6)==2 || par(6)==1
        P_0.loc = [0 0];
        % Defining S that is not needed for this case
        P_0.S = eye(2);
    else
        % Inital point was defined at the beginning
        % Defining matrices that are not needed for this case
        U_hat = eye(1);
        Sigma_hat = eye(1);
        V_hat = eye(1);
        P_0.S = eye(2);
    end
    % Checking whether initial point is feasible
    if par(6) ~= 2
        % For the inverse inflation, the initial point is not feasible
        [n,S] = F(P_0.loc,P_0.S,U_hat,Sigma_hat,V_hat,par);
        if n >= par(3)
            disp('Error. Initial point point is not feasible.')
            return
        else
            if par(7)==1
                disp('Initial point is feasible.')
            end
        end
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculation of the initial quadrilateral (Vertices are ordered 
% counter-clockwise)
    
    quadrilateralFound = 0; % When initial quadrilateral is found, this 
        % variable is 1
    dotSetFound = 0; % When no area could be found in any direction, this 
        % variable is 1. It is assumed that a dot set has been found.
    while quadrilateralFound == 0 % Search for the 1. vertex
        if par(7)==1
            fprintf('Checking the direction [%d,%d].\n', v(1),v(2))
        end
        [P(1),iferr,stat] = intersection(P_0.loc,v,init_step,P_0.S,...
            P_0.S,U_hat,Sigma_hat,V_hat,par);
        % Statistics
        stats.count(1:4,it) = stats.count(1:4,it)+stat(1:4);
        stats.steps{it} = [stats.steps{it} stat(5:6)];
        % If no errors, search for the 2. vertex
        if iferr == 0
            if par(7) == 1
                disp('The number of caluclated vertices is 1.')  
            end
            [P(3),iferr,stat] = intersection(P_0.loc,-v,init_step,P_0.S,...
                P(1).S,U_hat,Sigma_hat,V_hat,par);
            % Statistics
            stats.count(1:4,it) = stats.count(1:4,it)+stat(1:4);
            stats.steps{it} = [stats.steps{it} stat(5:6)];
            % If no errors, search for the 3. vertex
            if iferr == 0 && norm(P(1).loc-P(3).loc) >= 1e-3 
                % The vertices should not be too close together 
                % The default parameter is 1e-3
                if par(7) == 1
                    disp('The number of caluclated vertices is 2.')  
                end
                [P(2),iferr,stat] = new_point(P(1),P(3),U_hat,Sigma_hat,...
                    V_hat,caseDivideEdge,par);
                % Statistics
                stats.count(1:4,it) = stats.count(1:4,it)+stat(1:4);
                stats.steps{it} = [stats.steps{it} stat(5:6)];
                % If no errors, search for the 4. vertex
                if iferr == 0
                    if par(7) == 1
                        disp('The number of caluclated vertices is 3.')  
                    end
                    P(1).div = P(2).div; % Assigning subdivision history
                    [P_temp,iferr,stat] = new_point(P(3),P(1),U_hat,...
                        Sigma_hat,V_hat,caseDivideEdge,par);
                    % Statistics
                    stats.count(1:4,it) = stats.count(1:4,it)+stat(1:4);
                    stats.steps{it} = [stats.steps{it} stat(5:6)];
                    if iferr == 0
                        if par(7) == 1
                            disp('The number of caluclated vertices is 4.')
                            disp('Initial quadrilateral is constructed.')  
                        end
                        P=[P P_temp]; % Adding the fourth vertex
                        P(3).div = P(4).div; % Assigning subdivision 
                            % history
                        quadrilateralFound = 1; % Breaking the loop
                    end
                end
            end
        end
        if quadrilateralFound == 0
            % If the quadrilateral could not be constructed, then the
            % direction vector is rotated and the search is repeated
            if par(7) == 1
                fprintf('Rotating initial vector by %d degrees.', par(10))
            end
            v=[cosd(par(10)) -sind(par(10));sind(par(10)) cosd(par(10))]*v;
            if norm(v_init-v) <= 10e-5 
                % Criteria to break the loop in case of trying the same 
                % direction again. Default value is 10e-5
                disp(['Could not find area in any direction.',...
                    'Saving it as a point set and continuing with ',...
                    'the next iteration (segment).']) 
                if par(7) == 1
                    disp('Possibly, rotation angle should be decreased.')
                end
                AFS_cell{it} = vertcat(P_0(:).loc); % Saving the point
                dotSetFound = 1; % Labeling a dot set to skip the iteration
                quadrilateralFound = 1; % Breaking the loop
                stats.count(8,it) = 1; % Statistics
            end
        end
    end
    if dotSetFound == 1
        continue
        % In case of a point set, programm should skip to next initial
        % point, as there does not exist any area
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Main iteration for a single segment

    i = 4; % i represents the current amount of vertices
    gain = par(4)+1; % Some value to start the while loop   
    while gain > par(4)
        
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Finding a new vertex

        % Finding the maximal gain of area
        [gain,j]=max([P(:).div]); 
        % Statistics
        stats.Delta{it} = [stats.Delta{it} gain];
        % Next vertex after j is l. The last vertex has an edge with the 
        % first one.
        if j == i 
            l = 1;
        else
            l = j+1;
        end
        % Calculating a new vertex
        [P_new,iferr,stat] = new_point(P(j),P(l),U_hat,Sigma_hat,V_hat,...
            caseDivideEdge,par);
        % Statistics
        stats.count(1:4,it) = stats.count(1:4,it)+stat(1:4);
        stats.steps{it} = [stats.steps{it} stat(5:6)];
        % If no errors, then check the angle of the new vertex
        if iferr == 0
            % Direction vector for one edge P_new->P_j
            x1 = (P(j).loc-P_new.loc)/norm(P(j).loc-P_new.loc); 
            % Direction vector for the other edge P_new->P_l
            x2 = (P(l).loc-P_new.loc)/norm(P(l).loc-P_new.loc); 
            % Calculating the four quadrant inverse tan in degrees for both
            % vectors
            a1 = atan2d(x1(2),x1(1));
            a2 = atan2d(x2(2),x2(1));
            % Calculating the actual angles
            if a1 >= 0 && a2 <= 0
                angle = a1-a2;
            elseif a1 <= 0 && a2 >= 0
                angle = 360+a1-a2;
            elseif a1*a2 >= 0 && a1 >= a2
                angle = a1-a2;
            elseif a1*a2 >= 0 && a1 <= a2
                angle = 360-a2+a1;
            end
            if par(6) == 2 
                % The vertices are ordered in reverse for the inner
                % boundary
                angle = 360-angle;
            end
        end
        
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Checking whether the angle is valid
        
        if par(6) == 0 || par(6) == 2
            % It is only needed when optimization routine is used
            if (angle < par(9) && iferr == 0 && par(6) ~= 2) ||...
                (angle < par(12) && angle > par(11) && iferr == 0 &&...
                par(6) == 2)
                % Polygon inflation and inverse polygon inflation uses
                % different conditions.
                % Adding a new vertice in correct position
                P = [P(1:j) P_new P(j+1:end)]; 
                if par(7) == 1
                    fprintf(['New vertex in position %d. The number',...
                        ' of caluclated vertices is %d.\n'],j+1,i+1)
                end
                P(j).div = P(j+1).div; % Assigning Delta_i (gain-of-height)
                i = i+1; % Changing the current number of vertices
                caseDivideEdge = 1; % Search for a new vertex from the 
                % midpoint of an edge
            else
                % The new vertex must be wrong, trying again and dividing
                % the in a different place
                caseDivideEdge = caseDivideEdge+1;
                if caseDivideEdge <= 5
                    % Statistics
                    stats.count(5,it)=stats.count(5,it)+1;
                    % Error messages
                    if par(7)==1 && iferr == 0
                        fprintf(['New vertex in position %d must be ',...
                            'wrong. The inner angle was %d, but the ',... 
                            'maximal parameter was %d. Trying %d.',...
                                ' time.\n'] ...
                            ,j+1,angle,par(9),caseDivideEdge)
                    end
                    if par(7) == 1 && caseDivideEdge <= 5 && iferr == 1
                        fprintf(['New vertex in position %d must be ',...
                            'wrong. Pre-bisection results in an ',...
                            'infinite loop. Trying %d. time.\n'] ...
                            ,j+1,caseDivideEdge)
                    end
                else
                    % If all possible subdivisions have been tried, then a
                    % vertex should be removed.
                    % Statistics
                    stats.count(7,it) = stats.count(7,it)+1;
                    if removePreviousOrNext == 1
                        % Removing the next vertex
                        if j == i %Removing the first vertex
                            % Setting the Delta_i
                            P(j).div = max(P(j).div,P(1).div); 
                            P = [P(2:j)]; % Actual removal
                        else
                            % Setting the Delta_i
                            P(j).div = max(P(j).div,P(j+1).div); 
                            P = [P(1:j) P(j+2:end)]; % Actual removal
                        end
                        if par(7) == 1
                            disp(['Unsuccessfully tried to divide a ',...
                                'vertex 5 times. Removing the new and',...
                                ' the next vertex.'])
                        end
                    else
                        % Removing the previous vertex
                        if j == 1
                            % Setting the Delta_i
                            P(i).div = max(P(i).div,P(j).div);  
                        else
                            % Setting the Delta_i
                            P(j-1).div = max(P(j-1).div,P(j).div);  
                        end
                        P = [P(1:j-1) P(j+1:end)];  %Actual removal
                        if par(7) == 1
                        	disp(['Unsuccessfully tried to divide a ',...
                                'vertex 5 times. Removing the new and',...
                                ' the previous vertex.'])
                        end
                    end
                    if par(7) == 1
                        fprintf(['The number of caluclated vertices',...
                            ' is %d.\n'],i-1)
                    end
                    removePreviousOrNext = removePreviousOrNext*(-1);
                    i = i-1; % Changing the current number of vertices
                    caseDivideEdge = 1; % Search for a new vertex from the 
                    % midpoint of an edge
                end
            end    
        else
            % Adding a new vertex in the correct position when the angle
            % test was not needed
            P = [P(1:j) P_new P(j+1:end)]; 
            if par(7)==1
                fprintf(['New vertex in the position %d. The number of',...
                    ' caluclated vertices is %d.\n'],j+1,i+1)
            end
            P(j).div = P(j+1).div; % Assigning Delta_i (gain-of-height)
            i = i+1; % Changing the current number of vertices
            caseDivideEdge = 1; % Search for a new vertex from the midpoint
            % of an edge
        end
        
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Checking the angles of all vertices in the polyogon

        if par(6) == 0 || par(6) == 2
            vertexTested = 0; % Is set to 1 when all vertices are 
            % successfully tested. When a vertex is removed, the for-loop
            % is started over because the number of vertices has changed
            while vertexTested == 0
                for j = 1:i
                    % Here j n and l are thee consecutive vertices
                    n = j+1;
                    l = j+2;
                    if j == i-1
                        % If l is the first vertex
                        l = 1;
                    end
                    if j == i
                        % If n is the first vertex
                        n = 1;
                        l = 2;
                        % All vertices have been succesfully checked
                        vertexTested = 1; 
                    end
                    % Direction vector for one edge P_new->P_j
                    x1 = (P(j).loc-P(n).loc)/norm(P(j).loc-P(n).loc);
                    % Direction vector for the other edge P_new->P_l
                    x2 = (P(l).loc-P(n).loc)/norm(P(l).loc-P(n).loc);
                    % Calculating the four quadrant inverse tan in degrees
                    % for both vectors
                    a1 = atan2d(x1(2),x1(1));
                    a2 = atan2d(x2(2),x2(1));
                    % Calculating the actual angles
                    if a1 >= 0 && a2 <= 0
                        angle = a1-a2;
                    elseif a1 <= 0 && a2 >= 0
                        angle = 360+a1-a2;
                    elseif a1*a2 >= 0 && a1 >= a2
                        angle = a1-a2;
                    elseif a1*a2 >= 0 && a1 <= a2
                        angle = 360-a2+a1;
                    end
                    if par(6) == 2
                        % The vertices are ordered in reverse for the inner
                        % boundary
                        angle = 360-angle;
                    end
                    % Removal of a vertex
                    if (angle > par(9) && par(6) ~= 2) || ...
                            ((angle < par(11) || angle > par(12)) && ...
                            par(6) == 2)
                        if par(7) == 1
                            fprintf(['Inspection of all vertices ',...
                                'resulted in a removal of a vertex',...
                                ' from postion %d.\nThe inner angle ',...
                                'was %4.1f, but the maximal parameter ',...
                                'was %4.1f. The number of caluclated ',...
                                'vertices is %d.\n'],n,angle,...
                                par(9),i-1)
                        end
                        % Statistics
                        stats.count(6,it) = stats.count(6,it)+1;
                        if j == i-1 % Removing the last vertex
                            % Setting the Delta_i
                            P(j).div = max(P(j).div,P(j+1).div);
                            P = P(1:j); % Actual removal
                        elseif j == i % Removing the first vertex
                            % Setting the Delta_i
                            P(j).div = max(P(j).div,P(1).div);
                            P = P(2:j); % Actual removal
                        else
                            % Setting the Delta_i
                            P(j).div = max(P(j).div,P(j+1).div); 
                            P = [P(1:j) P(l:end)]; % Actual removal
                        end
                        i = i-1; % Changing the current number of vertices
                        break % Because the size of loop has changed
                    end
                end
            end
        end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot while running
    
%         loc_temp=vertcat(P(:).loc);
%         plot(loc_temp(:,1),loc_temp(:,2),'b-*')
%         %Set a breakpoint here
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% End of the while-loop

    end
    % Statistics
    stats.count(8,it) = i;
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot after the iteration

%     loc_temp=vertcat(P(:).loc);
%     plot(loc_temp(:,1),loc_temp(:,2),'b-*')
%     hold on
%     plot([loc_temp(end,1),loc_temp(1,1)],[loc_temp(end,2),...
%         loc_temp(1,2)],'b-')
%     hold on
%     grid on
%     axis square 
%     axis equal
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Switching from polygon inflation to inverse polygon inflation 

    if par(6) == 1 && it == 2 
        % These conditions mean that during the first iteration a
        % requriement for the inverse iteration was detected and during the
        % second iteration FIRPOL was calculated
        it = 1; % Overwrite old results from the first iteration
        % Note that the statistics are not overwritten
        par(6) = 2; 
        % Setting up other functions for the calculation of the inner
        % boundary.
        if par(7) == 1
            disp('Calculating inner boundary.')
        end
    end
    if par(6)==2 && it==3 
        % These conditions mean that during the second iteration FIRPOL was
        % calculated and during the third iteration, the inner boundary
        it=2; % Save the results in the second slot of AFS_cell
        % Note that the statistics are not overwritten
    end
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Saving the AFS
    
    AFS_cell{it}=vertcat(P(:).loc);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Deciding whether inverse iteration is needed in the next iteration

    if par(6) == 0 && it == 1
        loc_temp=vertcat(P(:).loc); % Saving the points outside of a struct
        quadrant = zeros(1,4); % The quadrants in which P is located
        for j=1:i 
            if loc_temp(j,1) >= 0 && loc_temp(j,2) >= 0
                quadrant(1,1)=1;
            elseif loc_temp(j,1) <= 0 && loc_temp(j,2) >= 0
                quadrant(1,2)=1;
            elseif loc_temp(j,1) <= 0 && loc_temp(j,2) <= 0
                quadrant(1,3)=1;
            elseif loc_temp(j,1) >= 0 && loc_temp(j,2) <= 0
                quadrant(1,4)=1;
            end
        end
        if quadrant(1,1)+quadrant(1,2)+quadrant(1,3)+quadrant(1,4) >= 3
            % If AFS in atleast three quadrants, inverse iteration needed
            par(6) = 1; 
            % Setting up other functions for inverse inflation. In the 
            % next iteration FIRPOL will be calculated
            it = 2; % it is changen to avoid breaking the function
            if par(7) == 1
                disp('Switching to inverse polyogn inflation.')
                disp('Calculating FIRPOL.')
            end
        end
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Breaking the loop in some cases

    if par(6) ~= 0 && par(6) ~= 1 && par(6) ~= 2
        % For the triangle, rectangle etc. only one iteration is needed, as
        % there is only one segment
        break
    end
    if par(6) == 2 && it == 2
        % After the inner boundary is calculated, the iteration should not
        % be repeated
        break 
    end
    if par(6) == 1 && it == 1
        % For FIRPOL only one iteration is needed. For the inverse polyogn
        % inflation the par(6) has already been changed to 2.
        break 
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plotting the AFS

figure
hold on
for i = 1:3
    if isempty(AFS_cell{i}) == 0
        plot([AFS_cell{i}(:,1); AFS_cell{i}(1,1)], [AFS_cell{i}(:,2);...
            AFS_cell{i}(1,2)],'b-*')
    end
end
axis square 
axis equal
hold off

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plotting some statistics

if par(7) == 1
%     % Plot the number of steps in pre-bisection and bisection stage
%     for it = 1:3
%         if isempty(stats.steps{it}) == 0
%             figure
%             hold on
%             plot(abs(stats.steps{it}(1,:)))
%             % Absolute value is needed because the sign before the number
%             % of steps shows the direction of the search in the
%             % pre-bisection stage. This is not relevant for the graphical
%             % representation
%             plot(stats.steps{it}(2,:))
%             hold off
%         end
%     end
%     % Plot the minimal Delta_i for each iteration
%     figure
%     hold on
%     for it = 1:3
%         if isempty(stats.Delta{it}) == 0
%             plot(stats.Delta{it}(:))
%         end
%     end
%     hold off

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Displaying the summary

    stats.count(1:8,4) = stats.count(1:8,1)+stats.count(1:8,2)+...
    stats.count(1:8,3);
    disp('Summary:')
    if par(6) == 0
        disp('Polygon inflation algorithm for three segments.')
        disp('  Segment1   Segment2   Segment3      Total')
        caseDisplay = 3;
    elseif par(6) == 1 && it == 1
        disp('    FIRPOL')
        caseDisplay = 1;
    elseif par(6) == 2 && isempty(stats.steps{1}) == 0
        disp(['Polygon inflation algorithm was switched to the inverse',...
            'polyogn inflation algorithm.'])
        disp('PolygonInf     FIRPOL   Inner B.      Total')
        caseDisplay = 3;
    elseif par(6) == 2 && isempty(stats.steps{1}) == 1
        disp('Inverse polyogn inflation algorithm.')
        disp('    FIRPOL   Inner B.      Total')
        caseDisplay = 2;
    elseif par(6) ~= 0 && par(6) ~= 1 && par(6) ~= 2
        disp('Polyogn inflation algorithm for a known figure.')
        caseDisplay = 1;
    else
        disp('Unknown case.')
    end
    switch caseDisplay
        case 1
            % Calculating some statistics about the iteration
            avgPreBisectionSteps(1) = mean(abs(stats.steps{1}(1,:)));
            avgBisectionSteps(1) = mean(stats.steps{1}(2,:));
            % Displaying statistics
            fprintf(['%10d     Number of times other starting matrix ',...
            'S2 is needed\n%10d     Number of times ga is needed\n',...
            '%10d     Number of times when an infinite loop inwards',...
            ' occurs\n%10d     Number of times when an infinite loop',...
            ' outwards occurs\n%10d     Number of times when a vertex',...
            ' was not added by the angle\n%10d     Number of times',...
            ' when a vertex was removed by the angle\n%10d     ',...
            'Number of times when a vertex was removed after 5',...
            ' subdivisions\n%10.3f     Average number of',...
            ' pre-bisection stage steps\n%10.3f     Average number',...
            ' of bisection stage steps\n%10d     Number of vertices\n'],...
            stats.count(1,1),stats.count(2,1),stats.count(3,1),...
            stats.count(4,1),stats.count(5,1),stats.count(6,1),...
            stats.count(7,1),avgPreBisectionSteps(1),...
            avgBisectionSteps(1),stats.count(8,1));
        case 2
            % Calculating some statistics about the iteration
            avgPreBisectionSteps(2) = mean(abs(stats.steps{2}(1,:)));
            avgPreBisectionSteps(3) = mean(abs(stats.steps{3}(1,:)));
            avgPreBisectionSteps(4) = mean(abs([stats.steps{2}(1,:) ...
                stats.steps{3}(1,:)]));    
            avgBisectionSteps(2) = mean(stats.steps{2}(2,:));
            avgBisectionSteps(3) = mean(stats.steps{3}(2,:));
            avgBisectionSteps(4) = mean([stats.steps{2}(2,:) ...
                stats.steps{3}(2,:)]);
            % Displaying statistics
            fprintf(['%10d %10d %10d     Number of times other ',...
            'starting matrix S2 is needed\n%10d %10d %10d     Number',...
            ' of times ga is needed\n%10d %10d %10d     Number of',...
            ' times when an infinite loop inwards occurs\n%10d',...
            ' %10d %10d     Number of times when an infinite loop',...
            ' outwards occurs\n%10d %10d %10d     Number of times',...
            ' when a vertex was not added by the angle\n%10d %10d %10d',...
            '      Number of times when a vertex was removed by the',...
            ' angle\n%10d %10d %10d     Number of times when a vertex ',...
            'was removed after 5 subdivisions\n%10.3f %10.3f %10.3f',...
            '     Average number of pre-bisection stage steps',...
            '\n%10.3f %10.3f %10.3f     Average number of ',...
            'bisection stage steps\n%10d %10d %10d     Number of',...
            ' vertices\n'],...
            stats.count(1,2),stats.count(1,3),stats.count(1,4),...
            stats.count(2,2),stats.count(2,3),stats.count(2,4),...
            stats.count(3,2),stats.count(3,3),stats.count(3,4),...
            stats.count(4,2),stats.count(4,3),stats.count(4,4),...
            stats.count(5,2),stats.count(5,3),stats.count(5,4),...
            stats.count(6,2),stats.count(6,3),stats.count(6,4),...
            stats.count(7,2),stats.count(7,3),stats.count(7,4),...
            avgPreBisectionSteps(2),avgPreBisectionSteps(3),...
            avgPreBisectionSteps(4),avgBisectionSteps(2),...
            avgBisectionSteps(3),avgBisectionSteps(4),...
            stats.count(8,2),stats.count(8,3),stats.count(8,4));
        case 3
            % Calculating some statistics about the iteration
            avgPreBisectionSteps(1) = mean(abs(stats.steps{1}(1,:)));
            avgPreBisectionSteps(2) = mean(abs(stats.steps{2}(1,:)));
            avgPreBisectionSteps(3) = mean(abs(stats.steps{3}(1,:)));
            avgPreBisectionSteps(4) = mean(abs([stats.steps{1}(1,:) ...
                stats.steps{2}(1,:) stats.steps{3}(1,:)]));    
            avgBisectionSteps(1) = mean(stats.steps{1}(2,:));
            avgBisectionSteps(2) = mean(stats.steps{2}(2,:));
            avgBisectionSteps(3) = mean(stats.steps{3}(2,:));
            avgBisectionSteps(4) = mean([stats.steps{1}(2,:) ...
                stats.steps{2}(2,:) stats.steps{3}(2,:)]);
            % Displaying statistics
            fprintf(['%10d %10d %10d %10d     Number of times other',...
            ' starting matrix S2 is needed\n%10d %10d %10d %10d',...
            '     Number of times ga is needed\n%10d %10d %10d %10d',...
            '     Number of times when an infinite loop inwards',...
            ' occurs\n%10d %10d %10d %10d     Number of times when',...
            ' an infinite loop outwards occurs\n%10d %10d %10d %10d',...
            '     Number of times when a vertex was not added by ',...
            'the angle\n%10d %10d %10d %10d     Number of times ',...
            'when a vertex was removed by the angle\n%10d %10d ',...
            '%10d %10d     Number of times when a vertex was removed',...
            ' after 5 subdivisions\n%10.3f %10.3f %10.3f %10.3f',...
            '     Average number of pre-bisection stage steps',...
            '\n%10.3f %10.3f %10.3f %10.3f     Average number ',...
            'of bisection stage steps\n%10d %10d %10d %10d     ',...
            'Number of vertices\n'],...
            stats.count(1,1),stats.count(1,2),stats.count(1,3),...
            stats.count(1,4),stats.count(2,1),stats.count(2,2),...
            stats.count(2,3),stats.count(2,4),stats.count(3,1),...
            stats.count(3,2),stats.count(3,3),stats.count(3,4),...
            stats.count(4,1),stats.count(4,2),stats.count(4,3),...
            stats.count(4,4),stats.count(5,1),stats.count(5,2),...
            stats.count(5,3),stats.count(5,4),stats.count(6,1),...
            stats.count(6,2),stats.count(6,3),stats.count(6,4),...
            stats.count(7,1),stats.count(7,2),stats.count(7,3),...
            stats.count(7,4),avgPreBisectionSteps(1),...
            avgPreBisectionSteps(2),avgPreBisectionSteps(3),...
            avgPreBisectionSteps(4),avgBisectionSteps(1),...
            avgBisectionSteps(2),avgBisectionSteps(3),...
            avgBisectionSteps(4),stats.count(8,1),stats.count(8,2),...
            stats.count(8,3),stats.count(8,4));
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Hausdorff distance for triangle and rectangle cases to the actual
% shapes

if par(6) == 3 && par(6) == 4
    HausdorffDistance = hdist(loc_temp,[],par);
    if par(7) == 1
        fprintf('Hausdorff Distance is %d.\n',HausdorffDistance)
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Some useful commands

% % The total number of iterations
% size(AFS_cell{1})
% size(AFS_cell{2})
% size(AFS_cell{3})
% size(AFS_cell{1})+size(AFS_cell{2})+size(AFS_cell{3})

% % Comparison of Hausdorff distance with FAC-PACK
% max([min([hdist(AFS_cell{1},AFS{1},par) hdist(AFS_cell{1},AFS{2},par) hdist(AFS_cell{1},AFS{3},par)]) ...
%      min([hdist(AFS_cell{2},AFS{1},par) hdist(AFS_cell{2},AFS{2},par) hdist(AFS_cell{2},AFS{3},par)]) ...
%      min([hdist(AFS_cell{3},AFS{1},par) hdist(AFS_cell{3},AFS{2},par) hdist(AFS_cell{3},AFS{3},par)])])

% % Comparison of Hausdorff distance with FAC-PACK for inverse iteration
% max([min([hdist(AFS_cell{1},AFS{1},par) hdist(AFS_cell{1},AFS{2},par)]) ...
%      min([hdist(AFS_cell{2},AFS{1},par) hdist(AFS_cell{2},AFS{2},par)])])

% % Comparison of plots with FAC-PACK
% plot([AFS_cell{1}(:,1); AFS_cell{1}(1,1)],[AFS_cell{1}(:,2); AFS_cell{1}(1,2)],'b-*')
% hold on
% plot([AFS_cell{2}(:,1); AFS_cell{2}(1,1)],[AFS_cell{2}(:,2); AFS_cell{2}(1,2)],'b-*')
% hold on
% plot([AFS_cell{3}(:,1); AFS_cell{3}(1,1)],[AFS_cell{3}(:,2); AFS_cell{3}(1,2)],'b-*')
% hold on
% plot([AFS{1}(:,1); AFS{1}(1,1)],[AFS{1}(:,2); AFS{1}(1,2)],'r-o')
% hold on
% plot([AFS{2}(:,1); AFS{2}(1,1)],[AFS{2}(:,2); AFS{2}(1,2)],'r-o')
% hold on
% plot([AFS{3}(:,1); AFS{3}(1,1)],[AFS{3}(:,2); AFS{3}(1,2)],'r-o')

% % Comparison of plots with FAC-PACK for inverse iteration
% plot([AFS_cell{1}(:,1); AFS_cell{1}(1,1)],[AFS_cell{1}(:,2); AFS_cell{1}(1,2)],'b-*')
% hold on
% plot([AFS_cell{2}(:,1); AFS_cell{2}(1,1)],[AFS_cell{2}(:,2); AFS_cell{2}(1,2)],'b-*')
% hold on
% plot([AFS{1}(:,1); AFS{1}(1,1)],[AFS{1}(:,2); AFS{1}(1,2)],'r-o')
% hold on
% plot([AFS{2}(:,1); AFS{2}(1,1)],[AFS{2}(:,2); AFS{2}(1,2)],'r-o')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% new_point: Finds a new vertice
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Uses two old vertices to give data to function intersection
% Input: The two vertices P1 and P2 that define the edge for subdivision.
    % Singular value decomposition matrices: U_hat, Sigma_hat and V_hat. 
    % The subdivision point (e.g. midpoint of the edge): caseDivideEdge.
    % Parameter vector: par.
% Output: The new vertex: P_new. 
    % The error notification: iferr.
    % Statistic parameters in vector stat.

function [P_new,iferr,stat]=new_point(P1,P2,U_hat,Sigma_hat,V_hat,...
    caseDivideEdge,par)
    % Determine the point to start the intersection.m
    switch caseDivideEdge
        case 1 % Divide at the midpoint of the edge
            M = P1.loc+1/2*(P2.loc-P1.loc); 
        case 2 % Divide at 1/3 of the edge
            M = P1.loc+1/3*(P2.loc-P1.loc);
        case 3 % Divide at 2/3 of the edge
            M = P1.loc+2/3*(P2.loc-P1.loc);
        case 4 % Divide at 1/4 of the edge
            M = P1.loc+1/4*(P2.loc-P1.loc);
        case 5 % Divide at 3/4 of the edge
            M = P1.loc+3/4*(P2.loc-P1.loc);
    end
    % Find perpendicular vector "to the right" of vector defined by P2-P1
    v = [P2.loc(2)-P1.loc(2) ; P1.loc(1)-P2.loc(1)]; 
    % Find the distance between P1 and P2
    d = norm(P1.loc-P2.loc); 
    % Find the new vertex using function intersection
    [P_new,iferr,stat] = intersection(M,v,d,P1.S,P2.S,U_hat,Sigma_hat,...
        V_hat,par);
    % Find the gain of height: Delta_i
    if iferr==0 % if no errors occured
        % Gain of height
        P_new.div = norm(M-P_new.loc); 
        % Weighted gain-of-area; does not work as well as gain of height        
%        P_new.div=1/4*d*norm(M-P_new.loc); 
    else
      P_new.div = 0; % To be overwritten; the new vertex was not found
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% intersection: Finds a new vertice with given point and direction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This function is responsible for pre-bisection and bisection stages
% stage
% Input: The point M and direction v for the search of a new vertex.
    % The lenght of the edge that is being subdivided: d. (It is used for
    % the pre-bisection step size)
    % Matrices S from both neighboring vertices: S_1, S_2.
    % Singular value decomposition matrices: U_hat, Sigma_hat and V_hat. 
    % Parameter vector: par.
% Output: The new vertex: P_new. 
    % The error notification: iferr.
    % Statistic parameters in vector stat.

function [P_new,iferr,stat]=intersection(M,v,d,S_1,S_2,U_hat,Sigma_hat,...
    V_hat,par)
    % Statistics
    stat = zeros(6,1);
    % Defining the variable, that lets to know about errors
    iferr = 0;
    % Defining the step size for pre-bisection stage
    h = par(5)*d;
    % Calculate the actual pre-bisection steps
    v = v/norm(v); % Normalizing direction vector
    ha = h*v(1); % Distance on abscisa axis
    hb = h*v(2); % Distance on ordinate axis
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Check whether the point M (usually, the midpoint of the edge) is feasible
    
    test_par = 1; % Test parameter. If 0 the point is feasible, if 1, not.
    % Fast test
    F_test = [1 M(1) M(2)]*V_hat'; % Temporary value, used in the next row.
    % This fast test is used only for par(6) == 0 (polygon inflation). For
    % FIRPOL it is included in the function F.
    if norm(min(F_test./norm(F_test(:))+par(1),0))^2 <= par(3) || ...
            par(6) ~= 0
        % Standard test with S1 (or the fast test for FIRPOL or test for
        % triangle, rectangle etc.) - actual numerical optimization
        [n,S] = F(M,S_1,U_hat,Sigma_hat,V_hat,par);
        if n <= par(3)
            test_par = 0; % The point was feasible
        elseif par(6) == 0 || par(6) == 2
            % Standard test with S2 (only for (inverse) polygon inflation)
            if par(7)==1
                disp(['Using lsqnonlin with S_1 was unsuccessful,',...
                    ' checking S_2'])
            end
            % Statisctics
            stat(1) = 1;
            % Actual numerical optimization
            [n,S] = F(M,S_2,U_hat,Sigma_hat,V_hat,par);
            if n<=par(3)
                test_par=0; % The point was feasible
            else
                % Genetic algorithm
                if par(7)==1
                    disp('lsqnonlin was unsuccessful, trying ga')
                end
                % Statisctics
                stat(2) = 1;
                % Finding approximate solution with genetic algorithm
                options.Display='none';
                [mat,n]=ga(@(S) fun_ga(M,S,U_hat,Sigma_hat,V_hat,par),...
                    4,[],[],[],[],[],[],[],[],options);
                S = [mat(1) mat(2);mat(3) mat(4)];
                % Correcting the solution with lsqnonlin
                [n,S] = F(M,S,U_hat,Sigma_hat,V_hat,par);
                if n <= par(3)
                    test_par = 0;  % The point was feasible
                else
                    if par(7) == 1
                        disp('ga unsuccessful, the point was not feasible')
                    end
                end
            end
        else
            if par(7) == 1
                disp('The point was not feasible')
            end
        end
    else
        n=10; % Placeholder value. If fast test has failed, 
            % no n is calculated.
        S=S_1; % If fast test has failed, no S is calculated.
        if par(7)==1
            disp('Fast test failed')
        end
    end
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Pre-bisection stage
    
    k = 0; % Counting the number of steps during pre-bisection stage
    if test_par == 0 % The point M was feasible; move outwards
        while n<=par(3)
            k = k+1; % Count number of steps
            M_old = M; % Save the old point
            M(1) = M(1)+ha; % Actual step
            M(2) = M(2)+hb; % Actual step
            S_old = S; % Save the old matrix S
            % Check whether feasible with previous S
            [n,S] = F(M,S,U_hat,Sigma_hat,V_hat,par);
            if k >= par(8) % Stop the loop, if max number of steps reached
                P_new.loc = [0 0]; % Placeholder value
                P_new.S = [0 0;0 0]; % Placeholder value
                P_new.div = 1; % Placeholder value
                iferr = 1; % Set error variable
                if par(7)==1
                    disp(['Search from a feasible point reached max',...
                        'number of steps (pre-bisection stage).'])
                end
                % Statistics
                stat(3)=1;
                return
            end
        end
    else % The point M was not feasible; move inwards
        while n >= par(3)
            k = k-1; % Count number of steps (in the negative direction)
            M_old = M; % Save the old point
            M(1) = M(1)-ha; % Actual step
            M(2) = M(2)-hb; % Actual step
            S_old = S; % Save the old matrix S
            % Check whether feasible with previous S (that was not
            % successful - therefore neigboring S should also be tested)
            [n,S] = F(M,S,U_hat,Sigma_hat,V_hat,par);
            if n>=par(3) % Check whether feasible with S_1
                [n,S] = F(M,S_1,U_hat,Sigma_hat,V_hat,par); 
                if n>=par(3) % Check whether feasible with S_2
                    [n,S] = F(M,S_2,U_hat,Sigma_hat,V_hat,par);
                end
            end
            % ga for concave case - a lot of time is needed
%             if n>=par(3)
%                 [mat,n]=ga(@(S) fun_ga(M,S,U_hat,Sigma_hat,V_hat,par)...
%                     ,4,[],[],[],[],[],[],[],[],options);
%                 S=[mat(1) mat(2);mat(3) mat(4)];
%                 [n,S]=F(M,S_2,U_hat,Sigma_hat,V_hat,par);
%                 if par(7)==1
%                      fprintf(['Using ga for concave case for the %d',...
%                          ' time \n'],-k)
%                 end
%             end
            if k <= -par(8) % Stop the loop, if max number of steps reached
                P_new.loc = [0 0]; % Placeholder value
                P_new.S = [0 0;0 0]; % Placeholder value
                P_new.div = 1; % Placeholder value
                iferr = 1; % Set error variable
                if par(7)==1
                    disp(['Search from a unfeasible point reached max',...
                        'number of steps (pre-bisection stage).'])
                end
                % Statistics
                stat(4)=1;
                return
            end
        end
        % M_old must be the point that is inside the set    
        tempM = M;
        M = M_old;
        M_old = tempM;
        tempS = S;
        S = S_old;
        S_old = tempS;
    end
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Bisection stage

    l = 0; % Counting the number of iterations
    while norm(M_old-M) > par(2) % The stopping criterion for the bisection
        % method
        l = l+1; % Count number of steps 
        x = (M_old+M)/2; % Halving the interval
        [n,S_mid] = F(x,S,U_hat,Sigma_hat,V_hat,par); % Checking the
        % feasibility
        if n <= par(3) % Midpoint was feasible
            M_old = x;
            S_old = S_mid;
        else % Midpoint was not feasible - testing with other matrix S
            [n,S_mid] = F(x,S_old,U_hat,Sigma_hat,V_hat,par);
            if n <= par(3)  % Midpoint was feasible
                M_old = x;
                S_old = S_mid;
            else % Midpoint was not feasible with S and S_old
                M = x;
                S = S_mid;
            end
        end
    end
    P_new.loc = M_old; % Choosing the point that is feasible
    P_new.S = S; % Adding information about S
    P_new.div = 1; % Placeholder value
    % Statistics
    stat(5) = k;
    stat(6) = l;
end
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% F: provides the solution to the minimization problem
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Gives the value that helps to decide whether a point is feasible
% Input: The alpha and beta values of the point: P.
    % Starting approximation for lsqnonlin: S_start.
    % Singular value decomposition matrices: U_hat, Sigma_hat and V_hat. 
    % Parameter vector: par.
% Output: The value of the minimization problem: n. For problems with no
    % optimization it is set to -1 for a feasible point and 1, for
    % unfeasible.
    % The new matrix S.

function [n,S] = F(P,S_start,U_hat,Sigma_hat,V_hat,par)
    switch par(6) % The function changes depending on the problem
        case 0 % Normal polygon inflation - with lsqnonlin
             options = optimoptions('lsqnonlin','Display','none',...
                 'FunctionTolerance',1e-12,'OptimalityTolerance',1e-12);
            [S,n] = lsqnonlin(@(S) fun(P,S,U_hat,Sigma_hat,V_hat,par),...
                S_start,[],[],options);
        case 1 % FIRPOL - similar to fast test
            F_test = [1 P(1) P(2)]*V_hat';
            if norm(min(F_test./norm(F_test(:))+par(1),0))^2 <= par(3)
                n = -1; % Feasible (the other check for <=par(3) is just a 
                % dummy check
            else
                n = 1; % Unfeasible
            end
            S=zeros(2); % Placeholder value

        case 2 % Inverse polygon inflation
            options = optimoptions('lsqnonlin','Display','none',...
                'FunctionTolerance',1e-12,'OptimalityTolerance',1e-12);
            [S,n] = lsqnonlin(@(S) fun(P,S,U_hat,Sigma_hat,V_hat,par),...
                S_start,[],[],options);
        case 3 % Triangle
            if abs(P(1))+abs(P(2)) <= 1 && P(2) >= 0
                n = -1; % Feasible
            else
                n = 1; % Unfeasible
            end
            S = zeros(2); % Placeholder value
        case 4 % Rectangle
            if abs(P(1)) <= 0.5 && abs(P(2)) <= 0.8
                n = -1; % Feasible
            else
                n = 1; % Unfeasible
            end        
            S = zeros(2); % Placeholder value     
        case 5 % Circle
            if P(1)^2+P(2)^2 <= 0.8 
                n = -1; % Feasible
            else
                n = 1; % Unfeasible
            end
            S = zeros(2); % Placeholder value  
        case 6 % Shape
            if P(1)^2-P(2)^2 <= 1 && P(1)^2-P(2)^2 >= -1 && ...
                    P(1)^2+P(2)^2 <= 2
                n = -1; % Feasible
            else
                n = 1; % Unfeasible
            end
            S = zeros(2); % Placeholder value  
        case 7 % Shape 2
            if abs(P(1))+abs(P(2)) <= 1 || abs(P(1)) <= 0.7 && ...
                    abs(P(2)) <= 0.7 || P(1)^2+(P(2)+0.5)^2 <= 0.4
                n = -1; % Feasible
            else
                n = 1; % Unfeasible
            end
            S = zeros(2); % Placeholder value  
        case 8  % Shape 3
            if abs(P(1)) <= 1.4 && abs(P(2)) <= 1.4 || ...
                    P(1)^2+(P(2)+1)^2 <= 0.8 %|| abs(4*P(1))+abs(P(2)) <= 2 
                n = -1; % Feasible
            else
                n = 1; % Unfeasible
            end
            S = zeros(2); % Placeholder value  
        case 9  % Two circles
            if P(1)^2+(P(2)-1.001)^2 <= 1 || P(1)^2+(P(2)+1.001)^2 <= 1
                n = -1; % Feasible
            else
                n = 1; % Unfeasible
            end
            S = zeros(2); % Placeholder value
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fun: The target function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This function is used in lsqnonlin. It ensures the nonnegativity of C
% and A and regularity of T.
% Input: The alpha and beta values of the point: P.
    % Starting approximation for lsqnonlin: S_start.
    % Singular value decomposition matrices: U_hat, Sigma_hat and V_hat. 
    % Parameter vector: par.
% Output: The vector that determines whether a point is feasible: c.

function c = fun(P,S,U_hat,Sigma_hat,V_hat,par)
    % Defining matrices according to the theory
    T = [1 P(1) P(2);1 S(1,:);1 S(2,:)];
    C = U_hat*Sigma_hat*pinv(T);
    A = T*V_hat';
    for i = 1:3
        C_norm = norm(C(:,i),'inf');
        A_norm = norm(A(i,:),'inf');
        fa(:,i) = min(0,C(:,i)/C_norm+par(1)); % Nonnegativity of C
        if par(6) ~= 2 || (par(6) == 2 && i ~= 1) 
            % Different for inner boundary (The first is not required)
            fb(i,:) = min(0,A(i,:)/A_norm+par(1)); % Nonnegativity of A
        end
    end
    fc = eye(3)-T*pinv(T); % Regularity of T
    c = [fa(:);fb(:);fc(:)];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fun_ga: The target function for the genetic algorithm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This function is used in ga. It ensures the nonnegativity of C
% and A and regularity of T.
% Input: The alpha and beta values of the point: P.
    % Starting approximation for lsqnonlin: S_start.
    % Singular value decomposition matrices: U_hat, Sigma_hat and V_hat. 
    % Parameter vector: par.
% Output: The value that determines whether a point is feasible: c.

function c = fun_ga(P,S,U_hat,Sigma_hat,V_hat,par)
    % Defining matrices according to the theory
    T = [1 P(1) P(2);1 S(1) S(2);1 S(3) S(4)];
    C = U_hat*Sigma_hat*pinv(T);
    A = T*V_hat';
    % fa, fb and fc are calculated according to the minimization problem 
    % (that is implied in the lsqnonlin but not ga)
    % Therefore, they are squared and summed
    fa = 0;
    fb = 0;
    for i = 1:3
        C_norm = norm(C(:,i),'inf');
        A_norm = norm(A(i,:),'inf');
        for j = 1:max(size(C))
            fa = fa+min(0,C(j,i)/C_norm+par(1))^2;
        end
        for k = 1:max(size(A))
            fb = fb+min(0,A(i,k)/A_norm+par(1))^2;
        end
    end
    fc = norm(eye(3)-T*pinv(T),'fro').^2;
    c = fa+fb+fc;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fun_init: The target function for the initialization (lsqnonlin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This function is used during initialization in lsqnonlin. It ensures the 
% nonnegativity of C and A and regularity of T.
% The only change from function fun is that P and S are combined in a
% single vector P.
% Input: The elements of matrix T as a starting approximation for
    % lsqnonlin: P
    % Singular value decomposition matrices: U_hat, Sigma_hat and V_hat. 
    % Parameter vector: par.
% Output: The value that determines whether a point is feasible: c.

function c = fun_init(P,U_hat,Sigma_hat,V_hat,par)
    % Defining matrices according to the theory
    T = [1 P(1) P(2);1 P(3) P(4);1 P(5) P(6)];
    C = U_hat*Sigma_hat*pinv(T);
    A = T*V_hat';
    for i = 1:3
        C_norm = norm(C(:,i),'inf');
        A_norm = norm(A(i,:),'inf');
        fa(:,i) = min(0,C(:,i)/C_norm+par(1));
        fb(i,:) = min(0,A(i,:)/A_norm+par(1));
    end
    fc = eye(3)-T*pinv(T);
    c = [fa(:);fb(:);fc(:)];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fun_ga_init: The target function for the initialization (ga)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This function is used during initialization in ga. It ensures the 
% nonnegativity of C and A and regularity of T.
% The only change from function fun_ga is that P and S are combined in a
% single vector P.
% Input: The elements of matrix T: P
    % Singular value decomposition matrices: U_hat, Sigma_hat and V_hat. 
    % Parameter vector: par.
% Output: The value that determines whether a point is feasible: c.

function c = fun_ga_init(P,U_hat,Sigma_hat,V_hat,par)
    % Defining matrices according to the theory
    T = [1 P(1) P(2);1 P(3) P(4);1 P(5) P(6)];
    C = U_hat*Sigma_hat*pinv(T);
    A = T*V_hat';
    % fa, fb and fc are calculated according to the minimization problem 
    % (that is implied in the lsqnonlin but not ga)
    % Therefore, they are squared and summed
    fa = 0;
    fb = 0;
    for i = 1:3
        C_norm = norm(C(:,i),'inf');
        A_norm = norm(A(i,:),'inf');
        for j = 1:max(size(C))
            fa = fa+min(0,C(j,i)/C_norm+par(1))^2;
        end
        for k = 1:max(size(A))
            fb = fb+min(0,A(i,k)/A_norm+par(1))^2;
        end
    end
    fc = norm(eye(3)-T*pinv(T),'fro').^2;
    c = fa+fb+fc;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% hdist: Calculate the Hausdorff distance
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This function calculates the Hausdorff distance between two polygons
% Input: Two polygons P1 and P2 for the calculation of Hausdorff distance.
    % Note, that for a triangle and rectangle the actual shape P2
    % is already implemented. Thus, P2 can be set to [] in this case.
    % Parameter vector: par.
% Output: Hausdorff distance: hd_val.

function hd_val = hdist(P1,P2,par)
    [i,~] = size(P1); % Save the number of vertices for P1
    if par(6) == 3 % Define P2 for a traingle
        P2 = [[-1 0];[1 0];[0 1]];
    elseif par(6) == 4 % Define P2 for a rectangle
        P2 = [[0.5 -0.8];[0.5 0.8];[-0.5 0.8];[-0.5 -0.8]];
    end
    [k,~] = size(P2); % Save the number of vertices for P2
    hd = zeros(i+k,1); % Define the vector where min distance from each 
        %vertex is saved
    if isempty(P2) == 0 && isempty(P1) == 0
        for j = 1:i % Go through all vertices of P1
            hdi = zeros(k,1);
            for m = 1:k % Go through all edges of P2 (defined by m and 
                    % m_next)
                m_next = m+1;
                if m == k % If the next vertex is the first
                    m_next = 1;
                end
                if norm(P1(j,:)-P2(m,:))^2+norm(P2(m_next,:)-P2(m,:))^2 ...
                        <= norm(P2(m_next,:)-P1(j,:))^2 
                    % Triangle is obtuse (Pythagorean inequality)
                    % Shortest distance is between j and m
                    hdi(m) = sqrt((P1(j,1)-P2(m,1)).^2+(P1(j,2)-...
                        P2(m,2)).^2);
                elseif norm(P1(j,:)-P2(m_next,:))^2+norm(P2(m_next,:)-...
                        P2(m,:))^2 <= norm(P2(m,:)-P1(j,:))^2 
                    % Triangle is obtuse (Pythagorean Inequality)
                    % Shortest distance is between j and m_next
                    hdi(m) = sqrt((P1(j,1)-P2(m_next,1)).^2+(P1(j,2)-...
                        P2(m_next,2)).^2);
                else % Triangle is acute or the obtuse angle corresponds 
                    % to the vertex j
                    % Shortest distance is the height of the triangle
                    % Cross product gives the area of paralelogram
                    a = P2(m,:)-P2(m_next,:);
                    b = P1(j,:)-P2(m_next,:);
                    hdi(m) = norm(cross([a 0],[b 0]))/norm(a);
                end
            end
            hd(j) = min(hdi(:)); % Save the minimal distance from the 
                % vertex j from P1 to an edge of P2
        end
        for j = 1:k % Go through all vertices of P2
            hdi = zeros(i,1);
            for m = 1:i % Go through all edges of P1 (defined by m and 
                    % m_next)
                m_next = m+1;
                if m == i
                    m_next = 1;
                end
                if norm(P2(j,:)-P1(m,:))^2+norm(P1(m_next,:)-P1(m,:))^2 ...
                        <= norm(P1(m_next,:)-P2(j,:))^2 
                    % Triangle is obtuse (Pythagorean inequality) 
                    % Shortest distance is between j and m
                    hdi(m) = sqrt((P1(m,1)-P2(j,1)).^2+(P1(m,2)-...
                        P2(j,2)).^2);
                elseif norm(P2(j,:)-P1(m_next,:))^2+norm(P1(m_next,:)-...
                        P1(m,:))^2 <= norm(P1(m,:)-P2(j,:))^2 
                    % Triangle is obtuse (Pythagorean inequality) 
                    % Shortest distance is between j and m_next
                    hdi(m) = sqrt((P1(m_next,1)-P2(j,1)).^2+...
                        (P1(m_next,2)-P2(j,2)).^2);
                else % Triangle is acute or the obtuse angle corresponds 
                    % to the vertex j
                    % Shortest distance is the height of the triangle
                    % Cross product gives the area of paralelogram
                    a = P1(m,:)-P1(m_next,:);
                    b = P2(j,:)-P1(m_next,:);
                    hdi(m) = norm(cross([a 0],[b 0]))/norm(a);
                end
            end
            hd(i+j) = min(hdi(:)); % Save the minimal distance from the 
                % vertex j from P2 to an edge of P1
        end
    end
    [hd_val,z] = max(hd); % Hausdorff distance is the maximum of the 
        % minimal distances from each vertex
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generate_data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The function generates data matrix D, as seen in "How to compute the Area of Feasible Solutions"
% Input: The number of the case z, it should be set to 1 (function can be 
    % expanded to other examples).
% Output: Data matrix D, concentration matrix C and absoptivity matrix A.

function [D,C,A] = generate_data(z)
    switch z
        case(1)
            t = linspace(0,20,21)';
            x = linspace(0,100,101)';
            k = [0.75 0.25]; % Default [0.75 0.25]
            C(:,1) = exp(-k(1)*t);
            C(:,2) = k(1)/(k(2)-k(1))*(exp(-k(1)*t)-exp(-k(2)*t));
            C(:,3) = 1-C(:,1)-C(:,2);
            A(1,:) = 0.95*exp(-(x-20).^2./500)+0.5;
            % Default 0.95 20 500 0.3
            A(2,:) = 0.9*exp(-(x-50).^2./500)+0.45;
            % Default 0.9 50 500 0.25
            A(3,:) = 0.7*exp(-(x-80).^2./500)+0.4;
            % Default 0.7 80 500 0.2
            D = C*A;
        case(2)
            D = eye(3,3);
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% To do (possibly?):

% Maximum number of funcalls as an extra stopping criterion; possibly could
    % be added for extra stability. Currently the script can simply be
    % stopped if it takes too long.
% Automatic recognition of problems - a double calculation with 
    % different parameters and a comparison of the Hausdorff distance
    % between them. Currently has to be done manually, if required.
% An error message if all vertices have been removed; currently a Matlab
    % error occurs.