Indoor/sensors/offline/FileReader.h

#ifndef FILEREADER_H
#define FILEREADER_H

#include <fstream>
#include <Indoor/Exception.h>
#include <vector>
#include <unordered_map>

#include "../../math/Interpolator.h"
#include "../../sensors/radio/WiFiMeasurements.h"
#include "../../sensors/pressure/BarometerData.h"
#include "../../sensors/imu/AccelerometerData.h"
#include "../../sensors/imu/GyroscopeData.h"
#include "../../sensors/imu/GravityData.h"
#include "../../sensors/imu/LinearAccelerationData.h"
#include "../../sensors/beacon/BeaconMeasurements.h"
#include "../../sensors/gps/GPSData.h"
#include "../../sensors/imu/CompassData.h"

#include "../../geo/Point2.h"
#include "../../grid/factory/v2/GridFactory.h"
#include "../../grid/factory/v2/Importance.h"
#include "../../floorplan/v2/Floorplan.h"
#include "../../floorplan/v2/FloorplanHelper.h"

#include "Splitter.h"
#include "Sensors.h"
#include "Listener.h"

#warning "adjust to to use the new splitter for all parsers [gps, compass, etc. already do!]"

namespace Offline {

	/**
	 * read and parse previously recorded ["offline"] files
	 */
	class FileReader {

	public:

		using GroundTruth = Interpolator<Timestamp, Point3>;

		/** all entries grouped by sensor */
		std::vector<TS<int>> groundTruth;
		std::vector<TS<WiFiMeasurements>> wifi;
		std::vector<TS<BeaconMeasurement>> beacon;
		std::vector<TS<AccelerometerData>> acc;
		std::vector<TS<GyroscopeData>> gyro;
		std::vector<TS<BarometerData>> barometer;
		std::vector<TS<LinearAccelerationData>> lin_acc;
		std::vector<TS<GravityData>> gravity;
		std::vector<TS<GPSData>> gps;
		std::vector<TS<CompassData>> compass;

		/** all entries in linear order as they appeared while recording */
		std::vector<Entry> entries;

		static constexpr char sep = ';';

	public:

		/** empty ctor. call open() */
		FileReader() {
			;
		}

		/** ctor with filename */
		FileReader(const std::string& file) {
			open(file);
		}

		/** open the given file */
		void open(const std::string& file) {
			clear();
			parse(file);
		}

		/** remove all parsed entries */
		void clear() {
			entries.clear();
			groundTruth.clear();
			wifi.clear();
			beacon.clear();
			acc.clear();
			gyro.clear();
			gps.clear();
			compass.clear();
			barometer.clear();
			lin_acc.clear();
			gravity.clear();
		}

		const std::vector<Entry>& getEntries() const {return entries;}


		const std::vector<TS<int>>& getGroundTruth() const {return groundTruth;}

		const std::vector<TS<WiFiMeasurements>>& getWiFiGroupedByTime() const {return wifi;}

		const std::vector<TS<BeaconMeasurement>>& getBeacons() const {return beacon;}

		const std::vector<TS<AccelerometerData>>& getAccelerometer() const {return acc;}

		const std::vector<TS<GyroscopeData>>& getGyroscope() const {return gyro;}

		const std::vector<TS<GPSData>>& getGPS() const {return gps;}

		const std::vector<TS<CompassData>>& getCompass() const {return compass;}

		const std::vector<TS<BarometerData>>& getBarometer() const {return barometer;}

		const std::vector<TS<LinearAccelerationData>>& getLinearAcceleration() const {return lin_acc;}

		const std::vector<TS<GravityData>>& getGravity() const {return gravity;}

		/** get an interpolateable ground-truth based on the time-clicks during recording */
		GroundTruth getGroundTruth(const Floorplan::IndoorMap* map, const std::vector<int> groundTruthPoints) const {

			// sanity check: given path [indices to ground-truth points within the map]
			// must have the same size as the number of clicks during recording
			Assert::equal(groundTruthPoints.size(), groundTruth.size(), "mismatch of ground-truth points between given path and recording");

			// allows getting a position on the ground-truth given a timestamp
			GroundTruth interpol;

			// all ground-truth points within the map
			static std::unordered_map<int, Point3> gt = FloorplanHelper::getGroundTruthPoints(map);

			// process each "tap smartphone when reaching ground-truth-point"
			for (const TS<int>& entry : groundTruth) {
				const Timestamp ts = Timestamp::fromMS(entry.ts);
				const int idx = entry.data;					// starting at 0, incrementing over time [1st point, 2nd points, 3d points, ...]
				const int id = groundTruthPoints[idx];		// convert point number to point-id within floorplan
				const auto& it = gt.find(id);
				if (it == gt.end()) {throw Exception("missing ground-truth point ID:" + std::to_string(id));}
				const Point3 pos = it->second;
				interpol.add(ts, pos);
			}

			// done
			return interpol;

		}

	private:

		void parse(const std::string& file) {

			std::ifstream inp(file);
			if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);}

			while(!inp.eof() && !inp.bad()) {

				uint64_t ts;
				char delim;
				int idx = -1;
				std::string data;

				inp >> ts;
				inp >> delim;
				inp >> idx;
				inp >> delim;
				inp >> data;

				if		(idx == (int)Sensor::WIFI)			{parseWiFi(ts, data);}
				else if	(idx == (int)Sensor::BEACON)		{parseBeacons(ts, data);}
				else if	(idx == (int)Sensor::GROUND_TRUTH)	{parseGroundTruth(ts, data);}
				else if	(idx == (int)Sensor::ACC)			{parseAccelerometer(ts, data);}
				else if	(idx == (int)Sensor::GYRO)			{parseGyroscope(ts, data);}
				else if (idx == (int)Sensor::BARO)			{parseBarometer(ts, data);}
				else if (idx == (int)Sensor::LIN_ACC)		{parseLinearAcceleration(ts,data);}
				else if (idx == (int)Sensor::GRAVITY)		{parseGravity(ts,data);}
				else if (idx == (int)Sensor::COMPASS)		{parseCompass(ts,data);}
				else if (idx == (int)Sensor::GPS)			{parseGPS(ts,data);}

				// TODO: this is a hack...
				// the loop is called one additional time after the last entry
				// and keeps the entries of entry

			}

			inp.close();

		}

		void parseLinearAcceleration(const uint64_t ts, const std::string& data){

			const auto pos1 = data.find(';');
			const auto pos2 = data.find(';', pos1+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);

			TS<LinearAccelerationData> elem(ts, LinearAccelerationData(std::stof(x), std::stof(y), std::stof(z)));
			lin_acc.push_back(elem);
			entries.push_back(Entry(Sensor::LIN_ACC, ts, lin_acc.size()-1));

		}

		void parseGravity(const uint64_t ts, const std::string& data){

			GravityData gravData;

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

			gravData.x = std::stof(data.substr(0, pos1));
			gravData.y = std::stof(data.substr(pos1+1, pos2-pos1-1));
			gravData.z = std::stof(data.substr(pos2+1));

			TS<GravityData> elem(ts, gravData);
			gravity.push_back(elem);
			entries.push_back(Entry(Sensor::GRAVITY, ts, gravity.size()-1));

			// inform listener
			//if (listener) {listener->onGravity(Timestamp::fromMS(ts), gravData);}

		}

		void parseAccelerometer(const uint64_t ts, const std::string& data) {

			const auto pos1 = data.find(';');
			const auto pos2 = data.find(';', pos1+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);

			const AccelerometerData accData(std::stof(x), std::stof(y), std::stof(z));
			TS<AccelerometerData> elem(ts, accData);
			acc.push_back(elem);
			entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1));

			// inform listener
			//if (listener) {listener->onAccelerometer(Timestamp::fromMS(ts), accData);}

		}

		void parseGyroscope(const uint64_t ts, const std::string& data) {

			const auto pos1 = data.find(';');
			const auto pos2 = data.find(';', pos1+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);

			const GyroscopeData gyroData(std::stof(x), std::stof(y), std::stof(z));
			TS<GyroscopeData> elem(ts, gyroData);
			gyro.push_back(elem);
			entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1));

			// inform listener
			//if (listener) {listener->onGyroscope(Timestamp::fromMS(ts), gyroData);}

		}

		void parseWiFi(const uint64_t ts, const std::string& data) {

			WiFiMeasurements wifi;
			Splitter s(data, sep);

			// the -1 is due to some old files containing a trailing ";" resulting in one additional stray column
			for (size_t i = 0; i < s.size()-1; i += 3) {

				const std::string mac = s.get(i+0);
				const float freq = s.getFloat(i+1);
				const float rssi = s.getFloat(i+2);

				// append AP to current scan-entry
				WiFiMeasurement e(AccessPoint(mac), rssi, freq, Timestamp::fromMS(ts));
				wifi.entries.push_back(e);

			}

			// add new wifi reading
			this->wifi.push_back(TS<WiFiMeasurements>(ts, wifi));
			entries.push_back(Entry(Sensor::WIFI, ts, this->wifi.size()-1));

			// inform listener
			//if (listener) {listener->onWiFi(Timestamp::fromMS(ts), wifi);}

		}

		void parseBeacons(const uint64_t ts, const std::string& data) {

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

			const std::string mac = data.substr(0, pos1);
			const std::string rssi = data.substr(pos1+1, pos2);
			const std::string txp = data.substr(pos2+1, pos3);

			//yes the timestamp is redundant here, but in case of multiusage...
			TS<BeaconMeasurement> e(ts, BeaconMeasurement(Timestamp::fromMS(ts), Beacon(mac), std::stoi(rssi)));
			beacon.push_back(e);
			entries.push_back(Entry(Sensor::BEACON, ts, beacon.size()-1));

		}

		void parseGroundTruth(const uint64_t ts, const std::string& data) {

			const auto pos1 = data.find(';');
			std::string gtIndex = data.substr(0, pos1);

			TS<int> elem(ts, std::stoi(gtIndex));
			groundTruth.push_back(elem);

		}

		void parseBarometer(const uint64_t ts, const std::string& data) {

			BarometerData baro;
			Splitter s(data, sep);

			baro.hPa = s.has(0) ? (s.getFloat(0)) : (NAN);

			TS<BarometerData> elem(ts, baro);
			barometer.push_back(elem);
			entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1));

			// inform listener
			//if (listener) {listener->onBarometer(Timestamp::fromMS(ts), baro);}

		}

		void parseCompass(const uint64_t ts, 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);

			TS<CompassData> elem(ts, compass);
			this->compass.push_back(elem);
			entries.push_back(Entry(Sensor::COMPASS, ts, this->compass.size()-1));

			// inform listener
			//if (listener) {listener->onCompass(Timestamp::fromMS(ts), compass);}

		}

		/** parse the given GPS entry */
		void parseGPS(const uint64_t ts, 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);

			TS<GPSData> elem(ts, gps);
			this->gps.push_back(elem);
			entries.push_back(Entry(Sensor::GPS, ts, this->gps.size()-1));

			// inform listener
			//if (listener) {listener->onGPS(Timestamp::fromMS(ts), gps);}

		}


	public:

		const Interpolator<uint64_t, Point3> getGroundTruthPath(Floorplan::IndoorMap* map, std::vector<int> gtPath) const {

			// finde alle positionen der waypoints im gtPath aus map
			std::unordered_map<int, Point3> waypointsMap;
			for(Floorplan::Floor* f : map->floors){
				float h = f->atHeight;
				for (Floorplan::GroundTruthPoint* gtp : f->gtpoints){

					//wenn die gleiche id 2x vergeben wurde, knallt es
					if(waypointsMap.find(gtp->id) == waypointsMap.end()){
						waypointsMap.insert({gtp->id, Point3(gtp->pos.x,gtp->pos.y, h)});
					}
					else{
						throw std::string("the floorplan's ground truth contains two points with identical id's!");
					}

				}
			}

			// bringe diese in richtige reihenfolge und füge timestamp hinzu
			Interpolator<uint64_t, Point3> interpol;

			int it = 0;
			for(int id : gtPath){
				auto itMap = waypointsMap.find(id);
				if(itMap == waypointsMap.end()) {throw std::string("waypoint not found in xml");}

				//the time, when the gt button was clicked on the app
				uint64_t tsGT = groundTruth[it++].ts;
				interpol.add(tsGT, itMap->second);

			}

			if(gtPath.empty() || waypointsMap.empty() || groundTruth.empty()){
				throw std::string("No Ground Truth points found within the map.xml file");
			}

			return interpol;
		}

	};

}

#endif // FILEREADER_H