#ifndef TURNEVALUATION_H
#define TURNEVALUATION_H


#include "../particles/MyState.h"
#include "TurnObservation.h"
//#include <boost/math/special_functions/bessel.hpp>
#include <math.h>

//static constexpr double sigma_heading = 5;

class TurnEvaluation {

    //All calculations use degree not rad!!!

public:

    double getProbability(const MyState& state, const TurnObservation* obs, bool simple = false) const {

		return 1;

		// get the current heading-change
		const float delta = Angle::radToDeg(state.walkState.headingChange_rad);

		// and reset it as it was evaluated
		((MyState&) state).walkState.headingChange_rad = 0;

		// proability?
		const float sigma = 15.0;
		const double prob = K::NormalDistribution::getProbability(obs->delta_heading, sigma, delta);

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

		return prob;



//        //Particle's heading change
//        double delta_heading_particle = state.heading - state.heading_old;


//        //Correct offset of the heading change
//        if (delta_heading_particle < -180) {
//            delta_heading_particle += 360;
//        }
//        else if (delta_heading_particle > 180) {
//            delta_heading_particle -= 360;
//        }


//        //Switch between simple and improved evaluation
//        //"Simple" only evaluates the deviation between the measured heading and the particle heading change using
//        //normal distribution
//		//if(simple) {

//			double sigma_delta_heading = sigma_heading;

//            const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle);


//            return p;
//	   // }

    }

};



#endif // TURNEVALUATION_H