Indoor/sensors/offline/OfflineAndroid.h

#ifndef OFFLINEANDROID_H
#define OFFLINEANDROID_H

#include <fstream>
#include <string>

#include "../../misc/Debug.h"
#include "../../Assertions.h"
#include "../../math/Interpolator.h"
#include "../../geo/Point3.h"
#include "../../data/Timestamp.h"
#include "../radio/WiFiMeasurements.h"
#include "../imu/AccelerometerData.h"
#include "../imu/GyroscopeData.h"

template <typename SensorData> struct OfflineEntry {

	Timestamp ts;
	SensorData data;

	/** ctor */
	OfflineEntry(const Timestamp ts, const SensorData& data) : ts(ts), data(data) {;}

};

struct GroundTruthID {
	int id;
	GroundTruthID() {;}
	GroundTruthID(const int id) : id(id) {;}
};

struct WalkedPath {
	std::vector<Point3> pos_cm;
};


/** listener for event callbacks */
class OfflineAndroidListener {
public:
	virtual void onGyroscope(const Timestamp ts, const GyroscopeData data) = 0;
	virtual void onAccelerometer(const Timestamp ts, const AccelerometerData data) = 0;
	virtual void onGravity(const Timestamp ts, const AccelerometerData data) = 0;
	virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) = 0;
};

/** read recorded android sensor data files */
class OfflineAndroid {


private:

	std::vector<OfflineEntry<WiFiMeasurements>> wifi;
	std::vector<OfflineEntry<GroundTruthID>> groundTruth;
	std::vector<OfflineEntry<GyroscopeData>> gyro;

	std::vector<OfflineEntry<AccelerometerData>> accel;
	std::vector<OfflineEntry<AccelerometerData>> gravity;

	WalkedPath walkedPath;

	const char* name = "OfflineData";

public:

	/** ctor */
	OfflineAndroid() {
		;
	}

	/** get all ground truth readings */
	const std::vector<OfflineEntry<GroundTruthID>>& getGroundTruth() const {return groundTruth;}

	/** get all WiFi readings */
	const std::vector<OfflineEntry<WiFiMeasurements>>& getWiFi() const {return wifi;}

	/** get all gyroscope readings */
	const std::vector<OfflineEntry<GyroscopeData>>& getGyroscope() const {return gyro;}

	/** get all accelerometer readings */
	const std::vector<OfflineEntry<AccelerometerData>>& getAccelerometer() const {return accel;}

	/** get all gravity readings */
	const std::vector<OfflineEntry<AccelerometerData>>& getGravity() const {return gravity;}


	/** get the walked path */
	const WalkedPath& getWalkedPath() const {return walkedPath;}

	/** get the interpolated walked path */
	Interpolator<Timestamp, Point3> getWalkedPathInterpolatorCM() const {
		Assert::equal(getWalkedPath().pos_cm.size(), getGroundTruth().size(), "entry-number mismatch!");
		Interpolator<Timestamp, Point3> interpol;
		for (const OfflineEntry<GroundTruthID>& entry : getGroundTruth()) {
			interpol.add(entry.ts, getWalkedPath().pos_cm[entry.data.id]);
		}
		return interpol;
	}

public:

	void parse(const std::string& file, OfflineAndroidListener* listener = nullptr) {

		Log::add(name, "parsing data file: " + file , false);
		Log::tick();

		// open the stream
		std::ifstream inp(file);

		// sanity check
		if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open " +file);}

		// parse one line
		while(!inp.eof()) {

			// temporals
			uint64_t ts;
			char delim;
			int32_t sensorID = 999999;			// avoids issues with the last line
			std::string sensorData;

			// read from file
			inp >> ts;
			inp >> delim;
			inp >> sensorID;
			inp >> delim;
			inp >> sensorData;

			if (delim == 0) {break;}

			parse(Timestamp::fromMS(ts), sensorID, sensorData, listener);

		}

		Log::tock();

		Log::add(name,
			"gyro(" + std::to_string(gyro.size()) + ") " +
			"accel(" + std::to_string(accel.size()) + ") "
			"wifi(" + std::to_string(wifi.size()) + ") " +
			"gt(" + std::to_string(groundTruth.size()) + ") "
		);

	}

private:

	/** parse the given data */
	void parse(const Timestamp ts, const int32_t sensorID, const std::string& sensorData, OfflineAndroidListener* listener) {

		// how to parse
		switch(sensorID) {

			case 0: {
				const AccelerometerData data = parseAccelerometer(sensorData);
				accel.push_back(OfflineEntry<AccelerometerData>(ts, data));
				if (listener) {listener->onAccelerometer(ts, data);}
				break;
			}

			case 1: {
				const AccelerometerData data = parseAccelerometer(sensorData);
				gravity.push_back(OfflineEntry<AccelerometerData>(ts, data));
				if (listener) {listener->onGravity(ts, data);}
				break;
			}

			case 3: {
				const GyroscopeData data = parseGyroscope(sensorData);
				gyro.push_back(OfflineEntry<GyroscopeData>(ts, data));
				if (listener) {listener->onGyroscope(ts, data);}
				break;
			}

			case 8: {
				const WiFiMeasurements data = parseWiFi(sensorData);
				wifi.push_back(OfflineEntry<WiFiMeasurements>(ts, data));
				if (listener) {listener->onWiFi(ts, data);}
				break;
			}

			case 99: {
				const GroundTruthID data = parseGroundTruthTick(sensorData);
				groundTruth.push_back(OfflineEntry<GroundTruthID>(ts, data));
				// TODO listener
				break;
			}

			case 100: {
				walkedPath = parseWalkedPath(sensorData);
				break;
			}

		}

	}


	/** parse the given WiFiObservation string "MAC;freq;RSSI;MAC;freq;RSSI;...." */
	static inline WiFiMeasurements parseWiFi(std::string data) {

		WiFiMeasurements obs;

		// process all APs
		while(!data.empty()) {

			const std::string mac = data.substr(0, 17);
			data = data.substr(17);
			Assert::isTrue(data[0] == ';', "unexpected character");
			data = data.substr(1);

			const std::string freq = data.substr(0, 4);
			data = data.substr(4);
			Assert::isTrue(data[0] == ';', "unexpected character");
			data = data.substr(1);

			const int pos = data.find(';');
			const std::string rssi = data.substr(0, pos);
			data = data.substr(pos);
			Assert::isTrue(data[0] == ';', "unexpected character");
			data = data.substr(1);

			const WiFiMeasurement e(mac, std::stof(rssi));
			obs.entries.push_back(e);

		}

		return obs;

	}

	static inline GyroscopeData parseGyroscope(const std::string& data) {

		const size_t pos1 = data.find(';', 0);
		const size_t pos2 = data.find(';', pos1+1);
		const size_t pos3 = data.find(';', pos2+1);

		Assert::isTrue(pos1 != std::string::npos, "format error");
		Assert::isTrue(pos2 != std::string::npos, "format error");
		Assert::isTrue(pos3 != std::string::npos, "format error");

		const std::string sx = data.substr(0, pos1);
		const std::string sy = data.substr(pos1+1, pos2-pos1-1);
		const std::string sz = data.substr(pos2+1, pos3-pos2-1);

		return GyroscopeData(std::stof(sx), std::stof(sy), std::stof(sz));

	}


	static inline AccelerometerData parseAccelerometer(const std::string& data) {

		const size_t pos1 = data.find(';', 0);
		const size_t pos2 = data.find(';', pos1+1);
		const size_t pos3 = data.find(';', pos2+1);

		Assert::isTrue(pos1 != std::string::npos, "format error");
		Assert::isTrue(pos2 != std::string::npos, "format error");
		Assert::isTrue(pos3 != std::string::npos, "format error");

		const std::string sx = data.substr(0, pos1);
		const std::string sy = data.substr(pos1+1, pos2-pos1-1);
		const std::string sz = data.substr(pos2+1, pos3-pos2-1);

		return AccelerometerData(std::stof(sx), std::stof(sy), std::stof(sz));

	}

	/** parse the given GroundTruth entry */
	static inline GroundTruthID parseGroundTruthTick(const std::string& data) {

		GroundTruthID id;
		const int pos = data.find(';');
		id.id = std::stoi(data.substr(0, pos));
		return id;

	}


	/** parse the given WalkedPath string "x,y,z;x,y,z;x,y,..." */
	static inline WalkedPath parseWalkedPath(std::string data) {

		WalkedPath path;

		// process all points
		while(!data.empty()) {

			const int pos1 = data.find(',');
			const int pos2 = data.find(',',  pos1+1);
			const int pos3 = data.find(';',  pos2+1);

			const std::string x = data.substr(0, pos1);
			const std::string y = data.substr(pos1+1, pos2-pos1-1);
			const std::string z = data.substr(pos2+1, pos3-pos2-1);

			path.pos_cm.push_back(Point3(std::stof(x), std::stof(y), std::stof(z)));

			data = data.substr(pos3+1);

		}

		return path;

	}

};

#endif // OFFLINEANDROID_H