#include <misc/fixc11.h>

#include <QGuiApplication>
#include <QQmlApplicationEngine>

#include <QApplication>
#include <QMainWindow>
#include <QGridLayout>

#include <Indoor/math/Interpolator.h>
#include <Indoor/grid/factory/v2/Importance.h>

#include <Indoor/nav/dijkstra/Dijkstra.h>
#include <Indoor/nav/dijkstra/DijkstraPath.h>

#include "sensors/SensorFactory.h"
#include "map/MapView.h"

#include <iostream>

#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/api/DummyAPI.h>
#include <Indoor/sensors/radio/WiFiGridNode.h>
#include <Indoor/sensors/radio/WiFiGridEstimator.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDist.h>

class LeListener : public SensorListener<WiFiSensorData>, public SensorListener<AccelerometerData>, public SensorListener<StepData> {

public:
	void onSensorData(const WiFiSensorData& data) override {
		const std::string str = "\n" + data.asString();
		qDebug(str.c_str());
	}
	void onSensorData(const AccelerometerData& data) override {
		const std::string str = data.asString();
		qDebug(str.c_str());
	}
	void onSensorData(const StepData& data) override {
		qDebug("STEP!");
	}

};

struct MyNode : public GridPoint, public GridNode, public WiFiGridNode<10> {

	float imp;
	MyNode() {;}
	MyNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}

	static void staticSerialize(std::ostream& out) {
		WiFiGridNode::staticSerialize(out);
	}

	static void staticDeserialize(std::istream& inp) {
		WiFiGridNode::staticDeserialize(inp);
	}

};

int main(int argc, char *argv[]) {


	int sizeOfNode = sizeof(MyNode);

	std::vector<Point3> points = {
		Point3(1140,530,1060), Point3(1140,2100,1060), Point3(1140,2880,1060), Point3(1140,4442.21,1060), Point3(1190,4840,1060), Point3(1660,4840,890), Point3(1660,5040,890), Point3(1180,5040,720), Point3(1180,4840,720), Point3(1180,4460,720), Point3(2350,4460,720), Point3(4440,4440,720), Point3(5183.26,4280,720), Point3(5800,4280,550), Point3(6110,4280,380), Point3(7680,4280,380), Point3(7680,3860,190), Point3(7680,3400,0), Point3(7400,3400,0), Point3(7400,4030,0)
	};
	std::vector<Point3> path;

	Interpolator<int, Point3> interpol;
	int ts = 0;
	Point3 last = points[0];
	for (Point3 p : points) {
		const float dist = p.getDistance(last);
		const float diffMS = (dist / 100.0f) / 1.5f * 1000;
		ts += diffMS;
		interpol.add(ts, p);
		last = p;
		path.push_back(p / 100.0f);
	}



	//QGuiApplication app(argc, argv);
	QApplication app(argc, argv);

	// OpenGL Setup
	QSurfaceFormat format;
	format.setDepthBufferSize(16);
	QSurfaceFormat::setDefaultFormat(format);


	Floorplan::IndoorMap* im = Floorplan::Reader::readFromFile("/mnt/data/workspaces/IndoorMap/maps/SHL21.xml");

	Grid<MyNode> grid(20);
	GridFactory<MyNode> gf(grid);

	// build
	//gf.build(im);
	//Importance::addImportance(grid, 0);
	//std::ofstream out("/tmp/grid.dat");
	//grid.write(out); out.close();

	// read
	std::ifstream inp("/tmp/grid.dat");
	grid.reset();
	grid.read(inp);

	// estimate WiFi signal strengths
	WiFiModelLogDist mdlLogDist(-40.0f, 2.25f);
	WiFiGridEstimator::estimate(grid, mdlLogDist, im);



	MapView* map = new MapView();
	map->setMinimumHeight(200);
	map->setMinimumWidth(200);
	map->setMap(im);
	//map->setPath(path);
	map->show();


	struct DijkstraMapper {
		Grid<MyNode>& grid;
		DijkstraMapper(Grid<MyNode>& grid) : grid(grid) {;}
		int getNumNeighbors(const MyNode& n) const {return grid.getNumNeighbors(n);}
		const MyNode* getNeighbor(const MyNode& n, const int idx) const {return &grid.getNeighbor(n, idx);}
		float getWeightBetween(const MyNode& n1, const MyNode& n2) const {return n1.getDistanceInCM(n2) / n2.imp;}

	};



	struct LeListener : public APIListener {

		Grid<MyNode>& grid;
		Dijkstra<MyNode>& d;
		const DijkstraNode<MyNode>* dnEnd;
		MapView* map;

		Point3 nextPos;
		Point3 pos1;
		Point3 pos2;

		LeListener(Grid<MyNode>& grid, Dijkstra<MyNode>& d, const DijkstraNode<MyNode>* dnEnd, MapView* map) : grid(grid), d(d), dnEnd(dnEnd), map(map) {


			std::thread t(&LeListener::update, this);
			t.detach();

		}

		/** the currently estimated path to the target has changed */
		virtual void onPathUpdate(const std::vector<Point3>& curPath) override {

		}

		/** the currently estimated position has changed */
		virtual void onPositionUpdate(const Point3 pos) override {

			nextPos = pos;

			// update the path to the target
			const GridPoint xxx(pos.x, pos.y, pos.z);
			const DijkstraNode<MyNode>* dnStart = d.getNode( grid.getNearestNode(xxx) );
			const DijkstraPath<MyNode> dp(dnStart, dnEnd);
			map->setPath(dp);

		}

		void update() {


				std::this_thread::sleep_for(std::chrono::milliseconds(700));

				const int transMS = 500;
				const int updateMS = 75;
				const float factor1 = ((float) updateMS / (float) transMS) * 0.7;
				const float factor2 = factor1 * 0.4f;
				const Point3 human(0, 0, 180);

				while(true) {


					std::this_thread::sleep_for(std::chrono::milliseconds(updateMS));

//					Point3 diff = (nextPos - pos1);
//					if (diff.length() > 30) {diff = diff.normalized() * 30;} else {diff *= factor1;}
//					pos1 += diff;
//					pos2 += diff*0.5;


					pos1 = pos1 * (1-factor1) + nextPos * (factor1);	// fast update
					pos2 = pos2 * (1-factor2) + nextPos * (factor2);	// slow update
					const Point3 dir = pos2 - pos1;

					map->setLookAt((pos1+human) / 100.0f, (dir) / 100.0f);

				}


		}

	};


	DummyAPI api("/mnt/data/workspaces/navindoor");
	api.setTarget(4);


	Dijkstra<MyNode> d;
	//GridPoint end(points.front().x, points.front().y, points.front().z);
	GridPoint end(api.getDst().x, api.getDst().y, api.getDst().z);
	d.build(&grid.getNearestNode(end), DijkstraMapper(grid));
	const DijkstraNode<MyNode>* dnEnd = d.getNode(grid.getNearestNode(end));

	api.addListener(new LeListener(grid, d, dnEnd, map));
	api.setSpeed(2);
	api.startNavigation();

//	auto run = [&] () {
//		//int ts = 0;
//		int ts = 97000;
//		while(ts < interpol.getMaxKey() && ts >= 0) {
//			std::this_thread::sleep_for(std::chrono::milliseconds(50));
//			//ts += 50;
//			ts -= 150;
//			const Point3 human(0, 0, 180);
////			const Point3 pos0 = interpol.get(ts-500);
////			const Point3 pos1 = interpol.get(ts+500);
//			const Point3 pos = interpol.get(ts);
////			//const Point3 dir = Point3(-50000, -50000, 50000);//pos0 - pos;
////			Point3 dir = pos1 - pos0; dir.z /= 2;		// only slight down/up looking
////			map->setLookAt((pos+human) / 100.0f, (dir) / 100.0f);

//			GridPoint cur(pos.x, pos.y, pos.z);
//			const DijkstraNode<MyNode>* dnStart = d.getNode( grid.getNearestNode(cur) );
//			DijkstraPath<MyNode> dp(dnStart, dnEnd);
//			map->setPath(dp);


////			Point3 next =
////					dp.getFromStart(2).element->inCentimeter() +
////					dp.getFromStart(4).element->inCentimeter() +
////					dp.getFromStart(6).element->inCentimeter();
////			next /= 3;
////			const Point3 dir = pos - next ;
//			static Point3 lastPos;
//			lastPos = lastPos * 0.85 + pos * 0.15;

//			const Point3 dir = lastPos - pos ;
//			map->setLookAt((pos+human) / 100.0f, (dir) / 100.0f);

//		}
//	};
//	std::thread t(run);


//	QQmlApplicationEngine engine;
//	engine.load(QUrl(QStringLiteral("qrc:/main.qml")));

	return app.exec();

}