
#include "BindRoad.h"

//#include <algorithm>
//#include <limits>
//#include <map>
//
//#include "log.h"
//#include "Profiler.h"
//#include "Traversal.h"
//#include "Utils.h"
//#include "World.h"

using namespace jf;

ShPtr_BindRoad BindRoad::m_spInstance = {};
ShPtr_BindRoad BindRoad::GetInstance() {
    if (nullptr == m_spInstance.get()) {
        ShPtr_BindRoad temp(new BindRoad());
        m_spInstance.swap(temp);
    }
    return m_spInstance;
}

BindRoad::BindRoad() : first(true), pending(true), condidateNum(MapConfig::algorithm_condidate_max), execNum(0) {}

void BindRoad::BindRoadLnn(const BindRoadInput &in, const SdNode *out) {
    //Rect2D r = P2R(in.ingps, 45, 45);
    //std::set<const SdNode *> ps = SdWorld::GetSingleton()->GetNodesInRect(r);
    //double min = std::numeric_limits<double>::max();
    //SdNodeId minNid = {};
    //for (const SdNode *p : ps) {
    //    double dist = DXYZ(P2XYZ(p->id_p.p), P2XYZ(in.ingps));
    //    if (dist < min) {
    //        min = dist;
    //        out = p;
    //    }
    //}
}

void BindRoad::BindRoadHmm(const BindRoadInput &in, BindRoadOutput &out) {
//    static std::vector<AlgoNodeData> candidates = {};
//    AlgoNodeData a = {};
//    if (candidates.empty()) {
//        first = true;
//    } else {
//        AlgoNodeData a = candidates[0];
//    }
//
//    const Point &ingps = in.ingps;
//    // jf_log_info("[algorithm::BindRoadHmm] exec Num: {}", execNum++);
//    Rect2D r = P2R(ingps, 45, 45);
//    SdWorld::GetSingleton()->LoadTileInRect(r);
//
//    // 第一次
//    if (first) {
//        // 找到附近点，作为候选点
//        std::set<const SdNode *> ps = SdWorld::GetSingleton()->GetNodesInRect(r);
//
//        // 将区域内所有点放入候选集
//        candidates.resize(ps.size());
//        std::transform(ps.begin(), ps.end(), candidates.begin(), [&ingps, &in, this](const SdNode *n) {
//            AlgoNodeData nd(n);
//            double dist = DXYZ(P2XYZ(n->id_p.p), P2XYZ(ingps));
//            nd.obsProb = CalcObsProb(dist);
//            nd.transProb = GetTransProb(n, in);
//            // 控制累积概率小于等于1
//            nd.accProb = nd.obsProb > 1 ? 1 : nd.obsProb * nd.transProb;
//            return nd;
//        });
//        // 按照距离输入点的距离排序
//        std::sort(candidates.begin(), candidates.end(), [&ingps, this](const AlgoNodeData &l, const AlgoNodeData &r) {
//            XYZ ingps_xyz = P2XYZ(ingps);
//            return DXYZ(P2XYZ(l.n->id_p.p), ingps_xyz) < DXYZ(P2XYZ(r.n->id_p.p), ingps_xyz);
//        });
//
//        // 如果最开始没有任何候选点说明加载附近区域的范围小了
//        if (candidates.size() == 0) {
//            out.e = BindRoadOutput::error::no_candidates_in_init;
//            // jf_log_error("[algorithm::BindRoadHmm] no candidates in init \n gps : {0:.15f},{1:.15f} \n TileId {2} {3}", ingps.lon, ingps.lat, P2TId(ingps).lonId, P2TId(ingps).latId);
//            first = true;
//            pending = true;
//            return;
//        }
//        pending = true;
//    }
//        // 非第一次
//    else {
//        // 新候选点包括旧候选点
//        std::set<AlgoNodeData> newCandidates_set(candidates.begin(), candidates.end());
//
//        // 选新点策略：与泰志的策略不同
//        // 泰志的策略是：从所有候选点开始往下缕，缕的距离在gps移动距离的[0.85, 1.15]倍距离之间的点被选中，gps的移动距离指当前gps与上一次概率最大的点的距离。
//        // 这里的策略是：从所有候选点开始往下缕1个点
//        // 所有候选点往下缕config::algorithm_path_len个点，1.用于保证连通性 ////2.用于计算转移概率
//        for (AlgoNodeData &candidate : candidates) {
//            std::set<AlgoNodeData> ret;
//            GetNextNNodes(MapConfig::algorithm_path_len, candidate.n, ret);
//            std::set_union(newCandidates_set.begin(), newCandidates_set.end(), ret.begin(), ret.end(), std::inserter(newCandidates_set, newCandidates_set.end()));
//        }
//        // 选择与当前gps点附近的候选点，其他所有的候选点排除
//        // 插值之后就可以用了
//        Rect2D r = P2R(ingps, 90, 90);
//        const std::set<const SdNode *> ps = SdWorld::GetSingleton()->GetNodesInRect(r);
//
//        jf::Utils::EraseIf(newCandidates_set, [&ps](const AlgoNodeData &d) { return !Utils::Contains(ps, d.n); });
//
//        std::vector<AlgoNodeData> newCandidates_vector(newCandidates_set.begin(), newCandidates_set.end());
//
//        // 计算所有新候选点的观察概率
//        std::transform(newCandidates_vector.begin(), newCandidates_vector.end(), newCandidates_vector.begin(), [this, &ingps](AlgoNodeData &nd) {
//            double dist = DXYZ(P2XYZ(ingps), P2XYZ(nd.n->id_p.p));
//            nd.obsProb = CalcObsProb(dist);
//            return nd;
//        });
//
//        // 计算所有候选点的概率×所有新候选点的观察概率，选出新候选点的累计概率最大值作为新候选点概率，其中，新候选点累积概率=转移概率（暂时为1）×新候选点观测概率×累积概率（到达新候选点的候选点概率）
//        for (AlgoNodeData &newCandidate : newCandidates_vector) {
//            double max = 0;
//            newCandidate.transProb = GetTransProb(newCandidate.n, in);
//            for (AlgoNodeData &candidate : candidates) {
//                // 新候选点累积概率=转移概率（暂时为1）×新候选点观测概率×累积概率（到达新候选点的候选点概率）
//                double prob = newCandidate.transProb * newCandidate.obsProb * candidate.accProb;
//                if (prob > max) {
//                    max = prob;
//                }
//            }
//            // 控制累积概率小于等于1
//            newCandidate.accProb = max > 1 ? 1 : max;
//        }
//
//        candidates = newCandidates_vector;
//    }
//
//    // 所有candidates用accProb排序
//    std::sort(candidates.begin(), candidates.end(), [](const AlgoNodeData &l, const AlgoNodeData &r) { return l.accProb > r.accProb; });
//
//    // 选择概率最大的config::algorithm_condidate_max个候选
//    candidates.resize(std::min(MapConfig::algorithm_condidate_max, int(candidates.size())));
//
//    // 对所有acc归一化
//    // 归一化的时候把大于1的都变成1了，因为比如实际gps点离某Node很近，那么1/距离会很大
//    double s = std::accumulate(candidates.begin(), candidates.end(), 0.0, [](double acc, const AlgoNodeData &nd) { return acc + nd.accProb; });
//    std::transform(candidates.begin(), candidates.end(), candidates.begin(), [s](AlgoNodeData &nd) {
//        AlgoNodeData n(nd);
//        n.accProb = n.accProb / s;
//        return n;
//    });
//
//    if (candidates.size() > 0) {
//        std::vector<AlgoNodeData> punishCandidates = candidates;
//        bool succ = CalcTransformProb(punishCandidates, first);
//        if (!succ) {
//            // jf_log_error("[algorithm::BindRoadHmm] choose best failed");
//            out.e = BindRoadOutput::error::failed_after_punish;
//            first = true;
//            pending = true;
//        }
//
//        // 设置out
//
//        // 设置pending
//        // 第一次一定是pending
//        if (first) {
//            out.e = BindRoadOutput::pending_after_init;
//        } else {
//            // 非第一次
//            if (pending) {
//                std::set<AlgoNodeData> arrivable;
//                GetNextNNodes(5, last_best.n, arrivable);
//                if (last_best.n != punishCandidates[0].n && Utils::Contains(arrivable, AlgoNodeData(punishCandidates[0].n))) {
//                    pending = false;
//                    out.e = BindRoadOutput::no_error;
//                } else {
//                    out.e = BindRoadOutput::pending_after_init;
//                }
//            } else {
//                out.e = BindRoadOutput::error::no_error;
//            }
//        }
//
//        out.inp = ingps;
//        for (int i = 0; i < std::min(out.candidateSize, punishCandidates.size()); ++i) {
//            AlgoNodeData &nd = punishCandidates[i];
//            jf::BindRoadOutput::nodeInfo node = {GetRid(nd.n), nd.n->id_p.p, (float)nd.obsProb, (float)nd.transProb, (float)nd.accProb};
//            out.nodeInfos.push_back(node);
//        }
//
//        last_best = punishCandidates[0];
//        auto &best = last_best;
//
//        TileId tid = P2TId(ingps);
//        std::string strBestId = {};
//        // jf_log_info("-------------------------------------");
//        // jf_log_info("[algorithm::BindRoadHmm]");
//        // jf_log_info("result state : {0}", out.e);
//        // jf_log_info("in : {0:.15f},{1:.15f},{2:.15f}", ingps.lon, ingps.lat, ingps.alt);
//        // jf_log_info("out : {0:.15f},{1:.15f},{2:.15f}", best.n->id_p.p.lon, best.n->id_p.p.lat, best.n->id_p.p.alt);
//        // jf_log_info("nid:{0}", best.n->id_p.ToString());
//        // jf_log_info("level:{0}", best.n->level);
//        // jf_log_info("big:{0}", best.n->speed_limit_big);
//        // jf_log_info("small:{0}", best.n->speed_limit_small);
//        // jf_log_info("wid:{0}", GetRid(best.n));
//        // jf_log_info("accProb:{0}", best.accProb);
//        // jf_log_info("obsProb:{0}", best.obsProb);
//        // jf_log_info("transProb:{0}", best.transProb);
//        // jf_log_info("TileId: {0}, {1}", tid.lonId, tid.latId);
//        // jf_log_info("execNum:{0}", execNum - 1);
//        first = false;
//    } else {
//        // jf_log_info("[algorithm::BindRoadHmm] algorithm restart");
//        // jf_log_error("[algorithm::BindRoadHmm] no candidates \n gps : {0:.15f},{1:.15f},{2:.15f} \n TileId {3} {4}", ingps.lon, ingps.lat, ingps.alt, P2TId(ingps).lonId, P2TId(ingps).latId);
//        out.e = BindRoadOutput::error::no_candidates;
//        first = true;
//        pending = true;
//    }
//}
//
//bool BindRoad::CalcTransformProb(std::vector<AlgoNodeData> &punishCandidates, bool first) {
//    // punishCandidates = candidates;
//
//    if (!first) {
//        // 换路惩罚：当本次换路，且之前不可达时，给予不可达的所有点惩罚
//        auto sameRoad = [](const AlgoNodeData &now, const AlgoNodeData &last) {
//            for (auto &r : now.n->outRids) {
//                if (Utils::Contains(last.n->outRids, r)) {
//                    return true;
//                }
//            }
//            return false;
//        };
//        if (!sameRoad(punishCandidates[0], last_best)) {
//            std::set<AlgoNodeData> arrivable;
//            GetNextNNodes(MapConfig::algorithm_path_len, last_best.n, arrivable);
//
//            std::transform(punishCandidates.begin(), punishCandidates.end(), punishCandidates.begin(), [this, &arrivable, &sameRoad](const AlgoNodeData &nd) {
//                AlgoNodeData punishNd = nd;
//                // 如果本路与上一时刻路不同，并且上一节点无法到达本节点
//                if (!sameRoad(nd, last_best) && !Utils::Contains(arrivable, nd)) {
//                    punishNd.accProb = punishNd.accProb / MapConfig::algorithm_change_road_punish_div;
//                    return punishNd;
//                }
//                return nd;
//            });
//            std::sort(punishCandidates.begin(), punishCandidates.end(), [](const AlgoNodeData &l, const AlgoNodeData &r) { return l.accProb > r.accProb; });
//        }
//
//        // if (punishCandidates[0].n != candidates[0].n) {
//        //     jf_log_info("punish forbid change road \n punish node:{0} lon: {1:0.15f} lat: {2:0.15f} wayId:{3} \n use node:{4} lon:{5:0.15f} lat:{6:0.15f} wayId:{7}", candidates[0].n->id_p.ToString(), candidates[0].n->id_p.p.lon,
//        //                 candidates[0].n->id_p.p.lat, GetRid(candidates[0].n), punishCandidates[0].n->id_p.ToString(), punishCandidates[0].n->id_p.p.lon, punishCandidates[0].n->id_p.p.lat, GetRid(punishCandidates[0].n));
//        // }
//    }
//
//    return true;
}

void BindRoad::GetNextNNodes(int n, const SdNode *from, std::set<AlgoNodeData> &ret) {
    //std::set<const SdNode *> nds;
    //Traversal::GetNextNNodes(n, from, nds);
    //for (const SdNode *nd : nds) {
    //    ret.insert(AlgoNodeData(nd));
    //}
}

double BindRoad::GetTransProb(const SdNode *n, const BindRoadInput &in) {
    //// rids
    //for (RoadId rid : n->outRids) {
    //    if (jf::Utils::Contains(in.prior_protect_rids, rid)) {
    //        return 1;
    //    } else if (jf::Utils::Contains(in.prior_punish_rids, rid)) {
    //        return 0.25;
    //    }
    //}

    //// nodes
    //if (jf::Utils::Contains(in.prior_protect_node, n->id_p)) {
    //    return 1;
    //} else if (jf::Utils::Contains(in.prior_punish_node, n->id_p)) {
    //    return 0.25;
    //}
    //return 0.5;
}

uint64_t BindRoad::GetRid(const SdNode *nd) {
    //// 如果出度只有一个，则算绑到了下一条路
    //if (nd->outRids.size() == 1) {
    //    return nd->outRids[0];
    //}
    //    // 如果出度不止一个，且有入度，则算绑到入度0
    //else if (nd->inRids.size() > 0) {
    //    return nd->inRids[0];
    //}
    //    // 否则返回出度0
    //else if (nd->outRids.size() > 0) {
    //    return nd->outRids[0];
    //} else {
    //    // jf_log_error("no input/output rid, nd: {}", jf::ToString(nd->id_p));
    //    return 0;
    //}
}