From 0e49ed5199e01525cf04d2ee92997000c16f9900 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Laura=20Kl=C3=BCnder?= <laura@codingcatgirl.de>
Date: Sat, 11 Nov 2023 14:57:16 +0100
Subject: [PATCH] time for some ugly range location testing

---
 src/c3nav/routing/api.py               | 14 ++---
 src/c3nav/routing/rangelocator.py      | 73 +++++++++++++++++++++++++-
 src/c3nav/site/static/site/js/c3nav.js |  4 +-
 3 files changed, 76 insertions(+), 15 deletions(-)

diff --git a/src/c3nav/routing/api.py b/src/c3nav/routing/api.py
index 7fdd3448..9212dbfa 100644
--- a/src/c3nav/routing/api.py
+++ b/src/c3nav/routing/api.py
@@ -16,6 +16,7 @@ from c3nav.routing.exceptions import LocationUnreachable, NoRouteFound, NotYetRo
 from c3nav.routing.forms import RouteForm
 from c3nav.routing.locator import Locator
 from c3nav.routing.models import RouteOptions
+from c3nav.routing.rangelocator import RangeLocator
 from c3nav.routing.router import Router
 
 
@@ -172,14 +173,5 @@ class RoutingViewSet(ViewSet):
 
     @action(detail=False)
     def locate_test(self, request):
-        position = RangingBeacon.objects.select_related('space__level').first()
-        if not position:
-            return Response(None)
-        location = CustomLocation(
-            level=position.space.level,
-            x=position.geometry.x,
-            y=position.geometry.y,
-            permissions=(),
-            icon='my_location'
-        )
-        return Response({'location': location.serialize(simple_geometry=True)})
+        locator = RangeLocator.load()
+        return Response(locator.locate(None, None))
diff --git a/src/c3nav/routing/rangelocator.py b/src/c3nav/routing/rangelocator.py
index 5c05a24d..e4101790 100644
--- a/src/c3nav/routing/rangelocator.py
+++ b/src/c3nav/routing/rangelocator.py
@@ -3,10 +3,15 @@ import threading
 from dataclasses import dataclass
 from typing import Self
 
+import numpy as np
+import scipy
 from django.conf import settings
+from scipy.optimize import least_squares
 
 from c3nav.mapdata.models import MapUpdate
 from c3nav.mapdata.models.geometry.space import RangingBeacon
+from c3nav.mapdata.utils.locations import CustomLocation
+from c3nav.mesh.messages import MeshMessageType
 from c3nav.routing.router import Router
 
 
@@ -35,7 +40,7 @@ class RangeLocator:
                 bssid=beacon.bssid,
                 x=int(beacon.geometry.x * 100),
                 y=int(beacon.geometry.y * 100),
-                z=int((router.altitude_for_point(beacon.space_id, beacon.geometry)+beacon.altitude) * 100),
+                z=int((router.altitude_for_point(beacon.space_id, beacon.geometry)+float(beacon.altitude)) * 100),
             )
 
         locator = cls(beacons=beacons)
@@ -65,4 +70,68 @@ class RangeLocator:
         return cls.cached
 
     def locate(self, scan, permissions=None):
-        return None
+        position = RangingBeacon.objects.select_related('space__level').first()
+        location = CustomLocation(
+            level=position.space.level,
+            x=position.geometry.x,
+            y=position.geometry.y,
+            permissions=(),
+            icon='my_location'
+        )
+
+        from c3nav.mesh.models import MeshNode
+        try:
+            node = MeshNode.objects.prefetch_last_messages(MeshMessageType.LOCATE_RANGE_RESULTS).get(
+                address="d4:f9:8d:2d:0d:f1"
+            )
+        except MeshNode.DoesNotExist:
+            raise
+        msg = node.last_messages[MeshMessageType.LOCATE_RANGE_RESULTS]
+
+        np_data = []
+        for range in msg.parsed.ranges:
+            try:
+                beacon = self.beacons[range.peer]
+            except KeyError:
+                continue
+            np_data.append((beacon.x, beacon.y, beacon.z, range.distance))
+        if len(np_data) < 3:
+            return {
+                "ranges": msg.parsed.tojson(msg.parsed)["ranges"],
+                "datetime": msg.datetime,
+                "location": None,
+            }
+        np_ranges = np.array(np_data)
+
+        def rate_guess(guess):
+            #print(guess)
+            #print(np_ranges[:, :3], guess[:3])
+            #print([float(i) for i in results.x])
+            return scipy.linalg.norm(np_ranges[:, :3]-guess[:3], axis=1)*guess[3]-np_ranges[:, 3]
+
+        initial_guess = np.append(np.average(np_ranges[:, :3], axis=0), 1)
+
+        results = least_squares(rate_guess, initial_guess)
+        #print(results)
+        #print([float(i) for i in results.x])
+
+        from pprint import pprint
+        pprint(msg.parsed.tojson(msg.parsed)["ranges"])
+        location = CustomLocation(
+            level=position.space.level,
+            x=results.x[0]/100,
+            y=results.x[1]/100,
+            permissions=(),
+            icon='my_location'
+        )
+
+        print("measured ranges:", ", ".join(("%.2f" % i) for i in tuple(np_ranges[:, 3])))
+        print("result ranges:", ", ".join(("%.2f" % i) for i in tuple(scipy.linalg.norm(np_ranges[:, :3] - results.x[:3], axis=1) * results.x[3])))
+        print("height:", results.x[2])
+        print("scale:", results.x[3])
+
+        return {
+            "ranges": msg.parsed.tojson(msg.parsed)["ranges"],
+            "datetime": msg.datetime,
+            "location": location.serialize()
+        }
diff --git a/src/c3nav/site/static/site/js/c3nav.js b/src/c3nav/site/static/site/js/c3nav.js
index 0d68b813..7fbae9ef 100644
--- a/src/c3nav/site/static/site/js/c3nav.js
+++ b/src/c3nav/site/static/site/js/c3nav.js
@@ -270,11 +270,11 @@ c3nav = {
     test_location: function() {
         $.getJSON('/api/routing/locate_test/', function(data) {
             console.log(data);
+            window.setTimeout(c3nav.test_location, 1000);
             c3nav._set_user_location(data.location);
-            window.setTimeout(c3nav.test_location, 1000);
         }).fail(function() {
-            c3nav._set_user_location(null);
             window.setTimeout(c3nav.test_location, 1000);
+            c3nav._set_user_location(null);
         });
     },