/*-----------------------------------------------------------------------------+
Interval Container Library
Author: Joachim Faulhaber
Copyright (c) 2007-2009: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
/** Example std_transform.cpp \file std_transform.cpp
    \brief Fill interval containers from user defined objects using std::transform.

    Example std_transform shows how algorithm std::transform can be used to
    fill interval containers from std::containers of objects of a user
    defined class.

    \include std_transform_/std_transform.cpp
*/
//[example_std_transform
#include <iostream>
#include <vector>
#include <algorithm>
#include <boost/icl/split_interval_map.hpp>
#include <boost/icl/separate_interval_set.hpp>

using namespace std;
using namespace boost;
using namespace boost::icl;

// Suppose we are working with a class called MyObject, containing some
// information about interval bounds e.g. _from, _to and some data members
// that carry associated information like e.g. _value.
class MyObject
{
public:
    MyObject(){}
    MyObject(int from, int to, int value): _from(from), _to(to), _value(value){}
    int from()const {return _from;}
    int to()const   {return _to;}
    int value()const{return _value;}
private:
    int _from;
    int _to;
    int _value;
};

// ... in order to use the std::transform algorithm to fill
// interval maps with MyObject data we need a function
// 'to_segment' that maps an object of type MyObject into
// the value type to the interval map we want to tranform to ...
pair<discrete_interval<int>, int> to_segment(const MyObject& myObj)
{
    return std::pair< discrete_interval<int>, int >
        (discrete_interval<int>::closed(myObj.from(), myObj.to()), myObj.value());
}

// ... there may be another function that returns the interval
// of an object only
discrete_interval<int> to_interval(const MyObject& myObj)
{
    return discrete_interval<int>::closed(myObj.from(), myObj.to());
}

// ... make_object computes a sequence of objects to test.
vector<MyObject> make_objects()
{
    vector<MyObject> object_vec;
    object_vec.push_back(MyObject(2,3,1));
    object_vec.push_back(MyObject(4,4,1));
    object_vec.push_back(MyObject(1,2,1));
    return object_vec;
}

// ... show_objects displays the sequence of input objects.
void show_objects(const vector<MyObject>& objects)
{
    vector<MyObject>::const_iterator iter = objects.begin();
    while(iter != objects.end())
    {
        cout << "([" << iter->from() << "," << iter->to() << "],"
             << iter->value() << ")";
        ++iter;
    }
}

void std_transform()
{
    // This time we want to transform objects into a splitting interval map:
    split_interval_map<int,int> segmap;
    vector<MyObject> myObjects = make_objects();

    // Display the input
    cout << "input sequence: "; show_objects(myObjects); cout << "\n\n";

    // Use an icl::inserter to fill the interval map via inserts
    std::transform(myObjects.begin(), myObjects.end(),
                   icl::inserter(segmap, segmap.end()),
                   to_segment);
    cout << "icl::inserting: " << segmap << endl;
    segmap.clear();

    // In order to compute aggregation results on associated values, we
    // usually want to use an icl::adder instead of an std or icl::inserter
    std::transform(myObjects.begin(), myObjects.end(),
                   icl::adder(segmap, segmap.end()),
                   to_segment);
    cout << "icl::adding   : " << segmap << "\n\n";

    separate_interval_set<int> segset;
    std::transform(myObjects.begin(), myObjects.end(),
                   icl::adder   (segset, segset.end()),
    // could be a  icl::inserter(segset, segset.end()), here: same effect
                   to_interval);

    cout << "Using std::transform to fill a separate_interval_set:\n\n";
    cout << "icl::adding   : " << segset << "\n\n";
}

int main()
{
    cout << ">>  Interval Container Library: Example std_transform.cpp  <<\n";
    cout << "------------------------------------------------------------\n";
    cout << "Using std::transform to fill a split_interval_map:\n\n";

    std_transform();
    return 0;
}

// Program output:
/*----------------------------------------------------------
>>  Interval Container Library: Example std_transform.cpp  <<
------------------------------------------------------------
Using std::transform to fill a split_interval_map:

input sequence: ([2,3],1)([4,4],1)([1,2],1)

icl::inserting: {([1,2)->1)([2,3]->1)([4,4]->1)}
icl::adding   : {([1,2)->1)([2,2]->2)((2,3]->1)([4,4]->1)}

Using std::transform to fill a separate_interval_set:

icl::adding   : {[1,3][4,4]}
----------------------------------------------------------*/
//]


//
// Add --log_level=message to see the messages!
//
#include <boost/test/unit_test.hpp>

#include <sstream>

#include "layers/smaxlayer.h"
#include "core/functions.h"
#include "core/utils.h"

#include "timer.h"
#include "test_utils.h"
#include "test_layers.h"

using namespace std;
using namespace boost;
using namespace boost::unit_test;
using namespace yann;
using namespace yann::test;

struct SoftmaxLayerTestFixture
{
  SoftmaxLayerTestFixture()
  {

  }
  ~SoftmaxLayerTestFixture()
  {

  }
};
// struct SoftmaxLayerTestFixture

BOOST_FIXTURE_TEST_SUITE(SoftmaxLayerTest, SoftmaxLayerTestFixture);

BOOST_AUTO_TEST_CASE(SoftmaxLayer_IO_Test)
{
  BOOST_TEST_MESSAGE("*** SoftmaxLayer IO test ...");

  const MatrixSize size = 7;
  SoftmaxLayer one(size);
  one.init(Layer::InitMode_Random, boost::none);

  BOOST_TEST_MESSAGE("SoftmaxLayer before writing to file: " << "\n" << one);
  ostringstream oss;
  oss << one;
  BOOST_CHECK(!oss.fail());

  SoftmaxLayer two(size);
  std::istringstream iss(oss.str());
  iss >> two;
  BOOST_CHECK(!iss.fail());
  BOOST_TEST_MESSAGE("SoftmaxLayer after loading from file: " << "\n" << two);

  BOOST_CHECK(one.is_equal(two, TEST_TOLERANCE));
}

BOOST_AUTO_TEST_CASE(SoftmaxLayer_FeedForward_Test)
{
  BOOST_TEST_MESSAGE("*** SoftmaxLayer FeedForward test ...");

  const MatrixSize size = 3;
  const MatrixSize batch_size = 2;

  VectorBatch input, expected_output;
  resize_batch(input, batch_size, size);
  resize_batch(expected_output, batch_size, size);

  input <<
      10, 10, 10,
      //////////
      10, 2,  6;
  expected_output <<
      0.33333, 0.33333, 0.33333,
      /////////////////////////
      0.98169, 0.00033, 0.01798;

  auto layer = make_unique<SoftmaxLayer>(size);
  BOOST_CHECK(layer);
  test_layer_feedforward(*layer, input, expected_output);
}

BOOST_AUTO_TEST_CASE(SoftmaxLayer_Backprop_Test)
{
  BOOST_TEST_MESSAGE("*** SoftmaxLayer Backprop test ...");

  const MatrixSize size = 3;
  const MatrixSize batch_size = 2;
  const double learning_rate = 1.0;
  const size_t epochs = 1000;

  VectorBatch input, expected_output;
  resize_batch(input, batch_size, size);
  resize_batch(expected_output, batch_size, size);

  input <<
      10, 10, 10,
      //////////
      10, 2,  6;

  expected_output <<
      0.8, 0.1, 0.1,
      /////////////
      0.2, 0.3, 0.5;

  auto layer = make_unique<SoftmaxLayer>(size);
  BOOST_CHECK(layer);
  test_layer_backprop(
      *layer,
      input,
      boost::none,
      expected_output,
      make_unique<QuadraticCost>(),
      learning_rate,
      epochs
  );
}

BOOST_AUTO_TEST_CASE(RandomBackprop_Test)
{
  BOOST_TEST_MESSAGE("*** SoftmaxLayer backprop with random inputs and weights ...");

  const MatrixSize size = 10;
  const MatrixSize batch_size = 5;
  const double learning_rate = 1.0;
  const size_t epochs = 20000;

  auto layer = make_unique<SoftmaxLayer>(size);
  BOOST_CHECK(layer);
  layer->init(Layer::InitMode_Random, Layer::InitContext(123));

  test_layer_backprop_from_random(
      *layer,
      batch_size,
      make_unique<QuadraticCost>(),
      learning_rate,
      epochs
  );
}

BOOST_AUTO_TEST_SUITE_END()


// ---------------------------------------------------------------------------
//
//  This file is part of PermLib.
//
// Copyright (c) 2009-2011 Thomas Rehn <thomas@carmen76.de>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. The name of the author may not be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
// OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
// IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
// NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// ---------------------------------------------------------------------------

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE BSGS generators test
#include <boost/test/unit_test.hpp>

#include <iostream>
#include <fstream>

#include <permlib/permutation.h>
#include <permlib/generator/product_replacement_generator.h>
#include <permlib/bsgs.h>
#include <permlib/construct/schreier_sims_construction.h>
#include <permlib/transversal/explicit_transversal.h>
#include <permlib/generator/bsgs_generator.h>

#include "test-common.h"

#include <boost/foreach.hpp>

using namespace permlib;
using namespace permlib::test;

BOOST_AUTO_TEST_CASE( product_replacement_generator )
{
	typedef Permutation PERM;
	typedef std::list<PERM::ptr> PERMlist;

	PERMlist generators;
	const unsigned int n = readGroup<PERM>(info_1997.filename, generators);

	boost::indirect_iterator<PERMlist::iterator> begin(generators.begin()), end(generators.end());
	std::vector<PERM> originalGenerators(begin, end);

	ProductReplacementGenerator<PERM> rng(n, generators.begin(), generators.end());
	for (unsigned int i=0; i<100; ++i)
		rng.next();

	unsigned int index = 0;
	BOOST_FOREACH(boost::shared_ptr<PERM> p, generators) {
		BOOST_CHECK( *p == originalGenerators[index] );
		++index;
	}
}

void testBSGS(const GroupInformation& info) {
	typedef Permutation PERM;
	typedef ExplicitTransversal<PERM> TRANS;

	BSGS<PERM, TRANS> bsgs = construct<PERM,TRANS>(info);
	BSGSGenerator<TRANS> bsgsGen(bsgs.U);

	unsigned int c = 0;
	while (bsgsGen.hasNext()) {
		PERM p = bsgsGen.next();
		++c;
	}
	BOOST_CHECK_EQUAL( c, info.order );
}

BOOST_AUTO_TEST_CASE( bsgs_generator )
{
	testBSGS(info_Klein4);
	testBSGS(info_1997);
	testBSGS(info_cyclic500);
	testBSGS(info_S6_3);
}


// Filename: move_example.cpp (part of MTL4)

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

inline mtl::dense2D<double> make_identity(int m, int n)
{
    mtl::dense2D<double>  A(m, n);
    A= 1.0;
    return A;
}

int main()
{
    mtl::dense2D<double>   A;
    A= make_identity(3, 4);

    std::cout << "A is\n" << A;
    return 0;
}


// Copyright (C) 2008-2018 Lorenzo Caminiti
// Distributed under the Boost Software License, Version 1.0 (see accompanying
// file LICENSE_1_0.txt or a copy at http://www.boost.org/LICENSE_1_0.txt).
// See: http://www.boost.org/doc/libs/release/libs/contract/doc/html/index.html

#include <boost/contract.hpp>
#include <vector>
#include <algorithm>
#include <limits>

int main() {
    std::vector<int> v;
    v.push_back(1);
    v.push_back(2);
    v.push_back(3);

    //[lambda
    int total = 0;
    std::for_each(v.cbegin(), v.cend(),
        // Contract for a lambda function.
        [&total] (int const x) {
            boost::contract::old_ptr<int> old_total =
                    BOOST_CONTRACT_OLDOF(total);
            boost::contract::check c = boost::contract::function()
                .precondition([&] {
                    BOOST_CONTRACT_ASSERT(
                            total + x <= std::numeric_limits<int>::max());
                })
                .postcondition([&] {
                    BOOST_CONTRACT_ASSERT(total == *old_total + x);
                })
            ;

            total += x; // Lambda function body.
        }
    );
    //]

    assert(total == 6);
    return 0;
}


#ifndef SEQUENTIAL_LINE_SEARCH_SEQUENTIAL_LINE_SEARCH_HPP
#define SEQUENTIAL_LINE_SEARCH_SEQUENTIAL_LINE_SEARCH_HPP

#include <Eigen/Core>
#include <memory>
#include <sequential-line-search/acquisition-function.hpp>
#include <sequential-line-search/current-best-selection-strategy.hpp>
#include <sequential-line-search/kernel-type.hpp>
#include <utility>

namespace sequential_line_search
{
    class PreferenceRegressor;
    class Slider;
    class PreferenceDataManager;

    std::pair<Eigen::VectorXd, Eigen::VectorXd> GenerateRandomSliderEnds(const int num_dims);
    std::pair<Eigen::VectorXd, Eigen::VectorXd> GenerateCenteredFixedLengthRandomSliderEnds(const int num_dims);

    /// \brief Optimizer class for performing sequential line search.
    ///
    /// \details This class assumes that the search space is [0, 1]^{D}.
    class SequentialLineSearchOptimizer
    {
    public:
        /// \brief Construct an optimizer instance.
        ///
        /// \param use_slider_enlargement When this is set true, slider spaces will be enlarged in post-processing. See
        /// [Koyama+17] for details.
        ///
        /// \param use_map_hyperparams When this is set true, the optimizer always perform the MAP estimation for the
        /// GPR kernel hyperparameters. When this is set false, the optimizer performs the MAP estimation only for
        /// goodness values.
        SequentialLineSearchOptimizer(
            const int                 num_dims,
            const bool                use_slider_enlargement = true,
            const bool                use_map_hyperparams    = true,
            const KernelType          kernel_type            = KernelType::ArdMatern52Kernel,
            const AcquisitionFuncType acquisition_func_type  = AcquisitionFuncType::ExpectedImprovement,
            const std::function<std::pair<Eigen::VectorXd, Eigen::VectorXd>(const int)>& initial_query_generator =
                GenerateRandomSliderEnds,
            const CurrentBestSelectionStrategy current_best_selection_strategy =
                CurrentBestSelectionStrategy::LargestExpectValue);

        /// \brief Specify (kernel and other) hyperparameter values.
        ///
        /// \details When the MAP estimation is enabled, the specified kernel hyperparameters will be used as the median
        /// of the prior distribution and the initial solution of the estimation. When the MAP estimation is not
        /// enabled, these values will be directly used.
        void SetHyperparams(const double kernel_signal_var            = 0.500,
                            const double kernel_length_scale          = 0.500,
                            const double noise_level                  = 0.005,
                            const double kernel_hyperparams_prior_var = 0.250,
                            const double btl_scale                    = 0.010);

        /// \brief Submit the result of user-performed line search and go to the next iteration step.
        ///
        /// \param slider_position The position of the chosen point in the slider space. It should be represented in [0,
        /// 1], where 0 means that the chosen position is the first end-point of the slider and 1 means that it is the
        /// second end-point.
        ///
        /// \details The computational effort for finding the global maximizer of the acquisition function will be
        /// automatically set by a naive heuristics.
        void SubmitFeedbackData(const double slider_position);

        /// \brief Submit the result of user-performed line search and go to the next iteration step.
        void SubmitFeedbackData(const double slider_position,
                                const int    num_map_estimation_iters,
                                const int    num_global_search_iters,
                                const int    num_local_search_iters);

        /// \brief Get the slider end-points.
        ///
        /// \details When the slider enlargement is not enabled, the first end-point is the maximizer among the observed
        /// points, and the second one is the maximizer of the acquisition function.
        std::pair<Eigen::VectorXd, Eigen::VectorXd> GetSliderEnds() const;

        /// \brief Calculate data point from a slider position.
        Eigen::VectorXd CalcPointFromSliderPosition(const double slider_position) const;

        /// \brief Get the point that has the highest value among the observed points.
        Eigen::VectorXd GetMaximizer() const;

        double GetPreferenceValueMean(const Eigen::VectorXd& point) const;
        double GetPreferenceValueStdev(const Eigen::VectorXd& point) const;
        double GetAcquisitionFuncValue(const Eigen::VectorXd& point) const;

        const Eigen::MatrixXd& GetRawDataPoints() const;

        void DampData(const std::string& directory_path) const;

        /// \brief Set the hyperparameter in the GP-UCB algorithm.
        ///
        /// \details This hyperparameter controls the trade-off of exploration and exploitation. Specifically, this
        /// hyperparameter corresponds to the square root of the beta in [Srinivas et al. ICML '10].
        ///
        /// If the acquisition function is not GP-UCB, this value will not be used.
        void SetGaussianProcessUpperConfidenceBoundHyperparam(const double hyperparam)
        {
            m_gaussian_process_upper_confidence_bound_hyperparam = hyperparam;
        }

    private:
        const bool m_use_slider_enlargement;
        const bool m_use_map_hyperparams;

        const CurrentBestSelectionStrategy m_current_best_selection_strategy;

        std::shared_ptr<PreferenceRegressor>   m_regressor;
        std::shared_ptr<Slider>                m_slider;
        std::shared_ptr<PreferenceDataManager> m_data;

        double m_kernel_signal_var;
        double m_kernel_length_scale;
        double m_noise_level;
        double m_kernel_hyperparams_prior_var;
        double m_btl_scale;

        const KernelType          m_kernel_type;
        const AcquisitionFuncType m_acquisition_func_type;

        double m_gaussian_process_upper_confidence_bound_hyperparam;
    };
} // namespace sequential_line_search

#endif // SEQUENTIAL_LINE_SEARCH_SEQUENTIAL_LINE_SEARCH_HPP


#ifndef __penglei_bitmanip_h__
#define __penglei_bitmanip_h__

#include <assert.h>
#include <limits.h>
//#include <boost/static_assert.hpp>
#include "config.hpp"

#if defined(__GNUC__) && __GNUC__ * 1000 + __GNUC_MINOR__ >= 4005
	#include <immintrin.h>
#endif

#if defined(_MSC_VER) && _MSC_VER >= 1500 || defined(__CYGWIN__)
	#include <intrin.h>
#endif

namespace nark {

// select best popcount implementations
//
#if defined(_MSC_VER) && _MSC_VER >= 1500 // VC 2008
//  different popcount instructions in different headers

	#define fast_popcount64 __popcnt64
	#define fast_clz32  __lzcnt
	#define fast_clz16  __lzcnt16

    #pragma intrinsic(_BitScanForward)
    #define fast_popcount32 __popcnt
    #if defined(_WIN64) || defined(_M_X64) || defined(_M_IA64)
        #pragma intrinsic(_BitScanForward64)
		#define fast_clz64  __lzcnt64
    #else
        inline int fast_clz64(unsigned __int64 x) {
			unsigned long lo32 = (unsigned long)(x & 0xFFFFFFFF);
            unsigned long hi32 = (unsigned long)(x >> 32);
            if (hi32)
				return __lzcnt(hi32);
            else
				return __lzcnt(lo32) + 32;
        }
        inline int fast_popcount64(unsigned __int64 x)
        {
            return
                __popcnt((unsigned)(x      )) +
                __popcnt((unsigned)(x >> 32));
        }
    #endif
        inline int fast_ctz32(unsigned int x) {
            assert(0 != x);
            unsigned long c;
            _BitScanForward(&c, x);
            return c;
        }
        inline int fast_ctz64(unsigned __int64 x) {
            assert(0 != x);
            unsigned long c = 0;
        #if defined(_WIN64) || defined(_M_X64) || defined(_M_IA64)
            _BitScanForward64(&c, x);
        #else
            unsigned long x1 = (unsigned long)(x & 0xFFFFFFFF);
            unsigned long x2 = (unsigned long)(x >> 32);
            if (x1) {
                _BitScanForward(&c, x1);
            } else {
                if (_BitScanForward(&c, x2))
	                c += 32;
				else
					c = 64; // all bits are zero
            }
        #endif
            return c;
        }
#elif defined(__GNUC__) && __GNUC__ >= 4 || \
      defined(__INTEL_COMPILER) && __INTEL_COMPILER >= 900 || \
      defined(__clang__)
    #define fast_popcount32 __builtin_popcount
	#define fast_clz32      __builtin_clz
	#define fast_ctz32      __builtin_ctz

    #ifdef ULONG_MAX
        #if ULONG_MAX == 0xFFFFFFFF
            inline int fast_popcount64(unsigned long long x) {
                return __builtin_popcount((unsigned)(x      )) +
                       __builtin_popcount((unsigned)(x >> 32));
            }
			#define fast_clz64      __builtin_clzll
            #define fast_ctz64      __builtin_ctzll
        #else
            #define fast_popcount64 __builtin_popcountl
			#define fast_clz64      __builtin_clzl
			#define fast_ctz64      __builtin_ctzl
        #endif
    #else
        #error "ULONG_MAX must be defined, use #include <limits.h>"
    #endif
#else // unknow or does not support popcount
  inline int fast_popcount32(unsigned int v) {
    v = v - ((v >> 1) & 0x55555555); // reuse input as temporary
    v = (v & 0x33333333) + ((v >> 2) & 0x33333333); // temp
    return (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
  }
  #if NARK_WORD_BITS == 64
    inline long long fast_popcount64(unsigned long long v) {
      v = v - ((v >> 1) & 0x5555555555555555ull);
      v = (v & 0x3333333333333333ull) + ((v >> 2) & 0x3333333333333333ull);
      v = (v + (v >> 4)) & 0x0F0F0F0F0F0F0F0Full;
      return (v * 0x0101010101010101ull) >> 56;
    }
  #else
    inline int fast_popcount64(unsigned long long v) {
		return fast_popcount32((unsigned)(v))
			 + fast_popcount32((unsigned)(v >> 32));
    }
  #endif
#endif // select best popcount implementations

#if NARK_WORD_BITS >= 64
  #if defined(__INTEL_COMPILER) && !defined(_MSC_VER)
	inline unsigned nark_bsr_u64(unsigned __int64 i) {
		unsigned j;
		__asm
		{
			bsr rax, i
			mov j, rax
		}
		return j;
	}
  #elif defined(_MSC_VER)
	inline int nark_bsr_u64(unsigned __int64 x) {
		unsigned long Index = 0;
		_BitScanReverse64(&Index, x);
		return Index;
	}
  #elif defined(__GNUC__) && __GNUC__ * 1000 + __GNUC_MINOR__ >= 4005
	#define nark_bsr_u64 __bsrq
  #endif
#endif

#if defined(_MSC_VER) && !defined(__INTEL_COMPILER)
	inline int nark_bsr_u32(unsigned long x) {
		unsigned long Index = 0;
		_BitScanReverse(&Index, x);
		return Index;
	}
#elif defined(__clang__)
	inline int nark_bsr_u32(unsigned x) { return 31 - __builtin_clz(x); }
#else
    // for gcc/clang/icc
	#define nark_bsr_u32 _bit_scan_reverse
#endif

#if defined(__CYGWIN__) && 0
// cygwin gcc bittest has problems
// other gcc has no bittest
//  #define NARK_BIT_TEST_USE_INTRINSIC
    #if NARK_WORD_BITS == 64
	  BOOST_STATIC_ASSERT(sizeof(long) == 8); // LP model
	  inline bool nark_bit_test(const long* a, long i) { return _bittest64(reinterpret_cast<const __int64*>(a), i) ? 1 : 0; }
	  inline bool nark_bit_test(const long long* a, long i) { return _bittest64(reinterpret_cast<const __int64*>(a), i) ? 1 : 0; }
	  inline bool nark_bit_test(const unsigned long* a, long i) { return _bittest64(reinterpret_cast<const __int64*>(a), i) ? 1 : 0; }
	  inline bool nark_bit_test(const unsigned long long* a, long i) { return _bittest64(reinterpret_cast<const __int64*>(a), i) ? 1 : 0; }

	  inline void nark_bit_set0(long* a, long i) { _bittestandreset64(reinterpret_cast<__int64*>(a), i); }
	  inline void nark_bit_set0(long long* a, long i) { _bittestandreset64(reinterpret_cast<__int64*>(a), i); }
	  inline void nark_bit_set0(unsigned long* a, long i) { _bittestandreset64(reinterpret_cast<__int64*>(a), i); }
	  inline void nark_bit_set0(unsigned long long* a, long i) { _bittestandreset64(reinterpret_cast<__int64*>(a), i); }

	  inline void nark_bit_set1(long* a, long i) { _bittestandset64(reinterpret_cast<__int64*>(a), i); }
	  inline void nark_bit_set1(long long* a, long i) { _bittestandset64(reinterpret_cast<__int64*>(a), i); }
	  inline void nark_bit_set1(unsigned long* a, long i) { _bittestandset64(reinterpret_cast<__int64*>(a), i); }
	  inline void nark_bit_set1(unsigned long long* a, long i) { _bittestandset64(reinterpret_cast<__int64*>(a), i); }
    #else
	  inline bool nark_bit_test(const int* a, long i) { return _bittest(a, i) ? 1 : 0; }
	  inline bool nark_bit_test(const unsigned int* a, long i) { return _bittest(reinterpret_cast<const int*>(a), i) ? 1 : 0; }
	  inline void nark_bit_set0(int* a, long i) { _bittestandreset(a, i); }
	  inline void nark_bit_set0(unsigned int* a, long i) { _bittestandreset(reinterpret_cast<int*>(a), i); }
	  inline void nark_bit_set1(int* a, long i) { _bittestandset(a, i); }
	  inline void nark_bit_set1(unsigned int* a, long i) { _bittestandset(reinterpret_cast<int*>(a), i); }
    #endif
#elif defined(_MSC_VER) || defined(__INTEL_COMPILER)
    #define NARK_BIT_TEST_USE_INTRINSIC
    #if NARK_WORD_BITS == 64
	  inline bool nark_bit_test(const __int64* a, __int64 i) { return _bittest64(a, i) ? 1 : 0; }
	  inline bool nark_bit_test(const unsigned __int64* a, __int64 i) { return _bittest64(reinterpret_cast<const __int64*>(a), i) ? 1 : 0; }
	  inline void nark_bit_set0(__int64* a, __int64 i) { _bittestandreset64(a, i); }
	  inline void nark_bit_set0(unsigned __int64* a, __int64 i) { _bittestandreset64(reinterpret_cast<__int64*>(a), i); }
	  inline void nark_bit_set1(__int64* a, __int64 i) { _bittestandset64(a, i); }
	  inline void nark_bit_set1(unsigned __int64* a, __int64 i) { _bittestandset64(reinterpret_cast<__int64*>(a), i); }
    #endif
	inline bool nark_bit_test(const long* a, long i) { return _bittest(a, i) ? 1 : 0; }
	inline bool nark_bit_test(const unsigned long* a, long i) { return _bittest(reinterpret_cast<const long*>(a), i) ? 1 : 0; }
	inline bool nark_bit_test(const int* a, long i) { return _bittest(reinterpret_cast<const long*>(a), i) ? 1 : 0; }
	inline bool nark_bit_test(const unsigned* a, long i) { return _bittest(reinterpret_cast<const long*>(a), i) ? 1 : 0; }

	inline void nark_bit_set0(int* a, long i) { _bittestandreset(reinterpret_cast<long*>(a), i); }
	inline void nark_bit_set0(unsigned* a, long i) { _bittestandreset(reinterpret_cast<long*>(a), i); }
	inline void nark_bit_set1(int* a, long i) { _bittestandset(reinterpret_cast<long*>(a), i); }
	inline void nark_bit_set1(unsigned* a, long i) { _bittestandset(reinterpret_cast<long*>(a), i); }
#endif

#if !defined(NARK_BIT_TEST_USE_INTRINSIC)
template<class Uint>
inline bool nark_bit_test(const Uint* a, size_t i) {
	return a[i/(sizeof(Uint)*8)] & (Uint(1)<<i%(sizeof(Uint)*8)) ? 1 : 0;
}
template<class Uint>
inline void nark_bit_set0(Uint* a, size_t i) {
	a[i/(sizeof(Uint)*8)] &= ~(Uint(1) << i % (sizeof(Uint) * 8));
}
template<class Uint>
inline void nark_bit_set1(Uint* a, size_t i) {
	a[i/(sizeof(Uint)*8)] |= (Uint(1) << i % (sizeof(Uint) * 8));
}
#endif

inline int fast_popcount(unsigned int x) { return fast_popcount32(x); }
inline NARK_IF_WORD_BITS_64(long long, int)
	   fast_popcount(unsigned long long x) { return fast_popcount64(x); }
#if ULONG_MAX > 0xFFFFFFFF
inline long fast_popcount(unsigned long x) { return fast_popcount64(x); }
#else
inline long fast_popcount(unsigned long x) { return fast_popcount32(x); }
#endif

#if defined(__BMI2__)
inline int fast_popcount_trail(unsigned int x, int n) { return fast_popcount32(_bzhi_u32(x, n)); }
#else
inline int fast_popcount_trail(unsigned int x, int n) { return fast_popcount32(x & ~(-1 << n)); }
#endif

#if defined(__BMI2__) && NARK_WORD_BITS >= 64
inline long long fast_popcount_trail(unsigned long long x, int n) { return fast_popcount64(_bzhi_u64(x, n)); }
#else
inline NARK_IF_WORD_BITS_64(long long, int)
fast_popcount_trail(unsigned long long x, int n) { return fast_popcount64(x & ~((unsigned long long)(-1) << n)); }
#endif

#if ULONG_MAX > 0xFFFFFFFF
inline long fast_popcount_trail(unsigned long x, int n) { return fast_popcount_trail((unsigned long long)x, n); }
#else
inline long fast_popcount_trail(unsigned long x, int n) { return fast_popcount_trail((unsigned int)x, n); }
#endif

inline int fast_clz(unsigned int x) { return fast_clz32(x); }
inline NARK_IF_WORD_BITS_64(long long, int)
	   fast_clz(unsigned long long x) { return fast_clz64(x); }
#if ULONG_MAX > 0xFFFFFFFF
inline int fast_clz(unsigned long x) { return fast_clz64(x); }
#else
inline int fast_clz(unsigned long x) { return fast_clz32(x); }
#endif

inline int fast_ctz(unsigned int x) { return fast_ctz32(x); }
inline int fast_ctz(unsigned long long x) { return fast_ctz64(x); }
#if ULONG_MAX > 0xFFFFFFFF
inline long fast_ctz(unsigned long x) { return fast_ctz64(x); }
#else
inline int fast_ctz(unsigned long x) { return fast_ctz32(x); }
#endif

template<class Uint>
static inline void bits_range_set0(Uint* data, size_t i, size_t k) {
	if (i == k) {
		return;
	}
	const static size_t UintBits = sizeof(Uint) * 8;
	size_t j = i / UintBits;
	if (j == (k - 1) / UintBits) {
		data[j] &= ~(Uint(-1) << i%UintBits)
			| (Uint(-2) << (k - 1) % UintBits);
	}
	else {
		if (i % UintBits)
			data[j++] &= ~(Uint(-1) << i%UintBits);
		while (j < k / UintBits)
			data[j++] = 0;
		if (k % UintBits)
			data[j] &= Uint(-1) << k%UintBits;
	}
}

template<class Uint>
static inline void bits_range_set1(Uint* data, size_t i, size_t k) {
	if (i == k) {
		return;
	}
	const static size_t UintBits = sizeof(Uint) * 8;
	size_t j = i / UintBits;
	if (j == (k - 1) / UintBits) {
		data[j] |= (Uint(-1) << i%UintBits)
			& ~(Uint(-2) << (k - 1) % UintBits);
	}
	else {
		if (i % UintBits)
			data[j++] |= (Uint(-1) << i%UintBits);
		while (j < k / UintBits)
			data[j++] = Uint(-1);
		if (k % UintBits)
			data[j] |= ~(Uint(-1) << k%UintBits);
	}
}

template<class Uint>
static inline void bits_range_set(Uint* data, size_t i, size_t k, bool val) {
	if (val)
		bits_range_set1(data, i, k);
	else
		bits_range_set0(data, i, k);
}

template<int Bits> class BitsToUint;
template<> class BitsToUint<32> { typedef unsigned int type; };
template<> class BitsToUint<64> { typedef unsigned long long type; };

} // namespace nark

#endif


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/

#pragma once

#include <Eigen/Core>

#include "kindr/math/LinearAlgebra.hpp"
#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/rotations/RotationDiffBase.hpp"
#include "kindr/rotations/Rotation.hpp"

namespace kindr {

/*! \class RotationMatrixDiff
 * \brief Time derivative of a rotation matrix.
 *
 * This class implements the time derivative of a rotation matrix using a Eigen::Matrix<Scalar, 3, 3> as data storage.
 *
 * \tparam PrimType_  Primitive data type of the coordinates.
 * \ingroup rotations
 */
template<typename PrimType_>
class RotationMatrixDiff : public RotationDiffBase<RotationMatrixDiff<PrimType_>>, private Eigen::Matrix<PrimType_, 3, 3> {
 private:
  /*! \brief The base type.
   */
  typedef Eigen::Matrix<PrimType_, 3, 3> Base;

 public:
  /*! \brief The implementation type.
   *  The implementation type is always an Eigen object.
   */
  typedef Base Implementation;

  typedef Base Matrix3x3;

  /*! \brief The primitive type.
   *  Float/Double
   */
  typedef PrimType_ Scalar;

  RotationMatrixDiff()
    : Base(Base::Zero()) {
  }

  /*! \brief Constructor using Eigen::Matrix<Scalar, 3, 3>.
   *  \param other   Eigen::Matrix<Scalar, 3, 3>
   */
  explicit RotationMatrixDiff(const Base& other) // explicit on purpose
    : Base(other) {
  }

  /*! \brief Constructor using nine scalars.
   *  \param r11     entry in row 1, col 1
   *  \param r12     entry in row 1, col 2
   *  \param r13     entry in row 1, col 3
   *  \param r21     entry in row 2, col 1
   *  \param r22     entry in row 2, col 2
   *  \param r23     entry in row 2, col 3
   *  \param r31     entry in row 3, col 1
   *  \param r32     entry in row 3, col 2
   *  \param r33     entry in row 3, col 3
   */
  RotationMatrixDiff(Scalar r11, Scalar r12, Scalar r13,
                     Scalar r21, Scalar r22, Scalar r23,
                     Scalar r31, Scalar r32, Scalar r33) {
    *this << r11,r12,r13,r21,r22,r23,r31,r32,r33;
  }

  /*! \brief Constructor using a time derivative with a different parameterization
   *
   * \param rotation  rotation
   * \param other     other time derivative
   */
  template<typename RotationDerived_, typename OtherDerived_>
  inline explicit RotationMatrixDiff(const RotationBase<RotationDerived_>& rotation, const RotationDiffBase<OtherDerived_>& other)
    : Base(internal::RotationDiffConversionTraits<RotationMatrixDiff, OtherDerived_, RotationDerived_>::convert(rotation.derived(), other.derived()).toImplementation()){
  }

  /*! \brief Cast to another representation of the time derivative of a rotation
   *  \param other   other rotation
   *  \returns reference
   */
  template<typename OtherDerived_, typename RotationDerived_>
  OtherDerived_ cast(const RotationBase<RotationDerived_>& rotation) const {
    return internal::RotationDiffConversionTraits<OtherDerived_, RotationMatrixDiff, RotationDerived_>::convert(rotation.derived(), *this);
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation (recommended only for advanced users)
   */
  inline Implementation& toImplementation() {
    return static_cast<Implementation&>(*this);
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation (recommended only for advanced users)
   */
  inline const Implementation& toImplementation() const {
    return static_cast<const Implementation&>(*this);
  }

  /*! \brief Reading access to the time derivative of the rotation matrix.
   *  \returns rotation matrix (matrix) with reading access
   */
  inline const Matrix3x3& matrix() const {
    return this->toImplementation();
  }

  /*! \brief Writing access to the time derivative of the rotation matrix.
   *  \returns rotation matrix (matrix) with writing access
   */
  inline Matrix3x3& matrix() {
    return this->toImplementation();
  }

  /*! \brief Sets all time derivatives to zero.
   *  \returns reference
   */
  RotationMatrixDiff& setZero() {
    this->toImplementation().setZero();
    return *this;
  }

  /*! \brief Used for printing the object with std::cout.
   *  \returns std::stream object
   */
  friend std::ostream& operator << (std::ostream& out, const RotationMatrixDiff& diff) {
    out << diff.toImplementation();
    return out;
  }
};

//! \brief Time derivative of a rotation quaternion with primitive type double
typedef RotationMatrixDiff<double> RotationMatrixDiffPD;
//! \brief Time derivative of a rotation quaternion with primitive type float
typedef RotationMatrixDiff<float> RotationMatrixDiffPF;
//! \brief Time derivative of a rotation quaternion with primitive type double
typedef RotationMatrixDiff<double> RotationMatrixDiffD;
//! \brief Time derivative of a rotation quaternion with primitive type float
typedef RotationMatrixDiff<float> RotationMatrixDiffF;

namespace internal {

template<typename PrimType_>
class RotationDiffConversionTraits<RotationMatrixDiff<PrimType_>, LocalAngularVelocity<PrimType_>, RotationMatrix<PrimType_>> {
 public:
  inline static RotationMatrixDiff<PrimType_> convert(const RotationMatrix<PrimType_>& rotationMatrix, const LocalAngularVelocity<PrimType_>& angularVelocity) {
    return RotationMatrixDiff<PrimType_>(rotationMatrix.matrix()*getSkewMatrixFromVector(angularVelocity.vector()));
  }
};

template<typename PrimType_>
class RotationDiffConversionTraits<RotationMatrixDiff<PrimType_>, GlobalAngularVelocity<PrimType_>, RotationMatrix<PrimType_>> {
 public:
  inline static RotationMatrixDiff<PrimType_> convert(const RotationMatrix<PrimType_>& rotationMatrix, const GlobalAngularVelocity<PrimType_>& angularVelocity) {
    return RotationMatrixDiff<PrimType_>(getSkewMatrixFromVector(angularVelocity.vector())*rotationMatrix.matrix());
  }
};

} // namespace internal
} // namespace kindr


// Copyright 2019 ETH Zürich, Thomas Schöps
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#include <random>

#include <Eigen/Geometry>
#include <gtest/gtest.h>
#include <libvis/eigen.h>
#include <libvis/image_display.h>
#include <libvis/libvis.h>
#include <libvis/logging.h>
#include <libvis/mesh.h>
#include <libvis/mesh_opengl.h>
#include <libvis/renderer.h>

#include "badslam/cuda_depth_processing.cuh"
#include "badslam/direct_ba.h"
#include "badslam/kernels.h"
#include "badslam/pairwise_frame_tracking.h"
#include "badslam/util.cuh"
#include "badslam/render_window.h"

// Some test results:
//
// 22:15:20.153 test_pairwise_frame_tra:343   INFO| --- Testing with gradients x / y ---
// 22:15:20.253 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.5 --> average error norm (convergence): 1.23053 --> average error norm (accuracy): 0.0275803
// 22:15:20.366 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.25 --> average error norm (convergence): 0.543878 --> average error norm (accuracy): 0.000406534
// 22:15:20.484 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.1 --> average error norm (convergence): 0.260922 --> average error norm (accuracy): 0.000267356
// 22:15:20.592 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.08 --> average error norm (convergence): 0.167879 --> average error norm (accuracy): 0.000228615
// 22:15:20.696 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.07 --> average error norm (convergence): 0.0626805 --> average error norm (accuracy): 0.000187236
// 22:15:20.792 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.06 --> average error norm (convergence): 0.0174566 --> average error norm (accuracy): 0.000157755
// 22:15:20.881 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.05 --> average error norm (convergence): 0.000274997 --> average error norm (accuracy): 0.000209946
// 22:15:20.968 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.025 --> average error norm (convergence): 0.000221819 --> average error norm (accuracy): 0.000190435
// 22:15:21.053 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.01 --> average error norm (convergence): 0.000229535 --> average error norm (accuracy): 0.000222226
// 22:15:21.131 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.001 --> average error norm (convergence): 0.000232132 --> average error norm (accuracy): 0.00109521
//
// 22:15:21.131 test_pairwise_frame_tra:343   INFO| --- Testing with brightness constancy assumption ---
// 22:15:21.197 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.5 --> average error norm (convergence): 1.74595 --> average error norm (accuracy): 0.00198717
// 22:15:21.275 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.25 --> average error norm (convergence): 1.85936 --> average error norm (accuracy): 0.000113487
// 22:15:21.354 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.1 --> average error norm (convergence): 0.279781 --> average error norm (accuracy): 4.22934e-05
// 22:15:21.435 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.08 --> average error norm (convergence): 0.0521283 --> average error norm (accuracy): 5.52821e-05
// 22:15:21.511 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.07 --> average error norm (convergence): 5.90658e-05 --> average error norm (accuracy): 3.67257e-05
// 22:15:21.583 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.06 --> average error norm (convergence): 5.90927e-05 --> average error norm (accuracy): 3.76441e-05
// 22:15:21.654 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.05 --> average error norm (convergence): 5.91582e-05 --> average error norm (accuracy): 5.6894e-05
// 22:15:21.721 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.025 --> average error norm (convergence): 5.93368e-05 --> average error norm (accuracy): 3.62716e-05
// 22:15:21.788 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.01 --> average error norm (convergence): 5.9312e-05 --> average error norm (accuracy): 5.09321e-05
// 22:15:21.852 test_pairwise_frame_tra:459   INFO| Distortion strenght: 0.001 --> average error norm (convergence): 5.93804e-05 --> average error norm (accuracy): 9.19018e-05
//
// 22:15:21.131 test_pairwise_frame_tra:343   INFO| --- Testing with gradient magnitudes ---
// 22:28:18.486 test_pairwise_frame_tra:504   INFO| Distortion strenght: 0.5 --> average error norm (convergence): 1.57414 --> average error norm (accuracy): 0.512028
// 22:28:18.597 test_pairwise_frame_tra:504   INFO| Distortion strenght: 0.25 --> average error norm (convergence): 0.903301 --> average error norm (accuracy): 1.59576
// 22:28:18.699 test_pairwise_frame_tra:504   INFO| Distortion strenght: 0.1 --> average error norm (convergence): 0.15644 --> average error norm (accuracy): 0.00174811
// 22:28:18.800 test_pairwise_frame_tra:504   INFO| Distortion strenght: 0.08 --> average error norm (convergence): 0.0837946 --> average error norm (accuracy): 0.00135416
// 22:28:18.896 test_pairwise_frame_tra:504   INFO| Distortion strenght: 0.07 --> average error norm (convergence): 0.0398299 --> average error norm (accuracy): 0.00130194
// 22:28:18.985 test_pairwise_frame_tra:504   INFO| Distortion strenght: 0.06 --> average error norm (convergence): 0.0124953 --> average error norm (accuracy): 0.00104814
// 22:28:19.070 test_pairwise_frame_tra:504   INFO| Distortion strenght: 0.05 --> average error norm (convergence): 0.00111669 --> average error norm (accuracy): 0.000687964
// 22:28:19.152 test_pairwise_frame_tra:504   INFO| Distortion strenght: 0.025 --> average error norm (convergence): 0.00112156 --> average error norm (accuracy): 0.000537154
// 22:28:19.229 test_pairwise_frame_tra:504   INFO| Distortion strenght: 0.01 --> average error norm (convergence): 0.00112214 --> average error norm (accuracy): 0.000340628
// 22:28:19.302 test_pairwise_frame_tra:504   INFO| Distortion strenght: 0.001 --> average error norm (convergence): 0.00111539 --> average error norm (accuracy): 0.000932258

using namespace vis;

static void EstimateNormals(
    const PinholeCamera4f& camera,
    Image<float>* depth,
    Image<u16>* normals) {
  normals->SetSize(depth->size());

  for (int y = 0; y < depth->height(); ++ y) {
    for (int x = 0; x < depth->width(); ++ x) {
      if (x < 1 || y < 1 || x >= depth->width() - 1 || y >= depth->height() - 1) {
        (*depth)(x, y) = 0;
        continue;
      }

      float float_depth = (*depth)(x, y);
      if (float_depth <= 0 || std::isinf(float_depth) || std::isnan(float_depth)) {
        (*depth)(x, y) = 0;
        continue;
      }

      float right_depth = (*depth)(x + 1, y);
      if (right_depth <= 0 || std::isinf(right_depth) || std::isnan(right_depth)) {
        (*depth)(x, y) = 0;
        continue;
      }

      float down_depth = (*depth)(x, y + 1);
      if (down_depth <= 0 || std::isinf(down_depth) || std::isnan(down_depth)) {
        (*depth)(x, y) = 0;
        continue;
      }

      Vec3f center_point = float_depth * camera.UnprojectFromPixelCenterConv(Vec2i(x, y));
      Vec3f right_point = right_depth * camera.UnprojectFromPixelCenterConv(Vec2i(x + 1, y));
      Vec3f down_point = down_depth * camera.UnprojectFromPixelCenterConv(Vec2i(x, y + 1));

      Vec3f to_right = right_point - center_point;
      Vec3f to_down = down_point - center_point;

      Vec3f normal = to_down.cross(to_right).normalized();
      (*normals)(x, y) = ImageSpaceNormalToU16(normal.x(), normal.y());
    }
  }
}

static void RenderImages(
    const PinholeCamera4f& camera,
    float min_render_depth,
    float max_render_depth,
    const shared_ptr<Renderer>& renderer,
    const shared_ptr<Mesh3fC3u8OpenGL>& mesh_opengl,
    bool use_gradmag,
    SE3f images_T_global[2],
    float raw_to_float_depth,
    CUDABuffer<u16>* tracked_depth_buffer,
    CUDABuffer<u16>* tracked_normals_buffer,
    CUDABufferPtr<uchar> tracked_color_buffer,
    CUDABuffer<u16>* base_normals_buffer,
    CUDABufferPtr<float> base_depth_buffer,
    CUDABufferPtr<uchar> base_color_buffer) {
  const int camera_width = camera.width();
  const int camera_height = camera.height();

  Image<float> depth_images[2];
  Image<Vec3u8> color_images[2];

  for (int i = 0; i < 2; ++ i) {
    renderer->BeginRendering(images_T_global[i], camera, min_render_depth, max_render_depth);
    mesh_opengl->Render(&renderer->shader_program());
    renderer->EndRendering();

    depth_images[i].SetSize(camera_width, camera_height);
    renderer->DownloadDepthResult(camera_width, camera_height, depth_images[i].data());

    color_images[i].SetSize(camera_width, camera_height);
    renderer->DownloadColorResult(camera_width, camera_height, reinterpret_cast<u8*>(color_images[i].data()));

    // Debug: show images
//       static ImageDisplay depth_image_display;
//       depth_image_display.Update(depth_images[i], "Rendered depth image", min_render_depth, max_render_depth);
//       static ImageDisplay color_image_display;
//       color_image_display.Update(color_images[i], "Rendered color image");
//       std::getchar();
  }

  // images[0] == tracked
  // images[1] == base

  // Derive tracked_normals_buffer and remove depth where no normal can be computed
  Image<u16> normal_image_0;
  EstimateNormals(camera, &depth_images[0], &normal_image_0);
  tracked_normals_buffer->UploadAsync(/*stream*/ 0, normal_image_0);

  // Upload depth_images[0] to tracked_depth_buffer. Need to scale to u16.
  Image<u16> tracked_depth_cpu(camera_width, camera_height);
  for (int y = 0; y < camera_height; ++ y) {
    for (int x = 0; x < camera_width; ++ x) {
      float float_depth = depth_images[0](x, y);
      if (float_depth <= 0 || std::isinf(float_depth) || std::isnan(float_depth)) {
        tracked_depth_cpu(x, y) = 0;
      } else {
        tracked_depth_cpu(x, y) = static_cast<u16>((depth_images[0](x, y) / raw_to_float_depth) + 0.5f);
      }
    }
  }
  tracked_depth_buffer->UploadAsync(/*stream*/ 0, tracked_depth_cpu);

  // Upload color_images[0] to a buffer for which tracked_color_texture is a texture
  if (use_gradmag) {
    Image<Vec4u8> tracked_color_cpu(camera_width, camera_height);
    for (int y = 0; y < camera_height; ++ y) {
      for (int x = 0; x < camera_width; ++ x) {
        const Vec3u8& color = color_images[0](x, y);
        tracked_color_cpu(x, y) = Vec4u8(color.x(), color.y(), color.z(), 0.299f * color.x() + 0.587f * color.y() + 0.114f * color.z());
      }
    }
    CUDABuffer<uchar4> temp_buffer(camera_height, camera_width);
    temp_buffer.UploadAsync(/*stream*/ 0, reinterpret_cast<Image<uchar4>&>(tracked_color_cpu));
    cudaTextureObject_t temp_texture;
    temp_buffer.CreateTextureObject(
        cudaAddressModeClamp, cudaAddressModeClamp,
        cudaFilterModeLinear, cudaReadModeNormalizedFloat,
        false, &temp_texture);
    ComputeSobelGradientMagnitudeCUDA(
        /*stream*/ 0,
        temp_texture,
        &tracked_color_buffer->ToCUDA());
    cudaDestroyTextureObject(temp_texture);
  } else {
    Image<u8> tracked_color_cpu(camera_width, camera_height);
    for (int y = 0; y < camera_height; ++ y) {
      for (int x = 0; x < camera_width; ++ x) {
        const Vec3u8& color = color_images[0](x, y);
        tracked_color_cpu(x, y) = 0.299f * color.x() + 0.587f * color.y() + 0.114f * color.z();
      }
    }
    tracked_color_buffer->UploadAsync(/*stream*/ 0, tracked_color_cpu);
  }

  // Derive base_normals_buffer and remove depth where no normal can be computed
  Image<u16> normal_image_1;
  EstimateNormals(camera, &depth_images[1], &normal_image_1);
  base_normals_buffer->UploadAsync(/*stream*/ 0, normal_image_1);

  // Upload data of image 1 for the base frame
  for (int y = 0; y < camera_height; ++ y) {
    for (int x = 0; x < camera_width; ++ x) {
      float float_depth = depth_images[1](x, y);
      if (float_depth < 0 || std::isinf(float_depth) || std::isnan(float_depth)) {
        depth_images[1](x, y) = 0;
      }
    }
  }
  base_depth_buffer->UploadAsync(/*stream*/ 0, depth_images[1]);

  // Upload color_images[1] to a buffer for which base_color_texture is a texture
  if (use_gradmag) {
    Image<uchar4> tracked_color_cpu(camera_width, camera_height);
    for (int y = 0; y < camera_height; ++ y) {
      for (int x = 0; x < camera_width; ++ x) {
        const Vec3u8& color = color_images[1](x, y);
        tracked_color_cpu(x, y) = make_uchar4(color.x(), color.y(), color.z(), 0.299f * color.x() + 0.587f * color.y() + 0.114f * color.z());
      }
    }
    CUDABuffer<uchar4> temp_buffer(camera_height, camera_width);
    temp_buffer.UploadAsync(/*stream*/ 0, tracked_color_cpu);
    cudaTextureObject_t temp_texture;
    temp_buffer.CreateTextureObject(
        cudaAddressModeClamp, cudaAddressModeClamp,
        cudaFilterModeLinear, cudaReadModeNormalizedFloat,
        false, &temp_texture);
    ComputeSobelGradientMagnitudeCUDA(
        /*stream*/ 0,
        temp_texture,
        &base_color_buffer->ToCUDA());
    cudaDestroyTextureObject(temp_texture);
  } else {
    Image<u8> base_color_cpu(camera_width, camera_height);
    for (int y = 0; y < camera_height; ++ y) {
      for (int x = 0; x < camera_width; ++ x) {
        const Vec3u8& color = color_images[1](x, y);
        base_color_cpu(x, y) = 0.299f * color.x() + 0.587f * color.y() + 0.114f * color.z();
      }
    }
    base_color_buffer->UploadAsync(/*stream*/ 0, base_color_cpu);
  }
}

TEST(Optimization, PairwiseFrameTracking) {
  srand(0);

  std::mt19937 generator(/*seed*/ 0);

  // Initialize and switch to an OpenGL context.
  OpenGLContext opengl_context;
  OpenGLContext no_opengl_context;

  opengl_context.InitializeWindowless();
  SwitchOpenGLContext(opengl_context, &no_opengl_context);

  // Create a synthetic scene using a heightmap. First generate vertices.
  constexpr int kHeightmapVerticesX = 61;
  constexpr int kHeightmapVerticesY = 61;
  constexpr float kHeightmapWidth = 5.f;
  constexpr float kHeightmapHeight = 5.f;
  constexpr float kHeightmapZDistance = 1.f;
  // Low variation to avoid occlusions.
  constexpr float kHeightmapZVariation = 0.05f;

  std::uniform_real_distribution<> z_distribution(-kHeightmapZVariation,
                                                  kHeightmapZVariation);
  std::uniform_int_distribution<> color_distribution(0, 255);

  shared_ptr<PointCloud<Point3fC3u8>> mesh_vertex_cloud(new PointCloud<Point3fC3u8>());
  mesh_vertex_cloud->Resize(kHeightmapVerticesY * kHeightmapVerticesX);

  int index = 0;
  for (int y = 0; y < kHeightmapVerticesY; ++ y) {
    for (int x = 0; x < kHeightmapVerticesX; ++ x) {
      Vec3f position;
      position.x() = ((x / (1.f * kHeightmapVerticesX - 1.f)) - 0.5f) * kHeightmapWidth;
      position.y() = ((y / (1.f * kHeightmapVerticesY - 1.f)) - 0.5f) * kHeightmapHeight;
      position.z() = kHeightmapZDistance + z_distribution(generator);
      // Make surface without occlusions by pulling back the surface at the
      // borders.
      position.z() -= 6 * sqrt(pow((x / (1.f * kHeightmapVerticesX - 1.f)) - 0.5f, 2) +
                               pow((y / (1.f * kHeightmapVerticesY - 1.f)) - 0.5f, 2));

      Vec3u8 color;
      color.x() = color_distribution(generator);
      color.y() = color_distribution(generator);
      color.z() = color_distribution(generator);

      mesh_vertex_cloud->at(index) = Point3fC3u8(position, color);
      ++ index;
    }
  }

  // Allocate mesh and insert vertices.
  Mesh3fCu8 mesh;
  *mesh.vertices_mutable() = mesh_vertex_cloud;
  vector<Triangle<u32>>& triangles = *mesh.triangles_mutable();

  // Write faces into mesh.
  int num_faces = 2 * (kHeightmapVerticesX - 1) * (kHeightmapVerticesY - 1);
  triangles.reserve(num_faces);
  for (int y = 0; y < kHeightmapVerticesY - 1; ++ y) {
    for (int x = 0; x < kHeightmapVerticesX - 1; ++ x) {
      // Top left.
      Triangle<u32> face;
      face.index(0) = x + (y + 1) * kHeightmapVerticesX;
      face.index(1) = (x + 1) + y * kHeightmapVerticesX;
      face.index(2) = x + y * kHeightmapVerticesX;
      triangles.push_back(face);

      // Bottom right.
      face.index(0) = x + (y + 1) * kHeightmapVerticesX;
      face.index(1) = (x + 1) + (y + 1) * kHeightmapVerticesX;
      face.index(2) = (x + 1) + y * kHeightmapVerticesX;
      triangles.push_back(face);
    }
  }

  // Transfer mesh to GPU memory.
  shared_ptr<Mesh3fC3u8OpenGL> mesh_opengl(new Mesh3fC3u8OpenGL(mesh));

  // Create camera.
  constexpr int kCameraWidth = 256;
  constexpr int kCameraHeight = 256;
  constexpr float kCameraFX = 0.5f * kCameraWidth;
  constexpr float kCameraFY = 0.5f * kCameraHeight;
  constexpr float kCameraCX = 0.5f * kCameraWidth - 0.5f;
  constexpr float kCameraCY = 0.5f * kCameraHeight - 0.5f;
  const float camera_parameters[4] = {kCameraFX, kCameraFY, kCameraCX, kCameraCY};
  PinholeCamera4f camera(kCameraWidth, kCameraHeight, camera_parameters);

  // Create RGB & depth renderer.
  RendererProgramStoragePtr renderer_program_storage(new RendererProgramStorage());
  shared_ptr<Renderer> renderer(new Renderer(
      /*render_color*/ true,
      /*render_depth*/ true,
      kCameraWidth, kCameraHeight,
      renderer_program_storage));

  // Initialize pairwise tracking.
  PairwiseFrameTrackingBuffers pairwise_tracking_buffers(kCameraWidth, kCameraHeight, /*num_scales*/ 3);
  PoseEstimationHelperBuffers pose_estimation_helper_buffers;

  int sparse_surfel_cell_size = 4;

  CUDABufferPtr<float> cfactor_buffer;
  cfactor_buffer.reset(new CUDABuffer<float>(
      (camera.height() - 1) / sparse_surfel_cell_size + 1,
      (camera.width() - 1) / sparse_surfel_cell_size + 1));
  cfactor_buffer->Clear(0, /*stream*/ 0);
  cudaDeviceSynchronize();

  DepthParameters depth_params;
  depth_params.a = 0;
  depth_params.cfactor_buffer = cfactor_buffer->ToCUDA();
  depth_params.raw_to_float_depth = 1. / 1000.;
  depth_params.baseline_fx = 40.f;
  depth_params.sparse_surfel_cell_size = sparse_surfel_cell_size;

  CUDABuffer<u16> tracked_depth_buffer(kCameraHeight, kCameraWidth);
  CUDABuffer<u16> tracked_normals_buffer(kCameraHeight, kCameraWidth);
  CUDABufferPtr<uchar> tracked_color_buffer;
  cudaTextureObject_t tracked_color_texture;

  CUDABuffer<u16> base_normals_buffer(kCameraHeight, kCameraWidth);
  CUDABufferPtr<uchar> base_color_buffer;
  cudaTextureObject_t base_color_texture;

  CUDABufferPtr<float> base_depth_buffer;
  CUDABufferPtr<uchar> base_calibrated_color_buffer;
  cudaTextureObject_t base_calibrated_color_texture;

  CreatePairwiseTrackingInputBuffersAndTextures(
      /*depth_width*/ kCameraWidth,
      /*depth_height*/ kCameraHeight,
      /*color_width*/ kCameraWidth,
      /*color_height*/ kCameraHeight,
      &base_depth_buffer,
      &base_calibrated_color_buffer,
      &base_color_buffer,
      &tracked_color_buffer,
      &base_calibrated_color_texture,
      &base_color_texture,
      &tracked_color_texture);

  LOG(INFO) << "NOTE: Testing photometric residuals only.";

  // Loop over tests.
  vector<bool> use_gradmag_values = {false, true};
  for (bool use_gradmag : use_gradmag_values) {
    LOG(INFO) << "--- Testing with use_gradmag: " << use_gradmag << " ---";

    vector<float> distortion_strengths = {/*2, 1, 0.75,*/ 0.5, 0.25, 0.1, 0.08, 0.07, 0.06, 0.05, 0.025, 0.01, 0.001};
    for (float distortion_strength : distortion_strengths) {
      srand(0);

      float convergence_error_norm_sum = 0;
      float accuracy_error_norm_sum = 0;
      constexpr int kNumTests = 10;
      for (int test = 0; test < kNumTests; ++ test) {
        // 1. Convergence test: distort the initial estimate, try to obtain the correct transformation.
        SE3f images_T_global[2];
        for (int i = 0; i < 2; ++ i) {
          images_T_global[i] = SE3f::exp(0.1f * SE3f::Tangent::Random());
        }

        RenderImages(
            camera,
            /*min_render_depth*/ 0.1f,
            /*max_render_depth*/ 2.0f * (kHeightmapZDistance + kHeightmapZVariation),
            renderer, mesh_opengl, use_gradmag,
            images_T_global, depth_params.raw_to_float_depth,
            &tracked_depth_buffer, &tracked_normals_buffer, tracked_color_buffer,
            &base_normals_buffer, base_depth_buffer, base_color_buffer);

        SE3f base_T_tracked_ground_truth = images_T_global[1] * images_T_global[0].inverse();
        SE3f base_T_tracked_initial_estimate = base_T_tracked_ground_truth * SE3f::exp(distortion_strength * SE3f::Tangent::Random());
        SE3f base_T_tracked_estimate;

        TrackFramePairwise(
            &pairwise_tracking_buffers,
            /*stream*/ 0,
            /*color_camera*/ camera,
            /*depth_camera*/ camera,
            depth_params,
            *cfactor_buffer,
            &pose_estimation_helper_buffers,
            /*render_window*/ nullptr,
            /*convergence_samples_file*/ nullptr,
            /*use_depth_residuals*/ false,
            /*use_descriptor_residuals*/ true,
            /*use_pyramid_level_0*/ true,
            use_gradmag,
            /* tracked frame */
            tracked_depth_buffer,
            tracked_normals_buffer,
            tracked_color_texture,
            /* base frame */
            *base_depth_buffer,
            base_normals_buffer,
            *base_color_buffer,
            base_color_texture,
            /* input / output poses */
            images_T_global[1].inverse(),
            /*test_different_initial_estimates*/ false,
            base_T_tracked_initial_estimate,
            base_T_tracked_initial_estimate,
            &base_T_tracked_estimate);

        // Check base_T_tracked_estimate against the ground truth
        Matrix<float, 6, 1> error = (base_T_tracked_estimate.inverse() * base_T_tracked_ground_truth).log();
        convergence_error_norm_sum += error.norm();

        // 2. Accuracy test: give the ground truth as initial estimate, verify that the tracking stays there.
        for (int i = 0; i < 2; ++ i) {
          images_T_global[i] = SE3f::exp(distortion_strength * SE3f::Tangent::Random());
        }

        RenderImages(
            camera,
            /*min_render_depth*/ 0.1f,
            /*max_render_depth*/ 2.0f * (kHeightmapZDistance + kHeightmapZVariation),
            renderer, mesh_opengl, use_gradmag,
            images_T_global, depth_params.raw_to_float_depth,
            &tracked_depth_buffer, &tracked_normals_buffer, tracked_color_buffer,
            &base_normals_buffer, base_depth_buffer, base_color_buffer);

        base_T_tracked_ground_truth = images_T_global[1] * images_T_global[0].inverse();
        base_T_tracked_initial_estimate = base_T_tracked_ground_truth;

        TrackFramePairwise(
            &pairwise_tracking_buffers,
            /*stream*/ 0,
            /*color_camera*/ camera,
            /*depth_camera*/ camera,
            depth_params,
            *cfactor_buffer,
            &pose_estimation_helper_buffers,
            /*render_window*/ nullptr,
            /*convergence_samples_file*/ nullptr,
            /*use_depth_residuals*/ false,
            /*use_descriptor_residuals*/ true,
            /*use_pyramid_level_0*/ true,
            use_gradmag,
            /* tracked frame */
            tracked_depth_buffer,
            tracked_normals_buffer,
            tracked_color_texture,
            /* base frame */
            *base_depth_buffer,
            base_normals_buffer,
            *base_color_buffer,
            base_color_texture,
            /* input / output poses */
            images_T_global[1].inverse(),
            /*test_different_initial_estimates*/ false,
            base_T_tracked_initial_estimate,
            base_T_tracked_initial_estimate,
            &base_T_tracked_estimate);

        // Check base_T_tracked_estimate against the ground truth
        error = (base_T_tracked_estimate.inverse() * base_T_tracked_ground_truth).log();
        accuracy_error_norm_sum += error.norm();
      }  // loop over tests

      LOG(INFO) << "Distortion strenght: " << distortion_strength
                << " --> average error norm (convergence): " << (convergence_error_norm_sum / kNumTests)
                << " --> average error norm (accuracy): " << (accuracy_error_norm_sum / kNumTests);
    }  // loop over distortion_strengths
  }  // loop over use_gradmag_values

  // Delete OpenGL objects before losing the OpenGL context
  renderer.reset();
  renderer_program_storage.reset();
  mesh_opengl.reset();

  SwitchOpenGLContext(no_opengl_context);
  opengl_context.Deinitialize();
}


/// \file
/// Maintainer: Felice Serena
///

#include "subsample.h"
#include "generic/random_sample.h"
#include <boost/log/trivial.hpp>

namespace MouseTrack {

SubSample::SubSample(int desiredMaxPoints)
    : _desiredMaxPoints(desiredMaxPoints) {
  // empty
}

void SubSample::desiredMaxPoints(int _new) { _desiredMaxPoints = _new; }

int SubSample::desiredMaxPoints() const { return _desiredMaxPoints; }

PointCloud SubSample::operator()(const PointCloud &inCloud) const {
  PointCloud cloud = random_sample(inCloud, _desiredMaxPoints);
  auto min = cloud.posMin();
  auto max = cloud.posMax();

  BOOST_LOG_TRIVIAL(debug) << "Subsampled point cloud to " << cloud.size()
                           << " points, xyz-min: [" << min[0] << ", " << min[1]
                           << ", " << min[2] << "], xyz-max: [" << max[0]
                           << ", " << max[1] << ", " << max[2]
                           << "] with desired points " << _desiredMaxPoints;
  return cloud;
}

} // namespace MouseTrack


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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 <boost/simd/operator/include/functions/is_equal.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/logical.hpp>

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

#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/false.hpp>
#include <boost/simd/include/constants/true.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/valmin.hpp>

NT2_TEST_CASE_TPL ( is_equal_integer,  BOOST_SIMD_SIMD_TYPES)
{
  using boost::simd::is_equal;
  using boost::simd::tag::is_equal_;
  using boost::simd::native;
  using boost::simd::True;
  using boost::simd::False;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<is_equal_(vT,vT)>::type r_t;

  NT2_TEST_EQUAL(is_equal(boost::simd::Valmax<vT>(), boost::simd::Valmin<vT>()), False<r_t>());
  NT2_TEST_EQUAL(is_equal(boost::simd::Valmax<vT>(), boost::simd::Valmax<vT>()), True<r_t>());
  NT2_TEST_EQUAL(is_equal(boost::simd::Valmin<vT>(), boost::simd::Valmax<vT>()), False<r_t>());
}

NT2_TEST_CASE_TPL ( is_equal_signed_integer,  BOOST_SIMD_SIMD_INTEGRAL_SIGNED_TYPES)
{
  using boost::simd::is_equal;
  using boost::simd::tag::is_equal_;
  using boost::simd::native;
  using boost::simd::True;
  using boost::simd::False;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<is_equal_(vT,vT)>::type r_t;

  NT2_TEST_EQUAL(is_equal(boost::simd::Zero<vT>(), boost::simd::Mone<vT>()), False<r_t>());
  NT2_TEST_EQUAL(is_equal(boost::simd::Zero<vT>(), boost::simd::One<vT>()) , False<r_t>());
}

NT2_TEST_CASE_TPL ( is_equal_real,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::is_equal;
  using boost::simd::tag::is_equal_;
  using boost::simd::native;
  using boost::simd::True;
  using boost::simd::False;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<is_equal_(vT,vT)>::type r_t;

  NT2_TEST_EQUAL(is_equal(boost::simd::Inf<vT>() , boost::simd::Inf<vT>()) , True<r_t>());
  NT2_TEST_EQUAL(is_equal(boost::simd::Minf<vT>(), boost::simd::Minf<vT>()), True<r_t>());
  NT2_TEST_EQUAL(is_equal(boost::simd::Inf<vT>() , boost::simd::Minf<vT>()), False<r_t>());
  NT2_TEST_EQUAL(is_equal(boost::simd::Nan<vT>() , boost::simd::Nan<vT>()) , False<r_t>());
  NT2_TEST_EQUAL(is_equal(boost::simd::Zero<vT>(), boost::simd::Mone<vT>()), False<r_t>());
  NT2_TEST_EQUAL(is_equal(boost::simd::Zero<vT>(), boost::simd::One<vT>()) , False<r_t>());
}

NT2_TEST_CASE_TPL ( is_equall,  BOOST_SIMD_SIMD_TYPES)
{
  using boost::simd::is_equal;
  using boost::simd::tag::is_equal_;
  using boost::simd::native;
  using boost::simd::True;
  using boost::simd::False;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef native< boost::simd::logical<T>, ext_t> vlT;
  typedef typename boost::dispatch::meta::call<is_equal_(vT,vT)>::type r_t;

  // specific values tests
  NT2_TEST_EQUAL(is_equal(boost::simd::True<vlT>(),boost::simd::False<vlT>()), False<r_t>());
  NT2_TEST_EQUAL(is_equal(boost::simd::True<vlT>(), boost::simd::True<vlT>()), True<r_t>());
  NT2_TEST_EQUAL(is_equal(boost::simd::False<vlT>(),boost::simd::False<vlT>()), True<r_t>());
  NT2_TEST_EQUAL(is_equal(boost::simd::False<vlT>(), boost::simd::True<vlT>()), False<r_t>());
} // end of test for floating_


// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "vertex_ellipse.h"

#include "g2o/stuff/macros.h"
#include "g2o/stuff/misc.h"

#ifdef G2O_HAVE_OPENGL
#include "g2o/EXTERNAL/freeglut/freeglut_minimal.h"
#include "g2o/stuff/opengl_primitives.h"
#include "g2o/stuff/opengl_wrapper.h"
#endif

#include <Eigen/Eigenvalues>
#include <iomanip>

using namespace std;

namespace g2o {

VertexEllipse::VertexEllipse()
    : RobotData(),
      _covariance(Matrix3F::Zero()),
      _UMatrix(Matrix2F::Zero()),
      _singularValues(Vector2F::Zero()) {}

VertexEllipse::~VertexEllipse() {}

void VertexEllipse::_updateSVD() const {
  Eigen::SelfAdjointEigenSolver<Matrix2F> eigenSolver(
      _covariance.block<2, 2>(0, 0));
  _UMatrix = eigenSolver.eigenvectors();
  _singularValues = eigenSolver.eigenvalues();
}

bool VertexEllipse::read(std::istream& is) {
  float cxx, cxy, cxt, cyy, cyt, ctt;
  is >> cxx >> cxy >> cxt >> cyy >> cyt >> ctt;
  _covariance(0, 0) = cxx;
  _covariance(0, 1) = cxy;
  _covariance(0, 2) = cxt;
  _covariance(1, 0) = cxy;
  _covariance(1, 1) = cyy;
  _covariance(1, 2) = cyt;
  _covariance(2, 0) = cxt;
  _covariance(2, 1) = cyt;
  _covariance(2, 2) = ctt;

  _updateSVD();

  int size;
  is >> size;
  for (int i = 0; i < size; i++) {
    float x, y;
    is >> x >> y;
    addMatchingVertex(x, y);
  }

  return true;
}

bool VertexEllipse::write(std::ostream& os) const {
  os << _covariance(0, 0) << " " << _covariance(0, 1) << " "
     << _covariance(0, 2) << " " << _covariance(1, 1) << " "
     << _covariance(1, 2) << " " << _covariance(2, 2) << " ";

  os << _matchingVertices.size() << " ";
  for (size_t i = 0; i < _matchingVertices.size(); i++) {
    os << _matchingVertices[i].x() << " " << _matchingVertices[i].y() << " ";
  }

  return os.good();
}

#ifdef G2O_HAVE_OPENGL
VertexEllipseDrawAction::VertexEllipseDrawAction()
    : DrawAction(typeid(VertexEllipse).name()) {
  _scaleFactor = 0;
}

bool VertexEllipseDrawAction::refreshPropertyPtrs(
    HyperGraphElementAction::Parameters* params_) {
  if (!DrawAction::refreshPropertyPtrs(params_)) return false;
  if (_previousParams) {
    _scaleFactor =
        _previousParams->makeProperty<DoubleProperty>(_typeName + "::", 1);
  } else {
    _scaleFactor = 0;
  }
  return true;
}

HyperGraphElementAction* VertexEllipseDrawAction::operator()(
    HyperGraph::HyperGraphElement* element,
    HyperGraphElementAction::Parameters* params_) {
  if (typeid(*element).name() != _typeName) return nullptr;

  refreshPropertyPtrs(params_);
  if (!_previousParams) {
    return this;
  }
  if (_show && !_show->value()) return this;

  VertexEllipse* that = dynamic_cast<VertexEllipse*>(element);

  glPushMatrix();

  float sigmaTheta = sqrt(that->covariance()(2, 2));
  float x = 0.1f * cosf(sigmaTheta);
  float y = 0.1f * sinf(sigmaTheta);

  glColor3f(1.f, 0.7f, 1.f);
  glBegin(GL_LINE_STRIP);
  glVertex3f(x, y, 0);
  glVertex3f(0, 0, 0);
  glVertex3f(x, -y, 0);
  glEnd();

  glColor3f(0.f, 1.f, 0.f);
  for (size_t i = 0; i < that->matchingVertices().size(); i++) {
    glBegin(GL_LINES);
    glVertex3f(0, 0, 0);
    glVertex3f(that->matchingVertices()[i].x(), that->matchingVertices()[i].y(),
               0);
    glEnd();
  }

  Matrix2F rot = that->U();
  float angle = std::atan2(rot(1, 0), rot(0, 0));
  glRotatef(angle * 180.0 / const_pi(), 0., 0., 1.);
  Vector2F sv = that->singularValues();
  glScalef(sqrt(sv(0)), sqrt(sv(1)), 1);

  glColor3f(1.f, 0.7f, 1.f);
  glBegin(GL_LINE_LOOP);
  for (int i = 0; i < 36; i++) {
    float rad = i * const_pi() / 18.0;
    glVertex2f(std::cos(rad), std::sin(rad));
  }
  glEnd();

  glPopMatrix();
  return this;
}
#endif

}  // namespace g2o


/*
** EPITECH PROJECT, 2017
** ECS Engine
** File description:
** Thomas Arbona
*/
#include <deque>
#include <chrono>
#include <algorithm>
#include <boost/geometry.hpp>
#include "engine/core/Game.hpp"
#include "PhysicsSystem.hpp"
#include "../Engine.hpp"
#include "../components/TransformComponent.hpp"
#include "../components/HitboxComponent.hpp"
#include "../components/PhysicsComponent.hpp"
#include "../components/HoldComponent.hpp"
#include "../components/ItemComponent.hpp"
#include "../components/TagComponent.hpp"
#include "../helpers/GeometryHelper.hpp"

const engine::Vec2D engine::PhysicsSystem::gravity{0., -400.};

void
engine::PhysicsSystem::update(Scene& scene, float tick)
{
    Entities& entities = scene.getEntities();
	_tick = tick;

    entities.each<PhysicsComponent, TransformComponent>([&](Entity const& e, auto& p, auto& t) {
        engine::Vec2D pos2D(t.position.X, t.position.Y);
        engine::Vec2D newPos2D;

        if (e.has<HoldComponent>()) {
            e.get<HoldComponent>().hasReachableEntity = false;
        }

        if (e.has<ItemComponent>() && p.isGrounded) {
            p.isGrounded = PhysicsSystem::isGrounded(entities, e);
            return;
        }

        p.velocity += engine::PhysicsSystem::gravity * _tick;
        newPos2D = pos2D + p.velocity * _tick;
        t.prevPosition = t.position;
        t.position.X = newPos2D.X;
        t.position.Y = newPos2D.Y;

        if (t.position.Y > t.prevPosition.Y) {
            p.isGrounded = false;
        }

        this->applyCollision(entities, e);
        this->applyDeplacement(entities, e);

        if (t.position.Y < -50.f) {
            e.kill();
        }

        p.isGrounded = PhysicsSystem::isGrounded(entities, e);
    }, false);
}

void
engine::PhysicsSystem::applyCollision(Entities& entities, Entity const& entity)
{
    if (!entity.has<HitboxComponent>())
        return;

    entity.get<PhysicsComponent, HitboxComponent, TransformComponent>([&](auto& p, auto& h, auto& t) {
        Manifold mf;
        float rebound = 1.f;
        float dist = std::sqrt(std::pow(t.position.X - t.prevPosition.X, 2) + std::pow(t.position.Y - t.prevPosition.Y, 2));
        std::size_t count = 0;

        if (dist > 2.f) {
            return this->applyCollisionFrac(entities, entity, dist);
        }

        GeometryHelper::transformHitbox(h, t);

        entities.each<HitboxComponent, TransformComponent>([&](Entity const& e2, auto& h2, auto& t2) {
            if (e2.getId() == entity.getId() || count >= PhysicsCollideMaxObj)
                return;

            GeometryHelper::transformHitbox(h2, t2);

            Manifold mf2 = GeometryHelper::polygonCollide(entity, e2, rebound * h2.rebound);
            if (mf2.isCollide && !mf2.hasError) {
                if (h.onCollide) {
                    h.onCollide(e2);
                } else if (h2.onCollide) {
                    h2.onCollide(entity);
                }
                if (e2.has<ItemComponent>()) {
                    if (entity.has<HoldComponent>()) {
                        auto& hc = entity.get<HoldComponent>();
                        hc.hasReachableEntity = true;
                        hc.reachableEntity = e2;
                    }
                } else {
                    count += 1;
                    mf.isCollide = true;
                    mf.normal += mf2.normal;
                    rebound *= h2.rebound;
                }
            }
        }, false);

        if (mf.isCollide) {
            mf.normal.normalize();
            p.velocity -= 2 * (p.velocity.dotProduct(mf.normal)) * mf.normal;
            p.velocity *= h.rebound * rebound;
            PhysicsSystem::patchCollision(entities, entity);
        }
    });
}

void
engine::PhysicsSystem::applyCollisionFrac(Entities& entities, Entity const& entity, float dist)
{
    entity.get<PhysicsComponent, HitboxComponent, TransformComponent>([&](auto& p, auto& h, auto& t) {
        float traveled = 0.f;
        irr::core::vector3df pPrevPosition;
        bool isCollide = false;

        auto tSave = t;
        auto _p = p;

        _p.velocity.normalize();
        t.position = t.prevPosition;

        while (std::fabs(traveled) <= std::fabs(dist) && !isCollide) {

            pPrevPosition = t.prevPosition;
            t.prevPosition = t.position;
            t.position.X += _p.velocity.X;
            t.position.Y += _p.velocity.Y;
            traveled += 1.f;

            GeometryHelper::transformHitbox(h, t);

            entities.each<HitboxComponent, TransformComponent>([&](auto const& e2, auto& h2, auto& t2) {
                if (e2.getId() == entity.getId())
                    return;

                GeometryHelper::transformHitbox(h2, t2);

                Manifold mf = GeometryHelper::polygonCollide(entity, e2, 0);
                if (mf.isCollide && !mf.hasError) {
                    isCollide = mf.isCollide;

                    if (h.onCollide) {
                        h.onCollide(e2);
                    } else if (h2.onCollide) {
                        h2.onCollide(entity);
                    }
                    t.prevPosition = tSave.prevPosition;
                    p.velocity *= 0.f;
                    PhysicsSystem::patchCollisionFrac(entity, e2);
                }
            }, false);
        }
    });
}

void
engine::PhysicsSystem::patchCollision(Entities& entities, Entity const& entity)
{
    auto& t = entity.get<TransformComponent>();
    auto& h = entity.get<HitboxComponent>();
    auto& p = entity.get<PhysicsComponent>();
    auto bound1 = t.prevPosition;
    auto bound2 = t.position;
    auto cursor = (bound1 + bound2) / 2;

    for (auto it = 0; it < 10; it += 1) {
        t.position = cursor;
        GeometryHelper::transformHitbox(h, t);
        if (PhysicsSystem::simpleCollideEntities(entities, entity)) {
            bound2 = cursor;
        } else {
            bound1 = cursor;
        }
        cursor = (bound1 + bound2) / 2;
    }

    float dist = std::fabs(std::sqrt(std::pow(t.position.X - bound1.X, 2) + std::pow(t.position.Y - bound1.Y, 2)));
    auto v = p.velocity;
    v.normalize();
    v *= dist;
    t.position = bound1;
    t.position.X += v.X;
    t.position.Y += v.Y;
}

void
engine::PhysicsSystem::patchCollisionFrac(Entity const& entity, Entity const& entity2)
{
    auto& t = entity.get<TransformComponent>();
    auto& h = entity.get<HitboxComponent>();
    auto bound1 = t.prevPosition;
    auto bound2 = t.position;
    auto cursor = (bound1 + bound2) / 2;

    for (auto it = 0; it < 10; it += 1) {
        t.position = cursor;
        GeometryHelper::transformHitbox(h, t);
        if (GeometryHelper::simplePolygonCollide(entity, entity2)) {
            bound2 = cursor;
        } else {
            bound1 = cursor;
        }
        cursor = (bound1 + bound2) / 2;
    }

    t.position = bound1;
}

void
engine::PhysicsSystem::applyDeplacement(Entities& entities, Entity const& entity)
{
    entity.get<PhysicsComponent, HitboxComponent, TransformComponent>([&](auto& p, auto& h, auto& t) {
        if (p.move.X == 0.f && p.move.Y == 0.f)
            return;

        Manifold gmf;
        std::size_t count = 0;
        auto origin = t.position;
        t.prevPosition = t.position;
        t.position.X += p.move.X * _tick;
        t.position.Y += p.move.Y * _tick;
        GeometryHelper::transformHitbox(h, t);

        entities.each<HitboxComponent, TransformComponent>([&](Entity const& e2, auto& h2, auto& t2) {
            if (e2.getId() == entity.getId() || count >= PhysicsCollideMaxObj)
                return;

            Manifold mf = GeometryHelper::polygonCollide(entity, e2, 0);
            if (mf.isCollide && !mf.hasError) {
                if (e2.has<ItemComponent>()) {
                    if (entity.has<HoldComponent>()) {
                        auto& hc = entity.get<HoldComponent>();
                        hc.hasReachableEntity = true;
                        hc.reachableEntity = e2;
                    }
                } else {
                    gmf.isCollide = true;
                }
            }
        }, false);

        if (gmf.isCollide) {
            t.position = t.prevPosition;
            if (p.isGrounded) {
                p.velocity.Y += 30.f;
            }
        }
        PhysicsSystem::patchDeplacement(entities, entity, origin);
    });
}

void
engine::PhysicsSystem::patchDeplacement(Entities& entities, Entity const& entity, irr::core::vector3df const& origin)
{
    entities.withTag("map", [&](Entity const& entMap) {
        auto& t = entity.get<TransformComponent>();
        auto& h = entity.get<HitboxComponent>();

        t.prevPosition = t.position;
        t.position = origin;
        t.position.Y -= 0.01;
        GeometryHelper::transformHitbox(h, t);
        t.position = t.prevPosition;
        if (!GeometryHelper::simplePolygonCollide(entity, entMap))
            return;

        auto prevPos = t.position;
        bool isOk = false;
        for (auto it = 0; it < 50; it += 1) {
            t.prevPosition = t.position;
            t.position.Y -= 0.02;
            GeometryHelper::transformHitbox(h, t);
            if (GeometryHelper::simplePolygonCollide(entity, entMap)) {
                isOk = true;
                t.position = t.prevPosition;
                break;
            }
        }

        if (!isOk)
            t.position = prevPos;
    });
}

bool
engine::PhysicsSystem::simpleCollideEntities(Entities& entities, Entity const& entity)
{
    if (!entity.has<HitboxComponent>())
        return false;
    bool isCollide = false;

    entity.get<PhysicsComponent, HitboxComponent, TransformComponent>([&](auto& p, auto& h, auto& t) {
        GeometryHelper::transformHitbox(h, t);

        entities.each<HitboxComponent, TransformComponent>([&](Entity const& e2, auto& h2, auto& t2) {
            if (e2.getId() == entity.getId() || e2.has<ItemComponent>() || isCollide)
                return;

            if (GeometryHelper::simplePolygonCollide(entity, e2))
                isCollide = true;
        }, false);
    });

    return isCollide;
}

bool
engine::PhysicsSystem::isGrounded(Entities& entities, Entity const& entity)
{
    bool isGrounded = false;

    entities.withTag("map", [&](Entity const& entMap) {
        if (isGrounded)
            return;

        auto& t = entity.get<TransformComponent>();
        auto& h = entity.get<HitboxComponent>();

        t.prevPosition = t.position;
        t.position.Y -= 0.5f;
        GeometryHelper::transformHitbox(h, t);
        t.position = t.prevPosition;
        isGrounded = GeometryHelper::simplePolygonCollide(entity, entMap);
    });

    return isGrounded;
}


//
// Copyright 2019 Miral Shah <miralshah2211@gmail.com>
//
// 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_GIL_IMAGE_PROCESSING_THRESHOLD_HPP
#define BOOST_GIL_IMAGE_PROCESSING_THRESHOLD_HPP

#include <limits>
#include <boost/gil/image.hpp>

namespace boost { namespace gil {

namespace detail {

template
<
    typename SourceChannelT,
    typename ResultChannelT,
    typename SrcView,
    typename DstView,
    typename Operator
>
void threshold_impl
(
    SrcView const& src_view,
    DstView const& dst_view,
    Operator const& threshold_op
)
{
    //template argument validation
    gil_function_requires<ImageViewConcept<SrcView>>();
    gil_function_requires<MutableImageViewConcept<DstView>>();
    gil_function_requires<ColorSpacesCompatibleConcept
    <
        typename color_space_type<SrcView>::type,
        typename color_space_type<DstView>::type>
    >();

    //iterate over the image chaecking each pixel value for the threshold
    for (std::ptrdiff_t y = 0; y < src_view.height(); y++)
    {
        typename SrcView::x_iterator src_it = src_view.row_begin(y);
        typename DstView::x_iterator dst_it = dst_view.row_begin(y);

        for (std::ptrdiff_t x = 0; x < src_view.width(); x++)
        {
            static_transform(src_it[x], dst_it[x], threshold_op);
        }
    }
}

} //namespace boost::gil::detail

enum class threshold_direction { regular, inverse };
enum class threshold_optimal_value { otsu, triangle };
enum class threshold_truncate_mode { threshold, zero };

/*!
Takes an image view and performes binary thresholding operation on each chennel.
If direction is regular:
values greater than or equal to threshold_value will be set to max_value else set to 0
If direction is inverse:
values greater than or equal to threshold_value will be set to 0 else set to max_value
*/
template <typename SrcView, typename DstView>
void threshold_binary
(
    SrcView const& src_view,
    DstView const& dst_view,
    typename channel_type<DstView>::type threshold_value,
    typename channel_type<DstView>::type max_value,
    threshold_direction direction = threshold_direction::regular
)
{
    //deciding output channel type and creating functor
    typedef typename channel_type<SrcView>::type source_channel_t;
    typedef typename channel_type<DstView>::type result_channel_t;

    if (direction == threshold_direction::regular)
    {
        detail::threshold_impl<source_channel_t, result_channel_t>(src_view, dst_view,
            [threshold_value, max_value](source_channel_t px) ->
                result_channel_t { return px >= threshold_value ? max_value : 0; });
    }
    else
    {
        detail::threshold_impl<source_channel_t, result_channel_t>(src_view, dst_view,
            [threshold_value, max_value](source_channel_t px) ->
                result_channel_t { return px >= threshold_value ? 0 : max_value; });
    }
}

/*!
Takes an image view and performes binary thresholding operation on each chennel.
If direction is regular:
values more or equal to threshold_value will be set to maximum numeric limit of channel else 0
If direction is inverse:
values more than or equal to threshold_value will be set to 0 else maximum numeric limit of channel
*/
template <typename SrcView, typename DstView>
void threshold_binary
(
    SrcView const& src_view,
    DstView const& dst_view,
    typename channel_type<DstView>::type threshold_value,
    threshold_direction direction = threshold_direction::regular
)
{
    //deciding output channel type and creating functor
    typedef typename channel_type<SrcView>::type source_channel_t;
    typedef typename channel_type<DstView>::type result_channel_t;

    result_channel_t max_value = std::numeric_limits<result_channel_t>::max();

    threshold_binary(src_view, dst_view, threshold_value, max_value, direction);
}

/*!
Takes an image view and performes truncating threshold operation on each chennel.
If mode is truncate and direction is regular:
values greater than threshold_value will be set to threshold_value else no change
If mode is truncate and direction is inverse:
values less than threshold_value will be set to threshold_value else no change
If mode is zeo and direction is regular:
values less than threshold_value will be set to 0 else no change
If mode is zero and direction is inverse:
values more than threshold_value will be set to 0 else no change
*/
template <typename SrcView, typename DstView>
void threshold_truncate
(
    SrcView const& src_view,
    DstView const& dst_view,
    typename channel_type<DstView>::type threshold_value,
    threshold_truncate_mode mode = threshold_truncate_mode::threshold,
    threshold_direction direction = threshold_direction::regular
)
{
    //deciding output channel type and creating functor
    typedef typename channel_type<SrcView>::type source_channel_t;
    typedef typename channel_type<DstView>::type result_channel_t;

    std::function<result_channel_t(source_channel_t)> threshold_logic;

    if (mode == threshold_truncate_mode::threshold)
    {
        if (direction == threshold_direction::regular)
        {
            detail::threshold_impl<source_channel_t, result_channel_t>(src_view, dst_view,
                [threshold_value](source_channel_t px) -> result_channel_t
                    { return px >= threshold_value ? threshold_value : px; });
        }
        else
        {
            detail::threshold_impl<source_channel_t, result_channel_t>(src_view, dst_view,
                [threshold_value](source_channel_t px) -> result_channel_t
                    { return px >= threshold_value ? px : threshold_value; });
        }
    }
    else
    {
        if (direction == threshold_direction::regular)
        {
            detail::threshold_impl<source_channel_t, result_channel_t>(src_view, dst_view,
                [threshold_value](source_channel_t px) -> result_channel_t
                    { return px >= threshold_value ? px : 0; });
        }
        else
        {
            detail::threshold_impl<source_channel_t, result_channel_t>(src_view, dst_view,
                [threshold_value](source_channel_t px) -> result_channel_t
                    { return px >= threshold_value ? 0 : px; });
        }
    }
}

}} //namespace boost::gil

#endif //BOOST_GIL_IMAGE_PROCESSING_THRESHOLD_HPP