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"
#include "../imu/CompassData.h"
#include "../gps/GPSData.h"
#include "../pressure/BarometerData.h"

#include "Splitter.h"
#include "Listener.h"
#include "Sensors.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;
};


/**
 * read sensor data files that were recorded using
 * the old java android app
 */
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<GravityData>> gravity;
	std::vector<OfflineEntry<CompassData>> compass;

	std::vector<OfflineEntry<BarometerData>> barometer;

	std::vector<OfflineEntry<GPSData>> gps;

	WalkedPath walkedPath;

	static constexpr char sep = ';';
	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<GravityData>>& getGravity() const {return gravity;}

	/** get all barometer readings */
	const std::vector<OfflineEntry<BarometerData>>& getBarometer() const {return barometer;}

	/** get all compass readings */
	const std::vector<OfflineEntry<CompassData>>& getCompass() const {return compass;}

	/** get all gps readings */
	const std::vector<OfflineEntry<GPSData>>& getGPS() const {return gps;}


	/** 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, Offline::Listener* 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, Offline::Listener* listener) {

		// how to parse
		switch(sensorID) {

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

			case (int) Offline::Sensor::GRAVITY: {
				const GravityData data = parseGravity(sensorData);
				gravity.push_back(OfflineEntry<GravityData>(ts, data));
				if (listener) {listener->onGravity(ts, data);}
				break;
			}

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

			case (int) Offline::Sensor::BARO: {
			    const BarometerData data = parseBarometer(sensorData);
				barometer.push_back(OfflineEntry<BarometerData>(ts, data));
				if (listener) {listener->onBarometer(ts, data);}
				break;
		    }

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

			case (int) Offline::Sensor::COMPASS: {
				const CompassData data = parseCompass(sensorData);
				compass.push_back(OfflineEntry<CompassData>(ts, data));
				if (listener) {listener->onCompass(ts, data);}
				break;
		    }

			case (int) Offline::Sensor::GPS: {
				const GPSData data = parseGPS(sensorData);
				gps.push_back(OfflineEntry<GPSData>(ts, data));
				if (listener) {listener->onGPS(ts, data);}
				break;
		    }

			case (int) Offline::Sensor::GROUND_TRUTH: {
				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(const Timestamp ts, 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), ts);
			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));

	}

	static inline GravityData parseGravity(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 GravityData(std::stof(sx), std::stof(sy), std::stof(sz));

	}

	/** parse the given Barometer entry */
	static inline BarometerData parseBarometer(const std::string& data) {

		BarometerData baro;
		const int pos = data.find(';');
		baro.hPa = std::stof(data.substr(0, pos));
		return baro;

	}

	/** 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;

	}

	/** parse the given Compass entry */
	static inline CompassData parseCompass(const std::string& data) {

		CompassData compass;
		Splitter s(data, sep);

		compass.azimuth = s.has(0) ? (s.getFloat(0)) : (NAN);
		compass.quality01 = s.has(1) ? (s.getFloat(1)) : (NAN);

		return compass;

	}

	/** parse the given GPS entry */
	static inline GPSData parseGPS(const std::string& data) {

		GPSData gps;
		Splitter s(data, sep);

		gps.lat = s.has(0) ? (s.getFloat(0)) : (NAN);
		gps.lon = s.has(1) ? (s.getFloat(1)) : (NAN);
		gps.alt = s.has(2) ? (s.getFloat(2)) : (NAN);
		gps.accuracy = s.has(3) ? (s.getFloat(3)) : (NAN);
		gps.speed = s.has(4) ? (s.getFloat(4)) : (NAN);

		return gps;

	}

};

#endif // OFFLINEANDROID_H