[osrm] 02/14: Imported Upstream version 4.9.0+ds

Sebastiaan Couwenberg sebastic at moszumanska.debian.org
Sat Dec 26 00:51:27 UTC 2015


This is an automated email from the git hooks/post-receive script.

sebastic pushed a commit to branch master
in repository osrm.

commit 2292c01f86a8e5fb856e73122431b2aaad35c62f
Author: Bas Couwenberg <sebastic at xs4all.nl>
Date:   Fri Dec 25 18:44:04 2015 +0100

    Imported Upstream version 4.9.0+ds
---
 .clang-tidy                                        |    4 +
 .gitignore                                         |    2 -
 .travis.yml                                        |  199 ++--
 CMakeLists.txt                                     |  164 ++-
 Doxyfile.in                                        |   43 +
 algorithms/coordinate_calculation.cpp              |   14 +-
 algorithms/coordinate_calculation.hpp              |    8 +-
 algorithms/geospatial_query.hpp                    |  180 ++++
 algorithms/graph_compressor.hpp                    |    2 +-
 algorithms/object_encoder.hpp                      |   12 +-
 algorithms/trip_brute_force.hpp                    |    4 +-
 algorithms/trip_farthest_insertion.hpp             |   11 +-
 appveyor-build.bat                                 |    4 +-
 benchmarks/static_rtree.cpp                        |  142 +--
 build-local.bat                                    |    2 +-
 cmake/FingerPrint-Config.cmake                     |    6 +-
 cmake/GetGitRevisionDescription.cmake              |  123 ---
 cmake/GetGitRevisionDescription.cmake.in           |   38 -
 cmake/check_luabind.cmake                          |    2 +-
 cmake/pkgconfig.in                                 |    6 +-
 contractor/contractor.hpp                          |  236 +++--
 contractor/contractor_options.cpp                  |   46 +-
 contractor/contractor_options.hpp                  |   18 +-
 contractor/processing_chain.cpp                    |  474 ++++-----
 contractor/processing_chain.hpp                    |   31 +-
 data_structures/concurrent_queue.hpp               |   85 --
 data_structures/deallocating_vector.hpp            |   14 +
 data_structures/edge_based_node.hpp                |   12 +-
 data_structures/external_memory_node.cpp           |    6 +-
 data_structures/external_memory_node.hpp           |    2 +-
 data_structures/hidden_markov_model.hpp            |    2 +-
 data_structures/import_edge.cpp                    |    5 +-
 data_structures/import_edge.hpp                    |   22 +
 data_structures/node_based_graph.hpp               |   10 +-
 data_structures/node_id.hpp                        |    8 +-
 data_structures/phantom_node.cpp                   |    9 +-
 data_structures/phantom_node.hpp                   |   32 +-
 data_structures/query_node.hpp                     |   12 +-
 data_structures/range_table.hpp                    |    5 +-
 data_structures/raster_source.cpp                  |    4 +-
 data_structures/rectangle.hpp                      |   48 +-
 data_structures/restriction.hpp                    |    4 +-
 data_structures/restriction_map.cpp                |    9 +-
 data_structures/route_parameters.cpp               |   48 +-
 data_structures/segment_information.hpp            |    7 +-
 data_structures/static_rtree.hpp                   |  767 ++------------
 datastore.cpp                                      |  900 ++++++++---------
 descriptors/description_factory.cpp                |   14 +-
 descriptors/json_descriptor.hpp                    |   43 +-
 extract.cpp                                        |   90 +-
 .../edge_based_graph_factory.cpp                   |  145 ++-
 .../edge_based_graph_factory.hpp                   |   34 +-
 extractor/extraction_containers.cpp                |  154 ++-
 extractor/extraction_containers.hpp                |    8 +-
 extractor/extraction_helper_functions.hpp          |   13 +-
 extractor/extraction_way.hpp                       |    2 +
 extractor/extractor.cpp                            |  395 +++++++-
 extractor/extractor.hpp                            |   33 +-
 extractor/extractor_callbacks.cpp                  |   81 +-
 extractor/extractor_callbacks.hpp                  |   14 +-
 extractor/extractor_options.cpp                    |   58 +-
 extractor/extractor_options.hpp                    |   14 +
 extractor/first_and_last_segment_of_way.hpp        |   43 +-
 extractor/internal_extractor_edge.hpp              |   64 +-
 extractor/lat                                      |    0
 extractor/restriction_parser.cpp                   |   23 +-
 extractor/restriction_parser.hpp                   |   13 +-
 extractor/scripting_environment.cpp                |    1 +
 extractor/source_coordinate.lat                    |    0
 {contractor => extractor}/speed_profile.hpp        |    0
 features/car/advisory.feature                      |   67 ++
 features/car/mode.feature                          |   40 +
 features/locate/locate.feature                     |  197 ----
 features/options/extract/help.feature              |   12 +-
 features/options/prepare/help.feature              |   15 +-
 features/options/routed/help.feature               |   12 +-
 features/step_definitions/data.rb                  |   10 +-
 features/step_definitions/distance_matrix.rb       |   38 +-
 features/step_definitions/locate.rb                |   51 -
 features/step_definitions/matching.rb              |  211 +---
 features/step_definitions/nearest.rb               |    4 +-
 features/step_definitions/options.rb               |    2 +-
 features/step_definitions/requests.rb              |    2 +-
 features/step_definitions/routability.rb           |    6 +-
 features/step_definitions/routing.rb               |  145 +--
 features/step_definitions/trip.rb                  |    2 +-
 features/support/config.rb                         |    4 +
 features/support/data.rb                           |   16 +-
 features/support/hooks.rb                          |    2 +-
 features/support/http.rb                           |   39 +-
 features/support/locate.rb                         |   12 -
 features/support/match.rb                          |   20 -
 features/support/nearest.rb                        |   12 -
 features/support/route.rb                          |  116 ++-
 features/support/trip.rb                           |   14 -
 features/testbot/64bit.feature                     |   23 +
 features/testbot/alternative.feature               |   38 +
 features/testbot/bearing_param.feature             |   80 +-
 features/testbot/distance_matrix.feature           |   79 ++
 features/testbot/matching_turns.feature            |   43 +-
 features/testbot/post.feature                      |   18 -
 features/testbot/snap.feature                      |   20 +
 features/testbot/status.feature                    |   50 +-
 features/testbot/uturn.feature                     |   24 +
 features/testbot/via.feature                       |   24 -
 include/osrm/libosrm_config.hpp                    |   37 +-
 {library => include/osrm}/osrm.hpp                 |   11 +-
 include/osrm/route_parameters.hpp                  |   10 +
 include/osrm/server_paths.hpp                      |   38 -
 include/osrm/strong_typedef.hpp                    |   68 ++
 library/osrm_impl.cpp                              |   51 +-
 library/osrm_impl.hpp                              |   10 +-
 plugins/distance_table.hpp                         |  164 ++-
 plugins/hello_world.hpp                            |   13 +-
 plugins/locate.hpp                                 |   79 --
 plugins/match.hpp                                  |  170 ++--
 plugins/nearest.hpp                                |   59 +-
 plugins/plugin_base.hpp                            |   90 +-
 plugins/timestamp.hpp                              |    7 +-
 plugins/trip.hpp                                   |  129 ++-
 plugins/viaroute.hpp                               |  413 ++++----
 prepare.cpp                                        |  124 +--
 profiles/bicycle.lua                               |    5 +-
 profiles/car.lua                                   |   42 +-
 routed.cpp                                         |  207 ++--
 routing_algorithms/alternative_path.hpp            |   43 +-
 routing_algorithms/direct_shortest_path.hpp        |  177 +---
 routing_algorithms/many_to_many.hpp                |   70 +-
 routing_algorithms/map_matching.hpp                |   34 +-
 routing_algorithms/routing_base.hpp                |  319 ++++--
 routing_algorithms/shortest_path.hpp               |  655 +++++++-----
 scripts/analyze.sh                                 |   19 +
 scripts/modernize.sh                               |    6 +
 scripts/tidy.sh                                    |    6 +
 .../update_depdendencies.sh                        |    2 +-
 server/api_grammar.hpp                             |   34 +-
 server/data_structures/datafacade_base.hpp         |   31 +-
 server/data_structures/internal_datafacade.hpp     |  183 ++--
 server/data_structures/shared_datafacade.hpp       |   66 +-
 server/http/reply.cpp                              |    6 +-
 server/request_handler.cpp                         |   70 +-
 server/request_parser.cpp                          |    9 +-
 server/server.hpp                                  |    4 +-
 test/.stxxl                                        |    2 +-
 third_party/fast-cpp-csv-parser/LICENSE            |   28 +
 third_party/fast-cpp-csv-parser/README.md          |  252 +++++
 third_party/fast-cpp-csv-parser/csv.h              | 1068 ++++++++++++++++++++
 third_party/variant/.gitignore                     |    4 +-
 third_party/variant/Jamroot                        |    2 +-
 third_party/variant/appveyor.yml                   |   22 +-
 third_party/variant/common.gypi                    |    2 +-
 third_party/variant/scripts/build-appveyor.bat     |   32 +
 third_party/variant/scripts/build-local.bat        |    7 +
 third_party/variant/test/bench_variant.cpp         |  107 +-
 third_party/variant/variant.hpp                    |   68 +-
 tools/components.cpp                               |   23 +-
 tools/io-benchmark.cpp                             |    4 +-
 tools/simpleclient.cpp                             |   15 +-
 tools/springclean.cpp                              |    4 +-
 tools/unlock_all_mutexes.cpp                       |    4 +-
 typedefs.h                                         |   19 +-
 unit_tests/algorithms/douglas_peucker.cpp          |    3 +-
 unit_tests/algorithms/graph_compressor.cpp         |   70 +-
 unit_tests/algorithms/string_util.cpp              |   10 -
 unit_tests/data_structures/binary_heap.cpp         |    3 +
 unit_tests/data_structures/dynamic_graph.cpp       |    5 +-
 unit_tests/data_structures/range_table.cpp         |   19 +-
 unit_tests/data_structures/static_graph.cpp        |    4 +-
 unit_tests/data_structures/static_rtree.cpp        |  318 +++---
 unit_tests/util/bearing.cpp                        |   72 ++
 util/git_sha.hpp => unit_tests/util_tests.cpp      |   11 +-
 util/bearing.cpp                                   |   65 --
 util/bearing.hpp                                   |   84 +-
 util/boost_filesystem_2_fix.hpp                    |  144 ---
 util/cast.hpp                                      |  180 +---
 util/container.hpp                                 |    2 +-
 util/datastore_options.hpp                         |   95 +-
 util/debug_geometry.hpp                            |  198 ++++
 util/fingerprint.cpp                               |    4 +-
 util/fingerprint.hpp                               |    1 -
 util/fingerprint_impl.hpp.in                       |   12 +-
 util/graph_loader.hpp                              |   37 +-
 util/integer_range.hpp                             |   12 +-
 util/iterator_range.hpp                            |   71 --
 util/json_logger.hpp                               |    5 +-
 util/json_renderer.hpp                             |    4 +-
 util/matching_debug_info.hpp                       |   10 +-
 util/routed_options.hpp                            |  141 ++-
 util/string_util.hpp                               |    7 -
 util/{git_sha.cpp.in => version.hpp.in}            |   12 +-
 util/xml_renderer.hpp                              |    2 +-
 191 files changed, 7695 insertions(+), 5502 deletions(-)

diff --git a/.clang-tidy b/.clang-tidy
new file mode 100644
index 0000000..de4303b
--- /dev/null
+++ b/.clang-tidy
@@ -0,0 +1,4 @@
+---
+Checks:          '-clang-analyzer-*,google-*,llvm-*,misc-*,readability-*,-google-build-explicit-make-pair,-google-explicit-constructor,-google-readability-braces-around-statements,-google-readability-casting,-google-readability-namespace-comments,-google-readability-function,-google-readability-todo,-google-runtime-int,-llvm-namespace-comment,-llvm-header-guard,-llvm-twine-local,-misc-argument-comment,-readability-braces-around-statements,-readability-identifier-naming'
+...
+
diff --git a/.gitignore b/.gitignore
index 6264b42..21608e3 100644
--- a/.gitignore
+++ b/.gitignore
@@ -40,8 +40,6 @@ Thumbs.db
 # build related files #
 #######################
 /build/
-/util/fingerprint_impl.hpp
-/util/git_sha.cpp
 /cmake/postinst
 
 # Eclipse related files #
diff --git a/.travis.yml b/.travis.yml
index 830599f..c065949 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -1,63 +1,146 @@
 language: cpp
-compiler:
-  - gcc
-#  - clang
-# Make sure CMake is installed
-install:
- - sudo apt-add-repository -y ppa:ubuntu-toolchain-r/test
- - sudo add-apt-repository -y ppa:boost-latest/ppa
- - sudo apt-get update >/dev/null
- - sudo apt-get -q install libbz2-dev libstxxl-dev libstxxl1 libxml2-dev libzip-dev lua5.1 liblua5.1-0-dev rubygems libtbb-dev
- - sudo apt-get -q install g++-4.8
- - sudo apt-get install libboost1.54-all-dev
- - sudo apt-get install libgdal-dev
- # luabind
- - curl https://gist.githubusercontent.com/DennisOSRM/f2eb7b948e6fe1ae319e/raw/install-luabind.sh | sudo bash
- # osmosis
- - curl -s https://gist.githubusercontent.com/DennisOSRM/803a64a9178ec375069f/raw/ | sudo bash
- # cmake
- - curl -s https://gist.githubusercontent.com/DennisOSRM/5fad9bee5c7f09fd7fc9/raw/ | sudo bash
-before_script:
- - rvm use 1.9.3
- - gem install bundler
- - bundle install
- - mkdir build
- - cd build
- - cmake .. $CMAKEOPTIONS -DBUILD_TOOLS=1
-script:
- - make
- - make tests
- - make benchmarks
- - ./algorithm-tests
- - ./datastructure-tests
- - cd ..
- - cucumber -p verify
-after_script:
-# - cd ..
-# - cucumber -p verify
+sudo: required
+dist: trusty
+
+notifications:
+  email: false
+
 branches:
   only:
     - master
     - develop
-cache:
-- bundler
-- apt
-env:
- - CMAKEOPTIONS="-DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_COMPILER=g++-4.8" OSRM_PORT=5000 OSRM_TIMEOUT=60
- - CMAKEOPTIONS="-DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_COMPILER=g++-4.8" OSRM_PORT=5010 OSRM_TIMEOUT=60
- - CMAKEOPTIONS="-DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=ON -DCMAKE_CXX_COMPILER=g++-4.8" OSRM_PORT=5020 OSRM_TIMEOUT=60
-notifications:
- slack: mapbox:4A6euphDwfxAQnhLurXbu6A1
- irc:
-  channels:
-    - irc.oftc.net#osrm
-  on_success: change
-  on_failure: always
-  use_notice: true
-  skip_join: false
-
-  recipients:
-    - patrick at mapbox.com
-  email:
-    on_success: change
-    on_failure: always
+
+matrix:
+  include:
+
+    # 1/ Linux Clang Builds
+
+    - os: linux
+      compiler: clang
+      addons: &clang38
+        apt:
+          sources: ['llvm-toolchain-precise', 'ubuntu-toolchain-r-test']
+          packages: ['clang-3.8', 'libbz2-dev', 'libstxxl-dev', 'libstxxl1', 'libxml2-dev', 'libzip-dev', 'lua5.1', 'liblua5.1-0-dev', 'rubygems-integration', 'libtbb-dev', 'libgdal-dev', 'libluabind-dev', 'libboost-all-dev']
+      env: COMPILER='clang++-3.8' BUILD_TYPE='Release'
+
+    - os: linux
+      compiler: clang
+      addons: &clang38
+        apt:
+          sources: ['llvm-toolchain-precise', 'ubuntu-toolchain-r-test']
+          packages: ['clang-3.8', 'libbz2-dev', 'libstxxl-dev', 'libstxxl1', 'libxml2-dev', 'libzip-dev', 'lua5.1', 'liblua5.1-0-dev', 'rubygems-integration', 'libtbb-dev', 'libgdal-dev', 'libluabind-dev', 'libboost-all-dev']
+      env: COMPILER='clang++-3.8' BUILD_TYPE='Release' BUILD_SHARED_LIBS=ON
+
+    - os: linux
+      compiler: clang
+      addons: *clang38
+      env: COMPILER='clang++-3.8' BUILD_TYPE='Debug'
+
+
+    # 2/ Linux GCC Builds
+    - os: linux
+      compiler: gcc
+      addons: &gcc48
+        apt:
+          sources: ['ubuntu-toolchain-r-test']
+          packages: ['g++-4.8', 'libbz2-dev', 'libstxxl-dev', 'libstxxl1', 'libxml2-dev', 'libzip-dev', 'lua5.1', 'liblua5.1-0-dev', 'rubygems-integration', 'libtbb-dev', 'libgdal-dev', 'libluabind-dev', 'libboost-all-dev']
+      env: COMPILER='g++-4.8' BUILD_TYPE='Release'
+
+    - os: linux
+      compiler: gcc
+      addons: *gcc48
+      env: COMPILER='g++-4.8' BUILD_TYPE='Debug'
+
+
+    - os: linux
+      compiler: gcc
+      addons: &gcc5
+        apt:
+          sources: ['ubuntu-toolchain-r-test']
+          packages: ['g++-5', 'libbz2-dev', 'libstxxl-dev', 'libstxxl1', 'libxml2-dev', 'libzip-dev', 'lua5.1', 'liblua5.1-0-dev', 'rubygems-integration', 'libtbb-dev', 'libgdal-dev', 'libluabind-dev', 'libboost-all-dev']
+      env: COMPILER='g++-5' BUILD_TYPE='Release'
+
+    - os: linux
+      compiler: gcc
+      addons: &gcc5
+        apt:
+          sources: ['ubuntu-toolchain-r-test']
+          packages: ['g++-5', 'libbz2-dev', 'libstxxl-dev', 'libstxxl1', 'libxml2-dev', 'libzip-dev', 'lua5.1', 'liblua5.1-0-dev', 'rubygems-integration', 'libtbb-dev', 'libgdal-dev', 'libluabind-dev', 'libboost-all-dev']
+      env: COMPILER='g++-5' BUILD_TYPE='Release' BUILD_SHARED_LIBS=ON
+
+    - os: linux
+      compiler: gcc
+      addons: *gcc5
+      env: COMPILER='g++-5' BUILD_TYPE='Debug'
+
+
+    # Disabled until tests all pass on OSX:
+    #
+    # 3/ OSX Clang Builds
+    #- os: osx
+    #  osx_image: xcode6.4
+    #  compiler: clang
+    #  env: COMPILER='clang++' BUILD_TYPE='Debug'
+
+    #- os: osx
+    #  osx_image: xcode6.4
+    #  compiler: clang
+    #  env: COMPILER='clang++' BUILD_TYPE='Release'
+
+    #- os: osx
+    #  osx_image: xcode6.4
+    #  compiler: clang
+    #  env: COMPILER='clang++' BUILD_TYPE='Release' BUILD_SHARED_LIBS=ON
+
+    #- os: osx
+    #  osx_image: xcode7
+    #  compiler: clang
+    #  env: COMPILER='clang++' BUILD_TYPE='Debug'
+
+    #- os: osx
+    #  osx_image: xcode7
+    #  compiler: clang
+    #  env: COMPILER='clang++' BUILD_TYPE='Release'
+
+    #- os: osx
+    #  osx_image: xcode7
+    #  compiler: clang
+    #  env: COMPILER='clang++' BUILD_TYPE='Release' BUILD_SHARED_LIBS=ON
+
+
+install:
+  - DEPS_DIR="${TRAVIS_BUILD_DIR}/deps"
+  - mkdir -p ${DEPS_DIR} && cd ${DEPS_DIR}
+  - |
+    if [[ "${TRAVIS_OS_NAME}" == "linux" ]]; then
+      CMAKE_URL="http://www.cmake.org/files/v3.3/cmake-3.3.2-Linux-x86_64.tar.gz"
+      mkdir cmake && travis_retry wget --quiet -O - ${CMAKE_URL} | tar --strip-components=1 -xz -C cmake
+      export PATH=${DEPS_DIR}/cmake/bin:${PATH}
+
+      OSMOSIS_URL="http://bretth.dev.openstreetmap.org/osmosis-build/osmosis-latest.tgz"
+      mkdir osmosis && travis_retry wget --quiet -O - ${OSMOSIS_URL} | tar -xz -C osmosis
+      export PATH=${DEPS_DIR}/osmosis/bin:${PATH}
+
+    elif [[ "${TRAVIS_OS_NAME}" == "osx" ]]; then
+      brew install cmake boost libzip libstxxl libxml2 lua51 luabind tbb GDAL osmosis
+    fi
+
+before_script:
+  - cd ${TRAVIS_BUILD_DIR}
+  - rvm use 1.9.3
+  - gem install bundler
+  - bundle install
+  - mkdir build && cd build
+  - export CXX=${COMPILER}
+  - export OSRM_PORT=5000 OSRM_TIMEOUT=60
+  - cmake .. -DCMAKE_BUILD_TYPE=${BUILD_TYPE} -DBUILD_SHARED_LIBS=${BUILD_SHARED_LIBS:-OFF} -DBUILD_TOOLS=1
+
+script:
+  - make --jobs=2
+  - make tests --jobs=2
+  - make benchmarks
+  - ./algorithm-tests
+  - ./datastructure-tests
+  - ./util-tests
+  - cd ..
+  - cucumber -p verify
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e0b8f10..b8fecc5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,13 +7,15 @@ This process created the file `CMakeCache.txt' and the directory `CMakeFiles'. P
 endif()
 
 project(OSRM C CXX)
+set(OSRM_VERSION_MAJOR 4)
+set(OSRM_VERSION_MINOR 9)
+set(OSRM_VERSION_PATCH 0)
+
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 include(CheckCXXCompilerFlag)
 include(FindPackageHandleStandardArgs)
 
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
-include(GetGitRevisionDescription)
-git_describe(GIT_DESCRIPTION)
 
 set(bitness 32)
 if(CMAKE_SIZEOF_VOID_P EQUAL 8)
@@ -28,29 +30,32 @@ if(WIN32 AND MSVC_VERSION LESS 1800)
 endif()
 
 option(ENABLE_JSON_LOGGING "Adds additional JSON debug logging to the response" OFF)
-option(WITH_TOOLS "Build OSRM tools" OFF)
+option(DEBUG_GEOMETRY "Enables an option to dump GeoJSON of the final routing graph" OFF)
 option(BUILD_TOOLS "Build OSRM tools" OFF)
 
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+include_directories(${CMAKE_CURRENT_BINARY_DIR})
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/)
-include_directories(${CMAKE_CURRENT_SOURCE_DIR}/third_party/)
-include_directories(${CMAKE_CURRENT_SOURCE_DIR}/third_party/libosmium/include/)
+include_directories(SYSTEM ${CMAKE_CURRENT_SOURCE_DIR}/third_party/)
+include_directories(SYSTEM ${CMAKE_CURRENT_SOURCE_DIR}/third_party/libosmium/include/)
 
-add_custom_target(FingerPrintConfigure ALL
-  ${CMAKE_COMMAND} -DSOURCE_DIR=${CMAKE_SOURCE_DIR}
-    -P ${CMAKE_CURRENT_SOURCE_DIR}/cmake/FingerPrint-Config.cmake
+add_custom_target(FingerPrintConfigure ALL ${CMAKE_COMMAND}
+  "-DOUTPUT_DIR=${CMAKE_CURRENT_BINARY_DIR}"
+  "-DSOURCE_DIR=${CMAKE_CURRENT_SOURCE_DIR}"
+  -P "${CMAKE_CURRENT_SOURCE_DIR}/cmake/FingerPrint-Config.cmake"
   COMMENT "Configuring revision fingerprint"
   VERBATIM)
 
-add_custom_target(tests DEPENDS datastructure-tests algorithm-tests)
+add_custom_target(tests DEPENDS datastructure-tests algorithm-tests util-tests)
 add_custom_target(benchmarks DEPENDS rtree-bench)
 
 set(BOOST_COMPONENTS date_time filesystem iostreams program_options regex system thread unit_test_framework)
 
 configure_file(
-  ${CMAKE_CURRENT_SOURCE_DIR}/util/git_sha.cpp.in
-  ${CMAKE_CURRENT_SOURCE_DIR}/util/git_sha.cpp
+  ${CMAKE_CURRENT_SOURCE_DIR}/util/version.hpp.in
+  ${CMAKE_CURRENT_BINARY_DIR}/util/version.hpp
 )
-file(GLOB ExtractorGlob extractor/*.cpp)
+file(GLOB ExtractorGlob extractor/*.cpp data_structures/hilbert_value.cpp)
 file(GLOB ImporterGlob data_structures/import_edge.cpp data_structures/external_memory_node.cpp data_structures/raster_source.cpp)
 add_library(IMPORT OBJECT ${ImporterGlob})
 add_library(LOGGER OBJECT util/simple_logger.cpp)
@@ -61,7 +66,7 @@ add_library(MERCATOR OBJECT util/mercator.cpp)
 add_library(ANGLE OBJECT util/compute_angle.cpp)
 
 set(ExtractorSources extract.cpp ${ExtractorGlob})
-add_executable(osrm-extract ${ExtractorSources} $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:FINGERPRINT> $<TARGET_OBJECTS:GITDESCRIPTION> $<TARGET_OBJECTS:IMPORT> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:MERCATOR>)
+add_executable(osrm-extract ${ExtractorSources} $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:FINGERPRINT> $<TARGET_OBJECTS:IMPORT> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:MERCATOR> $<TARGET_OBJECTS:COMPRESSEDEDGE> $<TARGET_OBJECTS:GRAPHCOMPRESSOR> $<TARGET_OBJECTS:RESTRICTION> $<TARGET_OBJECTS:ANGLE>)
 
 add_library(RESTRICTION OBJECT data_structures/restriction_map.cpp)
 add_library(COMPRESSEDEDGE OBJECT data_structures/compressed_edge_container.cpp)
@@ -69,7 +74,7 @@ add_library(GRAPHCOMPRESSOR OBJECT algorithms/graph_compressor.cpp)
 
 file(GLOB PrepareGlob contractor/*.cpp data_structures/hilbert_value.cpp {RestrictionMapGlob})
 set(PrepareSources prepare.cpp ${PrepareGlob})
-add_executable(osrm-prepare ${PrepareSources} $<TARGET_OBJECTS:ANGLE> $<TARGET_OBJECTS:FINGERPRINT> $<TARGET_OBJECTS:GITDESCRIPTION> $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:IMPORT> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:RESTRICTION> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:MERCATOR> $<TARGET_OBJECTS:COMPRESSEDEDGE> $<TARGET_OBJECTS:GRAPHCOMPRESSOR>)
+add_executable(osrm-prepare ${PrepareSources} $<TARGET_OBJECTS:ANGLE> $<TARGET_OBJECTS:FINGERPRINT> $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:IMPORT> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:RESTRICTION> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:MERCATOR> $<TARGET_OBJECTS:COMPRESSEDEDGE> $<TARGET_OBJECTS:GRAPHCOMPRESSOR>)
 
 file(GLOB ServerGlob server/*.cpp)
 file(GLOB DescriptorGlob descriptors/*.cpp)
@@ -80,6 +85,7 @@ file(GLOB HttpGlob server/http/*.cpp)
 file(GLOB LibOSRMGlob library/*.cpp)
 file(GLOB DataStructureTestsGlob unit_tests/data_structures/*.cpp data_structures/hilbert_value.cpp)
 file(GLOB AlgorithmTestsGlob unit_tests/algorithms/*.cpp algorithms/graph_compressor.cpp)
+file(GLOB UtilTestsGlob unit_tests/util/*.cpp)
 
 set(
   OSRMSources
@@ -91,8 +97,7 @@ set(
 )
 
 add_library(COORDINATE OBJECT ${CoordinateGlob})
-add_library(GITDESCRIPTION OBJECT util/git_sha.cpp)
-add_library(OSRM ${OSRMSources} $<TARGET_OBJECTS:ANGLE> $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:GITDESCRIPTION> $<TARGET_OBJECTS:FINGERPRINT> $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:RESTRICTION> $<TARGET_OBJECTS:PHANTOMNODE> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:MERCATOR> $<TARGET_OBJECTS:IMPORT>)
+add_library(OSRM ${OSRMSources} $<TARGET_OBJECTS:ANGLE> $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:FINGERPRINT> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:RESTRICTION> $<TARGET_OBJECTS:PHANTOMNODE> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:MERCATOR> $<TARGET_OBJECTS:IMPORT>)
 
 add_library(FINGERPRINT OBJECT util/fingerprint.cpp)
 add_dependencies(FINGERPRINT FingerPrintConfigure)
@@ -100,11 +105,12 @@ add_dependencies(OSRM FingerPrintConfigure)
 set_target_properties(FINGERPRINT PROPERTIES LINKER_LANGUAGE CXX)
 
 add_executable(osrm-routed routed.cpp ${ServerGlob} $<TARGET_OBJECTS:EXCEPTION>)
-add_executable(osrm-datastore datastore.cpp $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:FINGERPRINT> $<TARGET_OBJECTS:GITDESCRIPTION> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:MERCATOR>)
+add_executable(osrm-datastore datastore.cpp $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:FINGERPRINT> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:MERCATOR>)
 
 # Unit tests
 add_executable(datastructure-tests EXCLUDE_FROM_ALL unit_tests/datastructure_tests.cpp ${DataStructureTestsGlob} $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:PHANTOMNODE> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:MERCATOR> $<TARGET_OBJECTS:COMPRESSEDEDGE> $<TARGET_OBJECTS:GRAPHCOMPRESSOR> $<TARGET_OBJECTS:RESTRICTION> $<TARGET_OBJECTS:RASTERSOURCE>)
 add_executable(algorithm-tests EXCLUDE_FROM_ALL unit_tests/algorithm_tests.cpp ${AlgorithmTestsGlob} $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:PHANTOMNODE> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:RESTRICTION> $<TARGET_OBJECTS:COMPRESSEDEDGE>)
+add_executable(util-tests EXCLUDE_FROM_ALL unit_tests/util_tests.cpp ${UtilTestsGlob})
 
 # Benchmarks
 add_executable(rtree-bench EXCLUDE_FROM_ALL benchmarks/static_rtree.cpp $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:PHANTOMNODE> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:MERCATOR>)
@@ -116,18 +122,29 @@ endif()
 if(CMAKE_BUILD_TYPE MATCHES Debug)
   message(STATUS "Configuring OSRM in debug mode")
   if(NOT ${CMAKE_CXX_COMPILER_ID} STREQUAL "MSVC")
-    message(STATUS "adding profiling flags")
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fprofile-arcs -ftest-coverage -fno-inline")
-    set(CMAKE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fprofile-arcs -ftest-coverage -fno-inline")
+
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fno-inline -fno-omit-frame-pointer")
+
+  if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Og -ggdb")
+  endif()
+
   endif()
 endif()
 if(CMAKE_BUILD_TYPE MATCHES Release)
   message(STATUS "Configuring OSRM in release mode")
   # Check if LTO is available
-  set(LTO_FLAGS "")
   check_cxx_compiler_flag("-flto" LTO_AVAILABLE)
   if(LTO_AVAILABLE)
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -flto")
+    set(OLD_CXX_FLAGS ${CMAKE_CXX_FLAGS})
+    # GCC in addition allows parallelizing LTO
+    if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
+      include(ProcessorCount)
+      ProcessorCount(NPROC)
+      set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -flto=${NPROC}")
+    else()
+      set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -flto")
+    endif()
     set(CHECK_LTO_SRC "int main(){return 0;}")
     check_cxx_source_compiles("${CHECK_LTO_SRC}" LTO_WORKS)
     if(LTO_WORKS)
@@ -144,6 +161,11 @@ if(CMAKE_BUILD_TYPE MATCHES Release)
       set(CMAKE_AR     "/usr/bin/gcc-ar")
       set(CMAKE_RANLIB "/usr/bin/gcc-ranlib")
     endif()
+
+    if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU" AND "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "4.9.0")
+      message(STATUS "Disabling LTO on GCC < 4.9.0 since it is broken, see: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=57038")
+      set(CMAKE_CXX_FLAGS "${OLD_CXX_FLAGS}")
+    endif()
   endif()
 endif()
 
@@ -153,9 +175,7 @@ endif()
 
 # Configuring compilers
 if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
-  # using Clang
-  #  -Weverything -Wno-c++98-compat -Wno-shadow -Wno-exit-time-destructors
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wunreachable-code -pedantic -fPIC")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -pedantic -Wuninitialized -Wunreachable-code -Wstrict-overflow=2 -D_FORTIFY_SOURCE=2 -fPIC")
 elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU")
   set(COLOR_FLAG "-fdiagnostics-color=auto")
   check_cxx_compiler_flag("-fdiagnostics-color=auto" HAS_COLOR_FLAG)
@@ -163,7 +183,7 @@ elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU")
     set(COLOR_FLAG "")
   endif()
   # using GCC
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -pedantic -fPIC ${COLOR_FLAG}")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -pedantic -Wuninitialized -Wunreachable-code -Wstrict-overflow=1 -D_FORTIFY_SOURCE=2 ${COLOR_FLAG} -fPIC")
   if(WIN32) # using mingw
     add_definitions(-D_USE_MATH_DEFINES) # define M_PI, M_1_PI etc.
     add_definitions(-DWIN32)
@@ -184,6 +204,26 @@ elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "MSVC")
   target_link_libraries(osrm-extract wsock32 ws2_32)
 endif()
 
+# Configuring linker
+execute_process(COMMAND ${CMAKE_CXX_COMPILER} "-Wl,--version" ERROR_QUIET OUTPUT_VARIABLE LINKER_VERSION)
+# For ld.gold and ld.bfs (the GNU linkers) we optimize hard
+if("${LINKER_VERSION}" MATCHES "GNU gold" OR "${LINKER_VERSION}" MATCHES "GNU ld")
+  message(STATUS "Setting linker optimizations")
+  if(NOT ${CMAKE_CXX_COMPILER_ID} STREQUAL "MSVC")
+    # Tell compiler to put every function in separate section, linker can then match sections and functions
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ffunction-sections -fdata-sections")
+    # Tell linker to do dead code and data eminination during link time discarding sections
+    set(LINKER_FLAGS "${LINKER_FLAGS} -Wl,--gc-sections")
+  endif()
+  # Default linker optimization flags
+  set(LINKER_FLAGS "${LINKER_FLAGS} -Wl,-O1 -Wl,--hash-style=gnu -Wl,--sort-common")
+else()
+  message(STATUS "Using unknown linker, not setting linker optimizations")
+endif ()
+set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${LINKER_FLAGS}")
+set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} ${LINKER_FLAGS}")
+set(CMAKE_MODULE_LINKER_FLAGS "${CMAKE_MODULE_LINKER_FLAGS} ${LINKER_FLAGS}")
+
 # Activate C++11
 if(NOT ${CMAKE_CXX_COMPILER_ID} STREQUAL "MSVC")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 ")
@@ -211,12 +251,11 @@ if(UNIX AND NOT APPLE)
 endif()
 
 #Check Boost
-set(BOOST_MIN_VERSION "1.49.0")
-find_package(Boost ${BOOST_MIN_VERSION} COMPONENTS ${BOOST_COMPONENTS} REQUIRED)
+find_package(Boost 1.49.0 COMPONENTS ${BOOST_COMPONENTS} REQUIRED)
 if(NOT Boost_FOUND)
   message(FATAL_ERROR "Fatal error: Boost (version >= 1.49.0) required.\n")
 endif()
-include_directories(${Boost_INCLUDE_DIRS})
+include_directories(SYSTEM ${Boost_INCLUDE_DIRS})
 
 target_link_libraries(OSRM ${Boost_LIBRARIES})
 target_link_libraries(osrm-extract ${Boost_LIBRARIES})
@@ -225,6 +264,7 @@ target_link_libraries(osrm-routed ${Boost_LIBRARIES} ${OPTIONAL_SOCKET_LIBS} OSR
 target_link_libraries(osrm-datastore ${Boost_LIBRARIES})
 target_link_libraries(datastructure-tests ${Boost_LIBRARIES})
 target_link_libraries(algorithm-tests ${Boost_LIBRARIES} ${OPTIONAL_SOCKET_LIBS} OSRM)
+target_link_libraries(util-tests ${Boost_LIBRARIES})
 target_link_libraries(rtree-bench ${Boost_LIBRARIES})
 
 find_package(Threads REQUIRED)
@@ -247,12 +287,12 @@ target_link_libraries(osrm-routed ${TBB_LIBRARIES})
 target_link_libraries(datastructure-tests ${TBB_LIBRARIES})
 target_link_libraries(algorithm-tests ${TBB_LIBRARIES})
 target_link_libraries(rtree-bench ${TBB_LIBRARIES})
-include_directories(${TBB_INCLUDE_DIR})
+include_directories(SYSTEM ${TBB_INCLUDE_DIR})
 
 find_package( Luabind REQUIRED )
 include(check_luabind)
 
-include_directories(${LUABIND_INCLUDE_DIR})
+include_directories(SYSTEM ${LUABIND_INCLUDE_DIR})
 target_link_libraries(osrm-extract ${LUABIND_LIBRARY})
 target_link_libraries(osrm-prepare ${LUABIND_LIBRARY})
 
@@ -263,17 +303,18 @@ else()
   target_link_libraries(osrm-extract ${LUA_LIBRARY})
   target_link_libraries(osrm-prepare ${LUA_LIBRARY})
 endif()
-include_directories(${LUA_INCLUDE_DIR})
+include_directories(SYSTEM ${LUA_INCLUDE_DIR})
 
 find_package(EXPAT REQUIRED)
-include_directories(${EXPAT_INCLUDE_DIRS})
+include_directories(SYSTEM ${EXPAT_INCLUDE_DIRS})
 target_link_libraries(osrm-extract ${EXPAT_LIBRARIES})
 
 find_package(STXXL REQUIRED)
-include_directories(${STXXL_INCLUDE_DIR})
+include_directories(SYSTEM ${STXXL_INCLUDE_DIR})
 target_link_libraries(OSRM ${STXXL_LIBRARY})
 target_link_libraries(osrm-extract ${STXXL_LIBRARY})
 target_link_libraries(osrm-prepare ${STXXL_LIBRARY})
+target_link_libraries(datastructure-tests ${STXXL_LIBRARY})
 
 set(OpenMP_FIND_QUIETLY ON)
 find_package(OpenMP)
@@ -283,11 +324,11 @@ if(OPENMP_FOUND)
 endif()
 
 find_package(BZip2 REQUIRED)
-include_directories(${BZIP_INCLUDE_DIRS})
+include_directories(SYSTEM ${BZIP_INCLUDE_DIRS})
 target_link_libraries(osrm-extract ${BZIP2_LIBRARIES})
 
 find_package(ZLIB REQUIRED)
-include_directories(${ZLIB_INCLUDE_DIRS})
+include_directories(SYSTEM ${ZLIB_INCLUDE_DIRS})
 target_link_libraries(osrm-extract ${ZLIB_LIBRARY})
 target_link_libraries(osrm-routed ${ZLIB_LIBRARY})
 
@@ -296,16 +337,19 @@ if (ENABLE_JSON_LOGGING)
   add_definitions(-DENABLE_JSON_LOGGING)
 endif()
 
-if(WITH_TOOLS OR BUILD_TOOLS)
+if (DEBUG_GEOMETRY)
+  message(STATUS "Enabling final edge weight GeoJSON output option")
+  add_definitions(-DDEBUG_GEOMETRY)
+endif()
+
+if(BUILD_TOOLS)
   message(STATUS "Activating OSRM internal tools")
   find_package(GDAL)
   if(GDAL_FOUND)
     add_executable(osrm-components tools/components.cpp $<TARGET_OBJECTS:FINGERPRINT> $<TARGET_OBJECTS:IMPORT> $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:RESTRICTION> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:MERCATOR>)
     target_link_libraries(osrm-components ${TBB_LIBRARIES})
-    include_directories(${GDAL_INCLUDE_DIR})
-    target_link_libraries(
-      osrm-components
-      ${GDAL_LIBRARIES} ${Boost_LIBRARIES})
+    include_directories(SYSTEM ${GDAL_INCLUDE_DIR})
+    target_link_libraries(osrm-components ${GDAL_LIBRARIES} ${Boost_LIBRARIES})
     install(TARGETS osrm-components DESTINATION bin)
   else()
     message(FATAL_ERROR "libgdal and/or development headers not found")
@@ -313,16 +357,16 @@ if(WITH_TOOLS OR BUILD_TOOLS)
   add_executable(osrm-cli tools/simpleclient.cpp $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:COORDINATE>)
   target_link_libraries(osrm-cli ${Boost_LIBRARIES} ${OPTIONAL_SOCKET_LIBS} OSRM)
   target_link_libraries(osrm-cli ${TBB_LIBRARIES})
-  add_executable(osrm-io-benchmark tools/io-benchmark.cpp $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:GITDESCRIPTION> $<TARGET_OBJECTS:LOGGER>)
+  add_executable(osrm-io-benchmark tools/io-benchmark.cpp $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:LOGGER>)
   target_link_libraries(osrm-io-benchmark ${Boost_LIBRARIES})
-  add_executable(osrm-unlock-all tools/unlock_all_mutexes.cpp $<TARGET_OBJECTS:GITDESCRIPTION> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:EXCEPTION>)
+  add_executable(osrm-unlock-all tools/unlock_all_mutexes.cpp $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:EXCEPTION>)
   target_link_libraries(osrm-unlock-all ${Boost_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT})
   if(UNIX AND NOT APPLE)
     target_link_libraries(osrm-unlock-all rt)
   endif()
   add_executable(osrm-check-hsgr tools/check-hsgr.cpp $<TARGET_OBJECTS:FINGERPRINT> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:IMPORT>)
   target_link_libraries(osrm-check-hsgr ${Boost_LIBRARIES} ${TBB_LIBRARIES})
-  add_executable(osrm-springclean tools/springclean.cpp $<TARGET_OBJECTS:FINGERPRINT> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:GITDESCRIPTION> $<TARGET_OBJECTS:EXCEPTION>)
+  add_executable(osrm-springclean tools/springclean.cpp $<TARGET_OBJECTS:FINGERPRINT> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:EXCEPTION>)
   target_link_libraries(osrm-springclean ${Boost_LIBRARIES})
 
   install(TARGETS osrm-cli DESTINATION bin)
@@ -332,7 +376,7 @@ if(WITH_TOOLS OR BUILD_TOOLS)
   install(TARGETS osrm-springclean DESTINATION bin)
 endif()
 
-file(GLOB InstallGlob include/osrm/*.hpp library/osrm.hpp)
+file(GLOB InstallGlob include/osrm/*.hpp)
 file(GLOB VariantGlob third_party/variant/*.hpp)
 
 # Add RPATH info to executables so that when they are run after being installed
@@ -350,6 +394,7 @@ install(TARGETS osrm-prepare DESTINATION bin)
 install(TARGETS osrm-datastore DESTINATION bin)
 install(TARGETS osrm-routed DESTINATION bin)
 install(TARGETS OSRM DESTINATION lib)
+
 list(GET Boost_LIBRARIES 1 BOOST_LIBRARY_FIRST)
 get_filename_component(BOOST_LIBRARY_LISTING "${BOOST_LIBRARY_FIRST}" PATH)
 set(BOOST_LIBRARY_LISTING "-L${BOOST_LIBRARY_LISTING}")
@@ -358,6 +403,14 @@ foreach(lib ${Boost_LIBRARIES})
   string(REPLACE "lib" "" BOOST_LIBRARY_NAME ${BOOST_LIBRARY_NAME})
   set(BOOST_LIBRARY_LISTING "${BOOST_LIBRARY_LISTING} -l${BOOST_LIBRARY_NAME}")
 endforeach()
+list(GET TBB_LIBRARIES 1 TBB_LIBRARY_FIRST)
+get_filename_component(TBB_LIBRARY_LISTING "${TBB_LIBRARY_FIRST}" PATH)
+set(TBB_LIBRARY_LISTING "-L${TBB_LIBRARY_LISTING}")
+foreach(lib ${TBB_LIBRARIES})
+  get_filename_component(TBB_LIBRARY_NAME "${lib}" NAME_WE)
+  string(REPLACE "lib" "" TBB_LIBRARY_NAME ${TBB_LIBRARY_NAME})
+  set(TBB_LIBRARY_LISTING "${TBB_LIBRARY_LISTING} -l${TBB_LIBRARY_NAME}")
+endforeach()
 
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/cmake/pkgconfig.in libosrm.pc @ONLY)
 install(FILES ${PROJECT_BINARY_DIR}/libosrm.pc DESTINATION lib/pkgconfig)
@@ -366,3 +419,24 @@ if(BUILD_DEBIAN_PACKAGE)
   include(CPackDebianConfig)
   include(CPack)
 endif()
+
+# add a target to generate API documentation with Doxygen
+find_package(Doxygen)
+if(DOXYGEN_FOUND)
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile @ONLY)
+add_custom_target(doc
+${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile
+WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}
+COMMENT "Generating API documentation with Doxygen" VERBATIM
+)
+endif()
+
+# prefix compilation with ccache by default if available and on clang or gcc
+if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang" OR ${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU")
+  find_program(CCACHE_FOUND ccache)
+  if(CCACHE_FOUND)
+    set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache)
+    set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache)
+    set(ENV{CCACHE_CPP2} "true")
+  endif()
+endif()
diff --git a/Doxyfile.in b/Doxyfile.in
new file mode 100644
index 0000000..5842f6b
--- /dev/null
+++ b/Doxyfile.in
@@ -0,0 +1,43 @@
+PROJECT_NAME           = "Project OSRM"
+PROJECT_BRIEF          = "Open Source Routing Machine"
+BUILTIN_STL_SUPPORT    = YES
+
+EXTRACT_ALL            = YES
+EXTRACT_PRIVATE        = YES
+EXTRACT_PACKAGE        = YES
+EXTRACT_STATIC         = YES
+EXTRACT_LOCAL_CLASSES  = YES
+EXTRACT_ANON_NSPACES   = YES
+
+QUIET                  = YES
+
+INPUT                  = @CMAKE_CURRENT_SOURCE_DIR@
+USE_MDFILE_AS_MAINPAGE = @CMAKE_CURRENT_SOURCE_DIR@/README.md
+FILE_PATTERNS          = *.h *.hpp *.c *.cc *.cpp *.md
+RECURSIVE              = YES
+
+EXCLUDE                = @CMAKE_CURRENT_SOURCE_DIR@/third_party \
+                         @CMAKE_CURRENT_SOURCE_DIR@/build \
+                         @CMAKE_CURRENT_SOURCE_DIR@/unit_tests \
+			 @CMAKE_CURRENT_SOURCE_DIR@/benchmarks \
+                         @CMAKE_CURRENT_SOURCE_DIR@/features
+
+SOURCE_BROWSER         = YES
+
+CLANG_ASSISTED_PARSING = NO
+
+HTML_COLORSTYLE_HUE    = 217
+HTML_COLORSTYLE_SAT    = 71
+HTML_COLORSTYLE_GAMMA  = 50
+
+GENERATE_TREEVIEW      = YES
+
+HAVE_DOT               = @DOXYGEN_DOT_FOUND@
+CALL_GRAPH             = YES
+CALLER_GRAPH           = YES
+
+DOT_IMAGE_FORMAT       = svg
+INTERACTIVE_SVG        = YES
+DOT_GRAPH_MAX_NODES    = 500
+DOT_TRANSPARENT        = YES
+DOT_MULTI_TARGETS      = YES
diff --git a/algorithms/coordinate_calculation.cpp b/algorithms/coordinate_calculation.cpp
index 1400b29..ff7626e 100644
--- a/algorithms/coordinate_calculation.cpp
+++ b/algorithms/coordinate_calculation.cpp
@@ -49,7 +49,7 @@ constexpr static const float earth_radius = 6372797.560856f;
 namespace coordinate_calculation
 {
 
-double great_circle_distance(const int lat1,
+double haversine_distance(const int lat1,
                                                      const int lon1,
                                                      const int lat2,
                                                      const int lon2)
@@ -77,21 +77,21 @@ double great_circle_distance(const int lat1,
     return earth_radius * cHarv;
 }
 
-double great_circle_distance(const FixedPointCoordinate &coordinate_1,
+double haversine_distance(const FixedPointCoordinate &coordinate_1,
                                                      const FixedPointCoordinate &coordinate_2)
 {
-    return great_circle_distance(coordinate_1.lat, coordinate_1.lon, coordinate_2.lat,
+    return haversine_distance(coordinate_1.lat, coordinate_1.lon, coordinate_2.lat,
                                  coordinate_2.lon);
 }
 
-float euclidean_distance(const FixedPointCoordinate &coordinate_1,
+float great_circle_distance(const FixedPointCoordinate &coordinate_1,
                                                  const FixedPointCoordinate &coordinate_2)
 {
-    return euclidean_distance(coordinate_1.lat, coordinate_1.lon, coordinate_2.lat,
+    return great_circle_distance(coordinate_1.lat, coordinate_1.lon, coordinate_2.lat,
                               coordinate_2.lon);
 }
 
-float euclidean_distance(const int lat1,
+float great_circle_distance(const int lat1,
                                                  const int lon1,
                                                  const int lat2,
                                                  const int lon2)
@@ -224,7 +224,7 @@ float perpendicular_distance_from_projected_coordinate(
     BOOST_ASSERT(nearest_location.is_valid());
 
     const float approximate_distance =
-        euclidean_distance(query_location, nearest_location);
+        great_circle_distance(query_location, nearest_location);
     BOOST_ASSERT(0.f <= approximate_distance);
     return approximate_distance;
 }
diff --git a/algorithms/coordinate_calculation.hpp b/algorithms/coordinate_calculation.hpp
index 309982a..80ec7c2 100644
--- a/algorithms/coordinate_calculation.hpp
+++ b/algorithms/coordinate_calculation.hpp
@@ -36,15 +36,15 @@ struct FixedPointCoordinate;
 namespace coordinate_calculation
 {
     double
-    great_circle_distance(const int lat1, const int lon1, const int lat2, const int lon2);
+    haversine_distance(const int lat1, const int lon1, const int lat2, const int lon2);
 
-    double great_circle_distance(const FixedPointCoordinate &first_coordinate,
+    double haversine_distance(const FixedPointCoordinate &first_coordinate,
                                  const FixedPointCoordinate &second_coordinate);
 
-    float euclidean_distance(const FixedPointCoordinate &first_coordinate,
+    float great_circle_distance(const FixedPointCoordinate &first_coordinate,
                              const FixedPointCoordinate &second_coordinate);
 
-    float euclidean_distance(const int lat1, const int lon1, const int lat2, const int lon2);
+    float great_circle_distance(const int lat1, const int lon1, const int lat2, const int lon2);
 
     void lat_or_lon_to_string(const int value, std::string &output);
 
diff --git a/algorithms/geospatial_query.hpp b/algorithms/geospatial_query.hpp
new file mode 100644
index 0000000..96b4fc5
--- /dev/null
+++ b/algorithms/geospatial_query.hpp
@@ -0,0 +1,180 @@
+#ifndef GEOSPATIAL_QUERY_HPP
+#define GEOSPATIAL_QUERY_HPP
+
+#include "coordinate_calculation.hpp"
+#include "../typedefs.h"
+#include "../data_structures/phantom_node.hpp"
+#include "../util/bearing.hpp"
+
+#include <osrm/coordinate.hpp>
+
+#include <vector>
+#include <memory>
+#include <algorithm>
+
+// Implements complex queries on top of an RTree and builds PhantomNodes from it.
+//
+// Only holds a weak reference on the RTree!
+template <typename RTreeT> class GeospatialQuery
+{
+    using EdgeData = typename RTreeT::EdgeData;
+    using CoordinateList = typename RTreeT::CoordinateList;
+
+  public:
+    GeospatialQuery(RTreeT &rtree_, std::shared_ptr<CoordinateList> coordinates_)
+        : rtree(rtree_), coordinates(coordinates_)
+    {
+    }
+
+    // Returns nearest PhantomNodes in the given bearing range within max_distance.
+    // Does not filter by small/big component!
+    std::vector<PhantomNodeWithDistance>
+    NearestPhantomNodesInRange(const FixedPointCoordinate &input_coordinate,
+                               const float max_distance,
+                               const int bearing = 0,
+                               const int bearing_range = 180)
+    {
+        auto results =
+            rtree.Nearest(input_coordinate,
+                          [this, bearing, bearing_range, max_distance](const EdgeData &data)
+                          {
+                              return checkSegmentBearing(data, bearing, bearing_range);
+                          },
+                          [max_distance](const std::size_t, const float min_dist)
+                          {
+                              return min_dist > max_distance;
+                          });
+
+        return MakePhantomNodes(input_coordinate, results);
+    }
+
+    // Returns max_results nearest PhantomNodes in the given bearing range.
+    // Does not filter by small/big component!
+    std::vector<PhantomNodeWithDistance>
+    NearestPhantomNodes(const FixedPointCoordinate &input_coordinate,
+                        const unsigned max_results,
+                        const int bearing = 0,
+                        const int bearing_range = 180)
+    {
+        auto results = rtree.Nearest(input_coordinate,
+                                     [this, bearing, bearing_range](const EdgeData &data)
+                                     {
+                                         return checkSegmentBearing(data, bearing, bearing_range);
+                                     },
+                                     [max_results](const std::size_t num_results, const float)
+                                     {
+                                         return num_results >= max_results;
+                                     });
+
+        return MakePhantomNodes(input_coordinate, results);
+    }
+
+    // Returns the nearest phantom node. If this phantom node is not from a big component
+    // a second phantom node is return that is the nearest coordinate in a big component.
+    std::pair<PhantomNode, PhantomNode>
+    NearestPhantomNodeWithAlternativeFromBigComponent(const FixedPointCoordinate &input_coordinate,
+                                                      const int bearing = 0,
+                                                      const int bearing_range = 180)
+    {
+        bool has_small_component = false;
+        bool has_big_component = false;
+        auto results = rtree.Nearest(
+            input_coordinate,
+            [this, bearing, bearing_range, &has_big_component,
+             &has_small_component](const EdgeData &data)
+            {
+                auto use_segment =
+                    (!has_small_component || (!has_big_component && !data.component.is_tiny));
+                auto use_directions = std::make_pair(use_segment, use_segment);
+
+                if (use_segment)
+                {
+                    use_directions = checkSegmentBearing(data, bearing, bearing_range);
+                    if (use_directions.first || use_directions.second)
+                    {
+                        has_big_component = has_big_component || !data.component.is_tiny;
+                        has_small_component = has_small_component || data.component.is_tiny;
+                    }
+                }
+
+                return use_directions;
+            },
+            [&has_big_component](const std::size_t num_results, const float)
+            {
+                return num_results > 0 && has_big_component;
+            });
+
+        if (results.size() == 0)
+        {
+            return std::make_pair(PhantomNode{}, PhantomNode{});
+        }
+
+        BOOST_ASSERT(results.size() > 0);
+        return std::make_pair(MakePhantomNode(input_coordinate, results.front()).phantom_node,
+                              MakePhantomNode(input_coordinate, results.back()).phantom_node);
+    }
+
+  private:
+    std::vector<PhantomNodeWithDistance>
+    MakePhantomNodes(const FixedPointCoordinate &input_coordinate,
+                     const std::vector<EdgeData> &results) const
+    {
+        std::vector<PhantomNodeWithDistance> distance_and_phantoms(results.size());
+        std::transform(results.begin(), results.end(), distance_and_phantoms.begin(),
+                       [this, &input_coordinate](const EdgeData &data)
+                       {
+                           return MakePhantomNode(input_coordinate, data);
+                       });
+        return distance_and_phantoms;
+    }
+
+    PhantomNodeWithDistance MakePhantomNode(const FixedPointCoordinate &input_coordinate,
+                                                   const EdgeData &data) const
+    {
+        FixedPointCoordinate point_on_segment;
+        float ratio;
+        const auto current_perpendicular_distance = coordinate_calculation::perpendicular_distance(
+            coordinates->at(data.u), coordinates->at(data.v), input_coordinate, point_on_segment,
+            ratio);
+
+        auto transformed =
+            PhantomNodeWithDistance { PhantomNode{data, point_on_segment}, current_perpendicular_distance };
+
+        ratio = std::min(1.f, std::max(0.f, ratio));
+
+        if (SPECIAL_NODEID != transformed.phantom_node.forward_node_id)
+        {
+            transformed.phantom_node.forward_weight *= ratio;
+        }
+        if (SPECIAL_NODEID != transformed.phantom_node.reverse_node_id)
+        {
+            transformed.phantom_node.reverse_weight *= 1.f - ratio;
+        }
+        return transformed;
+    }
+
+    std::pair<bool, bool> checkSegmentBearing(const EdgeData &segment,
+                                              const float filter_bearing,
+                                              const float filter_bearing_range)
+    {
+        const float forward_edge_bearing =
+            coordinate_calculation::bearing(coordinates->at(segment.u), coordinates->at(segment.v));
+
+        const float backward_edge_bearing = (forward_edge_bearing + 180) > 360
+                                                ? (forward_edge_bearing - 180)
+                                                : (forward_edge_bearing + 180);
+
+        const bool forward_bearing_valid =
+            bearing::CheckInBounds(forward_edge_bearing, filter_bearing, filter_bearing_range) &&
+            segment.forward_edge_based_node_id != SPECIAL_NODEID;
+        const bool backward_bearing_valid =
+            bearing::CheckInBounds(backward_edge_bearing, filter_bearing, filter_bearing_range) &&
+            segment.reverse_edge_based_node_id != SPECIAL_NODEID;
+        return std::make_pair(forward_bearing_valid, backward_bearing_valid);
+    }
+
+    RTreeT &rtree;
+    const std::shared_ptr<CoordinateList> coordinates;
+};
+
+#endif
diff --git a/algorithms/graph_compressor.hpp b/algorithms/graph_compressor.hpp
index 75405c0..8096a92 100644
--- a/algorithms/graph_compressor.hpp
+++ b/algorithms/graph_compressor.hpp
@@ -29,7 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "../typedefs.h"
 
-#include "../contractor/speed_profile.hpp"
+#include "../extractor/speed_profile.hpp"
 #include "../data_structures/node_based_graph.hpp"
 
 #include <memory>
diff --git a/algorithms/object_encoder.hpp b/algorithms/object_encoder.hpp
index 3ffac41..581b396 100644
--- a/algorithms/object_encoder.hpp
+++ b/algorithms/object_encoder.hpp
@@ -28,14 +28,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef OBJECT_ENCODER_HPP
 #define OBJECT_ENCODER_HPP
 
-#include "../util/string_util.hpp"
-
 #include <boost/assert.hpp>
 #include <boost/archive/iterators/base64_from_binary.hpp>
 #include <boost/archive/iterators/binary_from_base64.hpp>
 #include <boost/archive/iterators/transform_width.hpp>
 
 #include <algorithm>
+#include <iterator>
 #include <string>
 #include <vector>
 
@@ -66,8 +65,8 @@ struct ObjectEncoder
         encoded.resize(sizeof(ObjectT));
         encoded.assign(base64_t(&data[0]),
                        base64_t(&data[0] + (data.size() - number_of_padded_chars)));
-        replaceAll(encoded, "+", "-");
-        replaceAll(encoded, "/", "_");
+        std::replace(begin(encoded), end(encoded), '+', '-');
+        std::replace(begin(encoded), end(encoded), '/', '_');
     }
 
     template <class ObjectT> static void DecodeFromBase64(const std::string &input, ObjectT &object)
@@ -75,9 +74,8 @@ struct ObjectEncoder
         try
         {
             std::string encoded(input);
-            // replace "-" with "+" and "_" with "/"
-            replaceAll(encoded, "-", "+");
-            replaceAll(encoded, "_", "/");
+            std::replace(begin(encoded), end(encoded), '-', '+');
+            std::replace(begin(encoded), end(encoded), '_', '/');
 
             std::copy(binary_t(encoded.begin()), binary_t(encoded.begin() + encoded.length()),
                       reinterpret_cast<char *>(&object));
diff --git a/algorithms/trip_brute_force.hpp b/algorithms/trip_brute_force.hpp
index 1b451e1..601971c 100644
--- a/algorithms/trip_brute_force.hpp
+++ b/algorithms/trip_brute_force.hpp
@@ -73,6 +73,8 @@ std::vector<NodeID> BruteForceTrip(const NodeIDIterator start,
                                    const std::size_t number_of_locations,
                                    const DistTableWrapper<EdgeWeight> &dist_table)
 {
+    (void)number_of_locations; // unused
+
     const auto component_size = std::distance(start, end);
 
     std::vector<NodeID> perm(start, end);
@@ -103,4 +105,4 @@ std::vector<NodeID> BruteForceTrip(const NodeIDIterator start,
 
 } // end namespace trip
 } // end namespace osrm
-#endif // TRIP_BRUTE_FORCE_HPP
\ No newline at end of file
+#endif // TRIP_BRUTE_FORCE_HPP
diff --git a/algorithms/trip_farthest_insertion.hpp b/algorithms/trip_farthest_insertion.hpp
index 5f41248..91f0aa4 100644
--- a/algorithms/trip_farthest_insertion.hpp
+++ b/algorithms/trip_farthest_insertion.hpp
@@ -54,6 +54,7 @@ GetShortestRoundTrip(const NodeID new_loc,
                      const std::size_t number_of_locations,
                      std::vector<NodeID> &route)
 {
+    (void)number_of_locations; // unused
 
     auto min_trip_distance = INVALID_EDGE_WEIGHT;
     NodeIDIter next_insert_point_candidate;
@@ -76,7 +77,13 @@ GetShortestRoundTrip(const NodeID new_loc,
 
         BOOST_ASSERT_MSG(dist_from != INVALID_EDGE_WEIGHT, "distance has invalid edge weight");
         BOOST_ASSERT_MSG(dist_to != INVALID_EDGE_WEIGHT, "distance has invalid edge weight");
-        BOOST_ASSERT_MSG(trip_dist >= 0, "previous trip was not minimal. something's wrong");
+        // This is not neccessarily true:
+        // Lets say you have an edge (u, v) with duration 100. If you place a coordinate exactly in
+        // the middle of the segment yielding (u, v'), the adjusted duration will be 100 * 0.5 = 50.
+        // Now imagine two coordinates. One placed at 0.99 and one at 0.999. This means (u, v') now
+        // has a duration of 100 * 0.99 = 99, but (u, v'') also has a duration of 100 * 0.995 = 99.
+        // In which case (v', v'') has a duration of 0.
+        // BOOST_ASSERT_MSG(trip_dist >= 0, "previous trip was not minimal. something's wrong");
 
         // from all possible insertions to the current trip, choose the shortest of all insertions
         if (trip_dist < min_trip_distance)
@@ -118,7 +125,7 @@ std::vector<NodeID> FindRoute(const std::size_t &number_of_locations,
     for (std::size_t j = 2; j < component_size; ++j)
     {
 
-        auto farthest_distance = 0;
+        auto farthest_distance = std::numeric_limits<int>::min();
         auto next_node = -1;
         NodeIDIter next_insert_point;
 
diff --git a/appveyor-build.bat b/appveyor-build.bat
index babefe1..a1e9603 100644
--- a/appveyor-build.bat
+++ b/appveyor-build.bat
@@ -6,6 +6,8 @@ ECHO ~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %~f0 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
 SET PROJECT_DIR=%CD%
 ECHO PROJECT_DIR^: %PROJECT_DIR%
+ECHO NUMBER_OF_PROCESSORS^: %NUMBER_OF_PROCESSORS%
+ECHO cmake^: && cmake --version
 
 ECHO activating VS command prompt ...
 SET PATH=C:\Program Files (x86)\MSBuild\14.0\Bin;%PATH%
@@ -50,7 +52,7 @@ set TBB_ARCH_PLATFORM=intel64/vc14
 
 ECHO calling cmake ....
 cmake .. ^
--G "Visual Studio 14 Win64" ^
+-G "Visual Studio 14 2015 Win64" ^
 -DBOOST_ROOT=%BOOST_ROOT% ^
 -DBoost_ADDITIONAL_VERSIONS=1.58 ^
 -DBoost_USE_MULTITHREADED=ON ^
diff --git a/benchmarks/static_rtree.cpp b/benchmarks/static_rtree.cpp
index 9d88331..ebe055c 100644
--- a/benchmarks/static_rtree.cpp
+++ b/benchmarks/static_rtree.cpp
@@ -25,16 +25,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 */
 
-#include "../data_structures/original_edge_data.hpp"
 #include "../data_structures/query_node.hpp"
-#include "../data_structures/shared_memory_vector_wrapper.hpp"
 #include "../data_structures/static_rtree.hpp"
-#include "../util/boost_filesystem_2_fix.hpp"
 #include "../data_structures/edge_based_node.hpp"
+#include "../algorithms/geospatial_query.hpp"
+#include "../util/timing_util.hpp"
 
 #include <osrm/coordinate.hpp>
 
 #include <random>
+#include <iostream>
 
 // Choosen by a fair W20 dice roll (this value is completely arbitrary)
 constexpr unsigned RANDOM_SEED = 13;
@@ -46,6 +46,7 @@ constexpr int32_t WORLD_MAX_LON = 180 * COORDINATE_PRECISION;
 using RTreeLeaf = EdgeBasedNode;
 using FixedPointCoordinateListPtr = std::shared_ptr<std::vector<FixedPointCoordinate>>;
 using BenchStaticRTree = StaticRTree<RTreeLeaf, ShM<FixedPointCoordinate, false>::vector, false>;
+using BenchQuery = GeospatialQuery<BenchStaticRTree>;
 
 FixedPointCoordinateListPtr LoadCoordinates(const boost::filesystem::path &nodes_file)
 {
@@ -66,99 +67,65 @@ FixedPointCoordinateListPtr LoadCoordinates(const boost::filesystem::path &nodes
     return coords;
 }
 
-void Benchmark(BenchStaticRTree &rtree, unsigned num_queries)
+template <typename QueryT>
+void BenchmarkQuery(const std::vector<FixedPointCoordinate> &queries,
+                    const std::string& name,
+                    QueryT query)
 {
-    std::mt19937 mt_rand(RANDOM_SEED);
-    std::uniform_int_distribution<> lat_udist(WORLD_MIN_LAT, WORLD_MAX_LAT);
-    std::uniform_int_distribution<> lon_udist(WORLD_MIN_LON, WORLD_MAX_LON);
-    std::vector<FixedPointCoordinate> queries;
-    for (unsigned i = 0; i < num_queries; i++)
-    {
-        queries.emplace_back(FixedPointCoordinate(lat_udist(mt_rand), lon_udist(mt_rand)));
-    }
+    std::cout << "Running " << name << " with " << queries.size() << " coordinates: " << std::flush;
 
-    {
-        const unsigned num_results = 5;
-        std::cout << "#### IncrementalFindPhantomNodeForCoordinate : " << num_results
-                  << " phantom nodes"
-                  << "\n";
-
-        TIMER_START(query_phantom);
-        std::vector<PhantomNode> phantom_node_vector;
-        for (const auto &q : queries)
-        {
-            phantom_node_vector.clear();
-            rtree.IncrementalFindPhantomNodeForCoordinate(q, phantom_node_vector, 3, num_results);
-            phantom_node_vector.clear();
-            rtree.IncrementalFindPhantomNodeForCoordinate(q, phantom_node_vector, 17, num_results);
-        }
-        TIMER_STOP(query_phantom);
-
-        std::cout << "Took " << TIMER_MSEC(query_phantom) << " msec for " << num_queries
-                  << " queries."
-                  << "\n";
-        std::cout << TIMER_MSEC(query_phantom) / ((double)num_queries) << " msec/query."
-                  << "\n";
-
-        std::cout << "#### LocateClosestEndPointForCoordinate"
-                  << "\n";
-    }
-
-    TIMER_START(query_endpoint);
-    FixedPointCoordinate result;
+    TIMER_START(query);
     for (const auto &q : queries)
     {
-        rtree.LocateClosestEndPointForCoordinate(q, result, 3);
+        auto result = query(q);
     }
-    TIMER_STOP(query_endpoint);
-
-    std::cout << "Took " << TIMER_MSEC(query_endpoint) << " msec for " << num_queries << " queries."
-              << "\n";
-    std::cout << TIMER_MSEC(query_endpoint) / ((double)num_queries) << " msec/query."
-              << "\n";
+    TIMER_STOP(query);
 
-    std::cout << "#### FindPhantomNodeForCoordinate"
-              << "\n";
+    std::cout << "Took " << TIMER_SEC(query) << " seconds "
+              << "(" << TIMER_MSEC(query) << "ms"
+              << ")  ->  " << TIMER_MSEC(query) / queries.size() << " ms/query "
+              << "(" << TIMER_MSEC(query) << "ms"
+              << ")" << std::endl;
+}
 
-    TIMER_START(query_node);
-    for (const auto &q : queries)
+void Benchmark(BenchStaticRTree &rtree, BenchQuery &geo_query, unsigned num_queries)
+{
+    std::mt19937 mt_rand(RANDOM_SEED);
+    std::uniform_int_distribution<> lat_udist(WORLD_MIN_LAT, WORLD_MAX_LAT);
+    std::uniform_int_distribution<> lon_udist(WORLD_MIN_LON, WORLD_MAX_LON);
+    std::vector<FixedPointCoordinate> queries;
+    for (unsigned i = 0; i < num_queries; i++)
     {
-        PhantomNode phantom;
-        rtree.FindPhantomNodeForCoordinate(q, phantom, 3);
+        queries.emplace_back(lat_udist(mt_rand), lon_udist(mt_rand));
     }
-    TIMER_STOP(query_node);
-
-    std::cout << "Took " << TIMER_MSEC(query_node) << " msec for " << num_queries << " queries."
-              << "\n";
-    std::cout << TIMER_MSEC(query_node) / ((double)num_queries) << " msec/query."
-              << "\n";
 
-    {
-        const unsigned num_results = 1;
-        std::cout << "#### IncrementalFindPhantomNodeForCoordinate : " << num_results
-                  << " phantom nodes"
-                  << "\n";
-
-        TIMER_START(query_phantom);
-        std::vector<PhantomNode> phantom_node_vector;
-        for (const auto &q : queries)
-        {
-            phantom_node_vector.clear();
-            rtree.IncrementalFindPhantomNodeForCoordinate(q, phantom_node_vector, 3, num_results);
-            phantom_node_vector.clear();
-            rtree.IncrementalFindPhantomNodeForCoordinate(q, phantom_node_vector, 17, num_results);
-        }
-        TIMER_STOP(query_phantom);
-
-        std::cout << "Took " << TIMER_MSEC(query_phantom) << " msec for " << num_queries
-                  << " queries."
-                  << "\n";
-        std::cout << TIMER_MSEC(query_phantom) / ((double)num_queries) << " msec/query."
-                  << "\n";
-
-        std::cout << "#### LocateClosestEndPointForCoordinate"
-                  << "\n";
-    }
+    BenchmarkQuery(queries, "raw RTree queries (1 result)", [&rtree](const FixedPointCoordinate &q)
+                   {
+                       return rtree.Nearest(q, 1);
+                   });
+    BenchmarkQuery(queries, "raw RTree queries (10 results)",
+                   [&rtree](const FixedPointCoordinate &q)
+                   {
+                       return rtree.Nearest(q, 10);
+                   });
+
+    BenchmarkQuery(queries, "big component alternative queries",
+                   [&geo_query](const FixedPointCoordinate &q)
+                   {
+                       return geo_query.NearestPhantomNodeWithAlternativeFromBigComponent(q);
+                   });
+    BenchmarkQuery(queries, "max distance 1000", [&geo_query](const FixedPointCoordinate &q)
+                   {
+                       return geo_query.NearestPhantomNodesInRange(q, 1000);
+                   });
+    BenchmarkQuery(queries, "PhantomNode query (1 result)", [&geo_query](const FixedPointCoordinate &q)
+                   {
+                       return geo_query.NearestPhantomNodes(q, 1);
+                   });
+    BenchmarkQuery(queries, "PhantomNode query (10 result)", [&geo_query](const FixedPointCoordinate &q)
+                   {
+                       return geo_query.NearestPhantomNodes(q, 10);
+                   });
 }
 
 int main(int argc, char **argv)
@@ -177,8 +144,9 @@ int main(int argc, char **argv)
     auto coords = LoadCoordinates(nodesPath);
 
     BenchStaticRTree rtree(ramPath, filePath, coords);
+    BenchQuery query(rtree, coords);
 
-    Benchmark(rtree, 10000);
+    Benchmark(rtree, query, 10000);
 
     return 0;
 }
diff --git a/build-local.bat b/build-local.bat
index e1ae94a..b26c415 100644
--- a/build-local.bat
+++ b/build-local.bat
@@ -11,7 +11,7 @@ SET CONFIGURATION=Release
 FOR /F "tokens=*" %%i in ('git rev-parse --abbrev-ref HEAD') do SET APPVEYOR_REPO_BRANCH=%%i
 ECHO APPVEYOR_REPO_BRANCH^: %APPVEYOR_REPO_BRANCH%
 
-SET PATH=C:\mb\windows-builds-64\tmp-bin\cmake-3.1.0-win32-x86\bin;%PATH%
+SET PATH=C:\mb\windows-builds-64\tmp-bin\cmake-3.4.0-win32-x86\bin;%PATH%
 SET PATH=C:\Program Files\7-Zip;%PATH%
 
 powershell Set-ExecutionPolicy -Scope CurrentUser -ExecutionPolicy Unrestricted -Force
diff --git a/cmake/FingerPrint-Config.cmake b/cmake/FingerPrint-Config.cmake
index 857dedd..d36b622 100644
--- a/cmake/FingerPrint-Config.cmake
+++ b/cmake/FingerPrint-Config.cmake
@@ -1,6 +1,6 @@
-set(OLDFILE ${SOURCE_DIR}/util/fingerprint_impl.hpp)
+set(OLDFILE ${OUTPUT_DIR}/util/fingerprint_impl.hpp)
 set(NEWFILE ${OLDFILE}.tmp)
-set(INFILE ${OLDFILE}.in)
+set(INFILE ${SOURCE_DIR}/util/fingerprint_impl.hpp.in)
 file(MD5 ${SOURCE_DIR}/prepare.cpp MD5PREPARE)
 file(MD5 ${SOURCE_DIR}/data_structures/static_rtree.hpp MD5RTREE)
 file(MD5 ${SOURCE_DIR}/util/graph_loader.hpp MD5GRAPH)
@@ -13,7 +13,7 @@ file(MD5 ${NEWFILE} MD5NEW)
 if (EXISTS ${OLDFILE})
     file(MD5 ${OLDFILE} MD5OLD)
     if(NOT ${MD5NEW} STREQUAL ${MD5OLD})
-	    file(REMOVE_RECURSE ${OLDFILE})
+        file(REMOVE_RECURSE ${OLDFILE})
         file(RENAME ${NEWFILE} ${OLDFILE})
     else()
         file(REMOVE_RECURSE ${NEWFILE})
diff --git a/cmake/GetGitRevisionDescription.cmake b/cmake/GetGitRevisionDescription.cmake
deleted file mode 100644
index 1bf0230..0000000
--- a/cmake/GetGitRevisionDescription.cmake
+++ /dev/null
@@ -1,123 +0,0 @@
-# - Returns a version string from Git
-#
-# These functions force a re-configure on each git commit so that you can
-# trust the values of the variables in your build system.
-#
-#  get_git_head_revision(<refspecvar> <hashvar> [<additional arguments to git describe> ...])
-#
-# Returns the refspec and sha hash of the current head revision
-#
-#  git_describe(<var> [<additional arguments to git describe> ...])
-#
-# Returns the results of git describe on the source tree, and adjusting
-# the output so that it tests false if an error occurs.
-#
-#  git_get_exact_tag(<var> [<additional arguments to git describe> ...])
-#
-# Returns the results of git describe --exact-match on the source tree,
-# and adjusting the output so that it tests false if there was no exact
-# matching tag.
-#
-# Requires CMake 2.6 or newer (uses the 'function' command)
-#
-# Original Author:
-# 2009-2010 Ryan Pavlik <rpavlik at iastate.edu> <abiryan at ryand.net>
-# http://academic.cleardefinition.com
-# Iowa State University HCI Graduate Program/VRAC
-#
-# Copyright Iowa State University 2009-2010.
-# Distributed under the Boost Software License, Version 1.0.
-# (See accompanying file LICENSE_1_0.txt or copy at
-# http://www.boost.org/LICENSE_1_0.txt)
-
-if(__get_git_revision_description)
-	return()
-endif()
-set(__get_git_revision_description YES)
-
-# We must run the following at "include" time, not at function call time,
-# to find the path to this module rather than the path to a calling list file
-get_filename_component(_gitdescmoddir ${CMAKE_CURRENT_LIST_FILE} PATH)
-
-function(get_git_head_revision _refspecvar _hashvar)
-	set(GIT_PARENT_DIR "${CMAKE_SOURCE_DIR}")
-	set(GIT_DIR "${GIT_PARENT_DIR}/.git")
-	while(NOT EXISTS "${GIT_DIR}")	# .git dir not found, search parent directories
-		set(GIT_PREVIOUS_PARENT "${GIT_PARENT_DIR}")
-		get_filename_component(GIT_PARENT_DIR ${GIT_PARENT_DIR} PATH)
-		if(GIT_PARENT_DIR STREQUAL GIT_PREVIOUS_PARENT)
-			# We have reached the root directory, we are not in git
-			set(${_refspecvar} "GITDIR-NOTFOUND" PARENT_SCOPE)
-			set(${_hashvar} "GITDIR-NOTFOUND" PARENT_SCOPE)
-			return()
-		endif()
-		set(GIT_DIR "${GIT_PARENT_DIR}/.git")
-	endwhile()
-	set(GIT_DATA "${CMAKE_CURRENT_BINARY_DIR}/CMakeFiles/git-data")
-	if(NOT EXISTS "${GIT_DATA}")
-		file(MAKE_DIRECTORY "${GIT_DATA}")
-	endif()
-
-	if(NOT EXISTS "${GIT_DIR}/HEAD")
-		return()
-	endif()
-	set(HEAD_FILE "${GIT_DATA}/HEAD")
-	configure_file("${GIT_DIR}/HEAD" "${HEAD_FILE}" COPYONLY)
-
-	configure_file("${_gitdescmoddir}/GetGitRevisionDescription.cmake.in"
-		"${GIT_DATA}/grabRef.cmake"
-		@ONLY)
-	include("${GIT_DATA}/grabRef.cmake")
-
-	set(${_refspecvar} "${HEAD_REF}" PARENT_SCOPE)
-	set(${_hashvar} "${HEAD_HASH}" PARENT_SCOPE)
-endfunction()
-
-function(git_describe _var)
-	if(NOT GIT_FOUND)
-		find_package(Git QUIET)
-	endif()
-	get_git_head_revision(refspec hash)
-	if(NOT GIT_FOUND)
-		set(${_var} "GIT-NOTFOUND" PARENT_SCOPE)
-		return()
-	endif()
-	if(NOT hash)
-		set(${_var} "HEAD-HASH-NOTFOUND" PARENT_SCOPE)
-		return()
-	endif()
-
-	# TODO sanitize
-	#if((${ARGN}" MATCHES "&&") OR
-	#	(ARGN MATCHES "||") OR
-	#	(ARGN MATCHES "\\;"))
-	#	message("Please report the following error to the project!")
-	#	message(FATAL_ERROR "Looks like someone's doing something nefarious with git_describe! Passed arguments ${ARGN}")
-	#endif()
-
-	#message(STATUS "Arguments to execute_process: ${ARGN}")
-
-	execute_process(COMMAND
-		"${GIT_EXECUTABLE}"
-		describe
-		${hash}
-		${ARGN}
-		WORKING_DIRECTORY
-		"${CMAKE_SOURCE_DIR}"
-		RESULT_VARIABLE
-		res
-		OUTPUT_VARIABLE
-		out
-		ERROR_QUIET
-		OUTPUT_STRIP_TRAILING_WHITESPACE)
-	if(NOT res EQUAL 0)
-		set(out "${out}-${res}-NOTFOUND")
-	endif()
-
-	set(${_var} "${out}" PARENT_SCOPE)
-endfunction()
-
-function(git_get_exact_tag _var)
-	git_describe(out --exact-match ${ARGN})
-	set(${_var} "${out}" PARENT_SCOPE)
-endfunction()
diff --git a/cmake/GetGitRevisionDescription.cmake.in b/cmake/GetGitRevisionDescription.cmake.in
deleted file mode 100644
index 888ce13..0000000
--- a/cmake/GetGitRevisionDescription.cmake.in
+++ /dev/null
@@ -1,38 +0,0 @@
-# 
-# Internal file for GetGitRevisionDescription.cmake
-#
-# Requires CMake 2.6 or newer (uses the 'function' command)
-#
-# Original Author:
-# 2009-2010 Ryan Pavlik <rpavlik at iastate.edu> <abiryan at ryand.net>
-# http://academic.cleardefinition.com
-# Iowa State University HCI Graduate Program/VRAC
-#
-# Copyright Iowa State University 2009-2010.
-# Distributed under the Boost Software License, Version 1.0.
-# (See accompanying file LICENSE_1_0.txt or copy at
-# http://www.boost.org/LICENSE_1_0.txt)
-
-set(HEAD_HASH)
-
-file(READ "@HEAD_FILE@" HEAD_CONTENTS LIMIT 1024)
-
-string(STRIP "${HEAD_CONTENTS}" HEAD_CONTENTS)
-if(HEAD_CONTENTS MATCHES "ref")
-	# named branch
-	string(REPLACE "ref: " "" HEAD_REF "${HEAD_CONTENTS}")
-	if(EXISTS "@GIT_DIR@/${HEAD_REF}")
-		configure_file("@GIT_DIR@/${HEAD_REF}" "@GIT_DATA@/head-ref" COPYONLY)
-	elseif(EXISTS "@GIT_DIR@/logs/${HEAD_REF}")
-		configure_file("@GIT_DIR@/logs/${HEAD_REF}" "@GIT_DATA@/head-ref" COPYONLY)
-		set(HEAD_HASH "${HEAD_REF}")
-	endif()
-else()
-	# detached HEAD
-	configure_file("@GIT_DIR@/HEAD" "@GIT_DATA@/head-ref" COPYONLY)
-endif()
-
-if(NOT HEAD_HASH)
-	file(READ "@GIT_DATA@/head-ref" HEAD_HASH LIMIT 1024)
-	string(STRIP "${HEAD_HASH}" HEAD_HASH)
-endif()
diff --git a/cmake/check_luabind.cmake b/cmake/check_luabind.cmake
index 18f0656..79253af 100644
--- a/cmake/check_luabind.cmake
+++ b/cmake/check_luabind.cmake
@@ -1,7 +1,7 @@
 INCLUDE (CheckCXXSourceCompiles)
 unset(LUABIND_WORKS CACHE)
 unset(LUABIND51_WORKS CACHE)
-set (LUABIND_CHECK_SRC "#include  \"lua.h\"\n#include <luabind/luabind.hpp>\n int main() { lua_State *myLuaState = luaL_newstate(); luabind::open(myLuaState);  return 0;}")
+set (LUABIND_CHECK_SRC "extern \"C\" {\n#include \"lua.h\"\n#include \"lauxlib.h\"\n}\n#include <luabind/open.hpp>\nint main() { lua_State *x = luaL_newstate(); luabind::open(x); }")
 set (CMAKE_TRY_COMPILE_CONFIGURATION ${CMAKE_BUILD_TYPE})
 set (CMAKE_REQUIRED_INCLUDES "${Boost_INCLUDE_DIR};${LUABIND_INCLUDE_DIR};${LUA_INCLUDE_DIR}")
 set (CMAKE_REQUIRED_LIBRARIES "${LUABIND_LIBRARY};${LUA_LIBRARY}")
diff --git a/cmake/pkgconfig.in b/cmake/pkgconfig.in
index e81feba..16263bf 100644
--- a/cmake/pkgconfig.in
+++ b/cmake/pkgconfig.in
@@ -1,11 +1,11 @@
 prefix=@CMAKE_INSTALL_PREFIX@
-includedir=${prefix}/include/osrm
+includedir=${prefix}/include
 libdir=${prefix}/lib
 
 Name: libOSRM
 Description: Project OSRM library
-Version: @GIT_DESCRIPTION@
+Version: v at OSRM_VERSION_MAJOR@. at OSRM_VERSION_MINOR@. at OSRM_VERSION_PATCH@
 Requires:
 Libs: -L${libdir} -lOSRM
-Libs.private: @BOOST_LIBRARY_LISTING@
+Libs.private: @BOOST_LIBRARY_LISTING@ @TBB_LIBRARY_LISTING@
 Cflags: -I${includedir}
diff --git a/contractor/contractor.hpp b/contractor/contractor.hpp
index d6f5642..07a21dc 100644
--- a/contractor/contractor.hpp
+++ b/contractor/contractor.hpp
@@ -50,6 +50,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <algorithm>
 #include <limits>
+#include <memory>
 #include <vector>
 
 class Contractor
@@ -157,6 +158,15 @@ class Contractor
 
   public:
     template <class ContainerT> Contractor(int nodes, ContainerT &input_edge_list)
+      : Contractor(nodes, input_edge_list, {}, {})
+    {
+    }
+
+    template <class ContainerT>
+    Contractor(int nodes,
+               ContainerT &input_edge_list,
+               std::vector<float> &&node_levels_)
+        : node_levels(std::move(node_levels_))
     {
         std::vector<ContractorEdge> edges;
         edges.reserve(input_edge_list.size() * 2);
@@ -172,7 +182,8 @@ class Contractor
                 SimpleLogger().Write(logWARNING)
                     << "Edge weight large -> "
                     << static_cast<unsigned int>(std::max(diter->weight, 1)) << " : "
-                    << static_cast<unsigned int>(diter->source) << " -> " << static_cast<unsigned int>(diter->target);
+                    << static_cast<unsigned int>(diter->source) << " -> "
+                    << static_cast<unsigned int>(diter->target);
             }
 #endif
             edges.emplace_back(diter->source, diter->target,
@@ -284,7 +295,7 @@ class Contractor
 
     ~Contractor() {}
 
-    void Run( double core_factor = 1.0 )
+    void Run(double core_factor = 1.0)
     {
         // for the preperation we can use a big grain size, which is much faster (probably cache)
         constexpr size_t InitGrainSize = 100000;
@@ -303,14 +314,14 @@ class Contractor
         ThreadDataContainer thread_data_list(number_of_nodes);
 
         NodeID number_of_contracted_nodes = 0;
-        std::vector<RemainingNodeData> remaining_nodes(number_of_nodes);
-        std::vector<float> node_priorities(number_of_nodes);
-        std::vector<NodePriorityData> node_data(number_of_nodes);
+        std::vector<NodePriorityData> node_data;
+        std::vector<float> node_priorities;
         is_core_node.resize(number_of_nodes, false);
 
+        std::vector<RemainingNodeData> remaining_nodes(number_of_nodes);
         // initialize priorities in parallel
         tbb::parallel_for(tbb::blocked_range<int>(0, number_of_nodes, InitGrainSize),
-                          [&remaining_nodes](const tbb::blocked_range<int> &range)
+                          [this, &remaining_nodes](const tbb::blocked_range<int> &range)
                           {
                               for (int x = range.begin(), end = range.end(); x != end; ++x)
                               {
@@ -318,25 +329,44 @@ class Contractor
                               }
                           });
 
-        std::cout << "initializing elimination PQ ..." << std::flush;
-        tbb::parallel_for(tbb::blocked_range<int>(0, number_of_nodes, PQGrainSize),
-                          [this, &node_priorities, &node_data, &thread_data_list](
-                              const tbb::blocked_range<int> &range)
-                          {
-                              ContractorThreadData *data = thread_data_list.getThreadData();
-                              for (int x = range.begin(), end = range.end(); x != end; ++x)
+        bool use_cached_node_priorities = !node_levels.empty();
+        if (use_cached_node_priorities)
+        {
+            std::cout << "using cached node priorities ..." << std::flush;
+            node_priorities.swap(node_levels);
+            std::cout << "ok" << std::endl;
+        }
+        else
+        {
+            node_data.resize(number_of_nodes);
+            node_priorities.resize(number_of_nodes);
+            node_levels.resize(number_of_nodes);
+
+            std::cout << "initializing elimination PQ ..." << std::flush;
+            tbb::parallel_for(tbb::blocked_range<int>(0, number_of_nodes, PQGrainSize),
+                              [this, &node_priorities, &node_data, &thread_data_list](
+                                  const tbb::blocked_range<int> &range)
                               {
-                                  node_priorities[x] =
-                                      this->EvaluateNodePriority(data, &node_data[x], x);
-                              }
-                          });
-        std::cout << "ok" << std::endl << "preprocessing " << number_of_nodes << " nodes ..."
-                  << std::flush;
+                                  ContractorThreadData *data = thread_data_list.getThreadData();
+                                  for (int x = range.begin(), end = range.end(); x != end; ++x)
+                                  {
+                                      node_priorities[x] =
+                                          this->EvaluateNodePriority(data, &node_data[x], x);
+                                  }
+                              });
+            std::cout << "ok" << std::endl;
+        }
+        BOOST_ASSERT(node_priorities.size() == number_of_nodes);
+
+        std::cout << "preprocessing " << number_of_nodes << " nodes ..." << std::flush;
 
+        unsigned current_level = 0;
         bool flushed_contractor = false;
-        while (number_of_nodes > 2 && number_of_contracted_nodes < static_cast<NodeID>(number_of_nodes * core_factor) )
+        while (number_of_nodes > 2 &&
+               number_of_contracted_nodes < static_cast<NodeID>(number_of_nodes * core_factor))
         {
-            if (!flushed_contractor && (number_of_contracted_nodes > static_cast<NodeID>(number_of_nodes * 0.65 * core_factor)))
+            if (!flushed_contractor && (number_of_contracted_nodes >
+                                        static_cast<NodeID>(number_of_nodes * 0.65 * core_factor)))
             {
                 DeallocatingVector<ContractorEdge> new_edge_set; // this one is not explicitely
                                                                  // cleared since it goes out of
@@ -355,28 +385,32 @@ class Contractor
                 // remaining graph
                 std::vector<NodeID> new_node_id_from_orig_id_map(number_of_nodes, UINT_MAX);
 
-                // build forward and backward renumbering map and remap ids in remaining_nodes and
-                // Priorities.
                 for (const auto new_node_id : osrm::irange<std::size_t>(0, remaining_nodes.size()))
                 {
+                    auto& node = remaining_nodes[new_node_id];
+                    BOOST_ASSERT(node_priorities.size() > node.id);
+                    new_node_priority[new_node_id] = node_priorities[node.id];
+                }
+
+                // build forward and backward renumbering map and remap ids in remaining_nodes
+                for (const auto new_node_id : osrm::irange<std::size_t>(0, remaining_nodes.size()))
+                {
+                    auto& node = remaining_nodes[new_node_id];
                     // create renumbering maps in both directions
-                    orig_node_id_from_new_node_id_map[new_node_id] = remaining_nodes[new_node_id].id;
-                    new_node_id_from_orig_id_map[remaining_nodes[new_node_id].id] = new_node_id;
-                    new_node_priority[new_node_id] =
-                        node_priorities[remaining_nodes[new_node_id].id];
-                    remaining_nodes[new_node_id].id = new_node_id;
+                    orig_node_id_from_new_node_id_map[new_node_id] = node.id;
+                    new_node_id_from_orig_id_map[node.id] = new_node_id;
+                    node.id = new_node_id;
                 }
                 // walk over all nodes
-                for (const auto i :
-                     osrm::irange<std::size_t>(0, contractor_graph->GetNumberOfNodes()))
+                for (const auto source :
+                     osrm::irange<NodeID>(0, contractor_graph->GetNumberOfNodes()))
                 {
-                    const NodeID source = i;
                     for (auto current_edge : contractor_graph->GetAdjacentEdgeRange(source))
                     {
                         ContractorGraph::EdgeData &data =
                             contractor_graph->GetEdgeData(current_edge);
                         const NodeID target = contractor_graph->GetTarget(current_edge);
-                        if (SPECIAL_NODEID == new_node_id_from_orig_id_map[i])
+                        if (SPECIAL_NODEID == new_node_id_from_orig_id_map[source])
                         {
                             external_edge_list.push_back({source, target, data});
                         }
@@ -411,7 +445,7 @@ class Contractor
                 contractor_graph.reset();
 
                 // create new graph
-                std::sort(new_edge_set.begin(), new_edge_set.end());
+                tbb::parallel_sort(new_edge_set.begin(), new_edge_set.end());
                 contractor_graph =
                     std::make_shared<ContractorGraph>(remaining_nodes.size(), new_edge_set);
 
@@ -423,14 +457,13 @@ class Contractor
                 thread_data_list.number_of_nodes = contractor_graph->GetNumberOfNodes();
             }
 
-            const int last = (int)remaining_nodes.size();
-            tbb::parallel_for(tbb::blocked_range<int>(0, last, IndependentGrainSize),
+            tbb::parallel_for(tbb::blocked_range<std::size_t>(0, remaining_nodes.size(), IndependentGrainSize),
                               [this, &node_priorities, &remaining_nodes, &thread_data_list](
-                                  const tbb::blocked_range<int> &range)
+                                  const tbb::blocked_range<std::size_t> &range)
                               {
                                   ContractorThreadData *data = thread_data_list.getThreadData();
                                   // determine independent node set
-                                  for (int i = range.begin(), end = range.end(); i != end; ++i)
+                                  for (auto i = range.begin(), end = range.end(); i != end; ++i)
                                   {
                                       const NodeID node = remaining_nodes[i].id;
                                       remaining_nodes[i].is_independent =
@@ -438,17 +471,45 @@ class Contractor
                                   }
                               });
 
-            const auto first = stable_partition(remaining_nodes.begin(), remaining_nodes.end(),
+            // sort all remaining nodes to the beginning of the sequence
+            const auto begin_independent_nodes = stable_partition(remaining_nodes.begin(), remaining_nodes.end(),
                                                 [](RemainingNodeData node_data)
                                                 {
                                                     return !node_data.is_independent;
                                                 });
-            const int first_independent_node = static_cast<int>(first - remaining_nodes.begin());
+            auto begin_independent_nodes_idx = std::distance(remaining_nodes.begin(), begin_independent_nodes);
+            auto end_independent_nodes_idx = remaining_nodes.size();
+
+            if (!use_cached_node_priorities)
+            {
+                // write out contraction level
+                tbb::parallel_for(
+                    tbb::blocked_range<std::size_t>(begin_independent_nodes_idx, end_independent_nodes_idx, ContractGrainSize),
+                    [this, remaining_nodes, flushed_contractor, current_level](const tbb::blocked_range<std::size_t> &range)
+                    {
+                        if (flushed_contractor)
+                        {
+                            for (int position = range.begin(), end = range.end(); position != end; ++position)
+                            {
+                                const NodeID x = remaining_nodes[position].id;
+                                node_levels[orig_node_id_from_new_node_id_map[x]] = current_level;
+                            }
+                        }
+                        else
+                        {
+                            for (int position = range.begin(), end = range.end(); position != end; ++position)
+                            {
+                                const NodeID x = remaining_nodes[position].id;
+                                node_levels[x] = current_level;
+                            }
+                        }
+                    });
+            }
 
             // contract independent nodes
             tbb::parallel_for(
-                tbb::blocked_range<int>(first_independent_node, last, ContractGrainSize),
-                [this, &remaining_nodes, &thread_data_list](const tbb::blocked_range<int> &range)
+                tbb::blocked_range<std::size_t>(begin_independent_nodes_idx, end_independent_nodes_idx, ContractGrainSize),
+                [this, &remaining_nodes, &thread_data_list](const tbb::blocked_range<std::size_t> &range)
                 {
                     ContractorThreadData *data = thread_data_list.getThreadData();
                     for (int position = range.begin(), end = range.end(); position != end; ++position)
@@ -457,16 +518,9 @@ class Contractor
                         this->ContractNode<false>(data, x);
                     }
                 });
-            // make sure we really sort each block
-            tbb::parallel_for(
-                thread_data_list.data.range(),
-                [&](const ThreadDataContainer::EnumerableThreadData::range_type &range)
-                {
-                    for (auto &data : range)
-                        std::sort(data->inserted_edges.begin(), data->inserted_edges.end());
-                });
+
             tbb::parallel_for(
-                tbb::blocked_range<int>(first_independent_node, last, DeleteGrainSize),
+                tbb::blocked_range<int>(begin_independent_nodes_idx, end_independent_nodes_idx, DeleteGrainSize),
                 [this, &remaining_nodes, &thread_data_list](const tbb::blocked_range<int> &range)
                 {
                     ContractorThreadData *data = thread_data_list.getThreadData();
@@ -477,6 +531,16 @@ class Contractor
                     }
                 });
 
+            // make sure we really sort each block
+            tbb::parallel_for(
+                thread_data_list.data.range(),
+                [&](const ThreadDataContainer::EnumerableThreadData::range_type &range)
+                {
+                    for (auto &data : range)
+                        tbb::parallel_sort(data->inserted_edges.begin(),
+                                           data->inserted_edges.end());
+                });
+
             // insert new edges
             for (auto &data : thread_data_list.data)
             {
@@ -502,23 +566,25 @@ class Contractor
                 data->inserted_edges.clear();
             }
 
-            tbb::parallel_for(
-                tbb::blocked_range<int>(first_independent_node, last, NeighboursGrainSize),
-                [this, &remaining_nodes, &node_priorities, &node_data, &thread_data_list](
-                    const tbb::blocked_range<int> &range)
-                {
-                    ContractorThreadData *data = thread_data_list.getThreadData();
-                    for (int position = range.begin(), end = range.end(); position != end; ++position)
+            if (!use_cached_node_priorities)
+            {
+                tbb::parallel_for(
+                    tbb::blocked_range<int>(begin_independent_nodes_idx, end_independent_nodes_idx, NeighboursGrainSize),
+                    [this, &node_priorities, &remaining_nodes, &node_data, &thread_data_list](
+                        const tbb::blocked_range<int> &range)
                     {
-                        NodeID x = remaining_nodes[position].id;
-                        this->UpdateNodeNeighbours(node_priorities, node_data, data, x);
-                    }
-                });
+                        ContractorThreadData *data = thread_data_list.getThreadData();
+                        for (int position = range.begin(), end = range.end(); position != end; ++position)
+                        {
+                            NodeID x = remaining_nodes[position].id;
+                            this->UpdateNodeNeighbours(node_priorities, node_data, data, x);
+                        }
+                    });
+            }
 
             // remove contracted nodes from the pool
-            number_of_contracted_nodes += last - first_independent_node;
-            remaining_nodes.resize(first_independent_node);
-            remaining_nodes.shrink_to_fit();
+            number_of_contracted_nodes += end_independent_nodes_idx - begin_independent_nodes_idx;
+            remaining_nodes.resize(begin_independent_nodes_idx);
             //            unsigned maxdegree = 0;
             //            unsigned avgdegree = 0;
             //            unsigned mindegree = UINT_MAX;
@@ -545,16 +611,37 @@ class Contractor
             //            quad: " << quaddegree;
 
             p.printStatus(number_of_contracted_nodes);
+            ++current_level;
         }
 
         if (remaining_nodes.size() > 2)
         {
-            // TODO: for small cores a sorted array of core ids might also work good
-            for (const auto& node : remaining_nodes)
-            {
-                auto orig_id = orig_node_id_from_new_node_id_map[node.id];
-                is_core_node[orig_id] = true;
-            }
+              if (orig_node_id_from_new_node_id_map.size() > 0)
+              {
+                  tbb::parallel_for(
+                      tbb::blocked_range<int>(0, remaining_nodes.size(), InitGrainSize),
+                      [this, &remaining_nodes](const tbb::blocked_range<int> &range)
+                      {
+                          for (int x = range.begin(), end = range.end(); x != end; ++x)
+                          {
+                              const auto orig_id = remaining_nodes[x].id;
+                              is_core_node[orig_node_id_from_new_node_id_map[orig_id]] = true;
+                          }
+                      });
+              }
+              else
+              {
+                  tbb::parallel_for(
+                      tbb::blocked_range<int>(0, remaining_nodes.size(), InitGrainSize),
+                      [this, &remaining_nodes](const tbb::blocked_range<int> &range)
+                      {
+                          for (int x = range.begin(), end = range.end(); x != end; ++x)
+                          {
+                              const auto orig_id = remaining_nodes[x].id;
+                              is_core_node[orig_id] = true;
+                          }
+                      });
+              }
         }
         else
         {
@@ -563,7 +650,8 @@ class Contractor
             is_core_node.clear();
         }
 
-        SimpleLogger().Write() << "[core] " << remaining_nodes.size() << " nodes " << contractor_graph->GetNumberOfEdges() << " edges." << std::endl;
+        SimpleLogger().Write() << "[core] " << remaining_nodes.size() << " nodes "
+                               << contractor_graph->GetNumberOfEdges() << " edges." << std::endl;
 
         thread_data_list.data.clear();
     }
@@ -573,6 +661,11 @@ class Contractor
         out_is_core_node.swap(is_core_node);
     }
 
+    inline void GetNodeLevels(std::vector<float> &out_node_levels)
+    {
+        out_node_levels.swap(node_levels);
+    }
+
     template <class Edge> inline void GetEdges(DeallocatingVector<Edge> &edges)
     {
         Percent p(contractor_graph->GetNumberOfNodes());
@@ -982,6 +1075,7 @@ class Contractor
     std::shared_ptr<ContractorGraph> contractor_graph;
     stxxl::vector<QueryEdge> external_edge_list;
     std::vector<NodeID> orig_node_id_from_new_node_id_map;
+    std::vector<float> node_levels;
     std::vector<bool> is_core_node;
     XORFastHash fast_hash;
 };
diff --git a/contractor/contractor_options.cpp b/contractor/contractor_options.cpp
index f0d434a..d483465 100644
--- a/contractor/contractor_options.cpp
+++ b/contractor/contractor_options.cpp
@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "contractor_options.hpp"
 
-#include "../util/git_sha.hpp"
+#include "util/version.hpp"
 #include "../util/simple_logger.hpp"
 
 #include <boost/filesystem.hpp>
@@ -48,19 +48,27 @@ ContractorOptions::ParseArguments(int argc, char *argv[], ContractorConfig &cont
     // declare a group of options that will be allowed both on command line and in config file
     boost::program_options::options_description config_options("Configuration");
     config_options.add_options()(
-        "restrictions,r",
-        boost::program_options::value<boost::filesystem::path>(&contractor_config.restrictions_path),
-        "Restrictions file in .osrm.restrictions format")(
-        "profile,p", boost::program_options::value<boost::filesystem::path>(&contractor_config.profile_path)
-                         ->default_value("profile.lua"),
+        "profile,p",
+        boost::program_options::value<boost::filesystem::path>(&contractor_config.profile_path)
+            ->default_value("profile.lua"),
         "Path to LUA routing profile")(
-        "threads,t", boost::program_options::value<unsigned int>(&contractor_config.requested_num_threads)
-                         ->default_value(tbb::task_scheduler_init::default_num_threads()),
+        "threads,t",
+        boost::program_options::value<unsigned int>(&contractor_config.requested_num_threads)
+            ->default_value(tbb::task_scheduler_init::default_num_threads()),
         "Number of threads to use")(
 		"core,k", boost::program_options::value<double>(&contractor_config.core_factor)
-						 ->default_value(1.0),"Percentage of the graph (in vertices) to contract [0.1]");
-
-
+						 ->default_value(1.0),"Percentage of the graph (in vertices) to contract [0..1]")(
+		"segment-speed-file", boost::program_options::value<std::string>(&contractor_config.segment_speed_lookup_path),
+						 "Lookup file containing nodeA,nodeB,speed data to adjust edge weights")(
+        "level-cache,o",
+        boost::program_options::value<bool>(&contractor_config.use_cached_priority)->default_value(false),
+        "Use .level file to retain the contaction level for each node from the last run.");
+
+#ifdef DEBUG_GEOMETRY
+    config_options.add_options()(
+		"debug-geometry", boost::program_options::value<std::string>(&contractor_config.debug_geometry_path)
+						 ,"Write out edge-weight debugging geometry data in GeoJSON format to this file");
+#endif
 
     // hidden options, will be allowed both on command line and in config file, but will not be
     // shown to the user
@@ -102,7 +110,7 @@ ContractorOptions::ParseArguments(int argc, char *argv[], ContractorConfig &cont
 
     if (option_variables.count("version"))
     {
-        SimpleLogger().Write() << g_GIT_DESCRIPTION;
+        SimpleLogger().Write() << OSRM_VERSION;
         return return_code::exit;
     }
 
@@ -114,11 +122,6 @@ ContractorOptions::ParseArguments(int argc, char *argv[], ContractorConfig &cont
 
     boost::program_options::notify(option_variables);
 
-    if (!option_variables.count("restrictions"))
-    {
-        contractor_config.restrictions_path = contractor_config.osrm_input_path.string() + ".restrictions";
-    }
-
     if (!option_variables.count("input"))
     {
         SimpleLogger().Write() << "\n" << visible_options;
@@ -130,11 +133,10 @@ ContractorOptions::ParseArguments(int argc, char *argv[], ContractorConfig &cont
 
 void ContractorOptions::GenerateOutputFilesNames(ContractorConfig &contractor_config)
 {
-    contractor_config.node_output_path = contractor_config.osrm_input_path.string() + ".nodes";
+    contractor_config.level_output_path = contractor_config.osrm_input_path.string() + ".level";
     contractor_config.core_output_path = contractor_config.osrm_input_path.string() + ".core";
-    contractor_config.edge_output_path = contractor_config.osrm_input_path.string() + ".edges";
-    contractor_config.geometry_output_path = contractor_config.osrm_input_path.string() + ".geometry";
     contractor_config.graph_output_path = contractor_config.osrm_input_path.string() + ".hsgr";
-    contractor_config.rtree_nodes_output_path = contractor_config.osrm_input_path.string() + ".ramIndex";
-    contractor_config.rtree_leafs_output_path = contractor_config.osrm_input_path.string() + ".fileIndex";
+    contractor_config.edge_based_graph_path = contractor_config.osrm_input_path.string() + ".ebg";
+    contractor_config.edge_segment_lookup_path = contractor_config.osrm_input_path.string() + ".edge_segment_lookup";
+    contractor_config.edge_penalty_path = contractor_config.osrm_input_path.string() + ".edge_penalties";
 }
diff --git a/contractor/contractor_options.hpp b/contractor/contractor_options.hpp
index 3836627..5932b78 100644
--- a/contractor/contractor_options.hpp
+++ b/contractor/contractor_options.hpp
@@ -45,16 +45,16 @@ struct ContractorConfig
 
     boost::filesystem::path config_file_path;
     boost::filesystem::path osrm_input_path;
-    boost::filesystem::path restrictions_path;
     boost::filesystem::path profile_path;
 
-    std::string node_output_path;
+    std::string level_output_path;
     std::string core_output_path;
-    std::string edge_output_path;
-    std::string geometry_output_path;
     std::string graph_output_path;
-    std::string rtree_nodes_output_path;
-    std::string rtree_leafs_output_path;
+    std::string edge_based_graph_path;
+
+    std::string edge_segment_lookup_path;
+    std::string edge_penalty_path;
+    bool use_cached_priority;
 
     unsigned requested_num_threads;
 
@@ -63,6 +63,12 @@ struct ContractorConfig
     //The remaining vertices form the core of the hierarchy 
     //(e.g. 0.8 contracts 80 percent of the hierarchy, leaving a core of 20%)
     double core_factor;
+
+    std::string segment_speed_lookup_path;
+
+#ifdef DEBUG_GEOMETRY
+    std::string debug_geometry_path;
+#endif
 };
 
 struct ContractorOptions
diff --git a/contractor/processing_chain.cpp b/contractor/processing_chain.cpp
index 574aea7..2bd5350 100644
--- a/contractor/processing_chain.cpp
+++ b/contractor/processing_chain.cpp
@@ -26,17 +26,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "processing_chain.hpp"
+#include "contractor.hpp"
 
 #include "contractor.hpp"
-#include "../algorithms/graph_compressor.hpp"
-#include "../algorithms/tarjan_scc.hpp"
-#include "../algorithms/crc32_processor.hpp"
-#include "../data_structures/compressed_edge_container.hpp"
+
 #include "../data_structures/deallocating_vector.hpp"
-#include "../data_structures/static_rtree.hpp"
-#include "../data_structures/restriction_map.hpp"
 
-#include "../util/git_sha.hpp"
+#include "../algorithms/crc32_processor.hpp"
 #include "../util/graph_loader.hpp"
 #include "../util/integer_range.hpp"
 #include "../util/lua_util.hpp"
@@ -46,6 +42,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "../util/timing_util.hpp"
 #include "../typedefs.h"
 
+#include <fast-cpp-csv-parser/csv.h>
+
 #include <boost/filesystem/fstream.hpp>
 #include <boost/program_options.hpp>
 
@@ -57,6 +55,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <thread>
 #include <vector>
 
+#include "../util/debug_geometry.hpp"
+
 Prepare::~Prepare() {}
 
 int Prepare::Run()
@@ -70,58 +70,49 @@ int Prepare::Run()
                   "changing EdgeBasedEdge type has influence on memory consumption!");
 #endif
 
+    if (config.core_factor > 1.0 || config.core_factor < 0) 
+    {
+       throw osrm::exception("Core factor must be between 0.0 to 1.0 (inclusive)");
+    }
+
     TIMER_START(preparing);
 
     // Create a new lua state
 
-    SimpleLogger().Write() << "Generating edge-expanded graph representation";
-
-    TIMER_START(expansion);
+    SimpleLogger().Write() << "Loading edge-expanded graph representation";
 
-    std::vector<EdgeBasedNode> node_based_edge_list;
     DeallocatingVector<EdgeBasedEdge> edge_based_edge_list;
-    std::vector<QueryNode> internal_to_external_node_map;
-    auto graph_size = BuildEdgeExpandedGraph(internal_to_external_node_map, node_based_edge_list,
-                                             edge_based_edge_list);
-
-    auto number_of_node_based_nodes = graph_size.first;
-    auto max_edge_id = graph_size.second;
-
-    TIMER_STOP(expansion);
-
-    SimpleLogger().Write() << "building r-tree ...";
-    TIMER_START(rtree);
-
-    FindComponents(max_edge_id, edge_based_edge_list, node_based_edge_list);
-
-    BuildRTree(node_based_edge_list, internal_to_external_node_map);
-
-    TIMER_STOP(rtree);
 
-    SimpleLogger().Write() << "writing node map ...";
-    WriteNodeMapping(internal_to_external_node_map);
+    size_t max_edge_id = LoadEdgeExpandedGraph(
+        config.edge_based_graph_path, edge_based_edge_list, config.edge_segment_lookup_path,
+        config.edge_penalty_path, config.segment_speed_lookup_path);
 
     // Contracting the edge-expanded graph
 
     TIMER_START(contraction);
     std::vector<bool> is_core_node;
+    std::vector<float> node_levels;
+    if (config.use_cached_priority)
+    {
+        ReadNodeLevels(node_levels);
+    }
     DeallocatingVector<QueryEdge> contracted_edge_list;
-    ContractGraph(max_edge_id, edge_based_edge_list, contracted_edge_list, is_core_node);
+    ContractGraph(max_edge_id, edge_based_edge_list, contracted_edge_list, is_core_node,
+                  node_levels);
     TIMER_STOP(contraction);
 
     SimpleLogger().Write() << "Contraction took " << TIMER_SEC(contraction) << " sec";
 
-    std::size_t number_of_used_edges =
-        WriteContractedGraph(max_edge_id, node_based_edge_list, contracted_edge_list);
+    std::size_t number_of_used_edges = WriteContractedGraph(max_edge_id, contracted_edge_list);
     WriteCoreNodeMarker(std::move(is_core_node));
+    if (!config.use_cached_priority)
+    {
+        WriteNodeLevels(std::move(node_levels));
+    }
 
     TIMER_STOP(preparing);
 
     SimpleLogger().Write() << "Preprocessing : " << TIMER_SEC(preparing) << " seconds";
-    SimpleLogger().Write() << "Expansion  : " << (number_of_node_based_nodes / TIMER_SEC(expansion))
-                           << " nodes/sec and " << ((max_edge_id + 1) / TIMER_SEC(expansion))
-                           << " edges/sec";
-
     SimpleLogger().Write() << "Contraction: " << ((max_edge_id + 1) / TIMER_SEC(contraction))
                            << " nodes/sec and " << number_of_used_edges / TIMER_SEC(contraction)
                            << " edges/sec";
@@ -131,85 +122,179 @@ int Prepare::Run()
     return 0;
 }
 
-void Prepare::FindComponents(unsigned max_edge_id,
-                             const DeallocatingVector<EdgeBasedEdge> &input_edge_list,
-                             std::vector<EdgeBasedNode> &input_nodes) const
+namespace std
 {
-    struct UncontractedEdgeData
-    {
-    };
-    struct InputEdge
+
+template <> struct hash<std::pair<OSMNodeID, OSMNodeID>>
+{
+    std::size_t operator()(const std::pair<OSMNodeID, OSMNodeID> &k) const
     {
-        unsigned source;
-        unsigned target;
-        UncontractedEdgeData data;
+        return OSMNodeID_to_uint64_t(k.first) ^ (OSMNodeID_to_uint64_t(k.second) << 12);
+    }
+};
+}
 
-        bool operator<(const InputEdge &rhs) const
-        {
-            return source < rhs.source || (source == rhs.source && target < rhs.target);
-        }
+std::size_t Prepare::LoadEdgeExpandedGraph(std::string const &edge_based_graph_filename,
+                                           DeallocatingVector<EdgeBasedEdge> &edge_based_edge_list,
+                                           const std::string &edge_segment_lookup_filename,
+                                           const std::string &edge_penalty_filename,
+                                           const std::string &segment_speed_filename)
+{
+    SimpleLogger().Write() << "Opening " << edge_based_graph_filename;
+    boost::filesystem::ifstream input_stream(edge_based_graph_filename, std::ios::binary);
 
-        bool operator==(const InputEdge &rhs) const
-        {
-            return source == rhs.source && target == rhs.target;
-        }
-    };
-    using UncontractedGraph = StaticGraph<UncontractedEdgeData>;
-    std::vector<InputEdge> edges;
-    edges.reserve(input_edge_list.size() * 2);
+    const bool update_edge_weights = segment_speed_filename != "";
 
-    for (const auto &edge : input_edge_list)
+    boost::filesystem::ifstream edge_segment_input_stream;
+    boost::filesystem::ifstream edge_fixed_penalties_input_stream;
+
+    if (update_edge_weights)
     {
-        BOOST_ASSERT_MSG(static_cast<unsigned int>(std::max(edge.weight, 1)) > 0,
-                         "edge distance < 1");
-        if (edge.forward)
+        edge_segment_input_stream.open(edge_segment_lookup_filename, std::ios::binary);
+        edge_fixed_penalties_input_stream.open(edge_penalty_filename, std::ios::binary);
+        if (!edge_segment_input_stream || !edge_fixed_penalties_input_stream)
         {
-            edges.push_back({edge.source, edge.target, {}});
+            throw osrm::exception("Could not load .edge_segment_lookup or .edge_penalties, did you "
+                                  "run osrm-extract with '--generate-edge-lookup'?");
         }
+    }
+
+    const FingerPrint fingerprint_valid = FingerPrint::GetValid();
+    FingerPrint fingerprint_loaded;
+    input_stream.read((char *)&fingerprint_loaded, sizeof(FingerPrint));
+    fingerprint_loaded.TestPrepare(fingerprint_valid);
+
+    size_t number_of_edges = 0;
+    size_t max_edge_id = SPECIAL_EDGEID;
+    input_stream.read((char *)&number_of_edges, sizeof(size_t));
+    input_stream.read((char *)&max_edge_id, sizeof(size_t));
 
-        if (edge.backward)
+    edge_based_edge_list.resize(number_of_edges);
+    SimpleLogger().Write() << "Reading " << number_of_edges << " edges from the edge based graph";
+
+    std::unordered_map<std::pair<OSMNodeID, OSMNodeID>, unsigned> segment_speed_lookup;
+
+    if (update_edge_weights)
+    {
+        SimpleLogger().Write() << "Segment speed data supplied, will update edge weights from "
+                               << segment_speed_filename;
+        io::CSVReader<3> csv_in(segment_speed_filename);
+        csv_in.set_header("from_node", "to_node", "speed");
+        uint64_t from_node_id;
+        uint64_t to_node_id;
+        unsigned speed;
+        while (csv_in.read_row(from_node_id, to_node_id, speed))
         {
-            edges.push_back({edge.target, edge.source, {}});
+            segment_speed_lookup[std::make_pair(OSMNodeID(from_node_id), OSMNodeID(to_node_id))] = speed;
         }
     }
 
-    // connect forward and backward nodes of each edge
-    for (const auto &node : input_nodes)
+    DEBUG_GEOMETRY_START(config);
+
+    // TODO: can we read this in bulk?  DeallocatingVector isn't necessarily
+    // all stored contiguously
+    for (; number_of_edges > 0; --number_of_edges)
     {
-        if (node.reverse_edge_based_node_id != SPECIAL_NODEID)
+        EdgeBasedEdge inbuffer;
+        input_stream.read((char *) &inbuffer, sizeof(EdgeBasedEdge));
+
+        if (update_edge_weights)
         {
-            edges.push_back({node.forward_edge_based_node_id, node.reverse_edge_based_node_id, {}});
-            edges.push_back({node.reverse_edge_based_node_id, node.forward_edge_based_node_id, {}});
+            // Processing-time edge updates
+            unsigned fixed_penalty;
+            edge_fixed_penalties_input_stream.read(reinterpret_cast<char *>(&fixed_penalty),
+                                                   sizeof(fixed_penalty));
+
+            int new_weight = 0;
+
+            unsigned num_osm_nodes = 0;
+            edge_segment_input_stream.read(reinterpret_cast<char *>(&num_osm_nodes),
+                                           sizeof(num_osm_nodes));
+            OSMNodeID previous_osm_node_id;
+            edge_segment_input_stream.read(reinterpret_cast<char *>(&previous_osm_node_id),
+                                           sizeof(previous_osm_node_id));
+            OSMNodeID this_osm_node_id;
+            double segment_length;
+            int segment_weight;
+            --num_osm_nodes;
+            for (; num_osm_nodes != 0; --num_osm_nodes)
+            {
+                edge_segment_input_stream.read(reinterpret_cast<char *>(&this_osm_node_id),
+                                               sizeof(this_osm_node_id));
+                edge_segment_input_stream.read(reinterpret_cast<char *>(&segment_length),
+                                               sizeof(segment_length));
+                edge_segment_input_stream.read(reinterpret_cast<char *>(&segment_weight),
+                                               sizeof(segment_weight));
+
+                auto speed_iter = segment_speed_lookup.find(
+                    std::make_pair(previous_osm_node_id, this_osm_node_id));
+                if (speed_iter != segment_speed_lookup.end())
+                {
+                    // This sets the segment weight using the same formula as the
+                    // EdgeBasedGraphFactory for consistency.  The *why* of this formula
+                    // is lost in the annals of time.
+                    int new_segment_weight =
+                        std::max(1, static_cast<int>(std::floor(
+                                        (segment_length * 10.) / (speed_iter->second / 3.6) + .5)));
+                    new_weight += new_segment_weight;
+
+                    DEBUG_GEOMETRY_EDGE( 
+                            new_segment_weight, 
+                            segment_length,
+                            previous_osm_node_id,
+                            this_osm_node_id);
+                }
+                else
+                {
+                    // If no lookup found, use the original weight value for this segment
+                    new_weight += segment_weight;
+
+                    DEBUG_GEOMETRY_EDGE(
+                            segment_weight,
+                            segment_length,
+                            previous_osm_node_id,
+                            this_osm_node_id);
+                }
+
+                previous_osm_node_id = this_osm_node_id;
+            }
+
+            inbuffer.weight = fixed_penalty + new_weight;
         }
+
+        edge_based_edge_list.emplace_back(std::move(inbuffer));
     }
 
-    tbb::parallel_sort(edges.begin(), edges.end());
-    auto new_end = std::unique(edges.begin(), edges.end());
-    edges.resize(new_end - edges.begin());
+    DEBUG_GEOMETRY_STOP();
+    SimpleLogger().Write() << "Done reading edges";
+    return max_edge_id;
+}
 
-    auto uncontractor_graph = std::make_shared<UncontractedGraph>(max_edge_id + 1, edges);
+void Prepare::ReadNodeLevels(std::vector<float> &node_levels) const
+{
+    boost::filesystem::ifstream order_input_stream(config.level_output_path, std::ios::binary);
 
-    TarjanSCC<UncontractedGraph> component_search(
-        std::const_pointer_cast<const UncontractedGraph>(uncontractor_graph));
-    component_search.run();
+    unsigned level_size;
+    order_input_stream.read((char *)&level_size, sizeof(unsigned));
+    node_levels.resize(level_size);
+    order_input_stream.read((char *)node_levels.data(), sizeof(float) * node_levels.size());
+}
 
-    for (auto &node : input_nodes)
-    {
-        auto forward_component = component_search.get_component_id(node.forward_edge_based_node_id);
-        BOOST_ASSERT(node.reverse_edge_based_node_id == SPECIAL_EDGEID ||
-                     forward_component ==
-                         component_search.get_component_id(node.reverse_edge_based_node_id));
-
-        const unsigned component_size = component_search.get_component_size(forward_component);
-        const bool is_tiny_component = component_size < 1000;
-        node.component_id = is_tiny_component ? (1 + forward_component) : 0;
-    }
+void Prepare::WriteNodeLevels(std::vector<float> &&in_node_levels) const
+{
+    std::vector<float> node_levels(std::move(in_node_levels));
+
+    boost::filesystem::ofstream order_output_stream(config.level_output_path, std::ios::binary);
+
+    unsigned level_size = node_levels.size();
+    order_output_stream.write((char *)&level_size, sizeof(unsigned));
+    order_output_stream.write((char *)node_levels.data(), sizeof(float) * node_levels.size());
 }
 
 void Prepare::WriteCoreNodeMarker(std::vector<bool> &&in_is_core_node) const
 {
-    std::vector<bool> is_core_node(in_is_core_node);
-    std::vector<char> unpacked_bool_flags(is_core_node.size());
+    std::vector<bool> is_core_node(std::move(in_is_core_node));
+    std::vector<char> unpacked_bool_flags(std::move(is_core_node.size()));
     for (auto i = 0u; i < is_core_node.size(); ++i)
     {
         unpacked_bool_flags[i] = is_core_node[i] ? 1 : 0;
@@ -224,11 +309,8 @@ void Prepare::WriteCoreNodeMarker(std::vector<bool> &&in_is_core_node) const
 }
 
 std::size_t Prepare::WriteContractedGraph(unsigned max_node_id,
-                                          const std::vector<EdgeBasedNode> &node_based_edge_list,
                                           const DeallocatingVector<QueryEdge> &contracted_edge_list)
 {
-    const unsigned crc32_value = CalculateEdgeChecksum(node_based_edge_list);
-
     // Sorting contracted edges in a way that the static query graph can read some in in-place.
     tbb::parallel_sort(contracted_edge_list.begin(), contracted_edge_list.end());
     const unsigned contracted_edge_count = contracted_edge_list.size();
@@ -284,9 +366,13 @@ std::size_t Prepare::WriteContractedGraph(unsigned max_node_id,
 
     SimpleLogger().Write() << "Serializing node array";
 
+    RangebasedCRC32 crc32_calculator;
+    const unsigned edges_crc32 = crc32_calculator(contracted_edge_list);
+    SimpleLogger().Write() << "Writing CRC32: " << edges_crc32;
+
     const unsigned node_array_size = node_array.size();
     // serialize crc32, aka checksum
-    hsgr_output_stream.write((char *)&crc32_value, sizeof(unsigned));
+    hsgr_output_stream.write((char *)&edges_crc32, sizeof(unsigned));
     // serialize number of nodes
     hsgr_output_stream.write((char *)&node_array_size, sizeof(unsigned));
     // serialize number of edges
@@ -335,205 +421,21 @@ std::size_t Prepare::WriteContractedGraph(unsigned max_node_id,
     return number_of_used_edges;
 }
 
-unsigned Prepare::CalculateEdgeChecksum(const std::vector<EdgeBasedNode> &node_based_edge_list)
-{
-    RangebasedCRC32 crc32;
-    if (crc32.using_hardware())
-    {
-        SimpleLogger().Write() << "using hardware based CRC32 computation";
-    }
-    else
-    {
-        SimpleLogger().Write() << "using software based CRC32 computation";
-    }
-
-    const unsigned crc32_value = crc32(node_based_edge_list);
-    SimpleLogger().Write() << "CRC32: " << crc32_value;
-
-    return crc32_value;
-}
-
-/**
-    \brief Setups scripting environment (lua-scripting)
-    Also initializes speed profile.
-*/
-void Prepare::SetupScriptingEnvironment(lua_State *lua_state, SpeedProfileProperties &speed_profile)
-{
-    // open utility libraries string library;
-    luaL_openlibs(lua_state);
-
-    // adjust lua load path
-    luaAddScriptFolderToLoadPath(lua_state, config.profile_path.string().c_str());
-
-    // Now call our function in a lua script
-    if (0 != luaL_dofile(lua_state, config.profile_path.string().c_str()))
-    {
-        std::stringstream msg;
-        msg << lua_tostring(lua_state, -1) << " occured in scripting block";
-        throw osrm::exception(msg.str());
-    }
-
-    if (0 != luaL_dostring(lua_state, "return traffic_signal_penalty\n"))
-    {
-        std::stringstream msg;
-        msg << lua_tostring(lua_state, -1) << " occured in scripting block";
-        throw osrm::exception(msg.str());
-    }
-    speed_profile.traffic_signal_penalty = 10 * lua_tointeger(lua_state, -1);
-    SimpleLogger().Write(logDEBUG)
-        << "traffic_signal_penalty: " << speed_profile.traffic_signal_penalty;
-
-    if (0 != luaL_dostring(lua_state, "return u_turn_penalty\n"))
-    {
-        std::stringstream msg;
-        msg << lua_tostring(lua_state, -1) << " occured in scripting block";
-        throw osrm::exception(msg.str());
-    }
-
-    speed_profile.u_turn_penalty = 10 * lua_tointeger(lua_state, -1);
-    speed_profile.has_turn_penalty_function = lua_function_exists(lua_state, "turn_function");
-}
-
-/**
-  \brief Build load restrictions from .restriction file
-  */
-std::shared_ptr<RestrictionMap> Prepare::LoadRestrictionMap()
-{
-    boost::filesystem::ifstream input_stream(config.restrictions_path,
-                                             std::ios::in | std::ios::binary);
-
-    std::vector<TurnRestriction> restriction_list;
-    loadRestrictionsFromFile(input_stream, restriction_list);
-
-    SimpleLogger().Write() << " - " << restriction_list.size() << " restrictions.";
-
-    return std::make_shared<RestrictionMap>(restriction_list);
-}
-
-/**
-  \brief Load node based graph from .osrm file
-  */
-std::shared_ptr<NodeBasedDynamicGraph>
-Prepare::LoadNodeBasedGraph(std::unordered_set<NodeID> &barrier_nodes,
-                            std::unordered_set<NodeID> &traffic_lights,
-                            std::vector<QueryNode> &internal_to_external_node_map)
-{
-    std::vector<NodeBasedEdge> edge_list;
-
-    boost::filesystem::ifstream input_stream(config.osrm_input_path,
-                                             std::ios::in | std::ios::binary);
-
-    std::vector<NodeID> barrier_list;
-    std::vector<NodeID> traffic_light_list;
-    NodeID number_of_node_based_nodes = loadNodesFromFile(
-        input_stream, barrier_list, traffic_light_list, internal_to_external_node_map);
-
-    SimpleLogger().Write() << " - " << barrier_list.size() << " bollard nodes, "
-                           << traffic_light_list.size() << " traffic lights";
-
-    // insert into unordered sets for fast lookup
-    barrier_nodes.insert(barrier_list.begin(), barrier_list.end());
-    traffic_lights.insert(traffic_light_list.begin(), traffic_light_list.end());
-
-    barrier_list.clear();
-    barrier_list.shrink_to_fit();
-    traffic_light_list.clear();
-    traffic_light_list.shrink_to_fit();
-
-    loadEdgesFromFile(input_stream, edge_list);
-
-    if (edge_list.empty())
-    {
-        SimpleLogger().Write(logWARNING) << "The input data is empty, exiting.";
-        return std::shared_ptr<NodeBasedDynamicGraph>();
-    }
-
-    return NodeBasedDynamicGraphFromEdges(number_of_node_based_nodes, edge_list);
-}
-
-/**
- \brief Building an edge-expanded graph from node-based input and turn restrictions
-*/
-std::pair<std::size_t, std::size_t>
-Prepare::BuildEdgeExpandedGraph(std::vector<QueryNode> &internal_to_external_node_map,
-                                std::vector<EdgeBasedNode> &node_based_edge_list,
-                                DeallocatingVector<EdgeBasedEdge> &edge_based_edge_list)
-{
-    lua_State *lua_state = luaL_newstate();
-    luabind::open(lua_state);
-
-    SpeedProfileProperties speed_profile;
-    SetupScriptingEnvironment(lua_state, speed_profile);
-
-    std::unordered_set<NodeID> barrier_nodes;
-    std::unordered_set<NodeID> traffic_lights;
-
-    auto restriction_map = LoadRestrictionMap();
-    auto node_based_graph =
-        LoadNodeBasedGraph(barrier_nodes, traffic_lights, internal_to_external_node_map);
-
-    CompressedEdgeContainer compressed_edge_container;
-    GraphCompressor graph_compressor(speed_profile);
-    graph_compressor.Compress(barrier_nodes, traffic_lights, *restriction_map, *node_based_graph,
-                              compressed_edge_container);
-
-    EdgeBasedGraphFactory edge_based_graph_factory(
-        node_based_graph, compressed_edge_container, barrier_nodes, traffic_lights,
-        std::const_pointer_cast<RestrictionMap const>(restriction_map),
-        internal_to_external_node_map, speed_profile);
-
-    compressed_edge_container.SerializeInternalVector(config.geometry_output_path);
-
-    edge_based_graph_factory.Run(config.edge_output_path, lua_state);
-    lua_close(lua_state);
-
-    edge_based_graph_factory.GetEdgeBasedEdges(edge_based_edge_list);
-    edge_based_graph_factory.GetEdgeBasedNodes(node_based_edge_list);
-    auto max_edge_id = edge_based_graph_factory.GetHighestEdgeID();
-
-    const std::size_t number_of_node_based_nodes = node_based_graph->GetNumberOfNodes();
-    return std::make_pair(number_of_node_based_nodes, max_edge_id);
-}
-
 /**
  \brief Build contracted graph.
  */
 void Prepare::ContractGraph(const unsigned max_edge_id,
                             DeallocatingVector<EdgeBasedEdge> &edge_based_edge_list,
                             DeallocatingVector<QueryEdge> &contracted_edge_list,
-                            std::vector<bool> &is_core_node)
+                            std::vector<bool> &is_core_node,
+                            std::vector<float> &inout_node_levels) const
 {
-    Contractor contractor(max_edge_id + 1, edge_based_edge_list);
+    std::vector<float> node_levels;
+    node_levels.swap(inout_node_levels);
+
+    Contractor contractor(max_edge_id + 1, edge_based_edge_list, std::move(node_levels));
     contractor.Run(config.core_factor);
     contractor.GetEdges(contracted_edge_list);
     contractor.GetCoreMarker(is_core_node);
-}
-
-/**
-  \brief Writing info on original (node-based) nodes
- */
-void Prepare::WriteNodeMapping(const std::vector<QueryNode> &internal_to_external_node_map)
-{
-    boost::filesystem::ofstream node_stream(config.node_output_path, std::ios::binary);
-    const unsigned size_of_mapping = internal_to_external_node_map.size();
-    node_stream.write((char *)&size_of_mapping, sizeof(unsigned));
-    if (size_of_mapping > 0)
-    {
-        node_stream.write((char *)internal_to_external_node_map.data(),
-                          size_of_mapping * sizeof(QueryNode));
-    }
-    node_stream.close();
-}
-
-/**
-    \brief Building rtree-based nearest-neighbor data structure
-
-    Saves tree into '.ramIndex' and leaves into '.fileIndex'.
- */
-void Prepare::BuildRTree(const std::vector<EdgeBasedNode> &node_based_edge_list,
-                         const std::vector<QueryNode> &internal_to_external_node_map)
-{
-    StaticRTree<EdgeBasedNode>(node_based_edge_list, config.rtree_nodes_output_path.c_str(),
-                               config.rtree_leafs_output_path.c_str(),
-                               internal_to_external_node_map);
+    contractor.GetNodeLevels(inout_node_levels);
 }
diff --git a/contractor/processing_chain.hpp b/contractor/processing_chain.hpp
index 4e12baf..0eb6555 100644
--- a/contractor/processing_chain.hpp
+++ b/contractor/processing_chain.hpp
@@ -28,10 +28,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef PROCESSING_CHAIN_HPP
 #define PROCESSING_CHAIN_HPP
 
+#include "contractor.hpp"
 #include "contractor_options.hpp"
-#include "edge_based_graph_factory.hpp"
 #include "../data_structures/query_edge.hpp"
 #include "../data_structures/static_graph.hpp"
+#include "../data_structures/deallocating_vector.hpp"
+#include "../data_structures/node_based_graph.hpp"
 
 struct SpeedProfileProperties;
 struct EdgeBasedNode;
@@ -48,8 +50,6 @@ class Prepare
 {
   public:
     using EdgeData = QueryEdge::EdgeData;
-    using InputEdge = DynamicGraph<EdgeData>::InputEdge;
-    using StaticEdge = StaticGraph<EdgeData>::InputEdge;
 
     explicit Prepare(ContractorConfig contractor_config) : config(std::move(contractor_config)) {}
     Prepare(const Prepare &) = delete;
@@ -58,34 +58,27 @@ class Prepare
     int Run();
 
   protected:
-    void SetupScriptingEnvironment(lua_State *myLuaState, SpeedProfileProperties &speed_profile);
-    unsigned CalculateEdgeChecksum(const std::vector<EdgeBasedNode> &node_based_edge_list);
     void ContractGraph(const unsigned max_edge_id,
                        DeallocatingVector<EdgeBasedEdge> &edge_based_edge_list,
                        DeallocatingVector<QueryEdge> &contracted_edge_list,
-                       std::vector<bool> &is_core_node);
+                       std::vector<bool> &is_core_node,
+                       std::vector<float> &node_levels) const;
     void WriteCoreNodeMarker(std::vector<bool> &&is_core_node) const;
+    void WriteNodeLevels(std::vector<float> &&node_levels) const;
+    void ReadNodeLevels(std::vector<float> &contraction_order) const;
     std::size_t WriteContractedGraph(unsigned number_of_edge_based_nodes,
-                                     const std::vector<EdgeBasedNode> &node_based_edge_list,
                                      const DeallocatingVector<QueryEdge> &contracted_edge_list);
-    std::shared_ptr<RestrictionMap> LoadRestrictionMap();
-    std::shared_ptr<NodeBasedDynamicGraph>
-    LoadNodeBasedGraph(std::unordered_set<NodeID> &barrier_nodes,
-                       std::unordered_set<NodeID> &traffic_lights,
-                       std::vector<QueryNode> &internal_to_external_node_map);
-    std::pair<std::size_t, std::size_t>
-    BuildEdgeExpandedGraph(std::vector<QueryNode> &internal_to_external_node_map,
-                           std::vector<EdgeBasedNode> &node_based_edge_list,
-                           DeallocatingVector<EdgeBasedEdge> &edge_based_edge_list);
-    void WriteNodeMapping(const std::vector<QueryNode> &internal_to_external_node_map);
     void FindComponents(unsigned max_edge_id,
                         const DeallocatingVector<EdgeBasedEdge> &edges,
                         std::vector<EdgeBasedNode> &nodes) const;
-    void BuildRTree(const std::vector<EdgeBasedNode> &node_based_edge_list,
-                    const std::vector<QueryNode> &internal_to_external_node_map);
 
   private:
     ContractorConfig config;
+    std::size_t LoadEdgeExpandedGraph(const std::string &edge_based_graph_path,
+                                      DeallocatingVector<EdgeBasedEdge> &edge_based_edge_list,
+                                      const std::string &edge_segment_lookup_path,
+                                      const std::string &edge_penalty_path,
+                                      const std::string &segment_speed_path);
 };
 
 #endif // PROCESSING_CHAIN_HPP
diff --git a/data_structures/concurrent_queue.hpp b/data_structures/concurrent_queue.hpp
deleted file mode 100644
index 7341a8c..0000000
--- a/data_structures/concurrent_queue.hpp
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
-
-Copyright (c) 2014, Project OSRM contributors
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without modification,
-are permitted provided that the following conditions are met:
-
-Redistributions of source code must retain the above copyright notice, this list
-of conditions and the following disclaimer.
-Redistributions in binary form must reproduce the above copyright notice, this
-list of conditions and the following disclaimer in the documentation and/or
-other materials provided with the distribution.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
-ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
-ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-*/
-
-#ifndef CONCURRENT_QUEUE_HPP
-#define CONCURRENT_QUEUE_HPP
-
-#include <boost/circular_buffer.hpp>
-#include <condition_variable>
-#include <mutex>
-
-template <typename Data> class ConcurrentQueue
-{
-  public:
-    explicit ConcurrentQueue(const size_t max_size) : m_internal_queue(max_size) {}
-
-    inline void push(const Data &data)
-    {
-        std::unique_lock<std::mutex> lock(m_mutex);
-        m_not_full.wait(lock, [this]
-                        {
-                            return m_internal_queue.size() < m_internal_queue.capacity();
-                        });
-        m_internal_queue.push_back(data);
-        m_not_empty.notify_one();
-    }
-
-    inline bool empty() const { return m_internal_queue.empty(); }
-
-    inline void wait_and_pop(Data &popped_value)
-    {
-        std::unique_lock<std::mutex> lock(m_mutex);
-        m_not_empty.wait(lock, [this]
-                         {
-                             return !m_internal_queue.empty();
-                         });
-        popped_value = m_internal_queue.front();
-        m_internal_queue.pop_front();
-        m_not_full.notify_one();
-    }
-
-    inline bool try_pop(Data &popped_value)
-    {
-        std::unique_lock<std::mutex> lock(m_mutex);
-        if (m_internal_queue.empty())
-        {
-            return false;
-        }
-        popped_value = m_internal_queue.front();
-        m_internal_queue.pop_front();
-        m_not_full.notify_one();
-        return true;
-    }
-
-  private:
-    boost::circular_buffer<Data> m_internal_queue;
-    std::mutex m_mutex;
-    std::condition_variable m_not_empty;
-    std::condition_variable m_not_full;
-};
-
-#endif // CONCURRENT_QUEUE_HPP
diff --git a/data_structures/deallocating_vector.hpp b/data_structures/deallocating_vector.hpp
index 51bebb9..72cb081 100644
--- a/data_structures/deallocating_vector.hpp
+++ b/data_structures/deallocating_vector.hpp
@@ -237,6 +237,12 @@ class DeallocatingVectorRemoveIterator
     }
 };
 
+template <typename ElementT, std::size_t ELEMENTS_PER_BLOCK>
+class DeallocatingVector;
+
+template<typename T, std::size_t S>
+void swap(DeallocatingVector<T, S>& lhs, DeallocatingVector<T, S>& rhs);
+
 template <typename ElementT, std::size_t ELEMENTS_PER_BLOCK = 8388608 / sizeof(ElementT)>
 class DeallocatingVector
 {
@@ -257,6 +263,8 @@ class DeallocatingVector
 
     ~DeallocatingVector() { clear(); }
 
+    friend void swap<>(DeallocatingVector<ElementT, ELEMENTS_PER_BLOCK>& lhs, DeallocatingVector<ElementT, ELEMENTS_PER_BLOCK>& rhs);
+
     void swap(DeallocatingVector<ElementT, ELEMENTS_PER_BLOCK> &other)
     {
         std::swap(current_size, other.current_size);
@@ -386,4 +394,10 @@ class DeallocatingVector
     }
 };
 
+template<typename T, std::size_t S>
+void swap(DeallocatingVector<T, S>& lhs, DeallocatingVector<T, S>& rhs)
+{
+    lhs.swap(rhs);
+}
+
 #endif /* DEALLOCATING_VECTOR_HPP */
diff --git a/data_structures/edge_based_node.hpp b/data_structures/edge_based_node.hpp
index 027644d..8efbf01 100644
--- a/data_structures/edge_based_node.hpp
+++ b/data_structures/edge_based_node.hpp
@@ -46,7 +46,7 @@ struct EdgeBasedNode
           u(SPECIAL_NODEID), v(SPECIAL_NODEID), name_id(0),
           forward_weight(INVALID_EDGE_WEIGHT >> 1), reverse_weight(INVALID_EDGE_WEIGHT >> 1),
           forward_offset(0), reverse_offset(0), packed_geometry_id(SPECIAL_EDGEID),
-          component_id(-1), fwd_segment_position(std::numeric_limits<unsigned short>::max()),
+          component{INVALID_COMPONENTID, false}, fwd_segment_position(std::numeric_limits<unsigned short>::max()),
           forward_travel_mode(TRAVEL_MODE_INACCESSIBLE),
           backward_travel_mode(TRAVEL_MODE_INACCESSIBLE)
     {
@@ -62,6 +62,7 @@ struct EdgeBasedNode
                            int forward_offset,
                            int reverse_offset,
                            unsigned packed_geometry_id,
+                           bool is_tiny_component,
                            unsigned component_id,
                            unsigned short fwd_segment_position,
                            TravelMode forward_travel_mode,
@@ -70,7 +71,7 @@ struct EdgeBasedNode
           reverse_edge_based_node_id(reverse_edge_based_node_id), u(u), v(v), name_id(name_id),
           forward_weight(forward_weight), reverse_weight(reverse_weight),
           forward_offset(forward_offset), reverse_offset(reverse_offset),
-          packed_geometry_id(packed_geometry_id), component_id(component_id),
+          packed_geometry_id(packed_geometry_id), component{component_id, is_tiny_component},
           fwd_segment_position(fwd_segment_position), forward_travel_mode(forward_travel_mode),
           backward_travel_mode(backward_travel_mode)
     {
@@ -90,8 +91,6 @@ struct EdgeBasedNode
 
     bool IsCompressed() const { return packed_geometry_id != SPECIAL_EDGEID; }
 
-    bool is_in_tiny_cc() const { return 0 != component_id; }
-
     NodeID forward_edge_based_node_id; // needed for edge-expanded graph
     NodeID reverse_edge_based_node_id; // needed for edge-expanded graph
     NodeID u;                          // indices into the coordinates array
@@ -102,7 +101,10 @@ struct EdgeBasedNode
     int forward_offset;          // prefix sum of the weight up the edge TODO: short must suffice
     int reverse_offset;          // prefix sum of the weight from the edge TODO: short must suffice
     unsigned packed_geometry_id; // if set, then the edge represents a packed geometry
-    unsigned component_id;
+    struct {
+        unsigned id : 31;
+        bool is_tiny : 1;
+    } component;
     unsigned short fwd_segment_position; // segment id in a compressed geometry
     TravelMode forward_travel_mode : 4;
     TravelMode backward_travel_mode : 4;
diff --git a/data_structures/external_memory_node.cpp b/data_structures/external_memory_node.cpp
index 72b8198..d144f52 100644
--- a/data_structures/external_memory_node.cpp
+++ b/data_structures/external_memory_node.cpp
@@ -31,7 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <limits>
 
 ExternalMemoryNode::ExternalMemoryNode(
-    int lat, int lon, unsigned int node_id, bool barrier, bool traffic_lights)
+    int lat, int lon, OSMNodeID node_id, bool barrier, bool traffic_lights)
     : QueryNode(lat, lon, node_id), barrier(barrier), traffic_lights(traffic_lights)
 {
 }
@@ -40,13 +40,13 @@ ExternalMemoryNode::ExternalMemoryNode() : barrier(false), traffic_lights(false)
 
 ExternalMemoryNode ExternalMemoryNode::min_value()
 {
-    return ExternalMemoryNode(0, 0, 0, false, false);
+    return ExternalMemoryNode(0, 0, MIN_OSM_NODEID, false, false);
 }
 
 ExternalMemoryNode ExternalMemoryNode::max_value()
 {
     return ExternalMemoryNode(std::numeric_limits<int>::max(), std::numeric_limits<int>::max(),
-                              std::numeric_limits<unsigned>::max(), false, false);
+                              MAX_OSM_NODEID, false, false);
 }
 
 bool ExternalMemoryNodeSTXXLCompare::operator()(const ExternalMemoryNode &left,
diff --git a/data_structures/external_memory_node.hpp b/data_structures/external_memory_node.hpp
index 83b88e7..a48d1a1 100644
--- a/data_structures/external_memory_node.hpp
+++ b/data_structures/external_memory_node.hpp
@@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 struct ExternalMemoryNode : QueryNode
 {
-    ExternalMemoryNode(int lat, int lon, NodeID id, bool barrier, bool traffic_light);
+    ExternalMemoryNode(int lat, int lon, OSMNodeID id, bool barrier, bool traffic_light);
 
     ExternalMemoryNode();
 
diff --git a/data_structures/hidden_markov_model.hpp b/data_structures/hidden_markov_model.hpp
index 15aa35b..e3efcea 100644
--- a/data_structures/hidden_markov_model.hpp
+++ b/data_structures/hidden_markov_model.hpp
@@ -140,7 +140,7 @@ template <class CandidateLists> struct HiddenMarkovModel
             for (const auto s : osrm::irange<std::size_t>(0u, viterbi[initial_timestamp].size()))
             {
                 viterbi[initial_timestamp][s] =
-                    emission_log_probability(candidates_list[initial_timestamp][s].second);
+                    emission_log_probability(candidates_list[initial_timestamp][s].distance);
                 parents[initial_timestamp][s] = std::make_pair(initial_timestamp, s);
                 pruned[initial_timestamp][s] =
                     viterbi[initial_timestamp][s] < osrm::matching::MINIMAL_LOG_PROB;
diff --git a/data_structures/import_edge.cpp b/data_structures/import_edge.cpp
index 15394f1..bf2829d 100644
--- a/data_structures/import_edge.cpp
+++ b/data_structures/import_edge.cpp
@@ -50,7 +50,7 @@ bool NodeBasedEdge::operator<(const NodeBasedEdge &other) const
 NodeBasedEdge::NodeBasedEdge()
     : source(SPECIAL_NODEID), target(SPECIAL_NODEID), name_id(0), weight(0), forward(false),
       backward(false), roundabout(false),
-      access_restricted(false), is_split(false), travel_mode(false)
+      access_restricted(false), startpoint(true), is_split(false), travel_mode(false)
 {
 }
 
@@ -62,11 +62,12 @@ NodeBasedEdge::NodeBasedEdge(NodeID source,
                              bool backward,
                              bool roundabout,
                              bool access_restricted,
+                             bool startpoint,
                              TravelMode travel_mode,
                              bool is_split)
     : source(source), target(target), name_id(name_id), weight(weight), forward(forward),
       backward(backward), roundabout(roundabout),
-      access_restricted(access_restricted), is_split(is_split), travel_mode(travel_mode)
+      access_restricted(access_restricted), startpoint(startpoint), is_split(is_split), travel_mode(travel_mode)
 {
 }
 
diff --git a/data_structures/import_edge.hpp b/data_structures/import_edge.hpp
index 8c5325c..449ded2 100644
--- a/data_structures/import_edge.hpp
+++ b/data_structures/import_edge.hpp
@@ -44,6 +44,7 @@ struct NodeBasedEdge
                            bool backward,
                            bool roundabout,
                            bool access_restricted,
+                           bool startpoint,
                            TravelMode travel_mode,
                            bool is_split);
 
@@ -55,10 +56,31 @@ struct NodeBasedEdge
     bool backward : 1;
     bool roundabout : 1;
     bool access_restricted : 1;
+    bool startpoint : 1;
     bool is_split : 1;
     TravelMode travel_mode : 4;
 };
 
+struct NodeBasedEdgeWithOSM : NodeBasedEdge
+{
+    explicit NodeBasedEdgeWithOSM(OSMNodeID source,
+                           OSMNodeID target,
+                           NodeID name_id,
+                           EdgeWeight weight,
+                           bool forward,
+                           bool backward,
+                           bool roundabout,
+                           bool access_restricted,
+                           bool startpoint,
+                           TravelMode travel_mode,
+                           bool is_split)
+        : NodeBasedEdge(SPECIAL_NODEID, SPECIAL_NODEID, name_id, weight, forward, backward, roundabout, access_restricted, startpoint, travel_mode, is_split),
+        osm_source_id(source), osm_target_id(target) {}
+
+    OSMNodeID osm_source_id;
+    OSMNodeID osm_target_id;
+};
+
 struct EdgeBasedEdge
 {
 
diff --git a/data_structures/node_based_graph.hpp b/data_structures/node_based_graph.hpp
index efdacb9..e58cfce 100644
--- a/data_structures/node_based_graph.hpp
+++ b/data_structures/node_based_graph.hpp
@@ -47,10 +47,10 @@ struct NodeBasedEdgeData
 
     NodeBasedEdgeData(int distance, unsigned edge_id, unsigned name_id,
             bool access_restricted, bool reversed,
-            bool roundabout, TravelMode travel_mode)
+            bool roundabout, bool startpoint, TravelMode travel_mode)
         : distance(distance), edge_id(edge_id), name_id(name_id),
           access_restricted(access_restricted), reversed(reversed),
-          roundabout(roundabout), travel_mode(travel_mode)
+          roundabout(roundabout), startpoint(startpoint), travel_mode(travel_mode)
     {
     }
 
@@ -60,6 +60,7 @@ struct NodeBasedEdgeData
     bool access_restricted : 1;
     bool reversed : 1;
     bool roundabout : 1;
+    bool startpoint : 1;
     TravelMode travel_mode : 4;
 
     bool IsCompatibleTo(const NodeBasedEdgeData &other) const
@@ -72,10 +73,10 @@ struct NodeBasedEdgeData
 using NodeBasedDynamicGraph = DynamicGraph<NodeBasedEdgeData>;
 
 /// Factory method to create NodeBasedDynamicGraph from NodeBasedEdges
-/// The since DynamicGraph expects directed edges, we need to insert
+/// Since DynamicGraph expects directed edges, we need to insert
 /// two edges for undirected edges.
 inline std::shared_ptr<NodeBasedDynamicGraph>
-NodeBasedDynamicGraphFromEdges(int number_of_nodes, const std::vector<NodeBasedEdge> &input_edge_list)
+NodeBasedDynamicGraphFromEdges(std::size_t number_of_nodes, const std::vector<NodeBasedEdge> &input_edge_list)
 {
     auto edges_list = directedEdgesFromCompressed<NodeBasedDynamicGraph::InputEdge>(input_edge_list,
         [](NodeBasedDynamicGraph::InputEdge& output_edge, const NodeBasedEdge& input_edge)
@@ -87,6 +88,7 @@ NodeBasedDynamicGraphFromEdges(int number_of_nodes, const std::vector<NodeBasedE
             output_edge.data.name_id = input_edge.name_id;
             output_edge.data.access_restricted = input_edge.access_restricted;
             output_edge.data.travel_mode = input_edge.travel_mode;
+            output_edge.data.startpoint = input_edge.startpoint;
         }
     );
 
diff --git a/data_structures/node_id.hpp b/data_structures/node_id.hpp
index 4d6fff0..0671a4d 100644
--- a/data_structures/node_id.hpp
+++ b/data_structures/node_id.hpp
@@ -32,10 +32,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 struct Cmp
 {
-    using value_type = NodeID;
-    bool operator()(const NodeID left, const NodeID right) const { return left < right; }
-    value_type max_value() { return 0xffffffff; }
-    value_type min_value() { return 0x0; }
+    using value_type = OSMNodeID;
+    bool operator()(const value_type left, const value_type right) const { return left < right; }
+    value_type max_value() { return MAX_OSM_NODEID; }
+    value_type min_value() { return MIN_OSM_NODEID; }
 };
 
 #endif // NODE_ID_HPP
diff --git a/data_structures/phantom_node.cpp b/data_structures/phantom_node.cpp
index eba6492..95c5bbb 100644
--- a/data_structures/phantom_node.cpp
+++ b/data_structures/phantom_node.cpp
@@ -42,6 +42,7 @@ PhantomNode::PhantomNode(NodeID forward_node_id,
                          int forward_offset,
                          int reverse_offset,
                          unsigned packed_geometry_id,
+                         bool is_tiny_component,
                          unsigned component_id,
                          FixedPointCoordinate &location,
                          unsigned short fwd_segment_position,
@@ -50,7 +51,7 @@ PhantomNode::PhantomNode(NodeID forward_node_id,
     : forward_node_id(forward_node_id), reverse_node_id(reverse_node_id), name_id(name_id),
       forward_weight(forward_weight), reverse_weight(reverse_weight),
       forward_offset(forward_offset), reverse_offset(reverse_offset),
-      packed_geometry_id(packed_geometry_id), component_id(component_id), location(location),
+      packed_geometry_id(packed_geometry_id), component{component_id, is_tiny_component}, location(location),
       fwd_segment_position(fwd_segment_position), forward_travel_mode(forward_travel_mode),
       backward_travel_mode(backward_travel_mode)
 {
@@ -60,7 +61,7 @@ PhantomNode::PhantomNode()
     : forward_node_id(SPECIAL_NODEID), reverse_node_id(SPECIAL_NODEID),
       name_id(std::numeric_limits<unsigned>::max()), forward_weight(INVALID_EDGE_WEIGHT),
       reverse_weight(INVALID_EDGE_WEIGHT), forward_offset(0), reverse_offset(0),
-      packed_geometry_id(SPECIAL_EDGEID), component_id(std::numeric_limits<unsigned>::max()),
+      packed_geometry_id(SPECIAL_EDGEID), component{INVALID_COMPONENTID, false},
       fwd_segment_position(0), forward_travel_mode(TRAVEL_MODE_INACCESSIBLE),
       backward_travel_mode(TRAVEL_MODE_INACCESSIBLE)
 {
@@ -96,11 +97,9 @@ bool PhantomNode::is_valid(const unsigned number_of_nodes) const
     return location.is_valid() &&
            ((forward_node_id < number_of_nodes) || (reverse_node_id < number_of_nodes)) &&
            ((forward_weight != INVALID_EDGE_WEIGHT) || (reverse_weight != INVALID_EDGE_WEIGHT)) &&
-           (name_id != INVALID_NAMEID);
+           (component.id != INVALID_COMPONENTID) && (name_id != INVALID_NAMEID);
 }
 
-bool PhantomNode::is_in_tiny_component() const { return component_id != 0; }
-
 bool PhantomNode::is_valid() const { return location.is_valid() && (name_id != INVALID_NAMEID); }
 
 bool PhantomNode::operator==(const PhantomNode &other) const { return location == other.location; }
diff --git a/data_structures/phantom_node.hpp b/data_structures/phantom_node.hpp
index 6704089..b12c4fe 100644
--- a/data_structures/phantom_node.hpp
+++ b/data_structures/phantom_node.hpp
@@ -47,6 +47,7 @@ struct PhantomNode
                 int forward_offset,
                 int reverse_offset,
                 unsigned packed_geometry_id,
+                bool is_tiny_component,
                 unsigned component_id,
                 FixedPointCoordinate &location,
                 unsigned short fwd_segment_position,
@@ -68,7 +69,9 @@ struct PhantomNode
         reverse_offset = other.reverse_offset;
 
         packed_geometry_id = other.packed_geometry_id;
-        component_id = other.component_id;
+
+        component.id = other.component.id;
+        component.is_tiny = other.component.is_tiny;
 
         location = foot_point;
         fwd_segment_position = other.fwd_segment_position;
@@ -85,7 +88,14 @@ struct PhantomNode
     int forward_offset;
     int reverse_offset;
     unsigned packed_geometry_id;
-    unsigned component_id;
+    struct ComponentType {
+        uint32_t id : 31;
+        bool is_tiny : 1;
+    } component;
+// bit-fields are broken on Windows
+#ifndef _MSC_VER
+    static_assert(sizeof(ComponentType) == 4, "ComponentType needs to 4 bytes big");
+#endif
     FixedPointCoordinate location;
     unsigned short fwd_segment_position;
     // note 4 bits would suffice for each,
@@ -105,23 +115,19 @@ struct PhantomNode
 
     bool is_valid() const;
 
-    bool is_in_tiny_component() const;
-
     bool operator==(const PhantomNode &other) const;
 };
 
+#ifndef _MSC_VER
 static_assert(sizeof(PhantomNode) == 48, "PhantomNode has more padding then expected");
+#endif
 
-using PhantomNodeArray = std::vector<std::vector<PhantomNode>>;
-
-class phantom_node_pair : public std::pair<PhantomNode, PhantomNode>
-{
-};
+using PhantomNodePair = std::pair<PhantomNode, PhantomNode>;
 
-struct PhantomNodeLists
+struct PhantomNodeWithDistance
 {
-    std::vector<PhantomNode> source_phantom_list;
-    std::vector<PhantomNode> target_phantom_list;
+    PhantomNode phantom_node;
+    double distance;
 };
 
 struct PhantomNodes
@@ -147,7 +153,7 @@ inline std::ostream &operator<<(std::ostream &out, const PhantomNode &pn)
         << "fwd-o: " << pn.forward_offset << ", "
         << "rev-o: " << pn.reverse_offset << ", "
         << "geom: " << pn.packed_geometry_id << ", "
-        << "comp: " << pn.component_id << ", "
+        << "comp: " << pn.component.is_tiny << " / " << pn.component.id << ", "
         << "pos: " << pn.fwd_segment_position << ", "
         << "loc: " << pn.location;
     return out;
diff --git a/data_structures/query_node.hpp b/data_structures/query_node.hpp
index f3e9904..0f32a53 100644
--- a/data_structures/query_node.hpp
+++ b/data_structures/query_node.hpp
@@ -38,32 +38,32 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 struct QueryNode
 {
-    using key_type = NodeID; // type of NodeID
+    using key_type = OSMNodeID; // type of NodeID
     using value_type = int;  // type of lat,lons
 
-    explicit QueryNode(int lat, int lon, NodeID node_id) : lat(lat), lon(lon), node_id(node_id) {}
+    explicit QueryNode(int lat, int lon, OSMNodeID node_id) : lat(lat), lon(lon), node_id(node_id) {}
     QueryNode()
         : lat(std::numeric_limits<int>::max()), lon(std::numeric_limits<int>::max()),
-          node_id(std::numeric_limits<unsigned>::max())
+          node_id(SPECIAL_OSM_NODEID)
     {
     }
 
     int lat;
     int lon;
-    NodeID node_id;
+    OSMNodeID node_id;
 
     static QueryNode min_value()
     {
         return QueryNode(static_cast<int>(-90 * COORDINATE_PRECISION),
                          static_cast<int>(-180 * COORDINATE_PRECISION),
-                         std::numeric_limits<NodeID>::min());
+                         MIN_OSM_NODEID);
     }
 
     static QueryNode max_value()
     {
         return QueryNode(static_cast<int>(90 * COORDINATE_PRECISION),
                          static_cast<int>(180 * COORDINATE_PRECISION),
-                         std::numeric_limits<NodeID>::max());
+                         MAX_OSM_NODEID);
     }
 
     value_type operator[](const std::size_t n) const
diff --git a/data_structures/range_table.hpp b/data_structures/range_table.hpp
index 4662f06..e750c90 100644
--- a/data_structures/range_table.hpp
+++ b/data_structures/range_table.hpp
@@ -33,9 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "shared_memory_vector_wrapper.hpp"
 
 #include <fstream>
-#include <vector>
 #include <array>
-
 /*
  * These pre-declarations are needed because parsing C++ is hard
  * and otherwise the compiler gets confused.
@@ -82,7 +80,8 @@ template <unsigned BLOCK_SIZE, bool USE_SHARED_MEMORY> class RangeTable
     }
 
     // construct table from length vector
-    explicit RangeTable(const std::vector<unsigned> &lengths)
+    template<typename VectorT>
+    explicit RangeTable(const VectorT &lengths)
     {
         const unsigned number_of_blocks = [&lengths]()
         {
diff --git a/data_structures/raster_source.cpp b/data_structures/raster_source.cpp
index f935b9d..9e880a2 100644
--- a/data_structures/raster_source.cpp
+++ b/data_structures/raster_source.cpp
@@ -42,8 +42,8 @@ RasterSource::RasterSource(RasterGrid _raster_data,
                            int _ymin,
                            int _ymax)
     : xstep(calcSize(_xmin, _xmax, _width)), ystep(calcSize(_ymin, _ymax, _height)),
-      raster_data(_raster_data), width(_width), height(_height), xmin(_xmin), xmax(_xmax),
-      ymin(_ymin), ymax(_ymax)
+      raster_data(std::move(_raster_data)), width(_width), height(_height), xmin(_xmin),
+      xmax(_xmax), ymin(_ymin), ymax(_ymax)
 {
     BOOST_ASSERT(xstep != 0);
     BOOST_ASSERT(ystep != 0);
diff --git a/data_structures/rectangle.hpp b/data_structures/rectangle.hpp
index 9066efc..7f6704a 100644
--- a/data_structures/rectangle.hpp
+++ b/data_structures/rectangle.hpp
@@ -121,35 +121,35 @@ struct RectangleInt2D
         switch (d)
         {
         case NORTH:
-            min_dist = coordinate_calculation::euclidean_distance(
+            min_dist = coordinate_calculation::great_circle_distance(
                 location, FixedPointCoordinate(max_lat, location.lon));
             break;
         case SOUTH:
-            min_dist = coordinate_calculation::euclidean_distance(
+            min_dist = coordinate_calculation::great_circle_distance(
                 location, FixedPointCoordinate(min_lat, location.lon));
             break;
         case WEST:
-            min_dist = coordinate_calculation::euclidean_distance(
+            min_dist = coordinate_calculation::great_circle_distance(
                 location, FixedPointCoordinate(location.lat, min_lon));
             break;
         case EAST:
-            min_dist = coordinate_calculation::euclidean_distance(
+            min_dist = coordinate_calculation::great_circle_distance(
                 location, FixedPointCoordinate(location.lat, max_lon));
             break;
         case NORTH_EAST:
-            min_dist = coordinate_calculation::euclidean_distance(
+            min_dist = coordinate_calculation::great_circle_distance(
                 location, FixedPointCoordinate(max_lat, max_lon));
             break;
         case NORTH_WEST:
-            min_dist = coordinate_calculation::euclidean_distance(
+            min_dist = coordinate_calculation::great_circle_distance(
                 location, FixedPointCoordinate(max_lat, min_lon));
             break;
         case SOUTH_EAST:
-            min_dist = coordinate_calculation::euclidean_distance(
+            min_dist = coordinate_calculation::great_circle_distance(
                 location, FixedPointCoordinate(min_lat, max_lon));
             break;
         case SOUTH_WEST:
-            min_dist = coordinate_calculation::euclidean_distance(
+            min_dist = coordinate_calculation::great_circle_distance(
                 location, FixedPointCoordinate(min_lat, min_lon));
             break;
         default:
@@ -170,25 +170,25 @@ struct RectangleInt2D
         const FixedPointCoordinate lower_right(min_lat, max_lon);
         const FixedPointCoordinate lower_left(min_lat, min_lon);
 
-        min_max_dist =
-            std::min(min_max_dist,
-                     std::max(coordinate_calculation::euclidean_distance(location, upper_left),
-                              coordinate_calculation::euclidean_distance(location, upper_right)));
+        min_max_dist = std::min(
+            min_max_dist,
+            std::max(coordinate_calculation::great_circle_distance(location, upper_left),
+                     coordinate_calculation::great_circle_distance(location, upper_right)));
 
-        min_max_dist =
-            std::min(min_max_dist,
-                     std::max(coordinate_calculation::euclidean_distance(location, upper_right),
-                              coordinate_calculation::euclidean_distance(location, lower_right)));
+        min_max_dist = std::min(
+            min_max_dist,
+            std::max(coordinate_calculation::great_circle_distance(location, upper_right),
+                     coordinate_calculation::great_circle_distance(location, lower_right)));
 
         min_max_dist =
             std::min(min_max_dist,
-                     std::max(coordinate_calculation::euclidean_distance(location, lower_right),
-                              coordinate_calculation::euclidean_distance(location, lower_left)));
+                     std::max(coordinate_calculation::great_circle_distance(location, lower_right),
+                              coordinate_calculation::great_circle_distance(location, lower_left)));
 
         min_max_dist =
             std::min(min_max_dist,
-                     std::max(coordinate_calculation::euclidean_distance(location, lower_left),
-                              coordinate_calculation::euclidean_distance(location, upper_left)));
+                     std::max(coordinate_calculation::great_circle_distance(location, lower_left),
+                              coordinate_calculation::great_circle_distance(location, upper_left)));
         return min_max_dist;
     }
 
@@ -198,14 +198,6 @@ struct RectangleInt2D
         const bool lons_contained = (location.lon >= min_lon) && (location.lon <= max_lon);
         return lats_contained && lons_contained;
     }
-
-    friend std::ostream &operator<<(std::ostream &out, const RectangleInt2D &rect)
-    {
-        out << rect.min_lat / COORDINATE_PRECISION << "," << rect.min_lon / COORDINATE_PRECISION
-            << " " << rect.max_lat / COORDINATE_PRECISION << ","
-            << rect.max_lon / COORDINATE_PRECISION;
-        return out;
-    }
 };
 
 #endif
diff --git a/data_structures/restriction.hpp b/data_structures/restriction.hpp
index a59fb1e..ecab4f9 100644
--- a/data_structures/restriction.hpp
+++ b/data_structures/restriction.hpp
@@ -36,8 +36,8 @@ struct TurnRestriction
 {
     union WayOrNode
     {
-        NodeID node;
-        EdgeID way;
+        OSMNodeID_weak node;
+        OSMEdgeID_weak way;
     };
     WayOrNode via;
     WayOrNode from;
diff --git a/data_structures/restriction_map.cpp b/data_structures/restriction_map.cpp
index 017ee32..eb685be 100644
--- a/data_structures/restriction_map.cpp
+++ b/data_structures/restriction_map.cpp
@@ -33,10 +33,16 @@ RestrictionMap::RestrictionMap(const std::vector<TurnRestriction> &restriction_l
     // a pair of starting edge and a list of all end nodes
     for (auto &restriction : restriction_list)
     {
+        // This downcasting is OK because when this is called, the node IDs have been
+        // renumbered into internal values, which should be well under 2^32
+        // This will be a problem if we have more than 2^32 actual restrictions
+        BOOST_ASSERT(restriction.from.node < std::numeric_limits<NodeID>::max());
+        BOOST_ASSERT(restriction.via.node < std::numeric_limits<NodeID>::max());
         m_restriction_start_nodes.insert(restriction.from.node);
         m_no_turn_via_node_set.insert(restriction.via.node);
 
-        RestrictionSource restriction_source = {restriction.from.node, restriction.via.node};
+        // This explicit downcasting is also OK for the same reason.
+        RestrictionSource restriction_source = {static_cast<NodeID>(restriction.from.node), static_cast<NodeID>(restriction.via.node)};
 
         std::size_t index;
         auto restriction_iter = m_restriction_map.find(restriction_source);
@@ -62,6 +68,7 @@ RestrictionMap::RestrictionMap(const std::vector<TurnRestriction> &restriction_l
             }
         }
         ++m_count;
+        BOOST_ASSERT(restriction.to.node < std::numeric_limits<NodeID>::max());
         m_restriction_bucket_list.at(index)
             .emplace_back(restriction.to.node, restriction.flags.is_only);
     }
diff --git a/data_structures/route_parameters.cpp b/data_structures/route_parameters.cpp
index df23ce2..61789bb 100644
--- a/data_structures/route_parameters.cpp
+++ b/data_structures/route_parameters.cpp
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <boost/fusion/container/vector.hpp>
 #include <boost/fusion/sequence/intrinsic.hpp>
 #include <boost/fusion/include/at_c.hpp>
+#include <boost/spirit/include/qi.hpp>
 
 #include <osrm/route_parameters.hpp>
 
@@ -60,11 +61,9 @@ void RouteParameters::setAlternateRouteFlag(const bool flag) { alternate_route =
 
 void RouteParameters::setUTurn(const bool flag)
 {
-    uturns.resize(coordinates.size(), uturn_default);
-    if (!uturns.empty())
-    {
-        uturns.back() = flag;
-    }
+    // the API grammar should make sure this never happens
+    BOOST_ASSERT(!uturns.empty());
+    uturns.back() = flag;
 }
 
 void RouteParameters::setAllUTurns(const bool flag)
@@ -117,6 +116,19 @@ void RouteParameters::addTimestamp(const unsigned timestamp)
     }
 }
 
+void RouteParameters::addBearing(
+    const boost::fusion::vector<int, boost::optional<int>> &received_bearing,
+        boost::spirit::qi::unused_type /* unused */, bool& pass)
+{
+    pass = false;
+    const int bearing = boost::fusion::at_c<0>(received_bearing);
+    const boost::optional<int> range = boost::fusion::at_c<1>(received_bearing);
+    if (bearing < 0 || bearing > 359) return;
+    if (range && (*range < 0 || *range > 180)) return;
+    bearings.emplace_back(std::make_pair(bearing,range));
+    pass = true;
+}
+
 void RouteParameters::setLanguage(const std::string &language_string)
 {
     language = language_string;
@@ -132,6 +144,31 @@ void RouteParameters::addCoordinate(
     coordinates.emplace_back(
         static_cast<int>(COORDINATE_PRECISION * boost::fusion::at_c<0>(received_coordinates)),
         static_cast<int>(COORDINATE_PRECISION * boost::fusion::at_c<1>(received_coordinates)));
+    is_source.push_back(true);
+    is_destination.push_back(true);
+    uturns.push_back(uturn_default);
+}
+
+void RouteParameters::addDestination(
+    const boost::fusion::vector<double, double> &received_coordinates)
+{
+    coordinates.emplace_back(
+        static_cast<int>(COORDINATE_PRECISION * boost::fusion::at_c<0>(received_coordinates)),
+        static_cast<int>(COORDINATE_PRECISION * boost::fusion::at_c<1>(received_coordinates)));
+    is_source.push_back(false);
+    is_destination.push_back(true);
+    uturns.push_back(uturn_default);
+}
+
+void RouteParameters::addSource(
+    const boost::fusion::vector<double, double> &received_coordinates)
+{
+    coordinates.emplace_back(
+        static_cast<int>(COORDINATE_PRECISION * boost::fusion::at_c<0>(received_coordinates)),
+        static_cast<int>(COORDINATE_PRECISION * boost::fusion::at_c<1>(received_coordinates)));
+    is_source.push_back(true);
+    is_destination.push_back(false);
+    uturns.push_back(uturn_default);
 }
 
 void RouteParameters::getCoordinatesFromGeometry(const std::string &geometry_string)
@@ -139,3 +176,4 @@ void RouteParameters::getCoordinatesFromGeometry(const std::string &geometry_str
     PolylineCompressor pc;
     coordinates = pc.decode_string(geometry_string);
 }
+
diff --git a/data_structures/segment_information.hpp b/data_structures/segment_information.hpp
index b22254f..d9cdc81 100644
--- a/data_structures/segment_information.hpp
+++ b/data_structures/segment_information.hpp
@@ -43,7 +43,8 @@ struct SegmentInformation
     NodeID name_id;
     EdgeWeight duration;
     float length;
-    short bearing; // more than enough [0..3600] fits into 12 bits
+    short pre_turn_bearing; // more than enough [0..3600] fits into 12 bits
+    short post_turn_bearing;
     TurnInstruction turn_instruction;
     TravelMode travel_mode;
     bool necessary;
@@ -58,7 +59,7 @@ struct SegmentInformation
                                 const bool is_via_location,
                                 const TravelMode travel_mode)
         : location(std::move(location)), name_id(name_id), duration(duration), length(length),
-          bearing(0), turn_instruction(turn_instruction), travel_mode(travel_mode),
+          pre_turn_bearing(0), post_turn_bearing(0), turn_instruction(turn_instruction), travel_mode(travel_mode),
           necessary(necessary), is_via_location(is_via_location)
     {
     }
@@ -70,7 +71,7 @@ struct SegmentInformation
                                 const TurnInstruction turn_instruction,
                                 const TravelMode travel_mode)
         : location(std::move(location)), name_id(name_id), duration(duration), length(length),
-          bearing(0), turn_instruction(turn_instruction), travel_mode(travel_mode),
+          pre_turn_bearing(0), post_turn_bearing(0), turn_instruction(turn_instruction), travel_mode(travel_mode),
           necessary(turn_instruction != TurnInstruction::NoTurn), is_via_location(false)
     {
     }
diff --git a/data_structures/static_rtree.hpp b/data_structures/static_rtree.hpp
index 314935a..6bc3678 100644
--- a/data_structures/static_rtree.hpp
+++ b/data_structures/static_rtree.hpp
@@ -30,19 +30,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "deallocating_vector.hpp"
 #include "hilbert_value.hpp"
-#include "phantom_node.hpp"
-#include "query_node.hpp"
 #include "rectangle.hpp"
 #include "shared_memory_factory.hpp"
 #include "shared_memory_vector_wrapper.hpp"
-#include "upper_bound.hpp"
 
-#include "../util/floating_point.hpp"
+#include "../util/bearing.hpp"
 #include "../util/integer_range.hpp"
 #include "../util/mercator.hpp"
 #include "../util/osrm_exception.hpp"
-#include "../util/simple_logger.hpp"
-#include "../util/timing_util.hpp"
 #include "../typedefs.h"
 
 #include <osrm/coordinate.hpp>
@@ -50,7 +45,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <boost/assert.hpp>
 #include <boost/filesystem.hpp>
 #include <boost/filesystem/fstream.hpp>
-#include <boost/thread.hpp>
 
 #include <tbb/parallel_for.h>
 #include <tbb/parallel_sort.h>
@@ -65,7 +59,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <string>
 #include <vector>
 
-// Implements a static, i.e. packed, R-tree
+// Static RTree for serving nearest neighbour queries
 template <class EdgeDataT,
           class CoordinateListT = std::vector<FixedPointCoordinate>,
           bool UseSharedMemory = false,
@@ -74,198 +68,16 @@ template <class EdgeDataT,
 class StaticRTree
 {
   public:
-    struct RectangleInt2D
-    {
-        RectangleInt2D() : min_lon(INT_MAX), max_lon(INT_MIN), min_lat(INT_MAX), max_lat(INT_MIN) {}
-
-        int32_t min_lon, max_lon;
-        int32_t min_lat, max_lat;
-
-        inline void InitializeMBRectangle(const std::array<EdgeDataT, LEAF_NODE_SIZE> &objects,
-                                          const uint32_t element_count,
-                                          const std::vector<QueryNode> &coordinate_list)
-        {
-            for (uint32_t i = 0; i < element_count; ++i)
-            {
-                min_lon = std::min(min_lon, std::min(coordinate_list.at(objects[i].u).lon,
-                                                     coordinate_list.at(objects[i].v).lon));
-                max_lon = std::max(max_lon, std::max(coordinate_list.at(objects[i].u).lon,
-                                                     coordinate_list.at(objects[i].v).lon));
-
-                min_lat = std::min(min_lat, std::min(coordinate_list.at(objects[i].u).lat,
-                                                     coordinate_list.at(objects[i].v).lat));
-                max_lat = std::max(max_lat, std::max(coordinate_list.at(objects[i].u).lat,
-                                                     coordinate_list.at(objects[i].v).lat));
-            }
-            BOOST_ASSERT(min_lat != std::numeric_limits<int>::min());
-            BOOST_ASSERT(min_lon != std::numeric_limits<int>::min());
-            BOOST_ASSERT(max_lat != std::numeric_limits<int>::min());
-            BOOST_ASSERT(max_lon != std::numeric_limits<int>::min());
-        }
-
-        inline void MergeBoundingBoxes(const RectangleInt2D &other)
-        {
-            min_lon = std::min(min_lon, other.min_lon);
-            max_lon = std::max(max_lon, other.max_lon);
-            min_lat = std::min(min_lat, other.min_lat);
-            max_lat = std::max(max_lat, other.max_lat);
-            BOOST_ASSERT(min_lat != std::numeric_limits<int>::min());
-            BOOST_ASSERT(min_lon != std::numeric_limits<int>::min());
-            BOOST_ASSERT(max_lat != std::numeric_limits<int>::min());
-            BOOST_ASSERT(max_lon != std::numeric_limits<int>::min());
-        }
-
-        inline FixedPointCoordinate Centroid() const
-        {
-            FixedPointCoordinate centroid;
-            // The coordinates of the midpoints are given by:
-            // x = (x1 + x2) /2 and y = (y1 + y2) /2.
-            centroid.lon = (min_lon + max_lon) / 2;
-            centroid.lat = (min_lat + max_lat) / 2;
-            return centroid;
-        }
-
-        inline bool Intersects(const RectangleInt2D &other) const
-        {
-            FixedPointCoordinate upper_left(other.max_lat, other.min_lon);
-            FixedPointCoordinate upper_right(other.max_lat, other.max_lon);
-            FixedPointCoordinate lower_right(other.min_lat, other.max_lon);
-            FixedPointCoordinate lower_left(other.min_lat, other.min_lon);
-
-            return (Contains(upper_left) || Contains(upper_right) || Contains(lower_right) ||
-                    Contains(lower_left));
-        }
-
-        inline float GetMinDist(const FixedPointCoordinate &location) const
-        {
-            const bool is_contained = Contains(location);
-            if (is_contained)
-            {
-                return 0.;
-            }
-
-            enum Direction
-            {
-                INVALID = 0,
-                NORTH = 1,
-                SOUTH = 2,
-                EAST = 4,
-                NORTH_EAST = 5,
-                SOUTH_EAST = 6,
-                WEST = 8,
-                NORTH_WEST = 9,
-                SOUTH_WEST = 10
-            };
-
-            Direction d = INVALID;
-            if (location.lat > max_lat)
-                d = (Direction)(d | NORTH);
-            else if (location.lat < min_lat)
-                d = (Direction)(d | SOUTH);
-            if (location.lon > max_lon)
-                d = (Direction)(d | EAST);
-            else if (location.lon < min_lon)
-                d = (Direction)(d | WEST);
-
-            BOOST_ASSERT(d != INVALID);
-
-            float min_dist = std::numeric_limits<float>::max();
-            switch (d)
-            {
-            case NORTH:
-                min_dist = coordinate_calculation::euclidean_distance(
-                    location, FixedPointCoordinate(max_lat, location.lon));
-                break;
-            case SOUTH:
-                min_dist = coordinate_calculation::euclidean_distance(
-                    location, FixedPointCoordinate(min_lat, location.lon));
-                break;
-            case WEST:
-                min_dist = coordinate_calculation::euclidean_distance(
-                    location, FixedPointCoordinate(location.lat, min_lon));
-                break;
-            case EAST:
-                min_dist = coordinate_calculation::euclidean_distance(
-                    location, FixedPointCoordinate(location.lat, max_lon));
-                break;
-            case NORTH_EAST:
-                min_dist = coordinate_calculation::euclidean_distance(
-                    location, FixedPointCoordinate(max_lat, max_lon));
-                break;
-            case NORTH_WEST:
-                min_dist = coordinate_calculation::euclidean_distance(
-                    location, FixedPointCoordinate(max_lat, min_lon));
-                break;
-            case SOUTH_EAST:
-                min_dist = coordinate_calculation::euclidean_distance(
-                    location, FixedPointCoordinate(min_lat, max_lon));
-                break;
-            case SOUTH_WEST:
-                min_dist = coordinate_calculation::euclidean_distance(
-                    location, FixedPointCoordinate(min_lat, min_lon));
-                break;
-            default:
-                break;
-            }
-
-            BOOST_ASSERT(min_dist != std::numeric_limits<float>::max());
-
-            return min_dist;
-        }
-
-        inline float GetMinMaxDist(const FixedPointCoordinate &location) const
-        {
-            float min_max_dist = std::numeric_limits<float>::max();
-            // Get minmax distance to each of the four sides
-            const FixedPointCoordinate upper_left(max_lat, min_lon);
-            const FixedPointCoordinate upper_right(max_lat, max_lon);
-            const FixedPointCoordinate lower_right(min_lat, max_lon);
-            const FixedPointCoordinate lower_left(min_lat, min_lon);
-
-            min_max_dist = std::min(
-                min_max_dist,
-                std::max(coordinate_calculation::euclidean_distance(location, upper_left),
-                         coordinate_calculation::euclidean_distance(location, upper_right)));
-
-            min_max_dist = std::min(
-                min_max_dist,
-                std::max(coordinate_calculation::euclidean_distance(location, upper_right),
-                         coordinate_calculation::euclidean_distance(location, lower_right)));
-
-            min_max_dist = std::min(
-                min_max_dist,
-                std::max(coordinate_calculation::euclidean_distance(location, lower_right),
-                         coordinate_calculation::euclidean_distance(location, lower_left)));
-
-            min_max_dist = std::min(
-                min_max_dist,
-                std::max(coordinate_calculation::euclidean_distance(location, lower_left),
-                         coordinate_calculation::euclidean_distance(location, upper_left)));
-            return min_max_dist;
-        }
-
-        inline bool Contains(const FixedPointCoordinate &location) const
-        {
-            const bool lats_contained = (location.lat >= min_lat) && (location.lat <= max_lat);
-            const bool lons_contained = (location.lon >= min_lon) && (location.lon <= max_lon);
-            return lats_contained && lons_contained;
-        }
-
-        inline friend std::ostream &operator<<(std::ostream &out, const RectangleInt2D &rect)
-        {
-            out << rect.min_lat / COORDINATE_PRECISION << "," << rect.min_lon / COORDINATE_PRECISION
-                << " " << rect.max_lat / COORDINATE_PRECISION << ","
-                << rect.max_lon / COORDINATE_PRECISION;
-            return out;
-        }
-    };
+    using Rectangle = RectangleInt2D;
+    using EdgeData = EdgeDataT;
+    using CoordinateList = CoordinateListT;
 
-    using RectangleT = RectangleInt2D;
+    static constexpr std::size_t MAX_CHECKED_ELEMENTS = 4 * LEAF_NODE_SIZE;
 
     struct TreeNode
     {
         TreeNode() : child_count(0), child_is_on_disk(false) {}
-        RectangleT minimum_bounding_rectangle;
+        Rectangle minimum_bounding_rectangle;
         uint32_t child_count : 31;
         bool child_is_on_disk : 1;
         uint32_t children[BRANCHING_FACTOR];
@@ -297,40 +109,17 @@ class StaticRTree
         std::array<EdgeDataT, LEAF_NODE_SIZE> objects;
     };
 
+    using QueryNodeType = mapbox::util::variant<TreeNode, EdgeDataT>;
     struct QueryCandidate
     {
-        explicit QueryCandidate(const float dist, const uint32_t n_id)
-            : min_dist(dist), node_id(n_id)
-        {
-        }
-        QueryCandidate() : min_dist(std::numeric_limits<float>::max()), node_id(UINT_MAX) {}
-        float min_dist;
-        uint32_t node_id;
         inline bool operator<(const QueryCandidate &other) const
         {
             // Attn: this is reversed order. std::pq is a max pq!
             return other.min_dist < min_dist;
         }
-    };
-
-    using IncrementalQueryNodeType = mapbox::util::variant<TreeNode, EdgeDataT>;
-    struct IncrementalQueryCandidate
-    {
-        explicit IncrementalQueryCandidate(const float dist, IncrementalQueryNodeType node)
-            : min_dist(dist), node(std::move(node))
-        {
-        }
-
-        IncrementalQueryCandidate() : min_dist(std::numeric_limits<float>::max()) {}
-
-        inline bool operator<(const IncrementalQueryCandidate &other) const
-        {
-            // Attn: this is reversed order. std::pq is a max pq!
-            return other.min_dist < min_dist;
-        }
 
         float min_dist;
-        IncrementalQueryNodeType node;
+        QueryNodeType node;
     };
 
     typename ShM<TreeNode, UseSharedMemory>::vector m_search_tree;
@@ -343,18 +132,14 @@ class StaticRTree
     StaticRTree() = delete;
     StaticRTree(const StaticRTree &) = delete;
 
+    template <typename CoordinateT>
     // Construct a packed Hilbert-R-Tree with Kamel-Faloutsos algorithm [1]
     explicit StaticRTree(const std::vector<EdgeDataT> &input_data_vector,
                          const std::string &tree_node_filename,
                          const std::string &leaf_node_filename,
-                         const std::vector<QueryNode> &coordinate_list)
+                         const std::vector<CoordinateT> &coordinate_list)
         : m_element_count(input_data_vector.size()), m_leaf_node_filename(leaf_node_filename)
     {
-        SimpleLogger().Write() << "constructing r-tree of " << m_element_count
-                               << " edge elements build on-top of " << coordinate_list.size()
-                               << " coordinates";
-
-        TIMER_START(construction);
         std::vector<WrappedInputElement> input_wrapper_vector(m_element_count);
 
         HilbertCode get_hilbert_number;
@@ -362,8 +147,8 @@ class StaticRTree
         // generate auxiliary vector of hilbert-values
         tbb::parallel_for(
             tbb::blocked_range<uint64_t>(0, m_element_count),
-            [&input_data_vector, &input_wrapper_vector, &get_hilbert_number, &coordinate_list](
-                const tbb::blocked_range<uint64_t> &range)
+            [&input_data_vector, &input_wrapper_vector, &get_hilbert_number,
+             &coordinate_list](const tbb::blocked_range<uint64_t> &range)
             {
                 for (uint64_t element_counter = range.begin(), end = range.end();
                      element_counter != end; ++element_counter)
@@ -402,8 +187,6 @@ class StaticRTree
 
             LeafNode current_leaf;
             TreeNode current_node;
-            // SimpleLogger().Write() << "reading " << tree_size << " tree nodes in " <<
-            // (sizeof(TreeNode)*tree_size) << " bytes";
             for (uint32_t current_element_index = 0; LEAF_NODE_SIZE > current_element_index;
                  ++current_element_index)
             {
@@ -497,13 +280,8 @@ class StaticRTree
         tree_node_file.write((char *)&m_search_tree[0], sizeof(TreeNode) * size_of_tree);
         // close tree node file.
         tree_node_file.close();
-
-        TIMER_STOP(construction);
-        SimpleLogger().Write() << "finished r-tree construction in " << TIMER_SEC(construction)
-                               << " seconds";
     }
 
-    // Read-only operation for queries
     explicit StaticRTree(const boost::filesystem::path &node_file,
                          const boost::filesystem::path &leaf_file,
                          const std::shared_ptr<CoordinateListT> coordinate_list)
@@ -543,9 +321,6 @@ class StaticRTree
 
         leaves_stream.open(leaf_file, std::ios::binary);
         leaves_stream.read((char *)&m_element_count, sizeof(uint64_t));
-
-        // SimpleLogger().Write() << tree_size << " nodes in search tree";
-        // SimpleLogger().Write() << m_element_count << " elements in leafs";
     }
 
     explicit StaticRTree(TreeNode *tree_node_ptr,
@@ -567,506 +342,130 @@ class StaticRTree
 
         leaves_stream.open(leaf_file, std::ios::binary);
         leaves_stream.read((char *)&m_element_count, sizeof(uint64_t));
-
-        // SimpleLogger().Write() << tree_size << " nodes in search tree";
-        // SimpleLogger().Write() << m_element_count << " elements in leafs";
     }
-    // Read-only operation for queries
 
-    bool LocateClosestEndPointForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                            FixedPointCoordinate &result_coordinate,
-                                            const unsigned zoom_level)
+    // Override filter and terminator for the desired behaviour.
+    std::vector<EdgeDataT> Nearest(const FixedPointCoordinate &input_coordinate,
+                                const std::size_t max_results)
     {
-        bool ignore_tiny_components = (zoom_level <= 14);
-
-        float min_dist = std::numeric_limits<float>::max();
-        float min_max_dist = std::numeric_limits<float>::max();
-
-        // initialize queue with root element
-        std::priority_queue<QueryCandidate> traversal_queue;
-        traversal_queue.emplace(0.f, 0);
-
-        while (!traversal_queue.empty())
-        {
-            const QueryCandidate current_query_node = traversal_queue.top();
-            traversal_queue.pop();
-
-            const bool prune_downward = (current_query_node.min_dist >= min_max_dist);
-            const bool prune_upward = (current_query_node.min_dist >= min_dist);
-            if (!prune_downward && !prune_upward)
-            { // downward pruning
-                TreeNode &current_tree_node = m_search_tree[current_query_node.node_id];
-                if (current_tree_node.child_is_on_disk)
-                {
-                    LeafNode current_leaf_node;
-                    LoadLeafFromDisk(current_tree_node.children[0], current_leaf_node);
-                    for (uint32_t i = 0; i < current_leaf_node.object_count; ++i)
-                    {
-                        EdgeDataT const &current_edge = current_leaf_node.objects[i];
-                        if (ignore_tiny_components && current_edge.component_id != 0)
-                        {
-                            continue;
-                        }
-
-                        float current_minimum_distance = coordinate_calculation::euclidean_distance(
-                            input_coordinate.lat, input_coordinate.lon,
-                            m_coordinate_list->at(current_edge.u).lat,
-                            m_coordinate_list->at(current_edge.u).lon);
-                        if (current_minimum_distance < min_dist)
-                        {
-                            // found a new minimum
-                            min_dist = current_minimum_distance;
-                            result_coordinate = m_coordinate_list->at(current_edge.u);
-                        }
-
-                        current_minimum_distance = coordinate_calculation::euclidean_distance(
-                            input_coordinate.lat, input_coordinate.lon,
-                            m_coordinate_list->at(current_edge.v).lat,
-                            m_coordinate_list->at(current_edge.v).lon);
-
-                        if (current_minimum_distance < min_dist)
-                        {
-                            // found a new minimum
-                            min_dist = current_minimum_distance;
-                            result_coordinate = m_coordinate_list->at(current_edge.v);
-                        }
-                    }
-                }
-                else
-                {
-                    min_max_dist = ExploreTreeNode(current_tree_node, input_coordinate, min_dist,
-                                                   min_max_dist, traversal_queue);
-                }
-            }
-        }
-        return result_coordinate.is_valid();
+        return Nearest(input_coordinate,
+                       [](const EdgeDataT &)
+                       {
+                           return std::make_pair(true, true);
+                       },
+                       [max_results](const std::size_t num_results, const float)
+                       {
+                           return num_results >= max_results;
+                       });
     }
 
-    bool IncrementalFindPhantomNodeForCoordinate(
-        const FixedPointCoordinate &input_coordinate,
-        std::vector<PhantomNode> &result_phantom_node_vector,
-        const unsigned max_number_of_phantom_nodes,
-        const float max_distance = 1100,
-        const unsigned max_checked_elements = 4 * LEAF_NODE_SIZE)
+    // Override filter and terminator for the desired behaviour.
+    template <typename FilterT, typename TerminationT>
+    std::vector<EdgeDataT> Nearest(const FixedPointCoordinate &input_coordinate,
+                                const FilterT filter,
+                                const TerminationT terminate)
     {
-        unsigned inspected_elements = 0;
-        unsigned number_of_elements_from_big_cc = 0;
-        unsigned number_of_elements_from_tiny_cc = 0;
-
+        std::vector<EdgeDataT> results;
         std::pair<double, double> projected_coordinate = {
             mercator::lat2y(input_coordinate.lat / COORDINATE_PRECISION),
             input_coordinate.lon / COORDINATE_PRECISION};
 
         // initialize queue with root element
-        std::priority_queue<IncrementalQueryCandidate> traversal_queue;
-        traversal_queue.emplace(0.f, m_search_tree[0]);
+        std::priority_queue<QueryCandidate> traversal_queue;
+        traversal_queue.push(QueryCandidate {0.f, m_search_tree[0]});
 
         while (!traversal_queue.empty())
         {
-            const IncrementalQueryCandidate current_query_node = traversal_queue.top();
-            if (current_query_node.min_dist > max_distance &&
-                inspected_elements > max_checked_elements)
+            const QueryCandidate current_query_node = traversal_queue.top();
+            if (terminate(results.size(), current_query_node.min_dist))
             {
+                traversal_queue = std::priority_queue<QueryCandidate>{};
                 break;
             }
-            traversal_queue.pop();
-
-            if (current_query_node.node.template is<TreeNode>())
-            { // current object is a tree node
-                const TreeNode &current_tree_node =
-                    current_query_node.node.template get<TreeNode>();
-                if (current_tree_node.child_is_on_disk)
-                {
-                    LeafNode current_leaf_node;
-                    LoadLeafFromDisk(current_tree_node.children[0], current_leaf_node);
-
-                    // current object represents a block on disk
-                    for (const auto i : osrm::irange(0u, current_leaf_node.object_count))
-                    {
-                        const auto &current_edge = current_leaf_node.objects[i];
-                        const float current_perpendicular_distance = coordinate_calculation::
-                            perpendicular_distance_from_projected_coordinate(
-                                m_coordinate_list->at(current_edge.u),
-                                m_coordinate_list->at(current_edge.v), input_coordinate,
-                                projected_coordinate);
-                        // distance must be non-negative
-                        BOOST_ASSERT(0.f <= current_perpendicular_distance);
-
-                        traversal_queue.emplace(current_perpendicular_distance, current_edge);
-                    }
-                }
-                else
-                {
-                    // for each child mbr get a lower bound and enqueue it
-                    for (const auto i : osrm::irange(0u, current_tree_node.child_count))
-                    {
-                        const int32_t child_id = current_tree_node.children[i];
-                        const TreeNode &child_tree_node = m_search_tree[child_id];
-                        const RectangleT &child_rectangle =
-                            child_tree_node.minimum_bounding_rectangle;
-                        const float lower_bound_to_element =
-                            child_rectangle.GetMinDist(input_coordinate);
-                        BOOST_ASSERT(0.f <= lower_bound_to_element);
-
-                        traversal_queue.emplace(lower_bound_to_element, child_tree_node);
-                    }
-                }
-            }
-            else
-            { // current object is a leaf node
-                ++inspected_elements;
-                // inspecting an actual road segment
-                const EdgeDataT &current_segment =
-                    current_query_node.node.template get<EdgeDataT>();
-
-                // continue searching for the first segment from a big component
-                if (number_of_elements_from_big_cc == 0 &&
-                    number_of_elements_from_tiny_cc >= max_number_of_phantom_nodes &&
-                    current_segment.is_in_tiny_cc())
-                {
-                    continue;
-                }
-
-                // check if it is smaller than what we had before
-                float current_ratio = 0.f;
-                FixedPointCoordinate foot_point_coordinate_on_segment;
-
-                // const float current_perpendicular_distance =
-                coordinate_calculation::perpendicular_distance_from_projected_coordinate(
-                    m_coordinate_list->at(current_segment.u),
-                    m_coordinate_list->at(current_segment.v), input_coordinate,
-                    projected_coordinate, foot_point_coordinate_on_segment, current_ratio);
-
-                // store phantom node in result vector
-                result_phantom_node_vector.emplace_back(current_segment,
-                                                        foot_point_coordinate_on_segment);
-
-                // Hack to fix rounding errors and wandering via nodes.
-                FixUpRoundingIssue(input_coordinate, result_phantom_node_vector.back());
 
-                // set forward and reverse weights on the phantom node
-                SetForwardAndReverseWeightsOnPhantomNode(current_segment,
-                                                         result_phantom_node_vector.back());
-
-                // update counts on what we found from which result class
-                if (current_segment.is_in_tiny_cc())
-                { // found an element in tiny component
-                    ++number_of_elements_from_tiny_cc;
-                }
-                else
-                { // found an element in a big component
-                    ++number_of_elements_from_big_cc;
-                }
-            }
-
-            // stop the search by flushing the queue
-            if (result_phantom_node_vector.size() >= max_number_of_phantom_nodes &&
-                number_of_elements_from_big_cc > 0)
-            {
-                traversal_queue = std::priority_queue<IncrementalQueryCandidate>{};
-            }
-        }
-#ifdef NDEBUG
-// SimpleLogger().Write() << "result_phantom_node_vector.size(): " <<
-// result_phantom_node_vector.size();
-// SimpleLogger().Write() << "max_number_of_phantom_nodes: " << max_number_of_phantom_nodes;
-// SimpleLogger().Write() << "number_of_elements_from_big_cc: " <<
-// number_of_elements_from_big_cc;
-// SimpleLogger().Write() << "number_of_elements_from_tiny_cc: " <<
-// number_of_elements_from_tiny_cc;
-// SimpleLogger().Write() << "inspected_elements: " << inspected_elements;
-// SimpleLogger().Write() << "max_checked_elements: " << max_checked_elements;
-// SimpleLogger().Write() << "pruned_elements: " << pruned_elements;
-#endif
-        return !result_phantom_node_vector.empty();
-    }
-
-    // Returns elements within max_distance.
-    // If the minium of elements could not be found in the search radius, widen
-    // it until the minimum can be satisfied.
-    bool IncrementalFindPhantomNodeForCoordinateWithDistance(
-        const FixedPointCoordinate &input_coordinate,
-        std::vector<std::pair<PhantomNode, double>> &result_phantom_node_vector,
-        const double max_distance,
-        const unsigned max_checked_elements = 4 * LEAF_NODE_SIZE)
-    {
-        unsigned inspected_elements = 0;
-
-        std::pair<double, double> projected_coordinate = {
-            mercator::lat2y(input_coordinate.lat / COORDINATE_PRECISION),
-            input_coordinate.lon / COORDINATE_PRECISION};
-
-        // initialize queue with root element
-        std::priority_queue<IncrementalQueryCandidate> traversal_queue;
-        traversal_queue.emplace(0.f, m_search_tree[0]);
-
-        while (!traversal_queue.empty())
-        {
-            const IncrementalQueryCandidate current_query_node = traversal_queue.top();
             traversal_queue.pop();
 
-            if (current_query_node.min_dist > max_distance ||
-                inspected_elements >= max_checked_elements)
-            {
-                break;
-            }
-
             if (current_query_node.node.template is<TreeNode>())
             { // current object is a tree node
                 const TreeNode &current_tree_node =
                     current_query_node.node.template get<TreeNode>();
                 if (current_tree_node.child_is_on_disk)
                 {
-                    LeafNode current_leaf_node;
-                    LoadLeafFromDisk(current_tree_node.children[0], current_leaf_node);
-
-                    // current object represents a block on disk
-                    for (const auto i : osrm::irange(0u, current_leaf_node.object_count))
-                    {
-                        const auto &current_edge = current_leaf_node.objects[i];
-                        const float current_perpendicular_distance = coordinate_calculation::
-                            perpendicular_distance_from_projected_coordinate(
-                                m_coordinate_list->at(current_edge.u),
-                                m_coordinate_list->at(current_edge.v), input_coordinate,
-                                projected_coordinate);
-                        // distance must be non-negative
-                        BOOST_ASSERT(0.f <= current_perpendicular_distance);
-
-                        if (current_perpendicular_distance <= max_distance)
-                        {
-                            traversal_queue.emplace(current_perpendicular_distance, current_edge);
-                        }
-                    }
+                    ExploreLeafNode(current_tree_node.children[0], input_coordinate,
+                                    projected_coordinate, traversal_queue);
                 }
                 else
                 {
-                    // for each child mbr get a lower bound and enqueue it
-                    for (const auto i : osrm::irange(0u, current_tree_node.child_count))
-                    {
-                        const int32_t child_id = current_tree_node.children[i];
-                        const TreeNode &child_tree_node = m_search_tree[child_id];
-                        const RectangleT &child_rectangle =
-                            child_tree_node.minimum_bounding_rectangle;
-                        const float lower_bound_to_element =
-                            child_rectangle.GetMinDist(input_coordinate);
-                        BOOST_ASSERT(0.f <= lower_bound_to_element);
-
-                        if (lower_bound_to_element <= max_distance)
-                        {
-                            traversal_queue.emplace(lower_bound_to_element, child_tree_node);
-                        }
-                    }
+                    ExploreTreeNode(current_tree_node, input_coordinate, traversal_queue);
                 }
             }
             else
-            { // current object is a leaf node
-                ++inspected_elements;
+            {
                 // inspecting an actual road segment
-                const EdgeDataT &current_segment =
-                    current_query_node.node.template get<EdgeDataT>();
+                const auto &current_segment = current_query_node.node.template get<EdgeDataT>();
 
-                // check if it is smaller than what we had before
-                float current_ratio = 0.f;
-                FixedPointCoordinate foot_point_coordinate_on_segment;
 
-                const float current_perpendicular_distance =
-                    coordinate_calculation::perpendicular_distance_from_projected_coordinate(
-                        m_coordinate_list->at(current_segment.u),
-                        m_coordinate_list->at(current_segment.v), input_coordinate,
-                        projected_coordinate, foot_point_coordinate_on_segment, current_ratio);
-
-                if (current_perpendicular_distance >= max_distance)
+                auto use_segment = filter(current_segment);
+                if (!use_segment.first && !use_segment.second)
                 {
-                    traversal_queue = std::priority_queue<IncrementalQueryCandidate>{};
                     continue;
                 }
 
                 // store phantom node in result vector
-                result_phantom_node_vector.emplace_back(
-                    PhantomNode(
-                        current_segment.forward_edge_based_node_id,
-                        current_segment.reverse_edge_based_node_id, current_segment.name_id,
-                        current_segment.forward_weight, current_segment.reverse_weight,
-                        current_segment.forward_offset, current_segment.reverse_offset,
-                        current_segment.packed_geometry_id, current_segment.component_id,
-                        foot_point_coordinate_on_segment, current_segment.fwd_segment_position,
-                        current_segment.forward_travel_mode, current_segment.backward_travel_mode),
-                    current_perpendicular_distance);
-
-                // Hack to fix rounding errors and wandering via nodes.
-                FixUpRoundingIssue(input_coordinate, result_phantom_node_vector.back().first);
-
-                // set forward and reverse weights on the phantom node
-                SetForwardAndReverseWeightsOnPhantomNode(current_segment,
-                                                         result_phantom_node_vector.back().first);
-            }
-
-            // stop the search by flushing the queue
-            if (inspected_elements >= max_checked_elements)
-            {
-                traversal_queue = std::priority_queue<IncrementalQueryCandidate>{};
-            }
-        }
-
-        return !result_phantom_node_vector.empty();
-    }
-
-    bool FindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                      PhantomNode &result_phantom_node,
-                                      const unsigned zoom_level)
-    {
-        const bool ignore_tiny_components = (zoom_level <= 14);
-        EdgeDataT nearest_edge;
-
-        float min_dist = std::numeric_limits<float>::max();
-        float min_max_dist = std::numeric_limits<float>::max();
-
-        std::priority_queue<QueryCandidate> traversal_queue;
-        traversal_queue.emplace(0.f, 0);
-
-        while (!traversal_queue.empty())
-        {
-            const QueryCandidate current_query_node = traversal_queue.top();
-            traversal_queue.pop();
+                results.push_back(std::move(current_segment));
 
-            const bool prune_downward = (current_query_node.min_dist > min_max_dist);
-            const bool prune_upward = (current_query_node.min_dist > min_dist);
-            if (!prune_downward && !prune_upward)
-            { // downward pruning
-                const TreeNode &current_tree_node = m_search_tree[current_query_node.node_id];
-                if (current_tree_node.child_is_on_disk)
+                if (!use_segment.first)
                 {
-                    LeafNode current_leaf_node;
-                    LoadLeafFromDisk(current_tree_node.children[0], current_leaf_node);
-                    for (uint32_t i = 0; i < current_leaf_node.object_count; ++i)
-                    {
-                        const EdgeDataT &current_edge = current_leaf_node.objects[i];
-                        if (ignore_tiny_components && current_edge.component_id != 0)
-                        {
-                            continue;
-                        }
-
-                        float current_ratio = 0.;
-                        FixedPointCoordinate nearest;
-                        const float current_perpendicular_distance =
-                            coordinate_calculation::perpendicular_distance(
-                                m_coordinate_list->at(current_edge.u),
-                                m_coordinate_list->at(current_edge.v), input_coordinate, nearest,
-                                current_ratio);
-
-                        BOOST_ASSERT(0. <= current_perpendicular_distance);
-
-                        if ((current_perpendicular_distance < min_dist) &&
-                            !osrm::epsilon_compare(current_perpendicular_distance, min_dist))
-                        { // found a new minimum
-                            min_dist = current_perpendicular_distance;
-                            result_phantom_node = {current_edge.forward_edge_based_node_id,
-                                                   current_edge.reverse_edge_based_node_id,
-                                                   current_edge.name_id,
-                                                   current_edge.forward_weight,
-                                                   current_edge.reverse_weight,
-                                                   current_edge.forward_offset,
-                                                   current_edge.reverse_offset,
-                                                   current_edge.packed_geometry_id,
-                                                   current_edge.component_id,
-                                                   nearest,
-                                                   current_edge.fwd_segment_position,
-                                                   current_edge.forward_travel_mode,
-                                                   current_edge.backward_travel_mode};
-                            nearest_edge = current_edge;
-                        }
-                    }
+                    results.back().forward_edge_based_node_id = SPECIAL_NODEID;
                 }
-                else
+                else if (!use_segment.second)
                 {
-                    min_max_dist = ExploreTreeNode(current_tree_node, input_coordinate, min_dist,
-                                                   min_max_dist, traversal_queue);
+                    results.back().reverse_edge_based_node_id = SPECIAL_NODEID;
                 }
             }
         }
 
-        if (result_phantom_node.location.is_valid())
-        {
-            // Hack to fix rounding errors and wandering via nodes.
-            FixUpRoundingIssue(input_coordinate, result_phantom_node);
-
-            // set forward and reverse weights on the phantom node
-            SetForwardAndReverseWeightsOnPhantomNode(nearest_edge, result_phantom_node);
-        }
-        return result_phantom_node.location.is_valid();
+        return results;
     }
 
   private:
-    inline void SetForwardAndReverseWeightsOnPhantomNode(const EdgeDataT &nearest_edge,
-                                                         PhantomNode &result_phantom_node) const
+    template <typename QueueT>
+    void ExploreLeafNode(const std::uint32_t leaf_id,
+                         const FixedPointCoordinate &input_coordinate,
+                         const std::pair<double, double> &projected_coordinate,
+                         QueueT &traversal_queue)
     {
-        const float distance_1 = coordinate_calculation::euclidean_distance(
-            m_coordinate_list->at(nearest_edge.u), result_phantom_node.location);
-        const float distance_2 = coordinate_calculation::euclidean_distance(
-            m_coordinate_list->at(nearest_edge.u), m_coordinate_list->at(nearest_edge.v));
-        const float ratio = std::min(1.f, distance_1 / distance_2);
-
-        using TreeWeightType = decltype(result_phantom_node.forward_weight);
-        static_assert(std::is_same<decltype(result_phantom_node.forward_weight),
-                                   decltype(result_phantom_node.reverse_weight)>::value,
-                      "forward and reverse weight type in tree must be the same");
-
-        if (SPECIAL_NODEID != result_phantom_node.forward_node_id)
-        {
-            const auto new_weight =
-                static_cast<TreeWeightType>(result_phantom_node.forward_weight * ratio);
-            result_phantom_node.forward_weight = new_weight;
-        }
-        if (SPECIAL_NODEID != result_phantom_node.reverse_node_id)
-        {
-            const auto new_weight =
-                static_cast<TreeWeightType>(result_phantom_node.reverse_weight * (1.f - ratio));
-            result_phantom_node.reverse_weight = new_weight;
-        }
-    }
+        LeafNode current_leaf_node;
+        LoadLeafFromDisk(leaf_id, current_leaf_node);
 
-    // fixup locations if too close to inputs
-    inline void FixUpRoundingIssue(const FixedPointCoordinate &input_coordinate,
-                                   PhantomNode &result_phantom_node) const
-    {
-        if (1 == std::abs(input_coordinate.lon - result_phantom_node.location.lon))
-        {
-            result_phantom_node.location.lon = input_coordinate.lon;
-        }
-        if (1 == std::abs(input_coordinate.lat - result_phantom_node.location.lat))
+        // current object represents a block on disk
+        for (const auto i : osrm::irange(0u, current_leaf_node.object_count))
         {
-            result_phantom_node.location.lat = input_coordinate.lat;
+            auto &current_edge = current_leaf_node.objects[i];
+            const float current_perpendicular_distance =
+                coordinate_calculation::perpendicular_distance_from_projected_coordinate(
+                    m_coordinate_list->at(current_edge.u), m_coordinate_list->at(current_edge.v),
+                    input_coordinate, projected_coordinate);
+            // distance must be non-negative
+            BOOST_ASSERT(0.f <= current_perpendicular_distance);
+
+            traversal_queue.push(QueryCandidate {current_perpendicular_distance, std::move(current_edge)});
         }
     }
 
     template <class QueueT>
-    inline float ExploreTreeNode(const TreeNode &parent,
-                                 const FixedPointCoordinate &input_coordinate,
-                                 const float min_dist,
-                                 const float min_max_dist,
-                                 QueueT &traversal_queue)
+    void ExploreTreeNode(const TreeNode &parent,
+                         const FixedPointCoordinate &input_coordinate,
+                         QueueT &traversal_queue)
     {
-        float new_min_max_dist = min_max_dist;
-        // traverse children, prune if global mindist is smaller than local one
         for (uint32_t i = 0; i < parent.child_count; ++i)
         {
             const int32_t child_id = parent.children[i];
-            const TreeNode &child_tree_node = m_search_tree[child_id];
-            const RectangleT &child_rectangle = child_tree_node.minimum_bounding_rectangle;
+            const auto &child_tree_node = m_search_tree[child_id];
+            const auto &child_rectangle = child_tree_node.minimum_bounding_rectangle;
             const float lower_bound_to_element = child_rectangle.GetMinDist(input_coordinate);
-            const float upper_bound_to_element = child_rectangle.GetMinMaxDist(input_coordinate);
-            new_min_max_dist = std::min(new_min_max_dist, upper_bound_to_element);
-            if (lower_bound_to_element > new_min_max_dist)
-            {
-                continue;
-            }
-            if (lower_bound_to_element > min_dist)
-            {
-                continue;
-            }
-            traversal_queue.emplace(lower_bound_to_element, child_id);
+            traversal_queue.push(QueryCandidate {lower_bound_to_element, m_search_tree[child_id]});
         }
-        return new_min_max_dist;
     }
 
     inline void LoadLeafFromDisk(const uint32_t leaf_id, LeafNode &result_node)
@@ -1077,8 +476,7 @@ class StaticRTree
         }
         if (!leaves_stream.good())
         {
-            leaves_stream.clear(std::ios::goodbit);
-            SimpleLogger().Write(logDEBUG) << "Resetting stale filestream";
+          throw osrm::exception("Could not read from leaf file.");
         }
         const uint64_t seek_pos = sizeof(uint64_t) + leaf_id * sizeof(LeafNode);
         leaves_stream.seekg(seek_pos);
@@ -1087,18 +485,11 @@ class StaticRTree
         BOOST_ASSERT_MSG(leaves_stream.good(), "Reading from leaf file failed.");
     }
 
-    inline bool EdgesAreEquivalent(const FixedPointCoordinate &a,
-                                   const FixedPointCoordinate &b,
-                                   const FixedPointCoordinate &c,
-                                   const FixedPointCoordinate &d) const
-    {
-        return (a == b && c == d) || (a == c && b == d) || (a == d && b == c);
-    }
-
-    inline void InitializeMBRectangle(RectangleT &rectangle,
-                                      const std::array<EdgeDataT, LEAF_NODE_SIZE> &objects,
-                                      const uint32_t element_count,
-                                      const std::vector<QueryNode> &coordinate_list)
+    template <typename CoordinateT>
+    void InitializeMBRectangle(Rectangle &rectangle,
+                               const std::array<EdgeDataT, LEAF_NODE_SIZE> &objects,
+                               const uint32_t element_count,
+                               const std::vector<CoordinateT> &coordinate_list)
     {
         for (uint32_t i = 0; i < element_count; ++i)
         {
diff --git a/datastore.cpp b/datastore.cpp
index 60c2754..504dd3d 100644
--- a/datastore.cpp
+++ b/datastore.cpp
@@ -38,7 +38,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "server/data_structures/datafacade_base.hpp"
 #include "server/data_structures/shared_datatype.hpp"
 #include "server/data_structures/shared_barriers.hpp"
-#include "util/boost_filesystem_2_fix.hpp"
 #include "util/datastore_options.hpp"
 #include "util/simple_logger.hpp"
 #include "util/osrm_exception.hpp"
@@ -46,7 +45,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "typedefs.h"
 
 #include <osrm/coordinate.hpp>
-#include <osrm/server_paths.hpp>
 
 using RTreeLeaf = BaseDataFacade<QueryEdge::EdgeData>::RTreeLeaf;
 using RTreeNode = StaticRTree<RTreeLeaf, ShM<FixedPointCoordinate, true>::vector, true>::TreeNode;
@@ -63,6 +61,7 @@ using QueryGraph = StaticGraph<QueryEdge::EdgeData>;
 
 #include <fstream>
 #include <string>
+#include <new>
 
 // delete a shared memory region. report warning if it could not be deleted
 void delete_region(const SharedDataType region)
@@ -94,515 +93,506 @@ void delete_region(const SharedDataType region)
     }
 }
 
-int main(const int argc, const char *argv[])
+int main(const int argc, const char *argv[]) try
 {
     LogPolicy::GetInstance().Unmute();
     SharedBarriers barrier;
 
-    try
-    {
 #ifdef __linux__
-        // try to disable swapping on Linux
-        const bool lock_flags = MCL_CURRENT | MCL_FUTURE;
-        if (-1 == mlockall(lock_flags))
-        {
-            SimpleLogger().Write(logWARNING) << "Process " << argv[0]
-                                             << " could not request RAM lock";
-        }
-#endif
-        try
-        {
-            boost::interprocess::scoped_lock<boost::interprocess::named_mutex> pending_lock(
-                barrier.pending_update_mutex);
-        }
-        catch (...)
-        {
-            // hard unlock in case of any exception.
-            barrier.pending_update_mutex.unlock();
-        }
-    }
-    catch (const std::exception &e)
+    // try to disable swapping on Linux
+    const bool lock_flags = MCL_CURRENT | MCL_FUTURE;
+    if (-1 == mlockall(lock_flags))
     {
-        SimpleLogger().Write(logWARNING) << "[exception] " << e.what();
+        SimpleLogger().Write(logWARNING) << "Process " << argv[0] << " could not request RAM lock";
     }
+#endif
 
     try
     {
-        SimpleLogger().Write(logDEBUG) << "Checking input parameters";
+        boost::interprocess::scoped_lock<boost::interprocess::named_mutex> pending_lock(
+            barrier.pending_update_mutex);
+    }
+    catch (...)
+    {
+        // hard unlock in case of any exception.
+        barrier.pending_update_mutex.unlock();
+    }
 
-        ServerPaths server_paths;
-        if (!GenerateDataStoreOptions(argc, argv, server_paths))
-        {
-            return 0;
-        }
+    SimpleLogger().Write(logDEBUG) << "Checking input parameters";
 
-        if (server_paths.find("hsgrdata") == server_paths.end())
-        {
-            throw osrm::exception("no hsgr file found");
-        }
-        if (server_paths.find("ramindex") == server_paths.end())
-        {
-            throw osrm::exception("no ram index file found");
-        }
-        if (server_paths.find("fileindex") == server_paths.end())
-        {
-            throw osrm::exception("no leaf index file found");
-        }
-        if (server_paths.find("nodesdata") == server_paths.end())
-        {
-            throw osrm::exception("no nodes file found");
-        }
-        if (server_paths.find("edgesdata") == server_paths.end())
-        {
-            throw osrm::exception("no edges file found");
-        }
-        if (server_paths.find("namesdata") == server_paths.end())
-        {
-            throw osrm::exception("no names file found");
-        }
-        if (server_paths.find("geometry") == server_paths.end())
-        {
-            throw osrm::exception("no geometry file found");
-        }
-        if (server_paths.find("core") == server_paths.end())
-        {
-            throw osrm::exception("no core file found");
-        }
+    std::unordered_map<std::string, boost::filesystem::path> server_paths;
+    if (!GenerateDataStoreOptions(argc, argv, server_paths))
+    {
+        return EXIT_SUCCESS;
+    }
 
-        ServerPaths::const_iterator paths_iterator = server_paths.find("hsgrdata");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        BOOST_ASSERT(!paths_iterator->second.empty());
-        const boost::filesystem::path &hsgr_path = paths_iterator->second;
-        paths_iterator = server_paths.find("timestamp");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        BOOST_ASSERT(!paths_iterator->second.empty());
-        const boost::filesystem::path &timestamp_path = paths_iterator->second;
-        paths_iterator = server_paths.find("ramindex");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        BOOST_ASSERT(!paths_iterator->second.empty());
-        const boost::filesystem::path &ram_index_path = paths_iterator->second;
-        paths_iterator = server_paths.find("fileindex");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        BOOST_ASSERT(!paths_iterator->second.empty());
-        const boost::filesystem::path index_file_path_absolute =
-            boost::filesystem::portable_canonical(paths_iterator->second);
-        const std::string &file_index_path = index_file_path_absolute.string();
-        paths_iterator = server_paths.find("nodesdata");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        BOOST_ASSERT(!paths_iterator->second.empty());
-        const boost::filesystem::path &nodes_data_path = paths_iterator->second;
-        paths_iterator = server_paths.find("edgesdata");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        BOOST_ASSERT(!paths_iterator->second.empty());
-        const boost::filesystem::path &edges_data_path = paths_iterator->second;
-        paths_iterator = server_paths.find("namesdata");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        BOOST_ASSERT(!paths_iterator->second.empty());
-        const boost::filesystem::path &names_data_path = paths_iterator->second;
-        paths_iterator = server_paths.find("geometry");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        BOOST_ASSERT(!paths_iterator->second.empty());
-        const boost::filesystem::path &geometries_data_path = paths_iterator->second;
-        paths_iterator = server_paths.find("core");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        BOOST_ASSERT(!paths_iterator->second.empty());
-        const boost::filesystem::path &core_marker_path = paths_iterator->second;
-
-        // determine segment to use
-        bool segment2_in_use = SharedMemory::RegionExists(LAYOUT_2);
-        const SharedDataType layout_region = [&]
-        {
-            return segment2_in_use ? LAYOUT_1 : LAYOUT_2;
-        }();
-        const SharedDataType data_region = [&]
-        {
-            return segment2_in_use ? DATA_1 : DATA_2;
-        }();
-        const SharedDataType previous_layout_region = [&]
-        {
-            return segment2_in_use ? LAYOUT_2 : LAYOUT_1;
-        }();
-        const SharedDataType previous_data_region = [&]
-        {
-            return segment2_in_use ? DATA_2 : DATA_1;
-        }();
+    if (server_paths.find("hsgrdata") == server_paths.end())
+    {
+        throw osrm::exception("no hsgr file found");
+    }
+    if (server_paths.find("ramindex") == server_paths.end())
+    {
+        throw osrm::exception("no ram index file found");
+    }
+    if (server_paths.find("fileindex") == server_paths.end())
+    {
+        throw osrm::exception("no leaf index file found");
+    }
+    if (server_paths.find("nodesdata") == server_paths.end())
+    {
+        throw osrm::exception("no nodes file found");
+    }
+    if (server_paths.find("edgesdata") == server_paths.end())
+    {
+        throw osrm::exception("no edges file found");
+    }
+    if (server_paths.find("namesdata") == server_paths.end())
+    {
+        throw osrm::exception("no names file found");
+    }
+    if (server_paths.find("geometry") == server_paths.end())
+    {
+        throw osrm::exception("no geometry file found");
+    }
+    if (server_paths.find("core") == server_paths.end())
+    {
+        throw osrm::exception("no core file found");
+    }
 
-        // Allocate a memory layout in shared memory, deallocate previous
-        SharedMemory *layout_memory =
-            SharedMemoryFactory::Get(layout_region, sizeof(SharedDataLayout));
-        SharedDataLayout *shared_layout_ptr = new (layout_memory->Ptr()) SharedDataLayout();
-
-        shared_layout_ptr->SetBlockSize<char>(SharedDataLayout::FILE_INDEX_PATH,
-                                              file_index_path.length() + 1);
-
-        // collect number of elements to store in shared memory object
-        SimpleLogger().Write() << "load names from: " << names_data_path;
-        // number of entries in name index
-        boost::filesystem::ifstream name_stream(names_data_path, std::ios::binary);
-        unsigned name_blocks = 0;
-        name_stream.read((char *)&name_blocks, sizeof(unsigned));
-        shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::NAME_OFFSETS, name_blocks);
-        shared_layout_ptr->SetBlockSize<typename RangeTable<16, true>::BlockT>(
-            SharedDataLayout::NAME_BLOCKS, name_blocks);
-        SimpleLogger().Write() << "name offsets size: " << name_blocks;
-        BOOST_ASSERT_MSG(0 != name_blocks, "name file broken");
-
-        unsigned number_of_chars = 0;
-        name_stream.read((char *)&number_of_chars, sizeof(unsigned));
-        shared_layout_ptr->SetBlockSize<char>(SharedDataLayout::NAME_CHAR_LIST, number_of_chars);
-
-        // Loading information for original edges
-        boost::filesystem::ifstream edges_input_stream(edges_data_path, std::ios::binary);
-        unsigned number_of_original_edges = 0;
-        edges_input_stream.read((char *)&number_of_original_edges, sizeof(unsigned));
-
-        // note: settings this all to the same size is correct, we extract them from the same struct
-        shared_layout_ptr->SetBlockSize<NodeID>(SharedDataLayout::VIA_NODE_LIST,
+    auto paths_iterator = server_paths.find("hsgrdata");
+    BOOST_ASSERT(server_paths.end() != paths_iterator);
+    BOOST_ASSERT(!paths_iterator->second.empty());
+    const boost::filesystem::path &hsgr_path = paths_iterator->second;
+    paths_iterator = server_paths.find("timestamp");
+    BOOST_ASSERT(server_paths.end() != paths_iterator);
+    BOOST_ASSERT(!paths_iterator->second.empty());
+    const boost::filesystem::path &timestamp_path = paths_iterator->second;
+    paths_iterator = server_paths.find("ramindex");
+    BOOST_ASSERT(server_paths.end() != paths_iterator);
+    BOOST_ASSERT(!paths_iterator->second.empty());
+    const boost::filesystem::path &ram_index_path = paths_iterator->second;
+    paths_iterator = server_paths.find("fileindex");
+    BOOST_ASSERT(server_paths.end() != paths_iterator);
+    BOOST_ASSERT(!paths_iterator->second.empty());
+    const boost::filesystem::path index_file_path_absolute =
+        boost::filesystem::canonical(paths_iterator->second);
+    const std::string &file_index_path = index_file_path_absolute.string();
+    paths_iterator = server_paths.find("nodesdata");
+    BOOST_ASSERT(server_paths.end() != paths_iterator);
+    BOOST_ASSERT(!paths_iterator->second.empty());
+    const boost::filesystem::path &nodes_data_path = paths_iterator->second;
+    paths_iterator = server_paths.find("edgesdata");
+    BOOST_ASSERT(server_paths.end() != paths_iterator);
+    BOOST_ASSERT(!paths_iterator->second.empty());
+    const boost::filesystem::path &edges_data_path = paths_iterator->second;
+    paths_iterator = server_paths.find("namesdata");
+    BOOST_ASSERT(server_paths.end() != paths_iterator);
+    BOOST_ASSERT(!paths_iterator->second.empty());
+    const boost::filesystem::path &names_data_path = paths_iterator->second;
+    paths_iterator = server_paths.find("geometry");
+    BOOST_ASSERT(server_paths.end() != paths_iterator);
+    BOOST_ASSERT(!paths_iterator->second.empty());
+    const boost::filesystem::path &geometries_data_path = paths_iterator->second;
+    paths_iterator = server_paths.find("core");
+    BOOST_ASSERT(server_paths.end() != paths_iterator);
+    BOOST_ASSERT(!paths_iterator->second.empty());
+    const boost::filesystem::path &core_marker_path = paths_iterator->second;
+
+    // determine segment to use
+    bool segment2_in_use = SharedMemory::RegionExists(LAYOUT_2);
+    const SharedDataType layout_region = [&]
+    {
+        return segment2_in_use ? LAYOUT_1 : LAYOUT_2;
+    }();
+    const SharedDataType data_region = [&]
+    {
+        return segment2_in_use ? DATA_1 : DATA_2;
+    }();
+    const SharedDataType previous_layout_region = [&]
+    {
+        return segment2_in_use ? LAYOUT_2 : LAYOUT_1;
+    }();
+    const SharedDataType previous_data_region = [&]
+    {
+        return segment2_in_use ? DATA_2 : DATA_1;
+    }();
+
+    // Allocate a memory layout in shared memory, deallocate previous
+    auto *layout_memory = SharedMemoryFactory::Get(layout_region, sizeof(SharedDataLayout));
+    auto *shared_layout_ptr = new (layout_memory->Ptr()) SharedDataLayout();
+
+    shared_layout_ptr->SetBlockSize<char>(SharedDataLayout::FILE_INDEX_PATH,
+                                          file_index_path.length() + 1);
+
+    // collect number of elements to store in shared memory object
+    SimpleLogger().Write() << "load names from: " << names_data_path;
+    // number of entries in name index
+    boost::filesystem::ifstream name_stream(names_data_path, std::ios::binary);
+    unsigned name_blocks = 0;
+    name_stream.read((char *)&name_blocks, sizeof(unsigned));
+    shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::NAME_OFFSETS, name_blocks);
+    shared_layout_ptr->SetBlockSize<typename RangeTable<16, true>::BlockT>(
+        SharedDataLayout::NAME_BLOCKS, name_blocks);
+    SimpleLogger().Write() << "name offsets size: " << name_blocks;
+    BOOST_ASSERT_MSG(0 != name_blocks, "name file broken");
+
+    unsigned number_of_chars = 0;
+    name_stream.read((char *)&number_of_chars, sizeof(unsigned));
+    shared_layout_ptr->SetBlockSize<char>(SharedDataLayout::NAME_CHAR_LIST, number_of_chars);
+
+    // Loading information for original edges
+    boost::filesystem::ifstream edges_input_stream(edges_data_path, std::ios::binary);
+    unsigned number_of_original_edges = 0;
+    edges_input_stream.read((char *)&number_of_original_edges, sizeof(unsigned));
+
+    // note: settings this all to the same size is correct, we extract them from the same struct
+    shared_layout_ptr->SetBlockSize<NodeID>(SharedDataLayout::VIA_NODE_LIST,
+                                            number_of_original_edges);
+    shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::NAME_ID_LIST,
+                                              number_of_original_edges);
+    shared_layout_ptr->SetBlockSize<TravelMode>(SharedDataLayout::TRAVEL_MODE,
                                                 number_of_original_edges);
-        shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::NAME_ID_LIST,
-                                                  number_of_original_edges);
-        shared_layout_ptr->SetBlockSize<TravelMode>(SharedDataLayout::TRAVEL_MODE,
-                                                    number_of_original_edges);
-        shared_layout_ptr->SetBlockSize<TurnInstruction>(SharedDataLayout::TURN_INSTRUCTION,
-                                                         number_of_original_edges);
-        // note: there are 32 geometry indicators in one unsigned block
-        shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::GEOMETRIES_INDICATORS,
-                                                  number_of_original_edges);
-
-        boost::filesystem::ifstream hsgr_input_stream(hsgr_path, std::ios::binary);
-
-        FingerPrint fingerprint_valid = FingerPrint::GetValid();
-        FingerPrint fingerprint_loaded;
-        hsgr_input_stream.read((char *)&fingerprint_loaded, sizeof(FingerPrint));
-        if (fingerprint_loaded.TestGraphUtil(fingerprint_valid))
-        {
-            SimpleLogger().Write(logDEBUG) << "Fingerprint checked out ok";
-        }
-        else
-        {
-            SimpleLogger().Write(logWARNING) << ".hsgr was prepared with different build. "
-                                                "Reprocess to get rid of this warning.";
-        }
+    shared_layout_ptr->SetBlockSize<TurnInstruction>(SharedDataLayout::TURN_INSTRUCTION,
+                                                     number_of_original_edges);
+    // note: there are 32 geometry indicators in one unsigned block
+    shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::GEOMETRIES_INDICATORS,
+                                              number_of_original_edges);
+
+    boost::filesystem::ifstream hsgr_input_stream(hsgr_path, std::ios::binary);
+
+    FingerPrint fingerprint_valid = FingerPrint::GetValid();
+    FingerPrint fingerprint_loaded;
+    hsgr_input_stream.read((char *)&fingerprint_loaded, sizeof(FingerPrint));
+    if (fingerprint_loaded.TestGraphUtil(fingerprint_valid))
+    {
+        SimpleLogger().Write(logDEBUG) << "Fingerprint checked out ok";
+    }
+    else
+    {
+        SimpleLogger().Write(logWARNING) << ".hsgr was prepared with different build. "
+                                            "Reprocess to get rid of this warning.";
+    }
 
-        // load checksum
-        unsigned checksum = 0;
-        hsgr_input_stream.read((char *)&checksum, sizeof(unsigned));
-        shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::HSGR_CHECKSUM, 1);
-        // load graph node size
-        unsigned number_of_graph_nodes = 0;
-        hsgr_input_stream.read((char *)&number_of_graph_nodes, sizeof(unsigned));
-
-        BOOST_ASSERT_MSG((0 != number_of_graph_nodes), "number of nodes is zero");
-        shared_layout_ptr->SetBlockSize<QueryGraph::NodeArrayEntry>(
-            SharedDataLayout::GRAPH_NODE_LIST, number_of_graph_nodes);
-
-        // load graph edge size
-        unsigned number_of_graph_edges = 0;
-        hsgr_input_stream.read((char *)&number_of_graph_edges, sizeof(unsigned));
-        // BOOST_ASSERT_MSG(0 != number_of_graph_edges, "number of graph edges is zero");
-        shared_layout_ptr->SetBlockSize<QueryGraph::EdgeArrayEntry>(
-            SharedDataLayout::GRAPH_EDGE_LIST, number_of_graph_edges);
-
-        // load rsearch tree size
-        boost::filesystem::ifstream tree_node_file(ram_index_path, std::ios::binary);
-
-        uint32_t tree_size = 0;
-        tree_node_file.read((char *)&tree_size, sizeof(uint32_t));
-        shared_layout_ptr->SetBlockSize<RTreeNode>(SharedDataLayout::R_SEARCH_TREE, tree_size);
-
-        // load timestamp size
-        std::string m_timestamp;
-        if (boost::filesystem::exists(timestamp_path))
-        {
-            boost::filesystem::ifstream timestamp_stream(timestamp_path);
-            if (!timestamp_stream)
-            {
-                SimpleLogger().Write(logWARNING) << timestamp_path
-                                                 << " not found. setting to default";
-            }
-            else
-            {
-                getline(timestamp_stream, m_timestamp);
-                timestamp_stream.close();
-            }
-        }
-        if (m_timestamp.empty())
-        {
-            m_timestamp = "n/a";
-        }
-        if (25 < m_timestamp.length())
-        {
-            m_timestamp.resize(25);
-        }
-        shared_layout_ptr->SetBlockSize<char>(SharedDataLayout::TIMESTAMP, m_timestamp.length());
-
-        // load core marker size
-        boost::filesystem::ifstream core_marker_file(core_marker_path, std::ios::binary);
-
-        uint32_t number_of_core_markers = 0;
-        core_marker_file.read((char *)&number_of_core_markers, sizeof(uint32_t));
-        shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::CORE_MARKER, number_of_core_markers);
-
-        // load coordinate size
-        boost::filesystem::ifstream nodes_input_stream(nodes_data_path, std::ios::binary);
-        unsigned coordinate_list_size = 0;
-        nodes_input_stream.read((char *)&coordinate_list_size, sizeof(unsigned));
-        shared_layout_ptr->SetBlockSize<FixedPointCoordinate>(SharedDataLayout::COORDINATE_LIST,
-                                                              coordinate_list_size);
-
-        // load geometries sizes
-        std::ifstream geometry_input_stream(geometries_data_path.string().c_str(),
-                                            std::ios::binary);
-        unsigned number_of_geometries_indices = 0;
-        unsigned number_of_compressed_geometries = 0;
-
-        geometry_input_stream.read((char *)&number_of_geometries_indices, sizeof(unsigned));
-        shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::GEOMETRIES_INDEX,
-                                                  number_of_geometries_indices);
-        boost::iostreams::seek(geometry_input_stream,
-                               number_of_geometries_indices * sizeof(unsigned), BOOST_IOS::cur);
-        geometry_input_stream.read((char *)&number_of_compressed_geometries, sizeof(unsigned));
-        shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::GEOMETRIES_LIST,
-                                                  number_of_compressed_geometries);
-        // allocate shared memory block
-        SimpleLogger().Write() << "allocating shared memory of "
-                               << shared_layout_ptr->GetSizeOfLayout() << " bytes";
-        SharedMemory *shared_memory =
-            SharedMemoryFactory::Get(data_region, shared_layout_ptr->GetSizeOfLayout());
-        char *shared_memory_ptr = static_cast<char *>(shared_memory->Ptr());
-
-        // read actual data into shared memory object //
-
-        // hsgr checksum
-        unsigned *checksum_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
-            shared_memory_ptr, SharedDataLayout::HSGR_CHECKSUM);
-        *checksum_ptr = checksum;
-
-        // ram index file name
-        char *file_index_path_ptr = shared_layout_ptr->GetBlockPtr<char, true>(
-            shared_memory_ptr, SharedDataLayout::FILE_INDEX_PATH);
-        // make sure we have 0 ending
-        std::fill(file_index_path_ptr,
-                  file_index_path_ptr +
-                      shared_layout_ptr->GetBlockSize(SharedDataLayout::FILE_INDEX_PATH),
-                  0);
-        std::copy(file_index_path.begin(), file_index_path.end(), file_index_path_ptr);
-
-        // Loading street names
-        unsigned *name_offsets_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
-            shared_memory_ptr, SharedDataLayout::NAME_OFFSETS);
-        if (shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_OFFSETS) > 0)
+    // load checksum
+    unsigned checksum = 0;
+    hsgr_input_stream.read((char *)&checksum, sizeof(unsigned));
+    shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::HSGR_CHECKSUM, 1);
+    // load graph node size
+    unsigned number_of_graph_nodes = 0;
+    hsgr_input_stream.read((char *)&number_of_graph_nodes, sizeof(unsigned));
+
+    BOOST_ASSERT_MSG((0 != number_of_graph_nodes), "number of nodes is zero");
+    shared_layout_ptr->SetBlockSize<QueryGraph::NodeArrayEntry>(SharedDataLayout::GRAPH_NODE_LIST,
+                                                                number_of_graph_nodes);
+
+    // load graph edge size
+    unsigned number_of_graph_edges = 0;
+    hsgr_input_stream.read((char *)&number_of_graph_edges, sizeof(unsigned));
+    // BOOST_ASSERT_MSG(0 != number_of_graph_edges, "number of graph edges is zero");
+    shared_layout_ptr->SetBlockSize<QueryGraph::EdgeArrayEntry>(SharedDataLayout::GRAPH_EDGE_LIST,
+                                                                number_of_graph_edges);
+
+    // load rsearch tree size
+    boost::filesystem::ifstream tree_node_file(ram_index_path, std::ios::binary);
+
+    uint32_t tree_size = 0;
+    tree_node_file.read((char *)&tree_size, sizeof(uint32_t));
+    shared_layout_ptr->SetBlockSize<RTreeNode>(SharedDataLayout::R_SEARCH_TREE, tree_size);
+
+    // load timestamp size
+    std::string m_timestamp;
+    if (boost::filesystem::exists(timestamp_path))
+    {
+        boost::filesystem::ifstream timestamp_stream(timestamp_path);
+        if (!timestamp_stream)
         {
-            name_stream.read((char *)name_offsets_ptr,
-                             shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_OFFSETS));
+            SimpleLogger().Write(logWARNING) << timestamp_path << " not found. setting to default";
         }
-
-        unsigned *name_blocks_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
-            shared_memory_ptr, SharedDataLayout::NAME_BLOCKS);
-        if (shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_BLOCKS) > 0)
+        else
         {
-            name_stream.read((char *)name_blocks_ptr,
-                             shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_BLOCKS));
+            getline(timestamp_stream, m_timestamp);
+            timestamp_stream.close();
         }
+    }
+    if (m_timestamp.empty())
+    {
+        m_timestamp = "n/a";
+    }
+    if (25 < m_timestamp.length())
+    {
+        m_timestamp.resize(25);
+    }
+    shared_layout_ptr->SetBlockSize<char>(SharedDataLayout::TIMESTAMP, m_timestamp.length());
+
+    // load core marker size
+    boost::filesystem::ifstream core_marker_file(core_marker_path, std::ios::binary);
+
+    uint32_t number_of_core_markers = 0;
+    core_marker_file.read((char *)&number_of_core_markers, sizeof(uint32_t));
+    shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::CORE_MARKER,
+                                              number_of_core_markers);
+
+    // load coordinate size
+    boost::filesystem::ifstream nodes_input_stream(nodes_data_path, std::ios::binary);
+    unsigned coordinate_list_size = 0;
+    nodes_input_stream.read((char *)&coordinate_list_size, sizeof(unsigned));
+    shared_layout_ptr->SetBlockSize<FixedPointCoordinate>(SharedDataLayout::COORDINATE_LIST,
+                                                          coordinate_list_size);
+
+    // load geometries sizes
+    std::ifstream geometry_input_stream(geometries_data_path.string().c_str(), std::ios::binary);
+    unsigned number_of_geometries_indices = 0;
+    unsigned number_of_compressed_geometries = 0;
+
+    geometry_input_stream.read((char *)&number_of_geometries_indices, sizeof(unsigned));
+    shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::GEOMETRIES_INDEX,
+                                              number_of_geometries_indices);
+    boost::iostreams::seek(geometry_input_stream, number_of_geometries_indices * sizeof(unsigned),
+                           BOOST_IOS::cur);
+    geometry_input_stream.read((char *)&number_of_compressed_geometries, sizeof(unsigned));
+    shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::GEOMETRIES_LIST,
+                                              number_of_compressed_geometries);
+    // allocate shared memory block
+    SimpleLogger().Write() << "allocating shared memory of " << shared_layout_ptr->GetSizeOfLayout()
+                           << " bytes";
+    SharedMemory *shared_memory =
+        SharedMemoryFactory::Get(data_region, shared_layout_ptr->GetSizeOfLayout());
+    char *shared_memory_ptr = static_cast<char *>(shared_memory->Ptr());
+
+    // read actual data into shared memory object //
+
+    // hsgr checksum
+    unsigned *checksum_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
+        shared_memory_ptr, SharedDataLayout::HSGR_CHECKSUM);
+    *checksum_ptr = checksum;
+
+    // ram index file name
+    char *file_index_path_ptr = shared_layout_ptr->GetBlockPtr<char, true>(
+        shared_memory_ptr, SharedDataLayout::FILE_INDEX_PATH);
+    // make sure we have 0 ending
+    std::fill(file_index_path_ptr,
+              file_index_path_ptr +
+                  shared_layout_ptr->GetBlockSize(SharedDataLayout::FILE_INDEX_PATH),
+              0);
+    std::copy(file_index_path.begin(), file_index_path.end(), file_index_path_ptr);
+
+    // Loading street names
+    unsigned *name_offsets_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
+        shared_memory_ptr, SharedDataLayout::NAME_OFFSETS);
+    if (shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_OFFSETS) > 0)
+    {
+        name_stream.read((char *)name_offsets_ptr,
+                         shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_OFFSETS));
+    }
 
-        char *name_char_ptr = shared_layout_ptr->GetBlockPtr<char, true>(
-            shared_memory_ptr, SharedDataLayout::NAME_CHAR_LIST);
-        unsigned temp_length;
-        name_stream.read((char *)&temp_length, sizeof(unsigned));
+    unsigned *name_blocks_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
+        shared_memory_ptr, SharedDataLayout::NAME_BLOCKS);
+    if (shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_BLOCKS) > 0)
+    {
+        name_stream.read((char *)name_blocks_ptr,
+                         shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_BLOCKS));
+    }
 
-        BOOST_ASSERT_MSG(temp_length ==
-                             shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_CHAR_LIST),
-                         "Name file corrupted!");
+    char *name_char_ptr = shared_layout_ptr->GetBlockPtr<char, true>(
+        shared_memory_ptr, SharedDataLayout::NAME_CHAR_LIST);
+    unsigned temp_length;
+    name_stream.read((char *)&temp_length, sizeof(unsigned));
 
-        if (shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_CHAR_LIST) > 0)
-        {
-            name_stream.read(name_char_ptr,
-                             shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_CHAR_LIST));
-        }
+    BOOST_ASSERT_MSG(temp_length ==
+                         shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_CHAR_LIST),
+                     "Name file corrupted!");
 
-        name_stream.close();
+    if (shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_CHAR_LIST) > 0)
+    {
+        name_stream.read(name_char_ptr,
+                         shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_CHAR_LIST));
+    }
 
-        // load original edge information
-        NodeID *via_node_ptr = shared_layout_ptr->GetBlockPtr<NodeID, true>(
-            shared_memory_ptr, SharedDataLayout::VIA_NODE_LIST);
+    name_stream.close();
 
-        unsigned *name_id_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
-            shared_memory_ptr, SharedDataLayout::NAME_ID_LIST);
+    // load original edge information
+    NodeID *via_node_ptr = shared_layout_ptr->GetBlockPtr<NodeID, true>(
+        shared_memory_ptr, SharedDataLayout::VIA_NODE_LIST);
 
-        TravelMode *travel_mode_ptr = shared_layout_ptr->GetBlockPtr<TravelMode, true>(
-            shared_memory_ptr, SharedDataLayout::TRAVEL_MODE);
+    unsigned *name_id_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
+        shared_memory_ptr, SharedDataLayout::NAME_ID_LIST);
 
-        TurnInstruction *turn_instructions_ptr =
-            shared_layout_ptr->GetBlockPtr<TurnInstruction, true>(
-                shared_memory_ptr, SharedDataLayout::TURN_INSTRUCTION);
+    TravelMode *travel_mode_ptr = shared_layout_ptr->GetBlockPtr<TravelMode, true>(
+        shared_memory_ptr, SharedDataLayout::TRAVEL_MODE);
 
-        unsigned *geometries_indicator_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
-            shared_memory_ptr, SharedDataLayout::GEOMETRIES_INDICATORS);
+    TurnInstruction *turn_instructions_ptr = shared_layout_ptr->GetBlockPtr<TurnInstruction, true>(
+        shared_memory_ptr, SharedDataLayout::TURN_INSTRUCTION);
 
-        OriginalEdgeData current_edge_data;
-        for (unsigned i = 0; i < number_of_original_edges; ++i)
-        {
-            edges_input_stream.read((char *)&(current_edge_data), sizeof(OriginalEdgeData));
-            via_node_ptr[i] = current_edge_data.via_node;
-            name_id_ptr[i] = current_edge_data.name_id;
-            travel_mode_ptr[i] = current_edge_data.travel_mode;
-            turn_instructions_ptr[i] = current_edge_data.turn_instruction;
+    unsigned *geometries_indicator_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
+        shared_memory_ptr, SharedDataLayout::GEOMETRIES_INDICATORS);
 
-            const unsigned bucket = i / 32;
-            const unsigned offset = i % 32;
-            const unsigned value = [&]
-            {
-                unsigned return_value = 0;
-                if (0 != offset)
-                {
-                    return_value = geometries_indicator_ptr[bucket];
-                }
-                return return_value;
-            }();
-            if (current_edge_data.compressed_geometry)
+    OriginalEdgeData current_edge_data;
+    for (unsigned i = 0; i < number_of_original_edges; ++i)
+    {
+        edges_input_stream.read((char *)&(current_edge_data), sizeof(OriginalEdgeData));
+        via_node_ptr[i] = current_edge_data.via_node;
+        name_id_ptr[i] = current_edge_data.name_id;
+        travel_mode_ptr[i] = current_edge_data.travel_mode;
+        turn_instructions_ptr[i] = current_edge_data.turn_instruction;
+
+        const unsigned bucket = i / 32;
+        const unsigned offset = i % 32;
+        const unsigned value = [&]
+        {
+            unsigned return_value = 0;
+            if (0 != offset)
             {
-                geometries_indicator_ptr[bucket] = (value | (1 << offset));
+                return_value = geometries_indicator_ptr[bucket];
             }
-        }
-        edges_input_stream.close();
-
-        // load compressed geometry
-        unsigned temporary_value;
-        unsigned *geometries_index_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
-            shared_memory_ptr, SharedDataLayout::GEOMETRIES_INDEX);
-        geometry_input_stream.seekg(0, geometry_input_stream.beg);
-        geometry_input_stream.read((char *)&temporary_value, sizeof(unsigned));
-        BOOST_ASSERT(temporary_value ==
-                     shared_layout_ptr->num_entries[SharedDataLayout::GEOMETRIES_INDEX]);
-
-        if (shared_layout_ptr->GetBlockSize(SharedDataLayout::GEOMETRIES_INDEX) > 0)
+            return return_value;
+        }();
+        if (current_edge_data.compressed_geometry)
         {
-            geometry_input_stream.read(
-                (char *)geometries_index_ptr,
-                shared_layout_ptr->GetBlockSize(SharedDataLayout::GEOMETRIES_INDEX));
+            geometries_indicator_ptr[bucket] = (value | (1 << offset));
         }
-        unsigned *geometries_list_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
-            shared_memory_ptr, SharedDataLayout::GEOMETRIES_LIST);
+    }
+    edges_input_stream.close();
+
+    // load compressed geometry
+    unsigned temporary_value;
+    unsigned *geometries_index_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
+        shared_memory_ptr, SharedDataLayout::GEOMETRIES_INDEX);
+    geometry_input_stream.seekg(0, geometry_input_stream.beg);
+    geometry_input_stream.read((char *)&temporary_value, sizeof(unsigned));
+    BOOST_ASSERT(temporary_value ==
+                 shared_layout_ptr->num_entries[SharedDataLayout::GEOMETRIES_INDEX]);
+
+    if (shared_layout_ptr->GetBlockSize(SharedDataLayout::GEOMETRIES_INDEX) > 0)
+    {
+        geometry_input_stream.read(
+            (char *)geometries_index_ptr,
+            shared_layout_ptr->GetBlockSize(SharedDataLayout::GEOMETRIES_INDEX));
+    }
+    unsigned *geometries_list_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
+        shared_memory_ptr, SharedDataLayout::GEOMETRIES_LIST);
 
-        geometry_input_stream.read((char *)&temporary_value, sizeof(unsigned));
-        BOOST_ASSERT(temporary_value ==
-                     shared_layout_ptr->num_entries[SharedDataLayout::GEOMETRIES_LIST]);
+    geometry_input_stream.read((char *)&temporary_value, sizeof(unsigned));
+    BOOST_ASSERT(temporary_value ==
+                 shared_layout_ptr->num_entries[SharedDataLayout::GEOMETRIES_LIST]);
 
-        if (shared_layout_ptr->GetBlockSize(SharedDataLayout::GEOMETRIES_LIST) > 0)
-        {
-            geometry_input_stream.read(
-                (char *)geometries_list_ptr,
-                shared_layout_ptr->GetBlockSize(SharedDataLayout::GEOMETRIES_LIST));
-        }
+    if (shared_layout_ptr->GetBlockSize(SharedDataLayout::GEOMETRIES_LIST) > 0)
+    {
+        geometry_input_stream.read(
+            (char *)geometries_list_ptr,
+            shared_layout_ptr->GetBlockSize(SharedDataLayout::GEOMETRIES_LIST));
+    }
 
-        // Loading list of coordinates
-        FixedPointCoordinate *coordinates_ptr =
-            shared_layout_ptr->GetBlockPtr<FixedPointCoordinate, true>(
-                shared_memory_ptr, SharedDataLayout::COORDINATE_LIST);
+    // Loading list of coordinates
+    FixedPointCoordinate *coordinates_ptr =
+        shared_layout_ptr->GetBlockPtr<FixedPointCoordinate, true>(
+            shared_memory_ptr, SharedDataLayout::COORDINATE_LIST);
 
-        QueryNode current_node;
-        for (unsigned i = 0; i < coordinate_list_size; ++i)
-        {
-            nodes_input_stream.read((char *)&current_node, sizeof(QueryNode));
-            coordinates_ptr[i] = FixedPointCoordinate(current_node.lat, current_node.lon);
-        }
-        nodes_input_stream.close();
+    QueryNode current_node;
+    for (unsigned i = 0; i < coordinate_list_size; ++i)
+    {
+        nodes_input_stream.read((char *)&current_node, sizeof(QueryNode));
+        coordinates_ptr[i] = FixedPointCoordinate(current_node.lat, current_node.lon);
+    }
+    nodes_input_stream.close();
 
-        // store timestamp
-        char *timestamp_ptr = shared_layout_ptr->GetBlockPtr<char, true>(
-            shared_memory_ptr, SharedDataLayout::TIMESTAMP);
-        std::copy(m_timestamp.c_str(), m_timestamp.c_str() + m_timestamp.length(), timestamp_ptr);
+    // store timestamp
+    char *timestamp_ptr =
+        shared_layout_ptr->GetBlockPtr<char, true>(shared_memory_ptr, SharedDataLayout::TIMESTAMP);
+    std::copy(m_timestamp.c_str(), m_timestamp.c_str() + m_timestamp.length(), timestamp_ptr);
 
-        // store search tree portion of rtree
-        char *rtree_ptr = shared_layout_ptr->GetBlockPtr<char, true>(
-            shared_memory_ptr, SharedDataLayout::R_SEARCH_TREE);
+    // store search tree portion of rtree
+    char *rtree_ptr = shared_layout_ptr->GetBlockPtr<char, true>(shared_memory_ptr,
+                                                                 SharedDataLayout::R_SEARCH_TREE);
 
-        if (tree_size > 0)
-        {
-            tree_node_file.read(rtree_ptr, sizeof(RTreeNode) * tree_size);
-        }
-        tree_node_file.close();
+    if (tree_size > 0)
+    {
+        tree_node_file.read(rtree_ptr, sizeof(RTreeNode) * tree_size);
+    }
+    tree_node_file.close();
 
-        // load core markers
-        std::vector<char> unpacked_core_markers(number_of_core_markers);
-        core_marker_file.read((char *)unpacked_core_markers.data(), sizeof(char)*number_of_core_markers);
+    // load core markers
+    std::vector<char> unpacked_core_markers(number_of_core_markers);
+    core_marker_file.read((char *)unpacked_core_markers.data(),
+                          sizeof(char) * number_of_core_markers);
 
-        unsigned *core_marker_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
-            shared_memory_ptr, SharedDataLayout::CORE_MARKER);
+    unsigned *core_marker_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
+        shared_memory_ptr, SharedDataLayout::CORE_MARKER);
 
-        for (auto i = 0u; i < number_of_core_markers; ++i)
-        {
-            BOOST_ASSERT(unpacked_core_markers[i] == 0 || unpacked_core_markers[i] == 1);
+    for (auto i = 0u; i < number_of_core_markers; ++i)
+    {
+        BOOST_ASSERT(unpacked_core_markers[i] == 0 || unpacked_core_markers[i] == 1);
 
-            if (unpacked_core_markers[i] == 1)
+        if (unpacked_core_markers[i] == 1)
+        {
+            const unsigned bucket = i / 32;
+            const unsigned offset = i % 32;
+            const unsigned value = [&]
             {
-                const unsigned bucket = i / 32;
-                const unsigned offset = i % 32;
-                const unsigned value = [&]
+                unsigned return_value = 0;
+                if (0 != offset)
                 {
-                    unsigned return_value = 0;
-                    if (0 != offset)
-                    {
-                        return_value = core_marker_ptr[bucket];
-                    }
-                    return return_value;
-                }();
-
-                core_marker_ptr[bucket] = (value | (1 << offset));
-            }
-        }
-
-        // load the nodes of the search graph
-        QueryGraph::NodeArrayEntry *graph_node_list_ptr =
-            shared_layout_ptr->GetBlockPtr<QueryGraph::NodeArrayEntry, true>(
-                shared_memory_ptr, SharedDataLayout::GRAPH_NODE_LIST);
-        if (shared_layout_ptr->GetBlockSize(SharedDataLayout::GRAPH_NODE_LIST) > 0)
-        {
-            hsgr_input_stream.read(
-                (char *)graph_node_list_ptr,
-                shared_layout_ptr->GetBlockSize(SharedDataLayout::GRAPH_NODE_LIST));
-        }
+                    return_value = core_marker_ptr[bucket];
+                }
+                return return_value;
+            }();
 
-        // load the edges of the search graph
-        QueryGraph::EdgeArrayEntry *graph_edge_list_ptr =
-            shared_layout_ptr->GetBlockPtr<QueryGraph::EdgeArrayEntry, true>(
-                shared_memory_ptr, SharedDataLayout::GRAPH_EDGE_LIST);
-        if (shared_layout_ptr->GetBlockSize(SharedDataLayout::GRAPH_EDGE_LIST) > 0)
-        {
-            hsgr_input_stream.read(
-                (char *)graph_edge_list_ptr,
-                shared_layout_ptr->GetBlockSize(SharedDataLayout::GRAPH_EDGE_LIST));
+            core_marker_ptr[bucket] = (value | (1 << offset));
         }
-        hsgr_input_stream.close();
+    }
 
-        // acquire lock
-        SharedMemory *data_type_memory =
-            SharedMemoryFactory::Get(CURRENT_REGIONS, sizeof(SharedDataTimestamp), true, false);
-        SharedDataTimestamp *data_timestamp_ptr =
-            static_cast<SharedDataTimestamp *>(data_type_memory->Ptr());
+    // load the nodes of the search graph
+    QueryGraph::NodeArrayEntry *graph_node_list_ptr =
+        shared_layout_ptr->GetBlockPtr<QueryGraph::NodeArrayEntry, true>(
+            shared_memory_ptr, SharedDataLayout::GRAPH_NODE_LIST);
+    if (shared_layout_ptr->GetBlockSize(SharedDataLayout::GRAPH_NODE_LIST) > 0)
+    {
+        hsgr_input_stream.read((char *)graph_node_list_ptr,
+                               shared_layout_ptr->GetBlockSize(SharedDataLayout::GRAPH_NODE_LIST));
+    }
 
-        boost::interprocess::scoped_lock<boost::interprocess::named_mutex> query_lock(
-            barrier.query_mutex);
+    // load the edges of the search graph
+    QueryGraph::EdgeArrayEntry *graph_edge_list_ptr =
+        shared_layout_ptr->GetBlockPtr<QueryGraph::EdgeArrayEntry, true>(
+            shared_memory_ptr, SharedDataLayout::GRAPH_EDGE_LIST);
+    if (shared_layout_ptr->GetBlockSize(SharedDataLayout::GRAPH_EDGE_LIST) > 0)
+    {
+        hsgr_input_stream.read((char *)graph_edge_list_ptr,
+                               shared_layout_ptr->GetBlockSize(SharedDataLayout::GRAPH_EDGE_LIST));
+    }
+    hsgr_input_stream.close();
 
-        // notify all processes that were waiting for this condition
-        if (0 < barrier.number_of_queries)
-        {
-            barrier.no_running_queries_condition.wait(query_lock);
-        }
+    // acquire lock
+    SharedMemory *data_type_memory =
+        SharedMemoryFactory::Get(CURRENT_REGIONS, sizeof(SharedDataTimestamp), true, false);
+    SharedDataTimestamp *data_timestamp_ptr =
+        static_cast<SharedDataTimestamp *>(data_type_memory->Ptr());
 
-        data_timestamp_ptr->layout = layout_region;
-        data_timestamp_ptr->data = data_region;
-        data_timestamp_ptr->timestamp += 1;
-        delete_region(previous_data_region);
-        delete_region(previous_layout_region);
-        SimpleLogger().Write() << "all data loaded";
+    boost::interprocess::scoped_lock<boost::interprocess::named_mutex> query_lock(
+        barrier.query_mutex);
 
-        shared_layout_ptr->PrintInformation();
-    }
-    catch (const std::exception &e)
+    // notify all processes that were waiting for this condition
+    if (0 < barrier.number_of_queries)
     {
-        SimpleLogger().Write(logWARNING) << "caught exception: " << e.what();
+        barrier.no_running_queries_condition.wait(query_lock);
     }
 
-    return 0;
+    data_timestamp_ptr->layout = layout_region;
+    data_timestamp_ptr->data = data_region;
+    data_timestamp_ptr->timestamp += 1;
+    delete_region(previous_data_region);
+    delete_region(previous_layout_region);
+    SimpleLogger().Write() << "all data loaded";
+
+    shared_layout_ptr->PrintInformation();
+}
+catch (const std::bad_alloc &e)
+{
+    SimpleLogger().Write(logWARNING) << "[exception] " << e.what();
+    SimpleLogger().Write(logWARNING) << "Please provide more memory or disable locking the virtual "
+                                        "address space (note: this makes OSRM swap, i.e. slow)";
+    return EXIT_FAILURE;
+}
+catch (const std::exception &e)
+{
+    SimpleLogger().Write(logWARNING) << "caught exception: " << e.what();
 }
diff --git a/descriptors/description_factory.cpp b/descriptors/description_factory.cpp
index 3609bc3..5086e0d 100644
--- a/descriptors/description_factory.cpp
+++ b/descriptors/description_factory.cpp
@@ -127,7 +127,7 @@ void DescriptionFactory::Run(const unsigned zoom_level)
     {
         // move down names by one, q&d hack
         path_description[i - 1].name_id = path_description[i].name_id;
-        path_description[i].length = coordinate_calculation::euclidean_distance(
+        path_description[i].length = coordinate_calculation::great_circle_distance(
             path_description[i - 1].location, path_description[i].location);
     }
 
@@ -230,18 +230,20 @@ void DescriptionFactory::Run(const unsigned zoom_level)
                 return;
             }
 
-            ++necessary_segments;
-
             if (first.is_via_location)
             { // mark the end of a leg (of several segments)
                 via_indices.push_back(necessary_segments);
             }
 
-            const double angle = coordinate_calculation::bearing(first.location, second.location);
-            first.bearing = static_cast<short>(angle * 10);
+            const double post_turn_bearing = coordinate_calculation::bearing(first.location, second.location);
+            const double pre_turn_bearing = coordinate_calculation::bearing(second.location, first.location);
+            first.post_turn_bearing = static_cast<short>(post_turn_bearing * 10);
+            first.pre_turn_bearing = static_cast<short>(pre_turn_bearing * 10);
+
+            ++necessary_segments;
         });
 
-    via_indices.push_back(necessary_segments + 1);
+    via_indices.push_back(necessary_segments);
     BOOST_ASSERT(via_indices.size() >= 2);
     return;
 }
diff --git a/descriptors/json_descriptor.hpp b/descriptors/json_descriptor.hpp
index eb5edbf..a4344d9 100644
--- a/descriptors/json_descriptor.hpp
+++ b/descriptors/json_descriptor.hpp
@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "../data_structures/segment_information.hpp"
 #include "../data_structures/turn_instructions.hpp"
 #include "../util/bearing.hpp"
+#include "../util/cast.hpp"
 #include "../util/integer_range.hpp"
 #include "../util/json_renderer.hpp"
 #include "../util/simple_logger.hpp"
@@ -43,7 +44,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <osrm/json_container.hpp>
 
+#include <limits>
 #include <algorithm>
+#include <string>
 
 template <class DataFacadeT> class JSONDescriptor final : public BaseDescriptor<DataFacadeT>
 {
@@ -100,9 +103,6 @@ template <class DataFacadeT> class JSONDescriptor final : public BaseDescriptor<
         if (INVALID_EDGE_WEIGHT == raw_route.shortest_path_length)
         {
             // We do not need to do much, if there is no route ;-)
-            json_result.values["status"] = 207;
-            json_result.values["status_message"] = "Cannot find route between points";
-            // osrm::json::render(reply.content, json_result);
             return;
         }
 
@@ -113,8 +113,6 @@ template <class DataFacadeT> class JSONDescriptor final : public BaseDescriptor<
         description_factory.SetStartSegment(
             raw_route.segment_end_coordinates.front().source_phantom,
             raw_route.source_traversed_in_reverse.front());
-        json_result.values["status"] = 0;
-        json_result.values["status_message"] = "Found route between points";
 
         // for each unpacked segment add the leg to the description
         for (const auto i : osrm::irange<std::size_t>(0, raw_route.unpacked_path_segments.size()))
@@ -293,14 +291,13 @@ template <class DataFacadeT> class JSONDescriptor final : public BaseDescriptor<
                                                      std::vector<Segment> &route_segments_list) const
     {
         osrm::json::Array json_instruction_array;
-
         // Segment information has following format:
         //["instruction id","streetname",length,position,time,"length","earth_direction",azimuth]
         unsigned necessary_segments_running_index = 0;
 
         struct RoundAbout
         {
-            RoundAbout() : start_index(INT_MAX), name_id(INVALID_NAMEID), leave_at_exit(INT_MAX) {}
+            RoundAbout() : start_index(std::numeric_limits<int>::max()), name_id(INVALID_NAMEID), leave_at_exit(std::numeric_limits<int>::max()) {}
             int start_index;
             unsigned name_id;
             int leave_at_exit;
@@ -327,18 +324,18 @@ template <class DataFacadeT> class JSONDescriptor final : public BaseDescriptor<
                     std::string current_turn_instruction;
                     if (TurnInstruction::LeaveRoundAbout == current_instruction)
                     {
-                        temp_instruction = cast::integral_to_string(
+                        temp_instruction = std::to_string(
                             cast::enum_to_underlying(TurnInstruction::EnterRoundAbout));
                         current_turn_instruction += temp_instruction;
                         current_turn_instruction += "-";
-                        temp_instruction = cast::integral_to_string(round_about.leave_at_exit + 1);
+                        temp_instruction = std::to_string(round_about.leave_at_exit + 1);
                         current_turn_instruction += temp_instruction;
                         round_about.leave_at_exit = 0;
                     }
                     else
                     {
                         temp_instruction =
-                            cast::integral_to_string(cast::enum_to_underlying(current_instruction));
+                            std::to_string(cast::enum_to_underlying(current_instruction));
                         current_turn_instruction += temp_instruction;
                     }
                     json_instruction_row.values.push_back(current_turn_instruction);
@@ -348,17 +345,27 @@ template <class DataFacadeT> class JSONDescriptor final : public BaseDescriptor<
                     json_instruction_row.values.push_back(necessary_segments_running_index);
                     json_instruction_row.values.push_back(std::round(segment.duration / 10.));
                     json_instruction_row.values.push_back(
-                        cast::integral_to_string(static_cast<unsigned>(segment.length)) + "m");
-                    const double bearing_value = (segment.bearing / 10.);
-                    json_instruction_row.values.push_back(bearing::get(bearing_value));
+                        std::to_string(static_cast<unsigned>(segment.length)) + "m");
+
+                    // post turn bearing
+                    const double post_turn_bearing_value = (segment.post_turn_bearing / 10.);
+                    json_instruction_row.values.push_back(bearing::get(post_turn_bearing_value));
                     json_instruction_row.values.push_back(
-                        static_cast<unsigned>(round(bearing_value)));
+                        static_cast<unsigned>(round(post_turn_bearing_value)));
+
                     json_instruction_row.values.push_back(segment.travel_mode);
 
+                    // pre turn bearing
+                    const double pre_turn_bearing_value = (segment.pre_turn_bearing / 10.);
+                    json_instruction_row.values.push_back(bearing::get(pre_turn_bearing_value));
+                    json_instruction_row.values.push_back(
+                        static_cast<unsigned>(round(pre_turn_bearing_value)));
+
+                    json_instruction_array.values.push_back(json_instruction_row);
+
                     route_segments_list.emplace_back(
                         segment.name_id, static_cast<int>(segment.length),
                         static_cast<unsigned>(route_segments_list.size()));
-                    json_instruction_array.values.push_back(json_instruction_row);
                 }
             }
             else if (TurnInstruction::StayOnRoundAbout == current_instruction)
@@ -372,8 +379,8 @@ template <class DataFacadeT> class JSONDescriptor final : public BaseDescriptor<
         }
 
         osrm::json::Array json_last_instruction_row;
-        temp_instruction = cast::integral_to_string(
-            cast::enum_to_underlying(TurnInstruction::ReachedYourDestination));
+        temp_instruction =
+            std::to_string(cast::enum_to_underlying(TurnInstruction::ReachedYourDestination));
         json_last_instruction_row.values.push_back(temp_instruction);
         json_last_instruction_row.values.push_back("");
         json_last_instruction_row.values.push_back(0);
@@ -382,6 +389,8 @@ template <class DataFacadeT> class JSONDescriptor final : public BaseDescriptor<
         json_last_instruction_row.values.push_back("0m");
         json_last_instruction_row.values.push_back(bearing::get(0.0));
         json_last_instruction_row.values.push_back(0.);
+        json_last_instruction_row.values.push_back(bearing::get(0.0));
+        json_last_instruction_row.values.push_back(0.);
         json_instruction_array.values.push_back(json_last_instruction_row);
 
         return json_instruction_array;
diff --git a/extract.cpp b/extract.cpp
index 5f88cc3..dabc19a 100644
--- a/extract.cpp
+++ b/extract.cpp
@@ -31,53 +31,59 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <boost/filesystem.hpp>
 
+#include <cstdlib>
 #include <exception>
+#include <new>
 
-int main(int argc, char *argv[])
+int main(int argc, char *argv[]) try
 {
-    try
+    LogPolicy::GetInstance().Unmute();
+    ExtractorConfig extractor_config;
+
+    const return_code result = ExtractorOptions::ParseArguments(argc, argv, extractor_config);
+
+    if (return_code::fail == result)
     {
-        LogPolicy::GetInstance().Unmute();
-        ExtractorConfig extractor_config;
-
-        const return_code result = ExtractorOptions::ParseArguments(argc, argv, extractor_config);
-
-        if (return_code::fail == result)
-        {
-            return 1;
-        }
-
-        if (return_code::exit == result)
-        {
-            return 0;
-        }
-
-        ExtractorOptions::GenerateOutputFilesNames(extractor_config);
-
-        if (1 > extractor_config.requested_num_threads)
-        {
-            SimpleLogger().Write(logWARNING) << "Number of threads must be 1 or larger";
-            return 1;
-        }
-
-        if (!boost::filesystem::is_regular_file(extractor_config.input_path))
-        {
-            SimpleLogger().Write(logWARNING)
-                << "Input file " << extractor_config.input_path.string() << " not found!";
-            return 1;
-        }
-
-        if (!boost::filesystem::is_regular_file(extractor_config.profile_path))
-        {
-            SimpleLogger().Write(logWARNING) << "Profile " << extractor_config.profile_path.string()
-                                             << " not found!";
-            return 1;
-        }
-        return extractor(extractor_config).run();
+        return EXIT_FAILURE;
     }
-    catch (const std::exception &e)
+
+    if (return_code::exit == result)
     {
-        SimpleLogger().Write(logWARNING) << "[exception] " << e.what();
-        return 1;
+        return EXIT_SUCCESS;
     }
+
+    ExtractorOptions::GenerateOutputFilesNames(extractor_config);
+
+    if (1 > extractor_config.requested_num_threads)
+    {
+        SimpleLogger().Write(logWARNING) << "Number of threads must be 1 or larger";
+        return EXIT_FAILURE;
+    }
+
+    if (!boost::filesystem::is_regular_file(extractor_config.input_path))
+    {
+        SimpleLogger().Write(logWARNING) << "Input file " << extractor_config.input_path.string()
+                                         << " not found!";
+        return EXIT_FAILURE;
+    }
+
+    if (!boost::filesystem::is_regular_file(extractor_config.profile_path))
+    {
+        SimpleLogger().Write(logWARNING) << "Profile " << extractor_config.profile_path.string()
+                                         << " not found!";
+        return EXIT_FAILURE;
+    }
+    return extractor(extractor_config).run();
+}
+catch (const std::bad_alloc &e)
+{
+    SimpleLogger().Write(logWARNING) << "[exception] " << e.what();
+    SimpleLogger().Write(logWARNING)
+        << "Please provide more memory or consider using a larger swapfile";
+    return EXIT_FAILURE;
+}
+catch (const std::exception &e)
+{
+    SimpleLogger().Write(logWARNING) << "[exception] " << e.what();
+    return EXIT_FAILURE;
 }
diff --git a/contractor/edge_based_graph_factory.cpp b/extractor/edge_based_graph_factory.cpp
similarity index 75%
rename from contractor/edge_based_graph_factory.cpp
rename to extractor/edge_based_graph_factory.cpp
index 92f1da9..f0c083d 100644
--- a/contractor/edge_based_graph_factory.cpp
+++ b/extractor/edge_based_graph_factory.cpp
@@ -26,12 +26,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "edge_based_graph_factory.hpp"
+#include "../algorithms/coordinate_calculation.hpp"
 #include "../data_structures/percent.hpp"
 #include "../util/compute_angle.hpp"
 #include "../util/integer_range.hpp"
 #include "../util/lua_util.hpp"
 #include "../util/simple_logger.hpp"
 #include "../util/timing_util.hpp"
+#include "../util/osrm_exception.hpp"
+
+#include "../util/debug_geometry.hpp"
 
 #include <boost/assert.hpp>
 
@@ -57,7 +61,8 @@ EdgeBasedGraphFactory::EdgeBasedGraphFactory(
 void EdgeBasedGraphFactory::GetEdgeBasedEdges(DeallocatingVector<EdgeBasedEdge> &output_edge_list)
 {
     BOOST_ASSERT_MSG(0 == output_edge_list.size(), "Vector is not empty");
-    m_edge_based_edge_list.swap(output_edge_list);
+    using std::swap; // Koenig swap
+    swap(m_edge_based_edge_list, output_edge_list);
 }
 
 void EdgeBasedGraphFactory::GetEdgeBasedNodes(std::vector<EdgeBasedNode> &nodes)
@@ -71,7 +76,14 @@ void EdgeBasedGraphFactory::GetEdgeBasedNodes(std::vector<EdgeBasedNode> &nodes)
         BOOST_ASSERT(m_node_info_list.at(node.v).lat != INT_MAX);
     }
 #endif
-    nodes.swap(m_edge_based_node_list);
+    using std::swap; // Koenig swap
+    swap(nodes, m_edge_based_node_list);
+}
+
+void EdgeBasedGraphFactory::GetStartPointMarkers(std::vector<bool> &node_is_startpoint)
+{
+    using std::swap; // Koenig swap
+    swap(m_edge_based_node_is_startpoint, node_is_startpoint);
 }
 
 unsigned EdgeBasedGraphFactory::GetHighestEdgeID()
@@ -160,7 +172,8 @@ void EdgeBasedGraphFactory::InsertEdgeBasedNode(const NodeID node_u,
                 forward_data.name_id, forward_geometry[i].second,
                 reverse_geometry[geometry_size - 1 - i].second, forward_dist_prefix_sum[i],
                 reverse_dist_prefix_sum[i], m_compressed_edge_container.GetPositionForID(edge_id_1),
-                INVALID_COMPONENTID, i, forward_data.travel_mode, reverse_data.travel_mode);
+                false, INVALID_COMPONENTID, i, forward_data.travel_mode, reverse_data.travel_mode);
+            m_edge_based_node_is_startpoint.push_back(forward_data.startpoint || reverse_data.startpoint);
             current_edge_source_coordinate_id = current_edge_target_coordinate_id;
 
             BOOST_ASSERT(m_edge_based_node_list.back().IsCompressed());
@@ -203,7 +216,8 @@ void EdgeBasedGraphFactory::InsertEdgeBasedNode(const NodeID node_u,
         m_edge_based_node_list.emplace_back(
             forward_data.edge_id, reverse_data.edge_id, node_u, node_v,
             forward_data.name_id, forward_data.distance, reverse_data.distance, 0, 0, SPECIAL_EDGEID,
-            INVALID_COMPONENTID, 0, forward_data.travel_mode, reverse_data.travel_mode);
+            false, INVALID_COMPONENTID, 0, forward_data.travel_mode, reverse_data.travel_mode);
+        m_edge_based_node_is_startpoint.push_back(forward_data.startpoint || reverse_data.startpoint);
         BOOST_ASSERT(!m_edge_based_node_list.back().IsCompressed());
     }
 }
@@ -220,8 +234,20 @@ void EdgeBasedGraphFactory::FlushVectorToStream(
     original_edge_data_vector.clear();
 }
 
+#ifdef DEBUG_GEOMETRY
+void EdgeBasedGraphFactory::Run(const std::string &original_edge_data_filename,
+                                lua_State *lua_state,
+                                const std::string &edge_segment_lookup_filename,
+                                const std::string &edge_penalty_filename,
+                                const bool generate_edge_lookup,
+                                const std::string &debug_turns_path)
+#else
 void EdgeBasedGraphFactory::Run(const std::string &original_edge_data_filename,
-                                lua_State *lua_state)
+                                lua_State *lua_state,
+                                const std::string &edge_segment_lookup_filename,
+                                const std::string &edge_penalty_filename,
+                                const bool generate_edge_lookup)
+#endif
 {
     TIMER_START(renumber);
     m_max_edge_id = RenumberEdges() - 1;
@@ -232,7 +258,16 @@ void EdgeBasedGraphFactory::Run(const std::string &original_edge_data_filename,
     TIMER_STOP(generate_nodes);
 
     TIMER_START(generate_edges);
-    GenerateEdgeExpandedEdges(original_edge_data_filename, lua_state);
+#ifdef DEBUG_GEOMETRY
+    GenerateEdgeExpandedEdges(original_edge_data_filename, lua_state,
+            edge_segment_lookup_filename,edge_penalty_filename, 
+            generate_edge_lookup, debug_turns_path);
+#else
+    GenerateEdgeExpandedEdges(original_edge_data_filename, lua_state,
+            edge_segment_lookup_filename,edge_penalty_filename, 
+            generate_edge_lookup);
+#endif
+
     TIMER_STOP(generate_edges);
 
     SimpleLogger().Write() << "Timing statistics for edge-expanded graph:";
@@ -311,13 +346,27 @@ void EdgeBasedGraphFactory::GenerateEdgeExpandedNodes()
         }
     }
 
+    BOOST_ASSERT(m_edge_based_node_list.size() == m_edge_based_node_is_startpoint.size());
+
     SimpleLogger().Write() << "Generated " << m_edge_based_node_list.size()
                            << " nodes in edge-expanded graph";
 }
 
 /// Actually it also generates OriginalEdgeData and serializes them...
+#ifdef DEBUG_GEOMETRY
 void EdgeBasedGraphFactory::GenerateEdgeExpandedEdges(
-    const std::string &original_edge_data_filename, lua_State *lua_state)
+    const std::string &original_edge_data_filename, lua_State *lua_state,
+    const std::string &edge_segment_lookup_filename,
+    const std::string &edge_fixed_penalties_filename,
+    const bool generate_edge_lookup, 
+    const std::string &debug_turns_path)
+#else
+void EdgeBasedGraphFactory::GenerateEdgeExpandedEdges(
+    const std::string &original_edge_data_filename, lua_State *lua_state,
+    const std::string &edge_segment_lookup_filename,
+    const std::string &edge_fixed_penalties_filename,
+    const bool generate_edge_lookup)
+#endif
 {
     SimpleLogger().Write() << "generating edge-expanded edges";
 
@@ -325,6 +374,14 @@ void EdgeBasedGraphFactory::GenerateEdgeExpandedEdges(
     unsigned original_edges_counter = 0;
 
     std::ofstream edge_data_file(original_edge_data_filename.c_str(), std::ios::binary);
+    std::ofstream edge_segment_file;
+    std::ofstream edge_penalty_file;
+
+    if (generate_edge_lookup)
+    {
+        edge_segment_file.open(edge_segment_lookup_filename.c_str(), std::ios::binary);
+        edge_penalty_file.open(edge_fixed_penalties_filename.c_str(), std::ios::binary);
+    }
 
     // writes a dummy value that is updated later
     edge_data_file.write((char *)&original_edges_counter, sizeof(unsigned));
@@ -342,9 +399,13 @@ void EdgeBasedGraphFactory::GenerateEdgeExpandedEdges(
 
     Percent progress(m_node_based_graph->GetNumberOfNodes());
 
+#ifdef DEBUG_GEOMETRY
+    DEBUG_TURNS_START(debug_turns_path);
+#endif
+
     for (const auto node_u : osrm::irange(0u, m_node_based_graph->GetNumberOfNodes()))
     {
-        progress.printStatus(node_u);
+        //progress.printStatus(node_u);
         for (const EdgeID e1 : m_node_based_graph->GetAdjacentEdgeRange(node_u))
         {
             if (m_node_based_graph->GetEdgeData(e1).reversed)
@@ -415,6 +476,8 @@ void EdgeBasedGraphFactory::GenerateEdgeExpandedEdges(
                 if (m_traffic_lights.find(node_v) != m_traffic_lights.end())
                 {
                     distance += speed_profile.traffic_signal_penalty;
+
+                    DEBUG_SIGNAL(node_v, m_node_info_list, speed_profile.traffic_signal_penalty);
                 }
 
                 // unpack last node of first segment if packed
@@ -437,7 +500,12 @@ void EdgeBasedGraphFactory::GenerateEdgeExpandedEdges(
                 if (turn_instruction == TurnInstruction::UTurn)
                 {
                     distance += speed_profile.u_turn_penalty;
-                }
+
+                    DEBUG_UTURN(node_v, m_node_info_list, speed_profile.u_turn_penalty);
+                } 
+
+                DEBUG_TURN(node_v, m_node_info_list, first_coordinate, turn_angle, turn_penalty);
+
                 distance += turn_penalty;
 
                 const bool edge_is_compressed = m_compressed_edge_container.HasEntryForID(e1);
@@ -462,11 +530,70 @@ void EdgeBasedGraphFactory::GenerateEdgeExpandedEdges(
                 BOOST_ASSERT(SPECIAL_NODEID != edge_data1.edge_id);
                 BOOST_ASSERT(SPECIAL_NODEID != edge_data2.edge_id);
 
+
+                // NOTE: potential overflow here if we hit 2^32 routable edges
+                BOOST_ASSERT(m_edge_based_edge_list.size() <= std::numeric_limits<NodeID>::max());
                 m_edge_based_edge_list.emplace_back(edge_data1.edge_id, edge_data2.edge_id,
                                   m_edge_based_edge_list.size(), distance, true, false);
+
+
+                // Here is where we write out the mapping between the edge-expanded edges, and
+                // the node-based edges that are originally used to calculate the `distance`
+                // for the edge-expanded edges.  About 40 lines back, there is:
+                //
+                //                 unsigned distance = edge_data1.distance;
+                //
+                // This tells us that the weight for an edge-expanded-edge is based on the weight
+                // of the *source* node-based edge.  Therefore, we will look up the individual
+                // segments of the source node-based edge, and write out a mapping between
+                // those and the edge-based-edge ID.
+                // External programs can then use this mapping to quickly perform
+                // updates to the edge-expanded-edge based directly on its ID.
+                if (generate_edge_lookup)
+                {
+                    unsigned fixed_penalty = distance - edge_data1.distance;
+                    edge_penalty_file.write(reinterpret_cast<const char *>(&fixed_penalty), sizeof(fixed_penalty));
+                    if (edge_is_compressed)
+                    {
+                        const auto node_based_edges = m_compressed_edge_container.GetBucketReference(e1);
+                        NodeID previous = node_u;
+
+                        const unsigned node_count = node_based_edges.size()+1;
+                        edge_segment_file.write(reinterpret_cast<const char *>(&node_count), sizeof(node_count));
+                        const QueryNode &first_node = m_node_info_list[previous];
+                        edge_segment_file.write(reinterpret_cast<const char *>(&first_node.node_id), sizeof(first_node.node_id));
+
+                        for (auto target_node : node_based_edges)
+                        {
+                            const QueryNode &from = m_node_info_list[previous];
+                            const QueryNode &to = m_node_info_list[target_node.first];
+                            const double segment_length = coordinate_calculation::great_circle_distance(from.lat, from.lon, to.lat, to.lon);
+
+                            edge_segment_file.write(reinterpret_cast<const char *>(&to.node_id), sizeof(to.node_id));
+                            edge_segment_file.write(reinterpret_cast<const char *>(&segment_length), sizeof(segment_length));
+                            edge_segment_file.write(reinterpret_cast<const char *>(&target_node.second), sizeof(target_node.second));
+                            previous = target_node.first;
+                        }
+                    }
+                    else
+                    {
+                        static const unsigned node_count = 2;
+                        const QueryNode from = m_node_info_list[node_u];
+                        const QueryNode to = m_node_info_list[node_v];
+                        const double segment_length = coordinate_calculation::great_circle_distance(from.lat, from.lon, to.lat, to.lon);
+                        edge_segment_file.write(reinterpret_cast<const char *>(&node_count), sizeof(node_count));
+                        edge_segment_file.write(reinterpret_cast<const char *>(&from.node_id), sizeof(from.node_id));
+                        edge_segment_file.write(reinterpret_cast<const char *>(&to.node_id), sizeof(to.node_id));
+                        edge_segment_file.write(reinterpret_cast<const char *>(&segment_length), sizeof(segment_length));
+                        edge_segment_file.write(reinterpret_cast<const char *>(&edge_data1.distance), sizeof(edge_data1.distance));
+                    }
+                }
             }
         }
     }
+
+    DEBUG_TURNS_STOP();
+
     FlushVectorToStream(edge_data_file, original_edge_data_vector);
 
     edge_data_file.seekp(std::ios::beg);
diff --git a/contractor/edge_based_graph_factory.hpp b/extractor/edge_based_graph_factory.hpp
similarity index 72%
rename from contractor/edge_based_graph_factory.hpp
rename to extractor/edge_based_graph_factory.hpp
index a9ef8ea..997c7ea 100644
--- a/contractor/edge_based_graph_factory.hpp
+++ b/extractor/edge_based_graph_factory.hpp
@@ -50,6 +50,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <unordered_set>
 #include <vector>
 
+#include <boost/filesystem/fstream.hpp>
+
 struct lua_State;
 
 class EdgeBasedGraphFactory
@@ -66,12 +68,25 @@ class EdgeBasedGraphFactory
                                    const std::vector<QueryNode> &node_info_list,
                                    SpeedProfileProperties speed_profile);
 
+#ifdef DEBUG_GEOMETRY
+    void Run(const std::string &original_edge_data_filename,
+             lua_State *lua_state,
+             const std::string &edge_segment_lookup_filename,
+             const std::string &edge_penalty_filename,
+             const bool generate_edge_lookup,
+             const std::string &debug_turns_path);
+#else
     void Run(const std::string &original_edge_data_filename,
-             lua_State *lua_state);
+             lua_State *lua_state,
+             const std::string &edge_segment_lookup_filename,
+             const std::string &edge_penalty_filename,
+             const bool generate_edge_lookup);
+#endif
 
     void GetEdgeBasedEdges(DeallocatingVector<EdgeBasedEdge> &edges);
 
     void GetEdgeBasedNodes(std::vector<EdgeBasedNode> &nodes);
+    void GetStartPointMarkers(std::vector<bool> &node_is_startpoint);
 
     unsigned GetHighestEdgeID();
 
@@ -82,6 +97,9 @@ class EdgeBasedGraphFactory
   private:
     using EdgeData = NodeBasedDynamicGraph::EdgeData;
 
+    //! maps index from m_edge_based_node_list to ture/false if the node is an entry point to the graph
+    std::vector<bool> m_edge_based_node_is_startpoint;
+    //! list of edge based nodes (compressed segments)
     std::vector<EdgeBasedNode> m_edge_based_node_list;
     DeallocatingVector<EdgeBasedEdge> m_edge_based_edge_list;
     unsigned m_max_edge_id;
@@ -99,8 +117,20 @@ class EdgeBasedGraphFactory
     void CompressGeometry();
     unsigned RenumberEdges();
     void GenerateEdgeExpandedNodes();
+#ifdef DEBUG_GEOMETRY
+    void GenerateEdgeExpandedEdges(const std::string &original_edge_data_filename,
+                                   lua_State *lua_state,
+                                   const std::string &edge_segment_lookup_filename,
+                                   const std::string &edge_fixed_penalties_filename,
+                                   const bool generate_edge_lookup,
+                                   const std::string &debug_turns_path);
+#else
     void GenerateEdgeExpandedEdges(const std::string &original_edge_data_filename,
-                                   lua_State *lua_state);
+                                   lua_State *lua_state,
+                                   const std::string &edge_segment_lookup_filename,
+                                   const std::string &edge_fixed_penalties_filename,
+                                   const bool generate_edge_lookup);
+#endif 
 
     void InsertEdgeBasedNode(const NodeID u, const NodeID v);
 
diff --git a/extractor/extraction_containers.cpp b/extractor/extraction_containers.cpp
index d10a2f6..48a626b 100644
--- a/extractor/extraction_containers.cpp
+++ b/extractor/extraction_containers.cpp
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <boost/filesystem.hpp>
 #include <boost/filesystem/fstream.hpp>
 #include <boost/ref.hpp>
+#include <boost/numeric/conversion/cast.hpp>
 
 #include <luabind/luabind.hpp>
 
@@ -50,11 +51,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <chrono>
 #include <limits>
 
+static const int WRITE_BLOCK_BUFFER_SIZE = 8000;
+
 ExtractionContainers::ExtractionContainers()
 {
     // Check if stxxl can be instantiated
     stxxl::vector<unsigned> dummy_vector;
-    name_list.push_back("");
+    // Insert the empty string, it has no data and is zero length
+    name_lengths.push_back(0);
 }
 
 ExtractionContainers::~ExtractionContainers()
@@ -63,7 +67,8 @@ ExtractionContainers::~ExtractionContainers()
     used_node_id_list.clear();
     all_nodes_list.clear();
     all_edges_list.clear();
-    name_list.clear();
+    name_char_data.clear();
+    name_lengths.clear();
     restrictions_list.clear();
     way_start_end_id_list.clear();
 }
@@ -115,13 +120,10 @@ void ExtractionContainers::WriteNames(const std::string& names_file_name) const
     boost::filesystem::ofstream name_file_stream(names_file_name, std::ios::binary);
 
     unsigned total_length = 0;
-    std::vector<unsigned> name_lengths;
-    for (const std::string &temp_string : name_list)
+
+    for (const unsigned &name_length : name_lengths)
     {
-        const unsigned string_length =
-            std::min(static_cast<unsigned>(temp_string.length()), 255u);
-        name_lengths.push_back(string_length);
-        total_length += string_length;
+        total_length += name_length;
     }
 
     // builds and writes the index
@@ -129,14 +131,25 @@ void ExtractionContainers::WriteNames(const std::string& names_file_name) const
     name_file_stream << name_index_range;
 
     name_file_stream.write((char *)&total_length, sizeof(unsigned));
+
+
     // write all chars consecutively
-    for (const std::string &temp_string : name_list)
+    char write_buffer[WRITE_BLOCK_BUFFER_SIZE];
+    unsigned buffer_len = 0;
+
+    for (const char &c : name_char_data)
     {
-        const unsigned string_length =
-            std::min(static_cast<unsigned>(temp_string.length()), 255u);
-        name_file_stream.write(temp_string.c_str(), string_length);
+        write_buffer[buffer_len++] = c;
+
+        if (buffer_len >= WRITE_BLOCK_BUFFER_SIZE)
+        {
+            name_file_stream.write(write_buffer, WRITE_BLOCK_BUFFER_SIZE);
+            buffer_len = 0;
+        }
     }
 
+    name_file_stream.write(write_buffer, buffer_len);
+
     name_file_stream.close();
     TIMER_STOP(write_name_index);
     std::cout << "ok, after " << TIMER_SEC(write_name_index) << "s" << std::endl;
@@ -171,7 +184,11 @@ void ExtractionContainers::PrepareNodes()
     auto ref_iter = used_node_id_list.begin();
     const auto all_nodes_list_end = all_nodes_list.end();
     const auto used_node_id_list_end = used_node_id_list.end();
-    auto internal_id = 0u;
+    // Note: despite being able to handle 64 bit OSM node ids, we can't
+    // handle > uint32_t actual usable nodes.  This should be OK for a while
+    // because we usually route on a *lot* less than 2^32 of the OSM
+    // graph nodes.
+    std::size_t internal_id = 0;
 
     // compute the intersection of nodes that were referenced and nodes we actually have
     while (node_iter != all_nodes_list_end && ref_iter != used_node_id_list_end)
@@ -187,11 +204,15 @@ void ExtractionContainers::PrepareNodes()
             continue;
         }
         BOOST_ASSERT(node_iter->node_id == *ref_iter);
-        external_to_internal_node_id_map[*ref_iter] = internal_id++;
+        external_to_internal_node_id_map[*ref_iter] = static_cast<NodeID>(internal_id++);
         node_iter++;
         ref_iter++;
     }
-    max_internal_node_id = internal_id;
+    if (internal_id > std::numeric_limits<NodeID>::max())
+    {
+        throw osrm::exception("There are too many nodes remaining after filtering, OSRM only supports 2^32 unique nodes");
+    }
+    max_internal_node_id = boost::numeric_cast<NodeID>(internal_id);
     TIMER_STOP(id_map);
     std::cout << "ok, after " << TIMER_SEC(id_map) << "s" << std::endl;
 
@@ -202,7 +223,7 @@ void ExtractionContainers::PrepareEdges(lua_State *segment_state)
     // Sort edges by start.
     std::cout << "[extractor] Sorting edges by start    ... " << std::flush;
     TIMER_START(sort_edges_by_start);
-    stxxl::sort(all_edges_list.begin(), all_edges_list.end(), CmpEdgeByStartID(), stxxl_memory);
+    stxxl::sort(all_edges_list.begin(), all_edges_list.end(), CmpEdgeByOSMStartID(), stxxl_memory);
     TIMER_STOP(sort_edges_by_start);
     std::cout << "ok, after " << TIMER_SEC(sort_edges_by_start) << "s" << std::endl;
 
@@ -217,21 +238,21 @@ void ExtractionContainers::PrepareEdges(lua_State *segment_state)
 
     while (edge_iterator != all_edges_list_end && node_iterator != all_nodes_list_end)
     {
-        if (edge_iterator->result.source < node_iterator->node_id)
+        if (edge_iterator->result.osm_source_id < node_iterator->node_id)
         {
             SimpleLogger().Write(LogLevel::logWARNING) << "Found invalid node reference " << edge_iterator->result.source;
             edge_iterator->result.source = SPECIAL_NODEID;
             ++edge_iterator;
             continue;
         }
-        if (edge_iterator->result.source > node_iterator->node_id)
+        if (edge_iterator->result.osm_source_id > node_iterator->node_id)
         {
             node_iterator++;
             continue;
         }
 
         // remove loops
-        if (edge_iterator->result.source == edge_iterator->result.target)
+        if (edge_iterator->result.osm_source_id == edge_iterator->result.osm_target_id)
         {
             edge_iterator->result.source = SPECIAL_NODEID;
             edge_iterator->result.target = SPECIAL_NODEID;
@@ -239,7 +260,7 @@ void ExtractionContainers::PrepareEdges(lua_State *segment_state)
             continue;
         }
 
-        BOOST_ASSERT(edge_iterator->result.source == node_iterator->node_id);
+        BOOST_ASSERT(edge_iterator->result.osm_source_id == node_iterator->node_id);
 
         // assign new node id
         auto id_iter = external_to_internal_node_id_map.find(node_iterator->node_id);
@@ -250,13 +271,24 @@ void ExtractionContainers::PrepareEdges(lua_State *segment_state)
         edge_iterator->source_coordinate.lon = node_iterator->lon;
         ++edge_iterator;
     }
+
+    // Remove all remaining edges. They are invalid because there are no corresponding nodes for
+    // them. This happens when using osmosis with bbox or polygon to extract smaller areas.
+    auto markSourcesInvalid = [](InternalExtractorEdge &edge)
+    {
+        SimpleLogger().Write(LogLevel::logWARNING) << "Found invalid node reference "
+                                                   << edge.result.source;
+        edge.result.source = SPECIAL_NODEID;
+        edge.result.osm_source_id = SPECIAL_OSM_NODEID;
+    };
+    std::for_each(edge_iterator, all_edges_list_end, markSourcesInvalid);
     TIMER_STOP(set_start_coords);
     std::cout << "ok, after " << TIMER_SEC(set_start_coords) << "s" << std::endl;
 
     // Sort Edges by target
     std::cout << "[extractor] Sorting edges by target   ... " << std::flush;
     TIMER_START(sort_edges_by_target);
-    stxxl::sort(all_edges_list.begin(), all_edges_list.end(), CmpEdgeByTargetID(),
+    stxxl::sort(all_edges_list.begin(), all_edges_list.end(), CmpEdgeByOSMTargetID(),
                 stxxl_memory);
     TIMER_STOP(sort_edges_by_target);
     std::cout << "ok, after " << TIMER_SEC(sort_edges_by_target) << "s" << std::endl;
@@ -278,25 +310,25 @@ void ExtractionContainers::PrepareEdges(lua_State *segment_state)
             continue;
         }
 
-        if (edge_iterator->result.target < node_iterator->node_id)
+        if (edge_iterator->result.osm_target_id < node_iterator->node_id)
         {
-            SimpleLogger().Write(LogLevel::logWARNING) << "Found invalid node reference " << edge_iterator->result.target;
+            SimpleLogger().Write(LogLevel::logWARNING) << "Found invalid node reference " << OSMNodeID_to_uint64_t(edge_iterator->result.osm_target_id);
             edge_iterator->result.target = SPECIAL_NODEID;
             ++edge_iterator;
             continue;
         }
-        if (edge_iterator->result.target > node_iterator->node_id)
+        if (edge_iterator->result.osm_target_id > node_iterator->node_id)
         {
             ++node_iterator;
             continue;
         }
 
-        BOOST_ASSERT(edge_iterator->result.target == node_iterator->node_id);
+        BOOST_ASSERT(edge_iterator->result.osm_target_id == node_iterator->node_id);
         BOOST_ASSERT(edge_iterator->weight_data.speed >= 0);
         BOOST_ASSERT(edge_iterator->source_coordinate.lat != std::numeric_limits<int>::min());
         BOOST_ASSERT(edge_iterator->source_coordinate.lon != std::numeric_limits<int>::min());
 
-        const double distance = coordinate_calculation::euclidean_distance(
+        const double distance = coordinate_calculation::great_circle_distance(
             edge_iterator->source_coordinate.lat, edge_iterator->source_coordinate.lon,
             node_iterator->lat, node_iterator->lon);
 
@@ -347,13 +379,23 @@ void ExtractionContainers::PrepareEdges(lua_State *segment_state)
         }
         ++edge_iterator;
     }
+
+    // Remove all remaining edges. They are invalid because there are no corresponding nodes for
+    // them. This happens when using osmosis with bbox or polygon to extract smaller areas.
+    auto markTargetsInvalid = [](InternalExtractorEdge &edge)
+    {
+        SimpleLogger().Write(LogLevel::logWARNING) << "Found invalid node reference "
+                                                   << edge.result.target;
+        edge.result.target = SPECIAL_NODEID;
+    };
+    std::for_each(edge_iterator, all_edges_list_end_, markTargetsInvalid);
     TIMER_STOP(compute_weights);
     std::cout << "ok, after " << TIMER_SEC(compute_weights) << "s" << std::endl;
 
     // Sort edges by start.
     std::cout << "[extractor] Sorting edges by renumbered start ... " << std::flush;
     TIMER_START(sort_edges_by_renumbered_start);
-    stxxl::sort(all_edges_list.begin(), all_edges_list.end(), CmpEdgeByStartThenTargetID(), stxxl_memory);
+    stxxl::sort(all_edges_list.begin(), all_edges_list.end(), CmpEdgeByInternalStartThenInternalTargetID(), stxxl_memory);
     TIMER_STOP(sort_edges_by_renumbered_start);
     std::cout << "ok, after " << TIMER_SEC(sort_edges_by_renumbered_start) << "s" << std::endl;
 
@@ -444,13 +486,14 @@ void ExtractionContainers::PrepareEdges(lua_State *segment_state)
 
 void ExtractionContainers::WriteEdges(std::ofstream& file_out_stream) const
 {
-    std::cout << "[extractor] Writing used egdes       ... " << std::flush;
+    std::cout << "[extractor] Writing used edges       ... " << std::flush;
     TIMER_START(write_edges);
     // Traverse list of edges and nodes in parallel and set target coord
-    unsigned number_of_used_edges = 0;
+    std::size_t used_edges_counter = 0;
+    unsigned used_edges_counter_buffer = 0;
 
     auto start_position = file_out_stream.tellp();
-    file_out_stream.write((char *)&number_of_used_edges, sizeof(unsigned));
+    file_out_stream.write((char *)&used_edges_counter_buffer, sizeof(unsigned));
 
     for (const auto& edge : all_edges_list)
     {
@@ -459,18 +502,29 @@ void ExtractionContainers::WriteEdges(std::ofstream& file_out_stream) const
             continue;
         }
 
-        file_out_stream.write((char*) &edge.result, sizeof(NodeBasedEdge));
-        number_of_used_edges++;
+        // IMPORTANT: here, we're using slicing to only write the data from the base
+        // class of NodeBasedEdgeWithOSM
+        NodeBasedEdge tmp = edge.result;
+        file_out_stream.write((char*) &tmp, sizeof(NodeBasedEdge));
+        used_edges_counter++;
+    }
+
+    if (used_edges_counter > std::numeric_limits<unsigned>::max())
+    {
+        throw osrm::exception("There are too many edges, OSRM only supports 2^32");
     }
     TIMER_STOP(write_edges);
     std::cout << "ok, after " << TIMER_SEC(write_edges) << "s" << std::endl;
 
     std::cout << "[extractor] setting number of edges   ... " << std::flush;
+
+    used_edges_counter_buffer = boost::numeric_cast<unsigned>(used_edges_counter);
+
     file_out_stream.seekp(start_position);
-    file_out_stream.write((char *)&number_of_used_edges, sizeof(unsigned));
+    file_out_stream.write((char *)&used_edges_counter_buffer, sizeof(unsigned));
     std::cout << "ok" << std::endl;
 
-    SimpleLogger().Write() << "Processed " << number_of_used_edges << " edges";
+    SimpleLogger().Write() << "Processed " << used_edges_counter << " edges";
 }
 
 void ExtractionContainers::WriteNodes(std::ofstream& file_out_stream) const
@@ -569,13 +623,13 @@ void ExtractionContainers::PrepareRestrictions()
     while (way_start_and_end_iterator != way_start_end_id_list_end &&
            restrictions_iterator != restrictions_list_end)
     {
-        if (way_start_and_end_iterator->way_id < restrictions_iterator->restriction.from.way)
+        if (way_start_and_end_iterator->way_id < OSMWayID(restrictions_iterator->restriction.from.way))
         {
             ++way_start_and_end_iterator;
             continue;
         }
 
-        if (way_start_and_end_iterator->way_id > restrictions_iterator->restriction.from.way)
+        if (way_start_and_end_iterator->way_id > OSMWayID(restrictions_iterator->restriction.from.way))
         {
             SimpleLogger().Write(LogLevel::logDEBUG) << "Restriction references invalid way: " << restrictions_iterator->restriction.from.way;
             restrictions_iterator->restriction.from.node = SPECIAL_NODEID;
@@ -584,9 +638,9 @@ void ExtractionContainers::PrepareRestrictions()
         }
 
         BOOST_ASSERT(way_start_and_end_iterator->way_id ==
-                     restrictions_iterator->restriction.from.way);
+                     OSMWayID(restrictions_iterator->restriction.from.way));
         // we do not remap the via id yet, since we will need it for the to node as well
-        const NodeID via_node_id = restrictions_iterator->restriction.via.node;
+        const OSMNodeID via_node_id = OSMNodeID(restrictions_iterator->restriction.via.node);
 
         // check if via is actually valid, if not invalidate
         auto via_id_iter = external_to_internal_node_id_map.find(via_node_id);
@@ -598,19 +652,19 @@ void ExtractionContainers::PrepareRestrictions()
             continue;
         }
 
-        if (way_start_and_end_iterator->first_segment_source_id == via_node_id)
+        if (OSMNodeID(way_start_and_end_iterator->first_segment_source_id) == via_node_id)
         {
             // assign new from node id
             auto id_iter = external_to_internal_node_id_map.find(
-                    way_start_and_end_iterator->first_segment_target_id);
+                    OSMNodeID(way_start_and_end_iterator->first_segment_target_id));
             BOOST_ASSERT(id_iter != external_to_internal_node_id_map.end());
             restrictions_iterator->restriction.from.node = id_iter->second;
         }
-        else if (way_start_and_end_iterator->last_segment_target_id == via_node_id)
+        else if (OSMNodeID(way_start_and_end_iterator->last_segment_target_id) == via_node_id)
         {
             // assign new from node id
             auto id_iter = external_to_internal_node_id_map.find(
-                    way_start_and_end_iterator->last_segment_source_id);
+                    OSMNodeID(way_start_and_end_iterator->last_segment_source_id));
             BOOST_ASSERT(id_iter != external_to_internal_node_id_map.end());
             restrictions_iterator->restriction.from.node = id_iter->second;
         }
@@ -637,7 +691,7 @@ void ExtractionContainers::PrepareRestrictions()
     while (way_start_and_end_iterator != way_start_end_id_list_end_ &&
            restrictions_iterator != restrictions_list_end_)
     {
-        if (way_start_and_end_iterator->way_id < restrictions_iterator->restriction.to.way)
+        if (way_start_and_end_iterator->way_id < OSMWayID(restrictions_iterator->restriction.to.way))
         {
             ++way_start_and_end_iterator;
             continue;
@@ -648,7 +702,7 @@ void ExtractionContainers::PrepareRestrictions()
             ++restrictions_iterator;
             continue;
         }
-        if (way_start_and_end_iterator->way_id > restrictions_iterator->restriction.to.way)
+        if (way_start_and_end_iterator->way_id > OSMWayID(restrictions_iterator->restriction.to.way))
         {
             SimpleLogger().Write(LogLevel::logDEBUG) << "Restriction references invalid way: " << restrictions_iterator->restriction.to.way;
             restrictions_iterator->restriction.to.way = SPECIAL_NODEID;
@@ -656,25 +710,25 @@ void ExtractionContainers::PrepareRestrictions()
             continue;
         }
         BOOST_ASSERT(way_start_and_end_iterator->way_id ==
-                     restrictions_iterator->restriction.to.way);
-        const NodeID via_node_id = restrictions_iterator->restriction.via.node;
+                     OSMWayID(restrictions_iterator->restriction.to.way));
+        const OSMNodeID via_node_id = OSMNodeID(restrictions_iterator->restriction.via.node);
 
         // assign new via node id
         auto via_id_iter = external_to_internal_node_id_map.find(via_node_id);
         BOOST_ASSERT(via_id_iter != external_to_internal_node_id_map.end());
         restrictions_iterator->restriction.via.node = via_id_iter->second;
 
-        if (way_start_and_end_iterator->first_segment_source_id == via_node_id)
+        if (OSMNodeID(way_start_and_end_iterator->first_segment_source_id) == via_node_id)
         {
             auto to_id_iter = external_to_internal_node_id_map.find(
-                    way_start_and_end_iterator->first_segment_target_id);
+                    OSMNodeID(way_start_and_end_iterator->first_segment_target_id));
             BOOST_ASSERT(to_id_iter != external_to_internal_node_id_map.end());
             restrictions_iterator->restriction.to.node = to_id_iter->second;
         }
-        else if (way_start_and_end_iterator->last_segment_target_id == via_node_id)
+        else if (OSMNodeID(way_start_and_end_iterator->last_segment_target_id) == via_node_id)
         {
             auto to_id_iter = external_to_internal_node_id_map.find(
-                    way_start_and_end_iterator->last_segment_source_id);
+                    OSMNodeID(way_start_and_end_iterator->last_segment_source_id));
             BOOST_ASSERT(to_id_iter != external_to_internal_node_id_map.end());
             restrictions_iterator->restriction.to.node = to_id_iter->second;
         }
diff --git a/extractor/extraction_containers.hpp b/extractor/extraction_containers.hpp
index 8b7a829..541ad35 100644
--- a/extractor/extraction_containers.hpp
+++ b/extractor/extraction_containers.hpp
@@ -61,20 +61,20 @@ class ExtractionContainers
     void WriteEdges(std::ofstream& file_out_stream) const;
     void WriteNames(const std::string& names_file_name) const;
   public:
-    using STXXLNodeIDVector = stxxl::vector<NodeID>;
+    using STXXLNodeIDVector = stxxl::vector<OSMNodeID>;
     using STXXLNodeVector = stxxl::vector<ExternalMemoryNode>;
     using STXXLEdgeVector = stxxl::vector<InternalExtractorEdge>;
-    using STXXLStringVector = stxxl::vector<std::string>;
     using STXXLRestrictionsVector = stxxl::vector<InputRestrictionContainer>;
     using STXXLWayIDStartEndVector = stxxl::vector<FirstAndLastSegmentOfWay>;
 
     STXXLNodeIDVector used_node_id_list;
     STXXLNodeVector all_nodes_list;
     STXXLEdgeVector all_edges_list;
-    STXXLStringVector name_list;
+    stxxl::vector<char> name_char_data;
+    stxxl::vector<unsigned> name_lengths;
     STXXLRestrictionsVector restrictions_list;
     STXXLWayIDStartEndVector way_start_end_id_list;
-    std::unordered_map<NodeID, NodeID> external_to_internal_node_id_map;
+    std::unordered_map<OSMNodeID, NodeID> external_to_internal_node_id_map;
     unsigned max_internal_node_id;
 
     ExtractionContainers();
diff --git a/extractor/extraction_helper_functions.hpp b/extractor/extraction_helper_functions.hpp
index e61e543..69ab456 100644
--- a/extractor/extraction_helper_functions.hpp
+++ b/extractor/extraction_helper_functions.hpp
@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <boost/regex.hpp>
 
 #include <limits>
+#include <string>
 
 bool simple_duration_is_valid(const std::string &s)
 {
@@ -89,18 +90,18 @@ unsigned parseDuration(const std::string &s)
         {
             if (1 == result.size())
             {
-                minutes = cast::string_to_int(result[0]);
+                minutes = std::stoul(result[0]);
             }
             if (2 == result.size())
             {
-                minutes = cast::string_to_int(result[1]);
-                hours = cast::string_to_int(result[0]);
+                minutes = std::stoul(result[1]);
+                hours = std::stoul(result[0]);
             }
             if (3 == result.size())
             {
-                seconds = cast::string_to_int(result[2]);
-                minutes = cast::string_to_int(result[1]);
-                hours = cast::string_to_int(result[0]);
+                seconds = std::stoul(result[2]);
+                minutes = std::stoul(result[1]);
+                hours = std::stoul(result[0]);
             }
             return (3600 * hours + 60 * minutes + seconds);
         }
diff --git a/extractor/extraction_way.hpp b/extractor/extraction_way.hpp
index 23fd50a..094c10a 100644
--- a/extractor/extraction_way.hpp
+++ b/extractor/extraction_way.hpp
@@ -50,6 +50,7 @@ struct ExtractionWay
         backward_speed = -1;
         duration = -1;
         roundabout = false;
+        is_startpoint = true;
         is_access_restricted = false;
         name.clear();
         forward_travel_mode = TRAVEL_MODE_DEFAULT;
@@ -120,6 +121,7 @@ struct ExtractionWay
     std::string name;
     bool roundabout;
     bool is_access_restricted;
+    bool is_startpoint;
     TravelMode forward_travel_mode : 4;
     TravelMode backward_travel_mode : 4;
 };
diff --git a/extractor/extractor.cpp b/extractor/extractor.cpp
index 2386f13..80bd33d 100644
--- a/extractor/extractor.cpp
+++ b/extractor/extractor.cpp
@@ -35,16 +35,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "scripting_environment.hpp"
 
 #include "../data_structures/raster_source.hpp"
-#include "../util/git_sha.hpp"
 #include "../util/make_unique.hpp"
 #include "../util/simple_logger.hpp"
 #include "../util/timing_util.hpp"
 #include "../util/lua_util.hpp"
+#include "../util/graph_loader.hpp"
 
 #include "../typedefs.h"
 
+#include "../data_structures/static_graph.hpp"
+#include "../data_structures/static_rtree.hpp"
+#include "../data_structures/restriction_map.hpp"
+#include "../data_structures/compressed_edge_container.hpp"
+
+#include "../algorithms/tarjan_scc.hpp"
+#include "../algorithms/crc32_processor.hpp"
+
 #include <boost/filesystem.hpp>
 #include <boost/filesystem/fstream.hpp>
+#include <boost/optional/optional.hpp>
 
 #include <luabind/luabind.hpp>
 
@@ -53,8 +62,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <tbb/parallel_for.h>
 #include <tbb/task_scheduler_init.h>
 
-#include <variant/optional.hpp>
-
 #include <cstdlib>
 
 #include <algorithm>
@@ -81,7 +88,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  * The result of this process are the following files:
  *  .names : Names of all streets, stored as long consecutive string with prefix sum based index
  *  .osrm  : Nodes and edges in a intermediate format that easy to digest for osrm-prepare
- *  .restrictions : Turn restrictions that are used my osrm-prepare to construct the edge-expanded graph
+ *  .restrictions : Turn restrictions that are used my osrm-prepare to construct the edge-expanded
+ * graph
  *
  */
 int extractor::run()
@@ -151,8 +159,7 @@ int extractor::run()
         // initialize vectors holding parsed objects
         tbb::concurrent_vector<std::pair<std::size_t, ExtractionNode>> resulting_nodes;
         tbb::concurrent_vector<std::pair<std::size_t, ExtractionWay>> resulting_ways;
-        tbb::concurrent_vector<mapbox::util::optional<InputRestrictionContainer>>
-            resulting_restrictions;
+        tbb::concurrent_vector<boost::optional<InputRestrictionContainer>> resulting_restrictions;
 
         // setup restriction parser
         const RestrictionParser restriction_parser(scripting_environment.get_lua_state());
@@ -249,21 +256,383 @@ int extractor::run()
             return 1;
         }
 
-        extraction_containers.PrepareData(config.output_file_name,
-                                          config.restriction_file_name,
-                                          config.names_file_name,
-                                          segment_state);
+        extraction_containers.PrepareData(config.output_file_name, config.restriction_file_name,
+                                          config.names_file_name, segment_state);
 
         TIMER_STOP(extracting);
         SimpleLogger().Write() << "extraction finished after " << TIMER_SEC(extracting) << "s";
+    }
+    catch (const std::exception &e)
+    {
+        SimpleLogger().Write(logWARNING) << e.what();
+        return 1;
+    }
+
+    try
+    {
+        // Transform the node-based graph that OSM is based on into an edge-based graph
+        // that is better for routing.  Every edge becomes a node, and every valid
+        // movement (e.g. turn from A->B, and B->A) becomes an edge
+        //
+        //
+        //    // Create a new lua state
+
+        SimpleLogger().Write() << "Generating edge-expanded graph representation";
+
+        TIMER_START(expansion);
+
+        std::vector<EdgeBasedNode> node_based_edge_list;
+        DeallocatingVector<EdgeBasedEdge> edge_based_edge_list;
+        std::vector<bool> node_is_startpoint;
+        std::vector<QueryNode> internal_to_external_node_map;
+        auto graph_size =
+            BuildEdgeExpandedGraph(internal_to_external_node_map, node_based_edge_list,
+                                   node_is_startpoint, edge_based_edge_list);
+
+        auto number_of_node_based_nodes = graph_size.first;
+        auto max_edge_id = graph_size.second;
+
+        TIMER_STOP(expansion);
+
+        SimpleLogger().Write() << "building r-tree ...";
+        TIMER_START(rtree);
+
+        FindComponents(max_edge_id, edge_based_edge_list, node_based_edge_list);
+
+        BuildRTree(std::move(node_based_edge_list), std::move(node_is_startpoint),
+                   internal_to_external_node_map);
+
+        TIMER_STOP(rtree);
+
+        SimpleLogger().Write() << "writing node map ...";
+        WriteNodeMapping(internal_to_external_node_map);
+
+        WriteEdgeBasedGraph(config.edge_graph_output_path, max_edge_id, edge_based_edge_list);
+
+        SimpleLogger().Write() << "Expansion  : "
+                               << (number_of_node_based_nodes / TIMER_SEC(expansion))
+                               << " nodes/sec and " << ((max_edge_id + 1) / TIMER_SEC(expansion))
+                               << " edges/sec";
         SimpleLogger().Write() << "To prepare the data for routing, run: "
-                               << "./osrm-prepare " << config.output_file_name
-                               << std::endl;
+                               << "./osrm-prepare " << config.output_file_name << std::endl;
     }
-    catch (std::exception &e)
+    catch (const std::exception &e)
     {
         SimpleLogger().Write(logWARNING) << e.what();
         return 1;
     }
+
     return 0;
 }
+
+/**
+    \brief Setups scripting environment (lua-scripting)
+    Also initializes speed profile.
+*/
+void extractor::SetupScriptingEnvironment(lua_State *lua_state,
+                                          SpeedProfileProperties &speed_profile)
+{
+    // open utility libraries string library;
+    luaL_openlibs(lua_state);
+
+    // adjust lua load path
+    luaAddScriptFolderToLoadPath(lua_state, config.profile_path.string().c_str());
+
+    // Now call our function in a lua script
+    if (0 != luaL_dofile(lua_state, config.profile_path.string().c_str()))
+    {
+        std::stringstream msg;
+        msg << lua_tostring(lua_state, -1) << " occured in scripting block";
+        throw osrm::exception(msg.str());
+    }
+
+    if (0 != luaL_dostring(lua_state, "return traffic_signal_penalty\n"))
+    {
+        std::stringstream msg;
+        msg << lua_tostring(lua_state, -1) << " occured in scripting block";
+        throw osrm::exception(msg.str());
+    }
+    speed_profile.traffic_signal_penalty = 10 * lua_tointeger(lua_state, -1);
+    SimpleLogger().Write(logDEBUG) << "traffic_signal_penalty: "
+                                   << speed_profile.traffic_signal_penalty;
+
+    if (0 != luaL_dostring(lua_state, "return u_turn_penalty\n"))
+    {
+        std::stringstream msg;
+        msg << lua_tostring(lua_state, -1) << " occured in scripting block";
+        throw osrm::exception(msg.str());
+    }
+
+    speed_profile.u_turn_penalty = 10 * lua_tointeger(lua_state, -1);
+    speed_profile.has_turn_penalty_function = lua_function_exists(lua_state, "turn_function");
+}
+
+void extractor::FindComponents(unsigned max_edge_id,
+                               const DeallocatingVector<EdgeBasedEdge> &input_edge_list,
+                               std::vector<EdgeBasedNode> &input_nodes) const
+{
+    struct UncontractedEdgeData
+    {
+    };
+    struct InputEdge
+    {
+        unsigned source;
+        unsigned target;
+        UncontractedEdgeData data;
+
+        bool operator<(const InputEdge &rhs) const
+        {
+            return source < rhs.source || (source == rhs.source && target < rhs.target);
+        }
+
+        bool operator==(const InputEdge &rhs) const
+        {
+            return source == rhs.source && target == rhs.target;
+        }
+    };
+    using UncontractedGraph = StaticGraph<UncontractedEdgeData>;
+    std::vector<InputEdge> edges;
+    edges.reserve(input_edge_list.size() * 2);
+
+    for (const auto &edge : input_edge_list)
+    {
+        BOOST_ASSERT_MSG(static_cast<unsigned int>(std::max(edge.weight, 1)) > 0,
+                         "edge distance < 1");
+        if (edge.forward)
+        {
+            edges.push_back({edge.source, edge.target, {}});
+        }
+
+        if (edge.backward)
+        {
+            edges.push_back({edge.target, edge.source, {}});
+        }
+    }
+
+    // connect forward and backward nodes of each edge
+    for (const auto &node : input_nodes)
+    {
+        if (node.reverse_edge_based_node_id != SPECIAL_NODEID)
+        {
+            edges.push_back({node.forward_edge_based_node_id, node.reverse_edge_based_node_id, {}});
+            edges.push_back({node.reverse_edge_based_node_id, node.forward_edge_based_node_id, {}});
+        }
+    }
+
+    tbb::parallel_sort(edges.begin(), edges.end());
+    auto new_end = std::unique(edges.begin(), edges.end());
+    edges.resize(new_end - edges.begin());
+
+    auto uncontractor_graph = std::make_shared<UncontractedGraph>(max_edge_id + 1, edges);
+
+    TarjanSCC<UncontractedGraph> component_search(
+        std::const_pointer_cast<const UncontractedGraph>(uncontractor_graph));
+    component_search.run();
+
+    for (auto &node : input_nodes)
+    {
+        auto forward_component = component_search.get_component_id(node.forward_edge_based_node_id);
+        BOOST_ASSERT(node.reverse_edge_based_node_id == SPECIAL_EDGEID ||
+                     forward_component ==
+                         component_search.get_component_id(node.reverse_edge_based_node_id));
+
+        const unsigned component_size = component_search.get_component_size(forward_component);
+        node.component.is_tiny = component_size < config.small_component_size;
+        node.component.id = 1 + forward_component;
+    }
+}
+
+/**
+  \brief Build load restrictions from .restriction file
+  */
+std::shared_ptr<RestrictionMap> extractor::LoadRestrictionMap()
+{
+    boost::filesystem::ifstream input_stream(config.restriction_file_name,
+                                             std::ios::in | std::ios::binary);
+
+    std::vector<TurnRestriction> restriction_list;
+    loadRestrictionsFromFile(input_stream, restriction_list);
+
+    SimpleLogger().Write() << " - " << restriction_list.size() << " restrictions.";
+
+    return std::make_shared<RestrictionMap>(restriction_list);
+}
+
+/**
+  \brief Load node based graph from .osrm file
+  */
+std::shared_ptr<NodeBasedDynamicGraph>
+extractor::LoadNodeBasedGraph(std::unordered_set<NodeID> &barrier_nodes,
+                              std::unordered_set<NodeID> &traffic_lights,
+                              std::vector<QueryNode> &internal_to_external_node_map)
+{
+    std::vector<NodeBasedEdge> edge_list;
+
+    boost::filesystem::ifstream input_stream(config.output_file_name,
+                                             std::ios::in | std::ios::binary);
+
+    std::vector<NodeID> barrier_list;
+    std::vector<NodeID> traffic_light_list;
+    NodeID number_of_node_based_nodes = loadNodesFromFile(
+        input_stream, barrier_list, traffic_light_list, internal_to_external_node_map);
+
+    SimpleLogger().Write() << " - " << barrier_list.size() << " bollard nodes, "
+                           << traffic_light_list.size() << " traffic lights";
+
+    // insert into unordered sets for fast lookup
+    barrier_nodes.insert(barrier_list.begin(), barrier_list.end());
+    traffic_lights.insert(traffic_light_list.begin(), traffic_light_list.end());
+
+    barrier_list.clear();
+    barrier_list.shrink_to_fit();
+    traffic_light_list.clear();
+    traffic_light_list.shrink_to_fit();
+
+    loadEdgesFromFile(input_stream, edge_list);
+
+    if (edge_list.empty())
+    {
+        SimpleLogger().Write(logWARNING) << "The input data is empty, exiting.";
+        return std::shared_ptr<NodeBasedDynamicGraph>();
+    }
+
+    return NodeBasedDynamicGraphFromEdges(number_of_node_based_nodes, edge_list);
+}
+
+/**
+ \brief Building an edge-expanded graph from node-based input and turn restrictions
+*/
+std::pair<std::size_t, std::size_t>
+extractor::BuildEdgeExpandedGraph(std::vector<QueryNode> &internal_to_external_node_map,
+                                  std::vector<EdgeBasedNode> &node_based_edge_list,
+                                  std::vector<bool> &node_is_startpoint,
+                                  DeallocatingVector<EdgeBasedEdge> &edge_based_edge_list)
+{
+    lua_State *lua_state = luaL_newstate();
+    luabind::open(lua_state);
+
+    SpeedProfileProperties speed_profile;
+    SetupScriptingEnvironment(lua_state, speed_profile);
+
+    std::unordered_set<NodeID> barrier_nodes;
+    std::unordered_set<NodeID> traffic_lights;
+
+    auto restriction_map = LoadRestrictionMap();
+    auto node_based_graph =
+        LoadNodeBasedGraph(barrier_nodes, traffic_lights, internal_to_external_node_map);
+
+    CompressedEdgeContainer compressed_edge_container;
+    GraphCompressor graph_compressor(speed_profile);
+    graph_compressor.Compress(barrier_nodes, traffic_lights, *restriction_map, *node_based_graph,
+                              compressed_edge_container);
+
+    EdgeBasedGraphFactory edge_based_graph_factory(
+        node_based_graph, compressed_edge_container, barrier_nodes, traffic_lights,
+        std::const_pointer_cast<RestrictionMap const>(restriction_map),
+        internal_to_external_node_map, speed_profile);
+
+    compressed_edge_container.SerializeInternalVector(config.geometry_output_path);
+
+    edge_based_graph_factory.Run(config.edge_output_path, lua_state,
+                                 config.edge_segment_lookup_path, config.edge_penalty_path,
+                                 config.generate_edge_lookup
+#ifdef DEBUG_GEOMETRY
+                                 ,
+                                 config.debug_turns_path
+#endif
+                                 );
+    lua_close(lua_state);
+
+    edge_based_graph_factory.GetEdgeBasedEdges(edge_based_edge_list);
+    edge_based_graph_factory.GetEdgeBasedNodes(node_based_edge_list);
+    edge_based_graph_factory.GetStartPointMarkers(node_is_startpoint);
+    auto max_edge_id = edge_based_graph_factory.GetHighestEdgeID();
+
+    const std::size_t number_of_node_based_nodes = node_based_graph->GetNumberOfNodes();
+    return std::make_pair(number_of_node_based_nodes, max_edge_id);
+}
+
+/**
+  \brief Writing info on original (node-based) nodes
+ */
+void extractor::WriteNodeMapping(const std::vector<QueryNode> &internal_to_external_node_map)
+{
+    boost::filesystem::ofstream node_stream(config.node_output_path, std::ios::binary);
+    const unsigned size_of_mapping = internal_to_external_node_map.size();
+    node_stream.write((char *)&size_of_mapping, sizeof(unsigned));
+    if (size_of_mapping > 0)
+    {
+        node_stream.write((char *)internal_to_external_node_map.data(),
+                          size_of_mapping * sizeof(QueryNode));
+    }
+    node_stream.close();
+}
+
+/**
+    \brief Building rtree-based nearest-neighbor data structure
+
+    Saves tree into '.ramIndex' and leaves into '.fileIndex'.
+ */
+void extractor::BuildRTree(std::vector<EdgeBasedNode> node_based_edge_list,
+                           std::vector<bool> node_is_startpoint,
+                           const std::vector<QueryNode> &internal_to_external_node_map)
+{
+    SimpleLogger().Write() << "constructing r-tree of " << node_based_edge_list.size()
+                           << " edge elements build on-top of "
+                           << internal_to_external_node_map.size() << " coordinates";
+
+    BOOST_ASSERT(node_is_startpoint.size() == node_based_edge_list.size());
+
+    // Filter node based edges based on startpoint
+    auto out_iter = node_based_edge_list.begin();
+    auto in_iter = node_based_edge_list.begin();
+    for (auto index : osrm::irange<std::size_t>(0, node_is_startpoint.size()))
+    {
+        BOOST_ASSERT(in_iter != node_based_edge_list.end());
+        if (node_is_startpoint[index])
+        {
+            *out_iter = *in_iter;
+            out_iter++;
+        }
+        in_iter++;
+    }
+    auto new_size = out_iter - node_based_edge_list.begin();
+    node_based_edge_list.resize(new_size);
+
+    TIMER_START(construction);
+    StaticRTree<EdgeBasedNode>(node_based_edge_list, config.rtree_nodes_output_path,
+                               config.rtree_leafs_output_path, internal_to_external_node_map);
+
+    TIMER_STOP(construction);
+    SimpleLogger().Write() << "finished r-tree construction in " << TIMER_SEC(construction)
+                           << " seconds";
+}
+
+void extractor::WriteEdgeBasedGraph(std::string const &output_file_filename,
+                                    size_t const max_edge_id,
+                                    DeallocatingVector<EdgeBasedEdge> const &edge_based_edge_list)
+{
+
+    std::ofstream file_out_stream;
+    file_out_stream.open(output_file_filename.c_str(), std::ios::binary);
+    const FingerPrint fingerprint = FingerPrint::GetValid();
+    file_out_stream.write((char *)&fingerprint, sizeof(FingerPrint));
+
+    std::cout << "[extractor] Writing edge-based-graph egdes       ... " << std::flush;
+    TIMER_START(write_edges);
+
+    size_t number_of_used_edges = edge_based_edge_list.size();
+    file_out_stream.write((char *)&number_of_used_edges, sizeof(size_t));
+    file_out_stream.write((char *)&max_edge_id, sizeof(size_t));
+
+    for (const auto &edge : edge_based_edge_list)
+    {
+        file_out_stream.write((char *)&edge, sizeof(EdgeBasedEdge));
+    }
+
+    TIMER_STOP(write_edges);
+    std::cout << "ok, after " << TIMER_SEC(write_edges) << "s" << std::endl;
+
+    SimpleLogger().Write() << "Processed " << number_of_used_edges << " edges";
+    file_out_stream.close();
+}
diff --git a/extractor/extractor.hpp b/extractor/extractor.hpp
index bc82241..c147f19 100644
--- a/extractor/extractor.hpp
+++ b/extractor/extractor.hpp
@@ -29,13 +29,38 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define EXTRACTOR_HPP
 
 #include "extractor_options.hpp"
+#include "edge_based_graph_factory.hpp"
+#include "../algorithms/graph_compressor.hpp"
 
 class extractor
 {
-public:
-  extractor(ExtractorConfig extractor_config) : config(std::move(extractor_config)) {}
+  public:
+    extractor(ExtractorConfig extractor_config) : config(std::move(extractor_config)) {}
     int run();
-private:
-   ExtractorConfig config;
+
+  private:
+    ExtractorConfig config;
+    void SetupScriptingEnvironment(lua_State *myLuaState, SpeedProfileProperties &speed_profile);
+    std::pair<std::size_t, std::size_t>
+    BuildEdgeExpandedGraph(std::vector<QueryNode> &internal_to_external_node_map,
+                           std::vector<EdgeBasedNode> &node_based_edge_list,
+                           std::vector<bool> &node_is_startpoint,
+                           DeallocatingVector<EdgeBasedEdge> &edge_based_edge_list);
+    void WriteNodeMapping(const std::vector<QueryNode> &internal_to_external_node_map);
+    void FindComponents(unsigned max_edge_id,
+                        const DeallocatingVector<EdgeBasedEdge> &edges,
+                        std::vector<EdgeBasedNode> &nodes) const;
+    void BuildRTree(std::vector<EdgeBasedNode> node_based_edge_list,
+                    std::vector<bool> node_is_startpoint,
+                    const std::vector<QueryNode> &internal_to_external_node_map);
+    std::shared_ptr<RestrictionMap> LoadRestrictionMap();
+    std::shared_ptr<NodeBasedDynamicGraph>
+    LoadNodeBasedGraph(std::unordered_set<NodeID> &barrier_nodes,
+                       std::unordered_set<NodeID> &traffic_lights,
+                       std::vector<QueryNode> &internal_to_external_node_map);
+
+    void WriteEdgeBasedGraph(std::string const &output_file_filename,
+                             size_t const max_edge_id,
+                             DeallocatingVector<EdgeBasedEdge> const &edge_based_edge_list);
 };
 #endif /* EXTRACTOR_HPP */
diff --git a/extractor/extractor_callbacks.cpp b/extractor/extractor_callbacks.cpp
index 6a50f16..2eb1a9b 100644
--- a/extractor/extractor_callbacks.cpp
+++ b/extractor/extractor_callbacks.cpp
@@ -35,6 +35,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "../util/container.hpp"
 #include "../util/simple_logger.hpp"
 
+#include <boost/optional/optional.hpp>
+
+#include <osmium/osm.hpp>
+
 #include <osrm/coordinate.hpp>
 
 #include <limits>
@@ -59,13 +63,13 @@ void ExtractorCallbacks::ProcessNode(const osmium::Node &input_node,
     external_memory.all_nodes_list.push_back(
         {static_cast<int>(input_node.location().lat() * COORDINATE_PRECISION),
          static_cast<int>(input_node.location().lon() * COORDINATE_PRECISION),
-         static_cast<NodeID>(input_node.id()),
+         OSMNodeID(input_node.id()),
          result_node.barrier,
          result_node.traffic_lights});
 }
 
 void ExtractorCallbacks::ProcessRestriction(
-    const mapbox::util::optional<InputRestrictionContainer> &restriction)
+    const boost::optional<InputRestrictionContainer> &restriction)
 {
     if (restriction)
     {
@@ -140,8 +144,8 @@ void ExtractorCallbacks::ProcessWay(const osmium::Way &input_way, const Extracti
         }
     }
 
-    if (forward_weight_data.type == InternalExtractorEdge::WeightType::INVALID
-     && backward_weight_data.type == InternalExtractorEdge::WeightType::INVALID)
+    if (forward_weight_data.type == InternalExtractorEdge::WeightType::INVALID &&
+        backward_weight_data.type == InternalExtractorEdge::WeightType::INVALID)
     {
         SimpleLogger().Write(logDEBUG) << "found way with bogus speed, id: " << input_way.id();
         return;
@@ -149,10 +153,12 @@ void ExtractorCallbacks::ProcessWay(const osmium::Way &input_way, const Extracti
 
     // Get the unique identifier for the street name
     const auto &string_map_iterator = string_map.find(parsed_way.name);
-    unsigned name_id = external_memory.name_list.size();
+    unsigned name_id = external_memory.name_lengths.size();
     if (string_map.end() == string_map_iterator)
     {
-        external_memory.name_list.push_back(parsed_way.name);
+        auto name_length = std::min<unsigned>(255u, parsed_way.name.size());
+        std::copy(parsed_way.name.c_str(), parsed_way.name.c_str() + name_length, std::back_inserter(external_memory.name_char_data));
+        external_memory.name_lengths.push_back(name_length);
         string_map.insert(std::make_pair(parsed_way.name, name_id));
     }
     else
@@ -169,7 +175,10 @@ void ExtractorCallbacks::ProcessWay(const osmium::Way &input_way, const Extracti
 
     std::transform(input_way.nodes().begin(), input_way.nodes().end(),
                    std::back_inserter(external_memory.used_node_id_list),
-                   [](const osmium::NodeRef& ref) { return ref.ref(); });
+                   [](const osmium::NodeRef &ref)
+                   {
+                       return OSMNodeID(ref.ref());
+                   });
 
     const bool is_opposite_way = TRAVEL_MODE_INACCESSIBLE == parsed_way.forward_travel_mode;
 
@@ -182,53 +191,51 @@ void ExtractorCallbacks::ProcessWay(const osmium::Way &input_way, const Extracti
                             [&](const osmium::NodeRef &first_node, const osmium::NodeRef &last_node)
                             {
                                 external_memory.all_edges_list.push_back(InternalExtractorEdge(
-                                    first_node.ref(), last_node.ref(), name_id, backward_weight_data,
-                                    true, false, parsed_way.roundabout, parsed_way.is_access_restricted,
+                                    OSMNodeID(first_node.ref()), OSMNodeID(last_node.ref()), name_id,
+                                    backward_weight_data, true, false, parsed_way.roundabout,
+                                    parsed_way.is_access_restricted, parsed_way.is_startpoint,
                                     parsed_way.backward_travel_mode, false));
                             });
 
         external_memory.way_start_end_id_list.push_back(
-            {
-                static_cast<EdgeID>(input_way.id()),
-                static_cast<NodeID>(input_way.nodes().back().ref()),
-                static_cast<NodeID>(input_way.nodes()[input_way.nodes().size() - 2].ref()),
-                static_cast<NodeID>(input_way.nodes()[1].ref()),
-                static_cast<NodeID>(input_way.nodes()[0].ref())
-             }
-        );
+            {OSMWayID(input_way.id()),
+             OSMNodeID(input_way.nodes().back().ref()),
+             OSMNodeID(input_way.nodes()[input_way.nodes().size() - 2].ref()),
+             OSMNodeID(input_way.nodes()[1].ref()),
+             OSMNodeID(input_way.nodes()[0].ref())});
     }
     else
     {
-        const bool forward_only = split_edge || TRAVEL_MODE_INACCESSIBLE == parsed_way.backward_travel_mode;
+        const bool forward_only =
+            split_edge || TRAVEL_MODE_INACCESSIBLE == parsed_way.backward_travel_mode;
         osrm::for_each_pair(input_way.nodes().cbegin(), input_way.nodes().cend(),
                             [&](const osmium::NodeRef &first_node, const osmium::NodeRef &last_node)
                             {
                                 external_memory.all_edges_list.push_back(InternalExtractorEdge(
-                                    first_node.ref(), last_node.ref(), name_id, forward_weight_data,
-                                    true, !forward_only, parsed_way.roundabout, parsed_way.is_access_restricted,
-                                    parsed_way.forward_travel_mode, split_edge));
+                                    OSMNodeID(first_node.ref()), OSMNodeID(last_node.ref()), name_id, forward_weight_data,
+                                    true, !forward_only, parsed_way.roundabout,
+                                    parsed_way.is_access_restricted, parsed_way.is_startpoint, parsed_way.forward_travel_mode,
+                                    split_edge));
                             });
         if (split_edge)
         {
             BOOST_ASSERT(parsed_way.backward_travel_mode != TRAVEL_MODE_INACCESSIBLE);
-            osrm::for_each_pair(input_way.nodes().cbegin(), input_way.nodes().cend(),
-                                [&](const osmium::NodeRef &first_node, const osmium::NodeRef &last_node)
-                                {
-                                    external_memory.all_edges_list.push_back(InternalExtractorEdge(
-                                        first_node.ref(), last_node.ref(), name_id, backward_weight_data,
-                                        false, true, parsed_way.roundabout, parsed_way.is_access_restricted,
-                                        parsed_way.backward_travel_mode, true));
-                                });
+            osrm::for_each_pair(
+                input_way.nodes().cbegin(), input_way.nodes().cend(),
+                [&](const osmium::NodeRef &first_node, const osmium::NodeRef &last_node)
+                {
+                    external_memory.all_edges_list.push_back(InternalExtractorEdge(
+                        OSMNodeID(first_node.ref()), OSMNodeID(last_node.ref()), name_id, backward_weight_data, false,
+                        true, parsed_way.roundabout, parsed_way.is_access_restricted,
+                        parsed_way.is_startpoint, parsed_way.backward_travel_mode, true));
+                });
         }
 
         external_memory.way_start_end_id_list.push_back(
-            {
-                static_cast<EdgeID>(input_way.id()),
-                static_cast<NodeID>(input_way.nodes().back().ref()),
-                static_cast<NodeID>(input_way.nodes()[input_way.nodes().size() - 2].ref()),
-                static_cast<NodeID>(input_way.nodes()[1].ref()),
-                static_cast<NodeID>(input_way.nodes()[0].ref())
-             }
-        );
+            {OSMWayID(input_way.id()),
+             OSMNodeID(input_way.nodes().back().ref()),
+             OSMNodeID(input_way.nodes()[input_way.nodes().size() - 2].ref()),
+             OSMNodeID(input_way.nodes()[1].ref()),
+             OSMNodeID(input_way.nodes()[0].ref())});
     }
 }
diff --git a/extractor/extractor_callbacks.hpp b/extractor/extractor_callbacks.hpp
index 25bbbab..0026b6f 100644
--- a/extractor/extractor_callbacks.hpp
+++ b/extractor/extractor_callbacks.hpp
@@ -28,12 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef EXTRACTOR_CALLBACKS_HPP
 #define EXTRACTOR_CALLBACKS_HPP
 
-#include "extraction_way.hpp"
 #include "../typedefs.h"
-
-#include <osmium/osm.hpp>
-
-#include <variant/optional.hpp>
+#include <boost/optional/optional_fwd.hpp>
 
 #include <string>
 #include <unordered_map>
@@ -42,6 +38,12 @@ struct ExternalMemoryNode;
 class ExtractionContainers;
 struct InputRestrictionContainer;
 struct ExtractionNode;
+struct ExtractionWay;
+namespace osmium
+{
+class Node;
+class Way;
+}
 
 /**
  * This class is uses by the extractor with the results of the
@@ -66,7 +68,7 @@ class ExtractorCallbacks
     void ProcessNode(const osmium::Node &current_node, const ExtractionNode &result_node);
 
     // warning: caller needs to take care of synchronization!
-    void ProcessRestriction(const mapbox::util::optional<InputRestrictionContainer> &restriction);
+    void ProcessRestriction(const boost::optional<InputRestrictionContainer> &restriction);
 
     // warning: caller needs to take care of synchronization!
     void ProcessWay(const osmium::Way &current_way, const ExtractionWay &result_way);
diff --git a/extractor/extractor_options.cpp b/extractor/extractor_options.cpp
index 23cbacc..b607dca 100644
--- a/extractor/extractor_options.cpp
+++ b/extractor/extractor_options.cpp
@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "extractor_options.hpp"
 
-#include "../util/git_sha.hpp"
+#include "util/version.hpp"
 #include "../util/ini_file.hpp"
 #include "../util/simple_logger.hpp"
 
@@ -42,6 +42,12 @@ ExtractorOptions::ParseArguments(int argc, char *argv[], ExtractorConfig &extrac
     // declare a group of options that will be allowed only on command line
     boost::program_options::options_description generic_options("Options");
     generic_options.add_options()("version,v", "Show version")("help,h", "Show this help message")(
+        /*
+         * TODO: re-enable this
+        "restrictions,r",
+        boost::program_options::value<boost::filesystem::path>(&extractor_config.restrictions_path),
+        "Restrictions file in .osrm.restrictions format")(
+        */
         "config,c", boost::program_options::value<boost::filesystem::path>(
                         &extractor_config.config_file_path)->default_value("extractor.ini"),
         "Path to a configuration file.");
@@ -55,7 +61,20 @@ ExtractorOptions::ParseArguments(int argc, char *argv[], ExtractorConfig &extrac
         "threads,t",
         boost::program_options::value<unsigned int>(&extractor_config.requested_num_threads)
             ->default_value(tbb::task_scheduler_init::default_num_threads()),
-        "Number of threads to use");
+        "Number of threads to use")(
+            "generate-edge-lookup",boost::program_options::value<bool>(
+                                                &extractor_config.generate_edge_lookup)->implicit_value(true)->default_value(false),
+                                 "Generate a lookup table for internal edge-expanded-edge IDs to OSM node pairs")(
+        "small-component-size",
+        boost::program_options::value<unsigned int>(&extractor_config.small_component_size)
+            ->default_value(1000),
+        "Number of nodes required before a strongly-connected-componennt is considered big (affects nearest neighbor snapping)");
+
+#ifdef DEBUG_GEOMETRY
+        config_options.add_options()("debug-turns",
+            boost::program_options::value<std::string>(&extractor_config.debug_turns_path),
+            "Write out GeoJSON with turn penalty data");
+#endif // DEBUG_GEOMETRY
 
     // hidden options, will be allowed both on command line and in config file, but will not be
     // shown to the user
@@ -64,6 +83,7 @@ ExtractorOptions::ParseArguments(int argc, char *argv[], ExtractorConfig &extrac
                                                 &extractor_config.input_path),
                                  "Input file in .osm, .osm.bz2 or .osm.pbf format");
 
+
     // positional option
     boost::program_options::positional_options_description positional_options;
     positional_options.add("input", 1);
@@ -90,7 +110,7 @@ ExtractorOptions::ParseArguments(int argc, char *argv[], ExtractorConfig &extrac
                                       option_variables);
         if (option_variables.count("version"))
         {
-            SimpleLogger().Write() << g_GIT_DESCRIPTION;
+            SimpleLogger().Write() << OSRM_VERSION;
             return return_code::exit;
         }
 
@@ -137,6 +157,14 @@ void ExtractorOptions::GenerateOutputFilesNames(ExtractorConfig &extractor_confi
     extractor_config.restriction_file_name = input_path.string();
     extractor_config.names_file_name = input_path.string();
     extractor_config.timestamp_file_name = input_path.string();
+    extractor_config.geometry_output_path = input_path.string();
+    extractor_config.edge_output_path = input_path.string();
+    extractor_config.edge_graph_output_path = input_path.string();
+    extractor_config.node_output_path = input_path.string();
+    extractor_config.rtree_nodes_output_path = input_path.string();
+    extractor_config.rtree_leafs_output_path = input_path.string();
+    extractor_config.edge_segment_lookup_path = input_path.string();
+    extractor_config.edge_penalty_path = input_path.string();
     std::string::size_type pos = extractor_config.output_file_name.find(".osm.bz2");
     if (pos == std::string::npos)
     {
@@ -159,6 +187,14 @@ void ExtractorOptions::GenerateOutputFilesNames(ExtractorConfig &extractor_confi
             extractor_config.restriction_file_name.append(".osrm.restrictions");
             extractor_config.names_file_name.append(".osrm.names");
             extractor_config.timestamp_file_name.append(".osrm.timestamp");
+            extractor_config.geometry_output_path.append(".osrm.geometry");
+            extractor_config.node_output_path.append(".osrm.nodes");
+            extractor_config.edge_output_path.append(".osrm.edges");
+            extractor_config.edge_graph_output_path.append(".osrm.ebg");
+            extractor_config.rtree_nodes_output_path.append(".osrm.ramIndex");
+            extractor_config.rtree_leafs_output_path.append(".osrm.fileIndex");
+            extractor_config.edge_segment_lookup_path.append(".osrm.edge_segment_lookup");
+            extractor_config.edge_penalty_path.append(".osrm.edge_penalties");
         }
         else
         {
@@ -166,6 +202,14 @@ void ExtractorOptions::GenerateOutputFilesNames(ExtractorConfig &extractor_confi
             extractor_config.restriction_file_name.replace(pos, 5, ".osrm.restrictions");
             extractor_config.names_file_name.replace(pos, 5, ".osrm.names");
             extractor_config.timestamp_file_name.replace(pos, 5, ".osrm.timestamp");
+            extractor_config.geometry_output_path.replace(pos, 5, ".osrm.geometry");
+            extractor_config.node_output_path.replace(pos, 5, ".osrm.nodes");
+            extractor_config.edge_output_path.replace(pos, 5, ".osrm.edges");
+            extractor_config.edge_graph_output_path.replace(pos, 5, ".osrm.ebg");
+            extractor_config.rtree_nodes_output_path.replace(pos, 5, ".osrm.ramIndex");
+            extractor_config.rtree_leafs_output_path.replace(pos, 5, ".osrm.fileIndex");
+            extractor_config.edge_segment_lookup_path.replace(pos,5, ".osrm.edge_segment_lookup");
+            extractor_config.edge_penalty_path.replace(pos,5, ".osrm.edge_penalties");
         }
     }
     else
@@ -174,5 +218,13 @@ void ExtractorOptions::GenerateOutputFilesNames(ExtractorConfig &extractor_confi
         extractor_config.restriction_file_name.replace(pos, 8, ".osrm.restrictions");
         extractor_config.names_file_name.replace(pos, 8, ".osrm.names");
         extractor_config.timestamp_file_name.replace(pos, 8, ".osrm.timestamp");
+        extractor_config.geometry_output_path.replace(pos, 8, ".osrm.geometry");
+        extractor_config.node_output_path.replace(pos, 8, ".osrm.nodes");
+        extractor_config.edge_output_path.replace(pos, 8, ".osrm.edges");
+        extractor_config.edge_graph_output_path.replace(pos, 8, ".osrm.ebg");
+        extractor_config.rtree_nodes_output_path.replace(pos, 8, ".osrm.ramIndex");
+        extractor_config.rtree_leafs_output_path.replace(pos, 8, ".osrm.fileIndex");
+        extractor_config.edge_segment_lookup_path.replace(pos,8, ".osrm.edge_segment_lookup");
+        extractor_config.edge_penalty_path.replace(pos,8, ".osrm.edge_penalties");
     }
 }
diff --git a/extractor/extractor_options.hpp b/extractor/extractor_options.hpp
index c164f6a..00d6f84 100644
--- a/extractor/extractor_options.hpp
+++ b/extractor/extractor_options.hpp
@@ -50,8 +50,22 @@ struct ExtractorConfig
     std::string restriction_file_name;
     std::string names_file_name;
     std::string timestamp_file_name;
+    std::string geometry_output_path;
+    std::string edge_output_path;
+    std::string edge_graph_output_path;
+    std::string node_output_path;
+    std::string rtree_nodes_output_path;
+    std::string rtree_leafs_output_path;
 
     unsigned requested_num_threads;
+    unsigned small_component_size;
+
+    bool generate_edge_lookup;
+    std::string edge_penalty_path;
+    std::string edge_segment_lookup_path;
+#ifdef DEBUG_GEOMETRY
+    std::string debug_turns_path;
+#endif
 };
 
 struct ExtractorOptions
diff --git a/extractor/first_and_last_segment_of_way.hpp b/extractor/first_and_last_segment_of_way.hpp
index 3a26be7..ead8b4c 100644
--- a/extractor/first_and_last_segment_of_way.hpp
+++ b/extractor/first_and_last_segment_of_way.hpp
@@ -36,21 +36,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 struct FirstAndLastSegmentOfWay
 {
-    EdgeID way_id;
-    NodeID first_segment_source_id;
-    NodeID first_segment_target_id;
-    NodeID last_segment_source_id;
-    NodeID last_segment_target_id;
+    OSMWayID way_id;
+    OSMNodeID first_segment_source_id;
+    OSMNodeID first_segment_target_id;
+    OSMNodeID last_segment_source_id;
+    OSMNodeID last_segment_target_id;
+
     FirstAndLastSegmentOfWay()
-        : way_id(std::numeric_limits<EdgeID>::max()),
-          first_segment_source_id(std::numeric_limits<NodeID>::max()),
-          first_segment_target_id(std::numeric_limits<NodeID>::max()),
-          last_segment_source_id(std::numeric_limits<NodeID>::max()),
-          last_segment_target_id(std::numeric_limits<NodeID>::max())
+        : way_id(SPECIAL_OSM_WAYID),
+          first_segment_source_id(SPECIAL_OSM_NODEID),
+          first_segment_target_id(SPECIAL_OSM_NODEID),
+          last_segment_source_id(SPECIAL_OSM_NODEID),
+          last_segment_target_id(SPECIAL_OSM_NODEID)
     {
     }
 
-    FirstAndLastSegmentOfWay(EdgeID w, NodeID fs, NodeID ft, NodeID ls, NodeID lt)
+    FirstAndLastSegmentOfWay(OSMWayID w, OSMNodeID fs, OSMNodeID ft, OSMNodeID ls, OSMNodeID lt)
         : way_id(w), first_segment_source_id(fs), first_segment_target_id(ft),
           last_segment_source_id(ls), last_segment_target_id(lt)
     {
@@ -58,19 +59,19 @@ struct FirstAndLastSegmentOfWay
 
     static FirstAndLastSegmentOfWay min_value()
     {
-        return {std::numeric_limits<EdgeID>::min(),
-                std::numeric_limits<NodeID>::min(),
-                std::numeric_limits<NodeID>::min(),
-                std::numeric_limits<NodeID>::min(),
-                std::numeric_limits<NodeID>::min()};
+        return {MIN_OSM_WAYID,
+                MIN_OSM_NODEID,
+                MIN_OSM_NODEID,
+                MIN_OSM_NODEID,
+                MIN_OSM_NODEID};
     }
     static FirstAndLastSegmentOfWay max_value()
     {
-        return {std::numeric_limits<EdgeID>::max(),
-                std::numeric_limits<NodeID>::max(),
-                std::numeric_limits<NodeID>::max(),
-                std::numeric_limits<NodeID>::max(),
-                std::numeric_limits<NodeID>::max()};
+        return {MAX_OSM_WAYID,
+                MAX_OSM_NODEID,
+                MAX_OSM_NODEID,
+                MAX_OSM_NODEID,
+                MAX_OSM_NODEID};
     }
 };
 
diff --git a/extractor/internal_extractor_edge.hpp b/extractor/internal_extractor_edge.hpp
index 2dc3c34..0df6e8c 100644
--- a/extractor/internal_extractor_edge.hpp
+++ b/extractor/internal_extractor_edge.hpp
@@ -63,29 +63,31 @@ struct InternalExtractorEdge
     };
 
     explicit InternalExtractorEdge()
-        : result(0, 0, 0, 0, false, false, false, false,
+        : result(MIN_OSM_NODEID, MIN_OSM_NODEID, 0, 0, false, false, false, false, true,
                 TRAVEL_MODE_INACCESSIBLE, false)
     {
     }
 
-    explicit InternalExtractorEdge(NodeID source,
-                                   NodeID target,
+    explicit InternalExtractorEdge(OSMNodeID source,
+                                   OSMNodeID target,
                                    NodeID name_id,
                                    WeightData weight_data,
                                    bool forward,
                                    bool backward,
                                    bool roundabout,
                                    bool access_restricted,
+                                   bool startpoint,
                                    TravelMode travel_mode,
                                    bool is_split)
-        : result(source,
-                 target,
+        : result(OSMNodeID(source),
+                 OSMNodeID(target),
                  name_id,
                  0,
                  forward,
                  backward,
                  roundabout,
                  access_restricted,
+                 startpoint,
                  travel_mode,
                  is_split),
           weight_data(std::move(weight_data))
@@ -93,7 +95,7 @@ struct InternalExtractorEdge
     }
 
     // data that will be written to disk
-    NodeBasedEdge result;
+    NodeBasedEdgeWithOSM result;
     // intermediate edge weight
     WeightData weight_data;
     // coordinate of the source node
@@ -101,19 +103,35 @@ struct InternalExtractorEdge
 
 
     // necessary static util functions for stxxl's sorting
-    static InternalExtractorEdge min_value()
+    static InternalExtractorEdge min_osm_value()
     {
-        return InternalExtractorEdge(0, 0, 0, WeightData(), false, false, false,
-                                     false, TRAVEL_MODE_INACCESSIBLE, false);
+        return InternalExtractorEdge(MIN_OSM_NODEID, MIN_OSM_NODEID, 0, WeightData(), false, false, false,
+                                     false, true, TRAVEL_MODE_INACCESSIBLE, false);
     }
-    static InternalExtractorEdge max_value()
+    static InternalExtractorEdge max_osm_value()
     {
-        return InternalExtractorEdge(SPECIAL_NODEID, SPECIAL_NODEID, 0, WeightData(), false,
-                                     false, false, false, TRAVEL_MODE_INACCESSIBLE, false);
+        return InternalExtractorEdge(MAX_OSM_NODEID, MAX_OSM_NODEID, 0, WeightData(), false,
+                                     false, false, false, true, TRAVEL_MODE_INACCESSIBLE, false);
     }
+
+    static InternalExtractorEdge min_internal_value()
+    {
+        auto v = min_osm_value();
+        v.result.source = 0;
+        v.result.target = 0;
+        return v;
+    }
+    static InternalExtractorEdge max_internal_value()
+    {
+        auto v = max_osm_value();
+        v.result.source = std::numeric_limits<NodeID>::max();
+        v.result.target = std::numeric_limits<NodeID>::max();
+        return v;
+    }
+
 };
 
-struct CmpEdgeByStartThenTargetID
+struct CmpEdgeByInternalStartThenInternalTargetID
 {
     using value_type = InternalExtractorEdge;
     bool operator()(const InternalExtractorEdge &lhs, const InternalExtractorEdge &rhs) const
@@ -123,32 +141,32 @@ struct CmpEdgeByStartThenTargetID
                (lhs.result.target <  rhs.result.target));
     }
 
-    value_type max_value() { return InternalExtractorEdge::max_value(); }
-    value_type min_value() { return InternalExtractorEdge::min_value(); }
+    value_type max_value() { return InternalExtractorEdge::max_internal_value(); }
+    value_type min_value() { return InternalExtractorEdge::min_internal_value(); }
 };
 
-struct CmpEdgeByStartID
+struct CmpEdgeByOSMStartID
 {
     using value_type = InternalExtractorEdge;
     bool operator()(const InternalExtractorEdge &lhs, const InternalExtractorEdge &rhs) const
     {
-        return lhs.result.source < rhs.result.source;
+        return lhs.result.osm_source_id < rhs.result.osm_source_id;
     }
 
-    value_type max_value() { return InternalExtractorEdge::max_value(); }
-    value_type min_value() { return InternalExtractorEdge::min_value(); }
+    value_type max_value() { return InternalExtractorEdge::max_osm_value(); }
+    value_type min_value() { return InternalExtractorEdge::min_osm_value(); }
 };
 
-struct CmpEdgeByTargetID
+struct CmpEdgeByOSMTargetID
 {
     using value_type = InternalExtractorEdge;
     bool operator()(const InternalExtractorEdge &lhs, const InternalExtractorEdge &rhs) const
     {
-        return lhs.result.target < rhs.result.target;
+        return lhs.result.osm_target_id < rhs.result.osm_target_id;
     }
 
-    value_type max_value() { return InternalExtractorEdge::max_value(); }
-    value_type min_value() { return InternalExtractorEdge::min_value(); }
+    value_type max_value() { return InternalExtractorEdge::max_osm_value(); }
+    value_type min_value() { return InternalExtractorEdge::min_osm_value(); }
 };
 
 #endif // INTERNAL_EXTRACTOR_EDGE_HPP
diff --git a/extractor/lat b/extractor/lat
deleted file mode 100644
index e69de29..0000000
diff --git a/extractor/restriction_parser.cpp b/extractor/restriction_parser.cpp
index 86174d4..afb1947 100644
--- a/extractor/restriction_parser.cpp
+++ b/extractor/restriction_parser.cpp
@@ -38,17 +38,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <boost/algorithm/string/predicate.hpp>
 #include <boost/ref.hpp>
 #include <boost/regex.hpp>
+#include <boost/optional/optional.hpp>
+
+#include <osmium/osm.hpp>
+#include <osmium/tags/regex_filter.hpp>
 
 #include <algorithm>
+#include <iterator>
 
 namespace
 {
 int lua_error_callback(lua_State *lua_state)
 {
     std::string error_msg = lua_tostring(lua_state, -1);
-    std::ostringstream error_stream;
-    error_stream << error_msg;
-    throw osrm::exception("ERROR occured in profile script:\n" + error_stream.str());
+    throw osrm::exception("ERROR occured in profile script:\n" + error_msg);
 }
 }
 
@@ -104,18 +107,18 @@ void RestrictionParser::ReadRestrictionExceptions(lua_State *lua_state)
 
 /**
  * Tries to parse an relation as turn restriction. This can fail for a number of
- * reasons, this the return type is a mapbox::util::optional<>.
+ * reasons, this the return type is a boost::optional<T>.
  *
  * Some restrictions can also be ignored: See the ```get_exceptions``` function
  * in the corresponding profile.
  */
-mapbox::util::optional<InputRestrictionContainer>
+boost::optional<InputRestrictionContainer>
 RestrictionParser::TryParse(const osmium::Relation &relation) const
 {
     // return if turn restrictions should be ignored
     if (!use_turn_restrictions)
     {
-        return mapbox::util::optional<InputRestrictionContainer>();
+        return {};
     }
 
     osmium::tags::KeyPrefixFilter filter(false);
@@ -129,14 +132,14 @@ RestrictionParser::TryParse(const osmium::Relation &relation) const
     // if it's a restriction, continue;
     if (std::distance(fi_begin, fi_end) == 0)
     {
-        return mapbox::util::optional<InputRestrictionContainer>();
+        return {};
     }
 
     // check if the restriction should be ignored
     const char *except = relation.get_value_by_key("except");
     if (except != nullptr && ShouldIgnoreRestriction(except))
     {
-        return mapbox::util::optional<InputRestrictionContainer>();
+        return {};
     }
 
     bool is_only_restriction = false;
@@ -164,7 +167,7 @@ RestrictionParser::TryParse(const osmium::Relation &relation) const
 
             if (!is_actually_restricted)
             {
-                return mapbox::util::optional<InputRestrictionContainer>();
+                return {};
             }
         }
     }
@@ -218,7 +221,7 @@ RestrictionParser::TryParse(const osmium::Relation &relation) const
             break;
         }
     }
-    return mapbox::util::optional<InputRestrictionContainer>(restriction_container);
+    return boost::make_optional(std::move(restriction_container));
 }
 
 bool RestrictionParser::ShouldIgnoreRestriction(const std::string &except_tag_string) const
diff --git a/extractor/restriction_parser.hpp b/extractor/restriction_parser.hpp
index f99335d..11f94ee 100644
--- a/extractor/restriction_parser.hpp
+++ b/extractor/restriction_parser.hpp
@@ -30,16 +30,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "../data_structures/restriction.hpp"
 
-#include <osmium/osm.hpp>
-#include <osmium/tags/regex_filter.hpp>
-
-#include <variant/optional.hpp>
+#include <boost/optional/optional.hpp>
 
 #include <string>
 #include <vector>
 
 struct lua_State;
-class ScriptingEnvironment;
+namespace osmium
+{
+class Relation;
+}
 
 /**
  * Parses the relations that represents turn restrictions.
@@ -63,8 +63,7 @@ class RestrictionParser
 {
   public:
     RestrictionParser(lua_State *lua_state);
-    mapbox::util::optional<InputRestrictionContainer>
-    TryParse(const osmium::Relation &relation) const;
+    boost::optional<InputRestrictionContainer> TryParse(const osmium::Relation &relation) const;
 
   private:
     void ReadUseRestrictionsSetting(lua_State *lua_state);
diff --git a/extractor/scripting_environment.cpp b/extractor/scripting_environment.cpp
index de96b11..7ccc08a 100644
--- a/extractor/scripting_environment.cpp
+++ b/extractor/scripting_environment.cpp
@@ -117,6 +117,7 @@ void ScriptingEnvironment::init_lua_state(lua_State *lua_state)
             .def_readwrite("name", &ExtractionWay::name)
             .def_readwrite("roundabout", &ExtractionWay::roundabout)
             .def_readwrite("is_access_restricted", &ExtractionWay::is_access_restricted)
+            .def_readwrite("is_startpoint", &ExtractionWay::is_startpoint)
             .def_readwrite("duration", &ExtractionWay::duration)
             .property("forward_mode", &ExtractionWay::get_forward_mode,
                       &ExtractionWay::set_forward_mode)
diff --git a/extractor/source_coordinate.lat b/extractor/source_coordinate.lat
deleted file mode 100644
index e69de29..0000000
diff --git a/contractor/speed_profile.hpp b/extractor/speed_profile.hpp
similarity index 100%
rename from contractor/speed_profile.hpp
rename to extractor/speed_profile.hpp
diff --git a/features/car/advisory.feature b/features/car/advisory.feature
new file mode 100644
index 0000000..db5e66d
--- /dev/null
+++ b/features/car/advisory.feature
@@ -0,0 +1,67 @@
+ at routing @maxspeed @car
+Feature: Car - Max speed restrictions
+OSRM will use 4/5 of the projected free-flow speed.
+
+    Background: Use specific speeds
+        Given the profile "car"
+        Given a grid size of 1000 meters
+
+    Scenario: Car - Advisory speed overwrites maxspeed
+        Given the node map
+            | a | b | c |
+
+        And the ways
+            | nodes | highway       | maxspeed | maxspeed:advisory |
+            | ab    | residential   | 90       | 45                |
+            | bc    | residential   |          | 45                |
+
+        When I route I should get
+            | from | to | route | speed        |
+            | a    | b  | ab    | 47 km/h +- 1 |
+            | b    | c  | bc    | 47 km/h +- 1 |
+
+    Scenario: Car - Advisory speed overwrites forward maxspeed
+        Given the node map
+            | a | b | c |
+
+        And the ways
+            | nodes | highway       | maxspeed:forward | maxspeed:advisory:forward |
+            | ab    | residential   | 90               | 45                        |
+            | bc    | residential   |                  | 45                        |
+
+        When I route I should get
+            | from | to | route | speed        |
+            | a    | b  | ab    | 47 km/h +- 1 |
+            | b    | c  | bc    | 47 km/h +- 1 |
+
+    Scenario: Car - Advisory speed overwrites backwards maxspeed
+        Given the node map
+            | a | b | c |
+
+        And the ways
+            | nodes | highway       | maxspeed:backward | maxspeed:advisory:backward |
+            | ab    | residential   | 90                | 45                         |
+            | bc    | residential   |                   | 45                         |
+
+        When I route I should get
+            | from | to | route | speed        |
+            | b    | a  | ab    | 47 km/h +- 1 |
+            | c    | b  | bc    | 47 km/h +- 1 |
+
+    Scenario: Car - Directional advisory speeds play nice with eachother
+        Given the node map
+            | a | b | c |
+
+        And the ways
+            | nodes | highway       | maxspeed:advisory | maxspeed:advisory:forward | maxspeed:advisory:backward |
+            | ab    | residential   | 90                | 45                        | 60                         |
+            | bc    | residential   | 90                | 60                        | 45                         |
+
+        When I route I should get
+            | from | to | route | speed        |
+            | a    | b  | ab    | 47 km/h +- 1 |
+            | b    | a  | ab    | 59 km/h +- 1 |
+            | b    | c  | bc    | 59 km/h +- 1 |
+            | c    | b  | bc    | 47 km/h +- 1 |
+
+
diff --git a/features/car/mode.feature b/features/car/mode.feature
new file mode 100644
index 0000000..ff156a0
--- /dev/null
+++ b/features/car/mode.feature
@@ -0,0 +1,40 @@
+ at routing @car @mode
+Feature: Car - Mode flag
+    Background:
+        Given the profile "car"
+
+    Scenario: Car - Mode when using a ferry
+        Given the node map
+            | a | b |   |
+            |   | c | d |
+
+        And the ways
+            | nodes | highway | route | duration |
+            | ab    | primary |       |          |
+            | bc    |         | ferry | 0:01     |
+            | cd    | primary |       |          |
+
+        When I route I should get
+            | from | to | route    | turns                       | modes |
+            | a    | d  | ab,bc,cd | head,right,left,destination | 1,2,1 |
+            | d    | a  | cd,bc,ab | head,right,left,destination | 1,2,1 |
+            | c    | a  | bc,ab    | head,left,destination       | 2,1   |
+            | d    | b  | cd,bc    | head,right,destination      | 1,2   |
+            | a    | c  | ab,bc    | head,right,destination      | 1,2   |
+            | b    | d  | bc,cd    | head,left,destination       | 2,1   |
+
+    Scenario: Car - Snapping when using a ferry
+        Given the node map
+            | a | b |   | c | d |   | e | f |
+
+        And the ways
+            | nodes | highway | route | duration |
+            | ab    | primary |       |          |
+            | bcde  |         | ferry | 0:10     |
+            | ef    | primary |       |          |
+
+        When I route I should get
+            | from | to | route | turns            | modes   | time  |
+            | c    | d  | bcde  | head,destination | 2       | 600s  |
+
+
diff --git a/features/locate/locate.feature b/features/locate/locate.feature
deleted file mode 100644
index 838bcd5..0000000
--- a/features/locate/locate.feature
+++ /dev/null
@@ -1,197 +0,0 @@
- at locate
-Feature: Locate - return nearest node
-
-    Background:
-        Given the profile "testbot"
-
-    Scenario: Locate - two ways crossing
-        Given the node map
-            |   |  | 0 | c | 1 |  |   |
-            |   |  |   |   |   |  |   |
-            | 7 |  |   | n |   |  | 2 |
-            | a |  | k | x | m |  | b |
-            | 6 |  |   | l |   |  | 3 |
-            |   |  |   |   |   |  |   |
-            |   |  | 5 | d | 4 |  |   |
-
-        And the ways
-            | nodes |
-            | axb   |
-            | cxd   |
-
-        When I request locate I should get
-            | in | out |
-            | 0  | c   |
-            | 1  | c   |
-            | 2  | b   |
-            | 3  | b   |
-            | 4  | d   |
-            | 5  | d   |
-            | 6  | a   |
-            | 7  | a   |
-            | a  | a   |
-            | b  | b   |
-            | c  | c   |
-            | d  | d   |
-            | k  | x   |
-            | l  | x   |
-            | m  | x   |
-            | n  | x   |
-
-    Scenario: Locate - inside a triangle
-        Given the node map
-            |   |  |   |   |   | c |   |   |   |  |   |
-            |   |  |   |   |   | 7 |   |   |   |  |   |
-            |   |  |   | y |   |   |   | z |   |  |   |
-            |   |  | 5 |   | 0 |   | 1 |   | 8 |  |   |
-            | 6 |  |   | 2 |   | 3 |   | 4 |   |  | 9 |
-            | a |  |   | x |   | u |   | w |   |  | b |
-
-        And the ways
-            | nodes |
-            | ab    |
-            | bc    |
-            | ca    |
-
-        When I request locate I should get
-            | in | out |
-            | 0  | c   |
-            | 1  | c   |
-            | 2  | a   |
-            | 3  | c   |
-            | 4  | b   |
-            | 5  | a   |
-            | 6  | a   |
-            | 7  | c   |
-            | 8  | b   |
-            | 9  | b   |
-            | x  | a   |
-            | y  | c   |
-            | z  | c   |
-            | w  | b   |
-
-    Scenario: Nearest - easy-west way
-        Given the node map
-            | 3 | 4 |   | 5 | 6 |
-            | 2 | a | x | b | 7 |
-            | 1 | 0 |   | 9 | 8 |
-
-        And the ways
-            | nodes |
-            | ab    |
-
-        When I request locate I should get
-            | in | out |
-            | 0  | a   |
-            | 1  | a   |
-            | 2  | a   |
-            | 3  | a   |
-            | 4  | a   |
-            | 5  | b   |
-            | 6  | b   |
-            | 7  | b   |
-            | 8  | b   |
-            | 9  | b   |
-
-    Scenario: Nearest - north-south way
-        Given the node map
-            | 1 | 2 | 3 |
-            | 0 | a | 4 |
-            |   | x |   |
-            | 9 | b | 5 |
-            | 8 | 7 | 6 |
-
-        And the ways
-            | nodes |
-            | ab    |
-
-        When I request locate I should get
-            | in | out |
-            | 0  | a   |
-            | 1  | a   |
-            | 2  | a   |
-            | 3  | a   |
-            | 4  | a   |
-            | 5  | b   |
-            | 6  | b   |
-            | 7  | b   |
-            | 8  | b   |
-            | 9  | b   |
-
-    Scenario: Nearest - diagonal 1
-        Given the node map
-            | 2 |   | 3 |   |   |   |
-            |   | a |   | 4 |   |   |
-            | 1 |   | x |   | 5 |   |
-            |   | 0 |   | y |   | 6 |
-            |   |   | 9 |   | b |   |
-            |   |   |   | 8 |   | 7 |
-
-        And the ways
-            | nodes |
-            | axyb  |
-
-        When I request locate I should get
-            | in | out |
-            | 0  | x   |
-            | 1  | a   |
-            | 2  | a   |
-            | 3  | a   |
-            | 4  | x   |
-            | 5  | y   |
-            | 6  | b   |
-            | 7  | b   |
-            | 8  | b   |
-            | 9  | y   |
-            | a  | a   |
-            | b  | b   |
-            | x  | x   |
-            | y  | y   |
-
-    Scenario: Nearest - diagonal 2
-        Given the node map
-            |   |   |   | 6 |   | 7 |
-            |   |   | 5 |   | b |   |
-            |   | 4 |   | y |   | 8 |
-            | 3 |   | x |   | 9 |   |
-            |   | a |   | 0 |   |   |
-            | 2 |   | 1 |   |   |   |
-
-        And the ways
-        | nodes |
-        | ab    |
-
-        When I request nearest I should get
-            | in | out |
-            | 0  | x   |
-            | 1  | a   |
-            | 2  | a   |
-            | 3  | a   |
-            | 4  | x   |
-            | 5  | y   |
-            | 6  | b   |
-            | 7  | b   |
-            | 8  | b   |
-            | 9  | y   |
-            | a  | a   |
-            | b  | b   |
-            | x  | x   |
-            | y  | y   |
-
-        Scenario: Locate - High lat/lon
-           Given the node locations
-            | node | lat | lon  |
-            | a    | -85 | -180 |
-            | b    | 0   | 0    |
-            | c    | 85  | 180  |
-            | x    | -84 | -180 |
-            | y    | 84  | 180  |
-
-           And the ways
-            | nodes |
-            | abc   |
-
-           When I request locate I should get
-            | in | out |
-            | x  | a   |
-            | y  | c   |
diff --git a/features/options/extract/help.feature b/features/options/extract/help.feature
index 3215e6a..722c4dc 100644
--- a/features/options/extract/help.feature
+++ b/features/options/extract/help.feature
@@ -15,7 +15,9 @@ Feature: osrm-extract command line options: help
         And stdout should contain "Configuration:"
         And stdout should contain "--profile"
         And stdout should contain "--threads"
-        And stdout should contain 12 lines
+        And stdout should contain "--generate-edge-lookup"
+        And stdout should contain "--small-component-size"
+        And stdout should contain 20 lines
         And it should exit with code 0
 
     Scenario: osrm-extract - Help, short
@@ -29,7 +31,9 @@ Feature: osrm-extract command line options: help
         And stdout should contain "Configuration:"
         And stdout should contain "--profile"
         And stdout should contain "--threads"
-        And stdout should contain 12 lines
+        And stdout should contain "--generate-edge-lookup"
+        And stdout should contain "--small-component-size"
+        And stdout should contain 20 lines
         And it should exit with code 0
 
     Scenario: osrm-extract - Help, long
@@ -43,5 +47,7 @@ Feature: osrm-extract command line options: help
         And stdout should contain "Configuration:"
         And stdout should contain "--profile"
         And stdout should contain "--threads"
-        And stdout should contain 12 lines
+        And stdout should contain "--generate-edge-lookup"
+        And stdout should contain "--small-component-size"
+        And stdout should contain 20 lines
         And it should exit with code 0
diff --git a/features/options/prepare/help.feature b/features/options/prepare/help.feature
index 59265e9..34ec3d5 100644
--- a/features/options/prepare/help.feature
+++ b/features/options/prepare/help.feature
@@ -13,11 +13,12 @@ Feature: osrm-prepare command line options: help
         And stdout should contain "--help"
         And stdout should contain "--config"
         And stdout should contain "Configuration:"
-        And stdout should contain "--restrictions"
         And stdout should contain "--profile"
         And stdout should contain "--threads"
         And stdout should contain "--core"
-        And stdout should contain 17 lines
+        And stdout should contain "--level-cache"
+        And stdout should contain "--segment-speed-file"
+        And stdout should contain 21 lines
         And it should exit with code 1
 
     Scenario: osrm-prepare - Help, short
@@ -29,11 +30,12 @@ Feature: osrm-prepare command line options: help
         And stdout should contain "--help"
         And stdout should contain "--config"
         And stdout should contain "Configuration:"
-        And stdout should contain "--restrictions"
         And stdout should contain "--profile"
         And stdout should contain "--threads"
         And stdout should contain "--core"
-        And stdout should contain 17 lines
+        And stdout should contain "--level-cache"
+        And stdout should contain "--segment-speed-file"
+        And stdout should contain 21 lines
         And it should exit with code 0
 
     Scenario: osrm-prepare - Help, long
@@ -45,9 +47,10 @@ Feature: osrm-prepare command line options: help
         And stdout should contain "--help"
         And stdout should contain "--config"
         And stdout should contain "Configuration:"
-        And stdout should contain "--restrictions"
         And stdout should contain "--profile"
         And stdout should contain "--threads"
         And stdout should contain "--core"
-        And stdout should contain 17 lines
+        And stdout should contain "--level-cache"
+        And stdout should contain "--segment-speed-file"
+        And stdout should contain 21 lines
         And it should exit with code 0
diff --git a/features/options/routed/help.feature b/features/options/routed/help.feature
index c0fca47..34c6ee2 100644
--- a/features/options/routed/help.feature
+++ b/features/options/routed/help.feature
@@ -25,9 +25,11 @@ Feature: osrm-routed command line options: help
         And stdout should contain "--port"
         And stdout should contain "--threads"
         And stdout should contain "--shared-memory"
+        And stdout should contain "--max-viaroute-size"
+        And stdout should contain "--max-trip-size"
         And stdout should contain "--max-table-size"
         And stdout should contain "--max-matching-size"
-        And stdout should contain 26 lines
+        And stdout should contain 30 lines
         And it should exit with code 0
 
     Scenario: osrm-routed - Help, short
@@ -51,9 +53,11 @@ Feature: osrm-routed command line options: help
         And stdout should contain "--port"
         And stdout should contain "--threads"
         And stdout should contain "--shared-memory"
+        And stdout should contain "--max-viaroute-size"
+        And stdout should contain "--max-trip-size"
         And stdout should contain "--max-table-size"
         And stdout should contain "--max-matching-size"
-        And stdout should contain 26 lines
+        And stdout should contain 30 lines
         And it should exit with code 0
 
     Scenario: osrm-routed - Help, long
@@ -77,7 +81,9 @@ Feature: osrm-routed command line options: help
         And stdout should contain "--port"
         And stdout should contain "--threads"
         And stdout should contain "--shared-memory"
+        And stdout should contain "--max-trip-size"
+        And stdout should contain "--max-table-size"
         And stdout should contain "--max-table-size"
         And stdout should contain "--max-matching-size"
-        And stdout should contain 26 lines
+        And stdout should contain 30 lines
         And it should exit with code 0
diff --git a/features/step_definitions/data.rb b/features/step_definitions/data.rb
index 0f33843..3e4ccc9 100644
--- a/features/step_definitions/data.rb
+++ b/features/step_definitions/data.rb
@@ -6,6 +6,10 @@ Given(/^the import format "(.*?)"$/) do |format|
   set_input_format format
 end
 
+Given /^the extract extra arguments "(.*?)"$/ do |args|
+    set_extract_args args
+end
+
 Given /^a grid size of (\d+) meters$/ do |meters|
   set_grid_size meters
 end
@@ -28,7 +32,7 @@ Given /^the node map$/ do |table|
         raise "*** invalid node name '#{name}', must me alphanumeric" unless name.match /[a-z0-9]/
         if name.match /[a-z]/
           raise "*** duplicate node '#{name}'" if name_node_hash[name]
-          add_osm_node name, *table_coord_to_lonlat(ci,ri)
+          add_osm_node name, *table_coord_to_lonlat(ci,ri), nil
         else
           raise "*** duplicate node '#{name}'" if location_hash[name]
           add_location name, *table_coord_to_lonlat(ci,ri)
@@ -43,7 +47,9 @@ Given /^the node locations$/ do |table|
     name = row['node']
     raise "*** duplicate node '#{name}'" if find_node_by_name name
     if name.match /[a-z]/
-      add_osm_node name, row['lon'].to_f, row['lat'].to_f
+      id = row['id']
+      id = id.to_i if id
+      add_osm_node name, row['lon'].to_f, row['lat'].to_f, id
     else
       add_location name, row['lon'].to_f, row['lat'].to_f
     end
diff --git a/features/step_definitions/distance_matrix.rb b/features/step_definitions/distance_matrix.rb
index 8c4e927..2143d37 100644
--- a/features/step_definitions/distance_matrix.rb
+++ b/features/step_definitions/distance_matrix.rb
@@ -1,19 +1,29 @@
 When /^I request a travel time matrix I should get$/ do |table|
-  
   no_route = 2147483647   # MAX_INT
   
   raise "*** Top-left cell of matrix table must be empty" unless table.headers[0]==""
   
-  nodes = []
+  waypoints = []
   column_headers = table.headers[1..-1]
   row_headers = table.rows.map { |h| h.first }
-  unless column_headers==row_headers
-    raise "*** Column and row headers must match in matrix table, got #{column_headers.inspect} and #{row_headers.inspect}"
-  end
-  column_headers.each do |node_name|
-    node = find_node_by_name(node_name)
-    raise "*** unknown node '#{node_name}" unless node
-    nodes << node
+  symmetric = Set.new(column_headers) == Set.new(row_headers)
+  if symmetric then
+    column_headers.each do |node_name|
+      node = find_node_by_name(node_name)
+      raise "*** unknown node '#{node_name}" unless node
+      waypoints << {:coord => node, :type => "loc"}
+    end
+  else
+    column_headers.each do |node_name|
+      node = find_node_by_name(node_name)
+      raise "*** unknown node '#{node_name}" unless node
+      waypoints << {:coord => node, :type => "dst"}
+    end
+    row_headers.each do |node_name|
+      node = find_node_by_name(node_name)
+      raise "*** unknown node '#{node_name}" unless node
+      waypoints << {:coord => node, :type => "src"}
+    end
   end
   
   reprocess
@@ -23,18 +33,18 @@ When /^I request a travel time matrix I should get$/ do |table|
     
     # compute matrix
     params = @query_params
-    response = request_table nodes, params
+    response = request_table waypoints, params
     if response.body.empty? == false
-      json = JSON.parse response.body
-      result = json['distance_table']
+      json_result = JSON.parse response.body
+      result = json_result["distance_table"]
     end
+
     
     # compare actual and expected result, one row at a time
     table.rows.each_with_index do |row,ri|
-      
       # fuzzy match
       ok = true
-      0.upto(nodes.size-1) do |i|
+      0.upto(result[ri].size-1) do |i|
         if FuzzyMatch.match result[ri][i], row[i+1]
           result[ri][i] = row[i+1]
         elsif row[i+1]=="" and result[ri][i]==no_route
diff --git a/features/step_definitions/locate.rb b/features/step_definitions/locate.rb
deleted file mode 100644
index 2fd8e62..0000000
--- a/features/step_definitions/locate.rb
+++ /dev/null
@@ -1,51 +0,0 @@
-When /^I request locate I should get$/ do |table|
-  reprocess
-  actual = []
-  OSRMLoader.load(self,"#{prepared_file}.osrm") do
-    table.hashes.each_with_index do |row,ri|
-      in_node = find_node_by_name row['in']
-      raise "*** unknown in-node '#{row['in']}" unless in_node
-
-      out_node = find_node_by_name row['out']
-      raise "*** unknown out-node '#{row['out']}" unless out_node
-
-      response = request_locate(in_node)
-      if response.code == "200" && response.body.empty? == false
-        json = JSON.parse response.body
-        if json['status'] == 0
-          coord =  json['mapped_coordinate']
-        end
-      end
-
-      got = {'in' => row['in'], 'out' => coord }
-
-      ok = true
-      row.keys.each do |key|
-        if key=='out'
-          if FuzzyMatch.match_location coord, out_node
-            got[key] = row[key]
-          else
-            row[key] = "#{row[key]} [#{out_node.lat},#{out_node.lon}]"
-            ok = false
-          end
-        end
-      end
-
-      unless ok
-        failed = { :attempt => 'locate', :query => @query, :response => response }
-        log_fail row,got,[failed]
-      end
-
-      actual << got
-    end
-  end
-  table.diff! actual
-end
-
-When /^I request locate (\d+) times I should get$/ do |n,table|
-  ok = true
-  n.to_i.times do
-    ok = false unless step "I request locate I should get", table
-  end
-  ok
-end
diff --git a/features/step_definitions/matching.rb b/features/step_definitions/matching.rb
index 3015f5a..c490b46 100644
--- a/features/step_definitions/matching.rb
+++ b/features/step_definitions/matching.rb
@@ -8,6 +8,17 @@ When /^I match I should get$/ do |table|
         response = request_url row['request']
       else
         params = @query_params
+        got = {}
+        row.each_pair do |k,v|
+          if k =~ /param:(.*)/
+            if v=='(nil)'
+              params[$1]=nil
+            elsif v!=nil
+              params[$1]=[v]
+            end
+            got[k]=v
+          end
+        end
         trace = []
         timestamps = []
         if row['trace']
@@ -19,24 +30,13 @@ When /^I match I should get$/ do |table|
           if row['timestamps']
               timestamps = row['timestamps'].split(" ").compact.map { |t| t.to_i}
           end
-          got = {'trace' => row['trace'] }
+          got = got.merge({'trace' => row['trace'] })
           response = request_matching trace, timestamps, params
         else
           raise "*** no trace"
         end
       end
 
-      row.each_pair do |k,v|
-        if k =~ /param:(.*)/
-          if v=='(nil)'
-            params[$1]=nil
-          elsif v!=nil
-            params[$1]=v
-          end
-          got[k]=v
-        end
-      end
-
       if response.body.empty? == false
         json = JSON.parse response.body
       end
@@ -52,178 +52,35 @@ When /^I match I should get$/ do |table|
       end
 
       sub_matchings = []
+      turns = ''
+      route = ''
+      duration = ''
       if response.code == "200"
         if table.headers.include? 'matchings'
           sub_matchings = json['matchings'].compact.map { |sub| sub['matched_points']}
         end
-      end
-
-      ok = true
-      encoded_result = ""
-      extended_target = ""
-      row['matchings'].split(',').each_with_index do |sub, sub_idx|
-        if sub_idx >= sub_matchings.length
-          ok = false
-          break
+        if table.headers.include? 'turns'
+          raise "*** Checking turns only support for matchings with one subtrace" unless json['matchings'].size == 1
+          turns = turn_list json['matchings'][0]['instructions']
         end
-        sub.length.times do |node_idx|
-          node = find_node_by_name(sub[node_idx])
-          out_node = sub_matchings[sub_idx][node_idx]
-          if FuzzyMatch.match_location out_node, node
-            encoded_result += sub[node_idx]
-            extended_target += sub[node_idx]
-          else
-            encoded_result += "? [#{out_node[0]},#{out_node[1]}]"
-            extended_target += "#{sub[node_idx]} [#{node.lat},#{node.lon}]"
-            ok = false
-          end
+        if table.headers.include? 'route'
+          raise "*** Checking route only support for matchings with one subtrace" unless json['matchings'].size == 1
+          route = way_list json['matchings'][0]['instructions']
+        if table.headers.include? 'duration'
+          raise "*** Checking duration only support for matchings with one subtrace" unless json['matchings'].size == 1
+          duration = json['matchings'][0]['route_summary']['total_time']
         end
-      end
-      if ok
-        got['matchings'] = row['matchings']
-        got['timestamps'] = row['timestamps']
-      else
-        got['matchings'] = encoded_result
-        row['matchings'] = extended_target
-        log_fail row,got, { 'matching' => {:query => @query, :response => response} }
-      end
-
-      actual << got
-    end
-  end
-  table.diff! actual
-end
-
-When /^I match with turns I should get$/ do |table|
-  reprocess
-  actual = []
-  OSRMLoader.load(self,"#{prepared_file}.osrm") do
-    table.hashes.each_with_index do |row,ri|
-      if row['request']
-        got = {'request' => row['request'] }
-        response = request_url row['request']
-      else
-        params = @query_params
-        trace = []
-        timestamps = []
-        if row['from'] and row['to']
-          node = find_node_by_name(row['from'])
-          raise "*** unknown from-node '#{row['from']}" unless node
-          trace << node
-
-          node = find_node_by_name(row['to'])
-          raise "*** unknown to-node '#{row['to']}" unless node
-          trace << node
-
-          got = {'from' => row['from'], 'to' => row['to'] }
-          response = request_matching trace, timestamps, params
-        elsif row['trace']
-          row['trace'].each_char do |n|
-            node = find_node_by_name(n.strip)
-            raise "*** unknown waypoint node '#{n.strip}" unless node
-            trace << node
-          end
-          if row['timestamps']
-              timestamps = row['timestamps'].split(" ").compact.map { |t| t.to_i}
-          end
-          got = {'trace' => row['trace'] }
-          response = request_matching trace, timestamps, params
-        else
-          raise "*** no trace"
         end
       end
 
-      row.each_pair do |k,v|
-        if k =~ /param:(.*)/
-          if v=='(nil)'
-            params[$1]=nil
-          elsif v!=nil
-            params[$1]=v
-          end
-          got[k]=v
-        end
-      end
-
-      if response.body.empty? == false
-        json = JSON.parse response.body
-      end
-      if response.body.empty? == false
-        if response.code == "200"
-          instructions = way_list json['matchings'][0]['instructions']
-          bearings = bearing_list json['matchings'][0]['instructions']
-          compasses = compass_list json['matchings'][0]['instructions']
-          turns = turn_list json['matchings'][0]['instructions']
-          modes = mode_list json['matchings'][0]['instructions']
-          times = time_list json['matchings'][0]['instructions']
-          distances = distance_list json['matchings'][0]['instructions']
-        end
-      end
-      
-      if table.headers.include? 'status'
-        got['status'] = json['status'].to_s
-      end
-      if table.headers.include? 'message'
-        got['message'] = json['status_message']
+      if table.headers.include? 'turns'
+        got['turns'] = turns
       end
-      if table.headers.include? '#'   # comment column
-        got['#'] = row['#']           # copy value so it always match
+      if table.headers.include? 'route'
+        got['route'] = route
       end
-
-      sub_matchings = []
-      if response.code == "200"
-        if table.headers.include? 'matchings'
-          sub_matchings = json['matchings'].compact.map { |sub| sub['matched_points']}
-
-          got['route'] = (instructions || '').strip
-          if table.headers.include?('distance')
-            if row['distance']!=''
-              raise "*** Distance must be specied in meters. (ex: 250m)" unless row['distance'] =~ /\d+m/
-            end
-            got['distance'] = instructions ? "#{json['route_summary']['total_distance'].to_s}m" : ''
-          end
-          if table.headers.include?('time')
-            raise "*** Time must be specied in seconds. (ex: 60s)" unless row['time'] =~ /\d+s/
-            got['time'] = instructions ? "#{json['route_summary']['total_time'].to_s}s" : ''
-          end
-          if table.headers.include?('speed')
-            if row['speed'] != '' && instructions
-              raise "*** Speed must be specied in km/h. (ex: 50 km/h)" unless row['speed'] =~ /\d+ km\/h/
-                time = json['route_summary']['total_time']
-                distance = json['route_summary']['total_distance']
-                speed = time>0 ? (3.6*distance/time).to_i : nil
-                got['speed'] =  "#{speed} km/h"
-            else
-              got['speed'] = ''
-            end
-          end
-          if table.headers.include? 'bearing'
-            got['bearing'] = instructions ? bearings : ''
-          end
-          if table.headers.include? 'compass'
-            got['compass'] = instructions ? compasses : ''
-          end
-          if table.headers.include? 'turns'
-            got['turns'] = instructions ? turns : ''
-          end
-          if table.headers.include? 'modes'
-            got['modes'] = instructions ? modes : ''
-          end
-          if table.headers.include? 'times'
-            got['times'] = instructions ? times : ''
-          end
-          if table.headers.include? 'distances'
-            got['distances'] = instructions ? distances : ''
-          end
-        end
-        if table.headers.include? 'start'
-          got['start'] = instructions ? json['route_summary']['start_point'] : nil
-        end
-        if table.headers.include? 'end'
-          got['end'] = instructions ? json['route_summary']['end_point'] : nil
-        end
-        if table.headers.include? 'geometry'
-            got['geometry'] = json['route_geometry']
-        end
+      if table.headers.include? 'duration'
+        got['duration'] = duration.to_s
       end
 
       ok = true
@@ -248,8 +105,12 @@ When /^I match with turns I should get$/ do |table|
         end
       end
       if ok
-        got['matchings'] = row['matchings']
-        got['timestamps'] = row['timestamps']
+        if table.headers.include? 'matchings'
+          got['matchings'] = row['matchings']
+        end
+        if table.headers.include? 'timestamps'
+          got['timestamps'] = row['timestamps']
+        end
       else
         got['matchings'] = encoded_result
         row['matchings'] = extended_target
diff --git a/features/step_definitions/nearest.rb b/features/step_definitions/nearest.rb
index 481c741..b0b5f94 100644
--- a/features/step_definitions/nearest.rb
+++ b/features/step_definitions/nearest.rb
@@ -9,10 +9,10 @@ When /^I request nearest I should get$/ do |table|
       out_node = find_node_by_name row['out']
       raise "*** unknown out-node '#{row['out']}" unless out_node
 
-      response = request_nearest(in_node)
+      response = request_nearest in_node, @query_params
       if response.code == "200" && response.body.empty? == false
         json = JSON.parse response.body
-        if json['status'] == 0
+        if json['status'] == 200
           coord =  json['mapped_coordinate']
         end
       end
diff --git a/features/step_definitions/options.rb b/features/step_definitions/options.rb
index e2e5066..62329d5 100644
--- a/features/step_definitions/options.rb
+++ b/features/step_definitions/options.rb
@@ -53,5 +53,5 @@ Then /^stdout should contain (\d+) lines?$/ do |lines|
 end
 
 Given (/^the query options$/) do |table|
-  @query_params = table.rows_hash
+  table.rows_hash.each { |k,v| @query_params << [k, v] }
 end
diff --git a/features/step_definitions/requests.rb b/features/step_definitions/requests.rb
index 41257e5..1c012d7 100644
--- a/features/step_definitions/requests.rb
+++ b/features/step_definitions/requests.rb
@@ -1,7 +1,7 @@
 When /^I request \/(.*)$/ do |path|
   reprocess
   OSRMLoader.load(self,"#{prepared_file}.osrm") do
-    @response = request_path path
+    @response = request_path path, []
   end
 end
 
diff --git a/features/step_definitions/routability.rb b/features/step_definitions/routability.rb
index a9241be..6b5134c 100644
--- a/features/step_definitions/routability.rb
+++ b/features/step_definitions/routability.rb
@@ -4,12 +4,12 @@ def test_routability_row i
     a = Location.new @origin[0]+(1+WAY_SPACING*i)*@zoom, @origin[1]
     b = Location.new @origin[0]+(3+WAY_SPACING*i)*@zoom, @origin[1]
     r = {}
-    r[:response] = request_route (direction=='forw' ? [a,b] : [b,a]), @query_params
+    r[:response] = request_route (direction=='forw' ? [a,b] : [b,a]), [], @query_params
     r[:query] = @query
     r[:json] = JSON.parse(r[:response].body)
 
-    r[:status] = route_status r[:response]
-    if r[:status].empty? == false
+    r[:status] = (route_status r[:response]) == 200 ? 'x' : nil
+    if r[:status] then
       r[:route] = way_list r[:json]['route_instructions']
 
       if r[:route]=="w#{i}"
diff --git a/features/step_definitions/routing.rb b/features/step_definitions/routing.rb
index 21fcf48..f1e9590 100644
--- a/features/step_definitions/routing.rb
+++ b/features/step_definitions/routing.rb
@@ -7,8 +7,26 @@ When /^I route I should get$/ do |table|
         got = {'request' => row['request'] }
         response = request_url row['request']
       else
-        params = @query_params
+        default_params = @query_params
+        user_params = []
+        got = {}
+        row.each_pair do |k,v|
+          if k =~ /param:(.*)/
+            if v=='(nil)'
+              user_params << [$1, nil]
+            elsif v!=nil
+              user_params << [$1, v]
+            end
+            got[k]=v
+          end
+        end
+        params = overwrite_params default_params, user_params
         waypoints = []
+        bearings = []
+        if row['bearings']
+          got['bearings'] = row['bearings']
+          bearings = row['bearings'].split(' ').compact
+        end
         if row['from'] and row['to']
           node = find_node_by_name(row['from'])
           raise "*** unknown from-node '#{row['from']}" unless node
@@ -18,38 +36,27 @@ When /^I route I should get$/ do |table|
           raise "*** unknown to-node '#{row['to']}" unless node
           waypoints << node
 
-          got = {'from' => row['from'], 'to' => row['to'] }
-          response = request_route waypoints, params
+          got = got.merge({'from' => row['from'], 'to' => row['to'] })
+          response = request_route waypoints, bearings, params
         elsif row['waypoints']
           row['waypoints'].split(',').each do |n|
             node = find_node_by_name(n.strip)
             raise "*** unknown waypoint node '#{n.strip}" unless node
             waypoints << node
           end
-          got = {'waypoints' => row['waypoints'] }
-          response = request_route waypoints, params
+          got = got.merge({'waypoints' => row['waypoints'] })
+          response = request_route waypoints, bearings, params
         else
           raise "*** no waypoints"
         end
       end
 
-      row.each_pair do |k,v|
-        if k =~ /param:(.*)/
-          if v=='(nil)'
-            params[$1]=nil
-          elsif v!=nil
-            params[$1]=v
-          end
-          got[k]=v
-        end
-      end
-
       if response.body.empty? == false
         json = JSON.parse response.body
       end
 
       if response.body.empty? == false
-        if json['status'] == 0
+        if json['status'] == 200
           instructions = way_list json['route_instructions']
           bearings = bearing_list json['route_instructions']
           compasses = compass_list json['route_instructions']
@@ -70,62 +77,64 @@ When /^I route I should get$/ do |table|
         got['#'] = row['#']           # copy value so it always match
       end
 
-      if response.code == "200"
-        if table.headers.include? 'start'
-          got['start'] = instructions ? json['route_summary']['start_point'] : nil
-        end
-        if table.headers.include? 'end'
-          got['end'] = instructions ? json['route_summary']['end_point'] : nil
-        end
-        if table.headers.include? 'geometry'
-            got['geometry'] = json['route_geometry']
-        end
-        if table.headers.include? 'route'
-          got['route'] = (instructions || '').strip
-          if table.headers.include?('alternative')
-            raise "*** No alternative found ***" unless json['found_alternative']
-            got['alternative'] = way_list json['alternative_instructions'].first
-          end
-          if table.headers.include?('distance')
-            if row['distance']!=''
-              raise "*** Distance must be specied in meters. (ex: 250m)" unless row['distance'] =~ /\d+m/
-            end
-            got['distance'] = instructions ? "#{json['route_summary']['total_distance'].to_s}m" : ''
-          end
-          if table.headers.include?('time')
-            raise "*** Time must be specied in seconds. (ex: 60s)" unless row['time'] =~ /\d+s/
-            got['time'] = instructions ? "#{json['route_summary']['total_time'].to_s}s" : ''
-          end
-          if table.headers.include?('speed')
-            if row['speed'] != '' && instructions
-              raise "*** Speed must be specied in km/h. (ex: 50 km/h)" unless row['speed'] =~ /\d+ km\/h/
-                time = json['route_summary']['total_time']
-                distance = json['route_summary']['total_distance']
-                speed = time>0 ? (3.6*distance/time).round : nil
-                got['speed'] =  "#{speed} km/h"
+      if table.headers.include? 'start'
+        got['start'] = instructions ? json['route_summary']['start_point'] : nil
+      end
+      if table.headers.include? 'end'
+        got['end'] = instructions ? json['route_summary']['end_point'] : nil
+      end
+      if table.headers.include? 'geometry'
+          got['geometry'] = json['route_geometry']
+      end
+      if table.headers.include? 'route'
+        got['route'] = (instructions || '').strip
+        if table.headers.include?('alternative')
+          got['alternative'] =
+            if json['found_alternative']
+              way_list json['alternative_instructions'].first
             else
-              got['speed'] = ''
+              ""
             end
+        end
+        if table.headers.include?('distance')
+          if row['distance']!=''
+            raise "*** Distance must be specied in meters. (ex: 250m)" unless row['distance'] =~ /\d+m/
           end
-          if table.headers.include? 'bearing'
-            got['bearing'] = instructions ? bearings : ''
-          end
-          if table.headers.include? 'compass'
-            got['compass'] = instructions ? compasses : ''
-          end
-          if table.headers.include? 'turns'
-            got['turns'] = instructions ? turns : ''
-          end
-          if table.headers.include? 'modes'
-            got['modes'] = instructions ? modes : ''
-          end
-          if table.headers.include? 'times'
-            got['times'] = instructions ? times : ''
-          end
-          if table.headers.include? 'distances'
-            got['distances'] = instructions ? distances : ''
+          got['distance'] = instructions ? "#{json['route_summary']['total_distance'].to_s}m" : ''
+        end
+        if table.headers.include?('time')
+          raise "*** Time must be specied in seconds. (ex: 60s)" unless row['time'] =~ /\d+s/
+          got['time'] = instructions ? "#{json['route_summary']['total_time'].to_s}s" : ''
+        end
+        if table.headers.include?('speed')
+          if row['speed'] != '' && instructions
+            raise "*** Speed must be specied in km/h. (ex: 50 km/h)" unless row['speed'] =~ /\d+ km\/h/
+              time = json['route_summary']['total_time']
+              distance = json['route_summary']['total_distance']
+              speed = time>0 ? (3.6*distance/time).round : nil
+              got['speed'] =  "#{speed} km/h"
+          else
+            got['speed'] = ''
           end
         end
+        if table.headers.include? 'bearing'
+          got['bearing'] = instructions ? bearings : ''
+        end
+        if table.headers.include? 'compass'
+          got['compass'] = instructions ? compasses : ''
+        end
+        if table.headers.include? 'turns'
+          got['turns'] = instructions ? turns : ''
+        end
+        if table.headers.include? 'modes'
+          got['modes'] = instructions ? modes : ''
+        end
+        if table.headers.include? 'times'
+          got['times'] = instructions ? times : ''
+        end
+        if table.headers.include? 'distances'
+          got['distances'] = instructions ? distances : ''
+        end
       end
 
       ok = true
diff --git a/features/step_definitions/trip.rb b/features/step_definitions/trip.rb
index 007717e..ecb4c77 100644
--- a/features/step_definitions/trip.rb
+++ b/features/step_definitions/trip.rb
@@ -38,7 +38,7 @@ When /^I plan a trip I should get$/ do |table|
           if v=='(nil)'
             params[$1]=nil
           elsif v!=nil
-            params[$1]=v
+            params[$1]=[v]
           end
           got[k]=v
         end
diff --git a/features/support/config.rb b/features/support/config.rb
index e61c9fc..434b4ec 100644
--- a/features/support/config.rb
+++ b/features/support/config.rb
@@ -10,3 +10,7 @@ end
 def set_profile profile
   @profile = profile
 end
+
+def set_extract_args args
+    @extract_args = args
+end
diff --git a/features/support/data.rb b/features/support/data.rb
index 0c70bb6..62ed7d3 100644
--- a/features/support/data.rb
+++ b/features/support/data.rb
@@ -123,8 +123,9 @@ def table_coord_to_lonlat ci,ri
   [@origin[0]+ci*@zoom, @origin[1]-ri*@zoom]
 end
 
-def add_osm_node name,lon,lat
-  node = OSM::Node.new make_osm_id, OSM_USER, OSM_TIMESTAMP, lon, lat
+def add_osm_node name,lon,lat,id
+  id = make_osm_id if id == nil
+  node = OSM::Node.new id, OSM_USER, OSM_TIMESTAMP, lon, lat
   node << { :name => name }
   node.uid = OSM_UID
   osm_db << node
@@ -273,12 +274,13 @@ def extract_data
   Dir.chdir TEST_FOLDER do
     log_preprocess_info
     log "== Extracting #{osm_file}.osm...", :preprocess
-    unless system "#{BIN_PATH}/osrm-extract #{osm_file}.osm#{'.pbf' if pbf?} --profile #{PROFILES_PATH}/#{@profile}.lua >>#{PREPROCESS_LOG_FILE} 2>&1"
+    unless system "#{BIN_PATH}/osrm-extract #{osm_file}.osm#{'.pbf' if pbf?} #{@extract_args} --profile #{PROFILES_PATH}/#{@profile}.lua >>#{PREPROCESS_LOG_FILE} 2>&1"
       log "*** Exited with code #{$?.exitstatus}.", :preprocess
       raise ExtractError.new $?.exitstatus, "osrm-extract exited with code #{$?.exitstatus}."
     end
     begin
-      ["osrm","osrm.names","osrm.restrictions"].each do |file|
+      ["osrm","osrm.names","osrm.restrictions","osrm.ebg","osrm.edges","osrm.fileIndex","osrm.geometry","osrm.nodes","osrm.ramIndex"].each do |file|
+        log "Renaming #{osm_file}.#{file} to #{extracted_file}.#{file}", :preprocess
         File.rename "#{osm_file}.#{file}", "#{extracted_file}.#{file}"
       end
     rescue Exception => e
@@ -296,14 +298,16 @@ def prepare_data
       raise PrepareError.new $?.exitstatus, "osrm-prepare exited with code #{$?.exitstatus}."
     end
     begin
-      ["osrm.hsgr","osrm.fileIndex","osrm.geometry","osrm.nodes","osrm.ramIndex","osrm.core"].each do |file|
+      ["osrm.hsgr","osrm.fileIndex","osrm.geometry","osrm.nodes","osrm.ramIndex","osrm.core","osrm.edges"].each do |file|
+        log "Renaming #{extracted_file}.#{file} to #{prepared_file}.#{file}", :preprocess
         File.rename "#{extracted_file}.#{file}", "#{prepared_file}.#{file}"
       end
     rescue Exception => e
       raise FileError.new nil, "failed to rename data file after preparing."
     end
     begin
-      ["osrm.names","osrm.edges","osrm.restrictions"].each do |file|
+      ["osrm.names","osrm.restrictions","osrm"].each do |file|
+        log "Copying #{extracted_file}.#{file} to #{prepared_file}.#{file}", :preprocess
         FileUtils.cp "#{extracted_file}.#{file}", "#{prepared_file}.#{file}"
       end
     rescue Exception => e
diff --git a/features/support/hooks.rb b/features/support/hooks.rb
index 2ad49dc..6af9a8f 100644
--- a/features/support/hooks.rb
+++ b/features/support/hooks.rb
@@ -15,7 +15,7 @@ Before do |scenario|
   end
   
   @load_method  = DEFAULT_LOAD_METHOD
-  @query_params = {}
+  @query_params = []
   @scenario_time = Time.now.strftime("%Y-%m-%dT%H:%m:%SZ")
   reset_data
   @has_logged_preprocess_info = false
diff --git a/features/support/http.rb b/features/support/http.rb
index 80dad8a..0b9fb9a 100644
--- a/features/support/http.rb
+++ b/features/support/http.rb
@@ -1,29 +1,32 @@
 require 'net/http'
 
-def generate_request_url path
-  if @http_method.eql? "POST"
-    pos = path.index('?') - 1
-    service = path[0..pos]
-    uri = URI.parse "#{HOST}/#{service}"
-  else
-    uri = URI.parse "#{HOST}/#{path}"
+# Converts an array [["param","val1"], ["param","val2"]] into ?param=val1&param=val2
+def params_to_url params
+  kv_pairs = params.map { |kv| kv[0].to_s + "=" + kv[1].to_s }
+  url = kv_pairs.size > 0 ? ("?" + kv_pairs.join("&")) : ""
+  return url
+end
+
+# Converts an array [["param","val1"], ["param","val2"]] into ["param"=>["val1", "val2"]]
+def params_to_map params
+  result = {}
+  params.each do |pair|
+    if not result.has_key? pair[0]
+      result[pair[0]] = []
+    end
+    result[pair[0]] << [pair[1]]
   end
 end
 
-def send_request uri, waypoints=[], options={}, timestamps=[]
-  @query = uri.to_s
+def send_request base_uri, parameters
   Timeout.timeout(OSRM_TIMEOUT) do
     if @http_method.eql? "POST"
-      datas = {}
-      if waypoints.length > 0
-        datas[:loc] = waypoints.compact.map { |w| "#{w.lat},#{w.lon}" }
-      end
-      if timestamps.length > 0
-        datas[:t] = timestamps.compact.map { |t| "#{t}" }
-      end
-      datas.merge! options
-      response = Net::HTTP.post_form uri, datas
+      uri = URI.parse base_uri
+      @query = uri.to_s
+      response = Net::HTTP.post_form uri, (params_to_map parameters)
     else
+      uri = URI.parse base_uri+(params_to_url parameters)
+      @query = uri.to_s
       response = Net::HTTP.get_response uri
     end
   end
diff --git a/features/support/locate.rb b/features/support/locate.rb
deleted file mode 100644
index a62e53a..0000000
--- a/features/support/locate.rb
+++ /dev/null
@@ -1,12 +0,0 @@
-require 'net/http'
-
-def request_locate_url path, node
-  @query = path
-
-  uri = generate_request_url path
-  response = send_request uri, [node]
-end
-
-def request_locate node
-  request_locate_url "locate?loc=#{node.lat},#{node.lon}", node
-end
diff --git a/features/support/match.rb b/features/support/match.rb
deleted file mode 100644
index 165dfe0..0000000
--- a/features/support/match.rb
+++ /dev/null
@@ -1,20 +0,0 @@
-require 'net/http'
-
-HOST = "http://127.0.0.1:#{OSRM_PORT}"
-
-def request_matching trace=[], timestamps=[], options={}
-  defaults = { 'output' => 'json', 'instructions' => 'true' }
-  locs = trace.compact.map { |w| "loc=#{w.lat},#{w.lon}" }
-  ts = timestamps.compact.map { |t| "t=#{t}" }
-  if ts.length > 0
-    trace_params = locs.zip(ts).map { |a| a.join('&')}
-  else
-    trace_params = locs
-  end
-  params = (trace_params + defaults.merge(options).to_param).join('&')
-  params = nil if params==""
-  
-  uri = generate_request_url ("match" + '?' + params)
-  response = send_request uri, trace, options, timestamps
-end
-
diff --git a/features/support/nearest.rb b/features/support/nearest.rb
deleted file mode 100644
index af52b49..0000000
--- a/features/support/nearest.rb
+++ /dev/null
@@ -1,12 +0,0 @@
-require 'net/http'
-
-def request_nearest_url path, node
-  @query = path
-  
-  uri = generate_request_url path
-  response = send_request uri, [node]
-end
-
-def request_nearest node
-  request_nearest_url "nearest?loc=#{node.lat},#{node.lon}", node
-end
diff --git a/features/support/route.rb b/features/support/route.rb
index ca353d6..935c5be 100644
--- a/features/support/route.rb
+++ b/features/support/route.rb
@@ -3,25 +3,10 @@ require 'net/http'
 HOST = "http://127.0.0.1:#{OSRM_PORT}"
 DESTINATION_REACHED = 15      #OSRM instruction code
 
-class Hash
-  def to_param(namespace = nil)
-    collect do |key, value|
-      "#{key}=#{value}"
-    end.sort
-  end
-end
-
-def request_path path, waypoints=[], options={}
-  locs = waypoints.compact.map { |w| "loc=#{w.lat},#{w.lon}" }
-  params = (locs + options.to_param).join('&')
-  params = nil if params==""
-  
-  if params == nil
-    uri = generate_request_url (path)
-  else
-    uri = generate_request_url (path + '?' + params)
-  end
-  response = send_request uri, waypoints, options
+def request_path service, params
+  uri = "#{HOST}/" + service
+  response = send_request uri, params
+  return response
 end
 
 def request_url path
@@ -36,40 +21,95 @@ rescue Timeout::Error
   raise "*** osrm-routed did not respond."
 end
 
-def request_route waypoints, params={}
-  defaults = { 'output' => 'json', 'instructions' => true, 'alt' => false }
-  request_path "viaroute", waypoints, defaults.merge(params)
+# Overwriters the default values in defaults.
+# e.g. [[a, 1], [b, 2]], [[a, 5], [d, 10]] => [[a, 5], [b, 2], [d, 10]]
+def overwrite_params defaults, other
+  merged = []
+  defaults.each do |k,v|
+    idx = other.index { |p| p[0] == k }
+    if idx == nil then
+      merged << [k, v]
+    else
+      merged << [k, other[idx][1]]
+    end
+  end
+  other.each do |k,v|
+    if merged.index { |pair| pair[0] == k} == nil then
+      merged << [k, v]
+    end
+  end
+
+  return merged
+end
+
+def request_route waypoints, bearings, user_params
+  raise "*** number of bearings does not equal the number of waypoints" unless bearings.size == 0 || bearings.size == waypoints.size
+
+  defaults = [['output','json'], ['instructions',true], ['alt',false]]
+  params = overwrite_params defaults, user_params
+  encoded_waypoint = waypoints.map{ |w| ["loc","#{w.lat},#{w.lon}"] }
+  if bearings.size > 0
+    encoded_bearings = bearings.map { |b| ["b", b.to_s]}
+    parasm = params.concat encoded_waypoint.zip(encoded_bearings).flatten! 1
+  else
+    params = params.concat encoded_waypoint
+  end
+
+  return request_path "viaroute", params
+end
+
+def request_nearest node, user_params
+  defaults = [['output', 'json']]
+  params = overwrite_params defaults, user_params
+  params << ["loc", "#{node.lat},#{node.lon}"]
+
+  return request_path "nearest", params
+end
+
+def request_table waypoints, user_params
+  defaults = [['output', 'json']]
+  params = overwrite_params defaults, user_params
+  params = params.concat waypoints.map{ |w| [w[:type],"#{w[:coord].lat},#{w[:coord].lon}"] }
+
+  return request_path "table", params
+end
+
+def request_trip waypoints, user_params
+  defaults = [['output', 'json']]
+  params = overwrite_params defaults, user_params
+  params = params.concat waypoints.map{ |w| ["loc","#{w.lat},#{w.lon}"] }
+
+  return request_path "trip", params
 end
 
-def request_table waypoints, params={}
-  defaults = { 'output' => 'json' }
-  request_path "table", waypoints, defaults.merge(params)
+def request_matching waypoints, timestamps, user_params
+  defaults = [['output', 'json']]
+  params = overwrite_params defaults, user_params
+  encoded_waypoint = waypoints.map{ |w| ["loc","#{w.lat},#{w.lon}"] }
+  if timestamps.size > 0
+    encoded_timestamps = timestamps.map { |t| ["t", t.to_s]}
+    parasm = params.concat encoded_waypoint.zip(encoded_timestamps).flatten! 1
+  else
+    params = params.concat encoded_waypoint
+  end
+
+  return request_path "match", params
 end
 
 def got_route? response
   if response.code == "200" && !response.body.empty?
     json = JSON.parse response.body
-    if json['status'] == 0
+    if json['status'] == 200
       return way_list( json['route_instructions']).empty? == false
     end
   end
-  false
+  return false
 end
 
 def route_status response
   if response.code == "200" && !response.body.empty?
     json = JSON.parse response.body
-    if json['status'] == 0
-      if way_list( json['route_instructions']).empty?
-        return 'Empty route'
-      else
-        return 'x'
-      end
-    elsif json['status'] == 207
-      ''
-    else
-      "Status #{json['status']}"
-    end
+    return json['status']
   else
     "HTTP #{response.code}"
   end
diff --git a/features/support/trip.rb b/features/support/trip.rb
deleted file mode 100644
index 6ea9010..0000000
--- a/features/support/trip.rb
+++ /dev/null
@@ -1,14 +0,0 @@
-require 'net/http'
-HOST = "http://127.0.0.1:#{OSRM_PORT}"
-
-def request_trip waypoints=[], params={}
-  defaults = { 'output' => 'json' }
-  locs = waypoints.compact.map { |w| "loc=#{w.lat},#{w.lon}" }
-
-  params = (locs + defaults.merge(params).to_param).join('&')
-  params = nil if params==""
-
-  uri = generate_request_url ("trip" + '?' + params)
-  response = send_request uri, waypoints, params
-end
-
diff --git a/features/testbot/64bit.feature b/features/testbot/64bit.feature
new file mode 100644
index 0000000..d2d0c8a
--- /dev/null
+++ b/features/testbot/64bit.feature
@@ -0,0 +1,23 @@
+ at testbot
+Feature: Support 64bit node IDs
+
+    # Without 64bit support, this test should fail
+    Scenario: 64bit overflow conflicts
+        Given the node locations
+            | node | lat       | lon       | id         |
+            | a    | 55.660778 | 12.573909 | 1          |
+            | b    | 55.660672 | 12.573693 | 2          |
+            | c    | 55.660128 | 12.572546 | 3          |
+            | d    | 55.660015 | 12.572476 | 4294967297 |
+            | e    | 55.660119 | 12.572325 | 4294967298 |
+            | x    | 55.660818 | 12.574051 | 4294967299 |
+            | y    | 55.660073 | 12.574067 | 4294967300 |
+
+        And the ways
+            | nodes |
+            | abc   |
+            | cdec  |
+
+        When I route I should get
+            | from | to | route | turns            |
+            | x    | y  | abc   | head,destination |
diff --git a/features/testbot/alternative.feature b/features/testbot/alternative.feature
new file mode 100644
index 0000000..d6ca030
--- /dev/null
+++ b/features/testbot/alternative.feature
@@ -0,0 +1,38 @@
+ at routing @testbot @alternative
+Feature: Alternative route
+
+    Background:
+        Given the profile "testbot"
+
+        And the node map
+            |   | b | c | d |   |   |
+            | a |   |   |   |   | z |
+            |   | g | h | i | j |   |
+
+        And the ways
+            | nodes |
+            | ab    |
+            | bc    |
+            | cd    |
+            | dz    |
+            | ag    |
+            | gh    |
+            | hi    |
+            | ij    |
+            | jz    |
+
+    Scenario: Enabled alternative
+        Given the query options
+            | alt | true |
+
+        When I route I should get
+            | from | to | route       | alternative    |
+            | a    | z  | ab,bc,cd,dz | ag,gh,hi,ij,jz |
+
+    Scenario: Disabled alternative
+        Given the query options
+            | alt | false |
+
+        When I route I should get
+            | from | to | route       | alternative |
+            | a    | z  | ab,bc,cd,dz |             |
diff --git a/features/testbot/bearing_param.feature b/features/testbot/bearing_param.feature
index 810f690..1efa817 100644
--- a/features/testbot/bearing_param.feature
+++ b/features/testbot/bearing_param.feature
@@ -1,4 +1,4 @@
- at routing @bearing_param @todo @testbot
+ at routing @bearing_param @testbot
 Feature: Bearing parameter
 
     Background:
@@ -7,6 +7,22 @@ Feature: Bearing parameter
 
     Scenario: Testbot - Intial bearing in simple case
         Given the node map
+            | a | b | c | d |
+
+        And the ways
+            | nodes |
+            | ad    |
+
+        When I route I should get
+            | from | to | bearings  | route | bearing |
+            | b    | c  | 90 90     | ad    | 90      |
+            | b    | c  | 180 90    |       |         |
+            | b    | c  | 80 100    | ad    | 90      |
+            | b    | c  | 79 100    |       |         |
+            | b    | c  | 79,11 100 | ad    | 90      |
+
+    Scenario: Testbot - Intial bearing in simple case
+        Given the node map
             | a |   |
             | 0 | c |
             | b |   |
@@ -17,13 +33,13 @@ Feature: Bearing parameter
             | bc    |
 
         When I route I should get
-            | from | to | param:bearing | route | bearing |
-            | 0    | c  | 0             | bc    | 45      |
-            | 0    | c  | 45            | bc    | 45      |
-            | 0    | c  | 85            | bc    | 45      |
-            | 0    | c  | 95            | ac    | 135     |
-            | 0    | c  | 135           | ac    | 135     |
-            | 0    | c  | 180           | ac    | 135     |
+            | from | to | bearings | route | bearing |
+            | 0    | c  | 0 0      |       |         |
+            | 0    | c  | 45 45    | bc    | 45 ~3%   |
+            | 0    | c  | 85 85    |       |         |
+            | 0    | c  | 95 95    |       |         |
+            | 0    | c  | 135 135  | ac    | 135 ~1%  |
+            | 0    | c  | 180 180  |       |         |
 
     Scenario: Testbot - Initial bearing on split way
         Given the node map
@@ -38,23 +54,25 @@ Feature: Bearing parameter
             | da    | yes    |
 
         When I route I should get
-            | from | to | param:bearing | route    | bearing |
-            | 0    | b  | 10            | ab       | 90      |
-            | 0    | b  | 90            | ab       | 90      |
-            | 0    | b  | 170           | ab       | 90      |
-            | 0    | b  | 190           | cd,da,ab | 270     |
-            | 0    | b  | 270           | cd,da,ab | 270     |
-            | 0    | b  | 350           | cd,da,ab | 270     |
-            | 1    | d  | 10            | cd       | 90      |
-            | 1    | d  | 90            | cd       | 90      |
-            | 1    | d  | 170           | cd       | 90      |
-            | 1    | d  | 190           | ab,bc,cd | 270     |
-            | 1    | d  | 270           | ab,bc,cd | 270     |
-            | 1    | d  | 350           | ab,bc,cd | 270     |
+            | from | to | bearings | route       | bearing       |
+            | 0    | b  | 10 10    | bc          | 0             |
+            | 0    | b  | 90 90    | ab          | 90            |
+            # The returned bearing is wrong here, it's based on the snapped
+            # coordinates, not the acutal edge bearing.  This should be
+            # fixed one day, but it's only a problem when we snap too vias
+            # to the same point - DP
+            #| 0    | b  | 170 170  | da          | 180           |
+            #| 0    | b  | 189 189  | da          | 180           |
+            | 0    | 1  | 90 270   | ab,bc,cd    | 90,0,270      |
+            | 1    | d  | 10 10    | bc          | 0             |
+            | 1    | d  | 90 90    | ab,bc,cd,da | 90,0,270,180  |
+            | 1    | 0  | 189 189  | da          | 180           |
+            | 1    | d  | 270 270  | cd          | 270           |
+            | 1    | d  | 349 349  |             |               |
 
     Scenario: Testbot - Initial bearing in all direction
         Given the node map
-            | h |  |   | a |   |  | b |
+            | h |  | q | a |   |  | b |
             |   |  |   |   |   |  |   |
             |   |  | p | i | j |  |   |
             | g |  | o | 0 | k |  | c |
@@ -82,12 +100,12 @@ Feature: Bearing parameter
             | ha    | yes    |
 
         When I route I should get
-            | from | to | param:bearing | route                   | bearing |
-            | 0    | a  | 0             | ia                      | 0       |
-            | 0    | a  | 45            | jb,bc,cd,de,ef,fg,gh,ha | 45      |
-            | 0    | a  | 90            | kc,cd,de,ef,fg,gh,ha    | 90      |
-            | 0    | a  | 135           | ld,de,ef,fg,gh,ha       | 135     |
-            | 0    | a  | 180           | me,de,ef,fg,gh,ha       | 180     |
-            | 0    | a  | 225           | nf,ef,fg,gh,ha          | 225     |
-            | 0    | a  | 270           | og,gh,ha                | 270     |
-            | 0    | a  | 315           | pn,ha                   | 315     |
+            | from | to | bearings | route                      | bearing                     |
+            | 0    | q  | 0 90     | ia,ab,bc,cd,de,ef,fg,gh,ha | 0,90,180,180,270,270,0,0,90 |
+            | 0    | a  | 45 90    | jb,bc,cd,de,ef,fg,gh,ha    | 45,180,180,270,270,0,0,90   |
+            | 0    | q  | 90 90    | kc,cd,de,ef,fg,gh,ha       | 90,180,270,270,0,0,90       |
+            | 0    | a  | 135 90   | ld,de,ef,fg,gh,ha          | 135,270,270,0,0,90          |
+            | 0    | a  | 180 90   | me,ef,fg,gh,ha             | 180,270,0,0,90              |
+            | 0    | a  | 225 90   | nf,fg,gh,ha                | 225,0,0,90                  |
+            | 0    | a  | 270 90   | og,gh,ha                   | 270,0,90                    |
+            | 0    | a  | 315 90   | ph,ha                      | 315,90                      |
diff --git a/features/testbot/distance_matrix.feature b/features/testbot/distance_matrix.feature
index 390ae98..4910126 100644
--- a/features/testbot/distance_matrix.feature
+++ b/features/testbot/distance_matrix.feature
@@ -100,3 +100,82 @@ Feature: Basic Distance Matrix
             | y | 500 | 0   | 300 | 200 |
             | d | 200 | 300 | 0   | 300 |
             | e | 300 | 400 | 100 | 0   |
+
+    Scenario: Testbot - Travel time matrix and with only one source
+        Given the node map
+            | a | b | c |
+            | d | e | f |
+
+        And the ways
+            | nodes |
+            | abc   |
+            | def   |
+            | ad    |
+            | be    |
+            | cf    |
+
+        When I request a travel time matrix I should get
+            |   | a   | b   | e   | f   |
+            | a | 0   | 100 | 200 | 300 |
+
+     Scenario: Testbot - Travel time 3x2 matrix
+        Given the node map
+            | a | b | c |
+            | d | e | f |
+
+        And the ways
+            | nodes |
+            | abc   |
+            | def   |
+            | ad    |
+            | be    |
+            | cf    |
+
+        When I request a travel time matrix I should get
+            |   | b   | e   | f   |
+            | a | 100 | 200 | 300 |
+            | b | 0   | 100 | 200 |
+
+    Scenario: Testbog - All coordinates are from same small component
+        Given a grid size of 300 meters
+        Given the extract extra arguments "--small-component-size 4"
+        Given the node map
+            | a | b |  | f |
+            | d | e |  | g |
+
+        And the ways
+            | nodes |
+            | ab    |
+            | be    |
+            | ed    |
+            | da    |
+            | fg    |
+
+        When I request a travel time matrix I should get
+            |   | f   | g   |
+            | f | 0   | 300 |
+            | g | 300 |  0  |
+
+    Scenario: Testbog - Coordinates are from different small component and snap to big CC
+        Given a grid size of 300 meters
+        Given the extract extra arguments "--small-component-size 4"
+        Given the node map
+            | a | b |  | f | h |
+            | d | e |  | g | i |
+
+        And the ways
+            | nodes |
+            | ab    |
+            | be    |
+            | ed    |
+            | da    |
+            | fg    |
+            | hi    |
+
+        When I request a travel time matrix I should get
+            |   | f   | g   | h   | i   |
+            | f | 0   | 300 | 0   | 300 |
+            | g | 300 |  0  | 300 | 0   |
+            | h | 0   | 300 | 0   | 300 |
+            | i | 300 |  0  | 300 | 0   |
+
diff --git a/features/testbot/matching_turns.feature b/features/testbot/matching_turns.feature
index 2d0a558..8bca4a2 100644
--- a/features/testbot/matching_turns.feature
+++ b/features/testbot/matching_turns.feature
@@ -5,6 +5,8 @@ Feature: Turn directions/codes
         Given the profile "testbot"
 
     Scenario: Turn directions
+        Given the query options
+            | instructions | true |
         Given the node map
             | o | p | a | b | c |
             | n |   |   |   | d |
@@ -31,7 +33,7 @@ Feature: Turn directions/codes
             | xo    |
             | xp    |
 
-        When I match with turns I should get
+        When I match I should get
             | trace | route | turns                         | matchings |
             | im    | xi,xm | head,left,destination         | im        |
             | io    | xi,xo | head,slight_left,destination  | io        |
@@ -79,4 +81,41 @@ Feature: Turn directions/codes
             | gm    | xg,xm | head,slight_left,destination  | gm        |
             | go    | xg,xo | head,straight,destination     | go        |
             | ga    | xg,xa | head,slight_right,destination | ga        |
-            | gc    | xg,xc | head,right,destination        | gc        |
\ No newline at end of file
+            | gc    | xg,xc | head,right,destination        | gc        |
+
+    Scenario: Turn directions
+        Given the query options
+            | instructions | true |
+        Given the node map
+            | o | p | a | b | c |
+            | n |   |   |   | d |
+            | m |   | x |   | e |
+            | l |   |   |   | f |
+            | k | j | i | h | g |
+
+        And the ways
+            | nodes |
+            | xa    |
+            | xb    |
+            | xc    |
+            | xd    |
+            | xe    |
+            | xf    |
+            | xg    |
+            | xh    |
+            | xi    |
+            | xj    |
+            | xk    |
+            | xl    |
+            | xm    |
+            | xn    |
+            | xo    |
+            | xp    |
+
+        When I match I should get
+            | trace | route | turns                         | matchings | duration |
+            | im    | xi,xm | head,left,destination         | im        | 80       |
+            | io    | xi,xo | head,slight_left,destination  | io        | 88       |
+            | ia    | xi,xa | head,straight,destination     | ia        | 80       |
+            | ic    | xi,xc | head,slight_right,destination | ic        | 88       |
+            | ie    | xi,xe | head,right,destination        | ie        | 60       |
diff --git a/features/testbot/post.feature b/features/testbot/post.feature
index ceacbb0..ac53177 100644
--- a/features/testbot/post.feature
+++ b/features/testbot/post.feature
@@ -62,24 +62,6 @@ Feature: POST request
             | d | 200 | 300 | 0   | 300 |
             | e | 300 | 400 | 100 | 0   |
 
-    Scenario: Testbot - locate POST request
-        Given the node locations
-            | node | lat | lon  |
-            | a    | -85 | -180 |
-            | b    | 0   | 0    |
-            | c    | 85  | 180  |
-            | x    | -84 | -180 |
-            | y    | 84  | 180  |
-
-        And the ways
-            | nodes |
-            | abc   |
-
-        When I request locate I should get
-            | in | out |
-            | x  | a   |
-            | y  | c   |
-            
     Scenario: Testbot - nearest POST request
         Given the node locations
             | node | lat     | lon  |
diff --git a/features/testbot/snap.feature b/features/testbot/snap.feature
index 01c590b..9b7bcbd 100644
--- a/features/testbot/snap.feature
+++ b/features/testbot/snap.feature
@@ -76,6 +76,26 @@ Feature: Snap start/end point to the nearest way
             | a    | k  | jkla      |
             | a    | l  | jkla      |
 
+    Scenario: Snapping in viaroute
+        Given the extract extra arguments "--small-component-size 4"
+        Given the node map
+            | a |   | c | e |
+            | b |   | d | f |
+
+        And the ways
+            | nodes |
+            | ab    |
+            | cd    |
+            | df    |
+            | fe    |
+            | ec    |
+
+        When I route I should get
+            | from | to | route |
+            | a    | b  | ab    |
+            | a    | d  | cd    |
+            | c    | d  | cd    |
+
     Scenario: Snap to correct way at large scales
         Given a grid size of 1000 meters
         Given the node map
diff --git a/features/testbot/status.feature b/features/testbot/status.feature
index 1b12b33..86d8259 100644
--- a/features/testbot/status.feature
+++ b/features/testbot/status.feature
@@ -14,8 +14,8 @@ Feature: Status messages
 
         When I route I should get
             | from | to | route | status | message                    |
-            | a    | b  | ab    | 0      | Found route between points |
-            | b    | a  | ab    | 0      | Found route between points |
+            | a    | b  | ab    | 200    | Found route between points |
+            | b    | a  | ab    | 200    | Found route between points |
 
     Scenario: No route found
         Given the node map
@@ -30,10 +30,10 @@ Feature: Status messages
 
         When I route I should get
             | from | to | route | status | message                          |
-            | a    | b  | ab    | 0      | Found route between points       |
-            | c    | d  | cd    | 0      | Found route between points       |
-            | a    | c  |       | 207    | Cannot find route between points |
-            | b    | d  |       | 207    | Cannot find route between points |
+            | a    | b  | ab    | 200    | Found route between points       |
+            | c    | d  | cd    | 200    | Found route between points       |
+            | a    | c  |       | 207    | Impossible route between points  |
+            | b    | d  |       | 207    | Impossible route between points  |
 
     Scenario: Malformed requests
         Given the node locations
@@ -46,22 +46,22 @@ Feature: Status messages
             | ab    |
 
         When I route I should get
-            | request                     | status | message                                    |
-            | viaroute?loc=1,1&loc=1.01,1 | 0      | Found route between points                 |
-            | nonsense                    | 400    | Bad Request                                |
-            | nonsense?loc=1,1&loc=1.01,1 | 400    | Bad Request                                |
-            |                             | 400    | Query string malformed close to position 0 |
-            | /                           | 400    | Query string malformed close to position 0 |
-            | ?                           | 400    | Query string malformed close to position 0 |
-            | viaroute/loc=               | 400    | Query string malformed close to position 9 |
-            | viaroute/loc=1              | 400    | Query string malformed close to position 9 |
-            | viaroute/loc=1,1            | 400    | Query string malformed close to position 9 |
-            | viaroute/loc=1,1,1          | 400    | Query string malformed close to position 9 |
-            | viaroute/loc=x              | 400    | Query string malformed close to position 9 |
-            | viaroute/loc=x,y            | 400    | Query string malformed close to position 9 |
-            | viaroute/loc=1,1&loc=       | 400    | Query string malformed close to position 9 |
-            | viaroute/loc=1,1&loc=1      | 400    | Query string malformed close to position 9 |
-            | viaroute/loc=1,1&loc=1,1    | 400    | Query string malformed close to position 9 |
-            | viaroute/loc=1,1&loc=1,1,1  | 400    | Query string malformed close to position 9 |
-            | viaroute/loc=1,1&loc=x      | 400    | Query string malformed close to position 9 |
-            | viaroute/loc=1,1&loc=x,y    | 400    | Query string malformed close to position 9 |
+            | request                     | status | message                                     |
+            | viaroute?loc=1,1&loc=1.01,1 | 200    | Found route between points                  |
+            | nonsense                    | 400    | Service not found                           |
+            | nonsense?loc=1,1&loc=1.01,1 | 400    | Service not found                           |
+            |                             | 400    | Query string malformed close to position 0  |
+            | /                           | 400    | Query string malformed close to position 0  |
+            | ?                           | 400    | Query string malformed close to position 0  |
+            | viaroute?loc=               | 400    | Query string malformed close to position 9  |
+            | viaroute?loc=1              | 400    | Query string malformed close to position 9  |
+            | viaroute?loc=1,1            | 400    | Invalid coordinates                         |
+            | viaroute?loc=1,1,1          | 400    | Query string malformed close to position 17 |
+            | viaroute?loc=x              | 400    | Query string malformed close to position 9  |
+            | viaroute?loc=x,y            | 400    | Query string malformed close to position 9  |
+            | viaroute?loc=1,1&loc=       | 400    | Query string malformed close to position 17 |
+            | viaroute?loc=1,1&loc=1      | 400    | Query string malformed close to position 17 |
+            | viaroute?loc=1,1&loc=1,1    | 200    | Found route between points                  |
+            | viaroute?loc=1,1&loc=1,1,1  | 400    | Query string malformed close to position 25 |
+            | viaroute?loc=1,1&loc=x      | 400    | Query string malformed close to position 17 |
+            | viaroute?loc=1,1&loc=x,y    | 400    | Query string malformed close to position 17 |
diff --git a/features/testbot/uturn.feature b/features/testbot/uturn.feature
index 943796e..26c6818 100644
--- a/features/testbot/uturn.feature
+++ b/features/testbot/uturn.feature
@@ -67,3 +67,27 @@ Feature: U-turns at via points
         When I route I should get
             | waypoints | route       | turns                              |
             | a,e,c     | ab,be,be,bc | head,right,uturn,right,destination |
+
+    Scenario: u-turn mixed with non-uturn vias
+        Given the node map
+            | a | 1 | b | 3 | c | 5 | d |
+            |   |   | 2 |   |   |   | 4 |
+            |   |   | e |   | f |   | g |
+
+        And the query options
+            | uturns | true |
+
+        And the ways
+            | nodes |
+            | ab    |
+            | bc    |
+            | cd    |
+            | be    |
+            | dg    |
+            | ef    |
+            | fg    |
+
+        When I route I should get
+            | waypoints | route                      |
+            | 1,2,3,4,5 | ab,be,be,bc,bc,cd,dg,dg,cd |
+
diff --git a/features/testbot/via.feature b/features/testbot/via.feature
index 393e1d0..a88b0f4 100644
--- a/features/testbot/via.feature
+++ b/features/testbot/via.feature
@@ -52,30 +52,6 @@ Feature: Via points
             | a,c,f     | ab,bcd,bcd,de,efg        |
             | a,c,f,h   | ab,bcd,bcd,de,efg,efg,gh |
 
-    Scenario: Alternative via points
-        Given the node map
-            |   | b | c | d |   |   |
-            | a |   |   |   |   | z |
-            |   | g | h | i | j |   |
-
-        And the ways
-            | nodes |
-            | ab    |
-            | bc    |
-            | cd    |
-            | dz    |
-            | ag    |
-            | gh    |
-            | hi    |
-            | ij    |
-            | jz    |
-
-        And the query options
-            | alt | true |
-
-        When I route I should get
-            | waypoints | route       | alternative    |
-            | a,z       | ab,bc,cd,dz | ag,gh,hi,ij,jz |
 
     Scenario: Duplicate via point
         Given the node map
diff --git a/include/osrm/libosrm_config.hpp b/include/osrm/libosrm_config.hpp
index 83772d0..677450a 100644
--- a/include/osrm/libosrm_config.hpp
+++ b/include/osrm/libosrm_config.hpp
@@ -25,33 +25,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 */
 
-#ifndef SERVER_CONFIG_HPP
-#define SERVER_CONFIG_HPP
+#ifndef LIBOSRM_CONFIG_HPP
+#define LIBOSRM_CONFIG_HPP
 
-#include <osrm/server_paths.hpp>
+#include <boost/filesystem/path.hpp>
 
-struct libosrm_config
+#include <unordered_map>
+#include <string>
+
+struct LibOSRMConfig
 {
-    libosrm_config(const libosrm_config &) = delete;
-    libosrm_config()
-        : max_locations_distance_table(100), max_locations_map_matching(-1),
-          use_shared_memory(true)
-    {
-    }
-
-    libosrm_config(ServerPaths paths,
-                   const bool sharedmemory_flag,
-                   const int max_table,
-                   const int max_matching)
-        : server_paths(std::move(paths)), max_locations_distance_table(max_table),
-          max_locations_map_matching(max_matching), use_shared_memory(sharedmemory_flag)
-    {
-    }
-
-    ServerPaths server_paths;
-    int max_locations_distance_table;
-    int max_locations_map_matching;
-    bool use_shared_memory;
+    std::unordered_map<std::string, boost::filesystem::path> server_paths;
+    int max_locations_trip = -1;
+    int max_locations_viaroute = -1;
+    int max_locations_distance_table = -1;
+    int max_locations_map_matching = -1;
+    bool use_shared_memory = true;
 };
 
 #endif // SERVER_CONFIG_HPP
diff --git a/library/osrm.hpp b/include/osrm/osrm.hpp
similarity index 85%
rename from library/osrm.hpp
rename to include/osrm/osrm.hpp
index 27acb40..cbf14d3 100644
--- a/library/osrm.hpp
+++ b/include/osrm/osrm.hpp
@@ -28,11 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef OSRM_HPP
 #define OSRM_HPP
 
-#include <osrm/libosrm_config.hpp>
-
 #include <memory>
 
-class OSRM_impl;
+struct LibOSRMConfig;
 struct RouteParameters;
 
 namespace osrm
@@ -46,12 +44,13 @@ struct Object;
 class OSRM
 {
   private:
+    class OSRM_impl;
     std::unique_ptr<OSRM_impl> OSRM_pimpl_;
 
   public:
-    explicit OSRM(libosrm_config &lib_config);
-    ~OSRM();
-    int RunQuery(RouteParameters &route_parameters, osrm::json::Object &json_result);
+    OSRM(LibOSRMConfig &lib_config);
+    ~OSRM(); // needed because we need to define it with the implementation of OSRM_impl
+    int RunQuery(const RouteParameters &route_parameters, osrm::json::Object &json_result);
 };
 
 #endif // OSRM_HPP
diff --git a/include/osrm/route_parameters.hpp b/include/osrm/route_parameters.hpp
index 6395ad6..6d2dda7 100644
--- a/include/osrm/route_parameters.hpp
+++ b/include/osrm/route_parameters.hpp
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <osrm/coordinate.hpp>
 
 #include <boost/fusion/container/vector/vector_fwd.hpp>
+#include <boost/spirit/include/qi.hpp>
 
 #include <string>
 #include <vector>
@@ -71,6 +72,8 @@ struct RouteParameters
 
     void addTimestamp(const unsigned timestamp);
 
+    void addBearing(const boost::fusion::vector<int, boost::optional<int>> &received_bearing, boost::spirit::qi::unused_type unused, bool& pass);
+
     void setLanguage(const std::string &language);
 
     void setGeometryFlag(const bool flag);
@@ -79,6 +82,10 @@ struct RouteParameters
 
     void addCoordinate(const boost::fusion::vector<double, double> &received_coordinates);
 
+    void addDestination(const boost::fusion::vector<double, double> &received_coordinates);
+
+    void addSource(const boost::fusion::vector<double, double> &received_coordinates);
+
     void getCoordinatesFromGeometry(const std::string &geometry_string);
 
     short zoom_level;
@@ -99,8 +106,11 @@ struct RouteParameters
     std::string language;
     std::vector<std::string> hints;
     std::vector<unsigned> timestamps;
+    std::vector<std::pair<const int,const boost::optional<int>>> bearings;
     std::vector<bool> uturns;
     std::vector<FixedPointCoordinate> coordinates;
+    std::vector<bool> is_destination;
+    std::vector<bool> is_source;
 };
 
 #endif // ROUTE_PARAMETERS_HPP
diff --git a/include/osrm/server_paths.hpp b/include/osrm/server_paths.hpp
deleted file mode 100644
index 669ee7d..0000000
--- a/include/osrm/server_paths.hpp
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
-
-Copyright (c) 2013, Project OSRM contributors
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without modification,
-are permitted provided that the following conditions are met:
-
-Redistributions of source code must retain the above copyright notice, this list
-of conditions and the following disclaimer.
-Redistributions in binary form must reproduce the above copyright notice, this
-list of conditions and the following disclaimer in the documentation and/or
-other materials provided with the distribution.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
-ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
-ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-*/
-
-#ifndef SERVER_PATH_H
-#define SERVER_PATH_H
-
-#include <boost/filesystem.hpp>
-
-#include <unordered_map>
-#include <string>
-
-using ServerPaths = std::unordered_map<std::string, boost::filesystem::path>;
-
-#endif // SERVER_PATH_H
diff --git a/include/osrm/strong_typedef.hpp b/include/osrm/strong_typedef.hpp
new file mode 100644
index 0000000..c2364b0
--- /dev/null
+++ b/include/osrm/strong_typedef.hpp
@@ -0,0 +1,68 @@
+#ifndef OSRM_STRONG_TYPEDEF_HPP
+#define OSRM_STRONG_TYPEDEF_HPP
+
+/*
+
+Copyright (c) 2015, Project OSRM contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+*/
+
+#include <type_traits>
+#include <functional>
+
+/* Creates strongly typed wrappers around scalar types.
+ * Useful for stopping accidental assignment of lats to lons,
+ * etc.  Also clarifies what this random "int" value is
+ * being used for.
+ */
+#define OSRM_STRONG_TYPEDEF(From, To)                 \
+  class To final {                                      \
+    static_assert(std::is_arithmetic<From>(), "");      \
+    From x;                                             \
+                                                        \
+   public:                                              \
+    To() = default;                                     \
+    explicit To(const From x_) : x(x_) {}                     \
+    explicit operator From&() { return x; }             \
+    explicit operator const From&() const { return x; } \
+    bool operator <(const To &z_) const { return x < static_cast<const From>(z_) ; } \
+    bool operator >(const To &z_) const { return x > static_cast<const From>(z_) ; } \
+    bool operator <=(const To &z_) const { return x <= static_cast<const From>(z_) ; } \
+    bool operator >=(const To &z_) const { return x >= static_cast<const From>(z_) ; } \
+    bool operator ==(const To &z_) const { return x == static_cast<const From>(z_) ; } \
+    bool operator !=(const To &z_) const { return x != static_cast<const From>(z_) ; } \
+  };                                                    \
+  inline From To##_to_##From(To to) { return static_cast<From>(to); } \
+  namespace std { \
+  template <> \
+  struct hash<To> \
+  { \
+    std::size_t operator()(const To& k) const \
+    { \
+      return std::hash<From>()(static_cast<const From>(k)); \
+    } \
+  }; \
+  }
+
+#endif // OSRM_STRONG_TYPEDEF_HPP
diff --git a/library/osrm_impl.cpp b/library/osrm_impl.cpp
index 075c65f..3f5b62b 100644
--- a/library/osrm_impl.cpp
+++ b/library/osrm_impl.cpp
@@ -34,11 +34,9 @@ class named_mutex;
 }
 
 #include "osrm_impl.hpp"
-#include "osrm.hpp"
 
 #include "../plugins/distance_table.hpp"
 #include "../plugins/hello_world.hpp"
-#include "../plugins/locate.hpp"
 #include "../plugins/nearest.hpp"
 #include "../plugins/timestamp.hpp"
 #include "../plugins/trip.hpp"
@@ -57,13 +55,15 @@ class named_mutex;
 #include <boost/interprocess/sync/scoped_lock.hpp>
 
 #include <osrm/route_parameters.hpp>
+#include <osrm/libosrm_config.hpp>
+#include <osrm/osrm.hpp>
 
 #include <algorithm>
 #include <fstream>
 #include <utility>
 #include <vector>
 
-OSRM_impl::OSRM_impl(libosrm_config &lib_config)
+OSRM::OSRM_impl::OSRM_impl(LibOSRMConfig& lib_config)
 {
     if (lib_config.use_shared_memory)
     {
@@ -81,51 +81,41 @@ OSRM_impl::OSRM_impl(libosrm_config &lib_config)
     RegisterPlugin(new DistanceTablePlugin<BaseDataFacade<QueryEdge::EdgeData>>(
         query_data_facade, lib_config.max_locations_distance_table));
     RegisterPlugin(new HelloWorldPlugin());
-    RegisterPlugin(new LocatePlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
     RegisterPlugin(new NearestPlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
     RegisterPlugin(new MapMatchingPlugin<BaseDataFacade<QueryEdge::EdgeData>>(
         query_data_facade, lib_config.max_locations_map_matching));
     RegisterPlugin(new TimestampPlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
-    RegisterPlugin(new ViaRoutePlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
-    RegisterPlugin(new RoundTripPlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
+    RegisterPlugin(new ViaRoutePlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade,
+                lib_config.max_locations_viaroute));
+    RegisterPlugin(new RoundTripPlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade,
+                lib_config.max_locations_trip));
 }
 
-OSRM_impl::~OSRM_impl()
+void OSRM::OSRM_impl::RegisterPlugin(BasePlugin *raw_plugin_ptr)
 {
-    delete query_data_facade;
-    for (PluginMap::value_type &plugin_pointer : plugin_map)
-    {
-        delete plugin_pointer.second;
-    }
-}
-
-void OSRM_impl::RegisterPlugin(BasePlugin *plugin)
-{
-    SimpleLogger().Write() << "loaded plugin: " << plugin->GetDescriptor();
-    if (plugin_map.find(plugin->GetDescriptor()) != plugin_map.end())
-    {
-        delete plugin_map.find(plugin->GetDescriptor())->second;
-    }
-    plugin_map.emplace(plugin->GetDescriptor(), plugin);
+    std::unique_ptr<BasePlugin> plugin_ptr(raw_plugin_ptr);
+    SimpleLogger().Write() << "loaded plugin: " << plugin_ptr->GetDescriptor();
+    plugin_map[plugin_ptr->GetDescriptor()] = std::move(plugin_ptr);
 }
 
-int OSRM_impl::RunQuery(RouteParameters &route_parameters, osrm::json::Object &json_result)
+int OSRM::OSRM_impl::RunQuery(const RouteParameters &route_parameters, osrm::json::Object &json_result)
 {
     const auto &plugin_iterator = plugin_map.find(route_parameters.service);
 
     if (plugin_map.end() == plugin_iterator)
     {
+        json_result.values["status_message"] = "Service not found";
         return 400;
     }
 
     increase_concurrent_query_count();
-    plugin_iterator->second->HandleRequest(route_parameters, json_result);
+    auto return_code = plugin_iterator->second->HandleRequest(route_parameters, json_result);
     decrease_concurrent_query_count();
-    return 200;
+    return static_cast<int>(return_code);
 }
 
 // decrease number of concurrent queries
-void OSRM_impl::decrease_concurrent_query_count()
+void OSRM::OSRM_impl::decrease_concurrent_query_count()
 {
     if (!barrier)
     {
@@ -147,7 +137,7 @@ void OSRM_impl::decrease_concurrent_query_count()
 }
 
 // increase number of concurrent queries
-void OSRM_impl::increase_concurrent_query_count()
+void OSRM::OSRM_impl::increase_concurrent_query_count()
 {
     if (!barrier)
     {
@@ -173,11 +163,12 @@ void OSRM_impl::increase_concurrent_query_count()
 }
 
 // proxy code for compilation firewall
-OSRM::OSRM(libosrm_config &lib_config) : OSRM_pimpl_(osrm::make_unique<OSRM_impl>(lib_config)) {}
+OSRM::OSRM(LibOSRMConfig &lib_config) : OSRM_pimpl_(osrm::make_unique<OSRM_impl>(lib_config)) {}
 
-OSRM::~OSRM() { OSRM_pimpl_.reset(); }
+// needed because unique_ptr needs the size of OSRM_impl for delete
+OSRM::~OSRM() {}
 
-int OSRM::RunQuery(RouteParameters &route_parameters, osrm::json::Object &json_result)
+int OSRM::RunQuery(const RouteParameters &route_parameters, osrm::json::Object &json_result)
 {
     return OSRM_pimpl_->RunQuery(route_parameters, json_result);
 }
diff --git a/library/osrm_impl.hpp b/library/osrm_impl.hpp
index a736c04..c223449 100644
--- a/library/osrm_impl.hpp
+++ b/library/osrm_impl.hpp
@@ -35,6 +35,7 @@ struct RouteParameters;
 
 #include <osrm/json_container.hpp>
 #include <osrm/libosrm_config.hpp>
+#include <osrm/osrm.hpp>
 
 #include <memory>
 #include <unordered_map>
@@ -43,16 +44,15 @@ struct RouteParameters;
 struct SharedBarriers;
 template <class EdgeDataT> class BaseDataFacade;
 
-class OSRM_impl
+class OSRM::OSRM_impl final
 {
   private:
-    using PluginMap = std::unordered_map<std::string, BasePlugin *>;
+    using PluginMap = std::unordered_map<std::string, std::unique_ptr<BasePlugin>>;
 
   public:
-    OSRM_impl(libosrm_config &lib_config);
+    OSRM_impl(LibOSRMConfig &lib_config);
     OSRM_impl(const OSRM_impl &) = delete;
-    virtual ~OSRM_impl();
-    int RunQuery(RouteParameters &route_parameters, osrm::json::Object &json_result);
+    int RunQuery(const RouteParameters &route_parameters, osrm::json::Object &json_result);
 
   private:
     void RegisterPlugin(BasePlugin *plugin);
diff --git a/plugins/distance_table.hpp b/plugins/distance_table.hpp
index d7a9804..c4ed250 100644
--- a/plugins/distance_table.hpp
+++ b/plugins/distance_table.hpp
@@ -67,21 +67,55 @@ template <class DataFacadeT> class DistanceTablePlugin final : public BasePlugin
 
     const std::string GetDescriptor() const override final { return descriptor_string; }
 
-    int HandleRequest(const RouteParameters &route_parameters,
+    Status HandleRequest(const RouteParameters &route_parameters,
                       osrm::json::Object &json_result) override final
     {
         if (!check_all_coordinates(route_parameters.coordinates))
         {
-            return 400;
+            json_result.values["status_message"] = "Coordinates are invalid";
+            return Status::Error;
+        }
+
+        const auto &input_bearings = route_parameters.bearings;
+        if (input_bearings.size() > 0 &&
+            route_parameters.coordinates.size() != input_bearings.size())
+        {
+            json_result.values["status_message"] =
+                "Number of bearings does not match number of coordinates";
+            return Status::Error;
+        }
+
+        auto number_of_sources =
+            std::count_if(route_parameters.is_source.begin(), route_parameters.is_source.end(),
+                          [](const bool is_source)
+                          {
+                              return is_source;
+                          });
+        auto number_of_destination =
+            std::count_if(route_parameters.is_destination.begin(),
+                          route_parameters.is_destination.end(), [](const bool is_destination)
+                          {
+                              return is_destination;
+                          });
+
+        if (max_locations_distance_table > 0 &&
+            (number_of_sources * number_of_destination >
+             max_locations_distance_table * max_locations_distance_table))
+        {
+            json_result.values["status_message"] =
+                "Number of entries " + std::to_string(number_of_sources * number_of_destination) +
+                " is higher than current maximum (" +
+                std::to_string(max_locations_distance_table * max_locations_distance_table) + ")";
+            return Status::Error;
         }
 
         const bool checksum_OK = (route_parameters.check_sum == facade->GetCheckSum());
-        unsigned max_locations =
-            std::min(static_cast<unsigned>(max_locations_distance_table),
-                     static_cast<unsigned>(route_parameters.coordinates.size()));
 
-        PhantomNodeArray phantom_node_vector(max_locations);
-        for (const auto i : osrm::irange(0u, max_locations))
+        std::vector<PhantomNodePair> phantom_node_source_vector(number_of_sources);
+        std::vector<PhantomNodePair> phantom_node_target_vector(number_of_destination);
+        auto phantom_node_source_out_iter = phantom_node_source_vector.begin();
+        auto phantom_node_target_out_iter = phantom_node_target_vector.begin();
+        for (const auto i : osrm::irange<std::size_t>(0u, route_parameters.coordinates.size()))
         {
             if (checksum_OK && i < route_parameters.hints.size() &&
                 !route_parameters.hints[i].empty())
@@ -90,39 +124,119 @@ template <class DataFacadeT> class DistanceTablePlugin final : public BasePlugin
                 ObjectEncoder::DecodeFromBase64(route_parameters.hints[i], current_phantom_node);
                 if (current_phantom_node.is_valid(facade->GetNumberOfNodes()))
                 {
-                    phantom_node_vector[i].emplace_back(std::move(current_phantom_node));
+                    if (route_parameters.is_source[i])
+                    {
+                        *phantom_node_source_out_iter =
+                            std::make_pair(current_phantom_node, current_phantom_node);
+                        if (route_parameters.is_destination[i])
+                        {
+                            *phantom_node_target_out_iter = *phantom_node_source_out_iter;
+                            phantom_node_target_out_iter++;
+                        }
+                        phantom_node_source_out_iter++;
+                    }
+                    else
+                    {
+                        BOOST_ASSERT(route_parameters.is_destination[i] &&
+                                     !route_parameters.is_source[i]);
+                        *phantom_node_target_out_iter =
+                            std::make_pair(current_phantom_node, current_phantom_node);
+                        phantom_node_target_out_iter++;
+                    }
                     continue;
                 }
             }
-            facade->IncrementalFindPhantomNodeForCoordinate(route_parameters.coordinates[i],
-                                                            phantom_node_vector[i], 1);
+            const int bearing = input_bearings.size() > 0 ? input_bearings[i].first : 0;
+            const int range = input_bearings.size() > 0
+                                  ? (input_bearings[i].second ? *input_bearings[i].second : 10)
+                                  : 180;
+            if (route_parameters.is_source[i])
+            {
+                *phantom_node_source_out_iter =
+                    facade->NearestPhantomNodeWithAlternativeFromBigComponent(
+                        route_parameters.coordinates[i], bearing, range);
+                // we didn't found a fitting node, return error
+                if (!phantom_node_source_out_iter->first.is_valid(facade->GetNumberOfNodes()))
+                {
+                    json_result.values["status_message"] =
+                        std::string("Could not find a matching segment for coordinate ") + std::to_string(i);
+                    return Status::NoSegment;
+                }
+
+                if (route_parameters.is_destination[i])
+                {
+                    *phantom_node_target_out_iter = *phantom_node_source_out_iter;
+                    phantom_node_target_out_iter++;
+                }
+                phantom_node_source_out_iter++;
+            }
+            else
+            {
+                BOOST_ASSERT(route_parameters.is_destination[i] && !route_parameters.is_source[i]);
 
-            BOOST_ASSERT(phantom_node_vector[i].front().is_valid(facade->GetNumberOfNodes()));
+                *phantom_node_target_out_iter =
+                    facade->NearestPhantomNodeWithAlternativeFromBigComponent(
+                        route_parameters.coordinates[i], bearing, range);
+                // we didn't found a fitting node, return error
+                if (!phantom_node_target_out_iter->first.is_valid(facade->GetNumberOfNodes()))
+                {
+                    json_result.values["status_message"] =
+                        std::string("Could not find a matching segment for coordinate ") + std::to_string(i);
+                    return Status::NoSegment;
+                }
+                phantom_node_target_out_iter++;
+            }
         }
 
-        // TIMER_START(distance_table);
-        std::shared_ptr<std::vector<EdgeWeight>> result_table =
-            search_engine_ptr->distance_table(phantom_node_vector);
-        // TIMER_STOP(distance_table);
+        BOOST_ASSERT((phantom_node_source_out_iter - phantom_node_source_vector.begin()) ==
+                     number_of_sources);
+        BOOST_ASSERT((phantom_node_target_out_iter - phantom_node_target_vector.begin()) ==
+                     number_of_destination);
+
+        // FIXME we should clear phantom_node_source_vector and phantom_node_target_vector after
+        // this
+        auto snapped_source_phantoms = snapPhantomNodes(phantom_node_source_vector);
+        auto snapped_target_phantoms = snapPhantomNodes(phantom_node_target_vector);
+
+        auto result_table =
+            search_engine_ptr->distance_table(snapped_source_phantoms, snapped_target_phantoms);
 
         if (!result_table)
         {
-            return 400;
+            json_result.values["status_message"] = "No distance table found";
+            return Status::EmptyResult;
         }
 
-        osrm::json::Array json_array;
-        const auto number_of_locations = phantom_node_vector.size();
-        for (const auto row : osrm::irange<std::size_t>(0, number_of_locations))
+        osrm::json::Array matrix_json_array;
+        for (const auto row : osrm::irange<std::size_t>(0, number_of_sources))
         {
             osrm::json::Array json_row;
-            auto row_begin_iterator = result_table->begin() + (row * number_of_locations);
-            auto row_end_iterator = result_table->begin() + ((row + 1) * number_of_locations);
+            auto row_begin_iterator = result_table->begin() + (row * number_of_destination);
+            auto row_end_iterator = result_table->begin() + ((row + 1) * number_of_destination);
             json_row.values.insert(json_row.values.end(), row_begin_iterator, row_end_iterator);
-            json_array.values.push_back(json_row);
+            matrix_json_array.values.push_back(json_row);
+        }
+        json_result.values["distance_table"] = matrix_json_array;
+
+        osrm::json::Array target_coord_json_array;
+        for (const auto &phantom : snapped_target_phantoms)
+        {
+            osrm::json::Array json_coord;
+            json_coord.values.push_back(phantom.location.lat / COORDINATE_PRECISION);
+            json_coord.values.push_back(phantom.location.lon / COORDINATE_PRECISION);
+            target_coord_json_array.values.push_back(json_coord);
+        }
+        json_result.values["destination_coordinates"] = target_coord_json_array;
+        osrm::json::Array source_coord_json_array;
+        for (const auto &phantom : snapped_source_phantoms)
+        {
+            osrm::json::Array json_coord;
+            json_coord.values.push_back(phantom.location.lat / COORDINATE_PRECISION);
+            json_coord.values.push_back(phantom.location.lon / COORDINATE_PRECISION);
+            source_coord_json_array.values.push_back(json_coord);
         }
-        json_result.values["distance_table"] = json_array;
-        // osrm::json::render(reply.content, json_object);
-        return 200;
+        json_result.values["source_coordinates"] = source_coord_json_array;
+        return Status::Ok;
     }
 
   private:
diff --git a/plugins/hello_world.hpp b/plugins/hello_world.hpp
index faf5b10..512fb07 100644
--- a/plugins/hello_world.hpp
+++ b/plugins/hello_world.hpp
@@ -30,7 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "plugin_base.hpp"
 
-#include "../util/cast.hpp"
 #include "../util/json_renderer.hpp"
 
 #include <osrm/json_container.hpp>
@@ -47,16 +46,16 @@ class HelloWorldPlugin final : public BasePlugin
     virtual ~HelloWorldPlugin() {}
     const std::string GetDescriptor() const override final { return descriptor_string; }
 
-    int HandleRequest(const RouteParameters &routeParameters,
+    Status HandleRequest(const RouteParameters &routeParameters,
                       osrm::json::Object &json_result) override final
     {
         std::string temp_string;
         json_result.values["title"] = "Hello World";
 
-        temp_string = cast::integral_to_string(routeParameters.zoom_level);
+        temp_string = std::to_string(routeParameters.zoom_level);
         json_result.values["zoom_level"] = temp_string;
 
-        temp_string = cast::integral_to_string(routeParameters.check_sum);
+        temp_string = std::to_string(routeParameters.check_sum);
         json_result.values["check_sum"] = temp_string;
         json_result.values["instructions"] = (routeParameters.print_instructions ? "yes" : "no");
         json_result.values["geometry"] = (routeParameters.geometry ? "yes" : "no");
@@ -68,7 +67,7 @@ class HelloWorldPlugin final : public BasePlugin
             (!routeParameters.jsonp_parameter.empty() ? "yes" : "no");
         json_result.values["language"] = (!routeParameters.language.empty() ? "yes" : "no");
 
-        temp_string = cast::integral_to_string(routeParameters.coordinates.size());
+        temp_string = std::to_string(routeParameters.coordinates.size());
         json_result.values["location_count"] = temp_string;
 
         osrm::json::Array json_locations;
@@ -82,7 +81,7 @@ class HelloWorldPlugin final : public BasePlugin
                 static_cast<double>(coordinate.lat / COORDINATE_PRECISION));
             json_coordinates.values.push_back(
                 static_cast<double>(coordinate.lon / COORDINATE_PRECISION));
-            json_location.values[cast::integral_to_string(counter)] = json_coordinates;
+            json_location.values[std::to_string(counter)] = json_coordinates;
             json_locations.values.push_back(json_location);
             ++counter;
         }
@@ -97,7 +96,7 @@ class HelloWorldPlugin final : public BasePlugin
             ++counter;
         }
         json_result.values["hints"] = json_hints;
-        return 200;
+        return Status::Ok;
     }
 
   private:
diff --git a/plugins/locate.hpp b/plugins/locate.hpp
deleted file mode 100644
index 144765a..0000000
--- a/plugins/locate.hpp
+++ /dev/null
@@ -1,79 +0,0 @@
-/*
-
-Copyright (c) 2015, Project OSRM contributors
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without modification,
-are permitted provided that the following conditions are met:
-
-Redistributions of source code must retain the above copyright notice, this list
-of conditions and the following disclaimer.
-Redistributions in binary form must reproduce the above copyright notice, this
-list of conditions and the following disclaimer in the documentation and/or
-other materials provided with the distribution.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
-ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
-ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-*/
-
-#ifndef LOCATE_HPP
-#define LOCATE_HPP
-
-#include "plugin_base.hpp"
-
-#include "../util/json_renderer.hpp"
-#include "../util/string_util.hpp"
-
-#include <osrm/json_container.hpp>
-
-#include <string>
-
-// locates the nearest node in the road network for a given coordinate.
-template <class DataFacadeT> class LocatePlugin final : public BasePlugin
-{
-  public:
-    explicit LocatePlugin(DataFacadeT *facade) : descriptor_string("locate"), facade(facade) {}
-    const std::string GetDescriptor() const override final { return descriptor_string; }
-
-    int HandleRequest(const RouteParameters &route_parameters,
-                      osrm::json::Object &json_result) override final
-    {
-        // check number of parameters
-        if (route_parameters.coordinates.empty() ||
-            !route_parameters.coordinates.front().is_valid())
-        {
-            return 400;
-        }
-
-        FixedPointCoordinate result;
-        if (!facade->LocateClosestEndPointForCoordinate(route_parameters.coordinates.front(),
-                                                        result))
-        {
-            json_result.values["status"] = 207;
-        }
-        else
-        {
-            json_result.values["status"] = 0;
-            osrm::json::Array json_coordinate;
-            json_coordinate.values.push_back(result.lat / COORDINATE_PRECISION);
-            json_coordinate.values.push_back(result.lon / COORDINATE_PRECISION);
-            json_result.values["mapped_coordinate"] = json_coordinate;
-        }
-        return 200;
-    }
-
-  private:
-    std::string descriptor_string;
-    DataFacadeT *facade;
-};
-
-#endif /* LOCATE_HPP */
diff --git a/plugins/match.hpp b/plugins/match.hpp
index 39ca324..f176956 100644
--- a/plugins/match.hpp
+++ b/plugins/match.hpp
@@ -78,6 +78,8 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
     TraceClassification
     classify(const float trace_length, const float matched_length, const int removed_points) const
     {
+        (void)removed_points; // unused
+
         const double distance_feature = -std::log(trace_length) + std::log(matched_length);
 
         // matched to the same point
@@ -91,13 +93,17 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
         return label_with_confidence;
     }
 
-    bool getCandiates(const std::vector<FixedPointCoordinate> &input_coords,
-                      const double gps_precision,
-                      std::vector<double> &sub_trace_lengths,
-                      osrm::matching::CandidateLists &candidates_lists)
+    osrm::matching::CandidateLists getCandidates(
+        const std::vector<FixedPointCoordinate> &input_coords,
+        const std::vector<std::pair<const int, const boost::optional<int>>> &input_bearings,
+        const double gps_precision,
+        std::vector<double> &sub_trace_lengths)
     {
+        osrm::matching::CandidateLists candidates_lists;
+
         double query_radius = 10 * gps_precision;
-        double last_distance = coordinate_calculation::great_circle_distance(input_coords[0], input_coords[1]);
+        double last_distance =
+            coordinate_calculation::haversine_distance(input_coords[0], input_coords[1]);
 
         sub_trace_lengths.resize(input_coords.size());
         sub_trace_lengths[0] = 0;
@@ -106,7 +112,8 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
             bool allow_uturn = false;
             if (0 < current_coordinate)
             {
-                last_distance = coordinate_calculation::great_circle_distance(input_coords[current_coordinate - 1], input_coords[current_coordinate]);
+                last_distance = coordinate_calculation::haversine_distance(
+                    input_coords[current_coordinate - 1], input_coords[current_coordinate]);
 
                 sub_trace_lengths[current_coordinate] +=
                     sub_trace_lengths[current_coordinate - 1] + last_distance;
@@ -125,24 +132,40 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
                 }
             }
 
-            std::vector<std::pair<PhantomNode, double>> candidates;
-            facade->IncrementalFindPhantomNodeForCoordinateWithMaxDistance(
-                    input_coords[current_coordinate], candidates, query_radius);
+            // Use bearing values if supplied, otherwise fallback to 0,180 defaults
+            auto bearing = input_bearings.size() > 0 ? input_bearings[current_coordinate].first : 0;
+            auto range = input_bearings.size() > 0
+                             ? (input_bearings[current_coordinate].second
+                                    ? *input_bearings[current_coordinate].second
+                                    : 10)
+                             : 180;
+            auto candidates = facade->NearestPhantomNodesInRange(input_coords[current_coordinate],
+                                                                 query_radius, bearing, range);
+
+            if (candidates.size() == 0)
+            {
+                break;
+            }
 
             // sort by foward id, then by reverse id and then by distance
-            std::sort(candidates.begin(), candidates.end(),
-                [](const std::pair<PhantomNode, double>& lhs, const std::pair<PhantomNode, double>& rhs) {
-                    return lhs.first.forward_node_id < rhs.first.forward_node_id ||
-                            (lhs.first.forward_node_id == rhs.first.forward_node_id &&
-                             (lhs.first.reverse_node_id < rhs.first.reverse_node_id ||
-                              (lhs.first.reverse_node_id == rhs.first.reverse_node_id &&
-                               lhs.second < rhs.second)));
+            std::sort(
+                candidates.begin(), candidates.end(),
+                [](const PhantomNodeWithDistance &lhs, const PhantomNodeWithDistance &rhs)
+                {
+                    return lhs.phantom_node.forward_node_id < rhs.phantom_node.forward_node_id ||
+                           (lhs.phantom_node.forward_node_id == rhs.phantom_node.forward_node_id &&
+                            (lhs.phantom_node.reverse_node_id < rhs.phantom_node.reverse_node_id ||
+                             (lhs.phantom_node.reverse_node_id ==
+                                  rhs.phantom_node.reverse_node_id &&
+                              lhs.distance < rhs.distance)));
                 });
 
-            auto new_end = std::unique(candidates.begin(), candidates.end(),
-                [](const std::pair<PhantomNode, double>& lhs, const std::pair<PhantomNode, double>& rhs) {
-                    return lhs.first.forward_node_id == rhs.first.forward_node_id &&
-                           lhs.first.reverse_node_id == rhs.first.reverse_node_id;
+            auto new_end = std::unique(
+                candidates.begin(), candidates.end(),
+                [](const PhantomNodeWithDistance &lhs, const PhantomNodeWithDistance &rhs)
+                {
+                    return lhs.phantom_node.forward_node_id == rhs.phantom_node.forward_node_id &&
+                           lhs.phantom_node.reverse_node_id == rhs.phantom_node.reverse_node_id;
                 });
             candidates.resize(new_end - candidates.begin());
 
@@ -152,28 +175,30 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
                 for (const auto i : osrm::irange<std::size_t>(0, compact_size))
                 {
                     // Split edge if it is bidirectional and append reverse direction to end of list
-                    if (candidates[i].first.forward_node_id != SPECIAL_NODEID &&
-                        candidates[i].first.reverse_node_id != SPECIAL_NODEID)
+                    if (candidates[i].phantom_node.forward_node_id != SPECIAL_NODEID &&
+                        candidates[i].phantom_node.reverse_node_id != SPECIAL_NODEID)
                     {
-                        PhantomNode reverse_node(candidates[i].first);
+                        PhantomNode reverse_node(candidates[i].phantom_node);
                         reverse_node.forward_node_id = SPECIAL_NODEID;
-                        candidates.push_back(std::make_pair(reverse_node, candidates[i].second));
+                        candidates.push_back(
+                            PhantomNodeWithDistance{reverse_node, candidates[i].distance});
 
-                        candidates[i].first.reverse_node_id = SPECIAL_NODEID;
+                        candidates[i].phantom_node.reverse_node_id = SPECIAL_NODEID;
                     }
                 }
             }
 
             // sort by distance to make pruning effective
             std::sort(candidates.begin(), candidates.end(),
-                [](const std::pair<PhantomNode, double>& lhs, const std::pair<PhantomNode, double>& rhs) {
-                    return lhs.second < rhs.second;
-                });
+                      [](const PhantomNodeWithDistance &lhs, const PhantomNodeWithDistance &rhs)
+                      {
+                          return lhs.distance < rhs.distance;
+                      });
 
             candidates_lists.push_back(std::move(candidates));
         }
 
-        return true;
+        return candidates_lists;
     }
 
     osrm::json::Object submatchingToJSON(const osrm::matching::SubMatching &sub,
@@ -221,16 +246,22 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
 
             if (route_parameters.geometry)
             {
-                subtrace.values["geometry"] = factory.AppendGeometryString(route_parameters.compression);
+                subtrace.values["geometry"] =
+                    factory.AppendGeometryString(route_parameters.compression);
             }
 
-
             if (route_parameters.print_instructions)
             {
                 std::vector<typename JSONDescriptor<DataFacadeT>::Segment> temp_segments;
-                subtrace.values["instructions"] = json_descriptor.BuildTextualDescription(factory, temp_segments);
+                subtrace.values["instructions"] =
+                    json_descriptor.BuildTextualDescription(factory, temp_segments);
             }
 
+            factory.BuildRouteSummary(factory.get_entire_length(), raw_route.shortest_path_length);
+            osrm::json::Object json_route_summary;
+            json_route_summary.values["total_distance"] = factory.summary.distance;
+            json_route_summary.values["total_time"] = factory.summary.duration;
+            subtrace.values["route_summary"] = json_route_summary;
         }
 
         subtrace.values["indices"] = osrm::json::make_array(sub.indices);
@@ -244,50 +275,68 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
         }
         subtrace.values["matched_points"] = points;
 
+        osrm::json::Array names;
+        for (const auto &node : sub.nodes)
+        {
+            names.values.emplace_back(facade->get_name_for_id(node.name_id));
+        }
+        subtrace.values["matched_names"] = names;
+
         return subtrace;
     }
 
-    int HandleRequest(const RouteParameters &route_parameters,
-                      osrm::json::Object &json_result) final override
+    Status HandleRequest(const RouteParameters &route_parameters,
+                         osrm::json::Object &json_result) final override
     {
+        // enforce maximum number of locations for performance reasons
+        if (max_locations_map_matching > 0 &&
+            static_cast<int>(route_parameters.coordinates.size()) > max_locations_map_matching)
+        {
+            json_result.values["status_message"] = "Too many coodindates";
+            return Status::Error;
+        }
+
         // check number of parameters
         if (!check_all_coordinates(route_parameters.coordinates))
         {
-            json_result.values["status"] = "Invalid coordinates.";
-            return 400;
+            json_result.values["status_message"] = "Invalid coordinates";
+            return Status::Error;
         }
 
         std::vector<double> sub_trace_lengths;
-        osrm::matching::CandidateLists candidates_lists;
         const auto &input_coords = route_parameters.coordinates;
         const auto &input_timestamps = route_parameters.timestamps;
+        const auto &input_bearings = route_parameters.bearings;
         if (input_timestamps.size() > 0 && input_coords.size() != input_timestamps.size())
         {
-            json_result.values["status"] = "Number of timestamps does not match number of coordinates .";
-            return 400;
+            json_result.values["status_message"] =
+                "Number of timestamps does not match number of coordinates";
+            return Status::Error;
         }
 
-        // enforce maximum number of locations for performance reasons
-        if (max_locations_map_matching > 0 &&
-            static_cast<int>(input_coords.size()) < max_locations_map_matching)
+        if (input_bearings.size() > 0 && input_coords.size() != input_bearings.size())
         {
-            json_result.values["status"] = "Too many coodindates.";
-            return 400;
+            json_result.values["status_message"] =
+                "Number of bearings does not match number of coordinates";
+            return Status::Error;
         }
 
         // enforce maximum number of locations for performance reasons
         if (static_cast<int>(input_coords.size()) < 2)
         {
-            json_result.values["status"] = "At least two coordinates needed.";
-            return 400;
+            json_result.values["status_message"] = "At least two coordinates needed";
+            return Status::Error;
         }
 
-        const bool found_candidates =
-            getCandiates(input_coords, route_parameters.gps_precision, sub_trace_lengths, candidates_lists);
-        if (!found_candidates)
+        const auto candidates_lists = getCandidates(
+            input_coords, input_bearings, route_parameters.gps_precision, sub_trace_lengths);
+        if (candidates_lists.size() != input_coords.size())
         {
-            json_result.values["status"] = "No suitable matching candidates found.";
-            return 400;
+            BOOST_ASSERT(candidates_lists.size() < input_coords.size());
+            json_result.values["status_message"] =
+                std::string("Could not find a matching segment for coordinate ") +
+                std::to_string(candidates_lists.size());
+            return Status::NoSegment;
         }
 
         // setup logging if enabled
@@ -300,12 +349,6 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
                                         route_parameters.matching_beta,
                                         route_parameters.gps_precision, sub_matchings);
 
-        if (sub_matchings.empty())
-        {
-            json_result.values["status"] = "No matchings found.";
-            return 400;
-        }
-
         osrm::json::Array matchings;
         for (auto &sub : sub_matchings)
         {
@@ -337,11 +380,13 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
             {
                 current_phantom_node_pair.source_phantom = sub.nodes[i];
                 current_phantom_node_pair.target_phantom = sub.nodes[i + 1];
+                BOOST_ASSERT(current_phantom_node_pair.source_phantom.is_valid());
+                BOOST_ASSERT(current_phantom_node_pair.target_phantom.is_valid());
                 raw_route.segment_end_coordinates.emplace_back(current_phantom_node_pair);
             }
             search_engine_ptr->shortest_path(
                 raw_route.segment_end_coordinates,
-                std::vector<bool>(raw_route.segment_end_coordinates.size(), true), raw_route);
+                std::vector<bool>(raw_route.segment_end_coordinates.size() + 1, true), raw_route);
 
             BOOST_ASSERT(raw_route.shortest_path_length != INVALID_EDGE_WEIGHT);
 
@@ -352,7 +397,14 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
             osrm::json::Logger::get()->render("matching", json_result);
         json_result.values["matchings"] = matchings;
 
-        return 200;
+        if (sub_matchings.empty())
+        {
+            json_result.values["status_message"] = "Cannot find matchings";
+            return Status::EmptyResult;
+        }
+
+        json_result.values["status_message"] = "Found matchings";
+        return Status::Ok;
     }
 
   private:
diff --git a/plugins/nearest.hpp b/plugins/nearest.hpp
index bf4cff3..2459819 100644
--- a/plugins/nearest.hpp
+++ b/plugins/nearest.hpp
@@ -49,30 +49,43 @@ template <class DataFacadeT> class NearestPlugin final : public BasePlugin
 
     const std::string GetDescriptor() const override final { return descriptor_string; }
 
-    int HandleRequest(const RouteParameters &route_parameters,
+    Status HandleRequest(const RouteParameters &route_parameters,
                       osrm::json::Object &json_result) override final
     {
         // check number of parameters
         if (route_parameters.coordinates.empty() ||
             !route_parameters.coordinates.front().is_valid())
         {
-            return 400;
+            return Status::Error;
+        }
+
+        const auto &input_bearings = route_parameters.bearings;
+        if (input_bearings.size() > 0 &&
+            route_parameters.coordinates.size() != input_bearings.size())
+        {
+            json_result.values["status_message"] =
+                "Number of bearings does not match number of coordinates";
+            return Status::Error;
         }
-        auto number_of_results = static_cast<std::size_t>(route_parameters.num_results);
-        std::vector<PhantomNode> phantom_node_vector;
-        facade->IncrementalFindPhantomNodeForCoordinate(route_parameters.coordinates.front(),
-                                                        phantom_node_vector,
-                                                        static_cast<int>(number_of_results));
 
-        if (phantom_node_vector.empty() || !phantom_node_vector.front().is_valid())
+        auto number_of_results = static_cast<std::size_t>(route_parameters.num_results);
+        const int bearing = input_bearings.size() > 0 ? input_bearings.front().first : 0;
+        const int range =
+            input_bearings.size() > 0
+                ? (input_bearings.front().second ? *input_bearings.front().second : 10)
+                : 180;
+        auto phantom_node_vector = facade->NearestPhantomNodes(route_parameters.coordinates.front(),
+                                                               number_of_results, bearing, range);
+
+        if (phantom_node_vector.empty())
         {
-            json_result.values["status"] = 207;
+            json_result.values["status_message"] =
+                std::string("Could not find a matching segments for coordinate");
+            return Status::NoSegment;
         }
         else
         {
-            // reply.status = http::Reply::ok;
-            json_result.values["status"] = 0;
-
+            json_result.values["status_message"] = "Found nearest edge";
             if (number_of_results > 1)
             {
                 osrm::json::Array results;
@@ -81,15 +94,13 @@ template <class DataFacadeT> class NearestPlugin final : public BasePlugin
                 for (const auto i :
                      osrm::irange<std::size_t>(0, std::min(number_of_results, vector_length)))
                 {
+                    const auto &node = phantom_node_vector[i].phantom_node;
                     osrm::json::Array json_coordinate;
                     osrm::json::Object result;
-                    json_coordinate.values.push_back(phantom_node_vector.at(i).location.lat /
-                                                     COORDINATE_PRECISION);
-                    json_coordinate.values.push_back(phantom_node_vector.at(i).location.lon /
-                                                     COORDINATE_PRECISION);
+                    json_coordinate.values.push_back(node.location.lat / COORDINATE_PRECISION);
+                    json_coordinate.values.push_back(node.location.lon / COORDINATE_PRECISION);
                     result.values["mapped coordinate"] = json_coordinate;
-                    result.values["name"] =
-                        facade->get_name_for_id(phantom_node_vector.at(i).name_id);
+                    result.values["name"] = facade->get_name_for_id(node.name_id);
                     results.values.push_back(result);
                 }
                 json_result.values["results"] = results;
@@ -97,16 +108,16 @@ template <class DataFacadeT> class NearestPlugin final : public BasePlugin
             else
             {
                 osrm::json::Array json_coordinate;
-                json_coordinate.values.push_back(phantom_node_vector.front().location.lat /
-                                                 COORDINATE_PRECISION);
-                json_coordinate.values.push_back(phantom_node_vector.front().location.lon /
-                                                 COORDINATE_PRECISION);
+                json_coordinate.values.push_back(
+                    phantom_node_vector.front().phantom_node.location.lat / COORDINATE_PRECISION);
+                json_coordinate.values.push_back(
+                    phantom_node_vector.front().phantom_node.location.lon / COORDINATE_PRECISION);
                 json_result.values["mapped_coordinate"] = json_coordinate;
                 json_result.values["name"] =
-                    facade->get_name_for_id(phantom_node_vector.front().name_id);
+                    facade->get_name_for_id(phantom_node_vector.front().phantom_node.name_id);
             }
         }
-        return 200;
+        return Status::Ok;
     }
 
   private:
diff --git a/plugins/plugin_base.hpp b/plugins/plugin_base.hpp
index acc8204..a05785a 100644
--- a/plugins/plugin_base.hpp
+++ b/plugins/plugin_base.hpp
@@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef BASE_PLUGIN_HPP
 #define BASE_PLUGIN_HPP
 
+#include "../data_structures/phantom_node.hpp"
+
 #include <osrm/coordinate.hpp>
 #include <osrm/json_container.hpp>
 #include <osrm/route_parameters.hpp>
@@ -39,24 +41,96 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 class BasePlugin
 {
   public:
+    enum class Status : int
+    {
+      Ok = 200,
+      EmptyResult = 207,
+      NoSegment = 208,
+      Error = 400
+    };
+
     BasePlugin() {}
     // Maybe someone can explain the pure virtual destructor thing to me (dennis)
     virtual ~BasePlugin() {}
     virtual const std::string GetDescriptor() const = 0;
-    virtual int HandleRequest(const RouteParameters &, osrm::json::Object &) = 0;
-    virtual bool
-    check_all_coordinates(const std::vector<FixedPointCoordinate> &coordinates) const final
+    virtual Status HandleRequest(const RouteParameters &, osrm::json::Object &) = 0;
+    virtual bool check_all_coordinates(const std::vector<FixedPointCoordinate> &coordinates,
+                                       const unsigned min = 2) const final
     {
-        if (2 > coordinates.size() || std::any_of(std::begin(coordinates), std::end(coordinates),
-                                                  [](const FixedPointCoordinate &coordinate)
-                                                  {
-                                                      return !coordinate.is_valid();
-                                                  }))
+        if (min > coordinates.size() || std::any_of(std::begin(coordinates), std::end(coordinates),
+                                                    [](const FixedPointCoordinate &coordinate)
+                                                    {
+                                                        return !coordinate.is_valid();
+                                                    }))
         {
             return false;
         }
         return true;
     }
+
+    // Decides whether to use the phantom node from a big or small component if both are found.
+    // Returns true if all phantom nodes are in the same component after snapping.
+    std::vector<PhantomNode> snapPhantomNodes(
+        const std::vector<std::pair<PhantomNode, PhantomNode>> &phantom_node_pair_list) const
+    {
+        const auto check_component_id_is_tiny =
+            [](const std::pair<PhantomNode, PhantomNode> &phantom_pair)
+        {
+            return phantom_pair.first.component.is_tiny;
+        };
+
+        // are all phantoms from a tiny cc?
+        const auto check_all_in_same_component =
+            [](const std::vector<std::pair<PhantomNode, PhantomNode>> &nodes)
+        {
+            const auto component_id = nodes.front().first.component.id;
+
+            return std::all_of(std::begin(nodes), std::end(nodes),
+                               [component_id](const PhantomNodePair &phantom_pair)
+                               {
+                                   return component_id == phantom_pair.first.component.id;
+                               });
+        };
+
+        const auto fallback_to_big_component =
+            [](const std::pair<PhantomNode, PhantomNode> &phantom_pair)
+        {
+            if (phantom_pair.first.component.is_tiny && phantom_pair.second.is_valid() &&
+                !phantom_pair.second.component.is_tiny)
+            {
+                return phantom_pair.second;
+            }
+            return phantom_pair.first;
+        };
+
+        const auto use_closed_phantom = [](const std::pair<PhantomNode, PhantomNode> &phantom_pair)
+        {
+            return phantom_pair.first;
+        };
+
+        const bool every_phantom_is_in_tiny_cc =
+            std::all_of(std::begin(phantom_node_pair_list), std::end(phantom_node_pair_list),
+                        check_component_id_is_tiny);
+        auto all_in_same_component = check_all_in_same_component(phantom_node_pair_list);
+
+        std::vector<PhantomNode> snapped_phantoms;
+        snapped_phantoms.reserve(phantom_node_pair_list.size());
+
+        // The only case we don't snap to the big component if all phantoms are in the same small
+        // component
+        if (every_phantom_is_in_tiny_cc && all_in_same_component)
+        {
+            std::transform(phantom_node_pair_list.begin(), phantom_node_pair_list.end(),
+                           std::back_inserter(snapped_phantoms), use_closed_phantom);
+        }
+        else
+        {
+            std::transform(phantom_node_pair_list.begin(), phantom_node_pair_list.end(),
+                           std::back_inserter(snapped_phantoms), fallback_to_big_component);
+        }
+
+        return snapped_phantoms;
+    }
 };
 
 #endif /* BASE_PLUGIN_HPP */
diff --git a/plugins/timestamp.hpp b/plugins/timestamp.hpp
index 8a5d208..899ef46 100644
--- a/plugins/timestamp.hpp
+++ b/plugins/timestamp.hpp
@@ -44,13 +44,14 @@ template <class DataFacadeT> class TimestampPlugin final : public BasePlugin
     {
     }
     const std::string GetDescriptor() const override final { return descriptor_string; }
-    int HandleRequest(const RouteParameters &route_parameters,
+    Status HandleRequest(const RouteParameters &route_parameters,
                       osrm::json::Object &json_result) override final
     {
-        json_result.values["status"] = 0;
+        (void)route_parameters; // unused
+
         const std::string timestamp = facade->GetTimestamp();
         json_result.values["timestamp"] = timestamp;
-        return 200;
+        return Status::Ok;
     }
 
   private:
diff --git a/plugins/trip.hpp b/plugins/trip.hpp
index 2567db4..a69e25e 100644
--- a/plugins/trip.hpp
+++ b/plugins/trip.hpp
@@ -42,7 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "../descriptors/json_descriptor.hpp"          // to make json output
 #include "../util/make_unique.hpp"
 #include "../util/timing_util.hpp"        // to time runtime
-#include "../util/simple_logger.hpp"      // for logging output
+//#include "../util/simple_logger.hpp"      // for logging output
 #include "../util/dist_table_wrapper.hpp" // to access the dist
                                           // table more easily
 
@@ -63,19 +63,24 @@ template <class DataFacadeT> class RoundTripPlugin final : public BasePlugin
     std::string descriptor_string;
     DataFacadeT *facade;
     std::unique_ptr<SearchEngine<DataFacadeT>> search_engine_ptr;
+    int max_locations_trip;
 
   public:
-    explicit RoundTripPlugin(DataFacadeT *facade) : descriptor_string("trip"), facade(facade)
+    explicit RoundTripPlugin(DataFacadeT *facade, int max_locations_trip)
+        : descriptor_string("trip"), facade(facade), max_locations_trip(max_locations_trip)
     {
         search_engine_ptr = osrm::make_unique<SearchEngine<DataFacadeT>>(facade);
     }
 
     const std::string GetDescriptor() const override final { return descriptor_string; }
 
-    void GetPhantomNodes(const RouteParameters &route_parameters,
-                         PhantomNodeArray &phantom_node_vector)
+    std::vector<PhantomNode> GetPhantomNodes(const RouteParameters &route_parameters)
     {
         const bool checksum_OK = (route_parameters.check_sum == facade->GetCheckSum());
+        const auto &input_bearings = route_parameters.bearings;
+
+        std::vector<PhantomNode> phantom_node_list;
+        phantom_node_list.reserve(route_parameters.coordinates.size());
 
         // find phantom nodes for all input coords
         for (const auto i : osrm::irange<std::size_t>(0, route_parameters.coordinates.size()))
@@ -88,18 +93,24 @@ template <class DataFacadeT> class RoundTripPlugin final : public BasePlugin
                 ObjectEncoder::DecodeFromBase64(route_parameters.hints[i], current_phantom_node);
                 if (current_phantom_node.is_valid(facade->GetNumberOfNodes()))
                 {
-                    phantom_node_vector[i].emplace_back(std::move(current_phantom_node));
+                    phantom_node_list.push_back(std::move(current_phantom_node));
                     continue;
                 }
             }
-            facade->IncrementalFindPhantomNodeForCoordinate(route_parameters.coordinates[i],
-                                                            phantom_node_vector[i], 1);
-            if (phantom_node_vector[i].size() > 1)
+            const int bearing = input_bearings.size() > 0 ? input_bearings[i].first : 0;
+            const int range = input_bearings.size() > 0
+                                  ? (input_bearings[i].second ? *input_bearings[i].second : 10)
+                                  : 180;
+            auto results = facade->NearestPhantomNodes(route_parameters.coordinates[i], 1, bearing, range);
+            if (results.empty())
             {
-                phantom_node_vector[i].erase(std::begin(phantom_node_vector[i]));
+                break;
             }
-            BOOST_ASSERT(phantom_node_vector[i].front().is_valid(facade->GetNumberOfNodes()));
+            phantom_node_list.push_back(std::move(results.front().phantom_node));
+            BOOST_ASSERT(phantom_node_list.back().is_valid(facade->GetNumberOfNodes()));
         }
+
+        return phantom_node_list;
     }
 
     // Object to hold all strongly connected components (scc) of a graph
@@ -116,25 +127,24 @@ template <class DataFacadeT> class RoundTripPlugin final : public BasePlugin
         //          => in_component = [0, 1, 2, 4, 5, 3, 6, 7, 8]
         //          => in_range = [0, 5]
         SCC_Component(std::vector<NodeID> in_component, std::vector<size_t> in_range)
-            : component(in_component), range(in_range)
+            : component(std::move(in_component)), range(std::move(in_range))
         {
-            range.push_back(in_component.size());
+            range.push_back(component.size());
 
-            BOOST_ASSERT_MSG(in_component.size() >= in_range.size(),
+            BOOST_ASSERT_MSG(component.size() >= range.size(),
                              "scc component and its ranges do not match");
-            BOOST_ASSERT_MSG(in_component.size() > 0, "there's no scc component");
-            BOOST_ASSERT_MSG(*std::max_element(in_range.begin(), in_range.end()) <=
-                                 in_component.size(),
+            BOOST_ASSERT_MSG(component.size() > 0, "there's no scc component");
+            BOOST_ASSERT_MSG(*std::max_element(range.begin(), range.end()) <= component.size(),
                              "scc component ranges are out of bound");
-            BOOST_ASSERT_MSG(*std::min_element(in_range.begin(), in_range.end()) >= 0,
+            BOOST_ASSERT_MSG(*std::min_element(range.begin(), range.end()) >= 0,
                              "invalid scc component range");
-            BOOST_ASSERT_MSG(std::is_sorted(std::begin(in_range), std::end(in_range)),
+            BOOST_ASSERT_MSG(std::is_sorted(std::begin(range), std::end(range)),
                              "invalid component ranges");
         };
 
         // constructor to use when whole graph is one single scc
         SCC_Component(std::vector<NodeID> in_component)
-            : component(in_component), range({0, in_component.size()}){};
+            : component(std::move(in_component)), range({0, component.size()}){};
 
         std::size_t GetNumberOfComponents() const
         {
@@ -203,7 +213,7 @@ template <class DataFacadeT> class RoundTripPlugin final : public BasePlugin
         json_result.values["permutation"] = json_permutation;
     }
 
-    InternalRouteResult ComputeRoute(const PhantomNodeArray &phantom_node_vector,
+    InternalRouteResult ComputeRoute(const std::vector<PhantomNode> &phantom_node_list,
                                      const RouteParameters &route_parameters,
                                      const std::vector<NodeID> &trip)
     {
@@ -212,50 +222,86 @@ template <class DataFacadeT> class RoundTripPlugin final : public BasePlugin
         PhantomNodes viapoint;
         const auto start = std::begin(trip);
         const auto end = std::end(trip);
+        // computes a roundtrip from the nodes in trip
         for (auto it = start; it != end; ++it)
         {
             const auto from_node = *it;
             // if from_node is the last node, compute the route from the last to the first location
             const auto to_node = std::next(it) != end ? *std::next(it) : *start;
 
-            viapoint =
-                PhantomNodes{phantom_node_vector[from_node][0], phantom_node_vector[to_node][0]};
+            viapoint = PhantomNodes{phantom_node_list[from_node], phantom_node_list[to_node]};
             min_route.segment_end_coordinates.emplace_back(viapoint);
         }
+        BOOST_ASSERT(min_route.segment_end_coordinates.size() == trip.size());
+
+        std::vector<bool> uturns(trip.size() + 1);
+        std::transform(trip.begin(), trip.end(), uturns.begin(),
+                       [&route_parameters](const NodeID idx)
+                       {
+                           return route_parameters.uturns[idx];
+                       });
+        uturns.back() = route_parameters.uturns[trip.front()];
 
-        search_engine_ptr->shortest_path(min_route.segment_end_coordinates, route_parameters.uturns,
-                                         min_route);
+        search_engine_ptr->shortest_path(min_route.segment_end_coordinates, uturns, min_route);
 
         BOOST_ASSERT_MSG(min_route.shortest_path_length < INVALID_EDGE_WEIGHT, "unroutable route");
         return min_route;
     }
 
-    int HandleRequest(const RouteParameters &route_parameters,
+    Status HandleRequest(const RouteParameters &route_parameters,
                       osrm::json::Object &json_result) override final
     {
+        if (max_locations_trip > 0 &&
+            (static_cast<int>(route_parameters.coordinates.size()) > max_locations_trip))
+        {
+            json_result.values["status_message"] =
+                "Number of entries " + std::to_string(route_parameters.coordinates.size()) +
+                " is higher than current maximum (" + std::to_string(max_locations_trip) + ")";
+            return Status::Error;
+        }
+
         // check if all inputs are coordinates
         if (!check_all_coordinates(route_parameters.coordinates))
         {
-            return 400;
+            json_result.values["status_message"] = "Invalid coordinates";
+            return Status::Error;
+        }
+
+        const auto &input_bearings = route_parameters.bearings;
+        if (input_bearings.size() > 0 &&
+            route_parameters.coordinates.size() != input_bearings.size())
+        {
+            json_result.values["status_message"] =
+                "Number of bearings does not match number of coordinates";
+            return Status::Error;
         }
 
         // get phantom nodes
-        PhantomNodeArray phantom_node_vector(route_parameters.coordinates.size());
-        GetPhantomNodes(route_parameters, phantom_node_vector);
-        const auto number_of_locations = phantom_node_vector.size();
+        auto phantom_node_list = GetPhantomNodes(route_parameters);
+        if (phantom_node_list.size() != route_parameters.coordinates.size())
+        {
+            BOOST_ASSERT(phantom_node_list.size() < route_parameters.coordinates.size());
+            json_result.values["status_message"] =
+                std::string("Could not find a matching segment for coordinate ") +
+                std::to_string(phantom_node_list.size());
+            return Status::NoSegment;
+        }
+
+        const auto number_of_locations = phantom_node_list.size();
 
         // compute the distance table of all phantom nodes
         const auto result_table = DistTableWrapper<EdgeWeight>(
-            *search_engine_ptr->distance_table(phantom_node_vector), number_of_locations);
+            *search_engine_ptr->distance_table(phantom_node_list, phantom_node_list),
+            number_of_locations);
 
         if (result_table.size() == 0)
         {
-            return 400;
+            return Status::Error;
         }
 
         const constexpr std::size_t BF_MAX_FEASABLE = 10;
         BOOST_ASSERT_MSG(result_table.size() == number_of_locations * number_of_locations,
-                         "Distance Table has wrong size.");
+                         "Distance Table has wrong size");
 
         // get scc components
         SCC_Component scc = SplitUnaccessibleLocations(number_of_locations, result_table);
@@ -312,17 +358,13 @@ template <class DataFacadeT> class RoundTripPlugin final : public BasePlugin
         // compute all round trip routes
         std::vector<InternalRouteResult> comp_route;
         comp_route.reserve(route_result.size());
-        for (std::size_t r = 0; r < route_result.size(); ++r)
+        for (auto &elem : route_result)
         {
-            comp_route.push_back(
-                ComputeRoute(phantom_node_vector, route_parameters, route_result[r]));
+            comp_route.push_back(ComputeRoute(phantom_node_list, route_parameters, elem));
         }
 
         TIMER_STOP(TRIP_TIMER);
 
-        SimpleLogger().Write() << "Trip calculation took: " << TIMER_MSEC(TRIP_TIMER) / 1000.
-                               << "s";
-
         // prepare JSON output
         // create a json object for every trip
         osrm::json::Array trip;
@@ -342,11 +384,16 @@ template <class DataFacadeT> class RoundTripPlugin final : public BasePlugin
             trip.values.push_back(std::move(scc_trip));
         }
 
-        json_result.values["trips"] = std::move(trip);
-
 
+        if (trip.values.empty())
+        {
+            json_result.values["status_message"] = "Cannot find trips";
+            return Status::EmptyResult;
+        }
 
-        return 200;
+        json_result.values["trips"] = std::move(trip);
+        json_result.values["status_message"] = "Found trips";
+        return Status::Ok;
     }
 };
 
diff --git a/plugins/viaroute.hpp b/plugins/viaroute.hpp
index f705fb5..f9a6737 100644
--- a/plugins/viaroute.hpp
+++ b/plugins/viaroute.hpp
@@ -1,201 +1,212 @@
-/*
-
-Copyright (c) 2015, Project OSRM contributors
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without modification,
-are permitted provided that the following conditions are met:
-
-Redistributions of source code must retain the above copyright notice, this list
-of conditions and the following disclaimer.
-Redistributions in binary form must reproduce the above copyright notice, this
-list of conditions and the following disclaimer in the documentation and/or
-other materials provided with the distribution.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
-ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
-ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-*/
-
-#ifndef VIA_ROUTE_HPP
-#define VIA_ROUTE_HPP
-
-#include "plugin_base.hpp"
-
-#include "../algorithms/object_encoder.hpp"
-#include "../data_structures/search_engine.hpp"
-#include "../descriptors/descriptor_base.hpp"
-#include "../descriptors/gpx_descriptor.hpp"
-#include "../descriptors/json_descriptor.hpp"
-#include "../util/integer_range.hpp"
-#include "../util/json_renderer.hpp"
-#include "../util/make_unique.hpp"
-#include "../util/simple_logger.hpp"
-#include "../util/timing_util.hpp"
-
-#include <osrm/json_container.hpp>
-
-#include <cstdlib>
-
-#include <algorithm>
-#include <memory>
-#include <string>
-#include <vector>
-
-template <class DataFacadeT> class ViaRoutePlugin final : public BasePlugin
-{
-  private:
-    DescriptorTable descriptor_table;
-    std::string descriptor_string;
-    std::unique_ptr<SearchEngine<DataFacadeT>> search_engine_ptr;
-    DataFacadeT *facade;
-
-  public:
-    explicit ViaRoutePlugin(DataFacadeT *facade) : descriptor_string("viaroute"), facade(facade)
-    {
-        search_engine_ptr = osrm::make_unique<SearchEngine<DataFacadeT>>(facade);
-
-        descriptor_table.emplace("json", 0);
-        descriptor_table.emplace("gpx", 1);
-        // descriptor_table.emplace("geojson", 2);
-    }
-
-    virtual ~ViaRoutePlugin() {}
-
-    const std::string GetDescriptor() const override final { return descriptor_string; }
-
-    int HandleRequest(const RouteParameters &route_parameters,
-                      osrm::json::Object &json_result) override final
-    {
-        if (!check_all_coordinates(route_parameters.coordinates))
-        {
-            return 400;
-        }
-
-        std::vector<phantom_node_pair> phantom_node_pair_list(route_parameters.coordinates.size());
-        const bool checksum_OK = (route_parameters.check_sum == facade->GetCheckSum());
-
-        for (const auto i : osrm::irange<std::size_t>(0, route_parameters.coordinates.size()))
-        {
-            if (checksum_OK && i < route_parameters.hints.size() &&
-                !route_parameters.hints[i].empty())
-            {
-                ObjectEncoder::DecodeFromBase64(route_parameters.hints[i],
-                                                phantom_node_pair_list[i]);
-                if (phantom_node_pair_list[i].first.is_valid(facade->GetNumberOfNodes()))
-                {
-                    continue;
-                }
-            }
-            std::vector<PhantomNode> phantom_node_vector;
-            if (facade->IncrementalFindPhantomNodeForCoordinate(route_parameters.coordinates[i],
-                                                                phantom_node_vector, 1))
-            {
-                BOOST_ASSERT(!phantom_node_vector.empty());
-                phantom_node_pair_list[i].first = phantom_node_vector.front();
-                if (phantom_node_vector.size() > 1)
-                {
-                    phantom_node_pair_list[i].second = phantom_node_vector.back();
-                }
-            }
-        }
-
-        auto check_component_id_is_tiny = [](const phantom_node_pair &phantom_pair)
-        {
-            return phantom_pair.first.is_in_tiny_component();
-        };
-
-        const bool every_phantom_is_in_tiny_cc =
-            std::all_of(std::begin(phantom_node_pair_list), std::end(phantom_node_pair_list),
-                        check_component_id_is_tiny);
-
-        // are all phantoms from a tiny cc?
-        const auto component_id = phantom_node_pair_list.front().first.component_id;
-
-        auto check_component_id_is_equal = [component_id](const phantom_node_pair &phantom_pair)
-        {
-            return component_id == phantom_pair.first.component_id;
-        };
-
-        const bool every_phantom_has_equal_id =
-            std::all_of(std::begin(phantom_node_pair_list), std::end(phantom_node_pair_list),
-                        check_component_id_is_equal);
-
-        auto swap_phantom_from_big_cc_into_front = [](phantom_node_pair &phantom_pair)
-        {
-            if (0 != phantom_pair.first.component_id && 0 == phantom_pair.second.component_id)
-            {
-                using namespace std;
-                swap(phantom_pair.first, phantom_pair.second);
-            }
-        };
-
-        // this case is true if we take phantoms from the big CC
-        if (!every_phantom_is_in_tiny_cc || !every_phantom_has_equal_id)
-        {
-            std::for_each(std::begin(phantom_node_pair_list), std::end(phantom_node_pair_list),
-                          swap_phantom_from_big_cc_into_front);
-        }
-
-        InternalRouteResult raw_route;
-        auto build_phantom_pairs =
-            [&raw_route](const phantom_node_pair &first_pair, const phantom_node_pair &second_pair)
-        {
-            raw_route.segment_end_coordinates.emplace_back(
-                PhantomNodes{first_pair.first, second_pair.first});
-        };
-        osrm::for_each_pair(phantom_node_pair_list, build_phantom_pairs);
-
-        if (1 == raw_route.segment_end_coordinates.size())
-        {
-            if (route_parameters.alternate_route)
-            {
-              search_engine_ptr->alternative_path(raw_route.segment_end_coordinates.front(),
-                                                  raw_route);
-            }
-            else
-            {
-                search_engine_ptr->direct_shortest_path(raw_route.segment_end_coordinates,
-                                                        route_parameters.uturns, raw_route);
-            }
-        }
-        else
-        {
-            search_engine_ptr->shortest_path(raw_route.segment_end_coordinates,
-                                             route_parameters.uturns, raw_route);
-        }
-
-        if (INVALID_EDGE_WEIGHT == raw_route.shortest_path_length)
-        {
-            SimpleLogger().Write(logDEBUG) << "Error occurred, single path not found";
-        }
-
-        std::unique_ptr<BaseDescriptor<DataFacadeT>> descriptor;
-        switch (descriptor_table.get_id(route_parameters.output_format))
-        {
-        case 1:
-            descriptor = osrm::make_unique<GPXDescriptor<DataFacadeT>>(facade);
-            break;
-        // case 2:
-        //      descriptor = osrm::make_unique<GEOJSONDescriptor<DataFacadeT>>();
-        //      break;
-        default:
-            descriptor = osrm::make_unique<JSONDescriptor<DataFacadeT>>(facade);
-            break;
-        }
-
-        descriptor->SetConfig(route_parameters);
-        descriptor->Run(raw_route, json_result);
-        return 200;
-    }
-};
-
-#endif // VIA_ROUTE_HPP
+/*
+
+Copyright (c) 2015, Project OSRM contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+*/
+
+#ifndef VIA_ROUTE_HPP
+#define VIA_ROUTE_HPP
+
+#include "plugin_base.hpp"
+
+#include "../algorithms/object_encoder.hpp"
+#include "../data_structures/search_engine.hpp"
+#include "../descriptors/descriptor_base.hpp"
+#include "../descriptors/gpx_descriptor.hpp"
+#include "../descriptors/json_descriptor.hpp"
+#include "../util/integer_range.hpp"
+#include "../util/json_renderer.hpp"
+#include "../util/make_unique.hpp"
+#include "../util/simple_logger.hpp"
+#include "../util/timing_util.hpp"
+
+#include <osrm/json_container.hpp>
+
+#include <cstdlib>
+
+#include <algorithm>
+#include <memory>
+#include <string>
+#include <vector>
+
+template <class DataFacadeT> class ViaRoutePlugin final : public BasePlugin
+{
+  private:
+    DescriptorTable descriptor_table;
+    std::string descriptor_string;
+    std::unique_ptr<SearchEngine<DataFacadeT>> search_engine_ptr;
+    DataFacadeT *facade;
+    int max_locations_viaroute;
+
+  public:
+    explicit ViaRoutePlugin(DataFacadeT *facade, int max_locations_viaroute)
+        : descriptor_string("viaroute"), facade(facade),
+          max_locations_viaroute(max_locations_viaroute)
+    {
+        search_engine_ptr = osrm::make_unique<SearchEngine<DataFacadeT>>(facade);
+
+        descriptor_table.emplace("json", 0);
+        descriptor_table.emplace("gpx", 1);
+        // descriptor_table.emplace("geojson", 2);
+    }
+
+    virtual ~ViaRoutePlugin() {}
+
+    const std::string GetDescriptor() const override final { return descriptor_string; }
+
+    Status HandleRequest(const RouteParameters &route_parameters,
+                      osrm::json::Object &json_result) override final
+    {
+        if (max_locations_viaroute > 0 &&
+            (static_cast<int>(route_parameters.coordinates.size()) > max_locations_viaroute))
+        {
+            json_result.values["status_message"] =
+                "Number of entries " + std::to_string(route_parameters.coordinates.size()) +
+                " is higher than current maximum (" + std::to_string(max_locations_viaroute) + ")";
+            return Status::Error;
+        }
+
+        if (!check_all_coordinates(route_parameters.coordinates))
+        {
+            json_result.values["status_message"] = "Invalid coordinates";
+            return Status::Error;
+        }
+
+        const auto &input_bearings = route_parameters.bearings;
+        if (input_bearings.size() > 0 &&
+            route_parameters.coordinates.size() != input_bearings.size())
+        {
+            json_result.values["status_message"] =
+                "Number of bearings does not match number of coordinate";
+            return Status::Error;
+        }
+
+        std::vector<PhantomNodePair> phantom_node_pair_list(route_parameters.coordinates.size());
+        const bool checksum_OK = (route_parameters.check_sum == facade->GetCheckSum());
+
+        for (const auto i : osrm::irange<std::size_t>(0, route_parameters.coordinates.size()))
+        {
+            if (checksum_OK && i < route_parameters.hints.size() &&
+                !route_parameters.hints[i].empty())
+            {
+                ObjectEncoder::DecodeFromBase64(route_parameters.hints[i],
+                                                phantom_node_pair_list[i].first);
+                if (phantom_node_pair_list[i].first.is_valid(facade->GetNumberOfNodes()))
+                {
+                    continue;
+                }
+            }
+            const int bearing = input_bearings.size() > 0 ? input_bearings[i].first : 0;
+            const int range = input_bearings.size() > 0
+                                  ? (input_bearings[i].second ? *input_bearings[i].second : 10)
+                                  : 180;
+            phantom_node_pair_list[i] = facade->NearestPhantomNodeWithAlternativeFromBigComponent(
+                route_parameters.coordinates[i], bearing, range);
+            // we didn't found a fitting node, return error
+            if (!phantom_node_pair_list[i].first.is_valid(facade->GetNumberOfNodes()))
+            {
+                json_result.values["status_message"] =
+                    std::string("Could not find a matching segment for coordinate ") +
+                    std::to_string(i);
+                return Status::NoSegment;
+            }
+            BOOST_ASSERT(phantom_node_pair_list[i].first.is_valid(facade->GetNumberOfNodes()));
+            BOOST_ASSERT(phantom_node_pair_list[i].second.is_valid(facade->GetNumberOfNodes()));
+        }
+
+        auto snapped_phantoms = snapPhantomNodes(phantom_node_pair_list);
+
+        InternalRouteResult raw_route;
+        auto build_phantom_pairs = [&raw_route](const PhantomNode &first_node,
+                                                const PhantomNode &second_node)
+        {
+            raw_route.segment_end_coordinates.push_back(PhantomNodes{first_node, second_node});
+        };
+        osrm::for_each_pair(snapped_phantoms, build_phantom_pairs);
+
+        if (1 == raw_route.segment_end_coordinates.size())
+        {
+            if (route_parameters.alternate_route)
+            {
+                search_engine_ptr->alternative_path(raw_route.segment_end_coordinates.front(),
+                                                    raw_route);
+            }
+            else
+            {
+                search_engine_ptr->direct_shortest_path(raw_route.segment_end_coordinates,
+                                                        route_parameters.uturns, raw_route);
+            }
+        }
+        else
+        {
+            search_engine_ptr->shortest_path(raw_route.segment_end_coordinates,
+                                             route_parameters.uturns, raw_route);
+        }
+
+        bool no_route = INVALID_EDGE_WEIGHT == raw_route.shortest_path_length;
+
+        std::unique_ptr<BaseDescriptor<DataFacadeT>> descriptor;
+        switch (descriptor_table.get_id(route_parameters.output_format))
+        {
+        case 1:
+            descriptor = osrm::make_unique<GPXDescriptor<DataFacadeT>>(facade);
+            break;
+        // case 2:
+        //      descriptor = osrm::make_unique<GEOJSONDescriptor<DataFacadeT>>();
+        //      break;
+        default:
+            descriptor = osrm::make_unique<JSONDescriptor<DataFacadeT>>(facade);
+            break;
+        }
+
+        descriptor->SetConfig(route_parameters);
+        descriptor->Run(raw_route, json_result);
+
+        // we can only know this after the fact, different SCC ids still
+        // allow for connection in one direction.
+        if (no_route)
+        {
+            auto first_component_id = snapped_phantoms.front().component.id;
+            auto not_in_same_component =
+                std::any_of(snapped_phantoms.begin(), snapped_phantoms.end(),
+                            [first_component_id](const PhantomNode &node)
+                            {
+                                return node.component.id != first_component_id;
+                            });
+            if (not_in_same_component)
+            {
+                json_result.values["status_message"] = "Impossible route between points";
+                return Status::EmptyResult;
+            }
+        }
+        else
+        {
+            json_result.values["status_message"] = "Found route between points";
+        }
+
+        return Status::Ok;
+    }
+};
+
+#endif // VIA_ROUTE_HPP
diff --git a/prepare.cpp b/prepare.cpp
index 5de3204..aaf9376 100644
--- a/prepare.cpp
+++ b/prepare.cpp
@@ -33,71 +33,77 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <tbb/task_scheduler_init.h>
 
+#include <cstdlib>
 #include <exception>
 #include <ostream>
+#include <new>
 
-int main(int argc, char *argv[])
+int main(int argc, char *argv[]) try
 {
-    try
+    LogPolicy::GetInstance().Unmute();
+    ContractorConfig contractor_config;
+
+    const return_code result = ContractorOptions::ParseArguments(argc, argv, contractor_config);
+
+    if (return_code::fail == result)
+    {
+        return EXIT_FAILURE;
+    }
+
+    if (return_code::exit == result)
+    {
+        return EXIT_SUCCESS;
+    }
+
+    ContractorOptions::GenerateOutputFilesNames(contractor_config);
+
+    if (1 > contractor_config.requested_num_threads)
+    {
+        SimpleLogger().Write(logWARNING) << "Number of threads must be 1 or larger";
+        return EXIT_FAILURE;
+    }
+
+    const unsigned recommended_num_threads = tbb::task_scheduler_init::default_num_threads();
+
+    if (recommended_num_threads != contractor_config.requested_num_threads)
+    {
+        SimpleLogger().Write(logWARNING) << "The recommended number of threads is "
+                                         << recommended_num_threads
+                                         << "! This setting may have performance side-effects.";
+    }
+
+    if (!boost::filesystem::is_regular_file(contractor_config.osrm_input_path))
     {
-        LogPolicy::GetInstance().Unmute();
-        ContractorConfig contractor_config;
-
-        const return_code result = ContractorOptions::ParseArguments(argc, argv, contractor_config);
-
-        if (return_code::fail == result)
-        {
-            return 1;
-        }
-
-        if (return_code::exit == result)
-        {
-            return 0;
-        }
-
-        ContractorOptions::GenerateOutputFilesNames(contractor_config);
-
-        if (1 > contractor_config.requested_num_threads)
-        {
-            SimpleLogger().Write(logWARNING) << "Number of threads must be 1 or larger";
-            return 1;
-        }
-
-        const unsigned recommended_num_threads = tbb::task_scheduler_init::default_num_threads();
-
-        if (recommended_num_threads != contractor_config.requested_num_threads)
-        {
-            SimpleLogger().Write(logWARNING) << "The recommended number of threads is "
-                                             << recommended_num_threads
-                                             << "! This setting may have performance side-effects.";
-        }
-
-        if (!boost::filesystem::is_regular_file(contractor_config.osrm_input_path))
-        {
-            SimpleLogger().Write(logWARNING)
-                << "Input file " << contractor_config.osrm_input_path.string() << " not found!";
-            return 1;
-        }
-
-        if (!boost::filesystem::is_regular_file(contractor_config.profile_path))
-        {
-            SimpleLogger().Write(logWARNING) << "Profile " << contractor_config.profile_path.string()
-                                             << " not found!";
-            return 1;
-        }
-
-        SimpleLogger().Write() << "Input file: " << contractor_config.osrm_input_path.filename().string();
-        SimpleLogger().Write() << "Restrictions file: " << contractor_config.restrictions_path.filename().string();
-        SimpleLogger().Write() << "Profile: " << contractor_config.profile_path.filename().string();
-        SimpleLogger().Write() << "Threads: " << contractor_config.requested_num_threads;
-
-        tbb::task_scheduler_init init(contractor_config.requested_num_threads);
-
-        return Prepare(contractor_config).Run();
+        SimpleLogger().Write(logWARNING)
+            << "Input file " << contractor_config.osrm_input_path.string() << " not found!";
+        return EXIT_FAILURE;
     }
-    catch (const std::exception &e)
+
+    if (!boost::filesystem::is_regular_file(contractor_config.profile_path))
     {
-        SimpleLogger().Write(logWARNING) << "[exception] " << e.what();
-        return 1;
+        SimpleLogger().Write(logWARNING) << "Profile " << contractor_config.profile_path.string()
+                                         << " not found!";
+        return EXIT_FAILURE;
     }
+
+    SimpleLogger().Write() << "Input file: "
+                           << contractor_config.osrm_input_path.filename().string();
+    SimpleLogger().Write() << "Profile: " << contractor_config.profile_path.filename().string();
+    SimpleLogger().Write() << "Threads: " << contractor_config.requested_num_threads;
+
+    tbb::task_scheduler_init init(contractor_config.requested_num_threads);
+
+    return Prepare(contractor_config).Run();
+}
+catch (const std::bad_alloc &e)
+{
+    SimpleLogger().Write(logWARNING) << "[exception] " << e.what();
+    SimpleLogger().Write(logWARNING)
+        << "Please provide more memory or consider using a larger swapfile";
+    return EXIT_FAILURE;
+}
+catch (const std::exception &e)
+{
+    SimpleLogger().Write(logWARNING) << "[exception] " << e.what();
+    return EXIT_FAILURE;
 }
diff --git a/profiles/bicycle.lua b/profiles/bicycle.lua
index fddd7d4..1ebbffd 100644
--- a/profiles/bicycle.lua
+++ b/profiles/bicycle.lua
@@ -90,12 +90,13 @@ surface_speeds = {
   ["sand"] = 3
 }
 
+-- these need to be global because they are accesed externaly
 traffic_signal_penalty          = 2
 use_turn_restrictions           = false
+u_turn_penalty                  = 20
 
 local obey_oneway               = true
 local ignore_areas              = true
-local u_turn_penalty            = 20
 local turn_penalty              = 60
 local turn_bias                 = 1.4
 -- reduce the driving speed by 30% for unsafe roads
@@ -169,7 +170,7 @@ function way_function (way, result)
   local public_transport = way:get_value_by_key("public_transport")
   local bridge = way:get_value_by_key("bridge")
   if (not highway or highway == '') and
-  (not use_public_transport or not route or route == '') and
+  (not route or route == '') and
   (not use_public_transport or not railway or railway=='') and
   (not amenity or amenity=='') and
   (not man_made or man_made=='') and
diff --git a/profiles/car.lua b/profiles/car.lua
index 2f7f19d..959558f 100644
--- a/profiles/car.lua
+++ b/profiles/car.lua
@@ -128,6 +128,8 @@ maxspeed_table = {
   ["uk:motorway"] = (70*1609)/1000
 }
 
+-- these need to be global because they are accesed externaly
+u_turn_penalty                  = 20
 traffic_signal_penalty          = 2
 use_turn_restrictions           = true
 
@@ -138,7 +140,6 @@ local turn_bias                 = 1.2
 
 local obey_oneway               = true
 local ignore_areas              = true
-local u_turn_penalty            = 20
 
 local abs = math.abs
 local min = math.min
@@ -181,15 +182,6 @@ local function parse_maxspeed(source)
   return n
 end
 
--- FIXME Why was this commented out?
--- function turn_function (angle)
---   -- print ("called at angle " .. angle )
---   local index = math.abs(math.floor(angle/10+0.5))+1 -- +1 'coz LUA starts as idx 1
---   local penalty = turn_cost_table[index]
---   -- print ("index: " .. index .. ", bias: " .. penalty )
---   return penalty
--- end
-
 function node_function (node, result)
   -- parse access and barrier tags
   local access = find_access_tag(node, access_tags_hierachy)
@@ -388,8 +380,8 @@ function way_function (way, result)
   end
 
   -- Override speed settings if explicit forward/backward maxspeeds are given
-  local maxspeed_forward = parse_maxspeed(way:get_value_by_key( "maxspeed:forward"))
-  local maxspeed_backward = parse_maxspeed(way:get_value_by_key( "maxspeed:backward"))
+  local maxspeed_forward = parse_maxspeed(way:get_value_by_key("maxspeed:forward"))
+  local maxspeed_backward = parse_maxspeed(way:get_value_by_key("maxspeed:backward"))
   if maxspeed_forward and maxspeed_forward > 0 then
     if 0 ~= result.forward_mode and 0 ~= result.backward_mode then
       result.backward_speed = result.forward_speed
@@ -400,6 +392,29 @@ function way_function (way, result)
     result.backward_speed = maxspeed_backward
   end
 
+  -- Override speed settings if advisory forward/backward maxspeeds are given
+  local advisory_speed = parse_maxspeed(way:get_value_by_key("maxspeed:advisory"))
+  local advisory_forward = parse_maxspeed(way:get_value_by_key("maxspeed:advisory:forward"))
+  local advisory_backward = parse_maxspeed(way:get_value_by_key("maxspeed:advisory:backward"))
+  -- apply bi-directional advisory speed first
+  if advisory_speed and advisory_speed > 0 then
+    if 0 ~= result.forward_mode then
+      result.forward_speed = advisory_speed
+    end
+    if 0 ~= result.backward_mode then
+      result.backward_speed = advisory_speed
+    end
+  end
+  if advisory_forward and advisory_forward > 0 then
+    if 0 ~= result.forward_mode and 0 ~= result.backward_mode then
+      result.backward_speed = result.forward_speed
+    end
+    result.forward_speed = advisory_forward
+  end
+  if advisory_backward and advisory_backward > 0 then
+    result.backward_speed = advisory_backward
+  end
+
   local width = math.huge
   local lanes = math.huge
   if result.forward_speed > 0 or result.backward_speed > 0 then
@@ -434,6 +449,9 @@ function way_function (way, result)
     end
     result.backward_speed = math.min(penalized_speed, scaled_speed)
   end
+
+  -- only allow this road as start point if it not a ferry
+  result.is_startpoint = result.forward_mode == mode_normal or result.backward_mode == mode_normal
 end
 
 function turn_function (angle)
diff --git a/routed.cpp b/routed.cpp
index c8186f7..b4404c1 100644
--- a/routed.cpp
+++ b/routed.cpp
@@ -25,12 +25,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 */
 
-#include "library/osrm.hpp"
 #include "server/server.hpp"
-#include "util/git_sha.hpp"
+#include "util/version.hpp"
 #include "util/routed_options.hpp"
 #include "util/simple_logger.hpp"
 
+#include <osrm/osrm.hpp>
+#include <osrm/libosrm_config.hpp>
+
 #ifdef __linux__
 #include <sys/mman.h>
 #endif
@@ -43,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <future>
 #include <iostream>
 #include <thread>
+#include <new>
 
 #ifdef _WIN32
 boost::function0<void> console_ctrl_function;
@@ -63,123 +66,133 @@ BOOL WINAPI console_ctrl_handler(DWORD ctrl_type)
 }
 #endif
 
-int main(int argc, const char *argv[])
+int main(int argc, const char *argv[]) try
 {
-    try
+    LogPolicy::GetInstance().Unmute();
+
+    bool trial_run = false;
+    std::string ip_address;
+    int ip_port, requested_thread_num;
+
+    LibOSRMConfig lib_config;
+    const unsigned init_result = GenerateServerProgramOptions(
+        argc, argv, lib_config.server_paths, ip_address, ip_port, requested_thread_num,
+        lib_config.use_shared_memory, trial_run, lib_config.max_locations_trip, lib_config.max_locations_viaroute,
+        lib_config.max_locations_distance_table,
+        lib_config.max_locations_map_matching);
+    if (init_result == INIT_OK_DO_NOT_START_ENGINE)
     {
-        LogPolicy::GetInstance().Unmute();
-
-        bool trial_run = false;
-        std::string ip_address;
-        int ip_port, requested_thread_num;
-
-        libosrm_config lib_config;
-        // make the behaviour of routed backward compatible
-        lib_config.use_shared_memory = false;
+        return EXIT_SUCCESS;
+    }
+    if (init_result == INIT_FAILED)
+    {
+        return EXIT_FAILURE;
+    }
 
-        const unsigned init_result = GenerateServerProgramOptions(
-            argc, argv, lib_config.server_paths, ip_address, ip_port, requested_thread_num,
-            lib_config.use_shared_memory, trial_run, lib_config.max_locations_distance_table,
-            lib_config.max_locations_map_matching);
-        if (init_result == INIT_OK_DO_NOT_START_ENGINE)
-        {
-            return 0;
-        }
-        if (init_result == INIT_FAILED)
+#ifdef __linux__
+    struct MemoryLocker final
+    {
+        explicit MemoryLocker(bool shouldLock_) : shouldLock(shouldLock_)
         {
-            return 1;
+            if (shouldLock && -1 == mlockall(MCL_CURRENT | MCL_FUTURE))
+            {
+                couldLock = false;
+                SimpleLogger().Write(logWARNING) << "memory could not be locked to RAM";
+            }
         }
-
-#ifdef __linux__
-        const int lock_flags = MCL_CURRENT | MCL_FUTURE;
-        if (-1 == mlockall(lock_flags))
+        ~MemoryLocker()
         {
-            SimpleLogger().Write(logWARNING) << argv[0] << " could not be locked to RAM";
+            if (shouldLock && couldLock)
+                (void)munlockall();
         }
+        bool shouldLock = false, couldLock = true;
+    } memoryLocker(lib_config.use_shared_memory);
 #endif
-        SimpleLogger().Write() << "starting up engines, " << g_GIT_DESCRIPTION;
+    SimpleLogger().Write() << "starting up engines, " << OSRM_VERSION;
 
-        if (lib_config.use_shared_memory)
-        {
-            SimpleLogger().Write(logDEBUG) << "Loading from shared memory";
-        }
+    if (lib_config.use_shared_memory)
+    {
+        SimpleLogger().Write(logDEBUG) << "Loading from shared memory";
+    }
+
+    SimpleLogger().Write(logDEBUG) << "Threads:\t" << requested_thread_num;
+    SimpleLogger().Write(logDEBUG) << "IP address:\t" << ip_address;
+    SimpleLogger().Write(logDEBUG) << "IP port:\t" << ip_port;
 
-        SimpleLogger().Write(logDEBUG) << "Threads:\t" << requested_thread_num;
-        SimpleLogger().Write(logDEBUG) << "IP address:\t" << ip_address;
-        SimpleLogger().Write(logDEBUG) << "IP port:\t" << ip_port;
 #ifndef _WIN32
-        int sig = 0;
-        sigset_t new_mask;
-        sigset_t old_mask;
-        sigfillset(&new_mask);
-        pthread_sigmask(SIG_BLOCK, &new_mask, &old_mask);
+    int sig = 0;
+    sigset_t new_mask;
+    sigset_t old_mask;
+    sigfillset(&new_mask);
+    pthread_sigmask(SIG_BLOCK, &new_mask, &old_mask);
 #endif
 
-        OSRM osrm_lib(lib_config);
-        auto routing_server = Server::CreateServer(ip_address, ip_port, requested_thread_num);
+    OSRM osrm_lib(lib_config);
+    auto routing_server = Server::CreateServer(ip_address, ip_port, requested_thread_num);
 
-        routing_server->GetRequestHandlerPtr().RegisterRoutingMachine(&osrm_lib);
+    routing_server->GetRequestHandlerPtr().RegisterRoutingMachine(&osrm_lib);
 
-        if (trial_run)
-        {
-            SimpleLogger().Write() << "trial run, quitting after successful initialization";
-        }
-        else
-        {
-            std::packaged_task<int()> server_task([&]() -> int
-                                                  {
-                                                      routing_server->Run();
-                                                      return 0;
-                                                  });
-            auto future = server_task.get_future();
-            std::thread server_thread(std::move(server_task));
+    if (trial_run)
+    {
+        SimpleLogger().Write() << "trial run, quitting after successful initialization";
+    }
+    else
+    {
+        std::packaged_task<int()> server_task([&]() -> int
+                                              {
+                                                  routing_server->Run();
+                                                  return 0;
+                                              });
+        auto future = server_task.get_future();
+        std::thread server_thread(std::move(server_task));
 
 #ifndef _WIN32
-            sigset_t wait_mask;
-            pthread_sigmask(SIG_SETMASK, &old_mask, nullptr);
-            sigemptyset(&wait_mask);
-            sigaddset(&wait_mask, SIGINT);
-            sigaddset(&wait_mask, SIGQUIT);
-            sigaddset(&wait_mask, SIGTERM);
-            pthread_sigmask(SIG_BLOCK, &wait_mask, nullptr);
-            SimpleLogger().Write() << "running and waiting for requests";
-            sigwait(&wait_mask, &sig);
+        sigset_t wait_mask;
+        pthread_sigmask(SIG_SETMASK, &old_mask, nullptr);
+        sigemptyset(&wait_mask);
+        sigaddset(&wait_mask, SIGINT);
+        sigaddset(&wait_mask, SIGQUIT);
+        sigaddset(&wait_mask, SIGTERM);
+        pthread_sigmask(SIG_BLOCK, &wait_mask, nullptr);
+        SimpleLogger().Write() << "running and waiting for requests";
+        sigwait(&wait_mask, &sig);
 #else
-            // Set console control handler to allow server to be stopped.
-            console_ctrl_function = std::bind(&Server::Stop, routing_server);
-            SetConsoleCtrlHandler(console_ctrl_handler, TRUE);
-            SimpleLogger().Write() << "running and waiting for requests";
-            routing_server->Run();
+        // Set console control handler to allow server to be stopped.
+        console_ctrl_function = std::bind(&Server::Stop, routing_server);
+        SetConsoleCtrlHandler(console_ctrl_handler, TRUE);
+        SimpleLogger().Write() << "running and waiting for requests";
+        routing_server->Run();
 #endif
-            SimpleLogger().Write() << "initiating shutdown";
-            routing_server->Stop();
-            SimpleLogger().Write() << "stopping threads";
+        SimpleLogger().Write() << "initiating shutdown";
+        routing_server->Stop();
+        SimpleLogger().Write() << "stopping threads";
 
-            auto status = future.wait_for(std::chrono::seconds(2));
+        auto status = future.wait_for(std::chrono::seconds(2));
 
-            if (status == std::future_status::ready)
-            {
-                server_thread.join();
-            }
-            else
-            {
-                SimpleLogger().Write(logWARNING) << "Didn't exit within 2 seconds. Hard abort!";
-                server_task.reset(); // just kill it
-            }
+        if (status == std::future_status::ready)
+        {
+            server_thread.join();
+        }
+        else
+        {
+            SimpleLogger().Write(logWARNING) << "Didn't exit within 2 seconds. Hard abort!";
+            server_task.reset(); // just kill it
         }
-
-        SimpleLogger().Write() << "freeing objects";
-        routing_server.reset();
-        SimpleLogger().Write() << "shutdown completed";
-    }
-    catch (const std::exception &e)
-    {
-        SimpleLogger().Write(logWARNING) << "exception: " << e.what();
-        return 1;
     }
-#ifdef __linux__
-    munlockall();
-#endif
 
-    return 0;
+    SimpleLogger().Write() << "freeing objects";
+    routing_server.reset();
+    SimpleLogger().Write() << "shutdown completed";
+}
+catch (const std::bad_alloc &e)
+{
+    SimpleLogger().Write(logWARNING) << "[exception] " << e.what();
+    SimpleLogger().Write(logWARNING)
+        << "Please provide more memory or consider using a larger swapfile";
+    return EXIT_FAILURE;
+}
+catch (const std::exception &e)
+{
+    SimpleLogger().Write(logWARNING) << "[exception] " << e.what();
+    return EXIT_FAILURE;
 }
diff --git a/routing_algorithms/alternative_path.hpp b/routing_algorithms/alternative_path.hpp
index 327bb32..59b772e 100644
--- a/routing_algorithms/alternative_path.hpp
+++ b/routing_algorithms/alternative_path.hpp
@@ -317,7 +317,7 @@ class AlternativeRouting final
 
             super::UnpackPath(
                 // -- packed input
-                packed_shortest_path,
+                packed_shortest_path.begin(), packed_shortest_path.end(),
                 // -- start of route
                 phantom_node_pair,
                 // -- unpacked output
@@ -338,8 +338,8 @@ class AlternativeRouting final
                 (packed_alternate_path.back() != phantom_node_pair.target_phantom.forward_node_id));
 
             // unpack the alternate path
-            super::UnpackPath(packed_alternate_path, phantom_node_pair,
-                              raw_route_data.unpacked_alternative);
+            super::UnpackPath(packed_alternate_path.begin(), packed_alternate_path.end(),
+                              phantom_node_pair, raw_route_data.unpacked_alternative);
 
             raw_route_data.alternative_path_length = length_of_via_path;
         }
@@ -401,8 +401,8 @@ class AlternativeRouting final
         // compute path <s,..,v> by reusing forward search from s
         while (!new_reverse_heap.Empty())
         {
-            super::RoutingStep(new_reverse_heap, existing_forward_heap, &s_v_middle,
-                               &upper_bound_s_v_path_length, min_edge_offset, false);
+            super::RoutingStep(new_reverse_heap, existing_forward_heap, s_v_middle,
+                               upper_bound_s_v_path_length, min_edge_offset, false);
         }
         // compute path <v,..,t> by reusing backward search from node t
         NodeID v_t_middle = SPECIAL_NODEID;
@@ -410,8 +410,8 @@ class AlternativeRouting final
         new_forward_heap.Insert(via_node, 0, via_node);
         while (!new_forward_heap.Empty())
         {
-            super::RoutingStep(new_forward_heap, existing_reverse_heap, &v_t_middle,
-                               &upper_bound_of_v_t_path_length, min_edge_offset, true);
+            super::RoutingStep(new_forward_heap, existing_reverse_heap, v_t_middle,
+                               upper_bound_of_v_t_path_length, min_edge_offset, true);
         }
         *real_length_of_via_path = upper_bound_s_v_path_length + upper_bound_of_v_t_path_length;
 
@@ -429,7 +429,7 @@ class AlternativeRouting final
         // partial unpacking, compute sharing
         // First partially unpack s-->v until paths deviate, note length of common path.
         const int64_t s_v_min_path_size =
-            std::min(packed_s_v_path.size(), packed_shortest_path.size()) - 1;
+            static_cast<int64_t>(std::min(packed_s_v_path.size(), packed_shortest_path.size())) - 1;
         for (const int64_t current_node : osrm::irange<int64_t>(0, s_v_min_path_size))
         {
             if (packed_s_v_path[current_node] == packed_shortest_path[current_node] &&
@@ -455,13 +455,14 @@ class AlternativeRouting final
         }
         // traverse partially unpacked edge and note common prefix
         const int64_t packed_path_length =
-            std::min(partially_unpacked_via_path.size(), partially_unpacked_shortest_path.size()) -
+            static_cast<int64_t>(std::min(partially_unpacked_via_path.size(),
+                                          partially_unpacked_shortest_path.size())) -
             1;
         for (int64_t current_node = 0; (current_node < packed_path_length) &&
-                                           (partially_unpacked_via_path[current_node] ==
-                                                partially_unpacked_shortest_path[current_node] &&
-                                            partially_unpacked_via_path[current_node + 1] ==
-                                                partially_unpacked_shortest_path[current_node + 1]);
+                                       (partially_unpacked_via_path[current_node] ==
+                                            partially_unpacked_shortest_path[current_node] &&
+                                        partially_unpacked_via_path[current_node + 1] ==
+                                            partially_unpacked_shortest_path[current_node + 1]);
              ++current_node)
         {
             EdgeID selected_edge =
@@ -471,8 +472,8 @@ class AlternativeRouting final
         }
 
         // Second, partially unpack v-->t in reverse order until paths deviate and note lengths
-        int64_t via_path_index = packed_v_t_path.size() - 1;
-        int64_t shortest_path_index = packed_shortest_path.size() - 1;
+        int64_t via_path_index = static_cast<int64_t>(packed_v_t_path.size()) - 1;
+        int64_t shortest_path_index = static_cast<int64_t>(packed_shortest_path.size()) - 1;
         for (; via_path_index > 0 && shortest_path_index > 0;
              --via_path_index, --shortest_path_index)
         {
@@ -670,8 +671,8 @@ class AlternativeRouting final
         new_reverse_heap.Insert(candidate.node, 0, candidate.node);
         while (new_reverse_heap.Size() > 0)
         {
-            super::RoutingStep(new_reverse_heap, existing_forward_heap, s_v_middle,
-                               &upper_bound_s_v_path_length, min_edge_offset, false);
+            super::RoutingStep(new_reverse_heap, existing_forward_heap, *s_v_middle,
+                               upper_bound_s_v_path_length, min_edge_offset, false);
         }
 
         if (INVALID_EDGE_WEIGHT == upper_bound_s_v_path_length)
@@ -685,8 +686,8 @@ class AlternativeRouting final
         new_forward_heap.Insert(candidate.node, 0, candidate.node);
         while (new_forward_heap.Size() > 0)
         {
-            super::RoutingStep(new_forward_heap, existing_reverse_heap, v_t_middle,
-                               &upper_bound_of_v_t_path_length, min_edge_offset, true);
+            super::RoutingStep(new_forward_heap, existing_reverse_heap, *v_t_middle,
+                               upper_bound_of_v_t_path_length, min_edge_offset, true);
         }
 
         if (INVALID_EDGE_WEIGHT == upper_bound_of_v_t_path_length)
@@ -853,12 +854,12 @@ class AlternativeRouting final
         {
             if (!forward_heap3.Empty())
             {
-                super::RoutingStep(forward_heap3, reverse_heap3, &middle, &upper_bound,
+                super::RoutingStep(forward_heap3, reverse_heap3, middle, upper_bound,
                                    min_edge_offset, true);
             }
             if (!reverse_heap3.Empty())
             {
-                super::RoutingStep(reverse_heap3, forward_heap3, &middle, &upper_bound,
+                super::RoutingStep(reverse_heap3, forward_heap3, middle, upper_bound,
                                    min_edge_offset, false);
             }
         }
diff --git a/routing_algorithms/direct_shortest_path.hpp b/routing_algorithms/direct_shortest_path.hpp
index 2abee98..2237d68 100644
--- a/routing_algorithms/direct_shortest_path.hpp
+++ b/routing_algorithms/direct_shortest_path.hpp
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define DIRECT_SHORTEST_PATH_HPP
 
 #include <boost/assert.hpp>
+#include <iterator>
 
 #include "routing_base.hpp"
 #include "../data_structures/search_engine_data.hpp"
@@ -62,143 +63,71 @@ class DirectShortestPathRouting final
                     const std::vector<bool> &uturn_indicators,
                     InternalRouteResult &raw_route_data) const
     {
-        engine_working_data.InitializeOrClearFirstThreadLocalStorage(
-            super::facade->GetNumberOfNodes());
-        engine_working_data.InitializeOrClearSecondThreadLocalStorage(
-            super::facade->GetNumberOfNodes());
-
-        QueryHeap &forward_heap = *(engine_working_data.forward_heap_1);
-        QueryHeap &reverse_heap = *(engine_working_data.reverse_heap_1);
-
-        QueryHeap &forward_core_heap = *(engine_working_data.forward_heap_2);
-        QueryHeap &reverse_core_heap = *(engine_working_data.reverse_heap_2);
+        (void)uturn_indicators; // unused
 
         // Get distance to next pair of target nodes.
         BOOST_ASSERT_MSG(1 == phantom_nodes_vector.size(),
                                          "Direct Shortest Path Query only accepts a single source and target pair. Multiple ones have been specified.");
-
         const auto& phantom_node_pair = phantom_nodes_vector.front();
+        const auto& source_phantom = phantom_node_pair.source_phantom;
+        const auto& target_phantom = phantom_node_pair.target_phantom;
 
+        engine_working_data.InitializeOrClearFirstThreadLocalStorage(
+            super::facade->GetNumberOfNodes());
+        QueryHeap &forward_heap = *(engine_working_data.forward_heap_1);
+        QueryHeap &reverse_heap = *(engine_working_data.reverse_heap_1);
         forward_heap.Clear();
         reverse_heap.Clear();
-        int distance = INVALID_EDGE_WEIGHT;
-        NodeID middle = SPECIAL_NODEID;
 
-        const EdgeWeight min_edge_offset =
-            std::min(-phantom_node_pair.source_phantom.GetForwardWeightPlusOffset(),
-                     -phantom_node_pair.source_phantom.GetReverseWeightPlusOffset());
+        BOOST_ASSERT(source_phantom.is_valid());
+        BOOST_ASSERT(target_phantom.is_valid());
 
-        // insert new starting nodes into forward heap, adjusted by previous distances.
-        if (phantom_node_pair.source_phantom.forward_node_id != SPECIAL_NODEID)
+        if (source_phantom.forward_node_id != SPECIAL_NODEID)
         {
-            forward_heap.Insert(
-                phantom_node_pair.source_phantom.forward_node_id,
-                -phantom_node_pair.source_phantom.GetForwardWeightPlusOffset(),
-                phantom_node_pair.source_phantom.forward_node_id);
+            forward_heap.Insert(source_phantom.forward_node_id,
+                                -source_phantom.GetForwardWeightPlusOffset(),
+                                source_phantom.forward_node_id);
         }
-        if ( phantom_node_pair.source_phantom.reverse_node_id != SPECIAL_NODEID)
+        if (source_phantom.reverse_node_id != SPECIAL_NODEID)
         {
-            forward_heap.Insert(
-                phantom_node_pair.source_phantom.reverse_node_id,
-                -phantom_node_pair.source_phantom.GetReverseWeightPlusOffset(),
-                phantom_node_pair.source_phantom.reverse_node_id);
+            forward_heap.Insert(source_phantom.reverse_node_id,
+                                -source_phantom.GetReverseWeightPlusOffset(),
+                                source_phantom.reverse_node_id);
         }
 
-        // insert new backward nodes into backward heap, unadjusted.
-        if (phantom_node_pair.target_phantom.forward_node_id != SPECIAL_NODEID)
+        if (target_phantom.forward_node_id != SPECIAL_NODEID)
         {
-            reverse_heap.Insert(phantom_node_pair.target_phantom.forward_node_id,
-                                 phantom_node_pair.target_phantom.GetForwardWeightPlusOffset(),
-                                 phantom_node_pair.target_phantom.forward_node_id);
+            reverse_heap.Insert(target_phantom.forward_node_id,
+                                target_phantom.GetForwardWeightPlusOffset(),
+                                target_phantom.forward_node_id);
         }
 
-        if (phantom_node_pair.target_phantom.reverse_node_id != SPECIAL_NODEID)
+        if (target_phantom.reverse_node_id != SPECIAL_NODEID)
         {
-            reverse_heap.Insert(phantom_node_pair.target_phantom.reverse_node_id,
-                                 phantom_node_pair.target_phantom.GetReverseWeightPlusOffset(),
-                                 phantom_node_pair.target_phantom.reverse_node_id);
+            reverse_heap.Insert(target_phantom.reverse_node_id,
+                                target_phantom.GetReverseWeightPlusOffset(),
+                                target_phantom.reverse_node_id);
         }
 
-        std::vector<std::pair<NodeID, EdgeWeight>> forward_entry_points;
-        std::vector<std::pair<NodeID, EdgeWeight>> reverse_entry_points;
+        int distance = INVALID_EDGE_WEIGHT;
+        std::vector<NodeID> packed_leg;
 
-        // run two-Target Dijkstra routing step.
-        while (0 < (forward_heap.Size() + reverse_heap.Size()) )
+        if (super::facade->GetCoreSize() > 0)
         {
-            if (!forward_heap.Empty())
-            {
-                if (super::facade->IsCoreNode(forward_heap.Min()))
-                {
-                    const NodeID node = forward_heap.DeleteMin();
-                    const int key = forward_heap.GetKey(node);
-                    forward_entry_points.emplace_back(node, key);
-                }
-                else
-                {
-                    super::RoutingStep(forward_heap, reverse_heap, &middle, &distance,
-                                       min_edge_offset, true);
-                }
-            }
-            if (!reverse_heap.Empty())
-            {
-                if (super::facade->IsCoreNode(reverse_heap.Min()))
-                {
-                    const NodeID node = reverse_heap.DeleteMin();
-                    const int key = reverse_heap.GetKey(node);
-                    reverse_entry_points.emplace_back(node, key);
-                }
-                else
-                {
-                    super::RoutingStep(reverse_heap, forward_heap, &middle, &distance,
-                                       min_edge_offset, false);
-                }
-            }
-        }
+            engine_working_data.InitializeOrClearSecondThreadLocalStorage(
+                super::facade->GetNumberOfNodes());
+            QueryHeap &forward_core_heap = *(engine_working_data.forward_heap_2);
+            QueryHeap &reverse_core_heap = *(engine_working_data.reverse_heap_2);
+            forward_core_heap.Clear();
+            reverse_core_heap.Clear();
 
-        // TODO check if unordered_set might be faster
-        // sort by id and increasing by distance
-        auto entry_point_comparator = [](const std::pair<NodeID, EdgeWeight>& lhs, const std::pair<NodeID, EdgeWeight>& rhs)
-            {
-                return lhs.first < rhs.first || (lhs.first == rhs.first && lhs.second < rhs.second);
-            };
-        std::sort(forward_entry_points.begin(), forward_entry_points.end(), entry_point_comparator);
-        std::sort(reverse_entry_points.begin(), reverse_entry_points.end(), entry_point_comparator);
-
-        NodeID last_id = SPECIAL_NODEID;
-        for (const auto p : forward_entry_points)
-        {
-            if (p.first == last_id)
-            {
-                continue;
-            }
-            forward_core_heap.Insert(p.first, p.second, p.first);
-            last_id = p.first;
-        }
-        last_id = SPECIAL_NODEID;
-        for (const auto p : reverse_entry_points)
-        {
-            if (p.first == last_id)
-            {
-                continue;
-            }
-            reverse_core_heap.Insert(p.first, p.second, p.first);
-            last_id = p.first;
-        }
 
-        // run two-target Dijkstra routing step on core with termination criterion
-        while (0 < (forward_core_heap.Size() + reverse_core_heap.Size()) &&
-               distance > (forward_core_heap.MinKey() + reverse_core_heap.MinKey()))
+            super::SearchWithCore(forward_heap, reverse_heap, forward_core_heap, reverse_core_heap,
+                                  distance, packed_leg);
+        }
+        else
         {
-            if (!forward_core_heap.Empty())
-            {
-                super::RoutingStep(forward_core_heap, reverse_core_heap, &middle, &distance,
-                                   min_edge_offset, true);
-            }
-            if (!reverse_core_heap.Empty())
-            {
-                super::RoutingStep(reverse_core_heap, forward_core_heap, &middle, &distance,
-                                   min_edge_offset, false);
-            }
+            super::Search(forward_heap, reverse_heap, distance, packed_leg);
         }
 
         // No path found for both target nodes?
@@ -209,39 +138,17 @@ class DirectShortestPathRouting final
             return;
         }
 
-        // Was a paths over one of the forward/reverse nodes not found?
-        BOOST_ASSERT_MSG((SPECIAL_NODEID == middle || INVALID_EDGE_WEIGHT != distance),
-                         "no path found");
-
-        std::vector<NodeID> packed_leg;
-        // we need to unpack sub path from core heaps
-        if(super::facade->IsCoreNode(middle))
-        {
-            std::vector<NodeID> packed_core_leg;
-            super::RetrievePackedPathFromHeap(forward_core_heap, reverse_core_heap, middle, packed_core_leg);
-            BOOST_ASSERT(packed_core_leg.size() > 0);
-            super::RetrievePackedPathFromSingleHeap(forward_heap, packed_core_leg.front(), packed_leg);
-            std::reverse(packed_leg.begin(), packed_leg.end());
-            packed_leg.insert(packed_leg.end(), packed_core_leg.begin(), packed_core_leg.end());
-            super::RetrievePackedPathFromSingleHeap(reverse_heap, packed_core_leg.back(), packed_leg);
-        }
-        else
-        {
-            super::RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle, packed_leg);
-        }
-
-
         BOOST_ASSERT_MSG(!packed_leg.empty(), "packed path empty");
 
+        raw_route_data.shortest_path_length = distance;
         raw_route_data.unpacked_path_segments.resize(1);
         raw_route_data.source_traversed_in_reverse.push_back(
             (packed_leg.front() != phantom_node_pair.source_phantom.forward_node_id));
         raw_route_data.target_traversed_in_reverse.push_back(
             (packed_leg.back() != phantom_node_pair.target_phantom.forward_node_id));
 
-        super::UnpackPath(packed_leg, phantom_node_pair, raw_route_data.unpacked_path_segments.front());
+        super::UnpackPath(packed_leg.begin(), packed_leg.end(), phantom_node_pair, raw_route_data.unpacked_path_segments.front());
 
-        raw_route_data.shortest_path_length = distance;
     }
 };
 
diff --git a/routing_algorithms/many_to_many.hpp b/routing_algorithms/many_to_many.hpp
index 2388804..c5dfb7c 100644
--- a/routing_algorithms/many_to_many.hpp
+++ b/routing_algorithms/many_to_many.hpp
@@ -67,11 +67,12 @@ class ManyToManyRouting final
     ~ManyToManyRouting() {}
 
     std::shared_ptr<std::vector<EdgeWeight>>
-    operator()(const PhantomNodeArray &phantom_nodes_array) const
+    operator()(const std::vector<PhantomNode> &phantom_sources_array, const std::vector<PhantomNode> &phantom_targets_array) const
     {
-        const auto number_of_locations = phantom_nodes_array.size();
+        const auto number_of_sources = phantom_sources_array.size();
+        const auto number_of_targets = phantom_targets_array.size();
         std::shared_ptr<std::vector<EdgeWeight>> result_table =
-            std::make_shared<std::vector<EdgeWeight>>(number_of_locations * number_of_locations,
+            std::make_shared<std::vector<EdgeWeight>>(number_of_targets * number_of_sources,
                                                       std::numeric_limits<EdgeWeight>::max());
 
         engine_working_data.InitializeOrClearFirstThreadLocalStorage(
@@ -82,25 +83,22 @@ class ManyToManyRouting final
         SearchSpaceWithBuckets search_space_with_buckets;
 
         unsigned target_id = 0;
-        for (const std::vector<PhantomNode> &phantom_node_vector : phantom_nodes_array)
+        for (const auto &phantom : phantom_targets_array)
         {
             query_heap.Clear();
             // insert target(s) at distance 0
 
-            for (const PhantomNode &phantom_node : phantom_node_vector)
+            if (SPECIAL_NODEID != phantom.forward_node_id)
             {
-                if (SPECIAL_NODEID != phantom_node.forward_node_id)
-                {
-                    query_heap.Insert(phantom_node.forward_node_id,
-                                      phantom_node.GetForwardWeightPlusOffset(),
-                                      phantom_node.forward_node_id);
-                }
-                if (SPECIAL_NODEID != phantom_node.reverse_node_id)
-                {
-                    query_heap.Insert(phantom_node.reverse_node_id,
-                                      phantom_node.GetReverseWeightPlusOffset(),
-                                      phantom_node.reverse_node_id);
-                }
+                query_heap.Insert(phantom.forward_node_id,
+                                  phantom.GetForwardWeightPlusOffset(),
+                                  phantom.forward_node_id);
+            }
+            if (SPECIAL_NODEID != phantom.reverse_node_id)
+            {
+                query_heap.Insert(phantom.reverse_node_id,
+                                  phantom.GetReverseWeightPlusOffset(),
+                                  phantom.reverse_node_id);
             }
 
             // explore search space
@@ -113,41 +111,39 @@ class ManyToManyRouting final
 
         // for each source do forward search
         unsigned source_id = 0;
-        for (const std::vector<PhantomNode> &phantom_node_vector : phantom_nodes_array)
+        for (const auto &phantom : phantom_sources_array)
         {
             query_heap.Clear();
-            for (const PhantomNode &phantom_node : phantom_node_vector)
+            // insert target(s) at distance 0
+
+            if (SPECIAL_NODEID != phantom.forward_node_id)
             {
-                // insert sources at distance 0
-                if (SPECIAL_NODEID != phantom_node.forward_node_id)
-                {
-                    query_heap.Insert(phantom_node.forward_node_id,
-                                      -phantom_node.GetForwardWeightPlusOffset(),
-                                      phantom_node.forward_node_id);
-                }
-                if (SPECIAL_NODEID != phantom_node.reverse_node_id)
-                {
-                    query_heap.Insert(phantom_node.reverse_node_id,
-                                      -phantom_node.GetReverseWeightPlusOffset(),
-                                      phantom_node.reverse_node_id);
-                }
+                query_heap.Insert(phantom.forward_node_id,
+                                 -phantom.GetForwardWeightPlusOffset(),
+                                  phantom.forward_node_id);
+            }
+            if (SPECIAL_NODEID != phantom.reverse_node_id)
+            {
+                query_heap.Insert(phantom.reverse_node_id,
+                                 -phantom.GetReverseWeightPlusOffset(),
+                                  phantom.reverse_node_id);
             }
 
             // explore search space
             while (!query_heap.Empty())
             {
-                ForwardRoutingStep(source_id, number_of_locations, query_heap,
+                ForwardRoutingStep(source_id, number_of_targets, query_heap,
                                    search_space_with_buckets, result_table);
             }
 
             ++source_id;
         }
-        BOOST_ASSERT(source_id == target_id);
+        // BOOST_ASSERT(source_id == target_id);
         return result_table;
     }
 
     void ForwardRoutingStep(const unsigned source_id,
-                            const unsigned number_of_locations,
+                            const unsigned number_of_targets,
                             QueryHeap &query_heap,
                             const SearchSpaceWithBuckets &search_space_with_buckets,
                             std::shared_ptr<std::vector<EdgeWeight>> result_table) const
@@ -167,12 +163,12 @@ class ManyToManyRouting final
                 const unsigned target_id = current_bucket.target_id;
                 const int target_distance = current_bucket.distance;
                 const EdgeWeight current_distance =
-                    (*result_table)[source_id * number_of_locations + target_id];
+                    (*result_table)[source_id * number_of_targets + target_id];
                 // check if new distance is better
                 const EdgeWeight new_distance = source_distance + target_distance;
                 if (new_distance >= 0 && new_distance < current_distance)
                 {
-                    (*result_table)[source_id * number_of_locations + target_id] =
+                    (*result_table)[source_id * number_of_targets + target_id] =
                         (source_distance + target_distance);
                 }
             }
diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp
index 58b0ad1..430682d 100644
--- a/routing_algorithms/map_matching.hpp
+++ b/routing_algorithms/map_matching.hpp
@@ -35,11 +35,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "../util/json_logger.hpp"
 #include "../util/matching_debug_info.hpp"
 
-#include <variant/variant.hpp>
+#include <cstddef>
 
 #include <algorithm>
+#include <deque>
 #include <iomanip>
 #include <numeric>
+#include <utility>
+#include <vector>
 
 namespace osrm
 {
@@ -54,7 +57,7 @@ struct SubMatching
     double confidence;
 };
 
-using CandidateList = std::vector<std::pair<PhantomNode, double>>;
+using CandidateList = std::vector<PhantomNodeWithDistance>;
 using CandidateLists = std::vector<CandidateList>;
 using HMM = HiddenMarkovModel<CandidateLists>;
 using SubMatchingList = std::vector<SubMatching>;
@@ -103,20 +106,23 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
                     osrm::matching::SubMatchingList &sub_matchings) const
     {
         BOOST_ASSERT(candidates_list.size() == trace_coordinates.size());
+        BOOST_ASSERT(candidates_list.size() > 1);
+
+        const bool use_timestamps = trace_timestamps.size() > 1;
 
         const auto median_sample_time = [&]() {
-            if (trace_timestamps.size() > 1)
+            if (use_timestamps)
             {
-                return GetMedianSampleTime(trace_timestamps);
+                return std::max(1u, GetMedianSampleTime(trace_timestamps));
             }
             else
             {
-                return 0u;
+                return 1u;
             }
         }();
         const auto max_broken_time = median_sample_time * osrm::matching::MAX_BROKEN_STATES;
         const auto max_distance_delta = [&]() {
-            if (trace_timestamps.size() > 1)
+            if (use_timestamps)
             {
                 return median_sample_time * osrm::matching::MAX_SPEED;
             }
@@ -158,7 +164,7 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
             bool trace_split = prev_unbroken_timestamps.empty();
 
             // use temporal information if available to determine a split
-            if (!trace_timestamps.empty())
+            if (use_timestamps)
             {
                 trace_split =
                     trace_split ||
@@ -214,7 +220,7 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
             const auto &current_timestamps_list = candidates_list[t];
             const auto &current_coordinate = trace_coordinates[t];
 
-            const auto great_circle_distance = coordinate_calculation::great_circle_distance(prev_coordinate, current_coordinate);
+            const auto haversine_distance = coordinate_calculation::haversine_distance(prev_coordinate, current_coordinate);
 
             // compute d_t for this timestamp and the next one
             for (const auto s : osrm::irange<std::size_t>(0u, prev_viterbi.size()))
@@ -229,7 +235,7 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
                     // how likely is candidate s_prime at time t to be emitted?
                     // FIXME this can be pre-computed
                     const double emission_pr =
-                        emission_log_probability(candidates_list[t][s_prime].second);
+                        emission_log_probability(candidates_list[t][s_prime].distance);
                     double new_value = prev_viterbi[s] + emission_pr;
                     if (current_viterbi[s_prime] > new_value)
                     {
@@ -241,10 +247,10 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
 
                     // get distance diff between loc1/2 and locs/s_prime
                     const auto network_distance = super::get_network_distance(
-                        forward_heap, reverse_heap, prev_unbroken_timestamps_list[s].first,
-                        current_timestamps_list[s_prime].first);
+                        forward_heap, reverse_heap, prev_unbroken_timestamps_list[s].phantom_node,
+                        current_timestamps_list[s_prime].phantom_node);
 
-                    const auto d_t = std::abs(network_distance - great_circle_distance);
+                    const auto d_t = std::abs(network_distance - haversine_distance);
 
                     // very low probability transition -> prune
                     if (d_t >= max_distance_delta)
@@ -257,7 +263,7 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
 
                     matching_debug.add_transition_info(prev_unbroken_timestamp, t, s, s_prime,
                                                        prev_viterbi[s], emission_pr, transition_pr,
-                                                       network_distance, great_circle_distance);
+                                                       network_distance, haversine_distance);
 
                     if (new_value > current_viterbi[s_prime])
                     {
@@ -362,7 +368,7 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
                 const auto location_index = reconstructed_indices[i].second;
 
                 matching.indices[i] = timestamp_index;
-                matching.nodes[i] = candidates_list[timestamp_index][location_index].first;
+                matching.nodes[i] = candidates_list[timestamp_index][location_index].phantom_node;
                 matching.length += model.path_lengths[timestamp_index][location_index];
 
                 matching_debug.add_chosen(timestamp_index, location_index);
diff --git a/routing_algorithms/routing_base.hpp b/routing_algorithms/routing_base.hpp
index d41250a..ecbc07e 100644
--- a/routing_algorithms/routing_base.hpp
+++ b/routing_algorithms/routing_base.hpp
@@ -32,7 +32,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "../data_structures/internal_route_result.hpp"
 #include "../data_structures/search_engine_data.hpp"
 #include "../data_structures/turn_instructions.hpp"
-// #include "../util/simple_logger.hpp"
 
 #include <boost/assert.hpp>
 
@@ -59,62 +58,86 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
     explicit BasicRoutingInterface(DataFacadeT *facade) : facade(facade) {}
     ~BasicRoutingInterface() {}
 
+    // min_edge_offset is needed in case we use multiple
+    // nodes as start/target nodes with different (even negative) offsets.
+    // In that case the termination criterion is not correct
+    // anymore.
+    //
+    // Example:
+    // forward heap: a(-100), b(0),
+    // reverse heap: c(0), d(100)
+    //
+    // a --- d
+    //   \ /
+    //   / \
+    // b --- c
+    //
+    // This is equivalent to running a bi-directional Dijkstra on the following graph:
+    //
+    //     a --- d
+    //    /  \ /  \
+    //   y    x    z
+    //    \  / \  /
+    //     b --- c
+    //
+    // The graph is constructed by inserting nodes y and z that are connected to the initial nodes
+    // using edges (y, a) with weight -100, (y, b) with weight 0 and,
+    // (d, z) with weight 100, (c, z) with weight 0 corresponding.
+    // Since we are dealing with a graph that contains _negative_ edges,
+    // we need to add an offset to the termination criterion.
     void RoutingStep(SearchEngineData::QueryHeap &forward_heap,
                      SearchEngineData::QueryHeap &reverse_heap,
-                     NodeID *middle_node_id,
-                     int *upper_bound,
-                     const int min_edge_offset,
-                     const bool forward_direction) const
+                     NodeID &middle_node_id,
+                     int &upper_bound,
+                     int min_edge_offset,
+                     const bool forward_direction,
+                     const bool stalling = true) const
     {
         const NodeID node = forward_heap.DeleteMin();
         const int distance = forward_heap.GetKey(node);
 
-        // const NodeID parentnode = forward_heap.GetData(node).parent;
-        // SimpleLogger().Write() << (forward_direction ? "[fwd] " : "[rev] ") << "settled edge ("
-        // << parentnode << "," << node << "), dist: " << distance;
-
         if (reverse_heap.WasInserted(node))
         {
             const int new_distance = reverse_heap.GetKey(node) + distance;
-            if (new_distance < *upper_bound)
+            if (new_distance < upper_bound)
             {
                 if (new_distance >= 0)
                 {
-                    *middle_node_id = node;
-                    *upper_bound = new_distance;
-                    //     SimpleLogger().Write() << "accepted middle node " << node << " at
-                    //     distance " << new_distance;
-                    // } else {
-                    //     SimpleLogger().Write() << "discared middle node " << node << " at
-                    //     distance " << new_distance;
+                    middle_node_id = node;
+                    upper_bound = new_distance;
                 }
             }
         }
 
-        if (distance + min_edge_offset > *upper_bound)
+        // make sure we don't terminate too early if we initialize the distance
+        // for the nodes in the forward heap with the forward/reverse offset
+        BOOST_ASSERT(min_edge_offset <= 0);
+        if (distance + min_edge_offset > upper_bound)
         {
-            // SimpleLogger().Write() << "min_edge_offset: " << min_edge_offset;
             forward_heap.DeleteAll();
             return;
         }
 
         // Stalling
-        for (const auto edge : facade->GetAdjacentEdgeRange(node))
+        if (stalling)
         {
-            const EdgeData &data = facade->GetEdgeData(edge);
-            const bool reverse_flag = ((!forward_direction) ? data.forward : data.backward);
-            if (reverse_flag)
+            for (const auto edge : facade->GetAdjacentEdgeRange(node))
             {
-                const NodeID to = facade->GetTarget(edge);
-                const int edge_weight = data.distance;
+                const EdgeData &data = facade->GetEdgeData(edge);
+                const bool reverse_flag = ((!forward_direction) ? data.forward : data.backward);
+                if (reverse_flag)
+                {
+                    const NodeID to = facade->GetTarget(edge);
+                    const int edge_weight = data.distance;
 
-                BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
+                    BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
 
-                if (forward_heap.WasInserted(to))
-                {
-                    if (forward_heap.GetKey(to) + edge_weight < distance)
+                    if (forward_heap.WasInserted(to))
                     {
-                        return;
+                        if (forward_heap.GetKey(to) + edge_weight < distance)
+                        {
+                            return;
+                        }
                     }
                 }
             }
@@ -149,34 +172,33 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
         }
     }
 
-    void UnpackPath(const std::vector<NodeID> &packed_path,
+    template <typename RandomIter>
+    void UnpackPath(RandomIter packed_path_begin,
+                    RandomIter packed_path_end,
                     const PhantomNodes &phantom_node_pair,
                     std::vector<PathData> &unpacked_path) const
     {
         const bool start_traversed_in_reverse =
-            (packed_path.front() != phantom_node_pair.source_phantom.forward_node_id);
+            (*packed_path_begin != phantom_node_pair.source_phantom.forward_node_id);
         const bool target_traversed_in_reverse =
-            (packed_path.back() != phantom_node_pair.target_phantom.forward_node_id);
+            (*std::prev(packed_path_end) != phantom_node_pair.target_phantom.forward_node_id);
 
-        const unsigned packed_path_size = static_cast<unsigned>(packed_path.size());
+        BOOST_ASSERT(std::distance(packed_path_begin, packed_path_end) > 0);
         std::stack<std::pair<NodeID, NodeID>> recursion_stack;
 
         // We have to push the path in reverse order onto the stack because it's LIFO.
-        for (unsigned i = packed_path_size - 1; i > 0; --i)
+        for (auto current = std::prev(packed_path_end); current != packed_path_begin;
+             current = std::prev(current))
         {
-            recursion_stack.emplace(packed_path[i - 1], packed_path[i]);
+            recursion_stack.emplace(*std::prev(current), *current);
         }
 
         std::pair<NodeID, NodeID> edge;
         while (!recursion_stack.empty())
         {
-            /*
-            Graphical representation of variables:
-
-            edge.first         edge.second
-                *------------------>*
-                       edge_id
-            */
+            // edge.first         edge.second
+            //     *------------------>*
+            //            edge_id
             edge = recursion_stack.top();
             recursion_stack.pop();
 
@@ -195,13 +217,9 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
                 }
             }
 
-            /*
-                Graphical representation of variables:
-
-                edge.first         edge.second
-                    *<------------------*
-                           edge_id
-            */
+            // edge.first         edge.second
+            //     *<------------------*
+            //            edge_id
             if (SPECIAL_EDGEID == smaller_edge_id)
             {
                 for (const auto edge_id : facade->GetAdjacentEdgeRange(edge.second))
@@ -302,12 +320,9 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
             {
                 BOOST_ASSERT(i < id_vector.size());
                 BOOST_ASSERT(phantom_node_pair.target_phantom.forward_travel_mode > 0);
-                unpacked_path.emplace_back(
-                    PathData{id_vector[i],
-                             phantom_node_pair.target_phantom.name_id,
-                             TurnInstruction::NoTurn,
-                             0,
-                             phantom_node_pair.target_phantom.forward_travel_mode});
+                unpacked_path.emplace_back(PathData{
+                    id_vector[i], phantom_node_pair.target_phantom.name_id, TurnInstruction::NoTurn,
+                    0, phantom_node_pair.target_phantom.forward_travel_mode});
             }
         }
 
@@ -412,6 +427,184 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
         }
     }
 
+    // assumes that heaps are already setup correctly.
+    void Search(SearchEngineData::QueryHeap &forward_heap,
+                SearchEngineData::QueryHeap &reverse_heap,
+                int &distance,
+                std::vector<NodeID> &packed_leg) const
+    {
+        NodeID middle = SPECIAL_NODEID;
+
+        // get offset to account for offsets on phantom nodes on compressed edges
+        const auto min_edge_offset = std::min(0, forward_heap.MinKey());
+        BOOST_ASSERT(min_edge_offset <= 0);
+        // we only every insert negative offsets for nodes in the forward heap
+        BOOST_ASSERT(reverse_heap.MinKey() >= 0);
+
+        // run two-Target Dijkstra routing step.
+        while (0 < (forward_heap.Size() + reverse_heap.Size()))
+        {
+            if (!forward_heap.Empty())
+            {
+                RoutingStep(forward_heap, reverse_heap, middle, distance, min_edge_offset, true);
+            }
+            if (!reverse_heap.Empty())
+            {
+                RoutingStep(reverse_heap, forward_heap, middle, distance, min_edge_offset, false);
+            }
+        }
+
+        // No path found for both target nodes?
+        if (INVALID_EDGE_WEIGHT == distance || SPECIAL_NODEID == middle)
+        {
+            return;
+        }
+
+        // Was a paths over one of the forward/reverse nodes not found?
+        BOOST_ASSERT_MSG((SPECIAL_NODEID != middle && INVALID_EDGE_WEIGHT != distance),
+                         "no path found");
+
+        RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle, packed_leg);
+    }
+
+    // assumes that heaps are already setup correctly.
+    void SearchWithCore(SearchEngineData::QueryHeap &forward_heap,
+                        SearchEngineData::QueryHeap &reverse_heap,
+                        SearchEngineData::QueryHeap &forward_core_heap,
+                        SearchEngineData::QueryHeap &reverse_core_heap,
+                        int &distance,
+                        std::vector<NodeID> &packed_leg) const
+    {
+        NodeID middle = SPECIAL_NODEID;
+
+        std::vector<std::pair<NodeID, EdgeWeight>> forward_entry_points;
+        std::vector<std::pair<NodeID, EdgeWeight>> reverse_entry_points;
+
+        // get offset to account for offsets on phantom nodes on compressed edges
+        const auto min_edge_offset = std::min(0, forward_heap.MinKey());
+        // we only every insert negative offsets for nodes in the forward heap
+        BOOST_ASSERT(reverse_heap.MinKey() >= 0);
+
+        // run two-Target Dijkstra routing step.
+        while (0 < (forward_heap.Size() + reverse_heap.Size()))
+        {
+            if (!forward_heap.Empty())
+            {
+                if (facade->IsCoreNode(forward_heap.Min()))
+                {
+                    const NodeID node = forward_heap.DeleteMin();
+                    const int key = forward_heap.GetKey(node);
+                    forward_entry_points.emplace_back(node, key);
+                }
+                else
+                {
+                    RoutingStep(forward_heap, reverse_heap, middle, distance, min_edge_offset,
+                                true);
+                }
+            }
+            if (!reverse_heap.Empty())
+            {
+                if (facade->IsCoreNode(reverse_heap.Min()))
+                {
+                    const NodeID node = reverse_heap.DeleteMin();
+                    const int key = reverse_heap.GetKey(node);
+                    reverse_entry_points.emplace_back(node, key);
+                }
+                else
+                {
+                    RoutingStep(reverse_heap, forward_heap, middle, distance, min_edge_offset,
+                                false);
+                }
+            }
+        }
+
+        // TODO check if unordered_set might be faster
+        // sort by id and increasing by distance
+        auto entry_point_comparator = [](const std::pair<NodeID, EdgeWeight> &lhs,
+                                         const std::pair<NodeID, EdgeWeight> &rhs)
+        {
+            return lhs.first < rhs.first || (lhs.first == rhs.first && lhs.second < rhs.second);
+        };
+        std::sort(forward_entry_points.begin(), forward_entry_points.end(), entry_point_comparator);
+        std::sort(reverse_entry_points.begin(), reverse_entry_points.end(), entry_point_comparator);
+
+        NodeID last_id = SPECIAL_NODEID;
+        for (const auto p : forward_entry_points)
+        {
+            if (p.first == last_id)
+            {
+                continue;
+            }
+            forward_core_heap.Insert(p.first, p.second, p.first);
+            last_id = p.first;
+        }
+        last_id = SPECIAL_NODEID;
+        for (const auto p : reverse_entry_points)
+        {
+            if (p.first == last_id)
+            {
+                continue;
+            }
+            reverse_core_heap.Insert(p.first, p.second, p.first);
+            last_id = p.first;
+        }
+
+        // get offset to account for offsets on phantom nodes on compressed edges
+        int min_core_edge_offset = 0;
+        if (forward_core_heap.Size() > 0)
+        {
+            min_core_edge_offset = std::min(min_core_edge_offset, forward_core_heap.MinKey());
+        }
+        if (reverse_core_heap.Size() > 0 && reverse_core_heap.MinKey() < 0)
+        {
+            min_core_edge_offset = std::min(min_core_edge_offset, reverse_core_heap.MinKey());
+        }
+        BOOST_ASSERT(min_core_edge_offset <= 0);
+
+        // run two-target Dijkstra routing step on core with termination criterion
+        while (0 < (forward_core_heap.Size() + reverse_core_heap.Size()) &&
+               distance > (forward_core_heap.MinKey() + reverse_core_heap.MinKey()))
+        {
+            if (!forward_core_heap.Empty())
+            {
+                RoutingStep(forward_core_heap, reverse_core_heap, middle, distance,
+                            min_core_edge_offset, true, false);
+            }
+            if (!reverse_core_heap.Empty())
+            {
+                RoutingStep(reverse_core_heap, forward_core_heap, middle, distance,
+                            min_core_edge_offset, false, false);
+            }
+        }
+
+        // No path found for both target nodes?
+        if (INVALID_EDGE_WEIGHT == distance || SPECIAL_NODEID == middle)
+        {
+            return;
+        }
+
+        // Was a paths over one of the forward/reverse nodes not found?
+        BOOST_ASSERT_MSG((SPECIAL_NODEID != middle && INVALID_EDGE_WEIGHT != distance),
+                         "no path found");
+
+        // we need to unpack sub path from core heaps
+        if (facade->IsCoreNode(middle))
+        {
+            std::vector<NodeID> packed_core_leg;
+            RetrievePackedPathFromHeap(forward_core_heap, reverse_core_heap, middle,
+                                       packed_core_leg);
+            BOOST_ASSERT(packed_core_leg.size() > 0);
+            RetrievePackedPathFromSingleHeap(forward_heap, packed_core_leg.front(), packed_leg);
+            std::reverse(packed_leg.begin(), packed_leg.end());
+            packed_leg.insert(packed_leg.end(), packed_core_leg.begin(), packed_core_leg.end());
+            RetrievePackedPathFromSingleHeap(reverse_heap, packed_core_leg.back(), packed_leg);
+        }
+        else
+        {
+            RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle, packed_leg);
+        }
+    }
+
     double get_network_distance(SearchEngineData::QueryHeap &forward_heap,
                                 SearchEngineData::QueryHeap &reverse_heap,
                                 const PhantomNode &source_phantom,
@@ -453,12 +646,12 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
         {
             if (0 < forward_heap.Size())
             {
-                RoutingStep(forward_heap, reverse_heap, &middle_node, &upper_bound, edge_offset,
+                RoutingStep(forward_heap, reverse_heap, middle_node, upper_bound, edge_offset,
                             true);
             }
             if (0 < reverse_heap.Size())
             {
-                RoutingStep(reverse_heap, forward_heap, &middle_node, &upper_bound, edge_offset,
+                RoutingStep(reverse_heap, forward_heap, middle_node, upper_bound, edge_offset,
                             false);
             }
         }
@@ -472,7 +665,7 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
             PhantomNodes nodes;
             nodes.source_phantom = source_phantom;
             nodes.target_phantom = target_phantom;
-            UnpackPath(packed_leg, nodes, unpacked_path);
+            UnpackPath(packed_leg.begin(), packed_leg.end(), nodes, unpacked_path);
 
             FixedPointCoordinate previous_coordinate = source_phantom.location;
             FixedPointCoordinate current_coordinate;
@@ -480,12 +673,12 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
             for (const auto &p : unpacked_path)
             {
                 current_coordinate = facade->GetCoordinateOfNode(p.node);
-                distance += coordinate_calculation::great_circle_distance(previous_coordinate,
-                                                                          current_coordinate);
+                distance += coordinate_calculation::haversine_distance(previous_coordinate,
+                                                                       current_coordinate);
                 previous_coordinate = current_coordinate;
             }
-            distance += coordinate_calculation::great_circle_distance(previous_coordinate,
-                                                                      target_phantom.location);
+            distance += coordinate_calculation::haversine_distance(previous_coordinate,
+                                                                   target_phantom.location);
         }
         return distance;
     }
diff --git a/routing_algorithms/shortest_path.hpp b/routing_algorithms/shortest_path.hpp
index eaa6d3c..601e394 100644
--- a/routing_algorithms/shortest_path.hpp
+++ b/routing_algorithms/shortest_path.hpp
@@ -28,12 +28,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef SHORTEST_PATH_HPP
 #define SHORTEST_PATH_HPP
 
-#include <boost/assert.hpp>
+#include "../typedefs.h"
 
 #include "routing_base.hpp"
+
 #include "../data_structures/search_engine_data.hpp"
 #include "../util/integer_range.hpp"
-#include "../typedefs.h"
+
+#include <boost/assert.hpp>
 
 template <class DataFacadeT>
 class ShortestPathRouting final
@@ -51,294 +53,483 @@ class ShortestPathRouting final
 
     ~ShortestPathRouting() {}
 
-    void operator()(const std::vector<PhantomNodes> &phantom_nodes_vector,
-                    const std::vector<bool> &uturn_indicators,
-                    InternalRouteResult &raw_route_data) const
+    // allows a uturn at the target_phantom
+    // searches source forward/reverse -> target forward/reverse
+    void SearchWithUTurn(QueryHeap &forward_heap,
+                         QueryHeap &reverse_heap,
+                         const bool search_from_forward_node,
+                         const bool search_from_reverse_node,
+                         const bool search_to_forward_node,
+                         const bool search_to_reverse_node,
+                         const PhantomNode &source_phantom,
+                         const PhantomNode &target_phantom,
+                         const int total_distance_to_forward,
+                         const int total_distance_to_reverse,
+                         int &new_total_distance,
+                         std::vector<NodeID> &leg_packed_path) const
     {
-        int distance1 = 0;
-        int distance2 = 0;
-        bool search_from_1st_node = true;
-        bool search_from_2nd_node = true;
-        NodeID middle1 = SPECIAL_NODEID;
-        NodeID middle2 = SPECIAL_NODEID;
-        std::vector<std::vector<NodeID>> packed_legs1(phantom_nodes_vector.size());
-        std::vector<std::vector<NodeID>> packed_legs2(phantom_nodes_vector.size());
-
-        engine_working_data.InitializeOrClearFirstThreadLocalStorage(
-            super::facade->GetNumberOfNodes());
-        engine_working_data.InitializeOrClearSecondThreadLocalStorage(
-            super::facade->GetNumberOfNodes());
-        engine_working_data.InitializeOrClearThirdThreadLocalStorage(
-            super::facade->GetNumberOfNodes());
+        forward_heap.Clear();
+        reverse_heap.Clear();
+        if (search_from_forward_node)
+        {
+            forward_heap.Insert(source_phantom.forward_node_id,
+                                total_distance_to_forward -
+                                    source_phantom.GetForwardWeightPlusOffset(),
+                                source_phantom.forward_node_id);
+        }
+        if (search_from_reverse_node)
+        {
+            forward_heap.Insert(source_phantom.reverse_node_id,
+                                total_distance_to_reverse -
+                                    source_phantom.GetReverseWeightPlusOffset(),
+                                source_phantom.reverse_node_id);
+        }
+        if (search_to_forward_node)
+        {
+            reverse_heap.Insert(target_phantom.forward_node_id,
+                                target_phantom.GetForwardWeightPlusOffset(),
+                                target_phantom.forward_node_id);
+        }
+        if (search_to_reverse_node)
+        {
+            reverse_heap.Insert(target_phantom.reverse_node_id,
+                                target_phantom.GetReverseWeightPlusOffset(),
+                                target_phantom.reverse_node_id);
+        }
+        BOOST_ASSERT(forward_heap.Size() > 0);
+        BOOST_ASSERT(reverse_heap.Size() > 0);
+        super::Search(forward_heap, reverse_heap, new_total_distance, leg_packed_path);
+    }
 
-        QueryHeap &forward_heap1 = *(engine_working_data.forward_heap_1);
-        QueryHeap &reverse_heap1 = *(engine_working_data.reverse_heap_1);
-        QueryHeap &forward_heap2 = *(engine_working_data.forward_heap_2);
-        QueryHeap &reverse_heap2 = *(engine_working_data.reverse_heap_2);
+    // If source and target are reverse on a oneway we need to find a path
+    // that connects the two. This is _not_ the shortest path in our model,
+    // as source and target are on the same edge based node.
+    // We force a detour by inserting "virtaul vias", which means we search a path
+    // from all nodes that are connected by outgoing edges to all nodes that are connected by
+    // incoming edges.
+    // ------^
+    // |     ^source
+    // |     ^
+    // |     ^target
+    // ------^
+    void SearchLoop(QueryHeap &forward_heap,
+                QueryHeap &reverse_heap,
+                const bool search_forward_node,
+                const bool search_reverse_node,
+                const PhantomNode &source_phantom,
+                const PhantomNode &target_phantom,
+                const int total_distance_to_forward,
+                const int total_distance_to_reverse,
+                int &new_total_distance_to_forward,
+                int &new_total_distance_to_reverse,
+                std::vector<NodeID> &leg_packed_path_forward,
+                std::vector<NodeID> &leg_packed_path_reverse) const
+    {
+        BOOST_ASSERT(source_phantom.forward_node_id == target_phantom.forward_node_id);
+        BOOST_ASSERT(source_phantom.reverse_node_id == target_phantom.reverse_node_id);
 
-        std::size_t current_leg = 0;
-        // Get distance to next pair of target nodes.
-        for (const PhantomNodes &phantom_node_pair : phantom_nodes_vector)
+        if (search_forward_node)
         {
-            forward_heap1.Clear();
-            forward_heap2.Clear();
-            reverse_heap1.Clear();
-            reverse_heap2.Clear();
-            int local_upper_bound1 = INVALID_EDGE_WEIGHT;
-            int local_upper_bound2 = INVALID_EDGE_WEIGHT;
-
-            middle1 = SPECIAL_NODEID;
-            middle2 = SPECIAL_NODEID;
-
-            const bool allow_u_turn = current_leg > 0 && uturn_indicators.size() > current_leg &&
-                                      uturn_indicators[current_leg - 1];
-            const EdgeWeight min_edge_offset =
-                std::min(-phantom_node_pair.source_phantom.GetForwardWeightPlusOffset(),
-                         -phantom_node_pair.source_phantom.GetReverseWeightPlusOffset());
-
-            // insert new starting nodes into forward heap, adjusted by previous distances.
-            if ((allow_u_turn || search_from_1st_node) &&
-                phantom_node_pair.source_phantom.forward_node_id != SPECIAL_NODEID)
-            {
-                forward_heap1.Insert(
-                    phantom_node_pair.source_phantom.forward_node_id,
-                    -phantom_node_pair.source_phantom.GetForwardWeightPlusOffset(),
-                    phantom_node_pair.source_phantom.forward_node_id);
-                // SimpleLogger().Write(logDEBUG) << "fwd-a2 insert: " <<
-                // phantom_node_pair.source_phantom.forward_node_id << ", w: " << -phantom_node_pair.source_phantom.GetForwardWeightPlusOffset();
-                forward_heap2.Insert(
-                    phantom_node_pair.source_phantom.forward_node_id,
-                    -phantom_node_pair.source_phantom.GetForwardWeightPlusOffset(),
-                    phantom_node_pair.source_phantom.forward_node_id);
-                // SimpleLogger().Write(logDEBUG) << "fwd-b2 insert: " <<
-                // phantom_node_pair.source_phantom.forward_node_id << ", w: " << -phantom_node_pair.source_phantom.GetForwardWeightPlusOffset();
-            }
-            if ((allow_u_turn || search_from_2nd_node) &&
-                phantom_node_pair.source_phantom.reverse_node_id != SPECIAL_NODEID)
-            {
-                forward_heap1.Insert(
-                    phantom_node_pair.source_phantom.reverse_node_id,
-                    -phantom_node_pair.source_phantom.GetReverseWeightPlusOffset(),
-                    phantom_node_pair.source_phantom.reverse_node_id);
-                // SimpleLogger().Write(logDEBUG) << "fwd-a2 insert: " <<
-                // phantom_node_pair.source_phantom.reverse_node_id << ", w: " << -phantom_node_pair.source_phantom.GetReverseWeightPlusOffset();
-                forward_heap2.Insert(
-                    phantom_node_pair.source_phantom.reverse_node_id,
-                    -phantom_node_pair.source_phantom.GetReverseWeightPlusOffset(),
-                    phantom_node_pair.source_phantom.reverse_node_id);
-                // SimpleLogger().Write(logDEBUG) << "fwd-b2 insert: " <<
-                // phantom_node_pair.source_phantom.reverse_node_id << ", w: " << -phantom_node_pair.source_phantom.GetReverseWeightPlusOffset();
-            }
-
-            // insert new backward nodes into backward heap, unadjusted.
-            if (phantom_node_pair.target_phantom.forward_node_id != SPECIAL_NODEID)
-            {
-                reverse_heap1.Insert(phantom_node_pair.target_phantom.forward_node_id,
-                                     phantom_node_pair.target_phantom.GetForwardWeightPlusOffset(),
-                                     phantom_node_pair.target_phantom.forward_node_id);
-                // SimpleLogger().Write(logDEBUG) << "rev-a insert: " <<
-                // phantom_node_pair.target_phantom.forward_node_id << ", w: " << phantom_node_pair.target_phantom.GetForwardWeightPlusOffset();
-            }
+            forward_heap.Clear();
+            reverse_heap.Clear();
 
-            if (phantom_node_pair.target_phantom.reverse_node_id != SPECIAL_NODEID)
-            {
-                reverse_heap2.Insert(phantom_node_pair.target_phantom.reverse_node_id,
-                                     phantom_node_pair.target_phantom.GetReverseWeightPlusOffset(),
-                                     phantom_node_pair.target_phantom.reverse_node_id);
-                // SimpleLogger().Write(logDEBUG) << "rev-a insert: " <<
-                // phantom_node_pair.target_phantom.reverse_node_id << ", w: " << phantom_node_pair.target_phantom.GetReverseWeightPlusOffset();
-            }
+            auto node_id = source_phantom.forward_node_id;
 
-            // run two-Target Dijkstra routing step.
-            while (0 < (forward_heap1.Size() + reverse_heap1.Size()))
+            for (const auto edge : super::facade->GetAdjacentEdgeRange(node_id))
             {
-                if (!forward_heap1.Empty())
+                const auto& data = super::facade->GetEdgeData(edge);
+                if (data.forward)
                 {
-                    super::RoutingStep(forward_heap1, reverse_heap1, &middle1, &local_upper_bound1,
-                                       min_edge_offset, true);
+                    auto target = super::facade->GetTarget(edge);
+                    auto offset = total_distance_to_forward + data.distance - source_phantom.GetForwardWeightPlusOffset();
+                    forward_heap.Insert(target, offset, target);
                 }
-                if (!reverse_heap1.Empty())
+
+                if (data.backward)
                 {
-                    super::RoutingStep(reverse_heap1, forward_heap1, &middle1, &local_upper_bound1,
-                                       min_edge_offset, false);
+                    auto target = super::facade->GetTarget(edge);
+                    auto offset = data.distance + target_phantom.GetForwardWeightPlusOffset();
+                    reverse_heap.Insert(target, offset, target);
                 }
             }
 
-            if (!reverse_heap2.Empty())
+            BOOST_ASSERT(forward_heap.Size() > 0);
+            BOOST_ASSERT(reverse_heap.Size() > 0);
+            super::Search(forward_heap, reverse_heap, new_total_distance_to_forward,
+                          leg_packed_path_forward);
+
+            // insert node to both endpoints to close the leg
+            leg_packed_path_forward.push_back(node_id);
+            std::reverse(leg_packed_path_forward.begin(), leg_packed_path_forward.end());
+            leg_packed_path_forward.push_back(node_id);
+            std::reverse(leg_packed_path_forward.begin(), leg_packed_path_forward.end());
+        }
+
+        if (search_reverse_node)
+        {
+            forward_heap.Clear();
+            reverse_heap.Clear();
+
+            auto node_id = source_phantom.reverse_node_id;
+
+            for (const auto edge : super::facade->GetAdjacentEdgeRange(node_id))
             {
-                while (0 < (forward_heap2.Size() + reverse_heap2.Size()))
+                const auto& data = super::facade->GetEdgeData(edge);
+                if (data.forward)
                 {
-                    if (!forward_heap2.Empty())
-                    {
-                        super::RoutingStep(forward_heap2, reverse_heap2, &middle2,
-                                           &local_upper_bound2, min_edge_offset, true);
-                    }
-                    if (!reverse_heap2.Empty())
-                    {
-                        super::RoutingStep(reverse_heap2, forward_heap2, &middle2,
-                                           &local_upper_bound2, min_edge_offset, false);
-                    }
+                    auto target = super::facade->GetTarget(edge);
+                    auto offset = total_distance_to_reverse + data.distance - source_phantom.GetReverseWeightPlusOffset();
+                    forward_heap.Insert(target, offset, target);
+                }
+
+                if (data.backward)
+                {
+                    auto target = super::facade->GetTarget(edge);
+                    auto offset = data.distance + target_phantom.GetReverseWeightPlusOffset();
+                    reverse_heap.Insert(target, offset, target);
                 }
             }
 
-            // No path found for both target nodes?
-            if ((INVALID_EDGE_WEIGHT == local_upper_bound1) &&
-                (INVALID_EDGE_WEIGHT == local_upper_bound2))
+            BOOST_ASSERT(forward_heap.Size() > 0);
+            BOOST_ASSERT(reverse_heap.Size() > 0);
+            super::Search(forward_heap, reverse_heap, new_total_distance_to_reverse,
+                          leg_packed_path_reverse);
+
+            // insert node to both endpoints to close the leg
+            leg_packed_path_reverse.push_back(node_id);
+            std::reverse(leg_packed_path_reverse.begin(), leg_packed_path_reverse.end());
+            leg_packed_path_reverse.push_back(node_id);
+            std::reverse(leg_packed_path_reverse.begin(), leg_packed_path_reverse.end());
+        }
+
+    }
+
+    // searches shortest path between:
+    // source forward/reverse -> target forward
+    // source forward/reverse -> target reverse
+    void Search(QueryHeap &forward_heap,
+                QueryHeap &reverse_heap,
+                const bool search_from_forward_node,
+                const bool search_from_reverse_node,
+                const bool search_to_forward_node,
+                const bool search_to_reverse_node,
+                const PhantomNode &source_phantom,
+                const PhantomNode &target_phantom,
+                const int total_distance_to_forward,
+                const int total_distance_to_reverse,
+                int &new_total_distance_to_forward,
+                int &new_total_distance_to_reverse,
+                std::vector<NodeID> &leg_packed_path_forward,
+                std::vector<NodeID> &leg_packed_path_reverse) const
+    {
+        if (search_to_forward_node)
+        {
+            forward_heap.Clear();
+            reverse_heap.Clear();
+            reverse_heap.Insert(target_phantom.forward_node_id,
+                                target_phantom.GetForwardWeightPlusOffset(),
+                                target_phantom.forward_node_id);
+
+            if (search_from_forward_node)
             {
-                raw_route_data.shortest_path_length = INVALID_EDGE_WEIGHT;
-                raw_route_data.alternative_path_length = INVALID_EDGE_WEIGHT;
-                return;
+                forward_heap.Insert(source_phantom.forward_node_id,
+                                    total_distance_to_forward -
+                                        source_phantom.GetForwardWeightPlusOffset(),
+                                    source_phantom.forward_node_id);
+            }
+            if (search_from_reverse_node)
+            {
+                forward_heap.Insert(source_phantom.reverse_node_id,
+                                    total_distance_to_reverse -
+                                        source_phantom.GetReverseWeightPlusOffset(),
+                                    source_phantom.reverse_node_id);
             }
+            BOOST_ASSERT(forward_heap.Size() > 0);
+            BOOST_ASSERT(reverse_heap.Size() > 0);
+            super::Search(forward_heap, reverse_heap, new_total_distance_to_forward,
+                          leg_packed_path_forward);
+        }
 
-            search_from_1st_node = true;
-            search_from_2nd_node = true;
-            if (SPECIAL_NODEID == middle1)
+        if (search_to_reverse_node)
+        {
+            forward_heap.Clear();
+            reverse_heap.Clear();
+            reverse_heap.Insert(target_phantom.reverse_node_id,
+                                target_phantom.GetReverseWeightPlusOffset(),
+                                target_phantom.reverse_node_id);
+            if (search_from_forward_node)
             {
-                search_from_1st_node = false;
+                forward_heap.Insert(source_phantom.forward_node_id,
+                                    total_distance_to_forward -
+                                        source_phantom.GetForwardWeightPlusOffset(),
+                                    source_phantom.forward_node_id);
             }
-            if (SPECIAL_NODEID == middle2)
+            if (search_from_reverse_node)
             {
-                search_from_2nd_node = false;
+                forward_heap.Insert(source_phantom.reverse_node_id,
+                                    total_distance_to_reverse -
+                                        source_phantom.GetReverseWeightPlusOffset(),
+                                    source_phantom.reverse_node_id);
             }
+            BOOST_ASSERT(forward_heap.Size() > 0);
+            BOOST_ASSERT(reverse_heap.Size() > 0);
+            super::Search(forward_heap, reverse_heap, new_total_distance_to_reverse,
+                          leg_packed_path_reverse);
+        }
+    }
+
+    void UnpackLegs(const std::vector<PhantomNodes> &phantom_nodes_vector,
+                    const std::vector<NodeID> &total_packed_path,
+                    const std::vector<std::size_t> &packed_leg_begin,
+                    const int shortest_path_length,
+                    InternalRouteResult &raw_route_data) const
+    {
+        raw_route_data.unpacked_path_segments.resize(packed_leg_begin.size() - 1);
+
+        raw_route_data.shortest_path_length = shortest_path_length;
+
+        for (const auto current_leg : osrm::irange<std::size_t>(0, packed_leg_begin.size() - 1))
+        {
+            auto leg_begin = total_packed_path.begin() + packed_leg_begin[current_leg];
+            auto leg_end = total_packed_path.begin() + packed_leg_begin[current_leg + 1];
+            const auto &unpack_phantom_node_pair = phantom_nodes_vector[current_leg];
+            super::UnpackPath(leg_begin, leg_end, unpack_phantom_node_pair,
+                              raw_route_data.unpacked_path_segments[current_leg]);
+
+            raw_route_data.source_traversed_in_reverse.push_back(
+                (*leg_begin != phantom_nodes_vector[current_leg].source_phantom.forward_node_id));
+            raw_route_data.target_traversed_in_reverse.push_back(
+                (*std::prev(leg_end) !=
+                 phantom_nodes_vector[current_leg].target_phantom.forward_node_id));
+        }
+    }
+
+    void operator()(const std::vector<PhantomNodes> &phantom_nodes_vector,
+                    const std::vector<bool> &uturn_indicators,
+                    InternalRouteResult &raw_route_data) const
+    {
+        BOOST_ASSERT(uturn_indicators.size() == phantom_nodes_vector.size() + 1);
+        engine_working_data.InitializeOrClearFirstThreadLocalStorage(
+            super::facade->GetNumberOfNodes());
 
-            // Was at most one of the two paths not found?
-            BOOST_ASSERT_MSG((INVALID_EDGE_WEIGHT != distance1 || INVALID_EDGE_WEIGHT != distance2),
-                             "no path found");
+        QueryHeap &forward_heap = *(engine_working_data.forward_heap_1);
+        QueryHeap &reverse_heap = *(engine_working_data.reverse_heap_1);
 
-            // Unpack paths if they exist
-            std::vector<NodeID> temporary_packed_leg1;
-            std::vector<NodeID> temporary_packed_leg2;
+        int total_distance_to_forward = 0;
+        int total_distance_to_reverse = 0;
+        bool search_from_forward_node = phantom_nodes_vector.front().source_phantom.forward_node_id != SPECIAL_NODEID;
+        bool search_from_reverse_node = phantom_nodes_vector.front().source_phantom.reverse_node_id != SPECIAL_NODEID;
 
-            BOOST_ASSERT(current_leg < packed_legs1.size());
-            BOOST_ASSERT(current_leg < packed_legs2.size());
+        std::vector<NodeID> prev_packed_leg_to_forward;
+        std::vector<NodeID> prev_packed_leg_to_reverse;
 
-            if (INVALID_EDGE_WEIGHT != local_upper_bound1)
+        std::vector<NodeID> total_packed_path_to_forward;
+        std::vector<std::size_t> packed_leg_to_forward_begin;
+        std::vector<NodeID> total_packed_path_to_reverse;
+        std::vector<std::size_t> packed_leg_to_reverse_begin;
+
+        std::size_t current_leg = 0;
+        // this implements a dynamic program that finds the shortest route through
+        // a list of vias
+        for (const auto &phantom_node_pair : phantom_nodes_vector)
+        {
+            int new_total_distance_to_forward = INVALID_EDGE_WEIGHT;
+            int new_total_distance_to_reverse = INVALID_EDGE_WEIGHT;
+
+            std::vector<NodeID> packed_leg_to_forward;
+            std::vector<NodeID> packed_leg_to_reverse;
+
+            const auto &source_phantom = phantom_node_pair.source_phantom;
+            const auto &target_phantom = phantom_node_pair.target_phantom;
+
+
+            BOOST_ASSERT(current_leg + 1 < uturn_indicators.size());
+            const bool allow_u_turn_at_via = uturn_indicators[current_leg + 1];
+
+            bool search_to_forward_node = target_phantom.forward_node_id != SPECIAL_NODEID;
+            bool search_to_reverse_node = target_phantom.reverse_node_id != SPECIAL_NODEID;
+
+            BOOST_ASSERT(!search_from_forward_node || source_phantom.forward_node_id != SPECIAL_NODEID);
+            BOOST_ASSERT(!search_from_reverse_node || source_phantom.reverse_node_id != SPECIAL_NODEID);
+
+            if (source_phantom.forward_node_id == target_phantom.forward_node_id &&
+                source_phantom.GetForwardWeightPlusOffset() > target_phantom.GetForwardWeightPlusOffset())
             {
-                super::RetrievePackedPathFromHeap(forward_heap1, reverse_heap1, middle1,
-                                                  temporary_packed_leg1);
+                search_to_forward_node = search_from_reverse_node;
             }
-
-            if (INVALID_EDGE_WEIGHT != local_upper_bound2)
+            if (source_phantom.reverse_node_id == target_phantom.reverse_node_id &&
+                source_phantom.GetReverseWeightPlusOffset() > target_phantom.GetReverseWeightPlusOffset())
             {
-                super::RetrievePackedPathFromHeap(forward_heap2, reverse_heap2, middle2,
-                                                  temporary_packed_leg2);
+                search_to_reverse_node = search_from_forward_node;
             }
 
-            // if one of the paths was not found, replace it with the other one.
-            if ((allow_u_turn && local_upper_bound1 > local_upper_bound2) ||
-                temporary_packed_leg1.empty())
+            BOOST_ASSERT(search_from_forward_node || search_from_reverse_node);
+
+            if(search_to_reverse_node || search_to_forward_node)
             {
-                temporary_packed_leg1.clear();
-                temporary_packed_leg1.insert(temporary_packed_leg1.end(),
-                                             temporary_packed_leg2.begin(),
-                                             temporary_packed_leg2.end());
-                local_upper_bound1 = local_upper_bound2;
+                if (allow_u_turn_at_via)
+                {
+                    SearchWithUTurn(forward_heap, reverse_heap, search_from_forward_node,
+                                    search_from_reverse_node, search_to_forward_node,
+                                    search_to_reverse_node, source_phantom, target_phantom,
+                                    total_distance_to_forward, total_distance_to_reverse,
+                                    new_total_distance_to_forward, packed_leg_to_forward);
+                    // if only the reverse node is valid (e.g. when using the match plugin) we actually need to move
+                    if (target_phantom.forward_node_id == SPECIAL_NODEID)
+                    {
+                        BOOST_ASSERT(target_phantom.reverse_node_id != SPECIAL_NODEID);
+                        new_total_distance_to_reverse = new_total_distance_to_forward;
+                        packed_leg_to_reverse = std::move(packed_leg_to_forward);
+                        new_total_distance_to_forward = INVALID_EDGE_WEIGHT;
+                    }
+                    else if (target_phantom.reverse_node_id != SPECIAL_NODEID)
+                    {
+                        new_total_distance_to_reverse = new_total_distance_to_forward;
+                        packed_leg_to_reverse = packed_leg_to_forward;
+                    }
+                }
+                else
+                {
+                    Search(forward_heap, reverse_heap, search_from_forward_node,
+                           search_from_reverse_node, search_to_forward_node, search_to_reverse_node,
+                           source_phantom, target_phantom, total_distance_to_forward,
+                           total_distance_to_reverse, new_total_distance_to_forward,
+                           new_total_distance_to_reverse, packed_leg_to_forward, packed_leg_to_reverse);
+                }
             }
-            if ((allow_u_turn && local_upper_bound2 > local_upper_bound1) ||
-                temporary_packed_leg2.empty())
+            else
             {
-                temporary_packed_leg2.clear();
-                temporary_packed_leg2.insert(temporary_packed_leg2.end(),
-                                             temporary_packed_leg1.begin(),
-                                             temporary_packed_leg1.end());
-                local_upper_bound2 = local_upper_bound1;
+                search_to_forward_node = target_phantom.forward_node_id != SPECIAL_NODEID;
+                search_to_reverse_node = target_phantom.reverse_node_id != SPECIAL_NODEID;
+                BOOST_ASSERT(search_from_reverse_node == search_to_reverse_node);
+                BOOST_ASSERT(search_from_forward_node == search_to_forward_node);
+                SearchLoop(forward_heap, reverse_heap, search_from_forward_node,
+                       search_from_reverse_node, source_phantom, target_phantom, total_distance_to_forward,
+                       total_distance_to_reverse, new_total_distance_to_forward,
+                       new_total_distance_to_reverse, packed_leg_to_forward, packed_leg_to_reverse);
             }
 
-            BOOST_ASSERT_MSG(!temporary_packed_leg1.empty() || !temporary_packed_leg2.empty(),
-                             "tempory packed paths empty");
-
-            BOOST_ASSERT((0 == current_leg) || !packed_legs1[current_leg - 1].empty());
-            BOOST_ASSERT((0 == current_leg) || !packed_legs2[current_leg - 1].empty());
+            // No path found for both target nodes?
+            if ((INVALID_EDGE_WEIGHT == new_total_distance_to_forward) &&
+                (INVALID_EDGE_WEIGHT == new_total_distance_to_reverse))
+            {
+                raw_route_data.shortest_path_length = INVALID_EDGE_WEIGHT;
+                raw_route_data.alternative_path_length = INVALID_EDGE_WEIGHT;
+                return;
+            }
 
-            if (!allow_u_turn && 0 < current_leg)
+            // we need to figure out how the new legs connect to the previous ones
+            if (current_leg > 0)
             {
-                const NodeID end_id_of_segment1 = packed_legs1[current_leg - 1].back();
-                const NodeID end_id_of_segment2 = packed_legs2[current_leg - 1].back();
-                BOOST_ASSERT(!temporary_packed_leg1.empty());
-                const NodeID start_id_of_leg1 = temporary_packed_leg1.front();
-                const NodeID start_id_of_leg2 = temporary_packed_leg2.front();
-                if ((end_id_of_segment1 != start_id_of_leg1) &&
-                    (end_id_of_segment2 != start_id_of_leg2))
+                bool forward_to_forward =
+                    (new_total_distance_to_forward != INVALID_EDGE_WEIGHT) &&
+                    packed_leg_to_forward.front() == source_phantom.forward_node_id;
+                bool reverse_to_forward =
+                    (new_total_distance_to_forward != INVALID_EDGE_WEIGHT) &&
+                    packed_leg_to_forward.front() == source_phantom.reverse_node_id;
+                bool forward_to_reverse =
+                    (new_total_distance_to_reverse != INVALID_EDGE_WEIGHT) &&
+                    packed_leg_to_reverse.front() == source_phantom.forward_node_id;
+                bool reverse_to_reverse =
+                    (new_total_distance_to_reverse != INVALID_EDGE_WEIGHT) &&
+                    packed_leg_to_reverse.front() == source_phantom.reverse_node_id;
+
+                BOOST_ASSERT(!forward_to_forward || !reverse_to_forward);
+                BOOST_ASSERT(!forward_to_reverse || !reverse_to_reverse);
+
+                // in this case we always need to copy
+                if (forward_to_forward && forward_to_reverse)
+                {
+                    // in this case we copy the path leading to the source forward node
+                    // and change the case
+                    total_packed_path_to_reverse = total_packed_path_to_forward;
+                    packed_leg_to_reverse_begin = packed_leg_to_forward_begin;
+                    forward_to_reverse = false;
+                    reverse_to_reverse = true;
+                }
+                else if (reverse_to_forward && reverse_to_reverse)
                 {
-                    std::swap(temporary_packed_leg1, temporary_packed_leg2);
-                    std::swap(local_upper_bound1, local_upper_bound2);
+                    total_packed_path_to_forward = total_packed_path_to_reverse;
+                    packed_leg_to_forward_begin = packed_leg_to_reverse_begin;
+                    reverse_to_forward = false;
+                    forward_to_forward = true;
                 }
+                BOOST_ASSERT(!forward_to_forward || !forward_to_reverse);
+                BOOST_ASSERT(!reverse_to_forward || !reverse_to_reverse);
 
-                // remove the shorter path if both legs end at the same segment
-                if (start_id_of_leg1 == start_id_of_leg2)
+                // in this case we just need to swap to regain the correct mapping
+                if (reverse_to_forward || forward_to_reverse)
                 {
-                    const NodeID last_id_of_packed_legs1 = packed_legs1[current_leg - 1].back();
-                    const NodeID last_id_of_packed_legs2 = packed_legs2[current_leg - 1].back();
-                    if (start_id_of_leg1 != last_id_of_packed_legs1)
-                    {
-                        packed_legs1 = packed_legs2;
-                        distance1 = distance2;
-                        BOOST_ASSERT(start_id_of_leg1 == temporary_packed_leg1.front());
-                    }
-                    else if (start_id_of_leg2 != last_id_of_packed_legs2)
-                    {
-                        packed_legs2 = packed_legs1;
-                        distance2 = distance1;
-                        BOOST_ASSERT(start_id_of_leg2 == temporary_packed_leg2.front());
-                    }
+                    total_packed_path_to_forward.swap(total_packed_path_to_reverse);
+                    packed_leg_to_forward_begin.swap(packed_leg_to_reverse_begin);
                 }
             }
-            BOOST_ASSERT(packed_legs1.size() == packed_legs2.size());
-
-            packed_legs1[current_leg].insert(packed_legs1[current_leg].end(),
-                                             temporary_packed_leg1.begin(),
-                                             temporary_packed_leg1.end());
-            BOOST_ASSERT(packed_legs1[current_leg].size() == temporary_packed_leg1.size());
-            packed_legs2[current_leg].insert(packed_legs2[current_leg].end(),
-                                             temporary_packed_leg2.begin(),
-                                             temporary_packed_leg2.end());
-            BOOST_ASSERT(packed_legs2[current_leg].size() == temporary_packed_leg2.size());
-
-            if (!allow_u_turn &&
-                (packed_legs1[current_leg].back() == packed_legs2[current_leg].back()) &&
-                phantom_node_pair.target_phantom.is_bidirected())
+
+            if (new_total_distance_to_forward != INVALID_EDGE_WEIGHT)
             {
-                const NodeID last_node_id = packed_legs2[current_leg].back();
-                search_from_1st_node &=
-                    !(last_node_id == phantom_node_pair.target_phantom.reverse_node_id);
-                search_from_2nd_node &=
-                    !(last_node_id == phantom_node_pair.target_phantom.forward_node_id);
-                BOOST_ASSERT(search_from_1st_node != search_from_2nd_node);
+                BOOST_ASSERT(target_phantom.forward_node_id != SPECIAL_NODEID);
+
+                packed_leg_to_forward_begin.push_back(total_packed_path_to_forward.size());
+                total_packed_path_to_forward.insert(total_packed_path_to_forward.end(),
+                                                    packed_leg_to_forward.begin(),
+                                                    packed_leg_to_forward.end());
+                search_from_forward_node = true;
+            }
+            else
+            {
+                total_packed_path_to_forward.clear();
+                packed_leg_to_forward_begin.clear();
+                search_from_forward_node = false;
+            }
+
+            if (new_total_distance_to_reverse != INVALID_EDGE_WEIGHT)
+            {
+                BOOST_ASSERT(target_phantom.reverse_node_id != SPECIAL_NODEID);
+
+                packed_leg_to_reverse_begin.push_back(total_packed_path_to_reverse.size());
+                total_packed_path_to_reverse.insert(total_packed_path_to_reverse.end(),
+                                                    packed_leg_to_reverse.begin(),
+                                                    packed_leg_to_reverse.end());
+                search_from_reverse_node = true;
+            }
+            else
+            {
+                total_packed_path_to_reverse.clear();
+                packed_leg_to_reverse_begin.clear();
+                search_from_reverse_node = false;
             }
 
-            distance1 += local_upper_bound1;
-            distance2 += local_upper_bound2;
+            prev_packed_leg_to_forward = std::move(packed_leg_to_forward);
+            prev_packed_leg_to_reverse = std::move(packed_leg_to_reverse);
+
+            total_distance_to_forward = new_total_distance_to_forward;
+            total_distance_to_reverse = new_total_distance_to_reverse;
+
             ++current_leg;
         }
 
-        if (distance1 > distance2)
+        BOOST_ASSERT(total_distance_to_forward != INVALID_EDGE_WEIGHT ||
+                     total_distance_to_reverse != INVALID_EDGE_WEIGHT);
+
+        // We make sure the fastest route is always in packed_legs_to_forward
+        if (total_distance_to_forward > total_distance_to_reverse)
         {
-            std::swap(packed_legs1, packed_legs2);
-        }
-        raw_route_data.unpacked_path_segments.resize(packed_legs1.size());
+            // insert sentinel
+            packed_leg_to_reverse_begin.push_back(total_packed_path_to_reverse.size());
+            BOOST_ASSERT(packed_leg_to_reverse_begin.size() == phantom_nodes_vector.size() + 1);
 
-        for (const std::size_t index : osrm::irange<std::size_t>(0, packed_legs1.size()))
+            UnpackLegs(phantom_nodes_vector, total_packed_path_to_reverse,
+                       packed_leg_to_reverse_begin, total_distance_to_reverse, raw_route_data);
+        }
+        else
         {
-            BOOST_ASSERT(!phantom_nodes_vector.empty());
-            BOOST_ASSERT(packed_legs1.size() == raw_route_data.unpacked_path_segments.size());
-
-            PhantomNodes unpack_phantom_node_pair = phantom_nodes_vector[index];
-            super::UnpackPath(
-                // -- packed input
-                packed_legs1[index],
-                // -- start and end of (sub-)route
-                unpack_phantom_node_pair,
-                // -- unpacked output
-                raw_route_data.unpacked_path_segments[index]);
+            // insert sentinel
+            packed_leg_to_forward_begin.push_back(total_packed_path_to_forward.size());
+            BOOST_ASSERT(packed_leg_to_forward_begin.size() == phantom_nodes_vector.size() + 1);
 
-            raw_route_data.source_traversed_in_reverse.push_back(
-                (packed_legs1[index].front() !=
-                 phantom_nodes_vector[index].source_phantom.forward_node_id));
-            raw_route_data.target_traversed_in_reverse.push_back(
-                (packed_legs1[index].back() !=
-                 phantom_nodes_vector[index].target_phantom.forward_node_id));
+            UnpackLegs(phantom_nodes_vector, total_packed_path_to_forward,
+                       packed_leg_to_forward_begin, total_distance_to_forward, raw_route_data);
         }
-        raw_route_data.shortest_path_length = std::min(distance1, distance2);
     }
 };
 
diff --git a/scripts/analyze.sh b/scripts/analyze.sh
new file mode 100755
index 0000000..872d495
--- /dev/null
+++ b/scripts/analyze.sh
@@ -0,0 +1,19 @@
+#!/usr/bin/env bash
+
+# Runs the Static Analyzer on the code base.
+# This is a wrapper intended to be used with like this:
+# 1/ analyze cmake ..
+# 2/ analyze cmake --build .
+
+exec scan-build -analyze-headers -no-failure-reports --keep-going --status-bugs \
+  -enable-checker alpha.core.BoolAssignment \
+  -enable-checker alpha.core.IdenticalExpr \
+  -enable-checker alpha.core.TestAfterDivZero \
+  -enable-checker alpha.deadcode.UnreachableCode \
+  -enable-checker alpha.security.ArrayBoundV2 \
+  -enable-checker alpha.security.MallocOverflow \
+  -enable-checker alpha.security.ReturnPtrRange \
+  -enable-checker security.FloatLoopCounter \
+  -enable-checker security.insecureAPI.rand \
+  -enable-checker security.insecureAPI.strcpy \
+  "${@}"
diff --git a/scripts/modernize.sh b/scripts/modernize.sh
new file mode 100755
index 0000000..ee0f2ed
--- /dev/null
+++ b/scripts/modernize.sh
@@ -0,0 +1,6 @@
+#!/usr/bin/env bash
+
+# Runs the Clang Modernizer in parallel on the code base.
+# Requires a compilation database in the build directory.
+
+git ls-files '*.cpp' | xargs -I{} -P $(nproc) clang-modernize -p build -final-syntax-check -format -style=file -summary -for-compilers=clang-3.4,gcc-4.8 -include . -exclude third_party {}
diff --git a/scripts/tidy.sh b/scripts/tidy.sh
new file mode 100755
index 0000000..7ecabfa
--- /dev/null
+++ b/scripts/tidy.sh
@@ -0,0 +1,6 @@
+#!/usr/bin/env bash
+
+# Runs the Clang Tidy Tool in parallel on the code base.
+# Requires a compilation database in the build directory.
+
+git ls-files '*.cpp' | grep -v third_party | xargs -I{} -P $(nproc) clang-tidy -p build -header-filter='.*' {}
diff --git a/update_depdendencies.sh b/scripts/update_depdendencies.sh
similarity index 92%
rename from update_depdendencies.sh
rename to scripts/update_depdendencies.sh
index aa2ede5..e443cfa 100755
--- a/update_depdendencies.sh
+++ b/scripts/update_depdendencies.sh
@@ -3,7 +3,7 @@
 OSMIUM_REPO=https://github.com/osmcode/libosmium.git
 OSMIUM_TAG=v2.3.0
 
-VARIANT_REPO=https://github.com/mapbox/variant.git 
+VARIANT_REPO=https://github.com/mapbox/variant.git
 VARIANT_TAG=v1.0
 
 VARIANT_LATEST=$(curl https://api.github.com/repos/mapbox/variant/releases/latest | jq ".tag_name")
diff --git a/server/api_grammar.hpp b/server/api_grammar.hpp
index 325f4c3..28bd1df 100644
--- a/server/api_grammar.hpp
+++ b/server/api_grammar.hpp
@@ -39,11 +39,21 @@ template <typename Iterator, class HandlerT> struct APIGrammar : qi::grammar<Ite
     explicit APIGrammar(HandlerT *h) : APIGrammar::base_type(api_call), handler(h)
     {
         api_call = qi::lit('/') >> string[boost::bind(&HandlerT::setService, handler, ::_1)] >>
-                   *(query) >> -(uturns);
-        query = ('?') >> (+(zoom | output | jsonp | checksum | location | hint | timestamp | u | cmp |
+                   -query;
+        query = ('?') >> +(zoom | output | jsonp | checksum | uturns | location_with_options | destination_with_options | source_with_options |  cmp |
                             language | instruction | geometry | alt_route | old_API | num_results |
-                            matching_beta | gps_precision | classify | locs));
-
+                            matching_beta | gps_precision | classify | locs);
+        // all combinations of timestamp, uturn, hint and bearing without duplicates
+        t_u = (u >> -timestamp) | (timestamp >> -u);
+        t_h = (hint >> -timestamp) | (timestamp >> -hint);
+        u_h = (u >> -hint) | (hint >> -u);
+        t_u_h = (hint >> -t_u) | (u >> -t_h) | (timestamp >> -u_h);
+        location_options = (bearing >> -t_u_h) | (t_u_h >> -bearing) | //
+                           (u >> bearing >> -t_h) | (timestamp >> bearing >> -u_h) | (hint >> bearing >> t_u) | //
+                           (t_h >> bearing >> -u) | (u_h >> bearing >> -timestamp) | (t_u >> bearing >> -hint);
+        location_with_options = location >> -location_options;
+        source_with_options = source >> -location_options;
+        destination_with_options = destination >> -location_options;
         zoom = (-qi::lit('&')) >> qi::lit('z') >> '=' >>
                qi::short_[boost::bind(&HandlerT::setZoomLevel, handler, ::_1)];
         output = (-qi::lit('&')) >> qi::lit("output") >> '=' >>
@@ -61,10 +71,18 @@ template <typename Iterator, class HandlerT> struct APIGrammar : qi::grammar<Ite
         location = (-qi::lit('&')) >> qi::lit("loc") >> '=' >>
                    (qi::double_ >> qi::lit(',') >>
                     qi::double_)[boost::bind(&HandlerT::addCoordinate, handler, ::_1)];
+        destination = (-qi::lit('&')) >> qi::lit("dst") >> '=' >>
+                   (qi::double_ >> qi::lit(',') >>
+                    qi::double_)[boost::bind(&HandlerT::addDestination, handler, ::_1)];
+        source = (-qi::lit('&')) >> qi::lit("src") >> '=' >>
+                   (qi::double_ >> qi::lit(',') >>
+                    qi::double_)[boost::bind(&HandlerT::addSource, handler, ::_1)];
         hint = (-qi::lit('&')) >> qi::lit("hint") >> '=' >>
                stringwithDot[boost::bind(&HandlerT::addHint, handler, ::_1)];
         timestamp = (-qi::lit('&')) >> qi::lit("t") >> '=' >>
                qi::uint_[boost::bind(&HandlerT::addTimestamp, handler, ::_1)];
+        bearing = (-qi::lit('&')) >> qi::lit("b") >> '=' >>
+               (qi::int_ >> -(qi::lit(',') >> qi::int_ | qi::attr(10)))[boost::bind(&HandlerT::addBearing, handler, ::_1, ::_2, ::_3)];
         u = (-qi::lit('&')) >> qi::lit("u") >> '=' >>
             qi::bool_[boost::bind(&HandlerT::setUTurn, handler, ::_1)];
         uturns = (-qi::lit('&')) >> qi::lit("uturns") >> '=' >>
@@ -93,10 +111,10 @@ template <typename Iterator, class HandlerT> struct APIGrammar : qi::grammar<Ite
         stringforPolyline = +(qi::char_("a-zA-Z0-9_.-[]{}@?|\\%~`^"));
     }
 
-    qi::rule<Iterator> api_call, query;
-    qi::rule<Iterator, std::string()> service, zoom, output, string, jsonp, checksum, location,
-        hint, timestamp, stringwithDot, stringwithPercent, language, instruction, geometry, cmp, alt_route, u,
-        uturns, old_API, num_results, matching_beta, gps_precision, classify, locs, stringforPolyline;
+    qi::rule<Iterator> api_call, query, location_options, location_with_options, destination_with_options, source_with_options, t_u, t_h, u_h, t_u_h;
+    qi::rule<Iterator, std::string()> service, zoom, output, string, jsonp, checksum, location, destination, source,
+        hint, timestamp, bearing, stringwithDot, stringwithPercent, language, geometry, cmp, alt_route, u,
+        uturns, old_API, num_results, matching_beta, gps_precision, classify, locs, instruction, stringforPolyline;
 
     HandlerT *handler;
 };
diff --git a/server/data_structures/datafacade_base.hpp b/server/data_structures/datafacade_base.hpp
index baea2bf..5c5c522 100644
--- a/server/data_structures/datafacade_base.hpp
+++ b/server/data_structures/datafacade_base.hpp
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <osrm/coordinate.hpp>
 
 #include <string>
+#include <boost/optional.hpp>
 
 using EdgeRange = osrm::range<EdgeID>;
 
@@ -92,22 +93,22 @@ template <class EdgeDataT> class BaseDataFacade
 
     virtual TravelMode GetTravelModeForEdgeID(const unsigned id) const = 0;
 
-    virtual bool LocateClosestEndPointForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                                    FixedPointCoordinate &result,
-                                                    const unsigned zoom_level = 18) = 0;
+    virtual std::vector<PhantomNodeWithDistance>
+    NearestPhantomNodesInRange(const FixedPointCoordinate &input_coordinate,
+                               const float max_distance,
+                               const int bearing = 0,
+                               const int bearing_range = 180) = 0;
 
-    virtual bool
-    IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                            std::vector<PhantomNode> &resulting_phantom_node_vector,
-                                            const unsigned number_of_results) = 0;
+    virtual std::vector<PhantomNodeWithDistance>
+    NearestPhantomNodes(const FixedPointCoordinate &input_coordinate,
+                        const unsigned max_results,
+                        const int bearing = 0,
+                        const int bearing_range = 180) = 0;
 
-    virtual bool
-    IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                            PhantomNode &resulting_phantom_node) = 0;
-    virtual bool IncrementalFindPhantomNodeForCoordinateWithMaxDistance(
-        const FixedPointCoordinate &input_coordinate,
-        std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
-        const double max_distance) = 0;
+    virtual std::pair<PhantomNode, PhantomNode>
+    NearestPhantomNodeWithAlternativeFromBigComponent(const FixedPointCoordinate &input_coordinate,
+                                                      const int bearing = 0,
+                                                      const int bearing_range = 180) = 0;
 
     virtual unsigned GetCheckSum() const = 0;
 
@@ -117,6 +118,8 @@ template <class EdgeDataT> class BaseDataFacade
 
     virtual std::string get_name_for_id(const unsigned name_id) const = 0;
 
+    virtual std::size_t GetCoreSize() const = 0;
+
     virtual std::string GetTimestamp() const = 0;
 };
 
diff --git a/server/data_structures/internal_datafacade.hpp b/server/data_structures/internal_datafacade.hpp
index 357ee25..a2f74e5 100644
--- a/server/data_structures/internal_datafacade.hpp
+++ b/server/data_structures/internal_datafacade.hpp
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "datafacade_base.hpp"
 
+#include "../../algorithms/geospatial_query.hpp"
 #include "../../data_structures/original_edge_data.hpp"
 #include "../../data_structures/query_node.hpp"
 #include "../../data_structures/query_edge.hpp"
@@ -39,12 +40,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "../../data_structures/static_graph.hpp"
 #include "../../data_structures/static_rtree.hpp"
 #include "../../data_structures/range_table.hpp"
-#include "../../util/boost_filesystem_2_fix.hpp"
 #include "../../util/graph_loader.hpp"
 #include "../../util/simple_logger.hpp"
 
 #include <osrm/coordinate.hpp>
-#include <osrm/server_paths.hpp>
+
+#include <boost/thread.hpp>
 
 #include <limits>
 
@@ -56,12 +57,14 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
     using QueryGraph = StaticGraph<typename super::EdgeData>;
     using InputEdge = typename QueryGraph::InputEdge;
     using RTreeLeaf = typename super::RTreeLeaf;
+    using InternalRTree = StaticRTree<RTreeLeaf, ShM<FixedPointCoordinate, false>::vector, false>;
+    using InternalGeospatialQuery = GeospatialQuery<InternalRTree>;
 
     InternalDataFacade() {}
 
     unsigned m_check_sum;
     unsigned m_number_of_nodes;
-    QueryGraph *m_query_graph;
+    std::unique_ptr<QueryGraph> m_query_graph;
     std::string m_timestamp;
 
     std::shared_ptr<ShM<FixedPointCoordinate, false>::vector> m_coordinate_list;
@@ -75,8 +78,8 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
     ShM<unsigned, false>::vector m_geometry_list;
     ShM<bool, false>::vector m_is_core_node;
 
-    boost::thread_specific_ptr<
-        StaticRTree<RTreeLeaf, ShM<FixedPointCoordinate, false>::vector, false>> m_static_rtree;
+    boost::thread_specific_ptr<InternalRTree> m_static_rtree;
+    boost::thread_specific_ptr<InternalGeospatialQuery> m_geospatial_query;
     boost::filesystem::path ram_index_path;
     boost::filesystem::path file_index_path;
     RangeTable<16, false> m_name_table;
@@ -117,7 +120,7 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
         // BOOST_ASSERT_MSG(0 != edge_list.size(), "edge list empty");
         SimpleLogger().Write() << "loaded " << node_list.size() << " nodes and " << edge_list.size()
                                << " edges";
-        m_query_graph = new QueryGraph(node_list, edge_list);
+        m_query_graph = std::unique_ptr<QueryGraph>(new QueryGraph(node_list, edge_list));
 
         BOOST_ASSERT_MSG(0 == node_list.size(), "node list not flushed");
         BOOST_ASSERT_MSG(0 == edge_list.size(), "edge list not flushed");
@@ -179,7 +182,7 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
         core_stream.read((char *)&number_of_markers, sizeof(unsigned));
 
         std::vector<char> unpacked_core_markers(number_of_markers);
-        core_stream.read((char *)unpacked_core_markers.data(), sizeof(char)*number_of_markers);
+        core_stream.read((char *)unpacked_core_markers.data(), sizeof(char) * number_of_markers);
 
         // in this case we have nothing to do
         if (number_of_markers <= 0)
@@ -227,8 +230,8 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
     {
         BOOST_ASSERT_MSG(!m_coordinate_list->empty(), "coordinates must be loaded before r-tree");
 
-        m_static_rtree.reset(
-            new StaticRTree<RTreeLeaf>(ram_index_path, file_index_path, m_coordinate_list));
+        m_static_rtree.reset(new InternalRTree(ram_index_path, file_index_path, m_coordinate_list));
+        m_geospatial_query.reset(new InternalGeospatialQuery(*m_static_rtree, m_coordinate_list));
     }
 
     void LoadStreetNames(const boost::filesystem::path &names_file)
@@ -252,96 +255,43 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
   public:
     virtual ~InternalDataFacade()
     {
-        delete m_query_graph;
         m_static_rtree.reset();
+        m_geospatial_query.reset();
     }
 
-    explicit InternalDataFacade(const ServerPaths &server_paths)
+    explicit InternalDataFacade(const std::unordered_map<std::string, boost::filesystem::path> &server_paths)
     {
-        // generate paths of data files
-        if (server_paths.find("hsgrdata") == server_paths.end())
-        {
-            throw osrm::exception("no hsgr file given in ini file");
-        }
-        if (server_paths.find("ramindex") == server_paths.end())
-        {
-            throw osrm::exception("no ram index file given in ini file");
-        }
-        if (server_paths.find("fileindex") == server_paths.end())
-        {
-            throw osrm::exception("no leaf index file given in ini file");
-        }
-        if (server_paths.find("geometries") == server_paths.end())
-        {
-            throw osrm::exception("no geometries file given in ini file");
-        }
-        if (server_paths.find("nodesdata") == server_paths.end())
-        {
-            throw osrm::exception("no nodes file given in ini file");
-        }
-        if (server_paths.find("coredata") == server_paths.end())
-        {
-            throw osrm::exception("no core file given in ini file");
-        }
-        if (server_paths.find("edgesdata") == server_paths.end())
-        {
-            throw osrm::exception("no edges file given in ini file");
-        }
-        if (server_paths.find("namesdata") == server_paths.end())
+        // cache end iterator to quickly check .find against
+        const auto end_it = end(server_paths);
+
+        const auto file_for = [&server_paths, &end_it](const std::string &path)
         {
-            throw osrm::exception("no names file given in ini file");
-        }
+            const auto it = server_paths.find(path);
+            if (it == end_it || !boost::filesystem::is_regular_file(it->second))
+                throw osrm::exception("no valid " + path + " file given in ini file");
+            return it->second;
+        };
+
+        ram_index_path = file_for("ramindex");
+        file_index_path = file_for("fileindex");
 
-        auto paths_iterator = server_paths.find("hsgrdata");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        const boost::filesystem::path &hsgr_path = paths_iterator->second;
-        paths_iterator = server_paths.find("timestamp");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        const boost::filesystem::path &timestamp_path = paths_iterator->second;
-        paths_iterator = server_paths.find("ramindex");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        ram_index_path = paths_iterator->second;
-        paths_iterator = server_paths.find("fileindex");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        file_index_path = paths_iterator->second;
-        paths_iterator = server_paths.find("nodesdata");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        const boost::filesystem::path &nodes_data_path = paths_iterator->second;
-        paths_iterator = server_paths.find("edgesdata");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        const boost::filesystem::path &edges_data_path = paths_iterator->second;
-        paths_iterator = server_paths.find("namesdata");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        const boost::filesystem::path &names_data_path = paths_iterator->second;
-        paths_iterator = server_paths.find("geometries");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        const boost::filesystem::path &geometries_path = paths_iterator->second;
-        paths_iterator = server_paths.find("coredata");
-        BOOST_ASSERT(server_paths.end() != paths_iterator);
-        const boost::filesystem::path &core_data_path = paths_iterator->second;
-
-        // load data
         SimpleLogger().Write() << "loading graph data";
-        AssertPathExists(hsgr_path);
-        LoadGraph(hsgr_path);
+        LoadGraph(file_for("hsgrdata"));
+
         SimpleLogger().Write() << "loading edge information";
-        AssertPathExists(nodes_data_path);
-        AssertPathExists(edges_data_path);
-        LoadNodeAndEdgeInformation(nodes_data_path, edges_data_path);
+        LoadNodeAndEdgeInformation(file_for("nodesdata"), file_for("edgesdata"));
+
         SimpleLogger().Write() << "loading core information";
-        AssertPathExists(core_data_path);
-        LoadCoreInformation(core_data_path);
+        LoadCoreInformation(file_for("coredata"));
+
         SimpleLogger().Write() << "loading geometries";
-        AssertPathExists(geometries_path);
-        LoadGeometries(geometries_path);
-        SimpleLogger().Write() << "loading r-tree";
-        AssertPathExists(ram_index_path);
-        AssertPathExists(file_index_path);
+        LoadGeometries(file_for("geometries"));
+
         SimpleLogger().Write() << "loading timestamp";
-        LoadTimestamp(timestamp_path);
+        LoadTimestamp(file_for("timestamp"));
+
         SimpleLogger().Write() << "loading street names";
-        AssertPathExists(names_data_path);
-        LoadStreetNames(names_data_path);
+        LoadStreetNames(file_for("namesdata"));
     }
 
     // search graph access
@@ -408,62 +358,52 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
         return m_travel_mode_list.at(id);
     }
 
-    bool LocateClosestEndPointForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                            FixedPointCoordinate &result,
-                                            const unsigned zoom_level = 18) override final
+
+    std::vector<PhantomNodeWithDistance>
+    NearestPhantomNodesInRange(const FixedPointCoordinate &input_coordinate,
+                               const float max_distance,
+                               const int bearing = 0,
+                               const int bearing_range = 180) override final
     {
         if (!m_static_rtree.get())
         {
             LoadRTree();
+            BOOST_ASSERT(m_geospatial_query.get());
         }
 
-        return m_static_rtree->LocateClosestEndPointForCoordinate(input_coordinate, result,
-                                                                  zoom_level);
+        return m_geospatial_query->NearestPhantomNodesInRange(input_coordinate, max_distance, bearing, bearing_range);
     }
 
-    bool IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                                 PhantomNode &resulting_phantom_node) override final
-    {
-        std::vector<PhantomNode> resulting_phantom_node_vector;
-        auto result = IncrementalFindPhantomNodeForCoordinate(input_coordinate,
-                                                              resulting_phantom_node_vector, 1);
-        if (result)
-        {
-            BOOST_ASSERT(!resulting_phantom_node_vector.empty());
-            resulting_phantom_node = resulting_phantom_node_vector.front();
-        }
-
-        return result;
-    }
-
-    bool
-    IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                            std::vector<PhantomNode> &resulting_phantom_node_vector,
-                                            const unsigned number_of_results) override final
+    std::vector<PhantomNodeWithDistance>
+    NearestPhantomNodes(const FixedPointCoordinate &input_coordinate,
+                        const unsigned max_results,
+                        const int bearing = 0,
+                        const int bearing_range = 180) override final
     {
         if (!m_static_rtree.get())
         {
             LoadRTree();
+            BOOST_ASSERT(m_geospatial_query.get());
         }
 
-        return m_static_rtree->IncrementalFindPhantomNodeForCoordinate(
-            input_coordinate, resulting_phantom_node_vector, number_of_results);
+        return m_geospatial_query->NearestPhantomNodes(input_coordinate, max_results, bearing, bearing_range);
     }
 
-    bool IncrementalFindPhantomNodeForCoordinateWithMaxDistance(
-        const FixedPointCoordinate &input_coordinate,
-        std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
-        const double max_distance) override final
+    std::pair<PhantomNode, PhantomNode>
+    NearestPhantomNodeWithAlternativeFromBigComponent(const FixedPointCoordinate &input_coordinate,
+                                                      const int bearing = 0,
+                                                      const int bearing_range = 180) override final
     {
         if (!m_static_rtree.get())
         {
             LoadRTree();
+            BOOST_ASSERT(m_geospatial_query.get());
         }
 
-        return m_static_rtree->IncrementalFindPhantomNodeForCoordinateWithDistance(
-            input_coordinate, resulting_phantom_node_vector, max_distance);
+        return m_geospatial_query->NearestPhantomNodeWithAlternativeFromBigComponent(input_coordinate, bearing, bearing_range);
     }
 
+
     unsigned GetCheckSum() const override final { return m_check_sum; }
 
     unsigned GetNameIndexFromEdgeID(const unsigned id) const override final
@@ -495,6 +435,11 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
         return m_via_node_list.at(id);
     }
 
+    virtual std::size_t GetCoreSize() const override final
+    {
+        return m_is_core_node.size();
+    }
+
     virtual bool IsCoreNode(const NodeID id) const override final
     {
         if (m_is_core_node.size() > 0)
diff --git a/server/data_structures/shared_datafacade.hpp b/server/data_structures/shared_datafacade.hpp
index 928a125..024a894 100644
--- a/server/data_structures/shared_datafacade.hpp
+++ b/server/data_structures/shared_datafacade.hpp
@@ -33,13 +33,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "datafacade_base.hpp"
 #include "shared_datatype.hpp"
 
+#include "../../algorithms/geospatial_query.hpp"
 #include "../../data_structures/range_table.hpp"
 #include "../../data_structures/static_graph.hpp"
 #include "../../data_structures/static_rtree.hpp"
-#include "../../util/boost_filesystem_2_fix.hpp"
 #include "../../util/make_unique.hpp"
 #include "../../util/simple_logger.hpp"
 
+#include <boost/thread.hpp>
+
 #include <algorithm>
 #include <limits>
 #include <memory>
@@ -57,6 +59,7 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
     using InputEdge = typename QueryGraph::InputEdge;
     using RTreeLeaf = typename super::RTreeLeaf;
     using SharedRTree = StaticRTree<RTreeLeaf, ShM<FixedPointCoordinate, true>::vector, true>;
+    using SharedGeospatialQuery = GeospatialQuery<SharedRTree>;
     using TimeStampedRTreePair = std::pair<unsigned, std::shared_ptr<SharedRTree>>;
     using RTreeNode = typename SharedRTree::TreeNode;
 
@@ -87,6 +90,7 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
     ShM<bool, true>::vector m_is_core_node;
 
     boost::thread_specific_ptr<std::pair<unsigned, std::shared_ptr<SharedRTree>>> m_static_rtree;
+    boost::thread_specific_ptr<SharedGeospatialQuery> m_geospatial_query;
     boost::filesystem::path file_index_path;
 
     std::shared_ptr<RangeTable<16, true>> m_name_table;
@@ -119,6 +123,7 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
             osrm::make_unique<SharedRTree>(
                 tree_ptr, data_layout->num_entries[SharedDataLayout::R_SEARCH_TREE],
                 file_index_path, m_coordinate_list)));
+        m_geospatial_query.reset(new SharedGeospatialQuery(*m_static_rtree->second, m_coordinate_list));
     }
 
     void LoadGraph()
@@ -273,7 +278,7 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
             if (!boost::filesystem::exists(file_index_path))
             {
                 SimpleLogger().Write(logDEBUG) << "Leaf file name " << file_index_path.string();
-                throw osrm::exception("Could not load leaf index file."
+                throw osrm::exception("Could not load leaf index file. "
                                       "Is any data loaded into shared memory?");
             }
 
@@ -379,60 +384,48 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
         return m_travel_mode_list.at(id);
     }
 
-    bool LocateClosestEndPointForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                            FixedPointCoordinate &result,
-                                            const unsigned zoom_level = 18) override final
+    std::vector<PhantomNodeWithDistance>
+    NearestPhantomNodesInRange(const FixedPointCoordinate &input_coordinate,
+                               const float max_distance,
+                               const int bearing = 0,
+                               const int bearing_range = 180) override final
     {
         if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first)
         {
             LoadRTree();
+            BOOST_ASSERT(m_geospatial_query.get());
         }
 
-        return m_static_rtree->second->LocateClosestEndPointForCoordinate(input_coordinate, result,
-                                                                          zoom_level);
+        return m_geospatial_query->NearestPhantomNodesInRange(input_coordinate, max_distance, bearing, bearing_range);
     }
 
-    bool IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                                 PhantomNode &resulting_phantom_node) override final
-    {
-        std::vector<PhantomNode> resulting_phantom_node_vector;
-        auto result = IncrementalFindPhantomNodeForCoordinate(input_coordinate,
-                                                              resulting_phantom_node_vector, 1);
-        if (result)
-        {
-            BOOST_ASSERT(!resulting_phantom_node_vector.empty());
-            resulting_phantom_node = resulting_phantom_node_vector.front();
-        }
-
-        return result;
-    }
-
-    bool
-    IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                            std::vector<PhantomNode> &resulting_phantom_node_vector,
-                                            const unsigned number_of_results) override final
+    std::vector<PhantomNodeWithDistance>
+    NearestPhantomNodes(const FixedPointCoordinate &input_coordinate,
+                        const unsigned max_results,
+                        const int bearing = 0,
+                        const int bearing_range = 180) override final
     {
         if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first)
         {
             LoadRTree();
+            BOOST_ASSERT(m_geospatial_query.get());
         }
 
-        return m_static_rtree->second->IncrementalFindPhantomNodeForCoordinate(
-            input_coordinate, resulting_phantom_node_vector, number_of_results);
+        return m_geospatial_query->NearestPhantomNodes(input_coordinate, max_results, bearing, bearing_range);
     }
 
-    bool IncrementalFindPhantomNodeForCoordinateWithMaxDistance(
-        const FixedPointCoordinate &input_coordinate,
-        std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
-        const double max_distance) override final
+    std::pair<PhantomNode, PhantomNode>
+    NearestPhantomNodeWithAlternativeFromBigComponent(const FixedPointCoordinate &input_coordinate,
+                                                      const int bearing = 0,
+                                                      const int bearing_range = 180) override final
     {
         if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first)
         {
             LoadRTree();
+            BOOST_ASSERT(m_geospatial_query.get());
         }
 
-        return m_static_rtree->second->IncrementalFindPhantomNodeForCoordinateWithDistance(
-            input_coordinate, resulting_phantom_node_vector, max_distance);
+        return m_geospatial_query->NearestPhantomNodeWithAlternativeFromBigComponent(input_coordinate, bearing, bearing_range);
     }
 
     unsigned GetCheckSum() const override final { return m_check_sum; }
@@ -471,6 +464,11 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
         return false;
     }
 
+    virtual std::size_t GetCoreSize() const override final
+    {
+        return m_is_core_node.size();
+    }
+
     std::string GetTimestamp() const override final { return m_timestamp; }
 };
 
diff --git a/server/http/reply.cpp b/server/http/reply.cpp
index 036d1ae..a3d3d96 100644
--- a/server/http/reply.cpp
+++ b/server/http/reply.cpp
@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "reply.hpp"
 
-#include "../../util/cast.hpp"
+#include <string>
 
 namespace http
 {
@@ -48,7 +48,7 @@ void reply::set_size(const std::size_t size)
     {
         if ("Content-Length" == h.name)
         {
-            h.value = cast::integral_to_string(size);
+            h.value = std::to_string(size);
         }
     }
 }
@@ -95,7 +95,7 @@ reply reply::stock_reply(const reply::status_type status)
     const std::string status_string = reply.status_to_string(status);
     reply.content.insert(reply.content.end(), status_string.begin(), status_string.end());
     reply.headers.emplace_back("Access-Control-Allow-Origin", "*");
-    reply.headers.emplace_back("Content-Length", cast::integral_to_string(reply.content.size()));
+    reply.headers.emplace_back("Content-Length", std::to_string(reply.content.size()));
     reply.headers.emplace_back("Content-Type", "text/html");
     return reply;
 }
diff --git a/server/request_handler.cpp b/server/request_handler.cpp
index d1c6e62..54f0395 100644
--- a/server/request_handler.cpp
+++ b/server/request_handler.cpp
@@ -31,7 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "http/reply.hpp"
 #include "http/request.hpp"
 
-#include "../library/osrm.hpp"
 #include "../util/json_renderer.hpp"
 #include "../util/simple_logger.hpp"
 #include "../util/string_util.hpp"
@@ -40,17 +39,21 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <osrm/route_parameters.hpp>
 #include <osrm/json_container.hpp>
+#include <osrm/osrm.hpp>
 
 #include <ctime>
 
 #include <algorithm>
 #include <iostream>
+#include <string>
 
 RequestHandler::RequestHandler() : routing_machine(nullptr) {}
 
 void RequestHandler::handle_request(const http::request &current_request,
                                     http::reply &current_reply)
 {
+    osrm::json::Object json_result;
+
     // parse command
     try
     {
@@ -92,40 +95,40 @@ void RequestHandler::handle_request(const http::request &current_request,
         const bool result =
             boost::spirit::qi::parse(api_iterator, request_string.end(), api_parser);
 
-        osrm::json::Object json_result;
         // check if the was an error with the request
-        if (!result || (api_iterator != request_string.end()))
+        if (result && api_iterator == request_string.end())
         {
-            current_reply = http::reply::stock_reply(http::reply::bad_request);
-            current_reply.content.clear();
-            const auto position = std::distance(request_string.begin(), api_iterator);
-
-            json_result.values["status"] = 400;
-            std::string message = "Query string malformed close to position ";
-            message += cast::integral_to_string(position);
-            json_result.values["status_message"] = message;
-            osrm::json::render(current_reply.content, json_result);
-            return;
-        }
-
-        // parsing done, lets call the right plugin to handle the request
-        BOOST_ASSERT_MSG(routing_machine != nullptr, "pointer not init'ed");
-
-        if (!route_parameters.jsonp_parameter.empty())
-        { // prepend response with jsonp parameter
-            const std::string json_p = (route_parameters.jsonp_parameter + "(");
-            current_reply.content.insert(current_reply.content.end(), json_p.begin(), json_p.end());
+            // parsing done, lets call the right plugin to handle the request
+            BOOST_ASSERT_MSG(routing_machine != nullptr, "pointer not init'ed");
+
+            if (!route_parameters.jsonp_parameter.empty())
+            { // prepend response with jsonp parameter
+                const std::string json_p = (route_parameters.jsonp_parameter + "(");
+                current_reply.content.insert(current_reply.content.end(), json_p.begin(), json_p.end());
+            }
+
+            const int return_code = routing_machine->RunQuery(route_parameters, json_result);
+            json_result.values["status"] = return_code;
+            // 4xx bad request return code
+            if (return_code / 100 == 4)
+            {
+                current_reply.status = http::reply::bad_request;
+                current_reply.content.clear();
+                route_parameters.output_format.clear();
+            }
+            else
+            {
+                // 2xx valid request
+                BOOST_ASSERT(return_code / 100 == 2);
+            }
         }
-        const auto return_code = routing_machine->RunQuery(route_parameters, json_result);
-        if (200 != return_code)
+        else
         {
-            current_reply = http::reply::stock_reply(http::reply::bad_request);
-            current_reply.content.clear();
-            json_result.values["status"] = 400;
-            std::string message = "Bad Request";
-            json_result.values["status_message"] = message;
-            osrm::json::render(current_reply.content, json_result);
-            return;
+            const auto position = std::distance(request_string.begin(), api_iterator);
+
+            current_reply.status = http::reply::bad_request;
+            json_result.values["status"] = http::reply::bad_request;
+            json_result.values["status_message"] = "Query string malformed close to position " + std::to_string(position);
         }
 
         current_reply.headers.emplace_back("Access-Control-Allow-Origin", "*");
@@ -135,7 +138,7 @@ void RequestHandler::handle_request(const http::request &current_request,
 
         // set headers
         current_reply.headers.emplace_back("Content-Length",
-                                           cast::integral_to_string(current_reply.content.size()));
+                                           std::to_string(current_reply.content.size()));
         if ("gpx" == route_parameters.output_format)
         { // gpx file
             osrm::json::gpx_render(current_reply.content, json_result.values["route"]);
@@ -165,10 +168,9 @@ void RequestHandler::handle_request(const http::request &current_request,
     }
     catch (const std::exception &e)
     {
-        current_reply = http::reply::stock_reply(http::reply::internal_server_error);
+        current_reply = http::reply::stock_reply(http::reply::internal_server_error);;
         SimpleLogger().Write(logWARNING) << "[server error] code: " << e.what()
                                          << ", uri: " << current_request.uri;
-        return;
     }
 }
 
diff --git a/server/request_parser.cpp b/server/request_parser.cpp
index 502e6e0..b52d730 100644
--- a/server/request_parser.cpp
+++ b/server/request_parser.cpp
@@ -328,11 +328,14 @@ osrm::tribool RequestParser::consume(request &current_request, const char input)
         }
         return osrm::tribool::no;
     case internal_state::expecting_newline_3:
-        if(input == '\n')
+        if (input == '\n')
         {
-            if(is_post_header)
+            if (is_post_header)
             {
-                current_request.uri.push_back('?');
+		if (content_length > 0)
+		{
+		    current_request.uri.push_back('?');
+		}
                 state = internal_state::post_request;
                 return osrm::tribool::indeterminate;
             }
diff --git a/server/server.hpp b/server/server.hpp
index 0ec3163..80fae97 100644
--- a/server/server.hpp
+++ b/server/server.hpp
@@ -31,7 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "connection.hpp"
 #include "request_handler.hpp"
 
-#include "../util/cast.hpp"
 #include "../util/integer_range.hpp"
 #include "../util/simple_logger.hpp"
 
@@ -44,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <memory>
 #include <thread>
 #include <vector>
+#include <string>
 
 class Server
 {
@@ -62,7 +62,7 @@ class Server
         : thread_pool_size(thread_pool_size), acceptor(io_service),
           new_connection(std::make_shared<http::Connection>(io_service, request_handler))
     {
-        const std::string port_string = cast::integral_to_string(port);
+        const auto port_string = std::to_string(port);
 
         boost::asio::ip::tcp::resolver resolver(io_service);
         boost::asio::ip::tcp::resolver::query query(address, port_string);
diff --git a/test/.stxxl b/test/.stxxl
index b1765e2..61cbf03 100644
--- a/test/.stxxl
+++ b/test/.stxxl
@@ -1 +1 @@
-disk=/tmp/stxxl,10,syscall
+disk=###.stxxl,20,memory
diff --git a/third_party/fast-cpp-csv-parser/LICENSE b/third_party/fast-cpp-csv-parser/LICENSE
new file mode 100644
index 0000000..da603a9
--- /dev/null
+++ b/third_party/fast-cpp-csv-parser/LICENSE
@@ -0,0 +1,28 @@
+Copyright (c) 2015, ben-strasser
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of fast-cpp-csv-parser nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
diff --git a/third_party/fast-cpp-csv-parser/README.md b/third_party/fast-cpp-csv-parser/README.md
new file mode 100644
index 0000000..546fa1e
--- /dev/null
+++ b/third_party/fast-cpp-csv-parser/README.md
@@ -0,0 +1,252 @@
+# Fast C++ Csv Parser
+
+This is a small, easy-to-use and fast header-only library for reading comma separated value (CSV) files. 
+
+## Features
+
+  * Automatically rearranges columns by parsing the header line.
+  * Disk I/O and CSV-parsing are overlapped using threads for efficiency.
+  * Parsing features such as escaped strings can be enabled and disabled at compile time using templates. You only pay in speed for the features you actually use.
+  * Can read multiple GB files in reasonable time.
+  * Support for custom columns separators (i.e. Tab separated value files are supported), quote escaped strings, automatic space trimming. 
+  * Works with `*`nix and Windows newlines and automatically ignores UTF-8 BOMs.
+  * Exception classes with enough context to format useful error messages. what() returns error messages ready to be shown to a user. 
+
+## Getting Started
+
+The following small example should contain most of the syntax you need to use the library.
+
+```cpp
+# include "csv.h"
+
+int main(){
+  io::CSVReader<3> in("ram.csv");
+  in.read_header(io::ignore_extra_column, "vendor", "size", "speed");
+  std::string vendor; int size; double speed;
+  while(in.read_row(vendor, size, speed)){
+    // do stuff with the data
+  }
+}
+```
+
+## Installation
+
+The library only needs a standard conformant C++11 compiler. It has no further dependencies. The library is completely contained inside a single header file and therefore it is sufficient to copy this file to some place on your include path. The library does not have to be explicitly build. 
+
+Note however, that std::future is used and some compiler (f.e. GCC) require you to link against additional libraries (i.e. -lpthread) to make it work. With GCC it is important to add -lpthread as the last item when linking, i.e. the order in 
+
+```
+g++ a.o b.o -o prog -lpthread
+```
+
+is important.
+
+Remember that the library makes use of C++11 features and therefore you have to enable support for it (f.e. add -std=C++0x or -std=gnu++0x).
+
+The library was developed and tested with GCC 4.6.1
+
+Note that VS2013 is not C++11 compilant and will therefore not work out of the box. See [here](https://code.google.com/p/fast-cpp-csv-parser/issues/detail?id=6) for what needs to be adjusted to make the code work.
+
+## Documentation
+
+The libary provides two classes: 
+
+  * `LineReader`: A class to efficiently read large files line by line.
+  * `CSVReader`: A class that efficiently reads large CSV files.
+
+Note that everything is contained in the `io` namespace.
+
+### `LineReader`
+
+```cpp
+class LineReader{
+public:
+  // Constructors
+  LineReader(some_string_type file_name);
+  LineReader(some_string_type file_name, std::FILE*file);
+
+  // Reading
+  char*next_line();
+
+  // File Location
+  void set_file_line(unsigned);
+  unsigned get_file_line(unsigned)const;
+  void set_file_name(some_string_type file_name);
+  const char*get_truncated_file_name()const;
+};
+```
+
+The constructor takes a file name and optionally a `stdio.h` file handle. If no file handle is provided the class tries to open the file and throws an `error::can_not_open_file exception` on failure. If a file handle is provided then the file name is only used to format error messages. The library will call `std::fclose` on the file handle. `some_string_type` can be a `std::string` or a `char*`.
+
+Lines are read by calling the `next_line` function. It returns a pointer to a null terminated C-string that contains the line. If the end of file is reached a null pointer is returned. The newline character is not included in the string. You may modify the string as long as you do not write past the null terminator. The string stays valid until the destructor is called or until next_line is called again. Windows and `*`nix newlines are handled transparently. UTF-8 BOMs are automatically  [...]
+
+**Important:** There is a limit of 2^24-1 characters per line. If this limit is exceeded a `error::line_length_limit_exceeded` exception is thrown.
+
+Looping over all the lines in a file can be done in the following way.
+```cpp
+LineReader in(...);
+while(char*line = in.next_line()){
+  ...
+}
+```
+
+The remaining functions are mainly used used to format error messages. The file line indicates the current position in the file, i.e., after the first `next_line` call it is 1 and after the second 2. Before the first call it is 0. The file name is truncated as internally C-strings are used to avoid `std::bad_alloc` exceptions during error reporting.
+
+**Note:** It is not possible to exchange the line termination character.
+
+### `CSVReader`
+
+`CSVReader` uses policies. These are classes with only static members to allow core functionality to be exchanged in an efficient way.
+
+```cpp
+template<
+  unsigned column_count,
+  class trim_policy = trim_chars<' ', '\t'>, 
+  class quote_policy = no_quote_escape<','>,
+  class overflow_policy = throw_on_overflow,
+  class comment_policy = no_comment
+>
+class CSVReader{
+public:
+  // Constructors
+  CSVReader(some_string_type file_name);
+  CSVReader(some_string_type file_name, std::FILE*file);
+
+  // Parsing Header
+  void read_header(ignore_column ignore_policy, some_string_type col_name1, some_string_type col_name2, ...);
+  void set_header(some_string_type col_name1, some_string_type col_name2, ...);
+  bool has_column(some_string_type col_name)const;
+
+  // Read
+  bool read_row(ColType1&col1, ColType2&col2, ...);
+
+  // File Location 
+  void set_file_line(unsigned);
+  unsigned get_file_line(unsigned)const;
+  void set_file_name(some_string_type file_name);
+  const char*get_truncated_file_name()const;
+};
+```
+
+The `column_count` template parameter indicates how many columns you want to read from the CSV file. This must not necessarily coincide with the actual number of columns in the file. The three policies govern various aspects of the parsing.
+
+The trim policy indicates what characters should be ignored at the begin and the end of every column. The default ignores spaces and tabs. This makes sure that
+
+```
+a,b,c
+1,2,3
+```
+
+is interpreted in the same way as
+
+```
+  a, b,   c
+1  , 2,   3
+```
+
+The trim_chars can take any number of template parameters. For example `trim_chars<' ', '\t', '_'> `is also valid. If no character should be trimmed use `trim_chars<>`.
+
+The quote policy indicates how string should be escaped. It also specifies the column separator. The predefined policies are:
+
+  * `no_quote_escape<sep>` : Strings are not escaped. "`sep`" is used as column separator.
+  * `double_quote_escape<sep, quote>` : Strings are escaped using quotes. Quotes are escaped using two consecutive quotes. "`sep`" is used as column separator and "`quote`" as quoting character.
+
+**Important**: When combining trimming and quoting the rows are first trimmed and then unquoted. A consequence is that spaces inside the quotes will be conserved. If you want to get rid of spaces inside the quotes, you need to remove them yourself.
+
+**Important**: Quoting can be quite expensive. Disable it if you do not need it.
+
+The overflow policy indicates what should be done if the integers in the input are too large to fit into the variables. There following policies are predefined:
+
+  * `throw_on_overflow` : Throw an `error::integer_overflow` or `error::integer_underflow` exception.
+  * `ignore_overflow` : Do nothing and let the overflow happen.
+  * `set_to_max_on_overflow` : Set the value to `numeric_limits<...>::max()` (or to the min-pendant).
+
+The comment policy allows to skip lines based on some criteria. Valid predefined policies are:
+
+  * `no_comment` : Do not ignore any line.
+  * `empty_line_comment` : Ignore all lines that are empty or only contains spaces and tabs. 
+  * `single_line_comment<com1, com2, ...>` : Ignore all lines that start with com1 or com2 or ... as the first character. There may not be any space between the beginning of the line and the comment character. 
+  * `single_and_empty_line_comment<com1, com2, ...>` : Ignore all empty lines and single line comments.
+
+Examples:
+
+  * `CSVReader<4, trim_chars<' '>, double_quote_escape<',','\"'> >` reads 4 columns from a normal CSV file with string escaping enabled.
+  * `CSVReader<3, trim_chars<' '>, no_quote_escape<'\t'>, single_line_comment<'#'> >` reads 3 columns from a tab separated file with string escaping disabled. Lines starting with a # are ignored.
+
+The constructors and the file location functions are exactly the same as for `LineReader`. See its documentation for details.
+
+There are three methods that deal with headers. The `read_header` methods reads a line from the file and rearranges the columns to match that order. It also checks whether all necessary columns are present. The `set_header` method does *not* read any input. Use it if the file does not have any header. Obviously it is impossible to rearrange columns or check for their availability when using it. The order in the file and in the program must match when using `set_header`. The `has_column`  [...]
+
+  * `ignore_no_column`: The default behavior, no flags are set
+  * `ignore_extra_column`: If a column with a name is in the file but not in the argument list, then it is silently ignored.
+  * `ignore_missing_column`: If a column with a name is not in the file but is in the argument list, then `read_row` will not modify the corresponding variable. 
+
+When using `ignore_column_missing` it is a good idea to initialize the variables passed to `read_row` with a default value, for example:
+
+```cpp
+// The file only contains column "a"
+CSVReader<2>in(...);
+in.read_header(ignore_missing_column, "a", "b");
+int a,b = 42;
+while(in.read_row(a,b)){
+  // a contains the value from the file
+  // b is left unchanged by read_row, i.e., it is 42
+}
+```
+
+If only some columns are optional or their default value depends on other columns you have to use `has_column`, for example:
+
+```cpp
+// The file only contains the columns "a" and "b"
+CSVReader<2>in(...);
+in.read_header(ignore_missing_column, "a", "b", "sum");
+if(!in.has_column("a") || !in.has_column("b"))
+  throw my_neat_error_class();
+bool has_sum = in.has_column("sum");
+int a,b,sum;
+while(in.read_row(a,b,sum)){
+  if(!has_sum)
+    sum = a+b;
+}
+```
+
+**Important**: Do not call `has_column` from within the read-loop. It would work correctly but significantly slowdown processing.
+
+If two columns have the same name an error::duplicated_column_in_header exception is thrown. If `read_header` is called but the file is empty a `error::header_missing` exception is thrown.
+
+The `read_row` function reads a line, splits it into the columns and arranges them correctly. It trims the entries and unescapes them. If requested the content is interpreted as integer or as floating point. The variables passed to read_row may be of the following types.
+
+  * builtin signed integer: These are `signed char`, `short`, `int`, `long` and `long long`. The input must be encoded as a base 10 ASCII number optionally preceded by a + or -. The function detects whether the integer is too large would overflow (or underflow) and behaves as indicated by overflow_policy.
+  * builtin unsigned integer: Just as the signed counterparts except that a leading + or - is not allowed.
+  * builtin floating point: These are `float`, `double` and `long double`. The input may have a leading + or -. The number must be base 10 encoded. The decimal point may either be a dot or a comma. (Note that a comma will only work if it is not also used as column separator or the number is escaped.) A base 10 exponent may be specified using the "1e10" syntax. The "e" may be lower- or uppercase. Examples for valid floating points are "1", "-42.42" and "+123.456E789". The input is rounded [...]
+  * `char`: The column content must be a single character.
+  * `std::string`: The column content is assigned to the string. The std::string is filled with the trimmed and unescaped version.
+  * `char*`: A pointer directly into the buffer. The string is trimmed and unescaped and null terminated. This pointer stays valid until read_row is called again or the CSVReader is destroyed. Use this for user defined types. 
+
+Note that there is no inherent overhead to using `char*` and then interpreting it compared to using one of the parsers directly build into `CSVReader`. The builtin number parsers are pure convenience. If you need a slightly different syntax then use `char*` and do the parsing yourself.
+
+## FAQ
+
+Q: The library is throwing a std::system_error with code -1. How to get it to work?
+
+A: Your compiler's std::thread implementation is broken. Define CSV\_IO\_NO\_THREAD to disable threading support.
+
+
+Q: My values are not just ints or strings. I want to parse my customized type. Is this possible?
+
+A: Read a `char*` and parse the string. At first this seems expensive but it is not as the pointer you get points directly into the memory buffer. In fact there is no inherent reason why a custom int-parser realized this way must be any slower than the int-parser build into the library. By reading a `char*` the library takes care of column reordering and quote escaping and leaves the actual parsing to you. Note that using a std::string is slower as it involves a memory copy.
+
+
+Q: I get lots of compiler errors when compiling the header! Please fix it. :(
+
+A: Have you enabled the C++11 mode of your compiler? If you use GCC you have to add -std=c++0x to the commandline. If this does not resolve the problem, then please open a ticket.
+
+
+Q: The library crashes when parsing large files! Please fix it. :(
+
+A: When using GCC have you linked against -lpthread? Read the installation section for details on how to do this. If this does not resolve the issue then please open a ticket. (The reason why it only crashes only on large files is that the first chuck is read synchronous and if the whole file fits into this chuck then no asynchronous call is performed.) Alternatively you can define CSV\_IO\_NO\_THREAD.
+
+
+Q: Does the library support UTF?
+
+A: The library has basic UTF-8 support, or to be more precise it does not break when passing UTF-8 strings through it. If you read a `char*` then you get a pointer to the UTF-8 string. You will have to decode the string on your own. The separator, quoting, and commenting characters used by the library can only be ASCII characters.
diff --git a/third_party/fast-cpp-csv-parser/csv.h b/third_party/fast-cpp-csv-parser/csv.h
new file mode 100644
index 0000000..3f0371d
--- /dev/null
+++ b/third_party/fast-cpp-csv-parser/csv.h
@@ -0,0 +1,1068 @@
+// Copyright: (2012-2014) Ben Strasser <code at ben-strasser.net>
+// License: BSD-3
+//
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// 1. Redistributions of source code must retain the above copyright notice,
+//    this list of conditions and the following disclaimer.
+//
+//2. Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+//
+//3. Neither the name of the copyright holder nor the names of its contributors
+//   may be used to endorse or promote products derived from this software
+//   without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef CSV_H
+#define CSV_H
+
+#include <vector>
+#include <string>
+#include <cstring>
+#include <algorithm>
+#include <utility>
+#include <cstdio>
+#include <exception>
+#ifndef CSV_IO_NO_THREAD
+#include <future>
+#endif
+#include <cassert>
+#include <cerrno>
+
+namespace io{
+        ////////////////////////////////////////////////////////////////////////////
+        //                                 LineReader                             //
+        ////////////////////////////////////////////////////////////////////////////
+
+        namespace error{
+                struct base : std::exception{
+                        virtual void format_error_message()const = 0;                          
+                       
+                        const char*what()const throw(){
+                                format_error_message();
+                                return error_message_buffer;
+                        }
+
+                        mutable char error_message_buffer[256];
+                };
+
+                const int max_file_name_length = 255;
+
+                struct with_file_name{
+                        with_file_name(){
+                                std::memset(file_name, 0, max_file_name_length+1);
+                        }
+                       
+                        void set_file_name(const char*file_name){
+                                std::strncpy(this->file_name, file_name, max_file_name_length);
+                                this->file_name[max_file_name_length] = '\0';
+                        }
+
+                        char file_name[max_file_name_length+1];
+                };
+
+                struct with_file_line{
+                        with_file_line(){
+                                file_line = -1;
+                        }
+                       
+                        void set_file_line(int file_line){
+                                this->file_line = file_line;
+                        }
+
+                        int file_line;
+                };
+
+                struct with_errno{
+                        with_errno(){
+                                errno_value = 0;
+                        }
+                       
+                        void set_errno(int errno_value){
+                                this->errno_value = errno_value;
+                        }
+
+                        int errno_value;
+                };
+
+                struct can_not_open_file :
+                        base,
+                        with_file_name,
+                        with_errno{
+                        void format_error_message()const{
+                                if(errno_value != 0)
+                                        std::snprintf(error_message_buffer, sizeof(error_message_buffer),
+                                                "Can not open file \"%s\" because \"%s\"."
+                                                , file_name, std::strerror(errno_value));
+                                else
+                                        std::snprintf(error_message_buffer, sizeof(error_message_buffer),
+                                                "Can not open file \"%s\"."
+                                                , file_name);
+                        }
+                };
+
+                struct line_length_limit_exceeded :
+                        base,
+                        with_file_name,
+                        with_file_line{
+                        void format_error_message()const{
+                                std::snprintf(error_message_buffer, sizeof(error_message_buffer),
+                                        "Line number %d in file \"%s\" exceeds the maximum length of 2^24-1."
+                                        , file_line, file_name);
+                        }
+                };
+        }
+
+        class LineReader{
+        private:
+                static const int block_len = 1<<24;
+                #ifndef CSV_IO_NO_THREAD
+                std::future<int>bytes_read;
+                #endif
+                FILE*file;
+                char*buffer;
+                int data_begin;
+                int data_end;
+
+                char file_name[error::max_file_name_length+1];
+                unsigned file_line;
+
+                void open_file(const char*file_name){
+                        // We open the file in binary mode as it makes no difference under *nix
+                        // and under Windows we handle \r\n newlines ourself.
+                        file = std::fopen(file_name, "rb");
+                        if(file == 0){
+                                int x = errno; // store errno as soon as possible, doing it after constructor call can fail.
+                                error::can_not_open_file err;
+                                err.set_errno(x);
+                                err.set_file_name(file_name);
+                                throw err;
+                        }
+                }
+
+                void init(){
+                        file_line = 0;
+
+                        // Tell the std library that we want to do the buffering ourself.
+                        std::setvbuf(file, 0, _IONBF, 0);
+
+                        try{
+                                buffer = new char[3*block_len];
+                        }catch(...){
+                                std::fclose(file);
+                                throw;
+                        }
+
+                        data_begin = 0;
+                        data_end = std::fread(buffer, 1, 2*block_len, file);
+
+                        // Ignore UTF-8 BOM
+                        if(data_end >= 3 && buffer[0] == '\xEF' && buffer[1] == '\xBB' && buffer[2] == '\xBF')
+                                data_begin = 3;
+
+                        #ifndef CSV_IO_NO_THREAD
+                        if(data_end == 2*block_len){
+                                bytes_read = std::async(std::launch::async, [=]()->int{
+                                        return std::fread(buffer + 2*block_len, 1, block_len, file);
+                                });
+                        }
+                        #endif
+                }
+
+        public:
+                LineReader() = delete;
+                LineReader(const LineReader&) = delete;
+                LineReader&operator=(const LineReader&) = delete;
+
+                LineReader(const char*file_name, FILE*file):
+                        file(file){
+                        set_file_name(file_name);
+                        init();
+                }
+
+                LineReader(const std::string&file_name, FILE*file):
+                        file(file){
+                        set_file_name(file_name.c_str());
+                        init();
+                }
+
+                explicit LineReader(const char*file_name){
+                        set_file_name(file_name);
+                        open_file(file_name);
+                        init();
+                }
+
+                explicit LineReader(const std::string&file_name){
+                        set_file_name(file_name.c_str());
+                        open_file(file_name.c_str());
+                        init();
+                }
+
+                void set_file_name(const std::string&file_name){
+                        set_file_name(file_name.c_str());
+                }
+
+                void set_file_name(const char*file_name){
+                        strncpy(this->file_name, file_name, error::max_file_name_length);
+                        this->file_name[error::max_file_name_length] = '\0';
+                }
+
+                const char*get_truncated_file_name()const{
+                        return file_name;
+                }
+
+                void set_file_line(unsigned file_line){
+                        this->file_line = file_line;
+                }
+
+                unsigned get_file_line()const{
+                        return file_line;
+                }
+
+                char*next_line(){
+                        if(data_begin == data_end)
+                                return 0;
+
+                        ++file_line;
+
+                        assert(data_begin < data_end);
+                        assert(data_end <= block_len*2);
+
+                        if(data_begin >= block_len){
+                                std::memcpy(buffer, buffer+block_len, block_len);
+                                data_begin -= block_len;
+                                data_end -= block_len;
+                                #ifndef CSV_IO_NO_THREAD
+                                if(bytes_read.valid())
+                                #endif
+                                {
+                                        #ifndef CSV_IO_NO_THREAD
+                                        data_end += bytes_read.get();
+                                        #else
+                                        data_end += std::fread(buffer + 2*block_len, 1, block_len, file);
+                                        #endif
+                                        std::memcpy(buffer+block_len, buffer+2*block_len, block_len);
+
+                                        #ifndef CSV_IO_NO_THREAD
+                                        bytes_read = std::async(std::launch::async, [=]()->int{
+                                                return std::fread(buffer + 2*block_len, 1, block_len, file);
+                                        });
+                                        #endif
+                                }
+                        }
+
+                        int line_end = data_begin;
+                        while(buffer[line_end] != '\n' && line_end != data_end){
+                                ++line_end;
+                        }
+
+                        if(line_end - data_begin + 1 > block_len){
+                                error::line_length_limit_exceeded err;
+                                err.set_file_name(file_name);
+                                err.set_file_line(file_line);
+                                throw err;
+                        }
+
+                        if(buffer[line_end] == '\n'){
+                                buffer[line_end] = '\0';
+                        }else{
+                                // some files are missing the newline at the end of the
+                                // last line
+                                ++data_end;
+                                buffer[line_end] = '\0';
+                        }
+
+                        // handle windows \r\n-line breaks
+                        if(line_end != data_begin && buffer[line_end-1] == '\r')
+                                buffer[line_end-1] = '\0';
+
+                        char*ret = buffer + data_begin;
+                        data_begin = line_end+1;
+                        return ret;
+                }
+
+                ~LineReader(){
+                        #ifndef CSV_IO_NO_THREAD
+                        // GCC needs this or it will crash.
+                        if(bytes_read.valid())
+                                bytes_read.get();
+                        #endif
+
+                        delete[] buffer;
+                        std::fclose(file);
+                }
+        };
+
+        ////////////////////////////////////////////////////////////////////////////
+        //                                 CSV                                    //
+        ////////////////////////////////////////////////////////////////////////////
+
+        namespace error{
+                const int max_column_name_length = 63;
+                struct with_column_name{
+                        with_column_name(){
+                                std::memset(column_name, 0, max_column_name_length+1);
+                        }
+                       
+                        void set_column_name(const char*column_name){
+                                std::strncpy(this->column_name, column_name, max_column_name_length);
+                                this->column_name[max_column_name_length] = '\0';
+                        }
+
+                        char column_name[max_column_name_length+1];
+                };
+
+
+                const int max_column_content_length = 63;
+
+                struct with_column_content{
+                        with_column_content(){
+                                std::memset(column_content, 0, max_column_content_length+1);
+                        }
+                       
+                        void set_column_content(const char*column_content){
+                                std::strncpy(this->column_content, column_content, max_column_content_length);
+                                this->column_content[max_column_content_length] = '\0';
+                        }
+
+                        char column_content[max_column_content_length+1];
+                };
+
+
+                struct extra_column_in_header :
+                        base,
+                        with_file_name,
+                        with_column_name{
+                        void format_error_message()const{
+                                std::snprintf(error_message_buffer, sizeof(error_message_buffer),
+                                        "Extra column \"%s\" in header of file \"%s\"."
+                                        , column_name, file_name);
+                        }
+                };
+
+                struct missing_column_in_header :
+                        base,
+                        with_file_name,
+                        with_column_name{
+                        void format_error_message()const{
+                                std::snprintf(error_message_buffer, sizeof(error_message_buffer),
+                                        "Missing column \"%s\" in header of file \"%s\"."
+                                        , column_name, file_name);
+                        }
+                };
+
+                struct duplicated_column_in_header :
+                        base,
+                        with_file_name,
+                        with_column_name{
+                        void format_error_message()const{
+                                std::snprintf(error_message_buffer, sizeof(error_message_buffer),
+                                        "Duplicated column \"%s\" in header of file \"%s\"."
+                                        , column_name, file_name);
+                        }
+                };
+
+                struct header_missing :
+                        base,
+                        with_file_name{
+                        void format_error_message()const{
+                                std::snprintf(error_message_buffer, sizeof(error_message_buffer),
+                                        "Header missing in file \"%s\"."
+                                        , file_name);
+                        }
+                };
+
+                struct too_few_columns :
+                        base,
+                        with_file_name,
+                        with_file_line{
+                        void format_error_message()const{
+                                std::snprintf(error_message_buffer, sizeof(error_message_buffer),
+                                        "Too few columns in line %d in file \"%s\"."
+                                        , file_line, file_name);
+                        }
+                };
+
+                struct too_many_columns :
+                        base,
+                        with_file_name,
+                        with_file_line{
+                        void format_error_message()const{
+                                std::snprintf(error_message_buffer, sizeof(error_message_buffer),
+                                        "Too many columns in line %d in file \"%s\"."
+                                        , file_line, file_name);
+                        }
+                };
+
+                struct escaped_string_not_closed :
+                        base,
+                        with_file_name,
+                        with_file_line{
+                        void format_error_message()const{
+                                std::snprintf(error_message_buffer, sizeof(error_message_buffer),
+                                        "Escaped string was not closed in line %d in file \"%s\"."
+                                        , file_line, file_name);
+                        }
+                };
+
+                struct integer_must_be_positive :
+                        base,
+                        with_file_name,
+                        with_file_line,
+                        with_column_name,
+                        with_column_content{
+                        void format_error_message()const{
+                                std::snprintf(error_message_buffer, sizeof(error_message_buffer),
+                                        "The integer \"%s\" must be positive or 0 in column \"%s\" in file \"%s\" in line \"%d\"."
+                                        , column_content, column_name, file_name, file_line);
+                        }
+                };
+
+                struct no_digit :
+                        base,
+                        with_file_name,
+                        with_file_line,
+                        with_column_name,
+                        with_column_content{
+                        void format_error_message()const{
+                                std::snprintf(error_message_buffer, sizeof(error_message_buffer),
+                                        "The integer \"%s\" contains an invalid digit in column \"%s\" in file \"%s\" in line \"%d\"."
+                                        , column_content, column_name, file_name, file_line);
+                        }
+                };
+
+                struct integer_overflow :
+                        base,
+                        with_file_name,
+                        with_file_line,
+                        with_column_name,
+                        with_column_content{
+                        void format_error_message()const{
+                                std::snprintf(error_message_buffer, sizeof(error_message_buffer),
+                                        "The integer \"%s\" overflows in column \"%s\" in file \"%s\" in line \"%d\"."
+                                        , column_content, column_name, file_name, file_line);
+                        }
+                };
+
+                struct integer_underflow :
+                        base,
+                        with_file_name,
+                        with_file_line,
+                        with_column_name,
+                        with_column_content{
+                        void format_error_message()const{
+                                std::snprintf(error_message_buffer, sizeof(error_message_buffer),
+                                        "The integer \"%s\" underflows in column \"%s\" in file \"%s\" in line \"%d\"."
+                                        , column_content, column_name, file_name, file_line);
+                        }
+                };
+
+                struct invalid_single_character :
+                        base,
+                        with_file_name,
+                        with_file_line,
+                        with_column_name,
+                        with_column_content{
+                        void format_error_message()const{
+                                std::snprintf(error_message_buffer, sizeof(error_message_buffer),
+                                        "The content \"%s\" of column \"%s\" in file \"%s\" in line \"%d\" is not a single character."
+                                        , column_content, column_name, file_name, file_line);
+                        }
+                };
+        }
+
+        typedef unsigned ignore_column;
+        static const ignore_column ignore_no_column = 0;
+        static const ignore_column ignore_extra_column = 1;
+        static const ignore_column ignore_missing_column = 2;
+
+        template<char ... trim_char_list>
+        struct trim_chars{
+        private:
+                constexpr static bool is_trim_char(char c){
+                        return false;
+                }
+       
+                template<class ...OtherTrimChars>
+                constexpr static bool is_trim_char(char c, char trim_char, OtherTrimChars...other_trim_chars){
+                        return c == trim_char || is_trim_char(c, other_trim_chars...);
+                }
+
+        public:
+                static void trim(char*&str_begin, char*&str_end){
+                        while(is_trim_char(*str_begin, trim_char_list...) && str_begin != str_end)
+                                ++str_begin;
+                        while(is_trim_char(*(str_end-1), trim_char_list...) && str_begin != str_end)
+                                --str_end;
+                        *str_end = '\0';
+                }
+        };
+
+
+        struct no_comment{
+                static bool is_comment(const char*line){
+                        return false;
+                }
+        };
+
+        template<char ... comment_start_char_list>
+        struct single_line_comment{
+        private:
+                constexpr static bool is_comment_start_char(char c){
+                        return false;
+                }
+       
+                template<class ...OtherCommentStartChars>
+                constexpr static bool is_comment_start_char(char c, char comment_start_char, OtherCommentStartChars...other_comment_start_chars){
+                        return c == comment_start_char || is_comment_start_char(c, other_comment_start_chars...);
+                }
+
+        public:
+
+                static bool is_comment(const char*line){
+                        return is_comment_start_char(*line, comment_start_char_list...);
+                }
+        };
+
+        struct empty_line_comment{
+                static bool is_comment(const char*line){
+                        if(*line == '\0')
+                                return true;
+                        while(*line == ' ' || *line == '\t'){
+                                ++line;
+                                if(*line == 0)
+                                        return true;
+                        }
+                        return false;
+                }
+        };
+
+        template<char ... comment_start_char_list>
+        struct single_and_empty_line_comment{
+                static bool is_comment(const char*line){
+                        return single_line_comment<comment_start_char_list...>::is_comment(line) || empty_line_comment::is_comment(line);
+                }
+        };
+
+        template<char sep>
+        struct no_quote_escape{
+                static const char*find_next_column_end(const char*col_begin){
+                        while(*col_begin != sep && *col_begin != '\0')
+                                ++col_begin;
+                        return col_begin;
+                }
+
+                static void unescape(char*&col_begin, char*&col_end){
+
+                }
+        };
+
+        template<char sep, char quote>
+        struct double_quote_escape{
+                static const char*find_next_column_end(const char*col_begin){
+                        while(*col_begin != sep && *col_begin != '\0')
+                                if(*col_begin != quote)
+                                        ++col_begin;
+                                else{
+                                        do{
+                                                ++col_begin;
+                                                while(*col_begin != quote){
+                                                        if(*col_begin == '\0')
+                                                                throw error::escaped_string_not_closed();
+                                                        ++col_begin;
+                                                }
+                                                ++col_begin;
+                                        }while(*col_begin == quote);
+                                }      
+                        return col_begin;      
+                }
+
+                static void unescape(char*&col_begin, char*&col_end){
+                        if(col_end - col_begin >= 2){
+                                if(*col_begin == quote && *(col_end-1) == quote){
+                                        ++col_begin;
+                                        --col_end;
+                                        char*out = col_begin;
+                                        for(char*in = col_begin; in!=col_end; ++in){
+                                                if(*in == quote && *(in+1) == quote){
+                                                         ++in;
+                                                }
+                                                *out = *in;
+                                                ++out;
+                                        }
+                                        col_end = out;
+                                        *col_end = '\0';
+                                }
+                        }
+                       
+                }
+        };
+
+        struct throw_on_overflow{
+                template<class T>
+                static void on_overflow(T&){
+                        throw error::integer_overflow();
+                }
+               
+                template<class T>
+                static void on_underflow(T&){
+                        throw error::integer_underflow();
+                }
+        };
+
+        struct ignore_overflow{
+                template<class T>
+                static void on_overflow(T&){}
+               
+                template<class T>
+                static void on_underflow(T&){}
+        };
+
+        struct set_to_max_on_overflow{
+                template<class T>
+                static void on_overflow(T&x){
+                        x = std::numeric_limits<T>::max();
+                }
+               
+                template<class T>
+                static void on_underflow(T&x){
+                        x = std::numeric_limits<T>::min();
+                }
+        };
+
+
+        namespace detail{
+                template<class quote_policy>
+                void chop_next_column(
+                        char*&line, char*&col_begin, char*&col_end
+                ){
+                        assert(line != nullptr);
+
+                        col_begin = line;
+                        // the col_begin + (... - col_begin) removes the constness
+                        col_end = col_begin + (quote_policy::find_next_column_end(col_begin) - col_begin);
+                       
+                        if(*col_end == '\0'){
+                                line = nullptr;
+                        }else{
+                                *col_end = '\0';
+                                line = col_end + 1;    
+                        }
+                }
+
+                template<class trim_policy, class quote_policy>
+                void parse_line(
+                        char*line,
+                        char**sorted_col,
+                        const std::vector<int>&col_order
+                ){
+                        for(std::size_t i=0; i<col_order.size(); ++i){
+                                if(line == nullptr)
+                                        throw ::io::error::too_few_columns();
+                                char*col_begin, *col_end;
+                                chop_next_column<quote_policy>(line, col_begin, col_end);
+
+                                if(col_order[i] != -1){
+                                        trim_policy::trim(col_begin, col_end);
+                                        quote_policy::unescape(col_begin, col_end);
+                                                               
+                                        sorted_col[col_order[i]] = col_begin;
+                                }
+                        }
+                        if(line != nullptr)
+                                throw ::io::error::too_many_columns();
+                }
+
+                template<unsigned column_count, class trim_policy, class quote_policy>
+                void parse_header_line(
+                        char*line,
+                        std::vector<int>&col_order,
+                        const std::string*col_name,
+                        ignore_column ignore_policy
+                ){
+                        col_order.clear();
+
+                        bool found[column_count];
+                        std::fill(found, found + column_count, false);
+                        while(line){
+                                char*col_begin,*col_end;
+                                chop_next_column<quote_policy>(line, col_begin, col_end);
+
+                                trim_policy::trim(col_begin, col_end);
+                                quote_policy::unescape(col_begin, col_end);
+                               
+                                for(unsigned i=0; i<column_count; ++i)
+                                        if(col_begin == col_name[i]){
+                                                if(found[i]){
+                                                        error::duplicated_column_in_header err;
+                                                        err.set_column_name(col_begin);
+                                                        throw err;
+                                                }
+                                                found[i] = true;
+                                                col_order.push_back(i);
+                                                col_begin = 0;
+                                                break;
+                                        }
+                                if(col_begin){
+                                        if(ignore_policy & ::io::ignore_extra_column)
+                                                col_order.push_back(-1);
+                                        else{
+                                                error::extra_column_in_header err;
+                                                err.set_column_name(col_begin);
+                                                throw err;
+                                        }
+                                }
+                        }
+                        if(!(ignore_policy & ::io::ignore_missing_column)){
+                                for(unsigned i=0; i<column_count; ++i){
+                                        if(!found[i]){
+                                                error::missing_column_in_header err;
+                                                err.set_column_name(col_name[i].c_str());
+                                                throw err;
+                                        }
+                                }
+                        }
+                }
+
+                template<class overflow_policy>
+                void parse(char*col, char &x){
+                        if(!*col)
+                                throw error::invalid_single_character();
+                        x = *col;
+                        ++col;
+                        if(*col)
+                                throw error::invalid_single_character();
+                }
+               
+                template<class overflow_policy>
+                void parse(char*col, std::string&x){
+                        x = col;
+                }
+
+                template<class overflow_policy>
+                void parse(char*col, const char*&x){
+                        x = col;
+                }
+
+                template<class overflow_policy>
+                void parse(char*col, char*&x){
+                        x = col;
+                }
+
+                template<class overflow_policy, class T>
+                void parse_unsigned_integer(const char*col, T&x){
+                        x = 0;
+                        while(*col != '\0'){
+                                if('0' <= *col && *col <= '9'){
+                                        T y = *col - '0';
+                                        if(x > (std::numeric_limits<T>::max()-y)/10){
+                                                overflow_policy::on_overflow(x);
+                                                return;
+                                        }
+                                        x = 10*x+y;
+                                }else
+                                        throw error::no_digit();
+                                ++col;
+                        }
+                }
+
+                template<class overflow_policy>void parse(char*col, unsigned char &x)
+                        {parse_unsigned_integer<overflow_policy>(col, x);}
+                template<class overflow_policy>void parse(char*col, unsigned short &x)
+                        {parse_unsigned_integer<overflow_policy>(col, x);}
+                template<class overflow_policy>void parse(char*col, unsigned int &x)
+                        {parse_unsigned_integer<overflow_policy>(col, x);}
+                template<class overflow_policy>void parse(char*col, unsigned long &x)
+                        {parse_unsigned_integer<overflow_policy>(col, x);}
+                template<class overflow_policy>void parse(char*col, unsigned long long &x)
+                        {parse_unsigned_integer<overflow_policy>(col, x);}
+               
+                template<class overflow_policy, class T>
+                void parse_signed_integer(const char*col, T&x){
+                        if(*col == '-'){
+                                ++col;
+
+                                x = 0;
+                                while(*col != '\0'){
+                                        if('0' <= *col && *col <= '9'){
+                                                T y = *col - '0';
+                                                if(x < (std::numeric_limits<T>::min()+y)/10){
+                                                        overflow_policy::on_underflow(x);
+                                                        return;
+                                                }
+                                                x = 10*x-y;
+                                        }else
+                                                throw error::no_digit();
+                                        ++col;
+                                }
+                                return;
+                        }else if(*col == '+')
+                                ++col;
+                        parse_unsigned_integer<overflow_policy>(col, x);
+                }      
+
+                template<class overflow_policy>void parse(char*col, signed char &x)
+                        {parse_signed_integer<overflow_policy>(col, x);}
+                template<class overflow_policy>void parse(char*col, signed short &x)
+                        {parse_signed_integer<overflow_policy>(col, x);}
+                template<class overflow_policy>void parse(char*col, signed int &x)
+                        {parse_signed_integer<overflow_policy>(col, x);}
+                template<class overflow_policy>void parse(char*col, signed long &x)
+                        {parse_signed_integer<overflow_policy>(col, x);}
+                template<class overflow_policy>void parse(char*col, signed long long &x)
+                        {parse_signed_integer<overflow_policy>(col, x);}
+
+                template<class T>
+                void parse_float(const char*col, T&x){
+                        bool is_neg = false;
+                        if(*col == '-'){
+                                is_neg = true;
+                                ++col;
+                        }else if(*col == '+')
+                                ++col;
+
+                        x = 0;
+                        while('0' <= *col && *col <= '9'){
+                                int y = *col - '0';
+                                x *= 10;
+                                x += y;
+                                ++col;
+                        }
+                       
+                        if(*col == '.'|| *col == ','){
+                                ++col;
+                                T pos = 1;
+                                while('0' <= *col && *col <= '9'){
+                                        pos /= 10;
+                                        int y = *col - '0';
+                                        ++col;
+                                        x += y*pos;
+                                }
+                        }
+
+                        if(*col == 'e' || *col == 'E'){
+                                ++col;
+                                int e;
+
+                                parse_signed_integer<set_to_max_on_overflow>(col, e);
+                               
+                                if(e != 0){
+                                        T base;
+                                        if(e < 0){
+                                                base = 0.1;
+                                                e = -e;
+                                        }else{
+                                                base = 10;
+                                        }
+       
+                                        while(e != 1){
+                                                if((e & 1) == 0){
+                                                        base = base*base;
+                                                        e >>= 1;
+                                                }else{
+                                                        x *= base;
+                                                        --e;
+                                                }
+                                        }
+                                        x *= base;
+                                }
+                        }else{
+                                if(*col != '\0')
+                                        throw error::no_digit();
+                        }
+
+                        if(is_neg)
+                                x = -x;
+                }
+
+                template<class overflow_policy> void parse(char*col, float&x) { parse_float(col, x); }
+                template<class overflow_policy> void parse(char*col, double&x) { parse_float(col, x); }
+                template<class overflow_policy> void parse(char*col, long double&x) { parse_float(col, x); }
+
+                template<class overflow_policy, class T>
+                void parse(char*col, T&x){
+                        // GCC evalutes "false" when reading the template and
+                        // "sizeof(T)!=sizeof(T)" only when instantiating it. This is why
+                        // this strange construct is used.
+                        static_assert(sizeof(T)!=sizeof(T),
+                                "Can not parse this type. Only buildin integrals, floats, char, char*, const char* and std::string are supported");
+                }
+
+        }
+
+        template<unsigned column_count,
+                class trim_policy = trim_chars<' ', '\t'>,
+                class quote_policy = no_quote_escape<','>,
+                class overflow_policy = throw_on_overflow,
+                class comment_policy = no_comment
+        >
+        class CSVReader{
+        private:
+                LineReader in;
+
+                char*(row[column_count]);
+                std::string column_names[column_count];
+
+                std::vector<int>col_order;
+
+                template<class ...ColNames>
+                void set_column_names(std::string s, ColNames...cols){
+                        column_names[column_count-sizeof...(ColNames)-1] = std::move(s);
+                        set_column_names(std::forward<ColNames>(cols)...);
+                }
+
+                void set_column_names(){}
+
+
+        public:
+                CSVReader() = delete;
+                CSVReader(const CSVReader&) = delete;
+                CSVReader&operator=(const CSVReader&);
+
+                template<class ...Args>
+                explicit CSVReader(Args...args):in(std::forward<Args>(args)...){
+                        std::fill(row, row+column_count, nullptr);
+                        col_order.resize(column_count);
+                        for(unsigned i=0; i<column_count; ++i)
+                                col_order[i] = i;
+                        for(unsigned i=1; i<=column_count; ++i)
+                                column_names[i-1] = "col"+std::to_string(i);
+                }
+
+                template<class ...ColNames>
+                void read_header(ignore_column ignore_policy, ColNames...cols){
+                        static_assert(sizeof...(ColNames)>=column_count, "not enough column names specified");
+                        static_assert(sizeof...(ColNames)<=column_count, "too many column names specified");
+                        try{
+                                set_column_names(std::forward<ColNames>(cols)...);
+
+                                char*line;
+                                do{
+                                        line = in.next_line();
+                                        if(!line)
+                                                throw error::header_missing();
+                                }while(comment_policy::is_comment(line));
+
+                                detail::parse_header_line
+                                        <column_count, trim_policy, quote_policy>
+                                        (line, col_order, column_names, ignore_policy);
+                        }catch(error::with_file_name&err){
+                                err.set_file_name(in.get_truncated_file_name());
+                                throw;
+                        }
+                }
+
+                template<class ...ColNames>
+                void set_header(ColNames...cols){
+                        static_assert(sizeof...(ColNames)>=column_count,
+                                "not enough column names specified");
+                        static_assert(sizeof...(ColNames)<=column_count,
+                                "too many column names specified");
+                        set_column_names(std::forward<ColNames>(cols)...);
+                        std::fill(row, row+column_count, nullptr);
+                        col_order.resize(column_count);
+                        for(unsigned i=0; i<column_count; ++i)
+                                col_order[i] = i;
+                }
+
+                bool has_column(const std::string&name) const {
+                        return col_order.end() != std::find(
+                                col_order.begin(), col_order.end(),
+                                        std::find(std::begin(column_names), std::end(column_names), name)
+                                - std::begin(column_names));
+                }
+
+                void set_file_name(const std::string&file_name){
+                        in.set_file_name(file_name);
+                }
+
+                void set_file_name(const char*file_name){
+                        in.set_file_name(file_name);
+                }
+
+                const char*get_truncated_file_name()const{
+                        return in.get_truncated_file_name();
+                }
+
+                void set_file_line(unsigned file_line){
+                        in.set_file_line(file_line);
+                }
+
+                unsigned get_file_line()const{
+                        return in.get_file_line();
+                }
+
+        private:
+                void parse_helper(std::size_t r){}
+
+                template<class T, class ...ColType>
+                void parse_helper(std::size_t r, T&t, ColType&...cols){                        
+                        if(row[r]){
+                                try{
+                                        try{
+                                                ::io::detail::parse<overflow_policy>(row[r], t);
+                                        }catch(error::with_column_content&err){
+                                                err.set_column_content(row[r]);
+                                                throw;
+                                        }
+                                }catch(error::with_column_name&err){
+                                        err.set_column_name(column_names[r].c_str());
+                                        throw;
+                                }
+                        }
+                        parse_helper(r+1, cols...);
+                }
+
+       
+        public:
+                template<class ...ColType>
+                bool read_row(ColType& ...cols){
+                        static_assert(sizeof...(ColType)>=column_count,
+                                "not enough columns specified");
+                        static_assert(sizeof...(ColType)<=column_count,
+                                "too many columns specified");
+                        try{
+                                try{
+       
+                                        char*line;
+                                        do{
+                                                line = in.next_line();
+                                                if(!line)
+                                                        return false;
+                                        }while(comment_policy::is_comment(line));
+                                       
+                                        detail::parse_line<trim_policy, quote_policy>
+                                                (line, row, col_order);
+               
+                                        parse_helper(0, cols...);
+                                }catch(error::with_file_name&err){
+                                        err.set_file_name(in.get_truncated_file_name());
+                                        throw;
+                                }
+                        }catch(error::with_file_line&err){
+                                err.set_file_line(in.get_file_line());
+                                throw;
+                        }
+
+                        return true;
+                }
+        };
+}
+#endif
+
diff --git a/third_party/variant/.gitignore b/third_party/variant/.gitignore
index 5620e5d..984cc2f 100644
--- a/third_party/variant/.gitignore
+++ b/third_party/variant/.gitignore
@@ -1,3 +1,5 @@
 .DS_Store
 out
-profiling
\ No newline at end of file
+profiling
+build
+deps
diff --git a/third_party/variant/Jamroot b/third_party/variant/Jamroot
index aead870..aa21375 100644
--- a/third_party/variant/Jamroot
+++ b/third_party/variant/Jamroot
@@ -1,4 +1,4 @@
-local BOOST_DIR = "/opt/boost" ;
+local BOOST_DIR = "/usr/local" ;
 
 #using clang : : ;
 
diff --git a/third_party/variant/appveyor.yml b/third_party/variant/appveyor.yml
index 9d7c599..050ad25 100644
--- a/third_party/variant/appveyor.yml
+++ b/third_party/variant/appveyor.yml
@@ -7,21 +7,11 @@ configuration:
   - Debug
   - Release
 
-install:
-  - SET PATH=c:\python27;%PATH%
-  - SET PATH=C:\Program Files (x86)\MSBuild\12.0\bin\;%PATH%
-  - git clone --quiet --depth 1 https://chromium.googlesource.com/external/gyp.git deps/gyp
-  # note windows requires --generator-output to be absolute
-  - python deps/gyp/gyp_main.py variant.gyp --depth=. -f msvs -G msvs_version=2013
-  - set MSBUILD_PLATFORM=%platform%
-  - if "%MSBUILD_PLATFORM%" == "x86" set MSBUILD_PLATFORM=Win32
-  - msbuild variant.sln /nologo /p:Configuration=%configuration%;Platform=%MSBUILD_PLATFORM%
-  - .\"%configuration%"\tests.exe
-
-build: OFF
+os: Visual Studio 2015
 
-test: OFF
-
-test_script: OFF
+install:
+  - CALL scripts\build-appveyor.bat
 
-deploy: OFF
+build: off
+test: off
+deploy: off
diff --git a/third_party/variant/common.gypi b/third_party/variant/common.gypi
index 67e333b..d4f09f8 100644
--- a/third_party/variant/common.gypi
+++ b/third_party/variant/common.gypi
@@ -3,7 +3,7 @@
     ["OS=='win'", {
           "target_defaults": {
             "default_configuration": "Release_x64",
-            "msbuild_toolset":"CTP_Nov2013",
+            "msbuild_toolset":"v140",
             "msvs_settings": {
               "VCCLCompilerTool": {
                 "ExceptionHandling": 1, # /EHsc
diff --git a/third_party/variant/scripts/build-appveyor.bat b/third_party/variant/scripts/build-appveyor.bat
new file mode 100644
index 0000000..9b9d7c8
--- /dev/null
+++ b/third_party/variant/scripts/build-appveyor.bat
@@ -0,0 +1,32 @@
+ at ECHO OFF
+SETLOCAL
+
+SET PATH=c:\python27;%PATH%
+
+ECHO activating VS command prompt
+IF /I "%PLATFORM"=="x64" (
+  CALL "C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\vcvarsall.bat" amd64
+) ELSE (
+  CALL "C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\vcvarsall.bat" x86
+)
+
+IF NOT EXIST deps\gyp git clone --depth 1 https://chromium.googlesource.com/external/gyp.git deps/gyp
+
+CALL deps\gyp\gyp.bat variant.gyp --depth=. ^
+-f msvs ^
+-G msvs_version=2015 ^
+--generator-output=build
+
+SET MSBUILD_PLATFORM=%platform%
+IF /I "%MSBUILD_PLATFORM%" == "x86" SET MSBUILD_PLATFORM=Win32
+
+
+msbuild ^
+build\variant.sln ^
+/nologo ^
+/toolsversion:14.0 ^
+/p:PlatformToolset=v140 ^
+/p:Configuration=%configuration% ^
+/p:Platform=%MSBUILD_PLATFORM%
+
+build\"%configuration%"\tests.exe
diff --git a/third_party/variant/scripts/build-local.bat b/third_party/variant/scripts/build-local.bat
new file mode 100644
index 0000000..34a2e61
--- /dev/null
+++ b/third_party/variant/scripts/build-local.bat
@@ -0,0 +1,7 @@
+ at ECHO OFF
+SETLOCAL
+
+SET platform=x64
+SET configuration=Release
+
+CALL scripts\build-appveyor.bat
\ No newline at end of file
diff --git a/third_party/variant/test/bench_variant.cpp b/third_party/variant/test/bench_variant.cpp
index f271245..474e3c3 100644
--- a/third_party/variant/test/bench_variant.cpp
+++ b/third_party/variant/test/bench_variant.cpp
@@ -7,7 +7,9 @@
 #include <boost/timer/timer.hpp>
 #include "variant.hpp"
 
-#define TEXT "Testing various variant implementations with a longish string ........................................."
+#define TEXT_SHORT "Test"
+#define TEXT_LONG "Testing various variant implementations with a longish string ........................................."
+#define NUM_SAMPLES 3
 //#define BOOST_VARIANT_MINIMIZE_SIZE
 
 using namespace mapbox;
@@ -52,9 +54,9 @@ struct dummy : boost::static_visitor<>
         : v_(v) {}
 
     template <typename T>
-    void operator() (T const& val) const
+    void operator() (T && val) const
     {
-        v_ = val;
+        v_ = std::move(val);
     }
     V & v_;
 };
@@ -66,9 +68,9 @@ struct dummy2 : util::static_visitor<>
         : v_(v) {}
 
     template <typename T>
-    void operator() (T const& val) const
+    void operator() (T && val) const
     {
-        v_.template set<T>(val);
+        v_ = std::move(val);
     }
     V & v_;
 };
@@ -79,7 +81,8 @@ void run_boost_test(std::size_t runs)
     h.data.reserve(runs);
     for (std::size_t i=0; i< runs; ++i)
     {
-        h.append_move(std::string(TEXT));
+        h.append_move(std::string(TEXT_SHORT));
+        h.append_move(std::string(TEXT_LONG));
         h.append_move(123);
         h.append_move(3.14159);
     }
@@ -98,7 +101,8 @@ void run_variant_test(std::size_t runs)
     h.data.reserve(runs);
     for (std::size_t i=0; i< runs; ++i)
     {
-        h.append_move(std::string(TEXT));
+        h.append_move(std::string(TEXT_SHORT));
+        h.append_move(std::string(TEXT_LONG));
         h.append_move(123);
         h.append_move(3.14159);
     }
@@ -125,77 +129,50 @@ int main (int argc, char** argv)
     const std::size_t NUM_RUNS = static_cast<std::size_t>(std::stol(argv[1]));
 
 #ifdef SINGLE_THREADED
+
+    for (std::size_t j = 0; j < NUM_SAMPLES; ++j)
     {
-        std::cerr << "custom variant: ";
-        boost::timer::auto_cpu_timer t;
-        run_variant_test(NUM_RUNS);
-    }
-    {
-        std::cerr << "boost variant: ";
-        boost::timer::auto_cpu_timer t;
-        run_boost_test(NUM_RUNS);
-    }
-    {
-        std::cerr << "custom variant: ";
-        boost::timer::auto_cpu_timer t;
-        run_variant_test(NUM_RUNS);
-    }
-    {
-        std::cerr << "boost variant: ";
-        boost::timer::auto_cpu_timer t;
-        run_boost_test(NUM_RUNS);
-    }
-#else
-    {
-        typedef std::vector<std::unique_ptr<std::thread>> thread_group;
-        typedef thread_group::value_type value_type;
-        thread_group tg;
-        std::cerr << "custom variant: ";
-        boost::timer::auto_cpu_timer timer;
-        for (std::size_t i=0; i<THREADS; ++i)
+
         {
-            tg.emplace_back(new std::thread(run_variant_test, NUM_RUNS));
+            std::cerr << "custom variant: ";
+            boost::timer::auto_cpu_timer t;
+            run_variant_test(NUM_RUNS);
         }
-        std::for_each(tg.begin(), tg.end(), [](value_type & t) {if (t->joinable()) t->join();});
-    }
-
-    {
-        typedef std::vector<std::unique_ptr<std::thread>> thread_group;
-        typedef thread_group::value_type value_type;
-        thread_group tg;
-        std::cerr << "boost variant: ";
-        boost::timer::auto_cpu_timer timer;
-        for (std::size_t i=0; i<THREADS; ++i)
         {
-            tg.emplace_back(new std::thread(run_boost_test, NUM_RUNS));
+            std::cerr << "boost variant: ";
+            boost::timer::auto_cpu_timer t;
+            run_boost_test(NUM_RUNS);
         }
-        std::for_each(tg.begin(), tg.end(), [](value_type & t) {if (t->joinable()) t->join();});
     }
 
+#else
+    for (std::size_t j = 0; j < NUM_SAMPLES; ++j)
     {
-        typedef std::vector<std::unique_ptr<std::thread>> thread_group;
-        typedef thread_group::value_type value_type;
-        thread_group tg;
-        std::cerr << "custom variant: ";
-        boost::timer::auto_cpu_timer timer;
-        for (std::size_t i=0; i<THREADS; ++i)
         {
-            tg.emplace_back(new std::thread(run_variant_test, NUM_RUNS));
+            typedef std::vector<std::unique_ptr<std::thread>> thread_group;
+            typedef thread_group::value_type value_type;
+            thread_group tg;
+            std::cerr << "custom variant: ";
+            boost::timer::auto_cpu_timer timer;
+            for (std::size_t i=0; i<THREADS; ++i)
+            {
+                tg.emplace_back(new std::thread(run_variant_test, NUM_RUNS));
+            }
+            std::for_each(tg.begin(), tg.end(), [](value_type & t) {if (t->joinable()) t->join();});
         }
-        std::for_each(tg.begin(), tg.end(), [](value_type & t) {if (t->joinable()) t->join();});
-    }
 
-    {
-        typedef std::vector<std::unique_ptr<std::thread>> thread_group;
-        typedef thread_group::value_type value_type;
-        thread_group tg;
-        std::cerr << "boost variant: ";
-        boost::timer::auto_cpu_timer timer;
-        for (std::size_t i=0; i<THREADS; ++i)
         {
-            tg.emplace_back(new std::thread(run_boost_test, NUM_RUNS));
+            typedef std::vector<std::unique_ptr<std::thread>> thread_group;
+            typedef thread_group::value_type value_type;
+            thread_group tg;
+            std::cerr << "boost variant: ";
+            boost::timer::auto_cpu_timer timer;
+            for (std::size_t i=0; i<THREADS; ++i)
+            {
+                tg.emplace_back(new std::thread(run_boost_test, NUM_RUNS));
+            }
+            std::for_each(tg.begin(), tg.end(), [](value_type & t) {if (t->joinable()) t->join();});
         }
-        std::for_each(tg.begin(), tg.end(), [](value_type & t) {if (t->joinable()) t->join();});
     }
 #endif
 
diff --git a/third_party/variant/variant.hpp b/third_party/variant/variant.hpp
index 837b162..90aacae 100644
--- a/third_party/variant/variant.hpp
+++ b/third_party/variant/variant.hpp
@@ -90,6 +90,21 @@ struct value_traits
         (direct_index == invalid_value) ? convertible_type<T, Types...>::index : direct_index;
 };
 
+// check if T is in Types...
+template <typename T, typename...Types>
+struct has_type;
+
+template <typename T, typename First, typename... Types>
+struct has_type<T, First, Types...>
+{
+    static constexpr bool value = std::is_same<T, First>::value
+        || has_type<T, Types...>::value;
+};
+
+template <typename T>
+struct has_type<T> : std::false_type {};
+//
+
 template <typename T, typename...Types>
 struct is_valid_type;
 
@@ -214,6 +229,20 @@ struct variant_helper<T, Types...>
             variant_helper<Types...>::copy(old_id, old_value, new_value);
         }
     }
+
+    VARIANT_INLINE static void direct_swap(const std::size_t id, void * lhs, void * rhs)
+    {
+        using std::swap; //enable ADL
+        if (id == sizeof...(Types))
+        {
+            // both lhs and rhs hold T
+            swap(*reinterpret_cast<T*>(lhs), *reinterpret_cast<T*>(rhs));
+        }
+        else
+        {
+            variant_helper<Types...>::direct_swap(id, lhs, rhs);
+        }
+    }
 };
 
 template<> struct variant_helper<>
@@ -221,6 +250,7 @@ template<> struct variant_helper<>
     VARIANT_INLINE static void destroy(const std::size_t, void *) {}
     VARIANT_INLINE static void move(const std::size_t, void *, void *) {}
     VARIANT_INLINE static void copy(const std::size_t, const void *, void *) {}
+    VARIANT_INLINE static void direct_swap(const std::size_t, void *, void *) {}
 };
 
 namespace detail {
@@ -561,16 +591,33 @@ public:
         helper_type::move(old.type_index, &old.data, &data);
     }
 
-    friend void swap(variant<Types...> & first, variant<Types...> & second)
+private:
+    VARIANT_INLINE void copy_assign(variant<Types...> const& rhs)
     {
-        using std::swap; //enable ADL
-        swap(first.type_index, second.type_index);
-        swap(first.data, second.data);
+        helper_type::destroy(type_index, &data);
+        type_index = detail::invalid_value;
+        helper_type::copy(rhs.type_index, &rhs.data, &data);
+        type_index = rhs.type_index;
     }
 
-    VARIANT_INLINE variant<Types...>& operator=(variant<Types...> other)
+    VARIANT_INLINE void move_assign(variant<Types...> && rhs)
     {
-        swap(*this, other);
+        helper_type::destroy(type_index, &data);
+        type_index = detail::invalid_value;
+        helper_type::move(rhs.type_index, &rhs.data, &data);
+        type_index = rhs.type_index;
+    }
+
+public:
+    VARIANT_INLINE variant<Types...>& operator=(variant<Types...> &&  other)
+    {
+        move_assign(std::move(other));
+        return *this;
+    }
+
+    VARIANT_INLINE variant<Types...>& operator=(variant<Types...> const& other)
+    {
+        copy_assign(other);
         return *this;
     }
 
@@ -580,7 +627,7 @@ public:
     VARIANT_INLINE variant<Types...>& operator=(T && rhs) noexcept
     {
         variant<Types...> temp(std::forward<T>(rhs));
-        swap(*this, temp);
+        move_assign(std::move(temp));
         return *this;
     }
 
@@ -589,13 +636,14 @@ public:
     VARIANT_INLINE variant<Types...>& operator=(T const& rhs)
     {
         variant<Types...> temp(rhs);
-        swap(*this, temp);
+        copy_assign(temp);
         return *this;
     }
 
     template<typename T>
     VARIANT_INLINE bool is() const
     {
+        static_assert(detail::has_type<T, Types...>::value, "invalid type in T in `is<T>()` for this variant");
         return (type_index == detail::direct_type<T, Types...>::index);
     }
 
@@ -613,7 +661,9 @@ public:
     }
 
     // get<T>()
-    template<typename T, typename std::enable_if<(detail::direct_type<T, Types...>::index != detail::invalid_value)>::type* = nullptr>
+    template<typename T, typename std::enable_if<
+                         (detail::direct_type<T, Types...>::index != detail::invalid_value)
+                         >::type* = nullptr>
     VARIANT_INLINE T& get()
     {
         if (type_index == detail::direct_type<T, Types...>::index)
diff --git a/tools/components.cpp b/tools/components.cpp
index d00713a..db0e526 100644
--- a/tools/components.cpp
+++ b/tools/components.cpp
@@ -76,9 +76,9 @@ void DeleteFileIfExists(const std::string &file_name)
 }
 }
 
-std::size_t LoadGraph(const char* path,
-                      std::vector<QueryNode>& coordinate_list,
-                      std::vector<TarjanEdge>& graph_edge_list)
+std::size_t LoadGraph(const char *path,
+                      std::vector<QueryNode> &coordinate_list,
+                      std::vector<TarjanEdge> &graph_edge_list)
 {
     std::ifstream input_stream(path, std::ifstream::in | std::ifstream::binary);
     if (!input_stream.is_open())
@@ -92,8 +92,7 @@ std::size_t LoadGraph(const char* path,
     std::vector<NodeID> barrier_node_list;
 
     auto number_of_nodes = loadNodesFromFile(input_stream, barrier_node_list,
-                                             traffic_light_node_list,
-                                             coordinate_list);
+                                             traffic_light_node_list, coordinate_list);
 
     loadEdgesFromFile(input_stream, edge_list);
 
@@ -131,15 +130,12 @@ int main(int argc, char *argv[])
     try
     {
         // enable logging
-        if (argc < 3)
+        if (argc < 2)
         {
-            SimpleLogger().Write(logWARNING) << "usage:\n" << argv[0]
-                                             << " <osrm>";
+            SimpleLogger().Write(logWARNING) << "usage:\n" << argv[0] << " <osrm>";
             return -1;
         }
 
-        SimpleLogger().Write() << "Using restrictions from file: " << argv[2];
-
         std::vector<TarjanEdge> graph_edge_list;
         auto number_of_nodes = LoadGraph(argv[1], coordinate_list, graph_edge_list);
 
@@ -208,7 +204,7 @@ int main(int argc, char *argv[])
                 if (source < target || SPECIAL_EDGEID == graph->FindEdge(target, source))
                 {
                     total_network_length +=
-                        100 * coordinate_calculation::euclidean_distance(
+                        100 * coordinate_calculation::great_circle_distance(
                                   coordinate_list[source].lat, coordinate_list[source].lon,
                                   coordinate_list[target].lat, coordinate_list[target].lon);
 
@@ -216,8 +212,9 @@ int main(int argc, char *argv[])
                     BOOST_ASSERT(source != SPECIAL_NODEID);
                     BOOST_ASSERT(target != SPECIAL_NODEID);
 
-                    const unsigned size_of_containing_component = std::min(
-                        tarjan->get_component_size(tarjan->get_component_id(source)), tarjan->get_component_size(tarjan->get_component_id(target)));
+                    const unsigned size_of_containing_component =
+                        std::min(tarjan->get_component_size(tarjan->get_component_id(source)),
+                                 tarjan->get_component_size(tarjan->get_component_id(target)));
 
                     // edges that end on bollard nodes may actually be in two distinct components
                     if (size_of_containing_component < 1000)
diff --git a/tools/io-benchmark.cpp b/tools/io-benchmark.cpp
index ae76327..41e41b2 100644
--- a/tools/io-benchmark.cpp
+++ b/tools/io-benchmark.cpp
@@ -25,7 +25,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 */
 
-#include "../util/git_sha.hpp"
+#include "util/version.hpp"
 #include "../util/osrm_exception.hpp"
 #include "../util/simple_logger.hpp"
 #include "../util/timing_util.hpp"
@@ -84,7 +84,7 @@ int main(int argc, char *argv[])
     boost::filesystem::path test_path;
     try
     {
-        SimpleLogger().Write() << "starting up engines, " << g_GIT_DESCRIPTION;
+        SimpleLogger().Write() << "starting up engines, " << OSRM_VERSION;
 
         if (1 == argc)
         {
diff --git a/tools/simpleclient.cpp b/tools/simpleclient.cpp
index ce590cc..17058a6 100644
--- a/tools/simpleclient.cpp
+++ b/tools/simpleclient.cpp
@@ -25,8 +25,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 */
 
-#include "../library/osrm.hpp"
-#include "../util/git_sha.hpp"
+#include "util/version.hpp"
 #include "../util/json_renderer.hpp"
 #include "../util/routed_options.hpp"
 #include "../util/simple_logger.hpp"
@@ -34,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <osrm/json_container.hpp>
 #include <osrm/libosrm_config.hpp>
 #include <osrm/route_parameters.hpp>
+#include <osrm/osrm.hpp>
 
 #include <string>
 
@@ -43,13 +43,14 @@ int main(int argc, const char *argv[])
     try
     {
         std::string ip_address;
-        int ip_port, requested_thread_num, max_locations_map_matching;
+        int ip_port, requested_thread_num;
         bool trial_run = false;
-        libosrm_config lib_config;
+        LibOSRMConfig lib_config;
         const unsigned init_result = GenerateServerProgramOptions(
             argc, argv, lib_config.server_paths, ip_address, ip_port, requested_thread_num,
-            lib_config.use_shared_memory, trial_run, lib_config.max_locations_distance_table,
-            max_locations_map_matching);
+            lib_config.use_shared_memory, trial_run, lib_config.max_locations_trip,
+            lib_config.max_locations_viaroute, lib_config.max_locations_distance_table,
+            lib_config.max_locations_map_matching);
 
         if (init_result == INIT_OK_DO_NOT_START_ENGINE)
         {
@@ -59,7 +60,7 @@ int main(int argc, const char *argv[])
         {
             return 1;
         }
-        SimpleLogger().Write() << "starting up engines, " << g_GIT_DESCRIPTION;
+        SimpleLogger().Write() << "starting up engines, " << OSRM_VERSION;
 
         OSRM routing_machine(lib_config);
 
diff --git a/tools/springclean.cpp b/tools/springclean.cpp
index 2d89106..18e5711 100644
--- a/tools/springclean.cpp
+++ b/tools/springclean.cpp
@@ -29,7 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "../data_structures/shared_memory_factory.hpp"
 #include "../server/data_structures/shared_datatype.hpp"
-#include "../util/git_sha.hpp"
+#include "util/version.hpp"
 #include "../util/simple_logger.hpp"
 
 void delete_region(const SharedDataType region)
@@ -77,7 +77,7 @@ int main()
     LogPolicy::GetInstance().Unmute();
     try
     {
-        SimpleLogger().Write() << "starting up engines, " << g_GIT_DESCRIPTION << "\n\n";
+        SimpleLogger().Write() << "starting up engines, " << OSRM_VERSION << "\n\n";
         SimpleLogger().Write() << "Releasing all locks";
         SimpleLogger().Write() << "ATTENTION! BE CAREFUL!";
         SimpleLogger().Write() << "----------------------";
diff --git a/tools/unlock_all_mutexes.cpp b/tools/unlock_all_mutexes.cpp
index 299aa61..953254b 100644
--- a/tools/unlock_all_mutexes.cpp
+++ b/tools/unlock_all_mutexes.cpp
@@ -25,7 +25,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 */
 
-#include "../util/git_sha.hpp"
+#include "util/version.hpp"
 #include "../util/simple_logger.hpp"
 #include "../server/data_structures/shared_barriers.hpp"
 
@@ -36,7 +36,7 @@ int main()
     LogPolicy::GetInstance().Unmute();
     try
     {
-        SimpleLogger().Write() << "starting up engines, " << g_GIT_DESCRIPTION;
+        SimpleLogger().Write() << "starting up engines, " << OSRM_VERSION;
         SimpleLogger().Write() << "Releasing all locks";
         SharedBarriers barrier;
         barrier.pending_update_mutex.unlock();
diff --git a/typedefs.h b/typedefs.h
index a8b75aa..dc77f39 100644
--- a/typedefs.h
+++ b/typedefs.h
@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define TYPEDEFS_H
 
 #include <limits>
+#include <osrm/strong_typedef.hpp>
+#include <cstddef>
 
 // Necessary workaround for Windows as VS doesn't implement C99
 #ifdef _MSC_VER
@@ -38,6 +40,21 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #endif
 #endif
 
+// OpenStreetMap node ids are higher than 2^32
+OSRM_STRONG_TYPEDEF(uint64_t, OSMNodeID)
+OSRM_STRONG_TYPEDEF(uint32_t, OSMWayID)
+
+static const OSMNodeID SPECIAL_OSM_NODEID = OSMNodeID(std::numeric_limits<std::uint64_t>::max());
+static const OSMWayID SPECIAL_OSM_WAYID = OSMWayID(std::numeric_limits<std::uint32_t>::max());
+
+static const OSMNodeID MAX_OSM_NODEID = OSMNodeID(std::numeric_limits<std::uint64_t>::max());
+static const OSMNodeID MIN_OSM_NODEID = OSMNodeID(std::numeric_limits<std::uint64_t>::min());
+static const OSMWayID MAX_OSM_WAYID = OSMWayID(std::numeric_limits<std::uint32_t>::max());
+static const OSMWayID MIN_OSM_WAYID = OSMWayID(std::numeric_limits<std::uint32_t>::min());
+
+using OSMNodeID_weak = std::uint64_t;
+using OSMEdgeID_weak = std::uint64_t;
+
 using NodeID = unsigned int;
 using EdgeID = unsigned int;
 using EdgeWeight = int;
@@ -45,7 +62,7 @@ using EdgeWeight = int;
 static const NodeID SPECIAL_NODEID = std::numeric_limits<unsigned>::max();
 static const EdgeID SPECIAL_EDGEID = std::numeric_limits<unsigned>::max();
 static const unsigned INVALID_NAMEID = std::numeric_limits<unsigned>::max();
-static const unsigned INVALID_COMPONENTID = std::numeric_limits<unsigned>::max();
+static const unsigned INVALID_COMPONENTID = 0;
 static const EdgeWeight INVALID_EDGE_WEIGHT = std::numeric_limits<int>::max();
 
 #endif /* TYPEDEFS_H */
diff --git a/unit_tests/algorithms/douglas_peucker.cpp b/unit_tests/algorithms/douglas_peucker.cpp
index 9654e4d..8a39209 100644
--- a/unit_tests/algorithms/douglas_peucker.cpp
+++ b/unit_tests/algorithms/douglas_peucker.cpp
@@ -30,11 +30,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <boost/test/unit_test.hpp>
 #include <boost/test/test_case_template.hpp>
-#include <boost/mpl/list.hpp>
 
 #include <osrm/coordinate.hpp>
 
-#include <iostream>
+#include <vector>
 
 BOOST_AUTO_TEST_SUITE(douglas_peucker)
 
diff --git a/unit_tests/algorithms/graph_compressor.cpp b/unit_tests/algorithms/graph_compressor.cpp
index 8f500eb..a3407c0 100644
--- a/unit_tests/algorithms/graph_compressor.cpp
+++ b/unit_tests/algorithms/graph_compressor.cpp
@@ -2,7 +2,7 @@
 #include "../../data_structures/compressed_edge_container.hpp"
 #include "../../data_structures/restriction_map.hpp"
 #include "../../data_structures/node_based_graph.hpp"
-#include "../../contractor/speed_profile.hpp"
+#include "../../extractor/speed_profile.hpp"
 #include "../../typedefs.h"
 
 #include <boost/test/unit_test.hpp>
@@ -28,14 +28,14 @@ BOOST_AUTO_TEST_CASE(long_road_test)
     using InputEdge = NodeBasedDynamicGraph::InputEdge;
     std::vector<InputEdge> edges = {
         // source, target, distance, edge_id, name_id, access_restricted, reversed, roundabout, travel_mode
-        {0, 1, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {1, 0, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {1, 2, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {2, 1, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {2, 3, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {3, 2, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {3, 4, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {4, 3, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT}
+        {0, 1, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {1, 0, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {1, 2, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {2, 1, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {2, 3, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {3, 2, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {3, 4, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {4, 3, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT}
     };
 
     BOOST_ASSERT(edges[0].data.IsCompatibleTo(edges[2].data));
@@ -70,18 +70,18 @@ BOOST_AUTO_TEST_CASE(loop_test)
     using InputEdge = NodeBasedDynamicGraph::InputEdge;
     std::vector<InputEdge> edges = {
         // source, target, distance, edge_id, name_id, access_restricted, forward, backward, roundabout, travel_mode
-        {0, 1, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {0, 5, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {1, 0, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {1, 2, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {2, 1, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {2, 3, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {3, 2, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {3, 4, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {4, 3, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {4, 5, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {5, 0, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {5, 4, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
+        {0, 1, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {0, 5, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {1, 0, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {1, 2, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {2, 1, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {2, 3, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {3, 2, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {3, 4, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {4, 3, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {4, 5, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {5, 0, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {5, 4, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
     };
 
     BOOST_ASSERT(edges.size() == 12);
@@ -127,12 +127,12 @@ BOOST_AUTO_TEST_CASE(t_intersection)
     using InputEdge = NodeBasedDynamicGraph::InputEdge;
     std::vector<InputEdge> edges = {
         // source, target, distance, edge_id, name_id, access_restricted, reversed, roundabout, travel_mode
-        {0, 1, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {1, 0, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {1, 2, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {1, 3, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {2, 1, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {3, 1, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
+        {0, 1, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {1, 0, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {1, 2, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {1, 3, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {2, 1, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {3, 1, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
     };
 
     BOOST_ASSERT(edges[0].data.IsCompatibleTo(edges[1].data));
@@ -165,10 +165,10 @@ BOOST_AUTO_TEST_CASE(street_name_changes)
     using InputEdge = NodeBasedDynamicGraph::InputEdge;
     std::vector<InputEdge> edges = {
         // source, target, distance, edge_id, name_id, access_restricted, forward, backward, roundabout, travel_mode
-        {0, 1, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {1, 0, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {1, 2, 1, SPECIAL_EDGEID, 1, false, false, false, TRAVEL_MODE_DEFAULT},
-        {2, 1, 1, SPECIAL_EDGEID, 1, false, false, false, TRAVEL_MODE_DEFAULT},
+        {0, 1, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {1, 0, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {1, 2, 1, SPECIAL_EDGEID, 1, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {2, 1, 1, SPECIAL_EDGEID, 1, false, false, false, true, TRAVEL_MODE_DEFAULT},
     };
 
     BOOST_ASSERT(edges[0].data.IsCompatibleTo(edges[1].data));
@@ -197,10 +197,10 @@ BOOST_AUTO_TEST_CASE(direction_changes)
     using InputEdge = NodeBasedDynamicGraph::InputEdge;
     std::vector<InputEdge> edges = {
         // source, target, distance, edge_id, name_id, access_restricted, reverse, roundabout, travel_mode
-        {0, 1, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {1, 0, 1, SPECIAL_EDGEID, 0, false, true,  false, TRAVEL_MODE_DEFAULT},
-        {1, 2, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
-        {2, 1, 1, SPECIAL_EDGEID, 0, false, false, false, TRAVEL_MODE_DEFAULT},
+        {0, 1, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {1, 0, 1, SPECIAL_EDGEID, 0, false, true,  false, true, TRAVEL_MODE_DEFAULT},
+        {1, 2, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
+        {2, 1, 1, SPECIAL_EDGEID, 0, false, false, false, true, TRAVEL_MODE_DEFAULT},
     };
 
     NodeBasedDynamicGraph graph(5, edges);
diff --git a/unit_tests/algorithms/string_util.cpp b/unit_tests/algorithms/string_util.cpp
index 865ddd5..cf82eba 100644
--- a/unit_tests/algorithms/string_util.cpp
+++ b/unit_tests/algorithms/string_util.cpp
@@ -34,16 +34,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 BOOST_AUTO_TEST_SUITE(string_util)
 
-BOOST_AUTO_TEST_CASE(replace_all_test)
-{
-    std::string input{"ababababababa"};
-    const std::string sub{"a"};
-    const std::string other{"c"};
-    replaceAll(input, sub, other);
-
-    BOOST_CHECK_EQUAL(input, "cbcbcbcbcbcbc");
-}
-
 BOOST_AUTO_TEST_CASE(json_escaping)
 {
     std::string input{"\b\\"};
diff --git a/unit_tests/data_structures/binary_heap.cpp b/unit_tests/data_structures/binary_heap.cpp
index 300d898..ee22a1f 100644
--- a/unit_tests/data_structures/binary_heap.cpp
+++ b/unit_tests/data_structures/binary_heap.cpp
@@ -32,7 +32,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <boost/test/test_case_template.hpp>
 #include <boost/mpl/list.hpp>
 
+#include <algorithm>
+#include <limits>
 #include <random>
+#include <vector>
 
 BOOST_AUTO_TEST_SUITE(binary_heap)
 
diff --git a/unit_tests/data_structures/dynamic_graph.cpp b/unit_tests/data_structures/dynamic_graph.cpp
index 5082278..df8eb21 100644
--- a/unit_tests/data_structures/dynamic_graph.cpp
+++ b/unit_tests/data_structures/dynamic_graph.cpp
@@ -26,15 +26,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "../../data_structures/dynamic_graph.hpp"
-#include "../../util/make_unique.hpp"
 #include "../../typedefs.h"
 
 #include <boost/test/unit_test.hpp>
 #include <boost/test/test_case_template.hpp>
-#include <boost/mpl/list.hpp>
 
-#include <random>
-#include <unordered_map>
+#include <vector>
 
 BOOST_AUTO_TEST_SUITE(dynamic_graph)
 
diff --git a/unit_tests/data_structures/range_table.cpp b/unit_tests/data_structures/range_table.cpp
index ec3b724..065840b 100644
--- a/unit_tests/data_structures/range_table.cpp
+++ b/unit_tests/data_structures/range_table.cpp
@@ -32,13 +32,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <boost/test/test_case_template.hpp>
 
 #include <numeric>
+#include <stxxl/vector>
 
 constexpr unsigned BLOCK_SIZE = 16;
 typedef RangeTable<BLOCK_SIZE, false> TestRangeTable;
 
 BOOST_AUTO_TEST_SUITE(range_table)
 
-void ConstructionTest(std::vector<unsigned> lengths, std::vector<unsigned> offsets)
+void ConstructionTest(stxxl::vector<unsigned> lengths, std::vector<unsigned> offsets)
 {
     BOOST_ASSERT(lengths.size() == offsets.size() - 1);
 
@@ -52,7 +53,7 @@ void ConstructionTest(std::vector<unsigned> lengths, std::vector<unsigned> offse
     }
 }
 
-void ComputeLengthsOffsets(std::vector<unsigned> &lengths,
+void ComputeLengthsOffsets(stxxl::vector<unsigned> &lengths,
                            std::vector<unsigned> &offsets,
                            unsigned num)
 {
@@ -76,7 +77,7 @@ void ComputeLengthsOffsets(std::vector<unsigned> &lengths,
 
 BOOST_AUTO_TEST_CASE(serialization_test)
 {
-    std::vector<unsigned> lengths;
+    stxxl::vector<unsigned> lengths;
     std::vector<unsigned> offsets;
     ComputeLengthsOffsets(lengths, offsets, (BLOCK_SIZE + 1) * 10);
 
@@ -98,10 +99,12 @@ BOOST_AUTO_TEST_CASE(serialization_test)
 BOOST_AUTO_TEST_CASE(construction_test)
 {
     // only offset empty block
-    ConstructionTest({1}, {0, 1});
+    stxxl::vector<unsigned> empty_lengths;
+    empty_lengths.push_back(1);
+    ConstructionTest(empty_lengths, {0, 1});
     // first block almost full => sentinel is last element of block
     // [0] {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, (16)}
-    std::vector<unsigned> almost_full_lengths;
+    stxxl::vector<unsigned> almost_full_lengths;
     std::vector<unsigned> almost_full_offsets;
     ComputeLengthsOffsets(almost_full_lengths, almost_full_offsets, BLOCK_SIZE);
     ConstructionTest(almost_full_lengths, almost_full_offsets);
@@ -109,7 +112,7 @@ BOOST_AUTO_TEST_CASE(construction_test)
     // first block full => sentinel is offset of new block, next block empty
     // [0]     {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}
     // [(153)] {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
-    std::vector<unsigned> full_lengths;
+    stxxl::vector<unsigned> full_lengths;
     std::vector<unsigned> full_offsets;
     ComputeLengthsOffsets(full_lengths, full_offsets, BLOCK_SIZE + 1);
     ConstructionTest(full_lengths, full_offsets);
@@ -117,13 +120,13 @@ BOOST_AUTO_TEST_CASE(construction_test)
     // first block full and offset of next block not sentinel, but the first differential value
     // [0]   {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}
     // [153] {(17), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
-    std::vector<unsigned> over_full_lengths;
+    stxxl::vector<unsigned> over_full_lengths;
     std::vector<unsigned> over_full_offsets;
     ComputeLengthsOffsets(over_full_lengths, over_full_offsets, BLOCK_SIZE + 2);
     ConstructionTest(over_full_lengths, over_full_offsets);
 
     // test multiple blocks
-    std::vector<unsigned> multiple_lengths;
+    stxxl::vector<unsigned> multiple_lengths;
     std::vector<unsigned> multiple_offsets;
     ComputeLengthsOffsets(multiple_lengths, multiple_offsets, (BLOCK_SIZE + 1) * 10);
     ConstructionTest(multiple_lengths, multiple_offsets);
diff --git a/unit_tests/data_structures/static_graph.cpp b/unit_tests/data_structures/static_graph.cpp
index 65704c6..ddadd7f 100644
--- a/unit_tests/data_structures/static_graph.cpp
+++ b/unit_tests/data_structures/static_graph.cpp
@@ -30,10 +30,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <boost/test/unit_test.hpp>
 #include <boost/test/test_case_template.hpp>
-#include <boost/mpl/list.hpp>
 
+#include <algorithm>
 #include <random>
-#include <unordered_map>
+#include <vector>
 
 BOOST_AUTO_TEST_SUITE(static_graph)
 
diff --git a/unit_tests/data_structures/static_rtree.cpp b/unit_tests/data_structures/static_rtree.cpp
index 5763647..2539d5f 100644
--- a/unit_tests/data_structures/static_rtree.cpp
+++ b/unit_tests/data_structures/static_rtree.cpp
@@ -26,20 +26,29 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "../../algorithms/coordinate_calculation.hpp"
+#include "../../algorithms/geospatial_query.hpp"
 #include "../../data_structures/static_rtree.hpp"
 #include "../../data_structures/query_node.hpp"
 #include "../../data_structures/edge_based_node.hpp"
 #include "../../util/floating_point.hpp"
 #include "../../typedefs.h"
 
+#include <boost/functional/hash.hpp>
 #include <boost/test/unit_test.hpp>
 #include <boost/test/test_case_template.hpp>
-#include <boost/mpl/list.hpp>
 
 #include <osrm/coordinate.hpp>
 
+#include <cstdint>
+#include <cmath>
+
+#include <algorithm>
+#include <memory>
 #include <random>
+#include <string>
+#include <utility>
 #include <unordered_set>
+#include <vector>
 
 BOOST_AUTO_TEST_SUITE(static_rtree)
 
@@ -47,11 +56,12 @@ constexpr uint32_t TEST_BRANCHING_FACTOR = 8;
 constexpr uint32_t TEST_LEAF_NODE_SIZE = 64;
 
 typedef EdgeBasedNode TestData;
-typedef StaticRTree<TestData,
-                    std::vector<FixedPointCoordinate>,
-                    false,
-                    TEST_BRANCHING_FACTOR,
-                    TEST_LEAF_NODE_SIZE> TestStaticRTree;
+using TestStaticRTree = StaticRTree<TestData,
+                                    std::vector<FixedPointCoordinate>,
+                                    false,
+                                    TEST_BRANCHING_FACTOR,
+                                    TEST_LEAF_NODE_SIZE>;
+using MiniStaticRTree = StaticRTree<TestData, std::vector<FixedPointCoordinate>, false, 2, 3>;
 
 // Choosen by a fair W20 dice roll (this value is completely arbitrary)
 constexpr unsigned RANDOM_SEED = 42;
@@ -60,112 +70,35 @@ static const int32_t WORLD_MAX_LAT = 90 * COORDINATE_PRECISION;
 static const int32_t WORLD_MIN_LON = -180 * COORDINATE_PRECISION;
 static const int32_t WORLD_MAX_LON = 180 * COORDINATE_PRECISION;
 
-class LinearSearchNN
+template <typename DataT> class LinearSearchNN
 {
   public:
     LinearSearchNN(const std::shared_ptr<std::vector<FixedPointCoordinate>> &coords,
-                   const std::vector<TestData> &edges)
+                   const std::vector<DataT> &edges)
         : coords(coords), edges(edges)
     {
     }
 
-    bool LocateClosestEndPointForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                            FixedPointCoordinate &result_coordinate)
+    std::vector<DataT> Nearest(const FixedPointCoordinate &input_coordinate,
+                               const unsigned num_results)
     {
-        float min_dist = std::numeric_limits<float>::max();
-        FixedPointCoordinate min_coord;
-        for (const TestData &e : edges)
-        {
-            const FixedPointCoordinate &start = coords->at(e.u);
-            const FixedPointCoordinate &end = coords->at(e.v);
-            float distance = coordinate_calculation::euclidean_distance(
-                input_coordinate.lat, input_coordinate.lon, start.lat, start.lon);
-            if (distance < min_dist)
-            {
-                min_coord = start;
-                min_dist = distance;
-            }
-
-            distance = coordinate_calculation::euclidean_distance(
-                input_coordinate.lat, input_coordinate.lon, end.lat, end.lon);
-            if (distance < min_dist)
-            {
-                min_coord = end;
-                min_dist = distance;
-            }
-        }
-
-        result_coordinate = min_coord;
-        return result_coordinate.is_valid();
-    }
-
-    bool FindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                      PhantomNode &result_phantom_node,
-                                      const unsigned zoom_level)
-    {
-        float min_dist = std::numeric_limits<float>::max();
-        TestData nearest_edge;
-        for (const TestData &e : edges)
-        {
-            if (e.component_id != 0)
-                continue;
-
-            float current_ratio = 0.;
-            FixedPointCoordinate nearest;
-            const float current_perpendicular_distance =
-                coordinate_calculation::perpendicular_distance(
-                    coords->at(e.u), coords->at(e.v), input_coordinate, nearest, current_ratio);
-
-            if ((current_perpendicular_distance < min_dist) &&
-                !osrm::epsilon_compare(current_perpendicular_distance, min_dist))
-            { // found a new minimum
-                min_dist = current_perpendicular_distance;
-                result_phantom_node = {e.forward_edge_based_node_id,
-                                       e.reverse_edge_based_node_id,
-                                       e.name_id,
-                                       e.forward_weight,
-                                       e.reverse_weight,
-                                       e.forward_offset,
-                                       e.reverse_offset,
-                                       e.packed_geometry_id,
-                                       e.component_id,
-                                       nearest,
-                                       e.fwd_segment_position,
-                                       e.forward_travel_mode,
-                                       e.backward_travel_mode};
-                nearest_edge = e;
-            }
-        }
+        std::vector<DataT> local_edges(edges);
 
-        if (result_phantom_node.location.is_valid())
-        {
-            // Hack to fix rounding errors and wandering via nodes.
-            if (1 == std::abs(input_coordinate.lon - result_phantom_node.location.lon))
-            {
-                result_phantom_node.location.lon = input_coordinate.lon;
-            }
-            if (1 == std::abs(input_coordinate.lat - result_phantom_node.location.lat))
-            {
-                result_phantom_node.location.lat = input_coordinate.lat;
-            }
-
-            const float distance_1 = coordinate_calculation::euclidean_distance(
-                coords->at(nearest_edge.u), result_phantom_node.location);
-            const float distance_2 = coordinate_calculation::euclidean_distance(
-                coords->at(nearest_edge.u), coords->at(nearest_edge.v));
-            const float ratio = std::min(1.f, distance_1 / distance_2);
-
-            if (SPECIAL_NODEID != result_phantom_node.forward_node_id)
+        std::nth_element(
+            local_edges.begin(), local_edges.begin() + num_results, local_edges.end(),
+            [this, &input_coordinate](const DataT &lhs, const DataT &rhs)
             {
-                result_phantom_node.forward_weight *= ratio;
-            }
-            if (SPECIAL_NODEID != result_phantom_node.reverse_node_id)
-            {
-                result_phantom_node.reverse_weight *= (1. - ratio);
-            }
-        }
-
-        return result_phantom_node.location.is_valid();
+                float current_ratio = 0.;
+                FixedPointCoordinate nearest;
+                const float lhs_dist = coordinate_calculation::perpendicular_distance(
+                    coords->at(lhs.u), coords->at(lhs.v), input_coordinate, nearest, current_ratio);
+                const float rhs_dist = coordinate_calculation::perpendicular_distance(
+                    coords->at(rhs.u), coords->at(rhs.v), input_coordinate, nearest, current_ratio);
+                return lhs_dist < rhs_dist;
+            });
+        local_edges.resize(num_results);
+
+        return local_edges;
     }
 
   private:
@@ -202,7 +135,7 @@ template <unsigned NUM_NODES, unsigned NUM_EDGES> struct RandomGraphFixture
         {
             int lat = lat_udist(g);
             int lon = lon_udist(g);
-            nodes.emplace_back(QueryNode(lat, lon, i));
+            nodes.emplace_back(QueryNode(lat, lon, OSMNodeID(i)));
             coords->emplace_back(FixedPointCoordinate(lat, lon));
         }
 
@@ -218,7 +151,7 @@ template <unsigned NUM_NODES, unsigned NUM_EDGES> struct RandomGraphFixture
             if (used_edges.find(std::pair<unsigned, unsigned>(
                     std::min(data.u, data.v), std::max(data.u, data.v))) == used_edges.end())
             {
-                data.component_id = 0;
+                data.component.id = 0;
                 edges.emplace_back(data);
                 used_edges.emplace(std::min(data.u, data.v), std::max(data.u, data.v));
             }
@@ -242,7 +175,7 @@ struct GraphFixture
             FixedPointCoordinate c(input_coords[i].first * COORDINATE_PRECISION,
                                    input_coords[i].second * COORDINATE_PRECISION);
             coords->emplace_back(c);
-            nodes.emplace_back(QueryNode(c.lat, c.lon, i));
+            nodes.emplace_back(QueryNode(c.lat, c.lon, OSMNodeID(i)));
         }
 
         for (const auto &pair : input_edges)
@@ -250,6 +183,12 @@ struct GraphFixture
             TestData d;
             d.u = pair.first;
             d.v = pair.second;
+            // We set the forward nodes to the target node-based-node IDs, just
+            // so we have something to test against.  Because this isn't a real
+            // graph, the actual values aren't important, we just need something
+            // to examine during tests.
+            d.forward_edge_based_node_id = pair.second;
+            d.reverse_edge_based_node_id = pair.first;
             edges.emplace_back(d);
         }
     }
@@ -280,23 +219,18 @@ void simple_verify_rtree(RTreeT &rtree,
     BOOST_TEST_MESSAGE("Verify end points");
     for (const auto &e : edges)
     {
-        FixedPointCoordinate result_u, result_v;
         const FixedPointCoordinate &pu = coords->at(e.u);
         const FixedPointCoordinate &pv = coords->at(e.v);
-        bool found_u = rtree.LocateClosestEndPointForCoordinate(pu, result_u, 1);
-        bool found_v = rtree.LocateClosestEndPointForCoordinate(pv, result_v, 1);
-        BOOST_CHECK(found_u && found_v);
-        float dist_u =
-            coordinate_calculation::euclidean_distance(result_u.lat, result_u.lon, pu.lat, pu.lon);
-        BOOST_CHECK_LE(dist_u, std::numeric_limits<float>::epsilon());
-        float dist_v =
-            coordinate_calculation::euclidean_distance(result_v.lat, result_v.lon, pv.lat, pv.lon);
-        BOOST_CHECK_LE(dist_v, std::numeric_limits<float>::epsilon());
+        auto result_u = rtree.Nearest(pu, 1);
+        auto result_v = rtree.Nearest(pv, 1);
+        BOOST_CHECK(result_u.size() == 1 && result_v.size() == 1);
+        BOOST_CHECK(result_u.front().u == e.u || result_u.front().v == e.u);
+        BOOST_CHECK(result_v.front().u == e.v || result_v.front().v == e.v);
     }
 }
 
 template <typename RTreeT>
-void sampling_verify_rtree(RTreeT &rtree, LinearSearchNN &lsnn, unsigned num_samples)
+void sampling_verify_rtree(RTreeT &rtree, LinearSearchNN<TestData> &lsnn, const std::vector<FixedPointCoordinate>& coords, unsigned num_samples)
 {
     std::mt19937 g(RANDOM_SEED);
     std::uniform_int_distribution<> lat_udist(WORLD_MIN_LAT, WORLD_MAX_LAT);
@@ -310,17 +244,22 @@ void sampling_verify_rtree(RTreeT &rtree, LinearSearchNN &lsnn, unsigned num_sam
     BOOST_TEST_MESSAGE("Sampling queries");
     for (const auto &q : queries)
     {
-        FixedPointCoordinate result_rtree;
-        rtree.LocateClosestEndPointForCoordinate(q, result_rtree, 1);
-        FixedPointCoordinate result_ln;
-        lsnn.LocateClosestEndPointForCoordinate(q, result_ln);
-        BOOST_CHECK_EQUAL(result_ln, result_rtree);
-
-        PhantomNode phantom_rtree;
-        rtree.FindPhantomNodeForCoordinate(q, phantom_rtree, 1);
-        PhantomNode phantom_ln;
-        lsnn.FindPhantomNodeForCoordinate(q, phantom_ln, 1);
-        BOOST_CHECK_EQUAL(phantom_rtree, phantom_ln);
+        auto result_rtree = rtree.Nearest(q, 1);
+        auto result_lsnn = lsnn.Nearest(q, 1);
+        BOOST_CHECK(result_rtree.size() == 1);
+        BOOST_CHECK(result_lsnn.size() == 1);
+        auto rtree_u = result_rtree.back().u;
+        auto rtree_v = result_rtree.back().v;
+        auto lsnn_u = result_lsnn.back().u;
+        auto lsnn_v = result_lsnn.back().v;
+
+        float current_ratio = 0.;
+        FixedPointCoordinate nearest;
+        const float rtree_dist = coordinate_calculation::perpendicular_distance(
+            coords[rtree_u], coords[rtree_v], q, nearest, current_ratio);
+        const float lsnn_dist = coordinate_calculation::perpendicular_distance(
+            coords[lsnn_u], coords[lsnn_v], q, nearest, current_ratio);
+        BOOST_CHECK_LE(std::abs(rtree_dist - lsnn_dist), std::numeric_limits<float>::epsilon());
     }
 }
 
@@ -350,10 +289,10 @@ void construction_test(const std::string &prefix, FixtureT *fixture)
     std::string nodes_path;
     build_rtree<FixtureT, RTreeT>(prefix, fixture, leaves_path, nodes_path);
     RTreeT rtree(nodes_path, leaves_path, fixture->coords);
-    LinearSearchNN lsnn(fixture->coords, fixture->edges);
+    LinearSearchNN<TestData> lsnn(fixture->coords, fixture->edges);
 
     simple_verify_rtree(rtree, fixture->coords, fixture->edges);
-    sampling_verify_rtree(rtree, lsnn, 100);
+    sampling_verify_rtree(rtree, lsnn, *fixture->coords, 100);
 }
 
 BOOST_FIXTURE_TEST_CASE(construct_half_leaf_test, TestRandomGraphFixture_LeafHalfFull)
@@ -381,51 +320,43 @@ BOOST_FIXTURE_TEST_CASE(construct_multiple_levels_test, TestRandomGraphFixture_M
     construction_test("test_5", this);
 }
 
-/*
- * Bug: If you querry a point that lies between two BBs that have a gap,
- * one BB will be pruned, even if it could contain a nearer match.
- */
+// Bug: If you querry a point that lies between two BBs that have a gap,
+// one BB will be pruned, even if it could contain a nearer match.
 BOOST_AUTO_TEST_CASE(regression_test)
 {
-    typedef std::pair<float, float> Coord;
-    typedef std::pair<unsigned, unsigned> Edge;
+    using Coord = std::pair<float, float>;
+    using Edge = std::pair<unsigned, unsigned>;
     GraphFixture fixture(
         {
-         Coord(40.0, 0.0),
-         Coord(35.0, 5.0),
+            Coord(40.0, 0.0), Coord(35.0, 5.0),
 
-         Coord(5.0, 5.0),
-         Coord(0.0, 10.0),
+            Coord(5.0, 5.0), Coord(0.0, 10.0),
 
-         Coord(20.0, 10.0),
-         Coord(20.0, 5.0),
+            Coord(20.0, 10.0), Coord(20.0, 5.0),
 
-         Coord(40.0, 100.0),
-         Coord(35.0, 105.0),
+            Coord(40.0, 100.0), Coord(35.0, 105.0),
 
-         Coord(5.0, 105.0),
-         Coord(0.0, 110.0),
+            Coord(5.0, 105.0), Coord(0.0, 110.0),
         },
         {Edge(0, 1), Edge(2, 3), Edge(4, 5), Edge(6, 7), Edge(8, 9)});
 
-    typedef StaticRTree<TestData, std::vector<FixedPointCoordinate>, false, 2, 3> MiniStaticRTree;
-
     std::string leaves_path;
     std::string nodes_path;
     build_rtree<GraphFixture, MiniStaticRTree>("test_regression", &fixture, leaves_path,
                                                nodes_path);
     MiniStaticRTree rtree(nodes_path, leaves_path, fixture.coords);
+    LinearSearchNN<TestData> lsnn(fixture.coords, fixture.edges);
 
     // query a node just right of the center of the gap
     FixedPointCoordinate input(20.0 * COORDINATE_PRECISION, 55.1 * COORDINATE_PRECISION);
-    FixedPointCoordinate result;
-    rtree.LocateClosestEndPointForCoordinate(input, result, 1);
-    FixedPointCoordinate result_ln;
-    LinearSearchNN lsnn(fixture.coords, fixture.edges);
-    lsnn.LocateClosestEndPointForCoordinate(input, result_ln);
-
-    // TODO: reactivate
-    // BOOST_CHECK_EQUAL(result_ln, result);
+    auto result_rtree = rtree.Nearest(input, 1);
+    auto result_ls = lsnn.Nearest(input, 1);
+
+    BOOST_CHECK(result_rtree.size() == 1);
+    BOOST_CHECK(result_ls.size() == 1);
+
+    BOOST_CHECK_EQUAL(result_ls.front().u, result_rtree.front().u);
+    BOOST_CHECK_EQUAL(result_ls.front().v, result_rtree.front().v);
 }
 
 void TestRectangle(double width, double height, double center_lat, double center_lon)
@@ -433,7 +364,7 @@ void TestRectangle(double width, double height, double center_lat, double center
     FixedPointCoordinate center(center_lat * COORDINATE_PRECISION,
                                 center_lon * COORDINATE_PRECISION);
 
-    TestStaticRTree::RectangleT rect;
+    TestStaticRTree::Rectangle rect;
     rect.min_lat = center.lat - height / 2.0 * COORDINATE_PRECISION;
     rect.max_lat = center.lat + height / 2.0 * COORDINATE_PRECISION;
     rect.min_lon = center.lon - width / 2.0 * COORDINATE_PRECISION;
@@ -451,30 +382,30 @@ void TestRectangle(double width, double height, double center_lat, double center
 
     /* Distance to line segments of rectangle */
     BOOST_CHECK_EQUAL(rect.GetMinDist(north),
-                      coordinate_calculation::euclidean_distance(
+                      coordinate_calculation::great_circle_distance(
                           north, FixedPointCoordinate(rect.max_lat, north.lon)));
     BOOST_CHECK_EQUAL(rect.GetMinDist(south),
-                      coordinate_calculation::euclidean_distance(
+                      coordinate_calculation::great_circle_distance(
                           south, FixedPointCoordinate(rect.min_lat, south.lon)));
     BOOST_CHECK_EQUAL(rect.GetMinDist(west),
-                      coordinate_calculation::euclidean_distance(
+                      coordinate_calculation::great_circle_distance(
                           west, FixedPointCoordinate(west.lat, rect.min_lon)));
     BOOST_CHECK_EQUAL(rect.GetMinDist(east),
-                      coordinate_calculation::euclidean_distance(
+                      coordinate_calculation::great_circle_distance(
                           east, FixedPointCoordinate(east.lat, rect.max_lon)));
 
     /* Distance to corner points */
     BOOST_CHECK_EQUAL(rect.GetMinDist(north_east),
-                      coordinate_calculation::euclidean_distance(
+                      coordinate_calculation::great_circle_distance(
                           north_east, FixedPointCoordinate(rect.max_lat, rect.max_lon)));
     BOOST_CHECK_EQUAL(rect.GetMinDist(north_west),
-                      coordinate_calculation::euclidean_distance(
+                      coordinate_calculation::great_circle_distance(
                           north_west, FixedPointCoordinate(rect.max_lat, rect.min_lon)));
     BOOST_CHECK_EQUAL(rect.GetMinDist(south_east),
-                      coordinate_calculation::euclidean_distance(
+                      coordinate_calculation::great_circle_distance(
                           south_east, FixedPointCoordinate(rect.min_lat, rect.max_lon)));
     BOOST_CHECK_EQUAL(rect.GetMinDist(south_west),
-                      coordinate_calculation::euclidean_distance(
+                      coordinate_calculation::great_circle_distance(
                           south_west, FixedPointCoordinate(rect.min_lat, rect.min_lon)));
 }
 
@@ -487,4 +418,63 @@ BOOST_AUTO_TEST_CASE(rectangle_test)
     TestRectangle(10, 10, 0, 0);
 }
 
+BOOST_AUTO_TEST_CASE(bearing_tests)
+{
+    using Coord = std::pair<float, float>;
+    using Edge = std::pair<unsigned, unsigned>;
+    GraphFixture fixture(
+        {
+            Coord(0.0, 0.0), Coord(10.0, 10.0),
+        },
+        {Edge(0, 1), Edge(1, 0)});
+
+    std::string leaves_path;
+    std::string nodes_path;
+    build_rtree<GraphFixture, MiniStaticRTree>("test_bearing", &fixture, leaves_path, nodes_path);
+    MiniStaticRTree rtree(nodes_path, leaves_path, fixture.coords);
+    GeospatialQuery<MiniStaticRTree> query(rtree, fixture.coords);
+
+    FixedPointCoordinate input(5.0 * COORDINATE_PRECISION, 5.1 * COORDINATE_PRECISION);
+
+    {
+        auto results = query.NearestPhantomNodes(input, 5);
+        BOOST_CHECK_EQUAL(results.size(), 2);
+        BOOST_CHECK_EQUAL(results.back().phantom_node.forward_node_id, 0);
+        BOOST_CHECK_EQUAL(results.back().phantom_node.reverse_node_id, 1);
+    }
+
+    {
+        auto results = query.NearestPhantomNodes(input, 5, 270, 10);
+        BOOST_CHECK_EQUAL(results.size(), 0);
+    }
+
+    {
+        auto results = query.NearestPhantomNodes(input, 5, 45, 10);
+        BOOST_CHECK_EQUAL(results.size(), 2);
+        BOOST_CHECK_EQUAL(results[0].phantom_node.forward_node_id, 1);
+        BOOST_CHECK_EQUAL(results[0].phantom_node.reverse_node_id, SPECIAL_NODEID);
+        BOOST_CHECK_EQUAL(results[1].phantom_node.forward_node_id, SPECIAL_NODEID);
+        BOOST_CHECK_EQUAL(results[1].phantom_node.reverse_node_id, 1);
+    }
+
+    {
+        auto results = query.NearestPhantomNodesInRange(input, 11000);
+        BOOST_CHECK_EQUAL(results.size(), 2);
+    }
+
+    {
+        auto results = query.NearestPhantomNodesInRange(input, 11000, 270, 10);
+        BOOST_CHECK_EQUAL(results.size(), 0);
+    }
+
+    {
+        auto results = query.NearestPhantomNodesInRange(input, 11000, 45, 10);
+        BOOST_CHECK_EQUAL(results.size(), 2);
+        BOOST_CHECK_EQUAL(results[0].phantom_node.forward_node_id, 1);
+        BOOST_CHECK_EQUAL(results[0].phantom_node.reverse_node_id, SPECIAL_NODEID);
+        BOOST_CHECK_EQUAL(results[1].phantom_node.forward_node_id, SPECIAL_NODEID);
+        BOOST_CHECK_EQUAL(results[1].phantom_node.reverse_node_id, 1);
+    }
+}
+
 BOOST_AUTO_TEST_SUITE_END()
diff --git a/unit_tests/util/bearing.cpp b/unit_tests/util/bearing.cpp
new file mode 100644
index 0000000..f0866de
--- /dev/null
+++ b/unit_tests/util/bearing.cpp
@@ -0,0 +1,72 @@
+/*
+
+Copyright (c) 2015, Project OSRM contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+*/
+
+#include "../../util/bearing.hpp"
+#include "../../typedefs.h"
+
+#include <boost/functional/hash.hpp>
+#include <boost/test/unit_test.hpp>
+#include <boost/test/test_case_template.hpp>
+
+BOOST_AUTO_TEST_SUITE(bearing)
+
+// Verify that the bearing-bounds checking function behaves as expected
+BOOST_AUTO_TEST_CASE(bearing_range_test)
+{
+    // Simple, non-edge-case checks
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(45, 45, 10));
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(35, 45, 10));
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(55, 45, 10));
+
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(34, 45, 10));
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(56, 45, 10));
+
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(34, 45, 10));
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(56, 45, 10));
+
+    // When angle+limit goes > 360
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(359, 355, 10));
+
+    // When angle-limit goes < 0
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(359, 5, 10));
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(354, 5, 10));
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(16, 5, 10));
+
+
+    // Checking other cases of wraparound
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(359, -5, 10));
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(344, -5, 10));
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(6, -5, 10));
+
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(-1, 5, 10));
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(-6, 5, 10));
+
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(-721, 5, 10));
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(719, 5, 10));
+}
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/util/git_sha.hpp b/unit_tests/util_tests.cpp
similarity index 86%
rename from util/git_sha.hpp
rename to unit_tests/util_tests.cpp
index b0a9a7c..26275ab 100644
--- a/util/git_sha.hpp
+++ b/unit_tests/util_tests.cpp
@@ -1,6 +1,6 @@
 /*
 
-Copyright (c) 2015, Project OSRM contributors
+Copyright (c) 2014, Project OSRM contributors
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without modification,
@@ -25,9 +25,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 */
 
-#ifndef GIT_SHA_HPP
-#define GIT_SHA_HPP
+#define BOOST_TEST_MODULE util tests
 
-extern char g_GIT_DESCRIPTION[];
+#include <boost/test/unit_test.hpp>
 
-#endif // GIT_SHA_HPP
+/*
+ * This file will contain an automatically generated main function.
+ */
diff --git a/util/bearing.cpp b/util/bearing.cpp
deleted file mode 100644
index 48fd7f9..0000000
--- a/util/bearing.cpp
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
-
-Copyright (c) 2015, Project OSRM contributors
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without modification,
-are permitted provided that the following conditions are met:
-
-Redistributions of source code must retain the above copyright notice, this list
-of conditions and the following disclaimer.
-Redistributions in binary form must reproduce the above copyright notice, this
-list of conditions and the following disclaimer in the documentation and/or
-other materials provided with the distribution.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
-ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
-ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-*/
-
-#include "bearing.hpp"
-
-std::string bearing::get(const double heading) noexcept
-{
-    if (heading <= 22.5)
-    {
-        return "N";
-    }
-    if (heading <= 67.5)
-    {
-        return "NE";
-    }
-    if (heading <= 112.5)
-    {
-        return "E";
-    }
-    if (heading <= 157.5)
-    {
-        return "SE";
-    }
-    if (heading <= 202.5)
-    {
-        return "S";
-    }
-    if (heading <= 247.5)
-    {
-        return "SW";
-    }
-    if (heading <= 292.5)
-    {
-        return "W";
-    }
-    if (heading <= 337.5)
-    {
-        return "NW";
-    }
-    return "N";
-}
diff --git a/util/bearing.hpp b/util/bearing.hpp
index 8b16ebc..5f3036a 100644
--- a/util/bearing.hpp
+++ b/util/bearing.hpp
@@ -28,11 +28,89 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef BEARING_HPP
 #define BEARING_HPP
 
+#include <boost/assert.hpp>
 #include <string>
 
-struct bearing
+namespace bearing
 {
-    static std::string get(const double heading) noexcept;
-};
+inline std::string get(const double heading)
+{
+    BOOST_ASSERT(heading >= 0);
+    BOOST_ASSERT(heading <= 360);
+
+    if (heading <= 22.5)
+    {
+        return "N";
+    }
+    if (heading <= 67.5)
+    {
+        return "NE";
+    }
+    if (heading <= 112.5)
+    {
+        return "E";
+    }
+    if (heading <= 157.5)
+    {
+        return "SE";
+    }
+    if (heading <= 202.5)
+    {
+        return "S";
+    }
+    if (heading <= 247.5)
+    {
+        return "SW";
+    }
+    if (heading <= 292.5)
+    {
+        return "W";
+    }
+    if (heading <= 337.5)
+    {
+        return "NW";
+    }
+    return "N";
+}
+
+// Checks whether A is between B-range and B+range, all modulo 360
+// e.g. A = 5, B = 5, range = 10 == true
+//      A = -6, B = 5, range = 10 == false
+//      A = -2, B = 355, range = 10 == true
+//      A = 6, B = 355, range = 10 == false
+//      A = 355, B = -2, range = 10 == true
+//
+// @param A the bearing to check, in degrees, 0-359, 0=north
+// @param B the bearing to check against, in degrees, 0-359, 0=north
+// @param range the number of degrees either side of B that A will still match
+// @return true if B-range <= A <= B+range, modulo 360
+inline bool CheckInBounds(const int A, const int B, const int range)
+{
+
+    if (range >= 180)
+        return true;
+    if (range <= 0)
+        return false;
+
+    // Map both bearings into positive modulo 360 space
+    const int normalized_B = (B < 0) ? (B % 360) + 360 : (B % 360);
+    const int normalized_A = (A < 0) ? (A % 360) + 360 : (A % 360);
+
+    if (normalized_B - range < 0)
+    {
+        return (normalized_B - range + 360 <= normalized_A && normalized_A < 360) ||
+               (0 <= normalized_A && normalized_A <= normalized_B + range);
+    }
+    else if (normalized_B + range > 360)
+    {
+        return (normalized_B - range <= normalized_A && normalized_A < 360) ||
+               (0 <= normalized_A && normalized_A <= normalized_B + range - 360);
+    }
+    else
+    {
+        return normalized_B - range <= normalized_A && normalized_A <= normalized_B + range;
+    }
+}
+}
 
 #endif // BEARING_HPP
diff --git a/util/boost_filesystem_2_fix.hpp b/util/boost_filesystem_2_fix.hpp
deleted file mode 100644
index 2e41abd..0000000
--- a/util/boost_filesystem_2_fix.hpp
+++ /dev/null
@@ -1,144 +0,0 @@
-/*
-
-Copyright (c) 2015, Project OSRM contributors
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without modification,
-are permitted provided that the following conditions are met:
-
-Redistributions of source code must retain the above copyright notice, this list
-of conditions and the following disclaimer.
-Redistributions in binary form must reproduce the above copyright notice, this
-list of conditions and the following disclaimer in the documentation and/or
-other materials provided with the distribution.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
-ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
-ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-*/
-
-#ifndef BOOST_FILE_SYSTEM_FIX_H
-#define BOOST_FILE_SYSTEM_FIX_H
-
-#include "osrm_exception.hpp"
-
-// #include <boost/any.hpp>
-#include <boost/filesystem.hpp>
-// #include <boost/program_options.hpp>
-
-// This is one big workaround for latest boost renaming woes.
-
-#if BOOST_FILESYSTEM_VERSION < 3
-#warning Boost Installation with Filesystem3 missing, activating workaround
-#include <cstdio>
-#endif
-
-namespace boost
-{
-namespace filesystem
-{
-
-// Validator for boost::filesystem::path, that verifies that the file
-// exists. The validate() function must be defined in the same namespace
-// as the target type, (boost::filesystem::path in this case), otherwise
-// it is not called
-// inline void validate(
-//     boost::any & v,
-//     const std::vector<std::string> & values,
-//     boost::filesystem::path *,
-//     int
-// ) {
-//     boost::program_options::validators::check_first_occurrence(v);
-//     const std::string & input_string =
-//         boost::program_options::validators::get_single_string(values);
-//     if(boost::filesystem::is_regular_file(input_string)) {
-//         v = boost::any(boost::filesystem::path(input_string));
-//     } else {
-//         throw osrm::exception(input_string + " not found");
-//     }
-// }
-
-// adapted from:
-// http://stackoverflow.com/questions/1746136/how-do-i-normalize-a-pathname-using-boostfilesystem
-inline boost::filesystem::path
-portable_canonical(const boost::filesystem::path &relative_path,
-                   const boost::filesystem::path &current_path = boost::filesystem::current_path())
-{
-    const boost::filesystem::path absolute_path =
-        boost::filesystem::absolute(relative_path, current_path);
-
-    boost::filesystem::path canonical_path;
-    for (auto path_iterator = absolute_path.begin(); path_iterator != absolute_path.end();
-         ++path_iterator)
-    {
-        if (".." == path_iterator->string())
-        {
-            // /a/b/.. is not necessarily /a if b is a symbolic link
-            if (boost::filesystem::is_symlink(canonical_path))
-            {
-                canonical_path /= *path_iterator;
-            }
-            else if (".." == canonical_path.filename())
-            {
-                // /a/b/../.. is not /a/b/.. under most circumstances
-                // We can end up with ..s in our result because of symbolic links
-                canonical_path /= *path_iterator;
-            }
-            else
-            {
-                // Otherwise it should be safe to resolve the parent
-                canonical_path = canonical_path.parent_path();
-            }
-        }
-        else if ("." == path_iterator->string())
-        {
-            // Ignore
-        }
-        else
-        {
-            // Just cat other path entries
-            canonical_path /= *path_iterator;
-        }
-    }
-    BOOST_ASSERT(canonical_path.is_absolute());
-    BOOST_ASSERT(boost::filesystem::exists(canonical_path));
-    return canonical_path;
-}
-
-#if BOOST_FILESYSTEM_VERSION < 3
-
-inline path temp_directory_path()
-{
-    char *buffer;
-    buffer = tmpnam(nullptr);
-
-    return path(buffer);
-}
-
-inline path unique_path(const path &) { return temp_directory_path(); }
-
-#endif
-}
-}
-
-#ifndef BOOST_FILESYSTEM_VERSION
-#define BOOST_FILESYSTEM_VERSION 3
-#endif
-
-inline void AssertPathExists(const boost::filesystem::path &path)
-{
-    if (!boost::filesystem::is_regular_file(path))
-    {
-        throw osrm::exception(path.string() + " not found.");
-    }
-}
-
-#endif /* BOOST_FILE_SYSTEM_FIX_H */
diff --git a/util/cast.hpp b/util/cast.hpp
index 6d6ff65..a9e97d6 100644
--- a/util/cast.hpp
+++ b/util/cast.hpp
@@ -28,158 +28,42 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CAST_HPP
 #define CAST_HPP
 
-#include <boost/spirit/include/karma.hpp>
-#include <boost/spirit/include/qi.hpp>
-
 #include <string>
+#include <sstream>
+#include <iomanip>
 #include <type_traits>
 
-struct cast
-{
-    // convert scoped enums to integers
-    template <typename Enumeration>
-    static auto enum_to_underlying(Enumeration const value) ->
-        typename std::underlying_type<Enumeration>::type
-    {
-        return static_cast<typename std::underlying_type<Enumeration>::type>(value);
-    }
-
-    template <typename Number>
-    static typename std::enable_if<std::is_integral<Number>::value, std::string>::type
-    integral_to_string(const Number value)
-    {
-        std::string output;
-        std::back_insert_iterator<std::string> sink(output);
-
-        if (8 == sizeof(Number))
-        {
-            boost::spirit::karma::generate(sink, boost::spirit::karma::long_long, value);
-        }
-        else
-        {
-            if (std::is_signed<Number>::value)
-            {
-                boost::spirit::karma::generate(sink, boost::spirit::karma::int_, value);
-            }
-            else
-            {
-                boost::spirit::karma::generate(sink, boost::spirit::karma::uint_, value);
-            }
-        }
-        return output;
-    }
-
-    static int string_to_int(const std::string &input)
-    {
-        auto first_digit = input.begin();
-        // Delete any trailing white-spaces
-        while (first_digit != input.end() && std::isspace(*first_digit))
-        {
-            ++first_digit;
-        }
-        int value = 0;
-        boost::spirit::qi::parse(first_digit, input.end(), boost::spirit::int_, value);
-        return value;
-    }
-
-    static unsigned string_to_uint(const std::string &input)
-    {
-        auto first_digit = input.begin();
-        // Delete any trailing white-spaces
-        while (first_digit != input.end() && (std::isspace(*first_digit) || '-' == *first_digit))
-        {
-            ++first_digit;
-        }
-        unsigned value = 0;
-        boost::spirit::qi::parse(first_digit, input.end(), boost::spirit::uint_, value);
-        return value;
-    }
-
-    static uint64_t string_to_uint64(const std::string &input)
-    {
-        auto first_digit = input.begin();
-        // Delete any trailing white-spaces
-        while (first_digit != input.end() && std::isspace(*first_digit))
-        {
-            ++first_digit;
-        }
-        uint64_t value = 0;
-        boost::spirit::qi::parse(first_digit, input.end(), boost::spirit::long_long, value);
-        return value;
-    }
+#include <boost/algorithm/string/classification.hpp>
+#include <boost/algorithm/string/trim.hpp>
 
-    // source: http://tinodidriksen.com/2011/05/28/cpp-convert-string-to-double-speed/
-    static double string_to_double(const char *p) noexcept
-    {
-        double r = 0.0;
-        bool neg = false;
-        if (*p == '-')
-        {
-            neg = true;
-            ++p;
-        }
-        while (*p >= '0' && *p <= '9')
-        {
-            r = (r * 10.0) + (*p - '0');
-            ++p;
-        }
-        if (*p == '.')
-        {
-            double f = 0.0;
-            int n = 0;
-            ++p;
-            while (*p >= '0' && *p <= '9')
-            {
-                f = (f * 10.0) + (*p - '0');
-                ++p;
-                ++n;
-            }
-            r += f / std::pow(10.0, n);
-        }
-        if (neg)
-        {
-            r = -r;
-        }
-        return r;
-    }
-
-    template <typename T> struct scientific_policy : boost::spirit::karma::real_policies<T>
-    {
-        //  we want the numbers always to be in fixed format
-        static int floatfield(T) { return boost::spirit::karma::real_policies<T>::fmtflags::fixed; }
-        static unsigned int precision(T) { return 6; }
-    };
-    using science_type = boost::spirit::karma::real_generator<double, scientific_policy<double>>;
-
-    static std::string double_fixed_to_string(const double value)
-    {
-        std::string output;
-        std::back_insert_iterator<std::string> sink(output);
-        boost::spirit::karma::generate(sink, science_type(), value);
-        if (output.size() >= 2 && output[output.size() - 2] == '.' &&
-            output[output.size() - 1] == '0')
-        {
-            output.resize(output.size() - 2);
-        }
-        return output;
-    }
-
-    static std::string double_to_string(const double value)
-    {
-        std::string output;
-        std::back_insert_iterator<std::string> sink(output);
-        boost::spirit::karma::generate(sink, value);
-        return output;
-    }
+namespace cast
+{
+template <typename Enumeration>
+inline auto enum_to_underlying(Enumeration const value) ->
+    typename std::underlying_type<Enumeration>::type
+{
+    return static_cast<typename std::underlying_type<Enumeration>::type>(value);
+}
 
-    static void double_with_two_digits_to_string(const double value, std::string &output)
-    {
-        // The largest 32-bit integer is 4294967295, that is 10 chars
-        // On the safe side, add 1 for sign, and 1 for trailing zero
-        char buffer[12];
-        sprintf(buffer, "%g", value);
-        output = buffer;
-    }
-};
+template <typename T, int Precision = 6> inline std::string to_string_with_precision(const T x)
+{
+    static_assert(std::is_arithmetic<T>::value, "integral or floating point type required");
+
+    std::ostringstream out;
+    out << std::fixed << std::setprecision(Precision) << x;
+    auto rv = out.str();
+
+    // Javascript has no separation of float / int, digits without a '.' are integral typed
+    // X.Y.0 -> X.Y
+    // X.0 -> X
+    boost::trim_right_if(rv, boost::is_any_of("0"));
+    boost::trim_right_if(rv, boost::is_any_of("."));
+    // Note:
+    //  - assumes the locale to use '.' as digit separator
+    //  - this is not identical to:  trim_right_if(rv, is_any_of('0 .'))
+
+    return rv;
+}
+}
 
 #endif // CAST_HPP
diff --git a/util/container.hpp b/util/container.hpp
index d455e94..14b88ee 100644
--- a/util/container.hpp
+++ b/util/container.hpp
@@ -98,7 +98,7 @@ Function for_each_pair(ContainerT &container, Function function)
     return for_each_pair(std::begin(container), std::end(container), function);
 }
 
-template <class Container> void append_to_container(Container &&a) {}
+template <class Container> void append_to_container(Container &&) {}
 
 template <class Container, typename T, typename... Args>
 void append_to_container(Container &&container, T value, Args &&... args)
diff --git a/util/datastore_options.hpp b/util/datastore_options.hpp
index 4cff3d3..4a8320c 100644
--- a/util/datastore_options.hpp
+++ b/util/datastore_options.hpp
@@ -28,22 +28,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef DATASTORE_OPTIONS_HPP
 #define DATASTORE_OPTIONS_HPP
 
-#include "boost_filesystem_2_fix.hpp"
-#include "git_sha.hpp"
+#include "util/version.hpp"
 #include "ini_file.hpp"
 #include "osrm_exception.hpp"
 #include "simple_logger.hpp"
 
-#include <osrm/server_paths.hpp>
-
 #include <boost/any.hpp>
 #include <boost/filesystem.hpp>
 #include <boost/program_options.hpp>
 
 #include <string>
+#include <unordered_map>
 
 // generate boost::program_options object for the routing part
-bool GenerateDataStoreOptions(const int argc, const char *argv[], ServerPaths &paths)
+bool GenerateDataStoreOptions(const int argc, const char *argv[], std::unordered_map<std::string, boost::filesystem::path> &paths)
 {
     // declare a group of options that will be allowed only on command line
     boost::program_options::options_description generic_options("Options");
@@ -68,9 +66,9 @@ bool GenerateDataStoreOptions(const int argc, const char *argv[], ServerPaths &p
         "ramindex", boost::program_options::value<boost::filesystem::path>(&paths["ramindex"]),
         ".ramIndex file")(
         "fileindex", boost::program_options::value<boost::filesystem::path>(&paths["fileindex"]),
-        ".fileIndex file")(
-        "core", boost::program_options::value<boost::filesystem::path>(&paths["core"]),
-        ".core file")(
+        ".fileIndex file")("core",
+                           boost::program_options::value<boost::filesystem::path>(&paths["core"]),
+                           ".core file")(
         "namesdata", boost::program_options::value<boost::filesystem::path>(&paths["namesdata"]),
         ".names file")("timestamp",
                        boost::program_options::value<boost::filesystem::path>(&paths["timestamp"]),
@@ -108,7 +106,7 @@ bool GenerateDataStoreOptions(const int argc, const char *argv[], ServerPaths &p
 
     if (option_variables.count("version"))
     {
-        SimpleLogger().Write() << g_GIT_DESCRIPTION;
+        SimpleLogger().Write() << OSRM_VERSION;
         return false;
     }
 
@@ -120,22 +118,22 @@ bool GenerateDataStoreOptions(const int argc, const char *argv[], ServerPaths &p
 
     boost::program_options::notify(option_variables);
 
-    const bool parameter_present = (paths.find("hsgrdata") != paths.end() &&
-                                    !paths.find("hsgrdata")->second.string().empty()) ||
-                                   (paths.find("nodesdata") != paths.end() &&
-                                    !paths.find("nodesdata")->second.string().empty()) ||
-                                   (paths.find("edgesdata") != paths.end() &&
-                                    !paths.find("edgesdata")->second.string().empty()) ||
-                                   (paths.find("geometry") != paths.end() &&
-                                    !paths.find("geometry")->second.string().empty()) ||
-                                   (paths.find("ramindex") != paths.end() &&
-                                    !paths.find("ramindex")->second.string().empty()) ||
-                                   (paths.find("fileindex") != paths.end() &&
-                                    !paths.find("fileindex")->second.string().empty()) ||
-                                   (paths.find("core") != paths.end() &&
-                                    !paths.find("core")->second.string().empty()) ||
-                                   (paths.find("timestamp") != paths.end() &&
-                                    !paths.find("timestamp")->second.string().empty());
+    const bool parameter_present =
+        (paths.find("hsgrdata") != paths.end() &&
+         !paths.find("hsgrdata")->second.string().empty()) ||
+        (paths.find("nodesdata") != paths.end() &&
+         !paths.find("nodesdata")->second.string().empty()) ||
+        (paths.find("edgesdata") != paths.end() &&
+         !paths.find("edgesdata")->second.string().empty()) ||
+        (paths.find("geometry") != paths.end() &&
+         !paths.find("geometry")->second.string().empty()) ||
+        (paths.find("ramindex") != paths.end() &&
+         !paths.find("ramindex")->second.string().empty()) ||
+        (paths.find("fileindex") != paths.end() &&
+         !paths.find("fileindex")->second.string().empty()) ||
+        (paths.find("core") != paths.end() && !paths.find("core")->second.string().empty()) ||
+        (paths.find("timestamp") != paths.end() &&
+         !paths.find("timestamp")->second.string().empty());
 
     if (parameter_present)
     {
@@ -223,58 +221,59 @@ bool GenerateDataStoreOptions(const int argc, const char *argv[], ServerPaths &p
     }
 
     path_iterator = paths.find("hsgrdata");
-    if (path_iterator == paths.end() || path_iterator->second.string().empty())
+    if (path_iterator == paths.end() || path_iterator->second.string().empty() ||
+        !boost::filesystem::is_regular_file(path_iterator->second))
     {
-        throw osrm::exception(".hsgr file must be specified");
+        throw osrm::exception("valid .hsgr file must be specified");
     }
-    AssertPathExists(path_iterator->second);
 
     path_iterator = paths.find("nodesdata");
-    if (path_iterator == paths.end() || path_iterator->second.string().empty())
+    if (path_iterator == paths.end() || path_iterator->second.string().empty() ||
+        !boost::filesystem::is_regular_file(path_iterator->second))
     {
-        throw osrm::exception(".nodes file must be specified");
+        throw osrm::exception("valid .nodes file must be specified");
     }
-    AssertPathExists(path_iterator->second);
 
     path_iterator = paths.find("edgesdata");
-    if (path_iterator == paths.end() || path_iterator->second.string().empty())
+    if (path_iterator == paths.end() || path_iterator->second.string().empty() ||
+        !boost::filesystem::is_regular_file(path_iterator->second))
     {
-        throw osrm::exception(".edges file must be specified");
+        throw osrm::exception("valid .edges file must be specified");
     }
-    AssertPathExists(path_iterator->second);
 
     path_iterator = paths.find("geometry");
-    if (path_iterator == paths.end() || path_iterator->second.string().empty())
+    if (path_iterator == paths.end() || path_iterator->second.string().empty() ||
+        !boost::filesystem::is_regular_file(path_iterator->second))
     {
-        throw osrm::exception(".geometry file must be specified");
+        throw osrm::exception("valid .geometry file must be specified");
     }
-    AssertPathExists(path_iterator->second);
 
     path_iterator = paths.find("ramindex");
-    if (path_iterator == paths.end() || path_iterator->second.string().empty())
+    if (path_iterator == paths.end() || path_iterator->second.string().empty() ||
+        !boost::filesystem::is_regular_file(path_iterator->second))
     {
-        throw osrm::exception(".ramindex file must be specified");
+        throw osrm::exception("valid .ramindex file must be specified");
     }
-    AssertPathExists(path_iterator->second);
 
     path_iterator = paths.find("fileindex");
-    if (path_iterator == paths.end() || path_iterator->second.string().empty())
+    if (path_iterator == paths.end() || path_iterator->second.string().empty() ||
+        !boost::filesystem::is_regular_file(path_iterator->second))
     {
-        throw osrm::exception(".fileindex file must be specified");
+        throw osrm::exception("valid .fileindex file must be specified");
     }
-    AssertPathExists(path_iterator->second);
 
     path_iterator = paths.find("namesdata");
-    if (path_iterator == paths.end() || path_iterator->second.string().empty())
+    if (path_iterator == paths.end() || path_iterator->second.string().empty() ||
+        !boost::filesystem::is_regular_file(path_iterator->second))
     {
-        throw osrm::exception(".names file must be specified");
+        throw osrm::exception("valid .names file must be specified");
     }
-    AssertPathExists(path_iterator->second);
 
     path_iterator = paths.find("timestamp");
-    if (path_iterator == paths.end() || path_iterator->second.string().empty())
+    if (path_iterator == paths.end() || path_iterator->second.string().empty() ||
+        !boost::filesystem::is_regular_file(path_iterator->second))
     {
-        throw osrm::exception(".timestamp file must be specified");
+        throw osrm::exception("valid .timestamp file must be specified");
     }
 
     return true;
diff --git a/util/debug_geometry.hpp b/util/debug_geometry.hpp
new file mode 100644
index 0000000..daa7e10
--- /dev/null
+++ b/util/debug_geometry.hpp
@@ -0,0 +1,198 @@
+/*
+
+Copyright (c) 2015, Project OSRM contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+*/
+
+#ifndef DEBUG_GEOMETRY_H
+#define DEBUG_GEOMETRY_H
+
+#include "../contractor/contractor_options.hpp"
+#include "../data_structures/query_node.hpp"
+
+#ifndef DEBUG_GEOMETRY
+
+inline void DEBUG_GEOMETRY_START(ContractorConfig & /* config */) {}
+inline void DEBUG_GEOMETRY_EDGE(int /* new_segment_weight */ , double /* segment_length */,
+        OSMNodeID /* previous_osm_node_id */, OSMNodeID /* this_osm_node_id */) {}
+inline void DEBUG_GEOMETRY_STOP() {}
+
+inline void DEBUG_TURNS_START(const std::string & /* debug_turns_filename */) {}
+inline void DEBUG_TURN( const NodeID /* node */, const std::vector<QueryNode>& /* m_node_info_list */,
+        const FixedPointCoordinate & /* first_coordinate */, const int /* turn_angle */,
+        const int /* turn_penalty */) {}
+inline void DEBUG_UTURN( const NodeID /* node */, const std::vector<QueryNode>& /* m_node_info_list */,
+        const int /* uturn_penalty */ ) {}
+inline void DEBUG_SIGNAL( const NodeID /* node */, const std::vector<QueryNode>& /* m_node_info_list */,
+        const int /* signal_penalty */ ) {}
+
+inline void DEBUG_TURNS_STOP() {}
+
+#else // DEBUG_GEOMETRY
+
+#include <boost/filesystem.hpp>
+#include <ctime>
+#include <string>
+#include <iomanip>
+#include <iostream>
+
+#include "../include/osrm/coordinate.hpp"
+#include "../algorithms/coordinate_calculation.hpp"
+
+boost::filesystem::ofstream debug_geometry_file;
+bool dg_output_debug_geometry = false;
+bool dg_first_debug_geometry = true;
+char dg_time_buffer[80];
+
+boost::filesystem::ofstream dg_debug_turns_file;
+bool dg_output_turn_debug = false;
+bool dg_first_turn_debug = true;
+
+inline void DEBUG_GEOMETRY_START(const ContractorConfig &config)
+{
+    time_t raw_time;
+    struct tm *timeinfo;
+    time(&raw_time);
+    timeinfo = localtime(&raw_time);
+    strftime(dg_time_buffer, 80, "%Y-%m-%d %H:%M %Z", timeinfo);
+
+    dg_output_debug_geometry = config.debug_geometry_path != "";
+
+    if (dg_output_debug_geometry)
+    {
+        debug_geometry_file.open(config.debug_geometry_path, std::ios::binary);
+        debug_geometry_file << "{\"type\":\"FeatureCollection\", \"features\":[" << std::endl;
+    }
+}
+
+inline void DEBUG_GEOMETRY_EDGE(int new_segment_weight, double segment_length, OSMNodeID previous_osm_node_id, OSMNodeID this_osm_node_id)
+{
+    if (dg_output_debug_geometry)
+    {
+        if (!dg_first_debug_geometry)
+            debug_geometry_file << "," << std::endl;
+        debug_geometry_file
+            << "{ \"type\":\"Feature\",\"properties\":{\"original\":false, "
+               "\"weight\":"
+            << new_segment_weight / 10.0 << ",\"speed\":"
+            << static_cast<int>(
+                   std::floor((segment_length / new_segment_weight) * 10. * 3.6))
+            << ",";
+        debug_geometry_file << "\"from_node\": " << previous_osm_node_id
+                            << ", \"to_node\": " << this_osm_node_id << ",";
+        debug_geometry_file << "\"timestamp\": \"" << dg_time_buffer << "\"},";
+        debug_geometry_file
+            << "\"geometry\":{\"type\":\"LineString\",\"coordinates\":[[!!"
+            << previous_osm_node_id << "!!],[!!" << this_osm_node_id << "!!]]}}"
+            << std::endl;
+        dg_first_debug_geometry = false;
+    }
+}
+
+inline void DEBUG_GEOMETRY_STOP()
+{
+    if (dg_output_debug_geometry)
+    {
+        debug_geometry_file << std::endl << "]}" << std::endl;
+        debug_geometry_file.close();
+    }
+}
+
+
+inline void DEBUG_TURNS_START(const std::string & debug_turns_path)
+{   
+    dg_output_turn_debug = debug_turns_path != "";
+    if (dg_output_turn_debug) 
+    {
+        dg_debug_turns_file.open(debug_turns_path);
+        dg_debug_turns_file << "{\"type\":\"FeatureCollection\", \"features\":[" << std::endl;
+    }
+} 
+
+inline void DEBUG_SIGNAL(
+        const NodeID node, 
+        const std::vector<QueryNode>& m_node_info_list,
+        const int traffic_signal_penalty)
+{
+    if (dg_output_turn_debug)
+    {
+        const QueryNode &nodeinfo = m_node_info_list[node];
+        if (!dg_first_turn_debug) dg_debug_turns_file << "," << std::endl;
+        dg_debug_turns_file << "{ \"type\":\"Feature\",\"properties\":{\"type\":\"trafficlights\",\"cost\":" << traffic_signal_penalty/10. << "},";
+        dg_debug_turns_file << " \"geometry\":{\"type\":\"Point\",\"coordinates\":[" << std::setprecision(12) << nodeinfo.lon/COORDINATE_PRECISION << "," << nodeinfo.lat/COORDINATE_PRECISION << "]}}";
+        dg_first_turn_debug = false;
+    }
+}
+
+inline void DEBUG_UTURN(
+        const NodeID node, 
+        const std::vector<QueryNode>& m_node_info_list,
+        const int traffic_signal_penalty)
+{
+    if (dg_output_turn_debug)
+    {
+        const QueryNode &nodeinfo = m_node_info_list[node];
+        if (!dg_first_turn_debug) dg_debug_turns_file << "," << std::endl;
+        dg_debug_turns_file << "{ \"type\":\"Feature\",\"properties\":{\"type\":\"trafficlights\",\"cost\":" << traffic_signal_penalty/10. << "},";
+        dg_debug_turns_file << " \"geometry\":{\"type\":\"Point\",\"coordinates\":[" << std::setprecision(12) << nodeinfo.lon/COORDINATE_PRECISION << "," << nodeinfo.lat/COORDINATE_PRECISION << "]}}";
+        dg_first_turn_debug = false;
+    }
+}
+
+
+inline void DEBUG_TURN(
+        const NodeID node, 
+        const std::vector<QueryNode>& m_node_info_list,
+        const FixedPointCoordinate & first_coordinate,
+        const int turn_angle,
+        const int turn_penalty)
+{
+    if (turn_penalty > 0 && dg_output_turn_debug) 
+    {
+        const QueryNode &v = m_node_info_list[node];
+
+        const float bearing_uv = coordinate_calculation::bearing(first_coordinate,v);
+        float uvw_normal = bearing_uv + turn_angle/2;
+        while (uvw_normal >= 360.) { uvw_normal -= 360.; }
+
+        if (!dg_first_turn_debug) dg_debug_turns_file << "," << std::endl;
+        dg_debug_turns_file << "{ \"type\":\"Feature\",\"properties\":{\"type\":\"turn\",\"cost\":" << turn_penalty/10. << ",\"turn_angle\":" << static_cast<int>(turn_angle) << ",\"normal\":" << static_cast<int>(uvw_normal) << "},";
+        dg_debug_turns_file << " \"geometry\":{\"type\":\"Point\",\"coordinates\":[" << std::setprecision(12) << v.lon/COORDINATE_PRECISION << "," << v.lat/COORDINATE_PRECISION << "]}}";
+        dg_first_turn_debug = false;
+    }
+}
+
+inline void DEBUG_TURNS_STOP()
+{
+    if (dg_output_turn_debug)
+    {
+        dg_debug_turns_file << std::endl << "]}" << std::endl;
+        dg_debug_turns_file.close();
+    }
+}
+
+#endif // DEBUG_GEOMETRY
+
+
+#endif // DEBUG_GEOMETRY_H
diff --git a/util/fingerprint.cpp b/util/fingerprint.cpp
index 8c7ab40..8a705b0 100644
--- a/util/fingerprint.cpp
+++ b/util/fingerprint.cpp
@@ -25,5 +25,5 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 */
 
-#include "fingerprint.hpp"
-#include "fingerprint_impl.hpp"
+#include "util/fingerprint.hpp"
+#include "util/fingerprint_impl.hpp"
diff --git a/util/fingerprint.hpp b/util/fingerprint.hpp
index 3894fc8..99576f9 100644
--- a/util/fingerprint.hpp
+++ b/util/fingerprint.hpp
@@ -52,7 +52,6 @@ class FingerPrint
 
     // initialize to {6ba7b810-9dad-11d1-80b4-00c04fd430c8}
     boost::uuids::uuid named_uuid;
-    bool has_64_bits;
 
 };
 
diff --git a/util/fingerprint_impl.hpp.in b/util/fingerprint_impl.hpp.in
index 98bb122..88e8436 100644
--- a/util/fingerprint_impl.hpp.in
+++ b/util/fingerprint_impl.hpp.in
@@ -25,16 +25,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 */
 
-#include "osrm_exception.hpp"
+#include "util/osrm_exception.hpp"
 
 #include <boost/uuid/name_generator.hpp>
+#include <boost/uuid/uuid_generators.hpp>
 
 #include <cstring>
 
 #include <algorithm>
 #include <string>
 
-#cmakedefine01 HAS64BITS
 #cmakedefine MD5PREPARE "${MD5PREPARE}"
 #cmakedefine MD5RTREE "${MD5RTREE}"
 #cmakedefine MD5GRAPH "${MD5GRAPH}"
@@ -47,6 +47,13 @@ FingerPrint FingerPrint::GetValid()
     fingerprint.magic_number = 1297240911;
     fingerprint.md5_prepare[32] = fingerprint.md5_tree[32] = fingerprint.md5_graph[32] = fingerprint.md5_objects[32] = '\0';
 
+    // 6ba7b810-9dad-11d1-80b4-00c04fd430c8 is a Well Known UUID representing the DNS
+    // namespace.  Its use here indicates that we own this part of the UUID space
+    // in the DNS realm.  *Usually*, we would then generate a UUID based on "something.project-osrm.org",
+    // but this whole class is a hack.  Anyway, named_uuid needs to be initialized to something
+    // before it can be used, and this is as good as anything else for these purposes.
+    fingerprint.named_uuid = boost::uuids::string_generator()( "{6ba7b810-9dad-11d1-80b4-00c04fd430c8}");
+
     boost::uuids::name_generator gen(fingerprint.named_uuid);
     std::string temp_string;
 
@@ -60,7 +67,6 @@ FingerPrint FingerPrint::GetValid()
     temp_string += fingerprint.md5_objects;
 
     fingerprint.named_uuid = gen(temp_string);
-    fingerprint.has_64_bits = HAS64BITS;
 
     return fingerprint;
 }
diff --git a/util/graph_loader.hpp b/util/graph_loader.hpp
index 71bc9e9..6ddd0eb 100644
--- a/util/graph_loader.hpp
+++ b/util/graph_loader.hpp
@@ -45,11 +45,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <cmath>
 
-#include <algorithm>
 #include <fstream>
-#include <iostream>
-#include <iomanip>
-#include <unordered_map>
+#include <ios>
 #include <vector>
 
 /**
@@ -57,8 +54,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  * The since the restrictions reference nodes using their external node id,
  * we need to renumber it to the new internal id.
 */
-unsigned loadRestrictionsFromFile(std::istream& input_stream,
-                                  std::vector<TurnRestriction>& restriction_list)
+unsigned loadRestrictionsFromFile(std::istream &input_stream,
+                                  std::vector<TurnRestriction> &restriction_list)
 {
     const FingerPrint fingerprint_valid = FingerPrint::GetValid();
     FingerPrint fingerprint_loaded;
@@ -74,14 +71,13 @@ unsigned loadRestrictionsFromFile(std::istream& input_stream,
     restriction_list.resize(number_of_usable_restrictions);
     if (number_of_usable_restrictions > 0)
     {
-        input_stream.read((char *) restriction_list.data(),
-                                number_of_usable_restrictions * sizeof(TurnRestriction));
+        input_stream.read((char *)restriction_list.data(),
+                          number_of_usable_restrictions * sizeof(TurnRestriction));
     }
 
     return number_of_usable_restrictions;
 }
 
-
 /**
  * Reads the beginning of an .osrm file and produces:
  *  - list of barrier nodes
@@ -132,36 +128,37 @@ NodeID loadNodesFromFile(std::istream &input_stream,
 /**
  * Reads a .osrm file and produces the edges.
  */
-NodeID loadEdgesFromFile(std::istream &input_stream,
-                         std::vector<NodeBasedEdge> &edge_list)
+NodeID loadEdgesFromFile(std::istream &input_stream, std::vector<NodeBasedEdge> &edge_list)
 {
     EdgeID m;
     input_stream.read(reinterpret_cast<char *>(&m), sizeof(unsigned));
     edge_list.resize(m);
     SimpleLogger().Write() << " and " << m << " edges ";
 
-    input_stream.read((char *) edge_list.data(), m * sizeof(NodeBasedEdge));
+    input_stream.read((char *)edge_list.data(), m * sizeof(NodeBasedEdge));
 
     BOOST_ASSERT(edge_list.size() > 0);
 
 #ifndef NDEBUG
     SimpleLogger().Write() << "Validating loaded edges...";
-    std::sort(edge_list.begin(), edge_list.end(),
-            [](const NodeBasedEdge& lhs, const NodeBasedEdge& rhs)
-            {
-                return (lhs.source < rhs.source) || (lhs.source == rhs.source && lhs.target < rhs.target);
-            });
+    tbb::parallel_sort(edge_list.begin(), edge_list.end(),
+                       [](const NodeBasedEdge &lhs, const NodeBasedEdge &rhs)
+                       {
+                           return (lhs.source < rhs.source) ||
+                                  (lhs.source == rhs.source && lhs.target < rhs.target);
+                       });
     for (auto i = 1u; i < edge_list.size(); ++i)
     {
-        const auto& edge = edge_list[i];
-        const auto& prev_edge = edge_list[i-1];
+        const auto &edge = edge_list[i];
+        const auto &prev_edge = edge_list[i - 1];
 
         BOOST_ASSERT_MSG(edge.weight > 0, "loaded null weight");
         BOOST_ASSERT_MSG(edge.forward, "edge must be oriented in forward direction");
         BOOST_ASSERT_MSG(edge.travel_mode != TRAVEL_MODE_INACCESSIBLE, "loaded non-accessible");
 
         BOOST_ASSERT_MSG(edge.source != edge.target, "loaded edges contain a loop");
-        BOOST_ASSERT_MSG(edge.source != prev_edge.source || edge.target != prev_edge.target, "loaded edges contain a multi edge");
+        BOOST_ASSERT_MSG(edge.source != prev_edge.source || edge.target != prev_edge.target,
+                         "loaded edges contain a multi edge");
     }
 #endif
 
diff --git a/util/integer_range.hpp b/util/integer_range.hpp
index 46b69ba..02de7f8 100644
--- a/util/integer_range.hpp
+++ b/util/integer_range.hpp
@@ -28,20 +28,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef INTEGER_RANGE_HPP
 #define INTEGER_RANGE_HPP
 
+#include <boost/assert.hpp>
+
 #include <type_traits>
 
+#include <cstdint>
+
 namespace osrm
 {
 
 template <typename Integer> class range
 {
   private:
-    Integer last;
+    const Integer last;
     Integer iter;
 
   public:
     range(Integer start, Integer end) noexcept : last(end), iter(start)
     {
+        BOOST_ASSERT_MSG(start <= end, "backwards counting ranges not suppoted");
         static_assert(std::is_integral<Integer>::value, "range type must be integral");
     }
 
@@ -50,7 +55,10 @@ template <typename Integer> class range
     const range &end() const noexcept { return *this; }
     Integer front() const noexcept { return iter; }
     Integer back() const noexcept { return last - 1; }
-    Integer size() const noexcept { return last - iter; }
+    std::size_t size() const noexcept
+    {
+        return static_cast<std::size_t>(last - iter);
+    }
 
     // Iterator functions
     bool operator!=(const range &) const noexcept { return iter < last; }
diff --git a/util/iterator_range.hpp b/util/iterator_range.hpp
deleted file mode 100644
index f4fa4f8..0000000
--- a/util/iterator_range.hpp
+++ /dev/null
@@ -1,71 +0,0 @@
-/*
-
-Copyright (c) 2015, Project OSRM contributors
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without modification,
-are permitted provided that the following conditions are met:
-
-Redistributions of source code must retain the above copyright notice, this list
-of conditions and the following disclaimer.
-Redistributions in binary form must reproduce the above copyright notice, this
-list of conditions and the following disclaimer in the documentation and/or
-other materials provided with the distribution.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
-ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
-ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-*/
-
-#ifndef ITERATOR_RANGE_HPP
-#define ITERATOR_RANGE_HPP
-
-namespace osrm
-{
-template <typename Iterator> class iter_range
-{
-  public:
-    iter_range(Iterator begin, Iterator end) noexcept : begin_(begin), end_(end) {}
-
-    Iterator begin() const noexcept { return begin_; }
-    Iterator end() const noexcept { return end_; }
-
-  private:
-    Iterator begin_;
-    Iterator end_;
-};
-
-// Convenience functions for template parameter inference,
-// akin to std::make_pair.
-
-template <typename Iterator>
-iter_range<Iterator> integer_range(Iterator begin, Iterator end) noexcept
-{
-    return iter_range<Iterator>(begin, end);
-}
-
-template <typename Reversable>
-iter_range<typename Reversable::reverse_iterator> reverse(Reversable *reversable) noexcept
-{
-    return iter_range<typename Reversable::reverse_iterator>(reversable->rbegin(),
-                                                             reversable->rend());
-}
-
-template <typename ConstReversable>
-iter_range<typename ConstReversable::const_reverse_iterator>
-const_reverse(const ConstReversable *const_reversable) noexcept
-{
-    return iter_range<typename ConstReversable::const_reverse_iterator>(const_reversable->crbegin(),
-                                                                        const_reversable->crend());
-}
-}
-
-#endif // ITERATOR_RANGE_HPP
diff --git a/util/json_logger.hpp b/util/json_logger.hpp
index b7de7cb..8726f78 100644
--- a/util/json_logger.hpp
+++ b/util/json_logger.hpp
@@ -30,7 +30,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <osrm/json_container.hpp>
 
-#include <boost/thread.hpp>
+#include <boost/thread/tss.hpp>
+
+#include <string>
+#include <unordered_map>
 
 namespace osrm
 {
diff --git a/util/json_renderer.hpp b/util/json_renderer.hpp
index b6b96f6..09d7bb2 100644
--- a/util/json_renderer.hpp
+++ b/util/json_renderer.hpp
@@ -76,7 +76,7 @@ struct Renderer : mapbox::util::static_visitor<>
     void operator()(const Array &array) const
     {
         out << "[";
-        for (auto it = array.values.cend(), end = array.values.cend(); it != end;)
+        for (auto it = array.values.cbegin(), end = array.values.cend(); it != end;)
         {
             mapbox::util::apply_visitor(Renderer(out), *it);
             if (++it != end)
@@ -111,7 +111,7 @@ struct ArrayRenderer : mapbox::util::static_visitor<>
 
     void operator()(const Number &number) const
     {
-        const std::string number_string = cast::double_fixed_to_string(number.value);
+        const std::string number_string = cast::to_string_with_precision(number.value);
         out.insert(out.end(), number_string.begin(), number_string.end());
     }
 
diff --git a/util/matching_debug_info.hpp b/util/matching_debug_info.hpp
index de48a84..28bc6ee 100644
--- a/util/matching_debug_info.hpp
+++ b/util/matching_debug_info.hpp
@@ -57,13 +57,13 @@ struct MatchingDebugInfo
         for (auto &elem : candidates_list)
         {
             osrm::json::Array timestamps;
-            for (unsigned s = 0; s < elem.size(); s++)
+            for (auto &elem_s : elem)
             {
                 osrm::json::Object state;
                 state.values["transitions"] = osrm::json::Array();
                 state.values["coordinate"] =
-                    osrm::json::make_array(elem[s].first.location.lat / COORDINATE_PRECISION,
-                                           elem[s].first.location.lon / COORDINATE_PRECISION);
+                    osrm::json::make_array(elem_s.phantom_node.location.lat / COORDINATE_PRECISION,
+                                           elem_s.phantom_node.location.lon / COORDINATE_PRECISION);
                 state.values["viterbi"] =
                     osrm::json::clamp_float(osrm::matching::IMPOSSIBLE_LOG_PROB);
                 state.values["pruned"] = 0u;
@@ -82,7 +82,7 @@ struct MatchingDebugInfo
                              const double emission_pr,
                              const double transition_pr,
                              const double network_distance,
-                             const double great_circle_distance)
+                             const double haversine_distance)
     {
         // json logger not enabled
         if (!logger)
@@ -94,7 +94,7 @@ struct MatchingDebugInfo
         transistion.values["to"] = osrm::json::make_array(current_t, current_state);
         transistion.values["properties"] = osrm::json::make_array(
             osrm::json::clamp_float(prev_viterbi), osrm::json::clamp_float(emission_pr),
-            osrm::json::clamp_float(transition_pr), network_distance, great_circle_distance);
+            osrm::json::clamp_float(transition_pr), network_distance, haversine_distance);
 
         osrm::json::get(*object, "states", prev_t, prev_state, "transitions")
             .get<mapbox::util::recursive_wrapper<osrm::json::Array>>()
diff --git a/util/routed_options.hpp b/util/routed_options.hpp
index 5701514..bfe8e5c 100644
--- a/util/routed_options.hpp
+++ b/util/routed_options.hpp
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef ROUTED_OPTIONS_HPP
 #define ROUTED_OPTIONS_HPP
 
-#include "git_sha.hpp"
+#include "util/version.hpp"
 #include "ini_file.hpp"
 #include "osrm_exception.hpp"
 #include "simple_logger.hpp"
@@ -36,15 +36,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <boost/any.hpp>
 #include <boost/program_options.hpp>
 
-#include <osrm/server_paths.hpp>
-
+#include <unordered_map>
 #include <fstream>
 #include <string>
 const static unsigned INIT_OK_START_ENGINE = 0;
 const static unsigned INIT_OK_DO_NOT_START_ENGINE = 1;
 const static unsigned INIT_FAILED = -1;
 
-inline void populate_base_path(ServerPaths &server_paths)
+inline void
+populate_base_path(std::unordered_map<std::string, boost::filesystem::path> &server_paths)
 {
     // populate the server_path object
     auto path_iterator = server_paths.find("base");
@@ -80,16 +80,7 @@ inline void populate_base_path(ServerPaths &server_paths)
     if (path_iterator == server_paths.end() ||
         !boost::filesystem::is_regular_file(path_iterator->second))
     {
-        if (path_iterator == server_paths.end())
-        {
-            SimpleLogger().Write() << "hsgrdata unset";
-        }
-        if (!boost::filesystem::is_regular_file(path_iterator->second))
-        {
-            SimpleLogger().Write() << "not a regular file";
-        }
-
-        throw osrm::exception(".hsgr not found: " + path_iterator->second.string());
+        throw osrm::exception(".hsgr not found");
     }
 
     path_iterator = server_paths.find("nodesdata");
@@ -145,67 +136,69 @@ inline void populate_base_path(ServerPaths &server_paths)
 }
 
 // generate boost::program_options object for the routing part
-inline unsigned GenerateServerProgramOptions(const int argc,
-                                             const char *argv[],
-                                             ServerPaths &paths,
-                                             std::string &ip_address,
-                                             int &ip_port,
-                                             int &requested_num_threads,
-                                             bool &use_shared_memory,
-                                             bool &trial,
-                                             int &max_locations_distance_table,
-                                             int &max_locations_map_matching)
+inline unsigned
+GenerateServerProgramOptions(const int argc,
+                             const char *argv[],
+                             std::unordered_map<std::string, boost::filesystem::path> &paths,
+                             std::string &ip_address,
+                             int &ip_port,
+                             int &requested_num_threads,
+                             bool &use_shared_memory,
+                             bool &trial,
+                             int &max_locations_trip,
+                             int &max_locations_viaroute,
+                             int &max_locations_distance_table,
+                             int &max_locations_map_matching)
 {
+    using boost::program_options::value;
+    using boost::filesystem::path;
+
     // declare a group of options that will be allowed only on command line
     boost::program_options::options_description generic_options("Options");
-    generic_options.add_options()("version,v", "Show version")("help,h", "Show this help message")(
-        "config,c", boost::program_options::value<boost::filesystem::path>(&paths["config"])
-                        ->default_value("server.ini"),
-        "Path to a configuration file")(
-        "trial", boost::program_options::value<bool>(&trial)->implicit_value(true),
-        "Quit after initialization");
+    generic_options.add_options()                                         //
+        ("version,v", "Show version")("help,h", "Show this help message") //
+        ("config,c", value<boost::filesystem::path>(&paths["config"])->default_value("server.ini"),
+         "Path to a configuration file") //
+        ("trial", value<bool>(&trial)->implicit_value(true), "Quit after initialization");
 
     // declare a group of options that will be allowed both on command line
     // as well as in a config file
     boost::program_options::options_description config_options("Configuration");
-    config_options.add_options()(
-        "hsgrdata", boost::program_options::value<boost::filesystem::path>(&paths["hsgrdata"]),
-        ".hsgr file")("nodesdata",
-                      boost::program_options::value<boost::filesystem::path>(&paths["nodesdata"]),
-                      ".nodes file")(
-        "edgesdata", boost::program_options::value<boost::filesystem::path>(&paths["edgesdata"]),
-        ".edges file")("geometry",
-                       boost::program_options::value<boost::filesystem::path>(&paths["geometries"]),
-                       ".geometry file")(
-        "ramindex", boost::program_options::value<boost::filesystem::path>(&paths["ramindex"]),
-        ".ramIndex file")(
-        "fileindex", boost::program_options::value<boost::filesystem::path>(&paths["fileindex"]),
-        "File index file")(
-        "namesdata", boost::program_options::value<boost::filesystem::path>(&paths["namesdata"]),
-        ".names file")("timestamp",
-                       boost::program_options::value<boost::filesystem::path>(&paths["timestamp"]),
-                       ".timestamp file")(
-        "ip,i", boost::program_options::value<std::string>(&ip_address)->default_value("0.0.0.0"),
-        "IP address")("port,p", boost::program_options::value<int>(&ip_port)->default_value(5000),
-                      "TCP/IP port")(
-        "threads,t", boost::program_options::value<int>(&requested_num_threads)->default_value(8),
-        "Number of threads to use")(
-        "shared-memory,s",
-        boost::program_options::value<bool>(&use_shared_memory)->implicit_value(true),
-        "Load data from shared memory")(
-        "max-table-size,m",
-        boost::program_options::value<int>(&max_locations_distance_table)->default_value(100),
-        "Max. locations supported in distance table query")(
-        "max-matching-size,m",
-        boost::program_options::value<int>(&max_locations_map_matching)->default_value(2),
-        "Max. locations supported in map matching query");
+    config_options.add_options()                                                             //
+        ("hsgrdata", value<boost::filesystem::path>(&paths["hsgrdata"]), ".hsgr file")       //
+        ("nodesdata", value<boost::filesystem::path>(&paths["nodesdata"]), ".nodes file")    //
+        ("edgesdata", value<boost::filesystem::path>(&paths["edgesdata"]), ".edges file")    //
+        ("geometry", value<boost::filesystem::path>(&paths["geometries"]), ".geometry file") //
+        ("ramindex", value<boost::filesystem::path>(&paths["ramindex"]), ".ramIndex file")   //
+        ("fileindex", value<boost::filesystem::path>(&paths["fileindex"]),
+         "File index file") //
+        ("namesdata", value<boost::filesystem::path>(&paths["namesdata"]),
+         ".names file") //
+        ("timestamp", value<boost::filesystem::path>(&paths["timestamp"]),
+         ".timestamp file") //
+        ("ip,i", value<std::string>(&ip_address)->default_value("0.0.0.0"),
+         "IP address") //
+        ("port,p", value<int>(&ip_port)->default_value(5000),
+         "TCP/IP port") //
+        ("threads,t", value<int>(&requested_num_threads)->default_value(8),
+         "Number of threads to use") //
+        ("shared-memory,s",
+         value<bool>(&use_shared_memory)->implicit_value(true)->default_value(false),
+         "Load data from shared memory") //
+        ("max-viaroute-size", value<int>(&max_locations_viaroute)->default_value(500),
+         "Max. locations supported in viaroute query") //
+        ("max-trip-size", value<int>(&max_locations_trip)->default_value(100),
+         "Max. locations supported in trip query") //
+        ("max-table-size", value<int>(&max_locations_distance_table)->default_value(100),
+         "Max. locations supported in distance table query") //
+        ("max-matching-size", value<int>(&max_locations_map_matching)->default_value(100),
+         "Max. locations supported in map matching query");
 
     // hidden options, will be allowed both on command line and in config
     // file, but will not be shown to the user
     boost::program_options::options_description hidden_options("Hidden options");
-    hidden_options.add_options()(
-        "base,b", boost::program_options::value<boost::filesystem::path>(&paths["base"]),
-        "base path to .osrm file");
+    hidden_options.add_options()("base,b", value<boost::filesystem::path>(&paths["base"]),
+                                 "base path to .osrm file");
 
     // positional option
     boost::program_options::positional_options_description positional_options;
@@ -232,7 +225,7 @@ inline unsigned GenerateServerProgramOptions(const int argc,
 
     if (option_variables.count("version"))
     {
-        SimpleLogger().Write() << g_GIT_DESCRIPTION;
+        SimpleLogger().Write() << OSRM_VERSION;
         return INIT_OK_DO_NOT_START_ENGINE;
     }
 
@@ -262,22 +255,26 @@ inline unsigned GenerateServerProgramOptions(const int argc,
     {
         throw osrm::exception("Number of threads must be a positive number");
     }
+    if (2 > max_locations_distance_table)
+    {
+        throw osrm::exception("Max location for distance table must be at least two");
+    }
+    if (2 > max_locations_map_matching)
+    {
+        throw osrm::exception("Max location for map matching must be at least two");
+    }
 
     if (!use_shared_memory && option_variables.count("base"))
     {
         return INIT_OK_START_ENGINE;
     }
-    if (use_shared_memory && !option_variables.count("base"))
+    else if (use_shared_memory && !option_variables.count("base"))
     {
         return INIT_OK_START_ENGINE;
     }
-    if (1 > max_locations_distance_table)
-    {
-        throw osrm::exception("Max location for distance table must be a positive number");
-    }
-    if (2 > max_locations_map_matching)
+    else if (use_shared_memory && option_variables.count("base"))
     {
-        throw osrm::exception("Max location for map matching must be at least two");
+        SimpleLogger().Write(logWARNING) << "Shared memory settings conflict with path settings.";
     }
 
     SimpleLogger().Write() << visible_options;
diff --git a/util/string_util.hpp b/util/string_util.hpp
index 6e80a4e..8075e65 100644
--- a/util/string_util.hpp
+++ b/util/string_util.hpp
@@ -28,8 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef STRING_UTIL_HPP
 #define STRING_UTIL_HPP
 
-#include <boost/algorithm/string.hpp>
-
 #include <cctype>
 
 #include <random>
@@ -83,11 +81,6 @@ template <int length, int precision> static inline char *printInt(char *buffer,
     return buffer;
 }
 
-inline void replaceAll(std::string &s, const std::string &sub, const std::string &other)
-{
-    boost::replace_all(s, sub, other);
-}
-
 inline std::string escape_JSON(const std::string &input)
 {
     // escape and skip reallocations if possible
diff --git a/util/git_sha.cpp.in b/util/version.hpp.in
similarity index 81%
rename from util/git_sha.cpp.in
rename to util/version.hpp.in
index e3686da..2eafd48 100644
--- a/util/git_sha.cpp.in
+++ b/util/version.hpp.in
@@ -25,7 +25,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 */
 
-#include "git_sha.hpp"
+#ifndef VERSION_HPP
+#define VERSION_HPP
 
-#define GIT_DESCRIPTION "${GIT_DESCRIPTION}"
-char g_GIT_DESCRIPTION[] = GIT_DESCRIPTION;
+#define OSRM_VERSION_MAJOR "@OSRM_VERSION_MAJOR@"
+#define OSRM_VERSION_MINOR "@OSRM_VERSION_MINOR@"
+#define OSRM_VERSION_PATCH "@OSRM_VERSION_PATCH@"
+
+#define OSRM_VERSION "v" OSRM_VERSION_MAJOR "." OSRM_VERSION_MINOR "." OSRM_VERSION_PATCH
+
+#endif // VERSION_HPP
diff --git a/util/xml_renderer.hpp b/util/xml_renderer.hpp
index f598100..44f7c2d 100644
--- a/util/xml_renderer.hpp
+++ b/util/xml_renderer.hpp
@@ -50,7 +50,7 @@ struct XMLToArrayRenderer : mapbox::util::static_visitor<>
 
     void operator()(const Number &number) const
     {
-        const std::string number_string = cast::double_fixed_to_string(number.value);
+        const std::string number_string = cast::to_string_with_precision(number.value);
         out.insert(out.end(), number_string.begin(), number_string.end());
     }
 

-- 
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/pkg-grass/osrm.git



More information about the Pkg-grass-devel mailing list