/**
  * Prints the list of roads along a path.
  *
  * @param path The list of roads to print.
  */
 public void printPath(List path) {
   System.out.println("Path: ");
   Iterator it = path.iterator();
   while (it.hasNext()) {
     SegmentNode sn = (SegmentNode) it.next();
     RoadSegment rs = (RoadSegment) segments.get(sn.segmentID);
     System.out.println(rs.printStreetName(streets));
   }
 }
  /** @param smiod */
  public void checkPath(StreetMobilityInfoOD smiod) {
    SegmentNode temp;
    RoadSegment rsTemp;
    // if the current end point is equal to an end point of the second road in
    // the list, then remove the first road in the list because it's redundant
    if (smiod.path.size() > 1) {
      temp = (SegmentNode) smiod.path.get(1);
      rsTemp = (RoadSegment) segments.get(temp.segmentID);
      if (smiod.rsEnd.distance(rsTemp.getStartPoint()) == 0
          || smiod.rsEnd.distance(rsTemp.getEndPoint()) == 0) {
        if (DEBUG_OD) System.out.println("Removing first node in list because it is redundant!");
        smiod.path.remove(0);
      }
    }

    // case where the node is about to start moving away from the right intersection
    // diagnosis: current location is equal to an end point on list
    // solution: update "current rs" and "rsEnd" with next value from path
    // then remove first entry in path
    //        Location loc = smiod.rsEnd;
    //        if (DEBUG_OD)System.out.println("Current location: "+loc);
    //        temp = (SegmentNode)smiod.path.get(1);
    //        rsTemp = (RoadSegment)segments.get(temp.segmentID);
    //        if (loc.distance(rsTemp.getEndPoint())<= StreetMobility.INTERSECTION_RESOLUTION )
    //        {
    //            if (DEBUG_OD)System.out.println("Avoiding U-turn!");
    //            smiod.current = rsTemp;
    //            smiod.rsEnd = rsTemp.getStartPoint();
    //            smiod.path.remove(0);
    //            smiod.path.remove(0);
    //        }
    //        else if (loc.distance(rsTemp.getStartPoint())<=StreetMobility.INTERSECTION_RESOLUTION)
    //        {
    //            if (DEBUG_OD)System.out.println("Avoiding U-turn!");
    //            smiod.current = rsTemp;
    //            smiod.rsEnd = rsTemp.getEndPoint();
    //            smiod.path.remove(0);
    //            smiod.path.remove(0);
    //        }

    // case where node is already at intersection for first segment on path
    // diagnosis: current location is equal to one of the points in the first segment

    //        temp = (SegmentNode)smiod.path.get(0);
    //        rsTemp = (RoadSegment)segments.get(temp.segmentID);
    //
    //        if (loc.distance(rsTemp.getEndPoint())<=StreetMobility.INTERSECTION_RESOLUTION )
    //        {
    //            if (DEBUG_OD)System.out.println("Moving immediately to next segment!");
    //            smiod.current = rsTemp;
    //            smiod.rsEnd = rsTemp.getStartPoint();
    //            smiod.path.remove(0);
    //        }
    //        else if (loc.distance(rsTemp.getStartPoint())<=StreetMobility.INTERSECTION_RESOLUTION)
    //        {
    //            if (DEBUG_OD)System.out.println("Moving immediately to next segment!");
    //            smiod.current = rsTemp;
    //            smiod.rsEnd = rsTemp.getEndPoint();
    //            smiod.path.remove(0);
    //        }

    temp = (SegmentNode) smiod.path.get(0);
    rsTemp = (RoadSegment) segments.get(temp.segmentID);
    // need to add current road onto list to perform U-turn.
    if (smiod.rsEnd.distance(rsTemp.getEndPoint()) > INTERSECTION_RESOLUTION
        && smiod.rsEnd.distance(rsTemp.getStartPoint()) > INTERSECTION_RESOLUTION) {
      smiod.path.addFirst(new SegmentNode(smiod.rsEnd, smiod.current.getSelfIndex(), true, false));
    }
  }
  /**
   * This method calculates the path from the origin to the destination road segment.
   *
   * @param smiod The SMIOD object.
   */
  public boolean calculatePath(
      StreetMobilityInfoOD smiod, RoadSegment origin, Location nextEnd, RoadSegment destination) {
    // TODO support loading from OD pairs
    // after calling constructor, objects for A* search must be created
    boolean found = false;

    // TODO the caller should determine direction
    if (DEBUG_OD) {
      System.out.println("Calculating path...");
      System.out.println("Origin:      " + origin.printStreetName(streets));
      System.out.println("Destination: " + destination.printStreetName(streets));
      System.out.println(
          "Distance: " + origin.getStartPoint().distance(destination.getStartPoint()));
    }

    if (nextEnd == null) {
      if (smiod.rsEnd.distance(origin.getEndPoint()) == 0) {
        nextEnd = origin.getEndPoint();
      } else {
        nextEnd = origin.getStartPoint();
      }
    }
    if (DEBUG_OD && DEBUG_VIS_OD && v != null) {
      v.colorSegment(origin, Color.RED);
    }

    boolean endStart;
    Location endPoint;
    if (destination.getStartPoint().distance(nextEnd)
        < destination.getEndPoint().distance(nextEnd)) {
      endPoint = destination.getStartPoint();
      endStart = true;
    } else {
      endPoint = destination.getEndPoint();
      endStart = false;
    }

    SegmentNode startNode =
        new SegmentNode(
            nextEnd, origin.getSelfIndex(), origin.getStartPoint().distance(nextEnd) == 0, true);
    SegmentNode endNode = new SegmentNode(endPoint, destination.getSelfIndex(), endStart, false);
    sni.dest = endNode;

    // try to find cached path
    if (hm != null) {
      LinkedList ll = (LinkedList) hm.get(nextEnd);
      if (ll != null) {
        ListIterator li = ll.listIterator();

        while (li.hasNext()) {
          LinkedList path = (LinkedList) li.next();
          if (!matches(startNode, endNode, path)) {
            continue;
          }
          smiod.path = (LinkedList) path.clone();
          smiod.path.removeFirst();
          if (DEBUG_OD) System.out.println("Found cached path!");
          return true;
          //                    break;
        }
      }
    }
    if (!found) {

      //      TODO support locations at arbitrary points on road
      smiod.destinationSN = endNode;
      smiod.destinationLocation = destination.getEndPoint();

      AStarSearch ass = new AStarSearch(hm, this); // TODO rename variable
      smiod.path = ass.findPath(startNode, endNode); // find the path
    }

    // no path found
    if (smiod.path.get(0) == null) {
      AStarSearch ass = new AStarSearch(hm, this); // TODO rename variable
      smiod.path = ass.findPath(startNode, endNode); // find the path
      if (v != null) {
        v.colorSegments(
            new RoadSegment[] {origin, destination}, new Color[] {Color.RED, Color.RED});
      }
      smiod.path.remove(0);
      System.out.println("No path found!");
      return false;
    }

    if (DEBUG_VIS_OD && v != null) {
      showPath(smiod.path.toArray(), Color.BLUE);
    }

    // check for strange double entry in list
    for (int i = 1; i < smiod.path.size(); i++) {
      if (((SegmentNode) smiod.path.get(smiod.path.size() - i)).segmentID
          == ((SegmentNode) smiod.path.get(smiod.path.size() - (i + 1))).segmentID) {
        if (DEBUG_OD) System.out.println("Removed redundant entry!");
        smiod.path.remove(smiod.path.size() - i);
      }
    }

    if (DEBUG_OD) printPath(smiod.path);
    if (hm != null) cachePath(smiod, startNode, endNode);

    return true; // path found!
  }
  /* (non-Javadoc)
   * @see jist.swans.field.StreetMobility#setNextRoad(jist.swans.field.StreetMobilityInfo)
   */
  public void setNextRoad(StreetMobilityInfo smi) {

    StreetMobilityInfoOD smiod = (StreetMobilityInfoOD) smi;

    if (smiod.path.size() > 0 || smiod.config == Constants.MOBILITY_STREET_RANDOM) {
      // get next intersection
      smiod.nextIs = intersections.findIntersectingRoads(smiod.rsEnd);

      if (smiod.path.size() == 0
          || (RECALCULATE_ALWAYS
              && JistAPI.getTime() > 30 * Constants.SECOND
              && smiod.path.size() > 2
              && smiod.speedSum > 0)) // calculate new path
      {
        // indicate that node should be taken off map
        if (smiod.config == Constants.MOBILITY_STREET_FLOW && smiod.path.size() == 0) {
          smi.nextRS = null;
          return;
        }

        if (DEBUG_OD) System.out.println("Calculating new path...");
        // recalculate path based on congestion info
        if (smiod.path.size() > 2 && smiod.nextEnd != null) {
          LinkedList oldPath = (LinkedList) smiod.path.clone();
          SegmentNode nextOne = (SegmentNode) smiod.path.getFirst();
          RoadSegment origin = (RoadSegment) segments.get(nextOne.segmentID);
          SegmentNode dest = smiod.destinationSN;
          RoadSegment rs = (RoadSegment) segments.get(smiod.destinationSN.segmentID);

          calculatePath(smiod, smi.current, smi.rsEnd, rs);

          if (!smiod.path.getLast().equals(dest))
            throw new RuntimeException("Destination altered!");

          if (smiod.current.equals(
              (RoadSegment) segments.get(((SegmentNode) smiod.path.getFirst()).segmentID)))
            smiod.path.remove();
          if (smiod.path.getFirst().equals(nextOne)) ;
          else {
            if (DEBUG_VIS_OD && v != null) {
              v.drawCircle(
                  20, smiod.rsEnd /*(smiod.rsEnd.distance(smi.current.getEndPoint())==0
                                ? smi.current.getStartPoint() : smi.current.getEndPoint())*/);
              oldPath.addLast(new SegmentNode(smi.rsEnd, smi.current.getSelfIndex(), true, true));
              smiod.path.addLast(
                  new SegmentNode(smi.rsEnd, smi.current.getSelfIndex(), true, true));
              showPath(oldPath.toArray(), Color.RED);
              showPath(smiod.path.toArray(), Color.ORANGE);
              smiod.path.removeLast();
              System.out.println("Foo!");
            }
          }
          checkPath(smiod);
          if (smiod.current.equals(
              (RoadSegment) segments.get(((SegmentNode) smiod.path.getFirst()).segmentID)))
            smiod.path = oldPath;

        } else {
          // this section makes sure that a valid path is found:
          // 1) there must be a path from origin to destination on the map
          // 2) the path must be longer than 4 so that it's substantial
          boolean valid = false;
          while (!valid || smiod.path.size() < 4) {
            RoadSegment nextDest = null;
            int segmentSize = sni.segment.size();

            while (nextDest == null
                || nextDest.getSelfIndex() == smiod.current.getSelfIndex()
                || nextDest.getEndPoint().distance(smiod.current.getEndPoint())
                    > threshold) // get new destination
            {
              int i = rnd.nextInt(segmentSize);
              nextDest = (RoadSegment) sni.segment.get(i);
            }
            valid = calculatePath(smiod, smi.current, smi.rsEnd, nextDest);
          }
          checkPath(smiod);
        }
        if (DEBUG_OD) System.out.println("Current rsEnd: " + smi.rsEnd);
        if (DEBUG_OD)
          System.out.println("Current road: " + smi.current.printStreetName(sni.streets));
      }

      SegmentNode sn = (SegmentNode) smiod.path.get(0);
      RoadSegment rs = (RoadSegment) segments.get(sn.segmentID);
      updateNextRoadSegment(smiod, rs); // update next road
      smiod.path.remove(0); // already used the first entry

      while (smiod.path.size() > 0) {
        // weird intersection type that confuses A*
        // getting on a street for no distance...
        sn = (SegmentNode) smiod.path.get(0); // was zero
        rs = (RoadSegment) segments.get(sn.segmentID);
        if (smiod.rsEnd.distance(rs.getStartPoint()) == 0
            || smiod.rsEnd.distance(rs.getEndPoint()) == 0) {
          if (DEBUG_OD) System.out.println("rsEnd: " + smiod.rsEnd);
          if (DEBUG_OD) System.out.println("Current street: " + rs.printStreetName(sni.streets));

          if (DEBUG_OD) System.out.println("Fixing A* bug...");
          updateNextRoadSegment(smiod, rs);
          smiod.path.remove(0);

        } else {
          break;
        }
      }
    } else if (smiod.config == Constants.MOBILITY_STREET_FLOW) {
      smiod.nextRS = null;
      smiod.nextEnd = null;
    } else if (smiod.config == Constants.MOBILITY_STREET_CIRCUIT) {
      smiod.path = (LinkedList) routes[smiod.routeIndex].clone();
      SegmentNode sn = (SegmentNode) smiod.path.get(0);
      RoadSegment rs = (RoadSegment) segments.get(sn.segmentID);
      updateNextRoadSegment(smiod, rs); // update next road
      smiod.path.remove(0); // already used the first entry
    }
  }