function roads_out = LEM_clean_roads(roads)
% LEM_clean_roads - remove invalid graph elements
% On input:
%     roads (raods struct): road graph info
%       (k).vertexes (nx3 array): vertexes for graph
%       (k).edges (mx2 array): vertex index pairs defining edges
% On output:
%     roads_out (roads struct): roads graph with:
%       same vertex edges removed
%       bi-directional edges removed
%       edges in the same direction remmoved
% Call:
%     roads2 = LEM_clean_roads(roads);
% Author:
%    T. Henderson
%    UU
%    Spring 2021
%

ANGLE_THRESH = 1*pi/180;

vertexes = roads.vertexes;
[num_vertexes,dummy] = size(vertexes);
edges = roads.edges;
[num_edges,dummy] = size(edges);

% remove same vertex edges
indexes = find(edges(:,1)==edges(:,2));
edges(indexes,:) = [];

% remove bi-directional edges
indexes = [];
for e1 = 1:num_edges-1
    v11 = edges(e1,1);
    v12 = edges(e1,2);
    for e2 = e1+1:num_edges
        v21 = edges(e2,1);
        v22 = edges(e2,2);
        if (v11==v22)&(v12==v21)
            indexes = [indexes; e1,e2];
        end
    end
end
if ~isempty(indexes)
    dups = indexes(:,2);
    edges(dups,:) = [];
end

% remove overlayed edges
for v = 1:num_vertexes
    pt1 = vertexes(v,:);
    nei = unique([edges(find(edges(:,1)==v),2);...
        edges(find(edges(:,2)==v),1)]);
    num_nei = length(nei);
    for n = 1:num_nei
        index1 = nei(n);
        pt2 = vertexes(index1,:);
        others = [nei(1:n-1);nei(n+1:end)];
        v12 = pt2 - pt1;
        v12 = v12/norm(v12);
        num_others = length(others);
        for n2 = 1:num_others
            index2 = others(n2);
            pt3 = vertexes(index2,:);
            v13 = pt3 - pt1;
            v13 = v13/norm(v13);
            theta = LEM_posori(acos(min(1,dot(v12,v13))));
            if theta<ANGLE_THRESH
                d12 = norm(pt2-pt1);
                d13 = norm(pt3-pt1);
                if d12>d13
                    ind1 = find(edges(:,1)==v&edges(:,2)==index1);
                    ind2 = find(edges(:,1)==index1&edges(:,2)==v);
                    if ~isempty(ind1)
                        edges(ind1,:) = [];
                    elseif ~isempty(ind2)
                        edges(ind2,:) = [];
                    end
                else
                    ind1 = find(edges(:,1)==v&edges(:,2)==index2);
                    ind2 = find(edges(:,1)==index2&edges(:,2)==v);
                    if ~isempty(ind1)
                        edges(ind1,:) = [];
                    elseif ~isempty(ind2)
                        edges(ind2,:) = [];
                    end
                end
            end
        end
    end
end

roads_out.vertexes = vertexes;
roads_out.edges = edges;
