//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//! OPENOR_INTERFACE_FILE(openOR_core)
//****************************************************************************
//!
//! @file
//! @ingroup openOR_core

#ifndef openOR_core_Math_ublasvector_hpp
#define openOR_core_Math_ublasvector_hpp

#if defined(_MSC_VER)
#  pragma warning(disable:4172)
#endif

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#if defined(_MSC_VER)
#  pragma warning(default:4172)
#endif

#include <openOR/Math/traits.hpp>
#include <openOR/Math/vectorconcept.hpp>
#include <openOR/Math/vector_types.hpp>

namespace openOR {
   namespace Math {

      //----------------------------------------------------------------------------
      // Adapting the corresponding traits
      //----------------------------------------------------------------------------

      template<typename VectorType>
      struct UBLASVectorAccess {
         typedef typename VectorType::reference AccessType;
         typedef typename VectorType::const_reference ConstAccessType;

         template <int I, typename PP> static
            typename if_const<PP, ConstAccessType, AccessType>::
         type get(PP& p) {
            assert(I < p.size());
            return p(I);
         }
      };

      template<typename Value, std::size_t d>
      struct VectorTraits<boost::numeric::ublas::bounded_vector<Value, d> > {
         typedef boost::numeric::ublas::bounded_vector<Value, d> VectorType;
         typedef boost::numeric::ublas::bounded_vector<typename ScalarTraits<Value>::RealType, d>  RealVectorType;
         typedef Value ValueType;
         typedef boost::numeric::ublas::bounded_vector<Value, 3> Vector3Type;
         typedef boost::numeric::ublas::bounded_vector<Value, 2> Vector2Type;
         typedef boost::mpl::int_<d> Dimension;
         typedef UBLASVectorAccess<VectorType> Access;
         typedef boost::mpl::true_ IsVector;

         static VectorType ZEROS;
      };

      template<typename Value, std::size_t d>
      typename VectorTraits< boost::numeric::ublas::bounded_vector<Value, d> >::VectorType
      VectorTraits< boost::numeric::ublas::bounded_vector<Value, d> >::ZEROS = boost::numeric::ublas::zero_vector<Value>(d);

      template<typename E>
      struct UBLASVectorExpressionTraits {
         typedef E VectorType;
         typedef typename E::value_type ValueType;
         typedef boost::numeric::ublas::bounded_vector<typename E::value_type, 3> Vector3Type;
         typedef boost::numeric::ublas::bounded_vector<typename E::value_type, 2> Vector2Type;
         typedef boost::mpl::int_<0> Dimension;
         typedef UBLASVectorAccess<E> Access;
         typedef boost::mpl::true_ IsVector;
      };

      template<class E>
      struct VectorTraits<boost::numeric::ublas::vector_expression<E> > : VectorTraits<E> {};

      template<class C>
      struct VectorTraits<boost::numeric::ublas::vector_container<C> > : UBLASVectorExpressionTraits<boost::numeric::ublas::vector_container<C> > {};

      template<class T, class A>
      struct VectorTraits<boost::numeric::ublas::vector<T, A> > : UBLASVectorExpressionTraits<boost::numeric::ublas::vector<T, A> > {};

      template<class E>
      struct VectorTraits<boost::numeric::ublas::vector_reference<E> > : UBLASVectorExpressionTraits<boost::numeric::ublas::vector_reference<E> > {};

      template<typename E, typename F>
      struct VectorTraits<boost::numeric::ublas::vector_unary<E, F> > : UBLASVectorExpressionTraits<boost::numeric::ublas::vector_unary<E, F> > {};

      template<typename E1, typename E2, typename F>
      struct VectorTraits<boost::numeric::ublas::vector_binary<E1, E2, F> > : UBLASVectorExpressionTraits<boost::numeric::ublas::vector_binary<E1, E2, F> > {};

      template<typename E1, typename E2, typename F>
      struct VectorTraits<boost::numeric::ublas::vector_binary_scalar1<E1, E2, F> > : UBLASVectorExpressionTraits<boost::numeric::ublas::vector_binary_scalar1<E1, E2, F> > {};

      template<typename E1, typename E2, typename F>
      struct VectorTraits<boost::numeric::ublas::vector_binary_scalar2<E1, E2, F> > : UBLASVectorExpressionTraits<boost::numeric::ublas::vector_binary_scalar2<E1, E2, F> > {};

      //----------------------------------------------------------------------------
      //! @brief
      //! @ingroup openOR_core
      //----------------------------------------------------------------------------
      template <class E>
         inline typename boost::numeric::ublas::vector_scalar_unary_traits<E, boost::numeric::ublas::vector_norm_2<E> >::
      result_type norm(const boost::numeric::ublas::vector_expression<E>& e) {
         return boost::numeric::ublas::norm_2(e);
      }

      //----------------------------------------------------------------------------
      //! @brief
      //! @ingroup openOR_core
      //----------------------------------------------------------------------------
      template <class E1, class E2>
         inline typename boost::numeric::ublas::vector_scalar_binary_traits < E1, E2,
                  boost::numeric::ublas::vector_inner_prod < E1, E2,
                  typename boost::numeric::ublas::promote_traits < typename E1::value_type,
                  typename E2::value_type >::promote_type > >::
      result_type dot(  const boost::numeric::ublas::vector_expression<E1> &e1,
                        const boost::numeric::ublas::vector_expression<E2> &e2)
      {
         return boost::numeric::ublas::inner_prod(e1, e2);
      }

   }
}

//namespace boost {
//   namespace numeric {
//      namespace ublas {
//
//         template<class E1, class E2>
//         BOOST_UBLAS_INLINE
//            typename vector_scalar_binary_traits < E1, E2, vector_inner_prod < E1, E2,
//                        typename promote_traits < typename E1::value_type,
//                        typename E2::value_type >::promote_type > >::
//         result_type operator% ( const vector_expression<E1> &e1,
//                                 const vector_expression<E2> &e2)
//         {
//            typedef typename vector_scalar_binary_traits < E1, E2, vector_inner_prod < E1, E2,
//                                 typename promote_traits < typename E1::value_type,
//                                 typename E2::value_type >::promote_type > >::expression_type expression_type;
//            return expression_type(e1(), e2());
//         }
//
//      }
//   }
//}

#endif


#pragma once
#include <Eigen/Dense>

namespace mtao::geometry::mesh {

    template <typename VDerived, typename CDerived>
        auto compactify(const Eigen::MatrixBase<VDerived>& V, const Eigen::MatrixBase<CDerived>& C) {

            constexpr static int VRows = VDerived::RowsAtCompileTime;
            using Scalar = typename VDerived::Scalar;
            using RetVerts = mtao::ColVectors<Scalar,VRows>;

            constexpr static int CRows = CDerived::RowsAtCompileTime;
            using Index = typename CDerived::Scalar;
            using RetCells = mtao::ColVectors<Index,CRows>;

            RetCells mC = C;
            std::map<int,int> reindexer;
            for(int i = 0; i < C.size(); ++i) {
                Index& v = mC(i);
                if(auto it = reindexer.find(v); it == reindexer.end()) {
                    size_t size = reindexer.size();
                    reindexer[v] = size;
                    v = size;
                } else {
                    v = it->second;
                }
            }
            RetVerts mV(V.rows(),reindexer.size());
            for(auto&& [a,b]: reindexer) {
                mV.col(b) = V.col(a);
            }

            return std::make_tuple(mV,mC);
        }
    template <typename VDerived, typename CDerived, typename Allocator>
        auto compactify(const Eigen::MatrixBase<VDerived>& V, const std::vector<CDerived, Allocator>& Cs) {

            constexpr static int VRows = VDerived::RowsAtCompileTime;
            using Scalar = typename VDerived::Scalar;
            using RetVerts = mtao::ColVectors<Scalar,VRows>;

            constexpr static int CRows = CDerived::RowsAtCompileTime;
            using Index = typename CDerived::Scalar;
            //using RetCells = mtao::ColVectors<Index,CRows>;

            std::vector<CDerived,Allocator> mCs = Cs;

            std::map<int,int> reindexer;
            for(auto&& mC: mCs) {
                for(int i = 0; i < mC.size(); ++i) {
                    Index& v = mC(i);
                    if(auto it = reindexer.find(v); it == reindexer.end()) {
                        size_t size = reindexer.size();
                        reindexer[v] = size;
                        v = size;
                    } else {
                        v = it->second;
                    }
                }
            }
            RetVerts mV(V.rows(),reindexer.size());
            for(auto&& [a,b]: reindexer) {
                mV.col(b) = V.col(a);
            }

            return std::make_tuple(mV,mCs);
        }
}


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

#include <sstream>

#include "layers/polllayer.h"
#include "core/functions.h"
#include "core/training.h"
#include "core/utils.h"

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

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

struct PollingLayerTestFixture
{
  PollingLayerTestFixture()
  {

  }
  ~PollingLayerTestFixture()
  {

  }

  void test_polling_op(
      const VectorBatch & input, const VectorBatch & expected,
      const MatrixSize & input_rows, const MatrixSize & input_cols,
      const MatrixSize & filter_size, enum PollingLayer::Mode mode)
  {
    VectorBatch output;
    output.resizeLike(expected);

    {
      // ensure we don't do allocations in eigen
      BlockAllocations block;
      PollingLayer::poll(input, input_rows, input_cols, filter_size, mode, output);
      BOOST_CHECK(expected.isApprox(output, TEST_TOLERANCE));
    }
  }

  void test_backprop_polling_op(
      const VectorBatch & input, const VectorBatch & input_expected, const VectorBatch & expected,
      const MatrixSize & input_rows, const MatrixSize & input_cols,
      const MatrixSize & filter_size, enum PollingLayer::Mode mode)
  {
    VectorBatch input2, gradient_input, output, gradient_output;
    input2.resizeLike(input);
    gradient_input.resizeLike(input);
    output.resizeLike(expected);
    gradient_output.resizeLike(expected);

    {
      // ensure we don't do allocations in eigen
      BlockAllocations block;
      PollingLayer::poll(input, input_rows, input_cols, filter_size, mode, output);
      gradient_output = output - expected;

      PollingLayer::poll_gradient_backprop(
          gradient_output, input, input_rows, input_cols,
          filter_size, mode, gradient_input);
      input2 = input - gradient_input;

      BOOST_CHECK(input_expected.isApprox(input2, TEST_TOLERANCE));
    }
  }
};
// struct PollingLayerTestFixture

BOOST_FIXTURE_TEST_SUITE(PollingLayerTest, PollingLayerTestFixture);

BOOST_AUTO_TEST_CASE(IO_Test)
{
  BOOST_TEST_MESSAGE("*** PollingLayer IO test ...");

  const MatrixSize input_rows = 3;
  const MatrixSize input_cols = 7;
  const MatrixSize filter_size = 2;
  PollingLayer one(input_cols, input_rows, filter_size, PollingLayer::PollMode_Max);
  one.init(Layer::InitMode_Random, boost::none);

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

  PollingLayer two(input_cols, input_rows, filter_size, PollingLayer::PollMode_Max);
  std::istringstream iss(oss.str());
  iss >> two;
  BOOST_CHECK(!iss.fail());
  BOOST_TEST_MESSAGE("PollingLayer after loading from file: " << "\n" << two);

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

//////////////////////////////////////////////////////////////////////
//
// MAX polling layer tests
//
BOOST_AUTO_TEST_CASE(PollOp_Max_Test)
{
  BOOST_TEST_MESSAGE("*** Polling Max operation test ...");

  const MatrixSize input_rows = 3;
  const MatrixSize input_cols = 6;
  const MatrixSize filter_size = 2;
  const MatrixSize input_size = input_cols * input_rows;
  const MatrixSize output_size = PollingLayer::get_output_size(input_cols, input_rows, filter_size);
  const MatrixSize batch_size = 2;

  VectorBatch input, expected;
  resize_batch(input, batch_size, input_size);
  resize_batch(expected, batch_size, output_size);

  input <<
      1, 2, 11, 10, 13, 18,
      3, 4, 12,  9, 14, 17,
      5, 6,  7,  8, 15, 16,
      /////////////////////
      2, 6,  8, 10, 13, 14,
      3, 2,  7,  9, 16, 15,
      4, 5, 13, 11, 16, 18;

  expected <<
      4, 12, 18,
      /////////
      6, 10, 16;

  test_polling_op(
      input, expected, input_rows, input_cols,
      filter_size, PollingLayer::PollMode_Max);
}

BOOST_AUTO_TEST_CASE(PollOp_Max_Backprop_Test)
{
  BOOST_TEST_MESSAGE("*** Polling Max Backprop operation test ...");

  const MatrixSize input_rows = 3;
  const MatrixSize input_cols = 6;
  const MatrixSize filter_size = 2;
  const MatrixSize input_size = input_cols * input_rows;
  const MatrixSize output_size = PollingLayer::get_output_size(input_cols, input_rows, filter_size);
  const MatrixSize batch_size = 2;

  VectorBatch input, input_expected, expected;
  resize_batch(input, batch_size, input_size);
  resize_batch(input_expected, batch_size, input_size);
  resize_batch(expected, batch_size, output_size);

  input <<
      1, 2, 11, 10, 13, 18,
      3, 4, 12,  9, 14, 17,
      5, 6,  7,  8, 15, 16,
      /////////////////////
      2, 6,  8, 10, 13, 14,
      3, 2,  7,  9, 16, 15,
      4, 5, 13, 11, 16, 18;

  expected <<
      14, 22,  8,  // +10, +10, -10,
      //////////
      16,  0, 26;  // +10, -10, +10,

  input_expected <<
      1,  2, 11, 10, 13,  8,
      3, 14, 22,  9, 14, 17,
      5,  6,  7,  8, 15, 16,
      /////////////////////
      2, 16,  8,  0, 13, 14,
      3,  2,  7,  9, 26, 15,
      4,  5, 13, 11, 16, 18;

  test_backprop_polling_op(
      input, input_expected, expected,
      input_rows, input_cols, filter_size, PollingLayer::PollMode_Max);
}

BOOST_AUTO_TEST_CASE(Max_FeedForward_Test)
{
  BOOST_TEST_MESSAGE("*** PollingLayer Max FeedForward test ...");

  const MatrixSize input_rows = 3;
  const MatrixSize input_cols = 6;
  const MatrixSize filter_size = 2;
  const MatrixSize input_size = input_cols * input_rows;
  const MatrixSize output_size = PollingLayer::get_output_size(input_cols, input_rows, filter_size);
  const MatrixSize batch_size = 2;
  const Value ww = 2;
  const Value bb = 0.5;

  VectorBatch input, expected_output;
  resize_batch(input, batch_size, input_size);
  resize_batch(expected_output, batch_size, output_size);

  input <<
      1, 2, 11, 10, 13, 18,
      3, 4, 12,  9, 14, 17,
      5, 6,  7,  8, 15, 16,
      /////////////////////
      2, 6,  8, 10, 13, 14,
      3, 2,  7,  9, 16, 15,
      4, 5, 13, 11, 16, 18;

  expected_output <<
      8.5,  24.5, 36.5,
      //////////////
      12.5, 20.5, 32.5;

  auto layer = make_unique<PollingLayer>(
      input_rows,
      input_cols,
      filter_size,
      PollingLayer::PollMode_Max);
  BOOST_CHECK(layer);
  layer->set_activation_function(make_unique<IdentityFunction>());
  layer->set_values(ww, bb);

  test_layer_feedforward(*layer, input, expected_output);
}

BOOST_AUTO_TEST_CASE(Max_Training_WithIdentity_Test)
{
  BOOST_TEST_MESSAGE("*** PollingLayer Max with Identity activation training test ...");

  const MatrixSize input_rows = 3;
  const MatrixSize input_cols = 6;
  const MatrixSize filter_size = 2;
  const MatrixSize input_size = input_cols * input_rows;
  const MatrixSize output_size = PollingLayer::get_output_size(input_cols, input_rows, filter_size);
  const MatrixSize batch_size = 2;
  const Value ww = 2;
  const Value bb = 0.1;

  VectorBatch input, expected;
  resize_batch(input, batch_size, input_size);
  resize_batch(expected, batch_size, output_size);

  input <<
      0.01, 0.02, 0.11, 0.10, 0.13, 0.18,
      0.03, 0.04, 0.12, 0.09, 0.14, 0.17,
      0.05, 0.06, 0.07, 0.08, 0.15, 0.16,
      /////////////////////
      0.02, 0.06, 0.08, 0.10, 0.13, 0.14,
      0.03, 0.02, 0.07, 0.09, 0.16, 0.15,
      0.04, 0.05, 0.13, 0.11, 0.16, 0.18;

  // we expect ww=3 and bb=0.5
  expected <<
      0.62, 0.86, 1.04,  // max=0.04, 0.12, 0.18,
      0.68, 0.80, 0.98;  // max=0.06, 0.10, 0.16;

  // create layer
  auto layer = make_unique<PollingLayer>(
      input_rows, input_cols,
      filter_size, PollingLayer::PollMode_Max);
  BOOST_CHECK(layer);
  layer->set_activation_function(make_unique<IdentityFunction>());
  layer->set_values(ww, bb);

  // test
  test_layer_training(
      *layer,
      input,
      expected,
      batch_size, // tests num
      make_unique<QuadraticCost>(),
      0.07,  // learning rate
      5000  // epochs
  );
}

BOOST_AUTO_TEST_CASE(Max_Training_WithSigmoid_Test)
{
  BOOST_TEST_MESSAGE("*** PollingLayer Max with Sigmoid activation training test ...");

  const MatrixSize input_rows = 3;
  const MatrixSize input_cols = 6;
  const MatrixSize filter_size = 2;
  const MatrixSize input_size = input_cols * input_rows;
  const MatrixSize output_size = PollingLayer::get_output_size(input_cols, input_rows, filter_size);
  const MatrixSize batch_size = 2;
  const Value ww = 2;
  const Value bb = 0.1;

  VectorBatch input, expected;
  resize_batch(input, batch_size, input_size);
  resize_batch(expected, batch_size, output_size);

  input <<
      0.01, 0.02, 0.11, 0.10, 0.13, 0.18,
      0.03, 0.04, 0.12, 0.09, 0.14, 0.17,
      0.05, 0.06, 0.07, 0.08, 0.15, 0.16,
      /////////////////////
      0.02, 0.06, 0.08, 0.10, 0.13, 0.14,
      0.03, 0.02, 0.07, 0.09, 0.16, 0.15,
      0.04, 0.05, 0.13, 0.11, 0.16, 0.18;

  // we expect ww=3 and bb=0.5
  expected <<
      0.65022, 0.70266, 0.73885,  // max=0.04, 0.12, 0.18,
      0.66374, 0.68997, 0.72711;  // max=0.06, 0.10, 0.16;

  // create layer
  auto layer = make_unique<PollingLayer>(
      input_rows, input_cols,
      filter_size, PollingLayer::PollMode_Max);
  BOOST_CHECK(layer);
  layer->set_activation_function(make_unique<SigmoidFunction>());
  layer->set_values(ww, bb);

  // test
  test_layer_training(
      *layer,
      input,
      expected,
      batch_size, // tests num
      make_unique<QuadraticCost>(),
      0.5,  // learning rate
      100000  // epochs
  );
}

//////////////////////////////////////////////////////////////////////
//
// AVG polling layer tests
//
BOOST_AUTO_TEST_CASE(PollOp_Avg_Test)
{
  BOOST_TEST_MESSAGE("*** Polling Avg operation test ...");

  const MatrixSize input_rows = 3;
  const MatrixSize input_cols = 6;
  const MatrixSize filter_size = 2;
  const MatrixSize input_size = input_cols * input_rows;
  const MatrixSize output_size = PollingLayer::get_output_size(input_cols, input_rows, filter_size);
  const MatrixSize batch_size = 2;

  VectorBatch input, expected;
  resize_batch(input, batch_size, input_size);
  resize_batch(expected, batch_size, output_size);

  input <<
      1, 2, 11, 10, 13, 18,
      3, 4, 12,  9, 14, 17,
      5, 6,  7,  8, 15, 16,
      /////////////////////
      2, 6,  8, 10, 13, 14,
      3, 2,  7,  9, 16, 15,
      4, 5, 13, 11, 16, 18;

  expected <<
      2.5, 10.5, 15.5,
      /////////////////
      3.25, 8.5, 14.5;

  test_polling_op(input, expected, input_rows, input_cols,
                  filter_size, PollingLayer::PollMode_Avg);
}

BOOST_AUTO_TEST_CASE(PollOp_Avg_Backprop_Test)
{
  BOOST_TEST_MESSAGE("*** Polling Avg Backprop operation test ...");

  const MatrixSize input_rows = 3;
  const MatrixSize input_cols = 6;
  const MatrixSize filter_size = 2;
  const MatrixSize input_size = input_cols * input_rows;
  const MatrixSize output_size = PollingLayer::get_output_size(input_cols, input_rows, filter_size);
  const MatrixSize batch_size = 2;

  VectorBatch input, input_expected, expected;
  resize_batch(input, batch_size, input_size);
  resize_batch(input_expected, batch_size, input_size);
  resize_batch(expected, batch_size, output_size);

  input <<
      1, 2, 11, 10, 13, 18,
      3, 4, 12,  9, 14, 17,
      5, 6,  7,  8, 15, 16,
      /////////////////////
      2, 6,  8, 10, 13, 14,
      3, 2,  7,  9, 16, 15,
      4, 5, 13, 11, 16, 18;

  expected <<
      3.5, 9.5, 13.5,     // +1, -1, -2
      /////////////////
      1.25, 8.5, 16.5;    // -2,  0, +2

  input_expected <<
      2, 3, 10,  9, 11, 16,
      4, 5, 11,  8, 12, 15,
      5, 6,  7,  8, 15, 16,
      /////////////////////
      0, 4,  8, 10, 15, 16,
      1, 0,  7,  9, 18, 17,
      4, 5, 13, 11, 16, 18;

  test_backprop_polling_op(
      input, input_expected, expected,
      input_rows, input_cols, filter_size, PollingLayer::PollMode_Avg);
}

BOOST_AUTO_TEST_CASE(Avg_FeedForward_Test)
{
  BOOST_TEST_MESSAGE("*** PollingLayer Avg FeedForward test ...");

  const MatrixSize input_rows = 3;
  const MatrixSize input_cols = 6;
  const MatrixSize filter_size = 2;
  const MatrixSize input_size = input_cols * input_rows;
  const MatrixSize output_size = PollingLayer::get_output_size(input_cols, input_rows, filter_size);
  const MatrixSize batch_size = 2;
  const Value ww = 2;
  const Value bb = 0.5;

  VectorBatch input, expected_output;
  resize_batch(input, batch_size, input_size);
  resize_batch(expected_output, batch_size, output_size);

  input <<
      1, 2, 11, 10, 13, 18,
      3, 4, 12,  9, 14, 17,
      5, 6,  7,  8, 15, 16,
      /////////////////////
      2, 6,  8, 10, 13, 14,
      3, 2,  7,  9, 16, 15,
      4, 5, 13, 11, 16, 18;

  expected_output <<
      5.5, 21.5, 31.5,
      /////////////////
      7.0, 17.5, 29.5;

  auto layer = make_unique<PollingLayer>(
      input_rows,
      input_cols,
      filter_size,
      PollingLayer::PollMode_Avg);
  BOOST_CHECK(layer);
  layer->set_activation_function(make_unique<IdentityFunction>());
  layer->set_values(ww, bb);

  test_layer_feedforward(*layer, input, expected_output);
}

BOOST_AUTO_TEST_CASE(Avg_Training_WithIdentity_Test)
{
  BOOST_TEST_MESSAGE("*** PollingLayer Avg with Identity activation training test ...");

  const MatrixSize input_rows = 3;
  const MatrixSize input_cols = 6;
  const MatrixSize filter_size = 2;
  const MatrixSize input_size = input_cols * input_rows;
  const MatrixSize output_size = PollingLayer::get_output_size(input_cols, input_rows, filter_size);
  const MatrixSize batch_size = 2;
  const Value ww = 2;
  const Value bb = 0.1;

  VectorBatch input, expected;
  resize_batch(input, batch_size, input_size);
  resize_batch(expected, batch_size, output_size);

  input <<
      0.01, 0.02, 0.11, 0.10, 0.13, 0.18,
      0.03, 0.04, 0.12, 0.09, 0.14, 0.17,
      0.05, 0.06, 0.07, 0.08, 0.15, 0.16,
      ///////////////////////////////////
      0.02, 0.06, 0.08, 0.10, 0.13, 0.14,
      0.03, 0.02, 0.07, 0.09, 0.16, 0.15,
      0.04, 0.05, 0.13, 0.11, 0.16, 0.18;

  expected <<
      0.2023, 0.1442, 0.1079,
      //////////////////////
      0.1968, 0.1587, 0.1152;

  // create layer
  auto layer = make_unique<PollingLayer>(
      input_rows, input_cols,
      filter_size, PollingLayer::PollMode_Avg);
  BOOST_CHECK(layer);
  layer->set_activation_function(make_unique<IdentityFunction>());
  layer->set_values(ww, bb);

  // test
  test_layer_training(
      *layer,
      input,
      expected,
      batch_size, // tests num
      make_unique<QuadraticCost>(),
      0.01,     // learning rate
      100000  // epochs
  );
}

BOOST_AUTO_TEST_CASE(Avg_Training_WithSigmoid_Test)
{
  BOOST_TEST_MESSAGE("*** PollingLayer Avg with Sigmoid activation training test ...");

  const MatrixSize input_rows = 3;
  const MatrixSize input_cols = 6;
  const MatrixSize filter_size = 2;
  const MatrixSize input_size = input_cols * input_rows;
  const MatrixSize output_size = PollingLayer::get_output_size(input_cols, input_rows, filter_size);
  const MatrixSize batch_size = 2;
  const Value ww = 2;
  const Value bb = 0.1;

  VectorBatch input, expected;
  resize_batch(input, batch_size, input_size);
  resize_batch(expected, batch_size, output_size);

  input <<
      0.01, 0.02, 0.11, 0.10, 0.13, 0.18,
      0.03, 0.04, 0.12, 0.09, 0.14, 0.17,
      0.05, 0.06, 0.07, 0.08, 0.15, 0.16,
      /////////////////////
      0.02, 0.06, 0.08, 0.10, 0.13, 0.14,
      0.03, 0.02, 0.07, 0.09, 0.16, 0.15,
      0.04, 0.05, 0.13, 0.11, 0.16, 0.18;

  expected <<
      0.2044, 0.1408, 0.1101,
      //////////////////////
      0.1976, 0.1550, 0.1157;

  // create layer
  auto layer = make_unique<PollingLayer>(
      input_rows, input_cols,
      filter_size, PollingLayer::PollMode_Avg);
  BOOST_CHECK(layer);
  layer->set_activation_function(make_unique<SigmoidFunction>());
  layer->set_values(ww, bb);

  // test
  test_layer_training(
      *layer,
      input,
      expected,
      batch_size, // tests num
      make_unique<CrossEntropyCost>(),
      1.0,  // learning rate
      10000  // epochs
  );
}

BOOST_AUTO_TEST_SUITE_END()


//  Copyright (c) 2006-2009, Bernhard Reiter
//
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

#include <boost/tree/algorithm.hpp>

#define BOOST_TEST_MODULE cursor_algorithm test
#include <boost/test/included/unit_test.hpp>
#include <boost/test/test_case_template.hpp>

#include "test_tree_traversal_data.hpp"
#include "fake_binary_tree.hpp"

using namespace boost::tree;

BOOST_FIXTURE_TEST_SUITE(cursor_algorithms_test, fake_binary_tree_fixture<int>)

BOOST_AUTO_TEST_CASE( test_leftmost )
{
    fake_root_tracking_binary_tree<int> frbt1(fbt1);
    fake_root_tracking_binary_tree<int>::cursor c = frbt1.root();
    to_leftmost(c);
    BOOST_CHECK(c.is_leaf());
    BOOST_CHECK(c == frbt1.root().begin().begin().begin());
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_successor, Order, orders )
{
    fake_root_tracking_binary_tree<int> frbt1(fbt1);
    fake_root_tracking_binary_tree<int>::cursor c = frbt1.root();
    fake_to_first(Order(), c);

    test_data_set mpo;
    mock_cursor_data(mpo);

    typedef typename test_data_set::index<Order>::type container_type;
    const container_type& order_index = mpo.get<Order>();

    typename container_type::const_iterator ci = order_index.begin();
    typename container_type::const_iterator cie = order_index.end();

    --cie;
    for (; ci!=cie; ++ci) {
        BOOST_CHECK_EQUAL(*c, ci->val);
        BOOST_CHECK_EQUAL(boost::tree::successor(Order(), c), true);
    }

    BOOST_CHECK_EQUAL(*c, ci->val);

    fake_root_tracking_binary_tree<int>::cursor d = frbt1.root();
    fake_to_last(Order(), d);
    BOOST_CHECK(c == d);

    BOOST_CHECK_EQUAL(boost::tree::successor(Order(), c), false);
    BOOST_CHECK(c == frbt1.root());
}

BOOST_AUTO_TEST_CASE( test_successor_ascending )
{
    fake_binary_tree<int, boost::tree::ascending_vertical_traversal_tag> fabt1(fbt1);
    fake_binary_tree<int, boost::tree::ascending_vertical_traversal_tag>::cursor c = fabt1.root();
    c.to_begin().to_end().to_begin();
    BOOST_CHECK_EQUAL(*c, 4);
    boost::tree::successor(ascending(), c);
    BOOST_CHECK_EQUAL(*c, 6);
    boost::tree::successor(ascending(), c);
    BOOST_CHECK_EQUAL(*c, 3);
    boost::tree::successor(ascending(), c);
    BOOST_CHECK_EQUAL(*c, 8);
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*
  Copyright 2017 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)
*/
//==================================================================================================
//! [fill]
#include <boost/simd/algorithm/fill.hpp>
#include <boost/simd/function/is_gtz.hpp>
#include <iostream>
#include <vector>

int main()
{
  namespace bs =  boost::simd;
  std::vector<float> d(7);

  bs::fill( d.data(), d.data()+7,25.5f );
   std::cout << "d =  (" ;
  for( auto e : d) std::cout << e << " ";
  std::cout << ") " << std::endl;
  return 0;
}
//! [fill]


/*
  Copyright (c) Evgeny Shulgin <izaronplatz@gmail.com>, 2018

  Distributed under the license described in the LICENSE file
*/

/// \file bits.hpp
/// \brief Provides fast implementations of common bit algorithms.
/// \author Evgeny Shulgin

#ifndef BIT_ALGORITHM_HPP
#define BIT_ALGORITHM_HPP

#include <boost/detail/workaround.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/utility/enable_if.hpp>

// Trying to access Microsoft Visual C++ intrinsics
#if (defined(BOOST_MSVC) && (BOOST_MSVC >= 1600)) || (defined(__clang__) && defined(__c2__)) || (defined(BOOST_INTEL) && defined(_MSC_VER)) && (defined(_M_IX86) || defined(_M_X64))
#   define BIT_ALGORITHM_MSVC
#   ifdef _M_X64
#       define BIT_ALGORITHM_MSVC_64
#   endif
// Trying to access GCC intrinsics
#elif defined(BOOST_GCC) || defined(__clang__) || (defined(BOOST_INTEL) && defined(__GNUC__))
#   define BIT_ALGORITHM_GCC
#endif

// Define specifiers for all functions
// It defines a function 'constexpr' if possible
// Although 'constexpr' implies 'inline', we still define forced inlining
#define BIT_ALGORITHM_FUNC_SPEC BOOST_CXX14_CONSTEXPR BOOST_FORCEINLINE
// If a function contains exactly one 'return', it can use C++11 constexpr
#define BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN BIT_ALGORITHM_FUNC_SPEC

#ifdef BIT_ALGORITHM_MSVC
#include <intrin.h>
#endif

namespace bit_algo {
namespace impl {

typedef unsigned char byte_type;
const unsigned int table_width = 8;

// ------- bit_count function implementation --------------

template <bool dummy_name = true>
struct count_table { static const byte_type table[]; };

template <>
struct count_table<false> { /* no table */ };

template <bool b>
const byte_type count_table<b>::table[] =
{
    // Automatically generated bit_count values for [0; 255]
    0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4, 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5,
    1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
    1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
    2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
    1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
    2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
    2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
    3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 4, 5, 5, 6, 5, 6, 6, 7, 5, 6, 6, 7, 6, 7, 7, 8
};

// ------- first_set function implementation --------------

template <bool dummy_name = true>
struct first_set_table { static const byte_type table[]; };

template <>
struct first_set_table<false> { /* no table */ };

template <bool b>
const byte_type first_set_table<b>::table[] =
{
    // Automatically generated first_set values for [0; 255]
    0, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1, 5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
    6, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1, 5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
    7, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1, 5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
    6, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1, 5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
    8, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1, 5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
    6, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1, 5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
    7, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1, 5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
    6, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1, 5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1
};

}

/// \fn bit_count ( IntType value )
/// \return Number of bits set to 1 in value of an integral type
///
/// \param value    The value to get its bit count
///
/// \note This function works regardless of the value sign.
template <typename T>
BIT_ALGORITHM_FUNC_SPEC typename boost::enable_if<
    boost::is_integral<T>, int
>::type bit_count(T value)
{
    int res = 0;
    while (value)
    {
        res += impl::count_table<true>
            ::table[value & ((1u << impl::table_width) - 1)];
        value >>= impl::table_width;
    }
    return res;
}

#if defined(BIT_ALGORITHM_MSVC)

// Using __popcnt variations
template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int bit_count(unsigned short value)
{
    return static_cast<int>(__popcnt16(value));
}

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int bit_count(unsigned int value)
{
    return static_cast<int>(__popcnt(value));
}

#if defined(BIT_ALGORITHM_MSVC_64)

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int bit_count(unsigned __int64 value)
{
    return static_cast<int>(__popcnt64(value));
}

// Casting signed args to unsigned
template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int bit_count(__int64 value)
{
    return static_cast<unsigned __int64>(bit_count(value));
}

#else // !defined(BIT_ALGORITHM_MSVC_64)

// Win32 doesn't have __popcnt64, emulate it with two 32 bit calls
template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int bit_count(unsigned long long value)
{
    return bit_count(static_cast<unsigned int>(value >> 32)) +
        bit_count(static_cast<unsigned int>(value)); // cuts out leading 32 bits
}

// Casting signed args to unsigned
template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int bit_count(long long value)
{
    return static_cast<unsigned long long>(bit_count(value));
}

#endif

// Casting signed args to unsigned
template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int bit_count(short value)
{
    return bit_count(static_cast<unsigned short>(value));
}

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int bit_count(int value)
{
    return bit_count(static_cast<unsigned int>(value));
}

#elif defined(BIT_ALGORITHM_GCC)

// Using __builtin_popcount variations
template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int bit_count(unsigned int value)
{
    return __builtin_popcount(value);
}

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int bit_count(unsigned long value)
{
    return __builtin_popcountl(value);
}

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int bit_count(unsigned long long value)
{
    return __builtin_popcountll(value);
}

// Casting signed args to unsigned
template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int bit_count(int value)
{
    return bit_count(static_cast<unsigned int>(value));
}

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int bit_count(long value)
{
    return bit_count(static_cast<unsigned long>(value));
}

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int bit_count(long long value)
{
    return bit_count(static_cast<unsigned long long>(value));
}

#endif // BIT_ALGORITHM_GCC

/// \fn parity ( IntType value )
/// \return true if the number of bits set to 1 in value is even,
///         false otherwise.
///
/// \param value    The value to get its parity
///
/// \note This function works regardless of the value sign.
template <typename T>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN typename boost::enable_if<
    boost::is_integral<T>, bool
>::type parity(T value)
{
    return bit_count<T>(value) & 1;
}

/// \fn first_set ( IntType value )
/// \return Returns one plus the index of the least significant 1-bit of value.
///         If x is zero, returns zero. If x is negative, return the maximum
///         bit index, as the sign bit is the least one.
///
/// \param value    The value to get its least significant bit set
template <typename T>
BIT_ALGORITHM_FUNC_SPEC typename boost::enable_if_c<
    boost::is_integral<T>::value, int
>::type first_set(T value)
{
    if (value == 0)
        return 0;

    int add = 0, res = 0;
    while (true)
    {
        res = impl::first_set_table<true>
            ::table[value & ((1u << impl::table_width) - 1)];
        if (res)
            break;
        value >>= impl::table_width;
        add += impl::table_width;
    }

    return add + res;
}

#if defined(BIT_ALGORITHM_MSVC)

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int first_set(unsigned long value)
{
    unsigned long index;
    if (_BitScanForward(&index, value))
        return index + 1;
    else
        return 0;
}

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int first_set(long value)
{
    return first_set(static_cast<unsigned long>(value));
}

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int first_set(unsigned int value)
{
    return first_set(static_cast<unsigned long>(value));
}

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int first_set(int value)
{
    return first_set(static_cast<unsigned int>(value));
}

#if defined(BIT_ALGORITHM_MSVC_64)

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int first_set(unsigned __int64 value)
{
    unsigned long index;
    if (_BitScanForward64(&index, value))
        return index + 1;
    else
        return 0;
}

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int first_set(__int64 value)
{
    return first_set(static_cast<unsigned __int64>(value));
}

#else // !defined(BIT_ALGORITHM_MSVC_64)

// Win32 doesn't have _BitScanForward64, emulate it with two 32 bit calls
template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int first_set(unsigned long long value)
{
    if (value == 0)
        return 0;
    int first_half = first_set(static_cast<unsigned int>(value)); // cuts last 32 bits
    if (!first_half)
        return 32 + first_set(static_cast<unsigned int>(value >> 32));
    return first_half;
}

// Casting signed args to unsigned
template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int first_set(long long value)
{
    return static_cast<unsigned long long>(first_set(value));
}

#endif // BIT_ALGORITHM_MSVC_64

#elif defined(BIT_ALGORITHM_GCC)

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int first_set(int value)
{
    return __builtin_ffs(value);
}

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int first_set(long value)
{
    return __builtin_ffsl(value);
}

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int first_set(long long value)
{
    return __builtin_ffsll(value);
}

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int first_set(unsigned int value)
{
    return first_set(static_cast<int>(value));
}

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int first_set(unsigned long value)
{
    return first_set(static_cast<long>(value));
}

template<>
BIT_ALGORITHM_FUNC_SPEC_SINGLE_RETURN int first_set(unsigned long long value)
{
    return first_set(static_cast<long long>(value));
}

#endif

}

#endif // BIT_ALGORITHM_HPP


#include <boost/test/floating_point_comparison.hpp>


//
// Created by Mamba on 2020/2/18.
//
// #define R_BUILD
// #define TEST

#ifdef R_BUILD
#include <Rcpp.h>
#include <RcppEigen.h>
using namespace Rcpp;
// [[Rcpp::depends(RcppEigen)]]
#else

#include <Eigen/Eigen>
#include "List.h"

#endif

#include <iostream>
#include "Data.h"
#include "Algorithm.h"
#include "Metric.h"
#include "abess.h"

using namespace Eigen;
using namespace std;

// int sign(double a)
// {
//   if (a > 0)
//   {
//     return 1;
//   }
//   else if (a < 0)
//   {
//     return -1;
//   }
//   else
//   {
//     return 0;
//   }
// }

// double det(double a[], double b[])
// {
//   return a[0] * b[1] - a[1] * b[0];
// }

// calculate the intersection of two lines
// if parallal, need_flag = false.
// void line_intersection(double line1[2][2], double line2[2][2], double intersection[], bool &need_flag)
// {
//   double xdiff[2], ydiff[2], d[2];
//   double div;

//   xdiff[0] = line1[0][0] - line1[1][0];
//   xdiff[1] = line2[0][0] - line2[1][0];
//   ydiff[0] = line1[0][1] - line1[1][1];
//   ydiff[1] = line2[0][1] - line2[1][1];

//   div = det(xdiff, ydiff);
//   if (div == 0)
//   {
//     need_flag = false;
//     return;
//   }
//   else
//   {
//     d[0] = det(line1[0], line1[1]);
//     d[1] = det(line2[0], line2[1]);

//     intersection[0] = det(d, xdiff) / div;
//     intersection[1] = det(d, ydiff) / div;
//     need_flag = true;
//     return;
//   }
// }

// // boundary: s=smin, s=max, lambda=lambda_min, lambda_max
// // line: crosses p and is parallal to u
// // calculate the intersections between boundary and line
// void cal_intersections(double p[], double u[], int s_min, int s_max, double lambda_min, double lambda_max, double a[], double b[])
// {
//   double line0[2][2], line_set[4][2][2], intersections[4][2];
//   bool need_flag[4];
//   int i, j;

//   line0[0][0] = double(p[0]);
//   line0[0][1] = double(p[1]);
//   line0[1][0] = double(p[0] + u[0]);
//   line0[1][1] = double(p[1] + u[1]);

//   line_set[0][0][0] = double(s_min);
//   line_set[0][0][1] = double(lambda_min);
//   line_set[0][1][0] = double(s_min);
//   line_set[0][1][1] = double(lambda_max);

//   line_set[1][0][0] = double(s_max);
//   line_set[1][0][1] = double(lambda_min);
//   line_set[1][1][0] = double(s_max);
//   line_set[1][1][1] = double(lambda_max);

//   line_set[2][0][0] = double(s_min);
//   line_set[2][0][1] = double(lambda_min);
//   line_set[2][1][0] = double(s_max);
//   line_set[2][1][1] = double(lambda_min);

//   line_set[3][0][0] = double(s_min);
//   line_set[3][0][1] = double(lambda_max);
//   line_set[3][1][0] = double(s_max);
//   line_set[3][1][1] = double(lambda_max);

//   for (i = 0; i < 4; i++)
//   {
//     line_intersection(line0, line_set[i], intersections[i], need_flag[i]);
//   }

//   // delete intersections beyond boundary
//   for (i = 0; i < 4; i++)
//   {
//     if (need_flag[i])
//     {
//       if ((intersections[i][0] < s_min - 0.0001) | (intersections[i][0] > s_max + 0.0001) | (intersections[i][1] < lambda_min - 0.001) | (intersections[i][1] > lambda_max + 0.001))
//       {
//         need_flag[i] = false;
//       }
//     }
//   }

//   // delecte repetitive intersections
//   for (i = 0; i < 4; i++)
//   {
//     if (need_flag[i])
//     {
//       for (j = i + 1; j < 4; j++)
//       {
//         if (need_flag[j])
//         {
//           if (abs(intersections[i][0] - intersections[j][0]) < 0.0001 && abs(intersections[i][1] - intersections[j][1]) < 0.0001)
//           {
//             need_flag[j] = false;
//           }
//         }
//       }
//     }
//   }

//   j = 0;
//   for (i = 0; i < 4; i++)
//   {
//     if (need_flag[i])
//     {
//       if (j == 2)
//       {
//         j += 1;
//       }
//       if (j == 1)
//       {
//         b[0] = intersections[i][0];
//         b[1] = intersections[i][1];
//         j += 1;
//       }
//       if (j == 0)
//       {
//         a[0] = intersections[i][0];
//         a[1] = intersections[i][1];
//         j += 1;
//       }
//     }
//   }

//   if (j != 2)
//   {
// #ifdef R_BUILD
//     Rcpp::Rcout << "---------------------------" << endl;
//     Rcpp::Rcout << "j: " << j << endl;
//     Rcpp::Rcout << "inetrsection numbers wrong" << j << endl;
//     Rcpp::Rcout << "p" << p[0] << "," << p[1] << endl;
//     Rcpp::Rcout << "u" << u[0] << "," << u[1] << endl;
//     Rcpp::Rcout << "s_min" << s_min << endl;
//     Rcpp::Rcout << "s_max" << s_max << endl;
//     Rcpp::Rcout << "lambda_min" << lambda_min << endl;
//     Rcpp::Rcout << "lambda_max" << lambda_max << endl;
//     Rcpp::Rcout << "intersections[0]" << intersections[0][0] << "," << intersections[0][1] << endl;
//     Rcpp::Rcout << "intersections[1]" << intersections[1][0] << "," << intersections[1][1] << endl;
//     Rcpp::Rcout << "intersections[2]" << intersections[2][0] << "," << intersections[2][1] << endl;
//     Rcpp::Rcout << "intersections[3]" << intersections[3][0] << "," << intersections[3][1] << endl;
//     Rcpp::Rcout << "need_flag[0]" << need_flag[0] << endl;
//     Rcpp::Rcout << "need_flag[1]" << need_flag[1] << endl;
//     Rcpp::Rcout << "need_flag[2]" << need_flag[2] << endl;
//     Rcpp::Rcout << "need_flag[3]" << need_flag[3] << endl;
// #else
//     cout << "---------------------------" << endl;
//     cout << "j: " << j << endl;
//     cout << "inetrsection numbers wrong" << j << endl;
//     cout << "p" << p[0] << "," << p[1] << endl;
//     cout << "u" << u[0] << "," << u[1] << endl;
//     cout << "s_min" << s_min << endl;
//     cout << "s_max" << s_max << endl;
//     cout << "lambda_min" << lambda_min << endl;
//     cout << "lambda_max" << lambda_max << endl;
//     cout << "intersections[0]" << intersections[0][0] << "," << intersections[0][1] << endl;
//     cout << "intersections[1]" << intersections[1][0] << "," << intersections[1][1] << endl;
//     cout << "intersections[2]" << intersections[2][0] << "," << intersections[2][1] << endl;
//     cout << "intersections[3]" << intersections[3][0] << "," << intersections[3][1] << endl;
//     cout << "need_flag[0]" << need_flag[0] << endl;
//     cout << "need_flag[1]" << need_flag[1] << endl;
//     cout << "need_flag[2]" << need_flag[2] << endl;
//     cout << "need_flag[3]" << need_flag[3] << endl;
// #endif
//   }

//   return;
// }

// void golden_section_search(Data &data, Algorithm *algorithm, Metric *metric, double p[], double u[], int s_min, int s_max, double log_lambda_min, double log_lambda_max, double best_arg[],
//                            Eigen::VectorXd &beta1, double &coef01, double &train_loss1, double &ic1, Eigen::MatrixXd &ic_sequence)
// {
//     int n = data.get_n();
//     Eigen::VectorXi full_mask(n);
//     for (int i = 0; i < n; i++)
//     {
//         full_mask(i) = int(i);
//     }
//     vector<Eigen::MatrixXd> full_group_XTX = group_XTX(data.x, data.g_index, data.g_size, data.n, data.p, data.g_num, algorithm->model_type);

//     Eigen::VectorXd beta_init = Eigen::VectorXd::Zero(data.get_p());
//     Eigen::VectorXd beta_temp1 = Eigen::VectorXd::Zero(data.get_p());
//     Eigen::VectorXd beta_temp2 = Eigen::VectorXd::Zero(data.get_p());
//     double train_loss_temp1, train_loss_temp2;
//     double coef0_temp1, coef0_temp2;
//     double coef0_init = 0.0;
//     int ic_row, ic_col;
//     double d_lambda = (log_lambda_max - log_lambda_min) / 99;

//     double invphi, invphi2, closs, dloss;
//     double a[2], b[2], c[2], d[2], h[2];

//     // stop condiction
//     double s_tol = 2;
//     double log_lambda_tol = (log_lambda_max - log_lambda_min) / 200;

//     invphi = (pow(5, 0.5) - 1.0) / 2.0;
//     invphi2 = (3.0 - pow(5, 0.5)) / 2.0;
//     cal_intersections(p, u, s_min, s_max, log_lambda_min, log_lambda_max, a, b);

//     h[0] = b[0] - a[0];
//     h[1] = b[1] - a[1];

//     c[0] = a[0] + invphi2 * h[0];
//     c[1] = a[1] + invphi2 * h[1];
//     d[0] = a[0] + invphi * h[0];
//     d[1] = a[1] + invphi * h[1];

//     if (h[0] > 0.0001)
//     {
//         c[0] = int(c[0]);
//         d[0] = ceil(d[0]);
//     }
//     else if (h[0] < -0.0001)
//     {
//         c[0] = ceil(c[0]);
//         d[0] = int(d[0]);
//     }
//     else
//     {
//         c[0] = round(c[0]);
//         d[0] = round(d[0]);
//     }

//     algorithm->update_train_mask(full_mask);
//     algorithm->update_sparsity_level(int(c[0]));
//     algorithm->update_lambda_level(exp(c[1]));
//     algorithm->update_beta_init(beta_init);
//     algorithm->update_coef0_init(coef0_init);
//     algorithm->update_group_XTX(full_group_XTX);
//     algorithm->fit();
//     if (algorithm->warm_start)
//     {
//         beta_init = algorithm->get_beta();
//         coef0_init = algorithm->get_coef0();
//     }
//     closs = metric->ic(algorithm, data);
//     coef0_temp1 = algorithm->get_coef0();
//     beta_temp1 = algorithm->get_beta();
//     train_loss_temp1 = metric->train_loss(algorithm, data);

//     ic_row = int(c[0]);
//     ic_col = floor((c[1] - log_lambda_min) / d_lambda);
//     if (std::isnan(ic_sequence(ic_row, ic_col)))
//     {
//         ic_sequence(ic_row, ic_col) = closs;
//     }
//     else
//     {
//         ic_sequence(ic_row, ic_col) = (ic_sequence(ic_row, ic_col) > closs) ? closs : ic_sequence(ic_row, ic_col);
//     }

//     algorithm->update_train_mask(full_mask);
//     algorithm->update_sparsity_level(int(d[0]));
//     algorithm->update_lambda_level(exp(d[1]));
//     algorithm->update_beta_init(beta_init);
//     algorithm->update_coef0_init(coef0_init);
//     algorithm->update_group_XTX(full_group_XTX);
//     algorithm->fit();
//     if (algorithm->warm_start)
//     {
//         beta_init = algorithm->get_beta();
//         coef0_init = algorithm->get_coef0();
//     }

//     dloss = metric->ic(algorithm, data);
//     coef0_temp2 = algorithm->get_coef0();
//     beta_temp2 = algorithm->get_beta();
//     train_loss_temp2 = metric->train_loss(algorithm, data);
//     ic_row = int(d[0]);
//     ic_col = floor((d[1] - log_lambda_min) / d_lambda);
//     if (std::isnan(ic_sequence(ic_row, ic_col)))
//     {
//         ic_sequence(ic_row, ic_col) = dloss;
//     }
//     else
//     {
//         ic_sequence(ic_row, ic_col) = (ic_sequence(ic_row, ic_col) > dloss) ? dloss : ic_sequence(ic_row, ic_col);
//     }

//     if (abs((invphi2 - invphi) * h[0]) <= s_tol && abs((invphi2 - invphi) * h[1]) < log_lambda_tol)
//     {
//         double min_loss;
//         double tmp_loss;
//         if (closs < dloss)
//         {
//             best_arg[0] = c[0];
//             best_arg[1] = c[1];
//             min_loss = closs;

//             beta1 = beta_temp1;
//             coef01 = coef0_temp1;
//             ic1 = closs;
//             train_loss1 = train_loss_temp1;
//         }
//         else
//         {
//             best_arg[0] = d[0];
//             best_arg[1] = d[1];
//             min_loss = dloss;

//             beta1 = beta_temp2;
//             coef01 = coef0_temp2;
//             ic1 = dloss;
//             train_loss1 = train_loss_temp2;
//         }
//         for (int i = 1; i < abs((invphi2 - invphi) * h[0]); i++)
//         {
//             algorithm->update_train_mask(full_mask);
//             algorithm->update_sparsity_level(int(c[0] + sign(h[0]) * i));
//             algorithm->update_lambda_level(exp(c[1]));
//             algorithm->update_beta_init(beta_init);
//             algorithm->update_coef0_init(coef0_init);
//             algorithm->update_group_XTX(full_group_XTX);
//             algorithm->fit();
//             if (algorithm->warm_start)
//             {
//                 beta_init = algorithm->get_beta();
//                 coef0_init = algorithm->get_coef0();
//             }

//             tmp_loss = metric->ic(algorithm, data);
//             ic_row = int(c[0]);
//             ic_col = floor((c[1] - log_lambda_min) / d_lambda);
//             if (std::isnan(ic_sequence(ic_row, ic_col)))
//             {
//                 ic_sequence(ic_row, ic_col) = tmp_loss;
//             }
//             else
//             {
//                 ic_sequence(ic_row, ic_col) = (ic_sequence(ic_row, ic_col) > tmp_loss) ? tmp_loss : ic_sequence(ic_row, ic_col);
//             }
//             if (tmp_loss < min_loss)
//             {
//                 best_arg[0] = c[0] + sign(h[0]) * i;
//                 best_arg[1] = c[1];
//                 min_loss = tmp_loss;

//                 beta1 = algorithm->get_beta();
//                 coef01 = algorithm->get_coef0();
//                 train_loss1 = metric->train_loss(algorithm, data);
//                 ic1 = min_loss;
//             }
//         }
//         return;
//     }
//     int tt = 0;
//     while (tt < 100)
//     {
//         tt++;
//         if (closs < dloss)
//         {
//             b[0] = d[0];
//             b[1] = d[1];
//             d[0] = c[0];
//             d[1] = c[1];
//             dloss = closs;
//             h[0] = b[0] - a[0];
//             h[1] = b[1] - a[1];

//             c[0] = a[0] + invphi2 * h[0];
//             c[1] = a[1] + invphi2 * h[1];
//             if (h[0] > 0.0001)
//             {
//                 c[0] = int(c[0]);
//             }
//             else if (h[0] < -0.0001)
//             {
//                 c[0] = ceil(c[0]);
//             }
//             else
//             {
//                 c[0] = round(c[0]);
//             }

//             algorithm->update_train_mask(full_mask);
//             algorithm->update_sparsity_level(int(c[0]));
//             algorithm->update_lambda_level(exp(c[1]));
//             algorithm->update_beta_init(beta_init);
//             algorithm->update_coef0_init(coef0_init);
//             algorithm->update_group_XTX(full_group_XTX);
//             algorithm->fit();
//             if (algorithm->warm_start)
//             {
//                 beta_init = algorithm->get_beta();
//                 coef0_init = algorithm->get_coef0();
//             }
//             closs = metric->ic(algorithm, data);
//             coef0_temp1 = algorithm->get_coef0();
//             beta_temp1 = algorithm->get_beta();
//             train_loss_temp1 = metric->train_loss(algorithm, data);
//             ic_row = int(c[0]);
//             ic_col = floor((c[1] - log_lambda_min) / d_lambda);
//             if (std::isnan(ic_sequence(ic_row, ic_col)))
//             {
//                 ic_sequence(ic_row, ic_col) = closs;
//             }
//             else
//             {
//                 ic_sequence(ic_row, ic_col) = (ic_sequence(ic_row, ic_col) > closs) ? closs : ic_sequence(ic_row, ic_col);
//             }
//         }

//         else
//         {
//             a[0] = c[0];
//             a[1] = c[1];
//             c[0] = d[0];
//             c[1] = d[1];
//             closs = dloss;
//             h[0] = b[0] - a[0];
//             h[1] = b[1] - a[1];

//             d[0] = a[0] + invphi * h[0];
//             d[1] = a[1] + invphi * h[1];

//             if (h[0] > 0.0001)
//             {
//                 d[0] = ceil(d[0]);
//             }
//             else if (h[0] < -0.0001)
//             {
//                 d[0] = int(d[0]);
//             }
//             else
//             {
//                 d[0] = round(d[0]);
//             }

//             algorithm->update_train_mask(full_mask);
//             algorithm->update_sparsity_level(int(d[0]));
//             algorithm->update_lambda_level(exp(d[1]));
//             algorithm->update_beta_init(beta_init);
//             algorithm->update_coef0_init(coef0_init);
//             algorithm->update_group_XTX(full_group_XTX);
//             algorithm->fit();
//             if (algorithm->warm_start)
//             {
//                 beta_init = algorithm->get_beta();
//                 coef0_init = algorithm->get_coef0();
//             }

//             dloss = metric->ic(algorithm, data);
//             coef0_temp2 = algorithm->get_coef0();
//             beta_temp2 = algorithm->get_beta();
//             train_loss_temp2 = metric->train_loss(algorithm, data);
//             ic_row = int(d[0]);
//             ic_col = floor((d[1] - log_lambda_min) / d_lambda);
//             if (std::isnan(ic_sequence(ic_row, ic_col)))
//             {
//                 ic_sequence(ic_row, ic_col) = dloss;
//             }
//             else
//             {
//                 ic_sequence(ic_row, ic_col) = (ic_sequence(ic_row, ic_col) > dloss) ? dloss : ic_sequence(ic_row, ic_col);
//             }
//         }

//         if ((abs((invphi2 - invphi) * h[0]) <= s_tol && abs((invphi2 - invphi) * h[1]) < log_lambda_tol) || tt == 50)
//         {
//             double min_loss;
//             double tmp_loss;
//             if (closs < dloss)
//             {
//                 best_arg[0] = c[0];
//                 best_arg[1] = c[1];
//                 min_loss = closs;

//                 beta1 = beta_temp1;
//                 coef01 = coef0_temp1;
//                 ic1 = closs;
//                 train_loss1 = train_loss_temp1;
//             }
//             else
//             {
//                 best_arg[0] = d[0];
//                 best_arg[1] = d[1];
//                 min_loss = dloss;

//                 beta1 = beta_temp2;
//                 coef01 = coef0_temp2;
//                 ic1 = dloss;
//                 train_loss1 = train_loss_temp2;
//             }
//             for (int i = 1; i < abs((invphi2 - invphi) * h[0]); i++)
//             {
//                 algorithm->update_train_mask(full_mask);
//                 algorithm->update_sparsity_level(int(c[0] + sign(h[0]) * i));
//                 algorithm->update_lambda_level(exp(c[1]));
//                 algorithm->update_beta_init(beta_init);
//                 algorithm->update_coef0_init(coef0_init);
//                 algorithm->update_group_XTX(full_group_XTX);
//                 algorithm->fit();
//                 if (algorithm->warm_start)
//                 {
//                     beta_init = algorithm->get_beta();
//                     coef0_init = algorithm->get_coef0();
//                 }
//                 tmp_loss = metric->ic(algorithm, data);

//                 ic_row = int(c[0]);
//                 ic_col = floor((c[1] - log_lambda_min) / d_lambda);
//                 if (std::isnan(ic_sequence(ic_row, ic_col)))
//                 {
//                     ic_sequence(ic_row, ic_col) = tmp_loss;
//                 }
//                 else
//                 {
//                     ic_sequence(ic_row, ic_col) = (ic_sequence(ic_row, ic_col) > tmp_loss) ? tmp_loss : ic_sequence(ic_row, ic_col);
//                 }
//                 if (tmp_loss < min_loss)
//                 {
//                     best_arg[0] = c[0] + sign(h[0]) * i;
//                     best_arg[1] = c[1];
//                     min_loss = tmp_loss;

//                     beta1 = algorithm->get_beta();
//                     coef01 = algorithm->get_coef0();
//                     train_loss1 = metric->train_loss(algorithm, data);
//                     ic1 = min_loss;
//                 }
//             }
//             return;
//         }
//     }
// }

// int GDC(int a, int b)
// {
//   int Max, Min;
//   Max = a > b ? a : b;
//   if (a == Max)
//     Min = b;
//   else
//     Min = a;
//   int z = Min;
//   while (Max % Min != 0)
//   {
//     z = Max % Min;
//     Max = Min;
//     Min = z;
//   }
//   return z;
// }

// void seq_search(Data &data, Algorithm *algorithm, Metric *metric, double p[], double u[], int s_min, int s_max, double log_lambda_min, double log_lambda_max, double best_arg[],
//                 Eigen::VectorXd &beta1, double &coef01, double &train_loss1, double &ic1, int nlambda, Eigen::MatrixXd &ic_sequence)
// {
//     vector<Eigen::MatrixXd> full_group_XTX = group_XTX(data.x, data.g_index, data.g_size, data.n, data.p, data.g_num, algorithm->model_type);

//     int n = data.get_n();
//     Eigen::VectorXi full_mask(n);
//     for (int i = 0; i < n; i++)
//     {
//         full_mask(i) = int(i);
//     }
//     Eigen::VectorXd beta_init = Eigen::VectorXd::Zero(data.get_p());
//     double coef0_init = 0.0;
//     Eigen::VectorXd beta_warm(data.get_p());
//     double coef0_warm = 0.0;

//     double d_lambda = (log_lambda_max - log_lambda_min) / (nlambda - 1);
//     int k_lambda = abs(round(u[1] / d_lambda));
//     cout << "d_lambda: " << d_lambda << ", k_lambda: " << k_lambda << ", u[1]: " << u[1] << ", u[0]: " << u[0] << endl;
//     if (abs(u[0]) != 1 && k_lambda != 1)
//     {
//         if (k_lambda == 0 && u[0] != 0)
//         {
//             u[0] = u[0] / abs(u[0]);
//         }
//         else if (u[0] == 0 && k_lambda != 0)
//         {
//             u[1] = u[1] / k_lambda;
//         }
//         else
//         {
//             int gdc;
//             gdc = GDC(k_lambda, abs(int(u[0])));
//             if (gdc)
//             {
//                 u[0] = round(u[0] / gdc);
//                 u[1] = u[1] / gdc;
//             }
//         }
//     }
//     cout << "u[0]: " << u[0] << ", u[1]: " << u[1] << endl;

//     int i = 0;
//     int j = 0;
//     int ic_row, ic_col;
//     Eigen::MatrixXd beta_all_1 = Eigen::MatrixXd::Zero(data.get_p(), (s_max - s_min + 1) * nlambda);
//     Eigen::MatrixXd beta_all_2 = Eigen::MatrixXd::Zero(data.get_p(), (s_max - s_min + 1) * nlambda);
//     Eigen::VectorXd coef0_all_1 = Eigen::VectorXd::Zero((s_max - s_min + 1) * nlambda);
//     Eigen::VectorXd coef0_all_2 = Eigen::VectorXd::Zero((s_max - s_min + 1) * nlambda);
//     Eigen::VectorXd train_loss_1 = Eigen::VectorXd::Zero((s_max - s_min + 1) * nlambda);
//     Eigen::VectorXd train_loss_2 = Eigen::VectorXd::Zero((s_max - s_min + 1) * nlambda);
//     Eigen::VectorXd ic_sequence_1 = Eigen::VectorXd::Zero((s_max - s_min + 1) * nlambda);
//     Eigen::VectorXd ic_sequence_2 = Eigen::VectorXd::Zero((s_max - s_min + 1) * nlambda);
//     beta_warm.setZero();

//     algorithm->update_train_mask(full_mask);
//     algorithm->update_sparsity_level(p[0] + i * u[0]);
//     algorithm->update_lambda_level(exp(p[1] + i * u[1]));
//     algorithm->update_beta_init(beta_init);
//     algorithm->update_coef0_init(coef0_init);
//     algorithm->update_group_XTX(full_group_XTX);

//     algorithm->fit();
//     if (algorithm->warm_start)
//     {
//         beta_init = algorithm->get_beta();
//         coef0_init = algorithm->get_coef0();
//     }
//     ic_sequence_1(i) = metric->ic(algorithm, data);
//     beta_all_1.col(i) = algorithm->get_beta();
//     coef0_all_1(i) = algorithm->get_coef0();
//     train_loss_1(i) = metric->train_loss(algorithm, data);

//     ic_sequence_2(j) = ic_sequence_1(i);
//     beta_all_2.col(j) = beta_all_1.col(i);
//     coef0_all_2(j) = coef0_all_1(i);
//     train_loss_2(j) = train_loss_1(i);

//     ic_row = p[0] + i * u[0] - s_min;
//     ic_col = (p[1] + i * u[1] - log_lambda_min) / d_lambda;
//     if (std::isnan(ic_sequence(ic_row, ic_col)))
//     {
//         ic_sequence(ic_row, ic_col) = ic_sequence_1(i);
//     }
//     else
//     {
//         ic_sequence(ic_row, ic_col) = ic_sequence(ic_row, ic_col) > ic_sequence_1(i) ? ic_sequence_1(i) : ic_sequence(ic_row, ic_col);
//     }
//     cout << "i: " << i << ", ic_sequence(" << ic_row << "," << ic_col << "): " << ic_sequence(ic_row, ic_col) << endl;

//     i++;
//     j++;
//     beta_warm = beta_init;
//     coef0_warm = coef0_init;
//     cout << "s_max: " << s_max << ", s_min: " << s_min << ", log_lambda_max + d_lambda * 1e-4:" << log_lambda_max + d_lambda * 1e-4 << ", log_lambda_min - d_lambda * 1e-4: " << log_lambda_min - d_lambda * 1e-4 << endl;
//     while ((p[0] + i * u[0] <= s_max) && (p[1] + i * u[1] <= log_lambda_max + d_lambda * 1e-4) && (p[0] + i * u[0] >= s_min) && (p[1] + i * u[1] >= log_lambda_min - d_lambda * 1e-4))
//     {
//         cout << "i:" << i << ", p[0]: " << p[0] << ", u[0]: " << u[0] << ", p[0] + i * u[0]: " << p[0] + i * u[0] << ", p[1]" << p[1] << ", u[1]: " << u[1] << ", p[1] + i * u[1] : " << p[1] + i * u[1] << endl;
//         algorithm->update_train_mask(full_mask);
//         algorithm->update_sparsity_level(p[0] + i * u[0]);
//         algorithm->update_lambda_level(exp(p[1] + i * u[1]));
//         algorithm->update_beta_init(beta_init);
//         algorithm->update_coef0_init(coef0_init);
//         algorithm->update_group_XTX(full_group_XTX);

//         algorithm->fit();
//         if (algorithm->warm_start)
//         {
//             beta_init = algorithm->get_beta();
//             coef0_init = algorithm->get_coef0();
//         }
//         ic_sequence_1(i) = metric->ic(algorithm, data);
//         beta_all_1.col(i) = algorithm->get_beta();
//         coef0_all_1(i) = algorithm->get_coef0();
//         train_loss_1(i) = metric->train_loss(algorithm, data);
//         ic_row = p[0] + i * u[0] - s_min;
//         ic_col = round((p[1] + i * u[1] - log_lambda_min) / d_lambda);
//         if (std::isnan(ic_sequence(ic_row, ic_col)))
//         {
//             ic_sequence(ic_row, ic_col) = ic_sequence_1(i);
//         }
//         else
//         {
//             ic_sequence(ic_row, ic_col) = ic_sequence(ic_row, ic_col) > ic_sequence_1(i) ? ic_sequence_1(i) : ic_sequence(ic_row, ic_col);
//         }
//         cout << "i: " << i << ", ic_sequence(" << ic_row << "," << ic_col << "): " << ic_sequence(ic_row, ic_col) << ", ic_sequence_1(i): " << ic_sequence_1(i) << endl;
//         i++;
//     }
//     cout << "out: i:" << i << ", p[0] - i * u[0]: " << p[0] - i * u[0] << ", p[1] - i * u[1] : " << p[1] - i * u[1] << endl;

//     beta_init = beta_warm;
//     coef0_init = coef0_warm;

//     while ((p[0] - j * u[0] <= s_max) && (p[1] - j * u[1] <= log_lambda_max + d_lambda * 1e-4) && (p[0] - j * u[0] >= s_min) && (p[1] - j * u[1] >= log_lambda_min - d_lambda * 1e-4))
//     {
//         cout << "j:" << j << ", p[0]: " << p[0] << ", u[0]: " << u[0] << ", p[0] - j * u[0]: " << p[0] - j * u[0] << ", p[1]" << p[1] << ", u[1]: " << u[1] << ", p[1] - j * u[1]" << p[1] - j * u[1] << endl;
//         algorithm->update_train_mask(full_mask);
//         algorithm->update_sparsity_level(p[0] - j * u[0]);
//         algorithm->update_lambda_level(exp(p[1] - j * u[1]));
//         algorithm->update_beta_init(beta_init);
//         algorithm->update_coef0_init(coef0_init);
//         algorithm->update_group_XTX(full_group_XTX);

//         algorithm->fit();
//         if (algorithm->warm_start)
//         {
//             beta_init = algorithm->get_beta();
//             coef0_init = algorithm->get_coef0();
//         }
//         ic_sequence_2(j) = metric->ic(algorithm, data);
//         beta_all_2.col(j) = algorithm->get_beta();
//         coef0_all_2(j) = algorithm->get_coef0();
//         train_loss_2(j) = metric->train_loss(algorithm, data);
//         ic_row = p[0] - j * u[0] - s_min;
//         ic_col = round((p[1] - j * u[1] - log_lambda_min) / d_lambda);
//         if (std::isnan(ic_sequence(ic_row, ic_col)))
//         {
//             ic_sequence(ic_row, ic_col) = ic_sequence_2(j);
//         }
//         else
//         {
//             ic_sequence(ic_row, ic_col) = ic_sequence(ic_row, ic_col) > ic_sequence_2(j) ? ic_sequence_2(j) : ic_sequence(ic_row, ic_col);
//         }
//         cout << "j: " << j << ", ic_sequence(" << ic_row << "," << ic_col << "): " << ic_sequence(ic_row, ic_col) << "ic_sequence_2(j): " << ic_sequence_2(j) << endl;

//         j++;
//     }
//     cout << "out: j:" << j << ", p[0] - j * u[0]: " << p[0] - j * u[0] << ", p[1] - j * u[1] : " << p[1] - j * u[1] << endl;
//     int minPosition_1, minPosition_2;
//     ic_sequence_1 = ic_sequence_1.head(i).eval();
//     ic_sequence_2 = ic_sequence_2.head(j).eval();
//     ic_sequence_1.minCoeff(&minPosition_1);
//     ic_sequence_2.minCoeff(&minPosition_2);
//     cout << "minPosition_1: " << minPosition_1 << ", minPosition_2: " << minPosition_2 << ",  ic_sequence_1.minCoeff(&minPosition_1): " << ic_sequence_1(minPosition_1) << ", ic_sequence_2.minCoeff(&minPosition_2): " << ic_sequence_2(minPosition_2) << endl;
//     cout << "ic_sequence_2(minPosition_2): " << ic_sequence_2(minPosition_2) << ", ic_sequence_2(minPosition_2+1): " << ic_sequence_2(minPosition_2 + 1) << ", ic_sequence_2(minPosition_2+1)< ic_sequence_2(minPosition_2): " << (ic_sequence_2(minPosition_2 + 1) < ic_sequence_2(minPosition_2)) << endl;
//     int minPosition;
//     if (ic_sequence_1(minPosition_1) < ic_sequence_2(minPosition_2))
//     {
//         minPosition = minPosition_1;
//         ic1 = ic_sequence_1(minPosition);
//         train_loss1 = train_loss_1(minPosition);
//         beta1 = beta_all_1.col(minPosition);
//         coef01 = coef0_all_1(minPosition);
//     }
//     else
//     {
//         minPosition = -minPosition_2;
//         ic1 = ic_sequence_2(minPosition_2);
//         train_loss1 = train_loss_2(minPosition_2);
//         beta1 = beta_all_2.col(minPosition_2);
//         coef01 = coef0_all_2(minPosition_2);
//     }
//     best_arg[0] = p[0] + (minPosition)*u[0];
//     best_arg[1] = p[1] + (minPosition)*u[1];
//     cout << "minPosition :" << minPosition << ",  best_arg[0]:" << best_arg[0] << " best_arg[1]: " << best_arg[1] << ", p[1] + (minPosition)*u[1]= " << p[1] << "+" << minPosition << "* " << u[1] << "= " << p[1] + (minPosition)*u[1] << ", (minPosition)*u[1]: " << (minPosition)*u[1] << endl;
//     return;
// }

// List pgs_path(Data &data, Algorithm *algorithm, Metric *metric, int s_min, int s_max, double log_lambda_min, double log_lambda_max, int powell_path, int nlambda)
// {
//     int n = data.get_n();
//     Eigen::VectorXi full_mask(n);
//     vector<Eigen::MatrixXd> full_group_XTX = group_XTX(data.x, data.g_index, data.g_size, data.n, data.p, data.g_num, algorithm->model_type);
//     for (int i = 0; i < n; i++)
//     {
//         full_mask(i) = i;
//     }
//     Eigen::MatrixXd beta_all = Eigen::MatrixXd::Zero(data.get_p(), 100);
//     Eigen::VectorXd coef0_all = Eigen::VectorXd::Zero(100);
//     Eigen::VectorXd train_loss_all = Eigen::VectorXd::Zero(100);
//     Eigen::VectorXd ic_all = Eigen::VectorXd::Zero(100);
//     Eigen::VectorXd lambda_chosen = Eigen::VectorXd::Zero(100);

//     Eigen::VectorXd beta_temp = Eigen::VectorXd::Zero(data.get_p());
//     double coef0_temp;
//     double train_loss_temp;
//     double ic_temp;
//     //double lambda_temp;

//     if (powell_path == 1)
//         nlambda = 100;
//     Eigen::MatrixXd ic_sequence = NAN * Eigen::MatrixXd::Ones(s_max - s_min + 1, nlambda);

//     double P[3][3], U[2][2];
//     int i;

//     P[0][0] = double(s_min);
//     P[0][1] = log_lambda_min;

//     U[1][0] = 1.; //search directions
//     U[1][1] = 0.;
//     U[0][0] = 0.;
//     U[0][1] = (log_lambda_max - log_lambda_min) / (nlambda - 1);

//     int ttt = 0;
//     if (powell_path == 1)
//         golden_section_search(data, algorithm, metric, P[0], U[1], s_min, s_max, log_lambda_min, log_lambda_max, P[0], beta_temp, coef0_temp, train_loss_temp, ic_temp, ic_sequence);
//     else
//         seq_search(data, algorithm, metric, P[0], U[1], s_min, s_max, log_lambda_min, log_lambda_max, P[0], beta_temp, coef0_temp, train_loss_temp, ic_temp, nlambda, ic_sequence);
//     beta_all.col(ttt) = beta_temp;
//     coef0_all(ttt) = coef0_temp;
//     train_loss_all(ttt) = train_loss_temp;
//     ic_all(ttt) = ic_temp;
//     lambda_chosen(ttt) = exp(P[0][1]);

//     while (ttt < 11)
//     {
//         ttt++;
//         for (i = 0; i < 2; i++)
//         {
//             if (powell_path == 1)
//                 golden_section_search(data, algorithm, metric, P[i], U[i], s_min, s_max, log_lambda_min, log_lambda_max, P[i + 1], beta_temp, coef0_temp, train_loss_temp, ic_temp, ic_sequence);
//             else
//                 seq_search(data, algorithm, metric, P[i], U[i], s_min, s_max, log_lambda_min, log_lambda_max, P[i + 1], beta_temp, coef0_temp, train_loss_temp, ic_temp, nlambda, ic_sequence);
//             beta_all.col(ttt) = beta_temp;
//             coef0_all(ttt) = coef0_temp;
//             train_loss_all(ttt) = train_loss_temp;
//             ic_all(ttt) = ic_temp;
//             lambda_chosen(ttt) = exp(P[i + 1][1]);
//             ttt++;
//         }
//         U[0][0] = U[1][0];
//         U[0][1] = U[1][1];
//         U[1][0] = P[2][0] - P[0][0];
//         U[1][1] = P[2][1] - P[0][1];
//         if ((!(abs(U[1][0]) <= 0.0001 && abs(U[1][1]) <= 0.0001)) && ttt < 11)
//         {
//             if (powell_path == 1)
//                 golden_section_search(data, algorithm, metric, P[0], U[1], s_min, s_max, log_lambda_min, log_lambda_max, P[0], beta_temp, coef0_temp, train_loss_temp, ic_temp, ic_sequence);
//             else
//                 seq_search(data, algorithm, metric, P[0], U[1], s_min, s_max, log_lambda_min, log_lambda_max, P[0], beta_temp, coef0_temp, train_loss_temp, ic_temp, nlambda, ic_sequence);
//             beta_all.col(ttt) = beta_temp;
//             coef0_all(ttt) = coef0_temp;
//             train_loss_all(ttt) = train_loss_temp;
//             ic_all(ttt) = ic_temp;
//             lambda_chosen(ttt) = exp(P[0][1]);
//         }
//         else
//         {
//             // P[0] is the best parameter.
//             algorithm->update_train_mask(full_mask);
//             algorithm->update_sparsity_level(int(P[0][0]));
//             algorithm->update_lambda_level(exp(P[0][1]));
//             algorithm->update_group_XTX(full_group_XTX);
//             algorithm->fit();

//             Eigen::VectorXd best_beta = algorithm->get_beta();
//             double best_coef0 = algorithm->get_coef0();
//             double best_train_loss = metric->train_loss(algorithm, data);
//             double best_ic = metric->ic(algorithm, data);

//             beta_all.col(ttt) = best_beta;
//             coef0_all(ttt) = best_coef0;
//             train_loss_all(ttt) = best_train_loss;
//             ic_all(ttt) = best_ic;
//             lambda_chosen(ttt) = exp(P[0][1]);

//             ttt++;
//             beta_all = beta_all.leftCols(ttt).eval();
//             coef0_all = coef0_all.head(ttt).eval();
//             train_loss_all = train_loss_all.head(ttt).eval();
//             ic_all = ic_all.head(ttt).eval();
//             lambda_chosen = lambda_chosen.head(ttt).eval();

//             if (data.is_normal)
//             {
//                 if (algorithm->model_type == 1)
//                 {
//                     for (int k = 0; k < ttt; k++)
//                     {
//                         beta_all.col(k) = sqrt(double(n)) * beta_all.col(k).cwiseQuotient(data.x_norm);
//                         coef0_all(k) = data.y_mean - beta_all.col(k).dot(data.x_mean);
//                     }
//                 }

//                 else if (data.data_type == 2)
//                 {
//                     for (int k = 0; k < ttt; k++)
//                     {
//                         beta_all.col(k) = sqrt(double(n)) * beta_all.col(k).cwiseQuotient(data.x_norm);
//                         coef0_all(k) = coef0_all(k) - beta_all.col(k).dot(data.x_mean);
//                     }
//                 }
//                 else
//                 {
//                     for (int k = 0; k < ttt; k++)
//                     {
//                         beta_all.col(k) = sqrt(double(n)) * beta_all.col(k).cwiseQuotient(data.x_norm);
//                     }
//                 }
//             }
//             int min_ic_index = 0;
//             double min_ic = ic_all.minCoeff(&min_ic_index);
//             if (min_ic == ic_all(ttt - 1))
//             {
//                 min_ic_index = ttt - 1;
//             }
// #ifdef R_BUILD
//             return List::create(Named("beta") = beta_all.col(min_ic_index).eval(), Named("coef0") = coef0_all(min_ic_index),
//                                 Named("train_loss") = train_loss_all(min_ic_index), Named("ic") = ic_all(min_ic_index),
//                                 Named("lambda") = lambda_chosen(min_ic_index),
//                                 Named("beta_all") = beta_all, Named("coef0_all") = coef0_all,
//                                 Named("train_loss_all") = train_loss_all,
//                                 Named("ic_all") = ic_all,
//                                 Named("lambda_all") = lambda_chosen,
//                                 Named("ic_mat") = ic_sequence);
// #else
//             List mylist;
//             mylist.add("beta", beta_all.col(min_ic_index).eval());
//             mylist.add("coef0", coef0_all(min_ic_index));
//             mylist.add("train_loss", train_loss_all(min_ic_index));
//             mylist.add("ic", ic_all(min_ic_index));
//             mylist.add("lambda", lambda_chosen(min_ic_index));
//             return mylist;
// #endif
//         }
//     }
// #ifdef R_BUILD
//     Rcpp::Rcout << "powell end wrong" << endl;
// #else
//     cout << "powell end wrong" << endl;
// #endif
// #ifdef R_BUILD
//     return List::create(Named("beta") = 0);
// #else
//     List mylist;
//     return mylist;
// #endif
// }


#define BOOST_TEST_MODULE NiftyRagTest

#include <boost/test/unit_test.hpp>
#include "xtensor/xarray.hpp"

#include <iostream>
#include <random>

#include "nifty/tools/runtime_check.hxx"
#include "nifty/graph/rag/grid_rag_stacked_2d.hxx"
#include "nifty/graph/rag/grid_rag_stacked_2d_hdf5.hxx"

void getStackedSegmentation(xt::xarray<uint32_t> & seg,
        const std::vector<std::size_t> & shape) {
    // random generator
    std::default_random_engine gen;
    std::uniform_int_distribution<int> distr(0,9);
    auto draw = std::bind(distr, gen);
    // segmentation
    uint32_t label = 0;
    for(auto z = 0; z < shape[0]; ++z) {
        if(z > 0) {
            ++label;
        }
        for(auto y = 0; y < shape[1]; ++y) {
            for(auto x = 0; x < shape[2]; ++x) {
                seg(z,y,x) = label;
                if( draw() > 8  && (y != shape[1] - 1 && x != shape[2] - 1) )
                    ++label;
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(StackedRagHdf5Test)
{
    typedef nifty::graph::Hdf5Labels<3, uint32_t> LabelsProxy;

    std::vector<std::size_t> shape({20,100,100});
    xt::xarray<uint32_t> seg({20L, 100L, 100L});
    getStackedSegmentation(seg, shape);
    uint32_t maxLabel = *(std::max_element(seg.begin(), seg.end()));
    std::cout << "MaxLabel: " << maxLabel << std::endl;
    auto segFile = nifty::hdf5::createFile("./seg_tmp.h5");
    std::vector<std::size_t> chunks({10,50,50});
    nifty::hdf5::Hdf5Array<uint32_t> labels(segFile,
            "data",
            shape.begin(),
            shape.end(),
            chunks.begin());
    std::vector<std::size_t> start({0,0,0});
    labels.writeSubarray(start.begin(), seg);
    LabelsProxy labelsProxy(labels, maxLabel);
    nifty::graph::GridRagStacked2D<LabelsProxy> rag(labelsProxy);
}

/*
BOOST_AUTO_TEST_CASE(StackedRagTest)
{

}
*/


// Copyright 2019 Peter Dimov
//
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt

#include <boost/endian/arithmetic.hpp>
#include <boost/endian/buffers.hpp>
#include <boost/core/lightweight_test.hpp>

template<class T> struct align
{
    char _;
    T v;

    explicit align( typename T::value_type y ): _(), v( y )
    {
    }
};

template<class T, class U> void test_buffer( U const & y, bool aligned )
{
    align<T> x( y );

    BOOST_TEST_EQ( sizeof(x), aligned? 2 * sizeof(U): 1 + sizeof(U) );

    BOOST_TEST_EQ( x.v.value(), y );
}

template<class T, class U> void test_arithmetic( U const & y, bool aligned )
{
    test_buffer<T>( y, aligned );

    align<T> x( y );

    BOOST_TEST_EQ( x.v + 7, y + 7 );
}

int main()
{
    using namespace boost::endian;

    // buffers

    test_buffer<big_float32_buf_t>( 3.1416f, false );
    test_buffer<big_float64_buf_t>( 3.14159, false );

    test_buffer<little_float32_buf_t>( 3.1416f, false );
    test_buffer<little_float64_buf_t>( 3.14159, false );

    test_buffer<native_float32_buf_t>( 3.1416f, false );
    test_buffer<native_float64_buf_t>( 3.14159, false );

    test_buffer<big_float32_buf_at>( 3.1416f, true );
    test_buffer<big_float64_buf_at>( 3.14159, true );

    test_buffer<little_float32_buf_at>( 3.1416f, true );
    test_buffer<little_float64_buf_at>( 3.14159, true );

    // arithmetic

    test_arithmetic<big_float32_t>( 3.1416f, false );
    test_arithmetic<big_float64_t>( 3.14159, false );

    test_arithmetic<little_float32_t>( 3.1416f, false );
    test_arithmetic<little_float64_t>( 3.14159, false );

    test_arithmetic<native_float32_t>( 3.1416f, false );
    test_arithmetic<native_float64_t>( 3.14159, false );

    test_arithmetic<big_float32_at>( 3.1416f, true );
    test_arithmetic<big_float64_at>( 3.14159, true );

    test_arithmetic<little_float32_at>( 3.1416f, true );
    test_arithmetic<little_float64_at>( 3.14159, true );

    return boost::report_errors();
}


/*
 * Copyright (c) 2011-2021, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef DART_OPTIMIZATION_GENERICMULTIOBJECTIVEPROBLEM_HPP_
#define DART_OPTIMIZATION_GENERICMULTIOBJECTIVEPROBLEM_HPP_

#include <cstddef>
#include <vector>

#include <Eigen/Dense>

#include "dart/optimization/Function.hpp"
#include "dart/optimization/MultiObjectiveProblem.hpp"

namespace dart {
namespace optimization {

class GenericMultiObjectiveProblem : public MultiObjectiveProblem
{
public:
  /// Constructor
  explicit GenericMultiObjectiveProblem(
      std::size_t dim, std::size_t integerDim = 0u);

  /// Destructor
  ~GenericMultiObjectiveProblem() override = default;

  /// \{ \name Objectives

  // Documentation inherited.
  std::size_t getObjectiveDimension() const override;

  /// Sets objective functions to be minimized.
  void setObjectiveFunctions(const std::vector<FunctionPtr>& objectives);

  /// Adds a minimum objective function
  void addObjectiveFunction(FunctionPtr objective);

  /// Returns the number objective functions.
  std::size_t getNumObjectiveFunctions() const;

  /// Returns objective functions
  const std::vector<FunctionPtr>& getObjectiveFunctions() const;

  /// Removes an objective function
  void removeObjectiveFunction(FunctionPtr function);

  /// Removes all objective functions
  void removeAllObjectiveFunctions();

  /// \}

  /// \{ \name Equality Constraints

  // Documentation inherited.
  std::size_t getEqConstraintDimension() const override;

  /// Adds equality constraint
  void addEqConstraintFunction(FunctionPtr eqConst);

  /// Returns number of equality constraints
  std::size_t getNumEqualityConstraintFunctions() const;

  /// Returns equality constraint
  FunctionPtr getEqConstraintFunction(std::size_t index) const;

  /// Removes equality constraint
  void removeEqConstraintFunction(FunctionPtr eqConst);

  /// Removes all equality constraints
  void removeAllEqConstraintFunctions();

  /// \}

  /// \{ \name Inequality Constraints

  // Documentation inherited.
  std::size_t getIneqConstraintDimension() const override;

  /// Adds inequality constraint. Inequality constraints must evaluate
  /// to LESS THAN or equal to zero (within some tolerance) to be satisfied.
  void addIneqConstraintFunction(FunctionPtr ineqConst);

  /// Returns number of inequality constraints
  std::size_t getNumIneqConstraintFunctions() const;

  /// Returns inequality constraint
  FunctionPtr getIneqConstraintFunction(std::size_t index) const;

  /// Removes inequality constraint
  void removeIneqConstraintFunction(FunctionPtr ineqConst);

  /// Removes all inequality constraints
  void removeAllIneqConstraintFunctions();

  /// \}

  /// \{ \name Evaluations

  /// Evaluates objectives
  Eigen::VectorXd evaluateObjectives(const Eigen::VectorXd& x) const override;

  /// Evaluates equality constraints
  Eigen::VectorXd evaluateEqConstraints(
      const Eigen::VectorXd& x) const override;

  /// Evaluates inequality constraints
  Eigen::VectorXd evaluateIneqConstraints(
      const Eigen::VectorXd& x) const override;

  /// Return dimension of fitness
  std::size_t getFitnessDimension() const;

  /// Evaluates fitness, which is [objectives, equality constraints, inequality
  /// constraints].
  Eigen::VectorXd evaluateFitness(const Eigen::VectorXd& x) const;

  /// \}

protected:
  /// Objective functions
  std::vector<FunctionPtr> mObjectiveFunctions;

  /// Equality constraint functions
  std::vector<FunctionPtr> mEqConstraintFunctions;

  /// Inequality constraint functions
  std::vector<FunctionPtr> mIneqConstraintFunctions;

private:
  /// Cache for objective dimension
  std::size_t mObjectiveDimension;

  /// Cache for equality constraint dimension
  std::size_t mEqConstraintDimension;

  /// Cache for inequality constraint dimension
  std::size_t mIneqConstraintDimension;
};

} // namespace optimization
} // namespace dart

#endif // DART_OPTIMIZATION_GENERICMULTIOBJECTIVEPROBLEM_HPP_


// Copyright Aleksey Gurtovoy 2000-2004
//
// 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://www.boost.org/libs/mpl for documentation.

// $Id: for_each.cpp 66257 2010-10-29 21:36:38Z eric_niebler $
// $Date: 2010-10-29 14:36:38 -0700 (Fri, 29 Oct 2010) $
// $Revision: 66257 $

#include <boost/mpl/for_each.hpp>

#include <boost/mpl/list.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/identity.hpp>
#include <boost/mpl/lambda.hpp>
#include <boost/bind.hpp>

#include <vector>
#include <iostream>
#include <algorithm>
#include <typeinfo>
#include <cassert>

namespace mpl = boost::mpl;

struct type_printer
{
    type_printer(std::ostream& s) : f_stream(&s) {}
    template< typename U > void operator()(mpl::identity<U>)
    {
        *f_stream << typeid(U).name() << '\n';
    }

 private:
    std::ostream* f_stream;
};

struct value_printer
{
    value_printer(std::ostream& s) : f_stream(&s) {}
    template< typename U > void operator()(U x)
    {
        *f_stream << x << '\n';
    }

 private:
    std::ostream* f_stream;
};

#ifdef __ICL
# pragma warning(disable:985)
#endif

void push_back(std::vector<int>* c, int i)
{
    c->push_back(i);
}

int main()
{
    typedef mpl::list<char,short,int,long,float,double> types;
    mpl::for_each< types,mpl::make_identity<mpl::_1> >(type_printer(std::cout));

    typedef mpl::range_c<int,0,10> numbers;
    std::vector<int> v;

    mpl::for_each<numbers>(
          boost::bind(&push_back, &v, _1)
        );

    mpl::for_each< numbers >(value_printer(std::cout));

    for (unsigned i = 0; i < v.size(); ++i)
        assert(v[i] == (int)i);

    return 0;
}


/***
 * Functional tests for the order book
 */
#define BOOST_TEST_MODULE orderbook
#include <boost/test/included/unit_test.hpp>

#include <book.hpp>

BOOST_AUTO_TEST_CASE( simple_tests )
{
   // two assets
   Asset asset1(1);
   Asset asset2(2);

   Book<int, double> book(asset1, asset2);
   // add something to the order book
   AssetOrder<int, double> order1(1, asset2, asset1, 10, 10);
   book.AddOrder(order1);
   BOOST_TEST( book.bids.size() == 1);
   BOOST_TEST( book.asks.size() == 0);
   // take half of the order
   AssetOrder<int, double> order2(2, asset1, asset2, 5, 10);
   book.AddOrder(order2);
   BOOST_TEST( book.bids.size() == 1);
   BOOST_TEST( book.asks.size() == 0);
   // take the rest of the order
   AssetOrder<int, double> order3(2, asset1, asset2, 5, 10);
   book.AddOrder(order3);
   BOOST_TEST( book.bids.size() == 0);
   BOOST_TEST( book.asks.size() == 0);
   // now let 2 bids sit on the books and 1 big ask swallow them up
   order1 = AssetOrder<int, double>(1, asset2, asset1, 10, 10);
   book.AddOrder(order1);
   order2 = AssetOrder<int, double>(2, asset2, asset1, 10, 7.5);
   book.AddOrder(order2);
   order3 = AssetOrder<int, double>(3, asset1, asset2, 20, 7.5);
   book.AddOrder(order3);
   BOOST_TEST( book.bids.size() == 0);
   BOOST_TEST( book.asks.size() == 0);
}

BOOST_AUTO_TEST_CASE( multiple_order_same_price )
{
   // Two assets
   Asset a1(1);
   Asset a2(2);

   Book<int, int> book(a1, a2);
   AssetOrder<int, int> order1(1, a2, a1, 10, 10);
   AssetOrder<int, int> order2(2, a2, a1, 10, 10);
   book.AddOrder(order2);
   book.AddOrder(order1);
   BOOST_TEST( book.bids.size() == 2);
   // Even though I added them in the wrong order, they should come out in the correct order
   auto bidItr = book.bids.begin();
   auto bidKey = (*bidItr).first;
   BOOST_TEST(bidKey.id == 1);
   // If I add one at a higher price, it should become the first
   AssetOrder<int, int> order3(3, a2, a1, 10, 11);
   book.AddOrder(order3);
   BOOST_TEST( (*book.bids.begin()).first.id == 3 );

   // now check the asks
   order1 = AssetOrder<int, int>(1, a1, a2, 10, 20);
   order2 = AssetOrder<int, int>(2, a1, a2, 10, 20);
   order3 = AssetOrder<int, int>(3, a1, a2, 10, 19);
   book.AddOrder(order2);
   book.AddOrder(order1);
   BOOST_TEST( book.asks.size() == 2);
   BOOST_TEST( (*book.asks.begin()).first.id == 1);
   book.AddOrder(order3);
   BOOST_TEST( (*book.asks.begin()).first.id == 3);

}


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

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/is_ngez.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/quarter.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/true.hpp>
#include <boost/simd/constant/false.hpp>
#include <boost/simd/logical.hpp>

STF_CASE_TPL (" is_ngez  _real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_ngez;
  using r_t = decltype(is_ngez(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(is_ngez(bs::Inf<T>()), r_t(false));
  STF_EQUAL(is_ngez(bs::Minf<T>()), r_t(true));
  STF_EQUAL(is_ngez(bs::Nan<T>()), r_t(true));
#endif
  STF_EQUAL(is_ngez(bs::Mzero<T>()), r_t(false));
  STF_EQUAL(is_ngez(bs::Half<T>()), r_t(false));
  STF_EQUAL(is_ngez(bs::Mone<T>()), r_t(true));
  STF_EQUAL(is_ngez(bs::One<T>()), r_t(false));
  STF_EQUAL(is_ngez(bs::Quarter<T>()), r_t(false));
  STF_EQUAL(is_ngez(bs::Two<T>()), r_t(false));
  STF_EQUAL(is_ngez(bs::Zero<T>()), r_t(false));
}

STF_CASE_TPL (" is_ngez _signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_ngez;
   using r_t = decltype(is_ngez(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_ngez(bs::Mone<T>()), r_t(true));
  STF_EQUAL(is_ngez(bs::One<T>()), r_t(false));
  STF_EQUAL(is_ngez(bs::Two<T>()), r_t(false));
  STF_EQUAL(is_ngez(bs::Zero<T>()), r_t(false));
}

STF_CASE_TPL (" is_ngez _unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_ngez;
  using r_t = decltype(is_ngez(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_ngez(bs::One<T>()), r_t(false));
  STF_EQUAL(is_ngez(bs::Two<T>()), r_t(false));
  STF_EQUAL(is_ngez(bs::Zero<T>()), r_t(false));
}

STF_CASE ( "is_ngez _bool")
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_ngez;
  using r_t = decltype(is_ngez(bool()));

  // return type conformity test
  STF_TYPE_IS(r_t, bool);

  // specific values tests
  STF_EQUAL(is_ngez(true), false);
  STF_EQUAL(is_ngez(false), false);
}
STF_CASE_TPL ( "is_ngez _logical", STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_ngez;
  using lT = bs::logical<T>;
  using r_t = decltype(is_ngez(lT()));

  // return type conformity test
  STF_TYPE_IS(r_t, lT);

  // specific values tests
  STF_EQUAL(is_ngez(bs::True<lT>()), bs::False<lT>());
  STF_EQUAL(is_ngez(bs::False<lT>()), bs::False<lT>());
}


/*******************************************************************************
 *         Copyright 2003-2012 LASMEA UMR 6602 CNRS/U.B.P
 *         Copyright 2011-2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
/*!
 * \file
**/
#ifndef NT2_TOOLBOX_STATISTICS_FUNCTIONS_NORMRND_HPP_INCLUDED
#define NT2_TOOLBOX_STATISTICS_FUNCTIONS_NORMRND_HPP_INCLUDED
#include <nt2/include/functor.hpp>
#include <nt2/core/container/dsl/details/generative.hpp>
#include <nt2/core/container/dsl/generator.hpp>
#include <nt2/sdk/meta/constant_adaptor.hpp>
#include <nt2/sdk/meta/generative_hierarchy.hpp>
#include <nt2/include/functor.hpp>

#include <nt2/sdk/parameters.hpp>
#include <boost/preprocessor/repetition/repeat_from_to.hpp>
#include <boost/preprocessor/arithmetic/inc.hpp>

/*!
 * \ingroup statistics
 * \defgroup statistics_norm normrnd
 *
 * \par Description
 * normal pseudo random numbers generator
 * \par
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/normrnd.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace nt2
 * {
 *   template <class A0>
 *     meta::call<tag::normrnd_(A0)>::type
 *     normrnd(const A1 &m, const A2 & sig, ... dimension(s));
 * }
 * \endcode
 *
**/

namespace nt2 { namespace tag
  {
    /*!
     * \brief Define the tag normrnd_ of functor normrnd
     *        in namespace nt2::tag for toolbox statistics
    **/
    struct normrnd_ : ext::unspecified_<normrnd_> { typedef ext::unspecified_<normrnd_> parent; };
  }

 NT2_FUNCTION_IMPLEMENTATION(nt2::tag::normrnd_, normrnd, 2)
 NT2_FUNCTION_IMPLEMENTATION(nt2::tag::normrnd_, normrnd, 3)
 NT2_FUNCTION_IMPLEMENTATION(nt2::tag::normrnd_, normrnd, 4)
 NT2_FUNCTION_IMPLEMENTATION(nt2::tag::normrnd_, normrnd, 5)
 NT2_FUNCTION_IMPLEMENTATION(nt2::tag::normrnd_, normrnd, 6)
 NT2_FUNCTION_IMPLEMENTATION(nt2::tag::normrnd_, normrnd, 7)
}

#endif

// /////////////////////////////////////////////////////////////////////////////
// End of normrnd.hpp
// /////////////////////////////////////////////////////////////////////////////


#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>

#include <dlib/image_io.h>
#include <dlib/opencv.h>

#include <FDRV/image_handling.hpp>

void get_splits(cv::Mat original, std::vector<ImageSplit> &splits, const int maxH, const int maxW)
{
	int height = original.rows, width = original.cols;
	int num_divisionsH = 0, num_divisionsW = 0;

	while (height > maxH)
	{
		num_divisionsH++;
		height /= 2;
	}
	while (width > maxW)
	{
		num_divisionsW++;
		width /= 2;
	}
	height = height % 2 == 0 ? height : height - (height % 2);
	width = width % 2 == 0 ? width : width - (width % 2);

	//cout << "NumDW - " << num_divisionsW * 2 << " NumDH - " << num_divisionsH * 2 << endl;
	if (num_divisionsH == 0 && num_divisionsW == 0)
	{
		splits.push_back(ImageSplit(0, 0, original));
	}
	else if (num_divisionsH && num_divisionsW)
	{
		int y = 0;
		for (int i = 0; i < num_divisionsH * 2; i++)
		{
			int x = 0;
			for (int j = 0; j < num_divisionsW * 2; j++)
			{
				splits.push_back(ImageSplit(width * j, height * i, original(cv::Rect(x, y, width, height))));
				x += width;
			}
			y += height;
		}
	}
	else if (num_divisionsH)
	{
		int y = 0;
		for (int i = 0; i < num_divisionsH * 2; i++)
		{
			splits.push_back(ImageSplit(0, height * i, original(cv::Rect(0, y, width, height))));
			y += height;
		}
	}
	else if (num_divisionsW)
	{
		int x = 0;
		for (int j = 0; j < num_divisionsW * 2; j++)
		{
			splits.push_back(ImageSplit(width * j, 0, original(cv::Rect(x, 0, width, height))));

			x += width;
		}
	}
}

void cv2Dlib(const cv::Mat source, dlib::matrix<dlib::rgb_pixel> *target)
{
	dlib::cv_image<dlib::bgr_pixel> image(source);

	target->set_size(image.nr(), image.nc());
	//cout << "Starting convertion" << endl;
	std::vector<thread_data> t_data;
	//cout << "Creating threads" << endl;
	/*
		struct thread_data
		{
			dlib::matrix<dlib::rgb_pixel> *target;
			dlib::cv_image<dlib::bgr_pixel> *source;
			int id;
			dlib::mutex *count_mutex;
			dlib::signaler *count_signaler;//(count_mutex);
			int thread_count;
		};
	*/
	int thread_ammount = std::thread::hardware_concurrency() > 4 ? 4 : std::thread::hardware_concurrency();
	dlib::mutex t_mutex;
	dlib::signaler count_signaler(t_mutex);
	for (int t_id = 0; t_id < thread_ammount; t_id++)
	{
		thread_data data;
		data.target = target;
		data.source = image;
		data.id = t_id;
		data.thread_count = &thread_ammount;
		data.mutex = &t_mutex;
		data.count_signaler = &count_signaler;

		t_data.push_back(data);
		dlib::create_new_thread(threaded_copy, (void *)&t_data[t_id]);
	}

	dlib::auto_mutex abcd(t_mutex);
	while (thread_ammount > 0)
	{
		count_signaler.wait();
	}
}

void threaded_copy(void *arg)
{
	thread_data *data = (thread_data *)arg;

	int start = data->id;
	int jump = std::thread::hardware_concurrency() > 4 ? 4 : std::thread::hardware_concurrency();
	//std::cout << start << std::endl;
	//std::cout << "Jump " << jump << std::endl;
	for (long r = start; r < data->source.nr(); r += jump)
	{
		for (long c = 0; c < data->source.nc(); c++)
		{
			dlib::assign_pixel((*data->target)(r, c), data->source(r, c));
		}
	}
	//cout << "Done my job -- closing" << endl;
	dlib::auto_mutex locker(*data->mutex);
	*data->thread_count = *data->thread_count - 1;
	// Now we signal this change.  This will cause one thread that is currently waiting
	// on a call to count_signaler.wait() to unblock.
	data->count_signaler->signal();
}