//此源码被清华学神尹成大魔王专业翻译分析并修改
//尹成QQ77025077
//尹成微信18510341407
//尹成所在QQ群721929980
//尹成邮箱 yinc13@mails.tsinghua.edu.cn
//尹成毕业于清华大学,微软区块链领域全球最有价值专家
//https://mvp.microsoft.com/zh-cn/PublicProfile/4033620
//版权所有（c）2012-2018比特币核心开发者
//根据MIT软件许可证分发，请参见随附的
//文件复制或http://www.opensource.org/licenses/mit-license.php。
#include <boost/test/unit_test.hpp>
#include <cuckoocache.h>
#include <script/sigcache.h>
#include <test/test_bitcoin.h>
#include <random.h>
#include <thread>

/*布谷缓存测试套件
 *
 *1）所有测试都应具有确定性结果（使用不安全的RAND）
 *具有确定性种子）
 *2）为便于测试，模板化了一些测试方法。
 *与新版本/比较
 *3）结果应视为回归测试，即行为
 *与预期相比有显著变化。这可以是正常的，取决于
 *更改的性质，但需要更新测试以反映新的
 *预期行为。例如，提高命中率可能会导致一些测试
 *使用Boost_检查关闭失败。
 *
 **/

BOOST_AUTO_TEST_SUITE(cuckoocache_tests);

/*测试是否没有从缓存中读取未插入的值。
 *
 *前200000个不安全的\getrandhash调用中没有重复
 **/

BOOST_AUTO_TEST_CASE(test_cuckoocache_no_fakes)
{
    SeedInsecureRand(true);
    CuckooCache::cache<uint256, SignatureCacheHasher> cc{};
    size_t megabytes = 4;
    cc.setup_bytes(megabytes << 20);
    for (int x = 0; x < 100000; ++x) {
        cc.insert(InsecureRand256());
    }
    for (int x = 0; x < 100000; ++x) {
        BOOST_CHECK(!cc.contains(InsecureRand256(), false));
    }
};

/*此帮助程序返回当条目数为兆字节*时的命中率。
 *插入到兆字节大小的缓存中
 **/

template <typename Cache>
static double test_cache(size_t megabytes, double load)
{
    SeedInsecureRand(true);
    std::vector<uint256> hashes;
    Cache set{};
    size_t bytes = megabytes * (1 << 20);
    set.setup_bytes(bytes);
    uint32_t n_insert = static_cast<uint32_t>(load * (bytes / sizeof(uint256)));
    hashes.resize(n_insert);
    for (uint32_t i = 0; i < n_insert; ++i) {
        uint32_t* ptr = (uint32_t*)hashes[i].begin();
        for (uint8_t j = 0; j < 8; ++j)
            *(ptr++) = InsecureRand32();
    }
    /*我们复制散列，因为
     *布谷缓存可能会覆盖插入的元素，因此测试是
     *“未来证明”。
     **/

    std::vector<uint256> hashes_insert_copy = hashes;
    /*做插入*/
    for (const uint256& h : hashes_insert_copy)
        set.insert(h);
    /*统计命中率*/
    uint32_t count = 0;
    for (const uint256& h : hashes)
        count += set.contains(h, false);
    double hit_rate = ((double)count) / ((double)n_insert);
    return hit_rate;
}

/*给定负载的标准化命中率。
 *
 *语义有点混乱，请看下面
 ＊解释。
 *
 *实例：
 *
 *1）在加载0.5时，我们期望有一个完美的命中率，因此我们乘以
 * 1
 *2）在加载2.0时，我们希望看到一半的条目，所以命中率非常理想。
 *为0.5。因此，如果我们看到0.4的命中率，0.4*2.0=0.8是
 *标准化命中率。
 *
 *这基本上是正确的语义，但根据
 *如何测量负载1.0左右，正如负载1.0之后您的标准化命中率一样。
 *变得非常完美，忽略新鲜感。
 **/

static double normalize_hit_rate(double hits, double load)
{
    return hits * std::max(load, 1.0);
}

/*检查从0.1到1.6的负载的命中率*/
BOOST_AUTO_TEST_CASE(cuckoocache_hit_rate_ok)
{
    /*为该测试选择的任意命中率阈值
     *作为性能的下限。
     **/

    double HitRateThresh = 0.98;
    size_t megabytes = 4;
    for (double load = 0.1; load < 2; load *= 2) {
        double hits = test_cache<CuckooCache::cache<uint256, SignatureCacheHasher>>(megabytes, load);
        BOOST_CHECK(normalize_hit_rate(hits, load) > HitRateThresh);
    }
}

/*此帮助程序检查删除的元素是否优先插入和
 *刷新键的命中率是合理的。*/

template <typename Cache>
static void test_cache_erase(size_t megabytes)
{
    double load = 1;
    SeedInsecureRand(true);
    std::vector<uint256> hashes;
    Cache set{};
    size_t bytes = megabytes * (1 << 20);
    set.setup_bytes(bytes);
    uint32_t n_insert = static_cast<uint32_t>(load * (bytes / sizeof(uint256)));
    hashes.resize(n_insert);
    for (uint32_t i = 0; i < n_insert; ++i) {
        uint32_t* ptr = (uint32_t*)hashes[i].begin();
        for (uint8_t j = 0; j < 8; ++j)
            *(ptr++) = InsecureRand32();
    }
    /*我们复制散列，因为
     *布谷缓存可能会覆盖插入的元素，因此测试是
     *“未来证明”。
     **/

    std::vector<uint256> hashes_insert_copy = hashes;

    /*插入上半部分*/
    for (uint32_t i = 0; i < (n_insert / 2); ++i)
        set.insert(hashes_insert_copy[i]);
    /*删除第一个季度*/
    for (uint32_t i = 0; i < (n_insert / 4); ++i)
        BOOST_CHECK(set.contains(hashes[i], true));
    /*插入下半部分*/
    for (uint32_t i = (n_insert / 2); i < n_insert; ++i)
        set.insert(hashes_insert_copy[i]);

    /*我们标记为已删除但仍存在的元素*/
    size_t count_erased_but_contained = 0;
    /*我们没有删除但较旧的元素*/
    size_t count_stale = 0;
    /*最近插入的元素*/
    size_t count_fresh = 0;

    for (uint32_t i = 0; i < (n_insert / 4); ++i)
        count_erased_but_contained += set.contains(hashes[i], false);
    for (uint32_t i = (n_insert / 4); i < (n_insert / 2); ++i)
        count_stale += set.contains(hashes[i], false);
    for (uint32_t i = (n_insert / 2); i < n_insert; ++i)
        count_fresh += set.contains(hashes[i], false);

    double hit_rate_erased_but_contained = double(count_erased_but_contained) / (double(n_insert) / 4.0);
    double hit_rate_stale = double(count_stale) / (double(n_insert) / 4.0);
    double hit_rate_fresh = double(count_fresh) / (double(n_insert) / 2.0);

//检查我们的命中率是完美的
    BOOST_CHECK_EQUAL(hit_rate_fresh, 1.0);
//检查陈旧元素的命中率是否比
//删除的元素。
    BOOST_CHECK(hit_rate_stale > 2 * hit_rate_erased_but_contained);
}

BOOST_AUTO_TEST_CASE(cuckoocache_erase_ok)
{
    size_t megabytes = 4;
    test_cache_erase<CuckooCache::cache<uint256, SignatureCacheHasher>>(megabytes);
}

template <typename Cache>
static void test_cache_erase_parallel(size_t megabytes)
{
    double load = 1;
    SeedInsecureRand(true);
    std::vector<uint256> hashes;
    Cache set{};
    size_t bytes = megabytes * (1 << 20);
    set.setup_bytes(bytes);
    uint32_t n_insert = static_cast<uint32_t>(load * (bytes / sizeof(uint256)));
    hashes.resize(n_insert);
    for (uint32_t i = 0; i < n_insert; ++i) {
        uint32_t* ptr = (uint32_t*)hashes[i].begin();
        for (uint8_t j = 0; j < 8; ++j)
            *(ptr++) = InsecureRand32();
    }
    /*我们复制散列，因为
     *布谷缓存可能会覆盖插入的元素，因此测试是
     *“未来证明”。
     **/

    std::vector<uint256> hashes_insert_copy = hashes;
    boost::shared_mutex mtx;

    {
        /*抓住锁确保我们释放插入件*/
        boost::unique_lock<boost::shared_mutex> l(mtx);
        /*插入上半部分*/
        for (uint32_t i = 0; i < (n_insert / 2); ++i)
            set.insert(hashes_insert_copy[i]);
    }

    /*旋转3个线程运行包含擦除。
     **/

    std::vector<std::thread> threads;
    /*删除第一个季度*/
    for (uint32_t x = 0; x < 3; ++x)
        /*每个线程用x copy by value放置
        **/

        threads.emplace_back([&, x] {
            boost::shared_lock<boost::shared_mutex> l(mtx);
            size_t ntodo = (n_insert/4)/3;
            size_t start = ntodo*x;
            size_t end = ntodo*(x+1);
            for (uint32_t i = start; i < end; ++i) {
                bool contains = set.contains(hashes[i], true);
                assert(contains);
            }
        });

    /*等待所有线程完成
     **/

    for (std::thread& t : threads)
        t.join();
    /*抓紧锁，确保我们观察到擦除*/
    boost::unique_lock<boost::shared_mutex> l(mtx);
    /*插入下半部分*/
    for (uint32_t i = (n_insert / 2); i < n_insert; ++i)
        set.insert(hashes_insert_copy[i]);

    /*我们标记为已删除但仍存在的元素*/
    size_t count_erased_but_contained = 0;
    /*我们没有删除但较旧的元素*/
    size_t count_stale = 0;
    /*最近插入的元素*/
    size_t count_fresh = 0;

    for (uint32_t i = 0; i < (n_insert / 4); ++i)
        count_erased_but_contained += set.contains(hashes[i], false);
    for (uint32_t i = (n_insert / 4); i < (n_insert / 2); ++i)
        count_stale += set.contains(hashes[i], false);
    for (uint32_t i = (n_insert / 2); i < n_insert; ++i)
        count_fresh += set.contains(hashes[i], false);

    double hit_rate_erased_but_contained = double(count_erased_but_contained) / (double(n_insert) / 4.0);
    double hit_rate_stale = double(count_stale) / (double(n_insert) / 4.0);
    double hit_rate_fresh = double(count_fresh) / (double(n_insert) / 2.0);

//检查我们的命中率是完美的
    BOOST_CHECK_EQUAL(hit_rate_fresh, 1.0);
//检查陈旧元素的命中率是否比
//删除的元素。
    BOOST_CHECK(hit_rate_stale > 2 * hit_rate_erased_but_contained);
}
BOOST_AUTO_TEST_CASE(cuckoocache_erase_parallel_ok)
{
    size_t megabytes = 4;
    test_cache_erase_parallel<CuckooCache::cache<uint256, SignatureCacheHasher>>(megabytes);
}

template <typename Cache>
static void test_cache_generations()
{
//此测试检查网络活动模拟的新点击
//利率永远不会低于99%，而且比
//99.9%小于1%。
    double min_hit_rate = 0.99;
    double tight_hit_rate = 0.999;
    double max_rate_less_than_tight_hit_rate = 0.01;
//因此，满足此规范的缓存显示为命中
//至少紧密命中率*（1-最大命中率比紧密命中率低）
//最小命中率*最大命中率小于紧命中率=0.999*99%+0.99*1%==99.89%
//低方差命中率。

//我们使用确定性值，但是这个测试也传递了许多
//具有非确定性值的迭代，因此它不“过适合”于
//FastRandomContext中的特定熵（true）和
//隐藏物。
    SeedInsecureRand(true);

//block_activity为一块网络活动建模。插入元素为
//添加到缓存。第一个N/4和最后一个N/4将被存储以供以后删除。
//中间的n/2不存储。这模拟了一个使用半
//最近（自上一个块以来）添加的事务的签名
//立即使用另一半。
    struct block_activity {
        std::vector<uint256> reads;
        block_activity(uint32_t n_insert, Cache& c) : reads()
        {
            std::vector<uint256> inserts;
            inserts.resize(n_insert);
            reads.reserve(n_insert / 2);
            for (uint32_t i = 0; i < n_insert; ++i) {
                uint32_t* ptr = (uint32_t*)inserts[i].begin();
                for (uint8_t j = 0; j < 8; ++j)
                    *(ptr++) = InsecureRand32();
            }
            for (uint32_t i = 0; i < n_insert / 4; ++i)
                reads.push_back(inserts[i]);
            for (uint32_t i = n_insert - (n_insert / 4); i < n_insert; ++i)
                reads.push_back(inserts[i]);
            for (const auto& h : inserts)
                c.insert(h);
        }
    };

    const uint32_t BLOCK_SIZE = 1000;
//我们期望窗口大小60在每个时代都能合理地运行。
//存储缓存大小的45%（约472K）。
    const uint32_t WINDOW_SIZE = 60;
    const uint32_t POP_AMOUNT = (BLOCK_SIZE / WINDOW_SIZE) / 2;
    const double load = 10;
    const size_t megabytes = 4;
    const size_t bytes = megabytes * (1 << 20);
    const uint32_t n_insert = static_cast<uint32_t>(load * (bytes / sizeof(uint256)));

    std::vector<block_activity> hashes;
    Cache set{};
    set.setup_bytes(bytes);
    hashes.reserve(n_insert / BLOCK_SIZE);
    std::deque<block_activity> last_few;
    uint32_t out_of_tight_tolerance = 0;
    uint32_t total = n_insert / BLOCK_SIZE;
//我们用最后一个模型来模拟一个滑窗。在每个
//步骤，每个最后一个窗口的“大小块”活动检查缓存
//弹出他们插入的哈希数，并标记这些已删除。
    for (uint32_t i = 0; i < total; ++i) {
        if (last_few.size() == WINDOW_SIZE)
            last_few.pop_front();
        last_few.emplace_back(BLOCK_SIZE, set);
        uint32_t count = 0;
        for (auto& act : last_few)
            for (uint32_t k = 0; k < POP_AMOUNT; ++k) {
                count += set.contains(act.reads.back(), true);
                act.reads.pop_back();
            }
//我们使用最后的.size（）而不是窗口大小
//第一个窗口大小迭代的行为，其中deque不是
//满了。
        double hit = (double(count)) / (last_few.size() * POP_AMOUNT);
//检查命中率是否高于最小命中率
        BOOST_CHECK(hit > min_hit_rate);
//更严格的检查，计算我们低于严格命中率的次数
//（隐含地，大于最小命中率）
        out_of_tight_tolerance += hit < tight_hit_rate;
    }
//检查超差发生的时间是否小于
//最大命中率
    BOOST_CHECK(double(out_of_tight_tolerance) / double(total) < max_rate_less_than_tight_hit_rate);
}
BOOST_AUTO_TEST_CASE(cuckoocache_generations)
{
    test_cache_generations<CuckooCache::cache<uint256, SignatureCacheHasher>>();
}

BOOST_AUTO_TEST_SUITE_END();


// Author(s): Jan Friso Groote
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file stochastic_linearization_test.cpp
/// \brief Add your file description here.

#include <boost/test/included/unit_test_framework.hpp>

#include <iostream>
#include <string>

#include "mcrl2/data/detail/rewrite_strategies.h"
#include "mcrl2/lps/linearise.h"
#include "mcrl2/utilities/logger.h"

using namespace mcrl2;
using namespace mcrl2::lps;

typedef data::rewriter::strategy rewrite_strategy;
typedef std::vector<rewrite_strategy> rewrite_strategy_vector;

void run_linearisation_instance(const std::string& spec, const t_lin_options& options, bool expect_success)
{
  if (expect_success)
  {
    lps::stochastic_specification s=linearise(spec, options);
    BOOST_CHECK(s != lps::stochastic_specification());
  }
  else
  {
    BOOST_CHECK_THROW(linearise(spec, options), mcrl2::runtime_error);
  }
}

void run_linearisation_test_case(const std::string& spec, const bool expect_success = true)
{
  // Set various rewrite strategies
  rewrite_strategy_vector rewrite_strategies = data::detail::get_test_rewrite_strategies(false);

  for (rewrite_strategy_vector::const_iterator i = rewrite_strategies.begin(); i != rewrite_strategies.end(); ++i)
  {
    std::clog << std::endl << "Testing with rewrite strategy " << *i << std::endl;

    t_lin_options options;
    options.rewrite_strategy=*i;

    std::clog << "  Default options" << std::endl;
    run_linearisation_instance(spec, options, expect_success);

    std::clog << "  Linearisation method regular2" << std::endl;
    options.lin_method=lmRegular2;
    run_linearisation_instance(spec, options, expect_success);

    std::clog << "  Linearisation method stack" << std::endl;
    options.lin_method=lmStack;
    run_linearisation_instance(spec, options, expect_success);

    std::clog << "  Linearisation method stack; binary enabled" << std::endl;
    options.binary=true;
    run_linearisation_instance(spec, options, expect_success);

    std::clog << "  Linearisation method regular; binary enabled" << std::endl;
    options.lin_method=lmRegular;
    run_linearisation_instance(spec, options, expect_success);

    std::clog << "  Linearisation method regular; no intermediate clustering" << std::endl;
    options.binary=false; // reset binary
    options.no_intermediate_cluster=true;
    run_linearisation_instance(spec, options, expect_success);
  }
}

BOOST_AUTO_TEST_CASE(Check_that_a_probability_distribution_works_well_in_combination_with_a_nonterminating_initial_process)
{
  const std::string spec =
    "act a:Bool;\n"
    "init dist x:Bool[1/2].a(x);\n";

  run_linearisation_test_case(spec,true);
}

BOOST_AUTO_TEST_CASE(Check_distribution_of_dist_over_plus)
{
  const std::string spec =
    "act a,b:Bool;\n"
    "init dist x:Bool[1/2].a(x).delta+dist y:Bool[1/2].a(y).delta;\n";

  run_linearisation_test_case(spec,true);
}

BOOST_AUTO_TEST_CASE(Check_distribution_of_dist_over_sum)
{
  const std::string spec =
    "act a:Bool#Bool;\n"
    "init dist x:Bool[1/2].sum y:Bool.a(x,y).delta;\n";

  run_linearisation_test_case(spec,true);
}

// The test below represents a problem as the variables that were moved
// to the front were not properly renamed. Problem reported by Olav Bunte.
BOOST_AUTO_TEST_CASE(renaming_of_initial_stochastic_variables)
{
  const std::string spec =
    "act\n"
    "        flip: Bool;\n"
    "        dice: Nat;\n"
    "\n"
    "proc\n"
    "        COIN(s: Nat, d: Nat) =\n"
    "                (s == 0) -> dist b1:Bool[1/2].(b1 -> flip(b1).COIN(1,0) <> flip(b1).COIN(2,0))\n"
    "                <> (s == 1) -> dist b1:Bool[1/2].(b1 -> flip(b1).COIN(3,0) <> flip(b1).COIN(4,0))\n"
    "                <> (s == 2) -> dist b1:Bool[1/2].(b1 -> flip(b1).COIN(5,0) <> flip(b1).COIN(6,0))\n"
    "                <> (s == 3) -> dist b1:Bool[1/2].(b1 -> flip(b1).COIN(1,0) <> flip(b1).COIN(7,1))\n"
    "                <> (s == 4) -> dist b1:Bool[1/2].(b1 -> flip(b1).COIN(7,2) <> flip(b1).COIN(7,3))\n"
    "                <> (s == 5) -> dist b1:Bool[1/2].(b1 -> flip(b1).COIN(7,4) <> flip(b1).COIN(7,5))\n"
    "                <> (s == 6) -> dist b1:Bool[1/2].(b1 -> flip(b1).COIN(2,0) <> flip(b1).COIN(7,6))\n"
    "                <> (s == 7) -> dice(d).COIN(s,d);\n"
    "\n"
    "init COIN(0, 0);\n";

  run_linearisation_test_case(spec,true);
}

boost::unit_test::test_suite* init_unit_test_suite(int, char*[])
{
  return nullptr;
}


/*
 * Version:  2017.07.31
 * Authors:  Members of the Team NAIST-Panasonic at the Amazon Robotics Challenge 2017:
 *           Gustavo A. Garcia R. <garcia-g at is.naist.jp> (Captain),
 *           Lotfi El Hafi, Felix von Drigalski, Wataru Yamazaki, Viktor Hoerig, Arnaud Delmotte,
 *           Akishige Yuguchi, Marcus Gall, Chika Shiogama, Kenta Toyoshima, Pedro Uriguen,
 *           Rodrigo Elizalde, Masaki Yamamoto, Yasunao Okazaki, Kazuo Inoue, Katsuhiko Asai,
 *           Ryutaro Futakuchi, Seigo Okada, Yusuke Kato, and Pin-Chu Yang
 *********************
 * Copyright 2017 Team NAIST-Panasonic
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *     http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *********************
 */

#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <ros/ros.h>
#include "std_msgs/Int16.h"
#include "geometry_msgs/Pose.h"
#include "std_msgs/Float64.h"

#include <Eigen/Core>

#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <librealsense/rs.h>
#include <librealsense/rs.hpp>

#include <chrono>
#include <iostream>

#include <opencv2/opencv.hpp>
#include <opencv2/xfeatures2d.hpp>

#include "RGBDFrame.h"
#include "UtilCvPclRs.h"

#define NOISY 3.5

cv::Mat color_image;
cv::Mat depth_image;

bool color_image_ishere;
bool color_info_ishere;
bool depth_image_ishere;
bool depth_info_ishere;

float color_info_fx;
float color_info_fy;
float color_info_ppx;
float color_info_ppy;
float color_info_tx;
float color_info_ty;
float color_info_tz;

float depth_info_fx;
float depth_info_fy;
float depth_info_ppx;
float depth_info_ppy;
float depth_info_tx;
float depth_info_ty;
float depth_info_tz;

cv::Mat depth_to_color_extrinsics;
cv::Mat color_K;
cv::Mat depth_K;
bool USE_HARDCODED_ROTATION = true;

std::vector<cv::Mat> listResultImg0;
std::vector<std::vector<cv::Point3f> > listFeaturesPos3d;
std::vector<std::vector<cv::Point3f> > listFeaturesNormal3d;
std::vector<cv::Mat> listFeaturesDescFull;

std::vector<std::vector<std::vector<cv::KeyPoint>>>keypoints1;
std::vector<std::vector<cv::Mat>> descriptors1;
cv::Ptr<cv::DescriptorMatcher> matcher;
cv::Ptr<cv::AKAZE> akaze;
cv::Ptr<cv::ORB> orb;

ros::Publisher pub_item_pose, pub_confidence;

int w, h, depth;
float scale;

int selectedObj = 1;

cv::Point3f transformByMat44(cv::Mat M, cv::Point3f p)
{
  cv::Point3f p2;
  float w = M.at<float>(3, 0) * p.x + M.at<float>(3, 1) * p.y + M.at<float>(3, 2) * p.z + M.at<float>(3, 3);
  p2.x = (M.at<float>(0, 0) * p.x + M.at<float>(0, 1) * p.y + M.at<float>(0, 2) * p.z + M.at<float>(0, 3)) / w;
  p2.y = (M.at<float>(1, 0) * p.x + M.at<float>(1, 1) * p.y + M.at<float>(1, 2) * p.z + M.at<float>(1, 3)) / w;
  p2.z = (M.at<float>(2, 0) * p.x + M.at<float>(2, 1) * p.y + M.at<float>(2, 2) * p.z + M.at<float>(2, 3)) / w;
  return p2;
}

void loadFeatures(const char *filename, std::vector<cv::Point3f>& featuresPos3d,
                  std::vector<cv::Point3f>& featuresNormal3d, cv::Mat& featuresDesc)
{
  cv::FileStorage fs(filename, cv::FileStorage::READ);
  printf("read keypoints\n");
  featuresPos3d = std::vector<cv::Point3f>(fs["keypoints"].size());
  for (int i = 0; i < fs["keypoints"].size(); i++)
    fs["keypoints"][i] >> featuresPos3d[i];
  printf("read normals\n");
  featuresNormal3d = std::vector<cv::Point3f>(fs["normals"].size());
  for (int i = 0; i < fs["normals"].size(); i++)
    fs["normals"][i] >> featuresNormal3d[i];
  printf("read features\n");
  fs["features"] >> featuresDesc;
  fs.release();
}

cv::Mat findPoseWith2PointsAndNormal(cv::Point3f p1, cv::Point3f n1, cv::Point3f p2, cv::Point3f origP1,
                                     cv::Point3f origN1, cv::Point3f origP2)
{
  cv::Point3f x = n1;
  cv::Point3f y = p2 - p1;
  y -= y.dot(x) * x;
  y /= sqrt(y.dot(y));
  cv::Point3f z = x.cross(y);

  cv::Point3f origX = origN1;
  cv::Point3f origY = origP2 - origP1;
  origY -= origY.dot(origX) * origX;
  origY /= sqrt(origY.dot(origY));
  cv::Point3f origZ = origX.cross(origY);

  cv::Point3f center = p1 - origP1.dot(origX) * x - origP1.dot(origY) * y - origP1.dot(origZ) * z;
  cv::Mat poseMat = cv::Mat::eye(4, 4, CV_32F);
  poseMat.at<float>(0, 3) = center.x;
  poseMat.at<float>(1, 3) = center.y;
  poseMat.at<float>(2, 3) = center.z;

  cv::Mat R1(3, 3, CV_32F);
  R1.at<float>(0, 0) = x.x;
  R1.at<float>(0, 1) = y.x;
  R1.at<float>(0, 2) = z.x;
  R1.at<float>(1, 0) = x.y;
  R1.at<float>(1, 1) = y.y;
  R1.at<float>(1, 2) = z.y;
  R1.at<float>(2, 0) = x.z;
  R1.at<float>(2, 1) = y.z;
  R1.at<float>(2, 2) = z.z;

  cv::Mat R2(3, 3, CV_32F);
  R2.at<float>(0, 0) = origX.x;
  R2.at<float>(0, 1) = origY.x;
  R2.at<float>(0, 2) = origZ.x;
  R2.at<float>(1, 0) = origX.y;
  R2.at<float>(1, 1) = origY.y;
  R2.at<float>(1, 2) = origZ.y;
  R2.at<float>(2, 0) = origX.z;
  R2.at<float>(2, 1) = origY.z;
  R2.at<float>(2, 2) = origZ.z;

  cv::Mat R = R1 * R2.inv();
  R.copyTo(poseMat(cv::Rect(0, 0, 3, 3)));

  return poseMat;
}

cv::Mat findPoseWithDepthCue(const std::vector<cv::Point3f> &obj, const std::vector<cv::Point3f> &objNormal,
                             const std::vector<cv::Point2f> &scene, RgbdFrame frame, cv::Mat &mask)
{
  bool planar_test = false;
  std::vector<cv::Point3f> newObj;
  std::vector<cv::Point3f> newObjNormal;
  std::vector<cv::Point2f> newScene;
  std::vector<cv::Point3f> newScenePoints;
  std::vector<cv::Point3f> newSceneNormal;
  mask = cv::Mat(obj.size(), 1, CV_8UC1);
  mask.setTo(cv::Scalar(0));
  std::vector<int> selectedId;
  for (int i = 0; i < scene.size(); i++)
  {
    int id = int(scene[i].y) * frame.imgPoints->width + int(scene[i].x);
    pcl::PointXYZ p1 = frame.imgPoints->points[id];
    cv::Point3f p(p1.x, p1.y, p1.z);
    if (sqrt(p.dot(p)) > 0)
    {
      newObj.push_back(obj[i]);
      newObjNormal.push_back(objNormal[i]);
      newScene.push_back(scene[i]);
      newScenePoints.push_back(p);
      newSceneNormal.push_back(pcl2cv(frame.imgNormals->points[id]));
      selectedId.push_back(i);
    }
  }
  if (newObj.size() < 4)
    return cv::Mat();
  std::vector<std::vector<int>> goodPairs(newObj.size());
  std::vector<std::vector<bool>> goodPairsTable(newObj.size());
  for (int i = 0; i < newObj.size(); i++)
  {
    goodPairsTable[i] = std::vector<bool>(newObj.size());
    for (int j = 0; j < newObj.size(); j++)
      goodPairsTable[i][j] = false;
  }
  for (int i = 0; i < newObj.size(); i++)
  {
    for (int j = i + 1; j < newObj.size(); j++)
    {
      float angleDiff = fabs(acos(newSceneNormal[i].dot(newSceneNormal[j])));
      float dist = sqrt((newObj[i] - newObj[j]).dot(newObj[i] - newObj[j]));
      float dist2 = sqrt((newScenePoints[i] - newScenePoints[j]).dot(newScenePoints[i] - newScenePoints[j]));
      if (dist > 0.95 * dist2 && dist < 1.05 * dist2 && (!planar_test || angleDiff < 30 * CV_PI / 180))
      {
        goodPairs[i].push_back(j);
        goodPairs[j].push_back(i);
        goodPairsTable[i][j] = true;
        goodPairsTable[j][i] = true;
      }
    }
  }
  cv::Mat bestPose;
  std::vector<int> bestValidPoints;
  for (int iter = 0; iter < 10000; iter++)
  {

    int id1 = rand() % newObj.size();
    if (goodPairs[id1].size() < 3)
      continue;
    int id2 = goodPairs[id1][rand() % goodPairs[id1].size()];

    cv::Mat pose = findPoseWith2PointsAndNormal(newScenePoints[id1], newSceneNormal[id1], newScenePoints[id2],
                                                newObj[id1], newObjNormal[id1], newObj[id2]);

    std::vector<int> validPoints;
    for (int i = 0; i < newObj.size(); i++)
    {
      cv::Point3f delta = newScenePoints[i] - multMatVec(pose, newObj[i]);
      float dist = delta.dot(delta);
      if (dist < 0.005 * 0.005)
        validPoints.push_back(i);
    }
    if (validPoints.size() > bestValidPoints.size())
    {

      bestPose = pose;
      bestValidPoints = validPoints;
    }
  }
  for (int i = 0; i < bestValidPoints.size(); i++)
  {
    mask.at<unsigned char>(selectedId[bestValidPoints[i]], 0) = 1;
  }
  return bestPose;
}

RgbdFrame extractDataFromRos(cv::Mat colorK, cv::Mat colorImg, cv::Mat depthK, cv::Mat depthImg, cv::Mat Extrinsic)
{
  RgbdFrame frame;

  frame.depth_intrin = rs::intrinsics();
  frame.depth_intrin.width = depthImg.cols;
  frame.depth_intrin.height = depthImg.rows;
  frame.depth_intrin.ppx = depthK.at<double>(0, 2);
  frame.depth_intrin.ppy = depthK.at<double>(1, 2);
  frame.depth_intrin.fx = depthK.at<double>(0, 0);
  frame.depth_intrin.fy = depthK.at<double>(1, 1);
  ((rs_intrinsics *)&frame.depth_intrin)->model = (rs_distortion)rs::distortion::inverse_brown_conrady;
  for (int i = 0; i < 5; i++)
    frame.depth_intrin.coeffs[i] = 0; // Approximated to 0 for now.

  frame.color_intrin = rs::intrinsics();
  frame.color_intrin.width = colorImg.cols;
  frame.color_intrin.height = colorImg.rows;
  frame.color_intrin.ppx = colorK.at<double>(0, 2);
  frame.color_intrin.ppy = colorK.at<double>(1, 2);
  frame.color_intrin.fx = colorK.at<double>(0, 0);
  frame.color_intrin.fy = colorK.at<double>(1, 1);
  ((rs_intrinsics *)&frame.color_intrin)->model = (rs_distortion)rs::distortion::modified_brown_conrady;
  for (int i = 0; i < 5; i++)
    frame.color_intrin.coeffs[i] = 0; // Approximated to 0 for now.

  for (int i = 0; i < 3; i++)
  {
    frame.depth_to_color.translation[i] = Extrinsic.at<double>(i, 3);
    for (int j = 0; j < 3; j++)
      frame.depth_to_color.rotation[j * 3 + i] = Extrinsic.at<double>(i, j);
  }
  frame.scale = 0.001;

  frame.depth = depthImg.clone();
  frame.img = colorImg.clone();

  return frame;
}

bool rayTriangleIntersect(const cv::Point3f &orig, const cv::Point3f &dir, const cv::Point3f &v0, const cv::Point3f &v1,
                          const cv::Point3f &v2, float &t)
{
  // Compute plane's normal.
  cv::Point3f v0v1 = v1 - v0;
  cv::Point3f v0v2 = v2 - v0;
  // No need to normalize.
  cv::Point3f N = v0v1.cross(v0v2);
  float area2 = sqrt(N.dot(N));

  // Step 1: Finding P.

  // Check if the ray and the plane are parallel.
  float NdotRayDirection = N.dot(dir);
  if (fabs(NdotRayDirection) < 0.00001) // Almost 0.
    return false; // They are parallel so they don't intersect.

  // Compute d parameter using equation 2.
  float d = N.dot(v0);

  // Compute t using equation 3.
  t = (N.dot(orig) + d) / NdotRayDirection;
  // Check if the triangle is in behind the ray.
  if (t < 0)
    return false; // The triangle is behind.

  // Compute the intersection point using equation 1.
  cv::Point3f P = orig + t * dir;

  // Step 2: Inside-outside test.

  cv::Point3f C; // Vector perpendicular to the triangle's plane.

  // Edge 0:
  cv::Point3f edge0 = v1 - v0;
  cv::Point3f vp0 = P - v0;
  C = edge0.cross(vp0);
  if (N.dot(C) < 0)
    return false; // P is on the right side.

  // Edge 1:
  cv::Point3f edge1 = v2 - v1;
  cv::Point3f vp1 = P - v1;
  C = edge1.cross(vp1);
  if (N.dot(C) < 0)
    return false; // P is on the right side.

  // Edge 2:
  cv::Point3f edge2 = v0 - v2;
  cv::Point3f vp2 = P - v2;
  C = edge2.cross(vp2);
  if (N.dot(C) < 0)
    return false; // P is on the right side.

  return true; // This ray hits the triangle.
}

// Get the raw data and build the current frames cloud.
RgbdFrame generatePointCloudOrganized2(cv::Mat colorK, cv::Mat colorImg, cv::Mat depthK, cv::Mat depthImg,
                                       cv::Mat Extrinsic)
{

  RgbdFrame frame = extractDataFromRos(colorK, colorImg, depthK, depthImg, Extrinsic);
  frame.imgDepth = cv::Mat(frame.img.rows, frame.img.cols, CV_32F);
  frame.imgDepth.setTo(cv::Scalar(0.0));
  frame.imgPoints->clear();
  frame.imgPoints->resize(frame.img.rows * frame.img.cols);
  frame.imgPoints->is_dense = false;
  frame.imgPoints->width = frame.img.cols;
  frame.imgPoints->height = frame.img.rows;
  for (int i = 0; i < frame.imgPoints->height; i++)
    for (int j = 0; j < frame.imgPoints->width; j++)
    {
      int id = i * frame.imgPoints->width + j;
      frame.imgPoints->points[id].x = 0.0;
      frame.imgPoints->points[id].y = 0.0;
      frame.imgPoints->points[id].z = 0.0;
    }

  // Depth dimension helpers.
  int dw = 0;
  int dh = 0;
  int dwh = 0;

  dw = frame.imgDepth.cols;
  dh = frame.imgDepth.rows;

  dwh = dw * dh;

  // Set up the cloud to be used.
  frame.rs_cloud_ptr->clear();
  frame.rs_cloud_ptr->resize(dwh);
  frame.rs_cloud_ptr->is_dense = false;
  frame.rs_cloud_ptr->width = dw;
  frame.rs_cloud_ptr->height = dh;

  static const float nan = std::numeric_limits<float>::quiet_NaN();

  // Iterate the data space.

  // First, iterate across columns:
  for (int dy = 0; dy < dh; dy++)
  {
    // Second, iterate across rows:
    for (int dx = 0; dx < dw; dx++)
    {
      uint i = dy * dw + dx;
      uint16_t depth_value = frame.depth.at<uint16_t>(dy, dx);

      if (depth_value == 0)
      {
        frame.rs_cloud_ptr->points[i].x = (float)nan;
        frame.rs_cloud_ptr->points[i].y = (float)nan;
        frame.rs_cloud_ptr->points[i].z = (float)nan;
        continue;
      }

      cv::Point2f depth_pixel((float)dx, (float)dy);
      float depth_in_meters = depth_value * frame.scale;

      cv::Point3f depth_point = frame.depthCameraDeproject(depth_pixel, depth_in_meters);
      cv::Point3f color_point = frame.depthCameraToColorCamera(depth_point);
      cv::Point2f color_pixel = frame.colorCameraProject(color_point);

      const int cx = (int)std::round(color_pixel.x);
      const int cy = (int)std::round(color_pixel.y);

      // Set up logic to remove bad points.
      bool depth_fail = true;
      bool color_fail = true;

      depth_fail = (depth_point.z > NOISY);
      color_fail = (cx < 0 || cy < 0 || cx > frame.img.cols || cy > frame.img.rows);

      // Cloud input pointers.

      // XYZ input access to cloud.
      float *dp_x;
      float *dp_y;
      float *dp_z;

      dp_x = &(frame.rs_cloud_ptr->points[i].x);
      dp_y = &(frame.rs_cloud_ptr->points[i].y);
      dp_z = &(frame.rs_cloud_ptr->points[i].z);

      // RGB input access to cloud.
      uint8_t *cp_r;
      uint8_t *cp_g;
      uint8_t *cp_b;

      cp_r = &(frame.rs_cloud_ptr->points[i].r);
      cp_g = &(frame.rs_cloud_ptr->points[i].g);
      cp_b = &(frame.rs_cloud_ptr->points[i].b);

      // Cloud input data.

      // Set up depth point data.
      float real_x = 0;
      float real_y = 0;
      float real_z = 0;
      float adjusted_x = 0;
      float adjusted_y = 0;
      float adjusted_z = 0;

      real_x = depth_point.x;
      real_y = depth_point.y;
      real_z = depth_point.z;

      // Adjust point to coordinates.
      adjusted_x = -1 * real_x;
      adjusted_y = -1 * real_y;
      adjusted_z = real_z;

      // Set up color point data.
      uint8_t *offset = frame.img.ptr<uint8_t>(cy) + cx * 3;

      uint8_t raw_r = 0;
      uint8_t raw_g = 0;
      uint8_t raw_b = 0;
      uint8_t adjusted_r = 0;
      uint8_t adjusted_g = 0;
      uint8_t adjusted_b = 0;

      // Cloud point evaluation.

      // If bad point, remove and skip.
      if (depth_fail || color_fail)
      {
        *dp_x = *dp_y = *dp_z = (float)nan;
        *cp_r = *cp_g = *cp_b = 255;
        continue;
      }

      // If valid point, add data to cloud.
      else
      {
        raw_r = raw_r = *(offset);
        raw_g = raw_g = *(offset + 1);
        raw_b = raw_b = *(offset + 2);

        // Adjust color arbitrarily.
        adjusted_r = raw_r;
        adjusted_g = raw_g;
        adjusted_b = raw_b;

        // Fill in cloud depth.
        *dp_x = adjusted_x;
        *dp_y = adjusted_y;
        *dp_z = adjusted_z;

        // Fill in cloud color.
        *cp_r = adjusted_r;
        *cp_g = adjusted_g;
        *cp_b = adjusted_b;
      }
    }
  }

  for (int i = 1; i < frame.depth.rows; i++)
  {
    for (int j = 1; j < frame.depth.cols; j++)
    {
      int x[4] = {j - 1, j, j, j - 1}, y[4] = {i - 1, i - 1, i, i};
      uint16_t depth_value[4];
      float depth_in_meters[4];
      cv::Point3f color_point[4];
      cv::Point2f color_pixel[4];
      bool valid = true;
      for (int k = 0; k < 4; k++)
      {
        depth_value[k] = frame.depth.at<uint16_t>(y[k], x[k]);
        depth_in_meters[k] = depth_value[k] * frame.scale;
        cv::Point2f depth_pixel((float)x[k], (float)y[k]);
        cv::Point3f depth_point = frame.depthCameraDeproject(depth_pixel, depth_in_meters[k]);
        color_point[k] = frame.depthCameraToColorCamera(depth_point);
        color_pixel[k] = frame.colorCameraProject(color_point[k]);
        if (depth_value[k] == 0
            || (color_pixel[k].x < 0 || color_pixel[k].y < 0 || color_pixel[k].x > frame.img.cols
                || color_pixel[k].y > frame.img.rows))
          valid = false;
      }
      if (!valid)
        continue;
      for (int k = 0; k < 2; k++)
      {
        cv::Point3f color_pointTri[3] = {color_point[0], color_point[2], (k == 0 ? color_point[1] : color_point[3])};
        cv::Point2f color_pixelTri[3] = {color_pixel[0], color_pixel[2], (k == 0 ? color_pixel[1] : color_pixel[3])};
        cv::Point2f pixMin = {std::min(color_pixelTri[0].x, std::min(color_pixelTri[1].x, color_pixelTri[2].x)),
                              std::min(color_pixelTri[0].y, std::min(color_pixelTri[1].y, color_pixelTri[2].y))};
        cv::Point2f pixMax = {std::max(color_pixelTri[0].x, std::max(color_pixelTri[1].x, color_pixelTri[2].x)),
                              std::max(color_pixelTri[0].y, std::max(color_pixelTri[1].y, color_pixelTri[2].y))};
        for (int x = (int)floor(pixMin.x); x < (int)ceil(pixMax.x); x++)
          for (int y = (int)floor(pixMin.y); y < (int)ceil(pixMax.y); y++)
          {
            if (x < 0 || y < 0 || x > frame.img.cols || y > frame.img.rows)
              continue;
            rs::float2 p = {float(x), float(y)};
            float u, v, w;
            Barycentric(rs2cv(p), color_pixelTri[0], color_pixelTri[1], color_pixelTri[2], u, v, w);
            if (u < 0 || u > 1 || v < 0 || v > 1)
              continue;
            cv::Point3f res = u * color_pointTri[0] + v * color_pointTri[1] + w * color_pointTri[2];
            frame.imgDepth.ptr<float>(y)[x] = sqrt(res.dot(res));
            int id = y * frame.imgPoints->width + x;
            frame.imgPoints->points[id].x = res.x;
            frame.imgPoints->points[id].y = res.y;
            frame.imgPoints->points[id].z = res.z;
          }
      }
    }
  }

  pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setNormalEstimationMethod(ne.AVERAGE_3D_GRADIENT);
  ne.setMaxDepthChangeFactor(0.02f);
  ne.setNormalSmoothingSize(10.0f);
  ne.setInputCloud(frame.imgPoints);
  ne.compute(*(frame.imgNormals));

  return frame;
}

bool allThere()
{
  if (color_image_ishere and color_info_ishere and depth_image_ishere and depth_info_ishere)
  {
    RgbdFrame frame = generatePointCloudOrganized2(color_K, color_image, depth_K, depth_image,
                                                   depth_to_color_extrinsics);

    cv::Mat frameImg;
    frameImg = frame.img.clone();

    std::vector<cv::KeyPoint> keypoints2;
    cv::Mat descriptors2;
    akaze->detectAndCompute(frameImg, cv::Mat(), keypoints2, descriptors2);

    if (keypoints2.size() == 0)
      return true;

    std_msgs::Float64 conf[5];
    std::vector<cv::DMatch> matches;
    std::vector<cv::DMatch> good_matches;
    matcher->match(listFeaturesDescFull[selectedObj], descriptors2, matches);

    double max_dist = 0;
    double min_dist = 100;
    // Quick calculation of max and min distances between keypoints.
    for (int i = 0; i < matches.size(); i++)
    {
      double dist = matches[i].distance;
      if (dist < min_dist)
        min_dist = dist;
      if (dist > max_dist)
        max_dist = dist;
    }

    for (int i = 0; i < matches.size(); i++)
    {
      if (matches[i].distance < 3 * min_dist)
        good_matches.push_back(matches[i]);
    }

    std::vector<cv::Point3f> obj;
    std::vector<cv::Point3f> normal;
    std::vector<cv::Point2f> scene;

    for (int i = 0; i < good_matches.size(); i++)
    {
      obj.push_back(listFeaturesPos3d[selectedObj][good_matches[i].queryIdx]);
      normal.push_back(listFeaturesNormal3d[selectedObj][good_matches[i].queryIdx]);
      scene.push_back(keypoints2[good_matches[i].trainIdx].pt);
    }

    cv::Mat mask;
    cv::Mat P = findPoseWithDepthCue(obj, normal, scene, frame, mask);
    std::vector<cv::Point> listPoints;

    matches = good_matches;
    good_matches.clear();
    for (int i = 0; i < matches.size(); i++)
      if (mask.at<unsigned char>(i, 0) > 0)
        good_matches.push_back(matches[i]);

    if (P.cols > 0)
    {

      cv::Point3f com(0, 0, 0.10); //Dummy COM for testing.

      //Publishing item pose into topic
      geometry_msgs::Pose itemPose;
      itemPose.position.x = com.x;
      itemPose.position.y = com.y;
      itemPose.position.z = com.z;
      itemPose.orientation.x = 0;
      itemPose.orientation.y = 0;
      itemPose.orientation.z = 0;
      itemPose.orientation.w = 0;
      pub_item_pose.publish(itemPose);

      conf[0].data = 1.0;
      cv::Scalar color(0, 255, 0);

      std::vector<cv::Point> listPoints;

      for (int i = 0; i < good_matches.size(); i++)
      {
        listPoints.push_back(keypoints2[good_matches[i].trainIdx].pt);
        cv::circle(frameImg, keypoints2[good_matches[i].trainIdx].pt, 3, cv::Scalar(255, 0, 0));
      }
      std::vector<std::vector<cv::Point>> hulls(1);
      cv::convexHull(cv::Mat(listPoints), hulls[0], false);
      cv::drawContours(frameImg, hulls, 0, cv::Scalar(255, 255, 0), 1, 8, std::vector<cv::Vec4i>(), 0, cv::Point());
      double area = hulls[0].size() >= 3 ? cv::contourArea(hulls[0]) : 0;
      //printf("Area: %lf px, ratio %lf\n", area, area/(frameImg.cols * frameImg.rows));
      if (good_matches.size() < 5 || area < frameImg.cols * frameImg.rows / 400)
      {
        color = cv::Scalar(0, 0, 255);
        conf[0].data = 0.33;
      }
      else if (good_matches.size() < 10 || area < frameImg.cols * frameImg.rows / 20)
      {
        color = cv::Scalar(0, 255, 255);
        conf[0].data = 0.66;
      }

      for (int i = 0; i < listFeaturesPos3d[selectedObj].size(); i++)
      {
        cv::Point3f p = multMatVec(P, listFeaturesPos3d[selectedObj][i]);
        p /= p.z;
        cv::Point3f uv = multMatVec(color_K, p);
        cv::circle(frameImg, cv::Point(uv.x, uv.y), 3, color);
      }

      char txt[255];
      sprintf(txt, "%lf, %lf, %lf", com.x, com.y, com.z);
      cv::putText(frameImg, txt, cv::Point(50, 50), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2);
      //printf("COM in camera reference frame: %s\n", txt);
    }

    // Show detected matches.
    char filename[255];
    sprintf(filename, "Good Matches & Object detection");
    cv::imshow(filename, frameImg);

    //Publish the highest confidence among the different faces
    pub_confidence.publish(conf[0]);

    cv::waitKey(1);

    color_image_ishere = false;
    depth_image_ishere = false;
  }
}

void colorImageCallback(const sensor_msgs::ImageConstPtr &msg)
{
  color_image = cv_bridge::toCvShare(msg, "bgr8")->image.clone();
  color_image_ishere = true;
  allThere();
}

void colorInfoCallback(const sensor_msgs::CameraInfoConstPtr &msg)
{
  color_K = cv::Mat(3, 3, CV_64F, (void *)&msg->K[0], cv::Mat::AUTO_STEP).clone();

  color_info_fx = msg->K[0];
  color_info_fy = msg->K[4];
  color_info_ppx = msg->K[2];
  color_info_ppy = msg->K[5];
  color_info_tx = msg->P[3];
  color_info_ty = msg->P[7];
  color_info_tz = msg->P[11];
  color_info_ishere = true;
}

void depthImageCallback(const sensor_msgs::ImageConstPtr &msg)
{
  depth_image = cv_bridge::toCvShare(msg, "16UC1")->image.clone();
  depth_image_ishere = true;
  allThere();
}

void depthInfoCallback(const sensor_msgs::CameraInfoConstPtr &msg)
{
  cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
  if (USE_HARDCODED_ROTATION)
  {
    R.at<double>(0, 0) = 0.999987;
    R.at<double>(0, 1) = 0.004618;
    R.at<double>(0, 2) = 0.002142;
    R.at<double>(1, 0) = -0.004655;
    R.at<double>(1, 1) = 0.999834;
    R.at<double>(1, 2) = 0.017596;
    R.at<double>(2, 0) = -0.002060;
    R.at<double>(2, 1) = -0.017605;
    R.at<double>(2, 2) = 0.999843;
    R = R.t(); // Transpose because RS uses column-major, and OpenCV uses row-major.
  }
  depth_to_color_extrinsics = cv::Mat::eye(4, 4, CV_64F);
  depth_to_color_extrinsics.at<double>(0, 3) = msg->P[3];
  depth_to_color_extrinsics.at<double>(1, 3) = msg->P[7];
  depth_to_color_extrinsics.at<double>(2, 3) = msg->P[11];
  R.copyTo(depth_to_color_extrinsics(cv::Rect(0, 0, 3, 3)));

  depth_K = cv::Mat(3, 3, CV_64F, (void *)&msg->K[0], cv::Mat::AUTO_STEP).clone();

  depth_info_fx = msg->K[0];
  depth_info_fy = msg->K[4];
  depth_info_ppx = msg->K[2];
  depth_info_ppy = msg->K[5];
  depth_info_tx = msg->P[3];
  depth_info_ty = msg->P[7];
  depth_info_tz = msg->P[11];
  depth_info_ishere = true;
}

void itemIdCallback(const std_msgs::Int16::ConstPtr &msg)
{
  char filename[255];
  if (selectedObj != msg->data && msg->data <= listResultImg0.size() - 1)
  {
    selectedObj = msg->data;
  }
}

int main(int argc, char **argv)
{
  scale = 2.0;

  const char *listFolders[] = {"scan1_bee", "scan1_crayola2", "scan1_kleenex1", "scan1_platespie", "scan1_reynold",
                               "scan1_robotsdvd", "scan1_salt1", "scan1_sponge", "scan1_tablecover", "scan1_windex"};

  int nbImg[] = {4, 4, 4, 4, 4, 4, 2, 4, 4, 4};

  for (int i = 0; i < 7; i++)
  {
    char filename[255];
    sprintf(filename, "/root/share/cloud/cloud_result/features_%s.yml", listFolders[i]);
    cv::Mat resultImg0;
    std::vector<cv::Point3f> featuresPos3d;
    std::vector<cv::Point3f> featuresNormal3d;
    cv::Mat featuresDescFull;
    printf("load %s\n", filename);
    loadFeatures(filename, featuresPos3d, featuresNormal3d, featuresDescFull);
    sprintf(filename, "%s/capture%d_cam%d_color.png", listFolders[i], 0, 0);
    resultImg0 = cv::imread(filename);
    listResultImg0.push_back(resultImg0);
    listFeaturesPos3d.push_back(featuresPos3d);
    listFeaturesNormal3d.push_back(featuresNormal3d);
    listFeaturesDescFull.push_back(featuresDescFull);
  }

  akaze = cv::AKAZE::create();

  matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

  ros::init(argc, argv, "tnp_image_viewer");
  ros::init(argc, argv, "tnp_plane_matching");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub_color_image, sub_depth_image;
  ros::Subscriber sub_color_info, sub_depth_info, sub_image_id;

  sub_color_image = it.subscribe("/tnp_image_converter/color/image_raw", 1, colorImageCallback);
  sub_color_info = nh.subscribe("/camera/color/camera_info", 1, colorInfoCallback);

  sub_depth_image = it.subscribe("/tnp_image_converter/depth/image_raw", 1, depthImageCallback);
  sub_depth_info = nh.subscribe("/camera/depth/camera_info", 1, depthInfoCallback);

  //Subscriber to selectedObj for changing between different objects
  sub_image_id = nh.subscribe("/tnp_vision/feature_based/target_item_id", 1, itemIdCallback);

  //Publishers for Item Pose and Confidence
  pub_item_pose = nh.advertise<geometry_msgs::Pose>("/tnp_vision/feature_based/item_pose", 1);
  pub_confidence = nh.advertise<std_msgs::Float64>("/tnp_vision/feature_based/confidence", 1);

  ros::spin();

  return 0;
}


#ifndef TEST_MATH_UNIT_FUNCTIONS_PROMOTE_TYPE_TEST_UTIL_HPP
#define TEST_MATH_UNIT_FUNCTIONS_PROMOTE_TYPE_TEST_UTIL_HPP

#include <stan/math/prim/scal.hpp>
#include <gtest/gtest.h>
#include <boost/typeof/typeof.hpp>
#include <boost/type_traits/is_same.hpp>

template <typename T, typename S>
void expect_type(S s) {
  typedef BOOST_TYPEOF_TPL(stan::math::promote_scalar<T>(s)) result_t;
  bool same = boost::is_same<S, result_t>::value;
  EXPECT_TRUE(same);
}

// pass:  expect_same_type<double, double>()
// fail:  expect_same_type<int, double>()
template <typename T, typename S>
void expect_same_type() {
  EXPECT_TRUE((boost::is_same<S, T>::value));
}

// E is expected value of promote_scalar_type<T, S>::type
template <typename E, typename T, typename S>
void expect_promote_type() {
  using stan::math::promote_scalar_type;
  return expect_same_type<E, typename promote_scalar_type<T, S>::type>();
}

#endif


#include <libdariadb/timeutil.h>
#include <cstdio>
#include <cstring>

#include "boost/date_time/gregorian/gregorian.hpp"
#include <boost/date_time/gregorian/greg_weekday.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

namespace dariadb {
namespace timeutil {

const boost::posix_time::ptime START = boost::posix_time::from_time_t(0);

Time from_ptime(boost::posix_time::ptime timestamp) {
  auto duration = timestamp - START;
  auto ns = duration.total_milliseconds();
  return ns;
}

Time current_time() {
  auto now = boost::posix_time::microsec_clock::universal_time();
  return from_ptime(now);
}

Time from_chrono(const std::chrono::high_resolution_clock::time_point &t) {
  auto now = t.time_since_epoch();
  return std::chrono::duration_cast<std::chrono::milliseconds>(now).count();
}

std::chrono::high_resolution_clock::time_point to_timepoint(Time t) {
  return std::chrono::high_resolution_clock::time_point(std::chrono::nanoseconds(t));
}

boost::posix_time::ptime to_ptime(Time timestamp) {
  boost::posix_time::ptime ptime = START + boost::posix_time::milliseconds(timestamp);
  return ptime;
}

DateTime to_datetime(Time t) {
  using namespace boost::gregorian;
  using namespace boost::posix_time;

  auto ptime = to_ptime(t);
  auto date = ptime.date();
  auto time = ptime.time_of_day();
  auto ymd = gregorian_calendar::from_day_number(date.day_number());

  DateTime result;
  result.year = ymd.year;
  result.month = (uint8_t)ymd.month;
  result.day = ymd.day;
  result.day_of_year = date.day_of_year();
  result.hour = (uint8_t)time.hours();
  result.minute = (uint8_t)time.minutes();
  result.second = (uint8_t)time.seconds();
  result.millisecond = (uint16_t)(time.total_milliseconds() % 1000);
  return result;
}

Time from_datetime(const DateTime &dt) {
  using namespace boost::gregorian;
  using namespace boost::posix_time;

  date d(dt.year, (date::month_type)dt.month, dt.day);
  ptime pt(d,
           hours(dt.hour) + minutes(dt.minute) + seconds(dt.second) +
               milliseconds(dt.millisecond));
  return from_ptime(pt);
}

int to_string(char *buffer, size_t buffer_size, Time t) {
  DateTime dt = to_datetime(t);

  int len = snprintf(buffer, buffer_size, "%04d-%02d-%02d %02d:%02d:%02d.%04d",
                     (int)dt.year, (int)dt.month, (int)dt.day, (int)dt.hour,
                     (int)dt.minute, (int)dt.second, (int)dt.millisecond);

  return len;
}

std::string to_string(Time t) {
  char buffer[512];
  int buff_size = sizeof(buffer);
  std::memset(buffer, 0, buff_size);
  int res = to_string(buffer, buff_size, t);

  if (res == 0 || res == buff_size) {
    return std::string("buffer to small");
  }
  return std::string(buffer);
}

/// construct from string "2002-01-20 23:59:59.000"
Time from_string(const std::string &s) {
  auto pt = boost::posix_time::time_from_string(s);
  return from_ptime(pt);
}

/// construct from string "20020131T235959"
Time from_iso_string(const std::string &s) {
  auto pt = boost::posix_time::from_iso_string(s);
  return from_ptime(pt);
}

Time from_days(const int day) {
  return from_hours(day * 24);
}

Time from_hours(const int h) {
  return from_minutes(h * 60);
}

Time from_minutes(const int m) {
  return from_seconds(m * 60);
}

Time from_seconds(const int s) {
  return Time(s) * 1000;
}

Time round_to_seconds(const Time t) {
  return (Time)(Time(float(t) / 1e3) * 1e3);
}

Time round_to_minutes(const Time t) {
  return t - (t % (60 * 1000));
}

Time round_to_hours(const Time t) {
  return t - (t % (3600 * 1000));
}

std::vector<std::string> predefinedIntervals() {
  std::vector<std::string> predefined = {"minute", "halfhour", "hour", "day",
                                         "week",   "month",    "year"};
  return predefined;
}

Time intervalName2time(const std::string &interval) {
  const Time minute = 1000 * 60;
  const Time hour = minute * 60;
  const Time day = hour * 24;
  if (interval == "minute") {
    return minute;
  }
  if (interval == "halfhour") {
    return minute * 30;
  }
  if (interval == "hour") {
    return hour;
  }
  if (interval == "day") {
    return day;
  }
  if (interval == "week") {
    return day * 7;
  }
  if (interval == "month") {
    return day * 31;
  }
  if (interval == "year") {
    return day * 365;
  }
  return 0;
}

bool intervalsLessCmp(const std::string &l, const std::string &r) {
  auto ltime = intervalName2time(l);
  auto rtime = intervalName2time(r);
  return ltime < rtime;
}

namespace {
Time last_moment(const boost::gregorian::date &d) {
  using namespace boost::posix_time;
  ptime pt(d, hours(23) + minutes(59) + seconds(59) + milliseconds(999));
  Time result = from_ptime(pt);
  return result;
}

Time first_moment(const boost::gregorian::date &d) {
  using namespace boost::posix_time;
  ptime pt(d, hours(0) + minutes(0) + seconds(0) + milliseconds(0));
  Time result = from_ptime(pt);
  return result;
}
} // namespace

std::pair<Time, Time> target_interval(const std::string &period, Time currentTime) {
  using namespace boost::gregorian;
  using namespace boost::posix_time;
  DateTime dt = to_datetime(currentTime);
  const time_duration last_sec = seconds(59) + milliseconds(999);
  const time_duration first_sec = seconds(0) + milliseconds(0);
  if (period == "minute") { // 59:999 sec of each minute
    date d(dt.year, (date::month_type)dt.month, dt.day);
    ptime pt_start(d, hours(dt.hour) + minutes(dt.minute) + first_sec);
    ptime pt_end(d, hours(dt.hour) + minutes(dt.minute) + last_sec);
    auto result = std::make_pair(from_ptime(pt_start), from_ptime(pt_end));
    return result;
  }

  if (period == "halfhour") { // 29:59:999 or 59:59:999
    date d(dt.year, (date::month_type)dt.month, dt.day);
    minutes mints_end(59);
    minutes mints_start(30);
    if (dt.minute < 30) {
      mints_end = minutes(29);
      mints_start = minutes(0);
    }
    ptime pt_end(d, hours(dt.hour) + mints_end + last_sec);
    ptime pt_start(d, hours(dt.hour) + mints_start + first_sec);
    return std::make_pair(from_ptime(pt_start), from_ptime(pt_end));
  }

  if (period == "hour") { // 59:59:999 of each hour
    date d(dt.year, (date::month_type)dt.month, dt.day);
    ptime pt_end(d, hours(dt.hour) + minutes(59) + last_sec);
    ptime pt_start(d, hours(dt.hour) + minutes(0) + first_sec);
    return std::make_pair(from_ptime(pt_start), from_ptime(pt_end));
  }

  if (period == "day") { // 23:59:999 of each day
    date d(dt.year, (date::month_type)dt.month, dt.day);
    return std::make_pair(first_moment(d), last_moment(d));
  }

  if (period == "week") { // 23::59::59:99 of sanday
    date end_day(dt.year, (date::month_type)dt.month, dt.day);
    while (true) {
      auto dow = end_day.day_of_week();
      if (dow == greg_weekday(boost::date_time::Sunday)) {
        break;
      }
      end_day += days(1);
    }

    date start_day(dt.year, (date::month_type)dt.month, dt.day);
    while (true) {
      auto dow = start_day.day_of_week();
      if (dow == greg_weekday(boost::date_time::Monday)) {
        break;
      }
      start_day -= days(1);
    }
    return std::make_pair(first_moment(start_day), last_moment(end_day));
  }

  if (period == "month") { // 23.59.59 of each last day of month
    date d(dt.year, (date::month_type)dt.month, dt.day);
    date d_start(dt.year, (date::month_type)dt.month, 1);
    auto d_end = d.end_of_month();
    return std::make_pair(first_moment(d_start), last_moment(d_end));
  }

  if (period == "year") {
    date d_start(dt.year, 1, 1);
    date d_end(dt.year, 12, 31);
    return std::make_pair(first_moment(d_start), last_moment(d_end));
  }
  return std::make_pair(MAX_TIME, MAX_TIME);
}

} // namespace timeutil
} // namespace dariadb


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <cstddef>
#include <iterator>
#include <string>

#include <algorithms/test_convex_hull.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <boost/geometry/domains/gis/io/wkt/read_wkt.hpp>

#include <boost/geometry/multi/core/point_order.hpp>
#include <boost/geometry/multi/core/point_type.hpp>

#include <boost/geometry/multi/views/detail/range_type.hpp>

#include <boost/geometry/multi/algorithms/num_points.hpp>
#include <boost/geometry/multi/algorithms/detail/for_each_range.hpp>

#include <boost/geometry/domains/gis/io/wkt/wkt.hpp>

#include <boost/geometry/multi/geometries/multi_point.hpp>
#include <boost/geometry/multi/geometries/multi_linestring.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>

template <typename P>
void test_all()
{
    typedef bg::model::multi_point<P> mp;
    typedef bg::model::multi_linestring<bg::model::linestring<P> > ml;
    typedef bg::model::multi_polygon<bg::model::polygon<P> > mpoly;
    test_geometry<mp>("multipoint((1.1 1.1), (2.5 2.1), (3.1 3.1), (4.9 1.1), (3.1 1.9))", 5, 4, 3.8);
    test_geometry<ml>("multilinestring((2 4, 3 4, 3 5), (4 3,4 4,5 4))", 6, 5, 3.0);
    test_geometry<mpoly>("multipolygon(((1 4,1 6,2 5,3 5,4 6,4 4,1 4)), ((4 2,4 3,6 3,6 2,4 2)))", 12, 7, 14.0);
}

int test_main(int, char* [])
{
    //test_all<bg::model::d2::point_xy<int> >();
    //test_all<bg::model::d2::point_xy<float> >();
    test_all<bg::model::d2::point_xy<double> >();

#ifdef HAVE_TTMATH
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

    return 0;
}


//=======================================================================
// Copyright (c) Aaron Windsor 2007
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef __PLANAR_CANONICAL_ORDERING_HPP__
#define __PLANAR_CANONICAL_ORDERING_HPP__

#include <vector>
#include <list>
#include <boost/config.hpp>
#include <boost/next_prior.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>

namespace boost
{

  namespace detail {
    enum planar_canonical_ordering_state
         {PCO_PROCESSED,
          PCO_UNPROCESSED,
          PCO_ONE_NEIGHBOR_PROCESSED,
          PCO_READY_TO_BE_PROCESSED};
  }

  template<typename Graph,
           typename PlanarEmbedding,
           typename OutputIterator,
           typename VertexIndexMap>
  void planar_canonical_ordering(const Graph& g,
                                 PlanarEmbedding embedding,
                                 OutputIterator ordering,
                                 VertexIndexMap vm)
  {

    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    typedef typename graph_traits<Graph>::edge_descriptor edge_t;
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator_t;
    typedef typename graph_traits<Graph>::adjacency_iterator
      adjacency_iterator_t;
    typedef typename std::pair<vertex_t, vertex_t> vertex_pair_t;
    typedef typename property_traits<PlanarEmbedding>::value_type
      embedding_value_t;
    typedef typename embedding_value_t::const_iterator embedding_iterator_t;
    typedef iterator_property_map
      <typename std::vector<vertex_t>::iterator, VertexIndexMap>
      vertex_to_vertex_map_t;
    typedef iterator_property_map
      <typename std::vector<std::size_t>::iterator, VertexIndexMap>
      vertex_to_size_t_map_t;

    std::vector<vertex_t> processed_neighbor_vector(num_vertices(g));
    vertex_to_vertex_map_t processed_neighbor
      (processed_neighbor_vector.begin(), vm);

    std::vector<std::size_t> status_vector(num_vertices(g), detail::PCO_UNPROCESSED);
    vertex_to_size_t_map_t status(status_vector.begin(), vm);

    std::list<vertex_t> ready_to_be_processed;

    vertex_t first_vertex = *vertices(g).first;
    vertex_t second_vertex;
    adjacency_iterator_t ai, ai_end;
    for(boost::tie(ai,ai_end) = adjacent_vertices(first_vertex,g); ai != ai_end; ++ai)
      {
        if (*ai == first_vertex)
          continue;
        second_vertex = *ai;
        break;
      }

    ready_to_be_processed.push_back(first_vertex);
    status[first_vertex] = detail::PCO_READY_TO_BE_PROCESSED;
    ready_to_be_processed.push_back(second_vertex);
    status[second_vertex] = detail::PCO_READY_TO_BE_PROCESSED;

    while(!ready_to_be_processed.empty())
      {
        vertex_t u = ready_to_be_processed.front();
        ready_to_be_processed.pop_front();

        if (status[u] != detail::PCO_READY_TO_BE_PROCESSED && u != second_vertex)
          continue;

        embedding_iterator_t ei, ei_start, ei_end;
        embedding_iterator_t next_edge_itr, prior_edge_itr;

        ei_start = embedding[u].begin();
        ei_end = embedding[u].end();
        prior_edge_itr = prior(ei_end);
        while(source(*prior_edge_itr, g) == target(*prior_edge_itr,g))
          prior_edge_itr = prior(prior_edge_itr);

        for(ei = ei_start; ei != ei_end; ++ei)
          {

            edge_t e(*ei); // e = (u,v)
            next_edge_itr = boost::next(ei) == ei_end ? ei_start : boost::next(ei);
            vertex_t v = source(e,g) == u ? target(e,g) : source(e,g);

            vertex_t prior_vertex = source(*prior_edge_itr, g) == u ?
              target(*prior_edge_itr, g) : source(*prior_edge_itr, g);
            vertex_t next_vertex = source(*next_edge_itr, g) == u ?
              target(*next_edge_itr, g) : source(*next_edge_itr, g);

            // Need prior_vertex, u, v, and next_vertex to all be
            // distinct. This is possible, since the input graph is
            // triangulated. It'll be true all the time in a simple
            // graph, but loops and parallel edges cause some complications.
            if (prior_vertex == v || prior_vertex == u)
              {
                prior_edge_itr = ei;
                continue;
              }

            //Skip any self-loops
            if (u == v)
                continue;

            // Move next_edge_itr (and next_vertex) forwards
            // past any loops or parallel edges
            while (next_vertex == v || next_vertex == u)
              {
                next_edge_itr = boost::next(next_edge_itr) == ei_end ?
                  ei_start : boost::next(next_edge_itr);
                next_vertex = source(*next_edge_itr, g) == u ?
                  target(*next_edge_itr, g) : source(*next_edge_itr, g);
              }

            if (status[v] == detail::PCO_UNPROCESSED)
              {
                status[v] = detail::PCO_ONE_NEIGHBOR_PROCESSED;
                processed_neighbor[v] = u;
              }
            else if (status[v] == detail::PCO_ONE_NEIGHBOR_PROCESSED)
              {
                vertex_t x = processed_neighbor[v];
                //are edges (v,u) and (v,x) adjacent in the planar
                //embedding? if so, set status[v] = 1. otherwise, set
                //status[v] = 2.

                if ((next_vertex == x &&
                     !(first_vertex == u && second_vertex == x)
                     )
                    ||
                    (prior_vertex == x &&
                     !(first_vertex == x && second_vertex == u)
                     )
                    )
                  {
                    status[v] = detail::PCO_READY_TO_BE_PROCESSED;
                  }
                else
                  {
                    status[v] = detail::PCO_READY_TO_BE_PROCESSED + 1;
                  }
              }
            else if (status[v] > detail::PCO_ONE_NEIGHBOR_PROCESSED)
              {
                //check the two edges before and after (v,u) in the planar
                //embedding, and update status[v] accordingly

                bool processed_before = false;
                if (status[prior_vertex] == detail::PCO_PROCESSED)
                  processed_before = true;

                bool processed_after = false;
                if (status[next_vertex] == detail::PCO_PROCESSED)
                  processed_after = true;

                if (!processed_before && !processed_after)
                    ++status[v];

                else if (processed_before && processed_after)
                    --status[v];

              }

            if (status[v] == detail::PCO_READY_TO_BE_PROCESSED)
              ready_to_be_processed.push_back(v);

            prior_edge_itr = ei;

          }

        status[u] = detail::PCO_PROCESSED;
        *ordering = u;
        ++ordering;

      }

  }

  template<typename Graph, typename PlanarEmbedding, typename OutputIterator>
  void planar_canonical_ordering(const Graph& g,
                                 PlanarEmbedding embedding,
                                 OutputIterator ordering
                                 )
  {
    planar_canonical_ordering(g, embedding, ordering, get(vertex_index,g));
  }

} //namespace boost

#endif //__PLANAR_CANONICAL_ORDERING_HPP__


/**
Common functions and typedefs.

@file cnet_common.hpp
@author Bastian
*/

#ifndef CNET_INTERNAL_HPP
#define CNET_INTERNAL_HPP

#include <map>
#include <fstream>

#include <Eigen/StdVector>
#include <Eigen/Dense>
#include <experimental/filesystem>
#include <mutex>
#include <condition_variable>
#include "cnet_cv2eigen.hpp"

#define INITIAL_QUEUE_SIZE 64
#define THREAD_RETRY_DELAY 2

namespace Cnet
{

	typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRm;
	typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> MatrixCm;
	typedef std::vector<MatrixRm, Eigen::aligned_allocator<MatrixRm > > AlignedStdVector;
	typedef std::vector<MatrixRm, Eigen::aligned_allocator<MatrixRm > > AlignedStdVector;
	//typedef std::vector<std::pair<MatrixRm, MatrixRm>, std::pair< Eigen::aligned_allocator< MatrixRm >, Eigen::aligned_allocator< MatrixRm > >  > AlignedStdPairVector;
	typedef std::map<int, Eigen::Vector4f, std::less<int>, Eigen::aligned_allocator<std::pair<const int, Eigen::Vector4f> > > AlignedMap;
	typedef Eigen::Map< Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > MatrixRmMap;
	typedef Eigen::Map< Eigen::VectorXf> VectorMap;

	inline void disable_multithreading()
	{
		Eigen::setNbThreads(1);
	}

	/*
	Checks if file exists...
	*/
	inline bool file_exists(const std::string& file_name)
	{
		std::ifstream infile(file_name);
		return infile.good();
	}

	/*
	Get filename from string
	*/
	inline std::string strip_filename(const std::string& path)
	{
		std::string stripped = path.substr(path.find_last_of("/\\") + 1);
		size_t dot_i = path.find_last_of('.');
		return stripped.substr(0, dot_i);
	}

	/*
	Struct to hold convolution parameters
	*/
	class ConvolutionParams
	{
	private:
		size_t _kernel_size;
		size_t _kernel_width;
		size_t _stride;
		size_t _padding;
	public:

		ConvolutionParams(size_t kernel_width, size_t stride, size_t padding)
		{
			_kernel_width = kernel_width;
			_kernel_size = (size_t)pow(kernel_width, 2.0);
			_stride = stride;
			_padding = padding;

		}

		inline size_t kernel_size()
		{
			return _kernel_size;
		}

		inline size_t padding()
		{
			return _padding;
		}

		inline size_t kernel_width()
		{
			return _kernel_width;
		}

		inline size_t stride()
		{
			return _stride;
		}

		void stride(size_t stride)
		{
			_stride = stride;
		}

		void padding(size_t padding)
		{
			_padding = padding;
		}

		void kernel_width(size_t kernel_width)
		{
			_kernel_width = kernel_width;
			_kernel_size = (size_t)pow(kernel_width, 2.0);
		}

	};

	/*
	Struct to hold Datasample
	*/
	class Entry
	{
	private:
		MatrixRm _data;
		bool _is_file;
		std::string _file_name;
		std::string _file_path;
	public:

		Entry(MatrixRm data, const std::string& fpath)
		{
			_data = data;
			_file_name = strip_filename(fpath);
			_file_path = fpath;
			_is_file = true;
		}

        explicit Entry(MatrixRm data)
		{
			_data = data;
			_is_file = false;
		}

		Entry()
		{
			_is_file = false;
		}

		MatrixRm* data()
		{
			return &_data;
		}

		std::string file_name()
		{
			return _file_name;
		}

		bool is_file()
		{
			return _is_file;
		}

		std::string file_path()
		{
			return _file_path;
		}

	};

	/*
	Template for thread-safe queue class
	*/
	template <class T> class DataQueue
	{
	public:

        explicit DataQueue(size_t max_size)
		{
			_max_size = max_size;
			_current_size = 0;
		}

		DataQueue()
		{
			_max_size = INITIAL_QUEUE_SIZE;
			_current_size = 0;
		}

		~DataQueue() = default;

		void push(T t)
		{
			std::unique_lock<std::mutex> lock(_m);
			_c.wait(lock, [this]() {return _queue.size() < _max_size; });
			_queue.push_back(t);
			_c.notify_all();
		}

		void push_streight(T t)
		{
			//std::unique_lock<std::mutex> lock(_m);
			//c.wait(lock, [this]() {return _queue.size() < _max_size; });
			_queue.push_back(t);
			//_c.notify_all();
		}

		T pop()
		{
			std::unique_lock<std::mutex> lock(_m);
			while (_queue.empty())
			{
				_c.wait(lock);
			}
			_c.notify_all();
			T val = _queue.front();
			_queue.pop_front();
			return val;
		}

		bool try_pop(T& item, std::chrono::milliseconds timeout)
		{
			std::unique_lock<std::mutex> lock(_m);
			if (!_c.wait_for(lock, timeout, [this] { return !_queue.empty(); }))
			{
				return false;
			}
			_c.notify_all();
			item = _queue.front();
			_queue.pop_front();
			return true;
		}

		T at(size_t idx)
		{
			std::unique_lock<std::mutex> lock(_m);
			while (_queue.empty())
			{
				_c.wait(lock);
			}
			_c.notify_all();
			T val = _queue[idx];
			return val;
		}

		size_t get_size()
		{
			return _queue.size();
		}

		size_t get_max_size()
		{
			return _max_size;
		}

		void set_max_size(size_t queue_size)
		{
			_max_size = queue_size;
		}

	private:
		std::deque<T> _queue;
		mutable std::mutex _m;
		std::condition_variable _c;
		size_t _max_size;
		size_t _current_size;
	};

	inline int load_image(MatrixRm* image, const std::string& path)
	{
		if (file_exists(path))
		{
			MatrixRm input;
			eigen2cv(input) = cv::imread(path, cv::IMREAD_GRAYSCALE);
			//scale down
			*image = MatrixRmMap(input.data(), 1, input.cols()*input.rows());
			//*image /= 255.f;
			//std::cout << "image:\n" << *image << std::endl;
			return 1;
		}
		return 0;
	}

	inline int load_image(MatrixRm& image, const std::string& path)
	{
		return load_image(&image, path);
	}

	void save_image(MatrixRm* image, std::string path)
	{
		auto width = (size_t) sqrt(image->cols());
		MatrixRm out = MatrixRmMap(image->data(), width, width);
		out *= 255;
		cv::Mat mat_out = eigen2cv(out);
		cv::imwrite(path, mat_out);
	}

	void save_image(MatrixRm& image, const std::string& path)
	{
		save_image(&image, path);
	}

	/*
	Method to extract image tiles.
	*/
	inline void extract_image_tiles(MatrixRm* image, AlignedStdVector tile_vec, const size_t tile_size, const size_t stride)
	{
		MatrixRm tile = MatrixRm::Zero(tile_size, tile_size);
		const auto input_width = (size_t)sqrt(image->cols());
		const auto num_tiles = (size_t)((image->cols() - tile_size) / 2) + 1;
		for (size_t row = 0; row < num_tiles; ++row)
		{
			const size_t current_row = row * input_width;
			for (size_t col = 0; col < num_tiles; ++col)
			{
				const size_t current_pos = (current_row*stride) + (col*stride);
				for (size_t kernel_row = 0; kernel_row < tile_size; ++kernel_row)
				{
					tile.block(kernel_row, 0, 1, tile_size) = (*image).block(0, current_pos + kernel_row*input_width, 1, tile_size);
				}
				tile_vec.push_back(tile);
			}
		}
	}

	/*
	Method to pad border with zeros.
	*/
	inline void pad_border(MatrixRm* in, MatrixRm* out, size_t offset)
	{
		if (offset == 0)
		{
			*out = *in;
		}
		else
		{
			const auto input_width = (size_t)sqrt(in->cols());
			*out = MatrixRm::Zero(in->rows(), (unsigned int)pow(2 * offset + input_width, 2.));
			const size_t steps = 2 * offset + input_width;
			size_t start = 0;
			for (size_t i = 0; i < input_width; ++i)
			{
				out->block(0, start, in->rows(), input_width) = in->block(0, i*input_width, in->rows(), input_width);
				start += steps;
			}
		}
	}

	/*
	Alternative method to add zeros to the border.
	*/
	inline void pad(MatrixRm* in, MatrixRm* out, size_t offset)
	{
		const size_t padded_width = (size_t)sqrt(in->cols()) + offset;
		*out = MatrixRm::Zero(in->rows(), (unsigned int)pow(padded_width, 2.));
		const auto org_width = (size_t)sqrt(in->cols());
		for (unsigned int channel = 0; channel < out->rows(); ++channel)
		{
			MatrixRmMap(out->row(channel).data(), padded_width, padded_width).block(offset /2, offset/2, org_width, org_width)
				= MatrixRmMap(in->row(channel).data(), org_width, org_width);
		}
	}

	/*
	Crops center of the image by given offset.
	*/
	inline void crop(MatrixRm* in, MatrixRm* out, size_t offset)
	{
		const size_t cropped_width = (size_t)sqrt(in->cols()) - offset;
		*out = MatrixRm(in->rows(), (unsigned int)pow(cropped_width, 2.));
		const auto org_width = (size_t)sqrt(in->cols());
		for (unsigned int channel = 0; channel < out->rows(); ++channel)
		{
			MatrixRmMap(out->row(channel).data(), cropped_width, cropped_width) = MatrixRmMap(in->row(channel).data(), org_width, org_width).block(offset / 2, offset / 2, cropped_width, cropped_width);
		}
	}

	/*
	Inserts zeros between values of a matrix by stride - 1.
	*/
	inline void pad_inner(MatrixRm* input, MatrixRm* out, const size_t stride)
	{
		const auto input_width = (size_t)sqrt(input->cols());
		const size_t inner_pad = stride - 1;
		const size_t out_width = input_width + (input_width*inner_pad - 1);
		*out = MatrixRm::Zero(input->rows(), (unsigned int)pow(out_width, 2.));
		size_t current_pos = 0;
		for (int i = 0; i < input->cols(); ++i)
		{
			out->block(0, current_pos, input->rows(), 1) = input->block(0, i, input->rows(), 1);
			if (++current_pos % out_width != 0)
			{
				current_pos += inner_pad;
			}
			else
			{
				current_pos += out_width;
			}
		}
	}

	/*
	Reverse's pad_inner method using stride - 1.
	*/
	inline void unpad_inner(MatrixRm* input, MatrixRm* out, const size_t org_width, const size_t stride)
	{
		const auto input_width = (size_t)sqrt(input->cols());
		const auto inner_pad = stride - 1;
		*out = MatrixRm::Zero(input->rows(), (unsigned int)pow(org_width, 2.));
		size_t current_pos = 0;
		for (int i = 0; i < out->cols(); ++i)
		{
			out->block(0, i, input->rows(), 1) = input->block(0, current_pos, input->rows(), 1);
			if (++current_pos % input_width != 0)
			{
				current_pos += inner_pad;
			}
			else
			{
				current_pos += input_width;
			}
		}
	}

	/*
	Performs im2col operation. As before, see Caffe's source code for further details. This can potentially implemented in a faster way....
	*/
	inline void im2col(MatrixRm* input, MatrixRm* out, const size_t kernel_width, const size_t kernel_size, const size_t stride)
	{
		const auto input_width = (size_t)sqrt(input->cols());
		const auto kernel_moves = (size_t)((input_width - kernel_width) / stride) + 1;
		*out = MatrixRm::Zero(input->rows()* kernel_size, (unsigned int)pow(kernel_moves, 2));
		size_t column_idx = 0;
		MatrixRm tile = MatrixRm::Zero(input->rows(), kernel_size);
		for (size_t row = 0; row < kernel_moves; ++row)
		{
			const size_t current_row = row * input_width;
			for (size_t col = 0; col < kernel_moves; ++col)
			{
				const size_t current_pos = (current_row*stride) + (col*stride);
				for (size_t kernel_row = 0; kernel_row < kernel_width; ++kernel_row)
				{
					tile.block(0, kernel_row*kernel_width, input->rows(), kernel_width) =
						input->block(0, current_pos + kernel_row*input_width, input->rows(), kernel_width);
				}
				out->col(column_idx++) = Eigen::Map<Eigen::VectorXf>(tile.data(), out->rows());
			}
		}
	}

	/*
	Performs im2col operation. See Caffe's source code for further details.
	*/
	inline void im2col(MatrixRm* input, MatrixRm* out, ConvolutionParams& conv_params)
	{
		im2col(input, out, conv_params.kernel_width(), conv_params.kernel_size(), conv_params.stride());
	}

	/*
	Reverse operation for im2col.
	*/
	inline void col2im(MatrixRm* input, const MatrixRm* im2col, MatrixRm* out, ConvolutionParams& conv_params)
	{
		const auto input_width = (size_t)sqrt(input->cols());
		const auto kernel_moves = (size_t)((input_width - conv_params.kernel_width()) / conv_params.stride()) + 1;
		*out = MatrixRm::Zero(input->rows(), input->cols());
		size_t col_cnt = 0;
		for (size_t row = 0; row < kernel_moves; ++row)
		{
			for (size_t col = 0; col < kernel_moves; ++col)
			{
				for (int ch = 0; ch < input->rows(); ++ch)
				{
					MatrixRm chunk = im2col->block(ch * conv_params.kernel_size(), col_cnt, conv_params.kernel_size(), 1);
					MatrixRmMap(out->row(ch).data(), input_width, input_width).block(row, col, conv_params.kernel_width(), conv_params.kernel_width()).array()
						+= MatrixRmMap(chunk.data(), conv_params.kernel_width(), conv_params.kernel_width()).array();
				}
				++col_cnt;
			}
		}
	}

	/*
	Alternative col2im method... still reversing the col2im.
	*/
	inline void col2im(MatrixRm* im2col, MatrixRm* out, const size_t output_width, ConvolutionParams& conv_params)
	{
		const size_t output_depth = (size_t)im2col->rows() / conv_params.kernel_size();
		*out = MatrixRm::Zero(output_depth, output_width * output_width);
		size_t row = 0;
		size_t col = 0;
		for (int i = 0; i < im2col->cols(); i++)
		{
			for (size_t j = 0; j < output_depth; j++)
			{
				MatrixRm chunk = im2col->block(j*conv_params.kernel_size(), 0, conv_params.kernel_size(), 1);
				MatrixRmMap(out->row(j).data(), output_width, output_width).block(col, row, conv_params.kernel_width(), conv_params.kernel_width()).array() +=
					MatrixRmMap(chunk.data(), conv_params.kernel_width(), conv_params.kernel_width()).array();

			}
			col += conv_params.stride();
			if ((col + conv_params.kernel_width()) > output_width)
			{
				col = 0;
				row += conv_params.stride();
			}
		}
	}

	/*
	Computes weight gradient as matrix multiplication.
	*/
	inline void compute_wgrad_gemm_cpu(MatrixRm* dout, MatrixRm* im2col_input, MatrixRm* wgrad, MatrixRm* dout_rs, const size_t w_rows, const size_t w_cols, const size_t num_filters)
	{
		const size_t row_len = (size_t)(dout->cols() * dout->rows()) / num_filters;
		*dout_rs = MatrixRmMap(dout->transpose().data(), w_rows, row_len);
		MatrixRm wgrad_tmp = *dout_rs * im2col_input->transpose();
		*wgrad = MatrixRmMap(wgrad_tmp.data(), w_rows, w_cols).array();
	}

	/*
	Computes input delta as matrix multiplication.
	*/
	inline void compute_dx_gemm_cpu(MatrixRm* dx, MatrixRm* dout_rs, MatrixRm* input, MatrixRm* w, ConvolutionParams& conv_params)
	{
		MatrixRm dx_col = w->transpose() * *dout_rs;
		col2im(input, &dx_col, dx, conv_params);
	}

	/*
	Computes backwards pass of a convolution layer as matrix multiplication...probably the wrong place for this.
	*/
	inline void backward_gemm_cpu(MatrixRm* dout, MatrixRm* dx, MatrixRm* input, MatrixRm* im2col_input, MatrixRm* w, MatrixRm* wgrad, ConvolutionParams& conv_params)
	{

		const size_t row_len = (size_t)(dout->cols() * dout->rows()) / wgrad->rows();
		MatrixRm dout_rs = MatrixRmMap(dout->transpose().data(), wgrad->rows(), row_len);
		MatrixRm raw_grads = dout_rs * im2col_input->transpose();
		*wgrad = MatrixRmMap(raw_grads.data(), wgrad->rows(), wgrad->cols());
		MatrixRm dx_col = w->transpose() * dout_rs;
		col2im(input, &dx_col, dx, conv_params);
	}

	inline void crop_center(MatrixRm* in, MatrixRm* out, size_t offset)
	{
		const auto input_width = (size_t)sqrt(in->cols());
		const size_t delta = input_width - (2 * offset);
		const size_t border = 2 * offset;
		*out = MatrixRm(in->rows(), (size_t)pow(delta, 2.));
		size_t start = (offset * input_width) + offset;
		for (size_t i = 0; i < delta; ++i)
		{
			out->block(0, i*delta, in->rows(), delta) = in->block(0, start, in->rows(), delta);
			start += delta + border;
		}
	}

	inline void pad_border(MatrixRm& in, MatrixRm& out, size_t offset)
	{
		const auto input_width = (size_t)sqrt(in.cols());
		const size_t steps = 2 * offset + input_width;
		size_t start = 0;
		for (size_t i = 0; i < input_width; ++i)
		{
			out.block(0, start, in.rows(), input_width) = in.block(0, i*input_width, in.rows(), input_width);
			start += steps;
		}
	}

	inline MatrixRm encode_one_hot_axis1(const size_t class_idx, const size_t num_classes)
	{
		MatrixRm one_hot_encoded_mat = MatrixRm::Zero(1, num_classes);
		for (size_t i = 0; i < num_classes; ++i)
		{
			if (class_idx == i)
			{
				one_hot_encoded_mat(0, i) = 1.f;
			}
		}
		return one_hot_encoded_mat;
	}

	/*
	* Function to encode labels to one-hot over x-axis, e.g. 2 -> [0, 1]
	* @param clas_idx
	* @param num_classes
	* @return the one-hot encoded vector
	*/
	inline MatrixRm encode_one_hot(const size_t class_idx, const size_t num_classes)
	{
		MatrixRm one_hot_encoded_mat = MatrixRm::Zero(1, num_classes);
		for (size_t i = 0; i < num_classes; ++i)
		{
			if (class_idx == i)
			{
				one_hot_encoded_mat(0, i) = 1.f;
			}
		}
		return one_hot_encoded_mat;
	}

	inline void encode_one_hot(MatrixRm& label, const size_t class_idx, const size_t num_classes)
	{
		label = MatrixRm::Zero(1, num_classes);
		for (size_t i = 0; i < num_classes; ++i)
		{
			if (class_idx == i)
			{
				label(0, i) = 1.f;
			}
		}
	}

	/*
	* Function to encode labels to one-hot over y-axis, e.g. 2 -> [0, 1]
	* @param clas_idx
	* @param num_classes
	* @return the one-hot encoded matrix
	*/
	/*
	MatrixRm encode_one_hot(const MatrixRm& label, const size_t num_classes)
	{
		MatrixRm one_hot_encoded_mat = MatrixRm::Zero(num_classes, label.cols());
		for (int i = 0; i < label.cols(); ++i)
		{
			for (size_t j = 0; j < num_classes; ++j)
			{
				if ((size_t)label(0, i) == j)
				{
					one_hot_encoded_mat(j, i) = 1.f;
				}
			}
		}
		return one_hot_encoded_mat;
	}
	*/

	inline void encode_label(AlignedStdVector& data_container, const size_t num_classes, const float label)
	{
		if (num_classes > 1)
		{
			MatrixRm label_mat = encode_one_hot((size_t)label, num_classes);
			data_container.push_back(label_mat);
		}
		else
		{
			MatrixRm label_mat(1, 1);
			label_mat(0, 0) = label;
			data_container.push_back(label_mat);
		}
	}

	/*
	* Argmax of a given input. Will determine the axis to perform action by the shape of outputs.
	* NOTE: Currently only 1d data is processed correctly.
	*/
	inline unsigned int argmax(MatrixRm* outputs)
	{
		unsigned int i = 0, j = 0;
		outputs->maxCoeff(&i, &j);
		return j;
	}

	inline unsigned int argmax(MatrixRm& outputs)
	{
		return argmax(&outputs);
	}

	inline void depthwise_argmax(MatrixRm& outputs, MatrixRm& argmaxed)
	{
		argmaxed = MatrixRm::Zero(1, outputs.cols());
		for (int col = 0; col < outputs.cols(); ++col)
		{
			unsigned int i = 0, j = 0;
			outputs.block(0, col, outputs.rows(), 1).maxCoeff(&i, &j);
			argmaxed(0, col) = (float)i;
		}
	}

	/*
	* Argmax of a given input. Will determine the axis to perform action by the shape of outputs.
	* NOTE: Currently only 1d data is processed correctly.
	*/
	inline void argmax(MatrixRm& outputs, MatrixRm& result)
	{
		for (unsigned int i = 0; i < outputs.cols(); ++i)
		{
			unsigned int x = 0, y = 0;
			outputs.block(0, i, outputs.rows(), 1).maxCoeff(&x, &y);
			result(0, i) = (float)y;
		}
	}

	inline MatrixRm to_matrix(std::vector<std::vector<float> > data)
	{
		MatrixRm sample = MatrixRm::Zero(data.size(), data[0].size());
		for (size_t i = 0; i < data.size(); i++)
		{
			for (size_t j = 0; j < data[i].size(); j++)
			{
				sample(i, j) = data[i][j];
			}
		}
		return sample;
	}

	/*
	MatrixRm to_matrix(std::vector<std::vector<float> > data)
	{
		MatrixRm sample = MatrixRm::Zero(data.size(), data[0].size());
		for (size_t i = 0; i < data.size(); i++)
		{
			for (size_t j = 0; j < data[i].size(); j++)
			{
				sample(i, j) = data[i][j];
			}
		}
		return sample;
	}
	*/
}
#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_ALGORITHM_UPPER_BOUND_HPP
#define BOOST_COMPUTE_ALGORITHM_UPPER_BOUND_HPP

#include <boost/compute/lambda.hpp>
#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/detail/binary_find.hpp>

namespace boost {
namespace compute {

/// Returns an iterator pointing to the first element in the sorted
/// range [\p first, \p last) that is not less than or equal to
/// \p value.
///
/// Space complexity: \Omega(1)
template<class InputIterator, class T>
inline InputIterator
upper_bound(InputIterator first,
            InputIterator last,
            const T &value,
            command_queue &queue = system::default_queue())
{
    using ::boost::compute::_1;

    InputIterator position =
        detail::binary_find(first, last, _1 > value, queue);

    return position;
}

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_ALGORITHM_UPPER_BOUND_HPP


#include "cor_algorithm/sources/utilities.h"
#include "cor_system/sources/logger.h"
#include "cor_data_structure/sources/geometry/uniform_grid_tmpl_impl.h"

#define BOOST_TEST_NO_LIB
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(uniform_grid)

BOOST_AUTO_TEST_CASE(uniform_grid_basic)
{
    typedef cor::data_structure::UniformGridTmpl<cor::RInt32, cor::type::Vector3I> G;
    typedef cor::type::Vector3I V;

    G g(V(4, 3, 2));

    cor::RInt32 i, j, k;
    cor::RInt32 ct;
    auto sz = g.get_size();

    ct = 0;
    for(i = 0 ; i < sz[2] ; i++)
    {
        for(j = 0 ; j < sz[1] ; j++)
        {
            for(k = 0 ; k < sz[0] ; k++)
            {
                g.ref(V(k, j, i)) = ct;
                ct++;
            }
        }
    }

    auto& a = g.ref_a();
    cor::RStringStream s;
    cor::RString rs = "";
    ct = 0;
    cor::RInt32 cta = 0;
    for(auto v : a)
    {
        s << v << ", ";
        if(ct > sz[0])
        {
            ct = 0;

            rs += s.str();
            s.str("");

        }
        BOOST_CHECK_EQUAL(v, cta);

        ct++;
        cta++;
    }

}

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright 2019 Mateusz Loskot <mateusz at loskot dot net>
// Copyright 2019 Miral Shah <miralshah2211@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#define BOOST_TEST_MODULE gil/test/extension/numeric/kernel
#include "unit_test.hpp"

#define BOOST_DISABLE_ASSERTS 1 // kernel_1d_adaptor assertions are too strict
#include <boost/gil/extension/numeric/kernel.hpp>
#include <vector>

namespace gil = boost::gil;

BOOST_AUTO_TEST_CASE(kernel_1d_default_constructor)
{
    gil::kernel_1d<int> k;
    BOOST_TEST(k.center() == 0);
    BOOST_TEST(k.left_size() == 0);
    BOOST_TEST(k.right_size() == -1); // TODO: Why not 0?
    // std::vector interface
    BOOST_TEST(k.size() == 0);
}

BOOST_AUTO_TEST_CASE(kernel_2d_default_constructor)
{
    gil::detail::kernel_2d<int> k;
    BOOST_TEST(k.center_y() == 0);
    BOOST_TEST(k.center_x() == 0);

    //BOOST_TEST(k.left_size() == 0);
    //BOOST_TEST(k.right_size() == -1);
    BOOST_TEST(k.upper_size() == 0);
    BOOST_TEST(k.lower_size() == -1);
    // std::vector interface
    BOOST_TEST(k.size() == 0);
}

BOOST_AUTO_TEST_CASE(kernel_1d_parameterized_constructor)
{
    gil::kernel_1d<int> k(9, 4);
    BOOST_TEST(k.center() == 4);
    BOOST_TEST(k.left_size() == 4);
    BOOST_TEST(k.right_size() == 4);
    // std::vector interface
    BOOST_TEST(k.size() == 9);
}

BOOST_AUTO_TEST_CASE(kernel_2d_parameterized_constructor)
{
    gil::detail::kernel_2d<int> k(9, 4, 4);
    BOOST_TEST(k.center_y() == 4);
    BOOST_TEST(k.center_x() == 4);
    BOOST_TEST(k.left_size() == 4);
    BOOST_TEST(k.right_size() == 4);
    BOOST_TEST(k.upper_size() == 4);
    BOOST_TEST(k.lower_size() == 4);
    // std::vector interface
    BOOST_TEST(k.size() == 9);
}

BOOST_AUTO_TEST_CASE(kernel_1d_parameterized_constructor_with_iterator)
{
    std::vector<int> v(9);
    gil::kernel_1d<int> k(v.cbegin(), v.size(), 4);
    BOOST_TEST(k.center() == 4);
    BOOST_TEST(k.left_size() == 4);
    BOOST_TEST(k.right_size() == 4);
    // std::vector interface
    BOOST_TEST(k.size() == 9);
}

BOOST_AUTO_TEST_CASE(kernel_2d_parameterized_constructor_with_iterator)
{
    std::vector<int> v(81);
    gil::detail::kernel_2d<int> k(v.cbegin(), v.size(), 4, 4);
    BOOST_TEST(k.center_y() == 4);
    BOOST_TEST(k.center_x() == 4);
    BOOST_TEST(k.left_size() == 4);
    BOOST_TEST(k.right_size() == 4);
    BOOST_TEST(k.upper_size() == 4);
    BOOST_TEST(k.lower_size() == 4);
    // std::vector interface
    BOOST_TEST(k.size() == 9);
}

BOOST_AUTO_TEST_CASE(kernel_1d_copy_constructor)
{
    gil::kernel_1d<int> d(9, 4);
    gil::kernel_1d<int> k(d);
    BOOST_TEST(k.center() == 4);
    BOOST_TEST(k.center() == d.center());
    BOOST_TEST(k.left_size() == d.left_size());
    BOOST_TEST(k.right_size() == d.right_size());
    // std::vector interface
    BOOST_TEST(k.size() == d.size());
}

BOOST_AUTO_TEST_CASE(kernel_2d_copy_constructor)
{
    gil::detail::kernel_2d<int> d(9, 4, 4);
    gil::detail::kernel_2d<int> k(d);
    BOOST_TEST(k.center_y() == 4);
    BOOST_TEST(k.center_x() == 4);
    BOOST_TEST(k.center_y() == d.center_y());
    BOOST_TEST(k.center_x() == d.center_x());
    BOOST_TEST(k.left_size() == d.left_size());
    BOOST_TEST(k.right_size() == d.right_size());
    BOOST_TEST(k.lower_size() == d.lower_size());
    BOOST_TEST(k.upper_size() == d.upper_size());
    // std::vector interface
    BOOST_TEST(k.size() == d.size());
}

BOOST_AUTO_TEST_CASE(kernel_1d_assignment_operator)
{
    gil::kernel_1d<int> d(9, 4);
    gil::kernel_1d<int> k;
    k = d;
    BOOST_TEST(k.center() == 4);
    BOOST_TEST(k.center() == d.center());
    BOOST_TEST(k.left_size() == d.left_size());
    BOOST_TEST(k.right_size() == d.right_size());
    // std::vector interface
    BOOST_TEST(k.size() == d.size());
}

BOOST_AUTO_TEST_CASE(kernel_2d_assignment_operator)
{
    gil::detail::kernel_2d<int> d(9, 4, 4);
    gil::detail::kernel_2d<int> k;
    k = d;
    BOOST_TEST(k.center_y() == 4);
    BOOST_TEST(k.center_x() == 4);
    BOOST_TEST(k.center_y() == d.center_y());
    BOOST_TEST(k.center_x() == d.center_x());
    BOOST_TEST(k.left_size() == d.left_size());
    BOOST_TEST(k.right_size() == d.right_size());
    BOOST_TEST(k.lower_size() == d.lower_size());
    BOOST_TEST(k.upper_size() == d.upper_size());
    // std::vector interface
    BOOST_TEST(k.size() == d.size());
}

BOOST_AUTO_TEST_CASE(kernel_1d_reverse_Kernel)
{
    gil::kernel_1d<int> d(12, 4);
    BOOST_TEST(d.center() == 4);
    auto k = gil::reverse_kernel(d);
    BOOST_TEST(k.center() == d.right_size());
    // std::vector interface
    BOOST_TEST(k.size() == d.size());
}

BOOST_AUTO_TEST_CASE(kernel_1d_fixed_default_constructor)
{
    gil::kernel_1d_fixed<int, 9> k;
    BOOST_TEST(k.center() == 0);
    BOOST_TEST(k.left_size() == 0);
    BOOST_TEST(k.right_size() == 8); // TODO: Why not 0 or -1 if not set?
    // std::array interface
    BOOST_TEST(k.size() == 9);
}

BOOST_AUTO_TEST_CASE(kernel_2d_fixed_default_constructor)
{
    gil::detail::kernel_2d_fixed<int, 9> k;
    BOOST_TEST(k.center_x() == 0);
    BOOST_TEST(k.center_y() == 0);
    BOOST_TEST(k.left_size() == 0);
    BOOST_TEST(k.right_size() == 8); // TODO: Why not 0 or -1 if not set?
    BOOST_TEST(k.upper_size() == 0);
    BOOST_TEST(k.lower_size() == 8);
    // std::array interface
    BOOST_TEST(k.size() == 9);
}

BOOST_AUTO_TEST_CASE(kernel_1d_fixed_parameterized_constructor)
{
    gil::kernel_1d_fixed<int, 9> k(4);
    BOOST_TEST(k.center() == 4);
    BOOST_TEST(k.left_size() == 4);
    BOOST_TEST(k.right_size() == 4);
    // std::vector interface
    BOOST_TEST(k.size() == 9);
}

BOOST_AUTO_TEST_CASE(kernel_2d_fixed_parameterized_constructor)
{
    gil::detail::kernel_2d_fixed<int, 9> k(4, 4);
    BOOST_TEST(k.center_x() == 4);
    BOOST_TEST(k.center_y() == 4);
    BOOST_TEST(k.left_size() == 4);
    BOOST_TEST(k.right_size() == 4);
    BOOST_TEST(k.upper_size() == 4);
    BOOST_TEST(k.lower_size() == 4);
    // std::vector interface
    BOOST_TEST(k.size() == 9);
}

BOOST_AUTO_TEST_CASE(kernel_1d_fixed_parameterized_constructor_with_iterator)
{
    // FIXME: The constructor should throw if v.size() < k.size()
    std::vector<int> v(9);
    gil::kernel_1d_fixed<int, 9> k(v.cbegin(), 4);
    BOOST_TEST((gil::kernel_1d_fixed<int, 9>::static_size) == 9);
    BOOST_TEST(k.center() == 4);
    BOOST_TEST(k.left_size() == 4);
    BOOST_TEST(k.right_size() == 4);
    // std::vector interface
    BOOST_TEST(k.size() == 9);
}

BOOST_AUTO_TEST_CASE(kernel_2d_fixed_parameterized_constructor_with_iterator)
{
//    // FIXME: The constructor should throw if v.size() < k.size()
    std::array<int, 81> v;
    gil::detail::kernel_2d_fixed<int, 9> k(v.cbegin(), 4, 4);
    BOOST_TEST((gil::detail::kernel_2d_fixed<int, 9>::static_size) == 9);
    BOOST_TEST(k.center_y() == 4);
    BOOST_TEST(k.center_x() == 4);
    BOOST_TEST(k.left_size() == 4);
    BOOST_TEST(k.right_size() == 4);
    BOOST_TEST(k.upper_size() == 4);
    BOOST_TEST(k.lower_size() == 4);
    // std::vector interface
    BOOST_TEST(k.size() == 9);
}

BOOST_AUTO_TEST_CASE(kernel_1d_fixed_copy_constructor)
{
    gil::kernel_1d_fixed<int, 9> d(4);
    gil::kernel_1d_fixed<int, 9> k(d);
    BOOST_TEST((gil::kernel_1d_fixed<int, 9>::static_size) == 9);
    BOOST_TEST(k.center() == 4);
    BOOST_TEST(k.center() == d.center());
    BOOST_TEST(k.left_size() == d.left_size());
    BOOST_TEST(k.right_size() == d.right_size());
    // std::vector interface
    BOOST_TEST(k.size() == d.size());
}

BOOST_AUTO_TEST_CASE(kernel_2d_fixed_copy_constructor)
{
    gil::detail::kernel_2d_fixed<int, 9> d(4, 4);
    gil::detail::kernel_2d_fixed<int, 9> k(d);
    BOOST_TEST((gil::detail::kernel_2d_fixed<int, 9>::static_size) == 9);
    BOOST_TEST(k.center_x() == 4);
    BOOST_TEST(k.center_y() == 4);
    BOOST_TEST(k.center_x() == d.center_x());
    BOOST_TEST(k.center_y() == d.center_y());
    BOOST_TEST(k.left_size() == d.left_size());
    BOOST_TEST(k.right_size() == d.right_size());
    BOOST_TEST(k.lower_size() == d.lower_size());
    BOOST_TEST(k.upper_size() == d.upper_size());
    // std::vector interface
    BOOST_TEST(k.size() == d.size());
}

BOOST_AUTO_TEST_CASE(kernel_1d_fixed_assignment_operator)
{
    gil::kernel_1d_fixed<int, 9> d(4);
    gil::kernel_1d_fixed<int, 9> k;
    k = d;
    BOOST_TEST((gil::kernel_1d_fixed<int, 9>::static_size) == 9);
    BOOST_TEST(k.center() == 4);
    BOOST_TEST(k.center() == d.center());
    BOOST_TEST(k.left_size() == d.left_size());
    BOOST_TEST(k.right_size() == d.right_size());
    // std::vector interface
    BOOST_TEST(k.size() == d.size());
}

BOOST_AUTO_TEST_CASE(kernel_2d_fixed_assignment_operator)
{
    gil::detail::kernel_2d_fixed<int, 9> d(4, 4);
    gil::detail::kernel_2d_fixed<int, 9> k;
    k = d;
    BOOST_TEST((gil::detail::kernel_2d_fixed<int, 9>::static_size) == 9);
    BOOST_TEST(k.center_x() == 4);
    BOOST_TEST(k.center_y() == 4);
    BOOST_TEST(k.center_x() == d.center_x());
    BOOST_TEST(k.center_y() == d.center_y());
    BOOST_TEST(k.left_size() == d.left_size());
    BOOST_TEST(k.right_size() == d.right_size());
    BOOST_TEST(k.lower_size() == d.lower_size());
    BOOST_TEST(k.upper_size() == d.upper_size());
    // std::vector interface
    BOOST_TEST(k.size() == d.size());
}

BOOST_AUTO_TEST_CASE(kernel_1d_fixed_reverse_Kernel)
{
    std::array<int, 3> values = {1, 2, 3};
    gil::kernel_1d_fixed<int, 3> d(values.begin(), 1);
    BOOST_TEST((gil::kernel_1d_fixed<int, 3>::static_size) == 3);
    BOOST_TEST(d == values);

    std::array<int, 3> values_rev = {3, 2, 1};
    auto k = gil::reverse_kernel(d);
    BOOST_TEST(k == values_rev);
}


#include "WikiSort.h"
#include <iostream>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include <fstream>
#include <sstream>
#include <string>
#include <boost/algorithm/string.hpp>
#include <iomanip>
#include <map>
    // Nick
    // George
    // Arthur
    // NGA

using namespace std;

WikiSort::WikiSort(): grandCount(0), nonStopCount(0), stopCount(0), numTitles(0) {

}
void WikiSort::analyze(string f) {

	fileName=f;
	readFile();
	reportAlpha();
	reportStop();
    reportTop();

    /*
	int uniqueNonStop = 0;
	int uniqueStop = 0;
    for (unordered_map<string, vector<int>>::iterator it=wordAcc.begin(); it != wordAcc.end(); it++) {
    	cout << setw(15) << it->first << "~ ";
    	int topFive = grandCount * .05;
    	if (isStop(it->first)) {
    		stopCount+= sumVec(it->second);
    		uniqueStop++;
    	}
    	else {
    		nonStopCount+= sumVec(it->second);
    		uniqueNonStop++;
    	}
    	for (int n = 0; n < it->second.size(); n++)
	    	cout << titles[n] << ":" << it->second[n] << " | ";

	    cout << endl << endl;
    }
//    for (unordered_set<string>::iterator it=stopWords.begin(); it != stopWords.end(); it++)
    //	cout << *it << endl;

    cout << "Grand Count: " << grandCount << " Stop Count: " << stopCount << " Non-Stop Count: " << nonStopCount << endl;
    cout << "Unique stop count: " << uniqueStop << " Unique non-stop count: " << uniqueNonStop << endl;*/

}
int WikiSort::sumVec(vector<int> v) {
	int total = 0;
	for (vector<int>::iterator it = v.begin(); it != v.end(); it++) total += *it;
	return total;
}

bool WikiSort::isStop(string s) {
	boost::algorithm::to_lower(s);
    unordered_set<string>::iterator element = stopWords.find(s);
    /*if (element != stopWords.end()) {
    	stopCount++;
    	return true;
    }
    else return false;*/
    return element != stopWords.end();
}
int WikiSort::vecMax(vector<int> a) {
	int max=0;
	for (int i = 0; i < a.size(); i++) if (a[i] > a[max]) max = i;
	return max;
}
void WikiSort::reportTop(){
	vector<string> values;
	int topFive = grandCount * 0.05;
	//cout << "top five: " << topFive << endl;
	int oldMax = -1;
	int maxVal=-1;
	string maxWord="";
	ofstream out("report.txt");
	vector<int> keepVec;
	for (int i = 0; i < topFive; i++) {
		if (wordAcc.empty()) break;
		maxVal=0;
		for (const auto& kv : wordAcc) {
			if (!isStop(kv.first)) {
				if (sumVec(kv.second) > maxVal) {
					maxVal = sumVec(kv.second);
					maxWord = kv.first;
					keepVec = kv.second;
				}
			}
		}
		wordAcc.erase(maxWord);
		oldMax=maxVal;
		out << maxWord << ", " << maxVal << ", " << titles[vecMax(keepVec)] << endl;
		//cout << "Top:" << i << " Count:" << maxVal << " Word:<" << maxWord << ">" << endl;
	}
	bool tied = true;
	string oldWord = maxWord;
	while (tied) {

		for (const auto& kv : wordAcc) {
			if (!isStop(kv.first)) {
				if (sumVec(kv.second) == maxVal) {
					maxVal = sumVec(kv.second);
					maxWord = kv.first;
					keepVec = kv.second;
				}
			}
		}
		if (oldWord == maxWord) tied=false;
		else {
			wordAcc.erase(maxWord);
			oldWord=maxWord;
			out << maxWord << ", " << maxVal << ", " << titles[vecMax(keepVec)] << endl;
			//cout << "Count:" << maxVal << " Word:<" << maxWord << ">" << endl;
		}
	}
}
void WikiSort::reportAlpha() {
	// put keys into a vector for sorting
	for(auto wordPair: wordAcc) {
		alphabeticKeys.push_back(wordPair.first);
	}
	sort(alphabeticKeys.begin(), alphabeticKeys.end());
	ofstream sortedFile("sorted_words.txt");
	for (auto key : alphabeticKeys) {
		sortedFile << key << " " << sumVec(wordAcc[key]) << endl;
	}

}
void WikiSort::reportStop() {
	float total = 0;
	float stop = 0;
	for (auto wordPair : wordAcc) {
		if (isStop(wordPair.first)) stop+= sumVec(wordPair.second);
		total+=sumVec(wordPair.second);
	}
	cout << (stop/total) * 100 << "% stop words" << endl;
}
void WikiSort::insertWord(string s, int n) {
	// insert string s into wordAcc and increment position n in wordAcc int vector
	unordered_map<string, vector<int>>::iterator it;
	it = wordAcc.find(s);
	if (it != wordAcc.end()) {
		//if (it->second.size() == n) it->second.push_back(1);
		if (it->second.size() <= n) {
			for (int i = 0; i < (it->second.size()-n); i++) it->second.push_back(0);
			it->second.push_back(1);
		}
		else (it->second)[n]++;
	}
	else {
		// insert a pair with string s and a vector	with size one holding one one
		//wordAcc.insert(pair<string, vector<int>>(s, vector<int>(1,1)));
		pair<string, vector<int>> emptyPair(s, vector<int>(numTitles,0));
		wordAcc.insert(emptyPair);
		insertWord(s,n);
		grandCount++;
	}
}

void WikiSort::readFile() {
    ifstream input(fileName);
    stringstream words;
    string line;
    string word;
    while (getline(input, line)) {
    	words.clear();
    	words.str(line);
    	while (words >> word)
	        articleVec.push_back(word);
    }
    input.close();
    input.open("stop.txt");
    while (getline(input, line)) {
    	stopWords.emplace(line);
    }
   stripRetrieve();
}

string noPunc(string word) {
    // remove punctuation, return new string.  recursive, also make lowercase
    if (ispunct(word.front()))
        return noPunc(word.substr(1, word.size()-1));
    else if (ispunct(word.back()))
        return noPunc(word.substr(0, word.size()-1));
    else return word;
}
void WikiSort::stripRetrieve() {
    // handle titles separately
    stringstream returnString;
    stringstream titleString;
    string iteratorBugFix;
    for (vector<string>::iterator it=articleVec.begin(); it != articleVec.end(); it++)
    {
        // iterating through vector containing words in article
        iteratorBugFix = *it;
        if (iteratorBugFix.size() > 0 && iteratorBugFix.at(0) == '<' && iteratorBugFix == "<TITLE>")
        {
               // found title
                it++;    // next word
                numTitles++;
                titleString.str("");
                string s = *it;
                int count = 0;
                while (s != "<\\TITLE>")//s.compare("</TITLE>") == 0)
                {
                    titleString << s << " ";
                    it++;
                    s=*it;
                }
//                titleString
                // title text has been iterated through
                titles.push_back(titleString.str().erase(titleString.str().size()-1)); // next iteration starts body of article

        }
        else
        {
        	// body text
        	string word=*it;
        	word = noPunc(word);
        	if (word.size() > 0) insertWord(word, numTitles-1);
        }
    }
}