#ifndef DATAMAP3_H
#define DATAMAP3_H


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

namespace Ray3D {

	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