#ifndef NAVMESHWALKWIFI_H
#define NAVMESHWALKWIFI_H

#include <vector>

#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../../geo/Heading.h"

#include "../../sensors/radio/WiFiMeasurements.h"
#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>

#include "NavMeshSub.h"
#include "NavMeshWalkParams.h"
#include "NavMeshWalkEval.h"

namespace NM {

    /** simple walker extended by a wifi anti impoverishment method
     * draw 10000 random particles within the building equaly
     * evaluate them with wifi and then draw cumulative from a list
     * instead of killing the particles who walk against walls
     * the number of new particles is restricted by ??? */
    template <typename Tria> class NavMeshWalkWifi {

    public:
        /** single result */
        struct ResultEntry {
            NavMeshLocation<Tria> location;
            Heading heading;
            double probability;
            ResultEntry() : heading(0) {;}
        };

    private:

        const NavMesh<Tria>& mesh;

        std::vector<NavMeshWalkEval<Tria>*> evals;

        std::vector<ResultEntry> wifiSamples;
        WiFiModel& wifiModel;
        WiFiObserverFree wifiProbability;
        double wifiCumWeight;
        DrawList<NavMeshLocation<Tria>> wifiSamplesDrawList;

        int hits = 0;
        int misses = 0;

    public:

        /** list of results */
        using ResultList = std::vector<ResultEntry>;

        /** ctor */
        NavMeshWalkWifi(const NavMesh<Tria>& mesh, WiFiModel& wifiModel) : mesh(mesh),
            wifiModel(wifiModel),
            wifiProbability(Settings::WiFiModel::sigma, wifiModel){

        }

        /** add a new evaluator to the walker */
        void addEvaluator(NavMeshWalkEval<Tria>* eval) {
            this->evals.push_back(eval);
        }

        /** update every transition step the WiFi */
        void updateWiFi(const WiFiMeasurements& wifiObs, Timestamp currentTime){

            this->wifiSamplesDrawList.reset();

            //todo: restrict this to specific region
            NavMeshRandom<Tria> rnd = mesh.getRandom();
            wifiCumWeight = 0;
            for(int i = 0; i < 10000; ++i){

                NavMeshLocation<Tria> tmpLocation = rnd.draw();
                double weight = wifiProbability.getProbability(tmpLocation.pos, currentTime, wifiObs);

                this->wifiSamplesDrawList.add(tmpLocation, weight);
            }
        }

        ResultEntry getOne(const NavMeshWalkParams<Tria>& params) {

            // sanity checks
            params.check();

            ResultEntry re;

            // to-be-walked distance;
            const float toBeWalkedDist = params.getToBeWalkedDistance();
            const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;

            // construct reachable region
            NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);

            // get the to-be-reached destination's position (using start+distance+heading)
            const Point2 dir = params.heading.asVector();
            const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);

            const Tria* dstTria = reachable.getContainingTriangle(dst);

            // is above destination reachable?
            if (dstTria) {

                re.heading = params.heading;										// heading was OK -> keep
                re.location.pos = dstTria->toPoint3(dst);							// new destination position
                re.location.tria = dstTria;											// new destination triangle
                re.probability = 1.0;
                ++hits;

            } else {
                re.location = wifiSamplesDrawList.get();
                re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());
                re.probability = 0.1;
                ++misses;
            }

            const int total = (hits + misses);
            if (total % 10000 == 0) {
                //std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
            }

            // calculate probability
            /*
            const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
            re.probability = 1.0;
            for (const NavMeshWalkEval<Tria>* eval : evals) {
                const double p1 = eval->getProbability(pwalk);
                re.probability *= p1;
            }
            */

            // done
            return re;

        }

        ResultList getMany(const NavMeshWalkParams<Tria>& params) {

            // sanity checks
            params.check();

            return {getOne(params)};
        }
    };

}

#endif // NAVMESHWALKWIFI_H