#include "NavControllerMesh.h"

#include "../ui/debug/SensorDataWidget.h"
#include "../ui/map/3D/MapView3D.h"
#include "../ui/map/2D/MapView2D.h"
#include "../ui/debug/InfoWidget.h"

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

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

#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>

#include <Indoor/navMesh/NavMesh.h>
#include <Indoor/navMesh/NavMeshTriangle.h>
#include <Indoor/floorplan/v2/Floorplan.h>

#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingPercent.h>
//#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKDE.h>

//#ifndef ANDROID
//#include <valgrind/callgrind.h>
//#endif

#include "Settings.h"

Q_DECLARE_METATYPE(const void*)

/** ctor */
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel) :
    NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) {

    // filter init
    std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh));

    // estimation
    //std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MeshBased::MyState>> estimation(new SMC::ParticleFilterEstimationWeightedAverage<MeshBased::MyState>());
    std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));



    // resampling
    //std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>> resample(new SMC::ParticleFilterResamplingSimple<MyState>());
    //std::unique_ptr<SMC::ParticleFilterResamplingKDE<MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingKDE<MyState, NM::NavMeshTriangle>(navMesh, 0.2, Point2(1,1)));
    //std::unique_ptr<SMC::ParticleFilterResamplingKLD<MyState>> resample(new SMC::ParticleFilterResamplingKLD<MyState>());
    //std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>> resample(new SMC::ParticleFilterResamplingPercent<MyState>(0.95));
    std::unique_ptr<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>());

    // eval and transition
    std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel));
    std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new MeshBased::PFTrans(navMesh));

    // setup the filter
    pf = std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>>(new SMC::ParticleFilter<MyState, MyControl, MyObservation>(Settings::numParticles, std::move(init)));
    pf->setTransition(std::move(transition));
    pf->setEvaluation(std::move(eval));
    pf->setEstimation(std::move(estimation));
    pf->setResampling(std::move(resample));

    pf->setNEffThreshold(0.75);         //before 0.75, edit by toni
    //pf->setNEffThreshold(0.65);		// still too low?
    //pf->setNEffThreshold(0.25);		// too low

    // attach as listener to all sensors
    SensorFactory::get().getAccelerometer().addListener(this);
    SensorFactory::get().getGyroscope().addListener(this);
    SensorFactory::get().getBarometer().addListener(this);
    SensorFactory::get().getWiFi().addListener(this);
    SensorFactory::get().getSteps().addListener(this);
    SensorFactory::get().getTurns().addListener(this);
    SensorFactory::get().getActivity().addListener(this);
    SensorFactory::get().getBLE().addListener(this);

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

}


void MeshBased::NavControllerMesh::start() {

    Assert::isFalse(running, "already started!");
    running = true;
    curCtrl.resetAfterTransition();	// ensure we start empty ;)
    tFilter = std::thread(&NavControllerMesh::filterUpdateLoop, this);
    tDisplay = std::thread(&NavControllerMesh::updateMapViewLoop, this);

    // start all sensors
    SensorFactory::get().getAccelerometer().start();
    SensorFactory::get().getGyroscope().start();
    SensorFactory::get().getBarometer().start();
    SensorFactory::get().getWiFi().start();
    SensorFactory::get().getBLE().start();

//#ifndef ANDROID
//		// #include <valgrind/callgrind.h>
//		// run with
//		// valgrind --tool=callgrind --quiet  --instr-atstart=no  ./yasmin
//		// show with
//		// kcachegrind callgrind.out.xxxx
//		CALLGRIND_START_INSTRUMENTATION;
//#endif

}

void MeshBased::NavControllerMesh::stop() {
    Assert::isTrue(running, "not started!");
    running = false;
    tFilter.join();
    tDisplay.join();
}


void MeshBased::NavControllerMesh::onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) {
    (void) sensor;
    (void) data;
    (void) ts;
    gotSensorData(ts);
}

void MeshBased::NavControllerMesh::onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) {
    (void) sensor;
    (void) ts;
    (void) data;
    gotSensorData(ts);
}

void MeshBased::NavControllerMesh::onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) {
    (void) sensor;
    (void) ts;
    (void) data;
    gotSensorData(ts);
}

void MeshBased::NavControllerMesh::onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) {
    (void) sensor;
    (void) ts;
    curObs.wifi = data;
    gotSensorData(ts);
}

void MeshBased::NavControllerMesh::onSensorData(Sensor<BeaconMeasurement>* sensor, const Timestamp ts, const BeaconMeasurement& data) {
    (void) sensor;
    (void) ts;
    curObs.ble.add(data);
    gotSensorData(ts);
}

void MeshBased::NavControllerMesh::onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) {
    (void) sensor;
    (void) ts;
    curObs.gps = data;
    gotSensorData(ts);
}

void MeshBased::NavControllerMesh::onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) {
    (void) sensor;
    (void) ts;
    curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent;		// set to zero after each transition
    gotSensorData(ts);
}

void MeshBased::NavControllerMesh::onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) {
    (void) sensor;
    (void) ts;
    curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent;			// set to zero after each transition
    gotSensorData(ts);
}

void MeshBased::NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) {
    (void) sensor;
    (void) ts;
    curCtrl.activity = data.curActivity;
    curObs.activity = data.curActivity;
    debugActivity(data.curActivity);
    gotSensorData(ts);
}

/** called when any sensor has received new data */
void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
    curObs.currentTime = ts;
    if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();}
}

void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity)  {
    QString act;
    switch(activity.curActivity) {
        case Activity::STANDING:        act = "STAY"; break;
        case Activity::WALKING:         act = "WALK"; break;
        case Activity::WALKING_DOWN:	act = "DOWN"; break;
        case Activity::WALKING_UP:		act = "UP"; break;
        default:                        act = "???"; break;
    }
    Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed");
}

    /** particle-filter update loop */
    void MeshBased::NavControllerMesh::filterUpdateLoop() {


        while(running && !Settings::Filter::useMainThread) {

//			// fixed update rate based on the systems time -> LIVE! even for offline data
//			const Timestamp ts1 = Timestamp::fromUnixTime();
//			doUpdate();
//			const Timestamp ts2 = Timestamp::fromUnixTime();
//			const Timestamp needed = ts2-ts1;
//			const Timestamp sleep = Timestamp::fromMS(500) - needed;
//			std::this_thread::sleep_for(std::chrono::milliseconds(sleep.ms()));

            const bool wasUpdated = filterUpdateIfNeeded();
            if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); }

        }
    }

    Timestamp lastTransition;

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

        static float avgSum = 0;
        static int avgCount = 0;

        const Timestamp diff = curObs.currentTime - lastTransition;
        if (curCtrl.numStepsSinceLastTransition > 0){

            // as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one
            const Timestamp err = diff - Settings::Filter::updateEvery;
            lastTransition = curObs.currentTime;

            const Timestamp ts1 = Timestamp::fromUnixTime();
            filterUpdate();
            const Timestamp ts2 = Timestamp::fromUnixTime();
            const Timestamp tsDiff = ts2-ts1;
            const QString filterTime = QString::number(diff.ms());
            avgSum += tsDiff.ms(); ++avgCount;
            //Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount));
            QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime));

            return true;

        } else if(diff >= Timestamp::fromMS(1000)) {

            filterUpdateEstimationOnly();
            lastTransition = curObs.currentTime;

            const QString evalUpdate = "evalUpdate";
            QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, evalUpdate));

            return true;

        } else {

            return false;

        }

    }

    DijkstraPath<MyGridNode> pathToDest;

    void MeshBased::NavControllerMesh::filterUpdateEstimationOnly() {

        //lastEst = curEst;
        MyState sCurEst = pf->updateEvaluationOnly(curObs);
        curEst.pos_m = sCurEst.pos.pos;
        curEst.head = sCurEst.heading;

        // inform listeners about the new estimation
        for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.pos_m);}

        Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
        Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");

        // update estimated path
        estPath.push_back(curEst.pos_m);
        Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
        Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
    }

    /** perform a filter-update (called from a background-loop) */
    void MeshBased::NavControllerMesh::filterUpdate() {

        //lastEst = curEst;
        MyState sCurEst = pf->update(&curCtrl, curObs);
        curEst.pos_m = sCurEst.pos.pos;
        curEst.head = sCurEst.heading;

        // inform listeners about the new estimation
        for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.pos_m);}

        Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
        Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");

        // update estimated path
        estPath.push_back(curEst.pos_m);
        Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
        Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");

        //TODO: Implement destination for Mesh
//        PFTrans* trans = (PFTrans*)pf->getTransition();
//        const MyGridNode* node = grid->getNodePtrFor(curEst.position);
//        if (node) {
//            try {
//                pathToDest = trans->modDestination.getShortestPath(*node);
//                Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
//                Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
//            } catch (...) {;}
//        }
        //	mainController->getMapView()->showGridImportance();

    }

    /** UI update loop */
    void MeshBased::NavControllerMesh::updateMapViewLoop() {

        while(running) {
            const Timestamp ts1 = Timestamp::fromUnixTime();
            updateMapView();
            const Timestamp ts2 = Timestamp::fromUnixTime();
            const Timestamp tsDiff = ts2-ts1;
            const QString mapViewTime = QString::number(tsDiff.ms());
            //QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime));
            std::this_thread::sleep_for(std::chrono::milliseconds(display_ms));
        }
    }