From 27f585ff51ec2c0ecd2caadac70646a32cb56615 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Laura=20Kl=C3=BCnder?= <laura@codingcatgirl.de>
Date: Fri, 27 Dec 2024 16:03:13 +0100
Subject: [PATCH] fix locate_range level getting

---
 src/c3nav/routing/locator.py | 11 +++++------
 src/c3nav/routing/router.py  | 17 +++++++++++++++--
 2 files changed, 20 insertions(+), 8 deletions(-)

diff --git a/src/c3nav/routing/locator.py b/src/c3nav/routing/locator.py
index eaff4ca6..acab7d70 100644
--- a/src/c3nav/routing/locator.py
+++ b/src/c3nav/routing/locator.py
@@ -288,7 +288,7 @@ class Locator:
         # initial guess i the average of all beacons, with scale 1
         initial_guess = np.average(np_ranges[:, :dimensions], axis=0)
 
-        # here the magic happens
+        # here the magic happen
         results = self.least_squares_func(
             fun=cost_func,
             # jac="3-point",
@@ -301,11 +301,12 @@ class Locator:
         )
 
         # create result
-        # todo: figure out level
+        router = Router.load()
+        restrictions = router.get_restrictions(permissions)
+
         result_pos = results.x
-        from c3nav.mapdata.models import Level
         location = CustomLocation(
-            level=Level.objects.first(),
+            level=router.levels[router.level_id_for_xyz(result_pos, restrictions)],
             x=result_pos[0]/100,
             y=result_pos[1]/100,
             permissions=(),
@@ -313,8 +314,6 @@ class Locator:
         )
         location.z = result_pos[2]/100
 
-        pprint(relevant_xyz)
-
         orig_xyz = None
         print('orig_addr', orig_addr)
         if orig_addr:
diff --git a/src/c3nav/routing/router.py b/src/c3nav/routing/router.py
index 48297977..8bcfde02 100644
--- a/src/c3nav/routing/router.py
+++ b/src/c3nav/routing/router.py
@@ -5,6 +5,7 @@ from collections import deque, namedtuple
 from dataclasses import dataclass, field
 from functools import reduce
 from itertools import chain
+from operator import itemgetter
 from typing import Optional, TypeVar, Generic, Mapping, Any, Sequence, TypeAlias, ClassVar, NamedTuple
 
 import numpy as np
@@ -391,7 +392,7 @@ class Router:
             raise LocationUnreachable
         return result
 
-    def space_for_point(self, level: int, point: PointCompatible, restrictions) -> Optional['RouterSpace']:
+    def space_for_point(self, level: int, point: PointCompatible, restrictions, max_distance=20) -> Optional['RouterSpace']:
         point = Point(point.x, point.y)
         level = self.levels[level]
         excluded_spaces = restrictions.spaces if restrictions else ()
@@ -402,7 +403,7 @@ class Router:
                 return self.spaces[space]
         spaces = (self.spaces[space] for space in level.spaces if space not in excluded_spaces)
         spaces = ((space, space.geometry.distance(point)) for space in spaces)
-        spaces = tuple((space, distance) for space, distance in spaces if distance < 20)
+        spaces = tuple((space, distance) for space, distance in spaces if distance < max_distance)
         if not spaces:
             return None
         return min(spaces, key=operator.itemgetter(1))[0]
@@ -410,6 +411,18 @@ class Router:
     def altitude_for_point(self, space: int, point: PointCompatible) -> float:
         return self.spaces[space].altitudearea_for_point(point).get_altitude(point)
 
+    def level_id_for_xyz(self, xyz: tuple[float, float, float], restrictions):
+        xy = Point(xyz[0], xyz[1])
+        z = xyz[2]
+        possible_levels = {}
+        for level_id, level in self.levels.items():
+            space = self.space_for_point(level=level_id, point=xy, restrictions=restrictions)
+            if space:
+                possible_levels[level_id] = abs(space.altitudearea_for_point(xy).get_altitude(xy)-z)
+        if possible_levels:
+            return min(possible_levels.items(), key=itemgetter(1))[0]
+        return min(self.levels.items(), key=lambda a: abs(float(a[1].base_altitude)-z))[0]
+
     def describe_custom_location(self, location):
         restrictions = self.get_restrictions(location.permissions)
         space = self.space_for_point(level=location.level.pk, point=location, restrictions=restrictions)