#include "Eval.h"

#include "Settings.h"

#include <Indoor/math/distribution/Normal.h>

double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3& particlePos, const std::vector<WiFiMeasurement>& measurements, std::shared_ptr<std::unordered_map<MACAddress, Kalman>> ftmKalmanFilters)
{
    double result = 1.0;

    for (WiFiMeasurement wifi : measurements)
    {
        if (wifi.getNumSuccessfulMeasurements() < 3)
        {
            continue;
        }

        const MACAddress& mac = wifi.getAP().getMAC();
        int nucIndex = Settings::nucIndex(mac);

        const Point3 apPos = Settings::CurrentPath.nucInfo(nucIndex).position;
        // particlePos.z = 1.3; // smartphone höhe
        const float apDist = particlePos.getDistance(apPos);

        // compute ftm distance
        float ftm_offset = Settings::CurrentPath.NUCs.at(mac).ftm_offset;
        float ftmDist = wifi.getFtmDist() + ftm_offset; // in m; plus static offset

        float rssi_pathloss = Settings::CurrentPath.NUCs.at(mac).rssi_pathloss;
        float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI());

        if (UseSensor == SensorMode::FTM)
        {
            if (ftmDist > 0)
            {
                //double sigma = wifi.getFtmDistStd()*wifi.getFtmDistStd(); // 3.5; // TODO
                double sigma = 5;

                if (ftmKalmanFilters != nullptr)
                {
                    Kalman& kalman = ftmKalmanFilters->at(mac);
                    ftmDist = kalman.predict(currentTime, ftmDist);
                    //sigma = std::sqrt(kalman.P(0, 0));

                    Assert::isTrue(sigma > 0, "sigma");

                    double x = Distribution::Normal<double>::getProbability(ftmDist, sigma, apDist);

                    result *= x;
                }
                else
                {
                    double x = Distribution::Normal<double>::getProbability(ftmDist, sigma, apDist);

                    result *= x;
                }
            }
        }
        else
        {
            // RSSI
            double sigma = 5;

            double x = Distribution::Normal<double>::getProbability(rssiDist, sigma, apDist);
            result *= x;
        }
    }

    return result;
}