function [ connectionType, distance ] = computeGraph( vertices, uncertaintyFactor )
%COMPUTEGRAPH Summary of this function goes here
%   connectionType:
%       0 - no connection
%       1 - the two points are on the same side of the spring
%       2 - the two points are on the opposit side of the spring

numOfVertices = length(vertices);
connectionType = zeros(numOfVertices);
distance = zeros(numOfVertices);

pixelUncertainty = 10 * uncertaintyFactor; % pixels





% ekvidisztáns egyenesek (szegmensek) meghatározása és felvitele a gráfba
% egy oldali pontokként
equidistantErrorRadiusSqr = pixelUncertainty.^2;
segments = [];
unusedVertices = vertices;

ss = 1; % segment start
se = 2; % segment end

disp('Segment detection');
while true
    isin = true;
    while isin && se < length(vertices)
        isin = isSegmentContains(vertices(ss:se,:), vertices(se+1,:), equidistantErrorRadiusSqr);
        se = se + 1;
    end
    
    if ~isin
        se = se - 1;
    end
    
    if se == length(vertices) && isSegmentContains(vertices(ss:se,:), vertices(1,:), equidistantErrorRadiusSqr)
        % looping segment
        
        segments = segments(2:end,:);
        
        spart = vertices(ss:end,:);
        lastss = ss;
        ss = 1;
        se = 2;
        isin = true;
        while isin && se < lastss
            isin = isSegmentContains([vertices(ss:se,:); spart], vertices(se+1,:), equidistantErrorRadiusSqr);
            se = se + 1;
        end
        
        if se - ss + 1 + length(spart) >= 3
            segments = [segments; ss,se];
        end
        segments = [segments; lastss,se];
        
        break;
    else
        % normal segment
        
        if se - ss + 1 >= 3
            segments = [segments; ss,se];
        end

        if se == length(vertices)
           break;
        end
    end
    
    ss = se;
    se = ss + 1;
end

if length(segments) < 2
    disp(['Not enough segments detected (',num2str(length(segments)),')']);
    return;
else
    disp([num2str(length(segments)),' segments detected']);
end

% szegmensek betöltése a gráfba, a felhasznált pontok törlése a
% használatlanok közül
usedMask = zeros(length(vertices),1);

for i=1:length(segments)
    
    %
	segVertices = extractVertices(segments(i,:),vertices);
    plot(segVertices(:,1),segVertices(:,2),'ro');
    %
    
    seg = extractIndecies(segments(i,:),vertices);
    
    usedMask(seg) = 1;
    
	for j=2:length(seg)
        v1 = seg(j-1);
        v2 = seg(j);
        dist = sqrt(sum((vertices(v2,:)-vertices(v1,:)).^2));
        
        connectionType(v1,v2) = 1;
        connectionType(v2,v1) = 1;
        
        distance(v1,v2) = dist;
        distance(v2,v1) = dist;
        
        plot(vertices([v1,v2],1),vertices([v1,v2],2),'r');
    end
end





% szegmensek párosítása párhuzamosságuk, periódusuk és fázisuk alapján
parallelError = 10*pi/180;          % radians
periodError = pixelUncertainty*0.3; % pixels
phaseError = pixelUncertainty*0.6;  % pixels

phaseErrorSqr = (phaseError)^2;

disp('Segment fitting');

matches = 0;

for i=1:(length(segments)-1)
    ind1 = extractIndecies(segments(i,:),vertices);
    seg1 = extractVertices(segments(i,:),vertices);
    line1 = leastSquareLine(seg1);
    angle1 = atan(line1(1));
    period1 = segmentPeriod(seg1);
    pairBisectorLines = pairBisectors(seg1);
    
    for j=(i+1):length(segments)
        ind2 = extractIndecies(segments(j,:),vertices);
        seg2 = extractVertices(segments(j,:),vertices);
        
        % parallel check
        line2 = leastSquareLine(seg2);
        
        if abs(angle1 - atan(line2(1))) > parallelError
           disp([num2str(i),' to ',num2str(j),': parallel fail']);
           continue; 
        end
        
        
        % period check
        period2 = segmentPeriod(seg2);
        
        if abs(period1-period2) > periodError
            disp([num2str(i),' to ',num2str(j),': period  fail']);
            continue; 
        end
        
        % phase check
        isin = false;
        for k=1:size(pairBisectorLines,1)
            line = pairBisectorLines(k,:);
            
            for l=1:length(seg2)
                
                distanceSqr = (line(1)*seg2(l,1)+line(2)*seg2(l,2)+line(3))^2/(line(1)^2+line(2)^2);
                
                if distanceSqr < phaseErrorSqr
                    isin = true;
                    % add connection to graph

                    v1 = seg1(k,:);
                    v2 = seg1(k+1,:);
                    vopp = seg2(l,:);
                    
                    plot([vopp(1);v1(1)],[vopp(2);v1(2)],'r');
                    plot([vopp(1);v2(1)],[vopp(2);v2(2)],'r');

                    dist = sqrt(sum((vopp-v1).^2));

                    connectionType(ind1(k),ind2(l)) = 2;
                    connectionType(ind2(l),ind1(k)) = 2;

                    distance(ind1(k),ind2(l)) = dist;
                    distance(ind2(l),ind1(k)) = dist;

                    dist = sqrt(sum((vopp-v2).^2));

                    connectionType(ind1(k+1),ind2(l)) = 2;
                    connectionType(ind2(l),ind1(k+1)) = 2;

                    distance(ind1(k+1),ind2(l)) = dist;
                    distance(ind2(l),ind1(k+1)) = dist;

                    break
                end
            end
        end
        
        if ~isin
            disp([num2str(i),' to ',num2str(j),': phase  fail']);
            continue;
        end
        
        % passed every test
        disp([num2str(i),' to ',num2str(j),': success']);
        matches = matches + 1;
        
        % the matches were added to the graph in the phase check for loop
        
    end
end

if matches < 1
   disp(['Not enough segment matches (',num2str(matches),')']);
   return;
else
    disp([num2str(matches),' segment matches found']);
end



% cikk-cakk mintázat kiigazítása a be nem húzott átlók hozzáadásával
completingOpponents = 0;
for v0=1:length(usedMask)
    % if point is unused or already has both opponent connection
    if ~usedMask(v0) || sum(connectionType(v0,:) == 2) > 1
       continue; 
    end
    
    v1conn = connectionType(v0,:) == 1;
    v1 = find(v1conn);
    for i=1:length(v1)
        v1nbin = connectionType(v1(i),:) == 2;
        v1n = find(v1nbin);
        
        if length(v1n) < 1
            continue;
        end
        
        % eliminate candidates with too much connections
        for j=1:length(v1n)
            if sum(connectionType(v1n(j),:) == 2) > 1
                v1nbin(v1n(j)) = false;
            end
        end
        
        v1n = find(v1nbin);
        
        if length(v1n) < 1
            continue;
        end
        
        % get closer candidate to basepoint
        dist = getDistance(vertices(v0,:),vertices(v1nbin,:));
        vin2 = find(dist == min(dist));
        vin = v1n(vin2(1));
        
        connectionType(v0,vin) = 2;
        connectionType(vin,v0) = 2;
        
        distance(v0,vin) = dist(vin2(1));
        distance(vin,v0) = dist(vin2(1));
        
        ver0 = vertices(v0,:);
        ver1 = vertices(vin,:);
        
        completingOpponents = completingOpponents + 1;
        
        plot([ver0(1);ver1(1)],[ver0(2);ver1(2)],'r');
    end
end

disp([num2str(completingOpponents),' new edges found by topological check']);




% megkezdett struktúra növesztése
disp('Growth method');
growingErrorRadius = pixelUncertainty * 2;

multipleDetections = 0;
addedPoints = 0;
addedEdges = 0;

while true
    
    % type one growth method
    exit = true;
    possiblev0 = find((sum(connectionType == 1,2) == 1) & (sum(connectionType == 2,2) == 2));
    while length(possiblev0) > 0
        exit = false;
        v0 = possiblev0(1);
        possiblev0 = possiblev0(2:end);
        v00 = find(connectionType(v0,:) == 1);
        vop = setdiff(find(connectionType(v0,:) == 2),find(connectionType(v00,:) == 2));

        ver0 = vertices(v0,:);
        ver00 = vertices(v00,:);
        verop = vertices(vop,:);

        line = leastSquareLine([ver0;ver00]);


        n = [-line(2) line(1)];
        nnorm = n./sqrt(sum(n.^2));
        origo = ver0-2.*nnorm.*((ver0-verop)*nnorm');

        dist = getDistance(origo,vertices(~usedMask,:));
        inside = find(dist <= growingErrorRadius);
        if length(inside) == 1
            addedPoints = addedPoints + 1;
            addedEdges = addedEdges + 2;

            vn = find(~usedMask);
            vn = vn(inside);

            vern = vertices(vn,:);

            usedMask(vn) = true;

            dist = getDistance(vern,ver0);
            connectionType(v0,vn) = 1;
            connectionType(vn,v0) = 1;
            distance(v0,vn) = dist;
            distance(vn,v0) = dist;
            plot([ver0(1);vern(1)],[ver0(2);vern(2)],'r');

            dist = getDistance(vern,verop);
            connectionType(vop,vn) = 2;
            connectionType(vn,vop) = 2;
            distance(vop,vn) = dist;
            distance(vn,vop) = dist;
            plot([verop(1);vern(1)],[verop(2);vern(2)],'r');

            vop2 = setdiff(find(connectionType(vop,:) == 1),find(connectionType(v0,:) == 2));
            if length(vop2) == 1
                % vop2 is known
                addedEdges = addedEdges + 1;

                verop2 = vertices(vop2,:);

                dist = getDistance(vern,verop2);
                connectionType(vop2,vn) = 2;
                connectionType(vn,vop2) = 2;
                distance(vop2,vn) = dist;
                distance(vn,vop2) = dist;
                plot([verop2(1);vern(1)],[verop2(2);vern(2)],'r');

                % eset: vn bekerül a possible listába, ha két opponentje van
                possiblev0 = [possiblev0 vn]; 
            else
                % eset: vop bekerül a possible listába, ha csak egy 1-ese van
                possiblev0 = [possiblev0 vop]; 
            end
        else
            if length(inside) > 1
                multipleDetections = multipleDetections + 1;
            end
            continue;
        end
    end
    
    
    % type two growth method (angle halfing)
    exit = true;
    %possiblev0 = find((sum(connectionType == 1,2) == 2) & (sum(connectionType == 2,2) == 1));
    while length(possiblev0) > 0
        exit = false;
        % TODO
    end
    
    if exit
        break;
    end
end

disp([num2str(addedPoints),' new points used']);
disp([num2str(addedEdges),' new edges added']);
disp([num2str(multipleDetections),' multiple detections found']);

end % computeGraph



% predicts connection between a used vertex and the unused vertices
function preds = getPredictions(index,vertices,connectionType,maxError)
    % preds = [error index index2 type]
    preds = [];
    %TODO;
end


function dist = getDistance(p1,p2)
    if size(p1,1) > size(p2,1)
        t = p1;
        p1 = p2;
        p2 = t;
    end
    if size(p1,1) == 1 && size(p2,1) > 1
        p1 = repmat(p1,length(p2),1);
    end
    dist = sqrt(sum((p1-p2).^2,2));
end

function isin = isSegmentContains(segment, vertex, equidistantErrorRadiusSqr)

% maybe this should be changed to a least square line fitting method where
% the fitting line and the period is calculated 

p1 = segment(end-1,:);
p2 = segment(end,:);
pn = p2 + (p2 - p1);

isin = sum(( pn-vertex ).^2) <= equidistantErrorRadiusSqr;
    
end

function line = leastSquareLine(vertices)
    % line = [a b c] where a, b and c is in a*x+b*y+c=0 form
    
    n = length(vertices);
    b1 = n*sum(vertices(:,1)'*vertices(:,2)) - sum(vertices(:,1))*sum(vertices(:,2));
    b1 = b1 / (n*sum(vertices(:,1).^2) - (sum(vertices(:,1)))^2);
    
    if abs(b1) ~= inf && ~isnan(b1)
        b2 = (sum(vertices(:,2))-b1*sum(vertices(:,1))) / n;
        a = -b1;
        b = 1;
        c = -b2;
    else
        v2 = [vertices(:,2), vertices(:,1)];
        line = leastSquareLine(v2);
        a = line(2);
        b = line(1);
        c = line(3);
    end
    
    line = [a b c];
end

function period = segmentPeriod(segment)
    period = 0;
    for i=2:length(segment)
       period = period + sqrt(sum((segment(i-1,:)-segment(i,:)).^2)); 
    end
    period = period / (length(segment)-1);
end

function indecies = extractIndecies(segment,vertices)
    if segment(1) <= segment(2)
       indecies = segment(1):segment(2);
    else
       indecies = [segment(1):length(vertices) 1:segment(2)];
    end
end

function coords = extractVertices(segment,vertices)
    if segment(1) <= segment(2)
       coords = vertices(segment(1):segment(2),:);
    else
       coords = [vertices(segment(1):end,:);vertices(1:segment(2),:)];
    end
end

function lines = pairBisectors(vertices)
    % lines = [a b c;...] where a, b and c is in a*x+b*y+c=0 form
    
    lines = zeros(length(vertices)-1,3);
    for i=2:length(vertices)
        midpoint = (vertices(i,:)+vertices(i-1,:))./2;
        a = (vertices(i-1,1)-vertices(i,1)); % x1-x2
        a = a / (vertices(i,2)-vertices(i-1,2)); % y2-y1
        b = midpoint(2)-a*midpoint(1);
        
        if abs(a) ~= inf && ~isnan(a)
            lines(i-1,:) = [-a 1 -b];
        else
            v2 = [vertices(i-1:i,2) vertices(i-1:i,1)];
            l2 = pairBisectors(v2);
            lines(i-1,:) = [l2(2) l2(1) l2(3)];
        end
    end
end
