#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 "FtmKalman.h"

#include "Settings.h"

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

#include "misc.h"

static float kalman_procNoiseDistStdDev = 1.2f;  // standard deviation of distance for process noise 
static float kalman_procNoiseVelStdDev = 0.1f;   // standard deviation of velocity for process noise 

static void poorMansKDE(const BBox3& bbox, std::vector<std::pair<Point2, double>>& density, std::pair<Point2, double>& maxElem, const std::function<double(Point2 pt)>& evalProc)
{
    density.clear();

    const float stepsize = 0.2;

    const float minX = bbox.getMin().x - 5;
    const float minY = bbox.getMin().y - 5;

    const float maxX = bbox.getMax().x + 5;
    const float maxY = bbox.getMax().y + 5;

    for (float y = minY; y < maxY; y += stepsize)
    {
        for (float x = minX; x < maxX; x += stepsize)
        {
            const Point2 pos(x, y);

            double P = evalProc(pos);

            density.push_back({ pos, P });
        }
    }

    auto maxElement = std::max_element(density.begin(), density.end(), [](std::pair<Point2, double> a, std::pair<Point2, double> b) {
        return a.second < b.second;
    });

    maxElem = *maxElement;
}

static void computeDensity(const BBox3& bbox, std::vector<std::pair<Point2, double>>& density, std::pair<Point2, double>& maxElem, const std::vector<WiFiMeasurement>& obs, bool useFtm, double sigma)
{
    poorMansKDE(bbox, density, maxElem, [&obs, useFtm, sigma](Point2 pt) {
        double p = 1.0;

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

            const MACAddress& mac = wifi.getAP().getMAC();
            int nucIndex = Settings::nucIndex(mac);

            // compute AP distance
            const Point3 apPos = Settings::CurrentPath.nucInfo(nucIndex).position;
            Point3 pos = Point3(pt.x, pt.y, 1.3); // smartphone höhe
            const float apDist = pos.getDistance(apPos);

            double dist = 0;
            if (useFtm)
            {
                // compute ftm distance
                float ftm_offset = Settings::CurrentPath.NUCs.at(mac).ftm_offset;
                float ftmDist = wifi.getFtmDist() + ftm_offset; // in m; plus static offset

                dist = ftmDist;
            }
            else
            {
                // compute rssi distance
                float rssi_pathloss = Settings::CurrentPath.NUCs.at(mac).rssi_pathloss;
                float rssiDist = LogDistanceModel::rssiToDistance(-40, 2.25 /*rssi_pathloss*/, wifi.getRSSI());

                dist = rssiDist;
            }

            if (dist > 0)
            {
                double d = apDist - dist;
                double x = Distribution::Normal<double>::getProbability(0, 3.5, d);

                p *= x;
            }
        }

        return p;
    });
}

static void plotDensity(Plotty& plot, std::vector<std::pair<Point2, double>>& density)
{
    plot.particles.clear();

    double min = std::numeric_limits<double>::max();
    double max = std::numeric_limits<double>::min();

    for (auto it = density.begin(); ; std::advance(it, 2))
    {
        if (it >= density.end())
            break;

        auto p = *it;

        const K::GnuplotPoint3 p3(p.first.x, p.first.y, 0.1);
        const double prob = std::pow(p.second, 0.25);

        plot.particles.add(p3, prob);

        if (prob > max) { max = prob; }
        if (prob < min) { min = prob; }
    }

    plot.splot.getAxisCB().setRange(min, max + 0.000001);
}

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);
    plot.plot();
    
    // wifi

    std::vector<WiFiMeasurement> obs;

    Timestamp lastTimestamp = Timestamp::fromMS(0);

    //const float sigma = 3.5;
    //const int movAvgWnd = 15;
    //std::array<MovingAVG<float>, 4> movAvgsFtm{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
    //std::array<MovingAVG<float>, 4> movAvgsRssi{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };

    std::vector<float> errorValuesFtm, errorValuesRssi;
    std::vector<int> timestamps;

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

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

        auto wifiFtm = fr.getWifiFtm()[e.idx].data;
        obs.push_back(wifiFtm);

        if (ts - lastTimestamp >= Timestamp::fromMS(500))
        {
            // Do update
            Plotta::Plotta test("test", "C:\\Temp\\Plotta\\probData.py");

            Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy();
            plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));

            float distErrorFtm = 0;
            float distErrorRssi = 0;

            // FTM
            {
                BBox3 bbox = FloorplanHelper::getBBox(map);
                std::vector<std::pair<Point2, double>> density;
                std::pair<Point2, double> maxElement;

                computeDensity(bbox, density, maxElement, obs, true, 3.5);

                Point2 estPos = maxElement.first;
                //plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1));
                plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));

                // Plot density
                plotDensity(plot, density);

                // Error
                distErrorFtm = gtPos.getDistance(estPos);
                errorStats.ftm.add(distErrorFtm);
            }

            // RSSI
            {
                BBox3 bbox = FloorplanHelper::getBBox(map);
                std::vector<std::pair<Point2, double>> density;
                std::pair<Point2, double> maxElement;

                computeDensity(bbox, density, maxElement, obs, false, 8);

                Point2 estPos = maxElement.first;
                //plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1));

                // Plot density
                //plotDensity(plot, density);

                // Error
                distErrorRssi = gtPos.getDistance(estPos);
                errorStats.rssi.add(distErrorRssi);
            }

            // draw wifi ranges
            plot.clearDistanceCircles();

            for (size_t i = 0; i < obs.size(); i++)
            {
                WiFiMeasurement wifi2 = obs[i];

                Point3 apPos = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).position;

                K::GnuplotColor color;
                switch (Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).ID)
                {
                case 1:  color = K::GnuplotColor::fromRGB(0, 255, 0); break;
                case 2:  color = K::GnuplotColor::fromRGB(0, 0, 255); break;
                case 3:  color = K::GnuplotColor::fromRGB(255, 255, 0); break;
                default: color = K::GnuplotColor::fromRGB(255, 0, 0); break;
                }

                plot.addDistanceCircle(apPos.xy(), wifi2.getFtmDist(), color);
            }

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

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

            plot.plot();
            //Sleep(250);
            printf("");

            lastTimestamp = ts;
            obs.clear();
        }
    }

    printErrorStats(errorStats);

    return errorStats;
}



int mainProp(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/tmp";

    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 << "==========================================================" << 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;
}