#ifndef NAVCONTROLLERMESH_H
#define NAVCONTROLLERMESH_H

#include "../sensors/AccelerometerSensor.h"
#include "../sensors/GyroscopeSensor.h"
#include "../sensors/BarometerSensor.h"
#include "../sensors/WiFiSensor.h"
#include "../sensors/SensorFactory.h"
#include "../sensors/StepSensor.h"
#include "../sensors/TurnSensor.h"
#include "../sensors/ActivitySensor.h"

#include <Indoor/navMesh/NavMeshLocation.h>
#include <Indoor/navMesh/NavMesh.h>

#include <Indoor/Assertions.h>
#include <thread>

//#include "State.h"
#include "FilterMesh.h"

#include "../Controller.h"
#include "../NavController.h"

namespace MeshBased {

	class NavControllerMesh : public NavController {

	private:

		NM::NavMesh<NM::NavMeshTriangle>* navMesh;
        WiFiModel* wifiModel;

        std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;

		MyObservation curObs;
		MyControl curCtrl;

	public:

        NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel);


		void start() override;

		void stop() override;


		void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override;

		void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override;

		void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override;

		void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override;

		void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override;

		void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override;

		void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override;

        void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ;


	private:

		/** called when any sensor has received new data */
		void gotSensorData(const Timestamp ts);

	//	void debugActivity(const ActivityData& activity);

		/** particle-filter update loop */
		void filterUpdateLoop();

		/** check whether its time for a filter update, and if so, execute the update and return true */
		bool filterUpdateIfNeeded();

		/** perform a filter-update (called from a background-loop) */
		void filterUpdate();

		/** UI update loop */
		void updateMapViewLoop();

	};

}

#endif // NAVCONTROLLERMESH_H