#pragma once

#include "../particles/MyState.h"
#include "BarometerObservation.h"
#include "barometric.h"
#include <KLib/math/distribution/Normal.h>

double g_BarometerObservation = 0.0;

class BarometerEvaluation {

public:

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

        //rho_z
        double barometerSigma = 0.3;

        //The height of the single floor levels.
        const static double floor_height[3] = {4.1, 3.4, 3.4};

        if(USE_BAROMETRIC_FORMULAR){
            //height the particle has climbed.
            double h_1 = 0.0;
            for(int i = std::min(state.z_nr_old, state.z_nr); i < std::max(state.z_nr_old, state.z_nr); i++){
               h_1 += floor_height[i];
            }

            if(h_1 != 0.0){
                // use the barometric formular to calculate the relative pressure
                // the calculation is done assuming sea level height at every floor.
                double mslp = BarometricFormular::s_getSeaLevelPressure();
                double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
                barometerSigma = std::abs(mslp - pressure);
            }

        }
        else {
            // constant value for sigma if we assume all floors are same in height
            barometerSigma = 0.30 / 1.0; //hPa
        }

        // evaluate the current particle with a normal distribution
		const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma/2, obs->hpa);

        //Just for the visualization. i'm a lazy bastard
        g_BarometerObservation = obs->hpa;

        assert(barometerProbability == barometerProbability);
        assert(state.hPa == state.hPa);
        assert(obs->hpa == obs->hpa);

        //std::cout << barometerProbability << std::endl;

        return  pow(2.0, barometerProbability);
        //return  barometerProbability;
    }
};