#ifndef algorithm_h
#define algorithm_h

#include <iostream>
#include <vector>

#include "CoordSys.h"
#include "FileSys.h"
#include "MapConfig.h"
#include "SdTypes.h"

namespace jf {

    class BindRoad;
    typedef std::shared_ptr<BindRoad> ShPtr_BindRoad;

/**
 * @brief 绑路算法
 *
 */
    class BindRoad {
    public:
        static ShPtr_BindRoad GetInstance();
        BindRoad();

        /**
         * @brief 算法需要的data
         *
         */
        struct AlgoNodeData {
            /**
             * @brief 节点
             *
             */
            const SdNode* n = nullptr;
            /**
             * @brief 观察概率
             *
             */
            double obsProb = 0.0;
            /**
             * @brief 转移概率
             *
             */
            double transProb = 0.0;
            /**
             * @brief 累计概率
             *
             */
            double accProb = 0.0;

            AlgoNodeData() : n(nullptr), obsProb(0), transProb(0), accProb(0) {}

            AlgoNodeData(const SdNode* inn) : n(inn), obsProb(1), transProb(1), accProb(1) {}
            // algoNodeData(const algoNodeData other) : n(other.n), obsProb(other.obsProb), transProb(other.transProb), accProb(other.accProb) {}
            AlgoNodeData(const AlgoNodeData& other) : n(other.n), obsProb(other.obsProb), transProb(other.transProb), accProb(other.accProb) {}
            AlgoNodeData& operator=(const AlgoNodeData& other) {
                n = other.n;
                obsProb = other.obsProb;
                transProb = other.transProb;
                accProb = other.accProb;
                return *this;
            }

            inline bool operator==(const AlgoNodeData& o) const { return (*n) == (*o.n); }

            inline bool operator!=(const AlgoNodeData& o) const { return !((*n) == (*o.n)); }
        };

        /**
         * @brief 临时绑到最近点
         *
         * @param p gps点位置
         * @param w 世界指针
         * @return std::pair<nodeId, nodeData>
         */
        void BindRoadLnn(const BindRoadInput& in, const SdNode* out);
        /**
         * @brief 算法
         *
         * @param p 本次gps点
         * @param w 世界指针
         * @return std::pair<nodeId, nodeData>
         */
        void BindRoadHmm(const BindRoadInput& in, BindRoadOutput& out);

    private:
        static ShPtr_BindRoad m_spInstance;

        int execNum;
        /**
         * @brief 第一次执行算法
         *
         */
        bool first;
        bool pending;

        // std::vector<Point> gpses;
        // std::vector<algoNodeData> candidates_;
        int condidateNum;

        bool CalcTransformProb(std::vector<AlgoNodeData>& punishCandidates, bool first);
        bool InitLastBest();
        void GetNextNNodes(int n, const SdNode* from, std::set<AlgoNodeData>& ret);
        double GetTransProb(const SdNode* n, const BindRoadInput& in);
        uint64_t GetRid(const SdNode* nd);
        // 地图有个特点，在路口会把很多点捏成一个点，所有很多点的经纬度完全相同，会导致在这些点之前跳跃
        // 为了防止这种事情发生，设置这个变量，如果上一个点和当前点的经纬度相同，则用上一次输出
        AlgoNodeData last_best;
    };

    inline bool operator<(const BindRoad::AlgoNodeData& l, const BindRoad::AlgoNodeData& r) { return l.n < r.n; }

// 降低观察概率的权重
    inline double CalcObsProb(double in) { return 1 / pow(in, 1.0 / 3); }
}  // namespace jf

#endif