// This file is part of the dune-stuff project:
//   https://github.com/wwu-numerik/dune-stuff
// Copyright holders: Rene Milk, Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#include "main.hxx"

#include <memory>

#include <boost/numeric/conversion/cast.hpp>

#if HAVE_DUNE_GRID
#include <dune/grid/yaspgrid.hh>
#endif

#include <dune/geometry/quadraturerules.hh>

#include <dune/stuff/common/float_cmp.hh>
#include <dune/stuff/grid/provider/cube.hh>
#include <dune/stuff/functions/constant.hh>
#include <dune/stuff/functions/combined.hh>

#include "functions.hh"

#if HAVE_DUNE_GRID

using namespace Dune;
using namespace Stuff;

template <class G>
class DifferenceFunctionType
{
  typedef typename G::template Codim<0>::Entity E;
  typedef typename G::ctype D;
  static const size_t d = G::dimension;
  typedef double R;
  static const size_t r  = 1;
  static const size_t rC = 1;

public:
  typedef Functions::Constant<E, D, d, R, r, rC> ConstantFunctionType;
  typedef Functions::Difference<ConstantFunctionType, ConstantFunctionType> value;
}; // struct DifferenceFunctionType

template <class DimDomain>
class DifferenceFunctionTest
    : public FunctionTest<
          typename DifferenceFunctionType<YaspGrid<DimDomain::value,
                                                   EquidistantOffsetCoordinates<double, DimDomain::value>>>::value>
{
protected:
  typedef YaspGrid<DimDomain::value, EquidistantOffsetCoordinates<double, DimDomain::value>> GridType;
  typedef typename DifferenceFunctionType<GridType>::value FunctionType;

  static std::shared_ptr<GridType> create_grid()
  {
    return Stuff::Grid::Providers::Cube<GridType>(0.0, 1.0, 4).grid_ptr();
  }

  static std::unique_ptr<FunctionType> create(const double ll, const double rr)
  {
    typedef typename DifferenceFunctionType<YaspGrid<DimDomain::value,
                                                     EquidistantOffsetCoordinates<double, DimDomain::value>>>::
        ConstantFunctionType ConstantFunctionType;
    auto left  = std::make_shared<ConstantFunctionType>(ll);
    auto right = std::make_shared<ConstantFunctionType>(rr);
    return std::unique_ptr<FunctionType>(new FunctionType(left, right));
  } // ... create(...)
};  // class DifferenceFunctionTest

typedef testing::Types<Int<1>, Int<2>, Int<3>> DimDomains;

TYPED_TEST_CASE(DifferenceFunctionTest, DimDomains);
TYPED_TEST(DifferenceFunctionTest, static_interface_check) { this->static_interface_check(); }
TYPED_TEST(DifferenceFunctionTest, dynamic_interface_check)
{
  this->dynamic_interface_check(*(this->create(1.0, 1.0)), *(this->create_grid()));
}
TYPED_TEST(DifferenceFunctionTest, evaluate_check)
{
  auto grid_ptr = this->create_grid();
  auto func     = this->create(1.0, 2.0);
  //  func->visualize(grid_ptr->leafGridView(), "foo");
  for (const auto& entity : Stuff::Common::entityRange(grid_ptr->leafGridView())) {
    const auto local_func  = func->local_function(entity);
    const auto& quadrature = QuadratureRules<double, TypeParam::value>::rule(
        entity.type(), boost::numeric_cast<int>(local_func->order() + 2));
    for (const auto& element : quadrature) {
      const auto& local_point = element.position();
      const auto val = local_func->evaluate(local_point);
      EXPECT_EQ(val[0], -1.0);
    }
  }
} // DifferenceFunctionTest, evaluate_check

#else // HAVE_DUNE_GRID

// no-compile placeholders to mark disabled tests in test-binary output
TEST(DISABLED_FlatTopFunctionTest, static_interface_check) {}
TEST(DISABLED_DifferenceFunctionTest, dynamic_interface_check) {}
TEST(DISABLED_DifferenceFunctionTest, evaluate_check) {}

#endif // HAVE_DUNE_GRID


#pragma once

#include <Eigen/Dense>

namespace kde1d {

namespace tools {

//! applies a function to each non-NaN value, otherwise returns NaN
//! @param x function argument.
//! @param func function to be applied.
template<typename T>
Eigen::MatrixXd
unaryExpr_or_nan(const Eigen::MatrixXd& x, const T& func)
{
  return x.unaryExpr([&func](double y) {
    if (std::isnan(y)) {
      return std::numeric_limits<double>::quiet_NaN();
    } else {
      return func(y);
    }
  });
}

//! applies a function to each non-NaN value, otherwise returns NaN
//! @param x function argument.
//! @param func function to be applied.
template<typename T>
Eigen::MatrixXd
unaryExpr_or_nan_int(const Eigen::MatrixXi& x, const T& func)
{
  return x.unaryExpr([&func](int y) {
    if (std::isnan(static_cast<double>(y))) {
      return std::numeric_limits<double>::quiet_NaN();
    } else {
      return func(y);
    }
  });
}

//! computes the inverse \f$ f^{-1} \f$ of a function \f$ f \f$ by the
//! bisection method.
//!
//! @param x evaluation points.
//! @param f the function to invert.
//! @param lb lower bound.
//! @param ub upper bound.
//! @param n_iter the number of iterations for the bisection.
//!
//! @return \f$ f^{-1}(x) \f$.
inline Eigen::VectorXd
invert_f(const Eigen::VectorXd& x,
         std::function<Eigen::VectorXd(const Eigen::VectorXd&)> f,
         const double lb,
         const double ub,
         int n_iter)
{
  Eigen::VectorXd xl = Eigen::VectorXd::Constant(x.size(), lb);
  Eigen::VectorXd xh = Eigen::VectorXd::Constant(x.size(), ub);
  Eigen::VectorXd x_tmp = x;
  for (int iter = 0; iter < n_iter; ++iter) {
    x_tmp = (xh + xl) / 2.0;
    Eigen::VectorXd fm = f(x_tmp) - x;
    xl = (fm.array() < 0).select(x_tmp, xl);
    xh = (fm.array() < 0).select(xh, x_tmp);
  }

  return x_tmp;
}

//! remove rows of a matrix which contain nan values or have zero weight
//! @param x the matrix.
//! @param a vector of weights that is either empty or whose size is equal to
//!   the number of columns of x.
inline void
remove_nans(Eigen::VectorXd& x, Eigen::VectorXd& weights)
{
  if ((weights.size() > 0) & (weights.size() != x.rows()))
    throw std::runtime_error("sizes of x and weights don't match.");

  // if an entry is nan or weight is zero, move it to the end
  size_t last = x.size() - 1;
  for (size_t i = 0; i < last + 1; i++) {
    bool is_nan = std::isnan(x(i));
    if (weights.size() > 0) {
      is_nan = is_nan | std::isnan(weights(i));
      is_nan = is_nan | (weights(i) == 0.0);
    }
    if (is_nan) {
      if (weights.size() > 0)
        std::swap(weights(i), weights(last));
      std::swap(x(i--), x(last--));
    }
  }

  // remove nan rows
  x.conservativeResize(last + 1);
  if (weights.size() > 0)
    weights.conservativeResize(last + 1);
}

inline Eigen::Matrix<size_t, Eigen::Dynamic, 1>
get_order(const Eigen::VectorXd& x)
{
  Eigen::Matrix<size_t, Eigen::Dynamic, 1> order(x.size());
  for (long i = 0; i < x.size(); ++i)
    order(i) = i;
  std::stable_sort(
    order.data(),
    order.data() + order.size(),
    [&](const size_t& a, const size_t& b) { return (x[a] < x[b]); });
  return order;
}

//! Computes bin counts for univariate data via the linear binning strategy.
//! @param x vector of observations
//! @param weights vector of weights for each observation.
inline Eigen::VectorXd
linbin(const Eigen::VectorXd& x,
       double lower,
       double upper,
       size_t num_bins,
       const Eigen::VectorXd& weights)
{
  Eigen::VectorXd gcnts = Eigen::VectorXd::Zero(num_bins + 1);
  double delta = (upper - lower) / num_bins;
  double rem, lxi;
  size_t li;
  for (long i = 0; i < x.size(); ++i) {
    lxi = (x(i) - lower) / delta;
    li = static_cast<size_t>(lxi);
    rem = lxi - li;
    if (li < num_bins) {
      gcnts(li) += (1 - rem) * weights(i);
      gcnts(li + 1) += rem * weights(i);
    }
  }

  return gcnts;
}

} // end kde1d tools

} // end kde1d


/*
 * Copyright 2017-2018, Justin Carpentier, LAAS-CNRS
 *
 * This file is part of eigenpy.
 * eigenpy is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * eigenpy is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with eigenpy.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <Eigen/Core>

#if EIGEN_VERSION_AT_LEAST(3,2,0)

#include "eigenpy/solvers/solvers.hpp"
#include "eigenpy/solvers/ConjugateGradient.hpp"

#if EIGEN_VERSION_AT_LEAST(3,3,5)
  #include "eigenpy/solvers/LeastSquaresConjugateGradient.hpp"
#endif

namespace eigenpy
{
  void exposeSolvers()
  {
    using namespace Eigen;
    ConjugateGradientVisitor< ConjugateGradient<MatrixXd,Lower|Upper> >::expose();
#if EIGEN_VERSION_AT_LEAST(3,3,5)
    LeastSquaresConjugateGradientVisitor< LeastSquaresConjugateGradient<MatrixXd, LeastSquareDiagonalPreconditioner<MatrixXd::Scalar> > >::expose();
#endif

    // Conjugate gradient with limited BFGS preconditioner
    ConjugateGradientVisitor< ConjugateGradient<MatrixXd,Lower|Upper,IdentityPreconditioner > >::expose("IdentityConjugateGradient");
//    ConjugateGradientVisitor< ConjugateGradient<MatrixXd,Lower|Upper,LimitedBFGSPreconditioner<double,Dynamic,Lower|Upper> > >::expose("LimitedBFGSConjugateGradient");

    boost::python::enum_<Eigen::ComputationInfo>("ComputationInfo")
    .value("Success",Eigen::Success)
    .value("NumericalIssue",Eigen::NumericalIssue)
    .value("NoConvergence",Eigen::NoConvergence)
    .value("InvalidInput",Eigen::InvalidInput)
    ;
  }
} // namespace eigenpy

#endif


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#ifndef BOOST_HANA_TEST_LAWS_COMPARABLE_HPP
#define BOOST_HANA_TEST_LAWS_COMPARABLE_HPP

#include <boost/hana/assert.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/concept/comparable.hpp>
#include <boost/hana/core/when.hpp>
#include <boost/hana/lazy.hpp>

#include <laws/base.hpp>

namespace boost { namespace hana { namespace test {
    template <typename T, typename = when<true>>
    struct TestComparable : TestComparable<T, laws> {
        using TestComparable<T, laws>::TestComparable;
    };

    template <typename T>
    struct TestComparable<T, laws> {
        template <typename Xs>
        TestComparable(Xs xs) {
            hana::for_each(xs, [](auto x) {
                static_assert(Comparable<decltype(x)>::value, "");
            });

            foreach2(xs, [](auto a, auto b) {

                // reflexivity
                BOOST_HANA_CHECK(
                    hana::equal(a, a)
                );

                // symmetry
                BOOST_HANA_CHECK(
                    hana::equal(a, b) ^implies^ hana::equal(b, a)
                );

                // `not_equal` is the negation of `equal`
                BOOST_HANA_CHECK(
                    hana::not_equal(a, b) ^iff^ hana::not_(hana::equal(a, b))
                );

                // equal.to and not_equal.to
                BOOST_HANA_CHECK(
                    equal.to(a)(b) ^iff^ hana::equal(a, b)
                );

                BOOST_HANA_CHECK(
                    not_equal.to(a)(b) ^iff^ hana::not_equal(a, b)
                );

                // comparing
                _injection<0> f{};
                BOOST_HANA_CHECK(
                    hana::comparing(f)(a, b) ^iff^ hana::equal(f(a), f(b))
                );
            });

            // transitivity
            foreach3(xs, [](auto a, auto b, auto c) {
                BOOST_HANA_CHECK(
                    hana::and_(hana::equal(a, b), hana::equal(b, c))
                        ^implies^ hana::equal(a, c)
                );
            });
        }
    };

    template <typename C>
    struct TestComparable<C, when<Constant<C>::value>>
        : TestComparable<C, laws>
    {
        template <typename Xs>
        TestComparable(Xs xs) : TestComparable<C, laws>{xs} {
            foreach2(xs, [](auto a, auto b) {
                BOOST_HANA_CHECK(
                    hana::value(hana::equal(a, b)) ^iff^
                        hana::equal(hana::value(a), hana::value(b))
                );
            });
        }
    };

    template <typename P>
    struct TestComparable<P, when<Product<P>::value>>
        : TestComparable<P, laws>
    {
        template <typename Products>
        TestComparable(Products products) : TestComparable<P, laws>{products} {
            foreach2(products, [](auto x, auto y) {
                BOOST_HANA_CHECK(
                    hana::equal(x, y) ^iff^
                    hana::and_(
                        hana::equal(hana::first(x), hana::first(y)),
                        hana::equal(hana::second(x), hana::second(y))
                    )
                );
            });
        }
    };

    template <typename S>
    struct TestComparable<S, when<Sequence<S>::value>>
        : TestComparable<S, laws>
    {
        template <int i>
        using x = _constant<i>;

        template <typename Xs>
        TestComparable(Xs xs) : TestComparable<S, laws>{xs} {
            constexpr auto list = make<S>;

            //////////////////////////////////////////////////////////////////
            // equal
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                list(),
                list()
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::equal(
                list(x<0>{}),
                list()
            )));
            BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::equal(
                list(),
                list(x<0>{})
            )));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                list(x<0>{}),
                list(x<0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::equal(
                list(x<0>{}, x<1>{}),
                list(x<0>{})
            )));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                list(x<0>{}, x<1>{}),
                list(x<0>{}, x<1>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::equal(
                list(x<0>{}, x<1>{}, x<2>{}, x<3>{}),
                list(x<0>{}, x<1>{}, x<2>{}, x<4>{})
            )));
        }
    };
}}} // end namespace boost::hana::test

#endif // !BOOST_HANA_TEST_LAWS_COMPARABLE_HPP


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_COMPUTER_VISION_ESSENTIAL_MATRIX_HPP
#define PIC_COMPUTER_VISION_ESSENTIAL_MATRIX_HPP

#include <vector>
#include <random>
#include <stdlib.h>

#include "../base.hpp"

#include "../util/math.hpp"
#include "../util/eigen_util.hpp"

#include "../computer_vision/triangulation.hpp"
#include "../computer_vision/camera_matrix.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
    #include "../externals/Eigen/SVD"
    #include "../externals/Eigen/Geometry"
#else
    #include <Eigen/Dense>
    #include <Eigen/SVD>
    #include <Eigen/Geometry>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief computeEssentialMatrix computes the essential matrix, E, from the fundamental
 * matrix, F12, and the two intrics matrices K1 and K2
 * @param F is the fundamental matrix which maps points from camera 1 into camera 2
 * @param K1 is the camera 1 intrics matrix
 * @param K2 is the camera 2 intrics matrix
 * @return
 */
PIC_INLINE Eigen::Matrix3d computeEssentialMatrix(Eigen::Matrix3d &F, Eigen::Matrix3d &K1, Eigen::Matrix3d &K2)
{
    Eigen::Matrix3d K2t = Eigen::Transpose<Eigen::Matrix3d>(K2);
    return K2t * F * K1;
}

/**
 * @brief computeEssentialMatrix computes the essential matrix, E, from the fundamental
 * matrix, F, and a single instrics camera, K.
 * @param F
 * @param K
 * @return
 */
PIC_INLINE Eigen::Matrix3d computeEssentialMatrix(Eigen::Matrix3d &F, Eigen::Matrix3d &K)
{
    return computeEssentialMatrix(F, K, K);
}

/**
 * @brief decomposeEssentialMatrix decomposes an essential matrix E.
 * @param E is the essential matrix. Input.
 * Note1: E = S * R
 * Note2: S = [t]_x
 * Note3: there are four possible cases:
 * 1:     [R1 |  t]
 * 2:     [R1 | -t]
 * 3:     [R2 |  t]
 * 4:     [R2 | -t]
 * @param R1 is one possible rotation matrix. Output.
 * @param R2 is one possible rotation matrix. Output.
 * @param t is the translation vector which is not normalized. Output.
 */
PIC_INLINE void decomposeEssentialMatrix(Eigen::Matrix3d &E, Eigen::Matrix3d &R1, Eigen::Matrix3d &R2, Eigen::Vector3d &t)
{
    //Solving the linear system
    Eigen::JacobiSVD< Eigen::MatrixXd > svd(E, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    //Z matrix
    Eigen::Matrix3d Z;
    Z.setZero();
    Z(0, 1) =  1.0;
    Z(1, 0) = -1.0;

    //W matrix
    Eigen::Matrix3d W;
    W.setZero();
    W(0, 1) = -1.0;
    W(1, 0) =  1.0;
    W(2, 2) =  1.0;

    //Rotation matrices R1 and R2
    Eigen::Matrix3d Vt = Eigen::Transpose<Eigen::Matrix3d>(V);
    Eigen::Matrix3d Wt = Eigen::Transpose<Eigen::Matrix3d>(W);

    Eigen::Matrix3d  UVt = U * Vt;
    double detUVt = UVt.determinant();

    R1 = detUVt * U * W  * Vt;
    R2 = detUVt * U * Wt * Vt;

    R1 = RotationMatrixRefinement(R1);
    R2 = RotationMatrixRefinement(R2);

    //Translation vector
    Eigen::Matrix3d Ut = Eigen::Transpose<Eigen::Matrix3d>(U);
    Eigen::Matrix3d S = U * Z * Ut;

    t[0] = S(2, 1);
    t[1] = S(0, 2);
    t[2] = S(1, 0);

    t.normalize();
}

/**
 * @brief decomposeEssentialMatrixWithConfiguration decomposes an essential matrix E.
 * @param E is the essential matrix.
 * @param K0
 * @param K1
 * @param points0
 * @param points1
 * @param R
 * @param t
 * @return
 */
PIC_INLINE bool decomposeEssentialMatrixWithConfiguration(Eigen::Matrix3d &E, Eigen::Matrix3d &K0, Eigen::Matrix3d &K1,
                                               std::vector< Eigen::Vector2f > &points0, std::vector< Eigen::Vector2f > &points1,
                                               Eigen::Matrix3d &R, Eigen::Vector3d &t)
{
    if(points0.size() != points1.size()) {
        return false;
    }

    Eigen::Matrix3d R0, R1;
    Eigen::Vector3d T;
    decomposeEssentialMatrix(E, R0, R1, T);

    //for each configuration (R0, -T), (R0, T), (R1, -T), (R1, T)
    //the sign of reconstructed points is checked
    int type = -1;
    int counter = -1;

    for(unsigned int j = 0; j < 4; j++) {

        Eigen::Matrix3d tmp_R = (j < 2) ? R0 : R1;
        Eigen::Vector3d tmp_T;

        if((j % 2) == 0) {
            tmp_T = T;
        } else {
            tmp_T = -T;
        }

        Eigen::Matrix34d M0 = getCameraMatrixIdentity(K0);
        Eigen::Matrix34d M1 = getCameraMatrix(K1, tmp_R, tmp_T);

        int tmp_counter = 0;
        for(unsigned int i = 0; i < points0.size(); i++) {
            //homogeneous coordinates
            Eigen::Vector3d point_0 = Eigen::Vector3d(points0[i][0], points0[i][1], 1.0);
            Eigen::Vector3d point_1 = Eigen::Vector3d(points1[i][0], points1[i][1], 1.0);

            Eigen::Vector4d p0 = triangulationHartleySturm(point_0, point_1, M0, M1);
            Eigen::Vector3d p0_euc = Eigen::Vector3d(p0[0], p0[1], p0[2]);
            Eigen::Vector3d p1 = rigidTransform(p0_euc, tmp_R, tmp_T);

            if((p0[2] >= 0.0) && (p1[2] >= 0.0)) {
                tmp_counter++;
            }

        }

        #ifdef PIC_DEBUG
            printf("Front: %d %d\n",tmp_counter,j);
        #endif

        if(tmp_counter > counter) {
            type = j;
            counter = tmp_counter;
        }
    }

    if(type > -1) {

        R = (type < 2) ? R0 : R1;

        if((type % 2) == 0) {
            t = T;
        } else {
            t = -T;
        }

        return true;

    } else {
        R.setZero();
        t.setZero();

        return false;
    }
}

#endif // PIC_DISABLE_EIGEN

} // end namespace pic

#endif // PIC_COMPUTER_VISION_ESSENTIAL_MATRIX_HPP


/*
 * Copyright Andrey Semashev 2020
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * https://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file countr_one.hpp
 *
 * This header defines \c countr_one algorithm, which counts the number of consecutive
 * least significant one bits in an integer.
 */

#ifndef BOOST_BIT_OPS_COUNTING_COUNTR_ONE_HPP_INCLUDED_
#define BOOST_BIT_OPS_COUNTING_COUNTR_ONE_HPP_INCLUDED_

#include <boost/bit_ops/detail/config.hpp>
#include <boost/bit_ops/detail/type_traits/enable_if.hpp>
#include <boost/bit_ops/detail/type_traits/is_integral.hpp>
#include <boost/bit_ops/detail/type_traits/is_unsigned.hpp>
#include <boost/bit_ops/counting/countr_zero.hpp>

namespace boost {
namespace bit_ops {

/*!
 * \brief Returns the number of consecutive least significant one bits in \a value
 *
 * \pre \a value must not be all-ones
 */
template< typename T >
inline typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_unsigned< T >::value,
    unsigned int
>::type countr_one_nf(T value) BOOST_NOEXCEPT
{
    return bit_ops::countr_zero_nz(static_cast< T >(~value));
}

//! Returns the number of consecutive least significant one bits in \a value
template< typename T >
inline typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_unsigned< T >::value,
    unsigned int
>::type countr_one(T value) BOOST_NOEXCEPT
{
    return bit_ops::countr_zero(static_cast< T >(~value));
}

} // namespace bit_ops
} // namespace boost

#endif // BOOST_BIT_OPS_COUNTING_COUNTR_ONE_HPP_INCLUDED_


/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: t; c-basic-offset: 4 -*- vim:set ts=4 sw=4 sts=4 noet: */
#include "method.h"

#include <cassert>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <iterator>
#include <sstream>
#include <string>
#include <vector>

#include <boost/uuid/uuid.hpp>
#include <boost/variant/static_visitor.hpp>

#include "db/ast-inserter.h"
#include "db/query.h"
#include "method/equation-grammar.h"
#include "method/equation-lexer.h"
#include "method/helper.h"
#include "method/parser.h"
#include "method/printer.h"

namespace flint {
namespace method {

namespace {

class VariantPrinter : public boost::static_visitor<> {
public:
	VariantPrinter(int k, int n, std::ostream *os)
		: k_(k)
		, n_(n)
		, os_(os)
	{}

	void operator()(const Compound &c) const {
		os_->put('(');
		*os_ << c.keyword;
		for (const auto &child : c.children) {
			os_->put(' ');
			boost::apply_visitor(*this, child);
		}
		os_->put(')');
	}

	void operator()(const std::string &s) const {
		if (s[0] != '%') {
			*os_ << s;
			return;
		}
		if (s == "%time") {
			if (n_ == 1) {
				*os_ << "(plus %time @dt)";
			} else {
				*os_ << "(plus %time (divide @dt "
					 << n_
					 << "))";
			}
			return;
		}
		*os_ << s << '#' << k_;
	}

	void operator()(int i) const {
		*os_ << i;
	}

	void operator()(const flint::lexer::Rational &r) const {
		*os_ << r.lexeme;
	}

	void operator()(const flint::lexer::Real &r) const {
		*os_ << r.lexeme;
	}

private:
	int k_;
	int n_;
	std::ostream *os_;
};

class Inserter : db::AstInserter {
public:
	explicit Inserter(sqlite3 *db)
		: db::AstInserter(db)
	{
	}

	bool PrintAndInsert(const boost::uuids::uuid &uuid,
						const Expr &lhs,
						const Expr &rhs)
	{
		std::ostringstream oss;
		if (lhs.which() == kExprIsString) {
			const std::string &id(boost::get<std::string>(lhs));
			if (!Even(uuid, id, rhs, 2, 2)) return false;
			if (!Even(uuid, id, rhs, 4, 2)) return false;
			if (!Even(uuid, id, rhs, 6, 1)) return false;
			if (!Even(uuid, id, rhs, 0, 1)) return false;
			return true;
		} else {
			assert(lhs.which() == kExprIsCompound);
			const Compound &c(boost::get<Compound>(lhs));
			if (c.children.size() != 2) {
				std::cerr << "unexpected expression with keyword: " << c.keyword << std::endl;
				return false;
			}
			const Expr &e(c.children.at(1));
			if (e.which() != kExprIsString) {
				std::cerr << "got an ill-formed derivative: " << c.keyword << std::endl;
				return false;
			}
			const std::string &id(boost::get<std::string>(e));
			std::string name;
			std::string math;

			// #1: k1 = dt * f(t_n, y_n)
			oss.str("");
			oss << id << "#1";
			name = oss.str();
			oss.str("");
			oss << "(times @dt ";
			boost::apply_visitor(Printer(&oss), rhs);
			oss.put(')');
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			// #2: y1 = y_n + k1/2
			oss.str("");
			oss << id << "#2";
			name = oss.str();
			oss.str("");
			oss << "(plus " << id << " (divide " << id << "#1 2))";
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			// #3: k2 = dt * f(t_n + dt/2, y1)
			oss.str("");
			oss << id << "#3";
			name = oss.str();
			oss.str("");
			oss << "(times @dt ";
			boost::apply_visitor(VariantPrinter(2, 2, &oss), rhs);
			oss.put(')');
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			// #4: y2 = y_n + k2/2
			oss.str("");
			oss << id << "#4";
			name = oss.str();
			oss.str("");
			oss << "(plus " << id << " (divide " << id << "#3 2))";
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			// #5: k3 = dt * f(t_n + dt/2, y2)
			oss.str("");
			oss << id << "#5";
			name = oss.str();
			oss.str("");
			oss << "(times @dt ";
			boost::apply_visitor(VariantPrinter(4, 2, &oss), rhs);
			oss.put(')');
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			// #6: y3 = y_n + k3
			oss.str("");
			oss << id << "#6";
			name = oss.str();
			oss.str("");
			oss << "(plus " << id << ' ' << id << "#5)";
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			// #7: k4 = dt * f(t_n + dt, y3)
			oss.str("");
			oss << id << "#7";
			name = oss.str();
			oss.str("");
			oss << "(times @dt ";
			boost::apply_visitor(VariantPrinter(6, 1, &oss), rhs);
			oss.put(')');
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			// #0: y_(n+1) = y_n + (k1 + 2*k2 + 2*k3 + k4)/6
			oss.str("");
			oss << id << "#0";
			name = oss.str();
			oss.str("");
			oss << "(plus " << id << " (divide (plus " << id << "#1 (plus (times 2 " << id << "#3) (plus (times 2 " << id << "#5) " << id << "#7))) 6))";
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			return true;
		}
	}

	bool PrintAndInsertWiener(const boost::uuids::uuid &,
							  const Expr &)
	{
		std::cerr << "The Euler method cannot solve SDEs, please use the Euler-Maruyama method instead." << std::endl;
		return false;
	}

private:
	bool Even(const boost::uuids::uuid &uuid, const std::string &id, const Expr &rhs,
			  int k, int n) {
		std::ostringstream oss;
		oss << id << '#' << k;
		std::string name = oss.str();
		oss.str("");
		boost::apply_visitor(VariantPrinter(k, n, &oss), rhs);
		std::string math = oss.str();
		return Insert(uuid, name.c_str(), math.c_str());
	}
};

}

bool Rk4(sqlite3 *db, const char *input, sqlite3 *output)
{
	return Parse<8, EquationLexer, EquationGrammar, Inserter>(db, input, output);
}

}
}


#include "PreviewControl.h"
#include <boost/timer/timer.hpp>
#include <string>

int main(int argc, char *argv[])
{
	FILE *gp;
	Vector2d com_pos, com_vel, com_acc;
	vector<Vector3d> foot_step_list;
	vector<Vector2d> cog_list, refzmp, outputzmp;

	boost::timer::cpu_timer process_timer;

	preview_control preview_control(0.01, 1.6, 0.27);

	foot_step_list.push_back(Vector3d(0.0,0.0,0.0));
	foot_step_list.push_back(Vector3d(0.32,0.0,0.05));
	foot_step_list.push_back(Vector3d(0.64,0.01,-0.05));
	foot_step_list.push_back(Vector3d(0.96,0.02,0.05));
	foot_step_list.push_back(Vector3d(1.28,0.02,0.0));
	foot_step_list.push_back(Vector3d(3.28,0.02,0.0));

	preview_control.interpolation_zmp_trajectory(foot_step_list);

	gp = popen("gnuplot -persist\n", "w");
	fprintf(gp, "set xrange [-0.005:0.023]\n");
	fprintf(gp, "set yrange [-0.06:0.06]\n");
	fprintf(gp, "set xlabel \"x\"\n");
	fprintf(gp, "set ylabel \"y\"\n");

	process_timer.start();
	while(preview_control.update(com_pos, com_vel, com_acc)){
		Matrix<double,2,1> temp_refzmp; Matrix<double,1,2> temp_outputzmp;
		preview_control.get_ref_zmp(temp_refzmp);
		preview_control.output_zmp(temp_outputzmp);
		cog_list.push_back(com_pos);
		refzmp.push_back(temp_refzmp);
		outputzmp.push_back(temp_outputzmp);
	}
	process_timer.stop();
	std::string timer_result = process_timer.format(9, "経過時間：%w秒\n" "ユーザーCPU処理時間：%u秒\n" "プロセスCPU処理時間：%s秒");
	std::cout << timer_result << std::endl;
	fprintf(gp, "plot '-' with lines lw 2, '-' with lines lw 2, '-' with lines lw 2\n");
	for(size_t i=0;i<cog_list.size();i++) fprintf(gp, "%f\t%f\n", cog_list[i](0), cog_list[i](1)); fprintf(gp, "e\n");
	for(size_t i=0;i<refzmp.size();i++) fprintf(gp, "%f\t%f\n", refzmp[i](0), refzmp[i](1)); fprintf(gp, "e\n");
	for(size_t i=0;i<outputzmp.size();i++) fprintf(gp, "%f\t%f\n", outputzmp[i](0), outputzmp[i](1)); fprintf(gp, "e\n");
	fprintf(gp, "exit\n");
	pclose(gp);

	return 0;
}


#pragma once

#include "cho_util/core/geometry/geometry_base.hpp"

#include <Eigen/Core>

namespace cho {
namespace core {

template <typename _Scalar, int N>
class Plane : public GeometryBase<Plane<_Scalar, N>> {
  using Scalar = _Scalar;

 public:
  explicit constexpr Plane() {}
  Plane(std::initializer_list<Scalar> args) : data_(args) {}

  const Scalar* GetPtr() const { return data_.data(); }
  Scalar* GetPtr() { return data_.data(); }
  static constexpr int GetSize() { return N * 2; }

  auto& GetData() { return data_; }
  const auto& GetData() const { return data_; }

  auto GetCenter() { return data_.col(0); }
  auto GetNormal() { return data_.col(1); }
  const auto GetCenter() const { return data_.col(0); }
  const auto GetNormal() const { return data_.col(1); }

 private:
  Eigen::Matrix<Scalar, N, 2> data_;

 public:
  friend class GeometryBase<Plane>;
  // EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

template <typename _Scalar, int N>
struct GeometryTraits<Plane<_Scalar, N>> {
  using Scalar = _Scalar;
};

}  // namespace core
}  // namespace cho


///////////////////////////////////////////////////////////////////////////////
// distribution::model::models::likelihood::iid::iid.hpp                     //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_MODEL_MODELS_LIKELIHOOD_IID_IID_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_MODEL_MODELS_LIKELIHOOD_IID_IID_HPP_ER_2009
#include <boost/statistics/detail/traits/add_cref.hpp>
#include <boost/statistics/detail/distribution_common/meta/value.hpp>
#include <boost/statistics/detail/distribution/model/key/model.hpp>
#include <boost/statistics/detail/distribution/model/key/dataset.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace model{

    template<typename M,typename D>
    struct iid_likelihood
    {
        public:

        typedef M model_type;
        typedef D dataset_type;
        typedef typename detail::traits::add_cref<M>::type cref_m_;
        typedef typename detail::traits::add_cref<D>::type cref_d_;
        typedef typename distribution::meta::value<M>::type value_type;

        iid_likelihood(){}

        template<typename Args>
        iid_likelihood(cref_m_ m,cref_d_ d):m_(m),d_(d){}

        template<typename Args>
        iid_likelihood(const Args& args)
            : m_(args[keyword::model]),d_(args[keyword::dataset]){}

        iid_likelihood(const iid_likelihood& that) : m_(that.m_), d_(that.d_){}

        iid_likelihood& operator=(const iid_likelihood& that)
        {
            if(&that!=this)
            {
                m_ = that.m_;
                d_ = that.d_;
            }
            return (*this);
        }

        cref_m_ model()const{ return this->m_; }
        cref_d_ dataset()const{ return this->d_; }

        private:
        cref_m_ m_;
        cref_d_ d_;
    };

}// model
}// distribution
}// detail
}// statistics
}// boost

#endif


#include <iostream>
#include <thread>
#include <chrono>
#include <vector>
#include <memory>
#include <string>
#include <random>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/progress.hpp>
#include <boost/program_options.hpp>

#include "Signal.h"
// #include "plotter.h"
#include "wsn.h"
#include "wsensor.h"

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

int main(int argc, char** argv)
{
    auto start = std::chrono::high_resolution_clock::now();

    std::string svg_name = "WSN.svg";
    bool b_input = false;
    std::string input;
    double node_density = def_lambda_SBS;
    double wired_fractoin = 0.0;
    bool b_verbose = false;
    int Total_iter = 100;
    bool bPlot = false;
    bool b_draw = false;

    po::options_description desc("Allowed options");
    desc.add_options()
    ("help", "Construct your network based on the following inputs.")
    ("dens", po::value<double>() ,"Density of the base stations, preference=1e-4 BS/1km2")
    ("if", po::value<string>(), "Input file name")
    ("of", po::value<string>(), "Output file name")
    ("svg", po::value<string>(), "Topology SVG file name")
    ("iter", po::value<int>(), "Number of iterations")
    ("verbose", po::value<bool>(), "verbose")
    ;

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

    if(vm.count("help"))
    {
        std::cout << desc << "\n";
        return 1;
    }

    if(vm.count("iter"))
        Total_iter = vm["iter"].as<int>();

    if(vm.count("dens"))
        node_density = vm["dens"].as<double>();

    if(vm.count("verbose"))
        b_verbose = vm["verbose"].as<bool>();

    std::string plot_name = "Hop_Count";
    if(vm.count("of"))
    {
        plot_name = vm["of"].as<string>();
        bPlot = true;
    }

    if(vm.count("if"))
    {
        input = vm["if"].as<string>();
        b_input = true;
    }

    if(vm.count("svg"))
    {
        svg_name = "SVG_"+vm["svg"].as<string>()+".svg";
        b_draw = true;
    }

    WSN wsn;
    m_nextId = 0; //TODO fix the id generator
    wsn.create_svg(svg_name);

    // Generate data on a disk with radius r with poisson point process
    double r = sqrt(1/M_PI)*sqrt(def_Area); // radius of the disk
    double xx0=r; double yy0=r;    // center of the disk
    wsn.set_center(xx0, yy0, r);

    if(b_input)
        wsn.load_nodes(input, false, 0, wired_fractoin);
    else
        wsn.generate_nodes(node_density, false, 0, wired_fractoin);

    std::this_thread::sleep_for( std::chrono::seconds(20) );

    wsn.draw_clusters(b_draw);

    wsn.save("clusters.csv");

    auto finish = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = finish - start;
    std::cout << "time = "<< elapsed.count() <<"Get out of my Audio, Adios!!"<< std::endl;

    return 0;
}


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/51-100/73/problem73.hpp"

BOOST_AUTO_TEST_SUITE( Problem73 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem73::solve(8);
        BOOST_CHECK_EQUAL(res, 3);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem73::solve();
        BOOST_CHECK_EQUAL(res, 7295372);
    }

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         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 <nt2/include/constants/properties.hpp>
#include <nt2/include/functions/splat.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

#include "../constant.hpp"

// unary minus applied to unsigned
#ifdef BOOST_MSVC
  #pragma warning(disable: 4146)
#endif

NT2_TEST_CASE_TPL(allbits, BOOST_SIMD_SIMD_TYPES)
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>  n_t;

  NT2_CHECK_CONSTANT(Allbits , nt2::Allbits<T>() , T);
}

NT2_TEST_CASE_TPL(bitincrement, BOOST_SIMD_SIMD_TYPES)
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>  n_t;

  NT2_CHECK_CONSTANT(Bitincrement , nt2::Bitincrement<T>() , T);
}

NT2_TEST_CASE_TPL(signmask, BOOST_SIMD_SIMD_TYPES)
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>  n_t;

  NT2_CHECK_CONSTANT(Signmask , nt2::Signmask<T>() , T);
}

NT2_TEST_CASE_TPL(sqrtvalmax, BOOST_SIMD_SIMD_TYPES)
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>  n_t;

  NT2_CHECK_CONSTANT(Sqrtvalmax , nt2::Sqrtvalmax<T>() , T);
}

NT2_TEST_CASE_TPL(valmax, BOOST_SIMD_SIMD_TYPES)
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>  n_t;

  NT2_CHECK_CONSTANT(Valmax , nt2::Valmax<T>() , T);
}

NT2_TEST_CASE_TPL(valmin, BOOST_SIMD_SIMD_TYPES)
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>  n_t;

  NT2_CHECK_CONSTANT(Valmin , nt2::Valmin<T>() , T);
}

NT2_TEST_CASE_TPL(maxleftshift, BOOST_SIMD_SIMD_TYPES)
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>  n_t;

  typedef typename boost::dispatch::meta::as_integer<n_t,signed>::type result_type;
  NT2_TEST_EXPR_TYPE( nt2::Maxleftshift<n_t>()
                    , boost::mpl::_
                    , (result_type)
                    );

  NT2_TEST_EQUAL( nt2::Maxleftshift<n_t>()
                , nt2::splat<result_type>(nt2::Maxleftshift<T>())
                );
}

NT2_TEST_CASE_TPL(ieee, BOOST_SIMD_SIMD_TYPES)
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>  n_t;

  typedef typename boost::dispatch::meta
                                  ::as_integer<n_t>::type type;

  typedef typename boost::dispatch::meta
                                  ::as_integer<n_t,signed>::type s_type;

  NT2_TEST_EXPR_TYPE( nt2::Nbmantissabits<n_t>()
                    , boost::mpl::_
                    , (type)
                    );

  NT2_TEST_EXPR_TYPE( nt2::Nbexponentbits<n_t>()
                    , boost::mpl::_
                    , (type)
                    );

  NT2_TEST_EXPR_TYPE( nt2::Maxexponent<n_t>()
                    , boost::mpl::_
                    , (type)
                    );

  NT2_TEST_EXPR_TYPE( nt2::Minexponent<n_t>()
                    , boost::mpl::_
                    , (type)
                    );

  NT2_TEST_EXPR_TYPE( nt2::Ldexpmask<n_t>()
                    , boost::mpl::_
                    , (type)
                    );

  NT2_TEST_EXPR_TYPE( nt2::Nbdigits<n_t>()
                    , boost::mpl::_
                    , (type)
                    );

  NT2_TEST_EQUAL( nt2::Nbmantissabits<n_t>()
                , nt2::splat<type>(nt2::Nbmantissabits<T>())
                );

  NT2_TEST_EQUAL( nt2::Nbexponentbits<n_t>()
                , nt2::splat<type>(nt2::Nbexponentbits<T>())
                );

  NT2_TEST_EQUAL( nt2::Maxexponent<n_t>()
                , nt2::splat<type>(nt2::Maxexponent<T>())
                );

  NT2_TEST_EQUAL( nt2::Minexponent<n_t>()
                , nt2::splat<type>(nt2::Minexponent<T>())
                );

  NT2_TEST_EQUAL( nt2::Ldexpmask<n_t>()
                , nt2::splat<type>(nt2::Ldexpmask<T>())
                );

  NT2_TEST_EQUAL( nt2::Nbdigits<n_t>()
                , nt2::splat<type>(nt2::Nbdigits<T>())
                );
}


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

#include <boost/hana/functional/arg.hpp>

#include <cstddef>
#include <utility>
namespace hana = boost::hana;

constexpr int to_int(char c)
{ return static_cast<int>(c) - 48; }

template <std::size_t N>
constexpr long long parse(const char (&arr)[N]) {
    long long number = 0, base = 1;
    for (std::size_t i = 0; i < N; ++i) {
        number += to_int(arr[N - 1 - i]) * base;
        base *= 10;
    }
    return number;
}

template <char ...c>
struct pick {
    static constexpr unsigned long n = parse<sizeof...(c)>({c...});

    template <typename ...T>
    constexpr auto operator()(T&& ...args) const
    { return hana::arg<n>(std::forward<T>(args)...); }
};

template <char ...c> constexpr pick<c...> operator"" _st() { return {}; }
template <char ...c> constexpr pick<c...> operator"" _nd() { return {}; }
template <char ...c> constexpr pick<c...> operator"" _rd() { return {}; }
template <char ...c> constexpr pick<c...> operator"" _th() { return {}; }

static_assert(1_st(1, '2', 3.3, 444) == 1, "");
static_assert(2_nd(1, '2', 3.3, 444) == '2', "");
static_assert(3_rd(1, '2', 3.3, 444) == 3.3, "");
static_assert(4_th(1, '2', 3.3, 444) == 444, "");

int main() { }


//==============================================================================
//         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/arithmetic/include/functions/touints.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/functions/ldexp.hpp>

NT2_TEST_CASE_TPL ( touints_real,  BOOST_SIMD_REAL_TYPES)
{

  using boost::simd::touints;
  using boost::simd::tag::touints_;
  typedef typename boost::dispatch::meta::call<touints_(T)>::type              r_t;
  typedef typename boost::dispatch::meta::as_integer<T, unsigned>::type wished_r_t;

  NT2_TEST_TYPE_IS( r_t, wished_r_t );
  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(touints(boost::simd::Inf<T>()) , boost::simd::Inf<r_t>());
  NT2_TEST_EQUAL(touints(boost::simd::Minf<T>()), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(touints(boost::simd::Nan<T>()) , boost::simd::Zero<r_t>());
#endif
  NT2_TEST_EQUAL(touints(boost::simd::One<T>()) , boost::simd::One<r_t>());
  NT2_TEST_EQUAL(touints(boost::simd::Zero<T>()), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(touints(T(2)*boost::simd::Valmax<r_t>()),  boost::simd::Valmax<r_t>());
  NT2_TEST_EQUAL(touints(T(2)*boost::simd::Valmin<r_t>()),  boost::simd::Valmin<r_t>());
  NT2_TEST_EQUAL(touints(boost::simd::Mone<T>()), boost::simd::Zero<r_t>());

  T v = T(1);
  r_t iv = 1;
  int N = int(sizeof(T)*8);
  for(int i=0; i < N ; i++, v*= 2, iv <<= 1)
  {
    NT2_TEST_EQUAL(touints(v), iv);
  }
  NT2_TEST_EQUAL(touints(boost::simd::ldexp(boost::simd::One<T>(), N)), boost::simd::Valmax<r_t>());
  NT2_TEST_EQUAL(touints(boost::simd::ldexp(boost::simd::One<T>(), N+1)), boost::simd::Valmax<r_t>());
}

NT2_TEST_CASE_TPL ( touints_unsigned_int,  BOOST_SIMD_UNSIGNED_TYPES)
{

  using boost::simd::touints;
  using boost::simd::tag::touints_;
  typedef typename boost::dispatch::meta::call<touints_(T)>::type               r_t;
  typedef typename boost::dispatch::meta::as_integer<T, unsigned>::type wished_r_t;

  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_EQUAL(touints(boost::simd::One<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(touints(boost::simd::Zero<T>()), boost::simd::Zero<r_t>());
} // end of test for unsigned_int_

NT2_TEST_CASE_TPL ( touints_signed_int,  BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{

  using boost::simd::touints;
  using boost::simd::tag::touints_;
  typedef typename boost::dispatch::meta::call<touints_(T)>::type               r_t;
  typedef typename boost::dispatch::meta::as_integer<T, unsigned>::type wished_r_t;

  NT2_TEST_TYPE_IS( r_t, wished_r_t );

//  NT2_TEST_EQUAL(touints(boost::simd::Mone<T>()), boost::simd::Valmax<r_t>());
  NT2_TEST_EQUAL(touints(boost::simd::Mone<T>()), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(touints(boost::simd::One<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(touints(boost::simd::Zero<T>()), boost::simd::Zero<r_t>());
} // end of test for signed_int_


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

// TEST=BConverterTest om-t

//  https://stackoverflow.com/questions/485525/round-for-float-in-c
//  http://www.boost.org/doc/libs/1_65_1/libs/numeric/conversion/doc/html/boost_numericconversion/converter___function_object.html

#include <cassert>
#include <iostream>
#include <iomanip>
#include <vector>
#include <boost/numeric/conversion/converter.hpp>

#include "BConverter.hh"
#include "OPTICKS_LOG.hh"

/*

In [59]: vv = np.arange(0, 32767, 1, dtype=np.float64)*1./(32767./451.)

In [60]: vv
Out[60]: array([   0.    ,    0.0138,    0.0275, ...,  450.9587,  450.9725,  450.9862])

In [71]: vv[3450:3453]
Out[71]: array([ 47.4853,  47.499 ,  47.5128])

*/

template<typename T>
struct ShortCompressor
{
    short _imax ;

    T _center ;
    T _extent ;
    T _max ;
    T _step ;
    T _eps ;
    T _half ;

    ShortCompressor( T center, T extent )
        :
        _imax(32767),
        _center(center),
        _extent(extent),
        _max(_imax),
        _step(_extent/_max),
        _eps(0.0001),
        _half(0.5)
    {
    }

    T value(int iv)
    {
        return _center + _step*(T(iv)+_half) ;
    }

    short ivalue0(T v)
    {
        return BConverter::shortnorm_old( v, _center, _extent ) ;
    }

    short ivalue1(T v)
    {
        return BConverter::shortnorm( v, _center, _extent ) ;
    }

    T fvalue(T v)
    {
        return _max*(v - _center)/_extent ;
    }

    void dump(int i0, int i1, int h0, int h1)
    {
        assert( i0 < _imax ) ;
        assert( i1 < _imax ) ;

        for(int i=i0 ; i < i1 ; i++ )
        {
            T v =  value(i) ;
            T fv = fvalue(v) ;

            short iv0 = ivalue0(v) ;
            short iv1 = ivalue1(v) ;
            bool highlight =  i >= h0 && i < h1 ;

            std::cout
                 << " i "
                 << std::setw(10) << i
                 << " "
                 << ( highlight ? "*" : " " )
                 << " v "
                 << std::setw(10) << std::fixed << v
                 << " fv "
                 << std::setw(10) << std::fixed << fv
                 << " iv0 "
                 << std::setw(10) << iv0
                 << " iv1 "
                 << std::setw(10) << iv1
                 << " " << ( iv0 != iv1 ? "#######" : " " )
                 << std::endl
                 ;

        }
    }

};

void test_ShortCompressor()
{
    int d0 = 3440 ;
    int h0 = 3450 ;

    //ShortCompressor<double> dcomp(0., 451.);
    //dcomp.dump( d0, d0+20, h0, h0+3 ) ;

    ShortCompressor<float> fcomp(0., 451.);
    fcomp.dump( d0, d0+20, h0, h0+3 ) ;
}

/**
test_BConverter_0
--------------------

Fails on macOS/clang : seems execeptions not percolating up thru the layers with clang ?
Have to directly catch where the exception is thrown, unlike on Linux.

::

    epsilon:boostrap blyth$ BConverterTest
    libc++abi.dylib: terminating with uncaught exception of type boost::numeric::negative_overflow: bad numeric conversion: negative overflow
    Abort trap: 6

**/

void test_BConverter_0(const std::vector<float>& fvs )
{
    for(unsigned i=0 ; i < fvs.size() ; i++)
    {
        float fv = fvs[i] ;

        unsigned char uc(0);

        try
        {
            uc = BConverter::my__float2uint_rn(fv ) ;
        }
        catch( boost::numeric::negative_overflow& e  )
        {
            std::cout << e.what() << std::endl ;
        }
        catch( boost::numeric::positive_overflow& e  )
        {
            std::cout << e.what() << std::endl ;
        }

        LOG(info)
            << " i " << std::setw(4) << i
            << " fv " << std::setw(16) << std::fixed << std::setprecision(8) << fv
            << " uc " << (int)uc
            ;

    }
}

void test_BConverter_1(const std::vector<float>& fvs )
{
    for(unsigned i=0 ; i < fvs.size() ; i++)
    {
        float fv = fvs[i] ;

        unsigned char uc(0);
        uc = BConverter::my__float2uint_rn_kludge(fv ) ;

        LOG(info)
            << " i " << std::setw(4) << i
            << " fv " << std::setw(16) << std::fixed << std::setprecision(8) << fv
            << " uc " << (int)uc
            ;

    }
}

void test_BConverter()
{
    std::vector<float> fvs = { -1.5f, 0.f, 0.5f, 100.5f, 200.5f, 254.5f, 254.5f , 255.0f, 255.45f, 255.49999f,  255.5f , 256.5f , 300.5f } ;

    //test_BConverter_0(fvs);
    test_BConverter_1(fvs);

}

int main(int argc, char** argv)
{
    OPTICKS_LOG(argc, argv);

    //test_ShortCompressor();

    test_BConverter();

    return 0 ;
}


#include <k4a/k4a.hpp>

#include <iostream>
#include <vector>
#include <array>

#include <Eigen/Dense>
#include <Eigen/Core>

#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
// #include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>

#include "k4a_grabber.h"

using namespace std;
using namespace boost;
using namespace pcl;

typedef pcl::PointXYZRGBA PointType;

Eigen::Matrix4d load_transformMat(const std::string &path)
{
    cv::FileStorage freader;
    freader.open(path, cv::FileStorage::READ);
    cv::Mat transformMat_;
    freader["transformMat"] >> transformMat_;
    Eigen::Matrix4d transformMat;
    cv::cv2eigen(transformMat_, transformMat);
    return transformMat;
}

int main(int argc, char **argv)
{
    // Eigen::Matrix4d transformMat = load_transformMat("../calibration_parameters.yaml");

    const uint32_t deviceCount = k4a::device::get_installed_count();
    if (deviceCount == 0)
    {
        cout << "no azure kinect devices detected!" << endl;
    }

    // PCL Visualizer
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Point Cloud Viewer"));

    // Point Cloud
    pcl::PointCloud<PointType>::Ptr cloud_1;
    // pcl::PointCloud<PointType>::Ptr cloud_2;

    // Retrieved Point Cloud Callback Function
    boost::mutex mutex_1;
    boost::function<void(const pcl::PointCloud<PointType>::ConstPtr &)> function_1 =
        [&cloud_1, &mutex_1](const pcl::PointCloud<PointType>::ConstPtr &ptr) {
            boost::mutex::scoped_lock lock(mutex_1);

            /* Point Cloud Processing */

            cloud_1 = ptr->makeShared();

            pcl::VoxelGrid<PointType> sampler;
            sampler.setInputCloud(cloud_1);
            sampler.setLeafSize(0.008f, 0.008f, 0.008f);
            sampler.filter(*cloud_1);
        };

    // boost::mutex mutex_2;
    // boost::function<void(const pcl::PointCloud<PointType>::ConstPtr &)> function_2 =
    //     [&cloud_2, &mutex_2](const pcl::PointCloud<PointType>::ConstPtr &ptr) {
    //         boost::mutex::scoped_lock lock(mutex_2);

    //         /* Point Cloud Processing */

    //         cloud_2 = ptr->makeShared();

    //         pcl::VoxelGrid<PointType> sampler;
    //         sampler.setInputCloud(cloud_2);
    //         sampler.setLeafSize(0.008f, 0.008f, 0.008f);
    //         sampler.filter(*cloud_2);
    //     };

    // KinectAzureDKGrabber
    boost::shared_ptr<pcl::Grabber> grabber_1 =
        boost::make_shared<pcl::KinectAzureDKGrabber>(0, K4A_DEPTH_MODE_WFOV_2X2BINNED, K4A_IMAGE_FORMAT_COLOR_BGRA32, K4A_COLOR_RESOLUTION_720P);
    // boost::shared_ptr<pcl::Grabber> grabber_2 =
    //     boost::make_shared<pcl::KinectAzureDKGrabber>(1, K4A_DEPTH_MODE_WFOV_2X2BINNED, K4A_IMAGE_FORMAT_COLOR_BGRA32, K4A_COLOR_RESOLUTION_720P);

    boost::shared_ptr<pcl::KinectAzureDKGrabber> grabber_1_ = boost::dynamic_pointer_cast<pcl::KinectAzureDKGrabber>(grabber_1);
    // boost::shared_ptr<pcl::KinectAzureDKGrabber> grabber_2_ = boost::dynamic_pointer_cast<pcl::KinectAzureDKGrabber>(grabber_2);

    // Register Callback Function
    boost::signals2::connection connection_1 = grabber_1->registerCallback(function_1);
    // boost::signals2::connection connection_2 = grabber_2->registerCallback(function_2);

    // Start Grabber
    grabber_1->start();
    // grabber_2->start();

    k4a::calibration calibration_1 = grabber_1_->getCalibration();
    // k4a::calibration calibration_2 = grabber_2_->getCalibration();
    k4a_calibration_intrinsic_parameters_t *intrinsics = &calibration_1.color_camera_calibration.intrinsics.parameters;
    Eigen::Matrix3f intrinsics_eigen_1;
    intrinsics_eigen_1 << intrinsics->param.fx, 0.0f, intrinsics->param.cx,
        0.0f, intrinsics->param.fy, intrinsics->param.cy,
        0.0f, 0.0f, 1.0f;
    // intrinsics = &calibration_2.color_camera_calibration.intrinsics.parameters;
    // Eigen::Matrix3f intrinsics_eigen_2;
    // intrinsics_eigen_2 << intrinsics->param.fx, 0.0f, intrinsics->param.cx,
    //     0.0f, intrinsics->param.fy, intrinsics->param.cy,
    //     0.0f, 0.0f, 1.0f;

    Eigen::Matrix4f extrinsics_eigen = Eigen::Matrix4f::Identity();
    viewer->setCameraParameters(intrinsics_eigen_1, extrinsics_eigen);

    pcl::PointCloud<PointType>::Ptr cloud(new PointCloud<PointType>);

    while (!viewer->wasStopped())
    {
        // Update Viewer
        viewer->spinOnce();
        boost::mutex::scoped_try_lock lock_1(mutex_1);
        // boost::mutex::scoped_try_lock lock_2(mutex_2);
        if (lock_1.owns_lock() && cloud_1 )//&& lock_2.owns_lock() && cloud_2)
        {
            //transform cloud_2 to cloud_1 coordinate
            // transformPointCloud(*cloud_2, *cloud_2, transformMat.cast<float>(), true);

            *cloud = *cloud_1 ; //+ *cloud_2;

            // Update Point Cloud
            if (!viewer->updatePointCloud(cloud, "cloud"))
            {
                viewer->addPointCloud(cloud, "cloud");
            }
        }
    }

    // Stop Grabber
    grabber_1->stop();
    // grabber_2->stop();

    // Disconnect Callback Function
    if (connection_1.connected())
    {
        connection_1.disconnect();
    }
    // if (connection_2.connected())
    // {
    //     connection_2.disconnect();
    // }
    return 0;
}


/*******************************************************************************
 * reservoir/aggregate.hpp
 *
 * Stuff around tlx::Aggregate
 *
 * Copyright (C) 2019 Lorenz Hübschle-Schneider <lorenz@4z2.de>
 *
 * All rights reserved. Published under the GNU General Public License 3
 ******************************************************************************/

#pragma once
#ifndef RESERVOIR_AGGREGATE_HEADER
#define RESERVOIR_AGGREGATE_HEADER

#include <tlx/math/aggregate.hpp>

#include <boost/serialization/split_free.hpp>

#include <ostream>

namespace tlx {

template <typename T>
std::ostream &operator<<(std::ostream &os, const Aggregate<T> &x) {
    if (x.count() > 1) {
        os << "avg=" << x.avg() << " stdev=" << x.stdev()
           << " count=" << x.count() << " range=[" << x.min() << ".." << x.max()
           << "]";
    } else {
        os << x.avg();
    }
    return os;
}

} // namespace tlx

namespace boost::serialization {

template <typename Archive, typename T>
void save(Archive &ar, const tlx::Aggregate<T> &x, unsigned int) {
    auto f = [&](size_t count, double mean, double nvar, const T &min,
                 const T &max) {
        ar << count;
        ar << mean;
        ar << nvar;
        ar << min;
        ar << max;
    };
    const_cast<tlx::Aggregate<T> &>(x).serialize(f);
}

template <typename Archive, typename T>
void load(Archive &ar, tlx::Aggregate<T> &x, unsigned int) {
    size_t count;
    double mean, nvar;
    T min, max;
    ar >> count;
    ar >> mean;
    ar >> nvar;
    ar >> min;
    ar >> max;
    x = tlx::Aggregate<T>(count, mean, nvar, min, max);
}

template <typename Archive, typename T>
inline void serialize(Archive &ar, tlx::Aggregate<T> &x,
                      const unsigned int file_version) {
    split_free(ar, x, file_version);
}

} // namespace boost::serialization

#endif // RESERVOIR_AGGREGATE_HEADER


/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * This Software includes and/or makes use of the following Third-Party Software
 * subject to its own license:
 *
 * 1. Boost Unit Test Framework
 * (https://www.boost.org/doc/libs/1_45_0/libs/test/doc/html/utf.html)
 * Copyright 2001 Boost software license, Gennadiy Rozental.
 *
 * DM20-0442
 */

#include <graphblas/graphblas.hpp>

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE ewisemult_vector_test_suite

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

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

namespace
{
    std::vector<double> v3a_dense = {12, 0, 7};

    std::vector<double> v4a_dense = {0, 0, 12, 7};
    std::vector<double> v4b_dense = {0,  1, 0, 2};
    std::vector<double> v4c_dense = {3,  6, 9, 1};

    std::vector<double> zero3_dense = {0, 0, 0};
    std::vector<double> zero4_dense = {0, 0, 0, 0};

    std::vector<double> twos3_dense = {2, 2, 2};
    std::vector<double> twos4_dense = {2, 2, 2, 2};

    std::vector<double> ans_4atwos4_dense = {0, 0, 24, 14};
    std::vector<double> ans_4a4b_dense = {0, 0, 0, 14};
}

// ***************************************************************************
// Tests without mask
// ***************************************************************************

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_vector_bad_dimensions)
{
    grb::Vector<double> v(v3a_dense, 0.);
    grb::Vector<double> u(v4a_dense, 0.);

    grb::Vector<double> result(3);

    // incompatible input dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseMult(result,
                        grb::NoMask(),
                        grb::NoAccumulate(),
                        grb::Times<double>(), v, u)),
        grb::DimensionException);

    // incompatible output vector dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseMult(result,
                        grb::NoMask(),
                        grb::NoAccumulate(),
                        grb::Times<double>(), u, u)),
        grb::DimensionException);
}

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_vector_reg)
{
    grb::Vector<double> u(v4a_dense, 0.);

    // ewise mult with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    grb::Vector<double> ans(ans_4atwos4_dense, 0.);

    grb::Vector<double> result(4);

    grb::eWiseMult(result,
                   grb::NoMask(),
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise mult with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    grb::Vector<double> ans2(ans_4a4b_dense, 0.);
    grb::eWiseMult(result,
                   grb::NoMask(),
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v2);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise mult with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    grb::Vector<double> ans3(zero4_dense, 0.);
    grb::eWiseMult(result,
                   grb::NoMask(),
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v3);
    BOOST_CHECK_EQUAL(result, ans3);
}

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_vector_stored_zero_result)
{
    grb::Vector<double> u(v4a_dense); //, 0.);

    // ewise mult with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    grb::Vector<double> ans(ans_4atwos4_dense); //, 0.);

    grb::Vector<double> result(4);

    grb::eWiseMult(result,
                   grb::NoMask(),
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise mult with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    grb::Vector<double> ans2(ans_4a4b_dense, 0.);
    ans2.setElement(1, 0.);
    grb::eWiseMult(result,
                   grb::NoMask(),
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v2);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise mult with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    grb::Vector<double> ans3(zero4_dense, 0.);
    grb::eWiseMult(result,
                   grb::NoMask(),
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v3);
    BOOST_CHECK_EQUAL(result, ans3);
}

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_semiring_vector_stored_zero_result)
{
    grb::Vector<double> u(v4a_dense); //, 0.);

    // ewise mult with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    grb::Vector<double> ans(ans_4atwos4_dense); //, 0.);

    grb::Vector<double> result(4);

    grb::eWiseMult(result,
                   grb::NoMask(),
                   grb::NoAccumulate(),
                   multiply_op(grb::ArithmeticSemiring<double>()),
                   u, v);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise mult with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    grb::Vector<double> ans2(ans_4a4b_dense, 0.);
    ans2.setElement(1, 0.);
    grb::eWiseMult(result,
                   grb::NoMask(),
                   grb::NoAccumulate(),
                   multiply_op(grb::ArithmeticSemiring<double>()),
                   u, v2);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise mult with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    grb::Vector<double> ans3(zero4_dense, 0.);
    grb::eWiseMult(result,
                   grb::NoMask(),
                   grb::NoAccumulate(),
                   multiply_op(grb::ArithmeticSemiring<double>()),
                   u, v3);
    BOOST_CHECK_EQUAL(result, ans3);
}

// ***************************************************************************
// Tests using a mask with REPLACE
// ***************************************************************************

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_vector_masked_replace_bad_dimensions)
{
    grb::Vector<double> v(v4a_dense, 0.);
    grb::Vector<double> u(v4b_dense, 0.);
    grb::Vector<double> mask(v3a_dense, 0.);
    grb::Vector<double> result(4);

    // incompatible Mask-Output dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseMult(result,
                        mask,
                        grb::NoAccumulate(),
                        grb::Times<double>(), v, u, grb::REPLACE)),
        grb::DimensionException);
}

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_vector_masked_replace_reg)
{
    std::vector<int> mask4_dense = {0, 1, 1, 0};
    grb::Vector<int> mask(mask4_dense, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise mult with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 0, 24, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseMult(result,
                   mask,
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise mult with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    grb::Vector<double> ans2(zero4_dense, 0.);
    grb::eWiseMult(result,
                   mask,
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v2, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise mult with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    grb::Vector<double> ans3(zero4_dense, 0.);
    grb::eWiseMult(result,
                   mask,
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v3, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans3);
}

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_vector_masked_replace_reg_stored_zero)
{
    std::vector<int> mask4_dense = {0, 1, 1, 0};
    grb::Vector<int> mask(mask4_dense);//, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise mult with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 0, 24, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseMult(result,
                   mask,
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise mult with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    grb::Vector<double> ans2(zero4_dense, 0.);
    grb::eWiseMult(result,
                   mask,
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v2, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise mult with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    grb::Vector<double> ans3(zero4_dense, 0.);
    grb::eWiseMult(result,
                   mask,
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v3, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans3);
}

// ***************************************************************************
// Tests using a mask with MERGE semantics
// ***************************************************************************

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_vector_masked_bad_dimensions)
{
    grb::Vector<double> v(v4a_dense, 0.);
    grb::Vector<double> u(v4b_dense, 0.);
    grb::Vector<double> mask(v3a_dense, 0.);
    grb::Vector<double> result(4);

    // incompatible Mask-Output dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseMult(result,
                        mask,
                        grb::NoAccumulate(),
                        grb::Times<double>(), v, u)),
        grb::DimensionException);
}

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_vector_masked_reg)
{
    std::vector<int> mask4_dense = {0, 1, 1, 0};
    grb::Vector<int> mask(mask4_dense, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    {
        // ewise mult with dense vector
        grb::Vector<double> v(twos4_dense, 0.);

        grb::Vector<double> result(twos4_dense);

        std::vector<double> ans_dense = {2, 0, 24, 2};
        grb::Vector<double> ans(ans_dense, 0.);

        grb::eWiseMult(result,
                       mask,
                       grb::NoAccumulate(),
                       grb::Times<double>(), u, v, grb::MERGE);
        BOOST_CHECK_EQUAL(result, ans);
    }

    {
        // ewise mult with sparse vector
        grb::Vector<double> v2(v4b_dense, 0.);

        grb::Vector<double> result(twos4_dense);

        std::vector<double> ans_dense = {2, 0, 0, 2};
        grb::Vector<double> ans2(ans_dense, 0.);

        grb::eWiseMult(result,
                       mask,
                       grb::NoAccumulate(),
                       grb::Times<double>(), u, v2, grb::MERGE);
        BOOST_CHECK_EQUAL(result, ans2);
    }

    {
        // ewise mult with empty vector
        grb::Vector<double> v3(zero4_dense, 0.);

        grb::Vector<double> result(twos4_dense);

        std::vector<double> ans_dense = {2, 0, 0, 2};
        grb::Vector<double> ans3(ans_dense, 0.);

        grb::eWiseMult(result,
                       mask,
                       grb::NoAccumulate(),
                       grb::Times<double>(), u, v3, grb::MERGE);
        BOOST_CHECK_EQUAL(result, ans3);
    }
}

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_vector_masked_reg_stored_zero)
{

    std::vector<int> mask4_dense = {0, 1, 1, 0};
    grb::Vector<int> mask(mask4_dense); //, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    {
        // ewise mult with dense vector
        grb::Vector<double> v(twos4_dense, 0.);

        grb::Vector<double> result(twos4_dense);

        std::vector<double> ans_dense = {2, 0, 24, 2};
        grb::Vector<double> ans(ans_dense, 0.);

        grb::eWiseMult(result,
                       mask,
                       grb::NoAccumulate(),
                       grb::Times<double>(), u, v, grb::MERGE);
        BOOST_CHECK_EQUAL(result, ans);
    }

    {
        // ewise mult with sparse vector
        grb::Vector<double> v2(v4b_dense, 0.);

        grb::Vector<double> result(twos4_dense);

        std::vector<double> ans_dense = {2, 0, 0, 2};
        grb::Vector<double> ans2(ans_dense, 0.);

        grb::eWiseMult(result,
                       mask,
                       grb::NoAccumulate(),
                       grb::Times<double>(), u, v2, grb::MERGE);
        BOOST_CHECK_EQUAL(result, ans2);
    }

    {
        // ewise mult with empty vector
        grb::Vector<double> v3(zero4_dense, 0.);

        grb::Vector<double> result(twos4_dense);

        std::vector<double> ans_dense = {2, 0, 0, 2};
        grb::Vector<double> ans3(ans_dense, 0.);

        grb::eWiseMult(result,
                       mask,
                       grb::NoAccumulate(),
                       grb::Times<double>(), u, v3, grb::MERGE);
        BOOST_CHECK_EQUAL(result, ans3);
    }
}

// ***************************************************************************
// Tests using a complemented mask with REPLACE semantics
// ***************************************************************************

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_vector_scmp_masked_replace_bad_dimensions)
{
    grb::Vector<double> v(v4a_dense, 0.);
    grb::Vector<double> u(v4b_dense, 0.);
    grb::Vector<double> mask(v3a_dense, 0.);
    grb::Vector<double> result(4);

    // incompatible Mask-Output dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseMult(result,
                        grb::complement(mask),
                        grb::NoAccumulate(),
                        grb::Times<double>(), v, u, grb::REPLACE)),
        grb::DimensionException);
}

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_vector_scmp_masked_replace_reg)
{
    std::vector<int> mask4_dense = {1, 0, 0, 1};
    grb::Vector<int> mask(mask4_dense, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise mult with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 0, 24, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseMult(result,
                   grb::complement(mask),
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise mult with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    grb::Vector<double> ans2(zero4_dense, 0.);
    grb::eWiseMult(result,
                   grb::complement(mask),
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v2, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise mult with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    grb::Vector<double> ans3(zero4_dense, 0.);
    grb::eWiseMult(result,
                   grb::complement(mask),
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v3, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans3);
}

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_vector_scmp_masked_replace_reg_stored_zero)
{
    std::vector<int> mask4_dense = {1, 0, 0, 1};
    grb::Vector<int> mask(mask4_dense);//, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    // ewise mult with dense vector
    grb::Vector<double> v(twos4_dense, 0.);
    std::vector<double> ans_4atwos4_dense2 = {0, 0, 24, 0};
    grb::Vector<double> ans(ans_4atwos4_dense2, 0.);

    grb::Vector<double> result(4);

    grb::eWiseMult(result,
                   grb::complement(mask),
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans);

    // ewise mult with sparse vector
    grb::Vector<double> v2(v4b_dense, 0.);
    grb::Vector<double> ans2(zero4_dense, 0.);
    grb::eWiseMult(result,
                   grb::complement(mask),
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v2, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans2);

    // ewise mult with empty vector
    grb::Vector<double> v3(zero4_dense, 0.);
    grb::Vector<double> ans3(zero4_dense, 0.);
    grb::eWiseMult(result,
                   grb::complement(mask),
                   grb::NoAccumulate(),
                   grb::Times<double>(), u, v3, grb::REPLACE);
    BOOST_CHECK_EQUAL(result, ans3);
}

// ***************************************************************************
// Tests using a complemented mask (with merge semantics)
// ***************************************************************************

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_vector_scmp_masked_bad_dimensions)
{
    grb::Vector<double> v(v4a_dense, 0.);
    grb::Vector<double> u(v4b_dense, 0.);
    grb::Vector<double> mask(v3a_dense, 0.);
    grb::Vector<double> result(4);

    // incompatible Mask-Output dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseMult(result,
                        grb::complement(mask),
                        grb::NoAccumulate(),
                        grb::Times<double>(), v, u)),
        grb::DimensionException);
}

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_vector_scmp_masked_reg)
{
    std::vector<int> mask4_dense = {1, 0, 0, 1};
    grb::Vector<int> mask(mask4_dense, 0);

    grb::Vector<double> u(v4a_dense, 0.);

    {
        // ewise mult with dense vector
        grb::Vector<double> v(twos4_dense, 0.);

        grb::Vector<double> result(twos4_dense);

        std::vector<double> ans_dense = {2, 0, 24, 2};
        grb::Vector<double> ans(ans_dense, 0.);

        grb::eWiseMult(result,
                       grb::complement(mask),
                       grb::NoAccumulate(),
                       grb::Times<double>(), u, v, grb::MERGE);
        BOOST_CHECK_EQUAL(result, ans);
    }

    {
        // ewise mult with sparse vector
        grb::Vector<double> v2(v4b_dense, 0.);

        grb::Vector<double> result(twos4_dense);

        std::vector<double> ans_dense = {2, 0, 0, 2};
        grb::Vector<double> ans2(ans_dense, 0.);

        grb::eWiseMult(result,
                       grb::complement(mask),
                       grb::NoAccumulate(),
                       grb::Times<double>(), u, v2, grb::MERGE);
        BOOST_CHECK_EQUAL(result, ans2);
    }

    {
        // ewise mult with empty vector
        grb::Vector<double> v3(zero4_dense, 0.);

        grb::Vector<double> result(twos4_dense);

        std::vector<double> ans_dense = {2, 0, 0, 2};
        grb::Vector<double> ans3(ans_dense, 0.);

        grb::eWiseMult(result,
                       grb::complement(mask),
                       grb::NoAccumulate(),
                       grb::Times<double>(), u, v3, grb::MERGE);
        BOOST_CHECK_EQUAL(result, ans3);
    }
}

// ***************************************************************************
BOOST_AUTO_TEST_CASE(test_ewisemult_vector_scmp_masked_reg_stored_zero)
{
    std::vector<int> mask4_dense = {1, 0, 0, 1};
    grb::Vector<int> mask(mask4_dense); //0);

    grb::Vector<double> u(v4a_dense, 0.);

    {
        // ewise mult with dense vector
        grb::Vector<double> v(twos4_dense, 0.);

        grb::Vector<double> result(twos4_dense);

        std::vector<double> ans_dense = {2, 0, 24, 2};
        grb::Vector<double> ans(ans_dense, 0.);

        grb::eWiseMult(result,
                       grb::complement(mask),
                       grb::NoAccumulate(),
                       grb::Times<double>(), u, v, grb::MERGE);
        BOOST_CHECK_EQUAL(result, ans);
    }

    {
        // ewise mult with sparse vector
        grb::Vector<double> v2(v4b_dense, 0.);

        grb::Vector<double> result(twos4_dense);

        std::vector<double> ans_dense = {2, 0, 0, 2};
        grb::Vector<double> ans2(ans_dense, 0.);

        grb::eWiseMult(result,
                       grb::complement(mask),
                       grb::NoAccumulate(),
                       grb::Times<double>(), u, v2, grb::MERGE);
        BOOST_CHECK_EQUAL(result, ans2);
    }

    {
        // ewise mult with empty vector
        grb::Vector<double> v3(zero4_dense, 0.);

        grb::Vector<double> result(twos4_dense);

        std::vector<double> ans_dense = {2, 0, 0, 2};
        grb::Vector<double> ans3(ans_dense, 0.);

        grb::eWiseMult(result,
                       grb::complement(mask),
                       grb::NoAccumulate(),
                       grb::Times<double>(), u, v3, grb::MERGE);
        BOOST_CHECK_EQUAL(result, ans3);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <Eigen/Core>

namespace swr {

using Float = double;

typedef Eigen::Matrix<Float, 3, 3> Matrix3F;
typedef Eigen::Matrix<Float, 4, 4> Matrix4F;
typedef Eigen::Matrix<Float, 2, 1> Vector2F;
typedef Eigen::Matrix<Float, 3, 1> Vector3F;
typedef Eigen::Matrix<Float, 4, 1> Vector4F;

} // namespace swr


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(interval)
{
  typedef tiny::MathTypes<float> MT;
  typedef MT::value_traits       VT;
  typedef MT::real_type          T;

  {
    geometry::Interval<T> I;

    BOOST_CHECK_EQUAL( I.lower(), VT::highest() );
    BOOST_CHECK_EQUAL( I.upper(), VT::lowest() );
    BOOST_CHECK( geometry::is_valid(I) == false );
  }

  // No overlap
  {
    geometry::Interval<T> A;
    geometry::Interval<T> B;

    A.lower() = -2.0f;
    A.upper() = -1.0f;
    B.lower() =  1.0f;
    B.upper() =  2.0f;

    BOOST_CHECK_EQUAL( A.lower(), -2.0f );
    BOOST_CHECK_EQUAL( A.upper(), -1.0f );
    BOOST_CHECK_EQUAL( B.lower(),  1.0f );
    BOOST_CHECK_EQUAL( B.upper(),  2.0f );

    BOOST_CHECK( geometry::is_valid(A) == true );
    BOOST_CHECK( geometry::is_valid(B) == true );

    BOOST_CHECK( geometry::overlap_interval_interval(A,B) == false);
    BOOST_CHECK( geometry::overlap_interval_interval(B,A) == false);
  }

  // Deep overlap
  {
    geometry::Interval<T> A;
    geometry::Interval<T> B;

    A.lower() = -2.0f;
    A.upper() =  1.0f;
    B.lower() = -1.0f;
    B.upper() =  2.0f;

    BOOST_CHECK_EQUAL( A.lower(), -2.0f );
    BOOST_CHECK_EQUAL( A.upper(),  1.0f );
    BOOST_CHECK_EQUAL( B.lower(), -1.0f );
    BOOST_CHECK_EQUAL( B.upper(),  2.0f );

    BOOST_CHECK( geometry::overlap_interval_interval(A,B) == true);
    BOOST_CHECK( geometry::overlap_interval_interval(B,A) == true);
  }

  // Touching case
  {
    geometry::Interval<T> A;
    geometry::Interval<T> B;

    A.lower() = -2.0f;
    A.upper() =  0.0f;
    B.lower() =  0.0f;
    B.upper() =  2.0f;

    BOOST_CHECK_EQUAL( A.lower(), -2.0f );
    BOOST_CHECK_EQUAL( A.upper(),  0.0f );
    BOOST_CHECK_EQUAL( B.lower(),  0.0f );
    BOOST_CHECK_EQUAL( B.upper(),  2.0f );

    BOOST_CHECK( geometry::overlap_interval_interval(A,B) == true);
    BOOST_CHECK( geometry::overlap_interval_interval(B,A) == true);
  }

  // Indentical case
  {
    geometry::Interval<T> A;
    geometry::Interval<T> B;

    A.lower() = -2.0f;
    A.upper() =  0.0f;
    B.lower() = -2.0f;
    B.upper() =  0.0f;

    BOOST_CHECK_EQUAL( A.lower(), -2.0f );
    BOOST_CHECK_EQUAL( A.upper(),  0.0f );
    BOOST_CHECK_EQUAL( B.lower(), -2.0f );
    BOOST_CHECK_EQUAL( B.upper(),  0.0f );

    BOOST_CHECK( geometry::overlap_interval_interval(A,B) == true);
    BOOST_CHECK( geometry::overlap_interval_interval(B,A) == true);
  }

  // Zero sized interval case
  {
    geometry::Interval<T> A;
    geometry::Interval<T> B;

    A.lower() = -2.0f;
    A.upper() = -2.0f;
    B.lower() = -2.0f;
    B.upper() =  0.0f;

    BOOST_CHECK_EQUAL( A.lower(), -2.0f );
    BOOST_CHECK_EQUAL( A.upper(), -2.0f );
    BOOST_CHECK_EQUAL( B.lower(), -2.0f );
    BOOST_CHECK_EQUAL( B.upper(),  0.0f );

    BOOST_CHECK( geometry::overlap_interval_interval(A,B) == true);
    BOOST_CHECK( geometry::overlap_interval_interval(B,A) == true);

    BOOST_CHECK( geometry::is_valid(A) == true );

  }

}

BOOST_AUTO_TEST_SUITE_END();


#include "include/MMVII_all.h"

// #include <Eigen/Dense>

namespace MMVII
{

/* ========================== */
/*          ::                */
/* ========================== */

/// Computation to get the point we have at end of iterating a rectangle
template <const int Dim> cPtxd<int,Dim> CalPEnd(const cPtxd<int,Dim> & aP0,const cPtxd<int,Dim> & aP1)
{
    cPtxd<int,Dim> aRes = aP0;
    aRes[Dim-1] = aP1[Dim-1] ;
    return aRes;
}

/* ========================== */
/*   cBorderPixBoxIterator    */
/* ========================== */

template <const int Dim>
  cBorderPixBoxIterator<Dim>::cBorderPixBoxIterator(tBPB & aBPB,const  tPt & aP0) :
      cPixBoxIterator<Dim>  (aBPB.PB(),aP0),
      mBPB                  (&aBPB)
{
}

template <const int Dim>
  cBorderPixBoxIterator<Dim>  & cBorderPixBoxIterator<Dim>::operator ++(int)
{
   return ++(*this);
}

template <const int Dim>
  cBorderPixBoxIterator<Dim>  & cBorderPixBoxIterator<Dim>::operator ++()
{
    //cPixBoxIterator<Dim>::operator ++ ();
    tPBI::operator ++ ();

    mBPB->IncrPt(tPBI::mPCur);
    return *this;
}

/* ========================== */
/*      cBorderPixBox         */
/* ========================== */

template <const int Dim>
  cBorderPixBox<Dim>::cBorderPixBox(const tPB & aPB,const tPt & aSz) :
    mPB     (aPB),
    mSz     (aSz),
    mBoxInt (mPB.Dilate(-mSz)),
    mX0     (mBoxInt.P0().x()),
    mX1     (mBoxInt.P1().x()),
    mBegin  (*this,mPB.P0()),
    mEnd    (*this,CalPEnd(mPB.P0(),mPB.P1()))
{
}
template <const int Dim>
  cBorderPixBox<Dim>::cBorderPixBox(const tPB & aPB,int aSz) :
      cBorderPixBox<Dim>(aPB,tPt::PCste(aSz))
{
}

template <const int Dim>
  cBorderPixBox<Dim>::cBorderPixBox(const cBorderPixBox<Dim> & aBPB) :
    cBorderPixBox<Dim>(mPB,mSz)
{
}

template <const int Dim>  cPixBox<Dim> &   cBorderPixBox<Dim>::PB() {return mPB;}

template <const int Dim>  void cBorderPixBox<Dim>::IncrPt(tPt & aP)
{
   if  ((aP.x()==mX0) && (mBoxInt.Inside(aP)))
      aP.x() = mX1;

}

/* ========================== */
/*          cPtxd             */
/* ========================== */

int NbPixVign(const int & aVign){return 1+2*aVign;}

template <const int Dim> int NbPixVign(const cPtxd<int,Dim> & aVign)
{
   int aRes = NbPixVign(aVign[0]);
   for (int aD=1 ;aD<Dim ; aD++)
       aRes *= NbPixVign(aVign[aD]);
   return aRes;
}

// static const int VeryBig = 1e9;
// const cPt2di  ThePSupImage( VeryBig, VeryBig);
// const cPt2di  ThePInfImage(-VeryBig,-VeryBig);

cPt2di  TAB4Corner[4] = {{1,1},{-1,1},{-1,-1},{1,-1}};

/*
template <class Type,const int Dim> cPtxd<Type,Dim>  cPtxd<Type,Dim>::PCste(const Type & aVal)
{
    cPtxd<Type,Dim> aRes;
    for (int aK=0 ; aK<Dim; aK++)
        aRes.mCoords[aK]= aVal;
    return aRes;
}
*/

/* ========================== */
/*          ::                */
/* ========================== */
    //  To test Error_Handler mecanism

static std::string MesNegSz="Negative size in rect object";
/*
static std::string  TestErHandler;
void TestBenchRectObjError(const std::string & aType,const std::string &  aMes,const char * aFile,int aLine)
{
   TestErHandler = aMes;
}
*/

/* ========================== */
/*          cPixBox           */
/* ========================== */

template <const int Dim>   cPixBox<Dim>::cPixBox(const cPtxd<int,Dim> & aP0,const cPtxd<int,Dim> & aP1,bool AllowEmpty) :
     cTplBox<int,Dim>(aP0,aP1,AllowEmpty),
     mBegin  (*this,aP0),
     mEnd    (*this,CalPEnd(aP0,aP1))
{
}

template <const int Dim>   cPixBox<Dim>::cPixBox(const cPixBox<Dim> & aR) :
   cPixBox<Dim>(aR.mP0,aR.mP1,true)
{
}

template <const int Dim> cPixBox<Dim>  cPixBox<Dim>::BoxWindow(const tPt & aC,int aSz)
{
    return cPixBox<Dim>(aC-cPtxd<int,Dim>::PCste(aSz),aC+cPtxd<int,Dim>::PCste(aSz+1));
}

template <const int Dim> cPixBox<Dim>  cPixBox<Dim>::BoxWindow(int aSz)
{
    return BoxWindow(cPtxd<int,Dim>::PCste(0),aSz);
}

template <const int Dim>   cPixBox<Dim>::cPixBox(const cTplBox<int,Dim> & aR) :
   cPixBox<Dim>(aR.P0(),aR.P1(),true)
{
}

template <const int Dim> tINT8  cPixBox<Dim>::IndexeLinear(const tPt & aP) const
{
   tINT8 aRes = 0;
   for (int aK=0 ; aK<Dim ; aK++)
      aRes += tINT8(aP[aK]-tBox::mP0[aK]) * tINT8(tBox::mSzCum[aK]);
   return aRes;
}

template <const int Dim> cPtxd<int,Dim>  cPixBox<Dim>::FromIndexeLinear(tINT8  anIndexe) const
{
   tPt aRes;
   for (int aK=0 ; aK<Dim ; aK++)
   {
      aRes[aK]  = tBox::mP0[aK] + anIndexe % tBox::mSz[aK];
      anIndexe /= tBox::mSz[aK];
   }
   return aRes;
}

template <const int Dim> int cPixBox<Dim>::Interiority(const int  aCoord,int aD) const
{
   return std::min(aCoord-tBox::mP0[aD],tBox::mP1[aD]-1-aCoord);
}

template <const int Dim> int cPixBox<Dim>::Interiority(const tPt &  aP,int aD) const
{
   return Interiority(aP[aD],aD);
}

template <const int Dim> int cPixBox<Dim>::Interiority(const tPt& aP  ) const
{
   int aRes = Interiority(aP,0);
   for (int aD=1 ; aD<Dim ; aD++)
       aRes = std::min(aRes,Interiority(aP,aD));

   return aRes;
}

template <const int Dim> int cPixBox<Dim>::WinInteriority(const tPt& aP,const tPt& aWin,int aD) const
{
   return Interiority(aP,aD)-aWin[aD];
}

template <const int Dim> cBorderPixBox<Dim>  cPixBox<Dim>::Border(int aSz) const
{
   return  cBorderPixBox<Dim>(*this,aSz);
}

template <const int Dim> cPtxd<int,Dim>  cPixBox<Dim>::CircNormProj(const tPt & aPt) const
{
    cPtxd<int,Dim>  aRes;
    for (int aD=0 ; aD<Dim ; aD++)
       aRes[aD] = tBox::mP0[aD] + mod(aPt[aD]-tBox::mP0[aD],tBox::mSz[aD]);
    return aRes;
}

/* ========================== */
/*       cParseBoxInOut       */
/* ========================== */

template <const int Dim> cParseBoxInOut<Dim>::cParseBoxInOut(const tBox & aBoxGlob,const tBox & aBoxIndexe) :
    mBoxGlob (aBoxGlob),
    mBoxIndex (aBoxIndexe)
{
}
        // static tThis  CreateFromMem(const tBox&, double AvalaibleMem);

template <const int Dim> cParseBoxInOut<Dim> cParseBoxInOut<Dim>::CreateFromSize(const tBox & aBox,const tPt & aSzTile)
{
   return  tThis(aBox,tBox(tPt::PCste(0),CByC2P(aBox.Sz(),aSzTile,DivSup<int>)));
}
template <const int Dim> cParseBoxInOut<Dim> cParseBoxInOut<Dim>::CreateFromSizeCste(const tBox & aBox,int aSz)
{
    return CreateFromSize(aBox,tPt::PCste(aSz));
}

template <const int Dim> cParseBoxInOut<Dim> cParseBoxInOut<Dim>::CreateFromSzMem(const tBox & aBox,double aSzMem)
{
    return CreateFromSizeCste(aBox,round_ni(pow(aSzMem,1/double(Dim))));
}

template <const int Dim>  const cPixBox<Dim> &  cParseBoxInOut<Dim>::BoxIndex() const
{
      return  mBoxIndex;
}

template <const int Dim>  cPtxd<int,Dim>  cParseBoxInOut<Dim>::Index2Glob(const tPt & anIndex) const
{
      return  CByC1P(mBoxGlob.FromNormaliseCoord(mBoxIndex.ToNormaliseCoord(anIndex)),round_ni);
}

template <const int Dim>  cPixBox<Dim> cParseBoxInOut<Dim>::BoxOut(const tPt & anIndex) const
{
      return  cPixBox<Dim>
              (
                   Index2Glob(anIndex),
                   Index2Glob(anIndex+tPt::PCste(1))
              );
}

template <const int Dim>  cPixBox<Dim> cParseBoxInOut<Dim>::BoxIn(const tPt & anIndex,const tPt& aDil) const
{
   return mBoxGlob.Inter(BoxOut(anIndex).Dilate(aDil));
}

template <const int Dim>  cPixBox<Dim> cParseBoxInOut<Dim>::BoxIn(const tPt & anIndex,int aDil) const
{
   return mBoxGlob.Inter(BoxOut(anIndex).Dilate(tPt::PCste(aDil)));
}

/* ========================== */
/*          cTplBox           */
/* ========================== */

template <class Type,const int Dim>
   cTplBox<Type,Dim>::cTplBox
   (
       const cPtxd<Type,Dim> & aP0,
       const cPtxd<Type,Dim> & aP1,
       bool AllowEmpty
   ) :
       mP0  (aP0),
       mP1  (aP1),
       mSz  (aP0),
       mNbElem (1)
{
    //for (int aK=Dim-1 ; aK>=0 ; aK--)
    for (int aK=0 ; aK<Dim ; aK++)
    {
       mSz[aK] = mP1[aK] - mP0[aK];
       if (AllowEmpty)
       {
          // make coherent but empty
          if (mSz[aK] <0)
          {
             mP1[aK] = mP0[aK];
             mSz[aK] = 0;
          }
       }
       else
       {
          MMVII_INTERNAL_ASSERT_strong(mSz[aK]>0,MesNegSz);
       }
       mSzCum[aK] = mNbElem;
       mNbElem *= mSz[aK];
    }
}

template <class Type,const int Dim> bool  cTplBox<Type,Dim>::IsEmpty() const
{
   return mNbElem == 0;
}

template <class Type,const int Dim> cTplBox<Type,Dim>  cTplBox<Type,Dim>::Inter(const tBox & aBox)const
{
  return tBox(PtSupEq(mP0,aBox.mP0),PtInfEq(mP1,aBox.mP1),true);
}

template <class Type,const int Dim> cTplBox<Type,Dim>  cTplBox<Type,Dim>::Dilate(const tPt & aPt) const
{
   return tBox(mP0-aPt,mP1+aPt);
}

template <class Type,const int Dim> cTplBox<Type,Dim>  cTplBox<Type,Dim>::Dilate(const Type & aVal) const
{
   return Dilate(tPt::PCste(aVal));
}

template <class Type,const int Dim> void cTplBox<Type,Dim>::AssertSameArea(const cTplBox<Type,Dim> & aR2) const
{
    MMVII_INTERNAL_ASSERT_strong((*this)==aR2,"Rect obj were expected to have identic area");
}
template <class Type,const int Dim> void cTplBox<Type,Dim>::AssertSameSz(const cTplBox<Type,Dim> & aR2) const
{
    MMVII_INTERNAL_ASSERT_strong(Sz()==aR2.Sz(),"Rect obj were expected to have identic size");
}

template <class Type,const int Dim> bool cTplBox<Type,Dim>::operator == (const tBox & aR2) const
{
    return (mP0==aR2.mP0) && (mP1==aR2.mP1);
}

template <class Type,const int Dim> bool cTplBox<Type,Dim>::IncludedIn(const tBox & aR2) const
{
    return SupEq(mP0,aR2.mP0) && InfEq(mP1,aR2.mP1) ;
}

template <class Type,const int Dim> cTplBox<Type,Dim> cTplBox<Type,Dim>::Translate(const cPtxd<Type,Dim> & aTr) const
{
   return cTplBox<Type,Dim>(mP0+aTr,mP1+aTr);
}

template <class Type,const int Dim> cPtxd<Type,Dim>  cTplBox<Type,Dim>::FromNormaliseCoord(const cPtxd<double,Dim> & aPN) const
{
    // MMVII_INTERNAL_ASSERT_strong(false,"To Change
    cPtxd<Type,Dim> aRes;
    for (int aK=0 ; aK<Dim ; aK++)
    {
        // aRes[aK] = mP0[aK] + round_down(mSz[aK]*aPN[aK]);
        aRes[aK] = mP0[aK] + tBaseNumTrait<Type>::RoundNearestToType(mSz[aK]*aPN[aK]);
    }
    return aRes;
    // return Proj(aRes);
}

template <class Type,const int Dim> cPtxd<double,Dim>  cTplBox<Type,Dim>::ToNormaliseCoord(const cPtxd<Type,Dim> & aP) const
{
    cPtxd<double,Dim> aRes;
    for (int aK=0 ; aK<Dim ; aK++)
    {
        aRes[aK] = (aP[aK]-mP0[aK]) / double(mSz[aK]);
    }
    return aRes;
}

template <class Type,const int Dim>  cPtxd<double,Dim>  cTplBox<Type,Dim>::RandomNormalised()
{
   cPtxd<double,Dim>  aRes;
   for (int aK=0 ; aK<Dim ; aK++)
   {
        aRes[aK] = RandUnif_0_1();
   }
   return aRes;
}

template <class Type,const int Dim> cPtxd<Type,Dim>   cTplBox<Type,Dim>::GeneratePointInside() const
{
   return Proj(FromNormaliseCoord(RandomNormalised()));
}

template <class Type,const int Dim> cTplBox<Type,Dim>  cTplBox<Type,Dim>::GenerateRectInside(double aPowSize) const
{
    cPtxd<Type,Dim> aP0;
    cPtxd<Type,Dim> aP1;
    for (int aK=0 ; aK<Dim ; aK++)
    {
        double aSzRed = pow(RandUnif_0_1(),aPowSize);
        double aX0 = (1-aSzRed) * RandUnif_0_1();
        double aX1 = aX0 + aSzRed;
        // int aI0 = round_down(aX0*mSz[aK]);
        // int aI1 = round_down(aX1*mSz[aK]);
        Type aI0 =  tBaseNumTrait<Type>::RoundNearestToType(aX0*mSz[aK]);
        Type aI1 =  tBaseNumTrait<Type>::RoundNearestToType(aX1*mSz[aK]);
        aI1 = std::min((mP1[aK]-1),std::max(aI1,aI0+1));
        aI0  = std::max((mP0[aK]),std::min(aI0,aI1-1));
        aP0[aK] = aI0;
        aP1[aK] = aI1;

    }
    return cTplBox<Type,Dim>(aP0,aP1);
}

/* ========================== */
/*       cTpxBoxOfPts         */
/* ========================== */

template <class Type,const int Dim>   cTplBoxOfPts<Type,Dim>::cTplBoxOfPts() :
   mNbPts (0),
   mP0    (tPt::PCste(0)),
   mP1    (tPt::PCste(0))
{
}

template <class Type,const int Dim>  int  cTplBoxOfPts<Type,Dim>::NbPts() const {return mNbPts;}

template <class Type,const int Dim>  const cPtxd<Type,Dim> &  cTplBoxOfPts<Type,Dim>::P0() const
{
   MMVII_INTERNAL_ASSERT_medium(mNbPts,"cTplBoxOfPts<Type,Dim>::P0()")
   return  mP0;
}

template <class Type,const int Dim>  const cPtxd<Type,Dim> &  cTplBoxOfPts<Type,Dim>::P1() const
{
   MMVII_INTERNAL_ASSERT_medium(mNbPts,"cTplBoxOfPts<Type,Dim>::P1()")
   return  mP1;
}

template <class Type,const int Dim>  cTplBox<Type,Dim>  cTplBoxOfPts<Type,Dim>::CurBox() const
{
    return  cTplBox<Type,Dim>(mP0,mP1);
}

template <class Type,const int Dim>  void  cTplBoxOfPts<Type,Dim>::Add(const tPt & aP)
{
   if (mNbPts==0)
   {
      mP0 = aP;
      mP1 = aP;
   }
   else
   {
      SetInfEq(mP0,aP);
      SetSupEq(mP1,aP);
   }
   mNbPts++;
}

/* ========================== */
/*       INSTANTIATION        */
/* ========================== */

#define MACRO_INSTATIATE_PRECT_DIM(DIM)\
template class cBorderPixBoxIterator<DIM>;\
template class cBorderPixBox<DIM>;\
template class cTplBox<tINT4,DIM>;\
template class cTplBoxOfPts<tINT4,DIM>;\
template  class cParseBoxInOut<DIM>;\
template class cTplBox<tREAL8,DIM>;\
template class cTplBoxOfPts<tREAL8,DIM>;\
template class cPixBox<DIM>;\
template  int NbPixVign(const cPtxd<int,DIM> & aVign);\
template class cDataGenUnTypedIm<DIM>;\
template <> const cPixBox<DIM> cPixBox<DIM>::TheEmptyBox(cPtxd<int,DIM>::PCste(0),cPtxd<int,DIM>::PCste(0),true);

MACRO_INSTATIATE_PRECT_DIM(1)
MACRO_INSTATIATE_PRECT_DIM(2)
MACRO_INSTATIATE_PRECT_DIM(3)

};