#ifndef WIFIRAYTRACE2D_H
#define WIFIRAYTRACE2D_H

#include "../../../geo/Point2.h"
#include "../../../geo/Line2.h"
#include "../../../geo/BBox2.h"
#include "../../../geo/Ray2.h"

#include "../../../floorplan/v2/Floorplan.h"
#include "../../../floorplan/v2/FloorplanHelper.h"

#include "../../../geo/volume/BVHDebug.h"

#include "DataMap2.h"
#include "MaterialOptions.h"

#include <random>

// http://www.realtimerendering.com/resources/RTNews/html/rtnv10n1.html#art3
// http://math.stackexchange.com/questions/13261/how-to-get-a-reflection-vector

// RADIO WAVES
// http://people.seas.harvard.edu/%7Ejones/es151/prop_models/propagation.html			Indoor Wireless RF Channel
// http://www.electronics-radio.com/articles/antennas-propagation/propagation-overview/radio-em-wave-reflection.php








struct Obstacle2D {
	Floorplan::Material mat;
	Line2 line;
	Obstacle2D(Floorplan::Material mat, Line2 line) : mat(mat), line(line) {;}
};


struct Obstacle2DWrapper {
	static std::vector<Point2> getVertices(const Obstacle2D& obs) {
		return {obs.line.p1, obs.line.p2};
	}

	static std::vector<Point2> getDebugLines(const Obstacle2D& obs) {
		return {obs.line.p1, obs.line.p2};
	}
};

struct Hit {
	const Obstacle2D* obstacle;
	float dist;
	Point2 pos;
	Point2 normal;
	Floorplan::Material material;
	bool stopHere = false;
	Hit() {;}
	Hit(const float dist, const Point2 pos, const Point2 normal) : dist(dist), pos(pos), normal(normal) {;}
};

struct StateRay2 : public Ray2 {

	/** already travelled distance from the AP (by all previous rays */
	float totalLength;

	/** attenuation taken since the start */
	float totalAttenuation;

	const Obstacle2D* lastObstacle;
	int depth = 0;

	/** empty ctor */
	StateRay2() : totalLength(NAN), totalAttenuation(NAN) {;}

	/** ctor */
	StateRay2(const Point2 start, const Point2 dir) : Ray2(start, dir), totalLength(0), totalAttenuation(0) {
		;
	}

	inline float getRSSI(const float addDist = 0) const {
		const float txp = -40;
		const float gamma = 1.2f;
		return (txp - 10*gamma*std::log10(totalLength + addDist)) - totalAttenuation;
	}

};



class WiFiRaytrace2D {

private:

	const Floorplan::Floor* floor;
	BBox2 bbox;
	Point2 apPos;

	BVH2Debug<Obstacle2D, BoundingVolumeCircle2, Obstacle2DWrapper> tree;

	DataMapSignal dm;

	struct Limit {
		static constexpr int RAYS = 2000;
		static constexpr int HITS = 25;
		static constexpr float RSSI = -110;
	};

public:

	/** ctor */
	WiFiRaytrace2D(const Floorplan::Floor* floor, const int gs, const Point2 apPos) : floor(floor), apPos(apPos) {

		// get the floor's 3D-bbox
		BBox3 bb3 = FloorplanHelper::getBBox(floor);

		// 2D only party
		bbox = BBox2(bb3.getMin().xy(), bb3.getMax().xy());

		// allocate
		dm.resize(bbox, gs);

		// build tree
		for (Floorplan::FloorObstacle* fo : floor->obstacles) {

			const Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(fo);
			const Floorplan::FloorObstacleDoor* door = dynamic_cast<Floorplan::FloorObstacleDoor*>(fo);

			if (line) {
				Obstacle2D obs(line->material, Line2(line->from, line->to));
				tree.add(obs, true);
			} else if (door) {
				Obstacle2D obs(door->material, Line2(door->from, door->to));
				tree.add(obs, true);
			}

		}

//		for (int i = 0; i < 200; ++i) {
//			tree.optimize(1);
//			tree.show(1500, false);
//			usleep(1000*100);
//		}



//		tree.show();
		tree.optimize(250);
//		int depth = tree.getDepth();
		tree.show(1500,false);

		constructNeighbors(dm);
		showNeighbors(dm);

		int i = 0;


	}

public:

	void showNeighbors(DataMapSignal& map) {

		static K::Gnuplot gp;
		K::GnuplotPlot plot;
		K::GnuplotPlotElementLines lines; plot.add(&lines);

		auto func = [&] (const int ix, const int iy, const DataMapSignalEntry& e) {

			const Point2 p1 = map.gridToPos(ix, iy);

			for (const int idx : e.neighbors) {
				const Point2 p2 = map.idxToPos(idx);

				K::GnuplotPoint2 gp1(p1.x, p1.y);
				K::GnuplotPoint2 gp2(p2.x, p2.y);
				lines.addSegment(gp1, gp2);

			}

		};

		map.forEachGrid(func);

		gp.draw(plot);
		gp.flush();

		int i = 0;


	}

	/** construct neighborship relations between nodes not intersected by walls */
	void constructNeighbors(DataMapSignal& map) {

		auto func = [&] (const int ix, const int iy, DataMapSignalEntry& e) {
			for (int dy = -1; dy <= +1; ++dy) {
				for (int dx = -1; dx <= +1; ++dx) {

					// x/y index for the potential neighbor
					const int ix2 = ix+dx;
					const int iy2 = iy+dy;

					// out of bounds?
					if (!map.containsGrid(ix2,iy2)) {continue;}

					// intersection test
					const Point2 p1 = map.gridToPos(ix, iy);
					const Point2 p2 = map.gridToPos(ix2, iy2);
					const Line2 line(p1,p2);
					const Point2 dir = (p2-p1).normalized();
					const Ray2 ray(p1, dir);

					bool isConnectable = true;
					auto onHit = [&] (const Obstacle2D& obs) {

						if (obs.line.getSegmentIntersection(line)) {isConnectable = false;}

					};

					tree.getHits(ray, onHit);

					if (isConnectable) {
						e.neighbors.push_back(map.getIndex(ix2, iy2));
					}

				}
			}
		};

		map.forEachGrid(func);

	}


	const DataMapSignal& estimate() {

		for (int i = 0; i < Limit::RAYS; ++i) {

			std::cout << "ray: " << i << std::endl;

			// angle between starting-rays
			const float angle = (float)M_PI*2.0f * i / Limit::RAYS;

			// direction
			const Point2 dir(std::cos(angle), std::sin(angle));

			// ray
			const StateRay2 ray(apPos, dir);

			// run!
			trace(ray);

		}

		return dm;

	}



private:

	static float dot(const Point2 p1, const Point2 p2) {
		return (p1.x * p2.x) + (p1.y * p2.y);
	}

	void trace(const StateRay2& ray) {


		// get the nearest intersection with the floorplan
		const Hit hit = getNearestHit(ray);

		// rasterize the ray's way onto the map
		rasterize(ray, hit);

		// continue?
		if (hit.stopHere) {return;}
		if (ray.getRSSI(hit.dist) < Limit::RSSI) {return;}
		if (ray.depth > Limit::HITS) {return;}

		// apply effects
		reflected(ray, hit);
		shadowed(ray, hit);

	}

	static inline float getAttenuation(const Hit& h) {
		return Materials::get().getAttributes(h.material).shadowing.attenuation;
	}

	static inline float getAttenuationForReflection(const Hit& h) {
		return Materials::get().getAttributes(h.material).reflection.attenuation;
	}

	/** perform reflection and continue tracing */
	void shadowed(const StateRay2& ray, const Hit& hit) {

		StateRay2 next = ray;							// continue into the same direction
		next.depth = ray.depth + 1;
		next.lastObstacle = hit.obstacle;
		next.start = hit.pos;
		next.totalLength += hit.dist;
		next.totalAttenuation += getAttenuation(hit);	// contribute attenuation

		trace(next);

	}

	/** perform reflection and continue tracing */
	void reflected(const StateRay2& ray, const Hit& hit) {

		Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized");
		Assert::isNear(1.0f, hit.normal.length(), 0.01f, "not normalized");

		// angle to wide or narrow? -> skip
		const float d = std::abs(dot(ray.dir, hit.normal));
		if (d < 0.05) {return;}		// parallel
		if (d > 0.95) {return;}		// perpendicular;

		static std::minstd_rand gen;
		static std::normal_distribution<float> dist(0, 0.02);
		const float mod = dist(gen);
		Point2 normalMod(
			hit.normal.x * std::cos(mod) - hit.normal.y * std::sin(mod),
			hit.normal.y * std::cos(mod) - hit.normal.x * std::sin(mod)
		);


		// reflected ray;
		StateRay2 reflected;
		reflected.depth = ray.depth + 1;
		reflected.lastObstacle = hit.obstacle;
		reflected.start = hit.pos;
		reflected.totalLength = ray.totalLength + hit.dist;
		reflected.totalAttenuation = ray.totalAttenuation + getAttenuationForReflection(hit);		// TODO
		reflected.dir = (ray.dir - normalMod * (dot(ray.dir, hit.normal)) * 2).normalized();		// slight variation

		trace(reflected);

	}


	static inline double crossVal(const Point2 v, const Point2 w) {
		return ((double)v.x*(double)w.y) - ((double)v.y*(double)w.x);
	}

	static inline void hitTest(const Ray2& ray, const Obstacle2D& obs, Hit& nearest) {

		const float minDist = 0.01;		// prevent errors hitting the same obstacle twice

		// do not hit the last obstacle again
		//if (ray.lastObstacle == fo) {continue;}

		// get the line
		Point2 hit;
		if ( obs.line.intersects(ray, hit) ) {			// TODO rounding issues?!
			const float dist = hit.getDistance(ray.start);
			if (dist > minDist && dist < nearest.dist) {
				nearest.obstacle = &obs;
				nearest.dist = dist;
				nearest.pos = hit;
				nearest.normal = (obs.line.p2 - obs.line.p1).perpendicular().normalized();
				nearest.material = obs.mat;
			}
		}

	}

	Hit getNearestHit(const StateRay2& ray) const {

		Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized!");

		const Line2 longRay(ray.start, ray.start + ray.dir*1000);

		//const float minDist = 0;//0.01;		// prevent errors hitting the same obstacle twice
		const float MAX = 999999;
		Hit nearest; nearest.dist = MAX;

		//int hits = 0;

		const auto onHit = [ray, &nearest] (const Obstacle2D& obs) {
			//++hits;
			//hitTest(longRay, obs, nearest);
			hitTest(ray, obs, nearest);
		};

		tree.getHits(ray, onHit);

		// no hit with floorplan: limit to bounding-box!
		if (nearest.dist == MAX) {
			//const BBox2 bb( Point2(0,0), Point2(w, h) );
			Point2 hit;
			for (Line2 l : bbox.lines()) {
				if (l.getSegmentIntersection(longRay, hit)) {
					nearest.obstacle = nullptr;
					nearest.dist = hit.getDistance(ray.start);
					nearest.pos = hit;
					nearest.normal = (l.p2 - l.p1).perpendicular().normalized();
					nearest.material = Floorplan::Material::UNKNOWN;
					nearest.stopHere = true;
				}
			}
		}

		return nearest;

	}

	/** rasterize the ray (current pos -> hit) onto the map */
	void rasterize(const StateRay2& ray, const Hit& hit) {

		const Point2 dir = hit.pos - ray.start;
		const Point2 dirN = dir.normalized();
		const Point2 step = dirN * (dm.getGridSize_cm()/100.0f) * 0.7f;		// TODO * 1.0 ??
		const int steps = dir.length() / step.length();

		// sanity check
		// ensure the direction towards the nearest intersection is the same as the ray's direction
		// otherwise the intersection-test is invalid
#ifdef WITH_ASSERTIONS
		if (dir.normalized().getDistance(ray.dir) > 0.1) {
			return;
			std::cout << "direction to the nearest hit is not the same direction as the ray has. incorrect intersection test?!" << std::endl;
		}
#endif

		for (int i = 0; i <= steps; ++i) {

			const Point2 dst = ray.start + (step*i);
			const float partLen = ray.start.getDistance(dst);
			//const float len = ray.totalLength + partLen;

//			const float curRSSI = dm.get(dst.x, dst.y);
			const float newRSSI = ray.getRSSI(partLen);
			const float totalLen = ray.totalLength + partLen;

//			// ray stronger than current rssi?
//			if (curRSSI == 0 || curRSSI < newRSSI) {
//				dm.set(dst.x, dst.y, newRSSI);
//			}

			dm.update(dst.x, dst.y, newRSSI, totalLen);

		}


	}




};

#endif // WIFIRAYTRACE2D_H