#ifdef WITH_TESTS

#include "../../Tests.h"
#include "../../../sensors/offline/OfflineAndroid.h"
#include "../../../sensors/radio/WiFiProbabilityFree.h"
#include "../../../sensors/radio/model/WiFiModelLogDist.h"
#include "../../../sensors/radio/model/WiFiModelLogDistCeiling.h"
#include "../../../sensors/radio/VAPGrouper.h"

#include "../../../geo/Point3.h"
#include "../../../math/Interpolator.h"

#include "../../../floorplan/v2/FloorplanReader.h"

#include <KLib/math/statistics/Statistics.h>
#include <KLib/math/optimization/NumOptAlgoDownhillSimplex.h>

std::vector<std::string> getTrainingFiles() {

	// all training files
	return {

//		"train/walks/path4_galaxy_forward.dat",
//		"train/walks/path4_galaxy_backward.dat",

//		"train/walks/path3_galaxy_forward.dat",
//		"train/walks/path3_galaxy_backward.dat",

//		"train/walks/path2_galaxy_forward.dat",
//		"train/walks/path2_galaxy_backward.dat",

//		"train/walks/path1_galaxy_forward.dat",
//		"train/walks/path1_galaxy_backward.dat",

		"train/walks/path4_nexus_forward.dat",
		"train/walks/path4_nexus_backward.dat",

		"train/walks/path3_nexus_forward.dat",
		"train/walks/path3_nexus_backward.dat",

		"train/walks/path2_nexus_forward.dat",
		"train/walks/path2_nexus_backward.dat",

		"train/walks/path1_nexus_forward.dat",
		"train/walks/path1_nexus_backward.dat",

	};

}

struct APParams {
	float txp;
	float exp;
	float waf;
	float offset;
};

/*
WiFiObservation groupVAP(const WiFiObservation& inp) {
	struct Debug {
		std::string mac;
		float rssi;
		Debug(const std::string& mac, const float rssi) : mac(mac), rssi(rssi) {;}
	};

	struct Params{
		float rssiSum = 0;
		int cnt = 0;
		std::vector<Debug> single;
	};
	std::unordered_map<MACAddress, Params> map;
	WiFiObservation out;
	for (const WiFiObservationEntry& e : inp.entries) {
		std::string mac = e.mac.asString();
		mac[mac.size()-1] = '9';
		map[mac].rssiSum += e.rssi;
		map[mac].cnt += 1;
		map[mac].single.push_back(Debug(e.mac.asString(), e.rssi));
	}
	for (auto it : map) {
		const Params& params = it.second;
		WiFiObservationEntry entry(it.first, params.rssiSum / params.cnt);
		out.entries.push_back(entry);
	}
	return out;
}
*/

struct WiFiEvalTestBase {

	// all training files
	std::vector<std::string> files = getTrainingFiles();

	// parser for each file
	std::vector<OfflineAndroid> parsers;


	// load the floorplan
	Floorplan::IndoorMap* im = Floorplan::Reader::readFromFile(getDataFile("SHL.xml"));

	// all access points within the floorplan
	std::vector<LocatedAccessPoint> aps;
	std::unordered_map<MACAddress, LocatedAccessPoint> apsMap;

	WiFiEvalTestBase() {

		for (Floorplan::Floor* f : im->floors) {
			for (Floorplan::AccessPoint* ap : f->accesspoints) {

//				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6C:A4:A9") {continue;}
				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:77:EB:99") {continue;}
				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6B:82:79") {continue;}
//				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6C:6E:F9") {continue;}

//				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6B:46:09") {continue;}
				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6B:64:99") {continue;}
				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6C:3A:A9") {continue;}
				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6B:DB:69") {continue;}




				aps.push_back(LocatedAccessPoint(*ap));
				apsMap.insert(std::pair<MACAddress, LocatedAccessPoint>(ap->mac, LocatedAccessPoint(*ap)));
			}
		}

		for (const std::string& file : files) {
			parsers.push_back(OfflineAndroid(getDataFile(file)));
		}

	}

	double getError(APParams params) {

		if (params.waf > 0) {return 99999;}

//		params.txp = -40;
//		params.offset = -6;

		double dB_err = 0;
		int cnt = 0;

		// model
		WiFiModelLogDistCeiling model(params.txp, params.exp, params.waf, im);

		VAPGrouper vg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);

		// evaluate each training file
		for (const OfflineAndroid& parser : parsers) {

			// ground-truth interpolator
			Interpolator<Timestamp, Point3> interpol = parser.getWalkedPathInterpolatorCM();

			// evaluate each WiFi reading
			for (const OfflineEntry<WiFiMeasurements>& _obs : parser.getWiFi()) {

				WiFiMeasurements obs = vg.group(_obs.data);

				// ground-truth at the time of measurement
				const Point3 pos = interpol.get(_obs.ts) / 100;

				// dB diff
				for (const WiFiMeasurement& wifiObs : obs.entries) {

					// only use configured APs
					auto it = apsMap.find(wifiObs.getAP().getMAC());
					if (it == apsMap.end()) {continue;}

					// get model RSSI
					const float mdlRSSI = model.getRSSI(it->second, pos) + params.offset;

					// difference to scan RSSI
					const float diff = std::abs(mdlRSSI - wifiObs.getRSSI());

					//stats[wifiObs.mac].add(diff);

					dB_err += diff;
					++cnt;

				}

			}

		}

		return dB_err / cnt;

	}


	std::unordered_map<MACAddress, K::Statistics<float>> getStats(APParams params) {

		std::unordered_map<MACAddress, K::Statistics<float>> stats;

		// model
		WiFiModelLogDistCeiling model(params.txp, params.exp, params.waf, im);

		// evaluate each training file
		for (const OfflineAndroid& parser : parsers) {

			// ground-truth interpolator
			Interpolator<Timestamp, Point3> interpol = parser.getWalkedPathInterpolatorCM();

			// evaluate each WiFi reading
			for (const OfflineEntry<WiFiMeasurements>& obs : parser.getWiFi()) {

				// ground-truth at the time of measurement
				const Point3 pos = interpol.get(obs.ts) / 100;

				// dB diff
				for (const WiFiMeasurement& wifiObs : obs.data.entries) {

					// only use configured APs
					auto it = apsMap.find(wifiObs.getAP().getMAC());
					if (it == apsMap.end()) {continue;}

					// get model RSSI
					const float mdlRSSI = model.getRSSI(it->second, pos) + params.offset;

					// difference to scan RSSI
					const float diff = std::abs(mdlRSSI - wifiObs.getRSSI());

					stats[wifiObs.getAP().getMAC()].add(diff);

				}

			}

		}

		return stats;

	}

};



TEST(WiFiEval, VAP) {

	WiFiEvalTestBase base;

	float errSum = 0;
	int cnt = 0;

	VAPGrouper vg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);

	// evaluate each training file
	for (const OfflineAndroid& parser : base.parsers) {

		// evaluate each WiFi reading
		for (const OfflineEntry<WiFiMeasurements>& obs : parser.getWiFi()) {

			WiFiMeasurements obsUngrouped = obs.data;
			WiFiMeasurements obsGrouped = vg.group(obsUngrouped);

			struct Comp {
				float rssiUngrouped = 0;
				float rssiGrouped = 0;
			};

			std::unordered_map<MACAddress, Comp> map;

			for (const WiFiMeasurement& entryUngrouped : obsUngrouped.entries) {
				map[entryUngrouped.getAP().getMAC()].rssiUngrouped = entryUngrouped.getRSSI();
			}

			for (const WiFiMeasurement& entryGrouped : obsGrouped.entries) {
				map[entryGrouped.getAP().getMAC()].rssiGrouped = entryGrouped.getRSSI();
			}

			for (auto it : map) {
				Comp c = it.second;
				if (c.rssiUngrouped != 0 && c.rssiGrouped != 0) {
					const float diff = std::abs(c.rssiGrouped - c.rssiUngrouped);
					errSum += diff;
					++cnt;
				}
			}

		}

	}

	std::cout << "average diff is: " << (errSum/cnt) << std::endl;

}

TEST(WiFiEval, optimize) {


	WiFiEvalTestBase base;

	APParams params;
	params.exp = 2;
	params.txp = -40;
	params.waf = -4;
	params.offset = -4;

	auto func = [&] (const float* data) {
		APParams* params = (APParams*) data;
		return base.getError(*params);
	};

	K::NumOptAlgoDownhillSimplex<float, 4> opt;
	opt.setMaxIterations(100);
	opt.setNumRestarts(3);
	opt.calculateOptimum(func, (float*) &params);

	std::cout << params.exp << ":" << params.txp << ":" << params.waf << ":" << params.offset << std::endl;
	std::cout << base.getError(params) << std::endl;

	for (auto it : base.getStats(params)) {
		std::cout << it.first.asString() << ": " << it.second.asString() << std::endl;
	}

}


TEST(aaiFiEval, getBestTXP_EXP) {

	// load the floorplan
	Floorplan::IndoorMap* im = Floorplan::Reader::readFromFile(getDataFile("SHL.xml"));

	// all access points within the floorplan
	std::vector<LocatedAccessPoint> aps;
	std::unordered_map<MACAddress, LocatedAccessPoint> apsMap;
	for (Floorplan::Floor* f : im->floors) {
		for (Floorplan::AccessPoint* ap : f->accesspoints) {

//			if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6C:A4:A9") {continue;}
//			if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6B:64:99") {continue;}
//			if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6B:82:79") {continue;}

			aps.push_back(LocatedAccessPoint(*ap));
			apsMap.insert(std::pair<MACAddress, LocatedAccessPoint>(ap->mac, LocatedAccessPoint(*ap)));

		}
	}

	std::vector<std::string> files = getTrainingFiles();


	// parser each file
	std::vector<OfflineAndroid> parsers;
	for (const std::string& file : files) {
		parsers.push_back(OfflineAndroid(getDataFile(file)));
	}

	std::unordered_map<MACAddress, K::Statistics<float>> stats;

	const float sigma = 8;
	const float waf = -4;
	const float txp = -40;

	// eval using different parameters
	for (float exp = 1.5; exp < 3.5; exp += 0.05) {
	//){ float exp = 2.5;

		double err = 0;
		double sum = 0;
		int cnt = 0;
		int apCnt = 0;

		// model
		//WiFiModelLogDist model(-40, exp);
		WiFiModelLogDistCeiling model(txp, exp, waf, im);
		WiFiObserverFree observer(sigma, model, aps);

		// evaluate each training file
		for (const OfflineAndroid& parser : parsers) {

			// ground-truth interpolator
			Interpolator<Timestamp, Point3> interpol = parser.getWalkedPathInterpolatorCM();

			// evaluate each WiFi reading
			for (const OfflineEntry<WiFiMeasurements>& obs : parser.getWiFi()) {

				// ground-truth at the time of measurement
				const Point3 pos = interpol.get(obs.ts) / 100;

				// probability
				sum += std::log(observer.getProbability(pos, obs.ts, obs.data, 0));
				++cnt;

				// dB diff
				for (const WiFiMeasurement& wifiObs : obs.data.entries) {

					auto it = apsMap.find(wifiObs.getAP().getMAC());
					if (it == apsMap.end()) {continue;}

					const float mdlRSSI = model.getRSSI(it->second, pos);
					const float diff = std::abs(mdlRSSI - wifiObs.getRSSI());

					//stats[wifiObs.mac].add(diff);

					err += diff;
					++apCnt;

				}

			}

		}

		std::cout << exp << ":" << (sum/cnt) << std::endl;
		std::cout << exp << ":" << (err/apCnt) << std::endl;
		std::cout << std::endl;



		int i = 0; (void) i;

	}

//	for (auto it : stats) {
//		std::cout << it.first.asString() << ": " << it.second.asString() << std::endl;
//	}




}


#endif