/*
 * © Copyright 2014 – Urheberrechtshinweis
 * Alle Rechte vorbehalten / All Rights Reserved
 *
 * Programmcode ist urheberrechtlich geschuetzt.
 * Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
 * Keine Verwendung ohne explizite Genehmigung.
 * (vgl. § 106 ff UrhG / § 97 UrhG)
 */

#ifndef WIFI_OPTIMIZER_LOG_DIST_CEILING_H
#define WIFI_OPTIMIZER_LOG_DIST_CEILING_H

#include "../../../floorplan/v2/Floorplan.h"
#include "../../../floorplan/v2/FloorplanHelper.h"

#include "../../../geo/BBox3.h"
#include "../../../misc/Debug.h"

#include "WiFiFingerprint.h"
#include "WiFiFingerprints.h"

#include "../model/WiFiModels.h"
//#include "../model/WiFiModelLogDistCeiling.h"

#include <KLib/math/optimization/NumOptAlgoDownhillSimplex.h>
#include <KLib/math/optimization/NumOptAlgoGenetic.h>
#include <KLib/math/optimization/NumOptAlgoRangeRandom.h>

#include <string>
#include <sstream>

#include "WiFiOptimizer.h"
#include "WiFiOptimizerStructs.h"

#include <functional>

namespace WiFiOptimizer {

    /**
     * optimize access-point parameters,
     * given several fingerprints using the log-dist-ceiling model
     */
    struct LogDistCeiling : public Base {


    public:

        /**
         * resulting optimization stats for one AP
         */
        struct Stats {

            /** average model<->scan error after optimzing */
            float error_db;

            /** number of fingerprints [= locations] that were used for optimzing */
            int usedFingerprins;

            /** resulting model<->scan error after optimzing for each individual fingerprints [= location] */
            std::vector<ErrorAtPosition> errors;

            /** get the location where the model estimation reaches the highest negative value [model estimation too low] */
            ErrorAtPosition getEstErrorMaxNeg() const {
                auto cmpErrAtPos = [] (const ErrorAtPosition& a, const ErrorAtPosition& b) {return a.getError_db() < b.getError_db();};
                return *std::min_element(errors.begin(), errors.end(), cmpErrAtPos);
            }

            /** get the location where the model estimation reaches the highest positive value [model estimation too high] */
            ErrorAtPosition getEstErrorMaxPos() const {
                auto cmpErrAtPos = [] (const ErrorAtPosition& a, const ErrorAtPosition& b) {return a.getError_db() < b.getError_db();};
                return *std::max_element(errors.begin(), errors.end(), cmpErrAtPos);
            }

        };

        using APFilter = std::function<bool(const Stats& stats, const MACAddress& mac)>;

        static inline bool NONE(const Stats& stats, const MACAddress& mac) {
            (void) stats; (void) mac;
            return false;
        }

        static inline bool  MIN_2_FPS(const Stats& stats, const MACAddress& mac) {
            (void) mac;
            return stats.usedFingerprins < 2;
        }

        static inline bool MIN_5_FPS(const Stats& stats, const MACAddress& mac) {
            (void) mac;
            return stats.usedFingerprins < 5;
        }

        static inline bool MIN_10_FPS(const Stats& stats, const MACAddress& mac) {
            (void) mac;
            return stats.usedFingerprins < 10;
        }



        /** parameters for one AP when using the LogDistCeiling model */
        struct APParams {

            float x;
            float y;
            float z;

            float txp;
            float exp;
            float waf;

            /** ctor */
            APParams() {;}

            /** ctor */
            APParams(float x, float y, float z, float txp, float exp, float waf) : x(x), y(y), z(z), txp(txp), exp(exp), waf(waf) {;}

            Point3 getPos() const {return Point3(x,y,z);}

            std::string asString() const {
                std::stringstream ss;
                ss << "Pos:" << getPos().asString() << " TXP:" << txp << " EXP:" << exp << " WAF:" << waf;
                return ss.str();
            }

            /** we add some constraints to the parameter range */
            bool outOfRange() const {
                return  (waf > 0) ||
                        (txp < -65) ||
                        (txp > -45) ||
                        (exp > 5) ||
                        (exp < 1);
            }

        };

        /** add MAC-info to params */
        struct APParamsMAC {
            MACAddress mac;
            APParams params;
            APParamsMAC(const MACAddress mac, const APParams& params) : mac(mac), params(params) {;}
        };

        struct OptResultStats {
            K::Statistics<float> avgApErrors;		// contains one average-error per optimized AP
            K::Statistics<float> singleFPErrors;	// contains one error for each fingerprint<->ap SIGNED
            K::Statistics<float> singleFPErrorsAbs;	// contains one error for each fingerprint<->ap ABSOLUTE

        };

        class APParamsList {

            std::vector<APParamsMAC> lst;

        public:

            /** ctor */
            APParamsList(const std::vector<APParamsMAC>& lst) : lst(lst) {

            }

            /** get the list */
            const std::vector<APParamsMAC>& get() const {
                return lst;
            }

            /** get params for the given mac [if known, otherwise nullptr] */
            const APParamsMAC* get (const MACAddress& mac) const {
                for (const APParamsMAC& ap : lst) {
                    if (ap.mac == mac) {return &ap;}
                }
                return nullptr;
            }

        };

    private:

        Floorplan::IndoorMap* map;

        Mode mode = Mode::QUALITY;

        const char* name = "WiFiOptLDC";

    public:

        /** ctor */
        LogDistCeiling(Floorplan::IndoorMap* map, const VAPGrouper& vg, const Mode mode = Mode::MEDIUM) : Base(vg), map(map), mode(mode) {
            ;
        }

        /** ctor */
        LogDistCeiling(Floorplan::IndoorMap* map, const VAPGrouper& vg, const WiFiFingerprints& fps, const Mode mode = Mode::MEDIUM) : Base(vg), map(map), mode(mode) {
            addFingerprints(fps);
        }

        /** optimize all known APs */
        APParamsList optimizeAll(APFilter filter, OptResultStats* dst = nullptr) const {

            // sanity check
            Assert::isFalse(getAllMACs().empty(), "no APs found for optimization! call addFingerprint() first!");

            K::Statistics<float> avgErrors;
            K::Statistics<float> singleErrors;
            K::Statistics<float> singleErrorsAbs;

            std::vector<APParamsMAC> res;
            for (const MACAddress& mac : getAllMACs()) {

                // perform optimization, get resulting parameters and optimization stats
                Stats stats;
                const APParams params = optimize(mac, stats);

                // filter based on stats (option to ignore/filter some access-points)
                if (!filter(stats, mac)) {
                    res.push_back(APParamsMAC(mac, params));
                    //errSum += stats.error_db;
                    //++errCnt;
                    avgErrors.add(stats.error_db);
                    for (const auto e : stats.errors) {
                        singleErrors.add(e.getError_db());
                        singleErrorsAbs.add(std::abs(e.getError_db()));
                    }
                } else {
                    Log::add(name, "ignoring opt-result for AP " + mac.asString() + " due to filter");
                    //std::cout << "ignored due to filter!" << std::endl;
                }

            }

            //const float avgErr = errSum / errCnt;
            //Log::add(name, "optimized APs: " + std::to_string(errCnt));
            //Log::add(name, "average AP error is: " + std::to_string(avgErr) + " dB");
            Log::add(name, "optimization result: ");
            Log::add(name, " - AvgPerAP   " + avgErrors.asString());
            Log::add(name, " - Single:    " + singleErrors.asString());
            Log::add(name, " - SingleAbs: " + singleErrorsAbs.asString());

            if (dst) {
                dst->avgApErrors = avgErrors;
                dst->singleFPErrors = singleErrors;
                dst->singleFPErrorsAbs = singleErrorsAbs;
            }

            // done
            return APParamsList(res);

        }


        /** optimize the given AP */
        APParams optimize(const MACAddress& mac, Stats& res) const {

            // starting parameters do not matter for the current optimizer!
            APParams params(0,0,0, -59, 3, -8.0);

            // get all position->rssi measurements for this AP to compare them with the corresponding model estimations
            const std::vector<RSSIatPosition>& entries = apMap.find(mac)->second;

            // log
            Log::add(name, "optimizing parameters for AP " + mac.asString() + " by using " + std::to_string(entries.size()) + " fingerprints", false);
            Log::tick();

            // get the map's size
            const BBox3 mapBBox = FloorplanHelper::getBBox(map);

            using LeOpt = K::NumOptAlgoRangeRandom<float>;
            const std::vector<LeOpt::MinMax> valRegion = {
                 LeOpt::MinMax(mapBBox.getMin().x - 20, mapBBox.getMax().x + 20),	// x
                 LeOpt::MinMax(mapBBox.getMin().y - 20, mapBBox.getMax().y + 20),	// y
                 LeOpt::MinMax(mapBBox.getMin().z -  6, mapBBox.getMax().z +  6),	// z
                 LeOpt::MinMax(-65, -45),		// txp
                 LeOpt::MinMax(1, 5),			// exp
                 LeOpt::MinMax(-15, -0),		// waf
             };




            LeOpt opt(valRegion);

            switch(mode) {
            case Mode::FAST:
                opt.setPopulationSize(100);
                opt.setNumIerations(50);
                break;
            case Mode::MEDIUM:
                opt.setPopulationSize(200);
                opt.setNumIerations(100);
                break;
            case Mode::QUALITY:
                opt.setPopulationSize(1500);
                opt.setNumIerations(150);
                break;
            }

            // error function
            auto func = [&] (const float* params) {
                return getErrorLogDistCeiling(mac, entries, params, nullptr);
            };

            opt.calculateOptimum(func, (float*) &params);

    //		using LeOpt = K::NumOptAlgoGenetic<float>;
    //		LeOpt opt(6);
    //		opt.setPopulationSize(750);
    //		opt.setMaxIterations(50);
    //		opt.setElitism(0.05f);
    //		opt.setMutation(0.75f);
    //		//opt.setValRange({0.5, 0.5, 0.5, 0.1, 0.1, 0.1});
    //		opt.setValRegion(valRegion);

    //		K::NumOptAlgoDownhillSimplex<float, 6> opt;
    //		opt.setMaxIterations(100);
    //		opt.setNumRestarts(10);

            opt.calculateOptimum(func, (float*) &params);
            res.error_db = getErrorLogDistCeiling(mac, entries, (float*)&params, &res);
            res.usedFingerprins = entries.size();

            Log::tock();
            Log::add(name, mac.asString() + ": " + params.asString() + " @ " + std::to_string(res.error_db) +" dB err");

            return params;

        }


    private:

        float getErrorLogDistCeiling(const MACAddress& mac, const std::vector<RSSIatPosition>& entries, const float* data, Stats* stats = nullptr) const {

            const APParams* params = (APParams*) data;

            // some sanity checks
            if (params->outOfRange()) {return 1e10;}

            // current position guess for the AP;
            const Point3 apPos_m = params->getPos();

            // add the AP [described by the current guess] to the signal-strength-prediction model
            // signal-strength-prediction-model...
            WiFiModelLogDistCeiling model(map);
            model.addAP(mac, WiFiModelLogDistCeiling::APEntry(apPos_m, params->txp, params->exp, params->waf));

            float err = 0;
            int cnt = 0;

            // process each measurement
            for (const RSSIatPosition& reading : entries) {

                // get the model-estimation for the fingerprint's position
                const float rssiModel = model.getRSSI(mac, reading.pos_m);

                // difference between estimation and measurement
                const float diff = std::abs(rssiModel - reading.rssi);

                // add error to stats object?
                if (stats) {
                    stats->errors.push_back(ErrorAtPosition(reading.pos_m, reading.rssi, rssiModel));
                }

                // adjust the error
                err += std::pow(std::abs(diff), 2.0);
                ++cnt;

                // max distance penality
                // [unlikely to get a reading for this AP here!]
                if (apPos_m.getDistance(reading.pos_m) > 150) {err += 999999;}

            }

            err /= cnt;
            err = std::sqrt(err);

            if (params->txp < -65) {err += 999999;}
            if (params->txp > -45) {err += 999999;}

            if (params->exp > 5) {err += 999999;}
            if (params->exp < 1.0) {err += 999999;}

            return err;

        }



    };

}


#endif // WIFI_OPTIMIZER_LOG_DIST_CEILING_H