#ifndef NAV_MESH_MAIN_H
#define NAV_MESH_MAIN_H

#include "mesh.h"
#include "filter.h"
#include "../Settings.h"

#include <memory>
#include <thread>

#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/offline/FileReader.h>
#include <Indoor/geo/Heading.h>
#include <Indoor/geo/Point2.h>
#include <Indoor/sensors/imu/TurnDetection.h>
#include <Indoor/sensors/imu/StepDetection.h>
#include <Indoor/sensors/imu/MotionDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>


void navMeshMain() {

    //std::string mapFile = "/apps/paper/diss/data/maps/museum31.xml";
    std::string mapFile = "../map/map42_ap.xml";

    // reading file
	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(mapFile);
    Offline::FileReader fr("../measurements/museum/Pixel/Path1_2468.csv");
    WiFiFingerprints fingerprints("../measurements/museum/Nexus/fingerprints/wifi_fp.dat");
    const std::string wifiModelFile = "../measurements/museum/wifimodel.dat";
    std::ifstream inp(wifiModelFile, std::ifstream::binary);

    // wifi
    WiFiModelLogDistCeiling WiFiModel(map);

    // with optimization
    if(Settings::WiFiModel::optimize){

        if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
            Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!");
            WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
            for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
                opt.addFingerprint(fp);
            }
            const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE);
            for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
                const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
                WiFiModel.addAP(ap.mac, entry);
            }

            WiFiModel.saveXML(wifiModelFile);
        } else {
            WiFiModel.loadXML(wifiModelFile);
        }

    } else {
        // without optimization
        WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
        Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml");
    }



    // mesh
	NM::NavMeshSettings set;
	MyNavMesh mesh;
	MyNavMeshFactory fac(&mesh, set);
	fac.build(map);

    const Point3 srcPath0(26, 43, 7.5);
    const Point3 srcPath1(62, 38, 1.8);

	// add shortest-path to destination
	//const Point3 dst(51, 45, 1.7);
    //const Point3 dst(25, 45, 0);
    //NM::NavMeshDijkstra::stamp<MyNavMeshTriangle>(mesh, dst);

	// debug show
	NM::NavMeshDebug dbg;
	dbg.addMesh(mesh);
	//dbg.addDijkstra(mesh);
	dbg.draw();

	// particle-filter
    const int numParticles = 1000;
    //auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1);	// known position
    auto init = std::make_unique<MyPFInitUniform>(&mesh);		// uniform distribution
    auto eval = std::make_unique<MyPFEval>(WiFiModel);
	auto trans = std::make_unique<MyPFTrans>(mesh);
    auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
    auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();

	// setup
	MyFilter pf(numParticles, std::move(init));
	pf.setEvaluation(std::move(eval));
	pf.setTransition(std::move(trans));
	pf.setResampling(std::move(resample));
	pf.setEstimation(std::move(estimate));
    pf.setNEffThreshold(1);

    // sensors
	MyControl ctrl;
	MyObservation obs;

    StepDetection sd;
    PoseDetection pd;
    TurnDetection td(&pd);
    RelativePressure relBaro;
    relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
    Timestamp lastTimestamp = Timestamp::fromMS(0);

    // parse each sensor-value within the offline data
    for (const Offline::Entry& e : fr.getEntries()) {

        const Timestamp ts = Timestamp::fromMS(e.ts);

        if (e.type == Offline::Sensor::WIFI) {
            obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data;

        } else if (e.type == Offline::Sensor::ACC) {
            if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) {
                ++ctrl.numStepsSinceLastEval;
            }
            const Offline::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
            pd.addAccelerometer(ts, _acc.data);

        } else if (e.type == Offline::Sensor::GYRO) {
            const Offline::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
            const float delta_gyro = td.addGyroscope(ts, _gyr.data);

            ctrl.headingChangeSinceLastEval += delta_gyro;

        } else if (e.type == Offline::Sensor::BARO) {
            relBaro.add(ts, fr.getBarometer()[e.idx].data);
            obs.relativePressure = relBaro.getPressureRealtiveToStart();
            obs.sigmaPressure = relBaro.getSigma();
        }

        if (ts.ms() - lastTimestamp.ms() > 500 && ctrl.numStepsSinceLastEval > 0) {

            obs.currentTime = ts;
//            if(ctrl.numStepsSinceLastEval > 0){
//                pf.updateTransitionOnly(&ctrl);
//                ctrl.afterEval();
//            }
//            MyState est = pf.updateEvaluationOnly(obs);
//            lastTimestamp = ts;



            MyState est = pf.update(&ctrl, obs);
            ctrl.afterEval();
            lastTimestamp = ts;

    //		try {
    //			MyNavMeshLocation loc = mesh.getLocationNearestTo(est.pos.pos);
    //			auto path = loc.tria->getPathToDestination<MyNavMeshTriangle>(loc.pos);
    //			dbg.addDijkstra(path);
    //		} catch (...) {;}

    //		const int d = (i * 1) % 360;
    //		dbg.plot.getView().setCamera(60, d);
            dbg.showParticles(pf.getParticles());
            dbg.setCurPos(est.pos.pos);

            //dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png");
            //dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30));

    //		std::cout << i << std::endl;

            dbg.draw();

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

        }

    }
}

#endif