@Test
  public void testCalculateTurnAngle() {
    // Graph for a fictional grid city with turn restrictions
    IntersectionVertex v1 = vertex("maple_1st", new Coordinate(2.0, 2.0), false);
    IntersectionVertex v2 = vertex("maple_2nd", new Coordinate(2.0, 1.0), false);

    PlainStreetEdge e1 = edge(v1, v2, 1.0, false);

    // Edge has same first and last angle.
    assertEquals(91, e1.getInAngle());
    assertEquals(91, e1.getOutAngle());

    // 2 new ones
    IntersectionVertex v3 = vertex("test2", new Coordinate(1.0, 1.0), false);

    // Second edge
    PlainStreetEdge e2 = edge(v2, v3, 1.0, false);

    assertEquals(0, e2.getInAngle());
    assertEquals(0, e2.getOutAngle());

    // Difference should be about 90.
    int diff = (e1.getOutAngle() - e2.getInAngle());
    assertEquals(91, diff);

    int turnAngle = costModel.calculateTurnAngle(e1, e2, options);
    assertEquals(269, turnAngle);
  }
    /**
     * The safest bike lane should have a safety weight no lower than the time weight of a flat
     * street. This method divides the safety lengths by the length ratio of the safest street,
     * ensuring this property.
     *
     * @param graph
     */
    private void applyBikeSafetyFactor(Graph graph) {
      _log.info(
          GraphBuilderAnnotation.register(
              graph,
              Variety.GRAPHWIDE,
              "Multiplying all bike safety values by " + (1 / bestBikeSafety)));
      HashSet<Edge> seenEdges = new HashSet<Edge>();
      for (Vertex vertex : graph.getVertices()) {
        for (Edge e : vertex.getOutgoing()) {
          if (!(e instanceof PlainStreetEdge)) {
            continue;
          }
          PlainStreetEdge pse = (PlainStreetEdge) e;

          if (!seenEdges.contains(e)) {
            seenEdges.add(e);
            pse.setBicycleSafetyEffectiveLength(
                pse.getBicycleSafetyEffectiveLength() / bestBikeSafety);
          }
        }
        for (Edge e : vertex.getIncoming()) {
          if (!(e instanceof PlainStreetEdge)) {
            continue;
          }
          PlainStreetEdge pse = (PlainStreetEdge) e;

          if (!seenEdges.contains(e)) {
            seenEdges.add(e);
            pse.setBicycleSafetyEffectiveLength(
                pse.getBicycleSafetyEffectiveLength() / bestBikeSafety);
          }
        }
      }
    }
    private PlainStreetEdge getEdgeForStreet(
        IntersectionVertex start,
        IntersectionVertex end,
        OSMWay way,
        long startNode,
        double length,
        StreetTraversalPermission permissions,
        LineString geometry,
        boolean back) {

      String id = "way " + way.getId() + " from " + startNode;
      id = unique(id);

      String name = way.getAssumedName();

      if (customNamer != null) {
        name = customNamer.name(way, name);
      }

      if (name == null) {
        name = id;
      }

      boolean steps = "steps".equals(way.getTag("highway"));
      if (steps) {
        // consider the elevation gain of stairs, roughly
        length *= 2;
      }

      PlainStreetEdge street =
          new PlainStreetEdge(start, end, geometry, name, length, permissions, back);
      street.setId(id);

      if (!way.hasTag("name")) {
        street.setBogusName(true);
      }
      street.setStairs(steps);

      /* TODO: This should probably generalized somehow? */
      if (way.isTagFalse("wheelchair") || (steps && !way.isTagTrue("wheelchair"))) {
        street.setWheelchairAccessible(false);
      }

      street.setSlopeOverride(wayPropertySet.getSlopeOverride(way));

      if (customNamer != null) {
        customNamer.nameWithEdge(way, street);
      }

      return street;
    }
    /**
     * Handle oneway streets, cycleways, and whatnot. See http://wiki.openstreetmap.org/wiki/Bicycle
     * for various scenarios, along with
     * http://wiki.openstreetmap.org/wiki/OSM_tags_for_routing#Oneway.
     *
     * @param end
     * @param start
     */
    private P2<PlainStreetEdge> getEdgesForStreet(
        IntersectionVertex start,
        IntersectionVertex end,
        OSMWay way,
        long startNode,
        StreetTraversalPermission permissions,
        LineString geometry) {
      // get geometry length in meters, irritatingly.
      Coordinate[] coordinates = geometry.getCoordinates();
      double d = 0;
      for (int i = 1; i < coordinates.length; ++i) {
        d += DistanceLibrary.distance(coordinates[i - 1], coordinates[i]);
      }

      LineString backGeometry = (LineString) geometry.reverse();

      Map<String, String> tags = way.getTags();

      if (permissions == StreetTraversalPermission.NONE) return new P2<PlainStreetEdge>(null, null);

      PlainStreetEdge street = null, backStreet = null;

      /*
       * pedestrian rules: everything is two-way (assuming pedestrians are allowed at all)
       * bicycle rules: default: permissions;
       *
       * cycleway=dismount means walk your bike -- the engine will automatically try walking
       * bikes any time it is forbidden to ride them, so the only thing to do here is to
       * remove bike permissions
       *
       * oneway=... sets permissions for cars and bikes oneway:bicycle overwrites these
       * permissions for bikes only
       *
       * now, cycleway=opposite_lane, opposite, opposite_track can allow once oneway has been
       * set by oneway:bicycle, but should give a warning if it conflicts with oneway:bicycle
       *
       * bicycle:backward=yes works like oneway:bicycle=no bicycle:backwards=no works like
       * oneway:bicycle=yes
       */

      String foot = way.getTag("foot");
      if ("yes".equals(foot) || "designated".equals(foot)) {
        permissions = permissions.add(StreetTraversalPermission.PEDESTRIAN);
      }

      if (OSMWithTags.isFalse(foot)) {
        permissions = permissions.remove(StreetTraversalPermission.PEDESTRIAN);
      }

      boolean forceBikes = false;
      String bicycle = way.getTag("bicycle");
      if ("yes".equals(bicycle) || "designated".equals(bicycle)) {
        permissions = permissions.add(StreetTraversalPermission.BICYCLE);
        forceBikes = true;
      }

      if (way.isTag("cycleway", "dismount") || "dismount".equals(bicycle)) {
        permissions = permissions.remove(StreetTraversalPermission.BICYCLE);
        if (forceBikes) {
          _log.warn(
              GraphBuilderAnnotation.register(graph, Variety.CONFLICTING_BIKE_TAGS, way.getId()));
        }
      }

      StreetTraversalPermission permissionsFront = permissions;
      StreetTraversalPermission permissionsBack = permissions;

      if (way.isTagTrue("oneway") || "roundabout".equals(tags.get("junction"))) {
        permissionsBack = permissionsBack.remove(StreetTraversalPermission.BICYCLE_AND_CAR);
      }
      if (way.isTag("oneway", "-1")) {
        permissionsFront = permissionsFront.remove(StreetTraversalPermission.BICYCLE_AND_CAR);
      }
      String oneWayBicycle = way.getTag("oneway:bicycle");
      if (OSMWithTags.isTrue(oneWayBicycle) || way.isTagFalse("bicycle:backwards")) {
        permissionsBack = permissionsBack.remove(StreetTraversalPermission.BICYCLE);
      }
      if ("-1".equals(oneWayBicycle)) {
        permissionsFront = permissionsFront.remove(StreetTraversalPermission.BICYCLE);
      }
      if (OSMWithTags.isFalse(oneWayBicycle) || way.isTagTrue("bicycle:backwards")) {
        if (permissions.allows(StreetTraversalPermission.BICYCLE)) {
          permissionsFront = permissionsFront.add(StreetTraversalPermission.BICYCLE);
          permissionsBack = permissionsBack.add(StreetTraversalPermission.BICYCLE);
        }
      }

      // any cycleway which is opposite* allows contraflow biking
      String cycleway = way.getTag("cycleway");
      String cyclewayLeft = way.getTag("cycleway:left");
      String cyclewayRight = way.getTag("cycleway:right");
      if ((cycleway != null && cycleway.startsWith("opposite"))
          || (cyclewayLeft != null && cyclewayLeft.startsWith("opposite"))
          || (cyclewayRight != null && cyclewayRight.startsWith("opposite"))) {

        permissionsBack = permissionsBack.add(StreetTraversalPermission.BICYCLE);
      }

      String access = way.getTag("access");
      boolean noThruTraffic =
          "destination".equals(access)
              || "private".equals(access)
              || "customers".equals(access)
              || "delivery".equals(access)
              || "forestry".equals(access)
              || "agricultural".equals(access);

      if (permissionsFront != StreetTraversalPermission.NONE) {
        street = getEdgeForStreet(start, end, way, startNode, d, permissionsFront, geometry, false);
        street.setNoThruTraffic(noThruTraffic);
      }
      if (permissionsBack != StreetTraversalPermission.NONE) {
        backStreet =
            getEdgeForStreet(end, start, way, startNode, d, permissionsBack, backGeometry, true);
        backStreet.setNoThruTraffic(noThruTraffic);
      }

      /* mark edges that are on roundabouts */
      if ("roundabout".equals(tags.get("junction"))) {
        if (street != null) street.setRoundabout(true);
        if (backStreet != null) backStreet.setRoundabout(true);
      }

      return new P2<PlainStreetEdge>(street, backStreet);
    }
    public void buildGraph(Graph graph) {
      this.graph = graph;

      // handle turn restrictions and road names in relations
      processRelations();

      // Remove all simple islands
      _nodes.keySet().retainAll(_nodesWithNeighbors);

      long wayIndex = 0;

      // store levels that cannot be parsed, and assign them a number
      int nextUnparsedLevel = 0;
      HashMap<String, Integer> unparsedLevels = new HashMap<String, Integer>();

      // figure out which nodes that are actually intersections
      Set<Long> possibleIntersectionNodes = new HashSet<Long>();
      for (OSMWay way : _ways.values()) {
        List<Long> nodes = way.getNodeRefs();
        for (long node : nodes) {
          if (possibleIntersectionNodes.contains(node)) {
            intersectionNodes.put(node, null);
          } else {
            possibleIntersectionNodes.add(node);
          }
        }
      }
      GeometryFactory geometryFactory = new GeometryFactory();

      /* build an ordinary graph, which we will convert to an edge-based graph */

      for (OSMWay way : _ways.values()) {

        if (wayIndex % 10000 == 0) _log.debug("ways=" + wayIndex + "/" + _ways.size());
        wayIndex++;

        WayProperties wayData = wayPropertySet.getDataForWay(way);

        if (!way.hasTag("name")) {
          String creativeName = wayPropertySet.getCreativeNameForWay(way);
          if (creativeName != null) {
            way.addTag("otp:gen_name", creativeName);
          }
        }
        Set<Alert> note = wayPropertySet.getNoteForWay(way);

        StreetTraversalPermission permissions =
            getPermissionsForEntity(way, wayData.getPermission());
        if (permissions == StreetTraversalPermission.NONE) continue;

        List<Long> nodes = way.getNodeRefs();

        IntersectionVertex startEndpoint = null, endEndpoint = null;

        ArrayList<Coordinate> segmentCoordinates = new ArrayList<Coordinate>();

        /* The otp:numeric_level tag adds a constant to level numbers depending on
          where they come from (level map, tags).
          This prevents a layer 1 from being equal to a level_map 1, because they may
          not represent the same thing. Worst case scenario, OTP will say
          "take elevator to level 1" when you're already on level 1.
        */
        final int LEVEL_MAP_LEVEL = 0;
        final int LEVEL_TAG_LEVEL = 100000;
        final int LAYER_TAG_LEVEL = 200000;
        final int OTHER_LEVEL = 300000;
        final int UNPARSED_LEVEL = 400000;

        if (!way.hasTag("otp:numeric_level")) {
          // Parse levels, if a level map was not already applied in processRelations
          String strLevel = null;
          int offset = LEVEL_MAP_LEVEL, intLevel;
          if (way.hasTag("level")) { // TODO: floating-point levels &c.
            strLevel = way.getTag("level");
            offset = LEVEL_TAG_LEVEL;
          } else if (way.hasTag("layer")) {
            strLevel = way.getTag("layer");
            offset = LAYER_TAG_LEVEL;
          }
          if (strLevel != null) {
            try {
              intLevel = Integer.parseInt(strLevel);
            } catch (NumberFormatException e) {
              // could not parse the level string as an integer
              // get a unique level number for this
              if (unparsedLevels.containsKey(strLevel)) {
                intLevel = unparsedLevels.get(strLevel);
              } else { // make a new unique ID
                intLevel = nextUnparsedLevel++;
                unparsedLevels.put(strLevel, intLevel);
              }
              offset = UNPARSED_LEVEL;
              _log.warn(
                  "Could not determine ordinality of layer "
                      + strLevel
                      + ". Elevators will work, but costing may be incorrect. "
                      + "A level map should be used in this situation.");
            }
          } else {
            // no string level description was available. assume ground level, but
            // don't assume it's connected to any other ground level.
            intLevel = 0;
            offset = OTHER_LEVEL;
          }
          if (noZeroLevels && intLevel >= 0) {
            // add 1 to human-readable representation of non-negative levels
            // in (-inf, -1] U [1, inf) locations like the US
            strLevel = Integer.toString(intLevel + 1);
          }
          way.addTag("otp:numeric_level", Integer.toString(intLevel + offset));
          way.addTag("otp:human_level", strLevel); // redunant
          levelHumanNames.put(intLevel + offset, strLevel);
        }

        /*
         * Traverse through all the nodes of this edge. For nodes which are not shared with
         * any other edge, do not create endpoints -- just accumulate them for geometry. For
         * nodes which are shared, create endpoints and StreetVertex instances.
         */
        Long startNode = null;
        OSMNode osmStartNode = null;
        for (int i = 0; i < nodes.size() - 1; i++) {
          Long endNode = nodes.get(i + 1);
          if (osmStartNode == null) {
            startNode = nodes.get(i);
            osmStartNode = _nodes.get(startNode);
          }
          OSMNode osmEndNode = _nodes.get(endNode);

          if (osmStartNode == null || osmEndNode == null) continue;

          LineString geometry;

          /*
           * skip vertices that are not intersections, except that we use them for
           * geometry
           */
          if (segmentCoordinates.size() == 0) {
            segmentCoordinates.add(getCoordinate(osmStartNode));
          }

          if (intersectionNodes.containsKey(endNode) || i == nodes.size() - 2) {
            segmentCoordinates.add(getCoordinate(osmEndNode));
            geometry =
                geometryFactory.createLineString(segmentCoordinates.toArray(new Coordinate[0]));
            segmentCoordinates.clear();
          } else {
            segmentCoordinates.add(getCoordinate(osmEndNode));
            continue;
          }

          /* generate endpoints */
          if (startEndpoint == null) { // first iteration on this way
            // make or get a shared vertex for flat intersections,
            // one vertex per level for multilevel nodes like elevators
            startEndpoint = getVertexForOsmNode(osmStartNode, way);
          } else { // subsequent iterations
            startEndpoint = endEndpoint;
          }

          endEndpoint = getVertexForOsmNode(osmEndNode, way);

          P2<PlainStreetEdge> streets =
              getEdgesForStreet(startEndpoint, endEndpoint, way, i, permissions, geometry);

          PlainStreetEdge street = streets.getFirst();

          if (street != null) {
            double safety = wayData.getSafetyFeatures().getFirst();
            street.setBicycleSafetyEffectiveLength(street.getLength() * safety);
            if (safety < bestBikeSafety) {
              bestBikeSafety = safety;
            }
            if (note != null) {
              street.setNote(note);
            }
          }

          PlainStreetEdge backStreet = streets.getSecond();
          if (backStreet != null) {
            double safety = wayData.getSafetyFeatures().getSecond();
            if (safety < bestBikeSafety) {
              bestBikeSafety = safety;
            }
            backStreet.setBicycleSafetyEffectiveLength(backStreet.getLength() * safety);
            if (note != null) {
              backStreet.setNote(note);
            }
          }

          /* Check if there are turn restrictions starting on this segment */
          List<TurnRestrictionTag> restrictionTags = turnRestrictionsByFromWay.get(way.getId());

          if (restrictionTags != null) {
            for (TurnRestrictionTag tag : restrictionTags) {
              if (tag.via == startNode) {
                TurnRestriction restriction = turnRestrictionsByTag.get(tag);
                restriction.from = backStreet;
              } else if (tag.via == endNode) {
                TurnRestriction restriction = turnRestrictionsByTag.get(tag);
                restriction.from = street;
              }
            }
          }

          restrictionTags = turnRestrictionsByToWay.get(way.getId());
          if (restrictionTags != null) {
            for (TurnRestrictionTag tag : restrictionTags) {
              if (tag.via == startNode) {
                TurnRestriction restriction = turnRestrictionsByTag.get(tag);
                restriction.to = street;
              } else if (tag.via == endNode) {
                TurnRestriction restriction = turnRestrictionsByTag.get(tag);
                restriction.to = backStreet;
              }
            }
          }
          startNode = endNode;
          osmStartNode = _nodes.get(startNode);
        }
      } // END loop over OSM ways

      /* build elevator edges */
      for (Long nodeId : multiLevelNodes.keySet()) {
        OSMNode node = _nodes.get(nodeId);
        // this allows skipping levels, e.g., an elevator that stops
        // at floor 0, 2, 3, and 5.
        // Converting to an Array allows us to
        // subscript it so we can loop over it in twos. Assumedly, it will stay
        // sorted when we convert it to an Array.
        // The objects are Integers, but toArray returns Object[]
        HashMap<Integer, IntersectionVertex> levels = multiLevelNodes.get(nodeId);

        /* first, build FreeEdges to disconnect from the graph,
        GenericVertices to serve as attachment points,
        and ElevatorBoard and ElevatorAlight edges
        to connect future ElevatorHop edges to.
        After this iteration, graph will look like (side view):
        +==+~~X

        +==+~~X

        +==+~~X

        + GenericVertex, X EndpointVertex, ~~ FreeEdge, == ElevatorBoardEdge/ElevatorAlightEdge
        Another loop will fill in the ElevatorHopEdges. */

        ArrayList<Vertex> onboardVertices = new ArrayList<Vertex>();

        Integer[] levelKeys = levels.keySet().toArray(new Integer[0]);
        Arrays.sort(levelKeys);

        for (Integer level : levelKeys) {
          // get the source node to hang all this stuff off of.
          String humanLevel = levelHumanNames.get(level);
          IntersectionVertex sourceVert = levels.get(level);
          String sourceVertLabel = sourceVert.getLabel();

          // create a Vertex to connect the FreeNode to.
          ElevatorOffboardVertex offboardVert =
              new ElevatorOffboardVertex(
                  graph, sourceVertLabel + "_middle", sourceVert.getX(), sourceVert.getY());

          new FreeEdge(sourceVert, offboardVert);
          new FreeEdge(offboardVert, sourceVert);

          // Create a vertex to connect the ElevatorAlight, ElevatorBoard, and
          // ElevatorHop edges to.
          ElevatorOnboardVertex onboardVert =
              new ElevatorOnboardVertex(
                  graph, sourceVertLabel + "_onboard", sourceVert.getX(), sourceVert.getY());

          new ElevatorBoardEdge(offboardVert, onboardVert);
          new ElevatorAlightEdge(onboardVert, offboardVert, humanLevel);

          // add it to the array so it can be connected later
          onboardVertices.add(onboardVert);
        }

        // -1 because we loop over it two at a time
        Integer vSize = onboardVertices.size() - 1;

        for (Integer i = 0; i < vSize; i++) {
          Vertex from = onboardVertices.get(i);
          Vertex to = onboardVertices.get(i + 1);

          StreetTraversalPermission permission = StreetTraversalPermission.PEDESTRIAN_AND_BICYCLE;

          // default to true
          boolean wheelchairAccessible = true;
          // check for bicycle=no, otherwise assume it's OK to take a bike
          if (node.isTagFalse("bicycle")) {
            permission = StreetTraversalPermission.PEDESTRIAN;
          }
          // check for wheelchair=no
          if (node.isTagFalse("wheelchair")) {
            wheelchairAccessible = false;
          }
          // The narrative won't be strictly correct, as it will show the elevator as part
          // of the cycling leg, but I think most cyclists will figure out that they
          // should really dismount.
          ElevatorHopEdge theEdge = new ElevatorHopEdge(from, to, permission);
          ElevatorHopEdge backEdge = new ElevatorHopEdge(to, from, permission);
          theEdge.wheelchairAccessible = wheelchairAccessible;
          backEdge.wheelchairAccessible = wheelchairAccessible;
        }
      } // END elevator edge loop

      /* unify turn restrictions */
      Map<Edge, TurnRestriction> turnRestrictions = new HashMap<Edge, TurnRestriction>();
      for (TurnRestriction restriction : turnRestrictionsByTag.values()) {
        turnRestrictions.put(restriction.from, restriction);
      }
      if (customNamer != null) {
        customNamer.postprocess(graph);
      }
      applyBikeSafetyFactor(graph);
      StreetUtils.pruneFloatingIslands(graph);
      StreetUtils.makeEdgeBased(graph, endpoints, turnRestrictions);
    } // END buildGraph()
  /**
   * 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;
  }