function [axes] = getAxes(platformVertices)
    for i = 1:size(platformVertices,1)
        %Get the normals of the edges
        axes(i, 1) =  (platformVertices(mod(i, size(platformVertices,1)) + 1, 2) - platformVertices(i,2));
        axes(i, 2) = -(platformVertices(mod(i, size(platformVertices,1)) + 1, 1) - platformVertices(i,1));
        axes(i, :) = axes(i, :) / norm(axes(i, :));
    end
end

function [projection] = project2D(vertices, ax)
    min = dot(ax, vertices(1, :));
    max = min;
    
    for i = 2:size(vertices,1)
        temp = dot(ax, vertices(i, :));
        if temp < min
            min = temp;
        elseif temp > max
            max = temp;
        end
    end
    
    projection = [min, max];
end

function [overlap, minOverlap, maxOverlap] = findOverlap(projectionA, projectionB)
    
    if projectionB(2) <= projectionA(1) || ...
       projectionA(2) <= projectionB(1)
        %Separating Axis Found
        overlap = 0;
        minOverlap = 0;
        maxOverlap = 0;
    else
        overlap = 1;
        minOverlap = max([projectionA(1), projectionB(1)]); %Get the biggest min
        maxOverlap = min([projectionA(2), projectionB(2)]); %Get the smallest max
    end 
end
%% The start scipt is below
%Squares
plat = [-1 1; -1 -1; 1 -1; 1 1];
offset = 2 * ones(4,2);
plat1 = plat + .75 * [1 0; 1 0; 1 0; 1 0] + offset;
plat2 = plat - .5 * [1 1; 1 1; 1 1; 1 1] + offset;
plat3 = plat + .5 * [1 1; 1 1; 1 1; 1 1] + offset;

%Plotting stuff
figure
axis equal
%plat1
line([plat1(1,1) plat1(2,1)], [plat1(1,2) plat1(2,2)], 'Color', [0 1 0]);
line([plat1(2,1) plat1(3,1)], [plat1(2,2) plat1(3,2)], 'Color', [0 1 0]);
line([plat1(3,1) plat1(4,1)], [plat1(3,2) plat1(4,2)], 'Color', [0 1 0]);
line([plat1(4,1) plat1(1,1)], [plat1(4,2) plat1(1,2)], 'Color', [0 1 0]);
%plat2
line([plat2(1,1) plat2(2,1)], [plat2(1,2) plat2(2,2)], 'Color', [1 0 0]);
line([plat2(2,1) plat2(3,1)], [plat2(2,2) plat2(3,2)], 'Color', [1 0 0]);
line([plat2(3,1) plat2(4,1)], [plat2(3,2) plat2(4,2)], 'Color', [1 0 0]);
line([plat2(4,1) plat2(1,1)], [plat2(4,2) plat2(1,2)], 'Color', [1 0 0]);
%plat3
line([plat3(1,1) plat3(2,1)], [plat3(1,2) plat3(2,2)], 'Color', [0 0 1]);
line([plat3(2,1) plat3(3,1)], [plat3(2,2) plat3(3,2)], 'Color', [0 0 1]);
line([plat3(3,1) plat3(4,1)], [plat3(3,2) plat3(4,2)], 'Color', [0 0 1]);
line([plat3(4,1) plat3(1,1)], [plat3(4,2) plat3(1,2)], 'Color', [0 0 1]);
%End Plotting stuff

wedgeX = [plat1(:,1) plat2(:,1) plat3(:,1)];
wedgeY = [plat1(:,2) plat2(:,2) plat3(:,2)];
NumFaces = max([size(plat1, 1), size(plat2, 1), size(plat3, 1)]);
NumDim = 2;
NumPlatforms = 3;

axes = NaN(NumFaces, NumDim, NumPlatforms);
%For a wedge we only have to find the normals of 3 sides since 2 sides are
%parallel
for i = 1:NumPlatforms
axes(:, :, i) = getAxes([wedgeX(1:4, i) wedgeY(1:4, i)]);
end

overlaps = NaN(NumFaces * NumPlatforms, 4);
for i = 1:NumPlatforms
    for j = 1:NumFaces
        projA = project2D([wedgeX(:, i), wedgeY(:, i)], axes(j, :, i));
        projB = project2D([wedgeX(:, mod(i, NumPlatforms) + 1), wedgeY(:, mod(i, NumPlatforms) + 1)], axes(j, :, i));
        [overlap, minOverlap, maxOverlap] = findOverlap(projA, projB);
        line([axes(j, 1, i) * -3, axes(j, 1, i) * 3], [axes(j, 2, i) * -3, axes(j, 2, i) * 3]);
        if overlap == 0
            'Not overlapping!'
            return
        else
%              [overlaps((i-1) * NumFaces + j, :)] = project2D([minOverlap, maxOverlap], [axes(j, 2, i), -axes(j, 1, i)]);
            %startOfLine
            overlaps((i - 1) * NumFaces + j, 1) = minOverlap * axes(j, 1, i);
            overlaps((i - 1) * NumFaces + j, 2) = minOverlap * axes(j, 2, i);
            %endOfLine
            overlaps((i - 1) * NumFaces + j, 3) = maxOverlap * axes(j, 1, i);
            overlaps((i - 1) * NumFaces + j, 4) = maxOverlap * axes(j, 2, i);
            hold on
            % plot(mean(overlaps(:, 1)), mean(overlaps(:, 2)), '*r');
            plot(overlaps(:, 1), overlaps(:, 2), 'xr');
            plot(overlaps(:, 3), overlaps(:, 4), 'og');
            midpoint = [overlaps(:,1) + (overlaps(:, 3) - overlaps(:,1)) / 2, overlaps(:,2) + (overlaps(:, 4) - overlaps(:,2)) / 2];
            plot(midpoint(:,1), midpoint(:,2), 'dk');
        end
    end
end

hold on
plot(mean(midpoint((midpoint(:,1) ~= 0),1)), mean(midpoint((midpoint(:,2) ~= 0),2)), 'm*');