#ifndef NAVCONTROLLERGRID_H
#define NAVCONTROLLERGRID_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 <Indoor/Assertions.h>
#include <thread>

#include "State.h"
#include "Filter.h"
#include "../Controller.h"

#include "../NavController.h"


namespace GridBased {

    class NavControllerGrid : public NavController {

    private:

        Grid<MyGridNode>* grid;
        WiFiModelLogDistCeiling wifiModel;

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

        DijkstraPath<MyGridNode> pathToDest;

        MyObservation curObs;
        MyControl curCtrl;


    public:

        NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid<MyGridNode>* grid);


        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<BeaconMeasurement>* sensor, const Timestamp ts, const BeaconMeasurement& 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 // NAVCONTROLLERGRID_H