/** @return vehicle waiting in the corner */
  private Vehicle waitingVehicle(int nextPos) {

    int pos = findOtherStreetIntersect(adjacentStreets.get(nextPos));
    int waitingPos = pos - 1;

    for (Vehicle vehicle : adjacentStreets.get(nextPos).getVehicles())
      if (vehicle.getPos() == waitingPos) return vehicle;

    return null;
  }
  private boolean carsInFront(int nextPos) {

    locked = true;

    for (Vehicle vehicle : vehicles)
      if (nextPos == vehicle.getPos()) {
        locked = false;
        return true;
      }

    locked = false;

    return false;
  }
  public void advance(Vehicle vehicle) {

    vehicle.setRunning(true);
    // Verify Intersection
    int nextPos = 1 + (vehicle.getPos());
    boolean intersect = false;
    Vehicle otherVehicle;
    for (int pos : adjacentStreets.keySet()) {

      //	System.out.print("Pos:"+pos + "==" + "nextPos"+nextPos);
      if (nextPos == pos) {

        intersect = true;
        break;
      }
    }
    System.out.println(vehicle.getName() + ":");
    // IF intersect
    if (intersect) {
      // IF free intersect is available
      if (freeIntersection(nextPos))
        // If nobody is waiting for the intersect. Vehicle arrive first
        // so
        // apply FIFO
        if ((otherVehicle = waitingVehicle(nextPos)) == null) { // nobody

          System.out.println(" Nobody is waiting");
          // is
          // waiting
          moveThruIntersection(vehicle, nextPos);
        } else if (IntersectPolicy.policy(vehicle, otherVehicle)) {
          moveThruIntersection(vehicle, nextPos);
          System.out.println("Policy Say Move First");
        } else {
          // Policy say I must stop
          vehicle.setRunning(false);
          vehicle.setStoppedByPolicy(true);
          System.out.println("Policy say stop");
        }
    } else if (carsInFront(nextPos)) {
      vehicle.setRunning(false);
      System.out.println("Car in front");
    } else {
      System.out.println("Move");
      vehicle.setPos(nextPos);
    }
  }