class voxelStereoVio {
  private:
    void processImu(imuData &imu_data) {
    double oldest_time = state_ptr->margtimestep();

    if (oldest_time > state_ptr->timestamp)
        oldest_time = -1;

    if (!initial_flag)
        oldest_time = imu_data.timestamp - odometry_options.init_options.init_window_time + state_ptr->calib_dt_imu_cam->value()(0) - 0.10;

    propagator_ptr->feedImu(imu_data, oldest_time);

    if (!initial_flag)
        initializer_ptr->feedImu(imu_data, oldest_time);
}
    void processImage(cameraData &image_measurements_const) {
    rT1 = boost::posix_time::microsec_clock::local_time();

    assert(!image_measurements_const.camera_ids.empty());
    assert(image_measurements_const.camera_ids.size() == image_measurements_const.images.size());

    for (size_t i = 0; i < image_measurements_const.camera_ids.size() - 1; i++)
        assert(image_measurements_const.camera_ids.at(i) != image_measurements_const.camera_ids.at(i + 1));

    // time test
    boost::posix_time::ptime rT_begin7 = boost::posix_time::microsec_clock::local_time();
    // time test

    cameraData image_measurements = image_measurements_const;

    for (size_t i = 0; i < image_measurements.camera_ids.size() && odometry_options.downsample_cameras; i++)
    {
        cv::Mat img = image_measurements.images.at(i);
        cv::Mat mask = image_measurements.masks.at(i);
        cv::Mat img_temp, mask_temp;
        cv::pyrDown(img, img_temp, cv::Size(img.cols / 2.0, img.rows / 2.0));
        image_measurements.images.at(i) = img_temp;
        cv::pyrDown(mask, mask_temp, cv::Size(mask.cols / 2.0, mask.rows / 2.0));
        image_measurements.masks.at(i) = mask_temp;
    }

    std::shared_ptr<frame> fh = std::make_shared<frame>();
    fh->makeImageLeft(image_measurements.images.at(0).data, image_measurements.masks.at(0).data, gammaPixel_ptr, image_measurements.timestamp);
    fh->makeImageRight(image_measurements.images.at(1).data, image_measurements.masks.at(1).data, gammaPixel_ptr, image_measurements.timestamp);
    fh->frame_id = frame_count;

    // time test
    boost::posix_time::ptime rT_end7 = boost::posix_time::microsec_clock::local_time();
    double time_cost = (rT_end7 - rT_begin7).total_microseconds() * 1e-6;
    sum_time_7 = sum_time_7 * n_time_7 + time_cost;
    n_time_7++;
    sum_time_7 = sum_time_7 / n_time_7;
    std::cout << std::fixed << std::setprecision(6) << "[image processing] time cost = " << sum_time_7 << std::endl;
    // time test

    newest_fh = fh;

    // time test
    boost::posix_time::ptime rT_begin1 = boost::posix_time::microsec_clock::local_time();
    // time test

    featureTracker->feedNewImage(image_measurements, fh);

    // time test
    boost::posix_time::ptime rT_end1 = boost::posix_time::microsec_clock::local_time();
    time_cost = (rT_end1 - rT_begin1).total_microseconds() * 1e-6;
    sum_time_1 = sum_time_1 * n_time_1 + time_cost;
    n_time_1++;
    sum_time_1 = sum_time_1 / n_time_1;
    std::cout << std::fixed << std::setprecision(6) << "[feature extraction and matching] time cost = " << sum_time_1 << std::endl;
    // time test

    rT2 = boost::posix_time::microsec_clock::local_time();

    if (!initial_flag)
    {
        initial_flag = tryToInitialize(image_measurements);

        if (!initial_flag)
        {
            double time_track = (rT2 - rT1).total_microseconds() * 1e-6;
            std::cout << std::fixed << "[processImage]: " << time_track << " seconds for tracking" << std::endl;
            return;
        }
    }

    if (featureTracker->getFeatureDatabase()->featureCheckParallax(frame_count))
        marginalize_status = MarginalizeStatus::OLD;
    else
    {
        if (odometry_options.use_keyframe)
            marginalize_status = MarginalizeStatus::NEW;
        else
            marginalize_status = MarginalizeStatus::OLD;
    }

    std::cout << "marginalize_status = " << marginalize_status << std::endl;

    featureUpdate(image_measurements);

    frame_count++;
}
    void featureUpdate(cameraData &image_measurements) {
    if (state_ptr->timestamp > image_measurements.timestamp)
    {
        std::cout << std::fixed << "Image received out of order, unable to do anything (prop dt = " << image_measurements.timestamp - state_ptr->timestamp << ")" << std::endl;
        return;
    }

    if (state_ptr->timestamp != image_measurements.timestamp)
    {
        propagator_ptr->propagateAndClone(state_ptr, image_measurements.timestamp);

        if (state_ptr->clones_frame.find(image_measurements.timestamp) == state_ptr->clones_frame.end())
            state_ptr->clones_frame[image_measurements.timestamp] = newest_fh;

        assert(state_ptr->clones_imu.size() == state_ptr->clones_frame.size());
    }

    rT3 = boost::posix_time::microsec_clock::local_time();

    if ((int)state_ptr->clones_imu.size() < std::min(state_ptr->options.max_clone_size, 5))
    {
        std::cout << "Waiting for enough clone states (" << (int)state_ptr->clones_imu.size() << " of "
            << std::min(state_ptr->options.max_clone_size, 5) << ")" << std::endl;
        return;
    }

    if (state_ptr->timestamp > image_measurements.timestamp)
    {
        std::cout << "Msckf unable to propagate the state forward in time" << std::endl;
        std::cout << std::fixed << "It has been " << image_measurements.timestamp - state_ptr->timestamp << " since last time we propagated" << std::endl;
        return;
    }

    std::vector<std::shared_ptr<feature>> features_lost, features_marg, features_slam;
    features_lost = featureTracker->getFeatureDatabase()->getOldFeatures(state_ptr->timestamp, false, true);

    if ((int)state_ptr->clones_imu.size() > state_ptr->options.max_clone_size || (int)state_ptr->clones_imu.size() > 5)
    {
        features_marg = featureTracker->getFeatureDatabase()->getFeatures(state_ptr->margtimestep(), false, true);
    }

    auto it_1 = features_lost.begin();
    while (it_1 != features_lost.end())
    {
        bool found_current_cam_id = false;
        for (const auto &cam_uv_pair : (*it_1)->uvs)
        {
            if (std::find(image_measurements.camera_ids.begin(), image_measurements.camera_ids.end(), cam_uv_pair.first)
                != image_measurements.camera_ids.end())
            {
                found_current_cam_id = true;
                break;
            }
        }

        if (found_current_cam_id)
        {
            it_1++;
        }
        else
        {
            it_1 = features_lost.erase(it_1);
        }
    }

    it_1 = features_lost.begin();
    while (it_1 != features_lost.end())
    {
        if (std::find(features_marg.begin(), features_marg.end(), (*it_1)) != features_marg.end())
            it_1 = features_lost.erase(it_1);
        else
            it_1++;
    }

    std::vector<std::shared_ptr<feature>> features_maxtracks;
    auto it_2 = features_marg.begin();

    while (it_2 != features_marg.end())
    {
        bool reached_max = false;

        for (const auto &cams : (*it_2)->timestamps)
        {
            if ((int)cams.second.size() > state_ptr->options.max_clone_size)
            {
                reached_max = true;
                break;
            }
        }

        if (reached_max)
        {
            features_maxtracks.push_back(*it_2);
            it_2 = features_marg.erase(it_2);
        }
        else
        {
            it_2++;
        }
    }

    if (state_ptr->options.max_slam_features > 0 && image_measurements.timestamp - startup_time >= odometry_options.dt_slam_delay &&
        (int)state_ptr->map_points.size() < state_ptr->options.max_slam_features)
    {
        int amount_to_add = (state_ptr->options.max_slam_features) - (int)state_ptr->map_points.size();
        int valid_amount = (amount_to_add > (int)features_maxtracks.size()) ? (int)features_maxtracks.size() : amount_to_add;

        if (valid_amount > 0)
        {
            features_slam.insert(features_slam.end(), features_maxtracks.end() - valid_amount, features_maxtracks.end());
            features_maxtracks.erase(features_maxtracks.end() - valid_amount, features_maxtracks.end());
        }
    }

    for (int i = 0; i < recent_voxels.size(); i++)
    {
        short kx = recent_voxels[i][0];
        short ky = recent_voxels[i][1];
        short kz = recent_voxels[i][2];

        voxelHashMap::iterator search = voxel_map.find(voxel(kx, ky, kz));
        auto &voxel_block = (search.value());

        if (odometry_options.use_all_points)
        {
            for (int j = 0; j < voxel_block.points.size(); j++)
            {
                std::shared_ptr<mapPoint> map_point_ptr = voxel_block.points[j];

                std::shared_ptr<feature> feature = featureTracker->getFeatureDatabase()->getFeature(map_point_ptr->feature_id);
                if (feature != nullptr)
                    features_slam.push_back(feature);

                assert(map_point_ptr->unique_camera_id != -1);

                bool current_host_cam = std::find(image_measurements.camera_ids.begin(), image_measurements.camera_ids.end(), map_point_ptr->unique_camera_id) != image_measurements.camera_ids.end();

                if (feature == nullptr && current_host_cam)
                    map_point_ptr->should_marg = true;

                if (map_point_ptr->update_fail_count > 1)
                    map_point_ptr->should_marg = true;
            }
        }
        else
        {
            std::shared_ptr<mapPoint> map_point_ptr = voxel_block.points.front();

            std::shared_ptr<feature> feature = featureTracker->getFeatureDatabase()->getFeature(map_point_ptr->feature_id);
            if (feature != nullptr)
                features_slam.push_back(feature);

            assert(map_point_ptr->unique_camera_id != -1);

            bool current_host_cam = std::find(image_measurements.camera_ids.begin(), image_measurements.camera_ids.end(), map_point_ptr->unique_camera_id) != image_measurements.camera_ids.end();

            if (feature == nullptr && current_host_cam)
                map_point_ptr->should_marg = true;

            if (map_point_ptr->update_fail_count > 1)
                map_point_ptr->should_marg = true;
        }
    }

    // time test
    boost::posix_time::ptime rT_begin6 = boost::posix_time::microsec_clock::local_time();
    // time test

    stateHelper::marginalizeSlam(state_ptr, voxel_map, points_history);

    // time test
    boost::posix_time::ptime rT_end6 = boost::posix_time::microsec_clock::local_time();
    double time_cost = (rT_end6 - rT_begin6).total_microseconds() * 1e-6;
    sum_time_6 = sum_time_6 * n_time_6 + time_cost;
    n_time_6++;
    sum_time_6 = sum_time_6 / n_time_6;
    std::cout << std::fixed << std::setprecision(6) << "[marginalizeSlam] time cost = " << sum_time_6 << std::endl;
    // time test

    std::vector<std::shared_ptr<feature>> features_slam_delayed, features_slam_update;

    for (size_t i = 0; i < features_slam.size(); i++)
    {
        if (state_ptr->map_points.find(features_slam.at(i)->feature_id) != state_ptr->map_points.end())
        {
            features_slam_update.push_back(features_slam.at(i));
        }
        else
        {
            features_slam_delayed.push_back(features_slam.at(i));
        }
    }

    std::vector<std::shared_ptr<feature>> features_up_msckf = features_lost;
    features_up_msckf.insert(features_up_msckf.end(), features_marg.begin(), features_marg.end());
    features_up_msckf.insert(features_up_msckf.end(), features_maxtracks.begin(), features_maxtracks.end());

    auto compare_feature = [](const std::shared_ptr<feature> &a, const std::shared_ptr<feature> &b) -> bool {
        size_t size_a = 0;
        size_t size_b = 0;

        for (const auto &pair : a->timestamps)
            size_a += pair.second.size();

        for (const auto &pair : b->timestamps)
            size_b += pair.second.size();

        return size_a < size_b;
    };
    std::sort(features_up_msckf.begin(), features_up_msckf.end(), compare_feature);

    if ((int)features_up_msckf.size() > state_ptr->options.max_msckf_in_update)
        features_up_msckf.erase(features_up_msckf.begin(), features_up_msckf.end() - state_ptr->options.max_msckf_in_update);

    // time test
    boost::posix_time::ptime rT_begin2 = boost::posix_time::microsec_clock::local_time();
    // time test

    updaterMsckf_ptr->update(state_ptr, features_up_msckf);

    // time test
    boost::posix_time::ptime rT_end2 = boost::posix_time::microsec_clock::local_time();
    time_cost = (rT_end2 - rT_begin2).total_microseconds() * 1e-6;
    sum_time_2 = sum_time_2 * n_time_2 + time_cost;
    n_time_2++;
    sum_time_2 = sum_time_2 / n_time_2;
    std::cout << std::fixed << std::setprecision(6) << "[state update without map points] time cost = " << sum_time_2 << std::endl;
    // time test

    propagator_ptr->invalidateCache();

    rT4 = boost::posix_time::microsec_clock::local_time();

    std::vector<std::shared_ptr<feature>> features_slam_update_temp;

    while (!features_slam_update.empty())
    {
        std::vector<std::shared_ptr<feature>> features_up_temp;

        features_up_temp.insert(features_up_temp.begin(), features_slam_update.begin(),
            features_slam_update.begin() + std::min(state_ptr->options.max_slam_in_update, (int)features_slam_update.size()));

        features_slam_update.erase(features_slam_update.begin(),
            features_slam_update.begin() + std::min(state_ptr->options.max_slam_in_update, (int)features_slam_update.size()));

        // time test
        boost::posix_time::ptime rT_begin3 = boost::posix_time::microsec_clock::local_time();
        // time test

        updaterSlam_ptr->update(state_ptr, voxel_map, features_up_temp);

        // time test
        boost::posix_time::ptime rT_end3 = boost::posix_time::microsec_clock::local_time();
        time_cost = (rT_end3 - rT_begin3).total_microseconds() * 1e-6;
        sum_time_3 = sum_time_3 * n_time_3 + time_cost;
        n_time_3++;
        sum_time_3 = sum_time_3 / n_time_3;
        std::cout << std::fixed << std::setprecision(6) << "[state update without map points] time cost = " << sum_time_3 << std::endl;
        // time test

        auto it = state_ptr->map_points.begin();

        while (it != state_ptr->map_points.end())
        {
            mapManagement::changeHostVoxel(voxel_map, (*it).second, odometry_options.voxel_size, odometry_options.max_num_points_in_voxel, odometry_options.min_distance_points, voxels_history);
            it++;
        }

        features_slam_update_temp.insert(features_slam_update_temp.end(), features_up_temp.begin(), features_up_temp.end());
        propagator_ptr->invalidateCache();
    }

    features_slam_update = features_slam_update_temp;
    rT5 = boost::posix_time::microsec_clock::local_time();

    // time test
    boost::posix_time::ptime rT_begin4 = boost::posix_time::microsec_clock::local_time();
    // time test

    updaterSlam_ptr->delayedInit(state_ptr, voxel_map, features_slam_delayed, voxels_history);

    // time test
    boost::posix_time::ptime rT_end4 = boost::posix_time::microsec_clock::local_time();
    time_cost = (rT_end4 - rT_begin4).total_microseconds() * 1e-6;
    sum_time_4 = sum_time_4 * n_time_4 + time_cost;
    n_time_4++;
    sum_time_4 = sum_time_4 / n_time_4;
    std::cout << std::fixed << std::setprecision(6) << "[map point registration] time cost = " << sum_time_4 << std::endl;
    // time test

    rT6 = boost::posix_time::microsec_clock::local_time();

    if (image_measurements.camera_ids.at(0) == 0)
    {
        // time test
        boost::posix_time::ptime rT_begin5 = boost::posix_time::microsec_clock::local_time();
        // time test

        triangulateActiveTracks(image_measurements);

        getRecentVoxel(image_measurements.timestamp, voxels_visit);

        // time test
        boost::posix_time::ptime rT_end5 = boost::posix_time::microsec_clock::local_time();
        time_cost = (rT_end5 - rT_begin5).total_microseconds() * 1e-6;
        sum_time_5 = sum_time_5 * n_time_5 + time_cost;
        n_time_5++;
        sum_time_5 = sum_time_5 / n_time_5;
        std::cout << std::fixed << std::setprecision(6) << "[suitable map point selection] time cost = " << sum_time_5 << std::endl;
        // time test

        good_features_msckf.clear();
    }

    for (auto const &feat : features_up_msckf)
    {
        good_features_msckf.push_back(feat->position_global);
        feat->to_delete = true;
    }

    featureTracker->getFeatureDatabase()->cleanUp();

    if ((int)state_ptr->clones_imu.size() > state_ptr->options.max_clone_size)
    {
        featureTracker->getFeatureDatabase()->cleanUpOldmeasurements(state_ptr->margtimestep());
    }

    if (marginalize_status == MarginalizeStatus::OLD)
    {
        stateHelper::marginalizeOldClone(state_ptr);
    }
    else
    {
        stateHelper::marginalizeNewClone(state_ptr);
    }

    rT7 = boost::posix_time::microsec_clock::local_time();

    double time_track = (rT2 - rT1).total_microseconds() * 1e-6;
    double time_prop = (rT3 - rT2).total_microseconds() * 1e-6;
    double time_msckf = (rT4 - rT3).total_microseconds() * 1e-6;
    double time_slam_update = (rT5 - rT4).total_microseconds() * 1e-6;
    double time_slam_delay = (rT6 - rT5).total_microseconds() * 1e-6;
    double time_marg = (rT7 - rT6).total_microseconds() * 1e-6;
    double time_total = (rT7 - rT1).total_microseconds() * 1e-6;

    // Time test
    /*
    std::cout << std::fixed << "[processImage]: " << time_track << " seconds for tracking" << std::endl;
    std::cout << std::fixed << "[featureUpdate]: " << time_prop << " seconds for propagation" << std::endl;
    std::cout << std::fixed << "[featureUpdate]: " << time_msckf << " seconds for MSCKF update (" << (int)features_up_msckf.size() << " feats)" << std::endl;

    if (state_ptr->options.max_slam_features > 0)
    {
        std::cout << std::fixed << "[featureUpdate]: " << time_slam_update << " seconds for SLAM update (" << (int)state_ptr->map_points.size() << " feats)" << std::endl;
        std::cout << std::fixed << "[featureUpdate]: " << time_slam_delay << " seconds for SLAM delayed init (" << (int)features_slam_delayed.size() << " feats)" << std::endl;
    }

    std::cout << std::fixed << "[featureUpdate]: " << time_marg << " seconds for re-tri & marg (" << (int)state_ptr->clones_imu.size() << " clones in state)" << std::endl;

    std::stringstream ss;
    ss << "[featureUpdate]: " << std::setprecision(4) << time_total << " seconds for total (camera";

    for (const auto &id : image_measurements.camera_ids) ss << " " << id;

    ss << ")" << std::endl;

    std::cout << ss.str();
    */
    // Time test

    if (timelastupdate != -1 && state_ptr->clones_imu.find(timelastupdate) != state_ptr->clones_imu.end())
    {
        Eigen::Matrix<double, 3, 1> dx = state_ptr->imu_ptr->getPos() - state_ptr->clones_imu.at(timelastupdate)->getPos();
        distance += dx.norm();
    }
    timelastupdate = image_measurements.timestamp;

    // display
    pubOdometry(state_ptr, timelastupdate);
    pubPath(state_ptr, timelastupdate);
    pubHistoryPoints(points_history, timelastupdate);
    pubWindowPoints(state_ptr, timelastupdate);
    pubHistoryVoxels(voxels_history, timelastupdate);
    pubVisitVoxels(voxels_visit, timelastupdate);
    // display

    std::cout << std::fixed << std::setprecision(3) << "q_GtoI = " << state_ptr->imu_ptr->getQuat()(0) << " " << state_ptr->imu_ptr->getQuat()(1) << " "
        << state_ptr->imu_ptr->getQuat()(2) << " " << state_ptr->imu_ptr->getQuat()(3) << std::endl;

    std::cout << std::fixed << std::setprecision(3) << "p_IinG = " << state_ptr->imu_ptr->getPos()(0) << " " << state_ptr->imu_ptr->getPos()(1) << " "
        << state_ptr->imu_ptr->getPos()(2) << std::endl;

    std::cout << std::fixed << std::setprecision(2) << "dist = " << distance << " meters" << std::endl;

    std::cout << std::fixed << std::setprecision(4) << "bg = " << state_ptr->imu_ptr->getBg()(0) << " " << state_ptr->imu_ptr->getBg()(1) << " "
        << state_ptr->imu_ptr->getBg()(2) << std::endl;

    std::cout << std::fixed << std::setprecision(4) << "ba = " << state_ptr->imu_ptr->getBa()(0) << " " << state_ptr->imu_ptr->getBa()(1) << " "
        << state_ptr->imu_ptr->getBa()(2) << std::endl;

    std::ofstream foutC(std::string(output_path + "/pose.txt"), std::ios::app);
    foutC.setf(std::ios::scientific, std::ios::floatfield);
    foutC.precision(6);
    foutC << std::fixed << image_measurements.timestamp + state_ptr->calib_dt_imu_cam->value()(0) << " " << state_ptr->imu_ptr->getPos()(0) << " " << state_ptr->imu_ptr->getPos()(1) << " " << state_ptr->imu_ptr->getPos()(2) << " "
        << state_ptr->imu_ptr->getQuat()(0) << " " << state_ptr->imu_ptr->getQuat()(1) << " " << state_ptr->imu_ptr->getQuat()(2) << " " << state_ptr->imu_ptr->getQuat()(3) << std::endl;
    foutC.close();

    if (state_ptr->options.do_calib_camera_timeoffset)
        std::cout << std::fixed << std::setprecision(5) << "camera-imu timeoffset = " << state_ptr->calib_dt_imu_cam->value()(0) << std::endl;

    if (state_ptr->options.do_calib_camera_intrinsics)
    {
        for (int i = 0; i < 2; i++)
        {
            std::shared_ptr<vec> calib = state_ptr->cam_intrinsics.at(i);

            std::cout << std::fixed << std::setprecision(3) << "cam" << (int)i << " intrinsics = " << calib->value()(0) << " " << calib->value()(1) << " "
                << calib->value()(2) << " " << calib->value()(3) << " " << calib->value()(4) << " " << calib->value()(5) << " "
                << calib->value()(6) << " " << calib->value()(7) << std::endl;

        }
    }

    if (state_ptr->options.do_calib_camera_pose)
    {
        for (int i = 0; i < 2; i++)
        {
            std::shared_ptr<poseJpl> calib = state_ptr->calib_cam_imu.at(i);

            std::cout << std::fixed << std::setprecision(3) << "cam" << (int)i << " extrinsics = " << calib->getQuat()(0) << " " << calib->getQuat()(1) << " "
                << calib->getQuat()(2) << " " << calib->getQuat()(3) << " " << calib->getPos()(0) << " " << calib->getPos()(1) << " "
                << calib->getPos()(2) << std::endl;
        }
    }

    if (state_ptr->options.do_calib_imu_intrinsics && state_ptr->options.imu_model == ImuModel::KALIBR)
    {
        std::cout << std::fixed << std::setprecision(3) << "q_GYROtoI = " << state_ptr->calib_imu_gyr->value()(0) << " " << state_ptr->calib_imu_gyr->value()(1) << " "
            << state_ptr->calib_imu_gyr->value()(2) << " " << state_ptr->calib_imu_gyr->value()(3) << std::endl;
    }

    if (state_ptr->options.do_calib_imu_intrinsics && state_ptr->options.imu_model == ImuModel::RPNG)
    {
        std::cout << std::fixed << std::setprecision(3) << "q_ACCtoI = " << state_ptr->calib_imu_acc->value()(0) << " " << state_ptr->calib_imu_acc->value()(1) << " "
            << state_ptr->calib_imu_acc->value()(2) << " " << state_ptr->calib_imu_acc->value()(3) << std::endl;
    }

    if (state_ptr->options.do_calib_imu_intrinsics && state_ptr->options.imu_model == ImuModel::KALIBR)
    {
        std::cout << std::fixed << std::setprecision(4) << "Dw = | " << state_ptr->calib_imu_dw->value()(0) << ", " << state_ptr->calib_imu_dw->value()(1) << ", "
            << state_ptr->calib_imu_dw->value()(2) << " | " << state_ptr->calib_imu_dw->value()(3) << ", " << state_ptr->calib_imu_dw->value()(4)
            << " | " << state_ptr->calib_imu_dw->value()(5) << " |" << std::endl;

        std::cout << std::fixed << std::setprecision(4) << "Da = | " << state_ptr->calib_imu_da->value()(0) << ", " << state_ptr->calib_imu_da->value()(1) << ", "
            << state_ptr->calib_imu_da->value()(2) << " | " << state_ptr->calib_imu_da->value()(3) << ", " << state_ptr->calib_imu_da->value()(4)
            << " | " << state_ptr->calib_imu_da->value()(5) << " |" << std::endl;
    }

    if (state_ptr->options.do_calib_imu_intrinsics && state_ptr->options.imu_model == ImuModel::RPNG)
    {
        std::cout << std::fixed << std::setprecision(4) << "Dw = | " << state_ptr->calib_imu_dw->value()(0) << " | " << state_ptr->calib_imu_dw->value()(1) << ", "
            << state_ptr->calib_imu_dw->value()(2) << " | " << state_ptr->calib_imu_dw->value()(3) << ", " << state_ptr->calib_imu_dw->value()(4)
            << ", " << state_ptr->calib_imu_dw->value()(5) << " |" << std::endl;

        std::cout << std::fixed << std::setprecision(4) << "Da = | " << state_ptr->calib_imu_da->value()(0) << " | " << state_ptr->calib_imu_da->value()(1) << ", "
            << state_ptr->calib_imu_da->value()(2) << " | " << state_ptr->calib_imu_da->value()(3) << ", " << state_ptr->calib_imu_da->value()(4)
            << ", " << state_ptr->calib_imu_da->value()(5) << " |" << std::endl;
    }

    if (state_ptr->options.do_calib_imu_intrinsics && state_ptr->options.do_calib_imu_g_sensitivity)
    {
        std::cout << std::fixed << std::setprecision(4) << "Tg = | " << state_ptr->calib_imu_tg->value()(0) << ", " << state_ptr->calib_imu_tg->value()(1) << ", "
            << state_ptr->calib_imu_tg->value()(2) << " | " << state_ptr->calib_imu_tg->value()(3) << ", " << state_ptr->calib_imu_tg->value()(4) << ", "
            << state_ptr->calib_imu_tg->value()(5) << " | " << state_ptr->calib_imu_tg->value()(6) << ", " << state_ptr->calib_imu_tg->value()(7) << ", "
            << state_ptr->calib_imu_tg->value()(8) << " |" << std::endl;
    }

    std::cout << std::endl;
}
    void triangulateActiveTracks(cameraData &image_measurements) {
    boost::posix_time::ptime retri_rT1, retri_rT2, retri_rT3;
    retri_rT1 = boost::posix_time::microsec_clock::local_time();

    assert(state_ptr->clones_imu.find(image_measurements.timestamp) != state_ptr->clones_imu.end());
    active_tracks_time = image_measurements.timestamp;

    // draw
    /*
    active_image = cv::Mat();
    featureTracker->displayActive(active_image, 255, 255, 255, 255, 255, 255, " ");
    if (!active_image.empty())
    {
        active_image = active_image(cv::Rect(0, 0, image_measurements.images.at(0).cols, image_measurements.images.at(0).rows));
    }
    */
    // draw

    active_tracks_pos_world.clear();
    active_tracks_uvd.clear();

    auto last_obs = featureTracker->getLastObs();
    auto last_ids = featureTracker->getLastIds();

    std::map<size_t, Eigen::Matrix3d> active_feat_linsys_A_new;
    std::map<size_t, Eigen::Vector3d> active_feat_linsys_b_new;
    std::map<size_t, int> active_feat_linsys_count_new;
    std::unordered_map<size_t, Eigen::Vector3d> active_tracks_pos_world_new;

    std::map<size_t, cv::Point2f> feat_uvs_in_cam_0;

    // display
    assert(state_ptr->clones_frame.find(active_tracks_time) != state_ptr->clones_frame.end());

    cv::Mat img_left = image_measurements.images.at(0);
    cv::Mat img_right = image_measurements.images.at(1);

    cv::Mat color_left = convertCvImage(img_left);
    cv::Mat color_right = convertCvImage(img_right);

    cv::Vec3b color_feat = cv::Vec3b(0, 255, 0);
    // display

    for (auto const &cam_id : image_measurements.camera_ids)
    {
        Eigen::Matrix3d R_GtoI = state_ptr->clones_imu.at(active_tracks_time)->getRot();
        Eigen::Vector3d p_IinG = state_ptr->clones_imu.at(active_tracks_time)->getPos();

        Eigen::Matrix3d R_ItoC = state_ptr->calib_cam_imu.at(cam_id)->getRot();
        Eigen::Vector3d p_IinC = state_ptr->calib_cam_imu.at(cam_id)->getPos();

        Eigen::Matrix3d R_GtoCi = R_ItoC * R_GtoI;
        Eigen::Vector3d p_CiinG = p_IinG - R_GtoCi.transpose() * p_IinC;

        assert(last_obs.find(cam_id) != last_obs.end());
        assert(last_ids.find(cam_id) != last_ids.end());

        for (size_t i = 0; i < last_obs.at(cam_id).size(); i++)
        {
            size_t feature_id = last_ids.at(cam_id).at(i);
            cv::Point2f pt_d = last_obs.at(cam_id).at(i).pt;

            // display
            if (pt_d.x > 5 && pt_d.x < wG[0] - 5 && pt_d.y > 5 && pt_d.y < hG[0] - 5)
            {
                if (cam_id == 0)
                    drawPointL(color_left, pt_d.x, pt_d.y, color_feat);
                else
                    drawPointL(color_right, pt_d.x, pt_d.y, color_feat);
            }
            // display

            if (cam_id == 0) feat_uvs_in_cam_0[feature_id] = pt_d;

            if (state_ptr->map_points.find(feature_id) != state_ptr->map_points.end()) continue;

            cv::Point2f pt_n = state_ptr->cam_intrinsics_cameras.at(cam_id)->undistortCV(pt_d);
            Eigen::Matrix<double, 3, 1> b_i;
            b_i << pt_n.x, pt_n.y, 1;
            b_i = R_GtoCi.transpose() * b_i;
            b_i = b_i / b_i.norm();
            Eigen::Matrix3d B_perp = quatType::skewSymmetric(b_i);

            Eigen::Matrix3d Ai = B_perp.transpose() * B_perp;
            Eigen::Vector3d bi = Ai * p_CiinG;

            if (active_feat_linsys_A.find(feature_id) == active_feat_linsys_A.end())
            {
                active_feat_linsys_A_new.insert({feature_id, Ai});
                active_feat_linsys_b_new.insert({feature_id, bi});
                active_feat_linsys_count_new.insert({feature_id, 1});
            }
            else
            {
                active_feat_linsys_A_new[feature_id] = Ai + active_feat_linsys_A[feature_id];
                active_feat_linsys_b_new[feature_id] = bi + active_feat_linsys_b[feature_id];
                active_feat_linsys_count_new[feature_id] = 1 + active_feat_linsys_count[feature_id];
            }

            if (active_feat_linsys_count_new.at(feature_id) > 3)
            {
                Eigen::Matrix3d A = active_feat_linsys_A_new[feature_id];
                Eigen::Vector3d b = active_feat_linsys_b_new[feature_id];
                Eigen::MatrixXd p_FinG = A.colPivHouseholderQr().solve(b);
                Eigen::MatrixXd p_FinCi = R_GtoCi * (p_FinG - p_CiinG);

                Eigen::JacobiSVD<Eigen::Matrix3d> svd(A);
                Eigen::MatrixXd singularValues;
                singularValues.resize(svd.singularValues().rows(), 1);
                singularValues = svd.singularValues();
                double cond_A = singularValues(0, 0) / singularValues(singularValues.rows() - 1, 0);

                if (std::abs(cond_A) <= odometry_options.featinit_options.max_cond_number && p_FinCi(2, 0) >= odometry_options.featinit_options.min_dist &&
                    p_FinCi(2, 0) <= odometry_options.featinit_options.max_dist && !std::isnan(p_FinCi.norm()))
                {
                    active_tracks_pos_world_new[feature_id] = p_FinG;
                }
            }
        }
    }

    // display
    cv::Mat stereo_image;
    cv::vconcat(color_left, color_right, stereo_image);

    /*
    cv::imshow("Stereo Image", stereo_image);
    cv::waitKey(1);
    */
    pubFeatImage(stereo_image, active_tracks_time);

    img_left.release();
    img_right.release();

    color_left.release();
    color_right.release();

    stereo_image.release();
    // display

    size_t total_triangulated = active_tracks_pos_world.size();

    active_feat_linsys_A = active_feat_linsys_A_new;
    active_feat_linsys_b = active_feat_linsys_b_new;
    active_feat_linsys_count = active_feat_linsys_count_new;
    active_tracks_pos_world = active_tracks_pos_world_new;
    retri_rT2 = boost::posix_time::microsec_clock::local_time();

    if (active_tracks_pos_world.empty() && state_ptr->map_points.empty()) return;

    for (const auto &feat : state_ptr->map_points)
    {
        Eigen::Vector3d p_FinG = feat.second->getPointXYZ(false);
        active_tracks_pos_world[feat.second->feature_id] = p_FinG;
    }

    std::shared_ptr<vec> distortion = state_ptr->cam_intrinsics.at(0);
    std::shared_ptr<poseJpl> calibration = state_ptr->calib_cam_imu.at(0);
    Eigen::Matrix<double, 3, 3> R_ItoC = calibration->getRot();
    Eigen::Matrix<double, 3, 1> p_IinC = calibration->getPos();

    std::shared_ptr<poseJpl> clone_Ii = state_ptr->clones_imu.at(active_tracks_time);
    Eigen::Matrix3d R_GtoIi = clone_Ii->getRot();
    Eigen::Vector3d p_IiinG = clone_Ii->getPos();

    for (const auto &feat : active_tracks_pos_world)
    {
        if (feat_uvs_in_cam_0.find(feat.first) == feat_uvs_in_cam_0.end()) continue;

        Eigen::Vector3d p_FinIi = R_GtoIi * (feat.second - p_IiinG);
        Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC;

        double depth = p_FinCi(2);
        Eigen::Vector2d uv_dist;

        if (feat_uvs_in_cam_0.find(feat.first) != feat_uvs_in_cam_0.end())
        {
            uv_dist << (double)feat_uvs_in_cam_0.at(feat.first).x, (double)feat_uvs_in_cam_0.at(feat.first).y;
        }
        else
        {
            Eigen::Vector2d uv_norm;
            uv_norm << p_FinCi(0) / depth, p_FinCi(1) / depth;
            uv_dist = state_ptr->cam_intrinsics_cameras.at(0)->distortD(uv_norm);
        }

        if (depth < 0.1) continue;

        int width = state_ptr->cam_intrinsics_cameras.at(0)->w();
        int height = state_ptr->cam_intrinsics_cameras.at(0)->h();

        if (uv_dist(0) < 0 || (int)uv_dist(0) >= width || uv_dist(1) < 0 || (int)uv_dist(1) >= height)
        {
            continue;
        }

        Eigen::Vector3d uvd;
        uvd << uv_dist, depth;
        active_tracks_uvd.insert({feat.first, uvd});
    }
    retri_rT3 = boost::posix_time::microsec_clock::local_time();

    // Time test
    /*
    std::cout << std::fixed << "[triangulateActiveTracks]: " << (retri_rT2 - retri_rT1).total_microseconds() * 1e-6 << " seconds for triangulation ("
        << total_triangulated << " tri of " << active_feat_linsys_A.size() << " active)" << std::endl;

    std::cout << std::fixed << "[triangulateActiveTracks]: " << (retri_rT3 - retri_rT2).total_microseconds() * 1e-6 << " seconds for re-projection into current" << std::endl;

    std::cout << std::fixed << "[triangulateActiveTracks]: " << (retri_rT3 - retri_rT1).total_microseconds() * 1e-6 << " seconds total" << std::endl;
    */
    // Time test
}
    void getRecentVoxel(double timestamp, pcl::PointCloud<pcl::PointXYZI>::Ptr voxels_visit) {
    std::vector<Eigen::Matrix<short, 3, 1>>().swap(recent_voxels);

    // display
    voxels_visit->clear();
    // display

    for (const auto &point : active_tracks_pos_world)
    {
        short kx = static_cast<short>(point.second[0] / odometry_options.voxel_size);
        short ky = static_cast<short>(point.second[1] / odometry_options.voxel_size);
        short kz = static_cast<short>(point.second[2] / odometry_options.voxel_size);

        for (short kxx = kx - odometry_options.nb_voxels_visited; kxx < kx + odometry_options.nb_voxels_visited + 1; kxx++)
        {
            for (short kyy = ky - odometry_options.nb_voxels_visited; kyy < ky + odometry_options.nb_voxels_visited + 1; kyy++)
            {
                for (short kzz = kz - odometry_options.nb_voxels_visited; kzz < kz + odometry_options.nb_voxels_visited + 1; kzz++)
                {
                    voxelHashMap::iterator search = voxel_map.find(voxel(kxx, kyy, kzz));

                    if(search != voxel_map.end())
                    {
                        auto &voxel_block = (search.value());

                        if (voxel_block.last_visit_time < timestamp && voxel_block.NumPoints() > 0)
                        {
                            Eigen::Matrix<short, 3, 1> voxel_temp;

                            voxel_temp[0] = kxx;
                            voxel_temp[1] = kyy;
                            voxel_temp[2] = kzz;

                            recent_voxels.push_back(voxel_temp);
                            voxel_block.last_visit_time = timestamp;

                            // display
                            pcl::PointXYZI point_temp;

                            point_temp.x = kxx >= 0 ? (kxx + 0.5) * odometry_options.voxel_size : (kxx - 0.5) * odometry_options.voxel_size;
                            point_temp.y = kyy >= 0 ? (kyy + 0.5) * odometry_options.voxel_size : (kyy - 0.5) * odometry_options.voxel_size;
                            point_temp.z = kzz >= 0 ? (kzz + 0.5) * odometry_options.voxel_size : (kzz - 0.5) * odometry_options.voxel_size;
                            point_temp.intensity = 1;

                            voxels_visit->points.push_back(point_temp);
                            // display
                        }
                    }
                }
            }
        }
    }
}
    bool tryToInitialize(cameraData &image_measurements) {
    camera_queue_init.push_back(image_measurements.timestamp);

    assert(newest_fh->timestamp == image_measurements.timestamp);
    frame_queue_init.push_back(newest_fh);

    double timestamp;
    Eigen::MatrixXd covariance;
    std::vector<std::shared_ptr<baseType>> order;
    auto init_rT1 = boost::posix_time::microsec_clock::local_time();

    bool success = initializer_ptr->initialize(timestamp, covariance, order, state_ptr->imu_ptr, true);

    if (success)
    {
        stateHelper::setInitialCovariance(state_ptr, covariance, order);

        state_ptr->timestamp = timestamp;
        startup_time = timestamp;

        featureTracker->getFeatureDatabase()->cleanUpOldmeasurements(state_ptr->timestamp);
        featureTracker->setNumFeatures(std::floor((double)odometry_options.num_pts / (double)2.0));

        auto init_rT2 = boost::posix_time::microsec_clock::local_time();
        std::cout << std::fixed << "[initialize]: Successful initialization in " << (init_rT2 - init_rT1).total_microseconds() * 1e-6 << " seconds" << std::endl;
        std::cout << std::fixed << "[initialize]: position = " << state_ptr->imu_ptr->getPos()(0) << ", " << state_ptr->imu_ptr->getPos()(1) << ", "
            << state_ptr->imu_ptr->getPos()(2) << std::endl;
        std::cout << std::fixed << "[initialize]: orientation = " << state_ptr->imu_ptr->getQuat()(0) << ", " << state_ptr->imu_ptr->getQuat()(1) << ", " << state_ptr->imu_ptr->getQuat()(2)
            << ", " << state_ptr->imu_ptr->getQuat()(3) << std::endl;
        std::cout << std::fixed << "[initialize]: velocity = " << state_ptr->imu_ptr->getVel()(0) << ", " << state_ptr->imu_ptr->getVel()(1) << ", "
            << state_ptr->imu_ptr->getVel()(2) << std::endl;
        std::cout << std::fixed << "[initialize]: bias accel = " << state_ptr->imu_ptr->getBa()(0) << ", " << state_ptr->imu_ptr->getBa()(1) << ", "
            << state_ptr->imu_ptr->getBa()(2) << std::endl;
        std::cout << std::fixed << "[initialize]: bias gyro = " << state_ptr->imu_ptr->getBg()(0) << ", " << state_ptr->imu_ptr->getBg()(1) << ", "
            << state_ptr->imu_ptr->getBg()(2) << std::endl;

        std::vector<double> camera_timestamps_to_init;
        for (size_t i = 0; i < camera_queue_init.size(); i++)
        {
            if (camera_queue_init.at(i) > timestamp)
                camera_timestamps_to_init.push_back(camera_queue_init.at(i));
        }

        std::vector<std::shared_ptr<frame>> camera_frames_to_init;
        for (size_t i = 0; i < frame_queue_init.size(); i++)
        {
            if (frame_queue_init.at(i)->timestamp > timestamp)
                camera_frames_to_init.push_back(frame_queue_init.at(i));
        }

        size_t clone_rate = (size_t)((double)camera_timestamps_to_init.size() / (double)odometry_options.state_options.max_clone_size) + 1;

        for (size_t i = 0; i < camera_timestamps_to_init.size(); i += clone_rate)
        {
            propagator_ptr->propagateAndClone(state_ptr, camera_timestamps_to_init.at(i));

            if (state_ptr->clones_frame.find(camera_frames_to_init.at(i)->timestamp) == state_ptr->clones_frame.end())
                state_ptr->clones_frame[camera_frames_to_init.at(i)->timestamp] = camera_frames_to_init.at(i);

            stateHelper::marginalizeOldClone(state_ptr);
        }

        std::cout << "[initialize]: Moved the state forward " << state_ptr->timestamp - timestamp << " seconds" << std::endl;
        camera_queue_init.clear();

        frame_queue_init.clear();

        return true;
    }
    else
    {
        auto init_rT2 = boost::posix_time::microsec_clock::local_time();
        std::cout << std::fixed << "[initialize]: Failed initialization in " << (init_rT2 - init_rT1).total_microseconds() * 1e-6 << " seconds" << std::endl;

        double oldest_time = image_measurements.timestamp - odometry_options.init_options.init_window_time - 0.10;

        auto it0 = camera_queue_init.begin();

        while (it0 != camera_queue_init.end())
        {
            if (*it0 < oldest_time)
                it0 = camera_queue_init.erase(it0);
            else
                it0++;
        }

        auto it1 = frame_queue_init.begin();

        while (it1 != frame_queue_init.end())
        {
            if ((*it1)->timestamp < oldest_time)
            {
                assert((*it1)->v_feat_ptr[0].size() == 0);
                assert((*it1)->v_feat_ptr[1].size() == 0);
                (*it1)->release();
                it1 = frame_queue_init.erase(it1);
            }
            else
                it1++;
        }

        return false;
    }
}
    std::queue<cameraData> camera_buffer;
    std::queue<imuData> imu_buffer;
    std::map<int, double> camera_last_timestamp;
    odometryOptions odometry_options;
    std::shared_ptr<trackKLT> featureTracker;
    std::shared_ptr<inertialInitializer> initializer_ptr;
    double startup_time;
    double current_time;
    double time_newest_imu;
    std::vector<std::pair<double, std::pair<Eigen::Vector3d, Eigen::Vector3d>>> imu_meas;
    std::vector<imuState> imu_states;
    std::shared_ptr<state> state_ptr;
    imuData last_imu_data;
    std::shared_ptr<propagator> propagator_ptr;
    std::shared_ptr<updaterMsckf> updaterMsckf_ptr;
    std::shared_ptr<updaterSlam> updaterSlam_ptr;
    std::shared_ptr<gammaPixel> gammaPixel_ptr;
    std::shared_ptr<frame> newest_fh;
    std::vector<double> camera_queue_init;
    std::vector<std::shared_ptr<frame>> frame_queue_init;
    voxelHashMap voxel_map;
    int frame_count;
    MarginalizeStatus marginalize_status;
    double last_time_image;
    std::vector<Eigen::Vector3d> good_features_msckf;
    double active_tracks_time = -1;
    std::unordered_map<size_t, Eigen::Vector3d> active_tracks_pos_world;
    std::unordered_map<size_t, Eigen::Vector3d> active_tracks_uvd;
    cv::Mat active_image;
    std::map<size_t, Eigen::Matrix3d> active_feat_linsys_A;
    std::map<size_t, Eigen::Vector3d> active_feat_linsys_b;
    std::map<size_t, int> active_feat_linsys_count;
    std::vector<Eigen::Matrix<short, 3, 1>> recent_voxels;
    std::ofstream of_statistics;
    boost::posix_time::ptime rT1, rT2, rT3, rT4, rT5, rT6, rT7;
    double timelastupdate = -1;
    double distance = 0;
    double sum_time_1 = 0.0;
    double sum_time_2 = 0.0;
    double sum_time_3 = 0.0;
    double sum_time_4 = 0.0;
    double sum_time_5 = 0.0;
    double sum_time_6 = 0.0;
    double sum_time_7 = 0.0;
    double sum_time_sum = 0.0;
    int n_time_1 = 0;
    int n_time_2 = 0;
    int n_time_3 = 0;
    int n_time_4 = 0;
    int n_time_5 = 0;
    int n_time_6 = 0;
    int n_time_7 = 0;
    int n_time_sum = 0;
  public:
    voxelStereoVio() {
    readParameters();

    initialValue();

    allocateMemory();

    sub_imu = nh.subscribe<sensor_msgs::Imu>(imu_topic, 500, &voxelStereoVio::imuHandler, this);

    auto sub_img_left = std::make_shared<message_filters::Subscriber<sensor_msgs::Image>>(nh, image_left_topic, 5);
    auto sub_img_right = std::make_shared<message_filters::Subscriber<sensor_msgs::Image>>(nh, image_right_topic, 5);
    auto sync = std::make_shared<message_filters::Synchronizer<SyncStereoImage>>(SyncStereoImage(10), *sub_img_left, *sub_img_right);
    sync->registerCallback(boost::bind(&voxelStereoVio::stereoImageHandler, this, _1, _2, 0, 1));

    sync_cam.push_back(sync);
    sync_subs_cam.push_back(sub_img_left);
    sync_subs_cam.push_back(sub_img_right);

    // display
    pub_feat_image = it.advertise("camera/stereo_feat_image", 5);
    pub_odom = nh.advertise<nav_msgs::Odometry>("vio/odom", 5);
    pub_path = nh.advertise<nav_msgs::Path>("vio/path", 5);
    pub_points_history = nh.advertise<sensor_msgs::PointCloud2>("vio/history_map_points", 2);
    pub_points_window = nh.advertise<sensor_msgs::PointCloud2>("vio/window_map_points", 2);
    pub_voxels_history = nh.advertise<sensor_msgs::PointCloud2>("vio/history_voxels", 2);
    pub_voxels_visit = nh.advertise<sensor_msgs::PointCloud2>("vio/visit_voxels", 2);

    points_history.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
    points_window.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
    voxels_history.reset(new pcl::PointCloud<pcl::PointXYZI>());
    voxels_visit.reset(new pcl::PointCloud<pcl::PointXYZI>());
    // display

    // odometry_options.recordParameters();
}
    void readParameters() {
    int para_int;
    double para_double;
    bool para_bool;
    std::string str_temp;

    nh.param<std::string>("common/image_left_topic", image_left_topic, "/cam0/image_raw");
    nh.param<std::string>("common/image_right_topic", image_right_topic, "/cam1/image_raw");
    nh.param<std::string>("common/imu_topic", imu_topic, "/imu0");
    nh.param<std::string>("output_path", output_path, "");

    nh.param<bool>("state_parameter/use_fej", para_bool, false); odometry_options.state_options.do_fej = para_bool;
    nh.param<bool>("state_parameter/calib_cam_extrinsics", para_bool, false); odometry_options.state_options.do_calib_camera_pose = para_bool;
    nh.param<bool>("state_parameter/calib_cam_intrinsics", para_bool, false); odometry_options.state_options.do_calib_camera_intrinsics = para_bool;
    nh.param<bool>("state_parameter/calib_cam_timeoffset", para_bool, false); odometry_options.state_options.do_calib_camera_timeoffset = para_bool;
    nh.param<bool>("state_parameter/calib_imu_intrinsics", para_bool, false); odometry_options.state_options.do_calib_imu_intrinsics = para_bool;
    nh.param<bool>("state_parameter/calib_imu_g_sensitivity", para_bool, false); odometry_options.state_options.do_calib_imu_g_sensitivity = para_bool;

    nh.param<std::string>("state_parameter/imu_intrinsics_model", str_temp, "kalibr");
    if (str_temp == "kalibr" || str_temp == "calibrated") odometry_options.state_options.imu_model = ImuModel::KALIBR;
    else if (str_temp == "rpng") odometry_options.state_options.imu_model = ImuModel::RPNG;
    else {
        std::cout << "Invalid IMU model: " << str_temp << std::endl;
        std::cout << "Please select a valid model: kalibr, rpng." << std::endl;
        std::exit(EXIT_FAILURE);
    }
    if (str_temp == "calibrated")
    {
        odometry_options.state_options.do_calib_imu_intrinsics = false;
        odometry_options.state_options.do_calib_imu_g_sensitivity = false;
    }

    nh.param<int>("state_parameter/max_clones", para_int, 11); odometry_options.state_options.max_clone_size = para_int;
    nh.param<int>("state_parameter/max_slam", para_int, 25); odometry_options.state_options.max_slam_features = para_int;
    nh.param<int>("state_parameter/max_slam_in_update", para_int, 1000); odometry_options.state_options.max_slam_in_update = para_int;
    nh.param<int>("state_parameter/max_msckf_in_update", para_int, 1000); odometry_options.state_options.max_msckf_in_update = para_int;

    nh.param<double>("initializer_parameter/init_window_time", para_double, 1.0); odometry_options.init_options.init_window_time = para_double;
    nh.param<double>("initializer_parameter/init_imu_thresh", para_double, 1.0); odometry_options.init_options.init_imu_thresh = para_double;
    nh.param<double>("initializer_parameter/init_max_disparity", para_double, 1.0); odometry_options.init_options.init_max_disparity = para_double;
    nh.param<int>("initializer_parameter/init_max_features", para_int, 50); odometry_options.init_options.init_max_features = para_int;
    nh.param<bool>("initializer_parameter/init_dyn_use", para_bool, false); odometry_options.init_options.init_dyn_use = para_bool;
    nh.param<bool>("initializer_parameter/init_dyn_mle_opt_calib", para_bool, false); odometry_options.init_options.init_dyn_mle_opt_calib = para_bool;
    nh.param<int>("initializer_parameter/init_dyn_mle_max_iter", para_int, 20); odometry_options.init_options.init_dyn_mle_max_iter = para_int;
    nh.param<int>("initializer_parameter/init_dyn_mle_max_threads", para_int, 20); odometry_options.init_options.init_dyn_mle_max_threads = para_int;
    nh.param<double>("initializer_parameter/init_dyn_mle_max_time", para_double, 5.0); odometry_options.init_options.init_dyn_mle_max_time = para_double;
    nh.param<int>("initializer_parameter/init_dyn_num_pose", para_int, 5); odometry_options.init_options.init_dyn_num_pose = para_int;
    nh.param<double>("initializer_parameter/init_dyn_min_deg", para_double, 45.0); odometry_options.init_options.init_dyn_min_deg = para_double;
    nh.param<double>("initializer_parameter/init_dyn_inflation_ori", para_double, 10.0); odometry_options.init_options.init_dyn_inflation_orientation = para_double;
    nh.param<double>("initializer_parameter/init_dyn_inflation_vel", para_double, 10.0); odometry_options.init_options.init_dyn_inflation_velocity = para_double;
    nh.param<double>("initializer_parameter/init_dyn_inflation_bg", para_double, 100.0); odometry_options.init_options.init_dyn_inflation_bias_gyro = para_double;
    nh.param<double>("initializer_parameter/init_dyn_inflation_ba", para_double, 100.0); odometry_options.init_options.init_dyn_inflation_bias_accel = para_double;
    nh.param<double>("initializer_parameter/init_dyn_min_rec_cond", para_double, 1e-15); odometry_options.init_options.init_dyn_min_rec_cond = para_double;

    std::vector<double> v_bias_acc, v_bias_gyr;
    nh.param<std::vector<double>>("initializer_parameter/init_dyn_bias_g", v_bias_gyr, std::vector<double>());
    nh.param<std::vector<double>>("initializer_parameter/init_dyn_bias_a", v_bias_acc, std::vector<double>());
    odometry_options.init_options.init_dyn_bias_g << v_bias_gyr.at(0), v_bias_gyr.at(1), v_bias_gyr.at(2);
    odometry_options.init_options.init_dyn_bias_a << v_bias_acc.at(0), v_bias_acc.at(1), v_bias_acc.at(2);

    nh.param<double>("initializer_parameter/gravity_mag", para_double, 9.81); odometry_options.init_options.gravity_mag = para_double;
    nh.param<bool>("initializer_parameter/downsample_cameras", para_bool, false); odometry_options.init_options.downsample_cameras = para_bool;

    double calib_camimu_dt_left, calib_camimu_dt_right;
    nh.param<double>("camera_parameter/timeshift_cam_imu_left", calib_camimu_dt_left, 0.0);
    nh.param<double>("camera_parameter/timeshift_cam_imu_right", calib_camimu_dt_right, 0.0);
    odometry_options.calib_camimu_dt = calib_camimu_dt_left;

    std::string dist_model_left, dist_model_right;
    nh.param<std::string>("camera_parameter/distortion_model_left", dist_model_left, "radtan");
    nh.param<std::string>("camera_parameter/distortion_model_right", dist_model_right, "radtan");

    std::vector<double> cam_calib_1_left = {1, 1, 0, 0};
    std::vector<double> cam_calib_1_right = {1, 1, 0, 0};
    std::vector<double> cam_calib_2_left = {0, 0, 0, 0};
    std::vector<double> cam_calib_2_right = {0, 0, 0, 0};
    nh.param<std::vector<double>>("camera_parameter/intrinsics_left", cam_calib_1_left, std::vector<double>());
    nh.param<std::vector<double>>("camera_parameter/intrinsics_right", cam_calib_1_right, std::vector<double>());
    nh.param<std::vector<double>>("camera_parameter/distortion_coeffs_left", cam_calib_2_left, std::vector<double>());
    nh.param<std::vector<double>>("camera_parameter/distortion_coeffs_right", cam_calib_2_right, std::vector<double>());
    Eigen::VectorXd cam_calib_left = Eigen::VectorXd::Zero(8);
    Eigen::VectorXd cam_calib_right = Eigen::VectorXd::Zero(8);
    cam_calib_left << cam_calib_1_left.at(0), cam_calib_1_left.at(1), cam_calib_1_left.at(2), cam_calib_1_left.at(3),
                      cam_calib_2_left.at(0), cam_calib_2_left.at(1), cam_calib_2_left.at(2), cam_calib_2_left.at(3);
    cam_calib_right << cam_calib_1_right.at(0), cam_calib_1_right.at(1), cam_calib_1_right.at(2), cam_calib_1_right.at(3),
                       cam_calib_2_right.at(0), cam_calib_2_right.at(1), cam_calib_2_right.at(2), cam_calib_2_right.at(3);

    cam_calib_left(0) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    cam_calib_left(1) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    cam_calib_left(2) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    cam_calib_left(3) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;

    cam_calib_right(0) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    cam_calib_right(1) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    cam_calib_right(2) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    cam_calib_right(3) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;

    std::vector<int> matrix_wh_left = {1, 1};
    nh.param<std::vector<int>>("camera_parameter/resolution_left", matrix_wh_left, std::vector<int>());
    matrix_wh_left.at(0) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    matrix_wh_left.at(1) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;

    std::vector<int> matrix_wh_right = {1, 1};
    nh.param<std::vector<int>>("camera_parameter/resolution_right", matrix_wh_right, std::vector<int>());
    matrix_wh_right.at(0) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    matrix_wh_right.at(1) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;

    assert(matrix_wh_left.at(0) == matrix_wh_right.at(0));
    assert(matrix_wh_left.at(1) == matrix_wh_right.at(1));

    wG[0] = matrix_wh_left.at(0);
    hG[0] = matrix_wh_left.at(1);

    std::vector<double> v_T_imu_cam_left;
    nh.param<std::vector<double>>("camera_parameter/T_imu_cam_left", v_T_imu_cam_left, std::vector<double>());
    Eigen::Matrix4d T_imu_cam_left = mat44FromArray(v_T_imu_cam_left);

    std::vector<double> v_T_imu_cam_right;
    nh.param<std::vector<double>>("camera_parameter/T_imu_cam_right", v_T_imu_cam_right, std::vector<double>());
    Eigen::Matrix4d T_imu_cam_right = mat44FromArray(v_T_imu_cam_right);

    Eigen::Matrix<double, 7, 1> cam_eigen_left;
    cam_eigen_left.block<4, 1>(0, 0) = quatType::rotToQuat(T_imu_cam_left.block<3, 3>(0, 0).transpose());
    cam_eigen_left.block<3, 1>(4, 0) = - T_imu_cam_left.block<3, 3>(0, 0).transpose() * T_imu_cam_left.block<3, 1>(0, 3);

    Eigen::Matrix<double, 7, 1> cam_eigen_right;
    cam_eigen_right.block<4, 1>(0, 0) = quatType::rotToQuat(T_imu_cam_right.block<3, 3>(0, 0).transpose());
    cam_eigen_right.block<3, 1>(4, 0) = - T_imu_cam_right.block<3, 3>(0, 0).transpose() * T_imu_cam_right.block<3, 1>(0, 3);

    if (dist_model_left == "equidistant")
    {
        odometry_options.init_options.camera_intrinsics.insert({0, std::make_shared<cameraEqui>(matrix_wh_left.at(0), matrix_wh_left.at(1))});
        odometry_options.init_options.camera_intrinsics.at(0)->setValue(cam_calib_left);

        odometry_options.camera_intrinsics.insert({0, std::make_shared<cameraEqui>(matrix_wh_left.at(0), matrix_wh_left.at(1))});
        odometry_options.camera_intrinsics.at(0)->setValue(cam_calib_left);
    }
    else
    {
        odometry_options.init_options.camera_intrinsics.insert({0, std::make_shared<cameraRadtan>(matrix_wh_left.at(0), matrix_wh_left.at(1))});
        odometry_options.init_options.camera_intrinsics.at(0)->setValue(cam_calib_left);

        odometry_options.camera_intrinsics.insert({0, std::make_shared<cameraRadtan>(matrix_wh_left.at(0), matrix_wh_left.at(1))});
        odometry_options.camera_intrinsics.at(0)->setValue(cam_calib_left);
    }
    odometry_options.init_options.camera_extrinsics.insert({0, cam_eigen_left});
    odometry_options.camera_extrinsics.insert({0, cam_eigen_left});

    if (dist_model_right == "equidistant")
    {
        odometry_options.init_options.camera_intrinsics.insert({1, std::make_shared<cameraEqui>(matrix_wh_right.at(0), matrix_wh_right.at(1))});
        odometry_options.init_options.camera_intrinsics.at(1)->setValue(cam_calib_right);

        odometry_options.camera_intrinsics.insert({1, std::make_shared<cameraEqui>(matrix_wh_right.at(0), matrix_wh_right.at(1))});
        odometry_options.camera_intrinsics.at(1)->setValue(cam_calib_right);
    }
    else
    {
        odometry_options.init_options.camera_intrinsics.insert({1, std::make_shared<cameraRadtan>(matrix_wh_right.at(0), matrix_wh_right.at(1))});
        odometry_options.init_options.camera_intrinsics.at(1)->setValue(cam_calib_right);

        odometry_options.camera_intrinsics.insert({1, std::make_shared<cameraRadtan>(matrix_wh_right.at(0), matrix_wh_right.at(1))});
        odometry_options.camera_intrinsics.at(1)->setValue(cam_calib_right);
    }
    odometry_options.init_options.camera_extrinsics.insert({1, cam_eigen_right});
    odometry_options.camera_extrinsics.insert({1, cam_eigen_right});

    nh.param<bool>("odometry_parameter/use_mask", para_bool, false); odometry_options.use_mask = para_bool;
    if (odometry_options.use_mask)
    {
        std::string mask_left_path, mask_right_path;
        nh.param<std::string>("camera_parameter/mask_left_path", str_temp, ""); mask_left_path = str_temp;
        nh.param<std::string>("camera_parameter/mask_right_path", str_temp, ""); mask_right_path = str_temp;

        if (!boost::filesystem::exists(mask_left_path))
        {
            std::cout << "Invalid mask path: " << mask_left_path << std::endl;
            std::exit(EXIT_FAILURE);
        }
        cv::Mat mask_left = cv::imread(mask_left_path, cv::IMREAD_GRAYSCALE);

        if (!boost::filesystem::exists(mask_right_path))
        {
            std::cout << "Invalid mask path: " << mask_right_path << std::endl;
            std::exit(EXIT_FAILURE);
        }
        cv::Mat mask_right = cv::imread(mask_right_path, cv::IMREAD_GRAYSCALE);

        if (mask_left.cols != odometry_options.camera_intrinsics.at(0)->w() || mask_left.rows != odometry_options.camera_intrinsics.at(0)->h())
        {
            std::cout << "Mask size does not match left camera!" << std::endl;
            std::exit(EXIT_FAILURE);
        }

        if (mask_right.cols != odometry_options.camera_intrinsics.at(1)->w() || mask_right.rows != odometry_options.camera_intrinsics.at(1)->h())
        {
            std::cout << "Mask size does not match right camera!" << std::endl;
            std::exit(EXIT_FAILURE);
        }

        odometry_options.masks.insert({0, mask_left});
        odometry_options.masks.insert({1, mask_right});
    }

    nh.param<double>("imu_parameter/gyroscope_noise_density", para_double, 1.6968e-04); odometry_options.init_options.sigma_w = para_double;
    nh.param<double>("imu_parameter/gyroscope_random_walk", para_double, 1.9393e-05); odometry_options.init_options.sigma_wb = para_double;
    nh.param<double>("imu_parameter/accelerometer_noise_density", para_double, 2.0000e-3); odometry_options.init_options.sigma_a = para_double;
    nh.param<double>("imu_parameter/accelerometer_random_walk", para_double, 3.0000e-03); odometry_options.init_options.sigma_ab = para_double;
    nh.param<double>("imu_parameter/sigma_pix", para_double, 1.0); odometry_options.init_options.sigma_pix = para_double;

    std::vector<double> v_Tw;
    nh.param<std::vector<double>>("imu_parameter/Tw", v_Tw, std::vector<double>());
    Eigen::Matrix3d Tw = mat33FromArray(v_Tw);
    std::vector<double> v_Ta;
    nh.param<std::vector<double>>("imu_parameter/Ta", v_Ta, std::vector<double>());
    Eigen::Matrix3d Ta = mat33FromArray(v_Ta);
    std::vector<double> v_R_acc_imu;
    nh.param<std::vector<double>>("imu_parameter/R_acc_imu", v_R_acc_imu, std::vector<double>());
    Eigen::Matrix3d R_acc_imu = mat33FromArray(v_R_acc_imu);
    std::vector<double> v_R_gyr_imu;
    nh.param<std::vector<double>>("imu_parameter/R_gyr_imu", v_R_gyr_imu, std::vector<double>());
    Eigen::Matrix3d R_gyr_imu = mat33FromArray(v_R_gyr_imu);
    std::vector<double> v_Tg;
    nh.param<std::vector<double>>("imu_parameter/Tg", v_Tg, std::vector<double>());
    Eigen::Matrix3d Tg = mat33FromArray(v_Tg);

    Eigen::Matrix3d Dw = Tw.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity());
    Eigen::Matrix3d Da = Ta.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity());
    Eigen::Matrix3d R_imu_acc = R_acc_imu.transpose();
    Eigen::Matrix3d R_imu_gyr = R_gyr_imu.transpose();

    if (std::isnan(Tw.norm()) || std::isnan(Dw.norm()))
    {
        std::cout << "gyroscope has bad intrinsic values!" << std::endl;
        std::cout << "Tw = " << Tw << std::endl;
        std::cout << "Dw = " << Dw << std::endl;
        std::exit(EXIT_FAILURE);
    }

    if (std::isnan(Ta.norm()) || std::isnan(Da.norm()))
    {
        std::cout << "accelerometer has bad intrinsic values!" << std::endl;
        std::cout << "Ta = " << Ta << std::endl;
        std::cout << "Da = " << Da << std::endl;
        std::exit(EXIT_FAILURE);
    }

    if (odometry_options.state_options.imu_model == ImuModel::KALIBR)
    {
        odometry_options.vec_dw << Dw.block<3, 1>(0, 0), Dw.block<2, 1>(1, 1), Dw(2, 2);
        odometry_options.vec_da << Da.block<3, 1>(0, 0), Da.block<2, 1>(1, 1), Da(2, 2);
    }
    else
    {
        odometry_options.vec_dw << Dw(0, 0), Dw.block<2, 1>(0, 1), Dw.block<3, 1>(0, 2);
        odometry_options.vec_da << Da(0, 0), Da.block<2, 1>(0, 1), Da.block<3, 1>(0, 2);
    }

    odometry_options.vec_tg << Tg.block<3, 1>(0, 0), Tg.block<3, 1>(0, 1), Tg.block<3, 1>(0, 2);
    odometry_options.q_imu_acc = quatType::rotToQuat(R_imu_acc);
    odometry_options.q_imu_gyr = quatType::rotToQuat(R_imu_gyr);

    nh.param<double>("odometry_parameter/dt_slam_delay", para_double, 2.0); odometry_options.dt_slam_delay = para_double;

    odometry_options.imu_noises.sigma_w = odometry_options.init_options.sigma_w;
    odometry_options.imu_noises.sigma_wb = odometry_options.init_options.sigma_wb;
    odometry_options.imu_noises.sigma_a = odometry_options.init_options.sigma_a;
    odometry_options.imu_noises.sigma_ab = odometry_options.init_options.sigma_ab;
    odometry_options.imu_noises.sigma_w_2 = std::pow(odometry_options.imu_noises.sigma_w, 2);
    odometry_options.imu_noises.sigma_wb_2 = std::pow(odometry_options.imu_noises.sigma_wb, 2);
    odometry_options.imu_noises.sigma_a_2 = std::pow(odometry_options.imu_noises.sigma_a, 2);
    odometry_options.imu_noises.sigma_ab_2 = std::pow(odometry_options.imu_noises.sigma_ab, 2);

    nh.param<double>("odometry_parameter/up_msckf_sigma_px", para_double, 1.0); odometry_options.msckf_options.sigma_pix = para_double;
    nh.param<double>("odometry_parameter/up_msckf_chi2_multipler", para_double, 5.0); odometry_options.msckf_options.chi2_multipler = para_double;
    nh.param<double>("odometry_parameter/up_slam_sigma_px", para_double, 1.0); odometry_options.slam_options.sigma_pix = para_double;
    nh.param<double>("odometry_parameter/up_slam_chi2_multipler", para_double, 5.0); odometry_options.slam_options.chi2_multipler = para_double;
    odometry_options.msckf_options.sigma_pix_sq = std::pow(odometry_options.msckf_options.sigma_pix, 2);
    odometry_options.slam_options.sigma_pix_sq = std::pow(odometry_options.slam_options.sigma_pix, 2);

    nh.param<bool>("odometry_parameter/downsample_cameras", para_bool, false); odometry_options.downsample_cameras = para_bool;
    nh.param<int>("odometry_parameter/num_pts", para_int, 150); odometry_options.num_pts = para_int;
    nh.param<int>("odometry_parameter/fast_threshold", para_int, 20); odometry_options.fast_threshold = para_int;
    nh.param<int>("odometry_parameter/patch_size_x", para_int, 5); odometry_options.patch_size_x = para_int;
    nh.param<int>("odometry_parameter/patch_size_y", para_int, 5); odometry_options.patch_size_y = para_int;
    nh.param<int>("odometry_parameter/min_px_dist", para_int, 10); odometry_options.min_px_dist = para_int;
    nh.param<std::string>("odometry_parameter/histogram_method", str_temp, "histogram");
    if (str_temp == "none") odometry_options.histogram_method = HistogramMethod::NONE;
    else if (str_temp == "histogram") odometry_options.histogram_method = HistogramMethod::HISTOGRAM;
    else if (str_temp == "clahe") odometry_options.histogram_method = HistogramMethod::CLAHE;
    else {
        std::cout << "Invalid feature histogram specified: " << str_temp << std::endl;
        std::cout << "Please select a valid histogram method: none, histogram, clahe." << std::endl;
        std::exit(EXIT_FAILURE);
    }
    nh.param<double>("odometry_parameter/track_frequency", para_double, 20.0); odometry_options.track_frequency = para_double;

    nh.param<bool>("odometry_parameter/use_huber", para_bool, true); odometry_options.state_options.use_huber = para_bool;
    nh.param<bool>("odometry_parameter/use_keyframe", para_bool, true); odometry_options.use_keyframe = para_bool;

    nh.param<bool>("feature_parameter/refine_features", para_bool, true); odometry_options.featinit_options.refine_features = para_bool;
    nh.param<int>("feature_parameter/max_runs", para_int, 5); odometry_options.featinit_options.max_runs = para_int;
    nh.param<double>("feature_parameter/init_lamda", para_double, 1e-3); odometry_options.featinit_options.init_lamda = para_double;
    nh.param<double>("feature_parameter/max_lamda", para_double, 1e10); odometry_options.featinit_options.max_lamda = para_double;
    nh.param<double>("feature_parameter/min_dx", para_double, 1e-6); odometry_options.featinit_options.min_dx = para_double;
    nh.param<double>("feature_parameter/min_dcost", para_double, 1e-6); odometry_options.featinit_options.min_dcost = para_double;
    nh.param<double>("feature_parameter/lam_mult", para_double, 10.0); odometry_options.featinit_options.lam_mult = para_double;
    nh.param<double>("feature_parameter/min_dist", para_double, 0.10); odometry_options.featinit_options.min_dist = para_double;
    nh.param<double>("feature_parameter/max_dist", para_double, 60.0); odometry_options.featinit_options.max_dist = para_double;
    nh.param<double>("feature_parameter/max_baseline", para_double, 40.0); odometry_options.featinit_options.max_baseline = para_double;
    nh.param<double>("feature_parameter/max_cond_number", para_double, 10000.0); odometry_options.featinit_options.max_cond_number = para_double;

    nh.param<int>("feature_parameter/keyframe_parallax", para_int, 10); setting_min_parallax = para_int;
    setting_min_parallax = setting_min_parallax / cam_calib_1_left.at(0);

    nh.param<double>("voxel_parameter/voxel_size", para_double, 0.1); odometry_options.voxel_size = odometry_options.state_options.voxel_size = para_double;
    nh.param<int>("voxel_parameter/max_num_points_in_voxel", para_int, 5); odometry_options.max_num_points_in_voxel = odometry_options.state_options.max_num_points_in_voxel = para_int;
    nh.param<double>("voxel_parameter/min_distance_points", para_double, 0.03); odometry_options.min_distance_points = odometry_options.state_options.min_distance_points = para_double;
    nh.param<int>("voxel_parameter/nb_voxels_visited", para_int, 1); odometry_options.nb_voxels_visited = para_int;
    nh.param<bool>("voxel_parameter/use_all_points", para_bool, false); odometry_options.use_all_points = para_bool;
}
    void allocateMemory() {
    state_ptr = std::make_shared<state>(odometry_options.state_options);

    state_ptr->calib_imu_dw->setValue(odometry_options.vec_dw);
    state_ptr->calib_imu_dw->setFej(odometry_options.vec_dw);
    state_ptr->calib_imu_da->setValue(odometry_options.vec_da);
    state_ptr->calib_imu_da->setFej(odometry_options.vec_da);
    state_ptr->calib_imu_tg->setValue(odometry_options.vec_tg);
    state_ptr->calib_imu_tg->setFej(odometry_options.vec_tg);
    state_ptr->calib_imu_gyr->setValue(odometry_options.q_imu_gyr);
    state_ptr->calib_imu_gyr->setFej(odometry_options.q_imu_gyr);
    state_ptr->calib_imu_acc->setValue(odometry_options.q_imu_acc);
    state_ptr->calib_imu_acc->setFej(odometry_options.q_imu_acc);

    Eigen::VectorXd temp_camimu_dt;
    temp_camimu_dt.resize(1);
    temp_camimu_dt(0) = odometry_options.calib_camimu_dt;
    state_ptr->calib_dt_imu_cam->setValue(temp_camimu_dt);
    state_ptr->calib_dt_imu_cam->setFej(temp_camimu_dt);

    state_ptr->cam_intrinsics_cameras = odometry_options.camera_intrinsics;

    for (int i = 0; i < 2; i++)
    {
        state_ptr->cam_intrinsics.at(i)->setValue(odometry_options.camera_intrinsics.at(i)->getValue());
        state_ptr->cam_intrinsics.at(i)->setFej(odometry_options.camera_intrinsics.at(i)->getValue());
        state_ptr->calib_cam_imu.at(i)->setValue(odometry_options.camera_extrinsics.at(i));
        state_ptr->calib_cam_imu.at(i)->setFej(odometry_options.camera_extrinsics.at(i));
    }

    int init_max_features = std::floor((double)odometry_options.init_options.init_max_features / (double)2.0);

    featureTracker = std::shared_ptr<trackKLT>(new trackKLT(state_ptr->cam_intrinsics_cameras, init_max_features, odometry_options.histogram_method,
        odometry_options.fast_threshold, odometry_options.patch_size_x, odometry_options.patch_size_y, odometry_options.min_px_dist));

    propagator_ptr = std::make_shared<propagator>(odometry_options.imu_noises, odometry_options.gravity_mag);

    initializer_ptr = std::make_shared<inertialInitializer>(odometry_options.init_options, featureTracker->getFeatureDatabase());

    updaterMsckf_ptr = std::make_shared<updaterMsckf>(odometry_options.msckf_options, odometry_options.featinit_options);

    updaterSlam_ptr = std::make_shared<updaterSlam>(odometry_options.slam_options, odometry_options.featinit_options, featureTracker->getFeatureDatabase());

    gammaPixel_ptr = std::make_shared<gammaPixel>();

    int wlvl = wG[0], hlvl = hG[0];

    while(wlvl % 2 == 0 && hlvl % 2 == 0 && wlvl * hlvl > 5000 && pyr_levels_used < PYR_LEVELS)
    {
        wlvl /=2;
        hlvl /=2;
        pyr_levels_used++;
    }

    for (int level = 1; level < pyr_levels_used; ++ level)
    {
        wG[level] = wG[0] >> level;
        hG[level] = hG[0] >> level;
    }

    frame_count = 0;
}
    void initialValue() {
    last_time_image = -1;

    last_imu_data.timestamp = -1;
    last_imu_data.gyr.setZero();
    last_imu_data.acc.setZero();

    startup_time = -1;
    current_time = -1;

    time_newest_imu = -1;
}
    void imuHandler(const sensor_msgs::Imu::ConstPtr &msg) {
    imuData imu_data;
    imu_data.timestamp = msg->header.stamp.toSec();
    imu_data.gyr << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z;
    imu_data.acc << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z;

    processImu(imu_data);

    time_newest_imu = imu_data.timestamp;
}
    void stereoImageHandler(const sensor_msgs::ImageConstPtr &msg_0, const sensor_msgs::ImageConstPtr &msg_1, int cam_0, int cam_1) {
    double timestamp = msg_0->header.stamp.toSec();
    double time_delta = 1.0 / odometry_options.track_frequency;

    if (camera_last_timestamp.find(cam_0) != camera_last_timestamp.end() && timestamp < camera_last_timestamp.at(cam_0) + time_delta) return;

    camera_last_timestamp[cam_0] = timestamp;

    cv_bridge::CvImageConstPtr cv_ptr_0;
    try
    {
        cv_ptr_0 = cv_bridge::toCvShare(msg_0, sensor_msgs::image_encodings::MONO8);
    }
    catch (cv_bridge::Exception &e)
    {
        std::cout << "cv_bridge exception: " << e.what() << std::endl;
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptr_1;
    try
    {
        cv_ptr_1 = cv_bridge::toCvShare(msg_1, sensor_msgs::image_encodings::MONO8);
    }
    catch (cv_bridge::Exception &e)
    {
        std::cout << "cv_bridge exception: " << e.what() << std::endl;
        return;
    }

    cameraData camera_data;
    camera_data.timestamp = cv_ptr_0->header.stamp.toSec();
    camera_data.camera_ids.push_back(cam_0);
    camera_data.camera_ids.push_back(cam_1);
    camera_data.images.push_back(cv_ptr_0->image.clone());
    camera_data.images.push_back(cv_ptr_1->image.clone());

    if (odometry_options.use_mask)
    {
        camera_data.masks.push_back(odometry_options.masks.at(cam_0));
        camera_data.masks.push_back(odometry_options.masks.at(cam_1));
    }
    else
    {
        camera_data.masks.push_back(cv::Mat::zeros(cv_ptr_0->image.rows, cv_ptr_0->image.cols, CV_8UC1));
        camera_data.masks.push_back(cv::Mat::zeros(cv_ptr_1->image.rows, cv_ptr_1->image.cols, CV_8UC1));
    }

    camera_buffer.push(camera_data);
}
    void run() {
    double timestamp_imu_in_camera = time_newest_imu - state_ptr->calib_dt_imu_cam->value()(0);

    while (!camera_buffer.empty() && camera_buffer.front().timestamp < timestamp_imu_in_camera)
    {
        // time test
        boost::posix_time::ptime rT_begin_sum = boost::posix_time::microsec_clock::local_time();
        // time test

        processImage(camera_buffer.front());

        // time test
        boost::posix_time::ptime rT_end_sum = boost::posix_time::microsec_clock::local_time();
        double time_cost = (rT_end_sum - rT_begin_sum).total_microseconds() * 1e-6;
        sum_time_sum = sum_time_sum * n_time_sum + time_cost;
        n_time_sum++;
        sum_time_sum = sum_time_sum / n_time_sum;
        std::cout << std::fixed << std::setprecision(6) << "[total] time cost = " << sum_time_sum << std::endl;
        // time test

        camera_buffer.pop();
    }
}
    ros::NodeHandle nh;
    std::string image_left_topic;
    std::string image_right_topic;
    std::string imu_topic;
    ros::Subscriber sub_imu;
    std::vector<ros::Subscriber> subs_cam;
    std::vector<std::shared_ptr<message_filters::Synchronizer<SyncStereoImage>>> sync_cam;
    std::vector<std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>>> sync_subs_cam;
    void pubFeatImage(cv::Mat &stereo_image, double &timestamp) {
    if (!stereo_image.empty() && pub_feat_image.getNumSubscribers() > 0)
    {
        try {
            sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", stereo_image).toImageMsg();
            msg->header.stamp = ros::Time().fromSec(timestamp);
            msg->header.frame_id = "camera_init";
            pub_feat_image.publish(msg);
        }
        catch (cv_bridge::Exception& e) {
            ROS_ERROR("cv_bridge exception: %s", e.what());
        }
    }
}
    void pubOdometry(std::shared_ptr<state> state_ptr, double &timestamp) {
    if (pub_odom.getNumSubscribers() > 0)
    {
        odom.header.frame_id = "camera_init";
        odom.child_frame_id = "body";
        odom.header.stamp = ros::Time().fromSec(timestamp);
        odom.pose.pose.orientation.x = state_ptr->imu_ptr->getQuat()(0);
        odom.pose.pose.orientation.y = state_ptr->imu_ptr->getQuat()(1);
        odom.pose.pose.orientation.z = state_ptr->imu_ptr->getQuat()(2);
        odom.pose.pose.orientation.w = state_ptr->imu_ptr->getQuat()(3);
        odom.pose.pose.position.x = state_ptr->imu_ptr->getPos()(0);
        odom.pose.pose.position.y = state_ptr->imu_ptr->getPos()(1);
        odom.pose.pose.position.z = state_ptr->imu_ptr->getPos()(2);
        pub_odom.publish(odom);
    }
}
    void setPoseStamp(geometry_msgs::PoseStamped &body_pose_out, std::shared_ptr<state> state_ptr) {
    body_pose_out.pose.position.x = state_ptr->imu_ptr->getPos()(0);
    body_pose_out.pose.position.y = state_ptr->imu_ptr->getPos()(1);
    body_pose_out.pose.position.z = state_ptr->imu_ptr->getPos()(2);

    body_pose_out.pose.orientation.x = state_ptr->imu_ptr->getQuat()(0);
    body_pose_out.pose.orientation.y = state_ptr->imu_ptr->getQuat()(1);
    body_pose_out.pose.orientation.z = state_ptr->imu_ptr->getQuat()(2);
    body_pose_out.pose.orientation.w = state_ptr->imu_ptr->getQuat()(3);
}
    void pubPath(std::shared_ptr<state> state_ptr, double &timestamp) {
    if (pub_path.getNumSubscribers() > 0)
    {
        setPoseStamp(msg_body_pose, state_ptr);
        msg_body_pose.header.stamp = ros::Time().fromSec(timestamp);
        msg_body_pose.header.frame_id = "camera_init";

        static int i = 0;
        i++;
        if (i % 10 == 0)
        {
            path.poses.push_back(msg_body_pose);
            path.header.stamp = ros::Time().fromSec(timestamp);
            path.header.frame_id ="camera_init";
            pub_path.publish(path);
        }
    }
}
    void pubHistoryPoints(pcl::PointCloud<pcl::PointXYZRGB>::Ptr points_history, double &timestamp) {
    if (pub_points_history.getNumSubscribers() > 0)
    {
        sensor_msgs::PointCloud2 point_cloud_msg;
        pcl::toROSMsg(*points_history, point_cloud_msg);
        point_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);
        point_cloud_msg.header.frame_id = "camera_init";
        pub_points_history.publish(point_cloud_msg);
    }
}
    void pubWindowPoints(std::shared_ptr<state> state_ptr, double &timestamp) {
    points_window->clear();

    if (pub_points_window.getNumSubscribers() > 0)
    {
        auto it0 = state_ptr->map_points.begin();
        while (it0 != state_ptr->map_points.end())
        {
            pcl::PointXYZRGB point_temp;
            point_temp.x = (*it0).second->getPointXYZ(false)[0];
            point_temp.y = (*it0).second->getPointXYZ(false)[1];
            point_temp.z = (*it0).second->getPointXYZ(false)[2];
            point_temp.r = (*it0).second->color;
            point_temp.g = (*it0).second->color;
            point_temp.b = (*it0).second->color;
            points_window->points.push_back(point_temp);

            it0++;
        }

        sensor_msgs::PointCloud2 point_cloud_msg;
        pcl::toROSMsg(*points_window, point_cloud_msg);
        point_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);
        point_cloud_msg.header.frame_id = "camera_init";
        pub_points_window.publish(point_cloud_msg);
    }
}
    void pubHistoryVoxels(pcl::PointCloud<pcl::PointXYZI>::Ptr voxels_history, double &timestamp) {
    if (pub_voxels_history.getNumSubscribers() > 0)
    {
        sensor_msgs::PointCloud2 point_cloud_msg;
        pcl::toROSMsg(*voxels_history, point_cloud_msg);
        point_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);
        point_cloud_msg.header.frame_id = "camera_init";
        pub_voxels_history.publish(point_cloud_msg);
    }

    voxels_history->clear();
}
    void pubVisitVoxels(pcl::PointCloud<pcl::PointXYZI>::Ptr voxels_visit, double &timestamp) {
    if (pub_voxels_visit.getNumSubscribers() > 0)
    {
        sensor_msgs::PointCloud2 point_cloud_msg;
        pcl::toROSMsg(*voxels_visit, point_cloud_msg);
        point_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);
        point_cloud_msg.header.frame_id = "camera_init";
        pub_voxels_visit.publish(point_cloud_msg);
    }
}
    image_transport::ImageTransport it;
    image_transport::Publisher pub_feat_image;
    ros::Publisher pub_odom;
    ros::Publisher pub_path;
    ros::Publisher pub_points_history;
    ros::Publisher pub_points_window;
    ros::Publisher pub_voxels_history;
    ros::Publisher pub_voxels_visit;
    geometry_msgs::PoseStamped msg_body_pose;
    nav_msgs::Odometry odom;
    nav_msgs::Path path;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr points_history;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr points_window;
    pcl::PointCloud<pcl::PointXYZI>::Ptr voxels_history;
    pcl::PointCloud<pcl::PointXYZI>::Ptr voxels_visit;
};

class feature {
  public:
    trackKLT(std::unordered_map<size_t, std::shared_ptr<cameraBase>> camera_calib_, int num_features_,
		HistogramMethod histogram_method_, int fast_threshold_, int patch_size_x_, int patch_size_y_, int min_px_dist_) {
  current_id = 1;

  pixelSelector_ptr = std::make_shared<pixelSelector>(wG[0], hG[0]);
}
    void feedNewImage(const cameraData &image_measurements, std::shared_ptr<frame> fh) {
  if (image_measurements.camera_ids.empty() || image_measurements.camera_ids.size() != image_measurements.images.size()
    || image_measurements.images.size() != image_measurements.masks.size())
  {
    std::cout << "[ERROR]: image_measurements data sizes do not match or empty." << std::endl;
    std::cout << "[ERROR]: - image_measurements.camera_ids.size() = " << image_measurements.camera_ids.size() << std::endl;
    std::cout << "[ERROR]: - image_measurements.images.size() = " << image_measurements.images.size() << std::endl;
    std::cout << "[ERROR]: - image_measurements.masks.size() = " << image_measurements.masks.size() << std::endl;
    std::exit(EXIT_FAILURE);
  }

  rT1 = boost::posix_time::microsec_clock::local_time();
  size_t num_images = image_measurements.images.size();

  for (size_t image_id = 0; image_id < num_images; image_id++)
  {
    size_t cam_id = image_measurements.camera_ids.at(image_id);

    cv::Mat image;

    if (histogram_method == HistogramMethod::HISTOGRAM)
    {
      cv::equalizeHist(image_measurements.images.at(image_id), image);
    }
    else if (histogram_method == HistogramMethod::CLAHE)
    {
      double eq_clip_limit = 10.0;
      cv::Size eq_win_size = cv::Size(8, 8);
      cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(eq_clip_limit, eq_win_size);
      clahe->apply(image_measurements.images.at(image_id), image);
    }
    else
    {
      image = image_measurements.images.at(image_id);
    }

    std::vector<cv::Mat> img_pyr;
    cv::buildOpticalFlowPyramid(image, img_pyr, win_size, pyr_levels);

    img_curr[cam_id] = image;
    img_pyramid_curr[cam_id] = img_pyr;
  }

  feedStereo(image_measurements, fh, 0, 1);
}
    std::shared_ptr<featureDatabase> getFeatureDatabase() {
  return database;
}
    std::unordered_map<size_t, std::vector<cv::KeyPoint>> getLastObs() {
  return pts_last;
}
    std::unordered_map<size_t, std::vector<size_t>> getLastIds() {
  return ids_last;
}
    void setNumFeatures(int num_features_) {
  num_features = num_features_;
}
    void displayActive(cv::Mat &img_out, int r1, int g1, int b1, int r2, int g2, int b2, std::string overlay = "") {
  std::map<size_t, cv::Mat> img_last_cache, img_mask_last_cache;
  std::unordered_map<size_t, std::vector<cv::KeyPoint>> pts_last_cache;
  {
    img_last_cache = img_last;
    img_mask_last_cache = img_mask_last;
    pts_last_cache = pts_last;
  }

  int max_width = -1;
  int max_height = -1;

  for (auto const &pair : img_last_cache)
  {
    if (max_width < pair.second.cols)
      max_width = pair.second.cols;
    if (max_height < pair.second.rows)
      max_height = pair.second.rows;
  }

  if (img_last_cache.empty() || max_width == -1 || max_height == -1)
    return;

  bool is_small = (std::min(max_width, max_height) < 400);

  bool image_new = ((int)img_last_cache.size() * max_width != img_out.cols || max_height != img_out.rows);

  if (image_new)
    img_out = cv::Mat(max_height, (int)img_last_cache.size() * max_width, CV_8UC3, cv::Scalar(0, 0, 0));

  int index_cam = 0;
  for (auto const &pair : img_last_cache)
  {
    cv::Mat img_temp;

    if (image_new)
      cv::cvtColor(img_last_cache[pair.first], img_temp, cv::COLOR_GRAY2RGB);
    else
      img_temp = img_out(cv::Rect(max_width * index_cam, 0, max_width, max_height));

    for (size_t i = 0; i < pts_last_cache[pair.first].size(); i++)
    {
      cv::Point2f pt_l = pts_last_cache[pair.first].at(i).pt;
      cv::circle(img_temp, pt_l, (is_small) ? 1 : 2, cv::Scalar(r1, g1, b1), cv::FILLED);

      cv::Point2f pt_l_top = cv::Point2f(pt_l.x - 3, pt_l.y - 3);
      cv::Point2f pt_l_bot = cv::Point2f(pt_l.x + 3, pt_l.y + 3);
      cv::rectangle(img_temp, pt_l_top, pt_l_bot, cv::Scalar(r2, g2, b2), 1);
    }

    auto txtpt = (is_small) ? cv::Point(10, 30) : cv::Point(30, 60);

    if (overlay == "")
    {
      cv::putText(img_temp, "CAM:" + std::to_string((int)pair.first), txtpt, cv::FONT_HERSHEY_COMPLEX_SMALL, (is_small) ? 1.5 : 3.0, cv::Scalar(0, 255, 0), 3);
    }
    else
    {
      cv::putText(img_temp, overlay, txtpt, cv::FONT_HERSHEY_COMPLEX_SMALL, (is_small) ? 1.5 : 3.0, cv::Scalar(0, 0, 255), 3);
    }

    cv::Mat mask = cv::Mat::zeros(img_mask_last_cache[pair.first].rows, img_mask_last_cache[pair.first].cols, CV_8UC3);
    mask.setTo(cv::Scalar(0, 0, 255), img_mask_last_cache[pair.first]);
    cv::addWeighted(mask, 0.1, img_temp, 1.0, 0.0, img_temp);

    img_temp.copyTo(img_out(cv::Rect(max_width * index_cam, 0, img_last_cache[pair.first].cols, img_last_cache[pair.first].rows)));
    index_cam++;
  }
}
  private:
    void feedStereo(const cameraData &image_measurements, std::shared_ptr<frame> fh, size_t image_id_left, size_t image_id_right) {
  size_t cam_id_left = image_measurements.camera_ids.at(image_id_left);
  size_t cam_id_right = image_measurements.camera_ids.at(image_id_right);

  cv::Mat img_left = img_curr.at(cam_id_left);
  cv::Mat img_right = img_curr.at(cam_id_right);
  std::vector<cv::Mat> img_pyr_left = img_pyramid_curr.at(cam_id_left);
  std::vector<cv::Mat> img_pyr_right = img_pyramid_curr.at(cam_id_right);
  cv::Mat mask_left = image_measurements.masks.at(image_id_left);
  cv::Mat mask_right = image_measurements.masks.at(image_id_right);
  rT2 = boost::posix_time::microsec_clock::local_time();

  if (pts_last[cam_id_left].empty() && pts_last[cam_id_right].empty())
  {
    std::vector<cv::KeyPoint> good_left, good_right;
    std::vector<size_t> good_ids_left, good_ids_right;
    performDetectionStereo(fh, img_pyr_left, img_pyr_right, mask_left, mask_right, cam_id_left, cam_id_right, good_left, good_right, good_ids_left, good_ids_right);

    img_last[cam_id_left] = img_left;
    img_last[cam_id_right] = img_right;
    img_pyramid_last[cam_id_left] = img_pyr_left;
    img_pyramid_last[cam_id_right] = img_pyr_right;
    img_mask_last[cam_id_left] = mask_left;
    img_mask_last[cam_id_right] = mask_right;
    pts_last[cam_id_left] = good_left;
    pts_last[cam_id_right] = good_right;
    ids_last[cam_id_left] = good_ids_left;
    ids_last[cam_id_right] = good_ids_right;

    return;
  }

  int pts_before_detect = (int)pts_last[cam_id_left].size();
  auto pts_left_old = pts_last[cam_id_left];
  auto pts_right_old = pts_last[cam_id_right];
  auto ids_left_old = ids_last[cam_id_left];
  auto ids_right_old = ids_last[cam_id_right];
  performDetectionStereo(fh, img_pyramid_last[cam_id_left], img_pyramid_last[cam_id_right], img_mask_last[cam_id_left],
    img_mask_last[cam_id_right], cam_id_left, cam_id_right, pts_left_old, pts_right_old, ids_left_old, ids_right_old);

  rT3 = boost::posix_time::microsec_clock::local_time();

  std::vector<uchar> mask_ll, mask_rr;
  std::vector<cv::KeyPoint> pts_left_new = pts_left_old;
  std::vector<cv::KeyPoint> pts_right_new = pts_right_old;

  parallel_for_(cv::Range(0, 2), lambdaBody([&](const cv::Range &range) {
                  for (int i = range.start; i < range.end; i++) {
                    bool is_left = (i == 0);
                    performMatching(img_pyramid_last[is_left ? cam_id_left : cam_id_right], is_left ? img_pyr_left : img_pyr_right,
                                    is_left ? pts_left_old : pts_right_old, is_left ? pts_left_new : pts_right_new,
                                    is_left ? cam_id_left : cam_id_right, is_left ? cam_id_left : cam_id_right,
                                    is_left ? mask_ll : mask_rr);
                  }
                }));
  rT4 = boost::posix_time::microsec_clock::local_time();

  rT5 = boost::posix_time::microsec_clock::local_time();

  if (mask_ll.empty() && mask_rr.empty())
  {
    img_last[cam_id_left] = img_left;
    img_last[cam_id_right] = img_right;
    img_pyramid_last[cam_id_left] = img_pyr_left;
    img_pyramid_last[cam_id_right] = img_pyr_right;
    img_mask_last[cam_id_left] = mask_left;
    img_mask_last[cam_id_right] = mask_right;
    pts_last[cam_id_left].clear();
    pts_last[cam_id_right].clear();
    ids_last[cam_id_left].clear();
    ids_last[cam_id_right].clear();
    std::cout << "[trackKLT::feedStereo]: Not enough points for RANSAC, resetting." << std::endl;

    return;
  }

  std::vector<cv::KeyPoint> good_left, good_right;
  std::vector<size_t> good_ids_left, good_ids_right;

  for (size_t i = 0; i < pts_left_new.size(); i++)
  {
    if (pts_left_new.at(i).pt.x < 0 || pts_left_new.at(i).pt.y < 0 || (int)pts_left_new.at(i).pt.x > img_left.cols ||
      (int)pts_left_new.at(i).pt.y > img_left.rows)
      continue;

    bool found_right = false;
    size_t index_right = 0;
    for (size_t n = 0; n < ids_right_old.size(); n++)
    {
      if (ids_left_old.at(i) == ids_right_old.at(n))
      {
        found_right = true;
        index_right = n;
        break;
      }
    }

    if (mask_ll[i] && found_right && mask_rr[index_right])
    {
      if (pts_right_new.at(index_right).pt.x < 0 || pts_right_new.at(index_right).pt.y < 0 ||
        (int)pts_right_new.at(index_right).pt.x >= img_right.cols || (int)pts_right_new.at(index_right).pt.y >= img_right.rows)
        continue;

      good_left.push_back(pts_left_new.at(i));
      good_right.push_back(pts_right_new.at(index_right));
      good_ids_left.push_back(ids_left_old.at(i));
      good_ids_right.push_back(ids_right_old.at(index_right));
    }
    else if (mask_ll[i])
    {
      good_left.push_back(pts_left_new.at(i));
      good_ids_left.push_back(ids_left_old.at(i));
    }
  }

  for (size_t i = 0; i < pts_right_new.size(); i++)
  {
    if (pts_right_new.at(i).pt.x < 0 || pts_right_new.at(i).pt.y < 0 || (int)pts_right_new.at(i).pt.x >= img_right.cols ||
      (int)pts_right_new.at(i).pt.y >= img_right.rows)
      continue;

    bool added_already = (std::find(good_ids_right.begin(), good_ids_right.end(), ids_right_old.at(i)) != good_ids_right.end());

    if (mask_rr[i] && !added_already)
    {
      good_right.push_back(pts_right_new.at(i));
      good_ids_right.push_back(ids_right_old.at(i));
    }
  }

  for (size_t i = 0; i < good_left.size(); i++)
  {
    cv::Point2f npt_l = camera_calib.at(cam_id_left)->undistortCV(good_left.at(i).pt);
    database->updateFeature(fh, good_ids_left.at(i), image_measurements.timestamp, cam_id_left, good_left.at(i).pt.x, good_left.at(i).pt.y, npt_l.x, npt_l.y);
  }
  for (size_t i = 0; i < good_right.size(); i++)
  {
    cv::Point2f npt_r = camera_calib.at(cam_id_right)->undistortCV(good_right.at(i).pt);
    database->updateFeature(fh, good_ids_right.at(i), image_measurements.timestamp, cam_id_right, good_right.at(i).pt.x, good_right.at(i).pt.y, npt_r.x, npt_r.y);
  }

  {
    img_last[cam_id_left] = img_left;
    img_last[cam_id_right] = img_right;
    img_pyramid_last[cam_id_left] = img_pyr_left;
    img_pyramid_last[cam_id_right] = img_pyr_right;
    img_mask_last[cam_id_left] = mask_left;
    img_mask_last[cam_id_right] = mask_right;
    pts_last[cam_id_left] = good_left;
    pts_last[cam_id_right] = good_right;
    ids_last[cam_id_left] = good_ids_left;
    ids_last[cam_id_right] = good_ids_right;
  }
  rT6 = boost::posix_time::microsec_clock::local_time();

  // Time test
  /*
  std::cout << std::fixed << "[trackKLT]: " << (rT2 - rT1).total_microseconds() * 1e-6 << " seconds for pyramid." << std::endl;
  std::cout << std::fixed << "[trackKLT]: " << (rT3 - rT2).total_microseconds() * 1e-6 << " seconds for detection (" << (int)pts_last[cam_id_left].size() - pts_before_detect << " detected)." << std::endl;
  std::cout << std::fixed << "[trackKLT]: " << (rT4 - rT3).total_microseconds() * 1e-6 << " seconds for temporal klt." << std::endl;
  std::cout << std::fixed << "[trackKLT]: " << (rT5 - rT4).total_microseconds() * 1e-6 << " seconds for stereo klt." << std::endl;
  std::cout << std::fixed << "[trackKLT]: " << (rT6 - rT5).total_microseconds() * 1e-6 << " seconds for feature DB update (" << (int)good_left.size() << " features)." << std::endl;
  std::cout << std::fixed << "[trackKLT]: " << (rT6 - rT1).total_microseconds() * 1e-6 << " seconds for total." << std::endl;
  */
  // Time test
}
    void performDetectionStereo(std::shared_ptr<frame> fh, const std::vector<cv::Mat> &img_0_pyr, const std::vector<cv::Mat> &img_1_pyr, const cv::Mat &mask_0, const cv::Mat &mask_1,
		size_t cam_id_left, size_t cam_id_right, std::vector<cv::KeyPoint> &pts_0, std::vector<cv::KeyPoint> &pts_1, std::vector<size_t> &ids_0, std::vector<size_t> &ids_1) {
  cv::Size size_close_0((int)((float)img_0_pyr.at(0).cols / (float)min_px_dist),
                        (int)((float)img_0_pyr.at(0).rows / (float)min_px_dist));
  cv::Mat grid_2d_close_0 = cv::Mat::zeros(size_close_0, CV_8UC1);
  float size_x_0 = (float)img_0_pyr.at(0).cols / (float)patch_size_x;
  float size_y_0 = (float)img_0_pyr.at(0).rows / (float)patch_size_y;

  cv::Size size_grid_0(patch_size_x, patch_size_y);
  cv::Mat grid_2d_grid_0 = cv::Mat::zeros(size_grid_0, CV_8UC1);
  cv::Mat mask_0_updated = mask_0.clone();

  auto it_0 = pts_0.begin();
  auto it_1 = ids_0.begin();

  while (it_0 != pts_0.end())
  {
    cv::KeyPoint kpt = *it_0;
    int x = (int)kpt.pt.x;
    int y = (int)kpt.pt.y;

    int edge = 10;
    if (x < edge || x >= img_0_pyr.at(0).cols - edge || y < edge || y >= img_0_pyr.at(0).rows - edge)
    {
      it_0 = pts_0.erase(it_0);
      it_1 = ids_0.erase(it_1);
      continue;
    }

    int x_close = (int)(kpt.pt.x / (float)min_px_dist);
    int y_close = (int)(kpt.pt.y / (float)min_px_dist);
    if (x_close < 0 || x_close >= size_close_0.width || y_close < 0 || y_close >= size_close_0.height)
    {
      it_0 = pts_0.erase(it_0);
      it_1 = ids_0.erase(it_1);
      continue;
    }

    int x_grid = std::floor(kpt.pt.x / size_x_0);
    int y_grid = std::floor(kpt.pt.y / size_y_0);

    if (x_grid < 0 || x_grid >= size_grid_0.width || y_grid < 0 || y_grid >= size_grid_0.height)
    {
      it_0 = pts_0.erase(it_0);
      it_1 = ids_0.erase(it_1);
      continue;
    }

    if (grid_2d_close_0.at<uint8_t>(y_close, x_close) > 127)
    {
      it_0 = pts_0.erase(it_0);
      it_1 = ids_0.erase(it_1);
      continue;
    }

    if (mask_0.at<uint8_t>(y, x) > 127)
    {
      it_0 = pts_0.erase(it_0);
      it_1 = ids_0.erase(it_1);
      continue;
    }

    grid_2d_close_0.at<uint8_t>(y_close, x_close) = 255;
    if (grid_2d_grid_0.at<uint8_t>(y_grid, x_grid) < 255)
    {
      grid_2d_grid_0.at<uint8_t>(y_grid, x_grid) += 1;
    }

    if (x - min_px_dist >= 0 && x + min_px_dist < img_0_pyr.at(0).cols && y - min_px_dist >= 0 && y + min_px_dist < img_0_pyr.at(0).rows)
    {
      cv::Point pt_1(x - min_px_dist, y - min_px_dist);
      cv::Point pt_2(x + min_px_dist, y + min_px_dist);
      cv::rectangle(mask_0_updated, pt_1, pt_2, cv::Scalar(255), -1);

      int begin_idx = x - min_px_dist + wG[0] * (y - min_px_dist);
      int end_idx = x + min_px_dist + wG[0] * (y + min_px_dist);

      for(int i = begin_idx; i <= end_idx; i++) fh->mask_left[i] = true;
    }

    it_0++;
    it_1++;
  }

  double min_feat_percent = 0.50;
  int num_featsneeded_0 = num_features - (int)pts_0.size();

  if (num_featsneeded_0 > std::min(20, (int)(min_feat_percent * num_features)))
  {
    pixelSelector_ptr->pixelSelectionLeft(fh, num_featsneeded_0);

    std::vector<cv::KeyPoint> kpts_0_new;
    std::vector<cv::Point2f> pts_0_new;

    for (int y = 0; y < hG[0]; y++)
    {
      for (int x = 0; x < wG[0]; x++)
      {
        int idx = x + y * wG[0];

        if (fh->selected_pixels_left[idx] == 0) continue;

        cv::KeyPoint kpt;
        kpt.pt.x = x;
        kpt.pt.y = y;

        kpts_0_new.push_back(kpt);
        pts_0_new.push_back(kpt.pt);
      }
    }

    std::vector<cv::KeyPoint> kpts_1_new;
    std::vector<cv::Point2f> pts_1_new;
    kpts_1_new = kpts_0_new;
    pts_1_new = pts_0_new;

    if (!pts_0_new.empty())
    {
      std::vector<uchar> mask;
      std::vector<float> error;
      cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 30, 0.01);
      cv::calcOpticalFlowPyrLK(img_0_pyr, img_1_pyr, pts_0_new, pts_1_new, mask, error, win_size, pyr_levels, term_crit, cv::OPTFLOW_USE_INITIAL_FLOW);

      for (size_t i = 0; i < pts_0_new.size(); i++)
      {
        bool oob_left = ((int)pts_0_new.at(i).x < 0 || (int)pts_0_new.at(i).x >= img_0_pyr.at(0).cols || (int)pts_0_new.at(i).y < 0 ||
                         (int)pts_0_new.at(i).y >= img_0_pyr.at(0).rows);
        bool oob_right = ((int)pts_1_new.at(i).x < 0 || (int)pts_1_new.at(i).x >= img_1_pyr.at(0).cols || (int)pts_1_new.at(i).y < 0 ||
                          (int)pts_1_new.at(i).y >= img_1_pyr.at(0).rows);

        if (!oob_left && !oob_right && mask[i] == 1)
        {
          kpts_0_new.at(i).pt = pts_0_new.at(i);
          kpts_1_new.at(i).pt = pts_1_new.at(i);

          pts_0.push_back(kpts_0_new.at(i));
          pts_1.push_back(kpts_1_new.at(i));

          size_t temp = ++current_id;
          ids_0.push_back(temp);
          ids_1.push_back(temp);
        }
        else if (!oob_left)
        {
          kpts_0_new.at(i).pt = pts_0_new.at(i);
          pts_0.push_back(kpts_0_new.at(i));

          size_t temp = ++current_id;
          ids_0.push_back(temp);
        }
      }
    }
  }

  cv::Size size_close_1((int)((float)img_1_pyr.at(0).cols / (float)min_px_dist), (int)((float)img_1_pyr.at(0).rows / (float)min_px_dist));
  cv::Mat grid_2d_close_1 = cv::Mat::zeros(size_close_1, CV_8UC1);
  float size_x_1 = (float)img_1_pyr.at(0).cols / (float)patch_size_x;
  float size_y_1 = (float)img_1_pyr.at(0).rows / (float)patch_size_y;

  cv::Size size_grid_1(patch_size_x, patch_size_y);
  cv::Mat grid_2d_grid_1 = cv::Mat::zeros(size_grid_1, CV_8UC1);
  cv::Mat mask_1_updated = mask_0.clone();

  it_0 = pts_1.begin();
  it_1 = ids_1.begin();

  while (it_0 != pts_1.end())
  {
    cv::KeyPoint kpt = *it_0;
    int x = (int)kpt.pt.x;
    int y = (int)kpt.pt.y;
    int edge = 10;
    if (x < edge || x >= img_1_pyr.at(0).cols - edge || y < edge || y >= img_1_pyr.at(0).rows - edge)
    {
      it_0 = pts_1.erase(it_0);
      it_1 = ids_1.erase(it_1);
      continue;
    }

    int x_close = (int)(kpt.pt.x / (float)min_px_dist);
    int y_close = (int)(kpt.pt.y / (float)min_px_dist);
    if (x_close < 0 || x_close >= size_close_1.width || y_close < 0 || y_close >= size_close_1.height)
    {
      it_0 = pts_1.erase(it_0);
      it_1 = ids_1.erase(it_1);
      continue;
    }

    int x_grid = std::floor(kpt.pt.x / size_x_1);
    int y_grid = std::floor(kpt.pt.y / size_y_1);
    if (x_grid < 0 || x_grid >= size_grid_1.width || y_grid < 0 || y_grid >= size_grid_1.height)
    {
      it_0 = pts_1.erase(it_0);
      it_1 = ids_1.erase(it_1);
      continue;
    }

    bool is_stereo = (std::find(ids_0.begin(), ids_0.end(), *it_1) != ids_0.end());

    if (grid_2d_close_1.at<uint8_t>(y_close, x_close) > 127 && !is_stereo)
    {
      it_0 = pts_1.erase(it_0);
      it_1 = ids_1.erase(it_1);
      continue;
    }

    if (mask_1.at<uint8_t>(y, x) > 127)
    {
      it_0 = pts_1.erase(it_0);
      it_1 = ids_1.erase(it_1);
      continue;
    }

    grid_2d_close_1.at<uint8_t>(y_close, x_close) = 255;

    if (grid_2d_grid_1.at<uint8_t>(y_grid, x_grid) < 255)
    {
      grid_2d_grid_1.at<uint8_t>(y_grid, x_grid) += 1;
    }

    if (x - min_px_dist >= 0 && x + min_px_dist < img_1_pyr.at(0).cols && y - min_px_dist >= 0 && y + min_px_dist < img_1_pyr.at(0).rows)
    {
      cv::Point pt_1(x - min_px_dist, y - min_px_dist);
      cv::Point pt_2(x + min_px_dist, y + min_px_dist);
      cv::rectangle(mask_1_updated, pt_1, pt_2, cv::Scalar(255), -1);

      int begin_idx = x - min_px_dist + wG[0] * (y - min_px_dist);
      int end_idx = x + min_px_dist + wG[0] * (y + min_px_dist);

      for(int i = begin_idx; i <= end_idx; i++) fh->mask_right[i] = true;
    }

    it_0++;
    it_1++;
  }

  int num_featsneeded_1 = num_features - (int)pts_1.size();
  if (num_featsneeded_1 > std::min(20, (int)(min_feat_percent * num_features)))
  {
    pixelSelector_ptr->pixelSelectionRight(fh, num_featsneeded_1);

    for (int y = 0; y < hG[0]; y++)
    {
      for (int x = 0; x < wG[0]; x++)
      {
        int idx = x + y * wG[0];

        if (fh->selected_pixels_right[idx] == 0) continue;

        cv::KeyPoint kpt;
        kpt.pt.x = x;
        kpt.pt.y = y;

        pts_1.push_back(kpt);
        size_t temp = ++current_id;
        ids_1.push_back(temp);
      }
    }
  }
}
    void performMatching(const std::vector<cv::Mat> &img_0_pyr, const std::vector<cv::Mat> &img_1_pyr, std::vector<cv::KeyPoint> &pts_0,
		std::vector<cv::KeyPoint> &pts_1, size_t id_0, size_t id_1, std::vector<uchar> &mask_out) {
  assert(kpts_0.size() == kpts_1.size());

  if (kpts_0.empty() || kpts_1.empty())
    return;

  std::vector<cv::Point2f> pts_0, pts_1;
  for (size_t i = 0; i < kpts_0.size(); i++)
  {
    pts_0.push_back(kpts_0.at(i).pt);
    pts_1.push_back(kpts_1.at(i).pt);
  }

  if (pts_0.size() < 10)
  {
    for (size_t i = 0; i < pts_0.size(); i++)
      mask_out.push_back((uchar)0);

    return;
  }

  std::vector<uchar> mask_klt;
  std::vector<float> error;
  cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 30, 0.01);
  cv::calcOpticalFlowPyrLK(img_0_pyr, img_1_pyr, pts_0, pts_1, mask_klt, error, win_size, pyr_levels, term_crit, cv::OPTFLOW_USE_INITIAL_FLOW);

  std::vector<cv::Point2f> pts_0_n, pts_1_n;
  for (size_t i = 0; i < pts_0.size(); i++)
  {
    pts_0_n.push_back(camera_calib.at(id_0)->undistortCV(pts_0.at(i)));
    pts_1_n.push_back(camera_calib.at(id_1)->undistortCV(pts_1.at(i)));
  }

  std::vector<uchar> mask_rsc;
  double max_focallength_img_0 = std::max(camera_calib.at(id_0)->getK()(0, 0), camera_calib.at(id_0)->getK()(1, 1));
  double max_focallength_img_1 = std::max(camera_calib.at(id_1)->getK()(0, 0), camera_calib.at(id_1)->getK()(1, 1));
  double max_focallength = std::max(max_focallength_img_0, max_focallength_img_1);
  cv::findFundamentalMat(pts_0_n, pts_1_n, cv::FM_RANSAC, 2.0 / max_focallength, 0.999, mask_rsc);

  for (size_t i = 0; i < mask_klt.size(); i++)
  {
    auto mask = (uchar)((i < mask_klt.size() && mask_klt[i] && i < mask_rsc.size() && mask_rsc[i]) ? 1 : 0);
    mask_out.push_back(mask);
  }

  for (size_t i = 0; i < pts_0.size(); i++)
  {
    kpts_0.at(i).pt = pts_0.at(i);
    kpts_1.at(i).pt = pts_1.at(i);
  }
}
    std::unordered_map<size_t, std::shared_ptr<cameraBase>> camera_calib;
    std::shared_ptr<featureDatabase> database;
    int num_features;
    HistogramMethod histogram_method;
    std::map<size_t, cv::Mat> img_last;
    std::map<size_t, cv::Mat> img_mask_last;
    std::unordered_map<size_t, std::vector<cv::KeyPoint>> pts_last;
    std::unordered_map<size_t, std::vector<size_t>> ids_last;
    std::atomic<size_t> current_id;
    std::shared_ptr<pixelSelector> pixelSelector_ptr;
    int threshold;
    int patch_size_x;
    int patch_size_y;
    int min_px_dist;
    int pyr_levels = 5;
    cv::Size win_size = cv::Size(15, 15);
    std::map<size_t, std::vector<cv::Mat>> img_pyramid_last;
    std::map<size_t, cv::Mat> img_curr;
    std::map<size_t, std::vector<cv::Mat>> img_pyramid_curr;
    boost::posix_time::ptime rT1, rT2, rT3, rT4, rT5, rT6, rT7;
};

/**
 * MIT License
 *
 * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#ifndef TSL_ROBIN_GROWTH_POLICY_H
#define TSL_ROBIN_GROWTH_POLICY_H

#include <algorithm>
#include <array>
#include <climits>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <iterator>
#include <limits>
#include <ratio>
#include <stdexcept>

#ifdef TSL_DEBUG
#    define tsl_rh_assert(expr) assert(expr)
#else
#    define tsl_rh_assert(expr) (static_cast<void>(0))
#endif

/**
 * If exceptions are enabled, throw the exception passed in parameter, otherwise call std::terminate.
 */
#if (defined(__cpp_exceptions) || defined(__EXCEPTIONS) || (defined (_MSC_VER) && defined (_CPPUNWIND))) && !defined(TSL_NO_EXCEPTIONS)
#    define TSL_RH_THROW_OR_TERMINATE(ex, msg) throw ex(msg)
#else
#    define TSL_RH_NO_EXCEPTIONS
#    ifdef NDEBUG
#        define TSL_RH_THROW_OR_TERMINATE(ex, msg) std::terminate()
#    else
#        include <iostream>
#        define TSL_RH_THROW_OR_TERMINATE(ex, msg) do { std::cerr << msg << std::endl; std::terminate(); } while(0)
#    endif
#endif

#if defined(__GNUC__) || defined(__clang__)
#    define TSL_RH_LIKELY(exp) (__builtin_expect(!!(exp), true))
#else
#    define TSL_RH_LIKELY(exp) (exp)
#endif

namespace tsl {
namespace rh {

/**
 * Grow the hash table by a factor of GrowthFactor keeping the bucket count to a power of two. It allows
 * the table to use a mask operation instead of a modulo operation to map a hash to a bucket.
 *
 * GrowthFactor must be a power of two >= 2.
 */
template<std::size_t GrowthFactor>
class power_of_two_growth_policy {
public:
    /**
     * Called on the hash table creation and on rehash. The number of buckets for the table is passed in parameter.
     * This number is a minimum, the policy may update this value with a higher value if needed (but not lower).
     *
     * If 0 is given, min_bucket_count_in_out must still be 0 after the policy creation and
     * bucket_for_hash must always return 0 in this case.
     */
    explicit power_of_two_growth_policy(std::size_t& min_bucket_count_in_out) {
        if(min_bucket_count_in_out > max_bucket_count()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        if(min_bucket_count_in_out > 0) {
            min_bucket_count_in_out = round_up_to_power_of_two(min_bucket_count_in_out);
            m_mask = min_bucket_count_in_out - 1;
        }
        else {
            m_mask = 0;
        }
    }

    /**
     * Return the bucket [0, bucket_count()) to which the hash belongs.
     * If bucket_count() is 0, it must always return 0.
     */
    std::size_t bucket_for_hash(std::size_t hash) const noexcept {
        return hash & m_mask;
    }

    /**
     * Return the number of buckets that should be used on next growth.
     */
    std::size_t next_bucket_count() const {
        if((m_mask + 1) > max_bucket_count() / GrowthFactor) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        return (m_mask + 1) * GrowthFactor;
    }

    /**
     * Return the maximum number of buckets supported by the policy.
     */
    std::size_t max_bucket_count() const {
        // Largest power of two.
        return (std::numeric_limits<std::size_t>::max() / 2) + 1;
    }

    /**
     * Reset the growth policy as if it was created with a bucket count of 0.
     * After a clear, the policy must always return 0 when bucket_for_hash is called.
     */
    void clear() noexcept {
        m_mask = 0;
    }

private:
    static std::size_t round_up_to_power_of_two(std::size_t value) {
        if(is_power_of_two(value)) {
            return value;
        }

        if(value == 0) {
            return 1;
        }

        --value;
        for(std::size_t i = 1; i < sizeof(std::size_t) * CHAR_BIT; i *= 2) {
            value |= value >> i;
        }

        return value + 1;
    }

    static constexpr bool is_power_of_two(std::size_t value) {
        return value != 0 && (value & (value - 1)) == 0;
    }

protected:
    static_assert(is_power_of_two(GrowthFactor) && GrowthFactor >= 2, "GrowthFactor must be a power of two >= 2.");

    std::size_t m_mask;
};

/**
 * Grow the hash table by GrowthFactor::num / GrowthFactor::den and use a modulo to map a hash
 * to a bucket. Slower but it can be useful if you want a slower growth.
 */
template<class GrowthFactor = std::ratio<3, 2>>
class mod_growth_policy {
public:
    explicit mod_growth_policy(std::size_t& min_bucket_count_in_out) {
        if(min_bucket_count_in_out > max_bucket_count()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        if(min_bucket_count_in_out > 0) {
            m_mod = min_bucket_count_in_out;
        }
        else {
            m_mod = 1;
        }
    }

    std::size_t bucket_for_hash(std::size_t hash) const noexcept {
        return hash % m_mod;
    }

    std::size_t next_bucket_count() const {
        if(m_mod == max_bucket_count()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        const double next_bucket_count = std::ceil(double(m_mod) * REHASH_SIZE_MULTIPLICATION_FACTOR);
        if(!std::isnormal(next_bucket_count)) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        if(next_bucket_count > double(max_bucket_count())) {
            return max_bucket_count();
        }
        else {
            return std::size_t(next_bucket_count);
        }
    }

    std::size_t max_bucket_count() const {
        return MAX_BUCKET_COUNT;
    }

    void clear() noexcept {
        m_mod = 1;
    }

private:
    static constexpr double REHASH_SIZE_MULTIPLICATION_FACTOR = 1.0 * GrowthFactor::num / GrowthFactor::den;
    static const std::size_t MAX_BUCKET_COUNT =
            std::size_t(double(
                    std::numeric_limits<std::size_t>::max() / REHASH_SIZE_MULTIPLICATION_FACTOR
            ));

    static_assert(REHASH_SIZE_MULTIPLICATION_FACTOR >= 1.1, "Growth factor should be >= 1.1.");

    std::size_t m_mod;
};

namespace detail {

#if SIZE_MAX >= ULLONG_MAX
#define TSL_RH_NB_PRIMES 51
#elif SIZE_MAX >= ULONG_MAX
#define TSL_RH_NB_PRIMES 40
#else
#define TSL_RH_NB_PRIMES 23
#endif

static constexpr const std::array<std::size_t, TSL_RH_NB_PRIMES> PRIMES = {{
    1u, 5u, 17u, 29u, 37u, 53u, 67u, 79u, 97u, 131u, 193u, 257u, 389u, 521u, 769u, 1031u,
    1543u, 2053u, 3079u, 6151u, 12289u, 24593u, 49157u,
#if SIZE_MAX >= ULONG_MAX
    98317ul, 196613ul, 393241ul, 786433ul, 1572869ul, 3145739ul, 6291469ul, 12582917ul,
    25165843ul, 50331653ul, 100663319ul, 201326611ul, 402653189ul, 805306457ul, 1610612741ul,
    3221225473ul, 4294967291ul,
#endif
#if SIZE_MAX >= ULLONG_MAX
    6442450939ull, 12884901893ull, 25769803751ull, 51539607551ull, 103079215111ull, 206158430209ull,
    412316860441ull, 824633720831ull, 1649267441651ull, 3298534883309ull, 6597069766657ull,
#endif
}};

template<unsigned int IPrime>
static constexpr std::size_t mod(std::size_t hash) { return hash % PRIMES[IPrime]; }

// MOD_PRIME[iprime](hash) returns hash % PRIMES[iprime]. This table allows for faster modulo as the
// compiler can optimize the modulo code better with a constant known at the compilation.
static constexpr const std::array<std::size_t(*)(std::size_t), TSL_RH_NB_PRIMES> MOD_PRIME = {{
    &mod<0>, &mod<1>, &mod<2>, &mod<3>, &mod<4>, &mod<5>, &mod<6>, &mod<7>, &mod<8>, &mod<9>, &mod<10>,
    &mod<11>, &mod<12>, &mod<13>, &mod<14>, &mod<15>, &mod<16>, &mod<17>, &mod<18>, &mod<19>, &mod<20>,
    &mod<21>, &mod<22>,
#if SIZE_MAX >= ULONG_MAX
    &mod<23>, &mod<24>, &mod<25>, &mod<26>, &mod<27>, &mod<28>, &mod<29>, &mod<30>, &mod<31>, &mod<32>,
    &mod<33>, &mod<34>, &mod<35>, &mod<36>, &mod<37> , &mod<38>, &mod<39>,
#endif
#if SIZE_MAX >= ULLONG_MAX
    &mod<40>, &mod<41>, &mod<42>, &mod<43>, &mod<44>, &mod<45>, &mod<46>, &mod<47>, &mod<48>, &mod<49>,
    &mod<50>,
#endif
}};

}

/**
 * Grow the hash table by using prime numbers as bucket count. Slower than tsl::rh::power_of_two_growth_policy in
 * general but will probably distribute the values around better in the buckets with a poor hash function.
 *
 * To allow the compiler to optimize the modulo operation, a lookup table is used with constant primes numbers.
 *
 * With a switch the code would look like:
 * \code
 * switch(iprime) { // iprime is the current prime of the hash table
 *     case 0: hash % 5ul;
 *             break;
 *     case 1: hash % 17ul;
 *             break;
 *     case 2: hash % 29ul;
 *             break;
 *     ...
 * }
 * \endcode
 *
 * Due to the constant variable in the modulo the compiler is able to optimize the operation
 * by a series of multiplications, substractions and shifts.
 *
 * The 'hash % 5' could become something like 'hash - (hash * 0xCCCCCCCD) >> 34) * 5' in a 64 bits environment.
 */
class prime_growth_policy {
public:
    explicit prime_growth_policy(std::size_t& min_bucket_count_in_out) {
        auto it_prime = std::lower_bound(detail::PRIMES.begin(),
                                         detail::PRIMES.end(), min_bucket_count_in_out);
        if(it_prime == detail::PRIMES.end()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        m_iprime = static_cast<unsigned int>(std::distance(detail::PRIMES.begin(), it_prime));
        if(min_bucket_count_in_out > 0) {
            min_bucket_count_in_out = *it_prime;
        }
        else {
            min_bucket_count_in_out = 0;
        }
    }

    std::size_t bucket_for_hash(std::size_t hash) const noexcept {
        return detail::MOD_PRIME[m_iprime](hash);
    }

    std::size_t next_bucket_count() const {
        if(m_iprime + 1 >= detail::PRIMES.size()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        return detail::PRIMES[m_iprime + 1];
    }

    std::size_t max_bucket_count() const {
        return detail::PRIMES.back();
    }

    void clear() noexcept {
        m_iprime = 0;
    }

private:
    unsigned int m_iprime;

    static_assert(std::numeric_limits<decltype(m_iprime)>::max() >= detail::PRIMES.size(),
                  "The type of m_iprime is not big enough.");
};

}
}

#endif

/**
 * MIT License
 *
 * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#ifndef TSL_ROBIN_HASH_H
#define TSL_ROBIN_HASH_H

#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <exception>
#include <iterator>
#include <limits>
#include <memory>
#include <stdexcept>
#include <tuple>
#include <type_traits>
#include <utility>
#include <vector>
#include "robin_growth_policy.h"

namespace tsl {

namespace detail_robin_hash {

template<typename T>
struct make_void {
    using type = void;
};

template<typename T, typename = void>
struct has_is_transparent: std::false_type {
};

template<typename T>
struct has_is_transparent<T, typename make_void<typename T::is_transparent>::type>: std::true_type {
};

template<typename U>
struct is_power_of_two_policy: std::false_type {
};

template<std::size_t GrowthFactor>
struct is_power_of_two_policy<tsl::rh::power_of_two_growth_policy<GrowthFactor>>: std::true_type {
};

// Only available in C++17, we need to be compatible with C++11
template<class T>
const T& clamp( const T& v, const T& lo, const T& hi) {
    return std::min(hi, std::max(lo, v));
}

template<typename T, typename U>
static T numeric_cast(U value, const char* error_message = "numeric_cast() failed.") {
    T ret = static_cast<T>(value);
    if(static_cast<U>(ret) != value) {
        TSL_RH_THROW_OR_TERMINATE(std::runtime_error, error_message);
    }

    const bool is_same_signedness = (std::is_unsigned<T>::value && std::is_unsigned<U>::value) ||
                                    (std::is_signed<T>::value && std::is_signed<U>::value);
    if(!is_same_signedness && (ret < T{}) != (value < U{})) {
        TSL_RH_THROW_OR_TERMINATE(std::runtime_error, error_message);
    }

    return ret;
}

using truncated_hash_type = std::uint_least32_t;

/**
 * Helper class that stores a truncated hash if StoreHash is true and nothing otherwise.
 */
template<bool StoreHash>
class bucket_entry_hash {
public:
    bool bucket_hash_equal(std::size_t /*hash*/) const noexcept {
        return true;
    }

    truncated_hash_type truncated_hash() const noexcept {
        return 0;
    }

protected:
    void set_hash(truncated_hash_type /*hash*/) noexcept {
    }
};

template<>
class bucket_entry_hash<true> {
public:
    bool bucket_hash_equal(std::size_t hash) const noexcept {
        return m_hash == truncated_hash_type(hash);
    }

    truncated_hash_type truncated_hash() const noexcept {
        return m_hash;
    }

protected:
    void set_hash(truncated_hash_type hash) noexcept {
        m_hash = truncated_hash_type(hash);
    }

private:
    truncated_hash_type m_hash;
};

/**
 * Each bucket entry has:
 * - A value of type `ValueType`.
 * - An integer to store how far the value of the bucket, if any, is from its ideal bucket
 *   (ex: if the current bucket 5 has the value 'foo' and `hash('foo') % nb_buckets` == 3,
 *        `dist_from_ideal_bucket()` will return 2 as the current value of the bucket is two
 *        buckets away from its ideal bucket)
 *   If there is no value in the bucket (i.e. `empty()` is true) `dist_from_ideal_bucket()` will be < 0.
 * - A marker which tells us if the bucket is the last bucket of the bucket array (useful for the
 *   iterator of the hash table).
 * - If `StoreHash` is true, 32 bits of the hash of the value, if any, are also stored in the bucket.
 *   If the size of the hash is more than 32 bits, it is truncated. We don't store the full hash
 *   as storing the hash is a potential opportunity to use the unused space due to the alignment
 *   of the bucket_entry structure. We can thus potentially store the hash without any extra space
 *   (which would not be possible with 64 bits of the hash).
 */
template<typename ValueType, bool StoreHash>
class bucket_entry: public bucket_entry_hash<StoreHash> {
    using bucket_hash = bucket_entry_hash<StoreHash>;

public:
    using value_type = ValueType;
    using distance_type = std::int_least16_t;

    bucket_entry() noexcept: bucket_hash(), m_dist_from_ideal_bucket(EMPTY_MARKER_DIST_FROM_IDEAL_BUCKET),
                             m_last_bucket(false)
    {
        tsl_rh_assert(empty());
    }

    bucket_entry(bool last_bucket) noexcept: bucket_hash(), m_dist_from_ideal_bucket(EMPTY_MARKER_DIST_FROM_IDEAL_BUCKET),
                                             m_last_bucket(last_bucket)
    {
        tsl_rh_assert(empty());
    }

    bucket_entry(const bucket_entry& other) noexcept(std::is_nothrow_copy_constructible<value_type>::value):
            bucket_hash(other),
            m_dist_from_ideal_bucket(EMPTY_MARKER_DIST_FROM_IDEAL_BUCKET),
            m_last_bucket(other.m_last_bucket)
    {
        if(!other.empty()) {
            ::new (static_cast<void*>(std::addressof(m_value))) value_type(other.value());
            m_dist_from_ideal_bucket = other.m_dist_from_ideal_bucket;
        }
    }

    /**
     * Never really used, but still necessary as we must call resize on an empty `std::vector<bucket_entry>`.
     * and we need to support move-only types. See robin_hash constructor for details.
     */
    bucket_entry(bucket_entry&& other) noexcept(std::is_nothrow_move_constructible<value_type>::value):
            bucket_hash(std::move(other)),
            m_dist_from_ideal_bucket(EMPTY_MARKER_DIST_FROM_IDEAL_BUCKET),
            m_last_bucket(other.m_last_bucket)
    {
        if(!other.empty()) {
            ::new (static_cast<void*>(std::addressof(m_value))) value_type(std::move(other.value()));
            m_dist_from_ideal_bucket = other.m_dist_from_ideal_bucket;
        }
    }

    bucket_entry& operator=(const bucket_entry& other)
            noexcept(std::is_nothrow_copy_constructible<value_type>::value)
    {
        if(this != &other) {
            clear();

            bucket_hash::operator=(other);
            if(!other.empty()) {
                ::new (static_cast<void*>(std::addressof(m_value))) value_type(other.value());
            }

            m_dist_from_ideal_bucket = other.m_dist_from_ideal_bucket;
            m_last_bucket = other.m_last_bucket;
        }

        return *this;
    }

    bucket_entry& operator=(bucket_entry&& ) = delete;

    ~bucket_entry() noexcept {
        clear();
    }

    void clear() noexcept {
        if(!empty()) {
            destroy_value();
            m_dist_from_ideal_bucket = EMPTY_MARKER_DIST_FROM_IDEAL_BUCKET;
        }
    }

    bool empty() const noexcept {
        return m_dist_from_ideal_bucket == EMPTY_MARKER_DIST_FROM_IDEAL_BUCKET;
    }

    value_type& value() noexcept {
        tsl_rh_assert(!empty());
        return *reinterpret_cast<value_type*>(std::addressof(m_value));
    }

    const value_type& value() const noexcept {
        tsl_rh_assert(!empty());
        return *reinterpret_cast<const value_type*>(std::addressof(m_value));
    }

    distance_type dist_from_ideal_bucket() const noexcept {
        return m_dist_from_ideal_bucket;
    }

    bool last_bucket() const noexcept {
        return m_last_bucket;
    }

    void set_as_last_bucket() noexcept {
        m_last_bucket = true;
    }

    template<typename... Args>
    void set_value_of_empty_bucket(distance_type dist_from_ideal_bucket,
                                   truncated_hash_type hash, Args&&... value_type_args)
    {
        tsl_rh_assert(dist_from_ideal_bucket >= 0);
        tsl_rh_assert(empty());

        ::new (static_cast<void*>(std::addressof(m_value))) value_type(std::forward<Args>(value_type_args)...);
        this->set_hash(hash);
        m_dist_from_ideal_bucket = dist_from_ideal_bucket;

        tsl_rh_assert(!empty());
    }

    void swap_with_value_in_bucket(distance_type& dist_from_ideal_bucket,
                                   truncated_hash_type& hash, value_type& value)
    {
        tsl_rh_assert(!empty());

        using std::swap;
        swap(value, this->value());
        swap(dist_from_ideal_bucket, m_dist_from_ideal_bucket);

        // Avoid warning of unused variable if StoreHash is false
        (void) hash;
        if(StoreHash) {
            const truncated_hash_type tmp_hash = this->truncated_hash();
            this->set_hash(hash);
            hash = tmp_hash;
        }
    }

    static truncated_hash_type truncate_hash(std::size_t hash) noexcept {
        return truncated_hash_type(hash);
    }

private:
    void destroy_value() noexcept {
        tsl_rh_assert(!empty());
        value().~value_type();
    }

public:
    static const distance_type DIST_FROM_IDEAL_BUCKET_LIMIT = 4096;
    static_assert(DIST_FROM_IDEAL_BUCKET_LIMIT <= std::numeric_limits<distance_type>::max() - 1,
                 "DIST_FROM_IDEAL_BUCKET_LIMIT must be <= std::numeric_limits<distance_type>::max() - 1.");

private:
    using storage = typename std::aligned_storage<sizeof(value_type), alignof(value_type)>::type;

    static const distance_type EMPTY_MARKER_DIST_FROM_IDEAL_BUCKET = -1;

    distance_type m_dist_from_ideal_bucket;
    bool m_last_bucket;
    storage m_value;
};

/**
 * Internal common class used by `robin_map` and `robin_set`.
 *
 * ValueType is what will be stored by `robin_hash` (usually `std::pair<Key, T>` for map and `Key` for set).
 *
 * `KeySelect` should be a `FunctionObject` which takes a `ValueType` in parameter and returns a
 *  reference to the key.
 *
 * `ValueSelect` should be a `FunctionObject` which takes a `ValueType` in parameter and returns a
 *  reference to the value. `ValueSelect` should be void if there is no value (in a set for example).
 *
 * The strong exception guarantee only holds if the expression
 * `std::is_nothrow_swappable<ValueType>::value && std::is_nothrow_move_constructible<ValueType>::value` is true.
 *
 * Behaviour is undefined if the destructor of `ValueType` throws.
 */
template<class ValueType,
         class KeySelect,
         class ValueSelect,
         class Hash,
         class KeyEqual,
         class Allocator,
         bool StoreHash,
         class GrowthPolicy>
class robin_hash: private Hash, private KeyEqual, private GrowthPolicy {
private:
    template<typename U>
    using has_mapped_type = typename std::integral_constant<bool, !std::is_same<U, void>::value>;

    static_assert(noexcept(std::declval<GrowthPolicy>().bucket_for_hash(std::size_t(0))), "GrowthPolicy::bucket_for_hash must be noexcept.");
    static_assert(noexcept(std::declval<GrowthPolicy>().clear()), "GrowthPolicy::clear must be noexcept.");

public:
    template<bool IsConst>
    class robin_iterator;

    using key_type = typename KeySelect::key_type;
    using value_type = ValueType;
    using size_type = std::size_t;
    using difference_type = std::ptrdiff_t;
    using hasher = Hash;
    using key_equal = KeyEqual;
    using allocator_type = Allocator;
    using reference = value_type&;
    using const_reference = const value_type&;
    using pointer = value_type*;
    using const_pointer = const value_type*;
    using iterator = robin_iterator<false>;
    using const_iterator = robin_iterator<true>;

private:
    /**
     * Either store the hash because we are asked by the `StoreHash` template parameter
     * or store the hash because it doesn't cost us anything in size and can be used to speed up rehash.
     */
    static constexpr bool STORE_HASH = StoreHash ||
                                       (
                                         (sizeof(tsl::detail_robin_hash::bucket_entry<value_type, true>) ==
                                          sizeof(tsl::detail_robin_hash::bucket_entry<value_type, false>))
                                         &&
                                         (sizeof(std::size_t) == sizeof(truncated_hash_type) ||
                                          is_power_of_two_policy<GrowthPolicy>::value)
                                         &&
                                          // Don't store the hash for primitive types with default hash.
                                          (!std::is_arithmetic<key_type>::value ||
                                           !std::is_same<Hash, std::hash<key_type>>::value)
                                       );

    /**
     * Only use the stored hash on lookup if we are explicitly asked. We are not sure how slow
     * the KeyEqual operation is. An extra comparison may slow things down with a fast KeyEqual.
     */
    static constexpr bool USE_STORED_HASH_ON_LOOKUP = StoreHash;

    /**
     * We can only use the hash on rehash if the size of the hash type is the same as the stored one or
     * if we use a power of two modulo. In the case of the power of two modulo, we just mask
     * the least significant bytes, we just have to check that the truncated_hash_type didn't truncated
     * more bytes.
     */
    static bool USE_STORED_HASH_ON_REHASH(size_type bucket_count) {
        (void) bucket_count;
        if(STORE_HASH && sizeof(std::size_t) == sizeof(truncated_hash_type)) {
            return true;
        }
        else if(STORE_HASH && is_power_of_two_policy<GrowthPolicy>::value) {
            tsl_rh_assert(bucket_count > 0);
            return (bucket_count - 1) <= std::numeric_limits<truncated_hash_type>::max();
        }
        else {
            return false;
        }
    }

    using bucket_entry = tsl::detail_robin_hash::bucket_entry<value_type, STORE_HASH>;
    using distance_type = typename bucket_entry::distance_type;

    using buckets_allocator = typename std::allocator_traits<allocator_type>::template rebind_alloc<bucket_entry>;
    using buckets_container_type = std::vector<bucket_entry, buckets_allocator>;

public:
    /**
     * The 'operator*()' and 'operator->()' methods return a const reference and const pointer respectively to the
     * stored value type.
     *
     * In case of a map, to get a mutable reference to the value associated to a key (the '.second' in the
     * stored pair), you have to call 'value()'.
     *
     * The main reason for this is that if we returned a `std::pair<Key, T>&` instead
     * of a `const std::pair<Key, T>&`, the user may modify the key which will put the map in a undefined state.
     */
    template<bool IsConst>
    class robin_iterator {
        friend class robin_hash;

    private:
        using bucket_entry_ptr = typename std::conditional<IsConst,
                                                           const bucket_entry*,
                                                           bucket_entry*>::type;

        robin_iterator(bucket_entry_ptr bucket) noexcept: m_bucket(bucket) {
        }

    public:
        using iterator_category = std::forward_iterator_tag;
        using value_type = const typename robin_hash::value_type;
        using difference_type = std::ptrdiff_t;
        using reference = value_type&;
        using pointer = value_type*;

        robin_iterator() noexcept {
        }

        // Copy constructor from iterator to const_iterator.
        template<bool TIsConst = IsConst, typename std::enable_if<TIsConst>::type* = nullptr>
        robin_iterator(const robin_iterator<!TIsConst>& other) noexcept: m_bucket(other.m_bucket) {
        }

        robin_iterator(const robin_iterator& other) = default;
        robin_iterator(robin_iterator&& other) = default;
        robin_iterator& operator=(const robin_iterator& other) = default;
        robin_iterator& operator=(robin_iterator&& other) = default;

        const typename robin_hash::key_type& key() const {
            return KeySelect()(m_bucket->value());
        }

        template<class U = ValueSelect, typename std::enable_if<has_mapped_type<U>::value && IsConst>::type* = nullptr>
        const typename U::value_type& value() const {
            return U()(m_bucket->value());
        }

        template<class U = ValueSelect, typename std::enable_if<has_mapped_type<U>::value && !IsConst>::type* = nullptr>
        typename U::value_type& value() const {
            return U()(m_bucket->value());
        }

        reference operator*() const {
            return m_bucket->value();
        }

        pointer operator->() const {
            return std::addressof(m_bucket->value());
        }

        robin_iterator& operator++() {
            while(true) {
                if(m_bucket->last_bucket()) {
                    ++m_bucket;
                    return *this;
                }

                ++m_bucket;
                if(!m_bucket->empty()) {
                    return *this;
                }
            }
        }

        robin_iterator operator++(int) {
            robin_iterator tmp(*this);
            ++*this;

            return tmp;
        }

        friend bool operator==(const robin_iterator& lhs, const robin_iterator& rhs) {
            return lhs.m_bucket == rhs.m_bucket;
        }

        friend bool operator!=(const robin_iterator& lhs, const robin_iterator& rhs) {
            return !(lhs == rhs);
        }

    private:
        bucket_entry_ptr m_bucket;
    };

public:
#if defined(__cplusplus) && __cplusplus >= 201402L
    robin_hash(size_type bucket_count,
               const Hash& hash,
               const KeyEqual& equal,
               const Allocator& alloc,
               float min_load_factor = DEFAULT_MIN_LOAD_FACTOR,
               float max_load_factor = DEFAULT_MAX_LOAD_FACTOR):
                                       Hash(hash),
                                       KeyEqual(equal),
                                       GrowthPolicy(bucket_count),
                                       m_buckets_data(
                                           [&]() {
                                               if(bucket_count > max_bucket_count()) {
                                                   TSL_RH_THROW_OR_TERMINATE(std::length_error,
                                                                             "The map exceeds its maximum bucket count.");
                                               }

                                               return bucket_count;
                                           }(), alloc
                                       ),
                                       m_buckets(m_buckets_data.empty()?static_empty_bucket_ptr():m_buckets_data.data()),
                                       m_bucket_count(bucket_count),
                                       m_nb_elements(0),
                                       m_grow_on_next_insert(false),
                                       m_try_shrink_on_next_insert(false)
    {
        if(m_bucket_count > 0) {
            tsl_rh_assert(!m_buckets_data.empty());
            m_buckets_data.back().set_as_last_bucket();
        }

        this->min_load_factor(min_load_factor);
        this->max_load_factor(max_load_factor);
    }
#else
    /**
     * C++11 doesn't support the creation of a std::vector with a custom allocator and 'count' default-inserted elements.
     * The needed contructor `explicit vector(size_type count, const Allocator& alloc = Allocator());` is only
     * available in C++14 and later. We thus must resize after using the `vector(const Allocator& alloc)` constructor.
     *
     * We can't use `vector(size_type count, const T& value, const Allocator& alloc)` as it requires the
     * value T to be copyable.
     */
    robin_hash(size_type bucket_count,
               const Hash& hash,
               const KeyEqual& equal,
               const Allocator& alloc,
               float min_load_factor = DEFAULT_MIN_LOAD_FACTOR,
               float max_load_factor = DEFAULT_MAX_LOAD_FACTOR):
                                       Hash(hash),
                                       KeyEqual(equal),
                                       GrowthPolicy(bucket_count),
                                       m_buckets_data(alloc),
                                       m_buckets(static_empty_bucket_ptr()),
                                       m_bucket_count(bucket_count),
                                       m_nb_elements(0),
                                       m_grow_on_next_insert(false),
                                       m_try_shrink_on_next_insert(false)
    {
        if(bucket_count > max_bucket_count()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The map exceeds its maximum bucket count.");
        }

        if(m_bucket_count > 0) {
            m_buckets_data.resize(m_bucket_count);
            m_buckets = m_buckets_data.data();

            tsl_rh_assert(!m_buckets_data.empty());
            m_buckets_data.back().set_as_last_bucket();
        }

        this->min_load_factor(min_load_factor);
        this->max_load_factor(max_load_factor);
    }
#endif

    robin_hash(const robin_hash& other): Hash(other),
                                         KeyEqual(other),
                                         GrowthPolicy(other),
                                         m_buckets_data(other.m_buckets_data),
                                         m_buckets(m_buckets_data.empty()?static_empty_bucket_ptr():m_buckets_data.data()),
                                         m_bucket_count(other.m_bucket_count),
                                         m_nb_elements(other.m_nb_elements),
                                         m_load_threshold(other.m_load_threshold),
                                         m_min_load_factor(other.m_min_load_factor),
                                         m_max_load_factor(other.m_max_load_factor),
                                         m_grow_on_next_insert(other.m_grow_on_next_insert),
                                         m_try_shrink_on_next_insert(other.m_try_shrink_on_next_insert)
    {
    }

    robin_hash(robin_hash&& other) noexcept(std::is_nothrow_move_constructible<Hash>::value &&
                                            std::is_nothrow_move_constructible<KeyEqual>::value &&
                                            std::is_nothrow_move_constructible<GrowthPolicy>::value &&
                                            std::is_nothrow_move_constructible<buckets_container_type>::value)
                                          : Hash(std::move(static_cast<Hash&>(other))),
                                            KeyEqual(std::move(static_cast<KeyEqual&>(other))),
                                            GrowthPolicy(std::move(static_cast<GrowthPolicy&>(other))),
                                            m_buckets_data(std::move(other.m_buckets_data)),
                                            m_buckets(m_buckets_data.empty()?static_empty_bucket_ptr():m_buckets_data.data()),
                                            m_bucket_count(other.m_bucket_count),
                                            m_nb_elements(other.m_nb_elements),
                                            m_load_threshold(other.m_load_threshold),
                                            m_min_load_factor(other.m_min_load_factor),
                                            m_max_load_factor(other.m_max_load_factor),
                                            m_grow_on_next_insert(other.m_grow_on_next_insert),
                                            m_try_shrink_on_next_insert(other.m_try_shrink_on_next_insert)
    {
        other.clear_and_shrink();
    }

    robin_hash& operator=(const robin_hash& other) {
        if(&other != this) {
            Hash::operator=(other);
            KeyEqual::operator=(other);
            GrowthPolicy::operator=(other);

            m_buckets_data = other.m_buckets_data;
            m_buckets = m_buckets_data.empty()?static_empty_bucket_ptr():
                                               m_buckets_data.data();
            m_bucket_count = other.m_bucket_count;
            m_nb_elements = other.m_nb_elements;

            m_load_threshold = other.m_load_threshold;
            m_min_load_factor = other.m_min_load_factor;
            m_max_load_factor = other.m_max_load_factor;

            m_grow_on_next_insert = other.m_grow_on_next_insert;
            m_try_shrink_on_next_insert = other.m_try_shrink_on_next_insert;
        }

        return *this;
    }

    robin_hash& operator=(robin_hash&& other) {
        other.swap(*this);
        other.clear();

        return *this;
    }

    allocator_type get_allocator() const {
        return m_buckets_data.get_allocator();
    }

    /*
     * Iterators
     */
    iterator begin() noexcept {
        std::size_t i = 0;
        while(i < m_bucket_count && m_buckets[i].empty()) {
            i++;
        }

        return iterator(m_buckets + i);
    }

    const_iterator begin() const noexcept {
        return cbegin();
    }

    const_iterator cbegin() const noexcept {
        std::size_t i = 0;
        while(i < m_bucket_count && m_buckets[i].empty()) {
            i++;
        }

        return const_iterator(m_buckets + i);
    }

    iterator end() noexcept {
        return iterator(m_buckets + m_bucket_count);
    }

    const_iterator end() const noexcept {
        return cend();
    }

    const_iterator cend() const noexcept {
        return const_iterator(m_buckets + m_bucket_count);
    }

    /*
     * Capacity
     */
    bool empty() const noexcept {
        return m_nb_elements == 0;
    }

    size_type size() const noexcept {
        return m_nb_elements;
    }

    size_type max_size() const noexcept {
        return m_buckets_data.max_size();
    }

    /*
     * Modifiers
     */
    void clear() noexcept {
        if(m_min_load_factor > 0.0f) {
            clear_and_shrink();
        }
        else {
            for(auto& bucket: m_buckets_data) {
                bucket.clear();
            }

            m_nb_elements = 0;
            m_grow_on_next_insert = false;
        }
    }

    template<typename P>
    std::pair<iterator, bool> insert(P&& value) {
        return insert_impl(KeySelect()(value), std::forward<P>(value));
    }

    template<typename P>
    iterator insert_hint(const_iterator hint, P&& value) {
        if(hint != cend() && compare_keys(KeySelect()(*hint), KeySelect()(value))) {
            return mutable_iterator(hint);
        }

        return insert(std::forward<P>(value)).first;
    }

    template<class InputIt>
    void insert(InputIt first, InputIt last) {
        if(std::is_base_of<std::forward_iterator_tag,
                           typename std::iterator_traits<InputIt>::iterator_category>::value)
        {
            const auto nb_elements_insert = std::distance(first, last);
            const size_type nb_free_buckets = m_load_threshold - size();
            tsl_rh_assert(m_load_threshold >= size());

            if(nb_elements_insert > 0 && nb_free_buckets < size_type(nb_elements_insert)) {
                reserve(size() + size_type(nb_elements_insert));
            }
        }

        for(; first != last; ++first) {
            insert(*first);
        }
    }

    template<class K, class M>
    std::pair<iterator, bool> insert_or_assign(K&& key, M&& obj) {
        auto it = try_emplace(std::forward<K>(key), std::forward<M>(obj));
        if(!it.second) {
            it.first.value() = std::forward<M>(obj);
        }

        return it;
    }

    template<class K, class M>
    iterator insert_or_assign(const_iterator hint, K&& key, M&& obj) {
        if(hint != cend() && compare_keys(KeySelect()(*hint), key)) {
            auto it = mutable_iterator(hint);
            it.value() = std::forward<M>(obj);

            return it;
        }

        return insert_or_assign(std::forward<K>(key), std::forward<M>(obj)).first;
    }

    template<class... Args>
    std::pair<iterator, bool> emplace(Args&&... args) {
        return insert(value_type(std::forward<Args>(args)...));
    }

    template<class... Args>
    iterator emplace_hint(const_iterator hint, Args&&... args) {
        return insert_hint(hint, value_type(std::forward<Args>(args)...));
    }

    template<class K, class... Args>
    std::pair<iterator, bool> try_emplace(K&& key, Args&&... args) {
        return insert_impl(key, std::piecewise_construct,
                                std::forward_as_tuple(std::forward<K>(key)),
                                std::forward_as_tuple(std::forward<Args>(args)...));
    }

    template<class K, class... Args>
    iterator try_emplace_hint(const_iterator hint, K&& key, Args&&... args) {
        if(hint != cend() && compare_keys(KeySelect()(*hint), key)) {
            return mutable_iterator(hint);
        }

        return try_emplace(std::forward<K>(key), std::forward<Args>(args)...).first;
    }

    /**
     * Here to avoid `template<class K> size_type erase(const K& key)` being used when
     * we use an `iterator` instead of a `const_iterator`.
     */
    iterator erase(iterator pos) {
        erase_from_bucket(pos);

        /**
         * Erase bucket used a backward shift after clearing the bucket.
         * Check if there is a new value in the bucket, if not get the next non-empty.
         */
        if(pos.m_bucket->empty()) {
            ++pos;
        }

        m_try_shrink_on_next_insert = true;

        return pos;
    }

    iterator erase(const_iterator pos) {
        return erase(mutable_iterator(pos));
    }

    iterator erase(const_iterator first, const_iterator last) {
        if(first == last) {
            return mutable_iterator(first);
        }

        auto first_mutable = mutable_iterator(first);
        auto last_mutable = mutable_iterator(last);
        for(auto it = first_mutable.m_bucket; it != last_mutable.m_bucket; ++it) {
            if(!it->empty()) {
                it->clear();
                m_nb_elements--;
            }
        }

        if(last_mutable == end()) {
            m_try_shrink_on_next_insert = true;
            return end();
        }

        /*
         * Backward shift on the values which come after the deleted values.
         * We try to move the values closer to their ideal bucket.
         */
        std::size_t icloser_bucket = static_cast<std::size_t>(first_mutable.m_bucket - m_buckets);
        std::size_t ito_move_closer_value = static_cast<std::size_t>(last_mutable.m_bucket - m_buckets);
        tsl_rh_assert(ito_move_closer_value > icloser_bucket);

        const std::size_t ireturn_bucket = ito_move_closer_value -
                                           std::min(ito_move_closer_value - icloser_bucket,
                                                    std::size_t(m_buckets[ito_move_closer_value].dist_from_ideal_bucket()));

        while(ito_move_closer_value < m_bucket_count && m_buckets[ito_move_closer_value].dist_from_ideal_bucket() > 0) {
            icloser_bucket = ito_move_closer_value -
                             std::min(ito_move_closer_value - icloser_bucket,
                                      std::size_t(m_buckets[ito_move_closer_value].dist_from_ideal_bucket()));

            tsl_rh_assert(m_buckets[icloser_bucket].empty());
            const distance_type new_distance = distance_type(m_buckets[ito_move_closer_value].dist_from_ideal_bucket() -
                                                             (ito_move_closer_value - icloser_bucket));
            m_buckets[icloser_bucket].set_value_of_empty_bucket(new_distance,
                                                                m_buckets[ito_move_closer_value].truncated_hash(),
                                                                std::move(m_buckets[ito_move_closer_value].value()));
            m_buckets[ito_move_closer_value].clear();

            ++icloser_bucket;
            ++ito_move_closer_value;
        }

        m_try_shrink_on_next_insert = true;

        return iterator(m_buckets + ireturn_bucket);
    }

    template<class K>
    size_type erase(const K& key) {
        return erase(key, hash_key(key));
    }

    template<class K>
    size_type erase(const K& key, std::size_t hash) {
        auto it = find(key, hash);
        if(it != end()) {
            erase_from_bucket(it);
            m_try_shrink_on_next_insert = true;

            return 1;
        }
        else {
            return 0;
        }
    }

    void swap(robin_hash& other) {
        using std::swap;

        swap(static_cast<Hash&>(*this), static_cast<Hash&>(other));
        swap(static_cast<KeyEqual&>(*this), static_cast<KeyEqual&>(other));
        swap(static_cast<GrowthPolicy&>(*this), static_cast<GrowthPolicy&>(other));
        swap(m_buckets_data, other.m_buckets_data);
        swap(m_buckets, other.m_buckets);
        swap(m_bucket_count, other.m_bucket_count);
        swap(m_nb_elements, other.m_nb_elements);
        swap(m_load_threshold, other.m_load_threshold);
        swap(m_min_load_factor, other.m_min_load_factor);
        swap(m_max_load_factor, other.m_max_load_factor);
        swap(m_grow_on_next_insert, other.m_grow_on_next_insert);
        swap(m_try_shrink_on_next_insert, other.m_try_shrink_on_next_insert);
    }

    /*
     * Lookup
     */
    template<class K, class U = ValueSelect, typename std::enable_if<has_mapped_type<U>::value>::type* = nullptr>
    typename U::value_type& at(const K& key) {
        return at(key, hash_key(key));
    }

    template<class K, class U = ValueSelect, typename std::enable_if<has_mapped_type<U>::value>::type* = nullptr>
    typename U::value_type& at(const K& key, std::size_t hash) {
        return const_cast<typename U::value_type&>(static_cast<const robin_hash*>(this)->at(key, hash));
    }

    template<class K, class U = ValueSelect, typename std::enable_if<has_mapped_type<U>::value>::type* = nullptr>
    const typename U::value_type& at(const K& key) const {
        return at(key, hash_key(key));
    }

    template<class K, class U = ValueSelect, typename std::enable_if<has_mapped_type<U>::value>::type* = nullptr>
    const typename U::value_type& at(const K& key, std::size_t hash) const {
        auto it = find(key, hash);
        if(it != cend()) {
            return it.value();
        }
        else {
            TSL_RH_THROW_OR_TERMINATE(std::out_of_range, "Couldn't find key.");
        }
    }

    template<class K, class U = ValueSelect, typename std::enable_if<has_mapped_type<U>::value>::type* = nullptr>
    typename U::value_type& operator[](K&& key) {
        return try_emplace(std::forward<K>(key)).first.value();
    }

    template<class K>
    size_type count(const K& key) const {
        return count(key, hash_key(key));
    }

    template<class K>
    size_type count(const K& key, std::size_t hash) const {
        if(find(key, hash) != cend()) {
            return 1;
        }
        else {
            return 0;
        }
    }

    template<class K>
    iterator find(const K& key) {
        return find_impl(key, hash_key(key));
    }

    template<class K>
    iterator find(const K& key, std::size_t hash) {
        return find_impl(key, hash);
    }

    template<class K>
    const_iterator find(const K& key) const {
        return find_impl(key, hash_key(key));
    }

    template<class K>
    const_iterator find(const K& key, std::size_t hash) const {
        return find_impl(key, hash);
    }

    template<class K>
    bool contains(const K& key) const {
        return contains(key, hash_key(key));
    }

    template<class K>
    bool contains(const K& key, std::size_t hash) const {
        return count(key, hash) != 0;
    }

    template<class K>
    std::pair<iterator, iterator> equal_range(const K& key) {
        return equal_range(key, hash_key(key));
    }

    template<class K>
    std::pair<iterator, iterator> equal_range(const K& key, std::size_t hash) {
        iterator it = find(key, hash);
        return std::make_pair(it, (it == end())?it:std::next(it));
    }

    template<class K>
    std::pair<const_iterator, const_iterator> equal_range(const K& key) const {
        return equal_range(key, hash_key(key));
    }

    template<class K>
    std::pair<const_iterator, const_iterator> equal_range(const K& key, std::size_t hash) const {
        const_iterator it = find(key, hash);
        return std::make_pair(it, (it == cend())?it:std::next(it));
    }

    /*
     * Bucket interface
     */
    size_type bucket_count() const {
        return m_bucket_count;
    }

    size_type max_bucket_count() const {
        return std::min(GrowthPolicy::max_bucket_count(), m_buckets_data.max_size());
    }

    /*
     * Hash policy
     */
    float load_factor() const {
        if(bucket_count() == 0) {
            return 0;
        }

        return float(m_nb_elements)/float(bucket_count());
    }

    float min_load_factor() const {
        return m_min_load_factor;
    }

    float max_load_factor() const {
        return m_max_load_factor;
    }

    void min_load_factor(float ml) {
        m_min_load_factor = clamp(ml, float(MINIMUM_MIN_LOAD_FACTOR),
                                      float(MAXIMUM_MIN_LOAD_FACTOR));
    }

    void max_load_factor(float ml) {
        m_max_load_factor = clamp(ml, float(MINIMUM_MAX_LOAD_FACTOR),
                                      float(MAXIMUM_MAX_LOAD_FACTOR));
        m_load_threshold = size_type(float(bucket_count())*m_max_load_factor);
    }

    void rehash(size_type count) {
        count = std::max(count, size_type(std::ceil(float(size())/max_load_factor())));
        rehash_impl(count);
    }

    void reserve(size_type count) {
        rehash(size_type(std::ceil(float(count)/max_load_factor())));
    }

    /*
     * Observers
     */
    hasher hash_function() const {
        return static_cast<const Hash&>(*this);
    }

    key_equal key_eq() const {
        return static_cast<const KeyEqual&>(*this);
    }

    /*
     * Other
     */
    iterator mutable_iterator(const_iterator pos) {
        return iterator(const_cast<bucket_entry*>(pos.m_bucket));
    }

private:
    template<class K>
    std::size_t hash_key(const K& key) const {
        return Hash::operator()(key);
    }

    template<class K1, class K2>
    bool compare_keys(const K1& key1, const K2& key2) const {
        return KeyEqual::operator()(key1, key2);
    }

    std::size_t bucket_for_hash(std::size_t hash) const {
        const std::size_t bucket = GrowthPolicy::bucket_for_hash(hash);
        tsl_rh_assert(bucket < m_bucket_count || (bucket == 0 && m_bucket_count == 0));

        return bucket;
    }

    template<class U = GrowthPolicy, typename std::enable_if<is_power_of_two_policy<U>::value>::type* = nullptr>
    std::size_t next_bucket(std::size_t index) const noexcept {
        tsl_rh_assert(index < bucket_count());

        return (index + 1) & this->m_mask;
    }

    template<class U = GrowthPolicy, typename std::enable_if<!is_power_of_two_policy<U>::value>::type* = nullptr>
    std::size_t next_bucket(std::size_t index) const noexcept {
        tsl_rh_assert(index < bucket_count());

        index++;
        return (index != bucket_count())?index:0;
    }

    template<class K>
    iterator find_impl(const K& key, std::size_t hash) {
        return mutable_iterator(static_cast<const robin_hash*>(this)->find(key, hash));
    }

    template<class K>
    const_iterator find_impl(const K& key, std::size_t hash) const {
        std::size_t ibucket = bucket_for_hash(hash);
        distance_type dist_from_ideal_bucket = 0;

        while(dist_from_ideal_bucket <= m_buckets[ibucket].dist_from_ideal_bucket()) {
            if(TSL_RH_LIKELY((!USE_STORED_HASH_ON_LOOKUP || m_buckets[ibucket].bucket_hash_equal(hash)) &&
               compare_keys(KeySelect()(m_buckets[ibucket].value()), key)))
            {
                return const_iterator(m_buckets + ibucket);
            }

            ibucket = next_bucket(ibucket);
            dist_from_ideal_bucket++;
        }

        return cend();
    }

    void erase_from_bucket(iterator pos) {
        pos.m_bucket->clear();
        m_nb_elements--;

        /**
         * Backward shift, swap the empty bucket, previous_ibucket, with the values on its right, ibucket,
         * until we cross another empty bucket or if the other bucket has a distance_from_ideal_bucket == 0.
         *
         * We try to move the values closer to their ideal bucket.
         */
        std::size_t previous_ibucket = static_cast<std::size_t>(pos.m_bucket - m_buckets);
        std::size_t ibucket = next_bucket(previous_ibucket);

        while(m_buckets[ibucket].dist_from_ideal_bucket() > 0) {
            tsl_rh_assert(m_buckets[previous_ibucket].empty());

            const distance_type new_distance = distance_type(m_buckets[ibucket].dist_from_ideal_bucket() - 1);
            m_buckets[previous_ibucket].set_value_of_empty_bucket(new_distance, m_buckets[ibucket].truncated_hash(),
                                                                  std::move(m_buckets[ibucket].value()));
            m_buckets[ibucket].clear();

            previous_ibucket = ibucket;
            ibucket = next_bucket(ibucket);
        }
    }

    template<class K, class... Args>
    std::pair<iterator, bool> insert_impl(const K& key, Args&&... value_type_args) {
        const std::size_t hash = hash_key(key);

        std::size_t ibucket = bucket_for_hash(hash);
        distance_type dist_from_ideal_bucket = 0;

        while(dist_from_ideal_bucket <= m_buckets[ibucket].dist_from_ideal_bucket()) {
            if((!USE_STORED_HASH_ON_LOOKUP || m_buckets[ibucket].bucket_hash_equal(hash)) &&
               compare_keys(KeySelect()(m_buckets[ibucket].value()), key))
            {
                return std::make_pair(iterator(m_buckets + ibucket), false);
            }

            ibucket = next_bucket(ibucket);
            dist_from_ideal_bucket++;
        }

        if(rehash_on_extreme_load()) {
            ibucket = bucket_for_hash(hash);
            dist_from_ideal_bucket = 0;

            while(dist_from_ideal_bucket <= m_buckets[ibucket].dist_from_ideal_bucket()) {
                ibucket = next_bucket(ibucket);
                dist_from_ideal_bucket++;
            }
        }

        if(m_buckets[ibucket].empty()) {
            m_buckets[ibucket].set_value_of_empty_bucket(dist_from_ideal_bucket, bucket_entry::truncate_hash(hash),
                                                         std::forward<Args>(value_type_args)...);
        }
        else {
            insert_value(ibucket, dist_from_ideal_bucket, bucket_entry::truncate_hash(hash),
                         std::forward<Args>(value_type_args)...);
        }

        m_nb_elements++;
        /*
         * The value will be inserted in ibucket in any case, either because it was
         * empty or by stealing the bucket (robin hood).
         */
        return std::make_pair(iterator(m_buckets + ibucket), true);
    }

    template<class... Args>
    void insert_value(std::size_t ibucket, distance_type dist_from_ideal_bucket,
                      truncated_hash_type hash, Args&&... value_type_args)
    {
        value_type value(std::forward<Args>(value_type_args)...);
        insert_value_impl(ibucket, dist_from_ideal_bucket, hash, value);
    }

    void insert_value(std::size_t ibucket, distance_type dist_from_ideal_bucket,
                      truncated_hash_type hash, value_type&& value)
    {
        insert_value_impl(ibucket, dist_from_ideal_bucket, hash, value);
    }

    /*
     * We don't use `value_type&& value` as last argument due to a bug in MSVC when `value_type` is a pointer,
     * The compiler is not able to see the difference between `std::string*` and `std::string*&&` resulting in
     * a compilation error.
     *
     * The `value` will be in a moved state at the end of the function.
     */
    void insert_value_impl(std::size_t ibucket, distance_type dist_from_ideal_bucket,
                           truncated_hash_type hash, value_type& value)
    {
        m_buckets[ibucket].swap_with_value_in_bucket(dist_from_ideal_bucket, hash, value);
        ibucket = next_bucket(ibucket);
        dist_from_ideal_bucket++;

        while(!m_buckets[ibucket].empty()) {
            if(dist_from_ideal_bucket > m_buckets[ibucket].dist_from_ideal_bucket()) {
                if(dist_from_ideal_bucket >= bucket_entry::DIST_FROM_IDEAL_BUCKET_LIMIT) {
                    /**
                     * The number of probes is really high, rehash the map on the next insert.
                     * Difficult to do now as rehash may throw an exception.
                     */
                    m_grow_on_next_insert = true;
                }

                m_buckets[ibucket].swap_with_value_in_bucket(dist_from_ideal_bucket, hash, value);
            }

            ibucket = next_bucket(ibucket);
            dist_from_ideal_bucket++;
        }

        m_buckets[ibucket].set_value_of_empty_bucket(dist_from_ideal_bucket, hash, std::move(value));
    }

    void rehash_impl(size_type count) {
        robin_hash new_table(count, static_cast<Hash&>(*this), static_cast<KeyEqual&>(*this),
                             get_allocator(), m_min_load_factor, m_max_load_factor);

        const bool use_stored_hash = USE_STORED_HASH_ON_REHASH(new_table.bucket_count());
        for(auto& bucket: m_buckets_data) {
            if(bucket.empty()) {
                continue;
            }

            const std::size_t hash = use_stored_hash?bucket.truncated_hash():
                                                     new_table.hash_key(KeySelect()(bucket.value()));

            new_table.insert_value_on_rehash(new_table.bucket_for_hash(hash), 0,
                                             bucket_entry::truncate_hash(hash), std::move(bucket.value()));
        }

        new_table.m_nb_elements = m_nb_elements;
        new_table.swap(*this);
    }

    void clear_and_shrink() noexcept {
        GrowthPolicy::clear();
        m_buckets_data.clear();
        m_buckets = static_empty_bucket_ptr();
        m_bucket_count = 0;
        m_nb_elements = 0;
        m_load_threshold = 0;
        m_grow_on_next_insert = false;
        m_try_shrink_on_next_insert = false;
    }

    void insert_value_on_rehash(std::size_t ibucket, distance_type dist_from_ideal_bucket,
                                truncated_hash_type hash, value_type&& value)
    {
        while(true) {
            if(dist_from_ideal_bucket > m_buckets[ibucket].dist_from_ideal_bucket()) {
                if(m_buckets[ibucket].empty()) {
                    m_buckets[ibucket].set_value_of_empty_bucket(dist_from_ideal_bucket, hash, std::move(value));
                    return;
                }
                else {
                    m_buckets[ibucket].swap_with_value_in_bucket(dist_from_ideal_bucket, hash, value);
                }
            }

            dist_from_ideal_bucket++;
            ibucket = next_bucket(ibucket);
        }
    }

    /**
     * Grow the table if m_grow_on_next_insert is true or we reached the max_load_factor.
     * Shrink the table if m_try_shrink_on_next_insert is true (an erase occurred) and
     * we're below the min_load_factor.
     *
     * Return true if the table has been rehashed.
     */
    bool rehash_on_extreme_load() {
        if(m_grow_on_next_insert || size() >= m_load_threshold) {
            rehash_impl(GrowthPolicy::next_bucket_count());
            m_grow_on_next_insert = false;

            return true;
        }

        if(m_try_shrink_on_next_insert) {
            m_try_shrink_on_next_insert = false;
            if(m_min_load_factor != 0.0f && load_factor() < m_min_load_factor) {
                reserve(size() + 1);

                return true;
            }
        }

        return false;
    }

public:
    static const size_type DEFAULT_INIT_BUCKETS_SIZE = 0;

    static constexpr float DEFAULT_MAX_LOAD_FACTOR = 0.5f;
    static constexpr float MINIMUM_MAX_LOAD_FACTOR = 0.2f;
    static constexpr float MAXIMUM_MAX_LOAD_FACTOR = 0.95f;

    static constexpr float DEFAULT_MIN_LOAD_FACTOR = 0.0f;
    static constexpr float MINIMUM_MIN_LOAD_FACTOR = 0.0f;
    static constexpr float MAXIMUM_MIN_LOAD_FACTOR = 0.15f;

    static_assert(MINIMUM_MAX_LOAD_FACTOR < MAXIMUM_MAX_LOAD_FACTOR,
                  "MINIMUM_MAX_LOAD_FACTOR should be < MAXIMUM_MAX_LOAD_FACTOR");
    static_assert(MINIMUM_MIN_LOAD_FACTOR < MAXIMUM_MIN_LOAD_FACTOR,
                  "MINIMUM_MIN_LOAD_FACTOR should be < MAXIMUM_MIN_LOAD_FACTOR");
    static_assert(MAXIMUM_MIN_LOAD_FACTOR < MINIMUM_MAX_LOAD_FACTOR,
                  "MAXIMUM_MIN_LOAD_FACTOR should be < MINIMUM_MAX_LOAD_FACTOR");

private:
    /**
     * Return an always valid pointer to an static empty bucket_entry with last_bucket() == true.
     */
    bucket_entry* static_empty_bucket_ptr() noexcept {
        static bucket_entry empty_bucket(true);
        return &empty_bucket;
    }

private:
    buckets_container_type m_buckets_data;

    /**
     * Points to m_buckets_data.data() if !m_buckets_data.empty() otherwise points to static_empty_bucket_ptr.
     * This variable is useful to avoid the cost of checking if m_buckets_data is empty when trying
     * to find an element.
     *
     * TODO Remove m_buckets_data and only use a pointer instead of a pointer+vector to save some space in the robin_hash object.
     * Manage the Allocator manually.
     */
    bucket_entry* m_buckets;

    /**
     * Used a lot in find, avoid the call to m_buckets_data.size() which is a bit slower.
     */
    size_type m_bucket_count;

    size_type m_nb_elements;

    size_type m_load_threshold;

    float m_min_load_factor;
    float m_max_load_factor;

    bool m_grow_on_next_insert;

    /**
     * We can't shrink down the map on erase operations as the erase methods need to return the next iterator.
     * Shrinking the map would invalidate all the iterators and we could not return the next iterator in a meaningful way,
     * On erase, we thus just indicate on erase that we should try to shrink the hash table on the next insert
     * if we go below the min_load_factor.
     */
    bool m_try_shrink_on_next_insert;
};

}

}

#endif

/**
 * MIT License
 *
 * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#ifndef TSL_ROBIN_MAP_H
#define TSL_ROBIN_MAP_H

#include <cstddef>
#include <functional>
#include <initializer_list>
#include <memory>
#include <type_traits>
#include <utility>
#include "robin_hash.h"

namespace tsl {

/**
 * Implementation of a hash map using open-addressing and the robin hood hashing algorithm with backward shift deletion.
 *
 * For operations modifying the hash map (insert, erase, rehash, ...), the strong exception guarantee
 * is only guaranteed when the expression `std::is_nothrow_swappable<std::pair<Key, T>>::value &&
 * std::is_nothrow_move_constructible<std::pair<Key, T>>::value` is true, otherwise if an exception
 * is thrown during the swap or the move, the hash map may end up in a undefined state. Per the standard
 * a `Key` or `T` with a noexcept copy constructor and no move constructor also satisfies the
 * `std::is_nothrow_move_constructible<std::pair<Key, T>>::value` criterion (and will thus guarantee the
 * strong exception for the map).
 *
 * When `StoreHash` is true, 32 bits of the hash are stored alongside the values. It can improve
 * the performance during lookups if the `KeyEqual` function takes time (if it engenders a cache-miss for example)
 * as we then compare the stored hashes before comparing the keys. When `tsl::rh::power_of_two_growth_policy` is used
 * as `GrowthPolicy`, it may also speed-up the rehash process as we can avoid to recalculate the hash.
 * When it is detected that storing the hash will not incur any memory penalty due to alignment (i.e.
 * `sizeof(tsl::detail_robin_hash::bucket_entry<ValueType, true>) ==
 * sizeof(tsl::detail_robin_hash::bucket_entry<ValueType, false>)`) and `tsl::rh::power_of_two_growth_policy` is
 * used, the hash will be stored even if `StoreHash` is false so that we can speed-up the rehash (but it will
 * not be used on lookups unless `StoreHash` is true).
 *
 * `GrowthPolicy` defines how the map grows and consequently how a hash value is mapped to a bucket.
 * By default the map uses `tsl::rh::power_of_two_growth_policy`. This policy keeps the number of buckets
 * to a power of two and uses a mask to map the hash to a bucket instead of the slow modulo.
 * Other growth policies are available and you may define your own growth policy,
 * check `tsl::rh::power_of_two_growth_policy` for the interface.
 *
 * `std::pair<Key, T>` must be swappable.
 *
 * `Key` and `T` must be copy and/or move constructible.
 *
 * If the destructor of `Key` or `T` throws an exception, the behaviour of the class is undefined.
 *
 * Iterators invalidation:
 *  - clear, operator=, reserve, rehash: always invalidate the iterators.
 *  - insert, emplace, emplace_hint, operator[]: if there is an effective insert, invalidate the iterators.
 *  - erase: always invalidate the iterators.
 */
template<class Key,
         class T,
         class Hash = std::hash<Key>,
         class KeyEqual = std::equal_to<Key>,
         class Allocator = std::allocator<std::pair<Key, T>>,
         bool StoreHash = false,
         class GrowthPolicy = tsl::rh::power_of_two_growth_policy<2>>
class robin_map {
private:
    template<typename U>
    using has_is_transparent = tsl::detail_robin_hash::has_is_transparent<U>;

    class KeySelect {
    public:
        using key_type = Key;

        const key_type& operator()(const std::pair<Key, T>& key_value) const noexcept {
            return key_value.first;
        }

        key_type& operator()(std::pair<Key, T>& key_value) noexcept {
            return key_value.first;
        }
    };

    class ValueSelect {
    public:
        using value_type = T;

        const value_type& operator()(const std::pair<Key, T>& key_value) const noexcept {
            return key_value.second;
        }

        value_type& operator()(std::pair<Key, T>& key_value) noexcept {
            return key_value.second;
        }
    };

    using ht = detail_robin_hash::robin_hash<std::pair<Key, T>, KeySelect, ValueSelect,
                                             Hash, KeyEqual, Allocator, StoreHash, GrowthPolicy>;

public:
    using key_type = typename ht::key_type;
    using mapped_type = T;
    using value_type = typename ht::value_type;
    using size_type = typename ht::size_type;
    using difference_type = typename ht::difference_type;
    using hasher = typename ht::hasher;
    using key_equal = typename ht::key_equal;
    using allocator_type = typename ht::allocator_type;
    using reference = typename ht::reference;
    using const_reference = typename ht::const_reference;
    using pointer = typename ht::pointer;
    using const_pointer = typename ht::const_pointer;
    using iterator = typename ht::iterator;
    using const_iterator = typename ht::const_iterator;

public:
    /*
     * Constructors
     */
    robin_map(): robin_map(ht::DEFAULT_INIT_BUCKETS_SIZE) {
    }

    explicit robin_map(size_type bucket_count,
                       const Hash& hash = Hash(),
                       const KeyEqual& equal = KeyEqual(),
                       const Allocator& alloc = Allocator()):
                m_ht(bucket_count, hash, equal, alloc)
    {
    }

    robin_map(size_type bucket_count,
              const Allocator& alloc): robin_map(bucket_count, Hash(), KeyEqual(), alloc)
    {
    }

    robin_map(size_type bucket_count,
              const Hash& hash,
              const Allocator& alloc): robin_map(bucket_count, hash, KeyEqual(), alloc)
    {
    }

    explicit robin_map(const Allocator& alloc): robin_map(ht::DEFAULT_INIT_BUCKETS_SIZE, alloc) {
    }

    template<class InputIt>
    robin_map(InputIt first, InputIt last,
              size_type bucket_count = ht::DEFAULT_INIT_BUCKETS_SIZE,
              const Hash& hash = Hash(),
              const KeyEqual& equal = KeyEqual(),
              const Allocator& alloc = Allocator()): robin_map(bucket_count, hash, equal, alloc)
    {
        insert(first, last);
    }

    template<class InputIt>
    robin_map(InputIt first, InputIt last,
              size_type bucket_count,
              const Allocator& alloc): robin_map(first, last, bucket_count, Hash(), KeyEqual(), alloc)
    {
    }

    template<class InputIt>
    robin_map(InputIt first, InputIt last,
              size_type bucket_count,
              const Hash& hash,
              const Allocator& alloc): robin_map(first, last, bucket_count, hash, KeyEqual(), alloc)
    {
    }

    robin_map(std::initializer_list<value_type> init,
              size_type bucket_count = ht::DEFAULT_INIT_BUCKETS_SIZE,
              const Hash& hash = Hash(),
              const KeyEqual& equal = KeyEqual(),
              const Allocator& alloc = Allocator()):
          robin_map(init.begin(), init.end(), bucket_count, hash, equal, alloc)
    {
    }

    robin_map(std::initializer_list<value_type> init,
              size_type bucket_count,
              const Allocator& alloc):
          robin_map(init.begin(), init.end(), bucket_count, Hash(), KeyEqual(), alloc)
    {
    }

    robin_map(std::initializer_list<value_type> init,
              size_type bucket_count,
              const Hash& hash,
              const Allocator& alloc):
          robin_map(init.begin(), init.end(), bucket_count, hash, KeyEqual(), alloc)
    {
    }

    robin_map& operator=(std::initializer_list<value_type> ilist) {
        m_ht.clear();

        m_ht.reserve(ilist.size());
        m_ht.insert(ilist.begin(), ilist.end());

        return *this;
    }

    allocator_type get_allocator() const { return m_ht.get_allocator(); }

    /*
     * Iterators
     */
    iterator begin() noexcept { return m_ht.begin(); }
    const_iterator begin() const noexcept { return m_ht.begin(); }
    const_iterator cbegin() const noexcept { return m_ht.cbegin(); }

    iterator end() noexcept { return m_ht.end(); }
    const_iterator end() const noexcept { return m_ht.end(); }
    const_iterator cend() const noexcept { return m_ht.cend(); }

    /*
     * Capacity
     */
    bool empty() const noexcept { return m_ht.empty(); }
    size_type size() const noexcept { return m_ht.size(); }
    size_type max_size() const noexcept { return m_ht.max_size(); }

    /*
     * Modifiers
     */
    void clear() noexcept { m_ht.clear(); }

    std::pair<iterator, bool> insert(const value_type& value) {
        return m_ht.insert(value);
    }

    template<class P, typename std::enable_if<std::is_constructible<value_type, P&&>::value>::type* = nullptr>
    std::pair<iterator, bool> insert(P&& value) {
        return m_ht.emplace(std::forward<P>(value));
    }

    std::pair<iterator, bool> insert(value_type&& value) {
        return m_ht.insert(std::move(value));
    }

    iterator insert(const_iterator hint, const value_type& value) {
        return m_ht.insert_hint(hint, value);
    }

    template<class P, typename std::enable_if<std::is_constructible<value_type, P&&>::value>::type* = nullptr>
    iterator insert(const_iterator hint, P&& value) {
        return m_ht.emplace_hint(hint, std::forward<P>(value));
    }

    iterator insert(const_iterator hint, value_type&& value) {
        return m_ht.insert_hint(hint, std::move(value));
    }

    template<class InputIt>
    void insert(InputIt first, InputIt last) {
        m_ht.insert(first, last);
    }

    void insert(std::initializer_list<value_type> ilist) {
        m_ht.insert(ilist.begin(), ilist.end());
    }

    template<class M>
    std::pair<iterator, bool> insert_or_assign(const key_type& k, M&& obj) {
        return m_ht.insert_or_assign(k, std::forward<M>(obj));
    }

    template<class M>
    std::pair<iterator, bool> insert_or_assign(key_type&& k, M&& obj) {
        return m_ht.insert_or_assign(std::move(k), std::forward<M>(obj));
    }

    template<class M>
    iterator insert_or_assign(const_iterator hint, const key_type& k, M&& obj) {
        return m_ht.insert_or_assign(hint, k, std::forward<M>(obj));
    }

    template<class M>
    iterator insert_or_assign(const_iterator hint, key_type&& k, M&& obj) {
        return m_ht.insert_or_assign(hint, std::move(k), std::forward<M>(obj));
    }

    /**
     * Due to the way elements are stored, emplace will need to move or copy the key-value once.
     * The method is equivalent to insert(value_type(std::forward<Args>(args)...));
     *
     * Mainly here for compatibility with the std::unordered_map interface.
     */
    template<class... Args>
    std::pair<iterator, bool> emplace(Args&&... args) {
        return m_ht.emplace(std::forward<Args>(args)...);
    }

    /**
     * Due to the way elements are stored, emplace_hint will need to move or copy the key-value once.
     * The method is equivalent to insert(hint, value_type(std::forward<Args>(args)...));
     *
     * Mainly here for compatibility with the std::unordered_map interface.
     */
    template<class... Args>
    iterator emplace_hint(const_iterator hint, Args&&... args) {
        return m_ht.emplace_hint(hint, std::forward<Args>(args)...);
    }

    template<class... Args>
    std::pair<iterator, bool> try_emplace(const key_type& k, Args&&... args) {
        return m_ht.try_emplace(k, std::forward<Args>(args)...);
    }

    template<class... Args>
    std::pair<iterator, bool> try_emplace(key_type&& k, Args&&... args) {
        return m_ht.try_emplace(std::move(k), std::forward<Args>(args)...);
    }

    template<class... Args>
    iterator try_emplace(const_iterator hint, const key_type& k, Args&&... args) {
        return m_ht.try_emplace_hint(hint, k, std::forward<Args>(args)...);
    }

    template<class... Args>
    iterator try_emplace(const_iterator hint, key_type&& k, Args&&... args) {
        return m_ht.try_emplace_hint(hint, std::move(k), std::forward<Args>(args)...);
    }

    iterator erase(iterator pos) { return m_ht.erase(pos); }
    iterator erase(const_iterator pos) { return m_ht.erase(pos); }
    iterator erase(const_iterator first, const_iterator last) { return m_ht.erase(first, last); }
    size_type erase(const key_type& key) { return m_ht.erase(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup to the value if you already have the hash.
     */
    size_type erase(const key_type& key, std::size_t precalculated_hash) {
        return m_ht.erase(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type erase(const K& key) { return m_ht.erase(key); }

    /**
     * @copydoc erase(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup to the value if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type erase(const K& key, std::size_t precalculated_hash) {
        return m_ht.erase(key, precalculated_hash);
    }

    void swap(robin_map& other) { other.m_ht.swap(m_ht); }

    /*
     * Lookup
     */
    T& at(const Key& key) { return m_ht.at(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    T& at(const Key& key, std::size_t precalculated_hash) { return m_ht.at(key, precalculated_hash); }

    const T& at(const Key& key) const { return m_ht.at(key); }

    /**
     * @copydoc at(const Key& key, std::size_t precalculated_hash)
     */
    const T& at(const Key& key, std::size_t precalculated_hash) const { return m_ht.at(key, precalculated_hash); }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    T& at(const K& key) { return m_ht.at(key); }

    /**
     * @copydoc at(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    T& at(const K& key, std::size_t precalculated_hash) { return m_ht.at(key, precalculated_hash); }

    /**
     * @copydoc at(const K& key)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    const T& at(const K& key) const { return m_ht.at(key); }

    /**
     * @copydoc at(const K& key, std::size_t precalculated_hash)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    const T& at(const K& key, std::size_t precalculated_hash) const { return m_ht.at(key, precalculated_hash); }

    T& operator[](const Key& key) { return m_ht[key]; }
    T& operator[](Key&& key) { return m_ht[std::move(key)]; }

    size_type count(const Key& key) const { return m_ht.count(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    size_type count(const Key& key, std::size_t precalculated_hash) const {
        return m_ht.count(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type count(const K& key) const { return m_ht.count(key); }

    /**
     * @copydoc count(const K& key) const
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type count(const K& key, std::size_t precalculated_hash) const { return m_ht.count(key, precalculated_hash); }

    iterator find(const Key& key) { return m_ht.find(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    iterator find(const Key& key, std::size_t precalculated_hash) { return m_ht.find(key, precalculated_hash); }

    const_iterator find(const Key& key) const { return m_ht.find(key); }

    /**
     * @copydoc find(const Key& key, std::size_t precalculated_hash)
     */
    const_iterator find(const Key& key, std::size_t precalculated_hash) const {
        return m_ht.find(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    iterator find(const K& key) { return m_ht.find(key); }

    /**
     * @copydoc find(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    iterator find(const K& key, std::size_t precalculated_hash) { return m_ht.find(key, precalculated_hash); }

    /**
     * @copydoc find(const K& key)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    const_iterator find(const K& key) const { return m_ht.find(key); }

    /**
     * @copydoc find(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    const_iterator find(const K& key, std::size_t precalculated_hash) const {
        return m_ht.find(key, precalculated_hash);
    }

    bool contains(const Key& key) const { return m_ht.contains(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    bool contains(const Key& key, std::size_t precalculated_hash) const {
        return m_ht.contains(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    bool contains(const K& key) const { return m_ht.contains(key); }

    /**
     * @copydoc contains(const K& key) const
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    bool contains(const K& key, std::size_t precalculated_hash) const {
        return m_ht.contains(key, precalculated_hash);
    }

    std::pair<iterator, iterator> equal_range(const Key& key) { return m_ht.equal_range(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    std::pair<iterator, iterator> equal_range(const Key& key, std::size_t precalculated_hash) {
        return m_ht.equal_range(key, precalculated_hash);
    }

    std::pair<const_iterator, const_iterator> equal_range(const Key& key) const { return m_ht.equal_range(key); }

    /**
     * @copydoc equal_range(const Key& key, std::size_t precalculated_hash)
     */
    std::pair<const_iterator, const_iterator> equal_range(const Key& key, std::size_t precalculated_hash) const {
        return m_ht.equal_range(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<iterator, iterator> equal_range(const K& key) { return m_ht.equal_range(key); }

    /**
     * @copydoc equal_range(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<iterator, iterator> equal_range(const K& key, std::size_t precalculated_hash) {
        return m_ht.equal_range(key, precalculated_hash);
    }

    /**
     * @copydoc equal_range(const K& key)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<const_iterator, const_iterator> equal_range(const K& key) const { return m_ht.equal_range(key); }

    /**
     * @copydoc equal_range(const K& key, std::size_t precalculated_hash)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<const_iterator, const_iterator> equal_range(const K& key, std::size_t precalculated_hash) const {
        return m_ht.equal_range(key, precalculated_hash);
    }

    /*
     * Bucket interface
     */
    size_type bucket_count() const { return m_ht.bucket_count(); }
    size_type max_bucket_count() const { return m_ht.max_bucket_count(); }

    /*
     *  Hash policy
     */
    float load_factor() const { return m_ht.load_factor(); }

    float min_load_factor() const { return m_ht.min_load_factor(); }
    float max_load_factor() const { return m_ht.max_load_factor(); }

    /**
     * Set the `min_load_factor` to `ml`. When the `load_factor` of the map goes
     * below `min_load_factor` after some erase operations, the map will be
     * shrunk when an insertion occurs. The erase method itself never shrinks
     * the map.
     *
     * The default value of `min_load_factor` is 0.0f, the map never shrinks by default.
     */
    void min_load_factor(float ml) { m_ht.min_load_factor(ml); }
    void max_load_factor(float ml) { m_ht.max_load_factor(ml); }

    void rehash(size_type count) { m_ht.rehash(count); }
    void reserve(size_type count) { m_ht.reserve(count); }

    /*
     * Observers
     */
    hasher hash_function() const { return m_ht.hash_function(); }
    key_equal key_eq() const { return m_ht.key_eq(); }

    /*
     * Other
     */

    /**
     * Convert a const_iterator to an iterator.
     */
    iterator mutable_iterator(const_iterator pos) {
        return m_ht.mutable_iterator(pos);
    }

    friend bool operator==(const robin_map& lhs, const robin_map& rhs) {
        if(lhs.size() != rhs.size()) {
            return false;
        }

        for(const auto& element_lhs: lhs) {
            const auto it_element_rhs = rhs.find(element_lhs.first);
            if(it_element_rhs == rhs.cend() || element_lhs.second != it_element_rhs->second) {
                return false;
            }
        }

        return true;
    }

    friend bool operator!=(const robin_map& lhs, const robin_map& rhs) {
        return !operator==(lhs, rhs);
    }

    friend void swap(robin_map& lhs, robin_map& rhs) {
        lhs.swap(rhs);
    }

private:
    ht m_ht;
};

/**
 * Same as `tsl::robin_map<Key, T, Hash, KeyEqual, Allocator, StoreHash, tsl::rh::prime_growth_policy>`.
 */
template<class Key,
         class T,
         class Hash = std::hash<Key>,
         class KeyEqual = std::equal_to<Key>,
         class Allocator = std::allocator<std::pair<Key, T>>,
         bool StoreHash = false>
using robin_pg_map = robin_map<Key, T, Hash, KeyEqual, Allocator, StoreHash, tsl::rh::prime_growth_policy>;

} // end namespace tsl

#endif

/**
 * MIT License
 *
 * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#ifndef TSL_ROBIN_SET_H
#define TSL_ROBIN_SET_H

#include <cstddef>
#include <functional>
#include <initializer_list>
#include <memory>
#include <type_traits>
#include <utility>
#include "robin_hash.h"

namespace tsl {

/**
 * Implementation of a hash set using open-addressing and the robin hood hashing algorithm with backward shift deletion.
 *
 * For operations modifying the hash set (insert, erase, rehash, ...), the strong exception guarantee
 * is only guaranteed when the expression `std::is_nothrow_swappable<Key>::value &&
 * std::is_nothrow_move_constructible<Key>::value` is true, otherwise if an exception
 * is thrown during the swap or the move, the hash set may end up in a undefined state. Per the standard
 * a `Key` with a noexcept copy constructor and no move constructor also satisfies the
 * `std::is_nothrow_move_constructible<Key>::value` criterion (and will thus guarantee the
 * strong exception for the set).
 *
 * When `StoreHash` is true, 32 bits of the hash are stored alongside the values. It can improve
 * the performance during lookups if the `KeyEqual` function takes time (or engenders a cache-miss for example)
 * as we then compare the stored hashes before comparing the keys. When `tsl::rh::power_of_two_growth_policy` is used
 * as `GrowthPolicy`, it may also speed-up the rehash process as we can avoid to recalculate the hash.
 * When it is detected that storing the hash will not incur any memory penalty due to alignment (i.e.
 * `sizeof(tsl::detail_robin_hash::bucket_entry<ValueType, true>) ==
 * sizeof(tsl::detail_robin_hash::bucket_entry<ValueType, false>)`) and `tsl::rh::power_of_two_growth_policy` is
 * used, the hash will be stored even if `StoreHash` is false so that we can speed-up the rehash (but it will
 * not be used on lookups unless `StoreHash` is true).
 *
 * `GrowthPolicy` defines how the set grows and consequently how a hash value is mapped to a bucket.
 * By default the set uses `tsl::rh::power_of_two_growth_policy`. This policy keeps the number of buckets
 * to a power of two and uses a mask to set the hash to a bucket instead of the slow modulo.
 * Other growth policies are available and you may define your own growth policy,
 * check `tsl::rh::power_of_two_growth_policy` for the interface.
 *
 * `Key` must be swappable.
 *
 * `Key` must be copy and/or move constructible.
 *
 * If the destructor of `Key` throws an exception, the behaviour of the class is undefined.
 *
 * Iterators invalidation:
 *  - clear, operator=, reserve, rehash: always invalidate the iterators.
 *  - insert, emplace, emplace_hint, operator[]: if there is an effective insert, invalidate the iterators.
 *  - erase: always invalidate the iterators.
 */
template<class Key,
         class Hash = std::hash<Key>,
         class KeyEqual = std::equal_to<Key>,
         class Allocator = std::allocator<Key>,
         bool StoreHash = false,
         class GrowthPolicy = tsl::rh::power_of_two_growth_policy<2>>
class robin_set {
private:
    template<typename U>
    using has_is_transparent = tsl::detail_robin_hash::has_is_transparent<U>;

    class KeySelect {
    public:
        using key_type = Key;

        const key_type& operator()(const Key& key) const noexcept {
            return key;
        }

        key_type& operator()(Key& key) noexcept {
            return key;
        }
    };

    using ht = detail_robin_hash::robin_hash<Key, KeySelect, void,
                                             Hash, KeyEqual, Allocator, StoreHash, GrowthPolicy>;

public:
    using key_type = typename ht::key_type;
    using value_type = typename ht::value_type;
    using size_type = typename ht::size_type;
    using difference_type = typename ht::difference_type;
    using hasher = typename ht::hasher;
    using key_equal = typename ht::key_equal;
    using allocator_type = typename ht::allocator_type;
    using reference = typename ht::reference;
    using const_reference = typename ht::const_reference;
    using pointer = typename ht::pointer;
    using const_pointer = typename ht::const_pointer;
    using iterator = typename ht::iterator;
    using const_iterator = typename ht::const_iterator;

    /*
     * Constructors
     */
    robin_set(): robin_set(ht::DEFAULT_INIT_BUCKETS_SIZE) {
    }

    explicit robin_set(size_type bucket_count,
                       const Hash& hash = Hash(),
                       const KeyEqual& equal = KeyEqual(),
                       const Allocator& alloc = Allocator()):
                    m_ht(bucket_count, hash, equal, alloc)
    {
    }

    robin_set(size_type bucket_count,
              const Allocator& alloc): robin_set(bucket_count, Hash(), KeyEqual(), alloc)
    {
    }

    robin_set(size_type bucket_count,
              const Hash& hash,
              const Allocator& alloc): robin_set(bucket_count, hash, KeyEqual(), alloc)
    {
    }

    explicit robin_set(const Allocator& alloc): robin_set(ht::DEFAULT_INIT_BUCKETS_SIZE, alloc) {
    }

    template<class InputIt>
    robin_set(InputIt first, InputIt last,
              size_type bucket_count = ht::DEFAULT_INIT_BUCKETS_SIZE,
              const Hash& hash = Hash(),
              const KeyEqual& equal = KeyEqual(),
              const Allocator& alloc = Allocator()): robin_set(bucket_count, hash, equal, alloc)
    {
        insert(first, last);
    }

    template<class InputIt>
    robin_set(InputIt first, InputIt last,
              size_type bucket_count,
              const Allocator& alloc): robin_set(first, last, bucket_count, Hash(), KeyEqual(), alloc)
    {
    }

    template<class InputIt>
    robin_set(InputIt first, InputIt last,
              size_type bucket_count,
              const Hash& hash,
              const Allocator& alloc): robin_set(first, last, bucket_count, hash, KeyEqual(), alloc)
    {
    }

    robin_set(std::initializer_list<value_type> init,
              size_type bucket_count = ht::DEFAULT_INIT_BUCKETS_SIZE,
              const Hash& hash = Hash(),
              const KeyEqual& equal = KeyEqual(),
              const Allocator& alloc = Allocator()):
          robin_set(init.begin(), init.end(), bucket_count, hash, equal, alloc)
    {
    }

    robin_set(std::initializer_list<value_type> init,
              size_type bucket_count,
              const Allocator& alloc):
          robin_set(init.begin(), init.end(), bucket_count, Hash(), KeyEqual(), alloc)
    {
    }

    robin_set(std::initializer_list<value_type> init,
              size_type bucket_count,
              const Hash& hash,
              const Allocator& alloc):
          robin_set(init.begin(), init.end(), bucket_count, hash, KeyEqual(), alloc)
    {
    }

    robin_set& operator=(std::initializer_list<value_type> ilist) {
        m_ht.clear();

        m_ht.reserve(ilist.size());
        m_ht.insert(ilist.begin(), ilist.end());

        return *this;
    }

    allocator_type get_allocator() const { return m_ht.get_allocator(); }

    /*
     * Iterators
     */
    iterator begin() noexcept { return m_ht.begin(); }
    const_iterator begin() const noexcept { return m_ht.begin(); }
    const_iterator cbegin() const noexcept { return m_ht.cbegin(); }

    iterator end() noexcept { return m_ht.end(); }
    const_iterator end() const noexcept { return m_ht.end(); }
    const_iterator cend() const noexcept { return m_ht.cend(); }

    /*
     * Capacity
     */
    bool empty() const noexcept { return m_ht.empty(); }
    size_type size() const noexcept { return m_ht.size(); }
    size_type max_size() const noexcept { return m_ht.max_size(); }

    /*
     * Modifiers
     */
    void clear() noexcept { m_ht.clear(); }

    std::pair<iterator, bool> insert(const value_type& value) {
        return m_ht.insert(value);
    }

    std::pair<iterator, bool> insert(value_type&& value) {
        return m_ht.insert(std::move(value));
    }

    iterator insert(const_iterator hint, const value_type& value) {
        return m_ht.insert_hint(hint, value);
    }

    iterator insert(const_iterator hint, value_type&& value) {
        return m_ht.insert_hint(hint, std::move(value));
    }

    template<class InputIt>
    void insert(InputIt first, InputIt last) {
        m_ht.insert(first, last);
    }

    void insert(std::initializer_list<value_type> ilist) {
        m_ht.insert(ilist.begin(), ilist.end());
    }

    /**
     * Due to the way elements are stored, emplace will need to move or copy the key-value once.
     * The method is equivalent to insert(value_type(std::forward<Args>(args)...));
     *
     * Mainly here for compatibility with the std::unordered_map interface.
     */
    template<class... Args>
    std::pair<iterator, bool> emplace(Args&&... args) {
        return m_ht.emplace(std::forward<Args>(args)...);
    }

    /**
     * Due to the way elements are stored, emplace_hint will need to move or copy the key-value once.
     * The method is equivalent to insert(hint, value_type(std::forward<Args>(args)...));
     *
     * Mainly here for compatibility with the std::unordered_map interface.
     */
    template<class... Args>
    iterator emplace_hint(const_iterator hint, Args&&... args) {
        return m_ht.emplace_hint(hint, std::forward<Args>(args)...);
    }

    iterator erase(iterator pos) { return m_ht.erase(pos); }
    iterator erase(const_iterator pos) { return m_ht.erase(pos); }
    iterator erase(const_iterator first, const_iterator last) { return m_ht.erase(first, last); }
    size_type erase(const key_type& key) { return m_ht.erase(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup to the value if you already have the hash.
     */
    size_type erase(const key_type& key, std::size_t precalculated_hash) {
        return m_ht.erase(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type erase(const K& key) { return m_ht.erase(key); }

    /**
     * @copydoc erase(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup to the value if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type erase(const K& key, std::size_t precalculated_hash) {
        return m_ht.erase(key, precalculated_hash);
    }

    void swap(robin_set& other) { other.m_ht.swap(m_ht); }

    /*
     * Lookup
     */
    size_type count(const Key& key) const { return m_ht.count(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    size_type count(const Key& key, std::size_t precalculated_hash) const { return m_ht.count(key, precalculated_hash); }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type count(const K& key) const { return m_ht.count(key); }

    /**
     * @copydoc count(const K& key) const
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type count(const K& key, std::size_t precalculated_hash) const { return m_ht.count(key, precalculated_hash); }

    iterator find(const Key& key) { return m_ht.find(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    iterator find(const Key& key, std::size_t precalculated_hash) { return m_ht.find(key, precalculated_hash); }

    const_iterator find(const Key& key) const { return m_ht.find(key); }

    /**
     * @copydoc find(const Key& key, std::size_t precalculated_hash)
     */
    const_iterator find(const Key& key, std::size_t precalculated_hash) const { return m_ht.find(key, precalculated_hash); }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    iterator find(const K& key) { return m_ht.find(key); }

    /**
     * @copydoc find(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    iterator find(const K& key, std::size_t precalculated_hash) { return m_ht.find(key, precalculated_hash); }

    /**
     * @copydoc find(const K& key)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    const_iterator find(const K& key) const { return m_ht.find(key); }

    /**
     * @copydoc find(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    const_iterator find(const K& key, std::size_t precalculated_hash) const { return m_ht.find(key, precalculated_hash); }

    bool contains(const Key& key) const { return m_ht.contains(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    bool contains(const Key& key, std::size_t precalculated_hash) const {
        return m_ht.contains(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    bool contains(const K& key) const { return m_ht.contains(key); }

    /**
     * @copydoc contains(const K& key) const
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    bool contains(const K& key, std::size_t precalculated_hash) const {
        return m_ht.contains(key, precalculated_hash);
    }

    std::pair<iterator, iterator> equal_range(const Key& key) { return m_ht.equal_range(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    std::pair<iterator, iterator> equal_range(const Key& key, std::size_t precalculated_hash) {
        return m_ht.equal_range(key, precalculated_hash);
    }

    std::pair<const_iterator, const_iterator> equal_range(const Key& key) const { return m_ht.equal_range(key); }

    /**
     * @copydoc equal_range(const Key& key, std::size_t precalculated_hash)
     */
    std::pair<const_iterator, const_iterator> equal_range(const Key& key, std::size_t precalculated_hash) const {
        return m_ht.equal_range(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<iterator, iterator> equal_range(const K& key) { return m_ht.equal_range(key); }

    /**
     * @copydoc equal_range(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<iterator, iterator> equal_range(const K& key, std::size_t precalculated_hash) {
        return m_ht.equal_range(key, precalculated_hash);
    }

    /**
     * @copydoc equal_range(const K& key)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<const_iterator, const_iterator> equal_range(const K& key) const { return m_ht.equal_range(key); }

    /**
     * @copydoc equal_range(const K& key, std::size_t precalculated_hash)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<const_iterator, const_iterator> equal_range(const K& key, std::size_t precalculated_hash) const {
        return m_ht.equal_range(key, precalculated_hash);
    }

    /*
     * Bucket interface
     */
    size_type bucket_count() const { return m_ht.bucket_count(); }
    size_type max_bucket_count() const { return m_ht.max_bucket_count(); }

    /*
     *  Hash policy
     */
    float load_factor() const { return m_ht.load_factor(); }

    float min_load_factor() const { return m_ht.min_load_factor(); }
    float max_load_factor() const { return m_ht.max_load_factor(); }

    /**
     * Set the `min_load_factor` to `ml`. When the `load_factor` of the set goes
     * below `min_load_factor` after some erase operations, the set will be
     * shrunk when an insertion occurs. The erase method itself never shrinks
     * the set.
     *
     * The default value of `min_load_factor` is 0.0f, the set never shrinks by default.
     */
    void min_load_factor(float ml) { m_ht.min_load_factor(ml); }
    void max_load_factor(float ml) { m_ht.max_load_factor(ml); }

    void rehash(size_type count) { m_ht.rehash(count); }
    void reserve(size_type count) { m_ht.reserve(count); }

    /*
     * Observers
     */
    hasher hash_function() const { return m_ht.hash_function(); }
    key_equal key_eq() const { return m_ht.key_eq(); }

    /*
     * Other
     */

    /**
     * Convert a const_iterator to an iterator.
     */
    iterator mutable_iterator(const_iterator pos) {
        return m_ht.mutable_iterator(pos);
    }

    friend bool operator==(const robin_set& lhs, const robin_set& rhs) {
        if(lhs.size() != rhs.size()) {
            return false;
        }

        for(const auto& element_lhs: lhs) {
            const auto it_element_rhs = rhs.find(element_lhs);
            if(it_element_rhs == rhs.cend()) {
                return false;
            }
        }

        return true;
    }

    friend bool operator!=(const robin_set& lhs, const robin_set& rhs) {
        return !operator==(lhs, rhs);
    }

    friend void swap(robin_set& lhs, robin_set& rhs) {
        lhs.swap(rhs);
    }

private:
    ht m_ht;
};

/**
 * Same as `tsl::robin_set<Key, Hash, KeyEqual, Allocator, StoreHash, tsl::rh::prime_growth_policy>`.
 */
template<class Key,
         class Hash = std::hash<Key>,
         class KeyEqual = std::equal_to<Key>,
         class Allocator = std::allocator<Key>,
         bool StoreHash = false>
using robin_pg_set = robin_set<Key, Hash, KeyEqual, Allocator, StoreHash, tsl::rh::prime_growth_policy>;

} // end namespace tsl

#endif

class
quatType
{
public:
static Eigen::Matrix<double, 4, 1> rotToQuat(const Eigen::Matrix<double, 3, 3> &rot) {}
static Eigen::Matrix<double, 3, 3> quatToRot(const Eigen::Matrix<double, 4, 1> &q) {}
static Eigen::Matrix<double, 3, 3> skewSymmetric(const Eigen::Matrix<double, 3, 1> &w) {}
static Eigen::Matrix<double, 4, 1> quatMultiply(const Eigen::Matrix<double, 4, 1> &q, const Eigen::Matrix<double, 4, 1> &p) {}
static Eigen::Matrix<double, 3, 1> vee(const Eigen::Matrix<double, 3, 3> &w_x) {}
static Eigen::Matrix<double, 3, 3> expSo3(const Eigen::Matrix<double, 3, 1> &w) {}
static Eigen::Matrix<double, 3, 1> logSo3(const Eigen::Matrix<double, 3, 3> &R) {}
static Eigen::Matrix4d expSe3(Eigen::Matrix<double, 6, 1> vec) {}
static Eigen::Matrix<double, 6, 1> logSe3(Eigen::Matrix4d mat) {}
static Eigen::Matrix4d hatSe3(const Eigen::Matrix<double, 6, 1> &vec) {}
static Eigen::Matrix4d invSe3(const Eigen::Matrix4d &T) {}
static Eigen::Matrix<double, 4, 1> inv(Eigen::Matrix<double, 4, 1> q) {}
static Eigen::Matrix<double, 4, 4> omega(Eigen::Matrix<double, 3, 1> w) {}
static Eigen::Matrix<double, 4, 1> quatNorm(Eigen::Matrix<double, 4, 1> q_t) {}
static Eigen::Matrix<double, 3, 3> JleftSo3(const Eigen::Matrix<double, 3, 1> &w) {}
static Eigen::Matrix<double, 3, 3> JrighySo3(const Eigen::Matrix<double, 3, 1> &w) {}
static Eigen::Matrix<double, 3, 1> rotToRpy(const Eigen::Matrix<double, 3, 3> &rot) {}
static Eigen::Matrix<double, 3, 3> rotX(double t) {}
static Eigen::Matrix<double, 3, 3> rotY(double t) {}
static Eigen::Matrix<double, 3, 3> rotZ(double t) {}
};

This code segment functions to: Provide mathematical operations for converting between different three-dimensional rotation representations, including rotation matrices, quaternions, and Euler angles, while implementing Lie group transformations and associated utility functions for spatial orientation calculations.

```cpp
[TASK_BEGIN]
class quatType
{
public:
    static Eigen::Matrix<double, 4, 1> rotToQuat(const Eigen::Matrix<double, 3, 3> &rot) {
        Eigen::Quaterniond q(rot);
        q.normalize();
        return q.coeffs();
    }

    static Eigen::Matrix<double, 3, 3> quatToRot(const Eigen::Matrix<double, 4, 1> &q) {
        Eigen::Quaterniond quat(q[3], q[0], q[1], q[2]);
        return quat.normalized().toRotationMatrix();
    }

    static Eigen::Matrix<double, 3, 3> skewSymmetric(const Eigen::Matrix<double, 3, 1> &w) {
        Eigen::Matrix3d w_x;
        w_x << 0, -w[2], w[1],
               w[2], 0, -w[0],
               -w[1], w[0], 0;
        return w_x;
    }

    static Eigen::Matrix<double, 4, 1> quatMultiply(const Eigen::Matrix<double, 4, 1> &q, const Eigen::Matrix<double, 4, 1> &p) {
        Eigen::Quaterniond q1(p[3], p[0], p[1], p[2]);
        Eigen::Quaterniond q2(q[3], q[0], q[1], q[2]);
        Eigen::Quaterniond result = q2 * q1;
        return Eigen::Vector4d(result.x(), result.y(), result.z(), result.w());
    }

    static Eigen::Matrix<double, 3, 1> vee(const Eigen::Matrix<double, 3, 3> &w_x) {
        return Eigen::Vector3d(w_x(2,1), w_x(0,2), w_x(1,0));
    }

    static Eigen::Matrix<double, 3, 3> expSo3(const Eigen::Matrix<double, 3, 1> &w) {
        double theta = w.norm();
        if(theta < 1e-7) return Eigen::Matrix3d::Identity();

        Eigen::Matrix3d w_x = skewSymmetric(w);
        return Eigen::Matrix3d::Identity() + (sin(theta)/theta)*w_x + ((1-cos(theta))/(theta*theta))*w_x*w_x;
    }

    static Eigen::Matrix<double, 3, 1> logSo3(const Eigen::Matrix<double, 3, 3> &R) {
        Eigen::AngleAxisd aa(R);
        return aa.angle() * aa.axis();
    }

    static Eigen::Matrix4d expSe3(Eigen::Matrix<double, 6, 1> vec) {
        Eigen::Vector3d w = vec.head(3);
        Eigen::Vector3d v = vec.tail(3);
        double theta = w.norm();

        Eigen::Matrix3d R = expSo3(w);
        Eigen::Matrix3d J = JleftSo3(w);

        Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
        T.block<3,3>(0,0) = R;
        T.block<3,1>(0,3) = J*v;
        return T;
    }

    static Eigen::Matrix<double, 6, 1> logSe3(Eigen::Matrix4d mat) {
        Eigen::Matrix3d R = mat.block<3,3>(0,0);
        Eigen::Vector3d p = mat.block<3,1>(0,3);
        Eigen::Vector3d w = logSo3(R);

        double theta = w.norm();
        Eigen::Matrix3d J = JleftSo3(w);
        Eigen::Vector3d v = J.inverse() * p;

        Eigen::Vector6d vec;
        vec << w, v;
        return vec;
    }

    static Eigen::Matrix4d hatSe3(const Eigen::Matrix<double, 6, 1> &vec) {
        Eigen::Matrix4d mat = Eigen::Matrix4d::Zero();
        mat.block<3,3>(0,0) = skewSymmetric(vec.head(3));
        mat.block<3,1>(0,3) = vec.tail(3);
        return mat;
    }

    static Eigen::Matrix4d invSe3(const Eigen::Matrix4d &T) {
        Eigen::Matrix4d invT = Eigen::Matrix4d::Identity();
        invT.block<3,3>(0,0) = T.block<3,3>(0,0).transpose();
        invT.block<3,1>(0,3) = -invT.block<3,3>(0,0) * T.block<3,1>(0,3);
        return invT;
    }

    static Eigen::Matrix<double, 4, 1> inv(Eigen::Matrix<double, 4, 1> q) {
        q.head(3) *= -1;
        return q;
    }

    static Eigen::Matrix<double, 4, 4> omega(Eigen::Matrix<double, 3, 1> w) {
        Eigen::Matrix4d Om;
        Om << 0, -w[0], -w[1], -w[2],
             w[0], 0, w[2], -w[1],
             w[1], -w[2], 0, w[0],
             w[2], w[1], -w[0], 0;
        return 0.5 * Om;
    }

    static Eigen::Matrix<double, 4, 1> quatNorm(Eigen::Matrix<double, 4, 1> q_t) {
        return q_t.normalized();
    }

    static Eigen::Matrix<double, 3, 3> JleftSo3(const Eigen::Matrix<double, 3, 1> &w) {
        double theta = w.norm();
        if(theta < 1e-7) return Eigen::Matrix3d::Identity();

        Eigen::Matrix3d w_x = skewSymmetric(w);
        return Eigen::Matrix3d::Identity() + ((1 - cos(theta))/(theta*theta))*w_x + ((theta - sin(theta))/(theta*theta*theta))*w_x*w_x;
    }

    static Eigen::Matrix<double, 3, 3> JrighySo3(const Eigen::Matrix<double, 3, 1> &w) {
        return JleftSo3(-w);
    }

    static Eigen::Matrix<double, 3, 1> rotToRpy(const Eigen::Matrix<double, 3, 3> &rot) {
        double roll = atan2(rot(2,1), rot(2,2));
        double pitch = atan2(-rot(2,0), sqrt(rot(2,1)*rot(2,1) + rot(2,2)*rot(2,2)));
        double yaw = atan2(rot(1,0), rot(0,0));
        return Eigen::Vector3d(roll, pitch, yaw);
    }

    static Eigen::Matrix<double, 3, 3> rotX(double t) {
        Eigen::Matrix3d R;
        R << 1, 0, 0,
             0, cos(t), -sin(t),
             0, sin(t), cos(t);
        return R;
    }

    static Eigen::Matrix<double, 3, 3> rotY(double t) {
        Eigen::Matrix3d R;
        R << cos(t), 0, sin(t),
             0, 1, 0,
             -sin(t), 0, cos(t);
        return R;
    }

    static Eigen::Matrix<double, 3, 3> rotZ(double t) {
        Eigen::Matrix3d R;
        R << cos(t), -sin(t), 0,
             sin(t), cos(t), 0,
             0, 0, 1;
        return R;
    }
};
[TASK_END]
```