#ifndef MYEVALUATION_H
#define MYEVALUATION_H

#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
#include "MyObservation.h"
#include "MyState.h"

#include "../frank/WiFiEvaluation.h"
#include "../frank/BeaconEvaluation.h"
#include "../toni/BarometerEvaluation.h"
#include "../lukas/StepEvaluation.h"
#include "../lukas/TurnEvaluation.h"

#include <atomic>

class MyEvaluation : public K::ParticleFilterEvaluation<MyState, MyObservation> {

private:

	WiFiEvaluation wifiEval;
	BeaconEvaluation beaconEval;
	BarometerEvaluation barometerEval;
	StepEvaluation stepEval;
	TurnEvaluation turnEval;

	bool useWifi = true;
	bool useStep = true;
	bool useTurn = true;
	bool useBaro = true;
	bool useIB = true;

public:

	void setUsage(bool useWifi, bool useStep, bool useTurn, bool useBaro, bool useIB) {
		this->useWifi = useWifi;
		this->useStep = useStep;
		this->useTurn = useTurn;
		this->useBaro = useBaro;
		this->useIB = useIB;
	}

	virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& observation) override {

		//if (observation.wifi) {
		wifiEval.nextObservation(observation.wifi);
		//}

		// evalulate each particle
		double sum = 0;
		for (K::Particle<MyState>& p : particles) {

//		std::atomic<double> sum(0);
//		#pragma omp parallel for
//		for (int i = 0; i < (int) particles.size(); ++i) {
//			K::Particle<MyState>& p = particles[i];

			double weight = 1.0;

			if (useWifi) {
				weight *= wifiEval.getProbability(p.state, observation);
			}

			if (useBaro && observation.barometer) {
				weight *= barometerEval.getProbability(p.state, observation.barometer);

                // set
                p.state.measurement_pressure = observation.barometer->hpa;
			}

			if (useIB) {
				weight *= beaconEval.getProbability(p.state, observation);
			}

			// CONTROL!
//			if (useStep) {
//				weight *= stepEval.getProbability(p.state, observation.step);
//			}

			// CONTROL!
//			if (useTurn) {
//				weight *= turnEval.getProbability(p.state, observation.turn, true);

//                //set
//                p.state.angularHeadingChange = observation.turn->delta_heading;
//			}

			// set and accumulate
            p.weight = weight;
			sum += p.weight;

		}

		// reset
		observation.step->steps = 0;
		observation.turn->delta_heading = 0;
		observation.turn->delta_motion = 0;

		return sum;

	}

};

#endif // MYEVALUATION_H