Indoor/wifi/estimate/ray3/DataMap3.h

#ifndef DATAMAP3_H
#define DATAMAP3_H


#include "../../../geo/BBox3.h"
#include <vector>
#include <functional>
#include <mutex>

template <typename T> class DataMap3 {

private:

	float sx_m;
	float sy_m;
	float sz_m;

	float x_m;
	float y_m;
	float z_m;

	int gridSize_cm;

	BBox3 bbox;

	int nx;
	int ny;
	int nz;

	T* data = nullptr;

public:

	/** ctor */
	DataMap3() {
		;
	}

	~DataMap3() {

		// cleanup
		cleanup();

	}

	DataMap3(const DataMap3&) = delete;
	DataMap3* operator = (const DataMap3& o) = delete;


	/** does the map contain the given indices? */
	bool containsGrid(const int x, const int y, const int z) const {
		return	(x >= 0) && (y >= 0) && (z >= 0) &&
				(x < nx) && (y < ny) && (z < nz);
	}

	/** does the map contain the given coordinate? */
	bool contain(const float x_m, const float y_m, const float z_m) const {
		return bbox.contains(Point3(x_m, y_m, z_m));
	}

	void resize(const BBox3 bbox, const int gridSize_cm) {

		// cleanup
		cleanup();

		this->bbox = bbox;

		// slightly increase to pervent out-of-bounds due to rounding
		float buffer_m = 1;

		// start-offset
		sx_m = bbox.getMin().x - buffer_m;
		sy_m = bbox.getMin().y - buffer_m;
		sz_m = bbox.getMin().z - buffer_m;

		// size in meter
		x_m = (bbox.getMax().x - bbox.getMin().x) + 2*buffer_m;
		y_m = (bbox.getMax().y - bbox.getMin().y) + 2*buffer_m;
		z_m = (bbox.getMax().z - bbox.getMin().z) + 2*buffer_m;

		// number of elements in the grid
		this->gridSize_cm = gridSize_cm;
		nx = (x_m*100) / gridSize_cm;
		ny = (y_m*100) / gridSize_cm;
		nz = (z_m*100) / gridSize_cm;

		// allocate and reset all to 0.0
		data = new T[nx*ny*nz];

	}

	/** get the used grid-size (in cm) */
	int getGridSize_cm() const {return gridSize_cm;}

	void set(const float x_m, const float y_m, const float z_m, const T val) {
		const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm);
		const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm);
		const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm);
		setGrid(ix, iy, iz, val);
	}

	T get(const float x_m, const float y_m, const float z_m) const {
		const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm );
		const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm );
		const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm );
		return getGrid(ix, iy, iz);
	}

	T& getRef(const float x_m, const float y_m, const float z_m) {
		const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm );
		const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm );
		const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm );
		return getGridRef(ix, iy, iz);
	}

	T getGrid(const int ix, const int iy, const int iz) const {
		Assert::isBetween(ix, 0, nx-1, "x out of range");
		Assert::isBetween(iy, 0, ny-1, "y out of range");
		Assert::isBetween(iz, 0, nz-1, "z out of range");
		const int idx = ix + iy*nx + iz*nx*ny;
		return data[idx];
	}

	T& getGridRef(const int ix, const int iy, const int iz) {
		Assert::isBetween(ix, 0, nx-1, "x out of range");
		Assert::isBetween(iy, 0, ny-1, "y out of range");
		Assert::isBetween(iz, 0, nz-1, "z out of range");
		const int idx = ix + iy*nx + iz*nx*ny;
		return data[idx];
	}

	void setGrid(const int ix, const int iy, const int iz, const T val) {
		Assert::isBetween(ix, 0, nx-1, "x out of range");
		Assert::isBetween(iy, 0, ny-1, "y out of range");
		Assert::isBetween(iz, 0, nz-1, "z out of range");
		const int idx = ix + iy*nx + iz*nx*ny;
		data[idx] = val;
	}

	void forEach(std::function<void(float,float,float,T)> func) const {
		for (int iz = 0; iz < nz; ++iz) {
			for (int iy = 0; iy < ny; ++iy) {
				for (int ix = 0; ix < nx; ++ix) {
					const float x = (ix * gridSize_cm / 100.0f) + sx_m;
					const float y = (iy * gridSize_cm / 100.0f) + sy_m;
					const float z = (iz * gridSize_cm / 100.0f) + sz_m;
					func(x,y,z, getGrid(ix, iy, iz));
				}
			}
		}
	}

	/*
	void dump() {

		std::ofstream os("/tmp/1.dat");
		const float s = 1;//gridSize_cm / 100.0f;
//		for (int y = 0; y < ny; ++y) {
//			for (int x = 0; x < nx; ++x) {
//				float rssi = data[x+y*nx];
//				rssi = (rssi == 0) ? (-100) : (rssi);
//				os << (x*s) << " " << (y*s) << " " << rssi << "\n";
//			}
//			os << "\n";
//		}
		for (int y = 0; y < ny; ++y) {
			for (int x = 0; x < nx; ++x) {
				float rssi = data[x+y*nx];
				rssi = (rssi == 0) ? (-100) : (rssi);
				os << rssi << " ";
			}
			os << "\n";
		}
		os.close();

	}
	*/

private:

	void cleanup() {
		delete[] data;
		data = nullptr;
	}


};



struct DataMap3SignalEntry {



	struct Entry {
		float rssi;
		float distanceToAP;
		Entry(float rssi, float distanceToAP) : rssi(rssi), distanceToAP(distanceToAP) {;}
	};

	std::vector<Entry> entries;

	void add(const float rssi, const float distanceToAP) {
		static std::mutex mtx;
		Entry e(rssi, distanceToAP);
		mtx.lock();
		entries.push_back(e);
		mtx.unlock();
	}

	float getMaxRSSI() const {
		auto comp = [] (const Entry& e1, const Entry& e2) {return e1.rssi < e2.rssi;};
		if (entries.empty()) {return -120;}
		auto it = std::max_element(entries.begin(), entries.end(), comp);
		return it->rssi;
	}


};

class DataMap3Signal : public DataMap3<DataMap3SignalEntry> {

public:

	/** update average */
	void update(const float x_m, const float y_m, const float z_m, const float rssi, const float distanceToAP) {

		DataMap3SignalEntry& entry = getRef(x_m, y_m, z_m);
		entry.add(rssi, distanceToAP);

	}

};

#endif // DATAMAP3_H