public double getTargetDistance(Vertex vertex) {
   int vertexIndex = vertex.getIndex();
   if (vertexIndex < distance.length) {
     if (distance[vertexIndex] > 0.0) {
       return distance[vertexIndex];
     } else {
       double d =
           distanceLibrary.fastDistance(
               realTargetCoordinate.y, realTargetCoordinate.x, vertex.getY(), vertex.getX());
       distance[vertexIndex] = d;
       return d;
     }
   } else {
     return distanceLibrary.fastDistance(
         realTargetCoordinate.y, realTargetCoordinate.x, vertex.getY(), vertex.getX());
   }
 }
 /**
  * Find transfer candidates for profile routing. TODO replace with an on-street search using the
  * existing profile router functions.
  */
 public Map<StopCluster, Double> findNearbyStopClusters(StopCluster sc, double radius) {
   Map<StopCluster, Double> ret = Maps.newHashMap();
   Envelope env = new Envelope(new Coordinate(sc.lon, sc.lat));
   env.expandBy(
       SphericalDistanceLibrary.metersToLonDegrees(radius, sc.lat),
       SphericalDistanceLibrary.metersToDegrees(radius));
   for (StopCluster cluster : stopClusterSpatialIndex.query(env)) {
     // TODO this should account for area-like nature of clusters. Use size of bounding boxes.
     double distance = distlib.distance(sc.lat, sc.lon, cluster.lat, cluster.lon);
     if (distance < radius) ret.put(cluster, distance);
   }
   return ret;
 }
  /**
   * Computes the angle from the first point to the last point of a LineString or MultiLineString.
   * TODO: put this method into org.opentripplanner.common.geometry.DirectionUtils
   *
   * @param geometry a LineString or a MultiLineString
   * @return
   */
  public synchronized double getFirstToLastSegmentAngle(Geometry geometry) {
    LineString line;
    if (geometry instanceof MultiLineString) {
      line = (LineString) geometry.getGeometryN(geometry.getNumGeometries() - 1);
    } else {
      assert geometry instanceof LineString;
      line = (LineString) geometry;
    }
    int numPoints = line.getNumPoints();
    Coordinate coord0 = line.getCoordinateN(0);
    Coordinate coord1 = line.getCoordinateN(numPoints - 1);
    int i = numPoints - 3;
    while (distanceLibrary.fastDistance(coord0, coord1) < 10 && i >= 0) {
      coord1 = line.getCoordinateN(i--);
    }

    geodeticCalculator.setStartingGeographicPoint(coord0.x, coord0.y);
    geodeticCalculator.setDestinationGeographicPoint(coord1.x, coord1.y);
    return geodeticCalculator.getAzimuth() * Math.PI / 180;
  }
  @Override
  public List<GraphPath> getPaths(RoutingRequest options) {

    final Graph graph = graphService.getGraph(options.getRouterId());
    if (options.rctx == null) {
      options.setRoutingContext(graph);
      options.rctx.pathParsers =
          new PathParser[] {new BasicPathParser(), new NoThruTrafficPathParser()};
    }

    if (!options.getModes().isTransit()) {
      return sptService.getShortestPathTree(options).getPaths();
    }

    // also fall back to A* for short trips
    double distance =
        distanceLibrary.distance(
            options.rctx.origin.getCoordinate(), options.rctx.target.getCoordinate());
    if (distance < shortPathCutoff) {
      log.debug("Falling back to A* for very short path");
      return shortPathService.getPaths(options);
    }

    RaptorDataService service = graph.getService(RaptorDataService.class);
    if (service == null) {
      log.warn("No raptor data.  Rebuild with RaptorDataBuilder");
      return Collections.emptyList();
    }
    RaptorData data = service.getData();

    // we multiply the initial walk distance to account for epsilon dominance.
    double initialWalk = options.getMaxWalkDistance() * WALK_EPSILON;
    options.setMaxWalkDistance(initialWalk);

    // do not even bother with obviously impossible walks
    double minWalk =
        options.rctx.origin.getDistanceToNearestTransitStop()
            + options.rctx.target.getDistanceToNearestTransitStop();
    if (options.getMaxWalkDistance() < minWalk) {
      options.setMaxWalkDistance(minWalk);
    }

    RoutingRequest walkOptions = options.clone();
    walkOptions.rctx.pathParsers = new PathParser[0];
    TraverseModeSet modes = options.getModes().clone();
    modes.setTransit(false);
    walkOptions.setModes(modes);
    RaptorSearch search = new RaptorSearch(data, options);

    if (data.maxTransitRegions != null) {
      Calendar tripDate = Calendar.getInstance(graph.getTimeZone());
      tripDate.setTime(new Date(1000L * options.dateTime));

      Calendar maxTransitStart = Calendar.getInstance(graph.getTimeZone());
      maxTransitStart.set(Calendar.YEAR, data.maxTransitRegions.startYear);
      maxTransitStart.set(Calendar.MONTH, data.maxTransitRegions.startMonth);
      maxTransitStart.set(Calendar.DAY_OF_MONTH, data.maxTransitRegions.startDay);

      int day = 0;
      while (tripDate.after(maxTransitStart)) {
        day++;
        tripDate.add(Calendar.DAY_OF_MONTH, -1);
      }
      if (day > data.maxTransitRegions.maxTransit.length || options.isWheelchairAccessible()) {
        day = -1;
      }

      search.maxTimeDayIndex = day;
    }

    int rushAheadRound = preliminaryRaptorSearch(data, options, walkOptions, search);

    long searchBeginTime = System.currentTimeMillis();

    double expectedWorstTime =
        1.5
            * distanceLibrary.distance(
                options.rctx.origin.getCoordinate(), options.rctx.target.getCoordinate())
            / options.getWalkSpeed();

    int foundSoFar = 0;

    double firstWalkDistance = 0;
    List<RaptorState> targetStates = new ArrayList<RaptorState>();

    do {
      int bestElapsedTime = Integer.MAX_VALUE;
      RETRY:
      do {
        for (int round = 0; round < options.getMaxTransfers() + 2; ++round) {
          if (!round(data, options, walkOptions, search, round)) break;

          long elapsed = System.currentTimeMillis() - searchBeginTime;
          if (elapsed > multiPathTimeout * 1000 && multiPathTimeout > 0 && targetStates.size() > 0)
            break RETRY;

          ArrayList<RaptorState> toRemove = new ArrayList<RaptorState>();
          for (RaptorState state : search.getTargetStates()) {
            if (state.nBoardings == 0 && options.getMaxWalkDistance() > initialWalk) {
              toRemove.add(state);
            }
          }
          if (search.getTargetStates().size() > 0) {
            if (firstWalkDistance == 0) {
              firstWalkDistance = options.getMaxWalkDistance();
            }
            for (RaptorState state : toRemove) {
              search.removeTargetState(state.walkPath);
            }
          }
          if (targetStates.size() >= options.getNumItineraries() && round >= rushAheadRound) {
            int oldBest = bestElapsedTime;
            for (RaptorState state : search.getTargetStates()) {
              final int elapsedTime = (int) Math.abs(state.arrivalTime - options.dateTime);
              if (elapsedTime < bestElapsedTime) {
                bestElapsedTime = elapsedTime;
              }
            }

            int improvement = oldBest - bestElapsedTime;
            if (improvement < 600 && bestElapsedTime < expectedWorstTime) break RETRY;
          }
        }

        if (foundSoFar < search.getTargetStates().size()) {
          foundSoFar = search.getTargetStates().size();
        } else if (foundSoFar > 0) {
          // we didn't find anything new in this round, and we already have
          // some paths, so bail out
          break;
        }
        options = options.clone();
        walkOptions = walkOptions.clone();
        if (search.getTargetStates().size() > 0 && bestElapsedTime < expectedWorstTime) {
          // we have found some paths so we no longer want to expand the max walk distance
          break RETRY;
        } else {
          options.setMaxWalkDistance(options.getMaxWalkDistance() * 2);
          walkOptions.setMaxWalkDistance(options.getMaxWalkDistance());

          options.setWalkReluctance(options.getWalkReluctance() * 2);
          walkOptions.setWalkReluctance(options.getWalkReluctance());
        }
        search.reset(options);

      } while (options.getMaxWalkDistance() < initialWalk * MAX_WALK_MULTIPLE
          && initialWalk < Double.MAX_VALUE);

      options = options.clone();
      walkOptions = walkOptions.clone();
      for (RaptorState state : search.getTargetStates()) {
        for (AgencyAndId trip : state.getTrips()) {
          options.bannedTrips.add(trip);
        }
      }

      if (search.getTargetStates().size() == 0) break; // no paths found; searching more won't help

      options.setMaxWalkDistance(firstWalkDistance);
      walkOptions.setMaxWalkDistance(firstWalkDistance);

      targetStates.addAll(search.getTargetStates());
      search = new RaptorSearch(data, options);

    } while (targetStates.size() < options.getNumItineraries());

    collectRoutesUsed(data, options, targetStates);

    if (targetStates.isEmpty()) {
      log.info("RAPTOR found no paths");
    }
    Collections.sort(targetStates);

    if (targetStates.size() > options.getNumItineraries())
      targetStates = targetStates.subList(0, options.getNumItineraries());

    List<GraphPath> paths = new ArrayList<GraphPath>();
    for (RaptorState targetState : targetStates) {
      // reconstruct path
      ArrayList<RaptorState> states = new ArrayList<RaptorState>();
      RaptorState cur = targetState;
      while (cur != null) {
        states.add(cur);
        cur = cur.getParent();
      }
      // states is in reverse order of time
      State state = getState(targetState.getRequest(), data, states);
      paths.add(new GraphPath(state, true));
    }

    return paths;
  }
  @Override
  public boolean shouldSkipTraversalResult(
      Vertex origin,
      Vertex target,
      State parent,
      State current,
      ShortestPathTree spt,
      RoutingRequest traverseOptions) {
    if (realTarget == null) return false;

    final Vertex vertex = current.getVertex();
    int vertexIndex = vertex.getIndex();
    if (vertexIndex < distance.length) {
      if (distance[vertexIndex] > 0.0) {
        targetDistance = distance[vertexIndex];
      } else {
        targetDistance =
            distanceLibrary.fastDistance(
                realTargetCoordinate.y, realTargetCoordinate.x, vertex.getY(), vertex.getX());
        distance[vertexIndex] = targetDistance;
        if (vertex instanceof TransitStop && targetDistance < bestTargetDistance) {
          bestTargetDistance = targetDistance;
        }
      }
    } else {
      targetDistance =
          distanceLibrary.fastDistance(
              realTargetCoordinate.y, realTargetCoordinate.x, vertex.getY(), vertex.getX());
    }

    final double remainingWalk = traverseOptions.maxWalkDistance - current.getWalkDistance();
    final double minWalk;
    double minTime = 0;
    if (targetDistance > remainingWalk) {
      // then we must have some transit + some walk.
      minWalk = this.distanceToNearestTransitStop + vertex.getDistanceToNearestTransitStop();
      minTime =
          options.isArriveBy() ? traverseOptions.getAlightSlack() : traverseOptions.getBoardSlack();

      if (current.getBackEdge() instanceof StreetEdge
          && transitLocalStreets != null
          && !transitLocalStreets.transferrable(vertex)) {
        return true;
      }
    } else {
      // could walk directly to destination
      if (targetDistance < distanceToNearestTransitStop
          || transitLocalStreets == null
          || !transitLocalStreets.transferrable(vertex)) minWalk = targetDistance;
      else minWalk = distanceToNearestTransitStop;
    }
    if (minWalk > remainingWalk) return true;

    final double optimisticDistance = current.getWalkDistance() + minWalk;

    final double walkTime = minWalk / speedUpperBound;
    minTime += (targetDistance - minWalk) / Raptor.MAX_TRANSIT_SPEED + walkTime;

    double stateTime = current.getOptimizedElapsedTime() + minTime;

    double walkDistance =
        FastMath.max(
            optimisticDistance * Raptor.WALK_EPSILON,
            optimisticDistance + transferTimeInWalkDistance);

    int i = 0;
    boolean prevBounded = !bounders.isEmpty();
    for (State bounder : bounders) {
      if (removedBoundingStates.contains(bounder)) continue;
      if (current.getWeight() + minTime + walkTime * (options.getWalkReluctance() - 1)
          > bounder.getWeight() * WORST_WEIGHT_DIFFERENCE_FACTOR) {
        return true;
      }
      int prevTime = previousArrivalTime.get(i++);

      if (walkDistance > bounder.getWalkDistance()
          && current.getNumBoardings() >= bounder.getNumBoardings()) {
        if (current.getElapsedTime() + minTime >= bounder.getElapsedTime()) {
          return true;
        } else if (prevTime > 0
            && (options.arriveBy
                ? (current.getTime() - minTime >= prevTime)
                : ((current.getTime() + minTime) <= prevTime))) {
          prevBounded = false;
        }
      } else {
        prevBounded = false;
      }

      // check that the new path is not much longer in time than the bounding path
      if (bounder.getOptimizedElapsedTime() * timeBoundFactor < stateTime) {
        return true;
      }
    }
    return prevBounded;
  }
  @Override
  public List<GraphPath> getPaths(RoutingRequest options) {

    if (options.rctx == null) {
      options.setRoutingContext(graphService.getGraph(options.getRouterId()));
      // move into setRoutingContext ?
      options.rctx.pathParsers =
          new PathParser[] {new BasicPathParser(), new NoThruTrafficPathParser()};
    }

    RemainingWeightHeuristic heuristic;
    if (options.getModes().isTransit()) {
      LOG.debug("Transit itinerary requested.");
      // always use the bidirectional heuristic because the others are not precise enough
      heuristic = new BidirectionalRemainingWeightHeuristic(options.rctx.graph);
    } else {
      LOG.debug("Non-transit itinerary requested.");
      heuristic = new DefaultRemainingWeightHeuristic();
    }

    // the states that will eventually be turned into paths and returned
    List<State> returnStates = new LinkedList<State>();

    BinHeap<State> pq = new BinHeap<State>();
    //        List<State> boundingStates = new ArrayList<State>();

    Vertex originVertex = options.rctx.origin;
    Vertex targetVertex = options.rctx.target;

    // increase maxWalk repeatedly in case hard limiting is in use
    WALK:
    for (double maxWalk = options.getMaxWalkDistance(); returnStates.isEmpty(); maxWalk *= 2) {
      if (maxWalk != Double.MAX_VALUE && maxWalk > MAX_WALK) {
        break;
      }
      LOG.debug("try search with max walk {}", maxWalk);
      // increase maxWalk if settings make trip impossible
      if (maxWalk
          < Math.min(
              distanceLibrary.distance(originVertex.getCoordinate(), targetVertex.getCoordinate()),
              originVertex.getDistanceToNearestTransitStop()
                  + targetVertex.getDistanceToNearestTransitStop())) continue WALK;
      options.setMaxWalkDistance(maxWalk);

      // cap search / heuristic weight
      final double AVG_TRANSIT_SPEED = 25; // m/sec
      double cutoff =
          (distanceLibrary.distance(originVertex.getCoordinate(), targetVertex.getCoordinate())
                  * 1.5)
              / AVG_TRANSIT_SPEED; // wait time is irrelevant in the heuristic
      cutoff += options.getMaxWalkDistance() * options.walkReluctance;
      options.maxWeight = cutoff;

      State origin = new State(options);
      // (used to) initialize heuristic outside loop so table can be reused
      heuristic.computeInitialWeight(origin, targetVertex);

      options.maxWeight = cutoff + 30 * 60 * options.waitReluctance;

      // reinitialize states for each retry
      HashMap<Vertex, List<State>> states = new HashMap<Vertex, List<State>>();
      pq.reset();
      pq.insert(origin, 0);
      long startTime = System.currentTimeMillis();
      long endTime = startTime + (int) (_timeouts[0] * 1000);
      LOG.debug("starttime {} endtime {}", startTime, endTime);
      QUEUE:
      while (!pq.empty()) {

        if (System.currentTimeMillis() > endTime) {
          LOG.debug("timeout at {} msec", System.currentTimeMillis() - startTime);
          if (returnStates.isEmpty()) break WALK; // disable walk distance increases
          else {
            storeMemory();
            break WALK;
          }
        }

        //                if (pq.peek_min_key() > options.maxWeight) {
        //                    LOG.debug("max weight {} exceeded", options.maxWeight);
        //                    break QUEUE;
        //                }

        State su = pq.extract_min();

        //                for (State bs : boundingStates) {
        //                    if (eDominates(bs, su)) {
        //                        continue QUEUE;
        //                    }
        //                }

        Vertex u = su.getVertex();

        if (traverseVisitor != null) {
          traverseVisitor.visitVertex(su);
        }

        if (u.equals(targetVertex)) {
          //                    boundingStates.add(su);
          returnStates.add(su);
          if (!options.getModes().isTransit()) break QUEUE;
          // options should contain max itineraries
          if (returnStates.size() >= _maxPaths) break QUEUE;
          if (returnStates.size() < _timeouts.length) {
            endTime = startTime + (int) (_timeouts[returnStates.size()] * 1000);
            LOG.debug(
                "{} path, set timeout to {}",
                returnStates.size(),
                _timeouts[returnStates.size()] * 1000);
          }
          continue QUEUE;
        }

        for (Edge e : options.isArriveBy() ? u.getIncoming() : u.getOutgoing()) {
          STATE:
          for (State new_sv = e.traverse(su); new_sv != null; new_sv = new_sv.getNextResult()) {
            if (traverseVisitor != null) {
              traverseVisitor.visitEdge(e, new_sv);
            }

            double h = heuristic.computeForwardWeight(new_sv, targetVertex);
            if (h == Double.MAX_VALUE) continue;
            //                    for (State bs : boundingStates) {
            //                        if (eDominates(bs, new_sv)) {
            //                            continue STATE;
            //                        }
            //                    }
            Vertex v = new_sv.getVertex();
            List<State> old_states = states.get(v);
            if (old_states == null) {
              old_states = new LinkedList<State>();
              states.put(v, old_states);
            } else {
              for (State old_sv : old_states) {
                if (eDominates(old_sv, new_sv)) {
                  continue STATE;
                }
              }
              Iterator<State> iter = old_states.iterator();
              while (iter.hasNext()) {
                State old_sv = iter.next();
                if (eDominates(new_sv, old_sv)) {
                  iter.remove();
                }
              }
            }
            if (traverseVisitor != null) traverseVisitor.visitEnqueue(new_sv);

            old_states.add(new_sv);
            pq.insert(new_sv, new_sv.getWeight() + h);
          }
        }
      }
    }
    storeMemory();

    // Make the states into paths and return them
    List<GraphPath> paths = new LinkedList<GraphPath>();
    for (State s : returnStates) {
      LOG.debug(s.toStringVerbose());
      paths.add(new GraphPath(s, true));
    }
    // sort by arrival time, though paths are already in order of increasing difficulty
    // Collections.sort(paths, new PathComparator(origin.getOptions().isArriveBy()));
    return paths;
  }