diff --git a/src/classification.jl b/src/classification.jl
index d640ac6..1e905bb 100644
--- a/src/classification.jl
+++ b/src/classification.jl
@@ -25,9 +25,18 @@ filter_highways(ways::Vector{OpenStreetMapX.Way}) = [way for way in ways if Open
 ### Filter and Classify Highways for Cars ###
 ##############################################
 
+function valid_roadway(way, levels::Set{Int}, classes::Dict{String, Int} = OpenStreetMapX.ROAD_CLASSES)
+    highway = get(way.tags, "highway", "")
+    if isempty(highway) || highway == "services" || !haskey(classes, highway)
+        return false
+    end
+    return OpenStreetMapX.visible(way) && classes[highway] in levels
+end
+
 filter_roadways(ways::Vector{OpenStreetMapX.Way}, classes::Dict{String, Int} = OpenStreetMapX.ROAD_CLASSES; levels::Set{Int} = Set(1:length(OpenStreetMapX.ROAD_CLASSES))) = [way for way in ways if way.tags["highway"] in keys(classes) && classes[way.tags["highway"]] in levels]
 
-classify_roadways(ways::Vector{OpenStreetMapX.Way}, classes::Dict{String, Int} = OpenStreetMapX.ROAD_CLASSES) = Dict{Int,Int}(way.id => classes[way.tags["highway"]] for  way in ways if haskey(classes, way.tags["highway"]))
+classify_roadway(way::Way, classes::Dict{String, Int} = OpenStreetMapX.ROAD_CLASSES) = classes[way.tags["highway"]]
+classify_roadways(ways::Vector{OpenStreetMapX.Way}, classes::Dict{String, Int} = OpenStreetMapX.ROAD_CLASSES) = Dict{Int,Int}(way.id => classify_roadway(way, classes) for way in ways if haskey(classes, way.tags["highway"]))
 
 ####################################################
 ### Filter and Classify Highways for Pedestrians ###
@@ -143,4 +152,4 @@ function filter_graph_features(features::Dict{Int,Tuple{String,String}}, graphFe
     end
     level = classes[class]
     Dict{Int,Int}(key => node for (key,node) in graphFeatures if classes[features[key][1]] == level) 
-end
\ No newline at end of file
+end
diff --git a/src/intersections.jl b/src/intersections.jl
index 5f52573..ff08a12 100644
--- a/src/intersections.jl
+++ b/src/intersections.jl
@@ -27,7 +27,7 @@ reverseway(w::OpenStreetMapX.Way) = (get(w.tags,"oneway", "") == "-1")
 """
 Compute the distance of a route for some `nodes` data
 """
-function distance(nodes::Dict{Int,T}, route::Vector{Int}) where T<:(Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF})
+function distance(nodes::Dict{Int,T}, route::AbstractVector{Int}) where T<:(Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF})
     if length(route) == 0
         return Inf
     end
@@ -85,3 +85,42 @@ function find_segments(nodes::Dict{Int,T}, highways::Vector{OpenStreetMapX.Way},
     end
     return segments
 end
+
+function get_edges_distances(nodes::Dict{Int,T}, highways::Vector{OpenStreetMapX.Way}, intersections::Dict{Int,Set{Int}}) where T<:Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF}
+    back = Dict{Tuple{Int,Int},Int}()
+    e = Tuple{Int,Int}[]
+    class = Int[]
+    weight_vals = Float64[]
+    function add_segment(way, path)
+        edge = (first(path), last(path))
+        weight = OpenStreetMapX.distance(nodes, path)
+        if haskey(back, edge)
+            i = back[edge]
+            if weight < weight_vals[i]
+                class[i] = classify_roadway(way)
+                weight_vals[i] = weight
+            end
+        else
+            push!(e, edge)
+            push!(class, classify_roadway(way))
+            push!(weight_vals, weight)
+            back[edge] = length(e)
+        end
+    end
+    for highway in highways
+        firstNode = 1
+        for j = 2:length(highway.nodes)
+            if highway.nodes[firstNode] != highway.nodes[j] && (haskey(intersections, highway.nodes[j]) || j == length(highway.nodes))
+                rev = reverseway(highway)
+                if !rev
+                    add_segment(highway, view(highway.nodes, firstNode:j))
+                end
+                if rev || !oneway(highway)
+                    add_segment(highway, view(highway.nodes, j:-1:firstNode))
+                end
+				firstNode = j
+            end
+        end
+    end
+    return e, class, weight_vals
+end
diff --git a/src/parseMap.jl b/src/parseMap.jl
index 4caf8c3..e77ea08 100644
--- a/src/parseMap.jl
+++ b/src/parseMap.jl
@@ -125,41 +125,32 @@ Internal constructor of `MapData` object
 function MapData(mapdata::OSMData, road_levels::Set{Int}, only_intersections::Bool=true;
 	 trim_to_connected_graph::Bool=false, remove_nodes::AbstractSet{Int}=Set{Int}())
 	#preparing data
-	bounds = mapdata.bounds
-	nodes = OpenStreetMapX.ENU(mapdata.nodes,OpenStreetMapX.center(bounds))
-	highways = OpenStreetMapX.filter_highways(OpenStreetMapX.extract_highways(mapdata.ways))
-	roadways = OpenStreetMapX.filter_roadways(highways, levels= road_levels)
-	if length(remove_nodes) > 0
-		delete!.(Ref(nodes), remove_nodes);
-		delcount = 0
-		for rno in length(roadways):-1:1
-			rr = roadways[rno]
-			for i in length(rr.nodes):-1:1
-				if rr.nodes[i] in remove_nodes
-					deleteat!(rr.nodes,i)
-					delcount += 1
-				end
+	roadways = filter(Base.Fix2(valid_roadway, road_levels), mapdata.ways)
+	if !isempty(remove_nodes)
+		for way in roadways
+			filter!(node -> !(node in remove_nodes), way.nodes)
+		end
+		filter!(way -> !isempty(way.nodes), roadways)
+	end
+
+	nodes = Dict{Int,ENU}()
+	lla_ref = OpenStreetMapX.center(mapdata.bounds)
+	for way in roadways # TODO use `intersections` instead of `roadways` if `only_intersections` ?
+		for node in way.nodes
+			if !haskey(nodes, node)
+				nodes[node] = ENU(mapdata.nodes[node], lla_ref)
 			end
-			length(rr.nodes) == 0 && deleteat!(roadways, rno)
 		end
 	end
+
 	intersections = OpenStreetMapX.find_intersections(roadways)
-	segments = OpenStreetMapX.find_segments(nodes,roadways,intersections)
-	#remove unuseful nodes
-	roadways_nodes = unique(vcat(collect(way.nodes for way in roadways)...))
-	nodes = Dict(key => nodes[key] for key in roadways_nodes)
 
 	# e - Edges in graph, stored as a tuple (source,destination)
 	# class - Road class of each edgey
 	if only_intersections && !trim_to_connected_graph
-		vals = Dict((segment.node0,segment.node1) => (segment.distance,segment.parent) for segment in segments)
-		e = collect(keys(vals))
-		vals = collect(values(vals))
-		weight_vals = map(val -> val[1],vals)
-		classified_roadways = OpenStreetMapX.classify_roadways(roadways)
-		class =  [classified_roadways[id] for id in map(val -> val[2],vals)]
+		e, class, weight_vals = get_edges_distances(nodes, roadways, intersections)
 	else
-		e,class = OpenStreetMapX.get_edges(nodes,roadways)
+		e, class = OpenStreetMapX.get_edges(nodes,roadways)
 		weight_vals = OpenStreetMapX.distance(nodes,e)
 	end
 	# (node id) => (graph vertex)
@@ -175,13 +166,18 @@ function MapData(mapdata::OSMData, road_levels::Set{Int}, only_intersections::Bo
 	end
 
 	if trim_to_connected_graph
-		rm_list = Set{Int}()
 		conn_components = sort!(LightGraphs.strongly_connected_components(g),
-        	lt=(x,y)->length(x)<length(y), rev=true)
-		remove_vs = vcat(conn_components[2:end]...)
-		rm_list = getindex.(Ref(n), remove_vs)
-		return  MapData(mapdata, road_levels, only_intersections, remove_nodes=Set{Int}(rm_list))
+			lt=(x,y)->length(x)<length(y), rev=true)
+		remove_nodes = Set{Int}()
+		I = 2:length(conn_components)
+		sizehint!(remove_nodes, sum(i -> length(conn_components[i]), I))
+		for i in I
+			for node in conn_components[i]
+				push!(remove_nodes, n[node])
+			end
+		end
+		return MapData(mapdata, road_levels, only_intersections, remove_nodes=remove_nodes)
 	else
-		return MapData(bounds,nodes,roadways,intersections,g,v,n,e,w,class)
+		return MapData(mapdata.bounds,nodes,roadways,intersections,g,v,n,e,w,class)
 	end
 end