%clear

tic %Start timer

% Cantilever Beam
inputVariable = [-108.797979211583,65.4435901909376,488.286882509882,235.094790586772,97.1191736430205,331.918421013197,175.731950573580,1050.77728508473,486.560552052767,104.785427229182,1234.01243148924,137.759820336610,1919.45284239946,522.052369672998,85.9028089769677,2266.72458336935,103.599913896207,3075.49878607669,541.543891303967,98.6392171290896,-71.3463258171122,49.2178987894281,1317.40382443206,83.0676994496304,218.359288429156,1013.96554469604,481.869437007546,1276.10572684622,104.548183536541,53.0824222272433,1164.20391095123,19.7310498422625,2314.97281639810,82.2142143628687,125.136216827434,1936.53000000000,456.560000000000,2205.78000000000,90.7800000000000,55.3800000000000,-58.8016823416695,949.586086931046,1358.02558867339,913.047861018152,218.899726360899,1013.44179992005,467.767024594834,1321.88877750984,889.020608060019,56.0274178412278,1011.65765475049,981.104619197526,2269.40349022673,932.020448154246,116.771211275464,1816.20000000000,417.230000000000,2195.25000000000,948.590000000000,54.5600000000000,-53.8673576299499,959.525237989423,390.022754316555,826.811407004563,173.207446220761,-69.5681018808572,1009.54891329966,1076.24691725608,449.291892556905,105.676682680107,1280.20193281421,859.569909668926,1954.88088340645,452.445026064402,91.9850266919233,2202.91341765870,921.668556747939,3659.16287338259,160.078702198548,98.6695638998031];
N = 16;         %Number of features

% MBB Beam
% inputVariable = [-4265.034285	42.82528376	1155.260374	144.9266098	173.4104155	-1243669.853	-12543.70098	23340030.26	235651.0428	108.7633277	-1194.070235	1990.138818	29959.87233	-24297.3968	149.7435892	-513.9794548	995.5768614	2234.646675	878.135887	218.0514633	-4567.406341	-4018.413461	2100.69534	924.3200639	136.015307	2196.50903	834.0281359	3055.965485	-18.87213008	125.5687262];
% N = 6;         %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
ghostOffsetDim = 100;
% Threshold Parameters
angleLimitThreshold = 0; %for identifying collinear components at the start of the program (e.g. 0, 5)
voidPenalty = 10;
solidPenalty = 10;
% Visualisation
showFeatureSpines = false;
showFeatureIds = false;

% Variable initialisation
intersectionInfo = cell(N,2); % records 
featureIdList = 1:N;
xy00=inputVariable(:); 

%Create Mesh
[xElCount,yElCount,nodeCoords,elementNodes,x,y] = CreateMesh(x0,y0,xDim,yDim,elSize);
%Create ghost layers
[xEnlargedElCount,yEnlargedElCount,nodeCoordsEnlarged,elementNodesEnlarged,xEnlarged,yEnlarged] = CreateMesh(-ghostOffsetDim:elSize,-ghostOffsetDim:elSize,xDim+ghostOffsetDim,yDim+ghostOffsetDim,elSize);
outerNodes = logical(nodeCoordsEnlarged(:,1) < x0 | nodeCoordsEnlarged(:,1) > xDim | nodeCoordsEnlarged(:,2) < y0 | nodeCoordsEnlarged(:,2) > yDim);
outerElems = logical(sum(outerNodes(elementNodesEnlarged(:,2:5,1)),2)>0);

% Original representation of the features
[~,~,H_master,~]=FeatureConstruction(xy00,featureVars,N,nodeCoords,p,alpha,epsilon,aggregationSelection,xElCount,yElCount);

% Minimise features extending outside the design domain
switch calculationSelection
case 'NodeBased' 
    xy00 = MinimiseOutsideDesignDomain(xy00,N,featureVars,xEnlargedElCount,yEnlargedElCount,nodeCoordsEnlarged,outerNodes,alpha,epsilon,p,ghostOffsetDim,xDim,yDim);
case 'ElementBased'
    xy00 = MinimiseOutsideDesignDomain2(xy00,N,featureVars,xEnlargedElCount,yEnlargedElCount,nodeCoordsEnlarged,outerElems,alpha,epsilon,p,ghostOffsetDim,xDim,yDim,elementNodesEnlarged,elSize);
otherwise
    warning('Unexpected calculation selection, reverted to NodeBased')
    xy00 = MinimiseOutsideDesignDomain(xy00,N,featureVars,xEnlargedElCount,yEnlargedElCount,nodeCoordsEnlarged,outerNodes,alpha,epsilon,p,ghostOffsetDim,xDim,yDim);
end

featureVarsTrim = xy00'; %Record of the feature design variables

% Find intersecting features
[Phi,~,~,H_features]=FeatureConstruction(xy00,featureVars,N,nodeCoords,p,alpha,epsilon,aggregationSelection,xElCount,yElCount);
[~,~,intersectingFeatures,~] = FeatureComparison(H_features,alpha,N,elementNodes,elSize);

featureCoords = reshape(xy00,featureVars,N)';
featureWidths = featureCoords(:,5);
featureAngles = atan2(featureCoords(:,4)-featureCoords(:,2),featureCoords(:,3)-featureCoords(:,1));
featureAngles(logical(featureAngles(:,1)<0),1) = featureAngles(logical(featureAngles(:,1)<0),1) + pi();

%Minimise the overlap of collinear components.
for i = 1:N
    intersectionInfo{i,1} = i;
    if isempty(intersectingFeatures{i})
        intersectionInfo{i,2} = 'No intersecting features';
    else
        interlist = intersectingFeatures{i};
        for j = 1:length(interlist)
            if abs(featureAngles(interlist(j))-featureAngles(i)) < pi()/180*angleLimitThreshold || abs(pi()-featureAngles(interlist(j))-featureAngles(i)) < pi()/180*angleLimitThreshold
                if featureWidths(i) < featureWidths(interlist(j))
                    reducingFeature = i;
                    fixedFeature = interlist(j);
                else
                    reducingFeature = interlist(j);
                    fixedFeature = i;
                end
                phiSubSelection=cell(2,1);
                phiSubSelection{1,1} = Phi{fixedFeature};
                phiSubSelection{2,1} = Phi{reducingFeature};
                aggregatedPhi = FeatureAggregation(phiSubSelection,size(phiSubSelection,1),aggregationSelection);
                H_subdomain = Heaviside(aggregatedPhi,alpha,xElCount,yElCount,epsilon);
                H_fixedFeatures = Heaviside(Phi{fixedFeature},alpha,xElCount,yElCount,epsilon);
                x_init = xy00(featureVars*reducingFeature-featureVars+1: featureVars*reducingFeature);
                switch calculationSelection
                case 'NodeBased' 
                    f = @(result)MinimiseIntersections(result,alpha,xElCount,yElCount,epsilon,nodeCoords(:,1),nodeCoords(:,2),p,H_subdomain,H_fixedFeatures,voidPenalty,solidPenalty);
                case 'ElementBased'
                    subdomainArea = AreaCalc(H_subdomain,elementNodes,elSize);
                    fixedFeaturesArea = AreaCalc(H_fixedFeatures,elementNodes,elSize);
                    f = @(result)MinimiseIntersections2(result,alpha,xElCount,yElCount,epsilon,nodeCoords(:,1),nodeCoords(:,2),p,subdomainArea,fixedFeaturesArea,voidPenalty,solidPenalty,elementNodes,elSize);
                otherwise
                    warning('Unexpected calculation selection, reverted to NodeBased')
                    f = @(result)MinimiseIntersections(result,alpha,xElCount,yElCount,epsilon,nodeCoords(:,1),nodeCoords(:,2),p,H_subdomain,H_fixedFeatures,voidPenalty,solidPenalty);
                end
                % Optimisation
                options = optimoptions(@lsqnonlin,'MaxFunEvals',2000);
                result = lsqnonlin(f,x_init,[],[],options);
                xy00(featureVars*(reducingFeature)-featureVars+1: featureVars*(reducingFeature)) = result(1: featureVars);
                [Phi,~,~,~]=FeatureConstruction(xy00,featureVars,N,nodeCoords,p,alpha,epsilon,aggregationSelection,xElCount,yElCount);
                [originalAreaSum,deltaAreaSum] = AreaDeviationFromInput(H_master,xy00,featureVars,N,nodeCoords,p,xElCount,yElCount,alpha,epsilon,aggregationSelection,elementNodes,elSize);
                intersectionInfo{i,j+1} = deltaAreaSum/originalAreaSum*100;
            else
                intersectionInfo{i,j+1} = 'Not collinear';
            end
        end
    end
end
intersectionInfo{N+1,1} = 'Finshed processing collinear features';
featureVarsCollinearIntersections = xy00'; %Record design variables after collinear feature intersection minimisation

% Execute feature comparison prior to further intersection minimisation
[Phi,~,~,H_features]=FeatureConstruction(xy00,featureVars,N,nodeCoords,p,alpha,epsilon,aggregationSelection,xElCount,yElCount);
[featureSize,uniqueSize,intersectingFeatures,~] = FeatureComparison(H_features,alpha,N,elementNodes,elSize);
[featureSizeRatio,I] = sort((featureSize/max(featureSize)),'descend');
uniqueRatio = uniqueSize./featureSize;

for k = 1:N
    selectedFeature = featureIdList(I(k));
    intersectionInfo{N+1+k,1} = selectedFeature;
    intersectingFeatureList = intersectingFeatures{selectedFeature};
    if isempty(intersectingFeatureList)
        intersectionInfo{N+1+k,2} = 'No intersecting features';
    else
        [~,I2] = sort(uniqueRatio(intersectingFeatureList));
        intersectingFeatureList = intersectingFeatureList(I2);
        for i = 1:length(intersectingFeatureList)
            remain_list = intersectingFeatureList;
            reducingFeature = intersectingFeatureList(i);
            remain_list(i) = [];
            phiSubSelection=cell(1,1);
            phiSubSelection{1} = Phi{selectedFeature};
            for j=1:length(remain_list) 
                phiSubSelection{j+1,1} = Phi{remain_list(j)}; 
            end
            aggregatedPhi = FeatureAggregation(phiSubSelection,size(phiSubSelection,1),aggregationSelection);
            H_fixedFeatures = Heaviside(aggregatedPhi,alpha,xElCount,yElCount,epsilon);
            phiSubSelection{length(intersectingFeatureList)+1,1} = Phi{reducingFeature};
            aggregatedPhi = FeatureAggregation(phiSubSelection,size(phiSubSelection,1),aggregationSelection);
            H_subdomain = Heaviside(aggregatedPhi,alpha,xElCount,yElCount,epsilon);
            x_init = xy00(featureVars*(reducingFeature)-featureVars+1: featureVars*(reducingFeature));
            switch calculationSelection
            case 'NodeBased' 
                f = @(result)MinimiseIntersections(result,alpha,xElCount,yElCount,epsilon,nodeCoords(:,1),nodeCoords(:,2),p,H_subdomain,H_fixedFeatures,voidPenalty,solidPenalty);
            case 'ElementBased'
                subdomainArea = AreaCalc(H_subdomain,elementNodes,elSize);
                fixedFeaturesArea = AreaCalc(H_fixedFeatures,elementNodes,elSize);
                f = @(result)MinimiseIntersections2(result,alpha,xElCount,yElCount,epsilon,nodeCoords(:,1),nodeCoords(:,2),p,subdomainArea,fixedFeaturesArea,voidPenalty,solidPenalty,elementNodes,elSize);
            otherwise
                warning('Unexpected calculation selection, reverted to NodeBased')
                f = @(result)MinimiseIntersections(result,alpha,xElCount,yElCount,epsilon,nodeCoords(:,1),nodeCoords(:,2),p,H_subdomain,H_fixedFeatures,voidPenalty,solidPenalty);
            end
            % Optimisation
            result = lsqnonlin(f,x_init);
            xy00(featureVars*(reducingFeature-1)+1: featureVars*(reducingFeature)) = result;

            [Phi,~,~,~]=FeatureConstruction(xy00,featureVars,N,nodeCoords,p,alpha,epsilon,aggregationSelection,xElCount,yElCount);
            [originalAreaSum,deltaAreaSum] = AreaDeviationFromInput(H_master,xy00,featureVars,N,nodeCoords,p,xElCount,yElCount,alpha,epsilon,aggregationSelection,elementNodes,elSize);
            intersectionInfo{N+1+k,i+1} = deltaAreaSum/originalAreaSum*100;
        end
    end
end

featureVarsAllIntersections = xy00';
% Re-trim required as the optimisation may shot features into the void region
switch calculationSelection
case 'NodeBased' 
    xy00 = MinimiseOutsideDesignDomain(xy00,N,featureVars,xEnlargedElCount,yEnlargedElCount,nodeCoordsEnlarged,outerNodes,alpha,epsilon,p,ghostOffsetDim,xDim,yDim);
case 'ElementBased'
    xy00 = MinimiseOutsideDesignDomain2(xy00,N,featureVars,xEnlargedElCount,yEnlargedElCount,nodeCoordsEnlarged,outerElems,alpha,epsilon,p,ghostOffsetDim,xDim,yDim,elementNodesEnlarged,elSize);
otherwise
    warning('Unexpected calculation selection, reverted to NodeBased')
    xy00 = MinimiseOutsideDesignDomain(xy00,N,featureVars,xEnlargedElCount,yEnlargedElCount,nodeCoordsEnlarged,outerNodes,alpha,epsilon,p,ghostOffsetDim,xDim,yDim);
end
featureVarsReTrim = xy00';

[Phi,~,~,~]=FeatureConstruction(xy00,featureVars,N,nodeCoords,p,alpha,epsilon,aggregationSelection,xElCount,yElCount);
FeaturePlot(Phi,N,alpha,epsilon,xDim,yDim,x,y,xElCount,yElCount,showFeatureSpines,xy00,featureVars,showFeatureIds)

toc %End timer


function xy00 = MinimiseOutsideDesignDomain(xy00,N,featureVars,xEnlargedElCount,yEnlargedElCount,nodeCoordsEnlarged,outerNodes,alpha,epsilon,p,ghostOffsetDim,xDim,yDim)

%Initial curtailment of the features to the boundary of the inflated domain (the boundary of the added ghost layers)
xy00 = FeatureHardTrim(xy00,N,featureVars,ghostOffsetDim,xDim,yDim);
for k = 1:N
    ctrlpnts = xy00(featureVars*k-featureVars+1: featureVars*k);
    phi = tPhi(ctrlpnts,nodeCoordsEnlarged(:,1),nodeCoordsEnlarged(:,2),p);
    H_sub = Heaviside(phi,alpha,xEnlargedElCount,yEnlargedElCount,epsilon);
    H_sub = H_sub(:);
    H_sub(outerNodes,1) = 0;
    x_init = ctrlpnts;
    f = @(result)MinimiseOutside(result,alpha,xEnlargedElCount,yEnlargedElCount,epsilon,nodeCoordsEnlarged(:,1),nodeCoordsEnlarged(:,2),p,H_sub);
    result = lsqnonlin(f,x_init);
    xy00(featureVars*k-featureVars+1: featureVars*k) = result;
end

end

function xy00 = MinimiseOutsideDesignDomain2(xy00,N,featureVars,xEnlargedElCount,yEnlargedElCount,nodeCoordsEnlarged,outerElems,alpha,epsilon,p,ghostOffsetDim,xDim,yDim,elementNodesEnlarged,elSize)

%Initial curtailment of the features to the boundary of the inflated domain (the boundary of the added ghost layers)
xy00 = FeatureHardTrim(xy00,N,featureVars,ghostOffsetDim,xDim,yDim);
for k = 1:N
    ctrlpnts = xy00(featureVars*k-featureVars+1: featureVars*k);
    phi = tPhi(ctrlpnts,nodeCoordsEnlarged(:,1),nodeCoordsEnlarged(:,2),p);
    H_sub = Heaviside(phi,alpha,xEnlargedElCount,yEnlargedElCount,epsilon);
    areaSub = AreaCalc(H_sub,elementNodesEnlarged,elSize);
    areaSub(outerElems,1) = 0;
    x_init = ctrlpnts;
    f = @(result)MinimiseOutside2(result,alpha,xEnlargedElCount,yEnlargedElCount,epsilon,nodeCoordsEnlarged(:,1),nodeCoordsEnlarged(:,2),p,areaSub,elementNodesEnlarged,elSize);
    result = lsqnonlin(f,x_init);
    xy00(featureVars*k-featureVars+1: featureVars*k) = result;
end

end

function xy00 = FeatureHardTrim(xy00,N,featureVars,ghostOffsetDim,xDim,yDim)

uppX = xDim+ghostOffsetDim;
uppY = yDim+ghostOffsetDim;
for k = 1:N
    ctrlpnts = xy00(featureVars*k-featureVars+1: featureVars*k);
    if ctrlpnts(1) < -ghostOffsetDim
        temp_t = (-ghostOffsetDim - ctrlpnts(1))/(ctrlpnts(3)-ctrlpnts(1));
        ctrlpnts(1) = -ghostOffsetDim;
        ctrlpnts(2) = ctrlpnts(2)*(1-temp_t) + ctrlpnts(4)*temp_t;
    elseif ctrlpnts(1) > uppX
        temp_t = (uppX - ctrlpnts(1))/(ctrlpnts(3)-ctrlpnts(1));
        ctrlpnts(1) = uppX;
        ctrlpnts(2) = ctrlpnts(2)*(1-temp_t) + ctrlpnts(4)*temp_t;        
    end
    if ctrlpnts(3) < -ghostOffsetDim
        temp_t = (-ghostOffsetDim - ctrlpnts(1))/(ctrlpnts(3)-ctrlpnts(1));
        ctrlpnts(3) = -ghostOffsetDim;
        ctrlpnts(4) = ctrlpnts(2)*(1-temp_t) + ctrlpnts(4)*temp_t;
    elseif ctrlpnts(3) > uppX
        temp_t = (uppX - ctrlpnts(1))/(ctrlpnts(3)-ctrlpnts(1));
        ctrlpnts(3) = uppX;
        ctrlpnts(4) = ctrlpnts(2)*(1-temp_t) + ctrlpnts(4)*temp_t;        
    end
    
    if ctrlpnts(2) < -ghostOffsetDim
        temp_t = (-ghostOffsetDim - ctrlpnts(2))/(ctrlpnts(4)-ctrlpnts(2));
        ctrlpnts(2) = -ghostOffsetDim;
        ctrlpnts(1) = ctrlpnts(1)*(1-temp_t) + ctrlpnts(3)*temp_t;
    elseif ctrlpnts(2) > uppY
        temp_t = (uppY - ctrlpnts(2))/(ctrlpnts(4)-ctrlpnts(2));
        ctrlpnts(2) = uppY;
        ctrlpnts(1) = ctrlpnts(1)*(1-temp_t) + ctrlpnts(3)*temp_t;       
    end
    if ctrlpnts(4) < -ghostOffsetDim
        temp_t = (-ghostOffsetDim - ctrlpnts(2))/(ctrlpnts(4)-ctrlpnts(2));
        ctrlpnts(4) = -ghostOffsetDim;
        ctrlpnts(3) = ctrlpnts(1)*(1-temp_t) + ctrlpnts(3)*temp_t;
    elseif ctrlpnts(4) > uppY
        temp_t = (uppY - ctrlpnts(2))/(ctrlpnts(4)-ctrlpnts(2));
        ctrlpnts(4) = uppY;
        ctrlpnts(3) = ctrlpnts(1)*(1-temp_t) + ctrlpnts(3)*temp_t;       
    end
    xy00(featureVars*k-featureVars+1: featureVars*k) = ctrlpnts;
end

end

function F=MinimiseOutside(result,alpha,xEnlargedElCount,yEnlargedElCount,epsilon,nodeCoordsEnlargedX,nodeCoordsEnlargedY,p,H_originalFeature) 

phi = tPhi(result,nodeCoordsEnlargedX,nodeCoordsEnlargedY,p);
H_updated = Heaviside(phi,alpha,xEnlargedElCount,yEnlargedElCount,epsilon);
F = (H_originalFeature(:) - H_updated(:));

end

function F=MinimiseOutside2(result,alpha,xEnlargedElCount,yEnlargedElCount,epsilon,nodeCoordsEnlargedX,nodeCoordsEnlargedY,p,areaOriginal,elementNodesEnlarged,elSize) 

phi = tPhi(result,nodeCoordsEnlargedX,nodeCoordsEnlargedY,p);
H_updated = Heaviside(phi,alpha,xEnlargedElCount,yEnlargedElCount,epsilon);
areaUpdated = AreaCalc(H_updated,elementNodesEnlarged,elSize);
F = (areaOriginal(:) - areaUpdated(:));

end

function F=MinimiseIntersections(result,alpha,xElCount,yElCount,epsilon,nodeCoordsX,nodeCoordsY,p,H_subdomain,H_fixedFeatures,voidPenalty,solidPenalty) 

phi = tPhi(result,nodeCoordsX,nodeCoordsY,p);
H_reducingFeature = Heaviside(phi(:),alpha,xElCount,yElCount,epsilon); % The feature being minimised

nonVoidNodes = logical(H_subdomain(:)); % Subdomain of feature n and intersecting features that are non-void
reducingFeatureUniqueSolidNodes = logical((H_subdomain(:) - H_fixedFeatures(:))==1); % The unique elements of feature m (the feature being minimised) that are solid

F = H_subdomain(:) - H_fixedFeatures(:) -  H_reducingFeature(:);

F(~nonVoidNodes) = F(~nonVoidNodes).*voidPenalty; %Elements originally void in the design domain (i.e. elements that do not form part of the subdomain)
F(reducingFeatureUniqueSolidNodes) = F(reducingFeatureUniqueSolidNodes).*solidPenalty; %Unique elements of feature m (the feature being minimised). 

end

function F=MinimiseIntersections2(result,alpha,xElCount,yElCount,epsilon,nodeCoordsX,nodeCoordsY,p,subdomainArea,fixedFeaturesArea,voidPenalty,solidPenalty,elementNodes,elSize) 

phi = tPhi(result,nodeCoordsX,nodeCoordsY,p);
H_reducingFeature = Heaviside(phi(:),alpha,xElCount,yElCount,epsilon); % The feature being minimised
reducingFeatureArea = AreaCalc(H_reducingFeature,elementNodes,elSize);

nonVoidElements = logical(subdomainArea(:)); % Subdomain of feature n and intersecting features that are non-void
reducingFeatureUniqueSolidElements = logical((subdomainArea(:) - fixedFeaturesArea(:))>0); % The unique elements of feature m (the feature being minimised) that are solid

F = subdomainArea(:) - fixedFeaturesArea(:) -  reducingFeatureArea(:);

F(~nonVoidElements) = F(~nonVoidElements).*voidPenalty; %Elements originally void in the design domain (i.e. elements that do not form part of the subdomain)
F(reducingFeatureUniqueSolidElements) = F(reducingFeatureUniqueSolidElements).*solidPenalty; %Unique elements of feature m (the feature being minimised). 

end