//#include "main.h"

#include "Settings.h"
#include "Plotty.h"

// navMesh
#include "navMesh/mesh.h"
#include "navMesh/filter.h"
#include "navMesh/meshPlotter.h"

#include <memory>
#include <thread>
#include <experimental/filesystem>

#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/imu/PoseDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/model/WiFiModels.h>
#include <Indoor/sensors/radio/setup/WiFiOptimizer.h>
#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
#include <Indoor/sensors/radio/setup/WiFiOptimizerPerFloor.h>


#include <Indoor/math/stats/Statistics.h>

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

#include <sys/stat.h>

Stats::Statistics<float> run(Settings::DataSetup setup, unsigned long numFile, std::string folder) {

	// reading file
	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
	Offline::FileReader fr(setup.training[numFile]);
	WiFiFingerprints fingerprints(setup.fingerprints);
	std::ifstream inp(setup.wifiModel, std::ifstream::binary);

	// ground truth
	std::vector<int> gtPath;
	for(int i = 0; i < setup.numGTPoints; ++i){gtPath.push_back(i);}
	Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
	Stats::Statistics<float> errorStats;

	//calculate distance of path
	std::vector<Interpolator<uint64_t, Point3>::InterpolatorEntry> gtEntries = gtInterpolator.getEntries();
    float distance = 0;
    for(unsigned long i = 1; i < gtEntries.size(); ++i){
		distance += gtEntries[i].value.getDistance(gtEntries[i-1].value);
	}

	std::cout << "Distance of Path: " << distance << std::endl;

	// error file
    const long int t = static_cast<long int>(time(nullptr));
	auto evalDir = std::experimental::filesystem::path(Settings::errorDir);
	evalDir.append(folder);

	if (!std::experimental::filesystem::exists(evalDir)) {
		std::experimental::filesystem::create_directory(evalDir);
	}

	std::ofstream errorFile;
	errorFile.open (evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv");

    std::ofstream activityFile;
    activityFile.open (evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_activity.csv");


	// wifi
//    WiFiModelLogDistCeiling WiFiModel(map);
//	WiFiModelPerFloor WiFiModelPerFloor(map);
//	WiFiModelPerBBox WiFiModelPerBBox(map);

	WiFiModel* wifiModel = nullptr;

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

        if (!inp.good() || (inp.peek()) || inp.eof()) {

			Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!");

			if (Settings::WiFiModel::useRegionalOpt) {

				// use a regional optimization scheme (one per floor)

				WiFiOptimizer::PerFloor opt(map, Settings::WiFiModel::vg_calib, WiFiOptimizer::Mode::QUALITY);
				WiFiOptimizer::LogDistCeiling ldc(map, Settings::WiFiModel::vg_calib, WiFiOptimizer::Mode::QUALITY);

				// add all fingerprints to the optimizer (optimizer will add them to the correct floor/model)
				for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
					opt.addFingerprint(fp);
				}

                wifiModel = dynamic_cast<WiFiModel*>(opt.optimizeAll(ldc.MIN_2_FPS));
				wifiModel->saveXML(setup.wifiModel);

			} else {

				/** NOTE: Funktioniert fürs Museum VIEL VIEL Besser */
				// use one model per AP for the whole map

                wifiModel = dynamic_cast<WiFiModel*>(new WiFiModelLogDistCeiling(map));
				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);
                    (dynamic_cast<WiFiModelLogDistCeiling*>(wifiModel))->addAP(ap.mac, entry);
				}

				wifiModel->saveXML(setup.wifiModel);

			}

		} else {

			// load WiFiModel from file. The factory will create the correct instance
			//WiFiModel->loadXML(setup.wifiModel);
			WiFiModelFactory fac(map);
			wifiModel = fac.loadXML(setup.wifiModel);

		}

	} else {
		// without optimization
        wifiModel = dynamic_cast<WiFiModel*>(new WiFiModelLogDistCeiling(map));
        (dynamic_cast<WiFiModelLogDistCeiling*>(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);

	//add destination
	NM::NavMeshDijkstra::stamp(mesh, gtEntries.back().value);

    const Point3 srcPath0(26, 43, 7.5f);
    const Point3 srcPath1(62, 38, 1.7f);
    const Point3 srcPath2(62, 38, 1.8f);
    const Point3 srcPath3(62, 38, 1.8f);

	// 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
	//MeshPlotter dbg;
	//dbg.addFloors(map);
	//dbg.addOutline(map);
	//dbg.addMesh(mesh);
	//dbg.addDijkstra(mesh);
	//dbg.draw();

	Plotty plot(map);
	plot.buildFloorplan();
	plot.setGroundTruth(gtPath);

	// particle-filter
	const int numParticles = Settings::numParticles;
	//auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath0);	// 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, *wifiModel);

    auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
    //auto resample = std::make_unique<SMC::ParticleFilterResamplingKDE<MyState, MyNavMeshTriangle>>(&mesh, Settings::KDE3D::gridSize, Settings::KDE3D::bandwidth);
	//auto resample = std::make_unique<SMC::ParticleFilterResamplingKDEPercent<MyState, MyNavMeshTriangle>>(&mesh, Settings::KDE3D::gridSize, Settings::KDE3D::bandwidth, 0.75);
	//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
	//auto resample = std::make_unique<SMC::ParticleFilterResamplingKLD<MyState>>();

	//auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, 0.2, Point2(1,1));
	auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
	//auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<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(0.5);

	// sensors
	MyControl ctrl;
	MyObservation obs;

	StepDetection sd;
	PoseDetection pd;
    TurnDetection td(dynamic_cast<PoseProvider*>(&pd));
	RelativePressure relBaro;
	ActivityDetector act;
	relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
	Timestamp lastTimestamp = Timestamp::fromMS(0);
	int gtActivity = 0;

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

        const Timestamp ts = Timestamp::fromMS(static_cast<long>(e.ts));

		if (e.type == Offline::Sensor::WIFI) {
            obs.wifi = fr.getWiFiGroupedByTime()[static_cast<unsigned long>(e.idx)].data;
            ctrl.wifi = fr.getWiFiGroupedByTime()[static_cast<unsigned long>(e.idx)].data;

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

			//simpleActivity walking / standing
            act.add(ts, fr.getAccelerometer()[static_cast<unsigned long>(e.idx)].data);

		} else if (e.type == Offline::Sensor::GYRO) {
            const Offline::TS<GyroscopeData>& _gyr = fr.getGyroscope()[static_cast<unsigned long>(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()[static_cast<unsigned long>(e.idx)].data);
			obs.relativePressure = relBaro.getPressureRealtiveToStart();
			obs.sigmaPressure = relBaro.getSigma();

			//simpleActivity stairs up / down
            act.add(ts, fr.getBarometer()[static_cast<unsigned long>(e.idx)].data);

            activityFile  << ts.ms() << " " << gtActivity << " " << static_cast<int>(act.get()) << "\n";

		} else if (e.type == Offline::Sensor::ACTIVITY) {

			//get the activity recorded by the user by pressing a button
            gtActivity = fr.getActivity()[static_cast<unsigned long>(e.idx)].data;
		}

		if (ctrl.numStepsSinceLastEval > 0) {

			obs.currentTime = ts;
			ctrl.currentTime = ts;

//            if(ctrl.numStepsSinceLastEval > 0){
//                pf.updateTransitionOnly(&ctrl);
//            }
			MyState est = pf.update(&ctrl, obs); //pf.updateEvaluationOnly(obs);
			ctrl.afterEval();
            Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0,0,0.1f);
			lastTimestamp = ts;

			ctrl.lastEstimate = est.pos.pos;

			//plot
			//dbg.showParticles(pf.getParticles());
			//dbg.setCurPos(est.pos.pos);
			//dbg.setGT(gtPos);
			//dbg.addEstimationNode(est.pos.pos);
			//dbg.addGroundTruthNode(gtPos);
			//dbg.setTimeInMinute(static_cast<int>(ts.sec()) / 60, static_cast<int>(static_cast<int>(ts.sec())%60));
			//dbg.draw();

			//get shortest path to destination and plot
			est.pos = mesh.getLocationNearestTo(est.pos.pos);
			std::vector<NM::NavMeshLocation<MyNavMeshTriangle>> path = est.pos.tria->getPathToDestination<MyNavMeshTriangle>(est.pos.pos);
			plot.setPathToDestination(path);

			plot.showParticles(pf.getParticles());
			plot.setCurEst(est.pos.pos);
			plot.setGroundTruth(gtPos);
			plot.addEstimationNode(est.pos.pos);
            plot.setActivity(static_cast<int>(act.get()));

			plot.plot();

			//error calc with penalty for wrong floor
            float errorFactor = 3.0f;
			Point3 gtPosError = Point3(gtPos.x, gtPos.y, errorFactor * gtPos.z);
			Point3 estError = Point3(est.pos.pos.x, est.pos.pos.y, errorFactor * est.pos.pos.z);
			float err_m = gtPosError.getDistance(estError);
			errorStats.add(err_m);
			errorFile << ts.ms() << " " << err_m << "\n";


			//stuff for drawing
//            plot.gp << "set terminal png size 1280,720\n";
//            plot.gp.setOutput("/tmp/videoSparkasse/" + std::to_string(i++) + ".png");

//            int degree = ((30 - i) % 360);
//            if (degree < 0){degree += 360;}
//            plot.gp << "set view 63,"<< degree << "\n";
//            plot.gp << "set autoscale xy\n";
//            plot.gp << "set autoscale z\n";
//            plot.plot();
		}
	}



	// get someting on console
	std::cout << "Statistical Analysis Filtering: " << std::endl;
	std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << " Std: " << errorStats.getStdDev() << std::endl;

	// save the statistical data in file
	errorFile << "========================================================== \n";
	errorFile << "Average of all statistical data: \n";
	errorFile << "Median: " << errorStats.getMedian() << "\n";
	errorFile << "Average: " << errorStats.getAvg() << "\n";
	errorFile << "Standard Deviation: " << errorStats.getStdDev() << "\n";
	errorFile << "75 Quantil: " << errorStats.getQuantile(0.75) << "\n";
	errorFile.close();

	//activityFile << "===================================================== \n";
	//activityFile << "Falsche Zustandswechsel: " << numFalseActivityChange;
	activityFile.close();

	/* plot in gp file */
	std::ofstream plotFile;
	plotFile.open(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".gp");
	plot.saveToFile(plotFile);
	plotFile.close();

	//save also a png image, just for a better overview
	plot.printOverview(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t));
	plot.plot();

	plot.closeStream();

	return errorStats;
}

int main() {

	Stats::Statistics<float> statsAVG;
	Stats::Statistics<float> statsMedian;
	Stats::Statistics<float> statsSTD;
	Stats::Statistics<float> statsQuantil;
	Stats::Statistics<float> tmp;

    std::string evaluationName = "Test_ParticleFilter_Standard";

	for(int i = 0; i < 1; ++i){


        for(unsigned long j = 0; j < Settings::data.Path0.training.size(); ++j){
			tmp = run(Settings::data.Path0, j, evaluationName);
			statsMedian.add(tmp.getMedian());
			statsAVG.add(tmp.getAvg());
			statsSTD.add(tmp.getStdDev());
			statsQuantil.add(tmp.getQuantile(0.75));
		}

//        for(int j = 0; j < Settings::data.Path1.training.size(); ++j){
//            tmp = run(Settings::data.Path1, j, evaluationName);
//            statsMedian.add(tmp.getMedian());
//            statsAVG.add(tmp.getAvg());
//            statsSTD.add(tmp.getStdDev());
//            statsQuantil.add(tmp.getQuantile(0.75));
//        }

//        for(int j = 0; j < Settings::data.Path2.training.size(); ++j){
//            tmp = run(Settings::data.Path2, j, evaluationName);
//            statsMedian.add(tmp.getMedian());
//            statsAVG.add(tmp.getAvg());
//            statsSTD.add(tmp.getStdDev());
//            statsQuantil.add(tmp.getQuantile(0.75));
//        }

//		for(int j = 0; j < Settings::data.Path3.training.size(); ++j){
//			tmp = run(Settings::data.Path3, j, evaluationName);
//			statsMedian.add(tmp.getMedian());
//			statsAVG.add(tmp.getAvg());
//			statsSTD.add(tmp.getStdDev());
//			statsQuantil.add(tmp.getQuantile(0.75));
//		}

//        for(int j = 0; j < 1; ++j){
//            tmp = run(Settings::data.Stairs, j, evaluationName);
//            statsMedian.add(tmp.getMedian());
//            statsAVG.add(tmp.getAvg());
//            statsSTD.add(tmp.getStdDev());
//            statsQuantil.add(tmp.getQuantile(0.75));
//        }

        std::cout << "Iteration " << i << " completed" << std::endl;
	}

	std::cout << "==========================================================" << std::endl;
	std::cout << "Average of all statistical data: " << std::endl;
	std::cout << "Median: " << statsMedian.getAvg() << std::endl;
	std::cout << "Average: " << statsAVG.getAvg() << std::endl;
	std::cout << "Standard Deviation: " << statsSTD.getAvg() << std::endl;
	std::cout << "75 Quantil: " << statsQuantil.getAvg() << std::endl;
	std::cout << "==========================================================" << std::endl;

	//EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS
	std::ofstream finalStatisticFile;
	finalStatisticFile.open (Settings::errorDir + evaluationName + ".csv", std::ios_base::app);

	finalStatisticFile << "========================================================== \n";
	finalStatisticFile << "Average of all statistical data: \n";
	finalStatisticFile << "Median: " << statsMedian.getAvg() << "\n";
	finalStatisticFile << "Average: " << statsAVG.getAvg() << "\n";
	finalStatisticFile << "Standard Deviation: " << statsSTD.getAvg() << "\n";
	finalStatisticFile << "75 Quantil: " << statsQuantil.getAvg() << "\n";
	finalStatisticFile << "========================================================== \n";

	finalStatisticFile.close();

	//std::this_thread::sleep_for(std::chrono::seconds(60));

}