/**
   * Sets the Agent up to proceed along an Edge
   *
   * @param edge the GeomPlanarGraphEdge to traverse next
   */
  void setupEdge(GeomPlanarGraphEdge edge) {

    // clean up on old edge
    if (currentEdge != null) {
      ArrayList<AgentCopy> traffic = world.edgeTraffic.get(currentEdge);
      traffic.remove(this);
    }
    currentEdge = edge;

    // update new edge traffic
    if (world.edgeTraffic.get(currentEdge) == null) {
      world.edgeTraffic.put(currentEdge, new ArrayList<AgentCopy>());
    }
    world.edgeTraffic.get(currentEdge).add(this);

    // set up the new segment and index info
    LineString line = edge.getLine();
    segment = new LengthIndexedLine(line);
    startIndex = segment.getStartIndex();
    endIndex = segment.getEndIndex();
    linkDirection = 1;

    // check to ensure that Agent is moving in the right direction
    double distanceToStart = line.getStartPoint().distance(location.geometry),
        distanceToEnd = line.getEndPoint().distance(location.geometry);
    if (distanceToStart <= distanceToEnd) { // closer to start
      currentIndex = startIndex;
      linkDirection = 1;
    } else if (distanceToEnd < distanceToStart) { // closer to end
      currentIndex = endIndex;
      linkDirection = -1;
    }
  }
  /** moves the agent along the path */
  public void step(SimState state) {
    // check that we've been placed on an Edge
    if (segment == null) {
      return;
    } // check that we haven't already reached our destination
    else if (reachedDestination) {
      return;
    }

    // make sure that we're heading in the right direction
    boolean toWork = ((Gridlock_NorfolkTEST) state).goToWork;
    if ((toWork && pathDirection < 0) || (!toWork && pathDirection > 0)) {
      flipPath();
    }

    // move along the current segment
    speed = progress(moveRate);
    currentIndex += speed;

    // check to see if the progress has taken the current index beyond its goal
    // given the direction of movement. If so, proceed to the next edge
    if (linkDirection == 1 && currentIndex > endIndex) {
      Coordinate currentPos = segment.extractPoint(endIndex);
      updatePosition(currentPos);
      transitionToNextEdge(currentIndex - endIndex);
    } else if (linkDirection == -1 && currentIndex < startIndex) {
      Coordinate currentPos = segment.extractPoint(startIndex);
      updatePosition(currentPos);
      transitionToNextEdge(startIndex - currentIndex);
    } else { // just update the position!
      Coordinate currentPos = segment.extractPoint(currentIndex);

      updatePosition(currentPos);
    }
  }
  /** Plots a path between the Agent's home Node and its work Node */
  private void findNewAStarPath(Gridlock_NorfolkTEST geoTest) {

    // get the home and work Nodes with which this Agent is associated
    Node currentJunction = geoTest.network.findNode(location.geometry.getCoordinate());
    Node destinationJunction = workNode;

    if (currentJunction == null) {
      return; // just a check
    }
    // find the appropriate A* path between them
    AStar pathfinder = new AStar();
    ArrayList<GeomPlanarGraphDirectedEdge> path =
        pathfinder.astarPath(currentJunction, destinationJunction);

    // if the path works, lay it in
    if (path != null && path.size() > 0) {

      // save it
      pathFromHomeToWork = path;

      // set up how to traverse this first link
      GeomPlanarGraphEdge edge = (GeomPlanarGraphEdge) path.get(0).getEdge();
      setupEdge(edge);

      // update the current position for this link
      updatePosition(segment.extractPoint(currentIndex));
    }
  }
 /**
  * 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;
 }
  private static List<SimpleFeature> processEdges(
      List<String> unvisited,
      Node startNode,
      GeometryFactory geometryFactory,
      SimpleFeatureType featureType,
      long startTime,
      int pathID,
      List<SimpleFeature> featuresList,
      double stepTime,
      int maxTime,
      int intersectionWait,
      int stepDistance) {

    LineString line;
    Node currentNode = startNode;
    int crossings = 0;
    int walkDistance = 0;
    long walkTime = startTime;
    while ((unvisited.size() > 0) && (walkTime <= maxTime)) {
      if (currentNode.getEdges().size() > 0) {
        int crossingWait = 0;
        if (currentNode.getEdges().size() > 2) {
          // This is an intersection - therefore delay for crossing
          crossingWait = intersectionWait;
          crossings++;
        }
        for (Edge edge : (List<Edge>) currentNode.getEdges()) {
          if (unvisited.contains(String.valueOf(edge.getID()))) {
            line = (LineString) edge.getObject();

            Coordinate pt = ((Point) currentNode.getObject()).getCoordinate();

            LengthIndexedLine lil = new LengthIndexedLine(line);

            if (lil.project(pt) == lil.getStartIndex()) {
              // start coordinate is at start of line
              for (int index = 0; index < lil.getEndIndex(); index += stepDistance) {
                Coordinate coordinate = lil.extractPoint(index);
                Point point = geometryFactory.createPoint(coordinate);
                SimpleFeature feature =
                    buildTimeFeatureFromGeometry(
                        featureType,
                        point,
                        walkTime,
                        crossings,
                        walkDistance,
                        String.valueOf(pathID));
                walkDistance += stepDistance;
                walkTime += stepTime + crossingWait;
                crossingWait = 0; // only perform wait once
                featuresList.add(feature);
              }
            } else if (lil.project(pt) == lil.getEndIndex()) {
              // start coordinate is at the end of the line
              for (int index = (int) lil.getEndIndex(); index >= 0; index -= stepDistance) {
                Coordinate coordinate = lil.extractPoint(index);
                Point point = geometryFactory.createPoint(coordinate);
                SimpleFeature feature =
                    buildTimeFeatureFromGeometry(
                        featureType,
                        point,
                        walkTime,
                        crossings,
                        walkDistance,
                        String.valueOf(pathID));
                walkDistance += stepDistance;
                walkTime += stepTime + crossingWait;
                crossingWait = 0; // only perform wait once
                featuresList.add(feature);
              }
            } else {
              LOGGER.error("Start coordinate did not match with Index!");
            }
            unvisited.remove(String.valueOf(edge.getID()));

            Node nextNode = edge.getOtherNode(currentNode);
            if (nextNode != null) {
              currentNode = nextNode;
            } else {
              break;
            }
          }
        }
      }
    }
    return featuresList;
  }