#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 "../../geo/Point2.h"
#include "../../grid/factory/v2/GridFactory.h"
#include "../../grid/factory/v2/Importance.h"
#include "../../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,
        LIN_ACC,
        GRAVITY,
    };

    /** 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;
    std::vector<TS<LinearAccelerationData>> lin_acc;
    std::vector<TS<GravityData>> gravity;

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

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

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

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);}
            else if (idx == 2)  {parseLinearAcceleration(ts,data);}
            else if (idx == 1)  {parseGravity(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){

        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<GravityData> elem(ts, GravityData(std::stof(x), std::stof(y), std::stof(z)));
        gravity.push_back(elem);
        entries.push_back(Entry(Sensor::GRAVITY, ts, gravity.size()-1));
    }

    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);

        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 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<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()) {

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

            std::string mac = tmp.substr(0, pos1);
            std::string freq = tmp.substr(pos1+1, pos2);
            std::string rssi = tmp.substr(pos2+1, pos3);

            tmp = tmp.substr(pos3);
            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) {

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

        const auto 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));

    }

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