#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, float sigma, std::array<float, 4> dist, std::array<Point2, 4> apPos, std::vector<std::pair<Point2, float>>& density, std::pair<Point2, float>& maxElem)
{
    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);

            float P = 1.0f;

            for (size_t i = 0; i < 4; i++)
            {
                // TODO: Was mit nan machen?
                if (!isnan(dist[i]))
                {
                    float d = pos.getDistance(apPos[i]) - dist[i];
                    float p = Distribution::Normal<float>::getProbability(0, sigma, d);
                    P *= p;
                }
            }

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

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

    maxElem = *maxElement;
}

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

    // 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 distance = 0;
    for (int i = 1; i < gtEntries.size(); ++i) {
        distance += gtEntries[i].value.getDistance(gtEntries[i - 1].value);
    }

    std::cout << "Distance of Path: " << distance << 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::array<Kalman, 4> ftmKalmanFilters{
        Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev),
        Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev),
        Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev),
        Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev)
    };

    std::array<Point2, 4> apPositions{
        Settings::data.CurrentPath.NUCs.at(Settings::NUC1).position.xy(),
        Settings::data.CurrentPath.NUCs.at(Settings::NUC2).position.xy(),
        Settings::data.CurrentPath.NUCs.at(Settings::NUC3).position.xy(),
        Settings::data.CurrentPath.NUCs.at(Settings::NUC4).position.xy(),
    };

    std::vector<WifiMeas> data = filterOfflineData(fr);

    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 WifiMeas& wifi : data)
    {
        Plotta::Plotta test("test", "C:\\Temp\\Plotta\\probData.py");

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

        float distErrorFtm = 0;
        float distErrorRssi = 0;

        //if (wifi.numSucessMeas() == 4)
        {
            // FTM
            {
                std::array<float, 4> dists = wifi.ftmDists;

                if (Settings::UseKalman)
                {
                    for (size_t i = 0; i < 4; i++)
                    {
                        if (!isnan(dists[i]))
                        {
                            dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]);
                        }
                    }
                }

                BBox3 bbox = FloorplanHelper::getBBox(map);
                std::vector<std::pair<Point2, float>> density;
                std::pair<Point2, float> maxElement;

                poorMansKDE(bbox, sigma, dists, apPositions, density, maxElement);

                Point2 estPos = maxElement.first;

                plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
                plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1));

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


                //std::vector<float> densityX, densityY, densityZ;
                //for (const auto& item : density)
                //{
                //    densityX.push_back(item.first.x);
                //    densityY.push_back(item.first.y);
                //    densityZ.push_back(item.second);
                //}

                //test.add("densityX", densityX);
                //test.add("densityY", densityY);
                //test.add("densityZ", densityZ);
            }

            // RSSI
            {
                std::array<float, 4> dists = wifi.rssiDists;

                if (Settings::UseKalman)
                {
                    for (size_t i = 0; i < 4; i++)
                    {
                        if (!isnan(dists[i]))
                        {
                            dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]);
                        }
                    }
                }


                BBox3 bbox = FloorplanHelper::getBBox(map);
                std::vector<std::pair<Point2, float>> density;
                std::pair<Point2, float> maxElement;

                poorMansKDE(bbox, sigma, dists, apPositions, density, maxElement);

                Point2 estPos = maxElement.first;

                plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
                plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1));

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


                //std::vector<float> densityX, densityY, densityZ;
                //for (const auto& item : density)
                //{
                //    densityX.push_back(item.first.x);
                //    densityY.push_back(item.first.y);
                //    densityZ.push_back(item.second);
                //}

                //test.add("densityX", densityX);
                //test.add("densityY", densityY);
                //test.add("densityZ", densityZ);
            }

            //std::cout << wifi.ts.ms() << " " << distError << "\n";

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

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

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

    std::cout << "Walk error:" << "\n";
    std::cout << "[m]   " << "   mean \t stdDev  median" << "\n";

    std::cout << "FTM   " << errorStats.ftm.getAvg() << "\t" << errorStats.ftm.getStdDev() << "\t" << errorStats.ftm.getMedian() << "\n";
    std::cout << "RSSI  " << errorStats.rssi.getAvg() << "\t" << errorStats.rssi.getStdDev() << "\t" << errorStats.rssi.getMedian() << "\n";
    std::cout << std::endl;

    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 < 6; walkIdx++)
    {
        std::cout << "Executing walk " << walkIdx << "\n";
        for (int i = 0; i < 1; ++i)
        {
            std::cout << "Start of iteration " << i << "\n";

            tmp = run(Settings::data.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;
}