#ifndef TURNEVALUATION_H
#define TURNEVALUATION_H


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

static double sigma_heading = 35;

class TurnEvaluation {

    //All calculations use degree not rad!!!

public:

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

        //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;
        }

        //use the von Mises distribution
        else {
            //Here some calculations must be done in rad

            double delta_heading_obs_rad = obs->delta_heading * 3.14159265359 / 180.0;
            double delta_motion_rad = obs -> delta_motion * 3.14159265359 / 180.0;

            //Equation for estimating kappa value of von Mises distribution
            //empirically estimated
            double kappa = 0.0;

            kappa = 5.0 / exp(2 * delta_motion_rad);

            double delta_heading_particle_rad = delta_heading_particle * 3.14159265359 / 180.0;



            //pdf von mises distribution (http://en.wikipedia.org/wiki/Von_Mises_distribution)
            const double p = exp(kappa * cos(delta_heading_obs_rad - delta_heading_particle_rad)) / (2.0 * 3.14159265359 * boost::math::cyl_bessel_i(0, kappa));

            return p;

        }

    }

};



#endif // TURNEVALUATION_H