private void makeEdges(StreetVertex v1, StreetVertex v2, String name) {
    LineString geometry =
        GeometryUtils.makeLineString(
            v1.getCoordinate().x, v1.getCoordinate().y, v2.getCoordinate().x, v2.getCoordinate().y);
    double length =
        SphericalDistanceLibrary.getInstance().distance(v1.getCoordinate(), v2.getCoordinate());
    new PlainStreetEdge(v1, v2, geometry, name, length, StreetTraversalPermission.ALL, false);

    geometry =
        GeometryUtils.makeLineString(
            v2.getCoordinate().x, v2.getCoordinate().y, v1.getCoordinate().x, v1.getCoordinate().y);
    new PlainStreetEdge(v2, v1, geometry, name, length, StreetTraversalPermission.ALL, true);
  }
 /**
  * FIXME OBA parentStation field is a string, not an AgencyAndId, so it has no agency/feed scope
  * But the DC regional graph has no parent stations pre-defined, so no use dealing with them for
  * now. However Trimet stops have "landmark" or Transit Center parent stations, so we don't use
  * the parent stop field.
  *
  * <p>Ideally in the future stop clusters will replicate and/or share implementation with GTFS
  * parent stations.
  *
  * <p>We can't use a similarity comparison, we need exact matches. This is because many street
  * names differ by only one letter or number, e.g. 34th and 35th or Avenue A and Avenue B.
  * Therefore normalizing the names before the comparison is essential. The agency must provide
  * either parent station information or a well thought out stop naming scheme to cluster stops --
  * no guessing is reasonable without that information.
  */
 public void clusterStops() {
   int psIdx = 0; // unique index for next parent stop
   LOG.info("Clustering stops by geographic proximity and name...");
   // Each stop without a cluster will greedily claim other stops without clusters.
   for (Stop s0 : stopForId.values()) {
     if (stopClusterForStop.containsKey(s0))
       continue; // skip stops that have already been claimed by a cluster
     String s0normalizedName = StopNameNormalizer.normalize(s0.getName());
     StopCluster cluster = new StopCluster(String.format("C%03d", psIdx++), s0normalizedName);
     // LOG.info("stop {}", s0normalizedName);
     // No need to explicitly add s0 to the cluster. It will be found in the spatial index query
     // below.
     Envelope env = new Envelope(new Coordinate(s0.getLon(), s0.getLat()));
     env.expandBy(
         SphericalDistanceLibrary.metersToLonDegrees(CLUSTER_RADIUS, s0.getLat()),
         SphericalDistanceLibrary.metersToDegrees(CLUSTER_RADIUS));
     for (TransitStop ts1 : stopSpatialIndex.query(env)) {
       Stop s1 = ts1.getStop();
       double geoDistance =
           SphericalDistanceLibrary.getInstance()
               .fastDistance(s0.getLat(), s0.getLon(), s1.getLat(), s1.getLon());
       if (geoDistance < CLUSTER_RADIUS) {
         String s1normalizedName = StopNameNormalizer.normalize(s1.getName());
         // LOG.info("   --> {}", s1normalizedName);
         // LOG.info("       geodist {} stringdist {}", geoDistance, stringDistance);
         if (s1normalizedName.equals(s0normalizedName)) {
           // Create a bidirectional relationship between the stop and its cluster
           cluster.children.add(s1);
           stopClusterForStop.put(s1, cluster);
         }
       }
     }
     cluster.computeCenter();
     stopClusterForId.put(cluster.id, cluster);
   }
   //        LOG.info("Done clustering stops.");
   //        for (StopCluster cluster : stopClusterForId.values()) {
   //            LOG.info("{} at {} {}", cluster.name, cluster.lat, cluster.lon);
   //            for (Stop stop : cluster.children) {
   //                LOG.info("   {}", stop.getName());
   //            }
   //        }
 }
@Path("/iso")
@XmlRootElement
@Autowire
public class IsoChrone extends RoutingResource {

  private static final Logger LOG = LoggerFactory.getLogger(IsoChrone.class);

  public static final String RESULT_TYPE_POINTS = "POINTS";

  public static final String RESULT_TYPE_SHED = "SHED";

  public static final String RESULT_TYPE_EDGES = "EDGES";

  private boolean showTooFastEdgesAsDebugGeomsANDnotUShapes = true;

  private List debugGeoms = null;

  private List tooFastTraversedEdgeGeoms = null;

  @Autowired GraphService graphService;

  @Autowired private SPTService sptService;

  @Autowired private GeometryIndex index;

  /** Walkspeed between user indicated position and road 3000 m/h = 0.83333 m/sec */
  public double offRoadWalkspeed = 0.8333;

  /**
   * To decide between edge-based or point-based calculation of sheds, i.e. hulls. Will be set later
   * again.
   */
  public long shedCalcMethodSwitchTimeInSec = 60 * 25;

  public double angleLimitForUShapeDetection = 20.0 * Math.PI / 180.0;

  public double distanceToleranceForUShapeDetection = 1.1; // in percent: e.g. 1.1 = 110%

  /**
   * To calculate the length of sub-edges and eventually to detect u-shaped roads, in m/sec (will be
   * set later dependent on mode)
   */
  public double maxUserSpeed = 1.3;

  private boolean usesCar = false;

  /**
   * Parameter for concave hull computation, i.e. the maximal (triangulation) edge length in degrees
   */
  public double concaveHullAlpha = 0.005;

  public boolean doSpeedTest =
      false; // to detect u-shaped roads etc., as an additional test besides the angle test

  private boolean noRoadNearBy = false;

  private DistanceLibrary distanceLibrary = SphericalDistanceLibrary.getInstance();

  /**
   * Calculates walksheds for a given location, based on time given to walk and the walk speed.
   *
   * <p>Depending on the value for the "output" parameter (i.e. "POINTS", "SHED" or "EDGES"), a
   * different type of GeoJSON geometry is returned. If a SHED is requested, then a ConcaveHull of
   * the EDGES/roads is returned. If that fails, a ConvexHull will be returned.
   *
   * <p>The ConcaveHull parameter is set to 0.005 degrees. The offroad walkspeed is assumed to be
   * 0.83333 m/sec (= 3km/h) until a road is hit.
   *
   * <p>Note that the set of EDGES/roads returned as well as POINTS returned may contain duplicates.
   * If POINTS are requested, then not the end-points are returned at which the max time is reached,
   * but instead all the graph nodes/crossings that are within the time limits.
   *
   * <p>In case there is no road near by within the given time, then a circle for the walktime limit
   * is created and returned for the SHED parameter. Otherwise the edge with the direction towards
   * the closest road. Note that the circle is calculated in Euclidian 2D coordinates, and
   * distortions towards an ellipse will appear if it is transformed/projected to the user location.
   *
   * <p>An example request may look like this:
   * localhost:8080/otp-rest-servlet/ws/iso?layers=traveltime&styles=mask&batch=true&fromPlace=51.040193121307176
   * %2C-114.04471635818481&toPlace
   * =51.09098935%2C-113.95179705&time=2012-06-06T08%3A00%3A00&mode=WALK&maxWalkDistance=10000&walkSpeed=1.38&walkTime=10.7&output=EDGES
   * Though the first parameters (i) layer, (ii) styles and (iii) batch could be discarded.
   *
   * @param walkmins Maximum number of minutes to walk.
   * @param output Can be set to "POINTS", "SHED" or "EDGES" to return different types of GeoJSON
   *     geometry. SHED returns a ConcaveHull or ConvexHull of the edges/roads. POINTS returns all
   *     graph nodes that are within the time limit.
   * @return a JSON document containing geometries (either points, lineStrings or a polygon).
   * @throws Exception
   * @author sstein---geo.uzh.ch
   */
  @GET
  @Produces({MediaType.APPLICATION_JSON})
  public String getIsochrone(
      @QueryParam("walkTime") @DefaultValue("15") double walkmins,
      @QueryParam("output") @DefaultValue("POINTS") String output)
      throws Exception {

    this.debugGeoms = new ArrayList();
    this.tooFastTraversedEdgeGeoms = new ArrayList();

    RoutingRequest sptRequestA = buildRequest(0);
    String from = sptRequestA.getFrom().toString();
    int pos = 1;
    float lat = 0;
    float lon = 0;
    for (String s : from.split(",")) {
      if (s.isEmpty()) {
        // no location
        Response.status(Status.BAD_REQUEST).entity("no position").build();
        return null;
      }
      try {
        float num = Float.parseFloat(s);
        if (pos == 1) {
          lat = num;
        }
        if (pos == 2) {
          lon = num;
        }
      } catch (Exception e) {
        throw new WebApplicationException(
            Response.status(Status.BAD_REQUEST)
                .entity(
                    "Could not parse position string to number. Require numerical lat & long coords.")
                .build());
      }
      pos++;
    }

    GeometryFactory gf = new GeometryFactory();

    Coordinate dropPoint = new Coordinate(lon, lat);

    int walkInMin = (int) Math.floor(walkmins);
    double walkInSec = walkmins * 60;
    LOG.debug(
        "given travel time: " + walkInMin + " mins + " + (walkInSec - (60 * walkInMin)) + " sec");
    // restrict the evaluated SPT size to 30mins for requests with walking < 30min
    // if larger walking times are requested we adjust the evaluated
    // graph dynamically by 1.3 * min -> this should save processing time
    if (walkInMin < 30) {
      sptRequestA.worstTime = sptRequestA.dateTime + (30 * 60);
    } else {
      sptRequestA.worstTime = sptRequestA.dateTime + Math.round(walkInMin * 1.3 * 60);
    }
    // set the switch-time for shed/area calculation, i.e. to decide if the hull is calculated based
    // on points or on edges
    TraverseModeSet modes = sptRequestA.modes;
    LOG.debug("mode(s): " + modes);
    if ((modes.contains(TraverseMode.TRANSIT))
        || (modes.contains(TraverseMode.BUSISH))
        || (modes.contains(TraverseMode.TRAINISH))) {
      shedCalcMethodSwitchTimeInSec =
          60 * 20; // 20min (use 20min for transit, since buses may not come all the time)
    } else if (modes.contains(TraverseMode.CAR)) {
      shedCalcMethodSwitchTimeInSec = 60 * 10; // 10min
    } else if (modes.contains(TraverseMode.BICYCLE)) {
      shedCalcMethodSwitchTimeInSec = 60 * 10; // 10min
    } else {
      shedCalcMethodSwitchTimeInSec = 60 * 20; // 20min
    }
    // set the maxUserSpeed, which is used later to check for u-type streets/crescents when
    // calculating sub-edges;
    // Note, that the car speed depends on the edge itself, so this value may be replaced later
    this.usesCar = false;
    int numberOfModes = modes.getModes().size();
    if (numberOfModes == 1) {
      if (modes.getWalk()) {
        this.maxUserSpeed = sptRequestA.getWalkSpeed();
      } else if (modes.getBicycle()) {
        this.maxUserSpeed = sptRequestA.getBikeSpeed();
      } else if (modes.getDriving()) {
        this.maxUserSpeed = sptRequestA.getCarSpeed();
        this.usesCar = true;
      }
    } else { // for all other cases (multiple-modes)
      // sstein: I thought I may set it to 36.111 m/sec = 130 km/h,
      // but maybe it is better to assume walk speed for transit, i.e. treat it like if the
      // person gets off the bus on the last crossing and walks the "last mile".
      this.maxUserSpeed = sptRequestA.getWalkSpeed();
    }

    if (doSpeedTest) {
      LOG.debug("performing angle and speed based test to detect u-shapes");
    } else {
      LOG.debug("performing only angle based test to detect u-shapes");
    }

    // TODO: OTP prefers to snap to car-roads/ways, which is not so nice, when walking,
    // and a footpath is closer by. So far there is no option to switch that off

    // create the ShortestPathTree
    try {
      sptRequestA.setRoutingContext(graphService.getGraph());
    } catch (Exception e) {
      // if we get an exception here, and in particular a VertexNotFoundException,
      // then it is likely that we chose a (transit) mode without having that (transit) modes data
      LOG.debug("cannot set RoutingContext: " + e.toString());
      LOG.debug("cannot set RoutingContext: setting mode=WALK");
      sptRequestA.setMode(TraverseMode.WALK); // fall back to walk mode
      sptRequestA.setRoutingContext(graphService.getGraph());
    }
    ShortestPathTree sptA = sptService.getShortestPathTree(sptRequestA);
    StreetLocation origin = (StreetLocation) sptRequestA.rctx.fromVertex;
    sptRequestA.cleanup(); // remove inserted points

    // create a LineString for display
    Coordinate pathToStreetCoords[] = new Coordinate[2];
    pathToStreetCoords[0] = dropPoint;
    pathToStreetCoords[1] = origin.getCoordinate();
    LineString pathToStreet = gf.createLineString(pathToStreetCoords);

    // get distance between origin and drop point for time correction
    double distanceToRoad =
        this.distanceLibrary.distance(origin.getY(), origin.getX(), dropPoint.y, dropPoint.x);
    long offRoadTimeCorrection = (long) (distanceToRoad / this.offRoadWalkspeed);

    //
    // --- filter the states ---
    //
    Set<Coordinate> visitedCoords = new HashSet<Coordinate>();
    ArrayList<Edge> allConnectingEdges = new ArrayList<Edge>();
    Coordinate coords[] = null;
    long maxTime = (long) walkInSec - offRoadTimeCorrection;
    // System.out.println("Reducing walktime from: " + (int)(walkmins * 60) + "sec to " + maxTime +
    // "sec due to initial walk of " + distanceToRoad
    // + "m");

    // if the initial walk is already to long, there is no need to parse...
    if (maxTime <= 0) {
      noRoadNearBy = true;
      long timeToWalk = (long) walkInSec;
      long timeBetweenStates = offRoadTimeCorrection;
      long timeMissing = timeToWalk;
      double fraction = (double) timeMissing / (double) timeBetweenStates;
      pathToStreet = getSubLineString(pathToStreet, fraction);
      LOG.debug(
          "no street found within giving travel time (for off-road walkspeed: {} m/sec)",
          this.offRoadWalkspeed);
    } else {
      noRoadNearBy = false;
      Map<ReversibleLineStringWrapper, Edge> connectingEdgesMap = Maps.newHashMap();
      for (State state : sptA.getAllStates()) {
        long et = state.getElapsedTimeSeconds();
        if (et <= maxTime) {
          // -- filter points, as the same coordinate may be passed several times due to the graph
          // structure
          // in a Calgary suburb family homes neighborhood with a 15min walkshed it filtered about
          // 250 points away (while 145 were finally displayed)
          if (visitedCoords.contains(state.getVertex().getCoordinate())) {
            continue;
          } else {
            visitedCoords.add(state.getVertex().getCoordinate());
          }
          // -- get all Edges needed later for the edge representation
          // and to calculate an edge-based walkshed
          // Note, it can happen that we get a null geometry here, e.g. for hop-edges!
          Collection<Edge> vertexEdgesIn = state.getVertex().getIncoming();
          for (Iterator<Edge> iterator = vertexEdgesIn.iterator(); iterator.hasNext(); ) {
            Edge edge = (Edge) iterator.next();
            Geometry edgeGeom = edge.getGeometry();
            if (edgeGeom != null) { // make sure we get only real edges
              if (edgeGeom instanceof LineString) {
                // allConnectingEdges.add(edge); // instead of this, use a map now, so we don't have
                // similar edge many times
                connectingEdgesMap.put(
                    new ReversibleLineStringWrapper((LineString) edgeGeom), edge);
              }
            }
          }
          Collection<Edge> vertexEdgesOut = state.getVertex().getOutgoing();
          for (Iterator<Edge> iterator = vertexEdgesOut.iterator(); iterator.hasNext(); ) {
            Edge edge = (Edge) iterator.next();
            Geometry edgeGeom = edge.getGeometry();
            if (edgeGeom != null) {
              if (edgeGeom instanceof LineString) {
                // allConnectingEdges.add(edge); // instead of this, use a map now, so we don't
                // similar edge many times
                connectingEdgesMap.put(
                    new ReversibleLineStringWrapper((LineString) edgeGeom), edge);
              }
            }
          }
        } // end : if(et < maxTime)
      }
      // --
      // points from list to array, for later
      coords = new Coordinate[visitedCoords.size()];
      int i = 0;
      for (Coordinate c : visitedCoords) coords[i++] = c;

      // connection edges from Map to List
      allConnectingEdges.clear();
      for (Edge tedge : connectingEdgesMap.values()) allConnectingEdges.add(tedge);
    }
    StringWriter sw = new StringWriter();
    GeoJSONBuilder json = new GeoJSONBuilder(sw);
    //
    // -- create the different outputs ---
    //
    try {
      if (output.equals(IsoChrone.RESULT_TYPE_POINTS)) {
        // in case there was no road we create a circle and
        // and return those points
        if (noRoadNearBy) {
          Geometry circleShape = createCirle(dropPoint, pathToStreet);
          coords = circleShape.getCoordinates();
        }
        // -- the states/nodes with time elapsed <= X min.
        LOG.debug("write multipoint geom with {} points", coords.length);
        json.writeGeom(gf.createMultiPoint(coords));
        LOG.debug("done");
      } else if (output.equals(IsoChrone.RESULT_TYPE_SHED)) {

        Geometry geomsArray[] = null;
        // in case there was no road we create a circle
        if (noRoadNearBy) {
          Geometry circleShape = createCirle(dropPoint, pathToStreet);
          json.writeGeom(circleShape);
        } else {
          if (maxTime > shedCalcMethodSwitchTimeInSec) { // eg., walkshed > 20 min
            // -- create a point-based walkshed
            // less exact and should be used for large walksheds with many edges
            LOG.debug("create point-based shed (not from edges)");
            geomsArray = new Geometry[coords.length];
            for (int j = 0; j < geomsArray.length; j++) {
              geomsArray[j] = gf.createPoint(coords[j]);
            }
          } else {
            // -- create an edge-based walkshed
            // it is more exact and should be used for short walks
            LOG.debug("create edge-based shed (not from points)");
            Map<ReversibleLineStringWrapper, LineString> walkShedEdges = Maps.newHashMap();
            // add the walk from the pushpin to closest street point
            walkShedEdges.put(new ReversibleLineStringWrapper(pathToStreet), pathToStreet);
            // get the edges and edge parts within time limits
            ArrayList<LineString> withinTimeEdges =
                this.getLinesAndSubEdgesWithinMaxTime(
                    maxTime,
                    allConnectingEdges,
                    sptA,
                    angleLimitForUShapeDetection,
                    distanceToleranceForUShapeDetection,
                    maxUserSpeed,
                    usesCar,
                    doSpeedTest);
            for (LineString ls : withinTimeEdges) {
              walkShedEdges.put(new ReversibleLineStringWrapper(ls), ls);
            }
            geomsArray = new Geometry[walkShedEdges.size()];
            int k = 0;
            for (LineString ls : walkShedEdges.values()) geomsArray[k++] = ls;
          } // end if-else: maxTime condition
          GeometryCollection gc = gf.createGeometryCollection(geomsArray);
          // create the concave hull, but in case it fails we just return the convex hull
          Geometry outputHull = null;
          LOG.debug(
              "create concave hull from {} geoms with edge length limit of about {} m (distance on meridian)",
              geomsArray.length,
              concaveHullAlpha * 111132);
          // 1deg at Latitude phi = 45deg is about 111.132km
          // (see wikipedia:
          // http://en.wikipedia.org/wiki/Latitude#The_length_of_a_degree_of_latitude)
          try {
            ConcaveHull hull = new ConcaveHull(gc, concaveHullAlpha);
            outputHull = hull.getConcaveHull();
          } catch (Exception e) {
            outputHull = gc.convexHull();
            LOG.debug("Could not generate ConcaveHull for WalkShed, using ConvexHull instead.");
          }
          LOG.debug("write shed geom");
          json.writeGeom(outputHull);
          LOG.debug("done");
        }
      } else if (output.equals(IsoChrone.RESULT_TYPE_EDGES)) {
        // in case there was no road we return only the suggested path to the street
        if (noRoadNearBy) {
          json.writeGeom(pathToStreet);
        } else {
          // -- if we would use only the edges from the paths to the origin we will miss
          // some edges that will be never on the shortest path (e.g. loops/crescents).
          // However, we can retrieve all edges by checking the times for each
          // edge end-point
          Map<ReversibleLineStringWrapper, LineString> walkShedEdges = Maps.newHashMap();
          // add the walk from the pushpin to closest street point
          walkShedEdges.put(new ReversibleLineStringWrapper(pathToStreet), pathToStreet);
          // get the edges and edge parts within time limits
          ArrayList<LineString> withinTimeEdges =
              this.getLinesAndSubEdgesWithinMaxTime(
                  maxTime,
                  allConnectingEdges,
                  sptA,
                  angleLimitForUShapeDetection,
                  distanceToleranceForUShapeDetection,
                  maxUserSpeed,
                  usesCar,
                  doSpeedTest);
          for (LineString ls : withinTimeEdges) {
            walkShedEdges.put(new ReversibleLineStringWrapper(ls), ls);
          }
          Geometry mls = null;
          LineString edges[] = new LineString[walkShedEdges.size()];
          int k = 0;
          for (LineString ls : walkShedEdges.values()) edges[k++] = ls;
          LOG.debug("create multilinestring from {} geoms", edges.length);
          mls = gf.createMultiLineString(edges);
          LOG.debug("write geom");
          json.writeGeom(mls);
          LOG.debug("done");
        }
      } else if (output.equals("DEBUGEDGES")) {
        // -- for debugging, i.e. display of detected u-shapes/crescents
        ArrayList<LineString> withinTimeEdges =
            this.getLinesAndSubEdgesWithinMaxTime(
                maxTime,
                allConnectingEdges,
                sptA,
                angleLimitForUShapeDetection,
                distanceToleranceForUShapeDetection,
                maxUserSpeed,
                usesCar,
                doSpeedTest);
        if (this.showTooFastEdgesAsDebugGeomsANDnotUShapes) {
          LOG.debug("displaying edges that are traversed too fast");
          this.debugGeoms = this.tooFastTraversedEdgeGeoms;
        } else {
          LOG.debug("displaying detected u-shaped roads/crescents");
        }
        LineString edges[] = new LineString[this.debugGeoms.size()];
        int k = 0;
        for (Iterator iterator = debugGeoms.iterator(); iterator.hasNext(); ) {
          LineString ls = (LineString) iterator.next();
          edges[k] = ls;
          k++;
        }
        Geometry mls = gf.createMultiLineString(edges);
        LOG.debug("write debug geom");
        json.writeGeom(mls);
        LOG.debug("done");
      }
    } catch (org.codehaus.jettison.json.JSONException e) {
      // TODO Auto-generated catch block
      e.printStackTrace();
    }
    return sw.toString();
  }

  /**
   * Creates a circle shape, using the JTS buffer algorithm. The method is used when there is no
   * street found within the given traveltime, e.g. when the pointer is placed on a field or in the
   * woods.<br>
   * TODO: Note it is actually not correct to do buffer calculation in Euclidian 2D, since the
   * resulting shape will be elliptical when projected.
   *
   * @param dropPoint the location given by the user
   * @param pathToStreet the path from the dropPoint to the street, used to retrieve the buffer
   *     distance
   * @return a Circle
   */
  private Geometry createCirle(Coordinate dropPoint, LineString pathToStreet) {
    double length = pathToStreet.getLength();
    GeometryFactory gf = new GeometryFactory();
    Point dp = gf.createPoint(dropPoint);
    Geometry buffer = dp.buffer(length);
    return buffer;
  }

  /**
   * Extraction of a sub-LineString from an existing line, starting from 0;
   *
   * @param ls the line from which we extract the sub LineString ()
   * @param fraction [0..1], the length until where we want the substring to go
   * @return the sub-LineString
   */
  LineString getSubLineString(LineString ls, double fraction) {
    if (fraction >= 1) return ls;
    LengthIndexedLine linRefLine = new LengthIndexedLine(ls);
    LineString subLine = (LineString) linRefLine.extractLine(0, fraction * ls.getLength());
    return subLine;
  }

  /**
   * Filters all input edges and returns all those as LineString geometries, that have at least one
   * end point within the time limits. If they have only one end point inside, then the sub-edge is
   * returned.
   *
   * @param maxTime the time limit in seconds that defines the size of the walkshed
   * @param allConnectingStateEdges all Edges that have been found to connect all states < maxTime
   * @param spt the ShortestPathTree generated for the pushpin drop point as origin
   * @param angleLimit the angle tolerance to detect roads with u-shapes, i.e. Pi/2 angles, in
   *     Radiant.
   * @param distanceTolerance in percent (e.g. 1.1 = 110%) for u-shape detection based on distance
   *     criteria
   * @param hasCar is travel mode by CAR?
   * @param performSpeedTest if true applies a test to each edge to check if the edge can be
   *     traversed in time. The test can detect u-shaped roads.
   * @return
   */
  ArrayList<LineString> getLinesAndSubEdgesWithinMaxTime(
      long maxTime,
      ArrayList<Edge> allConnectingStateEdges,
      ShortestPathTree spt,
      double angleLimit,
      double distanceTolerance,
      double userSpeed,
      boolean hasCar,
      boolean performSpeedTest) {

    LOG.debug("maximal userSpeed set to: " + userSpeed + " m/sec ");
    if (hasCar) {
      LOG.debug("travel mode is set to CAR, hence the given speed may be adjusted for each edge");
    }

    ArrayList<LineString> walkShedEdges = new ArrayList<LineString>();
    ArrayList<LineString> otherEdges = new ArrayList<LineString>();
    ArrayList<LineString> borderEdges = new ArrayList<LineString>();
    ArrayList<LineString> uShapes = new ArrayList<LineString>();
    int countEdgesOutside = 0;
    // -- determination of walkshed edges via edge states
    for (Iterator iterator = allConnectingStateEdges.iterator(); iterator.hasNext(); ) {
      Edge edge = (Edge) iterator.next();
      State sFrom = spt.getState(edge.getFromVertex());
      State sTo = spt.getState(edge.getToVertex());
      if ((sFrom != null) && (sTo != null)) {
        long fromTime = sFrom.getElapsedTimeSeconds();
        long toTime = sTo.getElapsedTimeSeconds();
        long dt = Math.abs(toTime - fromTime);
        Geometry edgeGeom = edge.getGeometry();
        if ((edgeGeom != null) && (edgeGeom instanceof LineString)) {
          LineString ls = (LineString) edgeGeom;
          // detect u-shape roads/crescents - they need to be treated separately
          boolean uShapeOrLonger =
              testForUshape(
                  edge,
                  maxTime,
                  fromTime,
                  toTime,
                  angleLimit,
                  distanceTolerance,
                  userSpeed,
                  hasCar,
                  performSpeedTest);
          if (uShapeOrLonger) {
            uShapes.add(ls);
          }

          // evaluate if an edge is completely within the time or only with one end
          if ((fromTime < maxTime) && (toTime < maxTime)) {
            // this one is within the time limit on both ends, however we need to do
            // a second test if we have a u-shaped road.
            if (uShapeOrLonger) {
              treatAndAddUshapeWithinTimeLimits(
                  maxTime, userSpeed, walkShedEdges, edge, fromTime, toTime, ls, hasCar);
            } else {
              walkShedEdges.add(ls);
            }
          } // end if:fromTime & toTime < maxTime
          else {
            // check if at least one end is inside, because then we need to
            // create the sub edge
            if ((fromTime < maxTime) || (toTime < maxTime)) {
              double lineDist = edge.getDistance();
              LineString inputLS = ls;
              double fraction = 1.0;
              if (fromTime < toTime) {
                double distanceToWalkInTimeMissing =
                    distanceToMoveInRemainingTime(
                        maxTime, fromTime, dt, userSpeed, edge, hasCar, uShapeOrLonger);
                fraction = (double) distanceToWalkInTimeMissing / (double) lineDist;
              } else {
                // toTime < fromTime : invert the edge direction
                inputLS = (LineString) ls.reverse();
                double distanceToWalkInTimeMissing =
                    distanceToMoveInRemainingTime(
                        maxTime, toTime, dt, userSpeed, edge, hasCar, uShapeOrLonger);
                fraction = (double) distanceToWalkInTimeMissing / (double) lineDist;
              }
              // get the subedge
              LineString subLine = this.getSubLineString(inputLS, fraction);
              borderEdges.add(subLine);
            } else {
              // this edge is completely outside - this should actually not happen
              // we will not do anything, just count
              countEdgesOutside++;
            }
          } // end else: fromTime & toTime < maxTime
        } // end if: edge instance of LineString
        else {
          // edge is not instance of LineString
          LOG.debug("edge not instance of LineString");
        }
      } // end if(sFrom && sTo != null) start Else
      else {
        // LOG.debug("could not retrieve state for edge-endpoint"); //for a 6min car ride, there can
        // be (too) many of such messages
        Geometry edgeGeom = edge.getGeometry();
        if ((edgeGeom != null) && (edgeGeom instanceof LineString)) {
          otherEdges.add((LineString) edgeGeom);
        }
      } // end else: sFrom && sTo != null
    } // end for loop over edges
    walkShedEdges.addAll(borderEdges);
    this.debugGeoms.addAll(uShapes);
    LOG.debug("number of detected u-shapes/crescents: " + uShapes.size());
    return walkShedEdges;
  }

  private void treatAndAddUshapeWithinTimeLimits(
      long maxTime,
      double userSpeed,
      ArrayList<LineString> walkShedEdges,
      Edge edge,
      long fromTime,
      long toTime,
      LineString ls,
      boolean hasCar) {

    // check if the u-shape can be traveled within the remaining time
    long dt = Math.abs(toTime - fromTime);
    double distanceToMoveInTimeMissing =
        distanceToMoveInRemainingTime(maxTime, fromTime, dt, userSpeed, edge, hasCar, true);
    double lineDist = edge.getDistance();
    double fraction = (double) distanceToMoveInTimeMissing / (double) lineDist;
    // get the sub-edge geom
    LineString subLine = null;
    if (fraction < 1.0) {
      // the u-shape is not fully walkable in maxTime
      subLine = this.getSubLineString(ls, fraction);
      walkShedEdges.add(subLine);
      // if it is smaller we need also to calculate the LS from the other side
      LineString reversedLine = (LineString) ls.reverse();
      double distanceToMoveInTimeMissing2 =
          distanceToMoveInRemainingTime(maxTime, toTime, dt, userSpeed, edge, hasCar, true);
      double fraction2 = (double) distanceToMoveInTimeMissing2 / (double) lineDist;
      LineString secondsubLine = this.getSubLineString(reversedLine, fraction2);
      ;
      walkShedEdges.add(secondsubLine);
    } else { // the whole u-shape is within the time
      // add only once
      walkShedEdges.add(ls);
    }
  }

  private boolean testForUshape(
      Edge edge,
      long maxTime,
      long fromTime,
      long toTime,
      double angleLimit,
      double distanceTolerance,
      double userSpeed,
      boolean hasCar,
      boolean performSpeedTest) {

    LineString ls = (LineString) edge.getGeometry();
    if (ls.getNumPoints() <= 3) { // first filter since u-shapes need at least 4 pts
      // this is the normal case
      return false;
    } else {
      // try to identify u-shapes by checking if the angle EndPoint-StartPoint-StartPoint+1
      // is about 90 degrees (using Azimuths on the sphere)
      double diffTo90Azimuths = 360;
      if (edge instanceof PlainStreetEdge) {
        double firstSegmentAngle = DirectionUtils.getFirstAngle(edge.getGeometry());
        if (firstSegmentAngle < 0) firstSegmentAngle = firstSegmentAngle + Math.PI;
        double firstToLastSegmentAngle = getFirstToLastSegmentAngle(edge.getGeometry());
        if (firstToLastSegmentAngle < 0)
          firstToLastSegmentAngle = firstToLastSegmentAngle + Math.PI;
        double diffAzimuths = Math.abs(firstToLastSegmentAngle - firstSegmentAngle);
        diffTo90Azimuths = Math.abs(diffAzimuths - (Math.PI / 2.0));
      } else {
        // this will happen in particular for transit routes
        // LOG.debug("Edge is not a PlainStreetEdge");
      }
      if (diffTo90Azimuths < angleLimit) {
        // no need to test further if we know its a u-shape
        // System.out.println("u-shape found, (spherical) angle: " + diffTo90Azimuths* 180/Math.PI);
        return true;
      } else {
        if (performSpeedTest) {
          // Use also a distance based criteria since the angle criteria may fail.
          // However a distance based one may fail as well for steep terrain.
          long dt = Math.abs(toTime - fromTime);
          double lineDist = edge.getDistance();
          double distanceToWalkInTimeMissing =
              distanceToMoveInRemainingTime(maxTime, fromTime, dt, userSpeed, edge, hasCar, false);
          double approxWalkableDistanceInTime = distanceToWalkInTimeMissing * distanceTolerance;
          if ((approxWalkableDistanceInTime < lineDist)) {
            return true;
          }
        }
        return false;
      }
    }
  }

  /**
   * Calculates what distance can be traveled with the remaining time and given speeds. For car use
   * the speed limit is taken from the edge itself. Slopes are accounted for when walking and
   * biking. A minimal slope of 0.06 (6m/100m) is necessary.
   *
   * @param maxTime in sec, the time we have left
   * @param fromTime in sec, the time when we enter the edge
   * @param traverseTime in sec, original edge traverse time needed to adjust the speed based
   *     calculation to slope effects
   * @param userSpeed in m/sec, dependent on traversal mode
   * @param edge the edge itself (used to the get the speed in car mode)
   * @param usesCar if we traverse the edge in car mode
   * @param hasUshape if know, indicate if the edge has a u-shape
   * @return the distance in meter that can be moved until maxTime
   */
  double distanceToMoveInRemainingTime(
      long maxTime,
      long fromTime,
      double traverseTime,
      double userSpeed,
      Edge edge,
      boolean usesCar,
      boolean hasUshape) {

    boolean isTooFast = false;
    String msg = "";

    double originalTravelSpeed =
        edge.getDistance() / traverseTime; // this may be wrong for u-shapes

    if (originalTravelSpeed < userSpeed) {
      // we may have slope effects
      if (edge instanceof PlainStreetEdge) {
        PlainStreetEdge pe = (PlainStreetEdge) edge;
        double maxSlope = pe.getElevationProfileSegment().getMaxSlope();
        // if we are over the slope limit, then we should use the slower speed
        if (maxSlope > 0.06) { // limit 6m/100m = 3.4 degree
          userSpeed = originalTravelSpeed;
        }
      }
    } else {
      // in this case we may have a u-shape, or the user speeds are too small, or something else.
      double vdiff = Math.abs(originalTravelSpeed - userSpeed);
      double vDiffPercent = vdiff / (userSpeed / 100.0);
      if (vDiffPercent > 20) {
        isTooFast = true;
        // [sstein Dec 2012]: Note, it seems like most of these edges are indeed of u-shape type,
        // i.e. small roads that come from and return from (the same) main road
        msg =
            "v_traversed is much faster than (allowed) v_user, edgeName: "
                + edge.getName()
                + ", >>> (in m/s): v_traversed="
                + (int) Math.floor(originalTravelSpeed)
                + ", v_maxUser="******", known u-shape, ";
        }
        if ((usesCar == false) && (hasUshape == false)) {
          this.tooFastTraversedEdgeGeoms.add(edge.getGeometry());
          LOG.debug(msg);
        } // otherwise we print msg below
      }
    }
    // correct speed for car use, as each road has its speed limits
    if (usesCar) {
      if (edge instanceof PlainStreetEdge) {
        PlainStreetEdge pe = (PlainStreetEdge) edge;
        userSpeed = pe.getCarSpeed();
        // we need to check again if the originalTravelSpeed is faster
        if ((isTooFast == true) && (originalTravelSpeed > userSpeed) && (hasUshape == false)) {
          this.tooFastTraversedEdgeGeoms.add(edge.getGeometry());
          LOG.debug(msg + "; setting v_PlainStreetEdge=" + (int) Math.floor(userSpeed));
        }
      }
    }
    // finally calculate how far we can travel with the remaining time
    long timeMissing = maxTime - fromTime;
    double distanceToWalkInTimeMissing = timeMissing * userSpeed;
    return distanceToWalkInTimeMissing;
  }

  private GeodeticCalculator geodeticCalculator = new GeodeticCalculator();

  /**
   * 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;
  }
}
示例#4
0
public class Raptor implements PathService {
  private static final Logger log = LoggerFactory.getLogger(Raptor.class);

  static final double MAX_TRANSIT_SPEED = 25;

  private static final int MAX_WALK_MULTIPLE = 8;

  public static final double WALK_EPSILON = 1.10;

  @Autowired private GraphService graphService;

  private List<ServiceDay> cachedServiceDays;

  private RaptorData cachedRaptorData;

  private double multiPathTimeout = 0; // seconds

  /** This is used for short paths (under shortPathCutoff). */
  RetryingPathServiceImpl shortPathService = new RetryingPathServiceImpl();

  /** The max length, in meters, that will use the shortPathService. */
  private double shortPathCutoff = 10000;

  @PostConstruct
  public void setup() {
    shortPathService.setGraphService(graphService);
    shortPathService.setSptService(sptService);
  }

  /**
   * Stop searching for additional itineraries (beyond the first one) after this many seconds have
   * elapsed, relative to the beginning of the search for the first itinerary. A negative or zero
   * value means search forever.
   */
  public void setMultiPathTimeout(double seconds) {
    multiPathTimeout = seconds;
  }

  // fallback for nontransit trips
  @Autowired public SPTService sptService;

  private DistanceLibrary distanceLibrary = SphericalDistanceLibrary.getInstance();

  @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;
  }

  private void collectRoutesUsed(
      RaptorData data, RoutingRequest options, List<RaptorState> targetStates) {
    // find start/end regions
    List<Integer> startRegions = getRegionsForVertex(data.regionData, options.rctx.fromVertex);
    int startRegion;
    if (startRegions.size() == 1) {
      startRegion = startRegions.get(0);
    } else {
      // on boundary
      return;
    }
    List<Integer> endRegions = getRegionsForVertex(data.regionData, options.rctx.toVertex);
    int endRegion;
    if (endRegions.size() == 1) {
      endRegion = endRegions.get(0);
    } else {
      // on boundary
      return;
    }

    HashSet<RaptorRoute> routes = data.regionData.routes[startRegion][endRegion];
    HashSet<RaptorStop> stops = data.regionData.stops[startRegion][endRegion];
    TARGETSTATE:
    for (RaptorState state : targetStates) {
      for (RaptorState dom : targetStates) {
        if (dom.nBoardings <= state.nBoardings && dom.arrivalTime < state.arrivalTime) {
          continue TARGETSTATE;
        }
      }
      synchronized (data) {
        while (state != null) {
          if (state.route != null) routes.add(state.route);
          if (state.stop != null) {
            stops.add(state.stop);
          }
          state = state.getParent();
        }
      }
    }
  }

  /**
   * This does preliminary search over just routes and stops that have been used in the past between
   * these regions.
   */
  private int preliminaryRaptorSearch(
      RaptorData data, RoutingRequest options, RoutingRequest walkOptions, RaptorSearch search) {
    // find start/end regions
    List<Integer> startRegions = getRegionsForVertex(data.regionData, options.rctx.fromVertex);
    int startRegion;
    // for trips that span regions, we can safely pick either region
    startRegion = startRegions.get(0);
    List<Integer> endRegions = getRegionsForVertex(data.regionData, options.rctx.toVertex);
    int endRegion;
    endRegion = endRegions.get(0);

    // create a reduced set of RaptorData with only the stops/routes previously seen on trips
    // from the start region to the end region
    RaptorData trimmedData = new RaptorData();
    trimmedData.raptorStopsForStopId = new HashMap<AgencyAndId, RaptorStop>();
    HashSet<RaptorStop> stops = data.regionData.stops[startRegion][endRegion];
    for (RaptorStop stop : stops) {
      trimmedData.raptorStopsForStopId.put(stop.stopVertex.getStopId(), stop);
    }

    trimmedData.regionData = data.regionData;
    trimmedData.routes = data.regionData.routes[startRegion][endRegion];
    trimmedData.stops = data.stops;
    // trimmedData.allowedStops = stops;
    trimmedData.routesForStop = data.routesForStop;

    double walkDistance = options.getMaxWalkDistance();
    options = options.clone();
    walkOptions = walkOptions.clone();
    if (walkDistance > 4000) {
      // this is a really long walk. We'll almost never actually need this. So let's do our
      // preliminary search over just 4km first.
      options.setMaxWalkDistance(4000);
      walkOptions.setMaxWalkDistance(4000);
    }

    int round;
    if (trimmedData.routes.size() > 0) {
      log.debug(
          "Doing preliminary search on limited route set ("
              + trimmedData.routes.size()
              + ", "
              + stops.size()
              + ")");
      round = doPreliminarySearch(options, walkOptions, search, trimmedData);
    } else {
      round = 0;
    }

    if (search.getTargetStates().size() == 0 && walkDistance > 5000) {
      // nothing found in preliminary search
      // so we'll do a search with full set of routes & stops, but still limited distance
      log.debug("Doing preliminary search at limited distance");
      round = doPreliminarySearch(options, walkOptions, search, data);
    }

    return round;
  }

  private int doPreliminarySearch(
      RoutingRequest options,
      RoutingRequest walkOptions,
      RaptorSearch search,
      RaptorData trimmedData) {
    RaptorSearch rushSearch = new RaptorSearch(trimmedData, options);
    int bestElapsedTime = Integer.MAX_VALUE;
    int round;
    for (round = 0; round < options.getMaxTransfers() + 2; round++) {
      if (!round(trimmedData, options, walkOptions, rushSearch, round)) break;

      if (rushSearch.getTargetStates().size() > 0) {
        int oldBest = bestElapsedTime;
        for (RaptorState state : rushSearch.getTargetStates()) {
          final int elapsedTime = (int) Math.abs(state.arrivalTime - options.dateTime);
          if (elapsedTime < bestElapsedTime) {
            bestElapsedTime = elapsedTime;
          }
        }
        int improvement = oldBest - bestElapsedTime;
        if (improvement < 600) break;
      }
    }
    for (RaptorState state : rushSearch.getTargetStates()) {
      search.bounder.addBounder(state.walkPath);
      search.addTargetState(state);
    }
    return round;
  }

  /**
   * Some vertices aren't associated with a region, because they're synthetic, or maybe for some
   * other region. So instead, we check their connected vertices, recursively, to try to find their
   * region.
   *
   * @param regionData
   * @param vertex
   * @return
   */
  static List<Integer> getRegionsForVertex(RegionData regionData, Vertex vertex) {
    return new ArrayList<Integer>(
        getRegionsForVertex(regionData, vertex, new HashSet<Vertex>(), 0));
  }

  /**
   * Internals of getRegionsForVertex; keeps track of seen vertices to avoid loops.
   *
   * @param regionData
   * @param vertex
   * @param seen
   * @param depth
   * @return
   */
  private static HashSet<Integer> getRegionsForVertex(
      RegionData regionData, Vertex vertex, HashSet<Vertex> seen, int depth) {
    seen.add(vertex);
    HashSet<Integer> regions = new HashSet<Integer>();
    int region = vertex.getGroupIndex();
    if (region >= 0) {
      regions.add(region);
    } else {
      for (Edge e : vertex.getOutgoing()) {
        final Vertex tov = e.getToVertex();
        if (!seen.contains(tov))
          regions.addAll(getRegionsForVertex(regionData, tov, seen, depth + 1));
      }
      for (Edge e : vertex.getIncoming()) {
        final Vertex fromv = e.getFromVertex();
        if (!seen.contains(fromv))
          regions.addAll(getRegionsForVertex(regionData, fromv, seen, depth + 1));
      }
    }
    return regions;
  }

  private State getState(RoutingRequest options, RaptorData data, ArrayList<RaptorState> states) {
    if (options.arriveBy) {
      return getStateArriveBy(data, states);
    } else {
      return getStateDepartAt(data, states);
    }
  }

  private State getStateDepartAt(RaptorData data, ArrayList<RaptorState> states) {
    State state = new State(states.get(0).getRequest());
    for (int i = states.size() - 1; i >= 0; --i) {
      RaptorState cur = states.get(i);
      if (cur.walkPath != null) { // a walking step
        GraphPath path = new GraphPath(cur.walkPath, false);
        for (Edge e : path.edges) {
          State oldState = state;
          state = e.traverse(state);
          if (state == null) {
            e.traverse(oldState);
          }
        }
      } else {
        // so, cur is at this point at a transit stop; we have a route to board
        if (cur.getParent() == null || !cur.getParent().interlining) {
          for (Edge e : state.getVertex().getOutgoing()) {
            if (e instanceof PreBoardEdge) {
              state = e.traverse(state);
              break;
            }
          }
          TransitBoardAlight board = cur.getRoute().boards[cur.boardStopSequence][cur.patternIndex];
          state = board.traverse(state);
        }
        // now traverse the hops and dwells until we find the alight we're looking for
        HOP:
        while (true) {
          for (Edge e : state.getVertex().getOutgoing()) {
            if (e instanceof PatternDwell) {
              state = e.traverse(state);
            } else if (e instanceof PatternHop) {
              state = e.traverse(state);
              if (cur.interlining) {
                for (Edge e2 : state.getVertex().getOutgoing()) {
                  RaptorState next = states.get(i - 1);
                  if (e2 instanceof PatternInterlineDwell) {
                    Stop toStop = ((TransitVertex) e2.getToVertex()).getStop();
                    Stop expectedStop = next.boardStop.stopVertex.getStop();
                    if (toStop.equals(expectedStop)) {
                      State newState = e2.traverse(state);
                      if (newState == null) continue;
                      if (newState.getTripId() != next.tripId) continue;
                      state = newState;
                      break HOP;
                    }
                  }
                }
              } else {
                for (Edge e2 : state.getVertex().getOutgoing()) {
                  if (e2 instanceof TransitBoardAlight) {
                    for (Edge e3 : e2.getToVertex().getOutgoing()) {
                      if (e3 instanceof PreAlightEdge) {
                        if (data.raptorStopsForStopId.get(
                                ((TransitStop) e3.getToVertex()).getStopId())
                            == cur.stop) {
                          state = e2.traverse(state);
                          state = e3.traverse(state);
                          break HOP;
                        }
                      }
                    }
                  }
                }
              }
            }
          }
        }
      }
    }
    return state;
  }

  private State getStateArriveBy(RaptorData data, ArrayList<RaptorState> states) {
    RoutingRequest options = states.get(0).getRequest();
    State state = new State(options.rctx.origin, options);
    for (int i = states.size() - 1; i >= 0; --i) {
      RaptorState cur = states.get(i);
      if (cur.walkPath != null) {
        GraphPath path = new GraphPath(cur.walkPath, false);
        for (ListIterator<Edge> it = path.edges.listIterator(path.edges.size());
            it.hasPrevious(); ) {
          Edge e = it.previous();
          State oldState = state;
          state = e.traverse(state);
          if (state == null) {
            e.traverse(oldState);
          }
        }
      } else {
        // so, cur is at this point at a transit stop; we have a route to alight from
        if (cur.getParent() == null || !cur.getParent().interlining) {
          for (Edge e : state.getVertex().getIncoming()) {
            if (e instanceof PreAlightEdge) {
              state = e.traverse(state);
            }
          }
          TransitBoardAlight alight =
              cur.getRoute().alights[cur.boardStopSequence - 1][cur.patternIndex];
          State oldState = state;
          state = alight.traverse(state);
          if (state == null) {
            state = alight.traverse(oldState);
          }
        }
        // now traverse the hops and dwells until we find the board we're looking for
        HOP:
        while (true) {
          for (Edge e : state.getVertex().getIncoming()) {
            if (e instanceof PatternDwell) {
              state = e.traverse(state);
            } else if (e instanceof PatternHop) {
              state = e.traverse(state);
              if (cur.interlining) {
                for (Edge e2 : state.getVertex().getIncoming()) {
                  RaptorState next = states.get(i - 1);
                  if (e2 instanceof PatternInterlineDwell) {
                    Stop fromStop = ((TransitVertex) e2.getFromVertex()).getStop();
                    Stop expectedStop = next.boardStop.stopVertex.getStop();
                    if (fromStop.equals(expectedStop)) {
                      State newState = e2.traverse(state);
                      if (newState == null) continue;
                      if (newState.getTripId() != next.tripId) continue;
                      state = newState;
                      break HOP;
                    }
                  }
                }
              } else {
                for (Edge e2 : state.getVertex().getIncoming()) {
                  if (e2 instanceof TransitBoardAlight) {
                    for (Edge e3 : e2.getFromVertex().getIncoming()) {
                      if (e3 instanceof PreBoardEdge) {
                        if (data.raptorStopsForStopId.get(
                                ((TransitStop) e3.getFromVertex()).getStopId())
                            == cur.stop) {
                          state = e2.traverse(state);
                          state = e3.traverse(state);
                          break HOP;
                        }
                      }
                    }
                  }
                }
              }
            }
          }
        }
      }
    }
    return state;
  }

  /**
   * Prune raptor data to include only routes and boardings which have trips today. Doesn't actually
   * improve speed
   */
  @SuppressWarnings("unchecked")
  private RaptorData pruneDataForServiceDays(Graph graph, ArrayList<ServiceDay> serviceDays) {

    if (serviceDays.equals(cachedServiceDays)) return cachedRaptorData;
    RaptorData data = graph.getService(RaptorDataService.class).getData();
    RaptorData pruned = new RaptorData();
    pruned.raptorStopsForStopId = data.raptorStopsForStopId;
    pruned.stops = data.stops;
    pruned.routes = new ArrayList<RaptorRoute>();
    pruned.routesForStop = new List[pruned.stops.length];

    for (RaptorRoute route : data.routes) {
      ArrayList<Integer> keep = new ArrayList<Integer>();

      for (int i = 0; i < route.boards[0].length; ++i) {
        Edge board = route.boards[0][i];
        int serviceId;
        if (board instanceof TransitBoardAlight) {
          serviceId = ((TransitBoardAlight) board).getPattern().getServiceId();
        } else {
          log.debug("Unexpected nonboard among boards");
          continue;
        }
        for (ServiceDay day : serviceDays) {
          if (day.serviceIdRunning(serviceId)) {
            keep.add(i);
            break;
          }
        }
      }
      if (keep.isEmpty()) continue;
      int nPatterns = keep.size();
      RaptorRoute prunedRoute = new RaptorRoute(route.getNStops(), nPatterns);
      for (int stop = 0; stop < route.getNStops() - 1; ++stop) {
        for (int pattern = 0; pattern < nPatterns; ++pattern) {
          prunedRoute.boards[stop][pattern] = route.boards[stop][keep.get(pattern)];
        }
      }
      pruned.routes.add(route);
      for (RaptorStop stop : route.stops) {
        List<RaptorRoute> routes = pruned.routesForStop[stop.index];
        if (routes == null) {
          routes = new ArrayList<RaptorRoute>();
          pruned.routesForStop[stop.index] = routes;
        }
        routes.add(route);
      }
    }
    for (RaptorStop stop : data.stops) {
      if (pruned.routesForStop[stop.index] == null) {
        pruned.routesForStop[stop.index] = Collections.emptyList();
      }
    }
    cachedServiceDays = serviceDays;
    cachedRaptorData = pruned;
    return pruned;
  }

  private boolean round(
      RaptorData data,
      RoutingRequest options,
      RoutingRequest walkOptions,
      final RaptorSearch search,
      int nBoardings) {

    log.debug("Round " + nBoardings);

    /* Phase 2: handle transit */
    List<RaptorState> createdStates = search.transitPhase(options, nBoardings);

    /* Phase 3: handle walking paths */

    return search.walkPhase(options, walkOptions, nBoardings, createdStates);
  }

  public RaptorStateSet getStateSet(RoutingRequest options) {

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

    RaptorData data = graph.getService(RaptorDataService.class).getData();

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

    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);

    for (int i = 0; i < options.getMaxTransfers() + 2; ++i) {
      if (!round(data, options, walkOptions, search, i)) break;
    }
    RaptorStateSet result = new RaptorStateSet();
    result.statesByStop = search.statesByStop;
    return result;
  }

  public double getShortPathCutoff() {
    return shortPathCutoff;
  }

  public void setShortPathCutoff(double shortPathCutoff) {
    this.shortPathCutoff = shortPathCutoff;
  }
}
public class TargetBound
    implements SearchTerminationStrategy,
        SkipTraverseResultStrategy,
        RemainingWeightHeuristic,
        ShortestPathTreeFactory {

  private static final long serialVersionUID = -5296036164138922096L;

  private static final long WORST_TIME_DIFFERENCE = 3600;

  private static final double WORST_WEIGHT_DIFFERENCE_FACTOR = 1.3;

  List<State> bounders;

  private Vertex realTarget;

  private DistanceLibrary distanceLibrary = SphericalDistanceLibrary.getInstance();

  private Coordinate realTargetCoordinate;

  private double distanceToNearestTransitStop;

  private TransitLocalStreetService transitLocalStreets;

  private double speedUpperBound;

  // this is saved so that it can be reused in various skipping functions
  private double targetDistance;

  private double speedWeight;

  /**
   * How much longer the worst path can be than the best in terms of time. Setting this lower will
   * cut off some less-walking more-time paths. Setting it higher will slow down the search a lot.
   */
  private double timeBoundFactor = 2;

  private List<Integer> previousArrivalTime = new ArrayList<Integer>();

  private RoutingRequest options;

  public ShortestPathTree spt = new ArrayMultiShortestPathTree(options);

  double[] distance = new double[AbstractVertex.getMaxIndex()];

  public double bestTargetDistance = Double.POSITIVE_INFINITY;

  public List<State> removedBoundingStates = new ArrayList<State>();

  private List<State> transitStopsVisited = new ArrayList<State>();

  private double transferTimeInWalkDistance;

  public TargetBound(RoutingRequest options) {
    this.options = options;
    if (options.rctx.target != null) {
      this.realTarget = options.rctx.target;
      this.realTargetCoordinate = realTarget.getCoordinate();
      this.distanceToNearestTransitStop = realTarget.getDistanceToNearestTransitStop();
      bounders = new ArrayList<State>();
      transitLocalStreets = options.rctx.graph.getService(TransitLocalStreetService.class);
      speedUpperBound = options.getSpeedUpperBound();
      this.speedWeight = options.getWalkReluctance() / speedUpperBound;
      this.transferTimeInWalkDistance = options.getTransferSlack() / options.getWalkSpeed();
    }
  }

  @Override
  public boolean shouldSearchContinue(
      Vertex origin,
      Vertex target,
      State current,
      ShortestPathTree spt,
      RoutingRequest traverseOptions) {
    final Vertex vertex = current.getVertex();
    if (vertex instanceof TransitStop
        || vertex instanceof TransitStopDepart
        || vertex instanceof TransitStopArrive) {
      transitStopsVisited.add(current);
    }
    if (vertex == realTarget) {
      addBounder(current);
    }
    return true;
  }

  public void addBounder(State bounder) {
    for (Iterator<State> it = bounders.iterator(); it.hasNext(); ) {
      State old = it.next();

      // exact dup
      if (old.getNumBoardings() == bounder.getNumBoardings()
          && old.getTime() == bounder.getTime()
          && old.getWalkDistance() == bounder.getWalkDistance()) return;
      if (bounder.dominates(old)) {
        it.remove();
        removedBoundingStates.add(old);
      } else if (bounder.getNumBoardings() <= old.getNumBoardings() && options.arriveBy
          ? (bounder.getTime() - WORST_TIME_DIFFERENCE > old.getTime())
          : (bounder.getTime() + WORST_TIME_DIFFERENCE < old.getTime())) {
        it.remove();
        removedBoundingStates.add(old);
      }
    }
    bounders.add(bounder);
    RaptorState state = (RaptorState) bounder.getExtension("raptorParent");
    if (state == null) {
      previousArrivalTime.add(-1);
      return;
    }
    RaptorStop stop = state.stop;
    // get previous alight at stop
    if (options.isArriveBy()) {
      final int nextDepartTime =
          getNextDepartTime(
              options, (state.arrivalTime - options.getBoardSlack()) - 2, stop.stopVertex);
      previousArrivalTime.add(
          (int) ((nextDepartTime - options.getAlightSlack()) - bounder.getElapsedTime()));
    } else {
      final int previousArriveTime =
          getPreviousArriveTime(
              options, state.arrivalTime - options.getAlightSlack() + 2, stop.stopVertex);
      previousArrivalTime.add(
          (int) (previousArriveTime + options.getAlightSlack() + bounder.getElapsedTime()));
    }
  }

  @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;
  }

  public static int getNextDepartTime(
      RoutingRequest request, int departureTime, Vertex stopVertex) {

    int bestArrivalTime = Integer.MAX_VALUE;

    request.arriveBy = false;

    // find the boards
    for (Edge preboard : stopVertex.getOutgoing()) {
      if (preboard instanceof PreBoardEdge) {
        Vertex departure = preboard.getToVertex(); // this is the departure vertex
        for (Edge board : departure.getOutgoing()) {
          if (board instanceof TransitBoardAlight) {
            State state = new State(board.getFromVertex(), departureTime, request);
            State result = board.traverse(state);
            if (result == null) continue;
            int time = (int) result.getTime();
            if (time < bestArrivalTime) {
              bestArrivalTime = time;
            }
          }
        }
      }
    }

    request.arriveBy = true;
    return bestArrivalTime;
  }

  public static int getPreviousArriveTime(
      RoutingRequest request, int arrivalTime, Vertex stopVertex) {

    int bestArrivalTime = -1;

    request.arriveBy = true;

    // find the alights
    for (Edge prealight : stopVertex.getIncoming()) {
      if (prealight instanceof PreAlightEdge) {
        Vertex arrival = prealight.getFromVertex(); // this is the arrival vertex
        for (Edge alight : arrival.getIncoming()) {
          if (alight instanceof TransitBoardAlight) {
            State state = new State(alight.getToVertex(), arrivalTime, request);
            State result = alight.traverse(state);
            if (result == null) continue;
            int time = (int) result.getTime();
            if (time > bestArrivalTime) {
              bestArrivalTime = time;
            }
          }
        }
      }
    }

    request.arriveBy = false;
    return bestArrivalTime;
  }

  @Override
  public double computeInitialWeight(State s, Vertex target) {
    return computeForwardWeight(s, target);
  }

  /**
   * This actually does have to be admissible, since when we find the target, it used to bound the
   * rest of the search.
   */
  @Override
  public double computeForwardWeight(State s, Vertex target) {
    return targetDistance * speedWeight;
  }

  @Override
  public double computeReverseWeight(State s, Vertex target) {
    return computeForwardWeight(s, target);
  }

  /** Reset the heuristic */
  @Override
  public void reset() {}

  public double getTimeBoundFactor() {
    return timeBoundFactor;
  }

  public void setTimeBoundFactor(double timeBoundFactor) {
    this.timeBoundFactor = timeBoundFactor;
  }

  @Override
  public ShortestPathTree create(RoutingRequest options) {
    return spt;
  }

  public void addSptStates(List<MaxWalkState> states) {
    for (MaxWalkState state : states) {
      if (state.getVertex() instanceof TransitStop) {
        transitStopsVisited.add(state);
      }
      spt.add(state);
    }
  }

  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());
    }
  }

  public List<State> getTransitStopsVisited() {
    return transitStopsVisited;
  }

  public void prepareForSearch() {
    transitStopsVisited.clear();
  }

  public void reset(RoutingRequest options) {
    this.options = options;
    if (realTarget != options.rctx.target) {
      this.realTarget = options.rctx.target;
      this.realTargetCoordinate = realTarget.getCoordinate();
      this.distanceToNearestTransitStop = realTarget.getDistanceToNearestTransitStop();
      bounders = new ArrayList<State>();
      Arrays.fill(distance, -1);
    }
    spt = new ArrayMultiShortestPathTree(options);
    transitLocalStreets = options.rctx.graph.getService(TransitLocalStreetService.class);
    speedUpperBound = options.getSpeedUpperBound();
    this.speedWeight = options.getWalkReluctance() / speedUpperBound;
  }
}
 public double getDistance() {
   return SphericalDistanceLibrary.getInstance()
       .distance(start.getLat(), start.getLon(), end.getLat(), end.getLon());
 }
/** {@link GraphBuilder} plugin that links up the stops of a transit network among themselves. */
public class StreetfulStopLinker implements GraphBuilder {
  private static Logger LOG = LoggerFactory.getLogger(StreetfulStopLinker.class);

  int maxDuration = 60 * 10;

  DistanceLibrary distanceLibrary = SphericalDistanceLibrary.getInstance();

  public List<String> provides() {
    return Arrays.asList("linking");
  }

  public List<String> getPrerequisites() {
    return Arrays.asList("street to transit");
  }

  @Override
  public void buildGraph(Graph graph, HashMap<Class<?>, Object> extra) {
    final Parser parser[] = new Parser[] {new Parser()};
    GeometryFactory geometryFactory = GeometryUtils.getGeometryFactory();
    EarliestArrivalSPTService earliestArrivalSPTService = new EarliestArrivalSPTService();
    earliestArrivalSPTService.maxDuration = (maxDuration);

    for (TransitStop ts : Iterables.filter(graph.getVertices(), TransitStop.class)) {
      // Only link street linkable stops
      if (!ts.isStreetLinkable()) continue;
      LOG.trace("linking stop '{}' {}", ts.getStop(), ts);

      // Determine the set of pathway/transfer destinations
      Set<TransitStop> pathwayDestinations = new HashSet<TransitStop>();
      for (Edge e : ts.getOutgoing()) {
        if (e instanceof PathwayEdge || e instanceof SimpleTransfer) {
          if (e.getToVertex() instanceof TransitStop) {
            TransitStop to = (TransitStop) e.getToVertex();
            pathwayDestinations.add(to);
          }
        }
      }

      int n = 0;
      RoutingRequest routingRequest = new RoutingRequest(TraverseMode.WALK);
      routingRequest.clampInitialWait = (0L);
      routingRequest.setRoutingContext(graph, ts, null);
      routingRequest.rctx.pathParsers = parser;
      ShortestPathTree spt = earliestArrivalSPTService.getShortestPathTree(routingRequest);

      if (spt != null) {
        for (State state : spt.getAllStates()) {
          Vertex vertex = state.getVertex();
          if (ts == vertex) continue;

          if (vertex instanceof TransitStop) {
            TransitStop other = (TransitStop) vertex;
            if (!other.isStreetLinkable()) continue;
            if (pathwayDestinations.contains(other)) {
              LOG.trace("Skipping '{}', {}, already connected.", other.getStop(), other);
              continue;
            }
            double distance = 0.0;
            GraphPath graphPath = new GraphPath(state, false);
            CoordinateArrayListSequence coordinates = new CoordinateArrayListSequence();

            for (Edge edge : graphPath.edges) {
              if (edge instanceof StreetEdge) {
                LineString geometry = edge.getGeometry();

                if (geometry != null) {
                  if (coordinates.size() == 0) {
                    coordinates.extend(geometry.getCoordinates());
                  } else {
                    coordinates.extend(geometry.getCoordinates(), 1);
                  }
                }

                distance += edge.getDistance();
              }
            }

            if (coordinates.size() < 2) { // Otherwise the walk step generator breaks.
              ArrayList<Coordinate> coordinateList = new ArrayList<Coordinate>(2);
              coordinateList.add(graphPath.states.get(1).getVertex().getCoordinate());
              State lastState = graphPath.states.getLast().getBackState();
              coordinateList.add(lastState.getVertex().getCoordinate());
              coordinates = new CoordinateArrayListSequence(coordinateList);
            }

            LineString geometry =
                geometryFactory.createLineString(
                    new PackedCoordinateSequence.Double(coordinates.toCoordinateArray()));
            LOG.trace("  to stop: '{}' {} ({}m) [{}]", other.getStop(), other, distance, geometry);
            new SimpleTransfer(ts, other, distance, geometry);
            n++;
          }
        }
      }

      LOG.trace("linked to {} others.", n);
      if (n == 0) {
        LOG.warn(graph.addBuilderAnnotation(new StopNotLinkedForTransfers(ts)));
      }
    }
  }

  @Override
  public void checkInputs() {
    // No inputs
  }

  private static class Parser extends PathParser {
    private static final int OTHER = 0;
    private static final int STREET = 1;
    private static final int LINK = 2;

    private final DFA DFA;

    Parser() {
      Nonterminal streets = star(STREET);

      Nonterminal itinerary = seq(LINK, streets, LINK);

      DFA = itinerary.toDFA().minimize();
    }

    @Override
    public int terminalFor(State state) {
      Edge edge = state.getBackEdge();

      if (edge instanceof StreetEdge) return STREET;
      if (edge instanceof StreetTransitLink) return LINK;

      return OTHER;
    }

    @Override
    protected DFA getDFA() {
      return this.DFA;
    }
  }
}
// @Component
public class MultiObjectivePathServiceImpl implements PathService {

  @Autowired public GraphService graphService;

  private static final Logger LOG = LoggerFactory.getLogger(MultiObjectivePathServiceImpl.class);

  private static final MonitoringStore store = MonitoringStoreFactory.getStore();

  private static final double MAX_WALK = 100000;

  private double[] _timeouts = new double[] {4, 2, 0.6, 0.4}; // seconds

  private double _maxPaths = 4;

  private TraverseVisitor traverseVisitor;

  private DistanceLibrary distanceLibrary = SphericalDistanceLibrary.getInstance();

  /** Give up on searching for itineraries after this many seconds have elapsed. */
  public void setTimeouts(List<Double> timeouts) {
    _timeouts = new double[timeouts.size()];
    int i = 0;
    for (Double d : timeouts) _timeouts[i++] = d;
  }

  public void setMaxPaths(double numPaths) {
    _maxPaths = numPaths;
  }

  public void setTraverseVisitor(TraverseVisitor traverseVisitor) {
    this.traverseVisitor = traverseVisitor;
  }

  @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;
  }

  private void storeMemory() {
    if (store.isMonitoring("memoryUsed")) {
      System.gc();
      long memoryUsed = Runtime.getRuntime().totalMemory() - Runtime.getRuntime().freeMemory();
      store.setLongMax("memoryUsed", memoryUsed);
    }
  }

  //    private boolean eDominates(State s0, State s1) {
  //        final double EPSILON = 0.05;
  //        return s0.getWeight() <= s1.getWeight() * (1 + EPSILON) &&
  //               s0.getTime() <= s1.getTime() * (1 + EPSILON) &&
  //               s0.getWalkDistance() <= s1.getWalkDistance() * (1 + EPSILON) &&
  //               s0.getNumBoardings() <= s1.getNumBoardings();
  //    }

  // TODO: move into an epsilon-dominance shortest path tree
  private boolean eDominates(State s0, State s1) {
    final double EPSILON = 0.05;
    if (s0.similarRouteSequence(s1)) {
      return s0.getWeight() <= s1.getWeight() * (1 + EPSILON)
          && s0.getElapsedTime() <= s1.getElapsedTime() * (1 + EPSILON)
          && s0.getWalkDistance() <= s1.getWalkDistance() * (1 + EPSILON)
          && s0.getNumBoardings() <= s1.getNumBoardings()
          && (s0.getWeight() < s1.getWeight()
              || s0.getElapsedTime() < s1.getElapsedTime()
              || s0.getWalkDistance() < s1.getWalkDistance()
              || s0.getNumBoardings() < s1.getNumBoardings());
    } else {
      return false;
    }
  }

  // private boolean eDominates(State s0, State s1) {
  //  final double EPSILON1 = 0.1;
  //  if (s0.similarTripSeq(s1)) {
  //      return  s0.getWeight()       <= s1.getWeight()       * (1 + EPSILON1) &&
  //              s0.getElapsedTime()  <= s1.getElapsedTime()  * (1 + EPSILON1) &&
  //              s0.getWalkDistance() <= s1.getWalkDistance() * (1 + EPSILON1) &&
  //              s0.getNumBoardings() <= s1.getNumBoardings();
  //  } else if (s0.getTripId() != null && s0.getTripId() == s1.getTripId()) {
  //      return  s0.getNumBoardings() <= s1.getNumBoardings() &&
  //    		  s0.getWeight()       <= s1.getWeight()       * (1 + EPSILON2) &&
  //              s0.getElapsedTime()  <= s1.getElapsedTime()  * (1 + EPSILON2) &&
  //              s0.getWalkDistance() <= s1.getWalkDistance() * (1 + EPSILON2);
  //  } else {
  //	  return false;
  //  }
  // }

  //    private boolean eDominates(State s0, State s1) {
  //        if (s0.similarTripSeq(s1)) {
  //            return s0.getWeight() <= s1.getWeight();
  //        } else if (s0.getTrip() == s1.getTrip()) {
  //            if (s0.getNumBoardings() < s1.getNumBoardings())
  //                return true;
  //            return s0.getWeight() <= s1.getWeight();
  //        } else {
  //            return false;
  //        }
  //    }

  //    private boolean eDominates(State s0, State s1) {
  //        final double EPSILON = 0.1;
  //        return s0.getWeight() <= s1.getWeight() * (1 + EPSILON) &&
  //                s0.getTime() <= s1.getTime() * (1 + EPSILON) &&
  //               s0.getNumBoardings() <= s1.getNumBoardings();
  //    }

}