#ifndef KLB_H
#define KLB_H

#include <chrono>

#include <Indoor/math/divergence/KullbackLeibler.h>
#include <Indoor/grid/factory/v2/GridFactory.h>

#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanReader.h>

#include <Indoor/grid/factory/v2/GridFactory.h>
#include <Indoor/grid/factory/v2/Importance.h>

#include <Indoor/geo/Heading.h>
#include <Indoor/geo/Point2.h>
#include <Indoor/sensors/offline/FileReader.h>

#include <Indoor/sensors/imu/TurnDetection.h>
#include <Indoor/sensors/imu/StepDetection.h>
#include <Indoor/sensors/imu/MotionDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/sensors/radio/WiFiGridEstimator.h>
#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>

#include <Indoor/math/MovingAVG.h>
#include <Indoor/math/FixedFrequencyInterpolator.h>
#include <Indoor/math/divergence/KullbackLeibler.h>
#include <Indoor/math/divergence/JensenShannon.h>
#include <Indoor/data/Timestamp.h>

//#include <KLib/math/statistics/Statistics.h>

#include <Indoor/smc/Particle.h>
#include <Indoor/smc/filtering/ParticleFilterMixing.h>
#include <Indoor/smc/filtering/ParticleFilterInitializer.h>
#include <Indoor/smc/filtering/ParticleFilterHistory.h>

#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>

#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingDivergence.h>

#include <Indoor/smc/merging/MarkovTransitionProbability.h>
#include <Indoor/smc/merging/mixing/MixingSamplerDivergency.h>
#include <Indoor/smc/merging/estimation/JointEstimationPosteriorOnly.h>

//#include <Indoor/smc/smoothing/BackwardSimulation.h>
//#include <Indoor/smc/CondensationBackwardFilter.h>
//#include <Indoor/smc/smoothing/sampling/ParticleTrajectorieSampler.h>
//#include <Indoor/smc/smoothing/sampling/CumulativeSampler.h>
#include <Indoor/smc/smoothing/BackwardFilterTransition.h>

#include "Structs.h"

#include "../Plotti.h"
#include "Logic.h"
#include "../Settings.h"

static double getKernelDensityProbability(std::vector<SMC::Particle<MyState>>& particles, MyState state, std::vector<SMC::Particle<MyState>>& samplesWifi){

    Distribution::KernelDensity<double, MyState> parzen([&](MyState state){
            int size = particles.size();
            double prob = 0;

            #pragma omp parallel for reduction(+:prob) num_threads(6)
            for(int i = 0; i < size; ++i){
                double distance = particles[i].state.position.getDistanceInCM(state.position);
                prob += Distribution::Normal<double>::getProbability(0, 100, distance) * particles[i].weight;
            }

            return prob;
        ;});

    std::vector<double> probsWifiV;
    std::vector<double> probsParticleV;

    //just for plottingstuff
    std::vector<SMC::Particle<MyState>> samplesParticles;

    const int step = 4;
    int i = 0;
    for(SMC::Particle<MyState> particle : samplesWifi){
        if(++i % step != 0){continue;}
        MyState state(GridPoint(particle.state.position.x_cm, particle.state.position.y_cm, particle.state.position.z_cm));

        double probiParticle = parzen.getProbability(state);
        probsParticleV.push_back(probiParticle);

        double probiwifi = particle.weight;
        probsWifiV.push_back(probiwifi);

        //samplesParticles.push_back(SMC::Particle<MyState>(state, probiParticle));
    }

    //make vectors
    Eigen::Map<Eigen::VectorXd> probsWifi(&probsWifiV[0], probsWifiV.size());
    Eigen::Map<Eigen::VectorXd> probsParticle(&probsParticleV[0], probsParticleV.size());

    //get divergence
    double kld = Divergence::KullbackLeibler<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
    //double kld = Divergence::JensenShannon<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);

    //plotti
    //plot.debugDistribution1(samplesWifi);
    //plot.debugDistribution1(samplesParticles);


    //estimate the mean
//    SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
//    const MyState estWifi = estimateWifi.estimate(samplesWifi);
//    plot.addEstimationNodeSmoothed(estWifi.position.inMeter());

    return kld;
}


static double kldFromMultivariatNormal(std::vector<SMC::Particle<MyState>>& particles, MyState state, std::vector<SMC::Particle<MyState>>& particleWifi){
    //kld: particle die resampling hatten nehmen und nv daraus schätzen. vergleiche mit wi-fi
    //todo put this in depletionhelper.h

    Point3 estPos = state.position.inMeter();

    //this is a hack! it is possible that the sigma of z is getting 0 and therefore the rank decreases to 2 and
    //no inverse matrix is possible
    std::mt19937_64 rng;
    // initialize the random number generator with time-dependent seed
    uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
    std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed>>32)};
    rng.seed(ss);
    // initialize a uniform distribution between -0.0001 and 0.0001
    std::uniform_real_distribution<double> unif(-0.0001, 0.0001);

    //create a gauss dist for the current particle approx.
    Eigen::MatrixXd m(particles.size(), 3);
    for(int i = 0; i < particles.size(); ++i){
        m(i,0) = (particles[i].state.position.x_cm / 100.0) + unif(rng);
        m(i,1) = (particles[i].state.position.y_cm / 100.0) + unif(rng);
        m(i,2) = (particles[i].state.position.z_cm / 100.0) + unif(rng);
    }

    Eigen::VectorXd mean(3);
    mean << estPos.x, estPos.y, estPos.z;

    Distribution::NormalDistributionN normParticle = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(m, mean);

    //create a gauss dist for wifi
    Eigen::MatrixXd covWifi(3,3);
    covWifi << Settings::WiFiModel::sigma, 0, 0,
    0, Settings::WiFiModel::sigma, 0,
    0, 0, 0.01;

    //estimate the mean
    SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
    const MyState estWifi = estimateWifi.estimate(particleWifi);

    Eigen::VectorXd meanWifi(3);
    meanWifi << estWifi.position.x_cm / 100.0, estWifi.position.y_cm / 100.0, estWifi.position.z_cm / 100.0;
    Distribution::NormalDistributionN normWifi(meanWifi, covWifi);

    //get the kld distance
    double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);

    //plot.debugDistribution1(particleWifi);

    //plot.drawNormalN1(normParticle);
    //plot.drawNormalN2(normWifi);

    //plot.addEstimationNodeSmoothed(estWifi.position.inMeter());

    return kld;
}



#endif // KLB_H