#ifndef WIFIRAYTRACE3D_H
#define WIFIRAYTRACE3D_H

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

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

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

#include "DataMap3.h"
#include "../../../geo/Ray3.h"
#include "MaterialOptions.h"
#include "../../../floorplan/3D/Obstacle3.h"
#include "../../../floorplan/3D/Builder.h"
//#include "ObstacleTree.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


// 3D
// http://graphics.stanford.edu/courses/cs148-10-summer/docs/2006--degreve--reflection_refraction.pdf

using namespace Floorplan3D;

namespace Ray3D {

	struct Intersection {
		Point3 pos;
		const Obstacle3D* obs;
		Intersection(const Point3 pos, const Obstacle3D* obs) : pos(pos), obs(obs) {;}
	};

	struct StateRay3 : public Ray3 {

		//std::vector<Intersection> stack;

		int depth = 0;
		float totalLen = 0;
		float totalAttenuation = 0;

		const Obstacle3D* isWithin = nullptr;

		/** empty ctor */
		StateRay3() {;}

		/** ctor */
		StateRay3(const Point3 start, const Point3 dir) : Ray3(start, dir) {
			;
		}

		StateRay3 enter(const Point3 hitPos, const Obstacle3D* obs) const {

			StateRay3 next = getNext(hitPos);
			Assert::isNull(this->isWithin, "code error: isWithin");
			next.totalAttenuation += Materials::get().getAttributes(obs->mat).shadowing.attenuation;
			next.isWithin = obs;
			return next;

		}

		StateRay3 leave(const Point3 hitPos, const Obstacle3D* obs) const {

			(void) obs;
			StateRay3 next = getNext(hitPos);
			next.isWithin = nullptr;
			return next;

		}

		StateRay3 reflectAt(const Point3 hitPos, const Obstacle3D* obs) const {

			StateRay3 next = getNext(hitPos);

			next.totalAttenuation += Materials::get().getAttributes(obs->mat).reflection.attenuation;
			next.isWithin = nullptr;		// AIR
			return next;

		}

	private:

		StateRay3 getNext(const Point3 hitPos) const {
			StateRay3 next = *this;
			++next.depth;
			next.totalLen += (start.getDistance(hitPos));
			next.start = hitPos;
			return next;
		}

	public:



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

		int getDepth() const {
			//return stack.size();
			return depth;
		}

		float getLength() const {
			return totalLen;
		}

	};

	struct Hit3 {

		const Obstacle3D* obstacle;
		Triangle3 obstacleTria;

		float dist;
		Point3 pos;
		Point3 normal;
		Floorplan::Material material;
		bool stopHere = false;
		bool invalid = false;

		Hit3() {;}
		Hit3(const float dist, const Point3 pos, const Point3 normal) : dist(dist), pos(pos), normal(normal) {;}

	};





	struct Obstacle3DWrapper {

		static std::vector<Point3> getVertices(const Obstacle3D& obs) {
			std::vector<Point3> pts;
			for (const Triangle3& tria : obs.triangles) {
				pts.push_back(tria.p1);
				pts.push_back(tria.p2);
				pts.push_back(tria.p3);
			}
			return pts;
		}

		static std::vector<Point3> getDebugLines(const Obstacle3D& obs) {
			std::vector<Point3> pts;
			for (const Triangle3& tria : obs.triangles) {
				pts.push_back(tria.p1); pts.push_back(tria.p2);
				pts.push_back(tria.p2); pts.push_back(tria.p3);
				pts.push_back(tria.p3); pts.push_back(tria.p1);
			}
			return pts;
		}

	};



	class WiFiRaytrace3D {

	private:

		BBox3 bbox;
		Point3 apPos;

		DataMap3Signal dm;

		BVH3Debug<Obstacle3D, BoundingVolumeSphere3, Obstacle3DWrapper> tree;

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

		std::vector<Point3> hitEnter;
		std::vector<Point3> hitLeave;
		std::vector<Point3> hitStop;

	public:

		/** ctor */
		WiFiRaytrace3D(const Floorplan::IndoorMap* map, const int gs, const Point3 apPos) : apPos(apPos) {

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

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

			Builder fac(map);
			std::vector<Obstacle3D> obstacles = fac.getMesh().elements;

			// build bounding volumes
			for (Obstacle3D& obs : obstacles) {
				tree.add(obs);
			}

			//tree.optimize();
			//tree.show(500, false);

			int xxx = 0; (void) xxx;

		}

		const std::vector<Point3>& getHitEnter() const {
			return hitEnter;
		}
		const std::vector<Point3>& getHitLeave() const {
			return hitLeave;
		}
		const std::vector<Point3>& getHitStop() const {
			return hitStop;
		}

		const DataMap3Signal& estimate() {

			std::minstd_rand gen;
			std::uniform_real_distribution<float> dx(-1.0, +1.0);
			std::uniform_real_distribution<float> dy(-1.0, +1.0);
			std::uniform_real_distribution<float> dz(-1.0, +1.0);

			#pragma omp parallel for
			for (int i = 0; i < Limit::RAYS; ++i) {

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

				// direction
				const Point3 dir = Point3(dx(gen), dy(gen), dz(gen)).normalized();

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

				// run!
				trace(ray);

			}

			return dm;

		}


	//#define USE_DEBUG


	private:

		void trace(const StateRay3& ray) {

			// get the nearest intersection with the floorplan
			const Hit3 nextHit = getNearestHit(ray);

			// stop?
			if (nextHit.invalid) {
	#ifdef USE_DEBUG
				hitStop.push_back(nextHit.pos);
	#endif
				return;
			}

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

			// continue?
			if ((nextHit.stopHere) || (ray.getRSSI(nextHit.dist) < Limit::RSSI) || (ray.getDepth() > Limit::HITS)) {
	#ifdef USE_DEBUG
				hitStop.push_back(nextHit.pos);
	#endif
				return;
			}


			// apply effects
			if (ray.isWithin) {
				leave(ray, nextHit);
	#ifdef USE_DEBUG
				hitLeave.push_back(nextHit.pos);
	#endif
			} else {
				enter(ray, nextHit);
				reflectAt(ray, nextHit);
	#ifdef USE_DEBUG
				hitEnter.push_back(nextHit.pos);
	#endif
			}


		}

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

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


		/** perform reflection and continue tracing */
		void leave(const StateRay3& ray, const Hit3& hit) {

			// continue into the same direction
			StateRay3 next = ray.leave(hit.pos, hit.obstacle);

			// continue
			trace(next);

		}

		/** perform reflection and continue tracing */
		void enter(const StateRay3& ray, const Hit3& hit) {

			// continue into the same direction
			StateRay3 next = ray.enter(hit.pos, hit.obstacle);

			// continue
			trace(next);

		}


		/** perform reflection and continue tracing */
		void reflectAt(const StateRay3& ray, const Hit3& hit) {

			if (hit.normal.length() < 0.9) {
				int i = 0; (void) i;
			}

			Assert::isNear(1.0f, ray.dir.length(), 0.01f, "ray's direction is not normalized");
			Assert::isNear(1.0f, hit.normal.length(), 0.01f, "obstacle's normal is not normalized");

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

			//static std::minstd_rand gen;
			//static std::normal_distribution<float> dist(0, 0.02);
			//const float mod = dist(gen);

			// reflected ray direction using surface normal
			const Point3 dir = ray.dir;
			const Point3 normal = hit.normal;
			const Point3 refDir = dir - (normal * 2 * dot(dir, normal));

			// reflected ray;
			StateRay3 reflected = ray.reflectAt(hit.pos, hit.obstacle);
			reflected.dir = refDir.normalized();

			// continue
			trace(reflected);

		}



		void hitTest(const StateRay3& ray, const Obstacle3D& obs, Hit3& nearest) {

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

			//bool dummy = obs.boundingSphere.intersects(ray);

			Point3 hitPoint;
			for (const Triangle3& tria : obs.triangles) {
				if (tria.intersects(ray, hitPoint)) {

					//if (!dummy) {
					//	throw Exception("issue!");
					//}

					const float dist = hitPoint.getDistance(ray.start);
					if (dist > minDist && dist < nearest.dist) {
						nearest.obstacle = &obs;
						nearest.dist = dist;
						nearest.pos = hitPoint;
						nearest.normal = tria.getNormal();
						nearest.material = obs.mat;
					}
				}
			}

		}


		Hit3 getNearestHit(const StateRay3& ray) {

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

			int hits = 0;
			const float MAX = 999999;
			Hit3 nearest; nearest.dist = MAX;


			// if the ray is currently within something, its only option is to get out of it
			if (ray.isWithin) {

				// check intersection only with the current obstacle to get out
				hitTest(ray, *ray.isWithin, nearest);

			} else {

	//			// check intersection with all walls
	//			for (const Obstacle3D& obs : obstacles) {

	//				// fast opt-out
	//				//if (!obs.boundingSphere.intersects(ray)) {continue;}

	//				hitTest(ray, obs, nearest);

	//			}


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

				tree.getHits(ray, onHit);

			}

			// no hit with floorplan: limit to bounding-box!
			if (nearest.dist == MAX) {
				nearest.invalid = true;
			}

			//std::cout << hits << std::endl;

			return nearest;

		}


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

			const Point3 dir = hit.pos - ray.start;
			const Point3 dirN = dir.normalized();
			const Point3 step = dirN * (dm.getGridSize_cm()/100.0f) * 1.0f;		// 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 Point3 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.getLength() + partLen;

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

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

			}


		}


		/*
		Sphere3 getSphereAround(const std::vector<Triangle3>& trias) {

			std::vector<Point3> pts;
			for (const Triangle3& tria : trias) {
				pts.push_back(tria.p1);
				pts.push_back(tria.p2);
				pts.push_back(tria.p3);
			}
			const Sphere3 sphere = Sphere3::around(pts);

			// sanity assertion
			for (const Point3& pt : pts) {
				Assert::isTrue(sphere.contains(pt), "bounding sphere error");
			}

			return sphere;

		}
		*/

	};

}

#endif // WIFIRAYTRACE3D_H