/*
 * © Copyright 2014 – Urheberrechtshinweis
 * Alle Rechte vorbehalten / All Rights Reserved
 *
 * Programmcode ist urheberrechtlich geschuetzt.
 * Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
 * Einige Aenderungen beigetragen von Toni Fetzer
 * Keine Verwendung ohne explizite Genehmigung.
 * (vgl. § 106 ff UrhG / § 97 UrhG)
 */

#ifndef FLOORPLANCEILINGS_H
#define FLOORPLANCEILINGS_H

#include "Floorplan.h"

namespace Floorplan {

	/**
	 * helper-class for floorplan ceilings
	 * e.g. to determine the number of ceilings between two given positions
	 */
	class Ceilings {

	private:

		/** position (height) of all ceilings (in meter) */
		std::vector<float> ceilingsAtHeight_m;

	public:

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

		/** ctor with the map to work with */
		Ceilings(const IndoorMap* map) {

			// sanity checks
			Assert::isTrue(map->floors.size() >= 1, "map has no floors?!");

			// get position of all ceilings
			for (Floorplan::Floor* f : map->floors) {

				const float h1 = f->atHeight;
				const float h2 = f->atHeight + f->height;

				if (std::find(ceilingsAtHeight_m.begin(), ceilingsAtHeight_m.end(), h1) == ceilingsAtHeight_m.end()) {
					ceilingsAtHeight_m.push_back(h1);
				}
				if (std::find(ceilingsAtHeight_m.begin(), ceilingsAtHeight_m.end(), h2) == ceilingsAtHeight_m.end()) {
					ceilingsAtHeight_m.push_back(h2);
				}

			}

		}

		std::vector<float> getCeilings() const {
			return ceilingsAtHeight_m;
		}

		void addCeiling(const float height_m) {
			ceilingsAtHeight_m.push_back(height_m);
		}

		void clear() {
			ceilingsAtHeight_m.clear();
		}

		/** get a fading number of floors between ap and person using sigmod in the area where the ceiling is */
		float numCeilingsBetweenFloat(const Point3 ap, const Point3 person) const {

			// sanity checks
			Assert::isNot0(ceilingsAtHeight_m.size(), "no ceilings available for testing! incorrect map?");

			// fading between floors using sigmoid
			const float near = 1.0;
			float cnt = 0;


			for (const float z : ceilingsAtHeight_m) {

				const float myDistToCeil = (ap.z < person.z) ? (person.z - z) : (z - person.z);
				if ( std::abs(myDistToCeil) < near ) {
					cnt += sigmoid(myDistToCeil * 6);
				} else if (ap.z < z && person.z >= z+near) {	// AP below celing, me above ceiling
					cnt += 1;
				} else if (ap.z > z && person.z <= z-near) {	// AP above ceiling, me below ceiling
					cnt += 1;
				}

			}

			return cnt;

		}

		float numCeilingsBetweenLinearInt(const Point3 ap, const Point3 person) const {

			// sanity checks
			Assert::isNot0(ceilingsAtHeight_m.size(), "no ceilings available for testing! incorrect map?");

			int cnt = 0;
			float sum = 0;
			for (float z = -1.0; z <= +1.0; z+= 0.25) {
				sum += numCeilingsBetween(ap, person + Point3(0,0,z));
				++cnt;
			}

			return sum/cnt;

		}

		/** get the number of ceilings between z1 and z2 */
		int numCeilingsBetween(const Point3 pos1, const Point3 pos2) const {

			// sanity checks
			Assert::isNot0(ceilingsAtHeight_m.size(), "no ceilings available for testing! incorrect map?");

			// find min and max height given the to-be-compared points
			const float zMin = std::min(pos1.z, pos2.z);
			const float zMax = std::max(pos1.z, pos2.z);

	#ifdef WITH_ASSERTIONS

            static uint64_t numNear = 0;
            static uint64_t numFar = 0;

			for (const float z : ceilingsAtHeight_m) {
				const float diff = std::min( std::abs(z-zMin), std::abs(z-zMax) );
				if (diff < 0.1) {++numNear;} else {++numFar;}
			}
			if ((numNear + numFar) > 150000) {
				Assert::isTrue(numNear < numFar*0.5,
				    "many requests to Floorplan::Ceilings::numCeilingsBetween address nodes (very) near to a ground! \
					due to rounding issues, determining the number of floors between AP and point-in-question is NOT possible! \
					expect very wrong outputs! \
					consider adding the person's height to the questioned positions: p += Point3(0,0,1.3) "
				);
			}
	#endif

			int cnt = 0;
			for (const float z : ceilingsAtHeight_m) {
				if (zMin < z && zMax > z) {++cnt;}
			}

			return cnt;

		}

	private:

		static inline float sigmoid(const float val) {
			return 1.0f / (1.0f + std::exp(-val));
		}

	};

}

#endif // FLOORPLANCEILINGS_H