#ifndef DATAMAP2_H
#define DATAMAP2_H

#include "../../../geo/BBox2.h"
#include <vector>
#include <functional>

template <typename T> class DataMap {

private:

	float sx_m;
	float sy_m;

	float x_m;
	float y_m;
	int gridSize_cm;

	BBox2 bbox;

	int nx;
	int ny;

	T* data = nullptr;

public:

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

	~DataMap() {

		// cleanup
		cleanup();

	}

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

	T& operator [] (const int idx) {
		return data[idx];
	}

	const T& operator [] (const int idx) const {
		return data[idx];
	}


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

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

	void resize(const DataMap& other) {
		resize(other.bbox, other.gridSize_cm);
	}

	void resize(const BBox2 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;

		// 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;

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

		// allocate and reset all to 0.0
		data = new T[nx*ny];
		//std::fill(&data[0], &data[nx*ny], 0);

	}

	/** 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 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);
		setGrid(ix, iy, val);
	}

	T& getRef(const int idx) const {
		return data[idx];
	}

	T get(const float x_m, const float y_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 );
		return getGrid(ix, iy);
	}

	T& getRef(const float x_m, const float y_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 );
		return getGridRef(ix, iy);
	}

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

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

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

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

	/** convert grid indices to point coordinates */
	Point2 gridToPos(const int ix, const int iy) const {
		return Point2(
			(ix * gridSize_cm / 100.0f) + sx_m,
			(iy * gridSize_cm / 100.0f) + sy_m
		);
	}

	/** convert 1D array index to point coordinates */
	Point2 idxToPos(const int idx) const {
		const int ix = idx % nx;
		const int iy = idx / nx;
		return gridToPos(ix, iy);
	}

	/** convert 2D to 1D index */
	int getIndex(const int ix, const int iy) const {
		return ix + iy*nx;
	}

	void forEach(std::function<void(float,float,const T&)> func) const {
		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;
				func(x,y,getGridRef(ix, iy));
			}
		}
	}

	void forEachGrid(std::function<void(int,int,T&)> func) {
		for (int iy = 0; iy < ny; ++iy) {
			for (int ix = 0; ix < nx; ++ix) {
				func(ix,iy,getGridRef(ix, iy));
			}
		}
	}

private:

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


};


struct DataMapNeighbors {

	/** reference to all neighbors */
	std::vector<int> neighbors;

};


struct DataMapSignalEntry : public DataMapNeighbors {

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

	std::vector<Entry> entries;

	void add(const DataMapSignalEntry& o) {
		for (const Entry& e : o.entries) {entries.push_back(e);}
	}

	void add(const float rssi, const float distanceToAP) {
		Entry e(rssi, distanceToAP);
		entries.push_back(e);
	}

	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;
	}

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

	float getAvgFirst() const {
		if (entries.empty()) {return -120;}
		if (entries.size()==1) {return entries.front().rssi;}
		std::vector<Entry> copy = entries;
		auto comp = [] (const Entry& e1, const Entry& e2) {return e1.rssi > e2.rssi;};
		std::sort(copy.begin(), copy.end(), comp);

		float sum = 0;
		int cnt = std::min((int)copy.size(), 15);
		for (int i = 0; i < cnt; ++i) {
			sum += copy[i].rssi;
		}
		return sum/cnt;

	}

//	float get2ndMaxRSSI() const {
//		if (entries.empty()) {return -120;}
//		if (entries.size()==1) {return entries.front().rssi;}
//		std::vector<Entry> copy = entries;
//		auto comp = [] (const Entry& e1, const Entry& e2) {return e1.rssi < e2.rssi;};
//		std::sort(copy.begin(), copy.end(), comp);
//		return copy[copy.size()-2].rssi;
//	}


};


class DataMapSignal : public DataMap<DataMapSignalEntry> {

public:

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

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

	}

};


class DataMap2Factory {

public:


	/** combine neighboring nodes into one */
	template <typename T> static void combine(const DataMap<T>& map, DataMap<T>& dst) {

		auto forEach = [&] (const float, const float, const T& n) {
			for (int idx : n.neighbors) {
				dst[idx].add(n);
			}
		};

		map.forEach(forEach);

	}

	/** fill empty fields with the values of their immediate neighbors */
	template <typename T> static void fillGaps(const DataMap<T>& map, DataMap<T>& dst) {

		auto forEach = [&] (const float, const float, const T& n) {
			if (n.entries.empty()) {
				for (int idx : n.neighbors) {
					dst[idx].add(n);
				}
			}
		};

		map.forEach(forEach);

	}


};

#endif // DATAMAP2_H