some fixes [multithreading,..]

needed interface changes [new options]
logger for android
wifi-ap-optimization
new test-cases

Assertions.h

@@ -64,7 +64,10 @@ namespace Assert {
 	}
 
 	template <typename T, typename STR> static inline void isNear(const T v1, const T v2, const T delta, const STR err) {
-		if (std::abs(v1-v2) > delta) {doThrow(err);}
+		if (std::abs(v1-v2) > delta) {
+			std::stringstream ss; ss << "\nexpected " << v1 << " +/- " << delta << " but is " << v2 << "\n";
+			doThrow(err+ss.str());
+		}
 	}
 
 	template <typename T, typename STR> static inline void isBetween(const T v, const T min, const T max, const STR err) {

data/Timestamp.h

@@ -1,6 +1,8 @@
 #ifndef TIMESTAMP_H
 #define TIMESTAMP_H
 
+#include <chrono>
+
 /**
  * helper-class to handle timestamps
  */
@@ -25,16 +27,32 @@ public:
 	/** get timestamp from the given value which represents seconds */
 	static inline Timestamp fromSec(const float sec)	{return Timestamp(sec*1000);}
 
+	/** get timestamp for the current unix-time */
+	static inline Timestamp fromUnixTime() {
+		auto now = std::chrono::system_clock::now();
+		auto duration = now.time_since_epoch();
+		auto millis = std::chrono::duration_cast<std::chrono::milliseconds>(duration).count();
+		return Timestamp(millis);
+	}
+
+	/** get timestamp for the current system-time */
+	static inline Timestamp fromRunningTime() {
+		static Timestamp startup = fromUnixTime();
+		return fromUnixTime() - startup;
+	}
+
+
 
 public:
 
 	/** get timestamp in milliseconds */
-	inline uint64_t ms() const {return _ms;}
+	inline int64_t ms() const {return _ms;}
 
 	/** get timestamp in seconds */
 	inline float sec() const {return _ms/1000.0f;}
 
 
+
 public:
 
 	/** is this timestamp zero? */
@@ -48,17 +66,23 @@ public:
 
 	/** smaller than the given one? */
 	bool operator < (const Timestamp& o) const {return _ms < o._ms;}
+	bool operator <= (const Timestamp& o) const {return _ms <= o._ms;}
 
 	/** greater than the given one? */
 	bool operator > (const Timestamp& o) const {return _ms > o._ms;}
+	bool operator >= (const Timestamp& o) const {return _ms >= o._ms;}
+
 
 
 	Timestamp operator - (const Timestamp& o) const {return Timestamp(_ms - o._ms);}
 
 	Timestamp operator + (const Timestamp& o) const {return Timestamp(_ms + o._ms);}
 
-	/** cast to float */
+	Timestamp operator * (const float val) const {return Timestamp(_ms * val);}
-	operator float () const {return sec();}
+
+
+//	/** cast to float */
+//	operator float () const {return sec();}
 
 
 };

floorplan/v2/FloorplanHelper.h

@@ -14,6 +14,27 @@ class FloorplanHelper {
 
 public:
 
+	/** align all floorplan values to the given grid size. needed for the grid factory */
+	static void align(Floorplan::IndoorMap* map, const int gridSize_cm) {
+		for (Floorplan::Floor* floor : map->floors) {
+			floor->atHeight = align_m(floor->atHeight, gridSize_cm);
+			floor->height = align_m(floor->height, gridSize_cm);
+			for (Floorplan::Stair* stair : floor->stairs) {
+				for (Floorplan::StairPart& part : ((Floorplan::StairFreeform*)stair)->parts) {
+					part.start.z = align_m(part.start.z, gridSize_cm);
+					part.end.z = align_m(part.end.z, gridSize_cm);
+				}
+			}
+		}
+	}
+
+	static inline float align_m(const float val_m, const int gridSize_cm) {
+		const float val_cm = val_m * 100;
+		const int snapped = std::round(val_cm / gridSize_cm);
+		const float res_cm = snapped * gridSize_cm;
+		return res_cm / 100.0f;
+	}
+
 	/** get a BBox for the whole map */
 	static BBox3 getBBox(const Floorplan::IndoorMap* map) {
 

floorplan/v2/FloorplanReader.h

@@ -5,6 +5,7 @@
 
 #include "Floorplan.h"
 
+#include "../../misc/Debug.h"
 #include "../../Assertions.h"
 #include "../../lib/tinyxml/tinyxml2.h"
 
@@ -18,10 +19,15 @@ namespace Floorplan {
 	 */
 	class Reader {
 
+	private:
+
+		static constexpr const char* name = "FPReader";
+
 	public:
 
 		/** read an IndoorMap from the given XML-file */
 		static IndoorMap* readFromFile(const std::string& file) {
+			Log::add(name, "reading floorplan from file: " + file);
 			setlocale(LC_NUMERIC, "C");
 			tinyxml2::XMLDocument doc;
 			const tinyxml2::XMLError res = doc.LoadFile(file.c_str());
@@ -31,6 +37,7 @@ namespace Floorplan {
 
 		/** read an IndoorMap from the given XMl-string */
 		static IndoorMap* readFromString(const std::string& str) {
+			Log::add(name, "reading floorplan from string");
 			setlocale(LC_NUMERIC, "C");
 			tinyxml2::XMLDocument doc;
 			const tinyxml2::XMLError res = doc.Parse(str.c_str(), str.length());
@@ -54,7 +61,7 @@ namespace Floorplan {
 
 		/** parse the <map> node */
 		static IndoorMap* parseMap(const XMLElem* el) {
-			std::cout << el->Name() << std::endl;
+			Log::add(name, "parsing the map");
 			IndoorMap* map = new IndoorMap();
 			map->width = el->FloatAttribute("width");
 			map->depth = el->FloatAttribute("depth");
@@ -76,6 +83,7 @@ namespace Floorplan {
 		/** parse one <floor> node */
 		static Floor* parseFloor(const XMLElem* el) {
 			Floor* floor = new Floor();
+			Log::add(name, std::string("parsing floor ") + el->Attribute("name"));
 			floor->atHeight = el->FloatAttribute("atHeight");
 			floor->height = el->FloatAttribute("height");
 			floor->name = el->Attribute("name");

geo/Angle.h

@@ -56,12 +56,12 @@ public:
 	}
 
 	/** convert degrees to radians */
-	static constexpr float degToRad(const float deg) {
+	static constexpr inline float degToRad(const float deg) {
 		return deg / 180.0f * M_PI;
 	}
 
 	/** convert radians to degrees */
-	static float radToDeg(const float rad) {
+	static constexpr inline float radToDeg(const float rad) {
 		return rad * 180.0f / M_PI;
 	}
 

geo/Point3.h

@@ -105,6 +105,10 @@ struct Point3 {
 				), 1.0f/norm);
 	}
 
+	std::string asString() const {
+		return "(" + std::to_string(x) + ", " + std::to_string(y) + ", " + std::to_string(z) + ")";
+	}
+
 private:
 
 	static inline bool eq(const float a, const float b, const float delta) {return std::abs(a-b) <= delta;}

grid/Grid.h

@@ -6,6 +6,8 @@
 #include <unordered_map>
 #include <algorithm>
 
+#include "../Assertions.h"
+
 #include "../Exception.h"
 #include "GridPoint.h"
 #include "GridNode.h"
@@ -47,8 +49,11 @@ public:
 
 	/** ctor with the grid's size (in cm) */
 	Grid(const int gridSize_cm) : gridSize_cm(gridSize_cm) {
-		static_assert((sizeof(T::_idx) > 0), "T must inherit from GridNode!");
+		//static_assert((sizeof(T::_idx) > 0), "T must inherit from GridNode!");
-		static_assert((sizeof(T::x_cm) > 0), "T must inherit from GridPoint!");
+		//static_assert((sizeof(T::x_cm) > 0), "T must inherit from GridPoint!");
+		StaticAssert::AinheritsB<T, GridNode>();		// "T must inherit from GridNode!"
+		StaticAssert::AinheritsB<T, GridPoint>();		// "T must inherit from GridPoint!"
+		Log::add(name, "empty grid with " + std::to_string(gridSize_cm) + "cm grid-size");
 	}
 
 	/** no-copy */
@@ -395,11 +400,13 @@ public:
 	/** serialize into the given stream */
 	void write(std::ostream& out) {
 
-		// serialize static
+		// size (in bytes) one node has. this is a sanity check whether the file matches the code!
-		T::staticSerialize(out);
+		const int nodeSize = sizeof(T);
+		out.write((const char*) &nodeSize, sizeof(nodeSize));
 
 		// number of nodes
 		const int numNodes = nodes.size();
+		Assert::isTrue(numNodes > 0, "grid says it contains 0 nodes. there must be some error!");
 		out.write((const char*) &numNodes, sizeof(numNodes));
 
 		// serialize
@@ -407,23 +414,37 @@ public:
 			out.write((const char*) &node, sizeof(T));
 		}
 
+		// serialize static parameters
+		T::staticSerialize(out);
+
+		out.flush();
+
 	}
 
 	/** deserialize from the given stream */
 	void read(std::istream& inp) {
 
-		// deserialize static
+		Log::add(name, "loading grid from input-stream");
-		T::staticDeserialize(inp);
+
+		// size (in bytes) one node has. this is a sanity check whether the file matches the code!
+		int nodeSize;
+		inp.read((char*) &nodeSize, sizeof(nodeSize));
+		Assert::equal(nodeSize, (int)sizeof(T), "sizeof(node) of the saved grid does not match sizeof(node) for the code!");
 
 		// number of nodes
 		int numNodes;
 		inp.read((char*) &numNodes, sizeof(numNodes));
+		Assert::isTrue(numNodes > 0, "grid-file says it contains 0 nodes. there must be some error!");
 
 		// allocate node-space
 		nodes.resize(numNodes);
 
 		// deserialize
 		inp.read((char*) nodes.data(), numNodes*sizeof(T));
+		Log::add(name, "deserialized " + std::to_string(nodes.size()) + " nodes");
+
+		// deserialize static parameters
+		T::staticDeserialize(inp);
 
 		// update
 		rebuildHashes();

grid/GridPoint.h

@@ -67,7 +67,12 @@ struct GridPoint {
 
 
 	/** cast to string */
-	operator std::string() const {return "(" + std::to_string(x_cm) + "," + std::to_string(y_cm) + "," + std::to_string(z_cm) + ")";}
+	operator std::string() const {return asString();}
+
+	/** get as string */
+	std::string asString() const {
+		return "(" + std::to_string(x_cm) + "," + std::to_string(y_cm) + "," + std::to_string(z_cm) + ")";
+	}
 
 	/** read-only array access */
 	float operator [] (const int idx) const {
@@ -79,4 +84,16 @@ struct GridPoint {
 
 };
 
+namespace std {
+	template<> struct hash<GridPoint> {
+		int64_t operator() (const GridPoint& gp) const {
+			return
+			    (((int64_t)gp.x_cm) << 0) +
+			    (((int64_t)gp.y_cm) << 8) +
+			    (((int64_t)gp.z_cm) << 16);
+		}
+	};
+}
+
+
 #endif // GRIDPOINT_H

grid/factory/GridImportance.h

@@ -130,7 +130,7 @@ public:
 
 	}
 
-	template <typename T> void addImportance(Grid<T>& g, DijkstraNode<T>* start, DijkstraNode<T>* end) {
+	template <typename T> void addImportance(Grid<T>& g, const DijkstraNode<T>* start, const DijkstraNode<T>* end) {
 
 		// routing path
 		DijkstraPath<T> path(end, start);

grid/factory/v2/Elevators.h

@@ -40,7 +40,6 @@ public:
 
 		const int gs_cm = grid.getGridSize_cm();
 
-
 		struct IntPos {
 			int x_cm;
 			int y_cm;

grid/factory/v2/GridFactory.h

@@ -136,7 +136,7 @@ public:
 	/** add the given floor to the grid */
 	void addFloor(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {
 
-		Log::add(name, "adding floor " + floor->name, true);
+		Log::add(name, "adding floor '" + floor->name + "'", true);
 		if (listener) {listener->onGridBuildUpdateMinor("adding floor " + floor->name);}
 
 		const BBox2 bbox = getFloorOutlineBBox(floor->outline);
@@ -148,6 +148,7 @@ public:
 
 		const int total = (x2-x1) / helper.gridSize();
 		int cur = 0;
+		int numNodes = 0;
 
 		// build grid-points for floor-outline
 		for(int x_cm = x1; x_cm < x2; x_cm += helper.gridSize()) {
@@ -169,10 +170,17 @@ public:
 				if (grid.hasNodeFor(t)) {continue;}
 				grid.add(t);
 
+				// debug
+				++numNodes;
+
 			}
+
 			if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}
+
 		}
 
+		Log::add(name, "added " + std::to_string(numNodes) + " nodes");
+
 		// connect the g
 		connectAdjacent(floor, z_cm);
 
@@ -225,9 +233,9 @@ public:
 	}
 
 	/** connect all neighboring nodes located on the given height-plane */
-	void connectAdjacent(const Floorplan::Floor* floor, const float z_cm) {
+	void connectAdjacent(const Floorplan::Floor* floor, const int z_cm) {
 
-		Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm), false);
+		Log::add(name, "connecting all adjacent nodes within floor '" + floor->name + "' (atHeight: " + std::to_string(z_cm) + "cm)", false);
 		Log::tick();
 
 		// connect adjacent grid-points

grid/factory/v2/Importance.h

@@ -86,8 +86,8 @@ public:
 		KNN<KNNArray<std::vector<T>>, 3> knnStairs(knnArrStairs);
 
 		// probability adjustments
-		Distribution::Normal<float> avoidWalls(0.0, 0.35);
+		Distribution::Triangle<float> avoidWalls(0.0, 0.45f);
-		Distribution::Normal<float> favorDoors(0.0f, 0.5f);
+		Distribution::Normal<float> favorDoors(0.0f, 0.4f);
 		Distribution::Normal<float> favorStairs(0.0f, 1.5f);
 
 		if (l) {
@@ -120,20 +120,33 @@ public:
 			// get the distance to the nearest stair
 			const float distToStair_m = Units::cmToM(knnStairs.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
 
-			const bool useNormal = (distToWall_m < distToDoor_m && distToWall_m < distToStair_m);
+			const bool useNormal = (distToWall_m*3.5 < distToDoor_m && distToWall_m*3.5 < distToStair_m);
 
 			// final probability
-			n1.navImportance = 1.0f;
+			n1.walkImportance = 1.0f;
-			n1.navImportance += favorDoors.getProbability(distToDoor_m) * 1.25f;
+			//n1.walkImportance += favorDoors.getProbability(distToDoor_m) * 0.75f;
-			n1.navImportance += favorStairs.getProbability(distToStair_m) * 30.5f;
+			n1.walkImportance += favorStairs.getProbability(distToStair_m) * 1.0f;
 
 			// use wall avoidance
 			if (useNormal) {
-				n1.navImportance -= avoidWalls.getProbability(distToWall_m) * 0.5f;
+				n1.walkImportance -= avoidWalls.getProbability(distToWall_m) * 0.4f;
 			}
 
+
+			// navigation importance is calculated using other formulae
+			n1.navImportance = 1.0f;
+			if (useNormal) {
+				n1.navImportance -= avoidWalls.getProbability(distToWall_m) * 0.4;
+			}
+			//n1.navImportance += favorDoors.getProbability(distToDoor_m) * 0.5;
+			n1.navImportance += favorStairs.getProbability(distToStair_m) * 1.0;
+
+
+
+
 			// sanity check
-			Assert::isTrue(n1.navImportance >= 0, "detected negative importance. does not make sense!");
+			Assert::isTrue(n1.walkImportance >= 0, "detected negative walk importance. does not make sense!");
+			Assert::isTrue(n1.navImportance >= 0, "detected negative nav importance. does not make sense!");
 
 		}
 

grid/factory/v2/Stairs2.h

@@ -112,7 +112,7 @@ public:
 			const Point3 p4 = quad.p4 * 100;
 
 			// get the z-value from one of the both triangles
-			int z_cm;
+//			int z_cm;
 			float u,v,w;
 			if (helper.bary(p, p1.xy(), p2.xy(), p3.xy(), u, v, w)) {
 				sn.z_cm = p1.z*u + p2.z*v + p3.z*w;
@@ -124,7 +124,7 @@ public:
 		}
 
 		// this might lead to stairs the start slightly above the starting-floor
-		// or end slightly below the ending floor. this would lead to DISCONNECTION!
+		// or ending slightly below the ending floor. this would lead to DISCONNECTION!
 		// therefore re-scale the z-values to ensure they start at floor1 and end at floor 2
 		float minZ = +9999999;
 		float maxZ = -9999999;

grid/walk/v2/GridWalker.h

@@ -1,6 +1,7 @@
 #ifndef GRIDWALKER_H
 #define GRIDWALKER_H
 
+#include "../../../data/Timestamp.h"
 #include "../../Grid.h"
 #include "../../../math/DrawList.h"
 #include "modules/WalkModule.h"
@@ -16,7 +17,7 @@ private:
 	/** all modules to evaluate */
 	std::vector<WalkModule<Node, WalkState>*> modules;
 
-	DrawList<const Node*> drawer;
+	RandomGenerator rnd;
 
 public:
 
@@ -26,14 +27,16 @@ public:
 	}
 
 	/** perform the walk based on the configured setup */
-	WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, float dist_m) {
+	WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, float dist_m, double& probability) {
+
+		Assert::isTrue(dist_m >= 0, "walk distance must not be negative!");
+		Assert::isTrue(dist_m < 10.0, "walking more than 10.0 meters at once does not make sense!");
 
 		// keep the starting state for reference
 		//const WalkState startState = _startState;
 
 		// the current state that is modified for each step
 		WalkState currentState = _startState;
-		updateBefore(currentState);
 
 		// get the node that corresponds to start;
 		const Node* startNode = grid.getNodePtrFor(currentState.position);
@@ -42,10 +45,26 @@ public:
 		// currently examined node
 		const Node* curNode = startNode;
 
+		// perform initial update
+		updateBefore(currentState, *curNode);
+
+
+		// add the previous walked-distance-error to the desired distance (is usually negative, thus dist_m is reduced)
+		dist_m += _startState.distance.error_m;
+
+		probability = 1.0;
+		int cnt = 0;
+
 		// until distance is reached
 		while (dist_m > 0) {
 
-			drawer.reset();
+			// only needed for a global (no-thread-safe) drawer
+			//drawer.reset();
+
+			// the rnd-generator is shared among several threads which should be fine
+			// the draw-list, however, is per-thread, which is mandatory!
+			// alternative to the generator would be seeding the interal one which prooved very unstable!
+			DrawList<const Node*> drawer(rnd); drawer.reserve(10);
 
 			// evaluate each neighbor
 			for (const Node& neighbor : grid.neighbors(*curNode)) {
@@ -53,8 +72,12 @@ public:
 				drawer.add(&neighbor, prob);
 			}
 
-			// pick a neighbor
+			// pick a neighbor and get its probability
-			const Node* nextNode = drawer.get();
+			double nodeProbability;
+			const Node* nextNode = drawer.get(nodeProbability);
+			if ( nodeProbability < probability ) {probability = nodeProbability;}	// keep the smallest one
+			//probability += nodeProbability;
+			//++cnt;
 
 			// inform
 			step(currentState, *curNode, *nextNode);
@@ -64,12 +87,17 @@ public:
 			curNode = nextNode;
 			currentState.position = *curNode;
 
-
 		}
 
+		//if (cnt != 0) {probability /= cnt;} else {probability = 1.0;}
+		probability = curNode->getWalkImportance();
+
 		// update after
 		updateAfter(currentState, *startNode, *curNode);
 
+		// calculate the walked-distance-error (is usually negative, as we walked a little too far)
+		currentState.distance.error_m = dist_m;
+
 		// done
 		return currentState;
 
@@ -78,10 +106,10 @@ public:
 private:
 
 	/** update the state before starting the random walk (e.g. based on sensor readings, ..) */
-	inline void updateBefore(WalkState& state) {
+	inline void updateBefore(WalkState& state, const Node& startNode) {
 
 		for (WalkModule<Node, WalkState>* mdl : modules) {
-			mdl->updateBefore(state);
+			mdl->updateBefore(state, startNode);
 		}
 
 	}

grid/walk/v2/modules/WalkModule.h

@@ -9,6 +9,22 @@ struct WalkState {
 	/** current position within the grid (-> in cm!) */
 	GridPoint position;
 
+	/** nested struct to prevent name clashes */
+	struct Distance {
+
+		/**
+		 * for every walk, the walker is given a desired distance
+		 * however, the walking distance depends on the grid size and can
+		 * therefore never be reached exactly. therefor we track the
+		 * error between desired and walked distance to ensure "in average"
+		 * the walked distance is correct
+		 */
+		float error_m;
+
+		Distance() : error_m(0) {;}
+
+	} distance;
+
 	/** ctor */
 	explicit WalkState(const GridPoint& position) : position(position) {;}
 
@@ -23,7 +39,7 @@ template <typename Node, typename WalkState> class WalkModule {
 public:
 
 	/** update the given WalkState before starting the walk. e.g. based on sensor readings */
-	virtual void updateBefore(WalkState& state) = 0;
+	virtual void updateBefore(WalkState& state, const Node& startNode) = 0;
 
 	/** get the probability p(e) from curNode to potentialNode */
 	virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const = 0;

grid/walk/v2/modules/WalkModuleActivityControl.h

@@ -0,0 +1,111 @@
+#ifndef WALKMODULEACTIVITYCONTROL_H
+#define WALKMODULEACTIVITYCONTROL_H
+
+#include "WalkModule.h"
+
+#include "../../../../geo/Heading.h"
+#include "../../../../math/Distributions.h"
+
+#include "../../../../sensors/pressure/ActivityButterPressure.h"
+
+
+/**
+ * use the currently detected activity (stay-on-floor, walk-up, walk-down, ..)
+ * from the system's control data to favor edges that resemble this activity
+ */
+template <typename Node, typename WalkState, typename Control> class WalkModuleActivityControl : public WalkModule<Node, WalkState> {
+
+private:
+
+	const Control* ctrl;
+
+public:
+
+	/** ctor */
+	WalkModuleActivityControl(const Control* ctrl) : ctrl(ctrl) {
+		;
+	}
+
+	virtual void updateBefore(WalkState& state, const Node& startNode) override {
+		(void) state;
+		(void) startNode;
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+		(void) state;
+		(void) startNode;
+		(void) endNode;
+
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+	}
+
+//	static double getProbability(const Node& lastNode, const Node& nextNode, const ActivityButterPressure::Activity activity) {
+
+//		switch (activity) {
+//		    case ActivityButterPressure::Activity::DOWN:
+//			    if (deltaZ_cm < 0)	{return 0.85;}
+//				if (deltaZ_cm == 0)	{return 0.10;}
+//		                            {return 0.05;}
+//		    case ActivityButterPressure::Activity::UP:
+//			    if (deltaZ_cm > 0)	{return 0.85;}
+//				if (deltaZ_cm == 0)	{return 0.10;}
+//		                            {return 0.05;}
+//		    case ActivityButterPressure::Activity::STAY:
+//			    if (deltaZ_cm == 0) {return 0.85;}
+//		                            {return 0.15;}
+//		    default:
+//			    throw Exception("not yet implemented");
+//		}
+
+//		return 1.0;
+
+//	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) state;
+		(void) startNode;
+
+		const int deltaZ_cm = potentialNode.z_cm - curNode.z_cm;
+
+		// TODO: general activity enum and activity-detector based on barometer and accelerometer?
+		const ActivityButterPressure::Activity activity = ctrl->activity;
+
+//		const float kappa = 0.75;
+
+		switch (activity) {
+			case ActivityButterPressure::Activity::DOWN:
+			    if (deltaZ_cm < 0)	{return 0.60;}
+				if (deltaZ_cm == 0)	{return 0.25;}
+		                            {return 0.15;}
+			case ActivityButterPressure::Activity::UP:
+			    if (deltaZ_cm > 0)	{return 0.60;}
+				if (deltaZ_cm == 0)	{return 0.25;}
+		                            {return 0.15;}
+			case ActivityButterPressure::Activity::STAY:
+			    if (deltaZ_cm == 0) {return 0.60;}
+		                            {return 0.40;}
+//			case ActivityButterPressure::Activity::DOWN:
+//			case ActivityButterPressure::Activity::UP:
+//				if (potentialNode.getType() == GridNode::TYPE_STAIR)	{return kappa;}
+//				if (potentialNode.getType() == GridNode::TYPE_ELEVATOR)	{return kappa;}
+//																		{return 1-kappa;}
+//			case ActivityButterPressure::Activity::STAY:
+//				if (potentialNode.getType() == GridNode::TYPE_DOOR)		{return kappa;}
+//				if (potentialNode.getType() == GridNode::TYPE_FLOOR)	{return kappa;}
+//																		{return 1-kappa;}
+			default:
+				throw Exception("not yet implemented");
+		}
+
+	}
+
+
+};
+
+#endif // WALKMODULEACTIVITYCONTROL_H

grid/walk/v2/modules/WalkModuleButterActivity.h

@@ -2,12 +2,30 @@
 #define WALKMODULEBUTTERACTIVITY_H
 
 #include "WalkModule.h"
-#include "WalkStateHeading.h"
 
 #include "../../../../geo/Heading.h"
 #include "../../../../math/Distributions.h"
 #include "../../../../sensors/pressure/ActivityButterPressure.h"
 
+DEPREACTED
+SEE WalkModuleActivityControl
+
+struct WalkStateBarometerActivity {
+
+	/** innser-struct to prevent name-clashes */
+	struct Barometer {
+
+		/** activity currently detected from the baromter */
+		ActivityButterPressure::Activity activity;
+
+		Barometer() : activity(ActivityButterPressure::Activity::STAY) {;}
+
+	} barometer;
+
+	/** ctor */
+	WalkStateBarometerActivity() : barometer() {;}
+
+};
 
 /** favor z-transitions */
 template <typename Node, typename WalkState> class WalkModuleButterActivity : public WalkModule<Node, WalkState> {
@@ -16,11 +34,15 @@ public:
 
 	/** ctor */
     WalkModuleButterActivity() {
-		;
+
+		// ensure templates WalkState inherits from 'WalkStateBarometerActivity'
+		StaticAssert::AinheritsB<WalkState, WalkStateBarometerActivity>();
+
 	}
 
-	virtual void updateBefore(WalkState& state) override {
+	virtual void updateBefore(WalkState& state, const Node& startNode) override {
 		(void) state;
+		(void) startNode;
 	}
 
 	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
@@ -43,11 +65,11 @@ public:
 
         const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm;
 
-        if(state.act == ActivityButterPressure::Activity::DOWN){
+		if(state.barometer.activity == ActivityButterPressure::Activity::DOWN){
             if (deltaZ_cm < 0) {return 0;}
             if (deltaZ_cm == 0) {return 0.1;}
             return 0.9;
-        } else if (state.act == ActivityButterPressure::Activity::UP){
+		} else if (state.barometer.activity == ActivityButterPressure::Activity::UP){
             if (deltaZ_cm > 0) {return 0;}
             if (deltaZ_cm == 0) {return 0.1;}
             return 0.9;

grid/walk/v2/modules/WalkModuleFavorZ.h

@@ -36,7 +36,7 @@ template <typename Node, typename WalkState> class WalkModuleFavorZ : public Wal
 private:
 
 	// force states to walk into the same z-direction for 30 edges
-	const int keepForXEdges = 12;
+	const int keepForXEdges = 8;
 
 
 public:
@@ -49,8 +49,9 @@ public:
 
 	}
 
-	virtual void updateBefore(WalkState& state) override {
+	virtual void updateBefore(WalkState& state, const Node& startNode) override {
 		(void) state;
+		(void) startNode;
 	}
 
 	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
@@ -93,15 +94,15 @@ public:
 		const int diff = potentialNode.z_cm - curNode.z_cm;
 
 		// tendence available + tendence match? -> high score!
-		if (tendence > 0 && diff > 0) {return 0.95;}
+		if (tendence > 0 && diff >= 0) {return 0.90;}
-		if (tendence < 0 && diff < 0) {return 0.95;}
+		if (tendence < 0 && diff <= 0) {return 0.90;}
 
 		// tendence available + tendence mismatch? -> very low score!
-		if (tendence > 0 && diff < 0) {return 0.05;}
+		if (tendence > 0 && diff < 0) {return 0.10;}
-		if (tendence < 0 && diff > 0) {return 0.05;}
+		if (tendence < 0 && diff > 0) {return 0.10;}
 
 		// no tendence available -> just favor z-transitions over non-z-transitions
-		return (diff != 0) ? (0.7) : (0.3);
+		return (diff != 0) ? (0.75) : (0.25);
 
 	}
 

grid/walk/v2/modules/WalkModuleFollowDestination.h

@@ -2,39 +2,77 @@
 #define WALKMODULEFOLLOWDESTINATION_H
 
 #include "WalkModule.h"
+#include "WalkStateHeading.h"
+
 #include "../../../../nav/dijkstra/Dijkstra.h"
+#include "../../../../nav/dijkstra/DijkstraPath.h"
+
+#include "../../../../math/Distributions.h"
+#include "../../../../Assertions.h"
+
 
 /**
- * favour edges p(e) that approach the destination
+ * favor nodes that approach a known destination
  */
 template <typename Node, typename WalkState> class WalkModuleFollowDestination : public WalkModule<Node, WalkState> {
 
 private:
 
+	const Grid<Node>& grid;
 	Dijkstra<Node> dijkstra;
+	const DijkstraNode<Node>* dnDest;
 
-	struct DijkstraMapper {
+	struct DijkstraAccess {
 		const Grid<Node>& grid;
-		DijkstraMapper(const Grid<Node>& grid) : grid(grid) {;}
+		DijkstraAccess(const Grid<Node>& grid) : grid(grid) {;}
 		int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
 		const Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
 		float getWeightBetween(const Node& n1, const Node& n2) const {
-			return n1.getDistanceInCM(n2) * n2.navImportance;
+			return n1.getDistanceInMeter(n2) / n2.getNavImportance();
 		}
 	};
 
 public:
 
-	/** ctor */
+	/** ctor WITHOUT known destination*/
-	WalkModuleFollowDestination(Grid<Node>& grid, const Node& destination) {
+	WalkModuleFollowDestination(const Grid<Node>& grid) : grid(grid) {
 
-		// shortest path calculation
+		// ensure the template WalkState inherits from 'WalkStateFavorZ'
-		dijkstra.build(&destination, DijkstraMapper(grid));
+		//StaticAssert::AinheritsB<WalkState, WalkStateFavorZ>();
 
 	}
 
-	virtual void updateBefore(WalkState& state) override {
+	/** ctor WITH known destination*/
+	WalkModuleFollowDestination(const Grid<Node>& grid, const Node& destination) : grid(grid) {
+		setDestination(destination);
+	}
+
+
+	/** set the desired destination node */
+	void setDestination(const Node& dest) {
+		DijkstraAccess acc(grid);
+		dijkstra.build(&dest, acc);
+		dnDest = dijkstra.getNode(dest);
+	}
+
+
+	/** get the shortest path from the given start to the configured destination */
+	DijkstraPath<Node> getShortestPath(const Node& start) {
+		const DijkstraNode<Node>* dnStart = dijkstra.getNode(start);
+		const DijkstraPath<Node> path(dnStart, dnDest);
+		return path;
+	}
+
+	virtual void updateBefore(WalkState& state, const Node& startNode) override {
 		(void) state;
+		(void) startNode;
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+		(void) state;
+		(void) startNode;
+		(void) endNode;
+
 	}
 
 	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
@@ -43,26 +81,28 @@ public:
 		(void) nextNode;
 	}
 
-	virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
 
 		(void) state;
 		(void) startNode;
 
-		const float kappa = 0.8;
 		const DijkstraNode<Node>* dnCur = dijkstra.getNode(curNode);
-		const DijkstraNode<Node>* dnNext = dijkstra.getNode(potentialNode);
+		const DijkstraNode<Node>* dnPot = dijkstra.getNode(potentialNode);
 
-		// probability
+		if (dnCur == nullptr) {return 1.0;}
-		return (dnNext->cumWeight < dnCur->cumWeight) ? (kappa) : (1.0 - kappa);
+		if (dnPot == nullptr) {return 1.0;}
+
+		constexpr double kappa = 0.70;
+
+		const float curDistToTarget = dnCur->cumWeight;
+		const float potDistToTarget = dnPot->cumWeight;
+
+		return (potDistToTarget < curDistToTarget) ? (kappa) : (1.0-kappa);
 
 	}
 
-	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
-		(void) state;
-		(void) startNode;
-		(void) endNode;
-	}
 
 };
 
+
 #endif // WALKMODULEFOLLOWDESTINATION_H

grid/walk/v2/modules/WalkModuleHeading.h

@@ -36,7 +36,9 @@ public:
 
 	}
 
-	virtual void updateBefore(WalkState& state) override {
+	virtual void updateBefore(WalkState& state, const Node& startNode) override {
+
+		(void) startNode;
 
 		// add noise
 		state.heading.direction += draw.get();

grid/walk/v2/modules/WalkModuleHeadingControl.h

@@ -19,14 +19,12 @@ private:
 	/** random noise */
 	Distribution::Normal<float> distNoise;
 
-	Control* ctrl;
+	const Control* ctrl;
-
-	//std::unordered_map<WalkState*, float> errorTracker;
 
 public:
 
 	/** ctor 3.0 should be OK! */
-	WalkModuleHeadingControl(Control* ctrl, const float sensorNoiseDegreesSigma) :
+	WalkModuleHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) :
 		dist(Distribution::VonMises<double>(0.0f, 2.0).getLUT()),
 		distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)),
 		ctrl(ctrl) {
@@ -37,17 +35,20 @@ public:
 	}
 
 
-	virtual void updateBefore(WalkState& state) override {
+	virtual void updateBefore(WalkState& state, const Node& startNode) override {
 
 		// NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
 		// reset this one after every transition!
-		Assert::isBetween(ctrl->turnAngle, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?");
+		Assert::isBetween(ctrl->turnSinceLastTransition_rad, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?");
 
 		// sensor noise
-		const float var = distNoise.draw();
+		float var = distNoise.draw();
+
+		// stair? -> increase variance
+		if (startNode.getType() == GridNode::TYPE_STAIR) {var *= 3;}
 
 		// adjust the state's heading using the control-data
-		state.heading.direction += ctrl->turnAngle + var;
+		state.heading.direction += ctrl->turnSinceLastTransition_rad + var;
 
 	}
 
@@ -61,8 +62,13 @@ public:
 
 		(void) state;
 
+		// ignore for stairs?
+		//if (nextNode.getType() == GridNode::TYPE_STAIR) {return;}
+
 		// for elevator edges [same (x,y) but different z] do not adjust anything
-		if (curNode.x_cm == nextNode.x_cm && curNode.y_cm == nextNode.y_cm && curNode.z_cm != nextNode.z_cm) {return;}
+		if (nextNode.getType() == GridNode::TYPE_ELEVATOR)	{return;}
+		if (curNode.getType() == GridNode::TYPE_ELEVATOR)	{return;}
+		//if (curNode.x_cm == nextNode.x_cm && curNode.y_cm == nextNode.y_cm && curNode.z_cm != nextNode.z_cm) {return;}
 
 		// get the heading denoted by the way from curNode to nextNode
 		const Heading head(curNode.x_cm, curNode.y_cm, nextNode.x_cm, nextNode.y_cm);
@@ -86,9 +92,13 @@ public:
 
 		(void) startNode;
 
+		// ignore for stairs?
+		//if (potentialNode.getType() == GridNode::TYPE_STAIR)	{return 1.0;}
 
 		// for elevator edges [same (x,y) but different z] just return 1
-		if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}
+		if (potentialNode.getType() == GridNode::TYPE_ELEVATOR)	{return 1.0;}
+		if (curNode.getType() == GridNode::TYPE_ELEVATOR)		{return 1.0;}
+		//if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}
 
 		// get the heading between curNode and potentialNode
 		const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);
@@ -99,16 +109,18 @@ public:
 		// get the difference
 		const float angularDiff = head.getDiffHalfRAD(stateHead);
 
+
 		if (angularDiff > Angle::degToRad(135))	{return 0.01;}
 		if (angularDiff > Angle::degToRad(90))	{return 0.02;}
 		if (angularDiff > Angle::degToRad(45))	{return 0.07;}
 												{return 0.90;}
 
+
 //		add error to allow stronger deviation with respect to the "BIG GLOBAL SCOPE"
 
 		// determine probability
-		const float prob = dist.getProbability(angularDiff);
+//		const float prob = dist.getProbability(angularDiff);
-		return prob;
+//		return prob;
 
 	}
 

grid/walk/v2/modules/WalkModuleNodeImportance.h

@@ -22,8 +22,9 @@ public:
 	}
 
 
-	virtual void updateBefore(WalkState& state) override {
+	virtual void updateBefore(WalkState& state, const Node& startNode) override {
 		(void) state;
+		(void) startNode;
 	}
 
 	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
@@ -44,7 +45,12 @@ public:
 		(void) startNode;
 		(void) curNode;
 
-		const double prob = potentialNode.getNavImportance();
+		//const double prob = potentialNode.getWalkImportance();
+
+		const float i1 = curNode.getWalkImportance();
+		const float i2 = potentialNode.getWalkImportance();
+		const double prob = (i2 > i1) ? (0.9) : (0.1);
+
 		return prob;
 
 	}

grid/walk/v2/modules/WalkModulePreventVisited.h

@@ -45,8 +45,9 @@ public:
 	}
 
 
-	virtual void updateBefore(WalkState& state) override {
+	virtual void updateBefore(WalkState& state, const Node& startNode) override {
 		(void) state;
+		(void) startNode;
 	}
 
 	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {

grid/walk/v2/modules/WalkModuleRelativePressureControl.h

@@ -46,8 +46,9 @@ public:
 		;
 	}
 
-	virtual void updateBefore(WalkState& state) override {
+	virtual void updateBefore(WalkState& state, const Node& startNode) override {
 		(void) state;
+		(void) startNode;
 	}
 
 	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {

grid/walk/v2/modules/WalkModuleSpread.h

@@ -45,8 +45,9 @@ public:
 	}
 
 
-	virtual void updateBefore(WalkState& state) override {
+	virtual void updateBefore(WalkState& state, const Node& startNode) override {
 		(void) state;
+		(void) startNode;
 	}
 
 	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {

main.cpp

@@ -19,13 +19,15 @@ int main(int argc, char** argv) {
 	// skip all tests starting with LIVE_
 	//::testing::GTEST_FLAG(filter) = "*Barometer*";
 
-	::testing::GTEST_FLAG(filter) = "*Stairs*";
+	//::testing::GTEST_FLAG(filter) = "*Distribution.T*";
 	//::testing::GTEST_FLAG(filter) = "*RingBuffer*";
 
 	//::testing::GTEST_FLAG(filter) = "*Grid.*";
 	//::testing::GTEST_FLAG(filter) = "*Dijkstra.*";
 
 	//::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModel*";
+	::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";
+
 
 
 	//::testing::GTEST_FLAG(filter) = "*Barometer*";

math/Distributions.h

@@ -7,5 +7,6 @@
 #include "distribution/Uniform.h"
 #include "distribution/VonMises.h"
 #include "distribution/Region.h"
+#include "distribution/Triangle.h"
 
 #endif // DISTRIBUTIONS_H

math/DrawList.h

@@ -21,8 +21,11 @@ template <typename T> class DrawList {
 		/** the cumulative probability up to this element */
 		double cumProbability;
 
+		/** the element's own probability */
+		double probability;
+
 		/** ctor */
-		Entry(T element, const double cumProbability) : element(element), cumProbability(cumProbability) {;}
+		Entry(T element, const double cumProbability, const double probability) : element(element), cumProbability(cumProbability), probability(probability) {;}
 
 		/** compare for searches */
 		bool operator < (const double val) const {return cumProbability < val;}
@@ -37,13 +40,30 @@ private:
 	/** all contained elements */
 	std::vector<Entry> elements;
 
-	/** random number generator */
+	/** the used random number generator */
-	RandomGenerator gen;
+	RandomGenerator& gen;
+
+
+private:
+
+	/** default random generator. fallback */
+	RandomGenerator defRndGen;
+
 
 public:
 
-	/** ctor */
+	/** ctor with random seed */
-	DrawList() : cumProbability(0) {
+	DrawList() : cumProbability(0), gen(defRndGen) {
+		;
+	}
+
+	/** ctor with custom seed */
+	DrawList(const uint32_t seed) : cumProbability(0), gen(defRndGen(seed)) {
+		;
+	}
+
+	/** ctor with custom RandomNumberGenerator */
+	DrawList(RandomGenerator& gen) : cumProbability(0), gen(gen) {
 		;
 	}
 
@@ -58,15 +78,20 @@ public:
 		elements.clear();
 	}
 
+	/** adjust the reserved list size */
+	void reserve(const size_t numElements) {
+		elements.reserve(numElements);
+	}
+
 	/** add a new user-element and its probability */
 	void add(T element, const double probability) {
 		Assert::isTrue(probability >= 0, "probability must not be negative!");
 		cumProbability += probability;
-		elements.push_back(Entry(element, cumProbability));
+		elements.push_back(Entry(element, cumProbability, probability));
 	}
 
 	/** get a random element based on its probability */
-	T get() {
+	T get(double& elemProbability) {
 
 		// generate random number between [0:cumProbability]
 		std::uniform_real_distribution<> dist(0, cumProbability);
@@ -81,6 +106,7 @@ public:
 		Assert::isFalse(tmp == elements.end(), "draw() did not find a valid element");
 
 		// done
+		elemProbability = (*tmp).probability;
 		return (*tmp).element;
 
 	}

math/Random.h

@@ -21,7 +21,7 @@ public:
 	RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;}
 
 	/** ctor with custom seed */
-	RandomGenerator(result_type) : std::minstd_rand(RANDOM_SEED) {;}
+	RandomGenerator(result_type seed) : std::minstd_rand(seed) {;}
 
 };
 

math/distribution/Triangle.h

@@ -0,0 +1,50 @@
+#ifndef TRIANGLE_H
+#define TRIANGLE_H
+
+#include <cmath>
+#include <random>
+#include "../Random.h"
+#include "../../Assertions.h"
+#include "Normal.h"
+
+namespace Distribution {
+
+	/**
+	 * distribution that forms a triangle
+	 * sigma defines the width (from mu to 0.0, half the width of the triangle's base)
+	 * all values outside of the triangle are zero
+	 */
+	template <typename T> class Triangle {
+
+	private:
+
+		const T mu;
+		const T sigma;
+		const T area;
+
+	public:
+
+		/** ctor */
+		Triangle(const T mu, const T sigma) : mu(mu), sigma(sigma), area(sigma*sigma) {
+
+		}
+
+		/** get probability for the given value */
+		T getProbability(const T val) const {
+			const T diff = std::abs(val - mu);
+			if (diff > sigma)	{return 0;}			// outside of triangle
+			return (sigma - diff) / area;			// inside the triangle
+
+		}
+
+		/** get the probability for the given value */
+		static T getProbability(const T mu, const T sigma, const T val) {
+			Triangle<T> dist(mu, sigma);
+			return dist.getProbability(val);
+		}
+
+	};
+
+}
+
+#endif // TRIANGLE_H

misc/Debug.h

@@ -6,6 +6,8 @@
 #include <iomanip>
 #include "Time.h"
 
+#include "log/LoggerCOUT.h"
+
 /** quick and dirty workaround */
 static decltype(Time::tick()) LogLastTick;
 
@@ -13,18 +15,36 @@ static decltype(Time::tick()) LogLastTick;
 
 class Log {
 
+private:
+
+	static Logger** getLoggerPtr() {
+		static Logger* logger = new LoggerCOUT();
+		return &logger;
+	}
+
+	static Logger* getLogger() {
+		return *getLoggerPtr();
+	}
+
 public:
 
+	/** set the to-be-used logger */
+	static void setLogger(Logger* logger) {
+		*getLoggerPtr() = logger;
+	}
+
 	static void add(const char* comp, const std::string what, const bool nl = true) {
-		addComp(comp);
+		std::stringstream out;
-		std::cout << what;
+		addComp(out, comp);
-		if (nl) {std::cout << std::endl;} else {std::cout << std::flush;}
+		out << what;
+		getLogger()->add(out.str(), nl);
 	}
 
 	static void add(const std::string& component, const std::string what, const bool nl = true) {
-		addComp(component.c_str());
+		std::stringstream out;
-		std::cout << what;
+		addComp(out, component.c_str());
-		if (nl) {std::cout << std::endl;} else {std::cout << std::flush;}
+		out << what;
+		getLogger()->add(out.str(), nl);
 	}
 
 
@@ -37,15 +57,17 @@ public:
 		const auto cur = Time::tick();
 		const int diff_ms = Time::diffMS(LogLastTick, cur);
 		LogLastTick = cur;
-		std::cout << " (took: " << diff_ms << "ms)" << std::endl;
+		std::stringstream out;
+		out << " (took: " << diff_ms << "ms)";
+		getLogger()->add(out.str(), true);
 	}
 
 private:
 
 
 
-	static void addComp(const char* component) {
+	static void addComp(std::ostream& out, const char* component) {
-		std::cout << "[" << std::setw(12) << std::setfill(' ') << component << "] ";
+		out << "[" << std::setw(12) << std::setfill(' ') << component << "] ";
 	}
 
 };

misc/log/Logger.h

@@ -0,0 +1,17 @@
+#ifndef LOGGER_H
+#define LOGGER_H
+
+#include <string>
+
+/** base-class for all loggers */
+class Logger {
+
+public:
+
+	virtual ~Logger() {;}
+
+	virtual void add(const std::string& str, const bool nl) = 0;
+
+};
+
+#endif // LOGGER_H

misc/log/LoggerAndroid.h

@@ -0,0 +1,19 @@
+#ifndef LOGGERANDROID_H
+#define LOGGERANDROID_H
+
+#include "Logger.h"
+
+class LoggerAndroid : public Logger {
+
+public:
+
+	virtual void add(const std::string& str, const bool nl) override {
+		(void) nl;
+#ifdef ANDROID
+		QMessageLogger().info(str.c_str());
+#endif
+	}
+
+};
+
+#endif // LOGGERANDROID_H

misc/log/LoggerCOUT.h

@@ -0,0 +1,18 @@
+#ifndef LOGGERCOUT_H
+#define LOGGERCOUT_H
+
+#include "Logger.h"
+#include <iostream>
+
+class LoggerCOUT : public Logger {
+
+public:
+
+	virtual void add(const std::string& str, const bool nl) override {
+		std::cout << str;
+		if (nl) {std::cout << std::endl;} else {std::cout << std::flush;}
+	}
+
+};
+
+#endif // LOGGERCOUT_H

misc/log/LoggerComposite.h

@@ -0,0 +1,30 @@
+#ifndef LOGGERCOMPOSITE_H
+#define LOGGERCOMPOSITE_H
+
+
+#include "Logger.h"
+#include <vector>
+
+class LoggerComposite : public Logger {
+
+private:
+
+	/** all contained loggers */
+	std::vector<Logger*> loggers;
+
+public:
+
+	/** add a new logger to this composite */
+	void addLogger(Logger* l) {
+		loggers.push_back(l);
+	}
+
+	virtual void add(const std::string& str, const bool nl) override {
+		for (Logger* l : loggers) {l->add(str, nl);}
+	}
+
+};
+
+
+
+#endif // LOGGERCOMPOSITE_H

nav/dijkstra/Dijkstra.h

@@ -30,7 +30,7 @@ public:
 	}
 
 	/** get the dijkstra-pendant for the given user-node. null if none matches */
-	DijkstraNode<T>* getNode(const T& userNode) const {
+	const inline DijkstraNode<T>* getNode(const T& userNode) const {
 		auto it = nodes.find(&userNode);
 		return (unlikely(it == nodes.end())) ? (nullptr) : (it->second);
 	}

nav/dijkstra/DijkstraPath.h

@@ -20,6 +20,11 @@ private:
 
 public:
 
+	/** empty ctor */
+	DijkstraPath() {
+		;
+	}
+
 	/** ctor from end- to start-node */
 	DijkstraPath(const DijkstraNode<T>* end, const DijkstraNode<T>* start) {
 

sensors/gps/GPSData.h

@@ -15,11 +15,11 @@ struct GPSData {
 	float accuracy;		// m [might be NAN]
 	float speed;		// m/s [might be NAN]
 
-	GPSData() : ts(), lat(NAN), lon(NAN), alt(NAN), accuracy(NAN), speed(NAN) {;}
+	GPSData() : tsReceived(), lat(NAN), lon(NAN), alt(NAN), accuracy(NAN), speed(NAN) {;}
 
-	GPSData(const Timestamp ts, const float lat, const float lon, const float alt) : ts(ts), lat(lat), lon(lon), alt(alt), accuracy(NAN), speed(NAN)  {;}
+	GPSData(const Timestamp tsReceived, const float lat, const float lon, const float alt) : tsReceived(tsReceived), lat(lat), lon(lon), alt(alt), accuracy(NAN), speed(NAN)  {;}
 
-	GPSData(const Timestamp ts, const float lat, const float lon, const float alt, const float accuracy) : ts(ts), lat(lat), lon(lon), alt(alt), accuracy(accuracy), speed(NAN)  {;}
+	GPSData(const Timestamp tsReceived, const float lat, const float lon, const float alt, const float accuracy) : tsReceived(tsReceived), lat(lat), lon(lon), alt(alt), accuracy(accuracy), speed(NAN)  {;}
 
 };
 

sensors/imu/AccelerometerData.h

@@ -2,6 +2,8 @@
 #define ACCELEROMETERDATA_H
 
 #include <cmath>
+#include <sstream>
+
 
 /** data received from an accelerometer sensor */
 struct AccelerometerData {
@@ -40,6 +42,12 @@ struct AccelerometerData {
 		return AccelerometerData(x/val, y/val, z/val);
 	}
 
+        std::string asString() const {
+            std::stringstream ss;
+            ss << "(" << x << "," << y << "," << z << ")";
+            return ss.str();
+        }
+
 };
 
 #endif // ACCELEROMETERDATA_H

sensors/imu/GyroscopeData.h

@@ -2,8 +2,12 @@
 #define GYROSCOPEDATA_H
 
 #include <cmath>
+#include <sstream>
 
-/** data received from a gyroscope sensor */
+/**
+ * data received from a gyroscope sensor
+ * IN RADIANS!
+ */
 struct GyroscopeData {
 
 	float x;
@@ -12,12 +16,19 @@ struct GyroscopeData {
 
 	GyroscopeData() : x(0), y(0), z(0) {;}
 
+	/** ctor from RADIANS */
 	GyroscopeData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
 
 	float magnitude() const {
 		return std::sqrt( x*x + y*y + z*z );
 	}
 
+        std::string asString() const {
+            std::stringstream ss;
+            ss << "(" << x << "," << y << "," << z << ")";
+            return ss.str();
+        }
+
 };
 
 #endif // GYROSCOPEDATA_H

sensors/offline/OfflineAndroid.h

@@ -12,6 +12,7 @@
 #include "../radio/WiFiMeasurements.h"
 #include "../imu/AccelerometerData.h"
 #include "../imu/GyroscopeData.h"
+#include "../pressure/BarometerData.h"
 
 template <typename SensorData> struct OfflineEntry {
 
@@ -41,6 +42,7 @@ public:
 	virtual void onAccelerometer(const Timestamp ts, const AccelerometerData data) = 0;
 	virtual void onGravity(const Timestamp ts, const AccelerometerData data) = 0;
 	virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) = 0;
+	virtual void onBarometer(const Timestamp ts, const BarometerData data) = 0;
 };
 
 /** read recorded android sensor data files */
@@ -56,6 +58,8 @@ private:
 	std::vector<OfflineEntry<AccelerometerData>> accel;
 	std::vector<OfflineEntry<AccelerometerData>> gravity;
 
+	std::vector<OfflineEntry<BarometerData>> barometer;
+
 	WalkedPath walkedPath;
 
 	const char* name = "OfflineData";
@@ -82,6 +86,9 @@ public:
 	/** get all gravity readings */
 	const std::vector<OfflineEntry<AccelerometerData>>& getGravity() const {return gravity;}
 
+	/** get all barometer readings */
+	const std::vector<OfflineEntry<BarometerData>>& getBarometer() const {return barometer;}
+
 
 	/** get the walked path */
 	const WalkedPath& getWalkedPath() const {return walkedPath;}
@@ -171,8 +178,15 @@ private:
 				break;
 			}
 
+		    case 5: {
+			    const BarometerData data = parseBarometer(sensorData);
+				barometer.push_back(OfflineEntry<BarometerData>(ts, data));
+				if (listener) {listener->onBarometer(ts, data);}
+				break;
+		    }
+
 			case 8: {
-				const WiFiMeasurements data = parseWiFi(sensorData);
+			    const WiFiMeasurements data = parseWiFi(ts, sensorData);
 				wifi.push_back(OfflineEntry<WiFiMeasurements>(ts, data));
 				if (listener) {listener->onWiFi(ts, data);}
 				break;
@@ -196,7 +210,7 @@ private:
 
 
 	/** parse the given WiFiObservation string "MAC;freq;RSSI;MAC;freq;RSSI;...." */
-	static inline WiFiMeasurements parseWiFi(std::string data) {
+	static inline WiFiMeasurements parseWiFi(const Timestamp ts, std::string data) {
 
 		WiFiMeasurements obs;
 
@@ -219,7 +233,7 @@ private:
 			Assert::isTrue(data[0] == ';', "unexpected character");
 			data = data.substr(1);
 
-			const WiFiMeasurement e(mac, std::stof(rssi));
+			const WiFiMeasurement e(mac, std::stof(rssi), ts);
 			obs.entries.push_back(e);
 
 		}
@@ -265,6 +279,16 @@ private:
 
 	}
 
+	/** parse the given Barometer entry */
+	static inline BarometerData parseBarometer(const std::string& data) {
+
+		BarometerData baro;
+		const int pos = data.find(';');
+		baro.hPa = std::stof(data.substr(0, pos));
+		return baro;
+
+	}
+
 	/** parse the given GroundTruth entry */
 	static inline GroundTruthID parseGroundTruthTick(const std::string& data) {
 

sensors/pressure/ActivityButterPressure.h

@@ -40,9 +40,9 @@ public:
 
     /** change this values for much success */
     const bool additionalLowpassFilter = false;
-    const int diffSize = 20; //the number values used for finding the activity.
+	const int diffSize = 20; //the number values used for finding the activity.
     const float threshold = 0.025; // if diffSize is getting smaller, treshold needs to be adjusted in the same direction!
-    Filter::ButterworthLP<float> butter =  Filter::ButterworthLP<float>(10,0.05f,2);
+	Filter::ButterworthLP<float> butter =  Filter::ButterworthLP<float>(10,0.1f,2);
     Filter::ButterworthLP<float> butter2 =  Filter::ButterworthLP<float>(10,0.1f,2);
     FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));
 
@@ -89,7 +89,7 @@ public:
         if(newInterpolatedValues == true){
 
             //getActivity
-            if(output.size() > diffSize){
+			if((int)output.size() > diffSize){
                 //diff
                 std::vector<float> diff;
                 for(int i = output.size() - diffSize; i < output.size() - 1; ++i){

sensors/radio/WiFiGridEstimator.h

@@ -18,23 +18,23 @@ class WiFiGridEstimator {
 public:
 
 
-	/**
+//	/**
-	 * convenience method
+//	 * convenience method
-	 */
+//	 */
-	template <typename Node> static void estimate(Grid<Node>& grid, WiFiModel& mdl, const Floorplan::IndoorMap* im) {
+//	template <typename Node> static void estimate(Grid<Node>& grid, WiFiModel& mdl, const Floorplan::IndoorMap* im) {
 
-		// list of all APs
+//		// list of all APs
-		std::vector<LocatedAccessPoint> aps;
+//		std::vector<LocatedAccessPoint> aps;
-		for (const Floorplan::Floor* f : im->floors) {
+//		for (const Floorplan::Floor* f : im->floors) {
-			for (const Floorplan::AccessPoint* ap : f->accesspoints) {
+//			for (const Floorplan::AccessPoint* ap : f->accesspoints) {
-				aps.push_back(LocatedAccessPoint(*ap));
+//				aps.push_back(LocatedAccessPoint(*ap));
-			}
+//			}
-		}
+//		}
 
-		// perform estimation
+//		// perform estimation
-		estimate(grid, mdl, aps);
+//		estimate(grid, mdl, aps);
 
-	}
+//	}
 
 	/**
 	 * perform a signal-strength estimation for all of the given access points
@@ -42,39 +42,50 @@ public:
 	 * store the estimated strength onto each node.
 	 * as nodes only provide a limited number of rssi-entries,
 	 * store only the strongest ones.
+	 *
+	 * as the smartphone is held above the ground, we do NOT want to estimate
+	 * the signal strength for the nodes (on the ground) but for the nodes
+	 * + the height the smartphone is held at
+	 *
 	 */
-	template <typename Node> static void estimate(Grid<Node>& grid, WiFiModel& mdl, const std::vector<AccessPoint> aps) {
+	template <typename Node> static void estimate(Grid<Node>& grid, WiFiModel& mdl, const float smartphoneAtHeight) {
 
 		// sanity checks
-		Assert::isTrue(Node::getMapAPs().empty(), "there are already some processed APs available!");
+		Assert::isTrue(Node::getMapAPs().empty(), "there are already APs stored on the grid nodes!");
 
-		// attach the access-points to the shared node-vector
+		// all APs known to the model
+		std::vector<AccessPoint> aps = mdl.getAllAPs();
+
+		// attach each access-points to a vector shared for all grid-nodes
 		for (const AccessPoint& ap : aps) {
 			Node::getMapAPs().push_back(ap);
 		}
 
+		// smartphone offset (meter above ground)
+		const Point3 smartphoneOffset(0,0,smartphoneAtHeight);
+
 		// process each node
 		for (Node& n : grid) {
 
 			// keep the strongest APs to attach to this node
 			std::vector<WiFiGridNodeAP> nodeAPs;
 
-			// process each AP
+			// process each AP known to the model
 			for (int apIdx = 0; apIdx < (int) aps.size(); ++apIdx) {
 
 				// estimate the signal-strength
-				const float rssi = mdl.getRSSI(aps[apIdx].getMAC(), n.inMeter());
+				const float rssi = mdl.getRSSI(aps[apIdx].getMAC(), n.inMeter() + smartphoneOffset);
 
-				// keep it
+				// (temporarily) keep it
 				nodeAPs.push_back(WiFiGridNodeAP(apIdx, rssi));
 
 			}
 
-			// sort all APs by signal strength
+			// now sort all the visible APs by signal strength
 			auto comp = [] (const WiFiGridNodeAP& ap1, const WiFiGridNodeAP& ap2) {return ap1.getRSSI() > ap2.getRSSI();};
 			std::sort(nodeAPs.begin(), nodeAPs.end(), comp);
 
-			// attach the strongest X to the node
+			// and finally attach the strongest X to the node
 			const int cnt = std::min( n.getMaxAPs(), (int) nodeAPs.size() );
 			for (int i = 0; i < cnt; ++i) {
 				n.strongestAPs[i] = nodeAPs[i];

sensors/radio/WiFiGridNode.h

@@ -3,6 +3,7 @@
 
 #include <cstdint>
 #include "AccessPoint.h"
+#include "../../misc/Debug.h"
 
 /**
  * rssi model-estimation for one AP, denoted by its index [among all APs present within the map]
@@ -100,9 +101,11 @@ template <int maxAccessPoints> struct WiFiGridNode {
 	 * returns 0 if unknown
 	 */
 	float getRSSI(const MACAddress mac) const {
-		for (const WiFiGridNodeAP ap : strongestAPs) {
+		for (const WiFiGridNodeAP& ap : strongestAPs) {
-			if (!ap.isValid()) {break;}			// reached the end
+			//std::cout << getMapAPs()[ap.getAPIdx()].getMAC().asString() << std::endl;
-			if (getMapAPs()[ap.getAPIdx()].getMAC() == mac) {return ap.getRSSI();}
+			//std::cout << mac.asString() << std::endl;
+			if (!ap.isValid()) {break;}												// reached the end of all APs visible on this node
+			if (getMapAPs()[ap.getAPIdx()].getMAC() == mac) {return ap.getRSSI();}	// does this APs MAC match with the requested MAC? -> found!
 		}
 		return 0;
 	}
@@ -124,6 +127,8 @@ template <int maxAccessPoints> struct WiFiGridNode {
 
 protected:
 
+	static constexpr const char* name = "WiFiGridNode";
+
 	/** serialize static members */
 	static void staticSerialize(std::ostream& out) {
 
@@ -134,6 +139,8 @@ protected:
 		out.write((const char*) &numAPs, sizeof(numAPs));
 		out.write((const char*) getMapAPs().data(), sizeof(getMapAPs()[0])*numAPs);
 
+		Log::add(name, "serialized " + std::to_string(numAPs) + " APs");
+
 	}
 
 	/** deserialize static members */
@@ -149,6 +156,10 @@ protected:
 		// deserialize APs within map
 		inp.read((char*) getMapAPs().data(), sizeof(getMapAPs()[0])*numAPs);
 
+		Log::add(name, "de-serialized " + std::to_string(numAPs) + " APs");
+		std::string aps; for (const AccessPoint& ap : getMapAPs()) {aps += ap.getMAC().asString() + " ";}
+		Log::add(name, aps);
+
 	}
 
 

sensors/radio/WiFiMeasurement.h

@@ -9,7 +9,7 @@
  */
 class WiFiMeasurement {
 
-private:
+public:
 
 	friend class VAPGrouper;
 

sensors/radio/WiFiMeasurements.h

@@ -13,6 +13,15 @@ struct WiFiMeasurements {
 	/** all contained measurements */
 	std::vector<WiFiMeasurement> entries;
 
+	/** convert to string */
+	std::string asString() const {
+		std::string res;
+		for (const WiFiMeasurement& m : entries) {
+			res += m.getAP().getMAC().asString() + ": " + std::to_string(m.getRSSI()) + "\n";
+		}
+		return res;
+	}
+
 };
 
 #endif // WIFIMEASUREMENTS_H

sensors/radio/WiFiProbabilityFree.h

@@ -4,6 +4,7 @@
 #include "WiFiProbability.h"
 #include "model/WiFiModel.h"
 #include "../../math/Distributions.h"
+#include "VAPGrouper.h"
 
 #include <unordered_map>
 
@@ -17,7 +18,7 @@ private:
 
 	const float sigma = 8.0f;
 
-	const float sigmaPerSecond = 1.5f;
+	const float sigmaPerSecond = 3.0f;
 
 	/** the RSSI prediction model */
 	WiFiModel& model;
@@ -25,23 +26,25 @@ private:
 	/** the map's floorplan */
 	Floorplan::IndoorMap* map;
 
-
 public:
 
 	WiFiObserverFree(const float sigma, WiFiModel& model) : sigma(sigma), model(model) {
 
 	}
 
-
+	double getProbability(const Point3& pos_m, const Timestamp curTime, const WiFiMeasurements& obs) const {
-	double getProbability(const Point3& pos, const Timestamp curTime, const WiFiMeasurements& obs) const {
 
 		double prob = 1.0;
+		int numMatchingAPs = 0;
 
 		// process each measured AP
 		for (const WiFiMeasurement& entry : obs.entries) {
 
+			// sanity check
+			Assert::isFalse(entry.getTimestamp().isZero(), "wifi measurement without timestamp. coding error?");
+
 			// get the model's RSSI (if possible!)
-			const float modelRSSI = model.getRSSI(entry.getAP().getMAC(), pos);
+			const float modelRSSI = model.getRSSI(entry.getAP().getMAC(), pos_m);
 
 			// NaN? -> AP not known to the model -> skip
 			if (modelRSSI != modelRSSI) {continue;}
@@ -53,21 +56,29 @@ public:
 			// the measurement's age
 			const Timestamp age = curTime - entry.getTimestamp();
 
+			Assert::isTrue(age.ms() >= 0, "found a negative wifi measurement age. this does not make sense");
+			Assert::isTrue(age.ms() <= 40000, "found a 40 second old wifi measurement. maybe there is a coding error?");
+
 			// sigma grows with measurement age
 			const float sigma = this->sigma + this->sigmaPerSecond * age.sec();
 
 			// update probability
 			prob *= Distribution::Normal<double>::getProbability(modelRSSI, sigma, scanRSSI);
+			//prob *= Distribution::Region<double>::getProbability(modelRSSI, sigma, scanRSSI);
 
+			++numMatchingAPs;
 
 		}
 
+		// sanity check
+		Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
+
 		return prob;
 
 	}
 
 	template <typename Node> double getProbability(const Node& n, const Timestamp curTime, const WiFiMeasurements& obs, const int age_ms = 0) const {
-		throw "todo??";
+		throw Exception("todo??");
 	}
 
 };

sensors/radio/WiFiProbabilityGrid.h

@@ -6,14 +6,17 @@
 #include "../../math/Distributions.h"
 #include "../../data/Timestamp.h"
 
+#include "WiFiGridNode.h"
 #include "WiFiProbability.h"
 
+#include <unordered_set>
 
 /**
  * probability is calculated by comparing pre-calculated wifi-signal-strengths
  * attached to each grid-node with a given WiFiMeasurements data structure
  */
-class WiFiObserverGrid : public WiFiProbability {
+template <typename Node> class WiFiObserverGrid : public WiFiProbability {
+
 
 private:
 
@@ -21,14 +24,23 @@ private:
 	float sigma = 8.0f;
 
 	/** additional sigma-per-second (measurement age) to*/
-	float sigmaPerSecond = 1.5;
+	float sigmaPerSecond = 3;
 
+	std::unordered_set<MACAddress> knownAPs;
 
 public:
 
 	/** ctor with uncertainty */
 	WiFiObserverGrid(const float sigma) : sigma(sigma) {
-		;
+
+		//StaticAssert::AinheritsB<Node, WiFiGridNode>();
+
+		for (const AccessPoint& ap : Node::getMapAPs()) {
+			knownAPs.insert(ap.getMAC());
+		}
+
+		Assert::isFalse(knownAPs.empty(), "no APs known to the grid nodes?!");
+
 	}
 
 	/**
@@ -36,43 +48,66 @@ public:
 	 * compares the predicted signal-strengths stored on the given node
 	 * with the provided WiFi measurements
 	 */
-	template <typename Node> double getProbability(const Node& n, const Timestamp curTime, const WiFiMeasurements& obs) const {
+	double getProbability(const Node& n, const Timestamp curTime, const WiFiMeasurements& obs) const {
 
-		double prob = 0;
+		// compile-time sanity check. Node must be a subclass off WiFiGridNode
+		//StaticAssert::AinheritsB<Node, WiFiGridNode>();
+
+		double prob = 1;
+		int numMatchingAPs = 0;
 
 		// process each observed measurement
 		for (const WiFiMeasurement& measurement : obs.entries) {
 
+			// if an AP is not known to any of the nodes, just skip it
+			if (knownAPs.find(measurement.getAP().getMAC()) == knownAPs.end()) {
+				continue;}
+
 			// determine the age for this measurement
 			const Timestamp age = curTime - measurement.getTimestamp();
 
 			// sigma grows with measurement age
-			const float sigma = this->sigma + this->sigmaPerSecond * age.sec();
+			float sigma = this->sigma + this->sigmaPerSecond * age.sec();
 
 			// the RSSI from the scan
 			const float measuredRSSI = measurement.getRSSI();
 
 			// the RSSI from the model (if available!)
-			const float modelRSSI = n.getRSSI(measurement.getAP().getMAC());
+			float modelRSSI = n.getRSSI(measurement.getAP().getMAC());
 
-			// no model RSSI available?
+			// if no model RSSI is available, that means,
-			if (modelRSSI == 0) {continue;}
+			// the AP in question is not / only barely visible at this location
+			// assume a very low signal-strength and increase the sigma
+			if (modelRSSI == 0) {
+				modelRSSI = -100;
+				sigma *= 2;
+			}
 
 			// compare both
 			const double p = Distribution::Normal<double>::getProbability(measuredRSSI, sigma, modelRSSI);
+			//const double p = Distribution::Region<double>::getProbability(measuredRSSI, sigma, modelRSSI);
 
 			// adjust using log
-			prob += std::log(p);
+			//prob += std::log(p);
+			prob *= p;
+
+			++numMatchingAPs;
 
 		}
 
-		//return std::pow(std::exp(prob), 0.1);
+		// sanity check
-		return std::exp(prob);
+//		Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
+//		if (numMatchingAPs == 0) {return 0;}
+
+		// as not every node has the same number of visible/matching APs
+		// we MUST return something like the average probability
+		return prob;
+		//return std::pow(prob, 1.0/3.0);
 
 	}
 
 	/** gnuplot debug dump */
-	template <typename Node> void dump(Grid<Node>& grid, const Timestamp curTime, const WiFiMeasurements& obs, const std::string& fileName) {
+	void dump(Grid<Node>& grid, const Timestamp curTime, const WiFiMeasurements& obs, const std::string& fileName) {
 
 		std::ofstream out(fileName);
 		out << "splot '-' with points palette\n";

sensors/radio/model/WiFiModel.h

@@ -16,6 +16,9 @@ public:
 //	/** get the given access-point's RSSI at the provided location */
 //	virtual float getRSSI(const LocatedAccessPoint& ap, const Point3 p) = 0;
 
+	/** get a list of all APs known to the model */
+	virtual std::vector<AccessPoint> getAllAPs() const = 0;
+
 	/**
 	 * get the RSSI expected at the given location (in meter)
 	 * for an AP identified by the given MAC.

sensors/radio/model/WiFiModelLogDist.h

@@ -36,6 +36,13 @@ public:
 		;
 	}
 
+	/** get a list of all APs known to the model */
+	std::vector<AccessPoint> getAllAPs() const {
+		std::vector<AccessPoint> aps;
+		for (const auto it : accessPoints) {aps.push_back(AccessPoint(it.first));}
+		return aps;
+	}
+
 	/** make the given AP (and its parameters) known to the model */
 	void addAP(const MACAddress& accessPoint, const APEntry& params) {
 

sensors/radio/model/WiFiModelLogDistCeiling.h

@@ -39,9 +39,12 @@ private:
 
 public:
 
-	/** ctor */
+	/** ctor with floorplan (needed for ceiling position) */
 	WiFiModelLogDistCeiling(const Floorplan::IndoorMap* map) {
 
+		// sanity checks
+		Assert::isTrue(map->floors.size() >= 1, "map has no floors?!");
+
 		// position of all ceilings
 		for (Floorplan::Floor* f : map->floors) {
 			ceilingsAtHeight_m.push_back(f->atHeight);
@@ -49,6 +52,25 @@ public:
 
 	}
 
+	/** get a list of all APs known to the model */
+	std::vector<AccessPoint> getAllAPs() const {
+		std::vector<AccessPoint> aps;
+		for (const auto it : accessPoints) {aps.push_back(AccessPoint(it.first));}
+		return aps;
+	}
+
+	/** load AP information from the floorplan. use the given fixed TXP/EXP/WAF for all APs */
+	void loadAPs(const Floorplan::IndoorMap* map, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f) {
+
+		for (const Floorplan::Floor* floor : map->floors) {
+			for (const Floorplan::AccessPoint* ap : floor->accesspoints) {
+				const APEntry ape(ap->getPos(floor), txp, exp, waf);
+				addAP(MACAddress(ap->mac), ape);
+			}
+		}
+
+	}
+
 	/** make the given AP (and its parameters) known to the model */
 	void addAP(const MACAddress& accessPoint, const APEntry& params) {
 
@@ -57,11 +79,18 @@ public:
 		Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-50:-30]");
 		Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]");
 
+		Assert::equal(accessPoints.find(accessPoint), accessPoints.end(), "AccessPoint already present!");
+
 		// add
 		accessPoints.insert( std::pair<MACAddress, APEntry>(accessPoint, params) );
 
 	}
 
+	/** remove all added APs */
+	void clear() {
+		accessPoints.clear();
+	}
+
 	float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const override {
 
 		// try to get the corresponding parameters
@@ -90,6 +119,28 @@ public:
 protected:
 
 	FRIEND_TEST(LogDistanceCeilingModel, numCeilings);
+	FRIEND_TEST(LogDistanceCeilingModel, numCeilingsFloat);
+
+
+	/** get the number of ceilings between z1 and z2 */
+	float numCeilingsBetweenFloat(const Point3 pos1, const Point3 pos2) const {
+
+
+		const float zMin = std::min(pos1.z, pos2.z);
+		const float zMax = std::max(pos1.z, pos2.z);
+
+		float cnt = 0;
+
+		for (const float z : ceilingsAtHeight_m) {
+			if (zMin < z && zMax > z) {
+				const float dmax = zMax - z;
+				cnt += (dmax > 1) ? (1) : (dmax);
+			}
+		}
+
+		return cnt;
+
+	}
 
 	/** get the number of ceilings between z1 and z2 */
 	int numCeilingsBetween(const Point3 pos1, const Point3 pos2) const {
@@ -98,6 +149,24 @@ protected:
 		const float zMin = std::min(pos1.z, pos2.z);
 		const float zMax = std::max(pos1.z, pos2.z);
 
+#ifdef WITH_ASSERTIONS
+
+		static int numNear = 0;
+		static int numFar = 0;
+		for (const float z : ceilingsAtHeight_m) {
+			const float diff = std::min( std::abs(z-zMin), std::abs(z-zMax) );
+			if (diff < 0.1) {++numNear;} else {++numFar;}
+		}
+		if ((numNear + numFar) > 150000) {
+			Assert::isTrue(numNear < numFar*0.1,
+			    "many requests to the WiFiModel address nodes (very) near to a ground! \
+			    due to rounding issues, determining the number of floors between AP and point-in-question is NOT possible! \
+			    expect very wrong outputs! \
+			    consider adding the person's height to the questioned positions: p += Point3(0,0,1.3) "
+			);
+		}
+#endif
+
 		for (const float z : ceilingsAtHeight_m) {
 			if (zMin < z && zMax > z) {++cnt;}
 		}

sensors/radio/setup/WiFiFingerprint.h

@@ -0,0 +1,94 @@
+#ifndef WIFIFINGERPRINT_H
+#define WIFIFINGERPRINT_H
+
+
+#include "../../../geo/Point3.h"
+#include "../WiFiMeasurements.h"
+
+#include <unordered_map>
+
+/**
+ * denotes a wifi fingerprint
+ * known position and several measurements conducted at this position
+ *
+ * as several measurements were conducted, each AP is usually contained more than once!
+ */
+struct WiFiFingerprint {
+
+
+	/** real-world-position that was measured */
+	Point3 pos_m;
+
+	/** measurements (APs) at the given location */
+	WiFiMeasurements measurements;
+
+
+	/** ctor */
+	WiFiFingerprint() {;}
+
+	/** ctor */
+	WiFiFingerprint(const Point3 pos_m) : pos_m(pos_m) {;}
+
+
+
+	/** as each AP is contained more than once (scanned more than once), group them by MAC and use the average RSSI */
+	WiFiMeasurements average() {
+
+		// group scans by MAC (all measurements for one AP)
+		std::unordered_map<MACAddress, WiFiMeasurements> group;
+		for (WiFiMeasurement& m : measurements.entries) {
+			group[m.getAP().getMAC()].entries.push_back(m);
+		}
+
+		// create the output that contains the AP's average
+		WiFiMeasurements res;
+		for (auto& it : group) {
+			const WiFiMeasurements& apMeasurements = it.second;
+			WiFiMeasurement avg = apMeasurements.entries.front();			// average starts with a copy of the first entry (to get all data-fields beside the rssi)
+			for (int i = 1; i < (int)apMeasurements.entries.size(); ++i) {		// sum up all other entries [1:end]
+				avg.rssi += apMeasurements.entries[i].rssi;
+			}
+			avg.rssi /= apMeasurements.entries.size();
+			res.entries.push_back(avg);										// add to output
+		}
+
+		// done
+		return res;
+
+	}
+
+	/** serialize */
+	void write(std::ostream& out) const {
+		out << "pos: " << pos_m.x << " " << pos_m.y << " " << pos_m.z << "\n";
+		out << "num: " << measurements.entries.size() << "\n";
+		for (const WiFiMeasurement& wm : measurements.entries) {
+			out << wm.getTimestamp().ms() << " " << wm.ap.getMAC().asString() << " " << wm.getRSSI() << "\n";
+		}
+	}
+
+	/** deserialize */
+	void read(std::istream& inp) {
+		std::string tmp;
+
+		// read the position
+		inp >> tmp; if ("pos:" != tmp) {throw "error";}
+		inp >> pos_m.x >> pos_m.y >> pos_m.z;
+
+		// number of entries
+		inp >> tmp; if ("num:" != tmp) {throw "error";}
+		int numEntries; inp >> numEntries;
+
+		// read the entries
+		for (int i = 0; i < numEntries; ++i) {
+			uint64_t ms; inp >> ms;
+			std::string mac; inp >> mac;
+			float rssi; inp >> rssi;
+			WiFiMeasurement wm(AccessPoint(MACAddress(mac)), rssi, Timestamp::fromMS(ms));
+			measurements.entries.push_back(wm);
+		}
+
+	}
+
+};
+
+#endif // WIFIFINGERPRINT_H

sensors/radio/setup/WiFiOptimizer.h

@@ -0,0 +1,247 @@
+#ifndef OPTIMIZER_H
+#define OPTIMIZER_H
+
+#include "../../../floorplan/v2/Floorplan.h"
+#include "../../../floorplan/v2/FloorplanHelper.h"
+
+#include "../VAPGrouper.h"
+#include "../../../geo/BBox3.h"
+#include "../../../misc/Debug.h"
+
+#include "WiFiFingerprint.h"
+#include "../model/WiFiModel.h"
+#include "../model/WiFiModelLogDistCeiling.h"
+
+#include <KLib/math/optimization/NumOptAlgoDownhillSimplex.h>
+#include <KLib/math/optimization/NumOptAlgoGenetic.h>
+#include <KLib/math/optimization/NumOptAlgoRangeRandom.h>
+
+#include <string>
+#include <sstream>
+
+struct WiFiOptimizer {
+
+private:
+
+	/** combine one RSSI measurement with the position the signal was measured at */
+	struct RSSIatPosition {
+
+		/** real-world position (in meter) */
+		const Point3 pos_m;
+
+		/** measured signal strength (for one AP) */
+		const float rssi;
+
+		/** ctor */
+		RSSIatPosition(const Point3 pos_m, const float rssi) : pos_m(pos_m), rssi(rssi) {;}
+
+	};
+
+public:
+
+	struct APParams {
+		float x;
+		float y;
+		float z;
+		float txp;
+		float exp;
+		float waf;
+		Point3 getPos() const {return Point3(x,y,z);}
+		APParams() {;}
+		APParams(float x, float y, float z, float txp, float exp, float waf) : x(x), y(y), z(z), txp(txp), exp(exp), waf(waf) {;}
+		std::string asString() {
+			std::stringstream ss;
+			ss << "Pos:" << getPos().asString() << " TXP:" << txp << " EXP:" << exp << " WAF:" << waf;
+			return ss.str();
+		}
+	};
+
+	/** add MAC-info to params */
+	struct APParamsMAC {
+		MACAddress mac;
+		APParams params;
+		APParamsMAC(const MACAddress mac, const APParams& params) : mac(mac), params(params) {;}
+	};
+
+private:
+
+	Floorplan::IndoorMap* map;
+	const VAPGrouper vg;
+
+	/** each MAC-Adress has several position->rssi entries */
+	std::unordered_map<MACAddress, std::vector<RSSIatPosition>> apMap;
+
+	const char* name = "WiFiOptimizer";
+
+public:
+
+	/** ctor */
+	WiFiOptimizer(Floorplan::IndoorMap* map, const VAPGrouper& vg) : map(map), vg(vg) {
+		;
+	}
+
+
+	/** add a new fingerprint to the optimizers data-source */
+	void addFingerprint(const WiFiFingerprint& fp) {
+
+		// group the fingerprint's measurements by VAP (if configured)
+		const WiFiMeasurements measurements = vg.group(fp.measurements);
+
+		// add each available AP to its slot (lookup map)
+		for (const WiFiMeasurement& m : measurements.entries) {
+			const RSSIatPosition rap(fp.pos_m, m.rssi);
+			apMap[m.getAP().getMAC()].push_back(rap);
+		}
+
+	}
+
+	/** get a list of all to-be-optimized access-points (given by their mac-address) */
+	std::vector<MACAddress> getAllMACs() const {
+		std::vector<MACAddress> res;
+		for (const auto& it : apMap) {res.push_back(it.first);}
+		return res;
+	}
+
+	/** optimize all known APs */
+	std::vector<APParamsMAC> optimizeAll() const {
+
+		// sanity chekc
+		Assert::isFalse(getAllMACs().empty(), "no APs found for optimization!");
+
+		float errSum = 0;
+		std::vector<APParamsMAC> res;
+		for (const MACAddress& mac : getAllMACs()) {
+			float err;
+			const APParams params = optimize(mac, err);
+			res.push_back(APParamsMAC(mac, params));
+			errSum += err;
+		}
+
+		const float avgErr = errSum / getAllMACs().size();
+		Log::add(name, "average AP error is: " + std::to_string(avgErr) + " dB");
+
+		return res;
+
+	}
+
+	/** optimize the given AP */
+	APParams optimize(const MACAddress& mac, float& errResult) const {
+
+		// starting parameters do not matter for the current optimizer!
+		APParams params(0,0,0, -40, 2.5, -4.0);
+		constexpr float hugeError = 1e10;
+
+		// get all position->rssi measurements for this AP to compare them with the corresponding model estimations
+		const std::vector<RSSIatPosition>& entries = apMap.find(mac)->second;
+
+
+		// signal-strength-prediction-model...
+		WiFiModelLogDistCeiling model(map);
+
+		auto func = [&] (const float* data) {
+
+			const APParams* params = (APParams*) data;
+
+			// some sanity checks
+			if (params->waf > 0)	{return hugeError;}
+
+			if (params->txp < -50)	{return hugeError;}
+			if (params->txp > -30)	{return hugeError;}
+
+			if (params->exp > 4)	{return hugeError;}
+			if (params->exp < 1)	{return hugeError;}
+
+			// current position guess for the AP;
+			const Point3 apPos_m = params->getPos();
+
+			// add the AP [described by the current guess] to the signal-strength-prediction model
+			model.clear();
+			model.addAP(mac, WiFiModelLogDistCeiling::APEntry(apPos_m, params->txp, params->exp, params->waf));
+
+			float err = 0;
+			int cnt = 0;
+
+			// process each measurement
+			for (const RSSIatPosition& reading : entries) {
+
+				// get the model-estimation for the fingerprint's position
+				const float rssiModel = model.getRSSI(mac, reading.pos_m);
+
+				// difference between estimation and measurement
+				const float diff = std::abs(rssiModel - reading.rssi);
+
+				// adjust the error
+				err += diff*diff;
+				++cnt;
+
+				// max distance penality
+				// [unlikely to get a reading for this AP here!]
+				if (apPos_m.getDistance(reading.pos_m) > 150) {err += 999999;}
+
+			}
+
+			err /= cnt;
+			err = std::sqrt(err);
+
+			if (params->txp < -50) {err += 999999;}
+			if (params->txp > -35) {err += 999999;}
+
+			if (params->exp > 3.5) {err += 999999;}
+			if (params->exp < 1.0) {err += 999999;}
+
+			return err;
+
+		};
+
+
+		//
+		const BBox3 mapBBox = FloorplanHelper::getBBox(map);
+
+		using LeOpt = K::NumOptAlgoRangeRandom<float>;
+		const std::vector<LeOpt::MinMax> valRegion = {
+			 LeOpt::MinMax(mapBBox.getMin().x - 20, mapBBox.getMax().x + 20),	// x
+			 LeOpt::MinMax(mapBBox.getMin().y - 20, mapBBox.getMax().y + 20),	// y
+			 LeOpt::MinMax(mapBBox.getMin().z -  5, mapBBox.getMax().z +  5),	// z
+			 LeOpt::MinMax(-50,-30),	// txp
+			 LeOpt::MinMax(1,3),		// exp
+			 LeOpt::MinMax(-10,-4),		// waf
+		 };
+
+
+		// log
+		Log::add(name, "optimizing parameters for AP " + mac.asString() + " by using " + std::to_string(entries.size()) + " fingerprints", false);
+		Log::tick();
+
+		LeOpt opt(valRegion);
+		opt.setPopulationSize(500);	// USE MORE FOR PRODUCTION
+		opt.setNumIerations(150);
+		opt.calculateOptimum(func, (float*) &params);
+
+//		using LeOpt = K::NumOptAlgoGenetic<float>;
+//		LeOpt opt(6);
+//		opt.setPopulationSize(750);
+//		opt.setMaxIterations(50);
+//		opt.setElitism(0.05f);
+//		opt.setMutation(0.75f);
+//		//opt.setValRange({0.5, 0.5, 0.5, 0.1, 0.1, 0.1});
+//		opt.setValRegion(valRegion);
+
+//		K::NumOptAlgoDownhillSimplex<float, 6> opt;
+//		opt.setMaxIterations(100);
+//		opt.setNumRestarts(10);
+
+		opt.calculateOptimum(func, (float*) &params);
+		errResult = func((float*)&params);
+
+		Log::tock();
+		Log::add(name, mac.asString() + ": " + params.asString() + " @ " + std::to_string(errResult) +" dB err");
+
+		return params;
+
+	}
+
+
+};
+
+
+#endif // OPTIMIZER_H

tests/grid/TestAll.cpp

@@ -60,7 +60,7 @@ TEST(TestAll, Nav) {
 
 	// plot path
 	K::GnuplotSplotElementLines path; path.setColorHex("#0000ff"); path.setLineWidth(2);
-	DijkstraNode<GP>* dn = d.getNode(end);
+	const DijkstraNode<GP>* dn = d.getNode(end);
 	while (dn->previous != nullptr) {
 		path.add(K::GnuplotPoint3(dn->element->x_cm, dn->element->y_cm, dn->element->z_cm+50));
 		dn = dn->previous;

tests/grid/TestGridWalk2HeadingControl.cpp

@@ -104,7 +104,7 @@ TEST(GridWalk2HeadingControl, LIVE_walkHeading) {
 
 
 	struct MyControl {
-		float turnAngle = 0;
+		float turnSinceLastTransition_rad = 0;
 	} ctrl;
 
 
@@ -139,7 +139,7 @@ TEST(GridWalk2HeadingControl, LIVE_walkHeading) {
 		// run
 		for (int i = 0; i < 30; ++i) {
 
-			ctrl.turnAngle = (i == 0) ? (rad) : (0);
+			ctrl.turnSinceLastTransition_rad = (i == 0) ? (rad) : (0);
 
 			p.clearParticles();
 

tests/grid/TestGridWalkV2.cpp

@@ -60,7 +60,7 @@ TEST(GridWalk2, LIVE_error) {
 
 
 	struct Control {
-		float turnAngle = 0;		// keep the angle as-is!
+		float turnSinceLastTransition_rad = 0;		// keep the angle as-is!
 	} ctrl;
 
 	GridWalker<MyNode1239, MyState23452> walker;

tests/grid/TestStairs.cpp

@@ -25,6 +25,8 @@
 
 // ENSURE UNIQUE CLASS NAME
 struct MyNode345092134 : public GridPoint, public GridNode {
+	float walkImportance = 0;
+	float getWalkImportance() const {return walkImportance;}
 	float navImportance = 0;
 	float getNavImportance() const {return navImportance;}
 	MyNode345092134() {;}

tests/math/TestDistribution.cpp

@@ -138,6 +138,35 @@ TEST(Distribution, Region1) {
 
 
 
+}
+
+TEST(Distribution, Triangle) {
+
+		std::ofstream out("/tmp/1.dat");
+		for (float x = -9; x <= +9; x += 0.025) {
+			//const float y = Distribution::Region<float>::getProbability(0, 3, 1.2, x);
+			const float y = Distribution::Triangle<float>::getProbability(3, 6, x);
+			out << x << " " << y << "\n";
+		}
+		out.close();
+
+	Distribution::Triangle<float> dist1(0, 3);
+	ASSERT_NEAR(1.0, distCheckArea(dist1, -20, +20), 0.01);
+
+	Distribution::Triangle<float> dist2(0, 1);
+	ASSERT_NEAR(1.0, distCheckArea(dist2, -20, +20), 0.01);
+
+	Distribution::Triangle<float> dist3(1, 3);
+	ASSERT_NEAR(1.0, distCheckArea(dist3, -20, +20), 0.01);
+
+	Distribution::Triangle<float> dist4(1, 2);
+	ASSERT_NEAR(1.0, distCheckArea(dist4, -20, +20), 0.01);
+
+	Distribution::Triangle<float> dist5(-1, 4);
+	ASSERT_NEAR(1.0, distCheckArea(dist5, -20, +20), 0.01);
+
+
+
 }
 
 #endif

tests/nav/dijkstra/TestDijkstra.cpp

@@ -34,15 +34,15 @@ TEST(Dijkstra, build) {
 	d.build(&grid[idx5], &grid[idx3], tmp, 99999);
 
 	// start node must be "idx5"
-	DijkstraNode<GP>* n = d.getNode(grid[idx5]);
+	const DijkstraNode<GP>* n = d.getNode(grid[idx5]);
 	ASSERT_EQ(&grid[idx5], n->element);	ASSERT_EQ(nullptr, n->previous); ASSERT_EQ(0, n->cumWeight);
 
 	// "idx1" (the center) is reached via idx5
-	DijkstraNode<GP>* n2 = d.getNode(grid[idx1]);
+	const DijkstraNode<GP>* n2 = d.getNode(grid[idx1]);
 	ASSERT_EQ(&grid[idx1], n2->element);	ASSERT_EQ(&grid[idx5], n2->previous->element);
 
 	// "idx3" (the target) is reached via idx1 (the center)
-	DijkstraNode<GP>* n3 = d.getNode(grid[idx3]);
+	const DijkstraNode<GP>* n3 = d.getNode(grid[idx3]);
 	ASSERT_EQ(&grid[idx3], n3->element);	ASSERT_EQ(&grid[idx1], n3->previous->element);
 
 

tests/sensors/radio/TestLogDistanceCeilingModel.cpp

@@ -77,5 +77,45 @@ TEST(LogDistanceCeilingModel, numCeilings) {
 
 }
 
+TEST(LogDistanceCeilingModel, numCeilingsFloat) {
+
+	// dummy floorplan
+	Floorplan::Floor* f0 = new Floorplan::Floor(); f0->atHeight = 0;
+	Floorplan::Floor* f1 = new Floorplan::Floor(); f1->atHeight = 3;
+	Floorplan::Floor* f2 = new Floorplan::Floor(); f2->atHeight = 7;
+
+	Floorplan::IndoorMap map;
+	map.floors.push_back(f0);
+	map.floors.push_back(f1);
+	map.floors.push_back(f2);
+
+	WiFiModelLogDistCeiling model(&map);
+
+	const float d = 0.01;
+
+	ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,-1), Point3(0,0,0)), d );
+	ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,0), Point3(0,0,-1)), d );
+
+	ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,0), Point3(0,0,1)), d );
+	ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,1), Point3(0,0,0)), d );
+
+	ASSERT_NEAR(0.5, model.numCeilingsBetweenFloat(Point3(0,0,-0.01), Point3(0,0,+0.50)), d );
+	ASSERT_NEAR(0.5, model.numCeilingsBetweenFloat(Point3(0,0,+0.50), Point3(0,0,-0.01)), d );
+
+	ASSERT_NEAR(0.2, model.numCeilingsBetweenFloat(Point3(0,0,2.99), Point3(0,0,3.20)), d );
+	ASSERT_NEAR(0.2, model.numCeilingsBetweenFloat(Point3(0,0,3.20), Point3(0,0,2.99)), d );
+
+	ASSERT_NEAR(1.0, model.numCeilingsBetweenFloat(Point3(0,0,6.99), Point3(0,0,8.33)), d );
+	ASSERT_NEAR(1.0, model.numCeilingsBetweenFloat(Point3(0,0,8.33), Point3(0,0,6.99)), d );
+	ASSERT_NEAR(2.0, model.numCeilingsBetweenFloat(Point3(0,0,0.00), Point3(0,0,8.33)), d );
+	ASSERT_NEAR(2.0, model.numCeilingsBetweenFloat(Point3(0,0,8.33), Point3(0,0,0.00)), d );
+
+	ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,7.00), Point3(0,0,99)), d );
+
+	ASSERT_NEAR(1, model.numCeilingsBetweenFloat(Point3(0,0,0), Point3(0,0,7)), d );
+	ASSERT_NEAR(3, model.numCeilingsBetweenFloat(Point3(0,0,-1), Point3(0,0,8)), d );
+
+}
+
 
 #endif

tests/sensors/radio/TestWiFiGrid.cpp

@@ -90,11 +90,11 @@ TEST(WiFiGridModelLogDist, create) {
 	model.addAP(ap3, WiFiModelLogDist::APEntry( Point3(0,20,0), -40, 1.5));
 	model.addAP(ap4, WiFiModelLogDist::APEntry( Point3(20,20,0), -40, 1.5));
 
-	std::vector<AccessPoint> aps = {
+//	std::vector<AccessPoint> aps = {
-		AccessPoint(ap1), AccessPoint(ap2), AccessPoint(ap3), AccessPoint(ap4)
+//		AccessPoint(ap1), AccessPoint(ap2), AccessPoint(ap3), AccessPoint(ap4)
-	};
+//	};
 
-	WiFiGridEstimator::estimate(grid, model, aps);
+	WiFiGridEstimator::estimate(grid, model, 0);
 
 
 	ASSERT_EQ(4, grid[0].getNumVisibleAPs());				// 4 APs visible at this node
@@ -117,7 +117,7 @@ TEST(WiFiGridModelLogDist, create) {
 	obs.entries.push_back(WiFiMeasurement(MACAddress("00:00:00:00:00:03"), -55, ts));
 	obs.entries.push_back(WiFiMeasurement(MACAddress("00:00:00:00:00:04"), -55, ts));
 
-	WiFiObserverGrid observer(5.0f);
+	WiFiObserverGrid<TestNode190231> observer(5.0f);
 	const TestNode190231& gn = grid.getNodeFor(GridPoint(1000,1000,0));
 	const float p = observer.getProbability(gn, ts, obs);
 

tests/sensors/radio/TestWiFiOptimizer.cpp

@@ -0,0 +1,83 @@
+#ifdef WITH_TESTS
+
+#include "../../Tests.h"
+#include "../../../sensors/radio/setup/WiFiOptimizer.h"
+#include "../../../sensors/radio/setup/WiFiFingerprint.h"
+#include "../../../misc/Debug.h"
+
+#include <random>
+
+/**
+ * test the wifi-optimizer by generating synthetic fingerprints and optimizing parameters from them
+ */
+TEST(WiFiOptimizer, optimize) {
+
+
+	const VAPGrouper vg(VAPGrouper::Mode::DISABLED, VAPGrouper::Aggregation::AVERAGE);
+
+	const MACAddress mac1("00:00:00:00:00:01");
+	const MACAddress mac2("00:00:00:00:00:02");
+
+	const Point3 pos1(10, 12, 5);
+	const Point3 pos2(20, -10, 2);
+
+	// building with one floor at 3.0 meters
+	Floorplan::IndoorMap map;
+	Floorplan::Floor floor1; floor1.atHeight = 3.0f;
+	Floorplan::FloorOutlinePolygon poly1; poly1.poly.points.push_back(Point2(-30, -30)); poly1.poly.points.push_back(Point2(+30, +30));
+	floor1.outline.push_back(&poly1);
+	map.floors.push_back(&floor1);
+
+	// add the two APs to the model
+	WiFiModelLogDistCeiling mdl(&map);
+	mdl.addAP(mac1, WiFiModelLogDistCeiling::APEntry(pos1, -40, 2, -4));
+	mdl.addAP(mac2, WiFiModelLogDistCeiling::APEntry(pos2, -40, 2, -4));
+
+	// generate some (synthetic) fingerprints
+	std::minstd_rand gen;
+	std::vector<WiFiFingerprint> fingerprints;
+	for (int i = 0; i < 50; ++i) {
+
+		std::uniform_real_distribution<float> distX(-30, +30);
+		std::uniform_real_distribution<float> distY(-30, +30);
+		std::uniform_real_distribution<float> distZ( -9,  +9);
+
+		// get a random position and calculate the model RSSIs
+		const Point3 randomPt(distX(gen), distY(gen), distZ(gen));
+		const float rssi1 = mdl.getRSSI(mac1, randomPt);
+		const float rssi2 = mdl.getRSSI(mac2, randomPt);
+
+		// construct a corresponding synthetic fingerprint
+		WiFiFingerprint fp(randomPt);
+		fp.measurements.entries.push_back(WiFiMeasurement(AccessPoint(mac1), rssi1));
+		fp.measurements.entries.push_back(WiFiMeasurement(AccessPoint(mac2), rssi2));
+		fingerprints.push_back(fp);
+
+	}
+
+	WiFiOptimizer opt(&map, vg);
+	for (const WiFiFingerprint& fp : fingerprints) {
+		opt.addFingerprint(fp);
+	}
+
+	ASSERT_EQ(2, opt.getAllMACs().size());
+	float errRes;
+
+	const WiFiOptimizer::APParams params1 = opt.optimize(mac1, errRes);
+	ASSERT_TRUE(errRes < 0.1);
+	ASSERT_NEAR(0, pos1.getDistance(params1.getPos()), 0.4);		// apx position estimation
+	ASSERT_NEAR(-40, params1.txp, 1.0);
+	ASSERT_NEAR(2, params1.exp, 0.1);
+	ASSERT_NEAR(-4, params1.waf, 0.5);
+
+	const WiFiOptimizer::APParams params2 = opt.optimize(mac2, errRes);
+	ASSERT_TRUE(errRes < 0.1);
+	ASSERT_NEAR(0, pos2.getDistance(params2.getPos()), 0.4);		// apx position estimation
+	ASSERT_NEAR(-40, params1.txp, 1.0);
+	ASSERT_NEAR(2, params2.exp, 0.1);
+	ASSERT_NEAR(-4, params2.waf, 0.5);
+
+
+}
+
+#endif