#include "NavController.h"

#include "Controller.h"
#include "../ui/map/3D/MapView3D.h"
#include "../ui/map/2D/MapView2D.h"

Q_DECLARE_METATYPE(const void*)

NavController::NavController(Controller* mainController, Floorplan::IndoorMap* im) : mainController(mainController), im(im) {

	// hacky.. but we need to call this one from the main thread!
	//mainController->getMapView()->showParticles(pf->getParticles());
	qRegisterMetaType<const void*>();

}

void NavController::updateMapView() {

	const float kappa1 = display_ms / 1000.0f;
	const float kappa2 = kappa1 * 0.7;

	const float myHeight_m = 1.80;

	curPosFast = curPosFast * (1-kappa1) + curEst.pos_m * (kappa1);
	curPosSlow = curPosSlow * (1-kappa2) + curEst.pos_m * (kappa2);

	const Point3 dir = (curPosFast - curPosSlow).normalized();
	const Point3 dir2 = Point3(dir.x, dir.y, -0.2).normalized();

	// how to update the camera
	if (cameraMode == 0) {
		mainController->getMapView3D()->setLookAt(curPosFast + Point3(0,0,myHeight_m), dir);
	} else if (cameraMode == 1) {
		mainController->getMapView3D()->setLookAt(curPosFast + Point3(0,0,myHeight_m) - dir2*4, dir2);
	} else if (cameraMode == 2) {
		const Point3 spectator = curPosFast + Point3(0,0,25) - dir*15;
		const Point3 spectatorDir = (curPosFast - spectator).normalized();
		mainController->getMapView3D()->setLookEye(spectator);
		mainController->getMapView3D()->setLookDir(spectatorDir);
	}

	mainController->getMapView3D()->setClipAbove(curEst.pos_m.z + 2);
	mainController->getMapView3D()->setCurrentEstimation(curEst.pos_m, dir);
	mainController->getMapView2D()->setCurrentEstimation(curEst.pos_m, dir);

}