private Vehicle findClosestWillingVehicle(TaxiRequest req) {
    List<Vehicle> vehicles = new ArrayList<Vehicle>(context.getVrpData().getVehicles().values());
    Collections.shuffle(vehicles, rnd);

    Vehicle bestVeh = null;
    //          double bestDistance = Double.MAX_VALUE;
    double bestDistance = Double.MAX_VALUE / 2;
    for (Vehicle veh : Iterables.filter(vehicles, TaxiSchedulerUtils.createIsIdle(scheduler))) {
      if (this.IsElectric) if (!this.hasEnoughCapacityForTask(veh)) continue;
      if (!vehicleWillingToServeRequest(veh, req)) continue;
      double distance = calculateSquaredDistance(req, veh);
      if (distance < bestDistance) {

        bestDistance = distance;
        bestVeh = veh;

      } else if (distance == bestDistance) {

        if (bestVeh == null) {
          bestVeh = veh;
          continue;
        }
        if (veh.getSchedule().getCurrentTask().getBeginTime()
            < bestVeh.getSchedule().getCurrentTask().getBeginTime()) bestVeh = veh;

        // FIFO, if distance is equal
      }
    }

    return bestVeh;
  }
  private Vehicle findHighestChargedIdleVehicleDistanceSort(TaxiRequest req) {
    Vehicle bestVeh = null;
    double bestSoc = 0;

    List<Vehicle> vehicles = new ArrayList<Vehicle>(context.getVrpData().getVehicles().values());
    Collections.shuffle(vehicles, rnd);

    for (Vehicle veh : Iterables.filter(vehicles, TaxiSchedulerUtils.createIsIdle(scheduler))) {
      if (this.IsElectric) if (!this.hasEnoughCapacityForTask(veh)) continue;
      double soc = this.getVehicleSoc(veh);
      if (soc > bestSoc) {
        bestSoc = soc;
        bestVeh = veh;
      } else if (soc == bestSoc) {
        if (bestVeh == null) {
          bestVeh = veh;
          continue;
        }
        if (this.calculateSquaredDistance(req, veh) < this.calculateSquaredDistance(req, bestVeh)) {
          bestVeh = veh;
        }
      }
    }

    return bestVeh;
  }