#include <vector>
#include <type_traits>
#include <iterator>

#include <Eigen/Core>
#include <iostream>

#include "utils.hpp"

namespace tridiagonal {
    using std::vector;
    using Eigen::MatrixBase;

    namespace internal {

        // #####################################################################################
        //
        //                                   SOLVER
        //
        // #####################################################################################

        template <typename IterA, typename IterB, typename IterC, typename IterD,
                typename A = typename std::iterator_traits<IterA>::value_type,
                typename B = typename std::iterator_traits<IterB>::value_type,
                typename C = typename std::iterator_traits<IterC>::value_type,
                typename D = typename std::iterator_traits<IterD>::value_type,
                typename std::enable_if<std::is_base_of<MatrixBase<A>, A>::value, int>::type = 0,
                typename std::enable_if<std::is_base_of<MatrixBase<B>, B>::value, int>::type = 0,
                typename std::enable_if<std::is_base_of<MatrixBase<C>, C>::value, int>::type = 0,
                typename std::enable_if<std::is_base_of<MatrixBase<D>, D>::value, int>::type = 0
        >
        vector<D> solve_off_tridiagonal(IterA lower_diagonal_begin, IterA lower_diagonal_end,
                                        IterB diagonal_begin, IterB diagonal_end,
                                        IterC upper_diagonal_begin, IterC upper_diagonal_end,
                                        IterD rhs_begin, IterD rhs_end){

            long n = std::distance(diagonal_begin, diagonal_end);

            if(n == 1){
                //
                //  | B_0 | | x_0 | = | D_0 |
                //
                D x = (*diagonal_begin).fullPivHouseholderQr().solve(*rhs_begin);
                return {x};
            }

            A const & a0 = *lower_diagonal_begin;
            B const & b0 = *diagonal_begin;
            C const & c0 = *upper_diagonal_begin;
            D const & d0 = *rhs_begin;

            auto b0inv_c0 = b0.inverse() * c0;
            auto b0inv_d0 = b0.inverse() * d0;

            ++lower_diagonal_begin; ++diagonal_begin; ++upper_diagonal_begin; ++rhs_begin;

            if(n == 2){
                //
                //  | B_0 C_0 | | x_0 | = | D_0 |
                //  | A_0 B_1 | | x_1 | = | D_1 |
                //

                // B_1' = B_1 - A_0 * B_0.inv() * C_0
                *diagonal_begin = *diagonal_begin - a0 * b0inv_c0;

                // D_1' = D_1 - A_0 * B_0.inv() * D_0
                *rhs_begin = *rhs_begin - a0 * b0inv_d0;

                vector<D> x_list = solve_off_tridiagonal(lower_diagonal_begin, lower_diagonal_end,
                                                         diagonal_begin, diagonal_end,
                                                         upper_diagonal_begin, upper_diagonal_end,
                                                         rhs_begin, rhs_end);

                D x = b0inv_d0 - b0inv_c0 * x_list[0];

                x_list.insert(x_list.begin(), x);

                return x_list;
            }

            --lower_diagonal_end; --diagonal_end; --upper_diagonal_end; --rhs_end;
            auto b0inv_a0 = b0.inverse() * a0;

            // B_1' = B_1 - A_1 * B_0.inv() * C_0
            *diagonal_begin = *diagonal_begin - *lower_diagonal_begin * b0inv_c0;

            // B_{n-1}' = B_{n-1} - C_{n-1} * B_0.inv() * A_0
            *diagonal_end = *diagonal_end - *upper_diagonal_end * b0inv_a0;

            // D_1' = D_1 - A_1 * B_0.inv() * D_0
            *rhs_begin = *rhs_begin - *lower_diagonal_begin * b0inv_d0;

            // D_{n-1}' = D_{n-1} - C_{n-1} * B_0.inv() * D_0
            *rhs_end = *rhs_end - *upper_diagonal_end * b0inv_d0;

            if(n == 3){
                //
                //  | B_0 C_0 A_0 | | x_0 | = | D_0 |
                //  | A_1 B_1 C_1 | | x_1 | = | D_1 |
                //  | C_2 A_2 B_2 | | x_2 | = | D_2 |
                //

                // C_1' = C_1 - A_1 * B_0.inv() * A_0
                *upper_diagonal_begin = *upper_diagonal_begin - *lower_diagonal_begin * b0inv_a0;

                // A_2' = A_2 - C_2 * B_0.inv() * C_0
                *lower_diagonal_end = *lower_diagonal_end - *upper_diagonal_end * b0inv_c0;

                ++lower_diagonal_begin; --upper_diagonal_end;
            }
            else{
                // A_1' = - A_1 * B_0.inv() * A_0
                *lower_diagonal_begin = -1 * *lower_diagonal_begin * b0inv_a0;

                // C_{n-1}' = - C_{n-1} * B_0*inv() * C_0
                *upper_diagonal_end = -1 * *upper_diagonal_end * b0inv_c0;
            }

            ++lower_diagonal_end; ++diagonal_end; ++upper_diagonal_end; ++rhs_end;
            vector<D> x_list = solve_off_tridiagonal(lower_diagonal_begin, lower_diagonal_end,
                                                     diagonal_begin, diagonal_end,
                                                     upper_diagonal_begin, upper_diagonal_end,
                                                     rhs_begin, rhs_end);

            // X_0 = B_0.inv() * (D_0 - C_0 * X_1 - A_0 * X_{n-1})
            D x = b0inv_d0 - b0inv_c0 * x_list[0] - b0inv_a0 * x_list[x_list.size() - 1];

            x_list.insert(x_list.begin(), x);

            return x_list;
        }

        // #####################################################################################
        //
        //                                  INPUT CHECKING
        //
        // #####################################################################################

        template <typename A, typename B, typename C, typename D,
                typename std::enable_if<std::is_base_of<MatrixBase<A>, A>::value, int>::type = 0,
                typename std::enable_if<std::is_base_of<MatrixBase<B>, B>::value, int>::type = 0,
                typename std::enable_if<std::is_base_of<MatrixBase<C>, C>::value, int>::type = 0,
                typename std::enable_if<std::is_base_of<MatrixBase<D>, D>::value, int>::type = 0
        >
        void check_valid_off_tridiagonal_number_of_matrices(vector<A> const &lower_diagonal,
                                                            vector<B> const &diagonal,
                                                            vector<C> const &upper_diagonal,
                                                            vector<D> const &rhs) {

            if(diagonal.size() != rhs.size()){
                throw std::invalid_argument(invalid_number_of_elements_message(lower_diagonal,
                                                                               diagonal,
                                                                               upper_diagonal,
                                                                               rhs));
            }

            if(diagonal.empty()){
                throw std::invalid_argument(invalid_number_of_elements_message(lower_diagonal,
                                                                               diagonal,
                                                                               upper_diagonal,
                                                                               rhs));
            }
            else if(diagonal.size() == 1){
                if(lower_diagonal.empty() and !upper_diagonal.empty()) {
                    throw std::invalid_argument(invalid_number_of_elements_message(lower_diagonal,
                                                                                diagonal,
                                                                                upper_diagonal,
                                                                                rhs));
                }
            }
            else if(diagonal.size() == 2){
                if(lower_diagonal.size() != 1 and upper_diagonal.size() != 1) {
                    throw std::invalid_argument(invalid_number_of_elements_message(lower_diagonal,
                                                                                diagonal,
                                                                                upper_diagonal,
                                                                                rhs));
                }
            }
            else if(diagonal.size() != lower_diagonal.size() or diagonal.size() != upper_diagonal.size()){
                throw std::invalid_argument(invalid_number_of_elements_message(lower_diagonal,
                                                                               diagonal,
                                                                               upper_diagonal,
                                                                               rhs));
            }
        }

        template <typename A, typename B, typename C, typename D,
                typename std::enable_if<std::is_base_of<MatrixBase<A>, A>::value, int>::type = 0,
                typename std::enable_if<std::is_base_of<MatrixBase<B>, B>::value, int>::type = 0,
                typename std::enable_if<std::is_base_of<MatrixBase<C>, C>::value, int>::type = 0,
                typename std::enable_if<std::is_base_of<MatrixBase<D>, D>::value, int>::type = 0
        >
        void check_valid_off_tridiagonal_matrix_dimensions(vector<A> const &lower_diagonal,
                                                           vector<B> const &diagonal,
                                                           vector<C> const &upper_diagonal,
                                                           vector<D> const &rhs) {
            unsigned long n = diagonal.size();

            if (n == 1){
                if (diagonal[0].rows() != rhs[0].rows()){
                    throw std::invalid_argument("Invalid matrix dimensions");
                }
            }
            else if(n == 2){
                // upper left corner
                if(diagonal[0].cols() != lower_diagonal[0].cols() or
                   diagonal[0].rows() != upper_diagonal[0].rows() or
                   diagonal[0].rows() != rhs[0].rows()) {
                    throw std::invalid_argument("Invalid matrix dimensions");
                }

                // bottom right corner
                if(diagonal[1].cols() != upper_diagonal[0].cols() or
                   diagonal[1].rows() != lower_diagonal[0].rows() or
                   diagonal[1].rows() != rhs[1].rows()){
                    throw std::invalid_argument("Invalid matrix dimensions");
                }
            }
            else{
                // upper left corner
                if(diagonal[0].cols() != lower_diagonal[1].cols() or
                   diagonal[0].cols() != upper_diagonal[n-1].cols() or
                   diagonal[0].rows() != upper_diagonal[0].rows() or
                   diagonal[0].rows() != lower_diagonal[0].rows() or
                   diagonal[0].rows() != rhs[0].rows()) {
                    throw std::invalid_argument("Invalid matrix dimensions");
                }

                // middle
                for(int i = 1; i < diagonal.size()-1; i++){
                    if(diagonal[i].cols() != upper_diagonal[i-1].cols() or
                       diagonal[i].cols() != lower_diagonal[i+1].cols() or
                       diagonal[i].rows() != upper_diagonal[i].rows() or
                       diagonal[i].rows() != lower_diagonal[i].rows() or
                       diagonal[i].rows() != rhs[i].rows()){
                        throw std::invalid_argument("Invalid matrix dimensions");
                    }
                }

                // bottom right corner
                if(diagonal[n-1].cols() != upper_diagonal[n-2].cols() or
                   diagonal[n-1].cols() != lower_diagonal[0].cols() or
                   diagonal[n-1].rows() != lower_diagonal[n-1].rows() or
                   diagonal[n-1].rows() != upper_diagonal[n-1].rows() or
                   diagonal[n-1].rows() != rhs[n-1].rows()){
                    throw std::invalid_argument("Invalid matrix dimensions");
                }
            }

            // check that all diagonal matrices are square
            for (auto const & d : diagonal){
                if (d.cols() != d.rows()){
                    throw std::invalid_argument("All diagonal matrices must be square");
                }
            }

            // check that all rhs matrices have same number of columns
            long col_size = rhs[0].cols();
            for (auto const & r : rhs) {
                if (r.cols() != col_size){
                    throw std::invalid_argument("All right-hand-side matrices must have the same number of columns.");
                }
            }
        }

    }

    // #####################################################################################
    //
    //                                INTERFACE FUNCTIONS
    //
    // #####################################################################################

    template <typename A, typename B, typename C, typename D,
            typename std::enable_if<!is_dynamic<A>::value, int>::type,
            typename std::enable_if<!is_dynamic<B>::value, int>::type,
            typename std::enable_if<!is_dynamic<C>::value, int>::type,
            typename std::enable_if<!is_dynamic<D>::value, int>::type,
            typename std::enable_if<A::RowsAtCompileTime == A::ColsAtCompileTime, int>::type,
            typename std::enable_if<A::RowsAtCompileTime == B::RowsAtCompileTime, int>::type,
            typename std::enable_if<A::ColsAtCompileTime == B::ColsAtCompileTime, int>::type,
            typename std::enable_if<A::RowsAtCompileTime == C::RowsAtCompileTime, int>::type,
            typename std::enable_if<A::ColsAtCompileTime == C::ColsAtCompileTime, int>::type,
            typename std::enable_if<A::RowsAtCompileTime == D::RowsAtCompileTime, int>::type
    >
    vector<D> solve_off_tridiagonal(vector<A> const & lower_diagonal,
                                    vector<B> const & diagonal,
                                    vector<C> const & upper_diagonal,
                                    vector<D> const & rhs) {

        internal::check_valid_off_tridiagonal_number_of_matrices(lower_diagonal, diagonal, upper_diagonal, rhs);

        vector<A> _lower_diagonal = lower_diagonal;
        vector<B> _diagonal = diagonal;
        vector<C> _upper_diagonal = upper_diagonal;
        vector<D> _rhs = rhs;

        return internal::solve_off_tridiagonal(_lower_diagonal.begin(), _lower_diagonal.end(),
                                               _diagonal.begin(), _diagonal.end(),
                                               _upper_diagonal.begin(), _upper_diagonal.end(),
                                               _rhs.begin(), _rhs.end());
    }

    template <typename A, typename B, typename C, typename D,
            typename std::enable_if<is_dynamic<A>::value or
                                    is_dynamic<B>::value or
                                    is_dynamic<C>::value or
                                    is_dynamic<D>::value, int>::type
    >
    vector<D> solve_off_tridiagonal(vector<A> const & lower_diagonal,
                                    vector<B> const & diagonal,
                                    vector<C> const & upper_diagonal,
                                    vector<D> const & rhs) {

        internal::check_valid_off_tridiagonal_number_of_matrices(lower_diagonal, diagonal, upper_diagonal, rhs);
        internal::check_valid_off_tridiagonal_matrix_dimensions(lower_diagonal, diagonal, upper_diagonal, rhs);

        vector<A> _lower_diagonal = lower_diagonal;
        vector<B> _diagonal = diagonal;
        vector<C> _upper_diagonal = upper_diagonal;
        vector<D> _rhs = rhs;

        return internal::solve_off_tridiagonal(_lower_diagonal.begin(), _lower_diagonal.end(),
                                               _diagonal.begin(), _diagonal.end(),
                                               _upper_diagonal.begin(), _upper_diagonal.end(),
                                               _rhs.begin(), _rhs.end());
    }

}


// Copyright Oleg Maximenko 2014.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://github.com/svgpp/svgpp for library home page.

#pragma once

#include <svgpp/definitions.hpp>
#include <svgpp/utility/gil/common.hpp>
#include <boost/gil/channel_algorithm.hpp>
#include <boost/gil/color_base_algorithm.hpp>

namespace svgpp
{

namespace gil_detail
{

namespace gil = boost::gil;

template<class CompositeModeTag, class ChannelValue>
struct composite_channel_fn;

template<class CompositeModeTag, class ChannelValue>
struct composite_alpha_fn;

template<class ChannelValue>
struct composite_arithmetic_channel_fn;

// TODO: default implementation for non-8 bit channels

// For signed channels we call unsigned analog, converting forward and back
template<class CompositeModeTag>
struct composite_channel_fn<CompositeModeTag, boost::int8_t>
{
  boost::int8_t operator()(boost::int8_t channel_a, boost::int8_t channel_b, boost::int8_t alpha_a, boost::int8_t alpha_b) const
  {
    typedef gil::detail::channel_convert_to_unsigned<boost::int8_t> to_unsigned;
    typedef gil::detail::channel_convert_from_unsigned<boost::int8_t> from_unsigned;
    composite_channel_fn<CompositeModeTag, boost::uint8_t> converter_unsigned;
    return from_unsigned()(converter_unsigned(
      to_unsigned()(channel_a), to_unsigned()(channel_b), to_unsigned()(alpha_a), to_unsigned()(alpha_b)));
  }
};

// Dca' = Sca + Dca x (1 - Sa)
template<>
struct composite_channel_fn<tag::value::over, boost::uint8_t>
{
  boost::uint8_t operator()(int channel_a, int channel_b, int alpha_a, int alpha_b) const
  {
    return clamp_channel_bits8(channel_a + channel_b * (255 - alpha_a) / 255);
  }
};

// Da'  = Sa + Da - Sa x Da
template<>
struct composite_alpha_fn<tag::value::over, boost::uint8_t>
{
  boost::uint8_t operator()(int alpha_a, int alpha_b) const
  {
    return clamp_channel_bits8(alpha_a + alpha_b - alpha_a * alpha_b / 255);
  }
};

// Dca' = Sca x Da
template<>
struct composite_channel_fn<tag::value::in, boost::uint8_t>
{
  boost::uint8_t operator()(int channel_a, int channel_b, int alpha_a, int alpha_b) const
  {
    return channel_a * alpha_b / 255;
  }
};

// Da'  = Sa x Da
template<>
struct composite_alpha_fn<tag::value::in, boost::uint8_t>
{
  boost::uint8_t operator()(int alpha_a, int alpha_b) const
  {
    return alpha_a * alpha_b / 255;
  }
};

// Dca' = Sca x (1 - Da)
template<>
struct composite_channel_fn<tag::value::out, boost::uint8_t>
{
  boost::uint8_t operator()(int channel_a, int channel_b, int alpha_a, int alpha_b) const
  {
    return channel_a * alpha_a * (255 - alpha_b) / 65535;
  }
};

// Da'  = Sa x (1 - Da)
template<>
struct composite_alpha_fn<tag::value::out, boost::uint8_t>
{
  boost::uint8_t operator()(int alpha_a, int alpha_b) const
  {
    return alpha_a * (255 - alpha_b) / 255;
  }
};

// Dca' = Sca x Da + Dca x (1 - Sa)
template<>
struct composite_channel_fn<tag::value::atop, boost::uint8_t>
{
  boost::uint8_t operator()(int channel_a, int channel_b, int alpha_a, int alpha_b) const
  {
    return (channel_a * alpha_a + channel_b * (255 - alpha_a)) * alpha_b / 65535;
  }
};

// Da'  = Da
template<>
struct composite_alpha_fn<tag::value::atop, boost::uint8_t>
{
  boost::uint8_t operator()(boost::uint8_t alpha_a, boost::uint8_t alpha_b) const
  {
    return alpha_b;
  }
};

// Dca' = Sca x (1 - Da) + Dca x (1 - Sa)
template<>
struct composite_channel_fn<tag::value::xor_, boost::uint8_t>
{
  boost::uint8_t operator()(int channel_a, int channel_b, int alpha_a, int alpha_b) const
  {
    return clamp_channel_bits8((channel_a * alpha_a * (255 - alpha_b) + channel_b * alpha_b * (255 - alpha_a)) / 65535);
  }
};

// Da'  = Sa + Da - 2 x Sa x Da
template<>
struct composite_alpha_fn<tag::value::xor_, boost::uint8_t>
{
  boost::uint8_t operator()(int alpha_a, int alpha_b) const
  {
    return clamp_channel_bits8((alpha_a + alpha_b - 2 * alpha_a * alpha_b) / 255);
  }
};

// result = k1*i1*i2 + k2*i1 + k3*i2 + k4
template<>
struct composite_arithmetic_channel_fn<boost::uint8_t>
{
  template<class Coefficient>
  composite_arithmetic_channel_fn(Coefficient k1, Coefficient k2, Coefficient k3, Coefficient k4)
    : k1_(k1 * 255), k2_(k2 * 255), k3_(k3 * 255), k4_(k4 * 255)
  {}

  boost::uint8_t operator()(int channel_a, int channel_b) const
  {
    return clamp_channel_bits8(k1_ * channel_a * channel_b / 65535 + k2_ * channel_a / 255 + k3_ * channel_b / 255 + k4_);
  }

private:
  int k1_, k2_, k3_, k4_;
};

} // namespace gil_detail

namespace gil_utility
{

namespace gil = boost::gil;

template<class CompositeModeTag>
struct composite_pixel
{
  template<class Color>
  Color operator()(const Color & pixa, const Color & pixb) const
  {
    typename gil::color_element_type<Color, gil::alpha_t>::type
      alpha_a = gil::get_color(pixa, gil::alpha_t()),
      alpha_b = gil::get_color(pixb, gil::alpha_t());

    Color result;

    gil::get_color(result, gil::red_t())
      = gil_detail::composite_channel_fn<CompositeModeTag, typename gil::color_element_type<Color, gil::red_t>::type>()(
        gil::get_color(pixa, gil::red_t()), gil::get_color(pixb, gil::red_t()),
        alpha_a, alpha_b);

    gil::get_color(result, gil::green_t())
      = gil_detail::composite_channel_fn<CompositeModeTag, typename gil::color_element_type<Color, gil::green_t>::type>()(
        gil::get_color(pixa, gil::green_t()), gil::get_color(pixb, gil::green_t()),
        alpha_a, alpha_b);

    gil::get_color(result, gil::blue_t())
      = gil_detail::composite_channel_fn<CompositeModeTag, typename gil::color_element_type<Color, gil::blue_t>::type>()(
        gil::get_color(pixa, gil::blue_t()), gil::get_color(pixb, gil::blue_t()),
        alpha_a, alpha_b);

    gil::get_color(result, gil::alpha_t()) =
      gil_detail::composite_alpha_fn<CompositeModeTag, typename gil::color_element_type<Color, gil::alpha_t>::type>()(alpha_a, alpha_b);

    return result;
  }
};

template<class Color>
struct composite_pixel_arithmetic
{
  template<class Coefficient>
  composite_pixel_arithmetic(Coefficient k1, Coefficient k2, Coefficient k3, Coefficient k4)
    : r_(k1, k2, k3, k4)
    , g_(k1, k2, k3, k4)
    , b_(k1, k2, k3, k4)
    , a_(k1, k2, k3, k4)
  {}

  Color operator()(const Color & pixa, const Color & pixb) const
  {
    Color result;
    gil::get_color(result, gil::red_t())   = r_(gil::get_color(pixa, gil::red_t())   , gil::get_color(pixb, gil::red_t())   );
    gil::get_color(result, gil::green_t()) = r_(gil::get_color(pixa, gil::green_t()) , gil::get_color(pixb, gil::green_t()) );
    gil::get_color(result, gil::blue_t())  = r_(gil::get_color(pixa, gil::blue_t())  , gil::get_color(pixb, gil::blue_t())  );
    gil::get_color(result, gil::alpha_t()) = r_(gil::get_color(pixa, gil::alpha_t()) , gil::get_color(pixb, gil::alpha_t()) );
    return result;
  }

private:
  gil_detail::composite_arithmetic_channel_fn<typename gil::color_element_type<Color, gil::red_t  >::type> r_;
  gil_detail::composite_arithmetic_channel_fn<typename gil::color_element_type<Color, gil::green_t>::type> g_;
  gil_detail::composite_arithmetic_channel_fn<typename gil::color_element_type<Color, gil::blue_t >::type> b_;
  gil_detail::composite_arithmetic_channel_fn<typename gil::color_element_type<Color, gil::alpha_t>::type> a_;
};

}}


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

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

 * See LICENSE for the license information

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

/**
 *  @file   testSubgraphConditioner.cpp
 *  @brief  Unit tests for SubgraphPreconditioner
 *  @author Frank Dellaert
 **/

#include <CppUnitLite/TestHarness.h>

#if 0

#include <tests/smallExample.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/iterative.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/base/numericalDerivative.h>

#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp>
using namespace boost::assign;

using namespace std;
using namespace gtsam;
using namespace example;

// define keys
// Create key for simulated planar graph
Symbol key(int x, int y) {
  return symbol_shorthand::X(1000*x+y);
}

/* ************************************************************************* */
TEST( SubgraphPreconditioner, planarOrdering ) {
  // Check canonical ordering
  Ordering expected, ordering = planarOrdering(3);
  expected +=
      key(3, 3), key(2, 3), key(1, 3),
      key(3, 2), key(2, 2), key(1, 2),
      key(3, 1), key(2, 1), key(1, 1);
  CHECK(assert_equal(expected,ordering));
}

/* ************************************************************************* */
/** unnormalized error */
static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
  double total_error = 0.;
  for(const GaussianFactor::shared_ptr& factor: fg)
    total_error += factor->error(x);
  return total_error;
}

/* ************************************************************************* */
TEST( SubgraphPreconditioner, planarGraph )
  {
  // Check planar graph construction
  GaussianFactorGraph A;
  VectorValues xtrue;
  boost::tie(A, xtrue) = planarGraph(3);
  LONGS_EQUAL(13,A.size());
  LONGS_EQUAL(9,xtrue.size());
  DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue

  // Check that xtrue is optimal
  GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate();
  VectorValues actual = optimize(*R1);
  CHECK(assert_equal(xtrue,actual));
}

/* ************************************************************************* */
TEST( SubgraphPreconditioner, splitOffPlanarTree )
{
  // Build a planar graph
  GaussianFactorGraph A;
  VectorValues xtrue;
  boost::tie(A, xtrue) = planarGraph(3);

  // Get the spanning tree and constraints, and check their sizes
  GaussianFactorGraph T, C;
  boost::tie(T, C) = splitOffPlanarTree(3, A);
  LONGS_EQUAL(9,T.size());
  LONGS_EQUAL(4,C.size());

  // Check that the tree can be solved to give the ground xtrue
  GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate();
  VectorValues xbar = optimize(*R1);
  CHECK(assert_equal(xtrue,xbar));
}

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

TEST( SubgraphPreconditioner, system )
{
  // Build a planar graph
  GaussianFactorGraph Ab;
  VectorValues xtrue;
  size_t N = 3;
  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b

  // Get the spanning tree and corresponding ordering
  GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
  boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
  SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
  SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));

  // Eliminate the spanning tree to build a prior
  SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
  VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1

  // Create Subgraph-preconditioned system
  VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
  SubgraphPreconditioner system(Ab2, Rc1, xbarShared);

  // Create zero config
  VectorValues zeros = VectorValues::Zero(xbar);

  // Set up y0 as all zeros
  VectorValues y0 = zeros;

  // y1 = perturbed y0
  VectorValues y1 = zeros;
  y1[1] = Vector2(1.0, -1.0);

  // Check corresponding x  values
  VectorValues expected_x1 = xtrue, x1 = system.x(y1);
  expected_x1[1] = Vector2(2.01, 2.99);
  expected_x1[0] = Vector2(3.01, 2.99);
  CHECK(assert_equal(xtrue, system.x(y0)));
  CHECK(assert_equal(expected_x1,system.x(y1)));

  // Check errors
  DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9);
  DOUBLES_EQUAL(3,error(Ab,x1),1e-9);
  DOUBLES_EQUAL(0,error(system,y0),1e-9);
  DOUBLES_EQUAL(3,error(system,y1),1e-9);

  // Test gradient in x
  VectorValues expected_gx0 = zeros;
  VectorValues expected_gx1 = zeros;
  CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue)));
  expected_gx1[2] = Vector2(-100., 100.);
  expected_gx1[4] = Vector2(-100., 100.);
  expected_gx1[1] = Vector2(200., -200.);
  expected_gx1[3] = Vector2(-100., 100.);
  expected_gx1[0] = Vector2(100., -100.);
  CHECK(assert_equal(expected_gx1,gradient(Ab,x1)));

  // Test gradient in y
  VectorValues expected_gy0 = zeros;
  VectorValues expected_gy1 = zeros;
  expected_gy1[2] = Vector2(2., -2.);
  expected_gy1[4] = Vector2(-2., 2.);
  expected_gy1[1] = Vector2(3., -3.);
  expected_gy1[3] = Vector2(-1., 1.);
  expected_gy1[0] = Vector2(1., -1.);
  CHECK(assert_equal(expected_gy0,gradient(system,y0)));
  CHECK(assert_equal(expected_gy1,gradient(system,y1)));

  // Check it numerically for good measure
  // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1)
  //  Vector numerical_g1 = numericalGradient<VectorValues> (error, y1, 0.001);
  //  Vector expected_g1 = (Vector(18) << 0., 0., 0., 0., 2., -2., 0., 0., -2., 2.,
  //      3., -3., 0., 0., -1., 1., 1., -1.);
  //  CHECK(assert_equal(expected_g1,numerical_g1));
}

/* ************************************************************************* */
TEST( SubgraphPreconditioner, conjugateGradients )
{
  // Build a planar graph
  GaussianFactorGraph Ab;
  VectorValues xtrue;
  size_t N = 3;
  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b

  // Get the spanning tree and corresponding ordering
  GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
  boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
  SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
  SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));

  // Eliminate the spanning tree to build a prior
  Ordering ordering = planarOrdering(N);
  SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
  VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1

  // Create Subgraph-preconditioned system
  VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
  SubgraphPreconditioner system(Ab2, Rc1, xbarShared);

  // Create zero config y0 and perturbed config y1
  VectorValues y0 = VectorValues::Zero(xbar);

  VectorValues y1 = y0;
  y1[1] = Vector2(1.0, -1.0);
  VectorValues x1 = system.x(y1);

  // Solve for the remaining constraints using PCG
  ConjugateGradientParameters parameters;
  VectorValues actual = conjugateGradients<SubgraphPreconditioner,
      VectorValues, Errors>(system, y1, parameters);
  CHECK(assert_equal(y0,actual));

  // Compare with non preconditioned version:
  VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters);
  CHECK(assert_equal(xtrue,actual2,1e-4));
}

#endif

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


// 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)

#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/multiplies.hpp>
#include <boost/mpl/pair.hpp>
#include <boost/mpl/plus.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/concept/constant.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/minus.hpp>
#include <boost/hana/mult.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/plus.hpp>

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

template <typename T, typename = std::enable_if_t<
  !hana::Constant<T>::value
>>
constexpr T sqrt(T x) {
  T inf = 0, sup = (x == 1 ? 1 : x/2);
  while (!((sup - inf) <= 1 || ((sup*sup <= x) && ((sup+1)*(sup+1) > x)))) {
    T mid = (inf + sup) / 2;
    bool take_inf = mid*mid > x ? 1 : 0;
    inf = take_inf ? inf : mid;
    sup = take_inf ? mid : sup;
  }

  return sup*sup <= x ? sup : inf;
}

template <typename T, typename = std::enable_if_t<
  hana::Constant<T>::value
>>
constexpr auto sqrt(T const&) {
  return hana::integral_c<typename T::value_type, sqrt(T::value)>;
}

namespace then {
namespace mpl = boost::mpl;

template <typename N>
struct sqrt
  : mpl::integral_c<typename N::value_type, ::sqrt(N::value)>
{ };

template <typename X, typename Y>
struct point {
  using x = X;
  using y = Y;
};

//! [distance-mpl]
template <typename P1, typename P2>
struct distance {
  using xs = typename mpl::minus<typename P1::x,
                                 typename P2::x>::type;
  using ys = typename mpl::minus<typename P1::y,
                                 typename P2::y>::type;
  using type = typename sqrt<
    typename mpl::plus<
      typename mpl::multiplies<xs, xs>::type,
      typename mpl::multiplies<ys, ys>::type
    >::type
  >::type;
};

static_assert(mpl::equal_to<
  distance<point<mpl::int_<3>, mpl::int_<5>>,
           point<mpl::int_<7>, mpl::int_<2>>>::type,
  mpl::int_<5>
>::value, "");
//! [distance-mpl]
}

namespace now {
namespace hana = boost::hana;
using namespace hana::literals;

template <typename X, typename Y>
struct _point {
  X x;
  Y y;
};
template <typename X, typename Y>
constexpr _point<X, Y> point(X x, Y y) { return {x, y}; }

//! [distance-hana]
template <typename P1, typename P2>
constexpr auto distance(P1 p1, P2 p2) {
  auto xs = p1.x - p2.x;
  auto ys = p1.y - p2.y;
  return sqrt(xs*xs + ys*ys);
}

BOOST_HANA_CONSTANT_CHECK(distance(point(3_c, 5_c), point(7_c, 2_c)) == 5_c);
//! [distance-hana]

void test() {

//! [distance-dynamic]
auto p1 = point(3, 5); // dynamic values now
auto p2 = point(7, 2); //
BOOST_HANA_RUNTIME_CHECK(distance(p1, p2) == 5); // same function works!
//! [distance-dynamic]

}
}

int main() {
  now::test();
}


/* ScaFES
 * Copyright (c) 2011-2016, ZIH, TU Dresden, Federal Republic of Germany.
 * For details, see the files COPYING and LICENSE in the base directory
 * of the package.
 */

/**
 *  @file ScaFES_Complex.hpp
 *  @brief Contains the class template Complex.
 */

#ifndef SCAFES_COMPLEX_HPP_
#define SCAFES_COMPLEX_HPP_

#include <iostream>
#include <iomanip>
#include <ios>
#include <cmath>
#include <cstdlib>
#include <type_traits>
#include <stdexcept>

#ifdef SCAFES_HAVE_BOOST
#include <boost/version.hpp>
#endif

#ifdef SCAFES_HAVE_BOOST_SERIALIZATION
namespace boost
{
namespace serialization
{
    class access;
}
}
#include <boost/serialization/version.hpp>
#if BOOST_VERSION < 105900
    #include <boost/serialization/pfto.hpp>
#endif
//#include <boost/serialization/complex.hpp>
#endif

namespace ScaFES
{
/*******************************************************************************
 ******************************************************************************/
/** \class Complex
 * @brief The class template \c Complex represents a complex number
 * of a given type \c TT (usually of type \c double).
 *
 * All important unary and binary operators are implemented resp. overloaded
 * and behave as expected. Especially, many operators work componentwise.
 */
template <typename TT> class Complex
{
public:
    /*----------------------------------------------------------------------
    | TYPE DEFINITIONS.
    ----------------------------------------------------------------------*/
    /** Re-export typename TT STL-like. */
    typedef TT value_type;

#ifdef SCAFES_HAVE_BOOST_SERIALIZATION
    /*----------------------------------------------------------------------
    | FRIEND CLASSES.
    ----------------------------------------------------------------------*/
    friend class boost::serialization::access;
#endif

    /*----------------------------------------------------------------------
    | LIFE CYCLE METHODS.
    ----------------------------------------------------------------------*/
    /** Creates the default constructor. */
    Complex<TT>();

    /** Creates a special constructor for threedimensional vectors:
     *  Both parts are initialized by given values. */
    Complex<TT>(TT const&, TT const&);

    /** Creates own constructor:
     *  \remark Only The REAL PART is initalized by the given value,
     *  the imaginary part remains ZERO! */
    Complex<TT>(TT const& re);

    /** Creates copy constructor. */
    Complex<TT>(ScaFES::Complex<TT> const& rhs);

    /** Creates the destructor. */
    ~Complex();

    /** Creates copy assignment operator using the copy-and-swap idiom. */
    ScaFES::Complex<TT>& operator=(ScaFES::Complex<TT> rhs);

    /*----------------------------------------------------------------------
    | GETTER METHODS.
    ----------------------------------------------------------------------*/
    /** Returns the real part of this complex number. */
    const TT& real() const;

    /** Returns the imaginary part of this complex number. */
    const TT& imag() const;

    /** Returns the element at position \c idx in the complex number. */
    const TT& operator[](std::size_t const& idx) const;

    /*----------------------------------------------------------------------
    | SETTER METHODS.
    ----------------------------------------------------------------------*/
    /** Returns a reference to the element at position \c idx in the complex
     * number.
     */
    TT& operator[](std::size_t const& idx);

    /*----------------------------------------------------------------------
    | COMPARISON METHODS.
    ----------------------------------------------------------------------*/
    /** Compares elementwise if this complex number is smaller
     *  than a given rhs complex number. */
    bool operator<(ScaFES::Complex<TT> const& rhs) const;

    /** Compares elementwise if this complex number is greater or equal
     *  than a a given rhs complex number. */
    bool operator>=(ScaFES::Complex<TT> const& rhs) const;

    /** Compares elementwise if this complex number is smaller or equal
     *  than a a given rhs complex number. */
    bool operator<=(ScaFES::Complex<TT> const& rhs) const;

    /** Compares elementwise if this complex number is greater than
     *  a given rhs complex number. */
    bool operator>(ScaFES::Complex<TT> const& rhs) const;

    /** Compares elementwise if this complex number is equal to a given rhs
     *  complex number. */
    bool operator==(ScaFES::Complex<TT> const& rhs) const;

    /** Compares elementwise if this complex number is unequal to
     * a given rhs complex number. */
    bool operator!=(ScaFES::Complex<TT> const& rhs) const;

    /*----------------------------------------------------------------------
    | WORK METHODS
    ----------------------------------------------------------------------*/
    /** Computes the dimension of the complex number,
     * i.e., all elements will be multiplied.
     */
    TT size() const;

    /** Returns the absolute value. */
    double fabs() const;

#ifdef SCAFES_HAVE_BOOST_SERIALIZATION
    /** Serializes this class. */
    template <class Archive>
    void serialize(Archive& ar, unsigned int const version);
#endif

    /*----------------------------------------------------------------------
    | ARITHMETIC METHODS.
    ----------------------------------------------------------------------*/
    /** Creates an assignment operator.
     * Only the REAL part will be assigned to the given scalar.
     */
    ScaFES::Complex<TT>& operator=(TT const& re);

    /** Computes the additive inverse of this complex number. */
    ScaFES::Complex<TT>& operator-();

    /** Adds this complex number to a given scalar (elementwise). */
    ScaFES::Complex<TT>& operator+=(TT const& sca);

    /** Subtracts this complex number from a given scalar (elementwise). */
    ScaFES::Complex<TT>& operator-=(TT const& sca);

    /** Multiplies this complex number by a given scalar (elementwise). */
    ScaFES::Complex<TT>& operator*=(TT const& sca);

    /** Divides this complex number by a given scalar (elementwise). */
    ScaFES::Complex<TT>& operator/=(TT const& sca);

    /** Adds a given rhs complex number to this complex number. */
    ScaFES::Complex<TT>& operator+=(ScaFES::Complex<TT> const& rhs);

    /** Subtracts a given rhs complex number from this complex number. */
    ScaFES::Complex<TT>& operator-=(ScaFES::Complex<TT> const& rhs);

    /** Multiplies this complex number by a given rhs complex number. */
    ScaFES::Complex<TT>& operator*=(ScaFES::Complex<TT> const& rhs);

    /** Divides this complex number by a given rhs complex number. */
    ScaFES::Complex<TT>& operator/=(ScaFES::Complex<TT> const& rhs);

    /*----------------------------------------------------------------------
    | FREE METHODS WHICH ARE FRIENDS OF THIS CLASS.
    ----------------------------------------------------------------------*/
    /** Prints a complex number in the way: '(re, im})'. */
    template <typename RR>
    friend std::ostream& operator<<(std::ostream& output,
                                    ScaFES::Complex<RR> const& t);

    /** Method to swap two complex numbers. */
    template <typename RR>
    friend void swap(ScaFES::Complex<RR>& first, ScaFES::Complex<RR>& second);

private:
    /*----------------------------------------------------------------------
    | MEMBER VARIABLES.
    ----------------------------------------------------------------------*/
    /** Real part of complex number. */
    TT mReal;

    /** Imaginary part of complex number. */
    TT mImag;
}; // End of class //

/*******************************************************************************
 * FREE METHODS.
 ******************************************************************************/
/** Swaps two objects of the class \c Complex. */
template <typename TT>
void swap(ScaFES::Complex<TT>& first, ScaFES::Complex<TT>& second);
/*----------------------------------------------------------------------------*/
/** Preprares writing a complex number to output. */
template <typename TT>
std::ostream& operator<<(std::ostream& output, ScaFES::Complex<TT> const& rhs);

/*******************************************************************************
 * LIFE CYCLE METHODS.
 ******************************************************************************/
template <typename TT>
inline Complex<TT>::Complex()
: mReal(static_cast<TT>(0)), mImag(static_cast<TT>(0))
{
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline Complex<TT>::Complex(TT const& re, TT const& im)
: mReal(re), mImag(im)
{
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline Complex<TT>::Complex(TT const& re)
: mReal(re), mImag(static_cast<TT>(0))
{
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline ScaFES::Complex<TT>& Complex<TT>::operator=(ScaFES::Complex<TT> rhs)
{
    swap(*this, rhs);
    return *this;
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline Complex<TT>::Complex(ScaFES::Complex<TT> const& rhs)
: mReal(rhs.real()), mImag(rhs.imag())
{
}
/*----------------------------------------------------------------------------*/
template <typename TT> inline Complex<TT>::~Complex()
{
}

/*******************************************************************************
 * GETTER METHODS.
 ******************************************************************************/
template <typename TT> inline const TT& Complex<TT>::real() const
{
    return this->mReal;
}
/*----------------------------------------------------------------------------*/
template <typename TT> inline const TT& Complex<TT>::imag() const
{
    return this->mImag;
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline const TT& Complex<TT>::operator[](std::size_t const& position) const
{
    if (0 == position)
    {
        return this->mReal;
    }
    else
    {
        return this->mImag;
    }
}

/*******************************************************************************
 * SETTER METHODS.
 ******************************************************************************/
template <typename TT>
inline TT& Complex<TT>::operator[](std::size_t const& pos)
{
    return const_cast<TT&>(static_cast<const ScaFES::Complex<TT>&>(*this)[pos]);
}

/*******************************************************************************
 * WORK METHODS.
 ******************************************************************************/
template <typename TT> inline TT Complex<TT>::size() const
{
    return (this->mReal * this->mImag);
}
/*----------------------------------------------------------------------------*/
template <typename TT> inline double Complex<TT>::fabs() const
{
    return (this->mReal * this->mReal + this->mImag * this->mImag);
}
#ifdef SCAFES_HAVE_BOOST_SERIALIZATION
/*----------------------------------------------------------------------------*/
template <typename TT>
template <class Archive>
inline void Complex<TT>::serialize(Archive& ar, unsigned int const version)
{
    if (1 <= version)
    {
        ar&(this->mReal);
        ar&(this->mImag);
    }
}
#endif
/*******************************************************************************
 * ARITHMETIC METHODS.
 ******************************************************************************/
template <typename TT> inline ScaFES::Complex<TT>& Complex<TT>::operator-()
{
    this->mReal *= static_cast<TT>(-1);
    this->mImag *= static_cast<TT>(-1);
    return *this;
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline ScaFES::Complex<TT>& Complex<TT>::operator=(TT const& sca)
{
    this->mReal = sca;
    this->mImag = sca;
    return *this;
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline ScaFES::Complex<TT>& Complex<TT>::operator+=(TT const& sca)
{
    this->mReal += sca;
    this->mImag += sca;
    return *this;
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline ScaFES::Complex<TT>& Complex<TT>::operator-=(TT const& sca)
{
    this->mReal -= sca;
    this->mImag -= sca;
    return *this;
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline ScaFES::Complex<TT>& Complex<TT>::operator*=(TT const& sca)
{
    this->mReal *= sca;
    this->mImag *= sca;
    return *this;
}
/*----------------------------------------------------------------------------*/
// TODO: Throw an exception if the given scalar is near to zero.
template <typename TT>
inline ScaFES::Complex<TT>& Complex<TT>::operator/=(TT const& sca)
{
    this->mReal /= sca;
    this->mImag /= sca;
    return *this;
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline ScaFES::Complex<TT>& Complex<TT>::
operator+=(ScaFES::Complex<TT> const& rhs)
{
    this->mReal += rhs.real();
    this->mImag += rhs.imag();
    return *this;
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline ScaFES::Complex<TT>& Complex<TT>::
operator-=(ScaFES::Complex<TT> const& rhs)
{
    this->mReal -= rhs.real();
    this->mImag -= rhs.imag();
    return *this;
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline ScaFES::Complex<TT>& Complex<TT>::
operator*=(ScaFES::Complex<TT> const& rhs)
{
    const TT tmpRe = this->mReal * rhs.real() - this->mImag * rhs.imag();
    this->mReal = tmpRe;
    const TT tmpIm = this->mReal * rhs.imag() + this->mImag * rhs.real();
    this->mImag = tmpIm;
    return *this;
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline ScaFES::Complex<TT>& Complex<TT>::
operator/=(ScaFES::Complex<TT> const& rhs)
{
    const TT denom = rhs.imag() * rhs.imag() + rhs.real() * rhs.real();
    if (::fabs(denom) < 2.2e-12)
    {
        throw std::runtime_error("Denominator is very near to zero.");
    }
    const TT tmpRe =
        (this->mReal * rhs.real() + this->mImag * rhs.imag()) / denom;
    this->mReal = tmpRe;
    const TT tmpIm =
        (this->mImag * rhs.real() - this->mReal * rhs.imag()) / denom;
    this->mImag = tmpIm;
    return *this;
}

/*******************************************************************************
 * COMPARISON METHODS.
 ******************************************************************************/
template <typename TT>
inline bool Complex<TT>::operator==(ScaFES::Complex<TT> const& rhs) const
{
    bool res = true;

    if (!(::fabs(this->mReal - rhs.real()) < 2.2e-15))
    {
        res = false;
    }

    if (!(::fabs(this->mImag - rhs.imag()) < 2.2e-15))
    {
        res = false;
    }

    return res;
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline bool Complex<TT>::operator!=(ScaFES::Complex<TT> const& rhs) const
{
    return !(*this == rhs);
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline bool Complex<TT>::operator<(ScaFES::Complex<TT> const& rhs) const
{
    bool res = true;

    if (!(this->mReal < rhs.real()))
    {
        res = false;
    }

    if (!(this->mImag < rhs.imag()))
    {
        res = false;
    }

    return res;
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline bool Complex<TT>::operator<=(ScaFES::Complex<TT> const& rhs) const
{
    bool res = true;

    if ((this->mReal > rhs.real()))
    {
        res = false;
    }

    if ((this->mImag > rhs.imag()))
    {
        res = false;
    }

    return res;
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline bool Complex<TT>::operator>=(ScaFES::Complex<TT> const& rhs) const
{
    bool res = true;

    if ((this->mReal < rhs.real()))
    {
        res = false;
    }

    if ((this->mImag < rhs.imag()))
    {
        res = false;
    }

    return res;
    // return !(*this < rhs); Does not work because we are working elementwise!
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline bool Complex<TT>::operator>(ScaFES::Complex<TT> const& rhs) const
{
    bool res = true;

    if (!(mReal > rhs.real()))
    {
        res = false;
    }

    if (!(this->mImag > rhs.imag()))
    {
        res = false;
    }

    return res;
    // return !(*this <= rhs); Does not work because we are working elementwise!
}

/*******************************************************************************
 * FREE METHODS WHICH ARE FRIENDS OF THIS CLASS.
 ******************************************************************************/
template <typename TT>
inline void swap(ScaFES::Complex<TT>& first, ScaFES::Complex<TT>& second)
{
    std::swap(first.mReal, second.mReal);
    std::swap(first.mImag, second.mImag);
}
/*----------------------------------------------------------------------------*/
template <typename TT>
inline std::ostream& operator<<(std::ostream& output,
                                ScaFES::Complex<TT> const& rhs)
{
    const TT EPSILON = static_cast<TT>(2.2e-12);

    if (fabs(rhs.real()) < EPSILON)
    {
        output << "[    0,";
    }
    else
    {
        output << "[" << ::std::setw(4) << ::std::right << rhs.real() << ";";
    }

    if (fabs(rhs.imag()) < EPSILON)
    {
        output << "    0i]";
    }
    else
    {
        output << ::std::setw(4) << ::std::right << rhs.imag() << "i]";
    }

    return output;
}

} // End of namespace. //

/*******************************************************************************
 ******************************************************************************/
#ifdef SCAFES_HAVE_BOOST_SERIALIZATION
namespace boost
{
namespace serialization
{
    /** Designed to set the boost serialization version of a class template. */
    template <typename TT> struct version<ScaFES::Complex<TT>>
    {
        /** Sets the version number for serialization. */
        BOOST_STATIC_CONSTANT(unsigned long int, value = 2);
    };
}
}
#endif

#endif


#include <Eigen/Dense>
#include <iostream>

using namespace std;
using namespace Eigen;

int main() {
  Array22f m;
  m << 1, 2,
      3, 4;
  Array44f a = Array44f::Constant(0.6);
  cout << "Here is the array a:" << endl << a << endl << endl;
  a.block<2, 2>(1, 1) = m;
  cout << "Here is now a with m copied into its central 2x2 block:" << endl << a << endl << endl;
  a.block(0, 0, 2, 3) = a.block(2, 1, 2, 3);
  cout << "Here is now a with bottom-right 2x3 block copied into top-left 2x2 block:" << endl << a << endl << endl;
}


#define NDEBUG
#include "ETL.h"

#include <iostream>
#include <string>
#include <Eigen/Dense>
#include <boost/algorithm/string.hpp>

#include <vector>

int main(int argc, char *argv[]) {
	ETL etl(argv[1], argv[2], argv[3]);

	vector<vector<string>> dataset = etl.readCSV();
	int rows = dataset.size();
	int cols = dataset[0].size();

	Eigen::MatrixXd dataMat = etl.CSVtoEigen(dataset,rows,cols);

	cout << dataMat << endl;
	return EXIT_SUCCESS;
}


// OptionData.hpp
//
// Encapsulate option data.
//
// (C) Datasim Education BV 2008-2016

#ifndef OptionData_HPP
#define OptionData_HPP
#include <iostream>
#include <algorithm> // for max()
#include <boost/parameter.hpp>

namespace OptionParams
{

		BOOST_PARAMETER_KEYWORD(Tag, strike)
		BOOST_PARAMETER_KEYWORD(Tag, expiration)
		BOOST_PARAMETER_KEYWORD(Tag, interestRate)
		BOOST_PARAMETER_KEYWORD(Tag, volatility)
		BOOST_PARAMETER_KEYWORD(Tag, dividend)
		BOOST_PARAMETER_KEYWORD(Tag, optionType)
		BOOST_PARAMETER_KEYWORD(Tag, Stockprice)
		BOOST_PARAMETER_KEYWORD(Tag, Beta)
		BOOST_PARAMETER_KEYWORD(Tag, Barrier)
		BOOST_PARAMETER_KEYWORD(Tag, Rebate)
}

// Encapsulate all data in one place
struct OptionData
{ // Option data + behaviour
	double S; //Stockprice
	double K;	//Strike price
	double T;	//Time to expiration
	double r;	//interest rate
	double sig;	//Volatility
	double l;
	// Extra data
	double D;		// dividend
	double beta;	//Beta
	double rebate;	//Rebate
	int type;		// 1 == call, -1 == put
	OptionData() : S(0.0), K(0.0), T(0.0), r(0.0), sig(0.0), D(0.0), beta(0.0), l(0.0), rebate(0.0), type(-1)
	{
	}
	explicit constexpr OptionData(double strike, double expiration, double interestRate,
		double volatility, double dividend, double b, double StockPrice, double barrier, double rebate, int PC)
		: K(strike), T(expiration), r(interestRate), sig(volatility), S(StockPrice), D(dividend), beta(b), l(barrier), rebate(rebate), type(PC)
	{}
	//The best way to instantiate the Optiondat class. Use Named parameter idiom to instantiate the optiondata
	template <typename ArgPack> OptionData(const ArgPack& args)
	{
		K = args[OptionParams::strike];
		T = args[OptionParams::expiration];
		r = args[OptionParams::interestRate];
		sig = args[OptionParams::volatility];
		D = args[OptionParams::dividend];
		type = args[OptionParams::optionType];
		beta = args[OptionParams::Beta];
		S = args[OptionParams::Stockprice];
		l = args[OptionParams::Barrier];
		rebate = args[OptionParams::Rebate];
	}
};

#endif


// solving A * X = B
// A symmetric in packed format
// sytrf() & sytrs()

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/lapack/computational/sptrf.hpp>
#include <boost/numeric/bindings/lapack/computational/sptri.hpp>
#include <boost/numeric/bindings/lapack/computational/sptrs.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/symmetric.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cin;
using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

typedef ublas::matrix<real_t, ublas::column_major> m_t;
typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;

typedef
  ublas::symmetric_matrix<real_t, ublas::lower, ublas::column_major> symml_t;
typedef
  ublas::symmetric_matrix<cmplx_t, ublas::lower, ublas::column_major> csymml_t;

typedef
  ublas::symmetric_matrix<real_t, ublas::upper, ublas::column_major> symmu_t;
typedef
  ublas::symmetric_matrix<cmplx_t, ublas::upper, ublas::column_major> csymmu_t;

template <typename M>
void init_symm2 (M& m) {
  for (int i = 0; i < m.size1(); ++i)
    for (int j = i; j < m.size1(); ++j)
      m (i, j) = m (j, i) = 1 + j - i;
}

int main (int argc, char **argv) {
  size_t n = 0;
  if (argc > 1) {
    n = atoi(argv [1]);
  }

  cout << endl;

  cout << "real symmetric\n" << endl;

  if (n <= 0) {
    cout << "n -> ";
    cin >> n;
  }
  if (n < 5) n = 5;
  cout << "min n = 5" << endl << endl;
  size_t nrhs = 2;
  symml_t sal (n, n);   // symmetric matrix
  symmu_t sau (n, n);   // symmetric matrix
  m_t x (n, nrhs);
  m_t bl (n, nrhs), bu (n, nrhs);  // RHS matrices

  init_symm2 (sal);
  print_m (sal, "sal");
  cout << endl;

  init_symm2 (sau);
  print_m (sau, "sau");
  cout << endl;

  for (int i = 0; i < x.size1(); ++i) {
    x (i, 0) = 1.;
    x (i, 1) = 2.;
  }
  bl = prod (sal, x);
  bu = prod (sau, x);

  print_m (bl, "bl");
  cout << endl;
  print_m (bu, "bu");
  cout << endl;

  std::vector<fortran_int_t> ipiv (n);

  int err = lapack::sptrf (sal, ipiv);
  if (err == 0) {
    symml_t isal (sal);
    lapack::sptrs (sal, ipiv, bl);
    print_m (bl, "xl");
    lapack::sptri (isal, ipiv);
    print_m (isal, "isal");
  }
  cout << endl;

  err = lapack::sptrf (sau, ipiv);
  if (err == 0) {
    symmu_t isau (sau);
    lapack::sptrs (sau, ipiv, bu);
    print_m (bu, "xu");
    lapack::sptri (isau, ipiv);
    print_m (isau, "isau");
  }
  else
    cout << "?" << endl;
  cout << endl;

  //////////////////////////////////////////////////////////
  cout << "\n==========================================\n" << endl;
  cout << "complex symmetric\n" << endl;

  csymml_t scal (n, n);   // symmetric matrix
  csymmu_t scau (n, n);   // symmetric matrix
  cm_t cx (n, 1);
  cm_t cbl (n, 1), cbu (n, 1);  // RHS

  init_symm2 (scal);
  init_symm2 (scau);
  scal *= cmplx_t (0.1, 0.25);
  scau *= cmplx_t (-1, -0.5);

  print_m (scal, "scal");
  cout << endl;
  print_m (scau, "scau");
  cout << endl;

  for (int i = 0; i < cx.size1(); ++i)
    cx (i, 0) = cmplx_t (1, -1);
  print_m (cx, "cx");
  cout << endl;
  cbl = prod (scal, cx);
  cbu = prod (scau, cx);
  print_m (cbl, "cbl");
  cout << endl;
  print_m (cbu, "cbu");
  cout << endl;

  int ierr = lapack::sptrf (scal, ipiv);
  if (ierr == 0) {
    csymml_t iscal (scal);
    lapack::sptrs (scal, ipiv, cbl);
    print_m (cbl, "cxl");
    lapack::sptri (iscal, ipiv);
    print_m (iscal, "iscal");
  }
  else
    cout << "?" << endl;
  cout << endl;

  ierr = lapack::sptrf (scau, ipiv);
  if (ierr == 0) {
    csymmu_t iscau (scau);
    lapack::sptrs (scau, ipiv, cbu);
    print_m (cbu, "cxu");
    lapack::sptri (iscau, ipiv);
    print_m (iscau, "iscau");
  }
  else
    cout << "?" << endl;
  cout << endl;

}


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

int main(int, char**)
{
    using namespace mtl;

    double                array[][5]= {{1., 2., 3., 4., 5.}, {4., 5., 6., 7., 8.},
				       {7., 8., 9., 8., 7.}, {6., 5., 4., 3., 2.}};
    dense2D<double>       A(array), B;
    compressed2D<double>  T;

    B= bands(A, 1, 3);
    std::cout << "\nbands(A, 1, 3) = \n" << B;

    T= bands(A, -1, 2);
    std::cout << "\ntri_diagonal(A):= bands(A, -1, 2) = \n" << T;

    return 0;
}


/* test_non_central_chi_squared.cpp
 *
 * Copyright Steven Watanabe 2011
 * Copyright Thijs van den Berg 2014
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/non_central_chi_squared_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/math/distributions/non_central_chi_squared.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::non_central_chi_squared_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME non_central_chi_squared
#define BOOST_MATH_DISTRIBUTION boost::math::non_central_chi_squared
#define BOOST_RANDOM_ARG1_TYPE double
#define BOOST_RANDOM_ARG1_NAME k
#define BOOST_RANDOM_ARG1_DEFAULT 1000.0
#define BOOST_RANDOM_ARG1_DISTRIBUTION(k) boost::uniform_real<>(0.00001, k)
#define BOOST_RANDOM_ARG2_TYPE double
#define BOOST_RANDOM_ARG2_NAME lambda
#define BOOST_RANDOM_ARG2_DEFAULT 1000.0
#define BOOST_RANDOM_ARG2_DISTRIBUTION(lambda) boost::uniform_real<>(0.00001, lambda)

#include "test_real_distribution.ipp"


#include <iostream>
#include <vector>
#include <map>
#include <numeric>

#include <Eigen/Dense>

using HouseProperties = std::map<std::string, double>;

struct House
{
    double price;

    HouseProperties properties;
};

struct Model
{
    double constant;
    std::map<std::string, double> coefs;

    Model() : constant(0.), coefs() {}
};

int main()
{
    std::cout << "Linear regression example\n";

    //selling price of houses
    //from: http://people.sc.fsu.edu/~jburkardt/datasets/regression/x26.txt
    std::vector<House> houses_data_set {
        { 25.9,{ { "bathroom",1.0 },{ "area", 3.4720 },{ "livingArea", 0.998 },{ "garage", 1.0 },{ "room",  7. },{ "bedroom", 4. },{ "age", 42. } } },
        { 29.5,{ { "bathroom",1.0 },{ "area", 3.5310 },{ "livingArea", 1.500 },{ "garage", 2.0 },{ "room",  7. },{ "bedroom", 4. },{ "age", 62. } } },
        { 27.9,{ { "bathroom",1.0 },{ "area", 2.2750 },{ "livingArea", 1.175 },{ "garage", 1.0 },{ "room",  6. },{ "bedroom", 3. },{ "age", 40. } } },
        { 25.9,{ { "bathroom",1.0 },{ "area", 4.0500 },{ "livingArea", 1.232 },{ "garage", 1.0 },{ "room",  6. },{ "bedroom", 3. },{ "age", 54. } } },
        { 29.9,{ { "bathroom",1.0 },{ "area", 4.4550 },{ "livingArea", 1.121 },{ "garage", 1.0 },{ "room",  6. },{ "bedroom", 3. },{ "age", 42. } } },
        { 29.9,{ { "bathroom",1.0 },{ "area", 4.4550 },{ "livingArea", 0.988 },{ "garage", 1.0 },{ "room",  6. },{ "bedroom", 3. },{ "age", 56. } } },
        { 30.9,{ { "bathroom",1.0 },{ "area", 5.8500 },{ "livingArea", 1.240 },{ "garage", 1.0 },{ "room",  7. },{ "bedroom", 3. },{ "age", 51. } } },
        { 28.9,{ { "bathroom",1.0 },{ "area", 9.5200 },{ "livingArea", 1.501 },{ "garage", 0.0 },{ "room",  6. },{ "bedroom", 3. },{ "age", 32. } } },
        { 84.9,{ { "bathroom",2.5 },{ "area", 9.8000 },{ "livingArea", 3.420 },{ "garage", 2.0 },{ "room", 10. },{ "bedroom", 5. },{ "age", 42. } } },
        { 82.9,{ { "bathroom",2.5 },{ "area",12.8000 },{ "livingArea", 3.000 },{ "garage", 2.0 },{ "room",  9. },{ "bedroom", 5. },{ "age", 14. } } },
        { 35.9,{ { "bathroom",1.0 },{ "area", 6.4350 },{ "livingArea", 1.225 },{ "garage", 2.0 },{ "room",  6. },{ "bedroom", 3. },{ "age", 32. } } },
        { 31.5,{ { "bathroom",1.0 },{ "area", 4.9883 },{ "livingArea", 1.552 },{ "garage", 1.0 },{ "room",  6. },{ "bedroom", 3. },{ "age", 30. } } },
        { 31.0,{ { "bathroom",1.0 },{ "area", 5.5200 },{ "livingArea", 0.975 },{ "garage", 1.0 },{ "room",  5. },{ "bedroom", 2. },{ "age", 30. } } },
        { 30.9,{ { "bathroom",1.0 },{ "area", 6.6660 },{ "livingArea", 1.121 },{ "garage", 2.0 },{ "room",  6. },{ "bedroom", 3. },{ "age", 32. } } },
        { 30.0,{ { "bathroom",1.0 },{ "area", 5.0000 },{ "livingArea", 1.020 },{ "garage", 0.0 },{ "room",  5. },{ "bedroom", 2. },{ "age", 46. } } },
        { 28.9,{ { "bathroom",1.0 },{ "area", 9.5200 },{ "livingArea", 1.501 },{ "garage", 0.0 },{ "room",  6. },{ "bedroom", 3. },{ "age", 32. } } },
        { 36.9,{ { "bathroom",1.5 },{ "area", 5.1500 },{ "livingArea", 1.664 },{ "garage", 2.0 },{ "room",  8. },{ "bedroom", 4. },{ "age", 50. } } },
        { 41.9,{ { "bathroom",1.5 },{ "area", 6.9020 },{ "livingArea", 1.488 },{ "garage", 1.5 },{ "room",  7. },{ "bedroom", 3. },{ "age", 22. } } },
        { 40.5,{ { "bathroom",1.5 },{ "area", 7.1020 },{ "livingArea", 1.376 },{ "garage", 1.0 },{ "room",  6. },{ "bedroom", 3. },{ "age", 17. } } },
        { 43.9,{ { "bathroom",1.0 },{ "area", 7.8000 },{ "livingArea", 1.500 },{ "garage", 1.5 },{ "room",  7. },{ "bedroom", 3. },{ "age", 23. } } },
        { 37.5,{ { "bathroom",1.0 },{ "area", 5.5200 },{ "livingArea", 1.256 },{ "garage", 2.0 },{ "room",  6. },{ "bedroom", 3. },{ "age", 40. } } },
        { 37.9,{ { "bathroom",1.5 },{ "area", 4.0000 },{ "livingArea", 1.690 },{ "garage", 1.0 },{ "room",  6. },{ "bedroom", 3. },{ "age", 22. } } },
        { 44.5,{ { "bathroom",1.5 },{ "area", 9.8900 },{ "livingArea", 1.820 },{ "garage", 2.0 },{ "room",  8. },{ "bedroom", 4. },{ "age", 50. } } },
        { 37.9,{ { "bathroom",1.5 },{ "area", 6.7265 },{ "livingArea", 1.652 },{ "garage", 1.0 },{ "room",  6. },{ "bedroom", 3. },{ "age", 44. } } },
        { 38.9,{ { "bathroom",1.5 },{ "area", 9.1500 },{ "livingArea", 1.777 },{ "garage", 2.0 },{ "room",  8. },{ "bedroom", 4. },{ "age", 48. } } },
        { 36.9,{ { "bathroom",1.0 },{ "area", 8.0000 },{ "livingArea", 1.504 },{ "garage", 2.0 },{ "room",  7. },{ "bedroom", 3. },{ "age",  3. } } },
        { 45.8,{ { "bathroom",1.5 },{ "area", 7.3262 },{ "livingArea", 1.831 },{ "garage", 1.5 },{ "room",  8. },{ "bedroom", 4. },{ "age", 31. } } }
        //this observation is not added to the list
        //it will be used to test the linear model
        //{ 41.0,{ { "bathroom",1.5 },{ "area", 5.0000 },{ "livingArea", 1.200 },{ "garage", 2.0 },{ "room",  6. },{ "bedroom", 3. },{ "age", 30. } } }
    };

    std::cout << houses_data_set.size() << " observations\n";

    //function to evaluate the price of a house knowing its properties and using a linear model
    const auto evaluatePrice = [&](const Model& model, const HouseProperties& house)
    {
        double price = model.constant;

        for (const auto& c : model.coefs)
        {
            auto hIt = house.find(c.first);
            if (hIt != house.end())
            {
                price += c.second * hIt->second;
            }
        }
        return price;
    };

    //function to evaluate the error of a model based on the training data
    const auto evaluateError = [&houses_data_set, &evaluatePrice](const Model& model)
    {
        double error = 0.;
        for (const auto& d : houses_data_set)
        {
            const auto p = evaluatePrice(model, d.properties);
            error += std::pow(d.price - p, 2);
        }
        return error / static_cast<double>(houses_data_set.size());
    };

    //consider a model
    Model model; //the model is empty
    model.constant = 3.; //a constant is set
    model.coefs["room"] = 1.; //a coefficient is set for the "room" property
    std::cout << "Error: " << evaluateError(model) << '\n'; //the error of the model is printed

    //definition of the predictors
    std::vector<std::string> variables{ "bathroom", "area", "livingArea", "garage", "room", "bedroom", "age" };

    //definition of the design matrix
    Eigen::MatrixXd X(houses_data_set.size(), variables.size() + 1);
    //definition of the observation vector
    Eigen::VectorXd Y(houses_data_set.size());

    int i = 0;
    for (const auto& d : houses_data_set)
    {
        int j = 0;
        //the first line of the matrix is set to 1 to compute the constant term of the linear model
        X(i, j++) = 1.;

        for (const auto& var : variables)
        {
            auto pIt = d.properties.find(var);
            if (pIt != d.properties.end())
                X(i, j) = pIt->second;
            j++;
        }

        Y(i) = d.price;
        ++i;
    }

    //solving the normal equation
    const Eigen::VectorXd B = (X.transpose() * X).ldlt().solve(X.transpose() * Y);

    i = 0;

    //convert the computation result into the model data structure
    model.constant = B(i++);
    std::cout << "Model constant: " << model.constant << '\n';
    model.coefs.clear();
    for (const auto& var : variables)
    {
        model.coefs[var] = B(i++);
        std::cout << "Model (" << var << "): " << model.coefs[var] << '\n';
    }

    //compute the mean price of the observation data
    double meanPrice = std::accumulate(std::begin(houses_data_set), std::end(houses_data_set), 0.,
        [](double a, const House& h) {return a + h.price; }) / static_cast<double>(houses_data_set.size());

    std::cout << "Mean price: " << meanPrice << '\n';

    //compute the residual error of the model considering the observation data
    double ssres = evaluateError(model);
    std::cout << "Residual: " << ssres << '\n';

    //compute the total sum of squares (compared to the mean price)
    double sstot = std::accumulate(std::begin(houses_data_set), std::end(houses_data_set), 0.,
        [&meanPrice](double a, const House& h) {return a + std::pow(h.price - meanPrice, 2); }) / static_cast<double>(houses_data_set.size());
    std::cout << "Total sum of squares: " << sstot << '\n';

    //compute the coefficient of determination
    double rSquared = 1. - ssres / sstot;
    std::cout << "Coefficient of determination R^2: " << rSquared << '\n';

    //let's consider another observation, not used to train our model
    const House anotherObservation = { 41., { { "bathroom",1.5 },{ "area", 5.0000 },{ "livingArea", 1.200 },{ "garage", 2.0 },{ "room",  6. },{ "bedroom", 3. },{ "age", 30. } } };

    //estimate its price and compare to the real price
    double estimatedPrice = evaluatePrice(model, anotherObservation.properties);
    std::cout << "Estimate price of a house: " << estimatedPrice << ", compared to " << anotherObservation.price << '\n';

    return 0;
}


// Copyright (c) 2016 Till Kolditz
//
// 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.

/*
 * File:   TestModuloInverseComputation.cpp
 * Author: Till Kolditz <till.kolditz@gmail.com>
 *
 * Created on 20. Februar 2017, 18:21
 */

#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <sstream>

#include <boost/multiprecision/cpp_int.hpp>

#include <Util/AlignedBlock.hpp>
#include <Util/Euclidean.hpp>
#include <Util/Stopwatch.hpp>

int main(
        int argc,
        char ** argv) {
    if (argc != 7) {
        std::cerr << "Usage: " << argv[0] << " <size [Bytes]> <totalnum [#values]> <A16 <= 2^15> <A32 <= 2^31> <A64 <= 2^63> <A128 <= 2^127>" << std::endl;
        return 1;
    }

    typedef boost::multiprecision::uint128_t uint128_t;

    const uint16_t MASK16 = 0x7FFF;
    const uint32_t MASK32 = 0x7FFFFFFFul;
    const uint64_t MASK64 = 0x7FFFFFFFFFFFFFFFull;
    const uint128_t MASK128("0x7FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF");

    size_t SIZE = strtoll(argv[1], nullptr, 0);
    size_t TOTALNUM = strtoll(argv[2], nullptr, 0);
    uint16_t A16 = MASK16 & static_cast<uint16_t>(strtol(argv[3], nullptr, 0)); // test for code widths up to 15 bits
    uint32_t A32 = MASK32 & static_cast<uint32_t>(strtol(argv[4], nullptr, 0)); // test for code widths up to 31 bits
    uint64_t A64 = MASK64 & static_cast<uint64_t>(strtoll(argv[5], nullptr, 0)); // test for code widths up to 63 bits
    uint128_t A128(argv[6]); // test for code widths up to 127 bits

    const size_t NUM16 = SIZE / sizeof(uint16_t);
    const size_t NUM32 = SIZE / sizeof(uint32_t);
    const size_t NUM64 = SIZE / sizeof(uint64_t);
    const size_t NUM128 = SIZE / sizeof(uint128_t);
    TOTALNUM += (NUM16 - (TOTALNUM & (NUM16 - 1)));
    AlignedBlock data(SIZE, SIZE);

    std::cout << std::fixed << std::setprecision(1);

#ifdef DEBUG
    std::cout << TOTALNUM << " numbers." << std::endl;
#endif

#ifdef DEBUG
    std::cout << "16-bit: " << (TOTALNUM / NUM16) << " iterations, each on " << NUM16 << " uint16_t." << std::endl;
#endif
    auto data16 = data.template begin<uint16_t>();
    Stopwatch sw;
    for (size_t k = 0; k < (TOTALNUM / NUM16); k++) {
        for (size_t i = 0; i < NUM16; ++i) {
            data16[0] = ext_euclidean(A16, 15);
        }
    }
    auto nanoseconds = sw.Current();
#ifdef DEBUG
    std::cout << "Computing " << TOTALNUM << " inverses for 1..15-bit codewords took " << nanoseconds << " ns." << std::endl;
    std::cout << "\t" << (static_cast<double>(nanoseconds) / static_cast<double>(TOTALNUM)) << " ns / inverse." << std::endl;
    std::cout << "\t" << A16 << " * " << data16[0] << " = " << ((A16 * data16[0]) & MASK16) << std::endl;
#else
    std::cout << A16 << '\t' << nanoseconds << '\t' << (static_cast<double>(nanoseconds) / static_cast<double>(TOTALNUM));
#endif

#ifdef DEBUG
    std::cout << "32-bit: " << (TOTALNUM / NUM32) << " iterations, each on " << NUM32 << " uint32_t." << std::endl;
#endif
    auto data32 = data.template begin<uint32_t>();
    sw.Reset();
    for (size_t k = 0; k < (TOTALNUM / NUM32); k++) {
        for (size_t i = 0; i < NUM32; ++i) {
            data32[i] = ext_euclidean(A32, 31);
        }
    }
    nanoseconds = sw.Current();
#ifdef DEBUG
    std::cout << "Computing " << TOTALNUM << " inverses for 16..31-bit codewords took " << nanoseconds << " ns." << std::endl;
    std::cout << "\t " << (static_cast<double>(nanoseconds) / static_cast<double>(TOTALNUM)) << " ns / inverse." << std::endl;
    std::cout << "\t" << A32 << " * " << data32[0] << " = " << ((A32 * data32[0]) & MASK32) << std::endl;
#else
    std::cout << '\t' << A32 << '\t' << nanoseconds << '\t' << (static_cast<double>(nanoseconds) / static_cast<double>(TOTALNUM));
#endif

#ifdef DEBUG
    std::cout << "64-bit: " << (TOTALNUM / NUM64) << " iterations, each on " << NUM64 << " uint64_t." << std::endl;
#endif
    auto data64 = data.template begin<uint64_t>();
    sw.Reset();
    for (size_t k = 0; k < (TOTALNUM / NUM64); k++) {
        for (size_t i = 0; i < NUM64; ++i) {
            data64[i] = ext_euclidean(A64, 63);
        }
    }
    nanoseconds = sw.Current();
#ifdef DEBUG
    std::cout << "Computing " << TOTALNUM << " inverses for 32..63-bit codewords took " << nanoseconds << " ns." << std::endl;
    std::cout << "\t " << (static_cast<double>(nanoseconds) / static_cast<double>(TOTALNUM)) << " ns / inverse." << std::endl;
    std::cout << "\t" << A64 << " * " << data64[0] << " = " << ((A64 * data64[0]) & MASK64) << std::endl;
#else
    std::cout << '\t' << A64 << '\t' << nanoseconds << '\t' << (static_cast<double>(nanoseconds) / static_cast<double>(TOTALNUM));
#endif

#ifdef DEBUG
    std::cout << "128-bit: " << (TOTALNUM / NUM64) << " iterations, each on " << NUM128 << " uint128_t." << std::endl;
#endif
    auto data128 = data.template begin<uint128_t>();
    sw.Reset();
    for (size_t k = 0; k < (TOTALNUM / NUM128); k++) {
        for (size_t i = 0; i < NUM128; ++i) {
            data128[i] = ext_euclidean(A128, 127);
        }
    }
    nanoseconds = sw.Current();
#ifdef DEBUG
    std::cout << "Computing " << TOTALNUM << " inverses for 64..127-bit codewords took " << nanoseconds << " ns." << std::endl;
    std::cout << "\t " << (static_cast<double>(nanoseconds) / static_cast<double>(TOTALNUM)) << " ns / inverse." << std::endl;
    std::cout << "\t" << A128 << " * " << data128[0] << " = " << ((A128 * data128[0]) & MASK128) << std::endl;
#else
    std::cout << '\t' << A128 << '\t' << nanoseconds << '\t' << (static_cast<double>(nanoseconds) / static_cast<double>(TOTALNUM)) << std::endl;
#endif
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      121105    K. Kumar          File created from content in other files.
 *      121210    D. Dirkx          Added function implementations for class.
 *
 *    References
 *
 *    Notes
 *
 */

#include <cmath>
#include <stdexcept>

#include <boost/exception/all.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"
#include "Tudat/Astrodynamics/Gravitation/centralJ2J3J4GravityModel.h"

namespace tudat
{
namespace gravitation
{

//! Compute gravitational acceleration due to J4.
Eigen::Vector3d computeGravitationalAccelerationDueToJ4(
        const Eigen::Vector3d& positionOfBodySubjectToAcceleration,
        const double gravitationalParameterOfBodyExertingAcceleration,
        const double equatorialRadiusOfBodyExertingAcceleration,
        const double j4CoefficientOfGravityField,
        const Eigen::Vector3d& positionOfBodyExertingAcceleration )
{
    // Set constant values reused for optimal computation of acceleration components.
    const double distanceBetweenBodies = ( positionOfBodySubjectToAcceleration
                                           - positionOfBodyExertingAcceleration ).norm( );

    const double preMultiplier = gravitationalParameterOfBodyExertingAcceleration
            / std::pow( distanceBetweenBodies, 6.0 ) * 4.375 * j4CoefficientOfGravityField
            * std::pow( equatorialRadiusOfBodyExertingAcceleration, 4.0 );

    const double scaledZCoordinate = ( positionOfBodySubjectToAcceleration.z( )
                                       - positionOfBodyExertingAcceleration.z( ) )
            / distanceBetweenBodies;

    const double scaledZCoordinateSquared = scaledZCoordinate * scaledZCoordinate;

    const double scaledZCoordinateToPower4 = scaledZCoordinateSquared * scaledZCoordinateSquared;

    const double factorForXAndYDirections = ( 3.0 / 7.0 - 6.0 * scaledZCoordinateSquared
                                              + 9.0 * scaledZCoordinateToPower4 )
            / distanceBetweenBodies;

    // Compute components of acceleration due to J4-effect.
    Eigen::Vector3d gravitationalAccelerationDueToJ4 = Eigen::Vector3d::Constant( preMultiplier );

    gravitationalAccelerationDueToJ4( orbital_element_conversions::xCartesianPositionIndex )
            *= ( positionOfBodySubjectToAcceleration.x( )
                 - positionOfBodyExertingAcceleration.x( ) ) * factorForXAndYDirections;

    gravitationalAccelerationDueToJ4( orbital_element_conversions::yCartesianPositionIndex )
            *= ( positionOfBodySubjectToAcceleration.y( )
                 - positionOfBodyExertingAcceleration.y( ) ) * factorForXAndYDirections;

    gravitationalAccelerationDueToJ4( orbital_element_conversions::zCartesianPositionIndex )
            *= ( 15.0 / 7.0 - 10.0 * scaledZCoordinateSquared + 9.0 * scaledZCoordinateToPower4 )
            * scaledZCoordinate;

    return gravitationalAccelerationDueToJ4;
}

//! Compute gravitational acceleration zonal sum.
Eigen::Vector3d computeGravitationalAccelerationZonalSum(
        const Eigen::Vector3d& positionOfBodySubjectToAcceleration,
        const double gravitationalParameterOfBodyExertingAcceleration,
        const double equatorialRadiusOfBodyExertingAcceleration,
        const std::map< int, double > zonalCoefficientsOfGravityField,
        const Eigen::Vector3d& positionOfBodyExertingAcceleration )
{
    // Check that only coefficients for the gravity field are given up to J2 (i.e., size of
    // vector is 3 at max), else throw an error.
    if ( zonalCoefficientsOfGravityField.size( ) > 3 )
    {
        boost::throw_exception(
                    boost::enable_error_info(
                        std::runtime_error(
                            "Currently, accelerations can only be computed up to J4." ) ) );
    }

    // Check if position of body subject to acceleration falls within the effective radius of
    // the central body. If so, throw an error.
    if ( ( positionOfBodySubjectToAcceleration - positionOfBodyExertingAcceleration ).norm( )
         < equatorialRadiusOfBodyExertingAcceleration )
    {
        boost::throw_exception(
                    boost::enable_error_info(
                        std::runtime_error(
                            "Position of body subject to acceleration is within effective radius."
                            ) ) );
    }

    // Set gravitational acceleration sum equal to central term contribution.
    Eigen::Vector3d gravitationalAccelerationSum
            = computeGravitationalAcceleration(
                positionOfBodySubjectToAcceleration,
                gravitationalParameterOfBodyExertingAcceleration,
                positionOfBodyExertingAcceleration );

    // Add contributions from zonal terms. The switch statement checks
    for ( std::map< int, double >::const_iterator mapIterator
          = zonalCoefficientsOfGravityField.begin( );
          mapIterator != zonalCoefficientsOfGravityField.end( ); mapIterator++ )
    {
        switch ( mapIterator->first )
        {
        case 2:

            gravitationalAccelerationSum += computeGravitationalAccelerationDueToJ2(
                        positionOfBodySubjectToAcceleration,
                        gravitationalParameterOfBodyExertingAcceleration,
                        equatorialRadiusOfBodyExertingAcceleration,
                        mapIterator->second,
                        positionOfBodyExertingAcceleration );

            break;

        case 3:

            gravitationalAccelerationSum += computeGravitationalAccelerationDueToJ3(
                        positionOfBodySubjectToAcceleration,
                        gravitationalParameterOfBodyExertingAcceleration,
                        equatorialRadiusOfBodyExertingAcceleration,
                        mapIterator->second,
                        positionOfBodyExertingAcceleration );

            break;

        case 4:

            gravitationalAccelerationSum += computeGravitationalAccelerationDueToJ4(
                        positionOfBodySubjectToAcceleration,
                        gravitationalParameterOfBodyExertingAcceleration,
                        equatorialRadiusOfBodyExertingAcceleration,
                        mapIterator->second,
                        positionOfBodyExertingAcceleration );

            break;

        default:

            boost::throw_exception(
                        boost::enable_error_info(
                            std::runtime_error(
                                "Degree must be 2, 3, or 4 in current implementation." ) ) );
        };
    }

    // Return total gravitational acceleration computed.
    return gravitationalAccelerationSum;
}

//! Get gravitational acceleration.
Eigen::Vector3d CentralJ2J3J4GravitationalAccelerationModel::getAcceleration( )
{
    // Sum and return constituent acceleration terms.
    return computeGravitationalAcceleration(
                this->positionOfBodySubjectToAcceleration,
                this->gravitationalParameter,
                this->positionOfBodyExertingAcceleration )
            + computeGravitationalAccelerationDueToJ2(
                this->positionOfBodySubjectToAcceleration,
                this->gravitationalParameter,
                this->equatorialRadius,
                this->j2GravityCoefficient,
                this->positionOfBodyExertingAcceleration )
            + computeGravitationalAccelerationDueToJ3(
                this->positionOfBodySubjectToAcceleration,
                this->gravitationalParameter,
                this->equatorialRadius,
                this->j3GravityCoefficient,
                this->positionOfBodyExertingAcceleration )
            + computeGravitationalAccelerationDueToJ4(
                this->positionOfBodySubjectToAcceleration,
                this->gravitationalParameter,
                this->equatorialRadius,
                this->j4GravityCoefficient,
                this->positionOfBodyExertingAcceleration );
}

} // namespace gravitation
} // namespace tudat


#include <gtest/gtest.h>
#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/mesh/test_utils/test_meshes.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <functional>

#include "../locallaplaceqfe.h"
#include "../qfeinterpolator.h"
#include "../qfeprovidertester.h"

namespace DebuggingFEM::test {

constexpr double Square(double x) { return x * x; }

struct TestPair {
  typedef Eigen::Matrix<double, 6, 1> Vector6d;

  TestPair(const Vector6d &a) {
    function = [a](Eigen::Vector2d x) {
      return a(0) * Square(x(0)) + a(1) * Square(x(1)) + a(2) * x(0) * x(1) +
             a(3) * x(0) + a(4) * x(1) + a(5);
    };
    // coefficients of quadratic polynomial |grad function|^2
    Vector6d b;
    b << 4.0 * Square(a(0)) + Square(a(2)), 4.0 * Square(a(1)) + Square(a(2)),
        4.0 * a(2) * (a(0) + a(1)), 2.0 * (2.0 * a(0) * a(3) + a(2) * a(4)),
        2.0 * (2.0 * a(1) * a(4) + a(2) * a(3)), Square(a(3)) + Square(a(4));
    // monomials integrated over [0, 1]^2
    Eigen::VectorXd integrated_monomials(6);
    integrated_monomials << 1.0 / 3.0, 1.0 / 3.0, 0.25, 0.5, 0.5, 1.0;
    // reference value for energy (H_1 seminorm)
    energy = integrated_monomials.dot(b);
  }

  std::function<double(Eigen::Vector2d x)> function;
  double energy;
};

TEST(DebuggingFEM, interpolateOntoQuadFE) {
  // reference
  Eigen::VectorXd a(6);
  a << 1.0, 4.0, 2.0, 3.0, 2.0, 1.0;
  TestPair test_pair(a);
  auto p = test_pair.function;
  double energy_ref = test_pair.energy;

  // to test
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3, 1.0 / 3.0);
  lf::assemble::UniformFEDofHandler dofh(mesh_p,
                                         {{lf::base::RefEl::kPoint(), 1},
                                          {lf::base::RefEl::kSegment(), 1},
                                          {lf::base::RefEl::kTria(), 0},
                                          {lf::base::RefEl::kQuad(), 1}});

  const lf::base::size_type N_dofs(dofh.NumDofs());
  lf::assemble::COOMatrix<double> mat(N_dofs, N_dofs);
  auto element_matrix_provider = DebuggingFEM::LocalLaplaceQFE2();
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, element_matrix_provider,
                                      mat);

  const Eigen::SparseMatrix<double> A(mat.makeSparse());
  Eigen::VectorXd eta = DebuggingFEM::interpolateOntoQuadFE(dofh, p);
  double energy = eta.dot(A * eta);

  EXPECT_NEAR(energy, energy_ref, 1.0e-8);
}

TEST(DebuggingFEM, QFEProviderTester) {
  // reference
  Eigen::VectorXd a(6);
  a << 1.0, 4.0, 2.0, 3.0, 2.0, 1.0;
  TestPair test_pair(a);
  auto p = test_pair.function;
  double energy_ref = test_pair.energy;

  // to test
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3, 1.0 / 3.0);
  lf::assemble::UniformFEDofHandler dofh(mesh_p,
                                         {{lf::base::RefEl::kPoint(), 1},
                                          {lf::base::RefEl::kSegment(), 1},
                                          {lf::base::RefEl::kTria(), 0},
                                          {lf::base::RefEl::kQuad(), 1}});

  auto element_matrix_provider = DebuggingFEM::LocalLaplaceQFE2();
  QFEProviderTester qfe_provider_tester(dofh, element_matrix_provider);
  double energy = qfe_provider_tester.energyOfInterpolant(p);

  EXPECT_NEAR(energy, energy_ref, 1.0e-8);
}

}  // namespace DebuggingFEM::test


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#define BOOST_TEST_MODULE icl::interval unit test
#include <libs/icl/test/disable_test_warnings.hpp>
#include <string>
#include <boost/mpl/list.hpp>
#include "../unit_test_unwarned.hpp"

// interval instance types
#include "../test_type_lists.hpp"
#include "../test_value_maker.hpp"
#include "../test_interval_laws.hpp"

#include <boost/icl/right_open_interval.hpp>
#include <boost/icl/left_open_interval.hpp>
#include <boost/icl/closed_interval.hpp>
#include <boost/icl/open_interval.hpp>

#include <boost/icl/discrete_interval.hpp>
#include <boost/icl/continuous_interval.hpp>

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

#include "../test_icl_interval_shared.hpp"
#include "../test_icl_interval.hpp"
#include "../test_icl_dynamic_interval.hpp"
#include "../test_icl_discrete_interval.hpp"
#include "../test_icl_continuous_interval.hpp"
#include "../test_icl_static_interval.hpp"

//==============================================================================
//= Traits
//==============================================================================
BOOST_AUTO_TEST_CASE
(fastest_icl_discrete_interval_traits)
{            discrete_interval_traits<discrete_type_1, discrete_interval<discrete_type_1> >(); }

//==============================================================================

//- sta.asy.{dis|con} ----------------------------------------------------------
BOOST_AUTO_TEST_CASE
(fastest_icl_right_open_interval_ctor_4_ordered_types)
{                       interval_ctor_4_ordered_types<right_open_interval<ordered_type_1> >(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_right_open_interval_4_ordered_types)
{          singelizable_interval_4_ordered_types<right_open_interval<discrete_type_1> >(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_right_open_interval_4_bicremental_types)
{          singelizable_interval_4_bicremental_types<right_open_interval<discrete_type_2> >(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_left_open_interval_ctor_4_ordered_types)
{                      interval_ctor_4_ordered_types<left_open_interval<ordered_type_2> >(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_left_open_interval_4_ordered_types_singelizable)
{         singelizable_interval_4_ordered_types<left_open_interval<signed_discrete_type_1> >(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_left_open_interval_4_bicremental_types)
{         singelizable_interval_4_bicremental_types<left_open_interval<discrete_type_4> >(); }

//- coverables -----------------------------------------------------------------
BOOST_AUTO_TEST_CASE
(fastest_cover_right_open_interval_4_bicremental_types)
{    coverable_asymmetric_interval_4_bicremental_types<right_open_interval<numeric_continuous_type_1> >(); }

BOOST_AUTO_TEST_CASE
(fastest_cover_left_open_interval_4_bicremental_types)
{    coverable_asymmetric_interval_4_bicremental_types<left_open_interval<numeric_continuous_type_3> >(); }

//- dyn.dis --------------------------------------------------------------------
BOOST_AUTO_TEST_CASE
(fastest_icl_discrete_interval_ctor_4_discrete_types_base)
{                     interval_ctor_4_ordered_types<discrete_interval<discrete_type_1> >(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_discrete_interval_ctor_4_discrete_types_dynamic)
{             dynamic_interval_ctor_4_ordered_types<discrete_interval<discrete_type_2> >(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_discrete_interval_4_ordered_types)
{        singelizable_interval_4_ordered_types<discrete_interval<discrete_type_3> >(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_discrete_interval_4_bicremental_types)
{        singelizable_interval_4_bicremental_types<discrete_interval<discrete_type_3> >(); }

//- dyn.con --------------------------------------------------------------------
BOOST_AUTO_TEST_CASE
(fastest_icl_continuous_interval_ctor_4_continuous_types_base)
{                       interval_ctor_4_ordered_types<continuous_interval<continuous_type_1> >(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_continuous_interval_ctor_4_continuous_types_dynamic)
{               dynamic_interval_ctor_4_ordered_types<continuous_interval<continuous_type_2> >(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_continuous_interval_4_continuous_types_singelizable)
{          singelizable_interval_4_ordered_types<continuous_interval<continuous_type_3> >(); }

//------------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE
(fastest_icl_distant_intervals_4_discrete_types)
{            distant_intervals_4_discrete_types<discrete_type_1, std::less>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_distant_intervals_4_numeric_continuous_types)
{            distant_intervals_4_numeric_continuous_types<numeric_continuous_type_1, std::less>(); }

//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE
(fastest_icl_dynamic_interval_bounds_4_bicremental_types)
{            dynamic_interval_bounds_4_bicremental_types<bicremental_type_2>(); }

//==============================================================================
//==============================================================================
BOOST_AUTO_TEST_CASE
(fastest_icl_interval_equal_4_integral_types)
{            interval_equal_4_integral_types<integral_type_2>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_less_4_integral_types)
{            interval_less_4_integral_types<integral_type_3>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_touches_4_bicremental_types)
{            interval_touches_4_bicremental_types<bicremental_type_1>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_touches_4_integral_types)
{            interval_touches_4_integral_types<integral_type_4>(); }

#ifndef BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_ctor_specific)
{            interval_ctor_specific(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_equal_4_bicremental_continuous_types)
{            interval_equal_4_bicremental_continuous_types<bicremental_continuous_type_1>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_infix_intersect_4_bicremental_types)
{            interval_infix_intersect_4_bicremental_types<bicremental_type_4>(); }

#else

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_infix_intersect_4_bicremental_types)
{            interval_infix_intersect_4_bicremental_types<discrete_type_2>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_subtract_4_bicremental_types)
{            interval_subtract_4_bicremental_types<bicremental_type_5>(); }

#endif // ndef BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_INVLOG_10_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_INVLOG_10_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Generates constant 1/log(10).

    @par Header <boost/simd/constant/invlog_10.hpp>

    @par Semantic:

    @code
    T r = Invlog_10<T>();
    @endcode

    is similar to:

    @code
    r =  T(0.4342944819032518276511289189166050822943970058036666);
    @endcode

**/
  template<typename T> T Invlog_10();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Generates constant 1/log(10).

      Generate the  constant invlog_10.

      @return The Invlog_10 constant for the proper type
    **/
    const boost::dispatch::functor<tag::invlog_10_> invlog_10 = {};
  }
} }
#endif

#include <boost/simd/constant/scalar/invlog_10.hpp>
#include <boost/simd/constant/simd/invlog_10.hpp>

#endif