#include "scancontext/Scancontext.h"

#include <Eigen/src/Geometry/Transform.h>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <glog/logging.h>

#include "protobuf/eigen_proto_utils.h"

// namespace SC2
// {

cv::Mat EigenMatrixToCvMat(const Eigen::MatrixXd& eigen_mat, bool normalize = true) {
    int rows = eigen_mat.rows();
    int cols = eigen_mat.cols();

    // 创建 CV_32F 类型的 cv::Mat（单通道浮点）
    cv::Mat cv_mat(rows, cols, CV_32F);

    // 逐元素复制数据
    for (int i = 0; i < rows; ++i) {
        for (int j = 0; j < cols; ++j) {
            cv_mat.at<float>(i, j) = static_cast<float>(eigen_mat(i, j));
        }
    }

    if (normalize) {
        // 归一化到 [0, 255]
        double min_val, max_val;
        cv::minMaxLoc(cv_mat, &min_val, &max_val);

        if (max_val > min_val) {
            cv_mat.convertTo(cv_mat, CV_8UC1, 255.0 / (max_val - min_val), -min_val * 255.0 / (max_val - min_val));
        } else {
            // 所有值相同，设为中值
            cv_mat.setTo(127);
            cv_mat.convertTo(cv_mat, CV_8UC1);
        }
    } else {
        // 不归一化，转为 CV_8UC1 需要手动缩放（可选）
        cv_mat.convertTo(cv_mat, CV_8UC1);
    }

    return cv_mat;
}

void coreImportTest (void)
{
    cout << "scancontext lib is successfully imported." << endl;
} // coreImportTest


float rad2deg(float radians)
{
    return radians * 180.0 / M_PI;
}

float deg2rad(float degrees)
{
    return degrees * M_PI / 180.0;
}


float xy2theta( const float & _x, const float & _y )
{
    if ( (_x >= 0) & (_y >= 0))
        return (180/M_PI) * atan(_y / _x);

    if ( (_x < 0) & (_y >= 0))
        return 180 - ( (180/M_PI) * atan(_y / (-_x)) );

    if ( (_x < 0) & (_y < 0))
        return 180 + ( (180/M_PI) * atan(_y / _x) );

    if ( (_x >= 0) & (_y < 0))
        return 360 - ( (180/M_PI) * atan((-_y) / _x) );

    return 0.;
} // xy2theta


 Eigen::MatrixXd circshift( Eigen::MatrixXd &_mat, int _num_shift )
{
    // shift columns to right direction
    assert(_num_shift >= 0);

    if( _num_shift == 0 )
    {
        Eigen::MatrixXd shifted_mat( _mat );
        return shifted_mat; // Early return
    }

    Eigen::MatrixXd shifted_mat = Eigen::MatrixXd::Zero( _mat.rows(), _mat.cols() );
    for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ )
    {
        int new_location = (col_idx + _num_shift) % _mat.cols();
        shifted_mat.col(new_location) = _mat.col(col_idx);
    }

    return shifted_mat;

} // circshift


std::vector<float> eig2stdvec( Eigen::MatrixXd _eigmat )
{
    std::vector<float> vec( _eigmat.data(), _eigmat.data() + _eigmat.size() );
    return vec;
} // eig2stdvec


double SCManager::distDirectSC ( Eigen::MatrixXd &_sc1, Eigen::MatrixXd &_sc2 )
{
    int num_eff_cols = 0; // i.e., to exclude all-nonzero sector
    double sum_sector_similarity = 0;
    for ( int col_idx = 0; col_idx < _sc1.cols(); col_idx++ )
    {
        Eigen::VectorXd col_sc1 = _sc1.col(col_idx);
        Eigen::VectorXd col_sc2 = _sc2.col(col_idx);

        if( (col_sc1.norm() == 0) | (col_sc2.norm() == 0) )
            continue; // don't count this sector pair.

        double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm());

        sum_sector_similarity = sum_sector_similarity + sector_similarity;
        num_eff_cols = num_eff_cols + 1;
    }

    double sc_sim = sum_sector_similarity / num_eff_cols;
    return 1.0 - sc_sim;

} // distDirectSC


int SCManager::fastAlignUsingVkey( Eigen::MatrixXd & _vkey1, Eigen::MatrixXd & _vkey2)
{
    int argmin_vkey_shift = 0;
    double min_veky_diff_norm = 10000000;
    for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ )
    {
        Eigen::MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx);

        Eigen::MatrixXd vkey_diff = _vkey1 - vkey2_shifted;

        double cur_diff_norm = vkey_diff.norm();
        if( cur_diff_norm < min_veky_diff_norm )
        {
            argmin_vkey_shift = shift_idx;
            min_veky_diff_norm = cur_diff_norm;
        }
    }

    return argmin_vkey_shift;

} // fastAlignUsingVkey


std::pair<double, int> SCManager::distanceBtnScanContext( Eigen::MatrixXd &_sc1, Eigen::MatrixXd &_sc2 )
{
    // 1. fast align using variant key (not in original IROS18)
    Eigen::MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 );
    Eigen::MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 );
    int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 );

    const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range
    std::vector<int> shift_idx_search_space { argmin_vkey_shift };
    for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ )
    {
        shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() );
        shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() );
    }
    std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end());

    // 2. fast columnwise diff
    int argmin_shift = 0;
    double min_sc_dist = 10000000;
    for ( int num_shift: shift_idx_search_space )
    {
        Eigen::MatrixXd sc2_shifted = circshift(_sc2, num_shift);
        double cur_sc_dist = distDirectSC( _sc1, sc2_shifted );
        if( cur_sc_dist < min_sc_dist )
        {
            argmin_shift = num_shift;
            min_sc_dist = cur_sc_dist;
        }
    }

    return make_pair(min_sc_dist, argmin_shift);

} // distanceBtnScanContext


Eigen::MatrixXd SCManager::makeScancontext( pcl::PointCloud<SCPointType> & _scan_down )
{
    TicTocV2 t_making_desc;

    int num_pts_scan_down = _scan_down.points.size();

    // main
    const int NO_POINT = -1000;
    Eigen::MatrixXd desc = NO_POINT * Eigen::MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR);

    SCPointType pt;
    float azim_angle, azim_range; // wihtin 2d plane
    int ring_idx, sctor_idx;
    for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++)
    {
        pt.x = _scan_down.points[pt_idx].x;
        pt.y = _scan_down.points[pt_idx].y;
        pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0).

        // xyz to ring, sector
        azim_range = sqrt(pt.x * pt.x + pt.y * pt.y);
        azim_angle = xy2theta(pt.x, pt.y);

        // if range is out of roi, pass
        if( azim_range > PC_MAX_RADIUS )
            continue;

        ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 );
        sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 );

        // taking maximum z
        if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0
            desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin
    }

    // reset no points to zero (for cosine dist later)
    for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ )
        for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ )
            if( desc(row_idx, col_idx) == NO_POINT )
                desc(row_idx, col_idx) = 0;

    t_making_desc.toc("PolarContext making");

    return desc;
} // SCManager::makeScancontext


Eigen::MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc )
{
    /*
     * summary: rowwise mean vector
    */
    Eigen::MatrixXd invariant_key(_desc.rows(), 1);
    for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ )
    {
        Eigen::MatrixXd curr_row = _desc.row(row_idx);
        invariant_key(row_idx, 0) = curr_row.mean();
    }

    return invariant_key;
} // SCManager::makeRingkeyFromScancontext


Eigen::MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc )
{
    /*
     * summary: columnwise mean vector
    */
    Eigen::MatrixXd variant_key(1, _desc.cols());
    for ( int col_idx = 0; col_idx < _desc.cols(); col_idx++ )
    {
        Eigen::MatrixXd curr_col = _desc.col(col_idx);
        variant_key(0, col_idx) = curr_col.mean();
    }

    return variant_key;
} // SCManager::makeSectorkeyFromScancontext


const Eigen::MatrixXd& SCManager::getConstRefRecentSCD(void)
{
    return polarcontexts_.back();
}


// Serialize current state to protobuf message
bool SCManager::SerializeToProto(scancontext::SCManagerState* proto) const {
    if (!proto)
        return false;

    // Serialize parameters
    proto->set_lidar_height(LIDAR_HEIGHT);
    proto->set_pc_num_ring(PC_NUM_RING);
    proto->set_pc_num_sector(PC_NUM_SECTOR);
    proto->set_pc_max_radius(PC_MAX_RADIUS);
    proto->set_sc_dist_thres(SC_DIST_THRES);

    CHECK_EQ(polarcontexts_.size(), polarcontext_invkeys_.size());
    CHECK_EQ(polarcontexts_.size(), polarcontext_vkeys_.size());
    CHECK_EQ(polarcontexts_.size(), polarcontext_invkeys_mat_.size());
    CHECK_EQ(polarcontexts_.size(), frame_ids_.size());

    // LOG(INFO) << polarcontexts_.front();

    // Serialize descriptors
    for (size_t i = 0; i < polarcontexts_.size(); ++i) {
        auto *desc_proto = proto->add_descriptors();

        // descriptor
        auto *mat_proto = desc_proto->mutable_sc_descriptor();
        const auto &mat = polarcontexts_[i];
        EigenProto::serialize(mat, mat_proto);

        // ringkey
        if (i < polarcontext_invkeys_.size()) {
            auto *rk_proto = desc_proto->mutable_ringkey();
            const auto &rk = polarcontext_invkeys_[i];
            EigenProto::serialize(rk, rk_proto);
        }

        // sectorkey
        if (i < polarcontext_vkeys_.size()) {
            auto *sk_proto = desc_proto->mutable_sectorkey();
            const auto &sk = polarcontext_vkeys_[i];
            EigenProto::serialize(sk, sk_proto);
        }

        // timestamp
        if (i < polarcontexts_timestamp_.size()) {
            desc_proto->set_timestamp(polarcontexts_timestamp_[i]);
        }

        if (i < polarcontext_invkeys_mat_.size()) {
            auto *invkey_mat_proto =
                desc_proto->mutable_polarcontext_invkeys_mat();
            const auto &invkey_vec = polarcontext_invkeys_mat_[i];
            invkey_mat_proto->set_rows(invkey_vec.size());
            invkey_mat_proto->set_cols(1);
            for (const auto &val : invkey_vec) {
                invkey_mat_proto->add_data(val);
            }
        }

        if (i < poses_.size()) {
            auto* pose_proto = desc_proto->mutable_pose();
            const auto& pose = poses_[i];
            EigenProto::Matrix4dToProto(pose, pose_proto);
        }

        if (i < frame_ids_.size()) {
            desc_proto->set_frame_id(frame_ids_[i]);
        }
    }

    proto->set_is_tree_batch_made(is_tree_batch_made);

    return true;
}

// Deserialize from protobuf message
bool SCManager::DeserializeFromProto(const scancontext::SCManagerState& proto) {

    // Restore parameters (optional: you may want to warn if mismatch)
    // LIDAR_HEIGHT = proto.lidar_height(); // usually fixed, 可选恢复
    // PC_NUM_RING = proto.pc_num_ring();
    // PC_NUM_SECTOR = proto.pc_num_sector();
    PC_MAX_RADIUS = proto.pc_max_radius();
    SC_DIST_THRES = proto.sc_dist_thres();

    // Clear old data
    polarcontexts_.clear();
    polarcontext_invkeys_.clear();
    polarcontext_vkeys_.clear();
    polarcontexts_timestamp_.clear();
    polarcontext_invkeys_mat_.clear();

    // Deserialize descriptors
    for (const auto& desc_proto : proto.descriptors()) {
        // descriptor
        Eigen::MatrixXd desc(desc_proto.sc_descriptor().rows(), desc_proto.sc_descriptor().cols());
        EigenProto::deserialize(desc_proto.sc_descriptor(), &desc);
        polarcontexts_.push_back(desc);

        // ringkey
        if (desc_proto.has_ringkey()) {
            Eigen::MatrixXd rk(desc_proto.ringkey().rows(), desc_proto.ringkey().cols());
            EigenProto::deserialize(desc_proto.ringkey(), &rk);
            polarcontext_invkeys_.push_back(rk);
        }

        // sectorkey
        if (desc_proto.has_sectorkey()) {
            Eigen::MatrixXd sk(desc_proto.sectorkey().rows(), desc_proto.sectorkey().cols());
            EigenProto::deserialize(desc_proto.sectorkey(), &sk);
            polarcontext_vkeys_.push_back(sk);
        }

        if (desc_proto.has_polarcontext_invkeys_mat()) {
          std::vector<float> invkey_vec;
          for (int i = 0; i < desc_proto.polarcontext_invkeys_mat().data_size();
               ++i) {
                invkey_vec.push_back(desc_proto.polarcontext_invkeys_mat().data(i));
          }
          polarcontext_invkeys_mat_.push_back(invkey_vec);
        }

        if (desc_proto.has_pose()) {
            Eigen::Matrix4d pose;
            EigenProto::ProtoToMatrix4d(desc_proto.pose(), &pose);
            poses_.push_back(pose);
        }

        if (true) {
            frame_ids_.push_back(desc_proto.frame_id());
        }

        // timestamp
        // if (desc_proto.has_timestamp()) {
        //     polarcontexts_timestamp_.push_back(desc_proto.timestamp());
        // }
    }

    LOG(INFO) << "Loaded " << polarcontexts_.size() << " scancontexts from proto.";
    LOG(INFO) << "Loaded polarcontext_invkeys size: " << polarcontext_invkeys_.size();
    LOG(INFO) << "Loaded polarcontext_vkeys size: " << polarcontext_vkeys_.size();
    LOG(INFO) << "Loaded polarcontext_invkeys_mat_ size: " << polarcontext_invkeys_mat_.size();
    LOG(INFO) << "Loaded poses_ size: " << poses_.size();

    // LOG(INFO) << polarcontexts_.front();

    is_tree_batch_made = proto.is_tree_batch_made();

    // ⚠️ 注意：树结构不保存，需要重新构建
    if (is_tree_batch_made && !polarcontext_invkeys_.empty()) {
        // 重建搜索树（可选）
        // makeAndSaveScancontextAndKeys(); // 或者提供单独的 rebuildTree() 函数
    }

    // cv::Mat first_scancontext = EigenMatrixToCvMat(polarcontexts_.front());
    // cv::imwrite("first_scancontext.png", first_scancontext);

    // CHECK(0);

    return true;

}

// Save to file
bool SCManager::SaveToFile(const std::string& filename) const {
    scancontext::SCManagerState proto;
    if (!SerializeToProto(&proto)) {
        return false;
    }

    std::ofstream ofs(filename, std::ios::binary);
    if (!ofs.is_open()) return false;

    return proto.SerializeToOstream(&ofs);

}

// Load from file
bool SCManager::LoadFromFile(const std::string& filename) {
    scancontext::SCManagerState proto;
    std::ifstream ifs(filename, std::ios::binary);
    if (!ifs.is_open()) return false;

    if (!proto.ParseFromIstream(&ifs)) {
        return false;
    }

    return DeserializeFromProto(proto);
}


void SCManager::saveScancontextAndKeys( Eigen::MatrixXd _scd )
{
    Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( _scd );
    Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( _scd );
    std::vector<float> polarcontext_invkey_vec = eig2stdvec( ringkey );

    polarcontexts_.push_back( _scd );
    polarcontext_invkeys_.push_back( ringkey );
    polarcontext_vkeys_.push_back( sectorkey );
    polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec );
} // SCManager::makeAndSaveScancontextAndKeys


void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud<SCPointType> & _scan_down,
    const Eigen::Matrix4d &pose, const size_t& frame_id )
{
    Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1

    Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc );
    Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc );
    std::vector<float> polarcontext_invkey_vec = eig2stdvec( ringkey );

    polarcontexts_.push_back( sc );
    polarcontext_invkeys_.push_back( ringkey );
    polarcontext_vkeys_.push_back( sectorkey );
    polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec );
    poses_.push_back( pose );
    frame_ids_.push_back(frame_id);

    // LOG(INFO) << "sc " << sc;
} // SCManager::makeAndSaveScancontextAndKeys

void SCManager::setSCdistThres(double _new_thres)
{
    SC_DIST_THRES = _new_thres;
} // SCManager::setThres

void SCManager::setMaximumRadius(double _max_r)
{
    PC_MAX_RADIUS = _max_r;
} // SCManager::setMaximumRadius


// auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query)
// auto curr_desc = polarcontexts_.back(); // current observation (query)
SCRelocInfo SCManager::detectLoopClosureIDBetweenSession (std::vector<float>& _curr_key, Eigen::MatrixXd& _curr_desc)
{
    int loop_idx { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID")

    auto& curr_key = _curr_key;
    auto& curr_desc = _curr_desc; // current observation (query)

    // step 0: if first, construct the tree in batch
    if( ! is_tree_batch_made ) // run only once
    {
        polarcontext_invkeys_to_search_.clear();
        polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() ) ;

        polarcontext_tree_batch_.reset();
        polarcontext_tree_batch_ = std::make_unique<InvKeyTree>(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ );

        is_tree_batch_made = true; // for running this block only once
    }

    double min_dist = 10000000; // init with somthing large
    int nn_align = 0;
    int nn_idx = 0;

    // step 1: knn search
    std::vector<size_t> candidate_indexes( NUM_CANDIDATES_FROM_TREE );
    std::vector<float> out_dists_sqr( NUM_CANDIDATES_FROM_TREE );

    nanoflann::KNNResultSet<float> knnsearch_result( NUM_CANDIDATES_FROM_TREE );
    knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] );
    polarcontext_tree_batch_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); // error here

    // step 2: pairwise distance (find optimal columnwise best-fit using cosine distance)
    TicTocV2 t_calc_dist;
    for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ )
    {
        Eigen::MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ];
        std::pair<double, int> sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate );

        double candidate_dist = sc_dist_result.first;
        int candidate_align = sc_dist_result.second;

        if( candidate_dist < min_dist )
        {
            min_dist = candidate_dist;
            nn_align = candidate_align;

            nn_idx = candidate_indexes[candidate_iter_idx];
        }
    }
    t_calc_dist.toc("Distance calc");

    // step 3: similarity threshold
    if( min_dist < SC_DIST_THRES )
        loop_idx = nn_idx;

    LOG(INFO) << "min_dist: " << min_dist;

    // To do: return also nn_align (i.e., yaw diff)
    float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE);
    // std::pair<int, float> result {loop_id, yaw_diff_rad};

    SCRelocInfo reloc_info;
    reloc_info.frame_id = frame_ids_[loop_idx];
    reloc_info.yaw_diff_rad = yaw_diff_rad;
    if (loop_idx >=0 && loop_idx < poses_.size()) {
        reloc_info.pose = poses_[loop_idx];
    } else {
        reloc_info.pose = Eigen::Matrix4d::Identity();
        reloc_info.frame_id = -1;
    }

    return reloc_info;

} // SCManager::detectLoopClosureIDBetweenSession

SCRelocInfo
SCManager::QueryMap(pcl::PointCloud<SCPointType> &_scan_down) {

    Eigen::MatrixXd scan_context = makeScancontext(_scan_down);
    Eigen::MatrixXd ring_key = makeRingkeyFromScancontext(scan_context);

    std::vector<float> polarcontext_invkey_vec = eig2stdvec( ring_key);

    auto result = detectLoopClosureIDBetweenSession(
        polarcontext_invkey_vec, scan_context);

    // LOG(INFO) << "QueryMap result: loop_id=" << result.first
    //           << ", yaw_diff_rad=" << result.second;

    LOG(INFO) << result.toString();

    // Do something with the keys
    return result;
}

SCRelocInfo SCManager::detectLoopClosureID ( void )
{
    int loop_idx { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID")

    auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query)
    auto curr_desc = polarcontexts_.back(); // current observation (query)

    /*
     * step 1: candidates from ringkey tree_
     */

    SCRelocInfo reloc_info;
    reloc_info.frame_id = -1;
    reloc_info.yaw_diff_rad = 0.;

    if( (int)polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1) {
        return reloc_info; // Early return
    }

    // tree_ reconstruction (not mandatory to make everytime)
    if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost
    {
        TicTocV2 t_tree_construction;

        polarcontext_invkeys_to_search_.clear();
        polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ;

        polarcontext_tree_.reset();
        polarcontext_tree_ = std::make_unique<InvKeyTree>(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ );
        // tree_ptr_->index->buildIndex(); // inernally called in the constructor of InvKeyTree (for detail, refer the nanoflann and KDtreeVectorOfVectorsAdaptor)
        t_tree_construction.toc("Tree construction");
    }
    tree_making_period_conter = tree_making_period_conter + 1;

    double min_dist = 10000000; // init with somthing large
    int nn_align = 0;
    int nn_idx = 0;

    // knn search
    std::vector<size_t> candidate_indexes( NUM_CANDIDATES_FROM_TREE );
    std::vector<float> out_dists_sqr( NUM_CANDIDATES_FROM_TREE );

    TicTocV2 t_tree_search;
    nanoflann::KNNResultSet<float> knnsearch_result( NUM_CANDIDATES_FROM_TREE );
    knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] );
    polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) );
    t_tree_search.toc("Tree search");

    /*
     *  step 2: pairwise distance (find optimal columnwise best-fit using cosine distance)
     */
    TicTocV2 t_calc_dist;
    for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ )
    {
        Eigen::MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ];
        std::pair<double, int> sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate );

        double candidate_dist = sc_dist_result.first;
        int candidate_align = sc_dist_result.second;

        if( candidate_dist < min_dist )
        {
            min_dist = candidate_dist;
            nn_align = candidate_align;

            nn_idx = candidate_indexes[candidate_iter_idx];
        }
    }
    t_calc_dist.toc("Distance calc");

    /*
     * loop threshold check
     */
    if( min_dist < SC_DIST_THRES )
    {
        loop_idx = nn_idx;

        // std::cout.precision(3);
        // cout << "[Loop found] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl;
        // cout << "[Loop found] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl;
    }
    else
    {
        std::cout.precision(3);
        // cout << "[Not loop] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl;
        // cout << "[Not loop] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl;
        return reloc_info;
    }

    // To do: return also nn_align (i.e., yaw diff)
    float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE);

    reloc_info.frame_id = frame_ids_[loop_idx];
    reloc_info.yaw_diff_rad = yaw_diff_rad;
    if (loop_idx >=0 && loop_idx < poses_.size()) {
        reloc_info.pose = poses_[loop_idx];
    } else {
        reloc_info.pose = Eigen::Matrix4d::Identity();
    }

    return reloc_info;

} // SCManager::detectLoopClosureID

// } // namespace SC2