// Copyright (c) 2011-2016 The Helveticum Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.
#include "arith_uint256.h"
#include "uint256.h"
#include "version.h"
#include "test/test_helveticum.h"

#include <boost/test/unit_test.hpp>
#include <stdint.h>
#include <sstream>
#include <iomanip>
#include <limits>
#include <cmath>
#include <string>
#include <stdio.h>

BOOST_FIXTURE_TEST_SUITE(uint256_tests, BasicTestingSetup)

const unsigned char R1Array[] =
    "\x9c\x52\x4a\xdb\xcf\x56\x11\x12\x2b\x29\x12\x5e\x5d\x35\xd2\xd2"
    "\x22\x81\xaa\xb5\x33\xf0\x08\x32\xd5\x56\xb1\xf9\xea\xe5\x1d\x7d";
const char R1ArrayHex[] = "7D1DE5EAF9B156D53208F033B5AA8122D2d2355d5e12292b121156cfdb4a529c";
const uint256 R1L = uint256(std::vector<unsigned char>(R1Array,R1Array+32));
const uint160 R1S = uint160(std::vector<unsigned char>(R1Array,R1Array+20));

const unsigned char R2Array[] =
    "\x70\x32\x1d\x7c\x47\xa5\x6b\x40\x26\x7e\x0a\xc3\xa6\x9c\xb6\xbf"
    "\x13\x30\x47\xa3\x19\x2d\xda\x71\x49\x13\x72\xf0\xb4\xca\x81\xd7";
const uint256 R2L = uint256(std::vector<unsigned char>(R2Array,R2Array+32));
const uint160 R2S = uint160(std::vector<unsigned char>(R2Array,R2Array+20));

const unsigned char ZeroArray[] =
    "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"
    "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00";
const uint256 ZeroL = uint256(std::vector<unsigned char>(ZeroArray,ZeroArray+32));
const uint160 ZeroS = uint160(std::vector<unsigned char>(ZeroArray,ZeroArray+20));

const unsigned char OneArray[] =
    "\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"
    "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00";
const uint256 OneL = uint256(std::vector<unsigned char>(OneArray,OneArray+32));
const uint160 OneS = uint160(std::vector<unsigned char>(OneArray,OneArray+20));

const unsigned char MaxArray[] =
    "\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff"
    "\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff";
const uint256 MaxL = uint256(std::vector<unsigned char>(MaxArray,MaxArray+32));
const uint160 MaxS = uint160(std::vector<unsigned char>(MaxArray,MaxArray+20));

std::string ArrayToString(const unsigned char A[], unsigned int width)
{
    std::stringstream Stream;
    Stream << std::hex;
    for (unsigned int i = 0; i < width; ++i)
    {
        Stream<<std::setw(2)<<std::setfill('0')<<(unsigned int)A[width-i-1];
    }
    return Stream.str();
}

inline uint160 uint160S(const char *str)
{
    uint160 rv;
    rv.SetHex(str);
    return rv;
}
inline uint160 uint160S(const std::string& str)
{
    uint160 rv;
    rv.SetHex(str);
    return rv;
}

BOOST_AUTO_TEST_CASE( basics ) // constructors, equality, inequality
{
    BOOST_CHECK(1 == 0+1);
    // constructor uint256(vector<char>):
    BOOST_CHECK(R1L.ToString() == ArrayToString(R1Array,32));
    BOOST_CHECK(R1S.ToString() == ArrayToString(R1Array,20));
    BOOST_CHECK(R2L.ToString() == ArrayToString(R2Array,32));
    BOOST_CHECK(R2S.ToString() == ArrayToString(R2Array,20));
    BOOST_CHECK(ZeroL.ToString() == ArrayToString(ZeroArray,32));
    BOOST_CHECK(ZeroS.ToString() == ArrayToString(ZeroArray,20));
    BOOST_CHECK(OneL.ToString() == ArrayToString(OneArray,32));
    BOOST_CHECK(OneS.ToString() == ArrayToString(OneArray,20));
    BOOST_CHECK(MaxL.ToString() == ArrayToString(MaxArray,32));
    BOOST_CHECK(MaxS.ToString() == ArrayToString(MaxArray,20));
    BOOST_CHECK(OneL.ToString() != ArrayToString(ZeroArray,32));
    BOOST_CHECK(OneS.ToString() != ArrayToString(ZeroArray,20));

    // == and !=
    BOOST_CHECK(R1L != R2L && R1S != R2S);
    BOOST_CHECK(ZeroL != OneL && ZeroS != OneS);
    BOOST_CHECK(OneL != ZeroL && OneS != ZeroS);
    BOOST_CHECK(MaxL != ZeroL && MaxS != ZeroS);

    // String Constructor and Copy Constructor
    BOOST_CHECK(uint256S("0x"+R1L.ToString()) == R1L);
    BOOST_CHECK(uint256S("0x"+R2L.ToString()) == R2L);
    BOOST_CHECK(uint256S("0x"+ZeroL.ToString()) == ZeroL);
    BOOST_CHECK(uint256S("0x"+OneL.ToString()) == OneL);
    BOOST_CHECK(uint256S("0x"+MaxL.ToString()) == MaxL);
    BOOST_CHECK(uint256S(R1L.ToString()) == R1L);
    BOOST_CHECK(uint256S("   0x"+R1L.ToString()+"   ") == R1L);
    BOOST_CHECK(uint256S("") == ZeroL);
    BOOST_CHECK(R1L == uint256S(R1ArrayHex));
    BOOST_CHECK(uint256(R1L) == R1L);
    BOOST_CHECK(uint256(ZeroL) == ZeroL);
    BOOST_CHECK(uint256(OneL) == OneL);

    BOOST_CHECK(uint160S("0x"+R1S.ToString()) == R1S);
    BOOST_CHECK(uint160S("0x"+R2S.ToString()) == R2S);
    BOOST_CHECK(uint160S("0x"+ZeroS.ToString()) == ZeroS);
    BOOST_CHECK(uint160S("0x"+OneS.ToString()) == OneS);
    BOOST_CHECK(uint160S("0x"+MaxS.ToString()) == MaxS);
    BOOST_CHECK(uint160S(R1S.ToString()) == R1S);
    BOOST_CHECK(uint160S("   0x"+R1S.ToString()+"   ") == R1S);
    BOOST_CHECK(uint160S("") == ZeroS);
    BOOST_CHECK(R1S == uint160S(R1ArrayHex));

    BOOST_CHECK(uint160(R1S) == R1S);
    BOOST_CHECK(uint160(ZeroS) == ZeroS);
    BOOST_CHECK(uint160(OneS) == OneS);
}

BOOST_AUTO_TEST_CASE( comparison ) // <= >= < >
{
    uint256 LastL;
    for (int i = 255; i >= 0; --i) {
        uint256 TmpL;
        *(TmpL.begin() + (i>>3)) |= 1<<(7-(i&7));
        BOOST_CHECK( LastL < TmpL );
        LastL = TmpL;
    }

    BOOST_CHECK( ZeroL < R1L );
    BOOST_CHECK( R2L < R1L );
    BOOST_CHECK( ZeroL < OneL );
    BOOST_CHECK( OneL < MaxL );
    BOOST_CHECK( R1L < MaxL );
    BOOST_CHECK( R2L < MaxL );

    uint160 LastS;
    for (int i = 159; i >= 0; --i) {
        uint160 TmpS;
        *(TmpS.begin() + (i>>3)) |= 1<<(7-(i&7));
        BOOST_CHECK( LastS < TmpS );
        LastS = TmpS;
    }
    BOOST_CHECK( ZeroS < R1S );
    BOOST_CHECK( R2S < R1S );
    BOOST_CHECK( ZeroS < OneS );
    BOOST_CHECK( OneS < MaxS );
    BOOST_CHECK( R1S < MaxS );
    BOOST_CHECK( R2S < MaxS );
}

BOOST_AUTO_TEST_CASE( methods ) // GetHex SetHex begin() end() size() GetLow64 GetSerializeSize, Serialize, Unserialize
{
    BOOST_CHECK(R1L.GetHex() == R1L.ToString());
    BOOST_CHECK(R2L.GetHex() == R2L.ToString());
    BOOST_CHECK(OneL.GetHex() == OneL.ToString());
    BOOST_CHECK(MaxL.GetHex() == MaxL.ToString());
    uint256 TmpL(R1L);
    BOOST_CHECK(TmpL == R1L);
    TmpL.SetHex(R2L.ToString());   BOOST_CHECK(TmpL == R2L);
    TmpL.SetHex(ZeroL.ToString()); BOOST_CHECK(TmpL == uint256());

    TmpL.SetHex(R1L.ToString());
    BOOST_CHECK(memcmp(R1L.begin(), R1Array, 32)==0);
    BOOST_CHECK(memcmp(TmpL.begin(), R1Array, 32)==0);
    BOOST_CHECK(memcmp(R2L.begin(), R2Array, 32)==0);
    BOOST_CHECK(memcmp(ZeroL.begin(), ZeroArray, 32)==0);
    BOOST_CHECK(memcmp(OneL.begin(), OneArray, 32)==0);
    BOOST_CHECK(R1L.size() == sizeof(R1L));
    BOOST_CHECK(sizeof(R1L) == 32);
    BOOST_CHECK(R1L.size() == 32);
    BOOST_CHECK(R2L.size() == 32);
    BOOST_CHECK(ZeroL.size() == 32);
    BOOST_CHECK(MaxL.size() == 32);
    BOOST_CHECK(R1L.begin() + 32 == R1L.end());
    BOOST_CHECK(R2L.begin() + 32 == R2L.end());
    BOOST_CHECK(OneL.begin() + 32 == OneL.end());
    BOOST_CHECK(MaxL.begin() + 32 == MaxL.end());
    BOOST_CHECK(TmpL.begin() + 32 == TmpL.end());
    BOOST_CHECK(GetSerializeSize(R1L, 0, PROTOCOL_VERSION) == 32);
    BOOST_CHECK(GetSerializeSize(ZeroL, 0, PROTOCOL_VERSION) == 32);

    CDataStream ss(0, PROTOCOL_VERSION);
    ss << R1L;
    BOOST_CHECK(ss.str() == std::string(R1Array,R1Array+32));
    ss >> TmpL;
    BOOST_CHECK(R1L == TmpL);
    ss.clear();
    ss << ZeroL;
    BOOST_CHECK(ss.str() == std::string(ZeroArray,ZeroArray+32));
    ss >> TmpL;
    BOOST_CHECK(ZeroL == TmpL);
    ss.clear();
    ss << MaxL;
    BOOST_CHECK(ss.str() == std::string(MaxArray,MaxArray+32));
    ss >> TmpL;
    BOOST_CHECK(MaxL == TmpL);
    ss.clear();

    BOOST_CHECK(R1S.GetHex() == R1S.ToString());
    BOOST_CHECK(R2S.GetHex() == R2S.ToString());
    BOOST_CHECK(OneS.GetHex() == OneS.ToString());
    BOOST_CHECK(MaxS.GetHex() == MaxS.ToString());
    uint160 TmpS(R1S);
    BOOST_CHECK(TmpS == R1S);
    TmpS.SetHex(R2S.ToString());   BOOST_CHECK(TmpS == R2S);
    TmpS.SetHex(ZeroS.ToString()); BOOST_CHECK(TmpS == uint160());

    TmpS.SetHex(R1S.ToString());
    BOOST_CHECK(memcmp(R1S.begin(), R1Array, 20)==0);
    BOOST_CHECK(memcmp(TmpS.begin(), R1Array, 20)==0);
    BOOST_CHECK(memcmp(R2S.begin(), R2Array, 20)==0);
    BOOST_CHECK(memcmp(ZeroS.begin(), ZeroArray, 20)==0);
    BOOST_CHECK(memcmp(OneS.begin(), OneArray, 20)==0);
    BOOST_CHECK(R1S.size() == sizeof(R1S));
    BOOST_CHECK(sizeof(R1S) == 20);
    BOOST_CHECK(R1S.size() == 20);
    BOOST_CHECK(R2S.size() == 20);
    BOOST_CHECK(ZeroS.size() == 20);
    BOOST_CHECK(MaxS.size() == 20);
    BOOST_CHECK(R1S.begin() + 20 == R1S.end());
    BOOST_CHECK(R2S.begin() + 20 == R2S.end());
    BOOST_CHECK(OneS.begin() + 20 == OneS.end());
    BOOST_CHECK(MaxS.begin() + 20 == MaxS.end());
    BOOST_CHECK(TmpS.begin() + 20 == TmpS.end());
    BOOST_CHECK(GetSerializeSize(R1S, 0, PROTOCOL_VERSION) == 20);
    BOOST_CHECK(GetSerializeSize(ZeroS, 0, PROTOCOL_VERSION) == 20);

    ss << R1S;
    BOOST_CHECK(ss.str() == std::string(R1Array,R1Array+20));
    ss >> TmpS;
    BOOST_CHECK(R1S == TmpS);
    ss.clear();
    ss << ZeroS;
    BOOST_CHECK(ss.str() == std::string(ZeroArray,ZeroArray+20));
    ss >> TmpS;
    BOOST_CHECK(ZeroS == TmpS);
    ss.clear();
    ss << MaxS;
    BOOST_CHECK(ss.str() == std::string(MaxArray,MaxArray+20));
    ss >> TmpS;
    BOOST_CHECK(MaxS == TmpS);
    ss.clear();
}

BOOST_AUTO_TEST_CASE( conversion )
{
    BOOST_CHECK(ArithToUint256(UintToArith256(ZeroL)) == ZeroL);
    BOOST_CHECK(ArithToUint256(UintToArith256(OneL)) == OneL);
    BOOST_CHECK(ArithToUint256(UintToArith256(R1L)) == R1L);
    BOOST_CHECK(ArithToUint256(UintToArith256(R2L)) == R2L);
    BOOST_CHECK(UintToArith256(ZeroL) == 0);
    BOOST_CHECK(UintToArith256(OneL) == 1);
    BOOST_CHECK(ArithToUint256(0) == ZeroL);
    BOOST_CHECK(ArithToUint256(1) == OneL);
    BOOST_CHECK(arith_uint256(R1L.GetHex()) == UintToArith256(R1L));
    BOOST_CHECK(arith_uint256(R2L.GetHex()) == UintToArith256(R2L));
    BOOST_CHECK(R1L.GetHex() == UintToArith256(R1L).GetHex());
    BOOST_CHECK(R2L.GetHex() == UintToArith256(R2L).GetHex());
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_LIBC_FUNCTIONS_SCALAR_FREXP_HPP_INCLUDED
#define NT2_TOOLBOX_LIBC_FUNCTIONS_SCALAR_FREXP_HPP_INCLUDED
#include <nt2/toolbox/libc/functions/frexp.hpp>
#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/include/at.hpp>
#include <nt2/sdk/meta/adapted_traits.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <cmath>

namespace nt2 { namespace ext
{
/////////////////////////////////////////////////////////////////////////////
// Implementation when type is single_
/////////////////////////////////////////////////////////////////////////////
NT2_FUNCTOR_IMPLEMENTATION(nt2::libc::tag::frexp_, tag::cpu_,
			   (A0)(A1),
			   (scalar_<single_<A0> >)
			   (scalar_<int32_<A1> >)
			   )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A1 & a1) const
    {
      return ::frexpf(a0, &a1);
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type is double_
/////////////////////////////////////////////////////////////////////////////
  NT2_FUNCTOR_IMPLEMENTATION(nt2::libc::tag::frexp_, tag::cpu_,
			     (A0)(A1),
			     (scalar_<double_<A0> >)
			     (scalar_<int32_<A1> >)
			     )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A1 & a1) const
    {
      return ::frexp(a0, &a1);
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is floating_
/////////////////////////////////////////////////////////////////////////////

  NT2_FUNCTOR_IMPLEMENTATION(nt2::libc::tag::frexp_, tag::cpu_,
                       (A0),
                       (scalar_<floating_<A0> >)
                      )
  {
    typedef boost::fusion::vector<A0,nt2::int32_t>             result_type;

    NT2_FUNCTOR_CALL(1)
    {
      result_type res;
      boost::fusion::at_c<0>(res) =nt2::libc::frexp(a0, boost::fusion::at_c<1>(res));
      return res;
    }
  };

} }
#endif


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_MESHDATA_HPP
#define MCL_MESHDATA_HPP 1

#include "Singleton.hpp"
#include <Eigen/Geometry>
#include <vector>
#include <unordered_map>

namespace mcl
{

//
// Stores all of the input mesh information.
// All const accessors are thread safe.
// Any non-const functions are not guaranteed to be.
//
class MeshData : public Singleton<MeshData>
{
public:
	typedef Eigen::Matrix<int,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> RowMatrixXi;
	typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> RowMatrixXd;

	MeshData() { clear(); }

	// Prints details about mesh
	std::string print_stats() const;

	// Resets all mesh data
	void clear();

	int dim() const { return mDim; }
	int num_meshes() const { return std::max(0, (int)m_V_offsets.size()-1); }

	// Computes mesh quantities, connectivities, etc...
	void initialize();
	bool needs_initialize() const { return needs_init; }

	// General queries
	bool is_texture_param() const;
	bool is_dynamic_vert(int idx) const;
	int mesh_idx_from_element(int elem_idx) const;

	// Get mesh features
	const RowMatrixXi& get_faces() const; // 2D = same as elements, 3D = same as facets
	const RowMatrixXi& get_boundary_edges() const; // same as facets if 2D
	const Eigen::VectorXi& get_boundary_verts() const;
	const RowMatrixXi& get_elements() const { return mP; } // 2D = triangles, 3D = tets
	const RowMatrixXi& get_facets() const { return mF; } // boundary, 2D = edges, 3D = triangles
	Eigen::Vector2i get_adjacent_faces(int e0, int e1) const; // returns faces of edge (-1 if not found)

	// Get mesh quantities
	const Eigen::VectorXd& get_mesh_measures() const; // per-mesh total area or volume
	const Eigen::VectorXd& get_measures() const; // per-element 2D=area, 3D=volume
	void update_measure(int idx, double m); // change per-element measure
	const Eigen::VectorXd& get_scalings() const; // per-element 2D=inv perim, 3D=inv surface area
	const std::vector<std::vector<int> >& get_graph() const; // per-vertex non-directed connectivity of elems

	// Initializer is the starting point of the optimization.
	// If not set before optimization starts, rest shape is used.
	const RowMatrixXd& get_initializer() const;
	void set_initializer(const RowMatrixXd& V) { mVinit = V; }

	// Rest shape defines the energy of the mesh.
	// By default the rest shape is whatever is passed to add_mesh
	const RowMatrixXd& get_rest() const { return mV; }
	void set_rest(const RowMatrixXd& V);

	// Returns offsets [start, end] for elements of the stacked matrix. E.g.,
	// mesh0 = E.block(offsets[0], 0, offsets[1], dim+1)
	// mesh1 = E.block(offsets[1], 0, offsets[2], dim+1)
	// etc...
	const Eigen::VectorXi& get_element_offsets() const { return m_P_offsets; }
	const Eigen::VectorXi& get_vertex_offsets() const { return m_V_offsets; }
	const Eigen::VectorXi& get_facet_offsets() const { return m_F_offsets; }
	const Eigen::VectorXi& get_face_offsets() const;

	// Handy hash map to get the feature indices. Returns -1 if not found.
	// Winding order does not matter.
	int get_face_index(int f0, int f1, int f2) const;
	int get_tet_index(int t0, int t1, int t2, int t3) const;

	// Returns index of the mesh, or -1 if there was an error.
	// If init=true, initialize() is called immediately which can be slow for many large meshes.
	int add_mesh(const RowMatrixXd &V, const RowMatrixXi &P, bool init=true); // auto computes faces
	int add_mesh(const RowMatrixXd &V, const RowMatrixXi &P, const RowMatrixXi &F, bool init=true);

	// Special case: adds a mesh to be parameterized.
	// In this case, the 2D texture coords are the dynamic variable (initializer) and
	// the 3D mesh is the rest mesh (incl. rest_volume=area, rest_surf_area=perimeter, etc...).
	// The input mesh must be a 3D triangular mesh.
	// If UVinit is not given, one is computed (Tutte).
	// TODO: Some validation and error checking on compatibility.
	int add_parameterized_mesh(const RowMatrixXd &V, const RowMatrixXi &F,
		const RowMatrixXd &UVinit = RowMatrixXd(), bool init=true);

 	// Computes (volume-weighted) masses with unit-volume density.
	// If negative, defaults are used: 1100 for volumetric, 0.4 for cloth/2D
	// See: https://www.engineeringtoolbox.com/density-solids-d_1265.html
	// Masses are not needed for static solves.
	// If masses are not computed/set, an empty vector is returned
	void compute_masses(double density_kg = -1);
	void set_masses(const Eigen::VectorXd &m) { m_masses=m; }
	const Eigen::VectorXd& get_masses() const { return m_masses; }
	double get_rest_radius() const;

	// Serialize
	template<class Archive> void save(Archive& archive) const;
	template<class Archive> void load(Archive& archive);

protected:

	// Edges if 2D, triangles if 3D
	void compute_facets(const RowMatrixXi &P, RowMatrixXi &F);

	// Boundary edges, vertices, etc.
	void init_boundary_mappings();

	// Updates per-element internal data
	void init_per_element_data();

	// Mappings to edge/face/etc...
	void init_feature_mapping();

	int mDim;
	bool needs_init;

	// Dynamic:
	RowMatrixXd mV; // rest vertices
	RowMatrixXd mVinit; // initializer
	RowMatrixXi mP; // (internal) elements
	RowMatrixXi mF; // facets
	Eigen::VectorXd m_masses; // per-vertex masses
	Eigen::VectorXd m_rest_bmin_mbax; // [bmin, bmax]

	// Stacked (computed) quantities [dynamic, kinematic]
	// Most are computed on initialize
	Eigen::VectorXd m_mesh_measures; // mesh-measure
	Eigen::VectorXi m_V_offsets; // indices into stacked V
	Eigen::VectorXi m_P_offsets; // indices into stacked P
	Eigen::VectorXi m_F_offsets; // indices into stacked F
	Eigen::VectorXd m_measures; // rest volume/area
	Eigen::VectorXd m_scalings; // inv surface areas/perimeter
	RowMatrixXi m_bnd_edges; // boundary edges
	Eigen::VectorXi m_bnd_verts; // boundary vertices

	// Mappings
	std::vector<std::vector<int> > m_V_graph; // vertex-to-vertex connectivity
	std::unordered_map<std::string, int> m_faces_map; // inds to face, [f0,f1,f2] -> f, f0<f1<f2
	std::unordered_map<std::string, int> m_tets_map; // inds to tet, [t0,t1,...] -> t, t0<t1<...
	std::unordered_map<std::string, Eigen::Vector2i> m_E_to_F; // adj faces, [e0,e1] -> [f0,f1]

}; // class MeshData

} // ns mcl

#endif


/**
 * @file math_test.cpp
 * @author Ryan Curtin
 *
 * Tests for everything in the math:: namespace.
 */
#include <mlpack/core/math/clamp.hpp>
#include <mlpack/core/math/random.hpp>
#include <mlpack/core/math/range.hpp>
#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace mlpack;
using namespace math;

BOOST_AUTO_TEST_SUITE(MathTest);

/**
 * Verify that the empty constructor creates an empty range.
 */
BOOST_AUTO_TEST_CASE(RangeEmptyConstructor)
{
  Range x = Range();

  // Just verify that it is empty.
  BOOST_REQUIRE_GT(x.Lo(), x.Hi());
}

/**
 * Verify that the point constructor correctly creates a range that is just a
 * point.
 */
BOOST_AUTO_TEST_CASE(RangePointConstructor)
{
  Range x(10.0);

  BOOST_REQUIRE_CLOSE(x.Lo(), x.Hi(), 1e-25);
  BOOST_REQUIRE_SMALL(x.Width(), 1e-5);
  BOOST_REQUIRE_CLOSE(x.Lo(), 10.0, 1e-25);
  BOOST_REQUIRE_CLOSE(x.Hi(), 10.0, 1e-25);
}

/**
 * Verify that the range constructor correctly creates the range.
 */
BOOST_AUTO_TEST_CASE(RangeConstructor)
{
  Range x(0.5, 5.5);

  BOOST_REQUIRE_CLOSE(x.Lo(), 0.5, 1e-25);
  BOOST_REQUIRE_CLOSE(x.Hi(), 5.5, 1e-25);
}

/**
 * Test that we get the width correct.
 */
BOOST_AUTO_TEST_CASE(RangeWidth)
{
  Range x(0.0, 10.0);

  BOOST_REQUIRE_CLOSE(x.Width(), 10.0, 1e-20);

  // Make it empty.
  x.Hi() = 0.0;

  BOOST_REQUIRE_SMALL(x.Width(), 1e-5);

  // Make it negative.
  x.Hi() = -2.0;

  BOOST_REQUIRE_SMALL(x.Width(), 1e-5);

  // Just one more test.
  x.Lo() = -5.2;
  x.Hi() = 5.2;

  BOOST_REQUIRE_CLOSE(x.Width(), 10.4, 1e-5);
}

/**
 * Test that we get the midpoint correct.
 */
BOOST_AUTO_TEST_CASE(RangeMidpoint)
{
  Range x(0.0, 10.0);

  BOOST_REQUIRE_CLOSE(x.Mid(), 5.0, 1e-5);

  x.Lo() = -5.0;

  BOOST_REQUIRE_CLOSE(x.Mid(), 2.5, 1e-5);
}

/**
 * Test that we can expand to include other ranges correctly.
 */
BOOST_AUTO_TEST_CASE(RangeIncludeOther)
{
  // We need to test both |= and |.
  // We have three cases: non-overlapping; overlapping; equivalent, and then a
  // couple permutations (switch left with right and make sure it still works).
  Range x(0.0, 2.0);
  Range y(3.0, 5.0);

  Range z(0.0, 2.0); // Used for operator|=().
  Range w;
  z |= y;
  w = x | y;

  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 5.0, 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 5.0, 1e-5);

  // Switch operator precedence.
  z = y;
  z |= x;
  w = y | x;

  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 5.0, 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 5.0, 1e-5);

  // Now make them overlapping.
  x = Range(0.0, 3.5);
  y = Range(3.0, 4.0);

  z = x;
  z |= y;
  w = x | y;

  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 4.0, 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 4.0, 1e-5);

  // Switch operator precedence.
  z = y;
  z |= x;
  w = y | x;

  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 4.0, 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 4.0, 1e-5);

  // Now the equivalent case.
  x = Range(0.0, 2.0);
  y = Range(0.0, 2.0);

  z = x;
  z |= y;
  w = x | y;

  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 2.0, 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 2.0, 1e-5);

  z = y;
  z |= x;
  w = y | x;

  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 2.0, 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 2.0, 1e-5);
}

/**
 * Test that we can 'and' ranges correctly.
 */
BOOST_AUTO_TEST_CASE(RangeIntersectOther)
{
  // We need to test both &= and &.
  // We have three cases: non-overlapping, overlapping; equivalent, and then a
  // couple permutations (switch left with right and make sure it still works).
  Range x(0.0, 2.0);
  Range y(3.0, 5.0);

  Range z(0.0, 2.0);
  Range w;
  z &= y;
  w = x & y;

  BOOST_REQUIRE_SMALL(z.Width(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Width(), 1e-5);

  // Reverse operator precedence.
  z = y;
  z &= x;
  w = y & x;

  BOOST_REQUIRE_SMALL(z.Width(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Width(), 1e-5);

  // Now make them overlapping.
  x = Range(0.0, 3.5);
  y = Range(3.0, 4.0);

  z = x;
  z &= y;
  w = x & y;

  BOOST_REQUIRE_CLOSE(z.Lo(), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 3.5, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 3.5, 1e-5);

  // Reverse operator precedence.
  z = y;
  z &= x;
  w = y & x;

  BOOST_REQUIRE_CLOSE(z.Lo(), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 3.5, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 3.5, 1e-5);

  // Now make them equivalent.
  x = Range(2.0, 4.0);
  y = Range(2.0, 4.0);

  z = x;
  z &= y;
  w = x & y;

  BOOST_REQUIRE_CLOSE(z.Lo(), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 4.0, 1e-5);
}

/**
 * Test multiplication of a range with a double.
 */
BOOST_AUTO_TEST_CASE(RangeMultiply)
{
  // We need to test both * and *=, as well as both cases of *.
  // We'll try with a couple of numbers: -1, 0, 2.
  // And we'll have a couple of cases for bounds: strictly less than zero;
  // including zero; and strictly greater than zero.
  //
  // So, nine total cases.
  Range x(-5.0, -3.0);
  Range y(-5.0, -3.0);
  Range z;
  Range w;

  y *= -1.0;
  z = x * -1.0;
  w = -1.0 * x;

  BOOST_REQUIRE_CLOSE(y.Lo(), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(y.Hi(), 5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Lo(), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 5.0, 1e-5);

  y = x;
  y *= 0.0;
  z = x * 0.0;
  w = 0.0 * x;

  BOOST_REQUIRE_SMALL(y.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(y.Hi(), 1e-5);
  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(z.Hi(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Hi(), 1e-5);

  y = x;
  y *= 2.0;
  z = x * 2.0;
  w = 2.0 * x;

  BOOST_REQUIRE_CLOSE(y.Lo(), -10.0, 1e-5);
  BOOST_REQUIRE_CLOSE(y.Hi(), -6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Lo(), -10.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), -6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), -10.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), -6.0, 1e-5);

  x = Range(-2.0, 2.0);
  y = x;

  y *= -1.0;
  z = x * -1.0;
  w = -1.0 * x;

  BOOST_REQUIRE_CLOSE(y.Lo(), -2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(y.Hi(), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Lo(), -2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), -2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 2.0, 1e-5);

  y = x;
  y *= 0.0;
  z = x * 0.0;
  w = 0.0 * x;

  BOOST_REQUIRE_SMALL(y.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(y.Hi(), 1e-5);
  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(z.Hi(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Hi(), 1e-5);

  y = x;
  y *= 2.0;
  z = x * 2.0;
  w = 2.0 * x;

  BOOST_REQUIRE_CLOSE(y.Lo(), -4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(y.Hi(), 4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Lo(), -4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), -4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 4.0, 1e-5);

  x = Range(3.0, 5.0);

  y = x;
  y *= -1.0;
  z = x * -1.0;
  w = -1.0 * x;

  BOOST_REQUIRE_CLOSE(y.Lo(), -5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(y.Hi(), -3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Lo(), -5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), -3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), -5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), -3.0, 1e-5);

  y = x;
  y *= 0.0;
  z = x * 0.0;
  w = 0.0 * x;

  BOOST_REQUIRE_SMALL(y.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(y.Hi(), 1e-5);
  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(z.Hi(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Hi(), 1e-5);

  y = x;
  y *= 2.0;
  z = x * 2.0;
  w = 2.0 * x;

  BOOST_REQUIRE_CLOSE(y.Lo(), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(y.Hi(), 10.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Lo(), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 10.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 10.0, 1e-5);
}

/**
 * Test equality operator.
 */
BOOST_AUTO_TEST_CASE(RangeEquality)
{
  // Three cases: non-overlapping, overlapping, equivalent.  We should also
  // consider empty ranges, which are not necessarily equal...
  Range x(0.0, 2.0);
  Range y(3.0, 5.0);

  // These are odd calls, but we don't want to use operator!= here.
  BOOST_REQUIRE_EQUAL((x == y), false);
  BOOST_REQUIRE_EQUAL((y == x), false);

  y = Range(1.0, 3.0);

  BOOST_REQUIRE_EQUAL((x == y), false);
  BOOST_REQUIRE_EQUAL((y == x), false);

  y = Range(0.0, 2.0);

  BOOST_REQUIRE_EQUAL((x == y), true);
  BOOST_REQUIRE_EQUAL((y == x), true);

  x = Range(1.0, -1.0); // Empty.
  y = Range(1.0, -1.0); // Also empty.

  BOOST_REQUIRE_EQUAL((x == y), true);
  BOOST_REQUIRE_EQUAL((y == x), true);

  // No need to test what it does if the empty ranges are different "ranges"
  // because we are not forcing behavior for that.
}

/**
 * Test inequality operator.
 */
BOOST_AUTO_TEST_CASE(RangeInequality)
{
  // We will use the same three cases as the RangeEquality test.
  Range x(0.0, 2.0);
  Range y(3.0, 5.0);

  // Again, odd calls, but we want to force use of operator!=.
  BOOST_REQUIRE_EQUAL((x != y), true);
  BOOST_REQUIRE_EQUAL((y != x), true);

  y = Range(1.0, 3.0);

  BOOST_REQUIRE_EQUAL((x != y), true);
  BOOST_REQUIRE_EQUAL((y != x), true);

  y = Range(0.0, 2.0);

  BOOST_REQUIRE_EQUAL((x != y), false);
  BOOST_REQUIRE_EQUAL((y != x), false);

  x = Range(1.0, -1.0); // Empty.
  y = Range(1.0, -1.0); // Also empty.

  BOOST_REQUIRE_EQUAL((x != y), false);
  BOOST_REQUIRE_EQUAL((y != x), false);
}

/**
 * Test strict less-than operator.
 */
BOOST_AUTO_TEST_CASE(RangeStrictLessThan)
{
  // Three cases: non-overlapping, overlapping, and equivalent.
  Range x(0.0, 2.0);
  Range y(3.0, 5.0);

  BOOST_REQUIRE_EQUAL((x < y), true);
  BOOST_REQUIRE_EQUAL((y < x), false);

  y = Range(1.0, 3.0);

  BOOST_REQUIRE_EQUAL((x < y), false);
  BOOST_REQUIRE_EQUAL((y < x), false);

  y = Range(0.0, 2.0);

  BOOST_REQUIRE_EQUAL((x < y), false);
  BOOST_REQUIRE_EQUAL((y < x), false);
}

/**
 * Test strict greater-than operator.
 */
BOOST_AUTO_TEST_CASE(RangeStrictGreaterThan)
{
  // Three cases: non-overlapping, overlapping, and equivalent.
  Range x(0.0, 2.0);
  Range y(3.0, 5.0);

  BOOST_REQUIRE_EQUAL((x > y), false);
  BOOST_REQUIRE_EQUAL((y > x), true);

  y = Range(1.0, 3.0);

  BOOST_REQUIRE_EQUAL((x > y), false);
  BOOST_REQUIRE_EQUAL((y > x), false);

  y = Range(0.0, 2.0);

  BOOST_REQUIRE_EQUAL((x > y), false);
  BOOST_REQUIRE_EQUAL((y > x), false);
}

/**
 * Test the Contains() operator.
 */
BOOST_AUTO_TEST_CASE(RangeContains)
{
  // We have three Range cases: strictly less than 0; overlapping 0; and
  // strictly greater than 0.  Then the numbers we check can be the same three
  // cases, including one greater than and one less than the range.  This should
  // be about 15 total cases.
  Range x(-2.0, -1.0);

  BOOST_REQUIRE(!x.Contains(-3.0));
  BOOST_REQUIRE(x.Contains(-2.0));
  BOOST_REQUIRE(x.Contains(-1.5));
  BOOST_REQUIRE(x.Contains(-1.0));
  BOOST_REQUIRE(!x.Contains(-0.5));
  BOOST_REQUIRE(!x.Contains(0.0));
  BOOST_REQUIRE(!x.Contains(1.0));

  x = Range(-1.0, 1.0);

  BOOST_REQUIRE(!x.Contains(-2.0));
  BOOST_REQUIRE(x.Contains(-1.0));
  BOOST_REQUIRE(x.Contains(0.0));
  BOOST_REQUIRE(x.Contains(1.0));
  BOOST_REQUIRE(!x.Contains(2.0));

  x = Range(1.0, 2.0);

  BOOST_REQUIRE(!x.Contains(-1.0));
  BOOST_REQUIRE(!x.Contains(0.0));
  BOOST_REQUIRE(!x.Contains(0.5));
  BOOST_REQUIRE(x.Contains(1.0));
  BOOST_REQUIRE(x.Contains(1.5));
  BOOST_REQUIRE(x.Contains(2.0));
  BOOST_REQUIRE(!x.Contains(2.5));

  // Now let's try it on an empty range.
  x = Range();

  BOOST_REQUIRE(!x.Contains(-10.0));
  BOOST_REQUIRE(!x.Contains(0.0));
  BOOST_REQUIRE(!x.Contains(10.0));

  // And an infinite range.
  x = Range(-DBL_MAX, DBL_MAX);

  BOOST_REQUIRE(x.Contains(-10.0));
  BOOST_REQUIRE(x.Contains(0.0));
  BOOST_REQUIRE(x.Contains(10.0));
}

/**
 * Test that Range::Contains() works on other Ranges.  It should return false
 * unless the ranges overlap at all.
 */
BOOST_AUTO_TEST_CASE(RangeContainsRange)
{
  // Empty ranges should not contain each other.
  Range a;
  Range b;

  BOOST_REQUIRE_EQUAL(a.Contains(b), false);
  BOOST_REQUIRE_EQUAL(b.Contains(a), false);

  // Completely disparate ranges.
  a = Range(-5.0, -3.0);
  b = Range(3.0, 5.0);

  BOOST_REQUIRE_EQUAL(a.Contains(b), false);
  BOOST_REQUIRE_EQUAL(b.Contains(a), false);

  // Overlapping at the end-point; this is containment of the end point.
  a = Range(-5.0, 0.0);
  b = Range(0.0, 5.0);

  BOOST_REQUIRE_EQUAL(a.Contains(b), true);
  BOOST_REQUIRE_EQUAL(b.Contains(a), true);

  // Partially overlapping.
  a = Range(-5.0, 2.0);
  b = Range(-2.0, 5.0);

  BOOST_REQUIRE_EQUAL(a.Contains(b), true);
  BOOST_REQUIRE_EQUAL(b.Contains(a), true);

  // One range encloses the other.
  a = Range(-5.0, 5.0);
  b = Range(-3.0, 3.0);

  BOOST_REQUIRE_EQUAL(a.Contains(b), true);
  BOOST_REQUIRE_EQUAL(b.Contains(a), true);

  // Identical ranges.
  a = Range(-3.0, 3.0);
  b = Range(-3.0, 3.0);

  BOOST_REQUIRE_EQUAL(a.Contains(b), true);
  BOOST_REQUIRE_EQUAL(b.Contains(a), true);

  // Single-point ranges.
  a = Range(0.0, 0.0);
  b = Range(0.0, 0.0);

  BOOST_REQUIRE_EQUAL(a.Contains(b), true);
  BOOST_REQUIRE_EQUAL(b.Contains(a), true);
}

BOOST_AUTO_TEST_SUITE_END();


#include "include/MMVII_all.h"

#include <algorithm>
#include <tuple>
#include <typeinfo>
#include <forward_list>
#include <unordered_map>
#include <functional>

//#include <Eigen/Dense>

namespace MMVII
{

class cTestMMV2Obj : public cMemCheck
{
    public :
        virtual ~cTestMMV2Obj()
        {
           TheNbObj--;
        }
        static int NbObj() {return TheNbObj;}
    // protected :
        cTestMMV2Obj()
        {
           TheNbObj++;
        }
    private :
       static int TheNbObj;
};
int cTestMMV2Obj::TheNbObj=0;

void TestCountSharePointer()
{
    {
       std::shared_ptr<cTestMMV2Obj> p0(new cTestMMV2Obj);
       std::shared_ptr<cTestMMV2Obj> p1(new cTestMMV2Obj);

       std::shared_ptr<cTestMMV2Obj> p2 = p0;
       p2 = p1;
       StdOut() << "NB OBJ " << cTestMMV2Obj::NbObj() << " =? 2\n";
    }
    StdOut() << "NB OBJ " << cTestMMV2Obj::NbObj() << " =? 0\n";
}

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

class cFonc1V;

class cDataFonc1V : public  cTestMMV2Obj
{
      public :
           virtual ~cDataFonc1V() {}
           virtual double GetVal(double anX) const = 0;
           virtual cFonc1V Derive() const = 0;
           virtual void Show(std::ostream &) const = 0;

           virtual bool IsCste0() const {return false;}
           virtual bool IsCste1() const {return false;}

      public :
};
// std::ostream & operator << (std::ostream & ofs,const Pt2dUi2  &p);

class  cFonc1V : public  std::shared_ptr<cDataFonc1V>
{
     public :
         double GetVal(double anX)  {return std::shared_ptr<cDataFonc1V>(*this)->GetVal(anX);}
         cFonc1V Derive() const     {return std::shared_ptr<cDataFonc1V>(*this)->Derive();}

         cFonc1V(double aCste);
         cFonc1V operator + (cFonc1V aF2);
         cFonc1V(cDataFonc1V* aPtr) : std::shared_ptr<cDataFonc1V>(aPtr) {}
};
// static cFonc1V X;

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

class cDataFonc1V_Cste  : public cDataFonc1V
{
      public :
         cDataFonc1V_Cste(double aCste) : mCste(aCste) {}

         double GetVal(double) const {return mCste;}
         cFonc1V Derive() const {return cFonc1V(0.0);}
         void Show(std::ostream & os) const {os << mCste;}

         bool IsCste0() const {return mCste==0;}
         bool IsCste1() const {return mCste==1;}

      private  :
         double mCste;
};
cFonc1V::cFonc1V(double aCste) :
    std::shared_ptr<cDataFonc1V>(new cDataFonc1V_Cste(aCste))
{
}

//=============================================
class cDataFonc1V_X  : public cDataFonc1V
{
      public :
           cDataFonc1V_X() {}
           ~cDataFonc1V_X() {}

           double GetVal(double aVal) const {return aVal;}
           cFonc1V Derive() const {return cFonc1V(1.0);}
           void Show(std::ostream & os) const {os << "X";}
      private  :
};

static cFonc1V X() { return (new cDataFonc1V_X);}

//=============================================
class cDataFonc1V_Som  : public cDataFonc1V
{
     public :
         cDataFonc1V_Som(cFonc1V aF1,cFonc1V aF2) : mF1(aF1) , mF2(aF2) {}

         double  GetVal(double aVal) const {return mF1->GetVal(aVal)+mF2->GetVal(aVal);}
         cFonc1V  Derive() const {return mF1->Derive()+mF2->Derive();}
         void Show(std::ostream & os) const
         {
               os << "(";
               mF1->Show(os);
               os << "+";
               mF2->Show(os);
               os << ")";
         }
     public :
         cFonc1V mF1;
         cFonc1V mF2;
};

cFonc1V cFonc1V::operator + (cFonc1V aF2)
{
   return cFonc1V(new cDataFonc1V_Som(*this,aF2));
}

void  TestSharedPointer()
{
   TestCountSharePointer();

   // new  cFonc1V(3.14);
   // new  cDataFonc1V_Cste(3.14);
   {
       cFonc1V aF = X()+3+(X()+4);
       StdOut() << "F(10) = " << aF.GetVal(10) << " Count= " << cTestMMV2Obj::NbObj() << "\n";
       StdOut() << "F'(10) = " << aF.Derive().GetVal(10)  << "\n";
       aF->Show(StdOut()) ; StdOut() << "\n";
   }
   StdOut()  << " Compte final " << cTestMMV2Obj::NbObj() << "\n";
}

/*************************************************************/
/*                                                           */
/*            cAppli_MMVII_TestCpp11                         */
/*                                                           */
/*************************************************************/

class cAppli_MMVII_TestCpp11 : public cMMVII_Appli
{
     public :
        cAppli_MMVII_TestCpp11(int argc,char** argv,const cSpecMMVII_Appli & aSpec) ;
        int Exe() override ;
        cCollecSpecArg2007 & ArgObl(cCollecSpecArg2007 & anArgObl) override {return anArgObl;}
        cCollecSpecArg2007 & ArgOpt(cCollecSpecArg2007 & anArgOpt) override {return anArgOpt;}

};

cAppli_MMVII_TestCpp11::cAppli_MMVII_TestCpp11 (int argc,char **argv,const cSpecMMVII_Appli & aSpec) :
  cMMVII_Appli (argc,argv,aSpec)
{
}

// Voir override et  final, pour verifier que le surcharge virtuelle est conforme a nos attentes

// Stong enum
enum class Cpp11Color { red, blue };   // scoped and strongly typed enum
enum class Cpp11TrafficLight { red, yellow, green };

int  ExternalFonc();

template <class Type> void PrintSzVect(const std::vector<Type> &V);
void PrintSzVectI(const std::vector<int> &V)
{
}

// Constructeur appellant un constructeur pour eviter duplication
// de code
class cCtsrCallCstr
{
    public :
       cCtsrCallCstr(const int & aV) : mV(aV) {}
       cCtsrCallCstr() : cCtsrCallCstr(333) {}
       int mV;
};

// initialisation d'un membre avec une valeur par defaut,
// mais qui peut etre override
class cWithMembGlobInit
{
    public :
       cWithMembGlobInit() {}
       cWithMembGlobInit(int aV) : mV(aV) {}
       int mV=55;
};
// Assertion statiques verifiees a la compilation
static_assert(1+1==2,"theorme fondamental");
// static_assert(1+1==3,"theorme pas trop utile");

// voir les variadic template, pas sur avoir rencontre le besoin
// 	template<typename Head, typename... Tail>

// auto + -> pour resoudre le probleme du scope dans la valeur de retour
template <class T1,class T2> auto Mul(const T1 & aV1,const T2 & aV2) -> decltype(aV1*aV2) {return aV1*aV2;}

struct sTab3
{
   public :

      sTab3(int x,int y,int z) :
        i {x,y,z}
      {
      }
      int i[3] ;
};

bool fCmpPt(const cPt2dr &aP1,const cPt2dr &aP2){return (aP1.x()+aP1.y()) <  (aP2.x()+aP2.y());}

double Div(double a,double b) {return a/b;}

class cUnikP
{
   public :
      ~cUnikP(){StdOut() << "KilledP=" << mMes << "\n";}
      cUnikP(const std::string & aMes) : mMes(aMes) {}
      std::string mMes;
};

std::unique_ptr<cUnikP>  Transfer(const std::string & aMes)
{
      return std::unique_ptr<cUnikP> (new cUnikP(aMes));
}

int cAppli_MMVII_TestCpp11::Exe()
{
   {
       std::string aDir,aFile;
       SplitDirAndFile(aDir,aFile,"./",false);
       SplitDirAndFile(aDir,aFile,mArgv[0],false);
       SplitDirAndFile(aDir,aFile,"/home/ubuntu/Desktop/MMM/micmac/MMVII/bin/",false);
   }
   {
        std::unique_ptr<cUnikP> aP (new cUnikP("T1"));
   }
   StdOut() << "END BLOCK T1\n";
   {
        std::unique_ptr<cUnikP> aP2 = Transfer("T2");
        StdOut() << " IN  BLOCK T2\n";
   }
   StdOut() << "END BLOCK T2\n";

   cPt2dr aP{1,2};
   std::vector<cPt2dr> aVP{{1,2},{3,1},{1,1}};  // Uniform initialization synta

   // Lambda expression fonctionne, mais g++ pas cool sur les erreurs !!!

   std::sort(aVP.begin(),aVP.end(), [](const cPt2dr &aP1,const cPt2dr &aP2){return (aP1.x()+aP1.y()) <  (aP2.x()+aP2.y());});
   std::sort(aVP.begin(),aVP.end(), fCmpPt);

   // Local struct
   struct sCmpPt {
       bool operator () (const cPt2dr &aP1,const cPt2dr &aP2){return (aP1.x()+aP1.y()) <  (aP2.x()+aP2.y());}
   };
   std::sort(aVP.begin(),aVP.end(), sCmpPt());

   std::string s = R"(\w\\\w)"; // Raw string , \ is \ !!
   std::string s2 = R"**(\w"()"\\\w)**"; // Raw string  avec "()" etc dedans ...
   StdOut() << "TEST RAW STRING " << s  << "###"  << s2 << "\n";

   auto aTuple = std::make_tuple(1,2.0,"3+2=5");
   StdOut() << " T0=> " << std::get<0>(aTuple) << " T2=>" << std::get<2>(aTuple) << " " << "\n";

   StdOut() << " Type1=> " << typeid(std::get<1>(aTuple)).name()  << "\n";

   char * C= nullptr; IgnoreUnused(C); // Un pointeur nul universel, + clean que (char *)

   cCtsrCallCstr aT;
   StdOut() << "cCtsrCallCstr => " << aT.mV << "\n";
   // PrintSzVect({1,2}); => Pb avec template et initializer , pas sur pb moo ou g++ ?
   PrintSzVectI({1,2});  // Ok sans template
   // Les const expression sont garanties evaluables a la compile
   constexpr auto i = 3+4;
   typedef decltype (1/2.0) tDouble;  // declaration de type a partir d'une expression
   tDouble aUnusedtDouble; IgnoreUnused(aUnusedtDouble);
   // i++;
   // auto l = constexpr 3+4;
   // l++;
   constexpr  int j = 2*i;  IgnoreUnused(j);
   // const constexpr l = 2;
   // constexpr int k = ExternalFonc(); => pas evaluable a la compile
   // Range
   std::vector<int>  aVI;
   for (auto & it : aVI)
   {
       StdOut() << it << "\n";
       it += 2;
   }
   // Range for "enum" values
   for (const auto x : { 2,3,5,7,11 }) StdOut() << "Prime ? "  << x << '\n';

   // >> and double range
   std::vector<std::vector<int>>  aVVI;
   for (const auto & it1 : aVVI)
   {
       for (const auto & it2 : it1)
       {
             if (it2) ;
       }
   }

   std::forward_list<int> aFLPrime{2,3,5,7,11,13};
   for (const auto &  it : aFLPrime) StdOut() << " " << it ;
   StdOut() <<  ENDL;

   std::unordered_map<std::string,int> aUMap{{"Un",1},{"deux",2},{"trois",3},{"Quatre",4}};
   for (const auto &  it : aUMap) StdOut() << " " << it.first<<","<<it.second ;
   StdOut() <<  ENDL;
   StdOut() << "aUMap[Un]=" <<aUMap["Un"] << " aUMap[un]=" << aUMap["un"] << "\n";
   StdOut() << " SZUM " << sizeof(aUMap) <<  ENDL;

   // Pas sur supporte par g++ ???
   auto Div2 = std::bind(Div,std::placeholders::_1,2.0);
   StdOut() <<  " 5/2.0= " << Div2(5.0) << ENDL;
   // auto Div2 = std::bind(Div,std::_1,2.0);
   // auto Div2 = std::bind(Div,std::_1,2.0);
   //auto Div2 = bind(Div,_1,2.0);

   //  revoir les function object, pas trop compris ....
   // voir unique_ptr  (+ ou - comme auto_ptr ?) pour les ptr temporaires a un bloc d'execution

   TestSharedPointer();
   return EXIT_SUCCESS;
}

tMMVII_UnikPApli Alloc_MMVII_Cpp11(int argc,char ** argv,const cSpecMMVII_Appli & aSpec)
{
   return tMMVII_UnikPApli(new cAppli_MMVII_TestCpp11(argc,argv,aSpec));
}

cSpecMMVII_Appli  TheSpecTestCpp11
(
     "Cpp11",
      Alloc_MMVII_Cpp11,
      "This command execute some test for to check my understanding of C++11",
      {eApF::Test},
      {eApDT::None},
      {eApDT::Console},
      __FILE__
);

};


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// See LICENSE file in the project root for full license information.
//
#include <boost/test/unit_test.hpp>
#include "armnnCaffeParser/ICaffeParser.hpp"
#include "ParserPrototxtFixture.hpp"

BOOST_AUTO_TEST_SUITE(CaffeParser)

struct MultiInputsOutputsFixture : public ParserPrototxtFixture<armnnCaffeParser::ICaffeParser>
{
    MultiInputsOutputsFixture()
    {
        m_Prototext = R"(
name: "MultiInputsOutputs"
layer {
  name: "input1"
  type: "Input"
  top: "input1"
  input_param { shape: { dim: 1 } }
}
layer {
  name: "input2"
  type: "Input"
  top: "input2"
  input_param { shape: { dim: 1 } }
}
layer {
    bottom: "input1"
    bottom: "input2"
    top: "add1"
    name: "add1"
    type: "Eltwise"
}
layer {
    bottom: "input2"
    bottom: "input1"
    top: "add2"
    name: "add2"
    type: "Eltwise"
}
        )";
        Setup({ }, { "add1", "add2" });
    }
};

BOOST_FIXTURE_TEST_CASE(MultiInputsOutputs, MultiInputsOutputsFixture)
{
    RunTest<1>({ { "input1",{ 12.0f } },{ "input2",{ 13.0f } } },
        { { "add1",{ 25.0f } },{ "add2",{ 25.0f } } });
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef STAN_MATH_TEST_FIXTURE_ODE_SHO_CTL_HPP
#define STAN_MATH_TEST_FIXTURE_ODE_SHO_CTL_HPP

#include <stan/math/rev.hpp>
#include <boost/numeric/odeint.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/prim/functor/harmonic_oscillator.hpp>
#include <test/unit/math/rev/functor/coupled_mm.hpp>
#include <test/unit/math/rev/functor/test_fixture_ode.hpp>
#include <test/unit/math/rev/functor/test_fixture_ode_sho.hpp>
#include <test/unit/util.hpp>
#include <iostream>
#include <sstream>
#include <vector>
#include <limits>
#include <string>

/**
 * Inheriting base type, various fixtures differs by the type of ODE functor
 * used in apply_solver calls, intended for different kind of
 * tests. This harmonic oscillator test class is intended for use with the
 * adjoint ODE solver which has additional control parameters.
 *
 */
template <typename T>
struct harmonic_oscillator_ctl_test
    : public harmonic_oscillator_ode_base<T>,
      public ODETestFixture<harmonic_oscillator_test<T>> {
  double rtol_f;
  double rtol_b;
  double rtol_q;
  Eigen::VectorXd atol_f;
  Eigen::VectorXd atol_b;
  double atol_q;
  int num_steps_check;
  int inter_poly;
  int solv_f;
  int solv_b;

  harmonic_oscillator_ctl_test()
      : harmonic_oscillator_ode_base<T>(),
        rtol_f(this->rtol / 8.0),
        rtol_b(this->rtol / 4.0),
        rtol_q(this->rtol),
        atol_f(Eigen::VectorXd::Constant(this->y0.size(), this->atol / 6.0)),
        atol_b(Eigen::VectorXd::Constant(this->y0.size(), this->atol / 3.0)),
        atol_q(this->atol),
        num_steps_check(100),
        inter_poly(CV_HERMITE),
        solv_f(CV_BDF),
        solv_b(CV_ADAMS) {}

  auto apply_solver() {
    std::tuple_element_t<0, T> sol;
    return sol(this->f_eigen, this->y0, this->t0, this->ts, nullptr,
               this->theta, this->x_r, this->x_i);
  }

  template <typename T1, typename T2>
  auto apply_solver(T1&& init, T2&& theta_in) {
    std::tuple_element_t<0, T> sol;
    return sol(this->f_eigen, init, this->t0, this->ts, nullptr, theta_in,
               this->x_r, this->x_i);
  }

  auto apply_solver_tol() {
    std::tuple_element_t<1, T> sol;
    return sol(this->f_eigen, this->y0, this->t0, this->ts, this->rtol,
               this->atol, this->max_num_step, nullptr, this->theta, this->x_r,
               this->x_i);
  }

  auto apply_solver_tol_ctl() {
    std::tuple_element_t<0, T> sol;
    return sol(this->f_eigen, this->y0, this->t0, this->ts, this->rtol_f,
               this->atol_f, this->rtol_b, this->atol_b, this->rtol_q,
               this->atol_q, this->max_num_step, this->num_steps_check,
               this->inter_poly, this->solv_f, this->solv_b, nullptr,
               this->theta, this->x_r, this->x_i);
  }

  void test_bad() {
    const auto y0_(this->y0);

    this->y0.resize(0);
    EXPECT_THROW_MSG(apply_solver(), std::invalid_argument,
                     "initial state has size 0");
    this->y0 = y0_;

    const auto t0_ = this->t0;
    this->t0 = 2.0;
    EXPECT_THROW_MSG(apply_solver(), std::domain_error,
                     "initial time is 2, but must be less than 0.1");
    this->t0 = t0_;

    const auto ts_ = this->ts;
    this->ts.resize(0);
    EXPECT_THROW_MSG(apply_solver(), std::invalid_argument, "times has size 0");
    this->ts = ts_;

    this->ts.resize(2);
    this->ts[0] = 3.0;
    this->ts[1] = 1.0;
    EXPECT_THROW_MSG(apply_solver(), std::domain_error,
                     "times is not a valid sorted vector");
    this->ts = ts_;

    const double rtol_f_ = this->rtol_f;
    this->rtol_f = -1;
    EXPECT_THROW_MSG(apply_solver_tol_ctl(), std::domain_error,
                     "relative_tolerance_forward");
    this->rtol_f = rtol_f_;

    const double atol_f_ = this->atol_f(0);
    this->atol_f(0) = -1;
    EXPECT_THROW_MSG(apply_solver_tol_ctl(), std::domain_error,
                     "absolute_tolerance_forward");
    this->atol_f(0) = atol_f_;

    this->atol_f.resize(1);
    this->atol_f(0) = atol_f_;
    EXPECT_THROW_MSG(apply_solver_tol_ctl(), std::invalid_argument,
                     "absolute_tolerance_forward");
    this->atol_f.resize(2);
    this->atol_f(0) = atol_f_;
    this->atol_f(1) = atol_f_;

    const double rtol_b_ = this->rtol_b;
    this->rtol_b = -1;
    EXPECT_THROW_MSG(apply_solver_tol_ctl(), std::domain_error,
                     "relative_tolerance_backward");
    this->rtol_b = rtol_b_;

    const double atol_b_ = this->atol_b(0);
    this->atol_b(0) = -1;
    EXPECT_THROW_MSG(apply_solver_tol_ctl(), std::domain_error,
                     "absolute_tolerance_backward");
    this->atol_b(0) = atol_b_;

    this->atol_b.resize(1);
    this->atol_b(0) = atol_b_;
    EXPECT_THROW_MSG(apply_solver_tol_ctl(), std::invalid_argument,
                     "absolute_tolerance_backward");
    this->atol_b.resize(2);
    this->atol_b(0) = atol_b_;
    this->atol_b(1) = atol_b_;

    const double rtol_q_ = this->rtol_q;
    this->rtol_q = -1;
    EXPECT_THROW_MSG(apply_solver_tol_ctl(), std::domain_error,
                     "relative_tolerance_quadrature");
    this->rtol_q = rtol_q_;

    const double atol_q_ = this->atol_q;
    this->atol_q = -1;
    EXPECT_THROW_MSG(apply_solver_tol_ctl(), std::domain_error,
                     "absolute_tolerance_quadrature");
    this->atol_q = atol_q_;

    const int max_num_step_ = this->max_num_step;
    this->max_num_step = -1;
    EXPECT_THROW_MSG(apply_solver_tol_ctl(), std::domain_error,
                     "max_num_steps");
    this->max_num_step = max_num_step_;

    const int num_steps_check_ = this->num_steps_check;
    this->num_steps_check = -1;
    EXPECT_THROW_MSG(apply_solver_tol_ctl(), std::domain_error,
                     "num_steps_between_checkpoints");
    this->num_steps_check = num_steps_check_;

    const int inter_poly_ = this->inter_poly;
    this->inter_poly = 0;
    EXPECT_THROW_MSG(apply_solver_tol_ctl(), std::invalid_argument,
                     "interpolation_polynomial");
    this->inter_poly = inter_poly_;

    const int solv_f_ = this->solv_f;
    this->solv_f = 0;
    EXPECT_THROW_MSG(apply_solver_tol_ctl(), std::invalid_argument,
                     "solver_forward");
    this->solv_f = solv_f_;

    const int solv_b_ = this->solv_b;
    this->solv_b = 0;
    EXPECT_THROW_MSG(apply_solver_tol_ctl(), std::invalid_argument,
                     "solver_backward");
    this->solv_b = solv_b_;

    const auto theta_ = this->theta;
    const auto x_r_ = this->x_r;
    const auto x_i_ = this->x_i;

    // NaN errors
    double nan = std::numeric_limits<double>::quiet_NaN();
    std::stringstream expected_is_nan;
    expected_is_nan << "is " << nan;

    this->y0[0] = nan;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "initial state");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_nan.str());
    this->y0 = y0_;

    this->t0 = nan;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "initial time");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_nan.str());
    this->t0 = t0_;

    this->ts[0] = nan;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "times");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_nan.str());
    this->ts = ts_;

    this->theta[0] = nan;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     "ode parameters and data");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_nan.str());
    this->theta = theta_;

    this->x_r.push_back(nan);
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     "ode parameters and data");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_nan.str());
    this->x_r = x_r_;

    // inf test
    std::stringstream expected_is_inf;
    expected_is_inf << "is " << std::numeric_limits<double>::infinity();
    std::stringstream expected_is_neg_inf;
    expected_is_neg_inf << "is " << -std::numeric_limits<double>::infinity();
    double inf = std::numeric_limits<double>::infinity();

    this->y0[0] = inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "initial state");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_inf.str());
    this->y0[0] = -inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "initial state");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_neg_inf.str());
    this->y0 = y0_;

    this->t0 = inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "initial time");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_inf.str());
    this->t0 = -inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "initial time");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_neg_inf.str());
    this->t0 = t0_;

    this->ts.back() = inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "times");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_inf.str());
    this->ts.back() = -inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "times");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_neg_inf.str());
    this->ts = ts_;

    this->theta[0] = inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     "ode parameters and data");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_inf.str());
    this->theta[0] = -inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     "ode parameters and data");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_neg_inf.str());
    this->theta = theta_;

    this->x_r = std::vector<double>{inf};
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     "ode parameters and data");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_inf.str());
    this->x_r[0] = -inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     "ode parameters and data");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_neg_inf.str());
    this->x_r = x_r_;
  }

  void test_value(double t0_in) {
    this->t0 = t0_in;
    for (size_t i = 0; i < this->ts.size(); ++i) {
      this->ts[i] = this->t0 + 0.1 * (i + 1);
    }

    this->rtol = 1e-8;
    this->atol = 1e-10;
    this->max_num_step = 1e6;
    auto res = apply_solver_tol();

    EXPECT_NEAR(0.995029, stan::math::value_of(res[0][0]), 1e-5);
    EXPECT_NEAR(-0.0990884, stan::math::value_of(res[0][1]), 1e-5);

    EXPECT_NEAR(-0.421907, stan::math::value_of(res[99][0]), 1e-5);
    EXPECT_NEAR(0.246407, stan::math::value_of(res[99][1]), 1e-5);
  }
};

#endif


#include <boost/test/unit_test.hpp>

#include "bithorded/lib/rounding.hpp"

using namespace std;

BOOST_AUTO_TEST_CASE( rounding_test )
{
	BOOST_CHECK_EQUAL( roundUp(0, 4), 0 );
	BOOST_CHECK_EQUAL( roundUp(2, 4), 4 );
	BOOST_CHECK_EQUAL( roundUp(3, 4), 4 );
	BOOST_CHECK_EQUAL( roundUp(4, 4), 4 );
	BOOST_CHECK_EQUAL( roundUp(5, 4), 8 );

	BOOST_CHECK_EQUAL( roundDown(0, 4), 0 );
	BOOST_CHECK_EQUAL( roundDown(2, 4), 0 );
	BOOST_CHECK_EQUAL( roundDown(3, 4), 0 );
	BOOST_CHECK_EQUAL( roundDown(4, 4), 4 );
	BOOST_CHECK_EQUAL( roundDown(5, 4), 4 );
}


// Copyright 2019 Hans Dembinski
//
// 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)

//[ guide_fill_profile

#include <boost/format.hpp>
#include <boost/histogram.hpp>
#include <cassert>
#include <iostream>
#include <sstream>
#include <tuple>

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

  // make a profile, it computes the mean of the samples in each histogram cell
  auto h = make_profile(axis::integer<>(0, 3));

  // mean is computed from the values marked with the sample() helper function
  h(0, sample(1)); // sample goes to cell 0
  h(0, sample(2)); // sample goes to cell 0
  h(1, sample(3)); // sample goes to cell 1
  h(sample(4), 1); // sample goes to cell 1; sample can be first or last argument

  // fills from tuples are also supported, 5 and 6 go to cell 2
  auto a = std::make_tuple(2, sample(5));
  auto b = std::make_tuple(sample(6), 2);
  h(a);
  h(b);

  // builtin accumulators have methods to access their state
  std::ostringstream os;
  for (auto&& x : indexed(h)) {
    // use `.` to access methods of accessor, like `index()`
    // use `->` to access methods of accumulator
    const auto i = x.index();
    const auto n = x->count();     // how many samples are in this bin
    const auto vl = x->value();    // mean value
    const auto vr = x->variance(); // estimated variance of the mean value
    os << boost::format("index %i count %i value %.1f variance %.1f\n") % i % n % vl % vr;
  }

  std::cout << os.str() << std::flush;

  assert(os.str() == "index 0 count 2 value 1.5 variance 0.5\n"
                     "index 1 count 2 value 3.5 variance 0.5\n"
                     "index 2 count 2 value 5.5 variance 0.5\n");
}

//]


#ifndef POINT_REFLECTOR_HPP
#define POINT_REFLECTOR_HPP

#include <Eigen/Dense>

namespace smmap
{
    class PointReflector
    {
        public:
            PointReflector()
                : PointReflector(0, 0, 0)
            {}

            PointReflector(double mid_x, double min_y, double max_y)
                : mid_x_(mid_x)
                , min_y_(min_y)
                , max_y_(max_y)
            {}

            Eigen::Vector3d reflect(const Eigen::Vector3d& in) const
            {
                Eigen::Vector3d out = in;
                out[0] = in[0] - 2*(in[0] - mid_x_);
                return out;
            }

            Eigen::Vector3d reflect(Eigen::Vector3d&& in) const
            {
                Eigen::Vector3d out = in;
                out[0] = in[0] - 2*(in[0] - mid_x_);
                return out;
            }

            double get_mid_x() const
            {
                return mid_x_;
            }

            double get_min_y() const
            {
                return min_y_;
            }

            double get_max_y() const
            {
                return max_y_;
            }

        private:
            const double mid_x_, min_y_, max_y_;
    };
}

#endif // POINT_REFLECTOR_HPP


#include <iostream>
#include <random>

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

#include <iostream>
#include <algorithm>

#include "saxencoder.h"

using namespace Akumuli::SAX;

BOOST_AUTO_TEST_CASE(Test_sax_word) {

    std::vector<int> input = {0, 1, 7, 0x7F, 0xFFFF, 0xFFFFFFF };
    SAXWord sword(input.begin(), input.end());
    std::vector<int> output;
    sword.read_n((int)input.size(), std::back_inserter(output));
    BOOST_REQUIRE_EQUAL_COLLECTIONS(input.begin(), input.end(), output.begin(), output.end());
}

BOOST_AUTO_TEST_CASE(Test_encoding) {

    std::vector<double> input = {
        0, 1, 2, 3, 0, 1, 2, 3, 0, 3, 0, 2, 1, 3
    };

    SAXEncoder encoder(4, 4);

    std::vector<std::string> words;

    for (auto x: input) {
        std::string w;
        w.resize(4);
        if (encoder.encode(x, &w[0], w.size())) {
            words.push_back(w);
        }
    }

    std::vector<std::string> expected = {
        "abcd",
        "bcda",
        "cdab",
        "dabc",
        "abcd",
        "bcda",
        "cdad",
        "dada",
        "adac",
        "dacb",
        "acbd",
    };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(words.begin(), words.end(), expected.begin(), expected.end());
}


#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <iostream>

// These need to be pulled out to parameters...
const float WHEEL_RAD = 0.1016; // meters
const float WHEELBASE = 0.466725; // meters
const float TRACK = 0.2667; // meters

namespace gazebo
{
  class Mecanum : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
    {
      // Store the pointer to the model
      this->model = _parent;

      mbl = 0;
      mbr = 0;
      mfl = 0;
      mfr = 0;

      mRosnode.reset(new ros::NodeHandle(""));

      mfl_sub = mRosnode->subscribe("front_left",1,&Mecanum::fl_cb, this);
      mfr_sub = mRosnode->subscribe("front_right",1,&Mecanum::fr_cb, this);
      mbl_sub = mRosnode->subscribe("back_left",1,&Mecanum::bl_cb, this);
      mbr_sub = mRosnode->subscribe("back_right",1,&Mecanum::br_cb, this);

      // Listen to the update event. This event is broadcast every
      // simulation iteration.
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
        boost::bind(&Mecanum::UpdateChild, this, _1));
      }

      public: void UpdateChild(const common::UpdateInfo & /*_info*/)
      {
        float r = WHEEL_RAD;
        float l1 = TRACK; // left -> right
        float l2 = WHEELBASE; // front -> back
        float l = 1.0/(2*(l1+l2));
        float x = r*(mfl/4.0 + mfr/4.0 + mbl/4.0 + mbr/4.0);
        float y = r*(-mfl/4.0 + mfr/4.0 + mbl/4.0 - mbr/4.0);

        float rot = r*(-l*mfl + l*mfr - l*mbl + l*mbr);
        math::Pose pose = this->model->GetWorldPose();
        float yaw = pose.rot.GetYaw();
        this->model->SetLinearVel(math::Vector3(
          x * cosf(yaw) - y * sinf(yaw),
          y * cosf(yaw) + x * sinf(yaw),
          0));
        this->model->SetAngularVel(math::Vector3(0, 0, rot));
        }

        public: void fl_cb(const std_msgs::Float32::ConstPtr & cmd_msg)
        {
          mfl = cmd_msg->data;
        }
        public: void fr_cb(const std_msgs::Float32::ConstPtr & cmd_msg)
        {
          mfr = cmd_msg->data;
        }
        public: void bl_cb(const std_msgs::Float32::ConstPtr & cmd_msg)
        {
          mbl = cmd_msg->data;
        }
        public: void br_cb(const std_msgs::Float32::ConstPtr & cmd_msg)
        {
          mbr = cmd_msg->data;
        }

      private:
        // Pointer to the model
        physics::ModelPtr model;

        // Pointer to the update event connection
        event::ConnectionPtr updateConnection;

        boost::shared_ptr<ros::NodeHandle> mRosnode;

        float mfl;
        float mfr;
        float mbl;
        float mbr;

        ros::Subscriber mfl_sub;
        ros::Subscriber mfr_sub;
        ros::Subscriber mbl_sub;
        ros::Subscriber mbr_sub;
      };

      // Register this plugin with the simulator
      GZ_REGISTER_MODEL_PLUGIN(Mecanum)
    }


/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2015, Daiki Maekawa and Chiba Institute of Technology.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above
 *    copyright notice, this list of conditions and the following
 *    disclaimer in the documentation and/or other materials provided
 *    with the distribution.
 * 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 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <fulanghua_ekf_2d/pose_estimator.h>

#include <boost/assign.hpp>

#include <Eigen/LU>
#include <Eigen/Dense>

namespace fulanghua {

double normalize_rad(double th) {
    while(th > M_PI) th -= 2 * M_PI;
    while(th < -M_PI) th += 2 * M_PI;

    return th;
}

PoseEstimator::PoseEstimator(ros::NodeHandle &node) :
    publish_pose_topic_(true),
    publish_odom_topic_(false),
    update_rate_(100),
    output_frame_("odom"),
    base_frame_("base_link"),
    world_frame_("map"),
    x_est_(Eigen::Vector4d::Zero()),
    cov_est_(Eigen::Matrix4d::Identity())
{
    ros::NodeHandle nh_private("~");

    nh_private.param("output_frame", output_frame_, output_frame_);
    nh_private.param("base_frame", base_frame_, base_frame_);

    double motion_x_err, motion_y_err, motion_ang_err, motion_vel_err;
    double observation_x_err, observation_y_err, observation_ang_err, observation_vel_err;

    nh_private.param("motion_x_err", motion_x_err, 0.1);
    nh_private.param("motion_y_err", motion_y_err, 0.1);
    nh_private.param("motion_ang_err", motion_ang_err, 0.1);
    nh_private.param("motion_vel_err", motion_vel_err, 0.05);

    nh_private.param("observation_x_err", observation_x_err, 0.1);
    nh_private.param("observation_y_err", observation_y_err, 0.1);
    nh_private.param("observation_ang_err", observation_ang_err, 0.1);
    nh_private.param("observation_vel_err", observation_vel_err, 0.05);

    motion_cov_ << motion_x_err * motion_x_err, 0.0, 0.0, 0.0,
                   0.0, motion_y_err * motion_y_err, 0.0, 0.0,
                   0.0, 0.0, motion_ang_err * motion_ang_err, 0.0,
                   0.0, 0.0, 0.0, motion_vel_err * motion_vel_err;

    observation_cov_ << observation_x_err * observation_x_err, 0.0, 0.0, 0.0,
                       0.0, observation_y_err * observation_y_err, 0.0, 0.0,
                       0.0, 0.0, observation_ang_err * observation_ang_err, 0.0,
                       0.0, 0.0, 0.0, observation_vel_err * observation_vel_err;

    nh_private.param("publish_pose_topic", publish_pose_topic_, publish_pose_topic_);
    nh_private.param("publish_odom_topic", publish_odom_topic_, publish_odom_topic_);

    imu_sub_ = node.subscribe("imu", 10, &PoseEstimator::imu_callback, this);
    odom_sub_ = node.subscribe("odom", 10, &PoseEstimator::odom_callback, this);

    gpos_meas_sub_ = node.subscribe("meas_gpos", 10, &PoseEstimator::gpos_meas_callback, this);

    if(publish_odom_topic_)
        odom_pub_ = node.advertise<nav_msgs::Odometry>("combined_odom", 10);

    if(publish_pose_topic_)
        pose_pub_ = node.advertise<geometry_msgs::PoseWithCovarianceStamped>("self_pose", 10);

}

void PoseEstimator::spin() {
    ros::Rate rate(update_rate_);
    ros::Time old_filter_stamp(0);
    bool meas_initialized = false;

    while(ros::ok()) {
        ros::spinOnce();
        rate.sleep();

        if(old_filter_stamp != ros::Time(0) && imu_stamp_ != ros::Time(0) && odom_stamp_ != ros::Time(0)) {
            meas_initialized = true;
        } else {
            meas_initialized = false;
        }

        if(meas_initialized) {
            ros::Time now = ros::Time::now();
            ros::Time filter_stamp = now;

            filter_stamp = std::min(filter_stamp, odom_stamp_);
            filter_stamp = std::min(filter_stamp, imu_stamp_);
            filter_stamp = std::min(filter_stamp, gpos_meas_stamp_);

            double dt = (now - old_filter_stamp).toSec();
            if(fabs(dt) < 0.0001) continue;

            if((ros::Time::now() - filter_stamp).toSec() > 1.0) {
                ROS_WARN("receive data are too old!");
                continue;
            }

            Eigen::Vector2d est_pos;
            double est_yaw;
            Eigen::Matrix3d est_cov;
            if(!estimate(est_pos, est_yaw, est_cov, filter_stamp, dt)) continue;

            if(publish_odom_topic_) publish_odom(filter_stamp, est_pos, est_yaw, est_cov);
            if(publish_pose_topic_) publish_pose(filter_stamp, est_pos, est_yaw, est_cov);

            geometry_msgs::TransformStamped tran;
            tran.header.stamp = filter_stamp;
            tran.header.frame_id = output_frame_;
            tran.child_frame_id = base_frame_;
            tran.transform.translation.x = est_pos.x();
            tran.transform.translation.y = est_pos.y();
            tran.transform.translation.z = 0.0;
            tran.transform.rotation = tf::createQuaternionMsgFromYaw(est_yaw);

            tf_broadcaster_.sendTransform(tran);

            old_filter_stamp = now;
        } else {
            old_filter_stamp = ros::Time::now();

            if(transformer_.canTransform(world_frame_, "odom", old_filter_stamp)) {
                transformer_.lookupTransform("odom", world_frame_, old_filter_stamp, old_odom_meas_);
            }
        }
    }
}

bool PoseEstimator::estimate(Eigen::Vector2d &pos, double &yaw, Eigen::Matrix3d &cov_xy_th, const ros::Time &filter_stamp, double dt) {

    tf::StampedTransform odom_meas;
    if(!transformer_.canTransform(world_frame_, "odom", filter_stamp)) {
        ROS_WARN("Failed transform of odom data");
        return false;
    } else {
        transformer_.lookupTransform("odom", world_frame_, filter_stamp, odom_meas);
    }

    double gpos_meas_x, gpos_meas_y;
    tf::StampedTransform gpos_meas;
    if(!transformer_.canTransform(world_frame_, "gpos_meas", filter_stamp)) {
        ROS_WARN("Failed transform of gpos_meas data");
        return false;
    } else {
        transformer_.lookupTransform("gpos_meas", world_frame_, filter_stamp, gpos_meas);
    }

    gpos_meas_x = gpos_meas.getOrigin().x();
    gpos_meas_y = gpos_meas.getOrigin().y();

    tf::StampedTransform imu_meas;
    if(!transformer_.canTransform(world_frame_, "imu", filter_stamp)) {
        ROS_WARN("Failed transform of imu data");
        return false;
    } else {
        transformer_.lookupTransform("imu", world_frame_, filter_stamp, imu_meas);
    }

    double odom_linear_x = (odom_meas.getOrigin().x() - old_odom_meas_.getOrigin().x()) / dt;

    double odom_angular_z;
    double tmp_r, tmp_p, odom_yaw;
    odom_meas.getBasis().getEulerYPR(odom_yaw, tmp_p, tmp_r);
    double tmp_old_r, tmp_old_p, old_odom_yaw;
    old_odom_meas_.getBasis().getEulerYPR(old_odom_yaw, tmp_old_p, tmp_old_r);
    odom_angular_z = (odom_yaw - old_odom_yaw) / dt;

    ///< @todo use tf_listener for imu frame
    double imu_roll, imu_pitch, imu_yaw;
    imu_meas.getBasis().getEulerYPR(imu_yaw, imu_pitch, imu_roll);

    //predict
    Eigen::Vector2d u(odom_linear_x, odom_angular_z);
    Eigen::Vector4d x_pred = motion_model(x_est_, u, dt);
    Eigen::Matrix4d JF = jacob_motion_model(x_pred, u, dt);
    Eigen::Matrix4d cov_pred = JF * cov_est_ * JF.transpose() + motion_cov_;

    //update
    Eigen::Vector4d x(gpos_meas_x, gpos_meas_y, imu_yaw, odom_linear_x);
    Eigen::Vector4d z = observation_model(x);
    Eigen::Matrix4d H = jacob_observation_model(x_pred);
    Eigen::Vector4d y = z - observation_model(x_pred);
    y(2) = normalize_rad(y(2));
    Eigen::Matrix4d S = H * cov_pred * H.transpose() + observation_cov_;
    Eigen::Matrix4d K = cov_pred * H.transpose() * S.inverse();

    //storage estimation results
    x_est_ = x_pred + K * y;
    cov_est_ = (Eigen::Matrix4d::Identity() - K * H) * cov_pred;

    old_odom_meas_ = odom_meas;

    pos.x() = x_est_(0);
    pos.y() = x_est_(1);
    yaw = x_est_(2);
    cov_xy_th = cov_est_.block(0, 0, 3, 3);

    return true;
}

void PoseEstimator::publish_odom(const ros::Time &stamp, const Eigen::Vector2d &pos, double yaw, const Eigen::Matrix3d &cov_xy_th) {
    nav_msgs::Odometry est_odom;
    est_odom.header.stamp = stamp;
    est_odom.header.frame_id = output_frame_;
    est_odom.child_frame_id = base_frame_;

    est_odom.pose.pose.position.x = pos.x();
    est_odom.pose.pose.position.y = pos.y();
    est_odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
    est_odom.pose.covariance = boost::assign::list_of
        (cov_xy_th(0)) (0) (0) (0) (0) (0)
        (0) (cov_xy_th(1)) (0) (0) (0) (0)
        (0) (0) (999999.9) (0) (0) (0)
        (0) (0) (0) (999999.9) (0) (0)
        (0) (0) (0) (0) (0) (cov_xy_th(2));

    odom_pub_.publish(est_odom);
}

void PoseEstimator::publish_pose(const ros::Time &stamp, const Eigen::Vector2d &pos, double yaw, const Eigen::Matrix3d &cov_xy_th) {
    geometry_msgs::PoseWithCovarianceStamped est_pose;
    est_pose.header.stamp = stamp;
    est_pose.header.frame_id = base_frame_;
    est_pose.pose.pose.position.x = pos.x();
    est_pose.pose.pose.position.y = pos.y();
    est_pose.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);

    est_pose.pose.covariance = boost::assign::list_of
        (cov_xy_th(0)) (0) (0) (0) (0) (0)
        (0) (cov_xy_th(1)) (0) (0) (0) (0)
        (0) (0) (999999.9) (0) (0) (0)
        (0) (0) (0) (999999.9) (0) (0)
        (0) (0) (0) (0) (0) (cov_xy_th(2));

    pose_pub_.publish(est_pose);
}

} //namespace fulanghua

int main(int argc, char *argv[]) {
    ros::init(argc, argv, "pose_estimator");

    ros::NodeHandle nh;
    fulanghua::PoseEstimator estimator(nh);
    estimator.spin();

    return 0;
}


#include "newton_solver.h"

#include "store.h"
#include "acap_solve_energy_gradient.h"
#include "muscle_energy_gradient.h"
#include "stablenh_energy_gradient.h"
#include <igl/polar_dec.h>
#include <Eigen/LU>
#include <Eigen/Cholesky>
#include <igl/Timer.h>

using Store = famu::Store;
using namespace Eigen;
using namespace std;

double famu::Energy(Store& store, VectorXd& dFvec){
	double EM = famu::muscle::energy(store, dFvec);
	double ENH = famu::stablenh::energy(store, dFvec);
	double EACAP = famu::acap::fastEnergy(store, dFvec);

	return EM + ENH + EACAP;
}

void famu::polar_dec(Store& store, VectorXd& dFvec){
	if(store.jinput["polar_dec"]){
		//project bones dF back to rotations
		if(store.jinput["reduced"]){
			for(int b =0; b < store.bone_tets.size(); b++){
				Eigen::Matrix3d _r, _t;
				Matrix3d dFb = Map<Matrix3d>(dFvec.segment<9>(9*b).data()).transpose();
				igl::polar_dec(dFb, _r, _t);

				dFvec[9*b+0] = _r(0,0);
	      		dFvec[9*b+1] = _r(0,1);
	      		dFvec[9*b+2] = _r(0,2);
	      		dFvec[9*b+3] = _r(1,0);
	      		dFvec[9*b+4] = _r(1,1);
	      		dFvec[9*b+5] = _r(1,2);
	      		dFvec[9*b+6] = _r(2,0);
	      		dFvec[9*b+7] = _r(2,1);
	      		dFvec[9*b+8] = _r(2,2);

			}

		}else{
			for(int t = 0; t < store.bone_tets.size(); t++){
				for(int i=0; i<store.bone_tets[t].size(); i++){
					int b =store.bone_tets[t][i];

					Eigen::Matrix3d _r, _t;
					Matrix3d dFb = Map<Matrix3d>(dFvec.segment<9>(9*b).data()).transpose();
					igl::polar_dec(dFb, _r, _t);

					dFvec[9*b+0] = _r(0,0);
		      		dFvec[9*b+1] = _r(0,1);
		      		dFvec[9*b+2] = _r(0,2);
		      		dFvec[9*b+3] = _r(1,0);
		      		dFvec[9*b+4] = _r(1,1);
		      		dFvec[9*b+5] = _r(1,2);
		      		dFvec[9*b+6] = _r(2,0);
		      		dFvec[9*b+7] = _r(2,1);
		      		dFvec[9*b+8] = _r(2,2);
				}
			}
		}
	}
}

double famu::line_search(int& tot_ls_its, Store& store, VectorXd& grad, VectorXd& drt){
	// Decreasing and increasing factors
	VectorXd x = store.dFvec;
	VectorXd xp = x;
	double step = 50;
    const double dec = 0.5;
    const double inc = 2.1;
    int pmax_linesearch = 100;
    int plinesearch = 1;//1 for armijo, 2 for wolfe
    double pftol = 1e-4;
    double pwolfe = 0.9;
    double pmax_step = 1e8;
    double pmin_step = 1e-20;

    // Check the value of step
    if(step <= double(0))
        std::invalid_argument("'step' must be positive");

    polar_dec(store, x);
    famu::acap::solve(store, x);
   	double fx = Energy(store, x);
    // Save the function value at the current x
    const double fx_init = fx;
    // Projection of gradient on the search direction
    const double dg_init = grad.dot(drt);
    // Make sure d points to a descent direction
    if(dg_init > 0)
        std::logic_error("the moving direction increases the objective function value");

    const double dg_test = pftol * dg_init;
    double width;

    int iter;
    for(iter = 0; iter < pmax_linesearch; iter++)
    {
        // x_{k+1} = x_k + step * d_k
        x.noalias() = xp + step * drt;
        polar_dec(store, x);

        // Evaluate this candidate
        famu::acap::solve(store, x);
       	fx = Energy(store, x);

        if(fx > fx_init + step * dg_test)
        {
            width = dec;
        } else {
            // Armijo condition is met
            if(plinesearch == 1)
                break;

            const double dg = grad.dot(drt);
            if(dg < pwolfe * dg_init)
            {
                width = inc;
            } else {
                // Regular Wolfe condition is met
                if(plinesearch == 2)
                    break;

                if(dg > -pwolfe * dg_init)
                {
                    width = dec;
                } else {
                    // Strong Wolfe condition is met
                    break;
                }
            }
        }

        if(iter >= pmax_linesearch)
            throw std::runtime_error("the line search routine reached the maximum number of iterations");

        if(step < pmin_step)
            throw std::runtime_error("the line search step became smaller than the minimum value allowed");

        if(step > pmax_step)
            throw std::runtime_error("the line search step became larger than the maximum value allowed");

        step *= width;
    }
    // cout<<"			ls iters: "<<iter<<endl;
    // cout<<"			step: "<<step<<endl;
    tot_ls_its += iter;
    return step;
}

void famu::sparse_to_dense(const Store& store, SparseMatrix<double, Eigen::RowMajor>& H, MatrixXd& denseHess){
	//Fill denseHess with 9x9 block diags from H
	//TODO: this should be done in the hessians code. coeffRef is expensive
	//FIX AFTER THE DEADLINE

	#pragma omp parallel for
	for(int i=0; i<store.dFvec.size()/9; i++){
		//loop through 9x9 block and fill denseH
		Matrix9d A;
		#pragma omp parallel for collapse(2)
		for(int j =0; j<9; j++){
			for(int k=0; k<9; k++){
				A(j, k) = H.coeffRef(9*i + j, 9*i +k);
			}
		}
		denseHess.block<9,9>(9*i, 0) = A;
	}
}

void famu::fastWoodbury(Store& store, const VectorXd& g, MatrixModesxModes X, VectorXd& BInvXDy, MatrixXd& denseHess, VectorXd& drt){
	//Woodbury parallel approach 1 (with reduction)

	Matrix<double, NUM_MODES, 1> DAg = Matrix<double, NUM_MODES, 1>::Zero();
	Matrix<double, NUM_MODES, 1> InvXDAg;
	FullPivLU<MatrixModesxModes> WoodburyDenseSolve;

	double aa = store.jinput["alpha_arap"];
	X = store.InvC*aa;
	#pragma omp parallel
	{
		MatrixModesxModes Xpriv = MatrixModesxModes::Zero();
		Matrix<double, NUM_MODES, 1> DAgpriv = Matrix<double, NUM_MODES, 1>::Zero();

		#pragma omp for
		for(int i=0; i<store.dFvec.size()/9; i++){
			Matrix9d A = denseHess.block<9,9>(9*i, 0);
			LDLT<Matrix9d> InvA;
			InvA.compute(A);
			store.vecInvA[i] = InvA;

			Vector9d invAg = InvA.solve(g.segment<9>(9*i));
			drt.segment<9>(9*i) = invAg;

			Matrix9xModes B = store.WoodB.block<9, NUM_MODES>(9*i, 0);
			Xpriv = Xpriv + -B.transpose()*InvA.solve(B);

			DAgpriv  = DAgpriv + -B.transpose()*invAg;
		}
		#pragma omp critical
		{
			X += Xpriv;
			DAg += DAgpriv;
		}
	}

	#pragma omp single
	{

		WoodburyDenseSolve.compute(X);
		InvXDAg = WoodburyDenseSolve.solve(DAg);

	}

	#pragma omp parallel for
	for(int i=0; i<store.dFvec.size()/9; i++){
		Matrix9xModes B = store.WoodB.block<9, NUM_MODES>(9*i, 0);

		Vector9d InvAtemp1 = store.vecInvA[i].solve(B*InvXDAg);
		drt.segment<9>(9*i) -=  InvAtemp1;
	}

	drt *= -1;
}

int famu::newton_static_solve(Store& store){
	int MAX_ITERS = store.jinput["NM_MAX_ITERS"];
	VectorXd muscle_grad, neo_grad, acap_grad;
	muscle_grad.resize(store.dFvec.size());
	neo_grad.resize(store.dFvec.size());
	acap_grad.resize(store.dFvec.size());

	SparseMatrix<double, Eigen::RowMajor> constHess(store.dFvec.size(), store.dFvec.size());
	constHess.setZero();

	constHess = store.neoHess + store.muscleHess + store.acapHess;// + store.ContactHess;
	constHess -= store.neoHess;

	MatrixXd denseHess = MatrixXd::Zero(store.dFvec.size(),  9);
	MatrixXd constDenseHess = MatrixXd::Zero(store.dFvec.size(),  9);
	sparse_to_dense(store, constHess, constDenseHess);

	VectorXd delta_dFvec = VectorXd::Zero(store.dFvec.size());
	VectorXd test_drt = delta_dFvec;
	VectorXd grad_dofs = VectorXd::Zero(store.dFvec.size());

	VectorXd BInvXDy = VectorXd::Zero(store.dFvec.size());
	MatrixModesxModes X;

	igl::Timer timer, timer1;
	double woodtimes =0;
	double linetimes =0;
	int tot_ls_its = 0;
	int iter =1;
	timer1.start();
	for(iter=1; iter<MAX_ITERS; iter++){
		grad_dofs.setZero();
		double prevfx = Energy(store, store.dFvec);

		famu::acap::solve(store, store.dFvec);
		famu::muscle::gradient(store, muscle_grad);
		famu::stablenh::gradient(store, neo_grad);
		famu::acap::fastGradient(store, acap_grad);
		grad_dofs = muscle_grad + neo_grad + acap_grad;

		// cout<<"		muscle grad: "<<muscle_grad.norm()<<endl;
		// cout<<"		neo grad: "<<neo_grad.norm()<<endl;
		// cout<<"		acap grad: "<<acap_grad.norm()<<endl;
		// cout<<"		total grad: "<<grad_dofs.norm()<<endl;

		if(grad_dofs != grad_dofs){
			cout<<"Error: nans in grad"<<endl;
			exit(0);
		}

		famu::stablenh::hessian(store, store.neoHess, store.denseNeoHess, store.jinput["woodbury"]);

		// if(!store.jinput["woodbury"]){

		// SparseMatrix<double, Eigen::RowMajor> hessFvec = store.neoHess + constHess;
		// store.NM_SPLU.factorize(hessFvec);
		// if(store.NM_SPLU.info()!=Success){
		// 	cout<<"SOLVER FAILED"<<endl;
		// 	cout<<store.NM_SPLU.info()<<endl;
		// }
		// delta_dFvec = -1*store.NM_SPLU.solve(grad_dofs);

		// }else{

			// //Sparse Woodbury code
			// hessFvec.setZero();
			// hessFvec = store.neoHess + constHess;
			// store.NM_SPLU.factorize(hessFvec);
			// if(store.NM_SPLU.info()!=Success){
			// 	cout<<"SOLVER FAILED"<<endl;
			// 	cout<<store.NM_SPLU.info()<<endl;
			// }
			// VectorXd InvAg = store.NM_SPLU.solve(grad_dofs);
			// MatrixXd CDAB = store.InvC + store.WoodD*store.NM_SPLU.solve(store.WoodB);
			// FullPivLU<MatrixXd>  WoodburyDenseSolve;
			// WoodburyDenseSolve.compute(CDAB);
			// VectorXd temp1 = store.WoodB*WoodburyDenseSolve.solve(store.WoodD*InvAg);;

			// VectorXd InvAtemp1 = store.NM_SPLU.solve(temp1);
			// test_drt =  -InvAg + InvAtemp1;

			//Dense Woodbury code
			denseHess = constDenseHess + store.denseNeoHess;
			timer.start();
			fastWoodbury(store, grad_dofs, X, BInvXDy, denseHess, delta_dFvec);
			timer.stop();
			woodtimes += timer.getElapsedTimeInMicroSec();
			// cout<<"		woodbury diff: "<<(delta_dFvec - test_drt).norm()<<endl;

		// }

		if(delta_dFvec != delta_dFvec){
			cout<<"Error: nans"<<endl;
			exit(0);
		}

		//line search
		double alpha = 0.1;
		timer.start();
		alpha = line_search(tot_ls_its, store, grad_dofs, delta_dFvec);
		timer.stop();
		linetimes += timer.getElapsedTimeInMicroSec();

		if(fabs(alpha)<1e-9 ){
			break;
		}

		store.dFvec += alpha*delta_dFvec;
		polar_dec(store, store.dFvec);
		double fx = Energy(store, store.dFvec);

		if(grad_dofs.squaredNorm()/grad_dofs.size()<1e-4 || fabs(fx - prevfx)<1e-3){
			break;
		}
	}
	timer1.stop();
	double nmtime = timer1.getElapsedTimeInMicroSec();

	// timer1.start();
	// double acap_energy = famu::acap::fastEnergy(store, store.dFvec);
	// timer1.stop();
	// double energy_time = timer1.getElapsedTimeInMicroSec();

	// timer1.start();
	// famu::acap::solve(store, store.dFvec);
	// timer1.stop();

	cout<<"-----------QS STEP INFO----------"<<endl;
	cout<<"V, T:"<<store.V.rows()<<", "<<store.T.rows()<<endl;
	cout<<"Threads: "<<Eigen::nbThreads()<<endl;
	cout<<"NM Iters: "<<iter<<endl;
	cout<<"Total NM time: "<<nmtime<<endl;
	// cout<<"Total Hess time: "<<woodtimes<<endl;
	// cout<<"Total LS time: "<<linetimes<<endl;
	// cout<<"LS iters: "<<tot_ls_its<<endl;
	// cout<<"Energy: "<<acap_energy<<endl;
	// cout<<"Energy Time: "<<energy_time<<endl;
	// cout<<"ACAP time: "<<timer1.getElapsedTimeInMicroSec()<<endl;
	// cout<<"dFvec: "<<store.dFvec.transpose()<<endl;
	cout<<"--------------------------------"<<endl;
    return iter;
}


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

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

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing divfix capabilities

    Computes the truncated division of its parameters.

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = divfix(x, y);
    @endcode

    The code is similar to:

    @code
    T r = trunc(x/y);
    @endcode

    for integral types, if y is @ref Zero, it returns @ref Valmax (resp. @ref Valmin)
    if x is positive (resp. negative) and @ref Zero if x is @ref Zero.
    Take also care that dividing @ref Valmin by -1 for signed integral types has
    undefined behaviour.

    @par Alias

    @c divtrunc

    @see  divides, rec, divs, divfloor,
    divround, divround2even

  **/
  const boost::dispatch::functor<tag::divfix_> divfix = {};
} }
#endif

#include <boost/simd/function/scalar/divfix.hpp>
#include <boost/simd/function/simd/divfix.hpp>

#endif


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/LU>
#include <iostream>

// ----------------
// regular C++ code
// ----------------

Eigen::MatrixXd mul(const Eigen::MatrixXd &xs, double fac)
{
    std::cout << "Double" << std::endl;
    return fac*xs;
}

Eigen::MatrixXi mul(const Eigen::MatrixXi &xs, int    fac)
{
    std::cout << "Int" << std::endl;
    return fac*xs;
}

// ----------------
// Python interface
// ----------------

namespace py = pybind11;

PYBIND11_MODULE(example,m)
{
  m.doc() = "pybind11 example plugin";

  // N.B. the order here is crucial, in the reversed order every "int" is converted to a "double"
  m.def("mul", py::overload_cast<const Eigen::MatrixXi &,int   >(&mul) );
  m.def("mul", py::overload_cast<const Eigen::MatrixXd &,double>(&mul) );
}


//==============================================================================
//          Copyright 2014    Jean-Thierry Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_SIMD_COMMON_CUMMAX_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_COMMON_CUMMAX_HPP_INCLUDED

#include <boost/simd/swar/functions/cummax.hpp>
#include <boost/simd/swar/functions/cummin.hpp>
#include <boost/simd/include/functions/simd/insert.hpp>
#include <boost/simd/include/functions/scalar/max.hpp>
#include <boost/simd/include/functions/simd/extract.hpp>
#include <boost/simd/swar/include/functions/shuffle.hpp>
#include <boost/simd/swar/functions/details/cum_helper.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT ( cummax_, tag::cpu_
                           , (A0)(X)
                           , ((simd_<arithmetic_<A0>,X>))
                           )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return helper<meta::cardinal_of<A0>::value>::unroll(a0);
    }

    private:

    template<std::size_t CARD, std::size_t PASS = CARD>
    struct helper
    {
      BOOST_FORCEINLINE static A0 unroll(const A0 & a0)
      {
        A0 tmp =  helper<CARD, PASS/2>::unroll(a0);
        return max(tmp,  shuffle<details::cum_helper<CARD, PASS>>(tmp));
      }
    };

    template<std::size_t CARD> struct helper<CARD, 0>
    {
      BOOST_FORCEINLINE static A0 unroll(const A0 & a0)
      {
        return a0;
      }
    };

  };
} } }

#endif


#include <armadillo>
#include <vector>
#include <set>
#include <boost/algorithm/cxx11/none_of.hpp>

using namespace arma;
using namespace std;
using boost::algorithm::none_of_equal;

struct Camino {
    int ciudadFinal;
    vector<int> recorrido;
    double costoCamino;
};

bool operator<(const Camino& lhs, const Camino& rhs)
{
    return lhs.costoCamino < rhs.costoCamino;
}

ostream& operator<<(ostream& os, const Camino& camino)
{
    os << "Recorrido: ";

    for (int ciudad : camino.recorrido)
        os << ciudad << ' ';

    os << "\nCosto del camino: " << camino.costoCamino << endl;

    return os;
}

class ArbolBusqueda {
public:
    ArbolBusqueda(string rutaArchivoDistancias);

    Camino hacerBusqueda();

private:
    multiset<Camino> listaCaminos;
    unsigned int cantidadCiudades;
    mat distancias;
};

ArbolBusqueda::ArbolBusqueda(string rutaArchivoDistancias)
{
    distancias.load(rutaArchivoDistancias);

    if (!distancias.is_square())
        throw runtime_error("La matriz leída no es cuadrada");

    cantidadCiudades = distancias.n_rows;

    // Insertar caminos iniciales que empiecen en las distintas ciudades
    // Los insertamos de forma desordenada
    Camino puntoPartida;
    puntoPartida.ciudadFinal = randi(1, distr_param(1, cantidadCiudades)).at(0);
    puntoPartida.recorrido = {puntoPartida.ciudadFinal};
    puntoPartida.costoCamino = 0;

    listaCaminos.insert(puntoPartida);
}

Camino ArbolBusqueda::hacerBusqueda()
{
    while (!listaCaminos.empty()) {
        //  1 - Saco el primer elemento de la lista
        //      Si no hay elementos, no encontré una solución
        Camino camino = *listaCaminos.begin();
        listaCaminos.erase(listaCaminos.begin());

        //  2 - Me fijo si es un estado objetivo
        //      Si es un estado objetivo, encontré la solución
        // El camino va a ser la solución si la cantidad de nodos es igual
        // a la cantidad de ciudades + 1 (porque después de pasar por todas las
        // ciudades, tiene que volver al punto de partida).
        if (camino.recorrido.size() == cantidadCiudades + 1)
            return camino;

        // Si el recorrido ya tiene nCiudades, entonces ya recorrió todas,
        // y solo le falta volver a la ciudad inicial.
        // Este camino camino va a ser una posible solución.
        if (camino.recorrido.size() == cantidadCiudades) {
            Camino posibleSolucion{camino};

            posibleSolucion.ciudadFinal = posibleSolucion.recorrido.front();
            posibleSolucion.recorrido.push_back(posibleSolucion.recorrido.front());
            posibleSolucion.costoCamino += distancias.at(camino.ciudadFinal - 1,
                                                         posibleSolucion.ciudadFinal - 1);

            listaCaminos.insert(posibleSolucion);
        }
        //  3 - Expando los vecinos a los que puede ir y los meto en listaCaminos.
        //      Los nuevos Caminos son insertados para que lista quede ordenada por
        //      el costo de cada camino.
        //      Si no hay vecinos a expandir, no inserto nada.
        else
            for (unsigned int ciudad = 1; ciudad <= cantidadCiudades; ++ciudad) {

                // Si la ciudad no está en el recorrido hecho, entonces puedo viajar
                // a esa ciudad.
                if (none_of_equal(camino.recorrido, ciudad)) {

                    // Creo un nuevo camino en base al actual
                    Camino nuevoCamino{camino};

                    // Actualizo el nuevo camino con la información sobre la
                    // nueva ciudad a la que se viajó.
                    nuevoCamino.ciudadFinal = ciudad;
                    nuevoCamino.recorrido.push_back(ciudad);
                    nuevoCamino.costoCamino += distancias.at(camino.ciudadFinal - 1,
                                                             nuevoCamino.ciudadFinal - 1);

                    // Inserto el nuevo camino
                    listaCaminos.insert(nuevoCamino);
                }
            }
    }

    // Si se llegó hasta acá, es porque no se encontró una solución
    throw runtime_error("No se encontró una solución");
}