Merge branch 'HL/#1540-rebalancing-bis' of github.com:LBNL-UCB-STI/be…

…am into HL/#1540-rebalancing-bis

src/main/scala/beam/agentsim/agents/modalbehaviors/ChoosesMode.scala

@@ -956,7 +956,7 @@ trait ChoosesMode {
                       .head
                 }
               val expensiveWalkTrip = EmbodiedBeamTrip(
-                Vector(originalWalkTripLeg.copy(cost = 100))
+                Vector(originalWalkTripLeg.copy(replanningPenalty = 100.0))
               )
 
               goto(FinishingModeChoice) using choosesModeData.copy(

src/main/scala/beam/agentsim/agents/modalbehaviors/ModeChoiceCalculator.scala

@@ -89,7 +89,7 @@ trait ModeChoiceCalculator extends HasServices {
       case _ =>
         embodiedBeamTrip.costEstimate
     }
-    totalCost
+    totalCost + embodiedBeamTrip.replanningPenalty
   }
 
   def computeAllDayUtility(

src/main/scala/beam/router/model/EmbodiedBeamLeg.scala

@@ -14,7 +14,8 @@ case class EmbodiedBeamLeg(
   asDriver: Boolean,
   cost: Double,
   unbecomeDriverOnCompletion: Boolean,
-  isPooledTrip: Boolean = false
+  isPooledTrip: Boolean = false,
+  replanningPenalty: Double = 0
 ) {
 
   val isHumanBodyVehicle: Boolean =

src/main/scala/beam/router/model/EmbodiedBeamTrip.scala

@@ -33,6 +33,9 @@ case class EmbodiedBeamTrip(legs: IndexedSeq[EmbodiedBeamLeg]) {
     !_.asDriver
   )
 
+  @transient
+  lazy val replanningPenalty: Double = legs.map(_.replanningPenalty).sum
+
   val totalTravelTimeInSecs: Int = legs.lastOption.map(_.beamLeg.endTime - legs.head.beamLeg.startTime).getOrElse(0)
 
   def beamLegs(): IndexedSeq[BeamLeg] =

src/main/scala/beam/sim/common/GeoUtils.scala

@@ -2,15 +2,20 @@ package beam.sim.common
 
 import beam.agentsim.events.SpaceTime
 import beam.sim.config.BeamConfig
+import beam.utils.ProfilingUtils
 import beam.utils.logging.ExponentialLazyLogging
+import beam.utils.map.GpxPoint
 import com.conveyal.r5.profile.StreetMode
-import com.conveyal.r5.streets.{Split, StreetLayer}
+import com.conveyal.r5.streets.{EdgeStore, Split, StreetLayer}
 import com.google.inject.{ImplementedBy, Inject}
-import com.vividsolutions.jts.geom.Envelope
+import com.vividsolutions.jts.geom.{Coordinate, Envelope}
+import org.matsim.api.core.v01
 import org.matsim.api.core.v01.Coord
 import org.matsim.api.core.v01.network.Link
 import org.matsim.core.utils.geometry.transformations.GeotoolsTransformation
 
+case class EdgeWithCoord(edgeIndex: Int, wgsCoord: Coordinate)
+
 /**
   * Created by sfeygin on 4/2/17.
   */
@@ -64,7 +69,21 @@ trait GeoUtils extends ExponentialLazyLogging {
   def getNearestR5Edge(streetLayer: StreetLayer, coordWGS: Coord, maxRadius: Double = 1E5): Int = {
     val theSplit = getR5Split(streetLayer, coordWGS, maxRadius, StreetMode.WALK)
     if (theSplit == null) {
-      Int.MinValue
+      val closestEdgesToTheCorners = ProfilingUtils
+        .timed("getEdgesCloseToBoundingBox", x => logger.info(x)) {
+          getEdgesCloseToBoundingBox(streetLayer)
+        }
+        .map { case (edgeWithCoord, gpxPoint) => edgeWithCoord }
+      val closest = closestEdgesToTheCorners.minBy { edge =>
+        val matsimUtmCoord = wgs2Utm(new v01.Coord(edge.wgsCoord.x, edge.wgsCoord.y))
+        distUTMInMeters(matsimUtmCoord, wgs2Utm(coordWGS))
+      }
+      val distUTM = distUTMInMeters(wgs2Utm(coordWGS), wgs2Utm(new v01.Coord(closest.wgsCoord.x, closest.wgsCoord.y)))
+      logger.warn(
+        s"""The split is `null` for StreetLayer.BoundingBox: ${streetLayer.getEnvelope}, coordWGS: $coordWGS, maxRadius: $maxRadius.
+           | Will return closest to the corner: $closest which is $distUTM meters far away""".stripMargin
+      )
+      closest.edgeIndex
     } else {
       theSplit.edge
     }
@@ -106,6 +125,69 @@ trait GeoUtils extends ExponentialLazyLogging {
     }
     theSplit
   }
+
+  def getEdgesCloseToBoundingBox(streetLayer: StreetLayer): Array[(EdgeWithCoord, GpxPoint)] = {
+    val cursor = streetLayer.edgeStore.getCursor()
+    val iter = new Iterator[EdgeStore#Edge] {
+      override def hasNext: Boolean = cursor.advance()
+
+      override def next(): EdgeStore#Edge = cursor
+    }
+
+    val boundingBox = streetLayer.envelope
+
+    val insideBoundingBox = iter
+      .flatMap { edge =>
+        Option(edge.getGeometry.getBoundary.getCoordinate).map { coord =>
+          EdgeWithCoord(edge.getEdgeIndex, coord)
+        }
+      }
+      .withFilter(x => boundingBox.contains(x.wgsCoord))
+      .toArray
+
+    /*
+    min => x0,y0
+    max => x1,y1
+x0,y1 (TOP LEFT)    ._____._____. x1,y1 (TOP RIGHT)
+                    |           |
+                    |           |
+                    .           .
+                    |           |
+                    |           |
+x0,y0 (BOTTOM LEFT) ._____._____. x1, y0 (BOTTOM RIGHT)
+     */
+
+    val bottomLeft = new Coord(boundingBox.getMinX, boundingBox.getMinY)
+    val topLeft = new Coord(boundingBox.getMinX, boundingBox.getMaxY)
+    val topRight = new Coord(boundingBox.getMaxX, boundingBox.getMaxY)
+    val bottomRight = new Coord(boundingBox.getMaxX, boundingBox.getMinY)
+    val midLeft = new Coord((bottomLeft.getX + topLeft.getX) / 2, (bottomLeft.getY + topLeft.getY) / 2)
+    val midTop = new Coord((topLeft.getX + topRight.getX) / 2, (topLeft.getY + topRight.getY) / 2)
+    val midRight = new Coord((topRight.getX + bottomRight.getX) / 2, (topRight.getY + bottomRight.getY) / 2)
+    val midBottom = new Coord((bottomLeft.getX + bottomRight.getX) / 2, (bottomLeft.getY + bottomRight.getY) / 2)
+
+    val corners = Array(
+      GpxPoint("BottomLeft", bottomLeft),
+      GpxPoint("TopLeft", topLeft),
+      GpxPoint("TopRight", topRight),
+      GpxPoint("BottomRight", bottomRight),
+      GpxPoint("MidLeft", midLeft),
+      GpxPoint("MidTop", midTop),
+      GpxPoint("MidRight", midRight),
+      GpxPoint("MidBottom", midBottom)
+    )
+
+    val closestEdges = corners.map { gpxPoint =>
+      val utmCornerCoord = wgs2Utm(gpxPoint.wgsCoord)
+      val closestEdge: EdgeWithCoord = insideBoundingBox.minBy {
+        case x =>
+          val utmCoord = wgs2Utm(new Coord(x.wgsCoord.x, x.wgsCoord.y))
+          distUTMInMeters(utmCornerCoord, utmCoord)
+      }
+      (closestEdge, gpxPoint)
+    }
+    closestEdges
+  }
 }
 
 object GeoUtils {
@@ -217,6 +299,7 @@ object GeoUtils {
       rad
     }
   }
+
 }
 
 class GeoUtilsImpl @Inject()(val beamConfig: BeamConfig) extends GeoUtils {

src/main/scala/beam/utils/map/EnvelopeToGpx.scala

@@ -0,0 +1,69 @@
+package beam.utils.map
+
+import com.typesafe.scalalogging.LazyLogging
+import com.vividsolutions.jts.geom.Envelope
+import org.matsim.api.core.v01.Coord
+
+class EnvelopeToGpx extends LazyLogging {
+
+  val geoUtils = new beam.sim.common.GeoUtils {
+    override def localCRS: String = "epsg:26910"
+  }
+
+  def render(envelope: Envelope, wgsCoord: Coord, outputPath: String): Unit = {
+    val start = System.currentTimeMillis()
+
+    // We have min(x0, y0) and max(x1,y1). Need to add two extra points to draw rectangle
+    /*
+    min => x0,y0
+    max => x1,y1
+x0,y1 .___________. x1,y1
+      |           |
+      |           |
+      |           |
+      |           |
+x0,y0 .___________. x1, y0
+     */
+
+    val envelopePoints = Array[GpxPoint](
+      GpxPoint("x0,y0", new Coord(envelope.getMinX, envelope.getMinY)),
+      GpxPoint("x0,y1", new Coord(envelope.getMinX, envelope.getMaxY)),
+      GpxPoint("x1,y1", new Coord(envelope.getMaxX, envelope.getMaxY)),
+      GpxPoint("x1,y0", new Coord(envelope.getMaxX, envelope.getMinY))
+    )
+
+    val gpxWriter = new GpxWriter(outputPath, geoUtils)
+    try {
+
+      val middle = GpxPoint(
+        "Middle",
+        new Coord((envelope.getMinX + envelope.getMaxX) / 2, (envelope.getMinY + envelope.getMaxY) / 2)
+      )
+      gpxWriter.drawMarker(middle)
+      val searchPoint = GpxPoint("Search", wgsCoord)
+      gpxWriter.drawMarker(searchPoint)
+      gpxWriter.drawSourceToDest(middle, searchPoint)
+
+      envelopePoints.foreach(point => gpxWriter.drawMarker(point))
+
+      gpxWriter.drawRectangle(envelopePoints)
+
+    } finally {
+      gpxWriter.close()
+    }
+    val end = System.currentTimeMillis()
+    logger.info(s"Created '$outputPath' in ${end - start} ms")
+  }
+}
+
+object EnvelopeToGpx {
+  def longitude(coord: Coord): Double = coord.getX
+
+  def latitude(coord: Coord): Double = coord.getY
+
+  def main(args: Array[String]): Unit = {
+    val en1 = new Envelope(-122.5447336, -122.3592068, 37.6989794, 37.843628)
+    val envelopeToGpx = new EnvelopeToGpx
+    envelopeToGpx.render(en1, new Coord(-123.180062255, 38.7728279981), "ex1.gpx")
+  }
+}

src/main/scala/beam/utils/GeoJsonToGpxConvertor.scala → src/main/scala/beam/utils/map/GeoJsonToGpxConvertor.scala

@@ -1,4 +1,6 @@
-package beam.utils
+package beam.utils.map
+
+import beam.utils.GeoJsonReader
 import com.typesafe.scalalogging.LazyLogging
 import com.vividsolutions.jts.geom.Geometry
 import org.matsim.api.core.v01.Coord