private void relaxNode(final Node n, PriorityQueue<Node> pendingNodes) {
    NodeData nData = nodeData.get(n.getId());
    double currTime = nData.getTime();
    double currCost = nData.getCost();
    for (Link l : n.getOutLinks().values()) {
      Node nn = l.getToNode();
      NodeData nnData = nodeData.get(nn.getId());
      if (nnData == null) {
        nnData = new NodeData();
        this.nodeData.put(nn.getId(), nnData);
      }
      double visitCost =
          currCost + tcFunction.getLinkTravelDisutility(l, currTime, PERSON, VEHICLE);
      double visitTime = currTime + ttFunction.getLinkTravelTime(l, currTime, PERSON, VEHICLE);

      if (visitCost < nnData.getCost()) {
        pendingNodes.remove(nn);
        nnData.visit(n.getId(), visitCost, visitTime);
        additionalComputationsHook(l, currTime);
        pendingNodes.add(nn);
      }
    }
  }
  @Override
  public Path calcLeastCostPath(
      Node fromNode, Node toNode, double starttime, Person person, Vehicle vehicle) {

    List<Node> nodes = new ArrayList<>();
    List<Link> links = new ArrayList<>();
    double travelTime = starttime;
    double travelCost = 0;
    Node currentNode = fromNode;
    if (networkPainter != null) {
      networkPainter.addForegroundPixel(fromNode.getId());
      networkPainter.addForegroundPixel(toNode.getId());
    }
    while (currentNode != toNode) {
      Object[] outLinks = currentNode.getOutLinks().values().toArray();
      // come up witha set of costs based on how much nearer (eucl) the link brings you to your
      // destination,
      // use them as probabilities of selecting the link
      double[] cumulativeWeights = new double[outLinks.length];
      double[] weights = new double[outLinks.length];
      double[] dists = new double[outLinks.length];
      double minCost = Double.POSITIVE_INFINITY;
      double maxCost = 0;
      Link selection = null;
      for (int i = 0; i < outLinks.length; i++) {
        double linkCost =
            CoordUtils.calcEuclideanDistance(
                toNode.getCoord(), ((Link) outLinks[i]).getToNode().getCoord());
        if (((Link) outLinks[i])
            .getToNode()
            .equals(toNode)) { // found destination, stop mucking about
          selection = (Link) outLinks[i];
          break;
        }
        dists[i] = linkCost;
        minCost = Math.min(minCost, linkCost);
        maxCost = Math.max(maxCost, linkCost);
      }
      for (int i = 0; i < outLinks.length; i++) {
        double linkCost = dists[i] - minCost;
        if (links.contains(outLinks[i]) || nodes.contains(((Link) outLinks[i]).getToNode())) {
          linkCost = 1e6 * (maxCost - minCost);
        }
        linkCost = Math.exp(-beta * linkCost);
        linkCost = linkCost == 1 ? 2 : linkCost;
        cumulativeWeights[i] = i > 0 ? cumulativeWeights[i - 1] + linkCost : linkCost;
        weights[i] = linkCost;
      }
      if (selection == null) {
        // sample if the destination has not been found
        double sampleProb =
            MatsimRandom.getRandom().nextDouble() * cumulativeWeights[cumulativeWeights.length - 1];
        for (int i = 0; i < outLinks.length; i++) {
          if (cumulativeWeights[i] >= sampleProb) {
            selection = (Link) outLinks[i];
            links.add(selection);
            nodes.add(currentNode);
            travelCost +=
                travelCosts.getLinkTravelDisutility(selection, travelTime, person, vehicle);
            travelTime += travelTimes.getLinkTravelTime(selection, travelTime, person, vehicle);
            break;
          }
        }
      }
      if (networkPainter != null) {
        networkPainter.addForegroundLine(selection.getId());
        try {
          Thread.sleep(200);
        } catch (InterruptedException e) {
          e.printStackTrace();
        }
      }
      currentNode = selection.getToNode();
    }
    if (networkPainter != null) {
      try {
        Thread.sleep(500);
        networkPainter.clearForeground();
      } catch (InterruptedException e) {
        e.printStackTrace();
      }
    }
    return new Path(nodes, links, travelTime, travelCost);
  }