#ifdef WITH_TESTS
#ifdef WITH_EIGEN

#include "../../../../smc/merging/mixing/MixingSamplerDivergency.h"
#include "../../../../smc/filtering/ParticleFilterMixing.h"

#include "../../../Tests.h"

namespace K {

    struct MyState {
        double x;
        double y;
        MyState() : x(0), y(0) {;}
        MyState(double x, double y) : x(x), y(y) {;}
        MyState& operator += (const MyState& other) {
            x += other.x;
            y += other.y;
            return *this;
        }
        MyState& operator /= (double d) {
            x /= d;
            y /= d;
            return *this;
        }
        MyState operator * (double d) const {
            return MyState(x*d, y*d);
        }
        MyState& operator = (const MyState& other) {
            this->x = other.x;
            this->y = other.y;
            return *this;
        }
        double distance(const MyState& o) const {
            return std::sqrt( (x-o.x)*(x-o.x) + (y-o.y)*(y-o.y) ) / 4.9;
        }
        bool belongsToRegion(const MyState& o) const {
            return distance(o) <= 1.0;
        }

    };

    struct MyControl {

    };

    struct MyObservation {
        double x;
        double y;
        MyObservation() : x(0), y(0) {;}
        void set(double x, double y) {this->x = x; this->y = y;}
    };

    class MyInitializer1 : public SMC::ParticleFilterInitializer<MyState> {
        void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
            for (SMC::Particle<MyState>& p : particles) {
                p.state.x = 0;
                p.state.y = 0;
                p.weight = 1.0 / particles.size();
            }
        }
    };

    class MyInitializer2 : public SMC::ParticleFilterInitializer<MyState> {
        void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
            for (SMC::Particle<MyState>& p : particles) {
                p.state.x = 1;
                p.state.y = 1;
                p.weight = 1.0 / particles.size();
            }
        }
    };


    TEST(Mixing, standard) {

        //init particle filters
        int numParticles = 1000;
        Eigen::MatrixXd transition(2,2);
        transition << 0.8, 0.2, 0.2, 0.8;

        SMC::ParticleFilterMixing<MyState, MyControl, MyObservation> mode1(numParticles, std::unique_ptr<MyInitializer1>(new MyInitializer1()), 0.5);
        SMC::ParticleFilterMixing<MyState, MyControl, MyObservation> mode2(numParticles, std::unique_ptr<MyInitializer2>(new MyInitializer2()), 0.5);

        std::vector<SMC::ParticleFilterMixing<MyState, MyControl, MyObservation>> modes;
        modes.push_back(mode1);
        modes.push_back(mode2);


        SMC::MixingSamplerDivergency<MyState, MyControl, MyObservation> mixer;

        //run the mixing and check results
        for(int t = 0; t < 100; ++t){
            mixer.mixAndSample(modes, transition);

            int cnt_zero = 0;
            int cnt_ones = 0;
            for(int i = 0; i < modes[0].getParticles().size(); ++i){

                if(modes[0].getParticles()[i].state.x == 0){
                    cnt_zero++;
                }else{
                    cnt_ones++;
                }
            }

            std::cout << "MODE1 Zeros: " << (double) cnt_zero /  numParticles<< std::endl;
            std::cout << "MODE1 Ones: " << (double) cnt_ones/ numParticles << std::endl;


            cnt_zero = 0;
            cnt_ones = 0;
            for(int i = 0; i < modes[0].getParticles().size(); ++i){

                if(modes[1].getParticles()[i].state.x == 0){
                    cnt_zero++;
                }else{
                    cnt_ones++;
                }
            }

            std::cout << "MODE2 Zeros: " << (double) cnt_zero /  numParticles<< std::endl;
            std::cout << "MODE2 Ones: " << (double) cnt_ones/ numParticles << std::endl;
        }



    }

    TEST(Mixing, differentParticleSize) {

    }


    TEST(Mixing, dynamicTransitionMatrix) {

    }

    TEST(Mixing, ThreeFilters) {

    }

}


#endif
#endif