//
// Created by James Noeckel on 10/1/20.
//

#include "joint_segmentation.h"
#include "matching_scores.h"
#include <Eigen/Dense>
#include "math/GaussianMixture.h"
#include <numeric>

#define MIN_CORR 0.3
#define MIN_VARIANCE 10.0

uint DEBUG_ID = 0;

void joint_segmentation(const std::vector<cv::Mat> &allImages, const std::vector<cv::Mat> &allMasks, cv::Mat &labels,
                        const Settings &settings, const std::vector<std::vector<int>> &groups,
                        std::mt19937 random_engine) {
    using namespace Eigen;
    int num_pixels = labels.total();
    std::vector<std::vector<int>> indexMap = groups;
    if (indexMap.empty()) {
        indexMap.emplace_back();
        indexMap.back().resize(allImages.size());
        std::iota(indexMap.back().begin(), indexMap.back().end(), 0);
    }
    std::vector<GaussianMixture> gmms(allImages.size());
    for (int imageInd = 0; imageInd < allImages.size(); ++imageInd) {
        const cv::Mat &img = allImages[imageInd];
        std::vector<int> indices;
        indices.reserve(img.total());
        //add all pixels that are both inside the known subset and unoccluded in this view to the training set
        for (int p = 0; p < img.total(); ++p) {
            if (labels.at<uchar>(p) == 0 && allMasks[imageInd].at<uchar>(p)) {
                indices.push_back(p);
            }
        }
        size_t n = indices.size();
        MatrixX3d colors(n, 3);
        for (size_t j = 0; j<n; ++j) {
            int p = indices[j];
            const auto &col = img.at<cv::Vec3b>(p);
            colors.row(j) = RowVector3d(col[0], col[1], col[2]);
        }
        gmms[imageInd] = GaussianMixture(settings.segmentation_gmm_components, 3, MIN_VARIANCE);
        int iters = gmms[imageInd].learn(colors);
        if (!gmms[imageInd].success()) {
            std::cout << "warning: gmm " << imageInd << " failed to learn from " << n
                      << " points after " << iters << " iterations" << std::endl;
        }
    }

    cv::Mat debugAvgProb = cv::Mat::zeros(labels.rows, labels.cols, CV_32FC1);
    cv::Mat debugCount = cv::Mat::ones(labels.rows, labels.cols, CV_32FC1) * 0.0001;
    for (int imgInd = 0; imgInd < allImages.size(); ++imgInd) {
        float maxProb = 0.0f;
        if (gmms[imgInd].success()) {
            cv::Mat debugGMMImg(labels.rows, labels.cols, CV_32FC1);
            for (int i = 0; i < labels.rows; ++i) {
                for (int j = 0; j < labels.cols; ++j) {
                    //RunningAverage avgProb(1);
                    const auto &col = allImages[imgInd].at<cv::Vec3b>(i, j);
                    RowVector3d color(col[0], col[1], col[2]);
                    double logp = gmms[imgInd].logp_data(color)(0);
                    double prob = std::exp(logp);
                    //avgProb.add(prob);
                    maxProb = std::max(maxProb, static_cast<float>(prob));
                    debugGMMImg.at<float>(i, j) = prob;
                    if (allMasks[imgInd].at<uchar>(i, j)) {
                        debugAvgProb.at<float>(i, j) += prob;
                        debugCount.at<float>(i, j) += 1;
                    }
                }
            }
            //convert to viewable image
            float scaleFac = 255.0f / maxProb;
            cv::Mat debugGMMImg2;
            debugGMMImg.convertTo(debugGMMImg2, CV_8UC1, scaleFac);
            cv::imwrite("debug_img_" + std::to_string(DEBUG_ID) + "_view_" + std::to_string(imgInd) + ".png", debugGMMImg2);
        }
    }
    debugAvgProb /= debugCount;
    cv::Mat debugAvgProb2;
    double minVal;
    double maxVal;
    cv::Point minLoc;
    cv::Point maxLoc;
    cv::minMaxLoc( debugAvgProb, &minVal, &maxVal, &minLoc, &maxLoc );

    debugAvgProb.convertTo(debugAvgProb2, CV_8UC1, 255.0 / maxVal);
    cv::imwrite("debug_img_" + std::to_string(DEBUG_ID) + "_avg_scale" + std::to_string(maxVal) + ".png", debugAvgProb2);

    DEBUG_ID++;

    //cv::Mat energies(labels.rows, labels.cols, CV_32FC2);

    /*auto result = std::make_unique<int[]>(num_pixels);   // stores result of optimization
    try {
        auto gc = new GCoptimizationGridGraph(labels.cols, labels.rows, 2);
        std::cout << "setting up energy function" << std::endl;
        for ( int i = 0; i < num_pixels; i++ ) {
            int col = i % labels.cols;
            int row = i / labels.cols;
//            std::cout << "row " << row << ", col " << col << std::endl;
            cv::Point point(col, row);
            uchar initCluster = labels.at<uchar>(point);
            //TODO: set smoothness costs
            //TODO: incorporate initial guess
            float totalNCC = 0.0f;
            size_t count = 0;
//            std::cout << "computing frontal scores" << std::endl;
            for (int img1 = 0; img1 < frontImages.size(); ++img1) {
                if (frontMasks[img1].at<uchar>(row, col) == 0) continue;
//                std::cout << "image1: " << img1 << " (" << frontImages[img1].rows << ", " << frontImages[img1].cols << ") and (" << frontMasks[img1].rows << ", " << frontMasks[img1].cols << ')' << std::endl;
                for (int img2 = img1+1; img2 < frontImages.size(); ++img2) {
                    if (frontMasks[img2].at<uchar>(row, col) == 0) continue;
//                    std::cout << "image2: " << img2 << " (" << frontImages[img2].rows << ", " << frontImages[img2].cols << ") and (" << frontMasks[img2].rows << ", " << frontMasks[img2].cols << ')' << std::endl;
                    float NCC = norm_cross_correlation(frontImages[img1], frontImages[img2], row, col, row, col, radius);
                    totalNCC += NCC;
                    ++count;
                }
            }
//            std::cout << "computing back scores" << std::endl;
            for (int img1 = 0; img1 < backImages.size(); ++img1) {
                if (backMasks[img1].at<uchar>(row, col) == 0) continue;
                for (int img2 = img1+1; img2 < backImages.size(); ++img2) {
                    if (backMasks[img2].at<uchar>(row, col) == 0) continue;
                    float NCC = norm_cross_correlation(backImages[img1], backImages[img2], row, col, row, col, radius);
                    totalNCC += NCC;
                    ++count;
                }
            }
//            std::cout << "setting data score" << std::endl;
            totalNCC /= count;
            totalNCC = (totalNCC + 1)/2.0f;
            //DEBUG: set mask to NCC score
            labels.at<uchar>(row, col) = static_cast<uchar>(std::round(std::max(0.0f, std::min(255.0f, totalNCC * 255))));
            //between 0 and 1
            gc->setDataCost(i, 1, (1.0f-totalNCC) * ENERGY_RESOLUTION);
            gc->setDataCost(i, 0, (1.0f-MIN_CORR) * ENERGY_RESOLUTION);
        }
        cv::imwrite("debug_img" + std::to_string(DEBUG_ID++) + ".png", labels);
        std::cout << "expansion... ";
        gc->expansion(1);
        std::cout << "done" << std::endl;
        for ( int  i = 0; i < num_pixels; i++ )
            result[i] = gc->whatLabel(i);
        delete gc;
    } catch (GCException &e) {
        e.Report();
    }

    for ( int i = 0; i < num_pixels; i++ ) {
        int col = i % labels.cols;
        int row = i / labels.cols;
        cv::Point point(col, row);
        labels.at<uchar>(point) = result[i];
    }*/
}


// Copyright (c) 2006-2007 Michael B. Edwin Rickert
//
// 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 )
//
// $LastChangedBy$ - $LastChangedDate$
//
// Dec 25, 2006 - dot names to directory based name fixes
// Dec  1, 2006 - Created

#define INDUSTRY_MATH_VECTOR_DN_LIMIT 6

#include <industry/math/vector.hpp>
#include <boost/test/auto_unit_test.hpp>

BOOST_AUTO_TEST_CASE( test_math_vector ) {
	using namespace industry::math;
	//vector< unsigned , 0 > should_fail_to_compile_1; //mm: tested 12/1/06 -- OK

	vector< unsigned , 1 > should_compile_1;
	vector< unsigned , 2 > should_compile_2;
	vector< unsigned , 3 > should_compile_3;
	vector< unsigned , 4 > should_compile_4;
	vector< unsigned , 5 > should_compile_5;
	vector< unsigned , 6 > should_compile_6;

	typedef vector< float, 3> vf3;
	vf3 v;
	BOOST_CHECK(square_magnitude<double>(v) == 0);
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "test_mad"

#include <boost/test/unit_test.hpp>
#include "nanocv/tensor.h"
#include "nanocv/math/mad.hpp"
#include "nanocv/math/abs.hpp"
#include "nanocv/math/epsilon.hpp"
#include "nanocv/tensor/mad.hpp"

namespace test
{
        using namespace ncv;

        template
        <
                typename top,
                typename tvector,
                typename tscalar = typename tvector::Scalar
        >
        tscalar test_mad(top op, const tvector& vec1, const tvector& vec2, tscalar wei)
        {
                vector_t cvec1 = vec1;
                vector_t cvec2 = vec2;

                op(cvec1.data(), wei, cvec1.size(), cvec2.data());

                return cvec2.sum();
        }

        void test_mad(size_t size)
        {
                vector_t vec1(size), vec2(size);
                vec1.setRandom();
                vec2.setRandom();

                scalar_t wei = vec1(0) + vec2(3);

                const scalar_t mad    = test_mad(ncv::math::mad<scalar_t>, vec1, vec2, wei);
                const scalar_t madul2 = test_mad(ncv::math::mad_unroll<scalar_t, 2>, vec1, vec2, wei);
                const scalar_t madul3 = test_mad(ncv::math::mad_unroll<scalar_t, 3>, vec1, vec2, wei);
                const scalar_t madul4 = test_mad(ncv::math::mad_unroll<scalar_t, 4>, vec1, vec2, wei);
                const scalar_t madul5 = test_mad(ncv::math::mad_unroll<scalar_t, 5>, vec1, vec2, wei);
                const scalar_t madul6 = test_mad(ncv::math::mad_unroll<scalar_t, 6>, vec1, vec2, wei);
                const scalar_t madul7 = test_mad(ncv::math::mad_unroll<scalar_t, 7>, vec1, vec2, wei);
                const scalar_t madul8 = test_mad(ncv::math::mad_unroll<scalar_t, 8>, vec1, vec2, wei);
                const scalar_t madeig = test_mad(ncv::tensor::mad<scalar_t>, vec1, vec2, wei);

                const scalar_t epsilon = math::epsilon1<scalar_t>();

                BOOST_CHECK_LE(math::abs(mad - mad), epsilon);
                BOOST_CHECK_LE(math::abs(mad - madul2), epsilon);
                BOOST_CHECK_LE(math::abs(mad - madul3), epsilon);
                BOOST_CHECK_LE(math::abs(mad - madul4), epsilon);
                BOOST_CHECK_LE(math::abs(mad - madul5), epsilon);
                BOOST_CHECK_LE(math::abs(mad - madul6), epsilon);
                BOOST_CHECK_LE(math::abs(mad - madul7), epsilon);
                BOOST_CHECK_LE(math::abs(mad - madul8), epsilon);
                BOOST_CHECK_LE(math::abs(mad - madeig), epsilon);
        }
}

BOOST_AUTO_TEST_CASE(test_mad)
{
        using namespace ncv;

        static const size_t min_size = 32 * 1024;
        static const size_t max_size = 4 * 1024 * 1024;

        for (size_t size = min_size; size <= max_size; size *= 2)
        {
                test::test_mad(size);
        }
}


//  Copyright John Maddock 2006.
//  Use, modification and distribution are 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)

#ifndef BOOST_MATH_SP_FACTORIALS_HPP
#define BOOST_MATH_SP_FACTORIALS_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/detail/unchecked_factorial.hpp>
#include <boost/array.hpp>
#ifdef BOOST_MSVC
#pragma warning(push) // Temporary until lexical cast fixed.
#pragma warning(disable: 4127 4701)
#endif
#include <boost/lexical_cast.hpp>
#ifdef BOOST_MSVC
#pragma warning(pop)
#endif
#include <boost/config/no_tr1/cmath.hpp>

namespace boost { namespace math
{

template <class T, class Policy>
inline T factorial(unsigned i, const Policy& pol)
{
   BOOST_STATIC_ASSERT(!boost::is_integral<T>::value);
   BOOST_MATH_STD_USING // Aid ADL for floor.

   if(i <= max_factorial<T>::value)
      return unchecked_factorial<T>(i);
   T result = boost::math::tgamma(static_cast<T>(i+1), pol);
   if(result > tools::max_value<T>())
      return result; // Overflowed value! (But tgamma will have signalled the error already).
   return floor(result + 0.5f);
}

template <class T>
inline T factorial(unsigned i)
{
   return factorial<T>(i, policies::policy<>());
}
/*
// Can't have these in a policy enabled world?
template<>
inline float factorial<float>(unsigned i)
{
   if(i <= max_factorial<float>::value)
      return unchecked_factorial<float>(i);
   return tools::overflow_error<float>(BOOST_CURRENT_FUNCTION);
}

template<>
inline double factorial<double>(unsigned i)
{
   if(i <= max_factorial<double>::value)
      return unchecked_factorial<double>(i);
   return tools::overflow_error<double>(BOOST_CURRENT_FUNCTION);
}
*/
template <class T, class Policy>
T double_factorial(unsigned i, const Policy& pol)
{
   BOOST_STATIC_ASSERT(!boost::is_integral<T>::value);
   BOOST_MATH_STD_USING  // ADL lookup of std names
   if(i & 1)
   {
      // odd i:
      if(i < max_factorial<T>::value)
      {
         unsigned n = (i - 1) / 2;
         return ceil(unchecked_factorial<T>(i) / (ldexp(T(1), (int)n) * unchecked_factorial<T>(n)) - 0.5f);
      }
      //
      // Fallthrough: i is too large to use table lookup, try the
      // gamma function instead.
      //
      T result = boost::math::tgamma(static_cast<T>(i) / 2 + 1, pol) / sqrt(constants::pi<T>());
      if(ldexp(tools::max_value<T>(), -static_cast<int>(i+1) / 2) > result)
         return ceil(result * ldexp(T(1), (i+1) / 2) - 0.5f);
   }
   else
   {
      // even i:
      unsigned n = i / 2;
      T result = factorial<T>(n, pol);
      if(ldexp(tools::max_value<T>(), -(int)n) > result)
         return result * ldexp(T(1), (int)n);
   }
   //
   // If we fall through to here then the result is infinite:
   //
   return policies::raise_overflow_error<T>("boost::math::double_factorial<%1%>(unsigned)", 0, pol);
}

template <class T>
inline T double_factorial(unsigned i)
{
   return double_factorial<T>(i, policies::policy<>());
}

namespace detail{

template <class T, class Policy>
T rising_factorial_imp(T x, int n, const Policy& pol)
{
   BOOST_STATIC_ASSERT(!boost::is_integral<T>::value);
   if(x < 0)
   {
      //
      // For x less than zero, we really have a falling
      // factorial, modulo a possible change of sign.
      //
      // Note that the falling factorial isn't defined
      // for negative n, so we'll get rid of that case
      // first:
      //
      bool inv = false;
      if(n < 0)
      {
         x += n;
         n = -n;
         inv = true;
      }
      T result = ((n&1) ? -1 : 1) * falling_factorial(-x, n, pol);
      if(inv)
         result = 1 / result;
      return result;
   }
   if(n == 0)
      return 1;
   //
   // We don't optimise this for small n, because
   // tgamma_delta_ratio is alreay optimised for that
   // use case:
   //
   return 1 / boost::math::tgamma_delta_ratio(x, static_cast<T>(n), pol);
}

template <class T, class Policy>
inline T falling_factorial_imp(T x, unsigned n, const Policy& pol)
{
   BOOST_STATIC_ASSERT(!boost::is_integral<T>::value);
   BOOST_MATH_STD_USING // ADL of std names
   if(x == 0)
      return 0;
   if(x < 0)
   {
      //
      // For x < 0 we really have a rising factorial
      // modulo a possible change of sign:
      //
      return (n&1 ? -1 : 1) * rising_factorial(-x, n, pol);
   }
   if(n == 0)
      return 1;
   if(x < n-1)
   {
      //
      // x+1-n will be negative and tgamma_delta_ratio won't
      // handle it, split the product up into three parts:
      //
      T xp1 = x + 1;
      unsigned n2 = itrunc((T)floor(xp1), pol);
      if(n2 == xp1)
         return 0;
      T result = boost::math::tgamma_delta_ratio(xp1, -static_cast<T>(n2), pol);
      x -= n2;
      result *= x;
      ++n2;
      if(n2 < n)
         result *= falling_factorial(x - 1, n - n2, pol);
      return result;
   }
   //
   // Simple case: just the ratio of two
   // (positive argument) gamma functions.
   // Note that we don't optimise this for small n,
   // because tgamma_delta_ratio is alreay optimised
   // for that use case:
   //
   return boost::math::tgamma_delta_ratio(x + 1, -static_cast<T>(n), pol);
}

} // namespace detail

template <class RT>
inline typename tools::promote_args<RT>::type
   falling_factorial(RT x, unsigned n)
{
   typedef typename tools::promote_args<RT>::type result_type;
   return detail::falling_factorial_imp(
      static_cast<result_type>(x), n, policies::policy<>());
}

template <class RT, class Policy>
inline typename tools::promote_args<RT>::type
   falling_factorial(RT x, unsigned n, const Policy& pol)
{
   typedef typename tools::promote_args<RT>::type result_type;
   return detail::falling_factorial_imp(
      static_cast<result_type>(x), n, pol);
}

template <class RT>
inline typename tools::promote_args<RT>::type
   rising_factorial(RT x, int n)
{
   typedef typename tools::promote_args<RT>::type result_type;
   return detail::rising_factorial_imp(
      static_cast<result_type>(x), n, policies::policy<>());
}

template <class RT, class Policy>
inline typename tools::promote_args<RT>::type
   rising_factorial(RT x, int n, const Policy& pol)
{
   typedef typename tools::promote_args<RT>::type result_type;
   return detail::rising_factorial_imp(
      static_cast<result_type>(x), n, pol);
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SP_FACTORIALS_HPP


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    timeGaussianFactorGraph.cpp
 * @brief   Time elimination with simple Kalman Smoothing example
 * @author  Frank Dellaert
 */

#include <time.h>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator += in Ordering
#include <CppUnitLite/TestHarness.h>
#include <tests/smallExample.h>

using namespace std;
using namespace gtsam;
using namespace example;
using namespace boost::assign;

/* ************************************************************************* */
// Create a Kalman smoother for t=1:T and optimize
double timeKalmanSmoother(int T) {
  GaussianFactorGraph smoother = createSmoother(T);
  clock_t start = clock();
  // Keys will come out sorted since keys() returns a set
  smoother.optimize(Ordering(smoother.keys()));
  clock_t end = clock ();
  double dif = (double)(end - start) / CLOCKS_PER_SEC;
  return dif;
}

/* ************************************************************************* */
// Create a planar factor graph and optimize
double timePlanarSmoother(int N, bool old = true) {
  GaussianFactorGraph fg = planarGraph(N).get<0>();
  clock_t start = clock();
  fg.optimize();
  clock_t end = clock ();
  double dif = (double)(end - start) / CLOCKS_PER_SEC;
  return dif;
}

/* ************************************************************************* */
// Create a planar factor graph and eliminate
double timePlanarSmootherEliminate(int N, bool old = true) {
  GaussianFactorGraph fg = planarGraph(N).get<0>();
  clock_t start = clock();
  fg.eliminateMultifrontal();
  clock_t end = clock ();
  double dif = (double)(end - start) / CLOCKS_PER_SEC;
  return dif;
}

///* ************************************************************************* */
//// Create a planar factor graph and join factors until matrix formation
//// This variation uses the original join factors approach
//double timePlanarSmootherJoinAug(int N, size_t reps) {
//  GaussianFactorGraph fgBase;
//  VectorValues config;
//  boost::tie(fgBase,config) = planarGraph(N);
//  Ordering ordering = fgBase.getOrdering();
//  Symbol key = ordering.front();
//
//  clock_t start = clock();
//
//  for (size_t i = 0; i<reps; ++i) {
//    // setup
//    GaussianFactorGraph fg(fgBase);
//
//    // combine some factors
//    GaussianFactor::shared_ptr joint_factor = removeAndCombineFactors(fg,key);
//
//    // create an internal ordering to render Ab
//    Ordering render;
//    render += key;
//    BOOST_FOREACH(const Symbol& k, joint_factor->keys())
//    if (k != key) render += k;
//
//    Matrix Ab = joint_factor->matrix_augmented(render,false);
//  }
//
//  clock_t end = clock ();
//  double dif = (double)(end - start) / CLOCKS_PER_SEC;
//  return dif;
//}

///* ************************************************************************* */
//// Create a planar factor graph and join factors until matrix formation
//// This variation uses the single-allocate version to create the matrix
//double timePlanarSmootherCombined(int N, size_t reps) {
//  GaussianFactorGraph fgBase;
//  VectorValues config;
//  boost::tie(fgBase,config) = planarGraph(N);
//  Ordering ordering = fgBase.getOrdering();
//  Symbol key = ordering.front();
//
//  clock_t start = clock();
//
//  for (size_t i = 0; i<reps; ++i) {
//    GaussianFactorGraph fg(fgBase);
//    fg.eliminateOneMatrixJoin(key);
//  }
//
//  clock_t end = clock ();
//  double dif = (double)(end - start) / CLOCKS_PER_SEC;
//  return dif;
//}

/* ************************************************************************* */
TEST(timeGaussianFactorGraph, linearTime)
{
  // Original T = 1000;

  // Alex's Results
  // T = 100000
  // 1907 (init)    : T - 1.65, 2T = 3.28
  //    int->size_t : T - 1.63, 2T = 3.27
  // 2100           : T - 1.52, 2T = 2.96

  int T = 100000;
  double time1 = timeKalmanSmoother(  T);  cout << "timeKalmanSmoother( T): " << time1;
  double time2 = timeKalmanSmoother(2*T);  cout << "  (2*T): " << time2 << endl;
  DOUBLES_EQUAL(2*time1,time2,0.2);
}

// Timing with N = 30
// 1741: 8.12, 8.12, 8.12, 8.14, 8.16
// 1742: 5.97, 5.97, 5.97, 5.99, 6.02
// 1746: 5.96, 5.96, 5.97, 6.00, 6.04
// 1748: 5.91, 5.92, 5.93, 5.95, 5.96
// 1839: 0.206956 0.206939 0.206213 0.206092 0.206780 // colamd !!!!

// Alex's Machine
// Initial:
// 1907 (N = 30)               :  0.14
//      (N = 100)         : 16.36
// Improved (int->size_t)
//      (N = 100)              : 15.39
// Using GSL/BLAS for updateAb : 12.87
// Using NoiseQR               : 16.33
// Using correct system        : 10.00

// Switch to 100*100 grid = 10K poses
// 1879: 15.6498 15.3851 15.5279

int size = 100;

/* ************************************************************************* */
TEST(timeGaussianFactorGraph, planar_old)
{
  cout << "Timing planar - original version" << endl;
  double time = timePlanarSmoother(size);
  cout << "timeGaussianFactorGraph : " << time << endl;
  //DOUBLES_EQUAL(5.97,time,0.1);
}

/* ************************************************************************* */
TEST(timeGaussianFactorGraph, planar_new)
{
  cout << "Timing planar - new version" << endl;
  double time = timePlanarSmoother(size, false);
  cout << "timeGaussianFactorGraph : " << time << endl;
  //DOUBLES_EQUAL(5.97,time,0.1);
}

/* ************************************************************************* */
TEST(timeGaussianFactorGraph, planar_eliminate_old)
{
  cout << "Timing planar Eliminate - original version" << endl;
  double time = timePlanarSmootherEliminate(size);
  cout << "timeGaussianFactorGraph : " << time << endl;
  //DOUBLES_EQUAL(5.97,time,0.1);
}

/* ************************************************************************* */
TEST(timeGaussianFactorGraph, planar_eliminate_new)
{
  cout << "Timing planar Eliminate - new version" << endl;
  double time = timePlanarSmootherEliminate(size, false);
  cout << "timeGaussianFactorGraph : " << time << endl;
  //DOUBLES_EQUAL(5.97,time,0.1);
}

//size_t reps = 1000;
///* ************************************************************************* */
//TEST(timeGaussianFactorGraph, planar_join_old)
//{
//  cout << "Timing planar join - old" << endl;
//  double time = timePlanarSmootherJoinAug(size, reps);
//  cout << "timePlanarSmootherJoinAug " << size << " : " << time << endl;
//  //DOUBLES_EQUAL(5.97,time,0.1);
//}
//
///* ************************************************************************* */
//TEST(timeGaussianFactorGraph, planar_join_new)
//{
//  cout << "Timing planar join - new" << endl;
//  double time = timePlanarSmootherCombined(size, reps);
//  cout << "timePlanarSmootherCombined " << size << " : " << time << endl;
//  //DOUBLES_EQUAL(5.97,time,0.1);
//}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */


// Copyright (c) 2015 The Dogecoin Core developers
// Copyright (c) 2021 CoinGreen Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "arith_uint256.h"
#include "chainparams.h"
#include "coingreen.h"
#include "test/test_bitcoin.h"

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(coingreen_tests, TestingSetup)

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    int nHeight = 1;
    const CChainParams& mainParams = Params(CBaseChainParams::MAIN);
    CAmount nSubsidy = 0;
    CAmount nSum = GENESIS_BLOCK_REWARD;
    uint256 prevHash = uint256S("0");

    int threshold = (INITIAL_SUPPLY-GENESIS_BLOCK_REWARD)/REWARD_PER_BLOCK;

    for (nHeight = 1; nHeight <= threshold; nHeight++) {
        nSubsidy = GetCoinGreenBlockSubsidy(nHeight, mainParams.GetConsensus(nHeight), prevHash);
        nSum += nSubsidy;
        BOOST_CHECK(nSubsidy == REWARD_PER_BLOCK * COIN);
    }
    nSubsidy = GetCoinGreenBlockSubsidy(nHeight, mainParams.GetConsensus(nHeight), prevHash);
    BOOST_CHECK(nSubsidy == (INITIAL_SUPPLY - threshold*REWARD_PER_BLOCK - GENESIS_BLOCK_REWARD) * COIN);
    nSum += nSubsidy;

    nSubsidy = GetCoinGreenBlockSubsidy(nHeight+1, mainParams.GetConsensus(nHeight), prevHash);
    BOOST_CHECK(nSubsidy == 0);

    nSubsidy = GetCoinGreenBlockSubsidy(nHeight+100, mainParams.GetConsensus(nHeight), prevHash);
    BOOST_CHECK(nSubsidy == 0);

    BOOST_CHECK(nSum/COIN == (INITIAL_SUPPLY- GENESIS_BLOCK_REWARD));

}

BOOST_AUTO_TEST_SUITE_END()


#include "jlann_ds.hpp"
#include "ANN/ANN.h"
#include <iostream>
#include <cmath>
#include <fstream>
#include <Eigen/Dense>
#include <string>
#include <boost/program_options.hpp>
#include <sstream>
#include <chrono>
#include <sys/resource.h>

using namespace std;
namespace po = boost::program_options;

using dtype = double;
using JLMatrix = typename jlann::JLAnnDS<dtype>::JLMatrix;
using JLVector = typename jlann::JLAnnDS<dtype>::JLVector;

class Timer {
public:
	Timer() { start_time = std::chrono::high_resolution_clock::now(); }

	void start() {
		start_time = std::chrono::high_resolution_clock::now();
	}

	double end() {
		return elapsed_seconds();
	}

	double elapsed_seconds() {
		auto end_time = std::chrono::high_resolution_clock::now();
		auto elapsed = std::chrono::duration_cast<std::chrono::duration<double>>(end_time - start_time);
		return elapsed.count();
	}
private:
	decltype(std::chrono::high_resolution_clock::now()) start_time;
};

template <typename T>
void parse_csv(string filename, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>& dataset) {
	ifstream fin(filename.c_str());
	int numberOfPoints;
	int dimension;
	fin >> numberOfPoints >> dimension;

	dataset.resize(numberOfPoints, dimension);

	string line;
	string line2;
	getline(fin, line);
	int i=0;
	while (getline(fin, line)) {
		istringstream iss(line);
		int j=0;
		while (getline(iss, line2, ',')) {
			T tmp;
			istringstream(line2) >> tmp;
			dataset(i, j++) = tmp;
			//point_array[i][j++] = tmp;
		}
		i++;
		if (i==numberOfPoints) {
			break;
		}
	}
	fin.close();
}

template <typename T>
void read_file(string filename, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>& dataset) {
	if (filename.find(".csv")!=string::npos) {
		parse_csv<T>(filename, dataset);
	}
}

void queryJl(jlann::JLAnnDS<dtype>* jlAnnDs, double epsilon, JLMatrix& dataset, JLMatrix& queries,
				Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>& groundtruth) {
	int numberOfQueries = queries.rows();
	int trueNn = 0;
	int ann = 0;
	double annQueryTime = 0;
	//for (int i=0; i<numberOfQueries; i++) {
	//	Timer timer;
	//	JLVector query = queries.row(i);
	//	int nnIndex = jlAnnDs->findNearestNeighbor(query, epsilon);
	//	annQueryTime += timer.elapsed_seconds();

	//	double nnDist = (dataset.row(groundtruth(i, 0))-query).norm();
	//	double annDist = (dataset.row(nnIndex)-query).norm();
	//	//cout << annDist << " <= " << "(1+" << epsilon << ")*" << nnDist << " = " << (1+epsilon)*nnDist << endl;

	//	if (nnIndex==groundtruth(i, 0)) {
	//		++trueNn;
	//	}
	//	for (int j=1; j<groundtruth.cols(); j++) {
	//		if (nnIndex==groundtruth(i, j)) {
	//			++ann;
	//		}
	//	}
	//}
	Timer batchTimer;
	std::vector<int> nns = jlAnnDs->findNearestNeighbors(queries, epsilon);
	annQueryTime = batchTimer.elapsed_seconds();

	for (int i=0; i<numberOfQueries; i++) {
		double nnDist = (dataset.row(groundtruth(i, 0))-queries.row(i)).norm();
		//double annDist = (dataset.row(nnIndex)-query).norm();
		//cout << annDist << " <= " << "(1+" << epsilon << ")*" << nnDist << " = " << (1+epsilon)*nnDist << endl;

		if (nns[i]==groundtruth(i, 0)) {
			++trueNn;
		}
		for (int j=1; j<groundtruth.cols(); j++) {
			if (nns[i]==groundtruth(i, j)) {
				++ann;
			}
		}
	}

	cout << "Total query time: " << annQueryTime << " -- Average query time: " << annQueryTime/numberOfQueries << endl;
	cout << "True nns percentage: " << (double)trueNn/numberOfQueries*100 << "%" << endl;
	cout << "Misses percentage: " << (double)(numberOfQueries-ann)/numberOfQueries*100 << "%" << endl;
}

int main(int argc, char* argv[]) {
	boost::program_options::options_description desc;
	desc.add_options()
		("input,i", po::value<string>(), "The input file (can be .ine, .csv); mandatory.")
		("query,q", po::value<string>(), "The query file (can be .ine, .csv); mandatory.")
		("groundtruth,g", po::value<string>(), "The groundtruth file (can be .ine, .csv); mandatory.")
		("e", po::value<double>()->value_name("Val")->default_value(0.1), "The approximation factor")
	;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("input")==0) {
		cout << desc << endl;
		return 1;
	}

	if (vm.count("query")==0) {
		cout << desc << endl;
		return 1;
	}

	if (vm.count("groundtruth")==0) {
		cout << desc << endl;
		return 1;
	}

	if (vm.count("e")==0) {
		cout << desc << endl;
		return 1;
	}

	string filename = vm["input"].as<string>();
	string query_filename = vm["query"].as<string>();
	string groundtruth_filename = vm["groundtruth"].as<string>();
	double epsilon = vm["e"].as<double>();

	cout << "Reading files. " << endl;
	JLMatrix dataset;
	read_file<dtype>(filename, dataset);
	cout << "Read dataset." << endl;

	JLMatrix queries;
	read_file<dtype>(query_filename, queries);
	cout << "Read queries." << endl;

	Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> groundtruth;
	read_file<int>(groundtruth_filename, groundtruth);
	cout << "Read groundtruth." << endl;

	Timer timer;
	std::cout << "Creating jlann ds." << std::endl;
	int projDimension = 16;//(int)2*std::ceil(std::log2(dataset.rows())/(std::log2(std::log2(dataset.rows()))));
	int projSearchSize = (int)std::sqrt(dataset.rows());
	jlann::JLAnnDS<dtype>* jlAnnDs = new jlann::JLAnnDS<dtype>(dataset, 1, projDimension, projSearchSize);
	jlAnnDs->createDs();
	double jlannPreprocessingTime = timer.end();
	cout << "Constructing jlann dataset took " << jlannPreprocessingTime << "s" << endl;

	queryJl(jlAnnDs, epsilon, dataset, queries, groundtruth);

    delete jlAnnDs;
	annClose();
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  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/simd/function/scalar/rem_pio2.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/pio_2.hpp>

STF_CASE_TPL (" rem_pio2",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::rem_pio2;
  using iT = bd::as_integer_t<T>;
  using r_t = decltype(rem_pio2(T()));

  {
    r_t res = rem_pio2(bs::Zero<T>());
    STF_ULP_EQUAL( res.first, bs::Zero<iT>(), 1);
    STF_ULP_EQUAL( res.second, bs::Zero<T>(), 1);
  }
} // end of test for floating_

// STF_CASE_TPL (" rem_pio2 targeted",  STF_IEEE_TYPES)
// {
//   namespace bs = boost::simd;
//   namespace bd = boost::dispatch;

//   using bs::rem_pio2;
//   using iT = bd::as_integer_t<T>;

//   iT n;
//   T x = bs::Pio_2<T>(), xr;
//   n = rem_pio2(x, xr, bd::as_<bs::tag::big_tag>());
//   STF_ULP_EQUAL( xr, bs::Zero<T>(), 0.5);
//   STF_ULP_EQUAL( n, bs::One<iT>(), 0);

//   n = rem_pio2(x, xr, bd::as_<bs::tag::medium_tag>());
//   STF_ULP_EQUAL( xr, bs::Zero<T>(), 0.5);
//   STF_ULP_EQUAL( n, bs::One<iT>(), 0);

//   n = rem_pio2(x, xr, bd::as_<bs::tag::small_tag>());
//   STF_ULP_EQUAL( xr, bs::Zero<T>(), 0.5);
//   STF_ULP_EQUAL( n, bs::One<iT>(), 0);

//   n = rem_pio2(x, xr, bd::as_<bs::tag::very_small_tag>());
//   STF_ULP_EQUAL( xr, bs::Zero<T>(), 0.5);
//   STF_ULP_EQUAL( n, bs::One<iT>(), 0);
// }


#ifndef __TENSORQUADRATURE__CLASS__
#define __TENSORQUADRATURE__CLASS__

#include <Eigen/Dense>

#include "UnivariateQuadrature.hpp"

class TensorProductQuadrature {
 public:
  TensorProductQuadrature(void);
  TensorProductQuadrature(const Eigen::VectorXi &lvl, UnivariateQuadrature &Q);
  void initQuadrature(const Eigen::VectorXi &lvl, UnivariateQuadrature &Q);
  const Eigen::VectorXd &get_weights(void) const;
  const Eigen::MatrixXd &get_points(void) const;
  int get_nPts(void) const;
  Eigen::VectorXi computeBase(const Eigen::VectorXi &lvl);

 protected:
  Eigen::VectorXd _weights;
  Eigen::MatrixXd _points;
  int _nPts;
};

#endif


#include "utils.hpp"

#include "cunits.hpp"
#include "graph.hpp"

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/range.hpp>

#include <algorithm>
#include <iterator>
#include <list>
#include <set>

using namespace std;

SU
find_path_su(const graph &g, const path &p)
{
  SU su;

  path::const_iterator i = p.begin();

  if (i != p.end())
    {
      su = boost::get(boost::edge_su, g, *i);

      while(++i != p.end())
        su = intersection(su, boost::get(boost::edge_su, g, *i));
    }

  return su;
}

// The function for sorting the list of sets.
static bool
stlos(const set<vertex> &s1, const set<vertex> &s2)
{
  return s1.size() > s2.size();
}

// This is the << operator for a cupath.
std::ostream &
operator << (std::ostream &os, const cupath &p)
{
  os << "cupath(" << p.first << ", " << p.second << ")";
  return os;
}

bool
vertex_in_path(const graph &g, const path &p, vertex v)
{
  for (const edge &e: p)
    if (source(e, g) == v || target(e, g) == v)
      return true;

  return false;
}

void
calc_sp_stats(const graph &g, dbl_acc &hop_acc, dbl_acc &len_acc)
{
  // Calculate stats for shortest paths.
  for (vertex src: boost::make_iterator_range(boost::vertices(g)))
    {
      vector<int> dist(num_vertices(g));
      vector<vertex> pred(num_vertices(g));

      boost::dijkstra_shortest_paths
        (g, src,
         boost::predecessor_map(&pred[0]).distance_map(&dist[0]));

      for (vertex dst: boost::make_iterator_range(boost::vertices(g)))
        if (src != dst)
          {
            // Make sure the path was found.
            assert(pred[dst] != dst);

            // Record the number of hops.
            int hops = 0;
            vertex c = dst;
            while(c != src)
              {
                c = pred[c];
                ++hops;
              }
            hop_acc(hops);

            // Record the path length.
            len_acc(dist[dst]);
          }
    }
}

double
calc_mcat(const graph &g, double mnh, double mht, double mnu, double ol)
{
  // The network capacity, i.e. the number of units of all links.
  double cap = 0;
  for (const auto &e: boost::make_iterator_range(edges (g)))
    cap += boost::get(boost::edge_nou, g, e);

  // The mean connection arrival time.
  double mcat = mnh * mht * mnu / (ol * cap);

  return mcat;
}

bool
load_graphviz(const string &file_name, graph &g)
{
  ifstream is(file_name);

  if (!is)
    {
      cerr << "Error opening a graphviz file '" << file_name << "'.\n";
      return false;
    }

  boost::dynamic_properties dp;

  dp.property("node_id", get(boost::vertex_name, g));
  dp.property("weight", get(boost::edge_weight, g));

  bool result = read_graphviz(is, g, dp);

  is.close();

  return result;
}


#ifndef SMOOTH__FEEDBACK__UTILS__SPARSE_HPP_
#define SMOOTH__FEEDBACK__UTILS__SPARSE_HPP_

#include <Eigen/Sparse>

#include <numeric>
#include <optional>

namespace smooth::feedback {

/**
 * @brief Block sparse matrix construction.
 *
 * @param blocks list of lists {{b00, b01}, {b10, b11 ...}} of (optional) sparse matrix blocks
 * @return the blocks as a single sparse matrix
 *
 * Non-present (std::nullopt-valued) blocks are considered zeros.
 *
 * @warning The block sizes must be consistent, i.e. all blocks in the same block-column must have
 * the same number of columns, and similarly for row-columns and rows.
 */
inline Eigen::SparseMatrix<double> sparse_block_matrix(
  const std::initializer_list<std::initializer_list<std::optional<Eigen::SparseMatrix<double>>>> &
    l)
{
  const auto n_rows = l.size();
  const auto n_cols = std::begin(l)->size();

  Eigen::VectorXi dims_rows = Eigen::VectorXi::Constant(n_rows, -1);
  Eigen::VectorXi dims_cols = Eigen::VectorXi::Constant(n_cols, -1);

  // figure block row and col dimensions
  for (auto krow = 0u; const auto & row : l) {
    for (auto kcol = 0u; const auto & item : row) {
      if (item.has_value()) {
        if (dims_cols(kcol) == -1) {
          dims_cols(kcol) = item->cols();
        } else {
          assert(dims_cols(kcol) == item->cols());
        }
        if (dims_rows(krow) == -1) {
          dims_rows(krow) = item->rows();
        } else {
          assert(dims_rows(krow) == item->rows());
        }
      }
      ++kcol;
    }
    ++krow;
  }

  // check that all dimensions are defined by input args
  assert(dims_rows.minCoeff() > -1);
  assert(dims_cols.minCoeff() > -1);

  // figure starting indices
  const auto n_row = std::accumulate(std::cbegin(dims_rows), std::cend(dims_rows), 0u);
  const auto n_col = std::accumulate(std::cbegin(dims_cols), std::cend(dims_cols), 0u);

  Eigen::SparseMatrix<double> ret(n_row, n_col);

  // allocate pattern
  Eigen::Matrix<decltype(ret)::StorageIndex, -1, 1> pattern(n_col);
  pattern.setZero();
  for (const auto & row : l) {
    for (auto kcol = 0u, col0 = 0u; const auto &item : row) {
      if (item.has_value()) {
        for (auto col = 0; col < dims_cols(kcol); ++col) {
          pattern(col0 + col) += item->outerIndexPtr()[col + 1] - item->outerIndexPtr()[col];
        }
      }
      col0 += dims_cols(kcol++);
    }
  }

  ret.reserve(pattern);

  // insert values
  for (auto krow = 0u, row0 = 0u; const auto &row : l) {
    for (auto kcol = 0u, col0 = 0u; const auto &item : row) {
      if (item.has_value()) {
        for (auto col = 0; col < dims_cols(kcol); ++col) {
          for (typename std::decay_t<decltype(*item)>::InnerIterator it(*item, col); it; ++it) {
            ret.insert(row0 + it.index(), col0 + col) = it.value();
          }
        }
      }
      col0 += dims_cols(kcol++);
    }
    row0 += dims_rows(krow++);
  }

  ret.makeCompressed();

  return ret;
}

/**
 * @brief nxn sparse identity matrix
 *
 * @param n matrix square dimension
 */
inline Eigen::SparseMatrix<double> sparse_identity(std::size_t n)
{
  Eigen::SparseMatrix<double> ret(n, n);
  ret.reserve(Eigen::Matrix<int, -1, 1>::Ones(n));
  for (auto i = 0u; i < n; ++i) { ret.insert(i, i) = 1; }
  return ret;
}

/**
 * @brief Compute X ⊗ In where X is sparse.
 *
 * @param X sparse matrix in compressed format
 * @param n identity matrix dimension
 *
 * The result has the same storage order as X.
 */
template<typename Derived>
inline auto kron_identity(const Eigen::SparseCompressedBase<Derived> & X, std::size_t n)
{
  Eigen::
    SparseMatrix<typename Derived::Scalar, Derived::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor>
      ret(X.rows() * n, X.cols() * n);

  Eigen::Matrix<int, -1, 1> pattern(X.outerSize() * n);

  for (auto i0 = 0u, i = 0u; i < X.outerSize(); ++i) {
    auto nnz_i = X.outerIndexPtr()[i + 1] - X.outerIndexPtr()[i];
    pattern.segment(i0, n).setConstant(nnz_i);
    i0 += n;
  }

  ret.reserve(pattern);

  for (auto i0 = 0u; i0 < X.outerSize(); ++i0) {
    for (typename std::decay_t<decltype(X)>::InnerIterator it(X, i0); it; ++it) {
      for (auto diag = 0u; diag < n; ++diag) {
        ret.insert(n * it.row() + diag, n * it.col() + diag) = it.value();
      }
    }
  }

  ret.makeCompressed();

  return ret;
}

/**
 * @brief Compute X ⊗ In where X is dense.
 *
 * @param X sparse matrix in compressed format
 * @param n identity matrix dimension
 *
 * The result is column-major.
 */
template<typename Derived>
inline auto kron_identity(const Eigen::MatrixBase<Derived> & X, std::size_t n)
{
  Eigen::SparseMatrix<typename Derived::Scalar> ret(X.rows() * n, X.cols() * n);

  ret.reserve(Eigen::Matrix<int, -1, 1>::Constant(ret.cols(), n * X.rows()));

  for (auto row = 0u; row < X.rows(); ++row) {
    for (auto col = 0u; col < X.cols(); ++col) {
      for (auto diag = 0u; diag < n; ++diag) {
        ret.insert(n * row + diag, n * col + diag) = X(row, col);
      }
    }
  }

  ret.makeCompressed();

  return ret;
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__UTILS__SPARSE_HPP_


// Copyright 2006. Peter Gottschling, Matthias Troyer, Rolf Bonderer
// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef LA_VECTOR_CONCEPTS_INCLUDE
#define LA_VECTOR_CONCEPTS_INCLUDE

#include <boost/numeric/linear_algebra/concepts.hpp>
#include <boost/numeric/linear_algebra/ets_concepts.hpp>

#ifdef __GXX_CONCEPTS__
#  include <concepts>
#else
#  include <boost/numeric/linear_algebra/pseudo_concept.hpp>
#endif

namespace math {

/** @addtogroup Concepts
 *  @{
 */

#ifdef __GXX_CONCEPTS__
concept VectorSpace<typename Vector, typename Scalar = typename Vector::value_type>
: AdditiveAbelianGroup<Vector>
{
    requires Field<Scalar>;
    requires Multiplicable<Scalar, Vector>;
    requires MultiplicableWithAssign<Vector, Scalar>;
    requires DivisibleWithAssign<Vector, Scalar>;

    requires std::Assignable<Vector, Multiplicable<Scalar, Vector>::result_type>;
    requires std::Assignable<Vector, Multiplicable<Vector, Scalar>::result_type>;
    requires std::Assignable<Vector, Divisible<Vector, Scalar>::result_type>;

    // Associated types of Field<Scalar> and AdditiveAbelianGroup<Vector> collide
    // typename result_type = AdditiveAbelianGroup<Vector>::result_type;
    // typename assign_result_type = AdditiveAbelianGroup<Vector>::assign_result_type;

    axiom Distributivity(Vector v, Vector w, Scalar a, Scalar b)
    {
	a * (v + w) == a * v + a * w;
	(a + b) * v == a * v + b * v;
	// The following properties are implied by the above, Field and Abelian group
	// Can we be sure that compilers can deduce/interfere it?
	(v + w) * a == v * a + w * a;
	v * (a + b) == v * a + v * b;
    }
}
#else
    //! Concept VectorSpace
    /*!
	\param Vector   The the type of a vector or a collection
        \param Scalar   The scalar over which the vector field is defined

	\par Requires:
	- Field < Scalar >;
	- Multiplicable <Scalar, Vector>;
	- MultiplicableWithAssign <Vector, Scalar>;
	- DivisibleWithAssign <Vector, Scalar>;
	- std::Assignable <Vector, Multiplicable<Scalar, Vector>::result_type>;
	- std::Assignable <Vector, Multiplicable<Vector, Scalar>::result_type>;
	- std::Assignable <Vector, Divisible<Vector, Scalar>::result_type>;

    */
    template <typename Vector, typename Scalar = typename Vector::value_type>
    struct VectorSpace
      : AdditiveAbelianGroup<Vector>
    {
	/// Invariant: Distributivity of scalars and vectors from left and from right
	axiom Distributivity(Vector v, Vector w, Scalar a, Scalar b)
	{
	    /// a * (v + w) == a * v + a * w;       // Scalar from left

	    /// Vector from right: (a + b) * v == a * v + b * v;

	    /// Scalar from right: (v + w) * a == v * a + w * a;

	    /// Vector from left:  v * (a + b) == v * a + v * b;
	}
    };
#endif

#ifdef __GXX_CONCEPTS__
concept Norm<typename N, typename Vector,
	     typename Scalar = typename Vector::value_type>
  : std::Callable1<N, Vector>
{
    requires VectorSpace<Vector, Scalar>;
    requires RealMagnitude<Scalar>;
    typename magnitude_type = MagnitudeType<Scalar>::type;
    requires std::Convertible<magnitude_type, Scalar>;

    typename result_type_norm = std::Callable1<N, Vector>::result_type;
    requires std::Convertible<result_type_norm, RealMagnitude<Scalar>::magnitude_type>;
    requires std::Convertible<result_type_norm, Scalar>;

    // Version with function instead functor, as used by Rolf and Matthias
    // Axioms there defined without norm functor and concept has only 2 types
#if 0
    typename result_type_norm;
    result_type_norm norm(const Vector&);
    requires std::Convertible<result_type_norm, magnitude_type>;
    requires std::Convertible<result_type_norm, Scalar>;
#endif

    axiom Positivity(N norm, Vector v, magnitude_type ref)
    {
	norm(v) >= zero(ref);
    }

    // The following is covered by RealMagnitude
    // requires AbsApplicable<Scalar>;
    // requires std::Convertible<AbsApplicable<Scalar>::result_type, magnitude_type>;
    // requires Multiplicable<magnitude_type>;

    axiom PositiveHomogeneity(N norm, Vector v, Scalar a)
    {
	norm(a * v) == abs(a) * norm(v);
    }

    axiom TriangleInequality(N norm, Vector u, Vector v)
    {
	norm(u + v) <= norm(u) + norm(v);
    }
}
#else
    //! Concept Norm
    /*!
        Semantic requirements of a norm

	\param N        Norm functor
	\param Vector   The the type of a vector or a collection
        \param Scalar   The scalar over which the vector field is defined

	\par Refinement of:
	- std::Callable1 <N, Vector>

	\par Associated types:
	- magnitude_type
	- result_type_norm

	\par Requires:
	- VectorSpace <Vector, Scalar>;
	- RealMagnitude < Scalar >;
	- std::Convertible <magnitude_type, Scalar>;
	- std::Convertible <result_type_norm, RealMagnitude<Scalar>::magnitude_type>;
	- std::Convertible <result_type_norm, Scalar>;

    */
template <typename N, typename Vector,
	  typename Scalar = typename Vector::value_type>
struct Norm
  : std::Callable1<N, Vector>
{
    /// Associated type to represent real values in teh Field of scalar (with default)
    /** By default MagnitudeType<Scalar>::type */
    typedef associated_type magnitude_type;

    /// Associated type for result of norm functor
    /** Automatically detected */
    typedef associated_type result_type_norm;

    /// Invariant: norm of vector is larger than zero
    axiom Positivity(N norm, Vector v, magnitude_type ref)
    {
	/// norm(v) >= zero(ref);
    }

    /// Invariant: positive homogeneity with scalar
    axiom PositiveHomogeneity(N norm, Vector v, Scalar a)
    {
	/// norm(a * v) == abs(a) * norm(v);
    }

    /// Invariant: triangle inequality
    axiom TriangleInequality(N norm, Vector u, Vector v)
    {
	/// norm(u + v) <= norm(u) + norm(v);
    }
};
#endif

#ifdef __GXX_CONCEPTS__
concept SemiNorm<typename N, typename Vector,
		 typename Scalar = typename Vector::value_type>
  : Norm<N, Vector, Scalar>
{
    axiom PositiveDefiniteness(N norm, Vector v, magnitude_type ref)
    {
	if (norm(v) == zero(ref))
	    v == zero(v);
	if (v == zero(v))
	    norm(v) == zero(ref);
    }
}
#else
    //! Concept SemiNorm
    /*!
        Semantic requirements of a semi-norm

	\param N        Norm functor
	\param Vector   The the type of a vector or a collection
        \param Scalar   The scalar over which the vector field is defined

	\par Refinement of:
	- Norm <N, Vector, Scalar>
    */
template <typename N, typename Vector,
	  typename Scalar = typename Vector::value_type>
struct SemiNorm
  : Norm<N, Vector, Scalar>
{
    /// The norm of a vector is zero if and only if the vector is the zero vector
    axiom PositiveDefiniteness(N norm, Vector v, magnitude_type ref)
    {
	/// if (norm(v) == zero(ref)) v == zero(v);

	/// if (v == zero(v)) norm(v) == zero(ref);
    }
};
#endif

#ifdef __GXX_CONCEPTS__
concept BanachSpace<typename N, typename Vector,
		    typename Scalar = typename Vector::value_type>
  : Norm<N, Vector, Scalar>,
    VectorSpace<Vector, Scalar>
{};
#else
    //! Concept BanachSpace
    /*!
        A Banach space is a vector space with a norm

	\param N        Norm functor
	\param Vector   The the type of a vector or a collection
        \param Scalar   The scalar over which the vector field is defined

	\par Refinement of:
	- Norm <N, Vector, Scalar>
	- VectorSpace <Vector, Scalar>

	\note
	- The (expressible) requirements of Banach Space are already given in Norm.
	- The difference between the requirements is the completeness of the
	  Banach space, i.e. that every Cauchy sequence w.r.t. norm(v-w) has a limit
	  in the space. Unfortunately, completeness is never satisfied for
	  finite precision arithmetic types.
	- Another subtle difference is that Norm is not a refinement of Vectorspace
    */
template <typename N, typename Vector,
	  typename Scalar = typename Vector::value_type>
struct BanachSpace
  : Norm<N, Vector, Scalar>,
    VectorSpace<Vector, Scalar>
{};
#endif

#ifdef __GXX_CONCEPTS__
concept InnerProduct<typename I, typename Vector,
		     typename Scalar = typename Vector::value_type>
  : std::Callable2<I, Vector, Vector>
{
    // Result of the inner product must be convertible to Scalar
    requires std::Convertible<std::Callable2<I, Vector, Vector>::result_type, Scalar>;

    // Let's try without this
    // requires ets::InnerProduct<I, Vector, Scalar>;

    requires HasConjugate<Scalar>;

    axiom ConjugateSymmetry(I inner, Vector v, Vector w)
    {
	inner(v, w) == conj(inner(w, v));
    }

    axiom SequiLinearity(I inner, Scalar a, Scalar b, Vector u, Vector v, Vector w)
    {
	inner(v, b * w) == b * inner(v, w);
	inner(u, v + w) == inner(u, v) + inner(u, w);
	// This implies the following (will compilers infere/deduce?)
	inner(a * v, w) == conj(a) * inner(v, w);
	inner(u + v, w) == inner(u, w) + inner(v, w);
    }

    requires RealMagnitude<Scalar>;
    typename magnitude_type = RealMagnitude<Scalar>::type;
    // requires FullLessThanComparable<magnitude_type>;

    axiom NonNegativity(I inner, Vector v, MagnitudeType<Scalar>::type magnitude)
    {
	// inner(v, v) == conj(inner(v, v)) implies inner(v, v) is real
	// ergo representable as magnitude type
	magnitude_type(inner(v, v)) >= zero(magnitude)
    }

    axiom NonDegeneracy(I inner, Vector v, Vector w, Scalar s)
    {
	if (v == zero(v))
	    inner(v, w) == zero(s);
	if (inner(v, w) == zero(s))
	    v == zero(v);
    }
};
#else
    //! Concept InnerProduct
    /*!
        Semantic requirements of a inner product

	\param I        The inner product functor
	\param Vector   The the type of a vector or a collection
        \param Scalar   The scalar over which the vector field is defined

	\par Refinement of:
	- std::Callable2 <I, Vector, Vector>

	\par Associated types:
	- magnitude_type

	\par Requires:
	- std::Convertible<std::Callable2 <I, Vector, Vector>::result_type, Scalar> ;
	  result of inner product convertible to scalar to be used in expressions
	- HasConjugate < Scalar >
	- RealMagnitude < Scalar > ; the scalar value needs a real magnitude type
    */
template <typename I, typename Vector,
          typename Scalar = typename Vector::value_type>
struct InnerProduct
  : std::Callable2<I, Vector, Vector>
{
    /// Associated type: the  real magnitude type of the scalar
    /** By default RealMagnitude<Scalar>::type */
    typename associated_type magnitude_type;
    // requires FullLessThanComparable<magnitude_type>;

    /// The arguments can be changed and the result is then the complex conjugate
    axiom ConjugateSymmetry(I inner, Vector v, Vector w)
    {
	/// inner(v, w) == conj(inner(w, v));
    }

    /// The inner product is linear in the second argument and conjugate linear in the first one
    /** The equalities are partly redundant with ConjugateSymmetry */
    axiom SequiLinearity(I inner, Scalar a, Scalar b, Vector u, Vector v, Vector w)
    {
	/// inner(v, b * w) == b * inner(v, w);

	/// inner(u, v + w) == inner(u, v) + inner(u, w);

	/// inner(a * v, w) == conj(a) * inner(v, w);

	/// inner(u + v, w) == inner(u, w) + inner(v, w);
    }

    /// The inner product of a vector with itself is not negative
    /** inner(v, v) == conj(inner(v, v)) implies inner(v, v) is representable as real */
    axiom NonNegativity(I inner, Vector v, MagnitudeType<Scalar>::type magnitude)
    {
	/// magnitude_type(inner(v, v)) >= zero(magnitude);
    }

    /// Non-degeneracy not representable with axiom
    axiom NonDegeneracy(I inner, Vector v, Vector w, Scalar s)
    {
	/// \f$\langle v, w\rangle = 0 \forall w \Leftrightarrow v = \vec{0}\f$
    }
};
#endif

#ifdef __GXX_CONCEPTS_
// A dot product is only a semantically special case of an inner product
// Questionable if we want such a concept
concept DotProduct<typename I, typename Vector,
		   typename Scalar = typename Vector::value_type>
  : InnerProduct<I, Vector, Scalar>
{};
#else
    //! Concept DotProduct
    /*!
        Semantic requirements of dot product. The dot product is a specific inner product.

	\param I        Norm functor
	\param Vector   The the type of a vector or a collection
        \param Scalar   The scalar over which the vector field is defined

	\par Refinement of:
	- InnerProduct <I, Vector, Scalar>
    */
template <typename I, typename Vector,
	  typename Scalar = typename Vector::value_type>
struct DotProduct
  : InnerProduct<I, Vector, Scalar>
{};
#endif

// Norm induced by inner product
// Might be moved to another place later
// Definition as class and function
// Conversion from scalar to magnitude_type is covered by norm concept
template <typename I, typename Vector,
	  typename Scalar = typename Vector::value_type>
  _GLIBCXX_WHERE(InnerProduct<I, Vector, Scalar>
		 && RealMagnitude<Scalar>)
struct induced_norm_t
{
    // Return type evtl. with macro to use concept definition
    typename magnitude_type_trait<Scalar>::type
    operator() (const I& inner, const Vector& v)
    {
	// Check whether inner product is positive real
	// assert(Scalar(abs(inner(v, v))) == inner(v, v));

	// Similar check while accepting small imaginary values
	// assert( (abs(inner(v, v)) - inner(v, v)) / abs(inner(v, v)) < 1e-6; )

	// Could also be defined with abs but that might introduce extra ops
	// typedef RealMagnitude<Scalar>::type magnitude_type;

	typedef typename magnitude_type_trait<Scalar>::type magnitude_type;
	return sqrt(static_cast<magnitude_type> (inner(v, v)));
    }
};

#if 0
template <typename I, typename Vector,
	  typename Scalar = typename Vector::value_type>
  LA_WHERE( InnerProduct<I, Vector, Scalar>
	    && RealMagnitude<Scalar> )
magnitude_type_trait<Scalar>::type
induced_norm(const I& inner, const Vector& v)
{
    return induced_norm_t<I, Vector, Scalar>() (inner, v);
}
#endif

#ifdef __GXX_CONCEPTS__

concept HilbertSpace<typename I, typename Vector,
		     typename Scalar = typename Vector::value_type,
		     typename N = induced_norm_t<I, Vector, Scalar> >
  : InnerProduct<I, Vector, Scalar>,
    BanachSpace<N, Vector, Scalar>
{
    axiom Consistency(Vector v)
    {
	math::induced_norm_t<I, Vector, Scalar>()(v) == N()(v);
    }
};
#else
    //! Concept HilbertSpace
    /*!
        A Hilbert space is a vector space with an inner product that induces a norm

	\param I        Inner product functor
	\param Vector   The the type of a vector or a collection
        \param Scalar   The scalar over which the vector field is defined
	\param N        Norm functor

	\par Refinement of:
	- InnerProduct <I, Vector, Scalar>
	- BanachSpace <N, Vector, Scalar>

	\note
	- The (expressible) requirements of Banach Space are already given in InnerProduct
	  (besides consistency of the functors).
	- A difference is that InnerProduct is not a refinement of Vectorspace
    */
template <typename I, typename Vector,
          typename Scalar = typename Vector::value_type,
	  typename N = induced_norm_t<I, Vector, Scalar> >
struct HilbertSpace
  : InnerProduct<I, Vector, Scalar>,
    BanachSpace<N, Vector, Scalar>
{
    /// Consistency between norm and induced norm
    axiom Consistency(Vector v)
    {
	/// math::induced_norm_t<I, Vector, Scalar>()(v) == N()(v);
    }
};
#endif // __GXX_CONCEPTS__

/*@}*/ // end of group Concepts

} // namespace math

#endif // LA_VECTOR_CONCEPTS_INCLUDE


/* round_test.cc                                            -*- C++ -*-
   Jeremy Barnes, 16 February 2007
   Copyright (c) 2007 Jeremy Barnes.  All rights reserved.

   Test of rounding function.
*/

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK

#include "jml/math/round.h"
#include <limits>
#include <boost/test/unit_test.hpp>
#include <boost/test/auto_unit_test.hpp>
#include <stdint.h>

using namespace ML;
using namespace std;

using boost::unit_test::test_suite;

template<size_t width>
struct equivalent_int {};

template<>
struct equivalent_int<4> {
    typedef uint32_t type;
};

template<>
struct equivalent_int<8> {
    typedef uint64_t type;
};

template<class X>
typename equivalent_int<sizeof(X)>::type
bitsof(X x)
{
    union {
        X x;
        typename equivalent_int<sizeof(X)>::type eq;
    } res;
    res.x = x;
    return res.eq;
}

template<class X>
void test_type()
{
    BOOST_CHECK_BITWISE_EQUAL(bitsof<X>(round(0.0)), bitsof<X>(0.0));
    BOOST_CHECK_BITWISE_EQUAL(bitsof<X>(round(-0.0)), bitsof<X>(-0.0));
    BOOST_CHECK_EQUAL(round((X) 1.0),  (X) 1.0);
    BOOST_CHECK_EQUAL(round((X)-1.0),  (X)-1.0);
    BOOST_CHECK_EQUAL(round((X) 0.5),  (X) 1.0);
    BOOST_CHECK_EQUAL(round((X)-0.5),  (X)-1.0);
    BOOST_CHECK_EQUAL(round((X) 0.49), (X) 0.0);
    BOOST_CHECK_EQUAL(round((X)-0.49), (X)-0.0);
    BOOST_CHECK_EQUAL(round((X) 0.49), (X) 0.0);
    BOOST_CHECK_EQUAL(round((X) 0.51), (X) 1.0);
    BOOST_CHECK_EQUAL(round((X)-0.51), (X)-1.0);
    BOOST_CHECK_EQUAL(round((X) 11.0),  (X) 11.0);
    BOOST_CHECK_EQUAL(round((X)-11.0),  (X)-11.0);
    BOOST_CHECK_EQUAL(round((X) 10.5),  (X) 11.0);
    BOOST_CHECK_EQUAL(round((X)-10.5),  (X)-11.0);
    BOOST_CHECK_EQUAL(round((X) 10.49), (X) 10.0);
    BOOST_CHECK_EQUAL(round((X)-10.49), (X)-10.0);
    BOOST_CHECK_EQUAL(round((X) 10.49), (X) 10.0);
    BOOST_CHECK_EQUAL(round((X) 10.51), (X) 11.0);
    BOOST_CHECK_EQUAL(round((X)-10.51), (X)-11.0);
    BOOST_CHECK_BITWISE_EQUAL(bitsof<X>(round(INFINITY)), bitsof<X>(INFINITY));
    BOOST_CHECK_BITWISE_EQUAL(bitsof<X>(round(-INFINITY)),
                              bitsof<X>(-INFINITY));
    BOOST_CHECK_BITWISE_EQUAL(bitsof<X>(round(numeric_limits<X>::quiet_NaN())),
                              bitsof<X>(numeric_limits<X>::quiet_NaN()));
}

BOOST_AUTO_TEST_CASE( test1 )
{
    test_type<float>();
    test_type<double>();
}


#pragma once
#include <Eigen/Core>
#include <type_traits>
#include "ear/helpers/assert.hpp"
#include "ear/helpers/output_gains.hpp"

namespace ear {

  /** @brief Check if two containers have intersecting elements
   */
  template <class InputIterator1, class InputIterator2>
  bool doIntersect(InputIterator1 first1, InputIterator1 last1,
                   InputIterator2 first2, InputIterator2 last2) {
    while (first1 != last1 && first2 != last2) {
      if (*first1 < *first2)
        ++first1;
      else if (*first2 < *first1)
        ++first2;
      else {
        return true;
      }
    }
    return false;
  }

  template <int N, int M>
  double interp(double x, const Eigen::Matrix<double, N, 1> &xp,
                const Eigen::Matrix<double, M, 1> &yp) {
    ear_assert(std::is_sorted(xp.begin(), xp.end()),
               "in interp: unsorted x values");
    ear_assert(xp.size() == yp.size(),
               "in interp: must have same number of x and y points");
    ear_assert(xp.size() >= 2, "in interp: must have at least 2 points");

    if (x <= xp(0)) {
      return yp(0);
    }
    for (int i = 0; i < xp.size() - 1; ++i) {
      if (xp(i + 1) > x) {
        double x0 = xp(i);
        double x1 = xp(i + 1);
        double y0 = yp(i);
        double y1 = yp(i + 1);
        return y0 + (y1 - y0) / (x1 - x0) * (x - x0);
      }
    }
    return yp(yp.size() - 1);
  }

  // write to a vector in a way which is compatible with OutputGains
  template <typename VecT, typename ValueT>
  void vec_write(VecT &vec, size_t i, ValueT value) {
    vec[i] = value;
  }
  template <typename ValueT>
  void vec_write(OutputGains &vec, size_t i, ValueT value) {
    vec.write(i, value);
  }

  /// `mask_write(out, mask, values)` is equivalent to numpy
  /// `out[mask] = values` for boolean masks
  template <typename OutT, typename MaskT, typename ValuesT>
  void mask_write(OutT &&out, const MaskT &mask, const ValuesT &values) {
    Eigen::Index out_size = out.size();
    Eigen::Index mask_size = mask.size();
    Eigen::Index values_size = values.size();

    ear_assert(
        out_size == mask_size,
        "in mask_write: out_size and mask_write must be the same length");

    Eigen::Index j = 0;
    for (Eigen::Index i = 0; i < mask_size; i++)
      if (mask[i]) vec_write(out, i, values[j++]);

    ear_assert(j == values_size,
               "in mask_size: length of values must equal the number of "
               "entries in mask");
  }

  /// make an eigen copy of a std::vector (or other type with size() and
  /// operator[]), for use when Eigen::Map can't be used, e.g. with
  /// std::vector<bool>
  template <typename EigenT, typename ParamT,
            typename std::enable_if<EigenT::RowsAtCompileTime == Eigen::Dynamic,
                                    int>::type = 0>
  inline EigenT copy_vector(const ParamT &x) {
    Eigen::Index size = x.size();
    EigenT rv(size);

    for (Eigen::Index i = 0; i < size; i++) rv[i] = x[i];

    return rv;
  }

  template <typename EigenT, typename ParamT,
            typename std::enable_if<EigenT::RowsAtCompileTime != Eigen::Dynamic,
                                    int>::type = 0>
  inline EigenT copy_vector(const ParamT &x) {
    Eigen::Index size = x.size();
    EigenT rv;
    ear_assert(rv.rows() == size,
               "in copy_vector: incorrect size vector for Eigen type used");

    for (Eigen::Index i = 0; i < size; i++) rv[i] = x[i];

    return rv;
  }
}  // namespace ear


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// 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
//
#include <boost/gil/extension/io/jpeg.hpp>

// Example to demonstrate a way to compute gradients along x-axis

using namespace boost::gil;

template <typename Out>
struct halfdiff_cast_channels {
    template <typename T> Out operator()(const T& in1, const T& in2) const {
        return Out((in2-in1)/2);
    }
};

template <typename SrcView, typename DstView>
void x_gradient(SrcView const& src, DstView const& dst)
{
    using dst_channel_t = typename channel_type<DstView>::type;

    for (int y = 0; y < src.height(); ++y)
    {
        typename SrcView::x_iterator src_it = src.row_begin(y);
        typename DstView::x_iterator dst_it = dst.row_begin(y);

        for (int x = 1; x < src.width() - 1; ++x)
        {
            static_transform(src_it[x - 1], src_it[x + 1], dst_it[x],
                halfdiff_cast_channels<dst_channel_t>());
        }
    }
}

template <typename SrcView, typename DstView>
void x_luminosity_gradient(SrcView const& src, DstView const& dst)
{
    using gray_pixel_t = pixel<typename channel_type<SrcView>::type, gray_layout_t>;
    x_gradient(color_converted_view<gray_pixel_t>(src), dst);
}

int main()
{
    rgb8_image_t img;
    read_image("test.jpg",img, jpeg_tag{});

    gray8s_image_t img_out(img.dimensions());
    fill_pixels(view(img_out),int8_t(0));

    x_luminosity_gradient(const_view(img), view(img_out));
    write_view("out-x_gradient.jpg",color_converted_view<gray8_pixel_t>(const_view(img_out)), jpeg_tag{});

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/abs.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <nt2/include/constants/valmin.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL(abs_real,  BOOST_SIMD_REAL_TYPES)
{
  using nt2::abs;
  using nt2::tag::abs_;
  typedef typename std::complex<T> cT;
  typedef typename boost::dispatch::meta::call<abs_(cT)>::type r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::Inf<T>())), nt2::Inf<T>());
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::Minf<T>())), nt2::Inf<T>());
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::Nan<T>())), nt2::Nan<T>());
#endif
   NT2_TEST_ULP_EQUAL(nt2::abs(cT(1)), T(1), 0);
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::Mone<T>())), nt2::One<T>());
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::One<T>())), nt2::One<T>());
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::Valmax<T>())), nt2::Valmax<T>());
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::Valmin<T>())), nt2::Valmax<T>());
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::Zero<T>())), nt2::Zero<T>());
   std::complex < T > b(3, 4);
   NT2_TEST_EQUAL(nt2::abs(b), T(5));
   for(int i=-10; i < 10; i++)
   {
     for(int j=-10; j < 10; j++)
     {
       T ti = T(i);
       T tj = T(j);
       NT2_TEST_EQUAL(nt2::abs(cT(ti, tj)), nt2::hypot(ti, tj));
     }
   }
}

NT2_TEST_CASE_TPL(abs_dry,  BOOST_SIMD_REAL_TYPES)
{
  using nt2::abs;
  using nt2::tag::abs_;
  typedef typename nt2::dry<T> dT;
  typedef typename boost::dispatch::meta::call<abs_(dT)>::type r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
   NT2_TEST_EQUAL(nt2::abs(dT(nt2::Inf<T>())), nt2::Inf<T>());
   NT2_TEST_EQUAL(nt2::abs(dT(nt2::Minf<T>())), nt2::Inf<T>());
   NT2_TEST_EQUAL(nt2::abs(dT(nt2::Nan<T>())), nt2::Nan<T>());
#endif
   NT2_TEST_ULP_EQUAL(nt2::abs(dT(1)), T(1), 0);
   NT2_TEST_EQUAL(nt2::abs(dT(nt2::Mone<T>())), nt2::One<T>());
   NT2_TEST_EQUAL(nt2::abs(dT(nt2::One<T>())), nt2::One<T>());
   NT2_TEST_EQUAL(nt2::abs(dT(nt2::Valmax<T>())), nt2::Valmax<T>());
   NT2_TEST_EQUAL(nt2::abs(dT(nt2::Valmin<T>())), nt2::Valmax<T>());
   NT2_TEST_EQUAL(nt2::abs(dT(nt2::Zero<T>())), nt2::Zero<T>());
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_CBRT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_CBRT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/real_splat.hpp>
#include <boost/simd/constant/third.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/function/simd/abs.hpp>
#include <boost/simd/function/simd/bitofsign.hpp>
#include <boost/simd/function/simd/bitwise_or.hpp>
#include <boost/simd/function/simd/divides.hpp>
#include <boost/simd/function/simd/fast_frexp.hpp>
#include <boost/simd/function/simd/fast_ldexp.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/is_equal.hpp>
#include <boost/simd/function/simd/is_eqz.hpp>
#include <boost/simd/function/simd/is_gez.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/simd/function/simd/multiplies.hpp>
#include <boost/simd/function/simd/negate.hpp>
#include <boost/simd/function/simd/sqr.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/twotomnmbo_3.hpp>
#include <boost/simd/constant/twotonmb.hpp>
#include <boost/simd/function/simd/is_less.hpp>
#endif

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/function/simd/is_inf.hpp>
#include <boost/simd/function/simd/logical_or.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(cbrt_
                          , (typename A0,typename A1,typename X)
                          , bd::cpu_
                          , bs::pack_<bd::single_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( ) const BOOST_NOEXCEPT
      {
        A0 z =  nt2::abs(a0);
        using int_type =  bs::as_integer_t<A0, signed>;
        using stype =  bd::scalar_of_t<A0>;
  #ifndef BOOST_SIMD_NO_DENORMALS
        auto denormal = is_less(z, Smallestposval<A0>());
        z = if_else(denormal, z*Twotonmb<A0>(), z);
        A0 f = if_else(denormal, Twotomnmbo_3<A0>(), One<A0>());
  #endif
        const A0 CBRT2  = Constant< A0, 0x3fa14518> ();
        const A0 CBRT4  = Constant< A0, 0x3fcb2ff5> ();
        const A0 CBRT2I = Constant< A0, 0x3f4b2ff5> ();
        const A0 CBRT4I = Constant< A0, 0x3f214518> ();
        int_type e;
        A0 x = fast_frexp(z, e);
        x = horn <stype, 0xbe09e49a,
                         0x3f0bf0fe,
                         0xbf745265,
                         0x3f91eb77,
                         0x3ece0609)
                 > (x);
        auto flag = is_gez(e);
        int_type e1 =  nt2::abs(e);
        int_type rem = e1;
        e1 /= Three<int_type>();
        rem -= e1*Three<int_type>();
        e = negate(e1, e);
        const A0 cbrt2 = if_else(flag, CBRT2, CBRT2I);
        const A0 cbrt4 = if_else(flag, CBRT4, CBRT4I);
        A0 fact = if_else(is_equal(rem, One<int_type>()), cbrt2, One<A0>());
        fact = if_else(is_equal(rem, Two<int_type>()), cbrt4, fact);
        x = fast_ldexp(x*fact, e);
        x -= (x-z/sqr(x))*Third<A0>();
  #ifndef BOOST_SIMD_NO_DENORMALS
        x = bitwise_or(x, bitofsign(a0))*f;
  #else
        x = bitwise_or(x, bitofsign(a0));
  #endif
  #ifndef BOOST_SIMD_NO_INFINITIES
        return if_else(logical_or(is_eqz(a0),is_inf(a0)), a0, x);
  #else
        return if_else(is_eqz(a0), a0, x);
  #endif
      }
   };

} }

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_SIMD_COMMON_EXPX2_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SIMD_COMMON_EXPX2_HPP_INCLUDED

#include <nt2/exponential/functions/expx2.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/include/constants/expx2c1.hpp>
#include <nt2/include/constants/expx2c2.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/maxlog.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/exp.hpp>
#include <nt2/include/functions/simd/floor.hpp>
#include <nt2/include/functions/simd/fma.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/is_greater.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/plus.hpp>
#include <nt2/include/functions/simd/signnz.hpp>
#include <nt2/include/functions/simd/sqr.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/functions/simd/is_equal.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( expx2_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0) const
    {
      A0 x =  nt2::abs(a0);
      // Represent x as an exact multiple of 1/32 plus a residual.
      A0 m = Expx2c1<A0>()*nt2::floor(fma(Expx2c2<A0>(), x, nt2::Half<A0>()));
      A0 f =  x-m;
      // x**2 = m**2 + 2mf + f**2
      A0 u = nt2::sqr(m);
      A0 u1 = fma(m+m,f,sqr(f));
      // u is exact, u1 is small.
      A0 r = nt2::if_else(nt2::gt(u+u1, nt2::Maxlog<A0>()),
                          nt2::Inf<A0>(),
                          nt2::exp(u)*nt2::exp(u1));
      #ifndef BOOST_SIMD_NO_INFINITIES
      r =  nt2::if_else(eq(x, Inf<A0>()), x, r);
      #endif
      return r;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( expx2_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                              ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0,  const A0 & s) const
    {
      A0 sgn =  signnz(s);
      A0 x =  a0*sgn;
      // Represent x as an exact multiple of 1/32 plus a residual.
      A0 m = Expx2c1<A0>()*nt2::floor(fma(Expx2c2<A0>(), x, nt2::Half<A0>()));
      A0 f =  x-m;
      // x**2 = m**2 + 2mf + f**2
      A0 u = sgn*nt2::sqr(m);
      A0 u1 = sgn*fma(m+m,f,sqr(f));
      // u is exact, u1 is small.
      A0 r = nt2::if_else(nt2::gt(u+u1, nt2::Maxlog<A0>()),
                          nt2::Inf<A0>(),
                          nt2::exp(u)*nt2::exp(u1));
      #ifndef BOOST_SIMD_NO_INFINITIES
      r =  nt2::if_else(eq(x, Inf<A0>()), x, r);
      #endif
      return r;
    }
  };
} }

#endif


#include "instancesegm.pb.h"            // for ResizeArgs (generated file)
#include "scanner/api/kernel.h"   // for VideoKernel and REGISTER_KERNEL
#include "scanner/api/op.h"       // for REGISTER_OP
#include "scanner/util/memory.h"  // for device-independent memory management
#include "scanner/util/opencv.h"  // for using OpenCV
#include "scanner/util/serialize.h"

#include <opencv2/ximgproc.hpp>

#include <iostream>               // for std::cout

#include <Eigen/Sparse>
typedef float var_t;
typedef Eigen::SparseMatrix<var_t> SpMat;
typedef Eigen::Triplet<var_t> T;
typedef char byte;

#include <ctime>
#include "opencv2/highgui.hpp"
#include "opencv2/core/utility.hpp"

void getPixelNeighbors(int height, int width, std::vector<std::vector<int>>& neighborId){

    for(int i=0; i<height; i++){
        for(int j=0; j<width; j++){

            if(i == 0){
                neighborId[i*width+j].push_back((i+1)*width+j);
            }else if(i == height-1){
                neighborId[i*width+j].push_back((i-1)*width+j);
            }else{
                neighborId[i*width+j].push_back((i+1)*width+j);
                neighborId[i*width+j].push_back((i-1)*width+j);
            }

            if(j == 0){
                neighborId[i*width+j].push_back(i*width+j+1);
            }else if(j == width-1){
                neighborId[i*width+j].push_back(i*width+j-1);
            }else{
                neighborId[i*width+j].push_back(i*width+j+1);
                neighborId[i*width+j].push_back(i*width+j-1);
            }

        }
    }

}

void getLabelPosition(var_t *img, int h, int w, std::map<int, std::vector<int>>& ht){
    for(int i=0; i<h; i++) {
        for (int j = 0; j < w; j++) {

            if(img[i*w+j] >= 1.0){
                int lbl = int(img[i*w+j]-1);
                ht[lbl].push_back(i*w+j);
            }

        }
    }
}

SpMat setU(int N, std::map<int, std::vector<int>>& ht, Eigen::VectorXf& y){
    std::vector<T> tripletList;

    for(std::map<int,std::vector<int>>::iterator it = ht.begin(); it != ht.end(); ++it) {
        std::vector<int> pixLocation = it->second;
        for(int i=0; i<pixLocation.size();i++){
            tripletList.push_back(T(pixLocation[i], pixLocation[i], 1.));
            y[pixLocation[i]] = float(it->first);
        }
    }

    SpMat U(N,N);
    U.setFromTriplets(tripletList.begin(), tripletList.end());
    return U;
}

void setDW(var_t* image, var_t* edges, int h, int w, std::vector<SpMat>& out, float sigma1, float sigma2){

    int N = h * w;
    std::vector<std::vector<int>> neighborId(N);
    getPixelNeighbors(h, w, neighborId);

    std::vector<T> tripletListD;
    std::vector<T> tripletListW;
    int M = 0;

    for(int i=0; i<neighborId.size(); i++){
        int x, y;
        y = i/w;
        x = i%w;
        var_t r1 = image[y*w*3+x*3+0];
        var_t g1 = image[y*w*3+x*3+1];
        var_t b1 = image[y*w*3+x*3+2];
        var_t e1 = edges[y*w+x];

        for(int j=0; j<neighborId[i].size(); j++){

            y = neighborId[i][j]/w;
            x = neighborId[i][j]%w;
            var_t r2 = image[y*w*3+x*3+0];
            var_t g2 = image[y*w*3+x*3+1];
            var_t b2 = image[y*w*3+x*3+2];

            var_t weight0 = exp(-((r1-r2)*(r1-r2) + (g1-g2)*(g1-g2)+ (b1-b2)*(b1-b2))/sigma1);
            var_t weight1 = exp(-(e1*e1)/sigma2);

            // std::cout<<weight0<<" "<<weight1<<std::endl;

            tripletListD.push_back(T(M, i, 1.));
            tripletListD.push_back(T(M, neighborId[i][j], -1.));
            tripletListW.push_back(T(M, M, weight0*weight1));

            M++;

        }
    }

    SpMat D(M, N);
    D.setFromTriplets(tripletListD.begin(), tripletListD.end());

    SpMat W(M, M);
    W.setFromTriplets(tripletListW.begin(), tripletListW.end());
    out.push_back(D);
    out.push_back(W);
}

var_t* segmentFromPoses(var_t *img, var_t *edges, var_t *poseData, int height, int width, float sigma1, float sigma2){
    std::map<int, std::vector<int>> ht;
    getLabelPosition(poseData, height, width, ht);
    Eigen::VectorXf y(height*width);
    SpMat U = setU(height*width, ht, y);
    std::vector<SpMat> DW;
    setDW(img, edges, height, width, DW, sigma1, sigma2);
    SpMat D = DW[0];
    SpMat W = DW[1];

    Eigen::VectorXf b = U*y;

    SpMat A = U + D.transpose()*W*D;

    Eigen::SimplicialCholesky <SpMat> solver(A);
    Eigen::VectorXf x = solver.solve(b);

    var_t *output = new var_t[height*width];
    for(int i=0; i<height*width; i++)
        output[i] = x[i];

    return output;
}

class InstanceSegmentKernel : public scanner::Kernel, public scanner::VideoKernel {
 public:
  InstanceSegmentKernel(const scanner::KernelConfig& config)
      : scanner::Kernel(config) {
    auto start = scanner::now();
    MySegmentArgs args;
    args.ParseFromArray(config.args.data(), config.args.size());
    sigma1 = args.sigma1();
    sigma2 = args.sigma2();
    if (profiler_) {
      profiler_->add_interval("constructor", start, scanner::now());
    }
  }

  void execute(const scanner::Elements& input_columns,
               scanner::Elements& output_columns) override {

    auto start = scanner::now();

    auto& img_col = input_columns[0];
    auto& poseimg_col = input_columns[1];
    auto& edge_col = input_columns[2];

    check_frame(scanner::CPU_DEVICE, img_col);
    check_frame(scanner::CPU_DEVICE, poseimg_col);
    check_frame(scanner::CPU_DEVICE, edge_col);

    // Image
    const scanner::Frame* frame = img_col.as_const_frame();
    cv::Mat image = scanner::frame_to_mat(frame);

    image.convertTo(image, cv::DataType<var_t>::type, 1.0/255.0);
    var_t *imgData = (var_t*)(image.data);

    // Pose image
    const scanner::Frame* pose_img_frame = poseimg_col.as_const_frame();
    cv::Mat poseImage = scanner::frame_to_mat(pose_img_frame);

    poseImage.convertTo(poseImage, cv::DataType<var_t>::type);
    var_t *poseData = (var_t*)(poseImage.data);

    // Edges image
    const scanner::Frame* edge_frame = edge_col.as_const_frame();
    cv::Mat edges = scanner::frame_to_mat(edge_frame);
//    cv::Mat edges;
//    cv::cvtColor(_edges, edges, CV_BGR2GRAY);

    edges.convertTo(edges, cv::DataType<var_t>::type, 1.0/255.0);
    var_t *edgesData = (var_t*)(edges.data);

//std::cout<<image.channels()<<std::endl;
//std::cout<<poseImage.channels()<<std::endl;
//std::cout<<edges.channels()<<std::endl;

    // Segmentation part
    start = scanner::now();

    int height = image.rows;
    int width = image.cols;

//    for(int i=0; i<height; i++) {
//        for (int j = 0; j < width; j++) {
//        if(i == 10){
//        std::cout<<edgesData[i*width+j]<<std::endl;
//        }
//        }
//        }

//cv::namedWindow( "Display window", cv::WINDOW_AUTOSIZE );// Create a window for display.
//    cv::imshow( "Display window", edges );                   // Show our image inside it.
//    cv::waitKey(0);

    var_t* segm_output = segmentFromPoses(imgData, edgesData, poseData, height, width, sigma1, sigma2);
     if (profiler_) {
      profiler_->add_interval("segmentFromPoses", start, scanner::now());
    }

    start = scanner::now();
    cv::Mat new_mask(height, width, CV_8U);
    for(int i=0; i<height; i++) {
        for (int j = 0; j < width; j++) {
            if(segm_output[i*width+j] > 1.5)
                new_mask.at<uchar>(i,j) = 255;
            else
                new_mask.at<uchar>(i,j) = 0;
        }
    }

    ProtoImage proto_image;
    int size = new_mask.total() * new_mask.elemSize();
    byte * bytes = new byte[size];  // you will have to delete[] that later
    std::memcpy(bytes, new_mask.data, size * sizeof(byte));
    proto_image.set_image_data(bytes, size * sizeof(byte));
    proto_image.set_h(height);
    proto_image.set_w(width);
    delete []bytes;

    size_t size2 = proto_image.ByteSize();
    scanner::u8* buffer = scanner::new_buffer(scanner::CPU_DEVICE, size2);
    proto_image.SerializeToArray(buffer, size2);

    scanner::insert_element(output_columns[0], buffer, size2);
    if (profiler_) {
      profiler_->add_interval("final part", start, scanner::now());
    }
  }

 private:
  cv::Ptr<cv::ximgproc::StructuredEdgeDetection> pDollar_;
  float sigma1;
  float sigma2;
};

// These functions run statically when the shared library is loaded to tell the
// Scanner runtime about your custom op.

REGISTER_OP(InstanceSegment).frame_input("frame").frame_input("poseimg").frame_input("edges").output("frame").protobuf_name("MySegmentArgs");

REGISTER_KERNEL(InstanceSegment, InstanceSegmentKernel)
    .device(scanner::DeviceType::CPU)
    .num_devices(1);


////////////////////////////////////////////////////////////////////////////////
/// Copyright 2018-present Xinyan DAI<xinyan.dai@outlook.com>
///
/// permission is hereby granted, free of charge, to any person obtaining a copy
/// of this software and associated documentation files (the "Software"), to
/// deal in the Software without restriction, including without limitation the
/// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
/// sell copies of the Software, and to permit persons to whom the Software is
/// furnished to do so, subject to the following conditions:
///
/// The above copyright notice and this permission notice shall be included in
/// all copies or substantial portions ofthe 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.

/// @version 0.1
/// @author  Xinyan DAI
/// @contact xinyan.dai@outlook.com
//////////////////////////////////////////////////////////////////////////////

#pragma once

#include <cmath>
#include <utility>
#include <vector>
#include <index/pq.hpp>
#include <boost/math/special_functions/beta.hpp>
#include "query/sorter/bucket_sorter.hpp"

#include "parameters.hpp"
#include "query.hpp"
#include "index/transformer/norm_range.hpp"

namespace ss {

    template<
            class DataType,
            class KeyType,
            class HashingIndexType,
            class Transformer
    >
    class RationalNormRanking : public Query<DataType > {

        using MapType   = unordered_map<KeyType, vector<int>, ss::SSHasher<KeyType> >;
        using IndexType = NormRangeIndex<DataType, KeyType, HashingIndexType, Transformer >;

    private:
        const MapType &                          _index_map;
        BucketSorter<DataType, KeyType > *       _sorter;

    public:
        ~RationalNormRanking() { if(_sorter) delete _sorter; }

        explicit RationalNormRanking(
                IndexType *                        index,
                const DataType *                   query,
                const Metric<DataType > &          metric,
                const Matrix<DataType > &          data,
                const parameter &                  para)
                : Query<DataType >(index, query, metric, data, para),
                  _index_map(index->getIndexMap()) {

            { /// construction here,

                /// calculate query's hash value by hash function
                KeyType query_hash = index->HashQuery(query, -1);

                /// distance function from query to bucket(designated by index_key)
                auto distor = [this, &query_hash, &para, index](const KeyType & index_key) {

                    std::pair<int, DataType > hash_dist_and_norm
                            = index->HashDistAndPercentile(index_key, query_hash);
                    int num_same_bit = para.num_bit - hash_dist_and_norm.first;
                    DataType u = hash_dist_and_norm.second;
                    return this->distance(num_same_bit, u);
                };
                _sorter = new BucketSorter<DataType, KeyType>(_index_map, distor);

            } /// end construction
        }

        DataType distance(int num_same_bit, DataType u) {
            DataType probability = num_same_bit / static_cast<DataType>(this->_para.num_bit);
            DataType shift       = 0.40f;
            probability          = shift + (1 - shift) * probability;
            DataType dist        = - u *  std::cos( M_PI * (1.0f - probability ) );
            return dist;
        }

        const vector<int> &  NextBucket() override {
            typename MapType::const_iterator bucket;
            while ( (bucket = _index_map.find(_sorter->NextBucket()) ) ==_index_map.end()) {}
            return bucket->second;
        }

        bool NextBucketExisted() const override {
            return _sorter->NextBucketExisted();
        }

    };

} // namespace ss


/* ----------------------------------------------------------------------------
 * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 * @file  main.cpp
 * @brief Trajectory optimization for a cart pole.
 * @author Alejandro Escontrela
 */

#include <gtdynamics/dynamics/DynamicsGraph.h>
#include <gtdynamics/factors/MinTorqueFactor.h>
#include <gtdynamics/universal_robot/Robot.h>
#include <gtdynamics/universal_robot/sdf.h>
#include <gtdynamics/utils/initialize_solution_utils.h>
#include <gtsam/base/Value.h>
#include <gtsam/base/Vector.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

#include <boost/algorithm/string/join.hpp>
#include <boost/optional.hpp>
#include <fstream>
#include <iostream>
#include <string>
#include <utility>

using namespace gtdynamics;
using gtsam::noiseModel::Isotropic, gtsam::noiseModel::Constrained;

int main(int argc, char** argv) {
  // Load the inverted pendulum.
  auto cp = CreateRobotFromFile(kUrdfPath + std::string("cart_pole.urdf"));
  int j0_id = cp.joint("j0")->id(), j1_id = cp.joint("j1")->id();
  cp = cp.fixLink("l0");
  cp.print();
  gtsam::Vector3 gravity(0, 0, -9.8);

  double T = 2, dt = 1. / 100;  // Time horizon (s) and timestep duration (s).
  int t_steps = static_cast<int>(std::ceil(T / dt));  // Timesteps.

  // Noise models:
  auto dynamics_model = Isotropic::Sigma(1, 1e-7);  // Dynamics constraints.
  auto pos_objectives_model = Isotropic::Sigma(1, 1e-5);  // Pos objectives.
  auto objectives_model = Isotropic::Sigma(1, 5e-3);  // Additional objectives.
  auto control_model = Isotropic::Sigma(1, 20);       // Controls.

  // Specify initial conditions and goal state.
  // State: x, dx/dt, d^2x/dt^2, theta, dtheta/dt, d^2theta/dt
  gtsam::Vector6 X_i = gtsam::Vector6::Constant(6, 0),
                 X_T = (gtsam::Vector(6) << 1, 0, 0, M_PI, 0, 0).finished();
  std::cout << "\nInitial State: " << X_i.transpose() << std::endl;
  std::cout << "Goal State: " << X_T.transpose() << std::endl << std::endl;

  // Create trajectory factor graph.
  auto graph_builder = DynamicsGraph(gravity);
  auto graph = graph_builder.trajectoryFG(
      cp, t_steps, dt, CollocationScheme::Trapezoidal);

  // Set the pendulum joint to be unactuated.
  for (int t = 0; t <= t_steps; t++)
    graph.addPrior(internal::TorqueKey(j1_id, t), 0.0, Constrained::All(1));

  // Add initial conditions to trajectory factor graph.
  graph.addPrior(internal::JointAngleKey(j0_id, 0), X_i[0], dynamics_model);
  graph.addPrior(internal::JointVelKey(j0_id, 0), X_i[1], dynamics_model);
  graph.addPrior(internal::JointAngleKey(j1_id, 0), X_i[3], dynamics_model);
  graph.addPrior(internal::JointVelKey(j1_id, 0), X_i[4], dynamics_model);

  // Add terminal conditions to the factor graph.
  graph.addPrior(internal::JointVelKey(j0_id, t_steps), X_T[1],
                 objectives_model);
  graph.addPrior(internal::JointAccelKey(j0_id, t_steps), X_T[2],
                 objectives_model);
  graph.addPrior(internal::JointVelKey(j1_id, t_steps), X_T[4],
                 objectives_model);
  graph.addPrior(internal::JointAccelKey(j1_id, t_steps), X_T[5],
                 objectives_model);

  // Insert position objective (x, theta) factor at every timestep or only at
  // the terminal state. Adding the position objective at every timestep will
  // force the system to converge to the desired state quicker at the cost of
  // more impulsive control actions.
  bool apply_pos_objective_all_dt = false;
  graph.addPrior(internal::JointAngleKey(j0_id, t_steps), X_T[0],
                 pos_objectives_model);
  graph.addPrior(internal::JointAngleKey(j1_id, t_steps), X_T[3],
                 pos_objectives_model);
  if (apply_pos_objective_all_dt) {
    for (int t = 0; t < t_steps; t++) {
      graph.addPrior(internal::JointAngleKey(j0_id, t), X_T[0],
                     pos_objectives_model);
      graph.addPrior(internal::JointAngleKey(j1_id, t), X_T[3],
                     pos_objectives_model);
    }
  }
  for (int t = 0; t <= t_steps; t++)
    graph.emplace_shared<MinTorqueFactor>(internal::TorqueKey(j0_id, t),
                                          control_model);

  // Initialize solution.
  auto init_vals = ZeroValuesTrajectory(cp, t_steps, 0, 0.0);
  gtsam::LevenbergMarquardtParams params;
  params.setMaxIterations(40);
  params.setVerbosityLM("SUMMARY");
  gtsam::LevenbergMarquardtOptimizer optimizer(graph, init_vals, params);
  auto results = optimizer.optimize();

  // Log the joint angles, velocities, accels, torques, and current goal pose.
  std::ofstream traj_file;
  traj_file.open("traj.csv");
  traj_file << "t,x,xdot,xddot,xtau,theta,thetadot,thetaddot,thetatau\n";
  double t_elapsed = 0;
  for (int t = 0; t <= t_steps; t++, t_elapsed += dt) {
    std::vector<gtsam::Key> keys = {
        internal::JointAngleKey(j0_id, t), internal::JointVelKey(j0_id, t),
        internal::JointAccelKey(j0_id, t), internal::TorqueKey(j0_id, t),
        internal::JointAngleKey(j1_id, t), internal::JointVelKey(j1_id, t),
        internal::JointAccelKey(j1_id, t), internal::TorqueKey(j1_id, t)};
    std::vector<std::string> vals = {std::to_string(t_elapsed)};
    for (auto&& k : keys) vals.push_back(std::to_string(results.atDouble(k)));
    traj_file << boost::algorithm::join(vals, ",") << "\n";
  }
  traj_file.close();

  return 0;
}


#ifndef rgbd_primitives_HPP_
#define rgbd_primitives_HPP_

#include <iostream>
#include <vector>
#include <algorithm>

#include <Eigen/Dense>
#include <Eigen/StdVector>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/PolygonMesh.h>
#include <pcl/common/transforms.h>

//using namespace pcl;
//using namespace pcl::io;

class rgbd_primitives {
  public:
    rgbd_primitives ();

    pcl::PolygonMesh::Ptr getCylinderWithTransform(Eigen::Isometry3d transform, double base, double top, double height);
    pcl::PolygonMesh::Ptr getCubeWithTransform(Eigen::Isometry3d transform, double xdim, double ydim, double zdim);
    pcl::PolygonMesh::Ptr getSphereWithTransform(Eigen::Isometry3d transform, double radius);

    pcl::PolygonMesh::Ptr getCylinder(double base, double top, double base_height, double top_height, int slices, int stacks);
    pcl::PolygonMesh::Ptr getCube(double xdim, double ydim, double zdim);
    pcl::PolygonMesh::Ptr getSphere(double radius);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr sampleMesh(pcl::PolygonMesh::Ptr &mesh, double pts_per_msquared);
    pcl::PointXYZRGB samplePointInTriangle(pcl::PointXYZRGB p0, pcl::PointXYZRGB p1,
                                             pcl::PointXYZRGB p2);
  private:
};

#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2018 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program 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 license for more details.
 */

#include <ql/math/ode/adaptiverungekutta.hpp>
#include <ql/methods/finitedifferences/schemes/methodoflinesscheme.hpp>

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/bind.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

namespace QuantLib {

    MethodOfLinesScheme::MethodOfLinesScheme(
        const Real eps,
        const Real relInitStepSize,
        const ext::shared_ptr<FdmLinearOpComposite> & map,
        const bc_set& bcSet)
    : dt_(Null<Real>()),
      eps_(eps),
      relInitStepSize_(relInitStepSize),
      map_(map),
      bcSet_(bcSet) {
    }

    Disposable<std::vector<Real> >
    MethodOfLinesScheme::apply(Time t, const std::vector<Real>& u) const {
        map_->setTime(t, t + 0.0001);
        bcSet_.applyBeforeApplying(*map_);

        const Array dxdt = -map_->apply(Array(u.begin(), u.end()));

        std::vector<Real> retVal(dxdt.begin(), dxdt.end());
        return retVal;
    }

    void MethodOfLinesScheme::step(array_type& a, Time t) {
        QL_REQUIRE(t-dt_ > -1e-8, "a step towards negative time given");

        const std::vector<Real> v =
           AdaptiveRungeKutta<Real>(eps_, relInitStepSize_*dt_)(
               boost::bind(&MethodOfLinesScheme::apply, this, _1, _2),
               std::vector<Real>(a.begin(), a.end()),
               t, std::max(0.0, t-dt_));

        Array y(v.begin(), v.end());

        bcSet_.applyAfterSolving(y);

        a = y;
    }

    void MethodOfLinesScheme::setStep(Time dt) {
        dt_ = dt;
    }
}