#include "main.h"

#include <array>
#include <memory>
#include <thread>
#include <filesystem>
#include <chrono>
#include <iostream>

#include <Indoor/math/stats/Statistics.h>

#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/sensors/offline/FileReader.h>
#include <Indoor/sensors/offline/Sensors.h>
#include <Indoor/sensors/radio/model/LogDistanceModel.h>
#include <Indoor/geo/Heading.h>
#include <Indoor/geo/Point2.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/math/MovingAVG.h>


#include "Settings.h"

#include "Plotty.h"
#include "Plotta.h"


#include "trilateration.h"
#include "misc.h"



static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::string folder)
{
    // reading file
    Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
    Offline::FileReader fr(setup.training[walkIdx], setup.HasNanoSecondTimestamps);

    // ground truth
    std::vector<int> gtPath = setup.gtPath;
    Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
    CombinedStats<float> errorStats;

    //calculate distance of path
    std::vector<Interpolator<uint64_t, Point3>::InterpolatorEntry> gtEntries = gtInterpolator.getEntries();
    double gtTotalDistance = 0;
    Stats::Statistics<double> gtWalkingSpeed;
    for (int i = 1; i < gtEntries.size(); ++i) {
        double distance = gtEntries[i].value.getDistance(gtEntries[i - 1].value);
        double timeDiff = static_cast<double>(gtEntries[i].key - gtEntries[i - 1].key);

        double walkingSpeed = distance / (timeDiff / 1000.0f); // m / s

        gtWalkingSpeed.add(walkingSpeed);
        gtTotalDistance += distance;
    }

    std::cout << "Distance of Path: " << gtTotalDistance << std::endl;
    std::cout << "GT walking speed: " << gtWalkingSpeed.getAvg() << "m/s (" << gtWalkingSpeed.getAvg()*3.6f << "km/h)" << std::endl;

    // debug show
//MeshPlotter dbg;
//dbg.addFloors(map);
//dbg.addOutline(map);
//dbg.addMesh(mesh);
////dbg.addDijkstra(mesh);
//dbg.draw();

    Plotty plot(map);
    plot.buildFloorplan();
    plot.setGroundTruth(gtPath);
    plot.setView(30, 0);
    // APs Positions
    for (auto& nucConfig : setup.NUCs)
    {
        plot.addCircle(10000 + nucConfig.second.ID, nucConfig.second.position.xy(), 0.1);
    }
    plot.plot();


    // Output dir
    auto outputDir = std::filesystem::path(Settings::outputDir);
    outputDir.append(Settings::CurrentPath.name + "_" + std::to_string(walkIdx));

    if (!std::filesystem::exists(outputDir)) {
        std::filesystem::create_directories(outputDir);
    }

    std::vector<WiFiMeasurement> obs;

    Timestamp lastTimestamp = Timestamp::fromMS(0);

    Plotta::Plotta plotta("test", "C:\\Temp\\Plotta\\dataTrilat.py");
    //plotta.add("apPos", apPositions);

    const int movAvgWnd = 10;
    std::unordered_map<MACAddress, MovingAVG<float>> movAvgsFtm;
    std::unordered_map<MACAddress, MovingAVG<float>> movAvgsRssi;

    for (auto& nucConfig : setup.NUCs)
    {
        movAvgsFtm.insert({ nucConfig.first, MovingAVG<float>(movAvgWnd) });
        movAvgsRssi.insert({ nucConfig.first, MovingAVG<float>(movAvgWnd) });
    }
    
    std::vector<float> errorValuesFtm, errorValuesRssi;
    std::vector<int> timestamps;

    std::vector<Point2> estPathFtm, estPathRssi;

    for (const Offline::Entry& e : fr.getEntries())
    {
        if (e.type != Offline::Sensor::WIFI_FTM && e.type != Offline::Sensor::GROUND_TRUTH) {
            continue;
        }

        // TIME
        const Timestamp ts = Timestamp::fromMS(e.ts);

        if (e.type == Offline::Sensor::WIFI_FTM)
        {
        auto wifiFtm = fr.getWifiFtm()[e.idx].data;
        obs.push_back(wifiFtm);
        }

        if (ts - lastTimestamp >= Timestamp::fromMS(500))
        {
            // Do update
            Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy();
            plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
            
            std::unordered_map<MACAddress, std::pair<float, float>> apPosDistMap;

            for (const WiFiMeasurement& wifi : obs)
            {
                if (wifi.getNumSuccessfulMeasurements() < 3)
                    continue;

                const MACAddress& mac = wifi.getAP().getMAC();
                float ftm_offset = setup.NUCs.at(mac).ftm_offset;
                float ftmDist = wifi.getFtmDist() + ftm_offset;

                float rssi_pathloss = setup.NUCs.at(mac).rssi_pathloss;
                float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI());

                movAvgsFtm[mac].add(ftmDist);
                movAvgsRssi[mac].add(rssiDist);

                apPosDistMap[mac] = { movAvgsFtm[mac].get(), movAvgsRssi[mac].get() };
            }

            if (apPosDistMap.size() > 3)
            {
                // Do update for real
                std::vector<MACAddress> macs;
                std::vector<Point2> apPositions;
                std::vector<float> ftmDists;
                std::vector<float> rssiDists;

                for (const auto& kvp : apPosDistMap)
                {
                    macs.push_back(kvp.first);
                    apPositions.push_back(setup.NUCs.at(kvp.first).position.xy());
                    ftmDists.push_back(kvp.second.first);
                    rssiDists.push_back(kvp.second.second);
                }

                Point2 estFtmPos = Trilateration::levenbergMarquardt(apPositions, ftmDists);
                Point2 estRssiPos = Trilateration::levenbergMarquardt(apPositions, rssiDists);

                // Error
                float distErrorFtm = gtPos.getDistance(estFtmPos);
                errorStats.ftm.add(distErrorFtm);
                estPathFtm.push_back(estFtmPos);

                float  distErrorRssi = gtPos.getDistance(estRssiPos);
                errorStats.rssi.add(distErrorRssi);
                estPathRssi.push_back(estRssiPos);

                errorValuesFtm.push_back(distErrorFtm);
                errorValuesRssi.push_back(distErrorRssi);
                timestamps.push_back(ts.ms());

                plotta.add("t", timestamps);
                plotta.add("errorFtm", errorValuesFtm);
                plotta.add("errorRssi", errorValuesRssi);
                plotta.frame();

                // Plot
                plot.setCurEst(Point3(estFtmPos.x, estFtmPos.y, 0.1));
                plot.addEstimationNode(Point3(estFtmPos.x, estFtmPos.y, 0.1));
                plot.addEstimationNode2(Point3(estRssiPos.x, estRssiPos.y, 0.1));

                // draw wifi ranges           
                if (Settings::PlotCircles)
                {
                    plot.clearDistanceCircles();
                    plot.splot.getCustom().str("");

                    for (size_t i = 0; i < 20; i++)
                    {
                        plot.splot.getCustom() << "unset label " << i << "\n";
                    }

                    for (size_t i = 0; i < ftmDists.size(); i++)
                    {
                        plot.addDistanceCircle(apPositions[i], ftmDists[i], K::GnuplotColor::fromRGB(255, 0, 0));
                        plot.addDistanceCircle(apPositions[i], rssiDists[i], K::GnuplotColor::fromRGB(0, 255, 0));
                    
                        // Distance labels
                        std::stringstream ss;
                        ss << setup.nuc(macs[i]).ID << ": " << ftmDists[i] << "m";
    
                        plot.addLabel(ss.str(), Point3(70, i*5, 0), i);
                    }
                }
            }

                // Png Output
                //if (Settings::PlotToPng)
                //{
                //    plot.gp.setTerminal("png", K::GnuplotSize(1280, 720));
                //    auto pngPath = outputDir / "png" / "trilat" / "frame.png";

                //    // clear folder
                //    //std::filesystem::remove_all(pngPath);
                //    forceDirectories(pngPath.parent_path());
                //    //std::filesystem::create_directory(pngPath);

                //    plot.gp.setOutput(appendFileSuffixToPath(pngPath, ts.ms()).string());
                //}

                plot.plot();
                std::this_thread::sleep_for(std::chrono::milliseconds(100));

            obs.clear();
            lastTimestamp = ts;
        }

        printf("");
    }

    plotta.add("estPathFtm", estPathFtm);
    plotta.add("estPathRssi", estPathRssi);
    plotta.frame();

    printErrorStats(errorStats);

    return errorStats;
}


int mainTrilat(int argc, char** argv)
{
    // global stats
    CombinedStats<float> statsAVG;
    CombinedStats<float> statsMedian;
    CombinedStats<float> statsSTD;
    CombinedStats<float> statsQuantil;
    CombinedStats<float> tmp;

    std::string evaluationName = "prologic/trilat";

    std::vector<Settings::DataSetup> setupsToRun = {
        //Settings::data.Path5,
        //Settings::data.Path7,
        //Settings::data.Path8,
        //Settings::data.Path9,
        //Settings::data.Path10,
        //Settings::data.Path11
        //Settings::data.Path20,
        Settings::data.Path21,
        //Settings::data.Path22,
    };

    for (Settings::DataSetup setupToRun : setupsToRun)
    {
        Settings::CurrentPath = setupToRun;

        for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++)
        {
            std::cout << "Executing walk " << walkIdx << "\n";
            for (int i = 0; i < 1; ++i)
            {
                std::cout << "Start of iteration " << i << "\n";

                tmp = run(Settings::CurrentPath, walkIdx, evaluationName);

                statsAVG.ftm.add(tmp.ftm.getAvg());
                statsMedian.ftm.add(tmp.ftm.getMedian());
                statsSTD.ftm.add(tmp.ftm.getStdDev());
                statsQuantil.ftm.add(tmp.ftm.getQuantile(0.75));

                statsAVG.rssi.add(tmp.rssi.getAvg());
                statsMedian.rssi.add(tmp.rssi.getMedian());
                statsSTD.rssi.add(tmp.rssi.getStdDev());
                statsQuantil.rssi.add(tmp.rssi.getQuantile(0.75));

                std::cout << "Iteration " << i << " completed" << std::endl;
            }
        }

        std::cout << "Results for path " << Settings::CurrentPath.name << std::endl;
        std::cout << "==========================================================" << std::endl;
        std::cout << "Average of all statistical data FTM: " << std::endl;
        std::cout << "Median: " << statsMedian.ftm.getAvg() << std::endl;
        std::cout << "Average: " << statsAVG.ftm.getAvg() << std::endl;
        std::cout << "Standard Deviation: " << statsSTD.ftm.getAvg() << std::endl;
        std::cout << "75 Quantil: " << statsQuantil.ftm.getAvg() << std::endl;
        std::cout << "==========================================================" << std::endl;

        std::cout << "==========================================================" << std::endl;
        std::cout << "Average of all statistical data RSSI: " << std::endl;
        std::cout << "Median: " << statsMedian.rssi.getAvg() << std::endl;
        std::cout << "Average: " << statsAVG.rssi.getAvg() << std::endl;
        std::cout << "Standard Deviation: " << statsSTD.rssi.getAvg() << std::endl;
        std::cout << "75 Quantil: " << statsQuantil.rssi.getAvg() << std::endl;
        std::cout << "==========================================================" << std::endl;
    }

    return 0;
}