src/main/java/com/bmwcarit/barefoot/roadmap/Route.java
public class Route extends Path<Road> {
    private final static SpatialOperator spatial = new Geography();

    private Double length = null;
    private Double time = null;

    /**
     * Creates a {@link Route} object. (This is a base case that consists of only one
     * {@link RoadPoint}.)
     *
     * @param point {@link RoadPoint} that is start and end point.
     */
    public Route(RoadPoint point) {
        super(point);
    }

    /**
     * Creates a {@link Route} object.
     *
     * @param source Source/start {@link RoadPoint} of the route.
     * @param target Target/end {@link RoadPoint} of the route.
     * @param roads Sequence of {@link Road}s that make the route.
     */
    public Route(RoadPoint source, RoadPoint target, List<Road> roads) {
        super(source, target, roads);
    }

    /**
     * Gets size of the route, i.e. the number of {@link Road}s that make the route.
     *
     * @return Number of the {@link Road}s that make the route.
     */
    public int size() {
        return path().size();
    }

    /**
     * Gets {@link Road} by index of the sequence of {@link Road} objects.
     *
     * @param index Index of the road to be returned from the sequence of roads.
     * @return Road at the given index.
     */
    public Road get(int index) {
        return path().get(index);
    }

    /**
     * Gets length of the {@link Route} in meters, uses cost function {@link Distance} to determine
     * the length.
     *
     * @return Length of the {@link Route} in meters.
     */
    public double length() {
        if (length != null) {
            return length;
        } else {
            length = this.cost(new Distance());
            return length;
        }
    }

    /**
     * Gets travel time for the {@link Route} in seconds, uses cost function {@link Time} to
     * determine travel time.
     *
     * @return Travel time of the {@link Route} in seconds.
     */
    public double time() {
        if (time != null) {
            return time;
        } else {
            time = cost(new Time());
            return time;
        }
    }

    @Override
    public RoadPoint source() {
        return (RoadPoint) super.source();
    }

    @Override
    public RoadPoint target() {
        return (RoadPoint) super.target();
    }

    @Override
    public boolean add(Path<Road> other) {
        time = null;
        length = null;
        return super.add(other);
    }

    /**
     * Gets geometry of the {@link Route} from start point to end point by concatenating the
     * geometries of the roads in the route's road sequence respectively.
     *
     * @return Geometry of the route.
     */
    public Polyline geometry() {
        Polyline geometry = new Polyline();

        geometry.startPath(source().geometry());

        if (source().edge().id() != target().edge().id()) {
            {
                double f = source().edge().length() * source().fraction(), s = 0;
                Point a = source().edge().geometry().getPoint(0);

                for (int i = 1; i < source().edge().geometry().getPointCount(); ++i) {
                    Point b = source().edge().geometry().getPoint(i);
                    s += spatial.distance(a, b);
                    a = b;

                    if (s <= f) {
                        continue;
                    }

                    geometry.lineTo(b);
                }
            }
            for (int i = 1; i < path().size() - 1; ++i) {
                Polyline segment = path().get(i).geometry();

                for (int j = 1; j < segment.getPointCount(); ++j) {
                    geometry.lineTo(segment.getPoint(j));
                }
            }
            {
                double f = target().edge().length() * target().fraction(), s = 0;
                Point a = target().edge().geometry().getPoint(0);

                for (int i = 1; i < target().edge().geometry().getPointCount() - 1; ++i) {
                    Point b = target().edge().geometry().getPoint(i);
                    s += spatial.distance(a, b);
                    a = b;

                    if (s >= f) {
                        break;
                    }

                    geometry.lineTo(b);
                }
            }
        } else {
            double sf = source().edge().length() * source().fraction();
            double tf = target().edge().length() * target().fraction();
            double s = 0;
            Point a = source().edge().geometry().getPoint(0);

            for (int i = 1; i < source().edge().geometry().getPointCount() - 1; ++i) {
                Point b = source().edge().geometry().getPoint(i);
                s += spatial.distance(a, b);
                a = b;

                if (s <= sf) {
                    continue;
                }
                if (s >= tf) {
                    break;
                }

                geometry.lineTo(b);
            }
        }

        geometry.lineTo(target().geometry());

        return geometry;
    }

    /**
     * Creates a {@link Route} object from its JSON representation.
     *
     * @param json JSON representation of the {@link Route}.
     * @param map {@link RoadMap} object as the reference of {@link RoadPoint}s and {@link Road}s.
     * @return {@link Route} object.
     * @throws JSONException thrown on JSON extraction or parsing error.
     */
    public static Route fromJSON(JSONObject json, RoadMap map) throws JSONException {
        LinkedList<Road> roads = new LinkedList<>();

        JSONObject jsontarget = json.getJSONObject("target");
        JSONObject jsonsource = json.getJSONObject("source");
        RoadPoint target = RoadPoint.fromJSON(jsontarget, map);
        RoadPoint source = RoadPoint.fromJSON(jsonsource, map);

        JSONArray jsonroads = json.getJSONArray("roads");

        for (int j = 0; j < jsonroads.length(); ++j) {
            JSONObject jsonroad = jsonroads.getJSONObject(j);
            roads.add(Road.fromJSON(jsonroad, map));
        }

        return new Route(source, target, roads);
    }

    /**
     * Gets a JSON representation of the {@link Route}.
     *
     * @return {@link JSONObject} object.
     * @throws JSONException thrown on JSON extraction or parsing error.
     */
    public JSONObject toJSON() throws JSONException {
        JSONObject json = new JSONObject();

        json.put("target", target().toJSON());
        json.put("source", source().toJSON());

        JSONArray jsonroads = new JSONArray();
        for (Road road : path()) {
            jsonroads.put(road.toJSON());
        }

        json.put("roads", jsonroads);

        return json;
    }
}
src/main/java/com/bmwcarit/barefoot/scheduler/Task.java
public abstract class Task implements Runnable {
    private final static Logger logger = LoggerFactory.getLogger(Task.class);
    protected final AtomicBoolean syncExecute = new AtomicBoolean();
    protected final Queue<Exception> exceptions = new ConcurrentLinkedQueue<>();
    final AtomicInteger syncChildren = new AtomicInteger(1);
    final AtomicBoolean cancelled = new AtomicBoolean(false);
    Group group = null;
    Task root = this;
    Task parent = null;
    Worker worker = null;

    void execute() {
        try {
            if (!root.cancelled.get() && !group.cancelled.get()) {
                run();
            } else {
                logger.debug("root or group cancelled, won't execute task {}", this);
            }
        } catch (Exception e) {
            syncExecute.set(false);
            if (parent != null) {
                parent.exceptions.add(e);
            } else if (group != null) {
                group.exceptions.add(e);
            }
        }

        if (syncExecute.get() == true) {
            if (syncChildren.decrementAndGet() == 0) {
                worker.enqueue(this);
                return;
            }
        } else {
            if (group != null) {
                if (group.syncTasks.decrementAndGet() == 0) {
                    group.syncLock.lock();
                    logger.trace("task {} signals group {}", this.toString(), group.toString());
                    group.syncCond.signalAll();
                    group.syncLock.unlock();
                }

                if (logger.isTraceEnabled()) {
                    logger.trace("task {} on worker {} decrements group {} task counter",
                            this.toString(), worker.workerid, group.toString());
                }
            }

            if (parent != null) {
                if (parent.syncChildren.decrementAndGet() == 0) {
                    logger.trace("task {} on worker {} spawns sync execute of parent {}",
                            this.toString(), worker.workerid, parent.toString());
                    worker.enqueue(parent);
                }

                if (logger.isTraceEnabled()) {
                    logger.trace("task {} on worker {} decrements parent {} children counter",
                            this.toString(), worker.workerid, parent.toString());
                }
            }
        }
    }

    protected void spawn(Task task) {
        if (root.cancelled.get()) {
            logger.debug("root {} cancelled, won't spawn task {}", root.toString(),
                    task.toString());
            return;
        }

        if (group.cancelled.get()) {
            logger.debug("group {} cancelled, won't spawn task {}", group.toString(),
                    task.toString());
            return;
        }

        if (logger.isTraceEnabled()) {
            logger.trace("task {} spawns child {}", this.toString(), task.toString());
        }

        task.root = this.root;
        task.parent = this;
        task.group = this.group;
        syncChildren.getAndIncrement();
        group.syncTasks.getAndIncrement();
        worker.enqueue(task);
    }

    protected void cancel() {
        root.cancelled.set(true);
    }

    protected boolean sync() {
        logger.trace("task {} waits for children", this.toString());
        worker.syncCount.getAndIncrement();
        while (syncChildren.get() > 1) {
            worker.execute();
        }
        worker.syncCount.getAndDecrement();
        worker.current = this;
        logger.trace("task {} synchronized", this.toString());

        return exceptions.isEmpty() && !root.cancelled.get();
    }
}
src/main/java/com/bmwcarit/barefoot/topology/Cost.java
public abstract class Cost<E extends AbstractEdge<E>> {
    /**
     * Abstract function to define a custom cost function for traversing an edge.
     *
     * @param edge {@link AbstractEdge} for which cost value shall be calculated.
     * @return Cost value for traversing the edge.
     */
    public abstract double cost(E edge);

    /**
     * Calculates cost value for traversing a certain fraction of the edge. It is assumed that costs
     * increase linearly while traversing an edge.
     *
     * @param edge {@link AbstractEdge} for which cost value shall be calculated.
     * @param fraction Fraction of the edge to be traversed.
     * @return Cost value for traversing a certain fraction of the edge.
     */
    public double cost(E edge, double fraction) {
        return cost(edge) * fraction;
    }
}
src/main/java/com/bmwcarit/barefoot/roadmap/RoadPoint.java
public class RoadPoint extends com.bmwcarit.barefoot.topology.Point<Road> {
    private static final SpatialOperator spatial = new Geography();
    private final Point geometry;
    private final double azimuth;

    /**
     * Creates a {@link RoadPoint}.
     *
     * @param road {@link Road} object of the point.
     * @param fraction Exact position on the {@link Road} defined as fraction <i>f</i>, with <i>0
     *        &le; f &le; 1</i>.
     */
    public RoadPoint(Road road, double fraction) {
        super(road, fraction);
        this.geometry = spatial.interpolate(road.geometry(), fraction);
        this.azimuth = spatial.azimuth(road.geometry(), fraction);
    }

    /**
     * Gets the geometry of the point on the {@link Road}.
     *
     * @return Geometry of the point on the road.
     */
    public Point geometry() {
        return geometry;
    }

    public double azimuth() {
        return azimuth;
    }

    /**
     * Gets the JSON representation of the {@link RoadPoint}.
     *
     * @return JSON representation of the {@link RoadPoint}.
     * @throws JSONException thrown on JSON extraction or parsing error.
     */
    public JSONObject toJSON() throws JSONException {
        JSONObject json = edge().toJSON();
        json.put("frac", fraction());
        return json;
    }

    /**
     * Creates a {@link RoadPoint} object from its JSON representation.
     *
     * @param json JSON representation of the {@link RoadPoint} object.
     * @param map {@link RoadMap} as reference of the {@link RoadPoint}.
     * @return {@link RoadPoint} object.
     * @throws JSONException thrown on JSON extraction or parsing error.
     */
    public static RoadPoint fromJSON(JSONObject json, RoadMap map) throws JSONException {
        Road road = Road.fromJSON(json, map);
        double fraction = json.getDouble("frac");
        return new RoadPoint(road, fraction);
    }
}
src/main/java/com/bmwcarit/barefoot/util/Tuple.java
public class Tuple<X, Y> implements Serializable {
    private static final long serialVersionUID = 1L;
    private X one = null;
    private Y two = null;

    /**
     * Creates a {@link Tuple} object.
     *
     * @param one First element.
     * @param two Second element.
     */
    public Tuple(X one, Y two) {
        this.one = one;
        this.two = two;
    }

    /**
     * Gets first element.
     *
     * @return First element, may be null if set to null previously.
     */
    public X one() {
        return one;
    }

    /**
     * Gets second element.
     *
     * @return Second element, may be null if set to null previously.
     */
    public Y two() {
        return two;
    }

    /**
     * Sets first element.
     *
     * @param one First element.
     */
    public void one(X one) {
        this.one = one;
    }

    /**
     * Sets second element.
     *
     * @param two Second element.
     */
    public void two(Y two) {
        this.two = two;
    }
}
src/main/java/com/bmwcarit/barefoot/scheduler/StaticScheduler.java
public class StaticScheduler {
    private static Scheduler scheduler = new Scheduler(Runtime.getRuntime().availableProcessors());

    static public class InlineScheduler {
        private final Group group;
        private final Task task;

        protected InlineScheduler(Group group) {
            this.group = group;
            this.task = null;
        }

        protected InlineScheduler(Task task) {
            this.group = null;
            this.task = task;
        }

        public void spawn(Task task) {
            if (this.task != null) {
                this.task.spawn(task);
            } else {
                this.group.spawn(task);
            }
        }

        public boolean sync() {
            if (this.task != null) {
                return task.sync();
            } else {
                return group.sync();
            }
        }
    }

    public static void reset(int numWorkers) {
        scheduler.shutdown();
        scheduler = new Scheduler(numWorkers);
    }

    public static void reset(int numWorkers, long spintime) {
        scheduler.shutdown();
        scheduler = new Scheduler(numWorkers, spintime);
    }

    public static InlineScheduler scheduler() {
        Task self = scheduler.self();
        if (self != null) {
            return new InlineScheduler(self);
        } else {
            return new InlineScheduler(scheduler.group());
        }
    }

    public static void stop() {
        scheduler.shutdown();
    }
}
src/main/java/com/bmwcarit/barefoot/topology/Router.java
public interface Router<E extends AbstractEdge<E>, P extends Point<E>> {

    /**
     * Gets path, i.e. a sequence of {@link AbstractEdge}s, from source {@link Point} to target
     * {@link Point} with minimum cost according to {@link Cost} function.
     *
     * @param source Source {@link Point} in the graph.
     * @param target Target {@link Point} in the graph.
     * @param cost Custom {@link Cost} function.
     * @return Path, i.e. a sequence of {@link AbstractEdge}s, from source {@link Point} to target
     *         {@link Point} with minimum cost according to {@link Cost} function. If there is no
     *         path from source to target, it returns null.
     */
    List<E> route(P source, P target, Cost<E> cost);

    /**
     * Gets path, i.e. a sequence of {@link AbstractEdge}s, from source {@link Point} to target
     * {@link Point} with minimum cost according to {@link Cost} function. Search depth of routing
     * can be bound by bounding {@link Cost} function and a maximum bounding cost value.
     *
     * @param source Source {@link Point} in the graph.
     * @param target Target {@link Point} in the graph.
     * @param cost Custom {@link Cost} function.
     * @param bound Bounding {@link Cost} function.
     * @param max Maximum bounding cost value to bound search depth.
     * @return Path, i.e. a sequence of {@link AbstractEdge}s, from source {@link Point} to target
     *         {@link Point} with minimum cost according to {@link Cost} function. If there is no
     *         path from source to target, it returns null.
     */
    List<E> route(P source, P target, Cost<E> cost, Cost<E> bound, Double max);

    /**
     * Gets path, i.e. a sequence of {@link AbstractEdge}s, from source {@link Point} to each target
     * {@link Point} with minimum cost according to {@link Cost} function.
     *
     * @param source Source {@link Point} in the graph.
     * @param targets Set of target {@link Point}s in the graph.
     * @param cost Custom {@link Cost} function.
     * @return Map of target {@link Point} to path, i.e. a sequence of {@link AbstractEdge}s, from
     *         source {@link Point} to target {@link Point} with minimum cost according to
     *         {@link Cost} function. If there is no path from source to target, it maps to null.
     */
    Map<P, List<E>> route(P source, Set<P> targets, Cost<E> cost);

    /**
     * Gets path, i.e. a sequence of {@link AbstractEdge}s, from source {@link Point} to each target
     * {@link Point} with minimum cost according to {@link Cost} function. Search depth of routing
     * can be bound by bounding {@link Cost} function and a maximum bounding cost value.
     *
     * @param source Source {@link Point} in the graph.
     * @param targets Set of target {@link Point}s in the graph.
     * @param cost Custom {@link Cost} function.
     * @param bound Bounding {@link Cost} function.
     * @param max Maximum bounding cost value to bound search depth.
     * @return Map of target {@link Point} to path, i.e. a sequence of {@link AbstractEdge}s, from
     *         source {@link Point} to target {@link Point} with minimum cost according to
     *         {@link Cost} function. If there is no path from source to target, it maps to null.
     */
    Map<P, List<E>> route(P source, Set<P> targets, Cost<E> cost, Cost<E> bound, Double max);

    /**
     * Gets path, i.e. a sequence of {@link AbstractEdge}s, to each target {@link Point} from
     * exactly that source {@link Point} that has minimum cost according to {@link Cost} function.
     *
     * @param sources Set of source {@link Point}s in the graph.
     * @param targets Set of target {@link Point}s in the graph.
     * @param cost Custom {@link Cost} function.
     * @return Map of target {@link Point} to a tuple of path, i.e. a sequence of
     *         {@link AbstractEdge}s, and source {@link Point} that have minimum cost according to
     *         {@link Cost} function for reaching the target. If there is no path to target, it maps
     *         to null.
     */
    Map<P, Tuple<P, List<E>>> route(Set<P> sources, Set<P> targets, Cost<E> cost);

    /**
     * Gets path, i.e. a sequence of {@link AbstractEdge}s, to each target {@link Point} from
     * exactly that source {@link Point} that has minimum cost according to {@link Cost} function.
     * Search depth of routing can be bound by bounding {@link Cost} function and a maximum bounding
     * cost value.
     *
     * @param sources Set of source {@link Point}s in the graph.
     * @param targets Set of target {@link Point}s in the graph.
     * @param cost Custom {@link Cost} function.
     * @param bound Bounding {@link Cost} function.
     * @param max Maximum bounding cost value to bound search depth.
     * @return Map of target {@link Point} to a tuple of path, i.e. a sequence of
     *         {@link AbstractEdge}s, and source {@link Point} that have minimum cost according to
     *         {@link Cost} function for reaching the target. If there is no path to target, it maps
     *         to null.
     */
    Map<P, Tuple<P, List<E>>> route(Set<P> sources, Set<P> targets, Cost<E> cost, Cost<E> bound,
            Double max);
}
src/main/java/com/bmwcarit/barefoot/scheduler/StaticScheduler.java
static public class InlineScheduler {
    private final Group group;
    private final Task task;

    protected InlineScheduler(Group group) {
        this.group = group;
        this.task = null;
    }

    protected InlineScheduler(Task task) {
        this.group = null;
        this.task = task;
    }

    public void spawn(Task task) {
        if (this.task != null) {
            this.task.spawn(task);
        } else {
            this.group.spawn(task);
        }
    }

    public boolean sync() {
        if (this.task != null) {
            return task.sync();
        } else {
            return group.sync();
        }
    }
}
src/main/java/com/bmwcarit/barefoot/roadmap/TimePriority.java
public class TimePriority extends Cost<Road> {
    private static final double heuristic_priority = 1;
    private static final Time time = new Time();

    /**
     * Gets time-priority cost for passing the road, i.e. traveling time multiplied with priority
     * cost factor.
     *
     * @return Time-priority cost for passing the road
     */
    @Override
    public double cost(Road road) {
        return time.cost(road) * Math.max(heuristic_priority, road.priority());
    }
}
src/main/java/com/bmwcarit/barefoot/roadmap/RoadMap.java
public class RoadMap extends Graph<Road> implements Serializable {
    private static final long serialVersionUID = 1L;
    private static final Logger logger = LoggerFactory.getLogger(RoadMap.class);
    private transient Index index = null;

    static Collection<Road> split(BaseRoad base) {
        ArrayList<Road> roads = new ArrayList<>();

        roads.add(new Road(base, Heading.forward));

        if (!base.oneway()) {
            roads.add(new Road(base, Heading.backward));
        }

        return roads;
    }

    private class Index implements SpatialIndex<RoadPoint>, Serializable {
        private static final long serialVersionUID = 1L;
        private final QuadTreeIndex index = new QuadTreeIndex();

        public void put(Road road) {
            int id = (int) road.base().id();

            if (index.contains(id)) {
                return;
            }

            index.add(id, road.base().wkb());
        }

        public void clear() {
            index.clear();
        }

        private Set<RoadPoint> split(Set<Tuple<Integer, Double>> points) {
            Set<RoadPoint> neighbors = new HashSet<>();

            /*
             * This uses the road
             */
            for (Tuple<Integer, Double> point : points) {
                neighbors.add(new RoadPoint(edges.get((long) point.one() * 2), point.two()));

                if (edges.containsKey((long) point.one() * 2 + 1)) {
                    neighbors.add(new RoadPoint(edges.get((long) point.one() * 2 + 1),
                            1.0 - point.two()));
                }
            }

            return neighbors;
        }

        @Override
        public Set<RoadPoint> nearest(Point c) {
            return split(index.nearest(c));
        }

        @Override
        public Set<RoadPoint> radius(Point c, double r) {
            return split(index.radius(c, r));
        }

        @Override
        public Set<RoadPoint> knearest(Point c, int k) {
            return split(index.knearest(c, k));
        }
    };

    /**
     * Loads and creates a {@link RoadMap} object from {@link BaseRoad} objects loaded with a
     * {@link RoadReader}.
     *
     * @param reader {@link RoadReader} to load {@link BaseRoad} objects.
     * @return {@link RoadMap} object.
     * @throws SourceException thrown if error occurs while loading roads.
     */
    public static RoadMap Load(RoadReader reader) throws SourceException {

        long memory = 0;

        System.gc();
        memory = Runtime.getRuntime().totalMemory() - Runtime.getRuntime().freeMemory();

        if (!reader.isOpen()) {
            reader.open();
        }

        logger.info("inserting roads ...");

        RoadMap roadmap = new RoadMap();

        int osmcounter = 0, counter = 0;
        BaseRoad road = null;
        while ((road = reader.next()) != null) {
            osmcounter += 1;

            for (Road uni : split(road)) {
                counter += 1;
                roadmap.add(uni);
            }

            if (osmcounter % 100000 == 0) {
                logger.info("inserted {} ({}) roads", osmcounter, counter);
            }
        }

        logger.info("inserted {} ({}) roads and finished", osmcounter, counter);

        reader.close();

        System.gc();
        memory = (Runtime.getRuntime().totalMemory() - Runtime.getRuntime().freeMemory()) - memory;
        logger.info("~{} megabytes used for road data (estimate)",
                Math.max(0, Math.round(memory / 1E6)));

        return roadmap;
    }

    /**
     * Constructs road network topology and spatial index.
     */
    @Override
    public RoadMap construct() {
        long memory = 0;

        System.gc();
        memory = Runtime.getRuntime().totalMemory() - Runtime.getRuntime().freeMemory();

        logger.info("index and topology constructing ...");

        super.construct();

        index = new Index();
        for (Road road : edges.values()) {
            index.put(road);
        }

        logger.info("index and topology constructed");

        System.gc();
        memory = (Runtime.getRuntime().totalMemory() - Runtime.getRuntime().freeMemory()) - memory;
        logger.info("~{} megabytes used for spatial index (estimate)",
                Math.max(0, Math.round(memory / 1E6)));

        return this;
    }

    /**
     * Destroys road network topology and spatial index. (Necessary if roads have been added and
     * road network topology and spatial index must be reconstructed.)
     */
    @Override
    public void deconstruct() {
        logger.info("destructing ...");

        super.deconstruct();

        index.clear();
        index = null;

        logger.info("destructed");
    }

    /**
     * Returns instance of {@link SpatialIndex} for spatial search of {@link Road} objects.
     *
     * @return Instance of {@link SpatialIndex} or <i>null</i>, if the map hasn't been constructed (
     *         {@link RoadMap#construct()}) or has been deconstructed (
     *         {@link RoadMap#deconstruct()}).
     */
    public SpatialIndex<RoadPoint> spatial() {
        if (index == null)
            throw new RuntimeException("index not constructed");
        else
            return index;
    }

    /**
     * Gets {@link RoadReader} of roads in this {@link RoadMap}.
     *
     * @return {@link RoadReader} object.
     */
    public RoadReader reader() {
        return new RoadReader() {
            Iterator<Road> iterator = null;
            HashSet<Short> exclusions = null;
            Polygon polygon = null;

            @Override
            public boolean isOpen() {
                return (iterator != null);
            }

            @Override
            public void open() throws SourceException {
                open(null, null);
            }

            @Override
            public void open(Polygon polygon, HashSet<Short> exclusions) throws SourceException {
                iterator = edges.values().iterator();
                this.exclusions = exclusions;
                this.polygon = polygon;
            }

            @Override
            public void close() throws SourceException {
                iterator = null;
            }

            @Override
            public BaseRoad next() throws SourceException {
                BaseRoad road = null;
                do {
                    if (!iterator.hasNext()) {
                        return null;
                    }

                    Road _road = iterator.next();

                    if (_road.id() % 2 == 1) {
                        continue;
                    }

                    road = _road.base();
                } while (road == null || exclusions != null && exclusions.contains(road.type())
                        || polygon != null
                                && !GeometryEngine.contains(polygon, road.geometry(),
                                        SpatialReference.create(4326))
                                && !GeometryEngine.overlaps(polygon, road.geometry(),
                                        SpatialReference.create(4326)));
                return road;
            }
        };
    }
}
src/main/java/com/bmwcarit/barefoot/markov/KState.java
public class KState<C extends StateCandidate<C, T, S>, T extends StateTransition, S extends Sample>
        extends StateMemory<C, T, S> {
    private final int k;
    private final long t;
    private final LinkedList<Tuple<Set<C>, S>> sequence;
    private final Map<C, Integer> counters;

    /**
     * Creates empty {@link KState} object with default parameters, i.e. capacity is unbounded.
     */
    public KState() {
        this.k = -1;
        this.t = -1;
        this.sequence = new LinkedList<>();
        this.counters = new HashMap<>();
    }

    /**
     * Creates a {@link KState} object from a JSON representation.
     *
     * @param json JSON representation of a {@link KState} object.
     * @param factory Factory for creation of state candidates and transitions.
     * @throws JSONException thrown on JSON extraction or parsing error.
     */
    public KState(JSONObject json, Factory<C, T, S> factory) throws JSONException {
        this.k = json.getInt("k");
        this.t = json.getLong("t");
        this.sequence = new LinkedList<>();
        this.counters = new HashMap<>();

        Map<String, C> candidates = new HashMap<>();
        JSONArray jsoncandidates = json.getJSONArray("candidates");
        for (int i = 0; i < jsoncandidates.length(); ++i) {
            JSONObject jsoncandidate = jsoncandidates.getJSONObject(i);
            C candidate = factory.candidate(jsoncandidate.getJSONObject("candidate"));
            int count = jsoncandidate.getInt("count");

            counters.put(candidate, count);
            candidates.put(candidate.id(), candidate);
        }

        JSONArray jsonsequence = json.getJSONArray("sequence");
        for (int i = 0; i < jsonsequence.length(); ++i) {
            JSONObject jsonseqelement = jsonsequence.getJSONObject(i);
            Set<C> vector = new HashSet<>();
            JSONArray jsonvector = jsonseqelement.getJSONArray("vector");
            for (int j = 0; j < jsonvector.length(); ++j) {
                JSONObject jsonelement = jsonvector.getJSONObject(j);

                String candid = jsonelement.getString("candid");
                String predid = jsonelement.getString("predid");

                C candidate = candidates.get(candid);
                C pred = candidates.get(predid);

                if (candidate == null || (!predid.isEmpty() && pred == null)) {
                    throw new JSONException("inconsistent JSON of KState object");
                }

                candidate.predecessor(pred);
                vector.add(candidate);
            }

            S sample = factory.sample(jsonseqelement.getJSONObject("sample"));

            sequence.add(new Tuple<>(vector, sample));
        }

        Collections.sort(sequence, new Comparator<Tuple<Set<C>, S>>() {
            @Override
            public int compare(Tuple<Set<C>, S> left, Tuple<Set<C>, S> right) {
                if (left.two().time() < right.two().time()) {
                    return -1;
                } else if (left.two().time() > right.two().time()) {
                    return +1;
                }
                return 0;
            }
        });
    }

    /**
     * Creates an empty {@link KState} object and sets <i>&kappa;</i> and <i>&tau;</i> parameters.
     *
     * @param k <i>&kappa;</i> parameter bounds the length of the state sequence to at most
     *        <i>&kappa;+1</i> states, if <i>&kappa; &ge; 0</i>.
     * @param t <i>&tau;</i> parameter bounds length of the state sequence to contain only states
     *        for the past <i>&tau;</i> milliseconds.
     */
    public KState(int k, long t) {
        this.k = k;
        this.t = t;
        this.sequence = new LinkedList<>();
        this.counters = new HashMap<>();
    }

    @Override
    public boolean isEmpty() {
        return counters.isEmpty();
    }

    @Override
    public int size() {
        return counters.size();
    }

    @Override
    public Long time() {
        if (sequence.isEmpty()) {
            return null;
        } else {
            return sequence.peekLast().two().time();
        }
    }

    @Override
    public S sample() {
        if (sequence.isEmpty()) {
            return null;
        } else {
            return sequence.peekLast().two();
        }
    }

    /**
     * Gets the sequence of measurements <i>z<sub>0</sub>, z<sub>1</sub>, ..., z<sub>t</sub></i>.
     *
     * @return List with the sequence of measurements.
     */
    public List<S> samples() {
        LinkedList<S> samples = new LinkedList<>();
        for (Tuple<Set<C>, S> element : sequence) {
            samples.add(element.two());
        }
        return samples;
    }

    @Override
    public void update(Set<C> vector, S sample) {
        if (vector.isEmpty()) {
            return;
        }

        if (!sequence.isEmpty() && sequence.peekLast().two().time() > sample.time()) {
            throw new RuntimeException("out-of-order state update is prohibited");
        }

        C estimate = estimate();
        for (C candidate : vector) {
            counters.put(candidate, 0);
            if (candidate.predecessor() == null) {
                candidate.predecessor(estimate);
            }
            if (candidate.predecessor() != null) {
                if (!counters.containsKey(candidate.predecessor())
                        || !sequence.peekLast().one().contains(candidate.predecessor())) {
                    throw new RuntimeException("inconsistent update vector");
                }
                counters.put(candidate.predecessor(), counters.get(candidate.predecessor()) + 1);
            }
        }

        if (!sequence.isEmpty()) {
            Tuple<Set<C>, S> last = sequence.peekLast();
            Set<C> deletes = new HashSet<>();

            for (C candidate : last.one()) {
                if (counters.get(candidate) == 0) {
                    deletes.add(candidate);
                }
            }

            for (C candidate : deletes) {
                remove(candidate, sequence.size() - 1);
            }
        }

        sequence.add(new Tuple<>(vector, sample));

        while ((t > 0 && sample.time() - sequence.peekFirst().two().time() > t)
                || (k >= 0 && sequence.size() > k + 1)) {
            Set<C> deletes = sequence.removeFirst().one();
            for (C candidate : deletes) {
                counters.remove(candidate);
            }

            for (C candidate : sequence.peekFirst().one()) {
                candidate.predecessor(null);
            }
        }

        assert (k < 0 || sequence.size() <= k + 1);
    }

    protected void remove(C candidate, int index) {
        while (index >= 0) {
            Set<C> vector = sequence.get(index).one();
            counters.remove(candidate);
            vector.remove(candidate);

            C predecessor = candidate.predecessor();
            if (predecessor == null) {
                return;
            }

            counters.put(predecessor, counters.get(predecessor) - 1);

            if (counters.get(predecessor) == 0) {
                candidate = predecessor;
                index -= 1;
            } else {
                return;
            }
        }
    }

    @Override
    public Set<C> vector() {
        if (sequence.isEmpty()) {
            return new HashSet<>();
        } else {
            return sequence.peekLast().one();
        }
    }

    @Override
    public C estimate() {
        if (sequence.isEmpty()) {
            return null;
        }

        C estimate = null;
        for (C candidate : sequence.peekLast().one()) {
            if (estimate == null || candidate.filtprob() > estimate.filtprob()) {
                estimate = candidate;
            }
        }
        return estimate;
    }

    /**
     * Gets the most likely sequence of state candidates <i>s<sub>0</sub>, s<sub>1</sub>, ...,
     * s<sub>t</sub></i>.
     *
     * @return List of the most likely sequence of state candidates.
     */
    public List<C> sequence() {
        if (sequence.isEmpty()) {
            return null;
        }

        C kestimate = estimate();
        LinkedList<C> ksequence = new LinkedList<>();

        for (int i = sequence.size() - 1; i >= 0; --i) {
            if (kestimate != null) {
                ksequence.push(kestimate);
                kestimate = kestimate.predecessor();
            }
        }

        return ksequence;
    }

    @Override
    public JSONObject toJSON() throws JSONException {
        JSONObject json = new JSONObject();
        JSONArray jsonsequence = new JSONArray();
        for (Tuple<Set<C>, S> element : sequence) {
            JSONObject jsonseqelement = new JSONObject();
            JSONArray jsonvector = new JSONArray();
            for (C candidate : element.one()) {
                JSONObject jsoncandidate = new JSONObject();
                jsoncandidate.put("candid", candidate.id());
                jsoncandidate.put("predid",
                        candidate.predecessor() == null ? "" : candidate.predecessor().id());
                jsonvector.put(jsoncandidate);
            }
            jsonseqelement.put("vector", jsonvector);
            jsonseqelement.put("sample", element.two().toJSON());
            jsonsequence.put(jsonseqelement);
        }

        JSONArray jsoncandidates = new JSONArray();
        for (Entry<C, Integer> entry : counters.entrySet()) {
            JSONObject jsoncandidate = new JSONObject();
            jsoncandidate.put("candidate", entry.getKey().toJSON());
            jsoncandidate.put("count", entry.getValue());
            jsoncandidates.put(jsoncandidate);
        }
        json.put("k", k);
        json.put("t", t);
        json.put("sequence", jsonsequence);
        json.put("candidates", jsoncandidates);

        return json;
    }
}


Question: import java.text.SimpleDateFormat;
import java.util.Collections;
import java.util.Comparator;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.Set;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.atomic.AtomicInteger;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import com.bmwcarit.barefoot.markov.Filter;
import com.bmwcarit.barefoot.markov.KState;
import com.bmwcarit.barefoot.road.Heading;
import com.bmwcarit.barefoot.roadmap.Distance;
import com.bmwcarit.barefoot.roadmap.Road;
import com.bmwcarit.barefoot.roadmap.RoadMap;
import com.bmwcarit.barefoot.roadmap.RoadPoint;
import com.bmwcarit.barefoot.roadmap.Route;
import com.bmwcarit.barefoot.roadmap.TimePriority;
import com.bmwcarit.barefoot.scheduler.StaticScheduler;
import com.bmwcarit.barefoot.scheduler.StaticScheduler.InlineScheduler;
import com.bmwcarit.barefoot.scheduler.Task;
import com.bmwcarit.barefoot.spatial.SpatialOperator;
import com.bmwcarit.barefoot.topology.Cost;
import com.bmwcarit.barefoot.topology.Router;
import com.bmwcarit.barefoot.util.Stopwatch;
import com.bmwcarit.barefoot.util.Tuple;
import com.esri.core.geometry.GeometryEngine;
import com.esri.core.geometry.WktExportFlags;
/*
 * Copyright (C) 2015, BMW Car IT GmbH
 *
 * Author: Sebastian Mattheis <sebastian.mattheis@bmw-carit.de>
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except
 * in compliance with the License. You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in
 * writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific
 * language governing permissions and limitations under the License.
 */

package com.bmwcarit.barefoot.matcher;




/**
 * Matcher filter for Hidden Markov Model (HMM) map matching. It is a HMM filter (@{link Filter})
 * and determines emission and transition probabilities for map matching with HMM.
 */
public class Matcher extends Filter<MatcherCandidate, MatcherTransition, MatcherSample> {
    private static final Logger logger = LoggerFactory.getLogger(Matcher.class);

    private final RoadMap map;
Answer:     private final Router<Road, RoadPoint> router;