#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>

TEST(MathFunctions, lgamma) { EXPECT_TRUE(stan::math::is_inf(lgamma(0.0))); }

TEST(MathFunctions, lgammaStanMathUsing) { using stan::math::lgamma; }

TEST(MathFunctions, lgamma_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();
  EXPECT_PRED1(boost::math::isnan<double>, stan::math::lgamma(nan));
  EXPECT_PRED1(boost::math::isnan<double>, stan::math::lgamma(0));
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/ext/std/integral_constant.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/logical.hpp>

#include <type_traits>
using namespace boost::hana;

int main() {
    auto ints = make<Tuple>(
        std::integral_constant<int, -10>{},
        std::integral_constant<int, -2>{},
        std::integral_constant<int, 0>{},
        std::integral_constant<int, 1>{},
        std::integral_constant<int, 3>{}
    );

    auto bools = make<Tuple>(std::true_type{}, std::false_type{});

    //////////////////////////////////////////////////////////////////////////
    // Logical
    //////////////////////////////////////////////////////////////////////////
    {
        auto t = test::ct_eq<3>{};
        auto e = test::ct_eq<4>{};

        // eval_if
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                eval_if(std::true_type{}, always(t), always(e)),
                t
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                eval_if(std::false_type{}, always(t), always(e)),
                e
            ));
        }

        // not_
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                not_(std::true_type{}),
                std::false_type{}
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                not_(std::false_type{}),
                std::true_type{}
            ));
        }

        // laws
        test::TestLogical<ext::std::IntegralConstant<int>>{ints};
        test::TestLogical<ext::std::IntegralConstant<bool>>{bools};
    }
}


/*
    Copyright (c) 2011-2014 University of Zurich

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in
    all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
    THE SOFTWARE.
*/

#include <plll/arithmetic.hpp>
#include "profiling.hpp"
#include <vector>
#include <sstream>
#include <boost/math/distributions/chi_squared.hpp>

using namespace plll;

std::string binary(const arithmetic::Integer & I)
{
    std::ostringstream ss;
    if (sign(I) < 0)
        ss << '-';
    if (isZero(I))
        ss << '0';
    else
    {
        for (int b = bitLength(I) - 1; b >= 0; --b)
            ss << (bit(I, b) ? "1" : "0");
    }
    return ss.str();
}

const char * RED = "\033[1;31m";
const char * DARKRED = "\033[0;31m";
const char * YELLOW = "\033[1;33m";
const char * DARKYELLOW = "\033[0;33m";
const char * GREEN = "\033[1;32m";
const char * DARKGREEN = "\033[0;32m";
const char * NORMAL = "\033[0m";

void test1part(arithmetic::RandomNumberGenerator & rng, unsigned long n)
{
    unsigned NN = n <= 10 ? 2500 + n * 250 : n <= 100 ? 500 : 100;
    signed long max = -1;
    std::vector<unsigned> pvalcnt(10);
    std::vector<unsigned long> gcnt(n);
    for (unsigned ii = 0; ii < NN; ++ii)
    {
        unsigned N = n <= 10 ? 5000 + 500 * n : n <= 100 ? 500 + 250 * n : 50 * n;
        std::vector<unsigned long> cnt(n);
        for (unsigned i = 0; i < N; ++i)
        {
            unsigned long x = rng.random((unsigned long)n);
            assert(x < n);
            ++cnt[x]; ++gcnt[x];
            if ((signed long)x > max)
                max = x;
        }
        if (n > 1)
        {
            double chis = 0.0;
            for (unsigned i = 0; i < n; ++i)
            {
                double s = (double)cnt[i] - (double)N / (double)n;
                chis += s * s / ((double)N / (double)n);
            }
            boost::math::chi_squared_distribution<double> dist = boost::math::chi_squared_distribution<double>(n - 1);
            double pval = chis > 0 ? 1.0 - boost::math::cdf(dist, chis) : 1.0;
//                std::cout << "chi^2 = " << chis << ", p-value = " << pval << "\n";

            unsigned pv = (unsigned)(pval * 10);
            if (pv == 10)
                --pv;
            ++pvalcnt[pv];
        }
    }
    if ((max < (signed long)n - 1) || (max < 0))
        std::cout << max << " " << n << "\n";
    double chis = 0.0;
    for (unsigned i = 0; i < 10; ++i)
    {
        double s = (double)pvalcnt[i] - (double)NN / (double)10;
        chis += s * s / ((double)NN / (double)10);
    }
    boost::math::chi_squared_distribution<double> dist = boost::math::chi_squared_distribution<double>(9);
    double pval = chis > 0 ? 1.0 - boost::math::cdf(dist, chis) : 1.0/+0.0;
    std::cout << n << ": chi^2 = " << chis << ", p-value = ";
    if (pval < 0.001)
        std::cout << RED;
    else if (pval < 0.01)
        std::cout << YELLOW;
    else if (pval < 0.1)
        std::cout << DARKYELLOW;
    else
        std::cout << GREEN;
    std::cout << pval << NORMAL << ":\t";
    for (unsigned i = 0; i < 10; ++i)
        std::cout << " " << pvalcnt[i];
    std::cout << "\n";
//    for (unsigned i = 0; i < n; ++i)
//        std::cout << " " << gcnt[i];
//    std::cout << "\n";
}

void test1(arithmetic::RandomNumberGenerator & rng)
{
    std::cout << "TEST 1 BEGIN\n";
    for (unsigned long n = 2; n < 50; ++n)
        test1part(rng, n);
    std::cout << "TEST 1 DONE\n";
}

void test2(arithmetic::RandomNumberGenerator & rng)
{
    std::cout << "TEST 2 BEGIN\n";
    for (unsigned N = 2; N < 190; ++N)
    {
        for (unsigned i = 0; i < 10; ++i)
        {
            arithmetic::Integer x = rng.randomBits(N);
            if ((bitLength(x) > N) || (sign(x) < 0))
                std::cout << "  " << N << " " << bitLength(x)  << " " << binary(x) << "\n";
            assert(bitLength(x) <= N);
        }
    }
    std::cout << "TEST 2 DONE\n";
}

void test3(arithmetic::RandomNumberGenerator & rng)
{
    std::cout << "TEST 3 BEGIN\n";
    for (unsigned N = 2; N < 190; ++N)
    {
        for (unsigned i = 0; i < 10; ++i)
        {
            arithmetic::Integer x = rng.randomLen(N);
            if (bitLength(x) != N)
                std::cout << "  " << N << " " << bitLength(x)  << " " << binary(x) << "\n";
            assert(bitLength(x) == N);
        }
    }
    std::cout << "TEST 3 DONE\n";
}

void test4part(arithmetic::RandomNumberGenerator & rng, unsigned long n)
{
    unsigned NN = n <= 10 ? 300 : n <= 100 ? 200 : 70;
    signed long max = -1;
    std::vector<unsigned> pvalcnt(10);
    for (unsigned ii = 0; ii < NN; ++ii)
    {
        unsigned N = n <= 10 ? 1000 + 100 * n : n <= 100 ? 1000 + 50 * n : 100 * n;
        std::vector<unsigned long> cnt(n);
        for (unsigned i = 0; i < N; ++i)
        {
            unsigned long x = arithmetic::convert<long>(rng.random(arithmetic::convert<arithmetic::Integer>(n)));
            assert(x < n);
            ++cnt[x];
            if ((signed long)x > max)
                max = x;
        }
        if (n > 1)
        {
            double chis = 0.0;
            for (unsigned i = 0; i < n; ++i)
            {
                double s = (double)cnt[i] - (double)N / (double)n;
                chis += s * s / ((double)N / (double)n);
            }
            boost::math::chi_squared_distribution<double> dist = boost::math::chi_squared_distribution<double>(n - 1);
            double pval = chis > 0 ? 1.0 - boost::math::cdf(dist, chis) : 1.0;
//                std::cout << "chi^2 = " << chis << ", p-value = " << pval << "\n";

            unsigned pv = (unsigned)(pval * 10);
            if (pv == 10)
                --pv;
            ++pvalcnt[pv];
        }
    }
    if ((max < (signed long)n - 1) || (max < 0))
        std::cout << max << " " << n << "\n";
    double chis = 0.0;
    for (unsigned i = 0; i < 10; ++i)
    {
        double s = (double)pvalcnt[i] - (double)NN / (double)10;
        chis += s * s / ((double)NN / (double)10);
    }
    boost::math::chi_squared_distribution<double> dist = boost::math::chi_squared_distribution<double>(9);
    double pval = chis > 0 ? 1.0 - boost::math::cdf(dist, chis) : 1.0/+0.0;
    std::cout << n << ": chi^2 = " << chis << ", p-value = ";
    if (pval < 0.001)
        std::cout << RED;
    else if (pval < 0.01)
        std::cout << YELLOW;
    else if (pval < 0.1)
        std::cout << DARKYELLOW;
    else
        std::cout << GREEN;
    std::cout << pval << NORMAL << ":\t";
    for (unsigned i = 0; i < 10; ++i)
        std::cout << " " << pvalcnt[i];
    std::cout << "\n";
}

void test4(arithmetic::RandomNumberGenerator & rng)
{
    std::cout << "TEST 4 BEGIN\n";
    for (unsigned long n = 2; n < 50; ++n)
        test4part(rng, n);
    std::cout << "TEST 4 DONE\n";
}

void test5(arithmetic::RandomNumberGenerator & rng)
{
}

void test6(arithmetic::RandomNumberGenerator & rng)
{
}

void test7(arithmetic::RandomNumberGenerator & rng)
{
}

void test8(arithmetic::RandomNumberGenerator & rng)
{
}

int main(int argc, char **argv)
{
    arithmetic::initArithmeticThreadAllocators();

    arithmetic::RandomNumberGenerator rng;
//    rng.randomizeTime();
    rng.randomizeDevRandom();

    if (argc > 1)
    {
        int n = atoi(argv[1]);
        if (n < 2)
            n = 2;
        for (int i = 0; i < 10; ++i)
            test1part(rng, n);
        return 0;
    }

    test1(rng);
    std::cout << "-----------------------------------------------------------\n";
//    test2(rng);
    std::cout << "-----------------------------------------------------------\n";
//    test3(rng);
    std::cout << "-----------------------------------------------------------\n";
    test4(rng);
    std::cout << "-----------------------------------------------------------\n";
    test5(rng);
    std::cout << "-----------------------------------------------------------\n";
    test6(rng);
    std::cout << "-----------------------------------------------------------\n";
    test7(rng);
    std::cout << "-----------------------------------------------------------\n";
    test8(rng);
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "main.h"
#include <Eigen/SVD>

template<typename MatrixType, typename JacobiScalar>
void jacobi(const MatrixType& m = MatrixType())
{
  typedef typename MatrixType::Scalar Scalar;
  typedef typename MatrixType::Index Index;
  Index rows = m.rows();
  Index cols = m.cols();

  enum {
    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
    ColsAtCompileTime = MatrixType::ColsAtCompileTime
  };

  typedef Matrix<JacobiScalar, 2, 1> JacobiVector;

  const MatrixType a(MatrixType::Random(rows, cols));

  JacobiVector v = JacobiVector::Random().normalized();
  JacobiScalar c = v.x(), s = v.y();
  JacobiRotation<JacobiScalar> rot(c, s);

  {
    Index p = internal::random<Index>(0, rows-1);
    Index q;
    do {
      q = internal::random<Index>(0, rows-1);
    } while (q == p);

    MatrixType b = a;
    b.applyOnTheLeft(p, q, rot);
    VERIFY_IS_APPROX(b.row(p), c * a.row(p) + internal::conj(s) * a.row(q));
    VERIFY_IS_APPROX(b.row(q), -s * a.row(p) + internal::conj(c) * a.row(q));
  }

  {
    Index p = internal::random<Index>(0, cols-1);
    Index q;
    do {
      q = internal::random<Index>(0, cols-1);
    } while (q == p);

    MatrixType b = a;
    b.applyOnTheRight(p, q, rot);
    VERIFY_IS_APPROX(b.col(p), c * a.col(p) - s * a.col(q));
    VERIFY_IS_APPROX(b.col(q), internal::conj(s) * a.col(p) + internal::conj(c) * a.col(q));
  }
}

void test_jacobi()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1(( jacobi<Matrix3f, float>() ));
    CALL_SUBTEST_2(( jacobi<Matrix4d, double>() ));
    CALL_SUBTEST_3(( jacobi<Matrix4cf, float>() ));
    CALL_SUBTEST_3(( jacobi<Matrix4cf, std::complex<float> >() ));

    int r = internal::random<int>(2, internal::random<int>(1,EIGEN_TEST_MAX_SIZE)/2),
        c = internal::random<int>(2, internal::random<int>(1,EIGEN_TEST_MAX_SIZE)/2);
    CALL_SUBTEST_4(( jacobi<MatrixXf, float>(MatrixXf(r,c)) ));
    CALL_SUBTEST_5(( jacobi<MatrixXcd, double>(MatrixXcd(r,c)) ));
    CALL_SUBTEST_5(( jacobi<MatrixXcd, std::complex<double> >(MatrixXcd(r,c)) ));
    // complex<float> is really important to test as it is the only way to cover conjugation issues in certain unaligned paths
    CALL_SUBTEST_6(( jacobi<MatrixXcf, float>(MatrixXcf(r,c)) ));
    CALL_SUBTEST_6(( jacobi<MatrixXcf, std::complex<float> >(MatrixXcf(r,c)) ));
    (void) r;
    (void) c;
  }
}


#include "matching-based-loopclosure/train-vocabulary.h"

#include <algorithm>
#include <cstdio>
#include <vector>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <descriptor-projection/descriptor-projection.h>
#include <descriptor-projection/map-track-extractor.h>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <loopclosure-common/types.h>
#include <vi-map/vi-map.h>
#include <vocabulary-tree/tree-builder.h>

#include "matching-based-loopclosure/detector-settings.h"
#include "matching-based-loopclosure/inverted-index-interface.h"
#include "matching-based-loopclosure/inverted-multi-index-interface.h"

DECLARE_string(data_directory);
DEFINE_int32(
    lc_number_of_vocabulary_words, 1000, "Number of words in the vocabulary.");
DEFINE_int32(
    lc_num_descriptors_to_train, 100000,
    "Number of descriptors used for training.");
DEFINE_int32(
    lc_product_quantization_num_components, 1,
    "Number of components for product quantization.");
DEFINE_int32(
    lc_product_quantization_num_dim_per_component, 5,
    "Number of components for product quantization.");
DEFINE_int32(
    lc_product_quantization_num_words, 256,
    "Number of words in the product vocabulary.");

DECLARE_string(load_map);

using descriptor_projection::DescriptorVector;
using descriptor_projection::FeatureAllocator;
using descriptor_projection::ProjectedDescriptorType;

namespace loop_closure {
typedef loop_closure::TreeBuilder<
    ProjectedDescriptorType,
    loop_closure::distance::L2<ProjectedDescriptorType>, FeatureAllocator>
    ProjectedTreeBuilder;

void MakeVocabulary(
    int num_words, const DescriptorVector& descriptors,
    int descriptor_dimensionality, Eigen::MatrixXf* words) {
  CHECK_NOTNULL(words);
  CHECK(!descriptors.empty());

  ProjectedDescriptorType descriptor_zero;
  descriptor_zero.setConstant(descriptor_dimensionality, 1, 0);

  // Create tree.
  static constexpr int kLevels = 1;
  ProjectedTreeBuilder builder(descriptor_zero);
  builder.kmeans().SetRestarts(1);
  builder.Build(descriptors, num_words, kLevels);
  VLOG(3) << "Done. Got " << builder.tree().centers().size() << " centers";

  const descriptor_projection::DescriptorVector& centers =
      builder.tree().centers();
  words->resize(descriptor_dimensionality, centers.size());
  for (size_t i = 0; i < centers.size(); ++i) {
    CHECK_EQ(centers[i].rows(), descriptor_dimensionality);
    words->block(0, i, descriptor_dimensionality, 1) = centers[i];
  }
}

void LoadBinaryFeaturesFromDataset(
    const vi_map::VIMap& map, loop_closure::DescriptorContainer* descriptors) {
  CHECK_NOTNULL(descriptors);

  // Get the descriptor-length.
  unsigned int descriptor_size = -1;
  unsigned int raw_descriptor_matching_threshold = 70;
  if (FLAGS_feature_descriptor_type == loop_closure::kFeatureDescriptorFREAK) {
    descriptor_size = loop_closure::kFreakDescriptorLengthBits;
  } else if (
      FLAGS_feature_descriptor_type == loop_closure::kFeatureDescriptorBRISK) {
    descriptor_size = loop_closure::kBriskDescriptorLengthBits;
  } else {
    CHECK(false) << "Unknown feature descriptor "
                 << FLAGS_feature_descriptor_type;
  }

  CHECK_NOTNULL(descriptors);

  vi_map::MissionIdList all_mission_ids;
  map.getAllMissionIds(&all_mission_ids);

  loop_closure::DescriptorContainer all_descriptors;
  for (const vi_map::MissionId& mission_id : all_mission_ids) {
    std::vector<descriptor_projection::Track> tracks;

    loop_closure::DescriptorContainer mission_descriptors;
    using descriptor_projection::CollectAndConvertDescriptors;
    CollectAndConvertDescriptors(
        map, mission_id, descriptor_size, raw_descriptor_matching_threshold,
        &mission_descriptors, &tracks);
    int old_size = all_descriptors.cols();
    all_descriptors.conservativeResize(
        Eigen::NoChange, old_size + mission_descriptors.cols());
    all_descriptors.block(
        0, old_size, mission_descriptors.rows(), mission_descriptors.cols()) =
        mission_descriptors;
    if (all_descriptors.cols() >= FLAGS_lc_num_descriptors_to_train) {
      break;
    }
  }

  if (all_descriptors.cols() > FLAGS_lc_num_descriptors_to_train) {
    LOG(WARNING) << "Truncated number of descriptors "
                 << FLAGS_lc_num_descriptors_to_train;
    all_descriptors.conservativeResize(
        Eigen::NoChange, FLAGS_lc_num_descriptors_to_train);
  }
}

void ProjectDescriptors(
    const Eigen::MatrixXf& projection_matrix,
    const loop_closure::DescriptorContainer& descriptors,
    DescriptorVector* projected_descriptors) {
  CHECK_NOTNULL(projected_descriptors);
  VLOG(3) << "Got " << descriptors.cols()
          << " descriptors to train the vocabulary tree from.";

  CHECK_NE(descriptors.cols(), 0);

  ProjectedDescriptorType descriptor_zero;
  descriptor_zero.setConstant(FLAGS_lc_target_dimensionality, 1, 0);

  projected_descriptors->resize(descriptors.cols(), descriptor_zero);
  for (int i = 0; i < descriptors.cols(); ++i) {
    aslam::common::FeatureDescriptorConstRef raw_descriptor(
        &descriptors.coeffRef(0, i), descriptors.rows());
    descriptor_projection::ProjectDescriptor(
        raw_descriptor, projection_matrix, FLAGS_lc_target_dimensionality,
        (*projected_descriptors)[i]);
    if (i % 10000 == 0) {
      LOG(INFO) << "Projected " << i << "/" << descriptors.cols();
    }
  }
}

void MakeProductVocabularies(
    const DescriptorVector& input_descriptors,
    const Eigen::MatrixXf& base_vocabulary,
    Eigen::MatrixXf* product_vocabulary) {
  CHECK_NOTNULL(product_vocabulary);
  Aligned<std::vector, DescriptorVector> residuals;
  residuals.resize(base_vocabulary.cols());

  for (const ProjectedDescriptorType& projected_descriptor :
       input_descriptors) {
    int best_word = 0;
    float best_distance = std::numeric_limits<float>::max();
    for (int i = 0; i < base_vocabulary.cols(); ++i) {
      float distance =
          (base_vocabulary.col(i) - projected_descriptor).squaredNorm();
      if (distance < best_distance) {
        best_distance = distance;
        best_word = i;
      }
    }
    // Calculate the residual w.r.t. to the closest vocabulary word.
    ProjectedDescriptorType descriptor_residual =
        projected_descriptor - base_vocabulary.col(best_word);
    residuals[best_word].push_back(descriptor_residual);
  }

  const int kHalfDescriptorLength = FLAGS_lc_target_dimensionality / 2;
  CHECK_LE(
      FLAGS_lc_product_quantization_num_components *
          FLAGS_lc_product_quantization_num_dim_per_component,
      kHalfDescriptorLength);

  // Now train kNumComponents k-means on the descriptors of every word.
  // Split up dimensions here and then run k-means on them.
  const int num_imi_words = base_vocabulary.cols();
  const int num_pq_words = FLAGS_lc_product_quantization_num_words;
  const int num_components = FLAGS_lc_product_quantization_num_components;
  const int num_dim_per_component =
      FLAGS_lc_product_quantization_num_dim_per_component;
  product_vocabulary->resize(
      num_dim_per_component, num_imi_words * num_components * num_pq_words);

  int num_components_stored = 0;
  const unsigned int num_descriptors_per_training = num_pq_words * 200u;

  for (const DescriptorVector& word_descriptors : residuals) {
    for (int component = 0;
         component < FLAGS_lc_product_quantization_num_components;
         ++component) {
      LOG(INFO) << "Training component " << num_components_stored << "/"
                << num_imi_words * num_components;
      DescriptorVector dim_for_component;
      dim_for_component.reserve(word_descriptors.size());
      const int start_block =
          component * FLAGS_lc_product_quantization_num_dim_per_component;
      const int block_size =
          FLAGS_lc_product_quantization_num_dim_per_component;
      for (const ProjectedDescriptorType& descriptor : word_descriptors) {
        ProjectedDescriptorType sub_descriptor =
            descriptor.block(start_block, 0, block_size, 1);
        dim_for_component.push_back(sub_descriptor);
        if (dim_for_component.size() >= num_descriptors_per_training) {
          break;
        }
      }
      LOG(INFO) << "Using " << dim_for_component.size() << " descriptors.";
      Eigen::MatrixXf words_product_vocabulary;
      MakeVocabulary(
          FLAGS_lc_product_quantization_num_words, dim_for_component,
          block_size, &words_product_vocabulary);
      // Now store product vocabulary for component.
      CHECK_LE(
          num_components_stored * num_pq_words + num_pq_words,
          product_vocabulary->cols());
      product_vocabulary->block(
          0, num_components_stored * num_pq_words, num_dim_per_component,
          num_pq_words) = words_product_vocabulary;
      ++num_components_stored;
    }
  }
  LOG(INFO) << "Done with product vocabulary";
}

void MakeVocabularies(
    const Eigen::MatrixXf& projection_matrix,
    const DescriptorVector& projected_descriptors) {
  VLOG(3) << "Creating vocabulary with " << FLAGS_lc_number_of_vocabulary_words
          << " words.";
  VLOG(3) << "Got " << projected_descriptors.size() << " descriptors.";

  const matching_based_loopclosure::MatchingBasedEngineSettings
      matching_based_settings;

  typedef matching_based_loopclosure::MatchingBasedEngineSettings::
      DetectorEngineType DetectorEngineType;

  if (matching_based_settings.detector_engine_type ==
          DetectorEngineType::kMatchingLDInvertedMultiIndex ||
      matching_based_settings.detector_engine_type ==
          DetectorEngineType::
              kMatchingLDInvertedMultiIndexProductQuantization) {
    CHECK_EQ(FLAGS_lc_target_dimensionality % 2, 0)
        << "Number of dimensions must be divisible by two.";
    const int kHalfDescriptorLength = FLAGS_lc_target_dimensionality / 2;
    // Split the descriptors in half:
    DescriptorVector projected_descriptors_first_half;
    DescriptorVector projected_descriptors_second_half;
    projected_descriptors_first_half.reserve(projected_descriptors.size());
    projected_descriptors_second_half.reserve(projected_descriptors.size());
    for (size_t i = 0; i < projected_descriptors.size(); ++i) {
      const ProjectedDescriptorType& projected_descriptor =
          projected_descriptors[i];
      ProjectedDescriptorType first_half =
          projected_descriptor.block(0, 0, kHalfDescriptorLength, 1);
      projected_descriptors_first_half.push_back(first_half);

      ProjectedDescriptorType second_half = projected_descriptor.block(
          kHalfDescriptorLength, 0, kHalfDescriptorLength, 1);
      projected_descriptors_second_half.push_back(second_half);
    }

    if (matching_based_settings.detector_engine_type ==
        DetectorEngineType::kMatchingLDInvertedMultiIndex) {
      loop_closure::InvertedMultiIndexVocabulary vocabulary;
      vocabulary.projection_matrix_ = projection_matrix;
      vocabulary.target_dimensionality_ = FLAGS_lc_target_dimensionality;

      LOG(INFO) << "Creating first vocabulary.";
      MakeVocabulary(
          FLAGS_lc_number_of_vocabulary_words, projected_descriptors_first_half,
          kHalfDescriptorLength, &vocabulary.words_first_half_);

      LOG(INFO) << "Creating second vocabulary.";
      MakeVocabulary(
          FLAGS_lc_number_of_vocabulary_words,
          projected_descriptors_second_half, kHalfDescriptorLength,
          &vocabulary.words_second_half_);

      std::ofstream out(
          FLAGS_lc_projected_quantizer_filename.c_str(), std::ios_base::binary);
      CHECK(out.is_open()) << "Failed to write quantizer file to "
                           << FLAGS_lc_projected_quantizer_filename;

      vocabulary.Save(&out);
    } else if (
        matching_based_settings.detector_engine_type ==
        DetectorEngineType::kMatchingLDInvertedMultiIndexProductQuantization) {
      // Trains a product quantizer from a given set of data points, resulting
      // in a set of cluster centers that can be passed onto an instance of
      // ProductQuantization using the SetClusterCenters function.
      // The template parameters are the number of components to be used for
      // product
      // quantization, the number of dimensions for each component, and the
      // number of
      // cluster centers.

      loop_closure::InvertedMultiIndexProductVocabulary vocabulary;
      vocabulary.projection_matrix_ = projection_matrix;
      vocabulary.target_dimensionality_ = FLAGS_lc_target_dimensionality;

      LOG(INFO) << "Creating first vocabulary.";
      MakeVocabulary(
          FLAGS_lc_number_of_vocabulary_words, projected_descriptors_first_half,
          kHalfDescriptorLength, &vocabulary.words_first_half_);

      LOG(INFO) << "Creating second vocabulary.";
      MakeVocabulary(
          FLAGS_lc_number_of_vocabulary_words,
          projected_descriptors_second_half, kHalfDescriptorLength,
          &vocabulary.words_second_half_);

      vocabulary.number_of_components =
          FLAGS_lc_product_quantization_num_components;
      vocabulary.number_of_centers = FLAGS_lc_product_quantization_num_words;
      vocabulary.number_of_dimensions_per_component =
          FLAGS_lc_product_quantization_num_dim_per_component;

      MakeProductVocabularies(
          projected_descriptors_first_half, vocabulary.words_first_half_,
          &vocabulary.quantizer_centers_1);

      MakeProductVocabularies(
          projected_descriptors_second_half, vocabulary.words_second_half_,
          &vocabulary.quantizer_centers_2);

      std::ofstream out(
          FLAGS_lc_projected_quantizer_filename.c_str(), std::ios_base::binary);
      CHECK(out.is_open()) << "Failed to write quantizer file to "
                           << FLAGS_lc_projected_quantizer_filename;

      LOG(INFO) << "Storing " << FLAGS_lc_projected_quantizer_filename;
      vocabulary.Save(&out);
    } else {
      LOG(FATAL) << "Training for this type is not supported.";
    }
  } else if (
      matching_based_settings.detector_engine_type ==
      DetectorEngineType::kMatchingLDInvertedIndex) {
    loop_closure::InvertedIndexVocabulary vocabulary;
    vocabulary.projection_matrix_ = projection_matrix;
    vocabulary.target_dimensionality_ = FLAGS_lc_target_dimensionality;
    LOG(INFO) << "Creating vocabulary.";
    MakeVocabulary(
        FLAGS_lc_number_of_vocabulary_words, projected_descriptors,
        FLAGS_lc_target_dimensionality, &vocabulary.words_);
    std::ofstream out(
        FLAGS_lc_projected_quantizer_filename.c_str(), std::ios_base::binary);
    CHECK(out.is_open()) << "Failed to write quantizer file to "
                         << FLAGS_lc_projected_quantizer_filename;

    vocabulary.Save(&out);
  } else {
    LOG(FATAL) << "Training for type is not supported.";
  }
}

void TrainProjectedVocabulary(const vi_map::VIMap& map) {
  CHECK_NE(FLAGS_lc_projected_quantizer_filename, "")
      << "You have to provide a filename to write the quantizer to.";

  CHECK_NE(FLAGS_lc_projection_matrix_filename, "")
      << "You have to provide a filename for the projection matrix.";

  std::ifstream deserializer(FLAGS_lc_projection_matrix_filename);
  CHECK(deserializer.is_open()) << "Cannot load projection matrix from file: "
                                << FLAGS_lc_projection_matrix_filename;

  Eigen::MatrixXf projection_matrix;
  common::Deserialize(&projection_matrix, &deserializer);

  CHECK_NE(0, projection_matrix.rows());

  loop_closure::DescriptorContainer raw_descriptors;
  LoadBinaryFeaturesFromDataset(map, &raw_descriptors);

  DescriptorVector projected_descriptors;
  ProjectDescriptors(
      projection_matrix, raw_descriptors, &projected_descriptors);

  MakeVocabularies(projection_matrix, projected_descriptors);
  std::cout << "Done." << std::endl;
}
}  // namespace loop_closure


#ifndef PYTHONIC_SCIPY_SPECIAL_HANKEL1_HPP
#define PYTHONIC_SCIPY_SPECIAL_HANKEL1_HPP

#include "pythonic/include/scipy/special/hankel1.hpp"

#include "pythonic/types/complex.hpp"
#include "pythonic/types/ndarray.hpp"
#include "pythonic/utils/functor.hpp"
#include "pythonic/utils/numpy_traits.hpp"

#define BOOST_MATH_THREAD_LOCAL thread_local
#include <boost/math/special_functions/hankel.hpp>

PYTHONIC_NS_BEGIN

namespace scipy
{
  namespace special
  {
    namespace details
    {
      template <class T0, class T1>
      std::complex<double> hankel1(T0 x, T1 y)
      {
        return boost::math::cyl_hankel_1(x, y);
      }
    }

#define NUMPY_NARY_FUNC_NAME hankel1
#define NUMPY_NARY_FUNC_SYM details::hankel1
#include "pythonic/types/numpy_nary_expr.hpp"
  }
}
PYTHONIC_NS_END

#endif


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#ifndef BOOST_SMALLEST_LAST_VERTEX_ORDERING_HPP
#define BOOST_SMALLEST_LAST_VERTEX_ORDERING_HPP
/*
   The smallest-last ordering is defined for the loopless graph G with
   vertices a(j), j = 1,2,...,n where a(j) is the j-th column of A and
   with edge (a(i),a(j)) if and only if columns i and j have a
   non-zero in the same row position.  The smallest-last ordering is
   determined recursively by letting list(k), k = n,...,1 be a column
   with least degree in the subgraph spanned by the un-ordered
   columns.
 */
#include <vector>
#include <algorithm>
#include <boost/graph/graph_traits.hpp>
#include <boost/pending/bucket_sorter.hpp>

namespace boost {

  template <class VertexListGraph, class Order, class Degree, class Marker>
  void
  smallest_last_vertex_ordering(const VertexListGraph& G, Order order,
                                Degree degree, Marker marker) {
    typedef typename graph_traits<VertexListGraph> GraphTraits;
    typedef typename GraphTraits::vertex_descriptor Vertex;
    //typedef typename GraphTraits::size_type size_type;
    typedef std::size_t size_type;

    const size_type num = num_vertices(G);

    typedef typename vertex_property_map<VertexListGraph, vertex_index_t>::type ID;
    typedef bucket_sorter<size_type, Vertex, Degree, ID> BucketSorter;

    BucketSorter degree_bucket_sorter(num, num, degree,
                                      get_vertex_property(G, vertex_index));

    smallest_last_vertex_ordering(G, order, degree, marker, degree_bucket_sorter);
  }

  template <class VertexListGraph, class Order, class Degree,
            class Marker, class BucketSorter>
  void
  smallest_last_vertex_ordering(const VertexListGraph& G, Order order,
                                Degree degree, Marker marker,
                                BucketSorter& degree_buckets) {
    typedef typename graph_traits<VertexListGraph> GraphTraits;
    typedef typename GraphTraits::vertex_descriptor Vertex;
    //typedef typename GraphTraits::size_type size_type;
    typedef std::size_t size_type;

    const size_type num = num_vertices(G);

    typename GraphTraits::vertex_iterator v, vend;
    for (boost::tie(v, vend) = vertices(G); v != vend; ++v) {
      put(marker, *v, num);
      put(degree, *v, out_degree(*v, G));
      degree_buckets.push(*v);
    }

    size_type minimum_degree = 1;
    size_type current_order = num - 1;

    while ( 1 ) {
      typedef typename BucketSorter::stack MDStack;
      MDStack minimum_degree_stack = degree_buckets[minimum_degree];
      while (minimum_degree_stack.empty())
        minimum_degree_stack = degree_buckets[++minimum_degree];

      Vertex node = minimum_degree_stack.top();
      put(order, current_order, node);

      if ( current_order == 0 ) //find all vertices
        break;

      minimum_degree_stack.pop();
      put(marker, node, 0); //node has been ordered.

      typename GraphTraits::adjacency_iterator v, vend;
      for (boost::tie(v,vend) = adjacent_vertices(node, G); v != vend; ++v)

        if ( get(marker,*v) > current_order ) { //*v is unordered vertex
          put(marker, *v, current_order);  //mark the columns adjacent to node

          //It is possible minimum degree goes down
          //Here we keep tracking it.
          put(degree, *v, get(degree, *v) - 1);
          minimum_degree = std::min(minimum_degree, get(degree, *v));

          //update the position of *v in the bucket sorter
          degree_buckets.update(*v);
        }

      current_order--;
    }

    //at this point, order[i] = v_i;
  }

}

#endif


/*
 * tsdf.cpp
 *
 *  Created on: Feb 1, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   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 "tsdf.hpp"

//libraries
#include <boost/python.hpp>
#include <Eigen/Dense>

//local
#include "../math/typedefs.hpp"
#include "../tsdf/ewa_viz.hpp"
#include "../tsdf/generator.hpp"
#include "../tsdf/ewa_common.hpp"
#include "../tsdf/interpolation_method.hpp"
#include "../tsdf/parameters.hpp"

namespace bp = boost::python;
namespace impl = tsdf;

namespace python_export {
namespace tsdf {
struct TSDF_scope_dummy{

};

void export_algorithms() {
	bp::scope tsdf_scope = bp::class_<TSDF_scope_dummy>("tsdf", bp::no_init);
	bp::enum_<impl::FilteringMethod>("FilteringMethod")
			.value("NONE", impl::FilteringMethod::NONE)
			.value("BILINEAR_IMAGE_SPACE", impl::FilteringMethod::BILINEAR_IMAGE_SPACE)
			.value("BILINEAR_VOXEL_SPACE", impl::FilteringMethod::BILINEAR_VOXEL_SPACE)
			.value("EWA_IMAGE_SPACE", impl::FilteringMethod::EWA_IMAGE_SPACE)
			.value("EWA_VOXEL_SPACE", impl::FilteringMethod::EWA_VOXEL_SPACE)
			.value("EWA_VOXEL_SPACE_INCLUSIVE", impl::FilteringMethod::EWA_VOXEL_SPACE_INCLUSIVE);

	bp::class_<impl::Parameters2d>("Parameters2d",
			bp::init<bp::optional<float,Eigen::Matrix3f,float,math::Vector2i, math::Vector2i, float, int,
				impl::FilteringMethod,float>>(bp::args("depth_unit_ratio","projection_matrix",
						"near_clipping_distance", "array_offset", "field_shape","voxel_size","narrow_band_width_voxels",
						"interpolation_method","smoothing_factor")))
			.def_readwrite("depth_unit_ratio", &impl::Parameters2d::depth_unit_ratio)
			.def_readwrite("projection_matrix", &impl::Parameters2d::projection_matrix)
			.def_readwrite("near_clipping_distance", &impl::Parameters2d::near_clipping_distance)
			.def_readwrite("array_offset", &impl::Parameters2d::array_offset)
			.def_readwrite("field_shape", &impl::Parameters2d::field_shape)
			.def_readwrite("voxel_size", &impl::Parameters2d::voxel_size)
			.def_readwrite("narrow_band_width_voxels", &impl::Parameters2d::narrow_band_width_voxels)
			.def_readwrite("interpolation_method", &impl::Parameters2d::interpolation_method)
			.def_readwrite("smoothing_factor", &impl::Parameters2d::smoothing_factor);

	bp::class_<impl::Parameters3d>("Parameters3d",
				bp::init<bp::optional<float,Eigen::Matrix3f,float,math::Vector3i, math::Vector3i, float, int,
					impl::FilteringMethod,float>>(bp::args("depth_unit_ratio","projection_matrix",
							"near_clipping_distance", "array_offset", "field_shape","voxel_size","narrow_band_width_voxels",
							"interpolation_method","smoothing_factor")))
				.def_readwrite("depth_unit_ratio", &impl::Parameters3d::depth_unit_ratio)
				.def_readwrite("projection_matrix", &impl::Parameters3d::projection_matrix)
				.def_readwrite("near_clipping_distance", &impl::Parameters3d::near_clipping_distance)
				.def_readwrite("array_offset", &impl::Parameters3d::array_offset)
				.def_readwrite("field_shape", &impl::Parameters3d::field_shape)
				.def_readwrite("voxel_size", &impl::Parameters3d::voxel_size)
				.def_readwrite("narrow_band_width_voxels", &impl::Parameters3d::narrow_band_width_voxels)
				.def_readwrite("interpolation_method", &impl::Parameters3d::interpolation_method)
				.def_readwrite("smoothing_factor", &impl::Parameters3d::smoothing_factor);

	bp::class_<impl::Generator2d>("Generator2d",
			bp::init<impl::Parameters2d>("parameters"))
		.def("generate", &impl::Generator2d::generate,
				"Generate a discrete implicit TSDF (Truncated Signed Distance "
				"Function) from the give depth image presumed to have been taken at the specified camera pose.",
				bp::args("depth_image","camera_pose","image_y_coordinate"));

	bp::class_<impl::Generator3d>("Generator3d",
				bp::init<impl::Parameters3d>("parameters"))
			.def("generate", &impl::Generator3d::generate,
					"Generate a discrete implicit TSDF (Truncated Signed Distance "
					"Function) from the give depth image presumed to have been taken at the specified camera pose.",
					bp::args("depth_image","camera_pose","image_y_coordinate"));

	bp::def("generate_tsdf_3d_ewa_image_visualization", &impl::generate_TSDF_3D_EWA_image_visualization,
			"Draw a visualization of voxel sampling over image space using Elliptical Weighed Average resampling approach."
					"To limit the density, only ellipses corresponding to voxels with abs(TSDF value) below the given truncation "
					"threshold will be drawn."
					"See generate_3d_TSDF_field_from_depth_image_EWA() for method description. "
					"Draws the projected ellipses corresponding to 1 standard deviation away from each voxel sampling center.",
			bp::args("depth_image",
					"depth_unit_ratio",
					"field",
					"camera_intrinsic_matrix",
					"camera_pose",
					"array_offset",
					"voxel_size",
					"scale",
					"tsdf_threshold",
					"gaussian_covariance_scale"));

	//DEBUG
	bp::def("sampling_area_heatmap_2d_ewa_image", &impl::sampling_area_heatmap_2D_EWA_image,
			"DEBUGGING PURPOSE ONLY -- compute matrix of 2D EWA sampling areas for each pixel (~2d voxel) in resulting TSDF",
			bp::args("image_y_coordinate",
					"depth_image",
					"depth_unit_ratio",
					"camera_intrinsic_matrix",
					"camera_pose",
					"array_offset",
					"field_size",
					"voxel_size",
					"narrow_band_width_voxels",
					"gaussian_covariance_scale"));
}
}  // namespace pe_tsdf
}  // namespace python_export


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/random/mersenne_twister.hpp>

#include <vector>

#include "CppProject/randomKepElem.hpp"

namespace randomKepElem
{
typedef double Real;
typedef std::vector< Real > Vector6;
typedef std::vector< Real > Vector2;
typedef std::vector< Real > Vector3;
typedef std::vector< Real > VectorOrb;
typedef std::vector< std::vector <Real > > Vector2D;
typedef boost::mt19937 generator_type; // mt19937 is a psuedo random number generator algorithm present in the boost library

//! compute random orbital elements from specific bounds
void randomKepElem( const Vector2 range_a,
					const Vector2 range_e,
					const Vector2 range_i,
					const Vector2 range_raan,
					const Vector2 range_w,
					const Vector2 range_E,
					const int limit,
					Vector2D& returnKepElem )
{
// generator type defined and initialized by a seed value
generator_type generator( 100 ); // don't change the seed value while debugging otherwise the psuedo random generated numbers will change

// define uniform random number distribution which produces type double values
// between [min,max) for each orbital element. For more details please refer to
// the following web link:
// http://www.boost.org/doc/libs/1_60_0/libs/random/example/random_demo.cpp
boost::uniform_real<> uni_dist_a( range_a[ 0 ], range_a[ 1 ] ); // distribution range for semi major axis
boost::uniform_real<> uni_dist_e( range_e[ 0 ], range_e[ 1 ] ); // distribution range for eccentricity
boost::uniform_real<> uni_dist_i( range_i[ 0 ], range_i[ 1 ] ); // distribution range for inclination
boost::uniform_real<> uni_dist_raan( range_raan[ 0 ], range_raan[ 1 ] ); // distribution range for right ascension of ascending node
boost::uniform_real<> uni_dist_w( range_w[ 0 ], range_w[ 1 ] ); // distribution range for argument of perigee
boost::uniform_real<> uni_dist_E( range_E[ 0 ], range_E[ 1 ] ); // distribution range for eccentric anomaly

boost::variate_generator<generator_type&, boost::uniform_real<> > a_uni(generator, uni_dist_a);
boost::variate_generator<generator_type&, boost::uniform_real<> > e_uni(generator, uni_dist_e);
boost::variate_generator<generator_type&, boost::uniform_real<> > i_uni(generator, uni_dist_i);
boost::variate_generator<generator_type&, boost::uniform_real<> > raan_uni(generator, uni_dist_raan);
boost::variate_generator<generator_type&, boost::uniform_real<> > w_uni(generator, uni_dist_w);
boost::variate_generator<generator_type&, boost::uniform_real<> > E_uni(generator, uni_dist_E);

// obtain random orbital elements. Random numbers are obtained from the uniform distribution
// by calling the generator function with empty arguments.
for(int i = 0; i < limit; i++)
{
	returnKepElem[ i ][ 0 ]	= a_uni();
	returnKepElem[ i ][ 1 ] = e_uni();
	returnKepElem[ i ][ 2 ] = i_uni();
	returnKepElem[ i ][ 3 ] = raan_uni();
	returnKepElem[ i ][ 4 ] = w_uni();
	returnKepElem[ i ][ 5 ] = E_uni();
}
// function ends
}

} // namespace randomKepElem


#include <cannon/geom/graph.hpp>

#include <boost/graph/astar_search.hpp>

using namespace cannon::geom;

void Graph::add_edge(Vertex u, Vertex v, double weight) {
  auto [e, added] = boost::add_edge(u, v, g_);
  boost::put(weights_, e, weight);
}

std::vector<Vertex> Graph::astar(Vertex start, Vertex goal,
                                 std::function<double(Vertex)> heur) {
  std::vector<Vertex> p(boost::num_vertices(g_));

  boost::astar_search(
      g_, start, heur,
      boost::predecessor_map(&p[0]).visitor(
          boost::default_astar_visitor()));

  std::list<Vertex> shortest_path;
  for (Vertex v = goal; ; v = p[v]) {
    shortest_path.push_front(v);
    if (p[v] == v)
      break;
  }

  return std::vector<Vertex>(shortest_path.begin(), shortest_path.end());
}


#ifndef IEOMPP_TYPES_EIGEN_MULTIPLY_ASSIGN_HPP_
#define IEOMPP_TYPES_EIGEN_MULTIPLY_ASSIGN_HPP_

#include "ieompp/types/function_matrix.hpp"

#include <cassert>

#include <Eigen/Dense>

namespace ieompp
{
    namespace types
    {
        template <typename Scalar, typename Index, int Options, int MaxRows, int MaxCols>
        void
        multiply_assign(const FunctionMatrix<Scalar, Index>& matrix,
                        Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options, MaxRows, MaxCols>& vector)
        {
            assert(is_quadratic(matrix));
            assert(matrix.cols() == vector.rows());

            const Index rows = matrix.rows();

            static Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options, MaxRows, MaxCols> temp;
            temp.resize(vector.rows());
            temp.setZero();

#ifdef _OPENMP
#pragma omp parallel for
#endif
            for(Index row = 0; row < rows; ++row) {
                auto& element = temp(row);
                for(Index col = 0; col < rows; ++col) {
                    element += matrix(row, col) * vector(col);
                }
            }

            vector.swap(temp);
        }
    }
}

#endif


#include <pangolin/pangolin.h>
#include <pangolin/video/video_record_repeat.h>
#include <pangolin/gl/gltexturecache.h>
#include <pangolin/gl/glpixformat.h>
#include <pangolin/handler/handler_image.h>
#include <pangolin/utils/file_utils.h>
#include <pangolin/utils/timer.h>
#include <pangolin/gl/gl.h>
#include <pangolin/gl/glcuda.h>

#include <Eigen/Dense>
#include <tdp/data/managed_image.h>

#include <tdp/preproc/depth.h>
#include <tdp/data/volume.h>
#include <tdp/data/managed_volume.h>
#include <tdp/data/image.h>
#include <tdp/eigen/dense.h>
#include <tdp/stats/hist.h>
#include <tdp/manifold/SE3.h>
#include <tdp/calibration/planeEstimation.h>

#include "gui.hpp"

void VideoViewer(const std::string& input_uri, const std::string& output_uri)
{
    // Open Video by URI
    pangolin::VideoRecordRepeat video(input_uri, output_uri);
    const size_t num_streams = video.Streams().size();
    if(num_streams == 0) {
        pango_print_error("No video streams from device.\n");
        return;
    }

    GUI gui(1200, 800, video );

    size_t w = video.Streams()[gui.iRGB].Width();
    size_t h = video.Streams()[gui.iRGB].Height();

    size_t wc = w;//+w%64; // for convolution
    size_t hc = h;//+h%64;

    // Define Camera Render Object (for view / scene browsing)
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(640,480,420,420,320,240,0.1,1000),
        pangolin::ModelViewLookAt(0,0.5,-3, 0,0,0, pangolin::AxisY)
        );
    // Add named OpenGL viewport to window and provide 3D Handler
    pangolin::View& d_cam = pangolin::CreateDisplay()
      .SetHandler(new pangolin::Handler3D(s_cam));
    gui.container().AddDisplay(d_cam);

    tdp::ManagedHostImage<float> debugA(wc, hc);
    tdp::ManagedHostImage<float> debugB(wc, hc);
    tdp::QuickView viewDebugA(wc,hc);
    gui.container().AddDisplay(viewDebugA);
    tdp::QuickView viewDebugB(wc,hc);
    gui.container().AddDisplay(viewDebugB);
    pangolin::DataLog logStats;
    pangolin::Plotter plotStats(&logStats, 0.f,100.f, 0.f,1.f, 10.f, 0.1f);
    gui.container().AddDisplay(plotStats);

    tdp::ManagedHostImage<float> d(wc, hc);
    tdp::ManagedDeviceImage<uint16_t> cuDraw(w, h);
    tdp::ManagedDeviceImage<float> cuD(wc, hc);
    tdp::ManagedDeviceImage<float> cuRho(wc, hc);
    tdp::ManagedDeviceImage<float> cuPlaneDeriv(wc, hc);

    tdp::SE3f T_wd;
    tdp::Camera<float> camD(Eigen::Vector4f(550,550,319.5,239.5));

    pangolin::Var<float> huberAlpha("ui.alpha", 0.3, 0., 1.);
    pangolin::Var<int> planeEstNumIter("ui.# iter", 100, 1, 1000);
    pangolin::Var<float> planeEstEps("ui.10^eps", -6, -10, 0);
    pangolin::Var<float> histDx("ui.Hist dx", 0.01, 1e-3, 0.01);
    pangolin::Var<float> sensorScale("ui.sensor scale", 1e-3, 1e-4, 1e-3);

    // Stream and display video
    //
    while(!pangolin::ShouldQuit())
    {
        glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
        glColor3f(1.0f, 1.0f, 1.0f);

        gui.NextFrames();

        tdp::Image<uint16_t> dRaw;
        if (!gui.ImageD(dRaw)) continue;
        CopyImage(dRaw, cuDraw, cudaMemcpyHostToDevice);
        ConvertDepthGpu(cuDraw, cuD, sensorScale, 0.1, 4.0);

        tdp::Vector3fda nd(0,0,-1);
        tdp::PlaneEstimation planeEstimator(&cuD,camD,huberAlpha.Get());
        //planeEstimator.Reset(&cuD,huberAlpha.Get());
        planeEstimator.Compute(nd, pow(10,planeEstEps), planeEstNumIter.Get());
        nd = planeEstimator.GetMinimum();
        std::cout << "n=" << nd.transpose()/nd.norm()
          << " d=" << 1./nd.norm() << std::endl;
        CopyImage(planeEstimator.cuF_,debugA,cudaMemcpyDeviceToHost);

        tdp::Hist hist;
        hist.Compute(debugA, histDx, true);
        logStats.Clear();
        for (size_t i=0; i<hist.hist_.size(); ++i)
          logStats.Log(hist.hist_[i]);
        //plotStats.Scroll(100,0);

        glEnable(GL_DEPTH_TEST);
        d_cam.Activate(s_cam);
        pangolin::glDrawAxis(1);
        //pangolin::RenderVbo(cuNbuf);

        glLineWidth(1.5f);
        glDisable(GL_DEPTH_TEST);

        gui.ShowFrames();
        viewDebugA.SetImage(debugA);

        ConvertDepthToInverseDepthGpu(cuDraw, cuRho, sensorScale, 0.1, 4.0);
        debugB.CopyFrom(cuRho,cudaMemcpyDeviceToHost);
        viewDebugB.SetImage(debugB);

        // leave in pixel orthographic for slider to render.
        pangolin::DisplayBase().ActivatePixelOrthographic();
        if(video.IsRecording()) {
          pangolin::glRecordGraphic(pangolin::DisplayBase().v.w-14.0f,
              pangolin::DisplayBase().v.h-14.0f, 7.0f);
        }
        pangolin::FinishFrame();
    }
}

int main( int argc, char* argv[] )
{
    const std::string dflt_output_uri = "pango://video.pango";

    if( argc > 1 ) {
        const std::string input_uri = std::string(argv[1]);
        const std::string output_uri = (argc > 2) ? std::string(argv[2]) : dflt_output_uri;
        try{
            VideoViewer(input_uri, output_uri);
        } catch (pangolin::VideoException e) {
            std::cout << e.what() << std::endl;
        }
    }else{
        const std::string input_uris[] = {
            "dc1394:[fps=30,dma=10,size=640x480,iso=400]//0",
            "convert:[fmt=RGB24]//v4l:///dev/video0",
            "convert:[fmt=RGB24]//v4l:///dev/video1",
            "openni:[img1=rgb]//",
            "test:[size=160x120,n=1,fmt=RGB24]//"
            ""
        };

        std::cout << "Usage  : VideoViewer [video-uri]" << std::endl << std::endl;
        std::cout << "Where video-uri describes a stream or file resource, e.g." << std::endl;
        std::cout << "\tfile:[realtime=1]///home/user/video/movie.pvn" << std::endl;
        std::cout << "\tfile:///home/user/video/movie.avi" << std::endl;
        std::cout << "\tfiles:///home/user/seqiemce/foo%03d.jpeg" << std::endl;
        std::cout << "\tdc1394:[fmt=RGB24,size=640x480,fps=30,iso=400,dma=10]//0" << std::endl;
        std::cout << "\tdc1394:[fmt=FORMAT7_1,size=640x480,pos=2+2,iso=400,dma=10]//0" << std::endl;
        std::cout << "\tv4l:///dev/video0" << std::endl;
        std::cout << "\tconvert:[fmt=RGB24]//v4l:///dev/video0" << std::endl;
        std::cout << "\tmjpeg://http://127.0.0.1/?action=stream" << std::endl;
        std::cout << "\topenni:[img1=rgb]//" << std::endl;
        std::cout << std::endl;

        // Try to open some video device
        for(int i=0; !input_uris[i].empty(); ++i )
        {
            try{
                pango_print_info("Trying: %s\n", input_uris[i].c_str());
                VideoViewer(input_uris[i], dflt_output_uri);
                return 0;
            }catch(pangolin::VideoException) { }
        }
    }

    return 0;
}


/****************************************************************************
**
** Copyright (C) 2017 TU Wien, ACIN, Vision 4 Robotics (V4R) group
** Contact: v4r.acin.tuwien.ac.at
**
** This file is part of V4R
**
** V4R is distributed under dual licenses - GPLv3 or closed source.
**
** GNU General Public License Usage
** V4R is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published
** by the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** V4R is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** Please review the following information to ensure the GNU General Public
** License requirements will be met: https://www.gnu.org/licenses/gpl-3.0.html.
**
**
** Commercial License Usage
** If GPL is not suitable for your project, you must purchase a commercial
** license to use V4R. Licensees holding valid commercial V4R licenses may
** use this file in accordance with the commercial license agreement
** provided with the Software or, alternatively, in accordance with the
** terms contained in a written agreement between you and TU Wien, ACIN, V4R.
** For licensing terms and conditions please contact office<at>acin.tuwien.ac.at.
**
**
** The copyright holder additionally grants the author(s) of the file the right
** to use, copy, modify, merge, publish, distribute, sublicense, and/or
** sell copies of their contributions without any restrictions.
**
****************************************************************************/

/**
 * @file main.cpp
 * @author Johann Prankl (prankl@acin.tuwien.ac.at)
 * @date 2017
 * @brief
 *
 */

#include <v4r/features/ImGradientDescriptor.h>
#include <Eigen/Dense>
#include <opencv2/imgproc/imgproc.hpp>

//#define IMGD_INTERPOLATED

namespace v4r {

using namespace std;

/************************************************************************************
 * Constructor/Destructor
 */
ImGradientDescriptor::ImGradientDescriptor(const Parameter &p) : param(p) {
  // param.computeRootGD = false;
}

ImGradientDescriptor::~ImGradientDescriptor() {}

/**
 * ComputeGradients
 */
void ImGradientDescriptor::ComputeGradients(const cv::Mat_<unsigned char> &im) {
  if (param.smooth)
    cv::blur(im, im_smooth, cv::Size(3, 3));
  else
    im_smooth = im;

  cv::Sobel(im_smooth, im_dx, CV_16S, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT);
  cv::Sobel(im_smooth, im_dy, CV_16S, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT);
}

/**
 * ComputeDescriptor
 */
void ImGradientDescriptor::ComputeDescriptor(std::vector<float> &desc, const cv::Mat_<float> &weight) {
  int h;
  short *ptr_dx, *ptr_dy;

  desc.clear();
  desc.resize(128, 0);

  int dv = (im_dx.rows - 2) / 4;
  int du = (im_dx.cols - 2) / 4;

  for (int v = 1; v < im_dx.rows - 1; v++) {
    ptr_dx = &im_dx(v, 1);
    ptr_dy = &im_dy(v, 1);

    for (int u = 1; u < im_dx.cols - 1; u++, ptr_dx++, ptr_dy++) {
      if (*ptr_dx > 0 && *ptr_dy > 0) {
        if (*ptr_dx > *ptr_dy)
          h = 0;
        else
          h = 1;
      } else if (*ptr_dx < 0 && *ptr_dy > 0) {
        if ((-*ptr_dx) < *ptr_dy)
          h = 2;
        else
          h = 3;
      } else if (*ptr_dx < 0 && *ptr_dy < 0) {
        if ((*ptr_dx) < *ptr_dy)
          h = 4;
        else
          h = 5;
      } else {
        if ((*ptr_dx) < (-*ptr_dy))
          h = 6;
        else
          h = 7;
      }

      // desc[(((v-1)/dv*4 + (u-1)/du))*8 + h] += float(sqrt(*ptr_dx * *ptr_dx + *ptr_dy * *ptr_dy)) * weight(v,u);
      desc[(((v - 1) / dv * 4 + (u - 1) / du)) * 8 + h] += float(fabs(*ptr_dx) + fabs(*ptr_dy)) * weight(v, u);
      // desc[(v-1)/dv*4*8 + (u-1)/du*8 + h] += float(fabs(*ptr_dx)+fabs(*ptr_dy)) * weight(v,u);

      // cout<<sqrt(*ptr_dx * *ptr_dx + *ptr_dy * *ptr_dy)<<" ";
    }
  }
}

/**
 * ComputeDescriptorInterpolate
 */
void ImGradientDescriptor::ComputeDescriptorInterpolate(std::vector<float> &desc, const cv::Mat_<float> &weight) {
  int h;
  // short *ptr_dx, *ptr_dy;

  desc.clear();
  desc.resize(128, 0);

  int dv = (im_dx.rows - 2) / 4;
  int du = (im_dx.cols - 2) / 4;
  float du_2 = float(du) / 2.;
  float dv_2 = float(dv) / 2.;
  float wx, wy, wx2, wy2;

  for (int v = 0; v < 4; v++) {
    for (int u = 0; u < 4; u++) {
      for (int y = 0; y < dv; y++) {
        for (int x = 0; x < du; x++) {
          int ug = u * du + x + 1;
          int vg = v * dv + y + 1;
          const float &dx = im_dx(vg, ug);
          const float &dy = im_dy(vg, ug);

          if (dx > 0 && dy > 0) {
            if (dx > dy)
              h = 0;
            else
              h = 1;
          } else if (dx < 0 && dy > 0) {
            if ((-dx) < dy)
              h = 2;
            else
              h = 3;
          } else if (dx < 0 && dy < 0) {
            if (dx < dy)
              h = 4;
            else
              h = 5;
          } else {
            if (dx < -dy)
              h = 6;
            else
              h = 7;
          }

          wx = 0.5 + .5 * (1. - (float(x) - du_2) / du_2);
          wy = 0.5 + .5 * (1. - (float(y) - dv_2) / dv_2);

          desc[(v * 4 + u) * 8 + h] += float(fabs(dx) + fabs(dy)) * weight(vg, ug) * fabs(wx) * fabs(wy);

          wx2 = 1. - fabs(wx);
          wy2 = 1. - fabs(wy);
          int u2 = u + sign(wx);
          int v2 = v + sign(wy);

          if (u2 >= 0 && u2 < 4) {
            desc[(v * 4 + u2) * 8 + h] += float(fabs(dx) + fabs(dy)) * weight(vg, ug) * fabs(wx2) * fabs(wy);
          }
          if (v2 >= 0 && v2 < 4) {
            desc[(v2 * 4 + u) * 8 + h] += float(fabs(dx) + fabs(dy)) * weight(vg, ug) * fabs(wx) * fabs(wy2);
          }
          if (u2 >= 0 && u2 < 4 && v2 >= 0 && v2 < 4) {
            desc[(v * 4 + u2) * 8 + h] += float(fabs(dx) + fabs(dy)) * weight(vg, ug) * fabs(wx2) * fabs(wy2);
          }
        }
      }
    }
  }
}

/**
 * ComputeLTGauss
 */
void ImGradientDescriptor::ComputeLTGauss(const cv::Mat_<unsigned char> &im) {
  if (lt_gauss.rows != im.rows || lt_gauss.cols != im.cols) {
    if (param.gauss_lin)
      ComputeLTGaussLin(im);
    else
      ComputeLTGaussCirc(im);
  }
}

/**
 * ComputeLTGaussCirc
 */
void ImGradientDescriptor::ComputeLTGaussCirc(const cv::Mat_<unsigned char> &im) {
  if (im.rows != im.cols || (im.rows - 2) % 4 != 0)
    throw std::runtime_error("[ImGradientDescriptor::ComputeLTGaussCirc] Invalid patch size!");

  lt_gauss = cv::Mat_<float>(im.rows, im.cols);

  float h_size = im.rows / 2;
  float invSqrSigma;

  invSqrSigma = param.sigma * (float)(h_size - 1);
  invSqrSigma = -1. / (invSqrSigma * invSqrSigma);

  for (int v = -h_size; v < h_size; v++) {
    for (int u = -h_size; u < h_size; u++) {
      lt_gauss(v + h_size, u + h_size) = exp(invSqrSigma * (u * u + v * v));
    }
  }

  /*cv::Mat_<unsigned char> tmp(im.rows,im.cols);
  for (int v=0; v<im.rows; v++)
    for (int u=0; u<im.cols; u++)
      tmp(v,u) = lt_gauss(v,u)*255;
  cv::imshow("image",tmp);
  cv::waitKey(0);*/
}

/**
 * ComputeLTGaussLin
 */
void ImGradientDescriptor::ComputeLTGaussLin(const cv::Mat_<unsigned char> &im) {
  if ((im.cols - 2) % 4 != 0 || (im.rows - 2) % 4 != 0)
    throw std::runtime_error("[ImGradientDescriptor::ComputeLTGaussLin] Invalid patch size!");

  lt_gauss.resize(im.rows, im.cols);

  float h_size = im.rows / 2;
  float invSqrSigma;

  invSqrSigma = param.sigma * (float)(h_size - 1);
  invSqrSigma = -1. / (invSqrSigma * invSqrSigma);

  for (int v = -h_size; v < h_size; v++) {
    for (int u = 0; u < im.cols; u++) {
      lt_gauss(v + h_size, u) = exp(invSqrSigma * (u * u));
    }
  }
}

/**
 * Normalize
 */
void ImGradientDescriptor::Normalize(std::vector<float> &desc) {
  float norm = 0;

  float *ptr = &desc[0];
  for (unsigned i = 0; i < desc.size(); i++, ptr++)
    norm += (*ptr * *ptr);

  if (norm > numeric_limits<float>::epsilon()) {
    norm = 1. / sqrt(norm);

    for (unsigned i = 0; i < desc.size(); i++)
      desc[i] *= norm;
  }
}

/**
 * Normalize
 */
void ImGradientDescriptor::Cut(std::vector<float> &desc) {
  float *ptr = &desc[0];
  float *ptr_end = &desc.back();
  ptr_end++;

  for (; ptr != ptr_end; ptr++)
    if (*ptr > param.thrCutDesc)
      *ptr = param.thrCutDesc;
}

/***************************************************************************************/

/**
 * compute gradient descriptor of center point (sift like)
 * we use a 3x3 sobel kernel
 * @param im image patch to compute the descriptor (min. 18x18 = 16x16 + 1px boarder)
 */
void ImGradientDescriptor::compute(const cv::Mat_<unsigned char> &im, std::vector<float> &desc) {
  ComputeLTGauss(im);

  ComputeGradients(im);

#ifdef IMGD_INTERPOLATED
  ComputeDescriptorInterpolate(desc, lt_gauss);
#else
  ComputeDescriptor(desc, lt_gauss);
#endif

  if (param.normalize) {
    Normalize(desc);  // to 1
    Cut(desc);        // cut 0.2
    Normalize(desc);  // renormalize to 1
  }

  if (param.computeRootGD) {
    Eigen::Map<Eigen::VectorXf> eig_desc(&desc[0], desc.size());
    float norm = eig_desc.lpNorm<1>();
    eig_desc.array() /= norm;
    eig_desc.array() = eig_desc.array().sqrt();
  }
}

/**
 * compute gradient descriptor of center point (sift like)
 * we use a 3x3 sobel kernel
 * @param im image patch to compute the descriptor (min. 18x18 = 16x16 + 1px boarder)
 */
void ImGradientDescriptor::compute(const cv::Mat_<unsigned char> &im, const cv::Mat_<float> &weight,
                                   std::vector<float> &desc) {
  ComputeGradients(im);

#ifdef IMGD_INTERPOLATED
  ComputeDescriptorInterpolate(desc, weight);
#else
  ComputeDescriptor(desc, weight);
#endif

  if (param.normalize) {
    Normalize(desc);  // to 1
    Cut(desc);        // cut 0.2
    Normalize(desc);  // renormalize to 1
  }

  if (param.computeRootGD) {
    Eigen::Map<Eigen::VectorXf> eig_desc(&desc[0], desc.size());
    float norm = eig_desc.lpNorm<1>();
    eig_desc.array() /= norm;
    eig_desc.array() = eig_desc.array().sqrt();
  }
}
}  // namespace v4r


/*
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  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.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

#include <boost/unordered_set.hpp>
#include <graphlab.hpp>
#include <graphlab/macros_def.hpp>
/**
 *
 * In this program we implement the "k-core" decomposition algorithm.
 * We use a parallel variant of
 *
 * V. Batagelj and M. Zaversnik, An O(m) algorithm for cores
 * decomposition of networks,
 *
 *  - Essentially, recursively remove everything with degree 1
 *  - Then recursively remove everything with degree 2
 *  - etc.
 */

/*
 * Each vertex maintains a "degree" count. If this value
 * is 0, the vertex is "deleted"
 */
typedef int vertex_data_type;

/*
 * Don't need any edges
 */
typedef graphlab::empty edge_data_type;

/*
 * Define the type of the graph
 */
typedef graphlab::distributed_graph<vertex_data_type,
                                    edge_data_type> graph_type;

// The current K to compute
size_t CURRENT_K;

/*
 * The core K-core implementation.
 * The basic concept is simple.
 * Each vertex maintains a count of the number of adjacent edges.
 * If a vertex receives a message, the message contains the number of
 * adjacent edges deleted. The vertex then updates its counter.
 * If the counter falls below K, it deletes itself
 * (set the adjacent count to 0) and signals each of its neighbors
 * with a message of 1.
 */
class k_core :
  public graphlab::ivertex_program<graph_type,
                                   graphlab::empty, // gathers are integral
                                   int>,   // messages are integral
  public graphlab::IS_POD_TYPE  {
public:
  // the last received message
  int msg;

  /* Each vertex can only signal once. I set this flag
   * if it is the first time this vertex falls below K, so I can
   * initiate scattering
   */
  bool just_deleted;

  k_core():msg(0),just_deleted(false) { }

  /* The message contains the number of adjacent edges deleted.
   * Store the message in the program, and reset the just_deleted flag
   */
  void init(icontext_type& context, const vertex_type& vertex,
            const message_type& message) {
    msg = message;
    just_deleted = false;
  }

  // gather is never invoked
  edge_dir_type gather_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::NO_EDGES;
  }

  /* On apply, if the vertex has not yet been deleted,
   * decrement the counter on the vertex.
   * If the adjacency count of the vertex falls below K,
   * the vertex shall be deleted.
   * We set the vertex data to 0 to designate that it is deleted
   * and Set the just_deleted flag to signal the neighbors in scatter
   */
  void apply(icontext_type& context, vertex_type& vertex,
             const gather_type& unused) {
    if (vertex.data() > 0) {
      vertex.data() -= msg;
      if (vertex.data() < CURRENT_K) {
        just_deleted = true;
        vertex.data() = 0;
      }
    }
  }

  /*
   * If the vertex is deleted, we signal all neighbors on the scatter
   */
  edge_dir_type scatter_edges(icontext_type& context,
                              const vertex_type& vertex) const {
    return just_deleted ?
      graphlab::ALL_EDGES : graphlab::NO_EDGES;
  }

  /*
   * For each neighboring vertex, if it is not yet deleted,
   * signal it.
   */
  void scatter(icontext_type& context,
               const vertex_type& vertex,
               edge_type& edge) const {
    vertex_type other = edge.source().id() == vertex.id() ?
      edge.target() : edge.source();
    if (other.data() > 0) {
      context.signal(other, 1);
    }
  }

};

// type of the synchronous_engine
typedef graphlab::synchronous_engine<k_core> engine_type;

/*
 * Called before any graph operation is performed.
 * Initializes all vertex data to the number of adjacent edges.
 * Can be called from a graph.transform_vertices()
 */
void initialize_vertex_values(graph_type::vertex_type& v) {
  v.data() = v.num_in_edges() + v.num_out_edges();
}

/*
 * Signals all non-deleted vertices with degree less than K.
 * Can be called from an engine.map_reduce_vertices()
 * We return empty since no reduction is performed. Only the map.
 */
graphlab::empty signal_vertices_at_k(engine_type::icontext_type& ctx,
                                     const graph_type::vertex_type& vertex) {
  if (vertex.data() > 0 && vertex.data() < CURRENT_K) {
    ctx.signal(vertex, 0);
  }
  return graphlab::empty();
}

/*
 * Counts the number of un-deleted vertices.
 */
size_t count_active_vertices(const graph_type::vertex_type& vertex) {
  return vertex.data() > 0;
}

/*
 * Counts the degree of each un-deleted vertex. Half of this
 * will be the size of the K-core graph.
 */
size_t double_count_active_edges(const graph_type::vertex_type& vertex) {
  return (size_t) vertex.data();
}

/*
 * Saves the graph in a tsv format with the condition that
 * the adjacent vertices have not yet been deleted.
 * This allows saving of the k-core graph.
 */
struct save_core_at_k {
  std::string save_vertex(graph_type::vertex_type) { return ""; }
  std::string save_edge(graph_type::edge_type e) {
    if (e.source().data() > 0 && e.target().data() > 0) {
      return graphlab::tostr(e.source().id()) + "\t" +
        graphlab::tostr(e.target().id()) + "\n";
    }
    else return "";
  }
};

int main(int argc, char** argv) {
  std::cout << "Computes a k-core decomposition of a graph.\n\n";

  graphlab::command_line_options clopts
    ("K-Core decomposition. This program "
     "computes the K-Core decomposition of a graph, for K ranging from [kmin] "
     "to [kmax]. The size of the remaining K-core graph at each K is printed. "
     "The [savecores] allow the saving of each K-Core graph in a TSV format"
     );
  std::string prefix, format;
  size_t kmin = 0;
  size_t kmax = (size_t)(-1);
  std::string savecores;
  clopts.attach_option("graph", prefix,
                       "Graph input. reads all graphs matching prefix*");
  clopts.attach_option("format", format,
                       "The graph format");
  clopts.attach_option("kmin", kmin,
                       "Compute the k-Core for k the range [kmin,kmax]");
  clopts.attach_option("kmax", kmax,
                       "Compute the k-Core for k the range [kmin,kmax]");
  clopts.attach_option("savecores", savecores,
                       "If non-empty, will save tsv of each core with prefix [savecores].K.");

  if(!clopts.parse(argc, argv)) return EXIT_FAILURE;
  if (prefix == "") {
    std::cout << "--graph is not optional\n";
    clopts.print_description();
    return EXIT_FAILURE;
  }
  else if (format == "") {
    std::cout << "--format is not optional\n";
    clopts.print_description();
    return EXIT_FAILURE;
  }
  else if (kmax < kmin) {
    std::cout << "kmax must be at least as large as kmin\n";
    clopts.print_description();
    return EXIT_FAILURE;
  }
  // Initialize control plane using mpi
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;
  // load graph
  graph_type graph(dc, clopts);
  graph.load_format(prefix, format);
  graph.finalize();
  dc.cout() << "Number of vertices: " << graph.num_vertices() << std::endl
            << "Number of edges:    " << graph.num_edges() << std::endl;

  graphlab::timer ti;

  graphlab::synchronous_engine<k_core> engine(dc, graph, clopts);

  // initialize the vertex data with the degree
  graph.transform_vertices(initialize_vertex_values);

  // for each K value
  for (CURRENT_K = kmin; CURRENT_K <= kmax; CURRENT_K++) {
    // signal all vertices with degree less than K
    engine.map_reduce_vertices<graphlab::empty>(signal_vertices_at_k);
    // recursively delete all vertices with degree less than K
    engine.start();
    // count the number of vertices and edges remaining
    size_t numv = graph.map_reduce_vertices<size_t>(count_active_vertices);
    size_t nume = graph.map_reduce_vertices<size_t>(double_count_active_edges) / 2;
    if (numv == 0) break;
    // Output the size of the graph
    dc.cout() << "K=" << CURRENT_K << ":  #V = "
              << numv << "   #E = " << nume << std::endl;

    // Saves the result if requested
    if (savecores != "") {
      graph.save(savecores + "." + graphlab::tostr(CURRENT_K) + ".",
                 save_core_at_k(),
                 false, /* no compression */
                 false, /* do not save vertex */
                 true, /* save edge */
                 clopts.get_ncpus()); /* one file per machine */
    }
  }

  graphlab::mpi_tools::finalize();
  return EXIT_SUCCESS;
} // End of main


#include <iostream>
#include <opencv2/imgproc/imgproc.hpp>
#include <boost/python.hpp>

#include "conversion.h"

namespace py = boost::python;

typedef unsigned char uchar_t;

/**
 * Displays an image, passed in from python as an ndarray.
 */
void
display(PyObject *img)
{
    NDArrayConverter cvt;
    cv::Mat mat { cvt.toMat(img) };

    cv::namedWindow("display", CV_WINDOW_NORMAL);
    cv::imshow("display", mat);
    cv::waitKey(0);
}

/**
 * Converts a grayscale image to a bilevel image.
 */
PyObject*
binarize(PyObject *grayImg, short threshold)
{
    NDArrayConverter cvt;
    cv::Mat img { cvt.toMat(grayImg) };
    for (int i = 0; i < img.rows; ++i)
    {
        uchar_t *ptr = img.ptr<uchar_t>(i);
        for (int j = 0; j < img.cols; ++j)
        {
            ptr[j] = ptr[j] < threshold ? 0 : 255;
        }
    }
    return cvt.toNDArray(img);
}

/**
 * Multiplies two ndarrays by first converting them to cv::Mat and returns
 * an ndarray containing the result back.
 */
PyObject*
mul(PyObject *left, PyObject *right)
{
    NDArrayConverter cvt;
    cv::Mat leftMat, rightMat;
    leftMat = cvt.toMat(left);
    rightMat = cvt.toMat(right);
    auto r1 = leftMat.rows, c1 = leftMat.cols, r2 = rightMat.rows,
         c2 = rightMat.cols;
    // Work only with 2-D matrices that can be legally multiplied.
    if (c1 != r2)
    {
        PyErr_SetString(PyExc_TypeError,
                        "Incompatible sizes for matrix multiplication.");
        py::throw_error_already_set();
    }
    cv::Mat result = leftMat * rightMat;

    PyObject* ret = cvt.toNDArray(result);

    return ret;
}

static void init()
{
    Py_Initialize();
    import_array();
}

BOOST_PYTHON_MODULE(examples)
{
    init();
    py::def("display", display);
    py::def("binarize", binarize);
    py::def("mul", mul);
}