tic %Start timer

% Inputs

% Cantilever beam initialisation (0.25% and 1.0%)
inputVariable = [-108.36	65.9	487.8	235.29	96.48	356.85	184.76	1040.61	483.48	105.5	431.72	71.33	1293.69	93.92	196.36	1245.17	143.88	1914.36	519.21	85.84	2269.28	105.81	2660.11	315.7	97.73	2645.57	307.42	3145.33	582.75	100.16	-145.07	55.99	599.23	52.07	118.83	215.46	97.41	859.37	44.72	165.1	1014.05	481.71	1262.57	124.12	53.07	1164.76	20.64	2306.01	80.78	124.69	1936.53	456.56	2205.78	90.78	55.38	2670.05	359.74	2736.7	303.29	10	-141.55	940.73	607.04	946.1	125.45	320.26	922.71	932.92	909.08	181.39	1018.73	475.91	1310.45	872.73	56.33	1214.9	971.25	2259.84	933.57	115.28	1816.2	417.23	2195.25	948.59	54.56	2695.11	611.72	2756.43	690.37	39.74	-84.33	944.25	382.37	805.4	126.92	280.21	837.99	1064.9	455.68	105.01	705.3	927.44	1336.24	900.86	201.07	1295.66	849.57	1948.78	457.13	91.71	2220.45	912.82	2711.07	655.59	97.76	2741.54	642.7	3114.16	437.41	102.03];

% % MBB Beam (1.5% and 5%)
% inputVariable = [-241	113.7	1024.66	126.37	189.24	377.89	-30.34	1103.79	106.96	126.07	785.37	-44.34	1413.37	404.81	138.34	1171.49	223.81	1515.27	477.95	137.97	1479.6	379.01	2218.49	818.44	35.11	1848.31	36.01	3094.23	39.68	93.77	-258.51	163.27	547.35	-91.95	164.74	252.44	773.53	1582.78	-356.56	135.13	1175.51	267.11	954.48	-74.64	78.5	1046.52	30.26	2092.21	29.95	86.48	1643.73	35.85	2189.42	31.75	83.81	2579.96	461.56	3295.12	-275.89	128.2	-277.76	918.42	544.08	961.21	143.29	350.37	970.28	988.74	967.05	91.64	1254.05	844.93	1414.07	988.59	81.69	1437.1	1016.58	2075.35	938.29	91.6	1480.87	474.69	2139.47	947.02	118.19	2419.04	622.09	2572.14	469.41	54.85	-16.95	968.2	301.52	730.22	127.09	164.89	905.38	1417.4	852.32	95.52	800.01	947.63	1780.25	974.17	104.91	1194.84	877.42	2250.99	845.93	131.67	2211.31	818.23	2843.56	193.32	123.8	2345.61	689.43	2783.33	242.77	65.58];


N = 24;         %Number of features
featureVars = 5;%Number of design variables for each feature 
% Constants
% Regularised Heaviside parameters
alpha = 0; 
epsilon = 0.3; 
% Feature construction parameters
p = 6; %TDF penalty parameter. 
aggregationSelection = 'MAX';
calculationSelection = 'NodeBased'; % Either 'NodeBased' or 'ElementBased'
% Design domain properties
x0 = 0;
y0 = 0;
xDim = 3000; %Domain x dimension
yDim = 1000; %Domain y dimension
elSize = 5;  %Mesh element size
% Threshold Parameters
smallCompThreshold = 0.1; %Threshold for considering features with a small area
smallRatioThreshold = 0.3; %Threshold for considering features with a small unique area
angleLimitThreshold = 10; %Threshold for considering collinear features (in degrees)
geomErrorLimit = 1; %Input precentage e.g. 1.0%, 2.5%, etc 
% Visualisation
showFeatureSpines = false;
showFeatureIds = false;

% Variable initialisation
deletedList = [];
dontTouchList = [];
attemptedList = [];
mergeInfo = cell(N,5); % records 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[xElCount,yElCount,nodeCoords,elementNodes,x,y]=CreateMesh(x0,y0,xDim,yDim,elSize); 

compIdList = 1:N;
xy00=inputVariable(:); 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[~,~,H,~]=FeatureConstruction(xy00,featureVars,N,nodeCoords,p,alpha,epsilon,aggregationSelection,xElCount,yElCount);

H_master = H; %The original implicit representation of the aggregated features. This is only used as an output metric

mergeLoop = 1;
originalN = N;
while length(attemptedList) < originalN
    [Phi,~,~,H_features]=FeatureConstruction(xy00,featureVars,N,nodeCoords,p,alpha,epsilon,aggregationSelection,xElCount,yElCount);
    [featureSize,uniqueSize,intersectingFeatures,intersectingFeatureSize] = FeatureComparison(H_features,alpha,N,elementNodes,elSize);
        
    optN = 0;
    % Checking for small components
    [featureSizeRatio,I] = sort((featureSize/max(featureSize)));
    uniqueRatio = uniqueSize./featureSize;
    Lia = 1;
    counter = 0;
    while Lia == 1
        counter = counter + 1;
        selectedFeature = compIdList(I(counter));
        Lia = ismember(selectedFeature,dontTouchList);
    end
    SmallCompCheck = featureSizeRatio(counter);
    SmallRatioCheck = uniqueRatio(I(counter));
    collinearCheck = 2*pi();
    if SmallCompCheck < smallCompThreshold
        optN = length(intersectingFeatures{I(counter)});
        optimisedFeatureIds = intersectingFeatures{I(counter)};
        reason = 'Small feature size';
        val = SmallCompCheck;
    end
    
    % Checking for large intersections
    if optN == 0
        [uniqueRatioSorted,I] = sort(uniqueSize./featureSize);
        featureSizeRatio = featureSize/max(featureSize);
        Lia = 1;
        counter = 0;
        while Lia == 1
            counter = counter + 1;
            selectedFeature = compIdList(I(counter));
            Lia = ismember(selectedFeature,dontTouchList);
        end
        SmallCompCheck = featureSizeRatio(I(counter));
        SmallRatioCheck = uniqueRatioSorted(counter);
        if SmallRatioCheck < smallRatioThreshold
            optN = length(intersectingFeatures{I(counter)});
            optimisedFeatureIds = intersectingFeatures{I(counter)};
            reason = 'Small unique area';
            val = SmallRatioCheck;
        end
    end
    % Checking for collinear components
    if optN == 0
        featureCoords = reshape(xy00,featureVars,N)';
        featureAngles = atan2(featureCoords(:,4)-featureCoords(:,2),featureCoords(:,3)-featureCoords(:,1));
        featureAngles(logical(featureAngles(:,1)<0),1) = featureAngles(logical(featureAngles(:,1)<0),1) + pi();
        
%         % Setting the angle of collinear features in the don't touch list to 2*pi(). 
%         % This prevents identifying features that are collinear with a feature not to be touched
%         for j = 1:length(dontTouchList)
%             [~,temp] = ismember(dontTouchList(j),compIdList);
%             featureAngles(temp,1) = 4*pi();
%         end

        bestCollinear = ones(N,2);
        for j = 1:N
            angle1 = min(abs(featureAngles(intersectingFeatures{j})-featureAngles(j)));
            angle2 = min(abs(featureAngles(intersectingFeatures{j})-featureAngles(j)-pi()));
            bestCollinear(j,1) = min(angle1,angle2);
        end
        bestCollinear(:,2) = featureSize/max(featureSize);
        [collinear_ratio,I] = sortrows(bestCollinear);
        Lia = 1;
        counter = 0;
        while Lia == 1
            counter = counter + 1;
            selectedFeature = compIdList(I(counter));
            Lia = ismember(selectedFeature,dontTouchList);
        end
        collinearCheck = collinear_ratio(counter,1);
        SmallCompCheck = collinear_ratio(counter,2);
        SmallRatioCheck = uniqueRatio(I(counter));
        if collinearCheck < angleLimitThreshold/180*pi()
            optN = length(intersectingFeatures{I(counter)});
            optimisedFeatureIds = intersectingFeatures{I(counter)};
            reason = 'Collinear features';
            val = collinearCheck/pi()*180;
        else
            break
        end
    end
    
    attemptedList(end+1) = selectedFeature; %track feature order
    mergeInfo{selectedFeature,1} = mergeLoop;
    if SmallRatioCheck == 0 || SmallRatioCheck == 1
        %%%% Metrics for output information only, not used in algorithm %%%%
        optimisedFeatureIds = [];
        result = [];
        [originalAreaSum,deltaAreaSum] = AreaDeviation(xy00,featureVars,N,nodeCoords,p,xElCount,yElCount,alpha,epsilon,aggregationSelection,elementNodes,elSize,optimisedFeatureIds,I,counter,result);
        if SmallRatioCheck == 0
            mergeInfo{selectedFeature,2} = "Embedded Feature";
        else
            mergeInfo{selectedFeature,2} = "Unattached Feature";
        end
        mergeInfo{selectedFeature,3} = [originalAreaSum, deltaAreaSum];
        mergeInfo{selectedFeature,6} = deltaAreaSum/originalAreaSum*100;
        % Calculate area deviation from original design (output only, not used for assessment)
        [~,mergeInfo{selectedFeature,7}] = AreaDeviationFromInput(H_master,xy00,featureVars,N,nodeCoords,p,xElCount,yElCount,alpha,epsilon,aggregationSelection,elementNodes,elSize);
        
        % update feature params
        N = N - 1;
        deletedList(end+1) = selectedFeature;
        xy00(featureVars*I(counter)-featureVars+1: featureVars*I(counter)) = [];
        compIdList(I(counter)) = []; % Removing ID from full list of orginal comps
    else
        x_init = [];
        phiSubSelection=cell(optN+1,1);
        phiSubSelection{1} = Phi{I(counter)};
        for i=1:optN 
            phiSubSelection{i+1,1} = Phi{optimisedFeatureIds(i)}; 
            x_init(end + 1:end + featureVars) = xy00(featureVars*(optimisedFeatureIds(i))-featureVars+1: featureVars*(optimisedFeatureIds(i)));
        end
        aggregatedPhi = FeatureAggregation(phiSubSelection,size(phiSubSelection,1),aggregationSelection);

        H_subDomain = Heaviside(aggregatedPhi,alpha,xElCount,yElCount,epsilon);
        H_subDomain=H_subDomain(:);
        
        switch calculationSelection
        case 'NodeBased' 
            f = @(result)MergeFunction(result,alpha,xElCount,yElCount,epsilon,nodeCoords,p,H_subDomain,optN,featureVars,aggregationSelection);
        case 'ElementBased'
            elemAreasOrig = AreaCalc(H_subDomain,elementNodes,elSize);
            f = @(result)MergeFunction2(result,alpha,xElCount,yElCount,epsilon,nodeCoords,p,elemAreasOrig,optN,featureVars,aggregationSelection,elementNodes,elSize);
        otherwise
            warning('Unexpected calculation selection, reverted to NodeBased')
            f = @(result)MergeFunction(result,alpha,xElCount,yElCount,epsilon,nodeCoords,p,H_subDomain,optN,featureVars,aggregationSelection);
        end
        result = lsqnonlin(f,x_init);
        
        % Check shape matching success
        [originalAreaSum,deltaAreaSum] = AreaDeviation(xy00,featureVars,N,nodeCoords,p,xElCount,yElCount,alpha,epsilon,aggregationSelection,elementNodes,elSize,optimisedFeatureIds,I,counter,result);
        
        % Store updated design information
        mergeInfo{selectedFeature,3} = [originalAreaSum, deltaAreaSum];
        mergeInfo{selectedFeature,6} = deltaAreaSum/originalAreaSum*100;
        mergeInfo{selectedFeature,8} = reason;
        mergeInfo{selectedFeature,9} = val;
        
        if deltaAreaSum/originalAreaSum > geomErrorLimit/100
            %wrong move!
            dontTouchList(end+1) = selectedFeature;
            mergeInfo{selectedFeature,2} = "Shape matching unsuccessful";
        else
            
            N = N - 1;
            deletedList(end+1) = selectedFeature;
            % update xy00
            for i=1:optN 
                xy00(featureVars*(optimisedFeatureIds(i))-featureVars+1: featureVars*(optimisedFeatureIds(i))) = result(featureVars*(i-1)+1: featureVars*i);
            end
            xy00(featureVars*I(counter)-featureVars+1: featureVars*I(counter)) = [];
            compIdList(I(counter)) = []; % Removing ID from full list of orginal comps
            
            mergeInfo{selectedFeature,2} = "Shape matching successful";
            % Calculate area deviation from original design (output only, not used for assessment)
            [~,mergeInfo{selectedFeature,7}] = AreaDeviationFromInput(H_master,xy00,featureVars,N,nodeCoords,p,xElCount,yElCount,alpha,epsilon,aggregationSelection,elementNodes,elSize);
        end
    end
    
    mergeInfo{selectedFeature,4} = N; %Remaining features
    mergeInfo{selectedFeature,5} = xy00; %Design variables
    fprintf('Cycle %d; Processed feature %d\n',mergeLoop,selectedFeature)
    mergeLoop = mergeLoop + 1;

end


% Final design representation
[Phi,PhiMax,H_new,~]=FeatureConstruction(xy00,featureVars,N,nodeCoords,p,alpha,epsilon,aggregationSelection,xElCount,yElCount);

% Final Area
elementFinalArea(:,1:4) = H_new(elementNodes(:,2:5));
elementFinalArea(:,5) = sum(elementFinalArea(:,1:4),2)/4*((elSize)^2);
finalArea = sum(elementFinalArea(:,5));

FeaturePlot(Phi,N,alpha,epsilon,xDim,yDim,x,y,xElCount,yElCount,showFeatureSpines,xy00,featureVars,showFeatureIds)

toc %End timer

function [originalAreaSum,deltaAreaSum] = AreaDeviation(xy00,featureVars,N,nodeCoords,p,xElCount,yElCount,alpha,epsilon,aggregationSelection,elementNodes,elSize,optimisedFeatureIds,I,counter,result)
% Check shape matching success
    [~,~,H_org,~]=FeatureConstruction(xy00,featureVars,N,nodeCoords,p,alpha,epsilon,aggregationSelection,xElCount,yElCount);
    originalArea(:,1:4) = H_org(elementNodes(:,2:5));
    originalArea(:,5) = sum(originalArea(:,1:4),2)/4*((elSize)^2);
    originalAreaSum = sum(originalArea(:,5));
    
    N2 = N - 1;
    if isempty(optimisedFeatureIds)
        xy00(featureVars*I(counter)-featureVars+1: featureVars*I(counter)) = [];
    else
        for i=1:length(optimisedFeatureIds)
            xy00(featureVars*(optimisedFeatureIds(i))-featureVars+1: featureVars*(optimisedFeatureIds(i))) = result(featureVars*(i-1)+1: featureVars*i);
        end
        xy00(featureVars*I(counter)-featureVars+1: featureVars*I(counter)) = [];
    end
    [~,~,H_new,~]=FeatureConstruction(xy00,featureVars,N2,nodeCoords,p,alpha,epsilon,aggregationSelection,xElCount,yElCount);
    deltaH = abs(H_new - H_org);
    deltaArea(:,1:4) = deltaH(elementNodes(:,2:5));
    deltaArea(:,5) = sum(deltaArea(:,1:4),2)/4*((elSize)^2);
    deltaAreaSum = sum(deltaArea(:,5));
end

% For node based comparison
function F=MergeFunction(result,alpha,xElCount,yElCount,epsilon,nodeCoords,p,H_original,optN,featureVars,aggregationSelection) 

    [~,~,H_new,~]=FeatureConstruction(result,featureVars,optN,nodeCoords,p,alpha,epsilon,aggregationSelection,xElCount,yElCount);
    F = H_new(:) - H_original(:);
    
end
% For element based comparison
function F=MergeFunction2(result,alpha,xElCount,yElCount,epsilon,nodeCoords,p,elemAreasOrig,optN,featureVars,aggregationSelection,elementNodes,elSize) 

    [~,~,H_new,~]=FeatureConstruction(result,featureVars,optN,nodeCoords,p,alpha,epsilon,aggregationSelection,xElCount,yElCount);
    elemAreasNew = AreaCalc(H_new,elementNodes,elSize);
    
    F = elemAreasNew(:) - elemAreasOrig(:);
    
end