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

    // ground truth
    Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, setup.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(setup.gtPath);
    plot.setView(30, 0);
    plot.plot();

    Plotta::Plotta plotta("test", "C:\\Temp\\Plotta\\dataTrilat.py");

    std::vector<Point2> 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(),
    };

    plotta.add("apPos", apPositions);

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

    const bool UseFTM = false;
    const int movAvgWnd = 10;
    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;

    std::vector<Point2> gtPath, estPathFtm, estPathRssi;

    for (const WifiMeas& wifi : data)
    {
        Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(wifi.ts.ms())).xy();
        plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
        gtPath.push_back(gtPos);

        float distErrorFtm = 0;
        float distErrorRssi = 0;

        //if (wifi.numSucessMeas() == 4)
        {
            // FTM
            {
                std::vector<float> avgDists;

                for (size_t i = 0; i < 4; i++)
                {
                    float dist = wifi.ftmDists[i];

                    if (!isnan(dist))
                    {
                        movAvgsFtm[i].add(dist);
                    }


                    if (movAvgsFtm[i].getNumUsed() == 0)
                    {
                        avgDists.push_back(0);
                    }
                    else
                    {
                        avgDists.push_back(movAvgsFtm[i].get());
                    }                   
                }

                Point2 estPos = Trilateration::calculateLocation2d(apPositions, avgDists);

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

                // draw wifi ranges           
                for (size_t i = 0; i < 4; i++)
                {
                    plot.addCircle(i + 1, apPositions[i], avgDists[i]);
                }

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


            // RSSI
            {
                std::vector<float> avgDists;

                for (size_t i = 0; i < 4; i++)
                {
                    float dist = wifi.rssiDists[i];

                    if (!isnan(dist))
                    {
                        movAvgsRssi[i].add(dist);
                    }


                    if (movAvgsRssi[i].getNumUsed() == 0)
                    {
                        avgDists.push_back(0);
                    }
                    else
                    {
                        avgDists.push_back(movAvgsRssi[i].get());
                    }
                }

                Point2 estPos = Trilateration::calculateLocation2d(apPositions, avgDists);

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

                // Error
                distErrorRssi = gtPos.getDistance(estPos);
                errorStats.rssi.add(distErrorRssi);
                estPathRssi.push_back(estPos);
            }
            
            //std::cout << wifi.ts.ms() << " " << distError << "\n";
            
            errorValuesFtm.push_back(distErrorFtm);
            errorValuesRssi.push_back(distErrorRssi);
            timestamps.push_back(wifi.ts.ms());

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

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

    plotta.add("gtPath", gtPath);
    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/tmp";

    for (size_t walkIdx = 0; walkIdx < Settings::data.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::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;
}