#include <thread>

#include "estimator.hpp"


void Estimator::run()
{
    std::cout << "\n Estimator is ready to process Keyframes!\n";
    
    while( !bexit_required_ ) {

        if( getNewKf() ) //如果 getNewKf() 返回 true，则说明有了新的关键帧 (KF)。
        {
            //如果当前 SLAM 状态 (pslamstate_) 处于 SLAM 模式，则执行以下操作:
            //applyLocalBA() :应用本地 BA(Best Estimate) 算法，更新当前 KF 的状态。这个函数会更新 KF 中的位置、姿态和地图点等信息。
            //mapFiltering() :对当前 KF 进行地图滤波，以减少噪声干扰。这个函数会计算 KF 与地图点之间的欧几里得距离，并对 KF 进行去噪处理。
            if( pslamstate_->slam_mode_ ) 
            {
                if( pslamstate_->debug_ )
                    std::cout << "\n [Estimator] Slam-Mode - Processing new KF #" << pnewkf_->kfid_;

                applyLocalBA();

                mapFiltering();

            } 
            
            //如果当前 SLAM 状态不处于 SLAM 模式，则输出一条消息，表示没有优化模式选择
            else {
                if( pslamstate_->debug_ )
                    std::cout << "\nNO OPITMIZATION (NEITHER SLAM MODE NOR SW MODE SELECTED) !\n";
            }
        } 
        
        //在循环外部，它使用一个时间为 20 微秒的线程休眠来等待下一个 KF 的到来。
        //这个时间是固定的，无论当前是否有新的 KF 到来。
        else {
            std::chrono::microseconds dura(20);
            std::this_thread::sleep_for(dura);
        }
    }

    //调用 poptimizer_->signalStopLocalBA() 来通知 optimizer 停止 localBA 优化。
    poptimizer_->signalStopLocalBA();
    
    //使用一个锁来保护 optim_mutex_，以确保 optimization 线程安全地退出。
    std::lock_guard<std::mutex> lock2(pmap_->optim_mutex_);

    //Estimator 线程正在退出。
    std::cout << "\n Estimator thread is exiting.\n";
}


void Estimator::applyLocalBA()
{
    //定义了一个常量 nmincstkfs，表示关键帧计数器的最大计数器为 1。
    int nmincstkfs = 1;
    if( pslamstate_->mono_ ) {
        nmincstkfs = 2;
    }

    //如果当前关键帧计数器小于这个值，说明当前关键帧数目较少，因此不需要进行 SLAM 优化。
    if( pnewkf_->kfid_ < nmincstkfs ) {
        return;
    }

    //检查当前关键帧是否为空 (即 pnewkf_->nb3dkps_ == 0),
    //如果为空，则说明当前关键帧中没有 3D 点，因此不需要进行 SLAM 优化。
    if( pnewkf_->nb3dkps_ == 0 ) {
        return;
    }

    if( pslamstate_->debug_ || pslamstate_->log_timings_ )
        Profiler::Start("1.BA_localBA");//Profiler::Start() 函数会在执行完毕当前函数之前，记录当前时间，以便进行性能分析。

    //使用一个 std::lock_guardstd::mutex 类型的锁来保护 pmap_->optim_mutex_ 变量，以确保只有单个线程能够访问该变量。
    //这是因为在 localBA 优化期间，需要对关键点进行重新计算，可能会导致其他线程访问该变量，从而出现竞争条件。使用锁来保护该变量可以保证线程安全。
    std::lock_guard<std::mutex> lock2(pmap_->optim_mutex_);

    // We signal that Estimator is performing BA
    //将当前 SLAM 状态 (pslamstate_) 中的 blocalba_is_on_ 变量设置为 true，表示 Estimator 正在执行 localBA 优化。
    pslamstate_->blocalba_is_on_ = true;

    bool use_robust_cost = true;
    //调用 poptimizer_->localBA() 函数，对当前关键帧进行 localBA 优化。
    //这个函数会计算当前关键帧与 SLAM 状态中所有已观测到的关键点之间的欧几里得距离，并选择一个距离最小的关键点作为当前关键帧的最优解。
    poptimizer_->localBA(*pnewkf_, use_robust_cost);

    // We signal that Estimator is stopping BA
    //将当前 SLAM 状态 (pslamstate_) 中的 blocalba_is_on_ 变量设置为 false，表示 Estimator 已经完成了 localBA 优化。
    //这个变量的值是可选的，因为 localBA 优化可以多次执行，直到找到最优解为止。
    pslamstate_->blocalba_is_on_ = false;
    
    //如果当前 SLAM 状态处于 debug_ 状态，或者设置了 log_timings_ 状态，则会调用 Profiler::StopAndDisplay() 函数，
    //并将 "1.BA_localBA" 作为参数传递给该函数。这将在 Estimator 的主循环中执行，通常在处理关键帧时使用，
    //以便在执行 localBA 优化时记录执行时间，以便进行性能分析。
    //Profiler::StopAndDisplay() 函数会在执行完毕当前函数之后，打印出调用堆栈信息和当前时间，以便进行调试和性能分析。
    if( pslamstate_->debug_ || pslamstate_->log_timings_ )
        Profiler::StopAndDisplay(pslamstate_->debug_, "1.BA_localBA");
}


void Estimator::mapFiltering()    //mapFiltering()函数基于一些条件对地图中的关键帧进行过滤。
{
    //如果fkf_filtering_ratio_（slamState类的一个变量）的值大于或等于1，则函数返回，不执行任何过滤。
    //fkf_filtering_ratio_:用于滤除关键帧的阈值，当某个关键帧的3D点匹配好的观测数量与总观测数量的比例小于该阈值时，该关键帧会被移除。
    if( pslamstate_->fkf_filtering_ratio_ >= 1. ) {
        return;
    }
    
    //如果新关键帧的ID（kfid_）小于20或blc_is_on_（slamState类的一个变量,即闭环检测）为真，则函数返回，不执行任何过滤。
    if( pnewkf_->kfid_ < 20 || pslamstate_->blc_is_on_ ) {
        return;
    }   

    //如果debug_或log_timings_（slamState类的两个变量）为真，则使用性能分析工具Profiler开始计时。
    if( pslamstate_->debug_ || pslamstate_->log_timings_ )
        Profiler::Start("1.BA_map-filtering");
    
    // 获取当前关键帧的可视化关键帧列表
    auto covkf_map = pnewkf_->getCovisibleKfMap();

    //// 遍历可视化关键帧列表
    for( auto it = covkf_map.rbegin() ; it != covkf_map.rend() ; it++ ) {

        int kfid = it->first;

        // 如果有新的关键帧可用或当前关键帧是地图中的第一个关键帧，跳出循环
        if( bnewkfavailable_ || kfid == 0 ) {
            break;
        }

        // 如果关键帧的 id 大于等于当前关键帧的 id，继续循环
        if( kfid >= pnewkf_->kfid_ ) {
            continue;
        }

        // Only useful if LC enabled
        // 如果当前关键帧是闭环检测触发的关键帧，继续循环
        if( pslamstate_->lckfid_ == kfid ) {
            continue;
        }

        // 根据关键帧 id 获取关键帧对象
        auto pkf = pmap_->getKeyframe(kfid);
        // 如果关键帧对象为空，从当前关键帧的可视化关键帧列表中移除该关键帧并继续循环
        if( pkf == nullptr ) {
            pnewkf_->removeCovisibleKf(kfid);
            continue;
        } 
        else if( (int)pkf->nb3dkps_ < pslamstate_->nmin_covscore_ / 2 ) {
            // 如果关键帧中三维特征点数量太少，从地图中移除该关键帧并继续循环
            std::lock_guard<std::mutex> lock(pmap_->map_mutex_);
            pmap_->removeKeyframe(kfid);
            continue;
        }

        // 统计该关键帧中能够匹配到的有效三维地图点数量
        size_t nbgoodobs = 0;
        size_t nbtot = 0;
        for( const auto &kp : pkf->getKeypoints3d() )//遍历该关键帧的所有3D点
        {
            auto plm = pmap_->getMapPoint(kp.lmid_);// 获取该关键点对应的地图点
            if( plm == nullptr ) {        // 如果地图点为空，则移除该关键点的观测关系
                pmap_->removeMapPointObs(kp.lmid_, kfid);
                continue;
            } 
            else if( plm->isBad() ) {    // 如果地图点被标记为坏点，则跳过该关键点
                continue;
            }
            else {    // 如果地图点是好点，则统计观测该点的关键帧数量
                size_t nbcokfs = plm->getKfObsSet().size();
                //如果次数超过4，则将该3D点视为可靠的观测，并增加nbgoodobs计数器。无论是否可靠，nbtot计数器也会增加。
                if( nbcokfs > 4 ) {
                    nbgoodobs++;
                }
            } 
            
            nbtot++;// 统计总的观测数
            
            // 如果已经有了新的关键帧，则退出循环
            if( bnewkfavailable_ ) {
                break;
            }
        }

        //在计算完所有3D点的被观测次数后，代码会计算一个被观测次数可靠的3D点比例ratio，该比例用于判断当前关键帧是否可以移除。
        //如果ratio大于阈值pslamstate_->fkf_filtering_ratio_，则认为该关键帧可靠，不需要移除。
        //否则，如果当前关键帧不是最新的关键帧并且也不是回环检测时刻的关键帧，则将其从地图中移除。
        float ratio = (float)nbgoodobs / nbtot;
        if( ratio > pslamstate_->fkf_filtering_ratio_ ) {

            // Only useful if LC enabled
            if( pslamstate_->lckfid_ == kfid ) {
                continue;
            }
            std::lock_guard<std::mutex> lock(pmap_->map_mutex_);
            pmap_->removeKeyframe(kfid);
        }
    }

    if( pslamstate_->debug_ || pslamstate_->log_timings_ )
        Profiler::StopAndDisplay(pslamstate_->debug_, "1.BA_map-filtering");
}

bool Estimator::getNewKf()
{
    //使用lock_guard实现互斥锁，确保线程安全。qkf_mutex_用于保护队列qpkfs_的互斥锁。
    std::lock_guard<std::mutex> lock(qkf_mutex_);

    // Check if new KF is available
    //检查队列qpkfs_是否为空。如果是空的，则没有新的关键帧可用，将bnewkfavailable_设为false，返回false表示获取新的关键帧失败。
    if( qpkfs_.empty() ) {
        bnewkfavailable_ = false;
        return false;
    }

    // In SLAM-mode, we only processed the last received KF
    // but we trick the covscore if several KFs were waiting
    // to make sure that they are all optimized
    //如果qpkfs_队列中有多个关键帧，则将除最后一个关键帧外的所有关键帧弹出，以便对其进行优化。
    //vkfids是一个int类型的向量，用于存储已弹出的关键帧的id，以便后面更新关键帧的协方差矩阵。
    std::vector<int> vkfids;
    vkfids.reserve(qpkfs_.size());
    while( qpkfs_.size() > 1 ) {
        qpkfs_.pop();
        vkfids.push_back(pnewkf_->kfid_);
    }
    //获取最后一个关键帧并从队列qpkfs_中弹出。
    pnewkf_ = qpkfs_.front();
    qpkfs_.pop();
    
    //如果弹出了多个关键帧，则遍历所有已弹出的关键帧，并将它们添加到当前关键帧的协方差矩阵中。
    //map_covkfs_是一个关键帧id到其协方差矩阵的映射。如果pslamstate_->debug_为true，则输出调试信息
    if( !vkfids.empty() ) {
        for( const auto &kfid : vkfids ) {
            pnewkf_->map_covkfs_[kfid] = pnewkf_->nb3dkps_;
        }

        if( pslamstate_->debug_ )
            std::cout << "\n ESTIMATOR is late!  Adding several KFs to BA...\n";
    }
    //将bnewkfavailable_设为false，表示不再有新的关键帧可用。
    bnewkfavailable_ = false;
    //返回true表示成功获取了新的关键帧。
    return true;
}


void Estimator::addNewKf(const std::shared_ptr<Frame> &pkf)   // 向队列中添加新的关键帧
{
    std::lock_guard<std::mutex> lock(qkf_mutex_);//首先获取一个互斥锁qkf_mutex_的锁保护共享资源；
    qpkfs_.push(pkf);                   //将指向关键帧的智能指针pkf添加到队列qpkfs_中
    bnewkfavailable_ = true;           //将成员变量bnewkfavailable_设置为true，表示有新的关键帧可用

    // We signal that a new KF is ready
    //如果当前SLAM系统的局部BA开启，且poptimizer_->stopLocalBA()函数返回值为false，则发送信号通知优化器暂停局部BA计算。
    if( pslamstate_->blocalba_is_on_ 
        && !poptimizer_->stopLocalBA() ) 
    {
        poptimizer_->signalStopLocalBA();
    }
}

void Estimator::reset()   //是重置Estimator类的状态
{
    std::lock_guard<std::mutex> lock2(pmap_->optim_mutex_);//首先获取一个互斥锁pmap_->optim_mutex_的锁保护共享资源

    //将成员变量bnewkfavailable_和bexit_required_都设置为false，表示SLAM系统没有新的关键帧可用，也没有退出的要求；
    bnewkfavailable_ = false;
    bexit_required_ = false; 

    //创建一个空的关键帧队列，将队列qpkfs_和空队列进行交换，这样原来的队列就被清空了。
    std::queue<std::shared_ptr<Frame>> empty;
    std::swap(qpkfs_, empty);
}