#include "Eval.h"

#include <array>
#include <vector>

#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;

    std::array<bool, 4> hadMeas = {false};

    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 = 8;

                if (ftmKalmanFilters != nullptr)
                {
                    Kalman& kalman = ftmKalmanFilters->at(mac);
                    ftmDist = kalman.predictAndUpdate(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;
                }

                // hadMeas[nucIndex] = true; TODO
            }
        }
        else
        {
            // RSSI
            double sigma = 8;

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

    // Use kalman to predict missing measurments
    //if (UseSensor == SensorMode::FTM && ftmKalmanFilters != nullptr)
    //{
    //    for (size_t i = 0; i < 4; i++)
    //    {
    //        if (!hadMeas[i])
    //        {
    //            double sigma = 5;

    //            Kalman& kalman = ftmKalmanFilters->at(Settings::nucFromIndex(i));

    //            if (!isnan(kalman.lastTimestamp))
    //            {
    //                KalmanPrediction prediction = kalman.predict(currentTime);
    //                sigma = std::sqrt(prediction.P[0]);

    //                sigma = sigma > 0 ? sigma : 5;

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

    //                float ftmDist = prediction.distance;
    //                double x = Distribution::Normal<double>::getProbability(ftmDist, sigma, apDist);

    //                if (x > 1e-80)
    //                {
    //                    Assert::isNot0(x, "");

    //                    volatile double oldResult = result;

    //                    result *= x;


    //                    Assert::isNot0(result, "");

    //                    printf("");
    //                }

    //            }
    //        }
    //    }
    //}

    return result;
}