content: /* amodeus - Copyright (c) 2018, ETH Zurich, Institute for Dynamic Systems and Control */
  package amodeus.amodeus.dispatcher.shared.fifs;
  
  import java.util.Collection;
  import java.util.HashSet;
  import java.util.Set;
  
  import org.matsim.api.core.v01.Coord;
  import org.matsim.api.core.v01.network.Link;
  import org.matsim.api.core.v01.network.Network;
  import org.matsim.core.network.NetworkUtils;
  
  import amodeus.amodeus.dispatcher.core.RoboTaxi;
  import amodeus.amodeus.dispatcher.util.TreeMaintainer;
  import ch.ethz.idsc.tensor.Tensor;
  import ch.ethz.idsc.tensor.Tensors;
  
  /* package */ class RoboTaxiHandler {
  
      private final TreeMaintainer<RoboTaxi> allRoboTaxis;
      private final Set<RoboTaxi> unassignedRoboTaxis = new HashSet<>();
      private final double maxSpeed;
  
      public RoboTaxiHandler(Network network) {
          maxSpeed = maxSpeed(network);
          double[] networkBounds = NetworkUtils.getBoundingBox(network.getNodes().values());
          this.allRoboTaxis = new TreeMaintainer<>(networkBounds, this::getRoboTaxiLoc);
      }
  
      public void update(Collection<RoboTaxi> newAllRoboTaxis, Collection<RoboTaxi> unassignedRoboTaxis) {
          newAllRoboTaxis.forEach(allRoboTaxis::add);
          this.unassignedRoboTaxis.addAll(unassignedRoboTaxis);
      }
  
      public void assign(RoboTaxi roboTaxi) {
          unassignedRoboTaxis.remove(roboTaxi);
      }
  
      public void clear() {
          unassignedRoboTaxis.clear();
          allRoboTaxis.clear();
      }
  
      public Set<RoboTaxi> getUnassignedRoboTaxis() {
          return unassignedRoboTaxis;
      }
  
      /** @param roboTaxi
       * @return {@link Coord} with {@link RoboTaxi} location */
      private Tensor getRoboTaxiLoc(RoboTaxi roboTaxi) {
          Coord coord = roboTaxi.getDivertableLocation().getCoord();
          return Tensors.vector(coord.getX(), coord.getY());
      }
  
      private static double maxSpeed(Network network) {
          return network.getLinks().values().stream() //
                  .mapToDouble(Link::getFreespeed) //
                  .max() //
                  .getAsDouble();
      }
  
      public Collection<RoboTaxi> getRoboTaxisWithinFreeSpeedDisk(Coord coord, double maxTime) {
          double distance = maxTime * maxSpeed;
          return allRoboTaxis.disk(coord.getX(), coord.getY(), distance);
      }
  }
  