1 files changed, 182 insertions, 0 deletions
diff --git a/src/host/layer23/src/misc/locate.c b/src/host/layer23/src/misc/locate.c
new file mode 100644
index 00000000..eb37285e
--- /dev/null
+++ b/src/host/layer23/src/misc/locate.c
@@ -0,0 +1,182 @@
+/* Algorithm to locate a destination by distance measurement:
+ */
+
+#include <stdio.h>
+#include <errno.h>
+#include <math.h>
+
+#include <osmocom/bb/misc/geo.h>
+#include <osmocom/bb/misc/locate.h>
+
+#define CIRCLE_PROBE	30.0
+#define FINETUNE_RADIUS	5.0
+
+extern double debug_long, debug_lat, debug_x_scale;
+extern FILE *debug_fp;
+extern int log_debug;
+
+static double finetune_x[6], finetune_y[6], finetune_dist[6];
+
+int locate_cell(struct probe *probe_first, double *min_x, double *min_y)
+{
+	struct probe *probe, *min_probe;
+	int i, test_steps, optimized;
+	double min_dist, dist, x, y, rad, temp;
+	double circle_probe, finetune_radius;
+
+	/* convert meters into degrees */
+	circle_probe = CIRCLE_PROBE / (EQUATOR_RADIUS * PI / 180.0);
+	finetune_radius = FINETUNE_RADIUS / (EQUATOR_RADIUS * PI / 180.0);
+
+	if (log_debug) {
+		fprintf(debug_fp, "<Folder>\n");
+		fprintf(debug_fp, "\t<name>Debug Locator</name>\n");
+		fprintf(debug_fp, "\t<open>0</open>\n");
+		fprintf(debug_fp, "\t<visibility>0</visibility>\n");
+	}
+
+	/* get probe of minimum distance */
+	min_probe = NULL;
+	probe = probe_first;
+	min_dist = 42;
+	i = 0;
+	while (probe) {
+		if (log_debug) {
+			fprintf(debug_fp, "\t<Placemark>\n");
+			fprintf(debug_fp, "\t\t<name>MEAS</name>\n");
+			fprintf(debug_fp, "\t\t<visibility>0</visibility>\n");
+			fprintf(debug_fp, "\t\t<LineString>\n");
+			fprintf(debug_fp, "\t\t\t<tessellate>1</tessellate>\n");
+			fprintf(debug_fp, "\t\t\t<coordinates>\n");
+			rad = 2.0 * 3.1415927 / 35;
+			for (i = 0; i < 35; i++) {
+				x = probe->x + probe->dist * sin(rad * i);
+				y = probe->y + probe->dist * cos(rad * i);
+				fprintf(debug_fp, "%.8f,%.8f\n", debug_long +
+					x * debug_x_scale, debug_lat + y);
+			}
+			fprintf(debug_fp, "\t\t\t</coordinates>\n");
+			fprintf(debug_fp, "\t\t</LineString>\n");
+			fprintf(debug_fp, "\t</Placemark>\n");
+		}
+
+		if (!min_probe || probe->dist < min_dist) {
+			min_probe = probe;
+			min_dist = probe->dist;
+		}
+		probe = probe->next;
+		i++;
+	}
+
+	if (i < 3) {
+		fprintf(stderr, "Need at least 3 points\n");
+		return -EINVAL;
+	}
+
+	/* calculate the number of steps to search for destination point */
+	test_steps = 2.0 * 3.1415927 * min_probe->dist / circle_probe;
+	rad = 2.0 * 3.1415927 / test_steps;
+
+	if (log_debug) {
+		fprintf(debug_fp, "\t<Placemark>\n");
+		fprintf(debug_fp, "\t\t<name>Smallest MEAS</name>\n");
+		fprintf(debug_fp, "\t\t<visibility>0</visibility>\n");
+		fprintf(debug_fp, "\t\t<LineString>\n");
+		fprintf(debug_fp, "\t\t\t<tessellate>1</tessellate>\n");
+		fprintf(debug_fp, "\t\t\t<coordinates>\n");
+	}
+
+	/* search on a circle for the location of the lowest distance
+	 * to the radius with the greatest distance */
+	min_dist = 42;
+	*min_x = *min_y = 42;
+	for (i = 0; i < test_steps; i++) {
+		x = min_probe->x + min_probe->dist * sin(rad * i);
+		y = min_probe->y + min_probe->dist * cos(rad * i);
+		if (log_debug)
+			fprintf(debug_fp, "%.8f,%.8f\n", debug_long +
+				x * debug_x_scale, debug_lat + y);
+		/* look for greatest distance */
+		dist = 0;
+		probe = probe_first;
+		while (probe) {
+			if (probe != min_probe) {
+				/* distance to the radius */
+				temp = distonplane(probe->x, probe->y, x, y);
+				temp -= probe->dist;
+				if (temp < 0)
+					temp = -temp;
+				if (temp > dist)
+					dist = temp;
+			}
+			probe = probe->next;
+		}
+		if (i == 0 || dist < min_dist) {
+			min_dist = dist;
+			*min_x = x;
+			*min_y = y;
+		}
+	}
+
+	if (log_debug) {
+		fprintf(debug_fp, "\t\t\t</coordinates>\n");
+		fprintf(debug_fp, "\t\t</LineString>\n");
+		fprintf(debug_fp, "\t</Placemark>\n");
+
+		fprintf(debug_fp, "\t<Placemark>\n");
+		fprintf(debug_fp, "\t\t<name>Finetune</name>\n");
+		fprintf(debug_fp, "\t\t<visibility>0</visibility>\n");
+		fprintf(debug_fp, "\t\t<LineString>\n");
+		fprintf(debug_fp, "\t\t\t<tessellate>1</tessellate>\n");
+		fprintf(debug_fp, "\t\t\t<coordinates>\n");
+	}
+
+	min_dist = 9999999999.0;
+tune_again:
+	if (log_debug)
+		fprintf(debug_fp, "%.8f,%.8f\n", debug_long +
+			*min_x * debug_x_scale, debug_lat + *min_y);
+
+	/* finetune the point */
+	rad = 2.0 * 3.1415927 / 6;
+	for (i = 0; i < 6; i++) {
+		x = *min_x + finetune_radius * sin(rad * i);
+		y = *min_y + finetune_radius * cos(rad * i);
+		/* search for the point with the lowest sum of distances */
+		dist = 0;
+		probe = probe_first;
+		while (probe) {
+			/* distance to the radius */
+			temp = distonplane(probe->x, probe->y, x, y);
+			temp -= probe->dist;
+			if (temp < 0)
+				temp = -temp;
+			dist += temp;
+			probe = probe->next;
+		}
+		finetune_dist[i] = dist;
+		finetune_x[i] = x;
+		finetune_y[i] = y;
+	}
+
+	optimized = 0;
+	for (i = 0; i < 6; i++) {
+		if (finetune_dist[i] < min_dist) {
+			min_dist = finetune_dist[i];
+			*min_x = finetune_x[i];
+			*min_y = finetune_y[i];
+			optimized = 1;
+		}
+	}
+	if (optimized)
+		goto tune_again;
+
+	if (log_debug) {
+		fprintf(debug_fp, "\t\t\t</coordinates>\n");
+		fprintf(debug_fp, "\t\t</LineString>\n");
+		fprintf(debug_fp, "\t</Placemark>\n");
+		fprintf(debug_fp, "</Folder>\n");
+	}
+
+	return 0;
+}