#ifndef GRID_ELEVATORS_H
#define GRID_ELEVATORS_H

#include <vector>
#include <set>

#include "../../Grid.h"
#include "Helper.h"
#include "../../../floorplan/v2/Floorplan.h"

#include <fstream>



template <typename T> class Elevators {

private:

	/** the grid to build into */
	Grid<T>& grid;

	/** calculation helper */
	Helper<T> helper;


public:

	/** ctor */
	Elevators(Grid<T>& grid) : grid(grid), helper(grid) {
		;
	}

	~Elevators() {
		;
	}


	/** build the given elevator */
	void build(const Floorplan::Floor* floor, const Floorplan::Elevator* elevator) {

		const int gs_cm = grid.getGridSize_cm();


		struct IntPos {
			int x_cm;
			int y_cm;
			IntPos(int x_cm, int y_cm) : x_cm(x_cm), y_cm(y_cm) {;}
		};

		// identify all grid-aligned nodes that belong to the elevator
		std::vector<IntPos> nodesWithin;
		const HelperPoly poly(elevator->getPoints());

		auto callback = [&] (const int x_cm, const int y_cm) {

			const GridPoint gp1(x_cm, y_cm, floor->getStartingZ()*100);		// starting floor
			const GridPoint gp2(x_cm, y_cm, floor->getEndingZ()*100);		// the floor above

			// ensure such a node is present in both floors (and thus a connection is possible)
			if (grid.hasNodeFor(gp1) && grid.hasNodeFor(gp2)) {
				nodesWithin.push_back(IntPos(x_cm, y_cm));
			}

		};
		poly.forEachGridPoint(gs_cm, callback);

		// now create the interconnection in z-direction
		const int z1_cm = std::ceil((floor->getStartingZ()*100+1) / gs_cm) * gs_cm;	// the next node above the current flor
		const int z2_cm = std::floor((floor->getEndingZ()*100-1) / gs_cm) * gs_cm;	// the last node below the next floor

		for (const IntPos nodePos : nodesWithin) {

			// create nodes BETWEEN the two floors (skip the floors themselves! -> floor1+gridSize <-> floor2-gridSize
			for (int z_cm = z1_cm; z_cm <= z2_cm; z_cm += gs_cm) {
				const GridPoint gp1(nodePos.x_cm, nodePos.y_cm, z_cm);			// the to-be-added node
				Assert::isFalse(grid.hasNodeFor(gp1), "elevator collission");	// such a node must not yet exist! otherwise we e.g. collide with a stari
				const int idx = grid.add(T(gp1.x_cm, gp1.y_cm, gp1.z_cm));		// create the node
				grid[idx].setType(GridNode::TYPE_ELEVATOR);						// set the node-type
			}

			// connect each of the new nodes with the node below it. NOW ALSO EXAMINE THE floor above (z2_cm + gs_cm)
			for (int z_cm = z1_cm; z_cm <= z2_cm + gs_cm; z_cm += gs_cm) {
				const GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, z_cm-gs_cm);
				const GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm);
				Assert::isTrue(grid.hasNodeFor(gpBelow), "missing node");
				Assert::isTrue(grid.hasNodeFor(gp), "missing node");
				T& n1 = (T&) grid.getNodeFor(gpBelow);
				T& n2 = (T&) grid.getNodeFor(gp);
				grid.connectBiDir(n1, n2);
			}

		}


	}

};

#endif // GRID_ELEVATORS_H