#ifndef STEPEVALUATION_H
#define STEPEVALUATION_H



#include "../particles/MyState.h"
#include "StepObservation.h"
#include <math.h>

static constexpr double mu_walk = 90;
static constexpr double sigma_walk = 30;
static constexpr double mu_stop = 0;
static constexpr double sigma_stop = 5;

class StepEvaluation {

public:

    double getProbability(const MyState& state, const StepObservation* obs) const {

		return 1;

		const float mdlWalked_m = state.walkState.distanceWalked_m;

		((MyState&)state).walkState.distanceWalked_m = 0;

		const float stepSize_m = 0.71;
		const float sensSigma_m = 0.05 + (0.05 * obs->steps);
		const float sensWalked_m = obs->steps * stepSize_m;

		if (obs->steps > 1) {
			int i = 0;
			int j = i+1; ++j;
		}

		const double prob = K::NormalDistribution::getProbability(sensWalked_m, sensSigma_m, mdlWalked_m);

		if (prob != prob) {
			throw 1;
		}

		return prob;

//		float a = 1.0;
//	   float mu_distance = 0;
//		float sigma_distance = 0;

//		if(obs->step) {
//			a = 1.0;
//			mu_distance = mu_walk;
//			sigma_distance = sigma_walk;
//		}

//		else {
//			a = 0.0;
//			mu_distance = mu_stop;
//			sigma_distance = sigma_stop;
//		}

//		//Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step
//		const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) +
//						(1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance);

//		return p;

    }

};



#endif // STEPEVALUATION_H