Androidでマップマッチングを行いたい

	public int calculateCurrentRoute(@NonNull Location currentLocation, double posTolerance,
	                                 @NonNull List<Location> routeNodes, int currentRoute,
	                                 boolean updateAndNotify) {
		while (currentRoute + 1 < routeNodes.size()) {
			// 現在地から現在のルートまでの距離を計算
			double dist = currentLocation.distanceTo(routeNodes.get(currentRoute));
			if (currentRoute > 0) {
				// 現在地から前のルートまでの直交距離を計算
				dist = RoutingHelperUtils.getOrthogonalDistance(currentLocation, routeNodes.get(currentRoute - 1),
						routeNodes.get(currentRoute));
			}
			boolean processed = false;
			// if we are still too far try to proceed many points
			// if not then look ahead only 3 in order to catch sharp turns
			boolean longDistance = dist >= 250;
			// 直交距離が最小となる次のルートを探す
			int newCurrentRoute = RoutingHelperUtils.lookAheadFindMinOrthogonalDistance(currentLocation, routeNodes, currentRoute, longDistance ? 15 : 8);
			double newDist = RoutingHelperUtils.getOrthogonalDistance(currentLocation, routeNodes.get(newCurrentRoute),
					routeNodes.get(newCurrentRoute + 1));
			if (longDistance) { // ルートポイントから現在地までの距離が250m以上の場合
				if (newDist < dist) { // 次のルートポイントまでの距離の方が近い場合
					if (log.isDebugEnabled()) {
						log.debug("Processed by distance : (new) " + newDist + " (old) " + dist); //$NON-NLS-1$//$NON-NLS-2$
					}
					// 通過済みにする
					processed = true;
				}
			} else if (newDist < dist || newDist < posTolerance / 8) { // 右辺は交差点などで次のポイントが近い場合を補足
				// 次のルートポイントまでの距離の方が近い or 前の方が近いけど、次のルートポイントまでの距離が極端に近い　posTolerance / 8以下の場合
				// newDist < posTolerance / 8 - 4-8 m (avoid distance 0 till next turn)
				if (dist > posTolerance) { // 現在のルートポイントから十分に離れている場合
					processed = true;
					if (log.isDebugEnabled()) {
						log.debug("Processed by distance : " + newDist + " " + dist); //$NON-NLS-1$//$NON-NLS-2$
					}
				} else {
					if (currentLocation.hasBearing() && !deviceHasBearing) { // デバイスが方位情報を持っているかチェック
						deviceHasBearing = true;
					}
					// lastFixedLocation.bearingTo -  gives artefacts during u-turn, so we avoid for devices with bearing
					if ((currentRoute > 0 || newCurrentRoute > 0) &&
							(currentLocation.hasBearing() || (!deviceHasBearing && lastFixedLocation != null))) {
						float bearingToRoute = currentLocation.bearingTo(routeNodes.get(currentRoute));
						float bearingRouteNext = routeNodes.get(newCurrentRoute).bearingTo(routeNodes.get(newCurrentRoute + 1));
						float bearingMotion = currentLocation.hasBearing() ? currentLocation.getBearing() : lastFixedLocation
								.bearingTo(currentLocation);
						double diff = Math.abs(MapUtils.degreesDiff(bearingMotion, bearingToRoute));
						double diffToNext = Math.abs(MapUtils.degreesDiff(bearingMotion, bearingRouteNext));
						if (diff > diffToNext) { //デバイスの進行方向と次のルートポイントの方向が一致している場合は通過
							if (log.isDebugEnabled()) {
								log.debug("Processed point bearing deltas : " + diff + " " + diffToNext);
							}
							processed = true;
						}
					}
				}
			}
			if (processed) {
				// that node already passed
				currentRoute = newCurrentRoute + 1;
				if (updateAndNotify) {
					route.updateCurrentRoute(newCurrentRoute + 1);
					app.getNotificationHelper().refreshNotification(NotificationType.NAVIGATION);
					fireRoutingDataUpdateEvent();
				}
			} else {
				break; // 通過済みじゃなければbreakしてその値をnewRouteにする
			}
		}
		return currentRoute;
	}

  fun findCurrentRoutePoint(location: Location): RoutePoint {
    var currentRouteIdx = currentPointIdx
    val currentLocationCoordinate = Coordinate(location.longitude, location.latitude)

    while (currentRouteIdx + 1 < points.size) {
      val distanceToCurrentRoutePoint =
        calculateDistanceToRoutePoint(currentRouteIdx, currentLocationCoordinate)
      val longDistanceFromCurrentRoutePoint = distanceToCurrentRoutePoint > 250
      val (newRouteIdx, distanceToNewCurrentRoute) = findLookAhead(
        location,
        currentRouteIdx,
        points,
        if (longDistanceFromCurrentRoutePoint) 15 else 8
      )
      val shouldCheck =
        distanceToNewCurrentRoute < distanceToCurrentRoutePoint || distanceToNewCurrentRoute < 8

      println("currentRoute: ${points[currentRouteIdx].name}")
      println("distanceToCurrentRoutePoint: $distanceToCurrentRoutePoint")
      println("newRoute: ${points[newRouteIdx].name}")
      println("distanceToNewCurrentRoute: $distanceToNewCurrentRoute")

      if (!shouldCheck) break

      if (longDistanceFromCurrentRoutePoint) {
        currentRouteIdx = newRouteIdx
        continue
      }

      // 近距離の場合のみ、bearingのチェックを行う
      val shouldUpdateRoute = checkBearingForClosePoint(
        currentLocationCoordinate,
        location,
        points,
        currentRouteIdx,
        newRouteIdx
      )

      if (shouldUpdateRoute) {
        currentRouteIdx = newRouteIdx
      } else {
        break
      }
    }
    val result = points[currentRouteIdx]
    println("result: ${result.name}")
    return points[currentRouteIdx]
  }

    @MockK
    private lateinit var mockLocation: Location

    private val routePoints = listOf(
        RoutePoint(35.6895, 139.6917, "新宿"),
        RoutePoint(35.692223, 139.7034052, "新宿３丁目"),
        RoutePoint(35.6968759, 139.7063985, "新宿７丁目セブン"),
        RoutePoint(35.7030794, 139.7392931, "飯田橋")
    )

    @Test
    fun 位置情報の更新をテスト() = runTest {
        every { mockLocation.latitude } returns 35.7003262
        every { mockLocation.longitude } returns 139.7333708
        every { mockLocation.accuracy } returns 10.0f
        every { mockLocation.hasAccuracy() } returns true
        every { mockLocation.bearing } returns 45f
        val progress = RouteProgress(routePoints, "Test Route", "Test Description")

        val currentRoutePoint = progress.findCurrentRoutePoint(mockLocation)

        assertNotNull(currentRoutePoint)
        assertEquals(35.7030794, currentRoutePoint.latitude, 0.0001)
        assertEquals(139.7392931, currentRoutePoint.longitude, 0.0001)
    }