public void doAlgorithm(Graph busGraph, AllPairsShortestPath allPairsShortestPath) {

    TimeStepper.step();

    this.busGraph = busGraph;
    this.allPairsShortestPath = allPairsShortestPath;

    if (count < 500) {
      count++;
      System.out.println("TimeStep " + count);
      BusCentralDatabase.printStateOfWorld();
    }

    if (BusCentralDatabase.getBusesInTheWorld().size() > 0) {
      // Firstly any bus that is at a stop in which a passenger could be dropped off should do so
      dropOffPassengers();

      // If there are unallocated passengers then we may need to change the paths that buses are
      // currently moving along
      if (BusCentralDatabase.getUnallocatedPassengers().size() > 0) {
        dynamicReroutingAndPassengerAllocation();
      }

      // Pick up passenger if bus is at destination
      pickupPassengers();

      // Tell all the buses to move along their routes to the next location
      incrimentBuses();
    }
  }
  private void incrimentBuses() {

    for (int i = 0; i < BusCentralDatabase.getBusesInTheWorld().size(); i++) {

      Bus bus = BusCentralDatabase.getBusesInTheWorld().get(i);
      bus.moveAlongPath();
    }
  }
  private void dropOffPassengers() {

    ArrayList<Bus> allBuses = BusCentralDatabase.getBusesInTheWorld();

    for (int i = 0; i < allBuses.size(); i++) {

      Bus bus = allBuses.get(i);

      if (bus.getCostToNextStop() == 0) {

        bus.dropOffPassengers();
      }
    }
  }
  private void pickupPassengers() {

    ArrayList<Bus> busesInTheWorld = BusCentralDatabase.getBusesInTheWorld();

    for (int i = 0; i < busesInTheWorld.size(); i++) {

      Bus bus = busesInTheWorld.get(i);

      if (bus.getCostToNextStop() == 0) {

        bus.pickupPassengers(TimeStepper.getTime());
      }
    }
  }
  // Sets the paths for all the buses.
  private void dynamicReroutingAndPassengerAllocation() {

    HashMap<Bus, LinkedList<Vertex>> BusesAndPotentialPaths =
        new HashMap<Bus, LinkedList<Vertex>>();

    /**
     * We need to decide how to reroute our buses when we have new passengers added to the world.
     * Simply perform this computation over all of the buses and store the total cost of doing that.
     */
    for (int q = 0; q < BusCentralDatabase.getBusesInTheWorld().size(); q++) {

      Bus firstBus = BusCentralDatabase.getBusesInTheWorld().get(q);
      // For the rootNode
      Vertex rootNode = firstBus.getCurrentStop();

      // If the bus is between stops then the rootNode will be the next node
      if (firstBus.getCostToNextStop() != 0) {

        rootNode = firstBus.getPath().get(1);
      }

      // Getting the list of pickups. These are the stops the will be part of the picking up of a
      // passenger
      ArrayList<Passenger> pickups =
          (ArrayList<Passenger>) BusCentralDatabase.getUnallocatedPassengers().clone();
      pickups.addAll(firstBus.getAssignedPassengers());
      ArrayList<Vertex> pickupStops = new ArrayList<Vertex>();

      for (int i = 0; i < pickups.size(); i++) {

        if (!pickupStops.contains(pickups.get(i).getStartingStop())) {

          pickupStops.add(pickups.get(i).getStartingStop());
        }
      }

      // Getting the list of dropoff. These include the destination of passengers currently on the
      // bus and passengers
      ArrayList<Vertex> dropoffs = new ArrayList<Vertex>();
      ArrayList<Passenger> busPassengers = firstBus.getPassengersOnBus();
      for (int j = 0; j < busPassengers.size(); j++) {

        if (!dropoffs.contains(busPassengers.get(j).getDestinationStop())) {

          dropoffs.add(busPassengers.get(j).getDestinationStop());
        }
      }

      for (int i = 0; i < pickups.size(); i++) {

        if (!dropoffs.contains(pickups.get(i).getDestinationStop())) {

          dropoffs.add(pickups.get(i).getDestinationStop());
        }
      }

      // This should have added all pickup and dropoff stops and the root. Now must pair the pickups
      // and dropoffs
      ArrayList<ArrayList<Vertex>> pairs = new ArrayList<ArrayList<Vertex>>();
      for (int i = 0; i < pickups.size(); i++) {

        Vertex start = pickups.get(i).getStartingStop();
        Vertex finish = pickups.get(i).getDestinationStop();
        ArrayList<Vertex> tuple = new ArrayList<Vertex>();
        tuple.add(start);
        tuple.add(finish);
        pairs.add(tuple);
      }

      LinkedList<Vertex> optimalPath = bestPath(rootNode, pickupStops, dropoffs, pairs);

      // You can't just do this because the bus may be between some stops

      if (firstBus.getCostToNextStop() > 0) {

        LinkedList<Vertex> path = new LinkedList<Vertex>();
        path.add(firstBus.getCurrentStop());
        path.addAll(optimalPath);
        optimalPath = path;
      }

      BusesAndPotentialPaths.put(firstBus, optimalPath);
    }

    int totalSystemCost = 0;

    for (int i = 0; i < BusCentralDatabase.getBusesInTheWorld().size(); i++) {

      Bus bus = BusCentralDatabase.getBusesInTheWorld().get(i);

      LinkedList<Vertex> path = (LinkedList<Vertex>) bus.getPath().clone();

      int pathCost = 0;

      if (bus.getCostToNextStop() > 0) {
        path.remove(0);
        pathCost = sizeOfPath(path);
        pathCost += bus.getCostToNextStop();
      } else {
        pathCost = sizeOfPath(path);
      }

      if (pathCost > totalSystemCost) {

        totalSystemCost = pathCost;
      }
    }

    HashMap BusesAndCosts = new HashMap<Bus, Integer>();

    for (int i = 0; i < BusesAndPotentialPaths.size(); i++) {

      Bus bus = BusCentralDatabase.getBusesInTheWorld().get(i);
      int pathCost = sizeOfPath(BusesAndPotentialPaths.get(bus));

      if (bus.getCostToNextStop() > 0) {

        LinkedList<Vertex> path = (LinkedList<Vertex>) BusesAndPotentialPaths.get(bus).clone();
        path.remove(0);
        pathCost = sizeOfPath(path);
        pathCost += bus.getCostToNextStop();
      }

      BusesAndCosts.put(bus, pathCost);
    }

    int min = Collections.min(BusesAndCosts.values());

    Iterator it = BusesAndCosts.entrySet().iterator();
    Bus bus = null;
    while (it.hasNext()) {

      Map.Entry<Bus, Integer> pairs = (Map.Entry<Bus, Integer>) it.next();

      if (pairs.getValue() == min) {

        bus = pairs.getKey();
        break;
      }
    }

    bus.setPath(BusesAndPotentialPaths.get(bus));

    int numberOfPickups = BusCentralDatabase.getUnallocatedPassengers().size();

    for (int i = 0; i < numberOfPickups; i++) {

      bus.assignPassenger(BusCentralDatabase.getUnallocatedPassengers().get(0));
    }

    //		System.out.println("TEST");

  }