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

	/*
	void blured(DataMap<T>& dst) const {

		const int s = 2;
		dst.resize(this->bbox, this->gridSize_cm);

		for (int iy = 0; iy < ny; ++iy) {
			for (int ix = 0; ix < nx; ++ix) {

				float valSum = 0;
				int cntSum = 0;

				for (int oy = -s; oy <= +s; ++oy) {
					for (int ox = -s; ox <= +s; ++ox) {

						const int x = ix+ox;
						const int y = iy+oy;

						if (containsGrid(x,y)) {
							valSum += getGrid(x,y);
							++cntSum;
						}

					}
				}

				dst.setGrid(ix, iy, valSum/cntSum);

			}
		}

	}
	*/

	/** 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 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 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];
	}

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

	void forEach(std::function<void(float,float,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,getGrid(ix, iy));
			}
		}
	}

	/*
	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 DataMapSignalEntry {

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


};

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

	}

};

#endif // DATAMAP2_H