#ifndef FILEREADER_H
#define FILEREADER_H

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

#include <vector>
#include <Indoor/math/Interpolator.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/sensors/pressure/BarometerData.h>
#include <Indoor/sensors/imu/AccelerometerData.h>
#include <Indoor/sensors/imu/GyroscopeData.h>
#include <Indoor/sensors/beacon/BeaconMeasurements.h>

#include <unordered_map>

#include <Indoor/geo/Point2.h>
#include <Indoor/grid/factory/v2/GridFactory.h>
#include <Indoor/grid/factory/v2/Importance.h>
#include <Indoor/floorplan/v2/Floorplan.h>

class FileReader {

public:

    template <typename T> struct TS {
        const uint64_t ts;
        T data;
        TS(const uint64_t ts) : ts(ts) {;}
        TS(const uint64_t ts, const T& data) : ts(ts), data(data) {;}
    };

    enum class Sensor {
        ACC,
        GYRO,
        WIFI,
        POS,
        BARO,
        BEACON,
    };

    /** entry for one sensor */
    struct Entry {
        Sensor type;
        uint64_t ts;
        int idx;
        Entry(Sensor type, uint64_t ts, int idx) : type(type), ts(ts), idx(idx) {;}
    };

    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;

    /** ALL entries */
    std::vector<Entry> entries;

public:

    FileReader(const std::string& file) {
        parse(file);
    }

    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<BarometerData>>& getBarometer() const {return barometer;}

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 == 8)	{parseWiFi(ts, data);}
            else if	(idx == 9)	{parseBeacons(ts, data);}
            else if	(idx == 99) {parseGroundTruth(ts, data);}
            else if	(idx == 0)  {parseAccelerometer(ts, data);}
            else if	(idx == 3)  {parseGyroscope(ts, data);}
            else if (idx == 5)	{parseBarometer(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 parseAccelerometer(const uint64_t ts, const std::string& data) {

        const int pos1 = data.find(';');
        const int 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<AccelerometerData> elem(ts, AccelerometerData(std::stof(x), std::stof(y), std::stof(z)));
        acc.push_back(elem);
        entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1));

    }

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

        const int pos1 = data.find(';');
        const int 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<GyroscopeData> elem(ts, GyroscopeData(std::stof(x), std::stof(y), std::stof(z)));
        gyro.push_back(elem);
        entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1));

    }

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

        std::string tmp = data;

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

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

            std::string mac = tmp.substr(0, 17);
            tmp = tmp.substr(17);
            assert(tmp[0] == ';');	tmp = tmp.substr(1);

            std::string freq = tmp.substr(0, 4);
            tmp = tmp.substr(4);
            assert(tmp[0] == ';');	tmp = tmp.substr(1);

            int pos = tmp.find(';');
            std::string rssi = tmp.substr(0, pos);
            tmp = tmp.substr(pos);
            assert(tmp[0] == ';'); tmp = tmp.substr(1);

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

    }

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

        std::string tmp = data;

        std::string mac = tmp.substr(0, 17);
        tmp = tmp.substr(17);
        assert(tmp[0] == ';');	tmp = tmp.substr(1);

        std::string rssi = tmp;

        //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 int 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) {

        const int pos1 = data.find(';');

        const std::string hPa = data.substr(0, pos1);

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

    }

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

        // finde alle positionen der waypoints im gtPath aus map
        std::unordered_map<int, Point2> waypointsMap;
        for(Floorplan::Floor* f : map.floors){
            for (Floorplan::POI* poi : f->pois){
                waypointsMap.insert({std::stoi(poi->name), poi->pos});
            }
        }

        // bringe aufgenommene gt punkte der app in unordered_map
        std::unordered_map<int, const uint64_t> waypointsApp;
        for(TS<int> val : groundTruth){
            waypointsApp.insert({val.data, val.ts});
        }

        // bringe diese in richtige reihenfolge und füge timestamp hinzu
        Interpolator<uint64_t, Point2> interpol;
        for(int id : gtPath){
            auto itMap = waypointsMap.find(id);
            if(itMap == waypointsMap.end()) {throw "not found";}

            auto itApp = waypointsApp.find(id);
            if(itApp == waypointsApp.end()) {throw "not found";}

            interpol.add(itApp->second, itMap->second);

        }

        return interpol;
    }

};

#endif // FILEREADER_H