// Copyright Aleksey Gurtovoy 2003-2004
// Copyright David Abrahams 2003-2004
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id: map.cpp 49268 2008-10-11 06:26:17Z agurtovoy $
// $Date: 2008-10-11 02:26:17 -0400 (Sat, 11 Oct 2008) $
// $Revision: 49268 $

#include <boost/mpl/map.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/erase_key.hpp>
#include <boost/mpl/erase_key.hpp>
#include <boost/mpl/contains.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/clear.hpp>
#include <boost/mpl/has_key.hpp>
#include <boost/mpl/order.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/empty.hpp>
#include <boost/mpl/begin_end.hpp>
#include <boost/type_traits/is_same.hpp>

#include <boost/mpl/aux_/test.hpp>

MPL_TEST_CASE()
{
    typedef map2<
          mpl::pair<int,unsigned>
        , mpl::pair<char,unsigned char>
        > m_;

    typedef erase_key<m_,char>::type m;

    MPL_ASSERT_RELATION( size<m>::type::value, ==, 1 );
    MPL_ASSERT_NOT(( empty<m> ));
    MPL_ASSERT(( is_same< clear<m>::type,map0<> > ));

    MPL_ASSERT(( is_same< at<m,int>::type,unsigned > ));
    MPL_ASSERT(( is_same< at<m,char>::type,void_ > ));
    MPL_ASSERT(( contains< m,mpl::pair<int,unsigned> > ));
    MPL_ASSERT_NOT(( contains< m,mpl::pair<int,int> > ));
    MPL_ASSERT_NOT(( contains< m,mpl::pair<char,unsigned char> > ));

    MPL_ASSERT_NOT(( has_key<m,char>::type ));
    MPL_ASSERT(( has_key<m,int>::type ));

    MPL_ASSERT_NOT(( is_same< order<m,int>::type, void_ > ));
    MPL_ASSERT(( is_same< order<m,char>::type,void_ > ));

    typedef begin<m>::type first;
    typedef end<m>::type last;

    MPL_ASSERT(( is_same< deref<first>::type,mpl::pair<int,unsigned> > ));
    MPL_ASSERT(( is_same< next<first>::type,last > ));

    typedef insert<m,mpl::pair<char,long> >::type m2;

    MPL_ASSERT_RELATION( size<m2>::type::value, ==, 2 );
    MPL_ASSERT_NOT(( empty<m2>::type ));
    MPL_ASSERT(( is_same< clear<m2>::type,map0<> > ));
    MPL_ASSERT(( is_same< at<m2,int>::type,unsigned > ));
    MPL_ASSERT(( is_same< at<m2,char>::type,long > ));

    MPL_ASSERT(( contains< m2,mpl::pair<int,unsigned> > ));
    MPL_ASSERT_NOT(( contains< m2,mpl::pair<int,int> > ));
    MPL_ASSERT_NOT(( contains< m2,mpl::pair<char,unsigned char> > ));
    MPL_ASSERT(( contains< m2,mpl::pair<char,long> > ));

    MPL_ASSERT(( has_key<m2,char>::type ));
    MPL_ASSERT_NOT(( has_key<m2,long>::type ));
    MPL_ASSERT_NOT(( is_same< order<m2,int>::type, void_ > ));
    MPL_ASSERT_NOT(( is_same< order<m2,char>::type, void_ > ));
    MPL_ASSERT_NOT(( is_same< order<m2,char>::type, order<m2,int>::type > ));

    typedef begin<m2>::type first2;
    typedef end<m2>::type last2;

    MPL_ASSERT(( is_same<deref<first2>::type,mpl::pair<int,unsigned> > ));
    typedef next<first2>::type iter;
    MPL_ASSERT(( is_same<deref<iter>::type,mpl::pair<char,long> > ));
    MPL_ASSERT(( is_same< next<iter>::type,last2 > ));

    typedef insert<m2,mpl::pair<int,unsigned> >::type s2_1;
    MPL_ASSERT(( is_same<m2,s2_1> ));

    typedef insert<m2,mpl::pair<long,unsigned> >::type m3;
    MPL_ASSERT_RELATION( size<m3>::type::value, ==, 3 );
    MPL_ASSERT(( has_key<m3,long>::type ));
    MPL_ASSERT(( has_key<m3,int>::type ));
    MPL_ASSERT(( has_key<m3,char>::type ));
    MPL_ASSERT(( contains< m3,mpl::pair<long,unsigned> > ));
    MPL_ASSERT(( contains< m3,mpl::pair<int,unsigned> > ));

    typedef insert<m,mpl::pair<char,long> >::type m1;
    MPL_ASSERT_RELATION( size<m1>::type::value, ==, 2 );
    MPL_ASSERT(( is_same< at<m1,int>::type,unsigned > ));
    MPL_ASSERT(( is_same< at<m1,char>::type,long > ));

    MPL_ASSERT(( contains< m1,mpl::pair<int,unsigned> > ));
    MPL_ASSERT_NOT(( contains< m1,mpl::pair<int,int> > ));
    MPL_ASSERT_NOT(( contains< m1,mpl::pair<char,unsigned char> > ));
    MPL_ASSERT(( contains< m1,mpl::pair<char,long> > ));

    MPL_ASSERT(( is_same< m1,m2 > ));

    typedef erase_key<m1,char>::type m_1;
    MPL_ASSERT(( is_same<m,m_1> ));
    MPL_ASSERT_RELATION( size<m_1>::type::value, ==, 1 );
    MPL_ASSERT(( is_same< at<m_1,char>::type,void_ > ));
    MPL_ASSERT(( is_same< at<m_1,int>::type,unsigned > ));

    typedef erase_key<m3,char>::type m2_1;
    MPL_ASSERT_RELATION( size<m2_1>::type::value, ==, 2 );
    MPL_ASSERT(( is_same< at<m2_1,char>::type,void_ > ));
    MPL_ASSERT(( is_same< at<m2_1,int>::type,unsigned > ));
    MPL_ASSERT(( is_same< at<m2_1,long>::type,unsigned > ));
}

MPL_TEST_CASE()
{
    typedef map0<> m;

    MPL_ASSERT_RELATION( size<m>::type::value, ==, 0 );
    MPL_ASSERT(( empty<m>::type ));

    MPL_ASSERT(( is_same< clear<m>::type,map0<> > ));
    MPL_ASSERT(( is_same< at<m,char>::type,void_ > ));

    MPL_ASSERT_NOT(( has_key<m,char>::type ));
    MPL_ASSERT_NOT(( has_key<m,int>::type ));
    MPL_ASSERT_NOT(( has_key<m,UDT>::type ));
    MPL_ASSERT_NOT(( has_key<m,incomplete>::type ));

    MPL_ASSERT_NOT(( has_key<m,char const>::type ));
    MPL_ASSERT_NOT(( has_key<m,int const>::type ));
    MPL_ASSERT_NOT(( has_key<m,UDT const>::type ));
    MPL_ASSERT_NOT(( has_key<m,incomplete const>::type ));

    MPL_ASSERT_NOT(( has_key<m,int*>::type ));
    MPL_ASSERT_NOT(( has_key<m,UDT*>::type ));
    MPL_ASSERT_NOT(( has_key<m,incomplete*>::type ));

    MPL_ASSERT_NOT(( has_key<m,int&>::type ));
    MPL_ASSERT_NOT(( has_key<m,UDT&>::type ));
    MPL_ASSERT_NOT(( has_key<m,incomplete&>::type ));

    typedef insert<m,mpl::pair<char,int> >::type m1;
    MPL_ASSERT_RELATION( size<m1>::type::value, ==, 1 );
    MPL_ASSERT(( is_same< at<m1,char>::type,int > ));

    typedef erase_key<m,char>::type m0_1;
    MPL_ASSERT_RELATION( size<m0_1>::type::value, ==, 0 );
    MPL_ASSERT(( is_same< at<m0_1,char>::type,void_ > ));
}

// Use a template for testing so that GCC will show us the actual types involved
template <class M>
void test()
{
    MPL_ASSERT_RELATION( size<M>::value, ==, 3 );

    typedef typename end<M>::type not_found;
    BOOST_MPL_ASSERT_NOT(( is_same<BOOST_DEDUCED_TYPENAME find<M,mpl::pair<int,int*> >::type,not_found> ));
    BOOST_MPL_ASSERT_NOT(( is_same<BOOST_DEDUCED_TYPENAME find<M,mpl::pair<long,long*> >::type,not_found> ));
    BOOST_MPL_ASSERT_NOT(( is_same<BOOST_DEDUCED_TYPENAME find<M,mpl::pair<char,char*> >::type,not_found> ));
    BOOST_MPL_ASSERT(( is_same<BOOST_DEDUCED_TYPENAME find<M,int>::type,not_found> ));
};

MPL_TEST_CASE()
{
    typedef map< mpl::pair<int,int*> > map_of_1_pair;
    typedef begin<map_of_1_pair>::type iter_to_1_pair;

    BOOST_MPL_ASSERT((
        is_same<
             deref<iter_to_1_pair>::type
           , mpl::pair<int,int*>
        >
    ));

    typedef map<
        mpl::pair<int,int*>
      , mpl::pair<long,long*>
      , mpl::pair<char,char*>
    > mymap;

    test<mymap>();
    test<mymap::type>();
}


////////////////////////////////////////////////////////////////////////////////
#include <polyfem/PolyhedronQuadrature.hpp>
#include <polyfem/TetQuadrature.hpp>
#include <polyfem/MeshUtils.hpp>
#include <geogram/mesh/mesh_io.h>
#include <igl/writeMESH.h>
#ifdef POLYFEM_WITH_MMG
#include <boost/filesystem.hpp>
#endif
#include <vector>
#include <cassert>
#include <iostream>
////////////////////////////////////////////////////////////////////////////////

namespace polyfem
{

namespace
{

#ifdef POLYFEM_WITH_MMG

// try to remesh volume if polyhedron creates more than this # of quadrature points
const int max_num_quadrature_points = 2048;

bool mmg_remesh_volume(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F, const Eigen::MatrixXi &T,
					   Eigen::MatrixXd &TV, Eigen::MatrixXi &TF, Eigen::MatrixXi &TT)
{
	using namespace boost;

	double scaling = (V.colwise().maxCoeff() - V.colwise().minCoeff()).maxCoeff();
	Eigen::RowVector3d translation = V.colwise().minCoeff();

	auto tmp_dir = filesystem::temp_directory_path();
	auto base_path = tmp_dir / filesystem::unique_path("polyfem_%%%%-%%%%-%%%%-%%%%");
	auto f_input = base_path;
	f_input += "_in.mesh";
	auto f_output = base_path;
	f_output += "_out.mesh";
	auto f_sol = base_path;
	f_sol += "_out.sol";

	TV = (V.rowwise() - translation) / scaling;
	igl::writeMESH(f_input.string(), TV, T, F);

	std::string app(POLYFEM_MMG_PATH);
	std::string cmd = app + " -ar 20 -hausd 0.01 -v 0 -in " + f_input.string() + " -out " + f_output.string();
#ifndef WIN32
	cmd += " &> /dev/null";
#endif
	std::cout << "Running command:\n" + cmd << std::endl;
	if (::system(cmd.c_str()) == 0)
	{
		GEO::Mesh M;
		GEO::mesh_load(f_output.string(), M);
		from_geogram_mesh(M, TV, TF, TT);
		TV = (scaling * TV).rowwise() + translation;

		filesystem::remove(f_input);
		filesystem::remove(f_output);
		filesystem::remove(f_sol);
		return true;
	}
	else
	{
		filesystem::remove(f_input);
		return false;
	}
}

#endif

template <class TriMat>
double transform_pts(const TriMat &tri, const Eigen::MatrixXd &pts, Eigen::MatrixXd &transformed)
{
	Eigen::Matrix3d matrix;
	matrix.row(0) = tri.row(1) - tri.row(0);
	matrix.row(1) = tri.row(2) - tri.row(0);
	matrix.row(2) = tri.row(3) - tri.row(0);

	transformed = pts * matrix;

	transformed.col(0).array() += tri(0, 0);
	transformed.col(1).array() += tri(0, 1);
	transformed.col(2).array() += tri(0, 2);

	return matrix.determinant();
}

} // anonymous namespace

////////////////////////////////////////////////////////////////////////////////

void PolyhedronQuadrature::get_quadrature(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F,
										  const Eigen::RowVector3d &kernel, const int order, Quadrature &quadr)
{
	std::string flags = "Qpq2.0";
	Eigen::VectorXi J;
	Eigen::MatrixXd VV, OV, TV;
	Eigen::MatrixXi OF, TF, tets;

	Quadrature tet_quadr_pts;
	TetQuadrature tet_quadr;
	tet_quadr.get_quadrature(4, tet_quadr_pts);
	// assert(tet_quadr_pts.weights.minCoeff() >= 0);

	double scaling = (V.colwise().maxCoeff() - V.colwise().minCoeff()).maxCoeff();
	Eigen::RowVector3d translation = V.colwise().minCoeff();

	polyfem::tertrahedralize_star_shaped_surface(V, F, kernel, TV, TF, tets);

#ifdef POLYFEM_WITH_MMG
	if (tet_quadr_pts.weights.size() * tets.rows() > max_num_quadrature_points)
	{
		Eigen::MatrixXd V0;
		Eigen::MatrixXi F0, T0;
		bool res = mmg_remesh_volume(TV, TF, tets, V0, F0, T0);

		// std::cout << "points per tet: "<< tet_quadr_pts.weights.size()<< std::endl;
		// std::cout << "#T before: " << tets.rows() << std::endl;
		// std::cout << "#T after: " << T0.rows() << std::endl;
		// std::cout << "res: "<< res << std::endl;

		if (res && T0.rows() < tets.rows())
		{
			TV = V0;
			TF = F0;
			tets = T0;
		}
	}
#endif

	// igl::write_triangle_mesh("poly_current.obj", VV, F);
	// igl::simplify_polyhedron(VV, F, OV, OF, J);
	// igl::write_triangle_mesh("poly_out.obj", OV, OF);

	// OV = (OV * scaling).rowwise() + translation;
	// int res = igl::copyleft::tetgen::tetrahedralize(V, F, flags, TV, tets, TF);
	// std::cout << "tetgen out" << std::endl;
	// assert(res == 0);

	// static int counter = 0;
	// std::stringstream ss;
	// ss << std::setw(6) << std::setfill('0') << counter++;
	// std::string s = ss.str();

	// if (res != 0) {
	// 	std::cerr << "Tetgen did not succeed. Returned code: " << res << std::endl;
	// 	igl::write_triangle_mesh("poly_" + s + ".obj", V, F);
	// } else {
	// igl::writeMESH("tet_" ".mesh", TV, tets, TF);
	// }

	// GEO::Mesh M;
	// GEO::mesh_load("tet_.o.mesh", M);
	// from_geogram_mesh(M, TV, TF, tets);
	// std::cout << tets.rows() << std::endl;

	// std::cout << "volume: " << volume(M) << std::endl;

	const long offset = tet_quadr_pts.weights.rows();
	quadr.points.resize(tets.rows() * offset, 3);
	quadr.weights.resize(tets.rows() * offset, 1);

	Eigen::MatrixXd transformed_points;

	for (long i = 0; i < tets.rows(); ++i)
	{
		Eigen::Matrix<double, 4, 3> tetra;
		const auto &indices = tets.row(i);
		tetra.row(0) = TV.row(indices(0));
		tetra.row(1) = TV.row(indices(1));
		tetra.row(2) = TV.row(indices(2));
		tetra.row(3) = TV.row(indices(3));

		// viewer.data.add_edges(triangle.row(0), triangle.row(1), Eigen::Vector3d(1,0,0).transpose());
		// viewer.data.add_edges(triangle.row(0), triangle.row(2), Eigen::Vector3d(1,0,0).transpose());
		// viewer.data.add_edges(triangle.row(2), triangle.row(1), Eigen::Vector3d(1,0,0).transpose());

		const double det = transform_pts(tetra, tet_quadr_pts.points, transformed_points);
		assert(det > 0);

		quadr.points.block(i * offset, 0, transformed_points.rows(), transformed_points.cols()) = transformed_points;
		quadr.weights.block(i * offset, 0, tet_quadr_pts.weights.rows(), tet_quadr_pts.weights.cols()) = tet_quadr_pts.weights * det;
	}

	// assert(quadr.weights.minCoeff() >= 0);
	// std::cout<<"#quadrature points: " << quadr.weights.size()<<" "<<quadr.weights.sum()<<std::endl;
}

} // namespace polyfem


//  (C) Copyright John Maddock 2005.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifdef TEST_STD_HEADERS
#include <random>
#else
#include <boost/tr1/random.hpp>
#endif

#include <boost/type_traits/is_arithmetic.hpp>
#include <boost/cstdint.hpp>
#include <boost/static_assert.hpp>
#include "verify_return.hpp"
#include <iostream>

template <class T>
void check_uniform(T*)
{
   typedef typename T::result_type result_type;
   BOOST_STATIC_ASSERT(::boost::is_arithmetic<result_type>::value);

   T t;
   result_type r = 0;
   verify_return_type((t.min)(), r);
   verify_return_type((t.max)(), r);
   verify_return_type(t(), r);
}

struct seed_architype
{
   typedef unsigned result_type;
   unsigned operator()()const
   {
      return 0;
   }
};

unsigned seed_proc();

class uniform_random_generator_architype
{
public:
   typedef unsigned long result_type;
   result_type operator()()
   { return 0; }
   result_type min BOOST_PREVENT_MACRO_SUBSTITUTION()const
   { return 0; }
   result_type max BOOST_PREVENT_MACRO_SUBSTITUTION()const
   { return 0; }

   static uniform_random_generator_architype& get()
   {
      static uniform_random_generator_architype r;
      return r;
   }
private:
   uniform_random_generator_architype();
   uniform_random_generator_architype(const uniform_random_generator_architype&);
   uniform_random_generator_architype& operator=(const uniform_random_generator_architype&);
};

class pseudo_random_generator_architype
{
public:
   pseudo_random_generator_architype(){}
   pseudo_random_generator_architype(const pseudo_random_generator_architype&){}
   pseudo_random_generator_architype& operator=(const pseudo_random_generator_architype&)
   { return *this; }

   typedef unsigned long result_type;
   result_type operator()()
   { return 0; }
   result_type min BOOST_PREVENT_MACRO_SUBSTITUTION()const
   { return 0; }
   result_type max BOOST_PREVENT_MACRO_SUBSTITUTION()const
   { return 0; }

   pseudo_random_generator_architype(unsigned long){}
   template <class Gen> pseudo_random_generator_architype(Gen&){}
   void seed(){}
   void seed(unsigned long){}
   template <class Gen> void seed(Gen&){}

   bool operator == (const pseudo_random_generator_architype&)const
   { return false; }
   bool operator != (const pseudo_random_generator_architype&)const
   { return false; }

#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
   template<class CharT, class Traits>
   friend std::basic_ostream<CharT,Traits>&
   operator<<(std::basic_ostream<CharT,Traits>& os,
            const pseudo_random_generator_architype& lcg)
   {
      return os;
   }

   template<class CharT, class Traits>
   friend std::basic_istream<CharT,Traits>&
   operator>>(std::basic_istream<CharT,Traits>& is,
            pseudo_random_generator_architype& lcg)
   {
      return is;
   }
#endif
};

class random_distribution_architype
{
public:
   random_distribution_architype(){}
   random_distribution_architype(const random_distribution_architype&){}
   random_distribution_architype& operator=(const random_distribution_architype&)
   { return *this; }

   typedef unsigned input_type;
   typedef double result_type;

   void reset(){}

   template <class U>
   result_type operator()(U& u)
   {
      return u();
   }
#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
   template<class CharT, class Traits>
   friend std::basic_ostream<CharT,Traits>&
   operator<<(std::basic_ostream<CharT,Traits>& os,
            const random_distribution_architype& lcg)
   {
      return os;
   }

   template<class CharT, class Traits>
   friend std::basic_istream<CharT,Traits>&
   operator>>(std::basic_istream<CharT,Traits>& is,
            random_distribution_architype& lcg)
   {
      return is;
   }
#endif
};

template <class T>
void check_pseudo_random(T* p)
{
   typedef typename T::result_type result_type;
   check_uniform(p);
   T t1;
   T t2(t1);
   t1 = t2;
   unsigned long s = 0;
   T t3(s);
   seed_architype seed;
   T t4(seed);
   t1.seed();
   t1.seed(s);
   t1.seed(seed);
   T t5(seed_proc);
   t1.seed(seed_proc);
   const T& x = t1;
   const T& y = t2;
   verify_return_type(x == y, true);
   verify_return_type(x == y, false);
#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
   std::cout << t1;
   std::cin >> t1;
#endif
}

template <class T>
void check_random_distribution(T* pd)
{
   T t1(*pd);
   t1 = *pd;
   uniform_random_generator_architype& gen = uniform_random_generator_architype::get();
   typedef typename T::input_type input_type;
   typedef typename T::result_type result_type;
   t1.reset();
   verify_return_type(t1(gen), result_type());
   const T& ct = t1;
#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
   std::cout << ct << std::endl;
   std::cin >> t1 >> std::ws;
#endif
}

template <class VG>
void check_generator(VG* g)
{
   typedef typename VG::engine_type engine_type;
   typedef typename VG::engine_value_type engine_value_type;
   typedef typename VG::distribution_type distribution_type;
   typedef typename distribution_type::input_type input_type;
   typedef typename VG::result_type result_type;
   verify_return_type((*g)(), result_type(0));
   const VG* cg = g;
   verify_return_type(&g->engine(), static_cast<engine_value_type*>(0));
   verify_return_type(&cg->engine(), static_cast<const engine_value_type*>(0));
   verify_return_type(&g->distribution(), static_cast<distribution_type*>(0));
   verify_return_type(&cg->distribution(), static_cast<const distribution_type*>(0));
}

template <class VG>
void check_generator_extended(VG* g)
{
   typedef typename VG::engine_type engine_type;
   typedef typename VG::engine_value_type engine_value_type;
   typedef typename VG::distribution_type distribution_type;
   typedef typename distribution_type::input_type input_type;
   typedef typename VG::result_type result_type;
   //verify_return_type((*g)(input_type(0)), result_type(0));
   const VG* cg = g;
   verify_return_type((cg->min)(), result_type(0));
   verify_return_type((cg->max)(), result_type(0));
}

int main()
{
   typedef std::tr1::linear_congruential< ::boost::int32_t, 16807, 0, 2147483647> lc_t;
   lc_t lc;
   BOOST_STATIC_ASSERT(lc_t::multiplier == 16807);
   BOOST_STATIC_ASSERT(lc_t::increment == 0);
   BOOST_STATIC_ASSERT(lc_t::modulus == 2147483647);
   check_pseudo_random(&lc);

   typedef std::tr1::mersenne_twister< ::boost::uint32_t,32,351,175,19,0xccab8ee7,11,7,0x31b6ab00,15,0xffe50000,17> mt11213b;
   mt11213b mt;
   check_pseudo_random(&mt);

   typedef std::tr1::subtract_with_carry< ::boost::int32_t, 24, 10, 24> sub_t;
   sub_t sub;
   check_pseudo_random(&sub);

   typedef std::tr1::subtract_with_carry_01<float, 24, 10, 24> ranlux_base_01;
   ranlux_base_01 rl1;
   check_pseudo_random(&rl1);
   typedef std::tr1::subtract_with_carry_01<double, 48, 10, 24> ranlux64_base_01;
   ranlux64_base_01 rl2;
   check_pseudo_random(&rl2);

   typedef std::tr1::discard_block< std::tr1::subtract_with_carry< ::boost::int32_t , (1<<24), 10, 24>, 223, 24> ranlux3;
   ranlux3 rl3;
   check_pseudo_random(&rl3);

   std::tr1::xor_combine<pseudo_random_generator_architype, 0, pseudo_random_generator_architype, 1> xorc;
   check_pseudo_random(&xorc);
   verify_return_type(xorc.base1(), pseudo_random_generator_architype());
   verify_return_type(xorc.base2(), pseudo_random_generator_architype());

#ifndef __SUNPRO_CC
   // we don't normally allow workarounds in here, but this
   // class is unsupported on this platform.
   std::tr1::random_device d;
   check_uniform(&d);
   verify_return_type(d.entropy(), double(0));
#endif

   uniform_random_generator_architype& gen = uniform_random_generator_architype::get();
   std::tr1::uniform_int<unsigned long> ui;
   check_random_distribution(&ui);
   typedef std::tr1::uniform_int<unsigned long>::result_type ui_r_t;
   verify_return_type((ui.min)(), ui_r_t());
   verify_return_type((ui.max)(), ui_r_t());
   //verify_return_type(ui(gen, ui_r_t()), ui_r_t());

   std::tr1::bernoulli_distribution bd;
   verify_return_type(bd.p(), double(0));
   check_random_distribution(&bd);

   std::tr1::geometric_distribution<> gd;
   verify_return_type(gd.p(), double(0));
   check_random_distribution(&gd);
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::geometric_distribution<>::result_type, int>::value));
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::geometric_distribution<>::input_type, double>::value));
   std::tr1::geometric_distribution<long, double> gd2(0.5);
   verify_return_type(gd2.p(), double(0));
   check_random_distribution(&gd2);
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::geometric_distribution<long, double>::result_type, long>::value));
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::geometric_distribution<long, double>::input_type, double>::value));

   std::tr1::poisson_distribution<> pd;
   verify_return_type(pd.mean(), double(0));
   check_random_distribution(&pd);
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::poisson_distribution<>::result_type, int>::value));
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::poisson_distribution<>::input_type, double>::value));
   std::tr1::poisson_distribution<long, double> pd2(0.5);
   verify_return_type(pd2.mean(), double(0));
   check_random_distribution(&pd2);
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::poisson_distribution<long, double>::result_type, long>::value));
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::poisson_distribution<long, double>::input_type, double>::value));

   std::tr1::binomial_distribution<> bind;
   verify_return_type(bind.p(), double(0));
   verify_return_type(bind.t(), int(0));
   check_random_distribution(&bind);
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::binomial_distribution<>::result_type, int>::value));
   //BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::binomial_distribution<>::input_type, double>::value));
   std::tr1::binomial_distribution<long, double> bind2(1, 0.5);
   verify_return_type(bind2.t(), long(0));
   check_random_distribution(&bind2);
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::binomial_distribution<long, double>::result_type, long>::value));
   //BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::binomial_distribution<long, double>::input_type, double>::value));

   std::tr1::uniform_real<> urd;
   check_random_distribution(&urd);
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::uniform_real<>::result_type, double>::value));
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::uniform_real<>::input_type, double>::value));
   std::tr1::uniform_real<long double> urd2(0.5L, 1.5L);
   verify_return_type((urd2.min)(), (long double)(0));
   verify_return_type((urd2.max)(), (long double)(0));
   check_random_distribution(&urd2);
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::uniform_real<long double>::result_type, long double>::value));
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::uniform_real<long double>::input_type, long double>::value));

   std::tr1::exponential_distribution<> exd;
   check_random_distribution(&exd);
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::exponential_distribution<>::result_type, double>::value));
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::exponential_distribution<>::input_type, double>::value));
   std::tr1::exponential_distribution<long double> exd2(0.5L);
   verify_return_type(exd2.lambda(), (long double)(0));
   check_random_distribution(&exd2);
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::exponential_distribution<long double>::result_type, long double>::value));
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::exponential_distribution<long double>::input_type, long double>::value));

   std::tr1::normal_distribution<> normd;
   check_random_distribution(&normd);
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::normal_distribution<>::result_type, double>::value));
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::normal_distribution<>::input_type, double>::value));
   std::tr1::normal_distribution<long double> normd2(0.5L, 0.1L);
   verify_return_type(normd2.mean(), (long double)(0));
   verify_return_type(normd2.sigma(), (long double)(0));
   check_random_distribution(&normd2);
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::normal_distribution<long double>::result_type, long double>::value));
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::normal_distribution<long double>::input_type, long double>::value));

   std::tr1::gamma_distribution<> gammad;
   check_random_distribution(&gammad);
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::gamma_distribution<>::result_type, double>::value));
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::gamma_distribution<>::input_type, double>::value));
   std::tr1::gamma_distribution<long double> gammad2(0.5L);
   verify_return_type(gammad2.alpha(), (long double)(0));
   check_random_distribution(&gammad2);
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::gamma_distribution<long double>::result_type, long double>::value));
   BOOST_STATIC_ASSERT((::boost::is_same<std::tr1::gamma_distribution<long double>::input_type, long double>::value));

   //
   // variate_generator:
   //
   std::tr1::variate_generator<uniform_random_generator_architype&, random_distribution_architype> vg1(uniform_random_generator_architype::get(), random_distribution_architype());
   check_generator(&vg1);
   std::tr1::variate_generator<uniform_random_generator_architype&, std::tr1::uniform_int<> > vg2(uniform_random_generator_architype::get(), std::tr1::uniform_int<>());
   check_generator_extended(&vg2);
   std::tr1::variate_generator<uniform_random_generator_architype*, std::tr1::uniform_int<> > vg3(&uniform_random_generator_architype::get(), std::tr1::uniform_int<>());
   check_generator_extended(&vg3);
   return 0;
}


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          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 __OPTIMISATION2_FUNCTION_FUNCTION_HPP__
#define __OPTIMISATION2_FUNCTION_FUNCTION_HPP__

#include <libv/lma/ttt/traits/functor_dispatch.hpp>
#include <libv/lma/ttt/mpl/unique_cat.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/find_if.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/remove.hpp>
#include <boost/mpl/deref.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/int.hpp>
#include <boost/fusion/include/push_back.hpp>
#include <boost/fusion/include/pair.hpp>

#include <libv/lma/lm/trait/size.hpp>
#include <libv/lma/lm/container/container.hpp>

namespace lma
{
  template<class T, class R = void> struct InterpretResidu;
  template<class T> struct InterpretResidu<T&> : mpl::false_ { typedef T type; };
  template<class T, size_t N> struct InterpretResidu<T(&)[N]> : mpl::true_ { typedef std::array<T,N> type; };

  template<class T> inline T& to_array(T& array, mpl::false_ const&)
  {
    return array;
  }

  template<class T, size_t N> inline typename std::array<T,N>::_AT_Type::_Type& to_array(std::array<T,N>& residual, mpl::true_ const&)
  {
    return residual._M_elems;
  }

  template<class F> struct Function
  {
    typedef ttt::FunctorTrait<decltype(&F::operator())> FunctorTrait;
    typedef typename FunctorTrait::ParametersType ParametersType;
    typedef ParametersType type;
    typedef InterpretResidu<typename FunctorTrait::Residu> IR;
    typedef typename IR::type ErreurType;

    const F& functor;

    Function(const F& f):functor(f){}

    template<class Input> bool operator()(const Input& input, ErreurType& error) const
    {
      return ttt::call(functor,to_array(error,IR()),input);
    }
  };

  template<class F> struct FunctionInfo
  {
    typedef typename F::ParametersType ParametersType;
    typedef typename F::ErreurType ErreurType;

    template<class A,class B> struct Sum : mpl::plus<A,mpl::int_<Size<B>::value>> {};
    typedef mpl::fold<
          ParametersType,
          mpl::int_<0>,
          Sum<mpl::_1,mpl::_2>
               > DDL;

    enum { ddl = DDL::type::value };
    enum { erreur_size = Size<ErreurType>::value };
  };

  template<class F> Function<F> make_function(const F& f) { return Function<F>(f); }

  template<class T> struct DoNothing {typedef T type;};

  template<class F> struct CreateListArg : Function<F>::ParametersType {};

  template<class V, class F> struct CatFunctionParameters : ttt::UniqueCat<V,CreateListArg<F>> {};

  template<class L, class F> struct CreateListParam
  {
    typedef typename
    mpl::fold<
              L,
              mpl::vector<>,
              CatFunctionParameters<mpl::_1,mpl::_2>
             >::type l;
    typedef typename mpl::apply<F,l>::type type;
  };

  template<class L, class F> struct FindOrder
  {
    typedef typename
    mpl::find_if<
                  L,
                  mpl::is_sequence<mpl::_1>
                >::type iterator;

    template<class List, class Order> struct Result { typedef typename List::type list_function; typedef typename Order::type list_parameter;};

    typedef typename
    mpl::if_<
              boost::is_same<
                              iterator,
                              typename mpl::end<L>::type
                            >,
              Result<DoNothing<L>,CreateListParam<L,F>>,
              Result<mpl::remove<L,typename mpl::deref<iterator>::type>, mpl::apply<F,typename mpl::deref<iterator>::type>>
            >::type type;
  };
}

namespace ttt
{

}
#endif


//
//  Copyright Silvin Lubecki 2010
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//

#include "depend_pch.h"
#include "StronglyConnectedComponents.h"
#include "Filter_ABC.h"
#include "StronglyConnectedComponentsVisitor_ABC.h"
#include "ComponentVisitor_ABC.h"
#include <boost/foreach.hpp>
#include <map>

using namespace depend;

// -----------------------------------------------------------------------------
// Name: StronglyConnectedComponents constructor
// Created: SLI 2010-08-23
// -----------------------------------------------------------------------------
StronglyConnectedComponents::StronglyConnectedComponents( const Visitable< DependencyVisitor_ABC >& metric, const Filter_ABC& filter )
    : filter_( filter )
{
    metric.Apply( *this );
}

// -----------------------------------------------------------------------------
// Name: StronglyConnectedComponents destructor
// Created: SLI 2010-08-23
// -----------------------------------------------------------------------------
StronglyConnectedComponents::~StronglyConnectedComponents()
{
    // NOTHING
}

namespace
{
    class Component : public Visitable< ComponentVisitor_ABC >
    {
    public:
        Component( const std::vector< std::string >& units )
            : units_( units )
        {}
        virtual void Apply( ComponentVisitor_ABC& visitor ) const
        {
            BOOST_FOREACH( const std::string& unit, units_ )
                visitor.NotifyUnit( unit );
        }
    private:
        const std::vector< std::string >& units_;
    };
    template< typename T, typename U >
    void Visit( T& components, const U& labels, const Filter_ABC& filter, StronglyConnectedComponentsVisitor_ABC& visitor )
    {
        typedef std::vector< std::string > T_Dependencies;
        typedef std::map< typename T::key_type, T_Dependencies > T_Components;
        T_Components sorted_components;
        BOOST_FOREACH( const typename T::value_type& component, components )
        {
            typename U::const_iterator it = labels.find( component.first );
            if( filter.Check( it->second ) )
                sorted_components[ component.second ].push_back( it->second );
        }
        BOOST_FOREACH( const typename T_Components::value_type& component, sorted_components )
            if( component.second.size() > 1 )
                visitor.NotifyComponent( Component( component.second ) );
    }
}

// -----------------------------------------------------------------------------
// Name: StronglyConnectedComponents::Apply
// Created: SLI 2011-04-06
// -----------------------------------------------------------------------------
void StronglyConnectedComponents::Apply( StronglyConnectedComponentsVisitor_ABC& visitor ) const
{
    typedef std::map< T_Graph::vertex_descriptor, T_Graph::vertices_size_type > T_Map;
    typedef boost::associative_property_map< T_Map > T_PropertyMap;
    T_Map mymap;
    T_PropertyMap pmap( mymap );
    boost::strong_components( graph_.graph(), pmap );
    ::Visit( mymap, labels_, filter_, visitor );
}

// -----------------------------------------------------------------------------
// Name: StronglyConnectedComponents::NotifyInternalDependency
// Created: SLI 2010-08-23
// -----------------------------------------------------------------------------
void StronglyConnectedComponents::NotifyInternalDependency( const std::string& fromModule, const std::string& toModule, const std::string& /*context*/ )
{
    labels_[ boost::add_vertex( fromModule, graph_ ) ] = fromModule;
    labels_[ boost::add_vertex( toModule, graph_ ) ] = toModule;
    boost::add_edge_by_label( fromModule, toModule, graph_ );
}

// -----------------------------------------------------------------------------
// Name: StronglyConnectedComponents::NotifyExternalDependency
// Created: SLI 2010-08-23
// -----------------------------------------------------------------------------
void StronglyConnectedComponents::NotifyExternalDependency( const std::string& /*fromModule*/, const std::string& /*toModule*/, const std::string& /*context*/ )
{
    // NOTHING
}


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

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include <tsid/math/utils.hpp>
#include <tsid/trajectories/trajectory-se3.hpp>
#include <tsid/trajectories/trajectory-euclidian.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_trajectory_se3 )
{
  using namespace tsid;
  using namespace trajectories;
  using namespace math;
  using namespace std;
  using namespace Eigen;
  using namespace pinocchio;

  SE3 M_ref = SE3::Identity();
  VectorXd M_vec(12);
  SE3ToVector(M_ref, M_vec);
  VectorXd zero = VectorXd::Zero(6);

  TrajectoryBase *traj = new TrajectorySE3Constant("traj_se3", M_ref);
  BOOST_CHECK(traj->has_trajectory_ended());
  BOOST_CHECK(traj->computeNext().pos.isApprox(M_vec));
  BOOST_CHECK(traj->operator ()(0.0).pos.isApprox(M_vec));

  TrajectorySample sample(12, 6);
  traj->getLastSample(sample);
  BOOST_CHECK(sample.pos.isApprox(M_vec));
  BOOST_CHECK(sample.vel.isApprox(zero));
  BOOST_CHECK(sample.acc.isApprox(zero));
}

BOOST_AUTO_TEST_CASE ( test_trajectory_euclidian )
{
  using namespace tsid;
  using namespace trajectories;
  using namespace std;
  using namespace Eigen;

  const unsigned int n = 5;
  VectorXd q_ref = VectorXd::Ones(n);
  VectorXd zero = VectorXd::Zero(n);
  TrajectoryBase *traj = new TrajectoryEuclidianConstant("traj_eucl", q_ref);

  BOOST_CHECK(traj->has_trajectory_ended());
  BOOST_CHECK(traj->computeNext().pos.isApprox(q_ref));
  BOOST_CHECK(traj->operator ()(0.0).pos.isApprox(q_ref));

  TrajectorySample sample(n);
  traj->getLastSample(sample);
  BOOST_CHECK(sample.pos.isApprox(q_ref));
  BOOST_CHECK(sample.vel.isApprox(zero));
  BOOST_CHECK(sample.acc.isApprox(zero));
}

BOOST_AUTO_TEST_SUITE_END ()


#ifndef SM_ROTATIONAL_KINEMATICS_HPP
#define SM_ROTATIONAL_KINEMATICS_HPP

#include <Eigen/Core>
#include <boost/shared_ptr.hpp>
#include <sm/assert_macros.hpp>
#include "rotations.hpp"

namespace sm { namespace kinematics {
    class RotationalKinematics
    {
    public:
      SM_DEFINE_EXCEPTION(Exception,std::runtime_error);
      virtual ~RotationalKinematics();
      typedef ::boost::shared_ptr<RotationalKinematics> Ptr;
      typedef ::boost::shared_ptr<const RotationalKinematics> ConstPtr;

      // Three functions that must be implemented.
      virtual Eigen::Matrix3d parametersToRotationMatrix(const Eigen::Vector3d & parameters, Eigen::Matrix3d * S = NULL) const = 0;
      virtual Eigen::Vector3d rotationMatrixToParameters(const Eigen::Matrix3d & rotationMatrix) const = 0;
      virtual Eigen::Matrix3d parametersToSMatrix(const Eigen::Vector3d & parameters) const = 0;
      virtual Eigen::Vector3d angularVelocityAndJacobian(const Eigen::Vector3d & /* p */, const Eigen::Vector3d & /* pdot */, Eigen::Matrix<double,3,6> * /* Jacobian */) const {SM_THROW(Exception,"Not implemented"); return Eigen::Vector3d::Zero();}
    };

  }} // namespace sm::kinematics

#endif


/* -----------------------------------------------------------------
 * File:    Recipe.cpp
 * Author:  Michael Gharbi <gharbi@mit.edu>
 * Created: 2015-07-24
 * -----------------------------------------------------------------
 *
 *
 *
 * ---------------------------------------------------------------*/

#include <cmath>
#include <algorithm>

#ifndef __ANDROID__
    #include <armadillo>
    #include "utils/image_io.h"
#endif

// HALIDE
#include "utils/static_image.h"

#include "recipe/hl_reconstruct.h"
#include "recipe/hl_lowpass.h"
#include "recipe/hl_highpass.h"
#include "recipe/hl_compute_features.h"
#include "recipe/hl_precompute_pyramid.h"

#include "print_helper.h"
#include "perf_measure.h"

#include "recipe/Recipe.h"

namespace xform {

Recipe::Recipe( Image<uint32_t>& unprocessed ) {
    m_model_width  = static_cast<int>(ceil(2.0f*unprocessed.width()/XFORM_WSIZE));
    m_model_height = static_cast<int>(ceil(2.0f*unprocessed.height()/XFORM_WSIZE));
    m_lp_width     = unprocessed.width()/XFORM_WSIZE;
    m_lp_height    = unprocessed.height()/XFORM_WSIZE;
    m_unprocessed_channels = 3;
    m_processed_channels   = 3;
}

#ifndef __ANDROID__
Recipe::Recipe( Image<uint32_t>& unprocessed, Image<uint32_t>& processed ):
    Recipe(unprocessed)
{
    fit(unprocessed, processed);
}
#endif

#ifndef __ANDROID__
void Recipe::fit( Image<uint32_t>& unprocessed, Image<uint32_t>& processed )
{
    PRINT("Fit recipe\n");
    m_lp_residual = std::make_shared<Image<uint32_t> >(m_lp_width,m_lp_height,m_processed_channels);
    m_hp_coefs    = std::make_shared<Image<float> >(m_model_width,m_model_height,nCoefMaps());
    m_qtable      = std::vector<float>(2*nCoefMaps());

    int width  = unprocessed.width();
    int height = unprocessed.height();

    Image<float> processed_hp(width, height, 3);

    // Compute and store lowpass of the processed image
    auto start = get_time();
    hl_lowpass(processed, *m_lp_residual);
    PRINT("  - lowpass:\t %ldms\n", get_duration(start,get_time()));

    // Compute highpass of the output
    start = get_time();
    hl_highpass(processed, *m_lp_residual, processed_hp);
    PRINT("  - highpass:\t %ldms\n", get_duration(start,get_time()));

    // Features
    start = get_time();
    Image<float> features(width, height, nFeatureChannels());
    hl_compute_features(unprocessed, features);
    PRINT("  - features:\t %ldms\n", get_duration(start,get_time()));

    start = get_time();
    regression(features, processed_hp);
    PRINT("  - regression:\t %ldms\n", get_duration(start,get_time()));

    quantize();
}
#endif

void Recipe::precompute_features( const Image<uint32_t>& unprocessed)
{
    m_lp_unprocessed = Image<uint32_t>(m_lp_width, m_lp_height);
    m_hp_unprocessed = Image<float>(unprocessed.width(), unprocessed.height(), 3);

    auto start = get_time();
    hl_lowpass(unprocessed, m_lp_unprocessed);
    PRINT("  - lowpass : %ldms\n", get_duration(start,get_time()));

    start = get_time();
    hl_highpass(unprocessed, m_lp_unprocessed, m_hp_unprocessed);
    PRINT("  - highpass : %ldms\n", get_duration(start,get_time()));

    start = get_time();
    m_pyramid_unprocessed = Image<float>(unprocessed.width(), unprocessed.height(), nPyrLevels()-1);
    hl_precompute_pyramid(unprocessed,m_pyramid_unprocessed);
    PRINT("  - precompute pyramid : %ldms\n", get_duration(start,get_time()));
}

void Recipe::reconstruct_with_features( Image<uint32_t> &output )
{
    dequantize();

    output = Image<uint32_t>(m_pyramid_unprocessed.width(),
            m_pyramid_unprocessed.height(), m_processed_channels);

    auto start = get_time();
    hl_reconstruct(
            m_pyramid_unprocessed,
            m_hp_unprocessed,
            *m_hp_coefs,
            *m_lp_residual,
            output
    );
    PRINT("  - reconstruct precomputed: %ldms\n", get_duration(start,get_time()));
}

void Recipe::reconstruct_image( const Image<uint32_t>& unprocessed, Image<uint32_t> &output )
{
    precompute_features(unprocessed);
    reconstruct_with_features(output);
}

#ifndef __ANDROID__
void Recipe::regression( Image<float>& features, Image<float>& target)
{
    const int h_mdl           = m_model_height;
    const int w_mdl           = m_model_width;
    const int h               = target.height();
    const int w               = target.width();
    const int n_targets       = target.channels();
    const int n_lumCoefs      = nLumCoefs();
    const int n_chromCoefs    = nChromCoefs();
    const int n_features      = features.channels();

    float* pDataFeat   = features.data();
    float* pDataTarget = target.data();
    float* pLuminCoef  = m_hp_coefs->data();
    float* pChromCoef  = pLuminCoef + h_mdl*w_mdl*n_lumCoefs;

    int wSize = XFORM_WSIZE;
    int step  = XFORM_WSIZE/2;

    #pragma omp parallel for
    for (int patch_idx = 0; patch_idx < h_mdl*w_mdl; ++patch_idx)
    {
        // temporary storage for the current patch
        float* X = new float[wSize*wSize*n_lumCoefs]();
        float* Y = new float[wSize*wSize*n_targets]();

        // Patch indices (in the recipe)
        int patch_x = patch_idx % w_mdl;
        int patch_y = patch_idx / w_mdl;

        // Pixel indices (in the image)
        int x_min   = patch_x*step;
        int x_max   = min(x_min+wSize, w);
        int y_min   = patch_y*step;
        int y_max   = min(y_min+wSize, h);
        int n_samples = (y_max-y_min)*(x_max-x_min);

        // Copy patch features
        int idx = 0;
        for (int f = 0; f < n_features; ++f)
        for (int y = y_min; y < y_max; ++y)
        for (int x = x_min; x < x_max; ++x)
        {
            X[idx] = pDataFeat[x+y*w+f*h*w];
            ++idx;
        }

        // Make features for the non-linear luminance curve
        int first_curve = idx;
        auto minmax     = std::minmax_element(X,X + n_samples);
        if(!XFORM_ADAPTIVE_LUMA_RANGE) {
            *minmax.first = 0.0f;
            *minmax.second = 1.0f;
        }
        float step_sz   = (*minmax.second-*minmax.first)/XFORM_LUMA_BANDS;
        float mini      = *minmax.first;
        for (int s = 0; s < XFORM_LUMA_BANDS-1; ++s){
            float thresh = mini+(s+1)*step_sz;
            for (idx = 0; idx < n_samples; ++idx)
            {
                X[first_curve + n_samples*s + idx] = std::max(X[idx]-thresh, 0.0f);
            }
        }

        // Copy target variables
        idx = 0;
        for (int f = 0; f < n_targets; ++f)
        for (int y = y_min; y < y_max; ++y)
        for (int x = x_min; x < x_max; ++x)
        {
            Y[idx] = pDataTarget[x+y*w+f*h*w];
            ++idx;
        }

        // Solve least-square regression
        arma::Mat<float> matX(X, n_samples,n_lumCoefs, false);
        arma::Mat<float> matY(Y, n_samples,n_targets, false);

        // For the luminance
        arma::Mat<float> result_lumin;
        arma::Mat<float> Xview = matX.cols(0,n_lumCoefs-1);
        arma::Mat<float> Yview = matY.cols(0,0);
        arma::Mat<float> regularizer = arma::eye<arma::Mat<float>>(n_lumCoefs, n_lumCoefs);
        regularizer(3,3) = 0.0f; //do not regularize affine offset
        arma::Mat<float> lhs = arma::trans(Xview) * Xview + XFORM_EPSILON*regularizer;
        arma::Mat<float> rhs = arma::trans(Xview) * Yview;
        arma::solve(result_lumin,lhs,rhs);

        // For the chrominance
        arma::Mat<float> result_chrom;
        Xview = matX.cols(0,n_chromCoefs-1);
        Yview = matY.cols(1,2);
        regularizer = arma::eye<arma::Mat<float>>(n_chromCoefs, n_chromCoefs);
        regularizer(3,3) = 0.0f; //do not regularize affine offset
        lhs = arma::trans(Xview) * Xview + XFORM_EPSILON*regularizer;
        rhs = arma::trans(Xview) * Yview;
        arma::solve(result_chrom,lhs,rhs);

        // Fill-in the luminance regression coefficients
        float* coefs = result_lumin.memptr();
        idx = 0;
        for (int f = 0; f < n_lumCoefs; ++f)
        {
            pLuminCoef[patch_x+patch_y*w_mdl+f*h_mdl*w_mdl] = coefs[idx];
            ++idx;
        }

        // Fill in the chrominance regression coefficients
        coefs = result_chrom.memptr();
        idx = 0;
        for (int c = 0; c < 2; ++c)
        for (int f = 0; f < n_chromCoefs; ++f)
        {
            pChromCoef[patch_x + patch_y*w_mdl + f*h_mdl*w_mdl + c*n_chromCoefs*h_mdl*w_mdl] = coefs[idx];
            ++idx;
        }
        coefs = nullptr;

        delete[] X; X = nullptr;
        delete[] Y; Y = nullptr;
    }
    pDataTarget = nullptr;
    pDataFeat   = nullptr;
}
#endif

void Recipe::quantize()
{
    const int h_mdl   = m_model_height;
    const int w_mdl   = m_model_width;
    const int n_feats = nCoefMaps();

    // Get values range per regression channel
    float* pCoef = m_hp_coefs->data();

    // Compute min and max for each channel
    for (int f = 0; f < n_feats; ++f){
        m_qtable[2*f]   = pCoef[f*h_mdl*w_mdl];
        m_qtable[2*f+1] = pCoef[f*h_mdl*w_mdl];
        for (int y = 0; y < h_mdl; ++y)
        for (int x = 0; x < w_mdl; ++x)
        {
            float val = pCoef[x + y*w_mdl + f*h_mdl*w_mdl];
            // min
            m_qtable[2*f] = min(m_qtable[2*f],val);
            // max
            m_qtable[2*f+1] = max(m_qtable[2*f+1],val);
        }
    }

    // Quantize
    for (int f = 0; f < n_feats; ++f)
    {
        float min = (m_qtable)[2*f];
        float max = (m_qtable)[2*f+1];
        float inv_rng = 1.0f/(max-min);
        for (int y = 0; y < h_mdl; ++y)
        for (int x = 0; x < w_mdl; ++x)
        {
            float *val = pCoef + (x+y*w_mdl+f*h_mdl*w_mdl);
            *val -= min;
            *val *= inv_rng;
            *val = floor(255.0f*(*val)+0.5f);
        }
    }
}

void Recipe::dequantize()
{
    const int h_mdl        = m_model_height;
    const int w_mdl        = m_model_width;
    const int n_feats = nCoefMaps();

    float* pCoef  = m_hp_coefs->data();

    // Dequantize
    for (int f = 0; f < n_feats; ++f)
    {
        float min = m_qtable[2*f];
        float max = m_qtable[2*f+1];
        float rng = (max-min)/255.0f;
        for (int y = 0; y < h_mdl; ++y)
        for (int x = 0; x < w_mdl; ++x) {
            float *val = pCoef + (x+y*w_mdl+f*h_mdl*w_mdl);
            *val *= rng;
            *val += min;
        }
    }
}

} //namespace xform


//=======================================================================
// Copyright 2002 Indiana University.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <boost/config.hpp>
#include <boost/concept_archetype.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/graph/graph_archetypes.hpp>

typedef boost::default_constructible_archetype<
    boost::sgi_assignable_archetype<> >
    dist_value;

// So generate_infinity works...
namespace std
{
template <> class numeric_limits< dist_value >
{
public:
    static dist_value max BOOST_PREVENT_MACRO_SUBSTITUTION()
    {
        return boost::static_object< dist_value >::get();
    }
};
}

dist_value abs(const dist_value& x) { return x; }
std::size_t abs(std::size_t x) { return x; }

int main()
{
    using namespace boost;
    typedef default_constructible_archetype<
        sgi_assignable_archetype< equality_comparable_archetype<> > >
        vertex_t;
    {
        typedef incidence_graph_archetype< vertex_t, directed_tag,
            allow_parallel_edge_tag >
            IncidenceGraph;
        typedef vertex_list_graph_archetype< vertex_t, directed_tag,
            allow_parallel_edge_tag, IncidenceGraph >
            graph_t;
        graph_t& g = static_object< graph_t >::get();
        vertex_t s;
        typedef graph_traits< graph_t >::edge_descriptor edge_t;
        readable_property_map_archetype< edge_t, std::size_t > weight;
        readable_property_map_archetype< vertex_t, int > index;
        read_write_property_map_archetype< vertex_t, std::size_t > distance;
        dijkstra_shortest_paths(g, s,
            vertex_index_map(index).weight_map(weight).distance_map(distance));

        dijkstra_shortest_paths_no_color_map(g, s,
            vertex_index_map(index).weight_map(weight).distance_map(distance));
    }
    {
        typedef incidence_graph_archetype< vertex_t, directed_tag,
            allow_parallel_edge_tag >
            IncidenceGraph;
        typedef vertex_list_graph_archetype< vertex_t, directed_tag,
            allow_parallel_edge_tag, IncidenceGraph >
            Graph;
        vertex_t s;
        typedef graph_traits< Graph >::edge_descriptor edge_t;
        readable_property_map_archetype< edge_t, std::size_t > weight;
        typedef property_graph_archetype< Graph, vertex_index_t, std::size_t >
            graph_t;
        graph_t& g = static_object< graph_t >::get();
        read_write_property_map_archetype< vertex_t, vertex_t > pred;
        dijkstra_shortest_paths(g, s, predecessor_map(pred).weight_map(weight));

        dijkstra_shortest_paths_no_color_map(
            g, s, predecessor_map(pred).weight_map(weight));
    }
    {
        typedef incidence_graph_archetype< vertex_t, directed_tag,
            allow_parallel_edge_tag >
            IncidenceGraph;
        typedef vertex_list_graph_archetype< vertex_t, directed_tag,
            allow_parallel_edge_tag, IncidenceGraph >
            Graph;
        vertex_t s;
        typedef property_graph_archetype< Graph, edge_weight_t, std::size_t >
            graph_t;
        graph_t& g = static_object< graph_t >::get();
        read_write_property_map_archetype< vertex_t, vertex_t > pred;
        readable_property_map_archetype< vertex_t, int > index;
        dijkstra_shortest_paths(
            g, s, predecessor_map(pred).vertex_index_map(index));

        dijkstra_shortest_paths_no_color_map(
            g, s, predecessor_map(pred).vertex_index_map(index));
    }
    {
        typedef incidence_graph_archetype< vertex_t, directed_tag,
            allow_parallel_edge_tag >
            IncidenceGraph;
        typedef vertex_list_graph_archetype< vertex_t, directed_tag,
            allow_parallel_edge_tag, IncidenceGraph >
            graph_t;
        graph_t& g = static_object< graph_t >::get();
        vertex_t s;
        typedef graph_traits< graph_t >::edge_descriptor edge_t;
        readable_property_map_archetype< edge_t, dist_value > weight;
        readable_property_map_archetype< vertex_t, int > index;
        read_write_property_map_archetype< vertex_t, color_value_archetype >
            color;
        read_write_property_map_archetype< vertex_t, dist_value > distance;
        typedef binary_function_archetype< dist_value, dist_value, dist_value >
            Combine;
        Combine combine = static_object< Combine >::get();
        typedef binary_predicate_archetype< dist_value, dist_value > Compare;
        Compare compare = static_object< Compare >::get();
        dijkstra_visitor<> vis;

        dijkstra_shortest_paths(g, s,
            color_map(color)
                .vertex_index_map(index)
                .weight_map(weight)
                .distance_map(distance)
                .distance_combine(combine)
                .distance_compare(compare)
                .visitor(vis));

        dijkstra_shortest_paths_no_color_map(g, s,
            vertex_index_map(index)
                .weight_map(weight)
                .distance_map(distance)
                .distance_combine(combine)
                .distance_compare(compare)
                .visitor(vis));
    }
    return 0;
}


//==============================================================================
//         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 BOOST_SIMD_SWAR_FUNCTIONS_SIMD_COMMON_SPLIT_LOW_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_COMMON_SPLIT_LOW_HPP_INCLUDED

#include <boost/simd/swar/functions/split_low.hpp>
#include <boost/simd/include/functions/simd/extract.hpp>
#include <boost/simd/include/functions/simd/make.hpp>
#include <boost/simd/preprocessor/make.hpp>
#include <boost/simd/sdk/meta/is_upgradable.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/upgrade.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT_IF         ( split_low_
                                      , tag::cpu_
                                      , (A0)(X)
                                      , (simd::meta::is_upgradable<A0>)
                                      , ((simd_<arithmetic_<A0>,X>))
                                      )
  {
    typedef typename dispatch::meta::upgrade<A0>::type  result_type;
    typedef typename meta::scalar_of<result_type>::type sA1;

    BOOST_FORCEINLINE result_type operator()(const A0& a0) const
    {
      return eval(a0,typename simd::meta::cardinal_of<result_type>::type());
    }

    #define M0(z, n, t) static_cast<sA1>(extract<n>(a0))

    BOOST_SIMD_PP_IMPLEMENT_WITH_MAKE(1, M0)

    #undef M0
  };
} } }

#endif


/// \file   python_module.cpp
///
/// \brief
///
/// \authors    Maarten P. Scholl
/// \date       2020-08-30
/// \copyright  Copyright 2017-2020 The Institute for New Economic Thinking,
///             Oxford Martin School, University of Oxford
///
///             Licensed under the Apache License, Version 2.0 (the "License");
///             you may not use this file except in compliance with the License.
///             You may obtain a copy of the License at
///
///                 http://www.apache.org/licenses/LICENSE-2.0
///
///             Unless required by applicable law or agreed to in writing,
///             software distributed under the License is distributed on an "AS
///             IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
///             express or implied. See the License for the specific language
///             governing permissions and limitations under the License.
///
///             You may obtain instructions to fulfill the attribution
///             requirements in CITATION.cff
///
#include <esl/mathematics/python_module_mathematics.hpp>

#ifdef WITH_PYTHON

#include <esl/mathematics/variable.hpp>

#define BOOST_BIND_GLOBAL_PLACEHOLDERS
#include <boost/python.hpp>
using namespace boost::python;

#include <adept/Stack.h>

typedef adept::internal::BinaryOperation<double, adept::Active<double>, adept::internal::Multiply, adept::Active<double> > binary_operation_t;

using namespace esl;

BOOST_PYTHON_MODULE(_mathematics)
{
    class_<variable>( "variable", init<>())
        .def(init<double>())
        .def(self == self)
        .def(self != self)
        .def(self < self)
        .def(self <= self)
        .def(self > self)
        .def(self >= self)
        .def(self + self)
        .def(self += self)
        .def(self - self)
        .def(self -= self)
        .def(self * self)
        .def(self *= self)
        .def(self / self)
        .def(self /= self)
        //.def("value", &variable::value)
        ;

    class_<adept::Stack>("stack", init<>())
        .def("pause_recording", &adept::Stack::pause_recording)
        .def("continue_recording", &adept::Stack::continue_recording)
        .def("new_recording", &adept::Stack::new_recording)
        .def("compute_adjoint", &adept::Stack::compute_adjoint)

    ;

    // TODO: verify that all opeartions on this are exported
    class_<binary_operation_t>("binary_operation_t", no_init)
        .def(self == self)
        .def(self != self)
        .def(self < self)
        .def(self <= self)
        .def(self > self)
        .def(self >= self)
        .def(self + self)
       // .def(self + variable())
        .def(self - self)
     //   .def(self - variable())
        .def(self * self)
      //  .def(self * variable())
        .def(self / self)
    //    .def(self / variable())
        ;

// TODO: export these
//    def("get_gradients", &adept::get_gradients);
//    stack_.independent(&active_[0], active_.size());
//    stack_.dependent(&values_[0], values_.size());
//    stack_.jacobian(jacobian);

}

#endif


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

#include "position_decoder.hpp"

#include <libpml/empty_parameter_config.hpp>
#include <libpml/listener_position.hpp>
#include <libpml/string_parameter.hpp>

#include <libvisr/parameter_type.hpp>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <algorithm>
#include <iostream>
#include <limits>
#include <sstream>

namespace visr
{
namespace rcl
{

  PositionDecoder::PositionDecoder( SignalFlowContext const & context,
                                    char const * name,
                                    CompositeComponent * parent,
                                    panning::XYZ const &offsetKinect,
                                    float qw /*=1.0f*/,
                                    float qx /*= 0.0f*/,
                                    float qy /*= 0.0f*/,
                                    float qz /*= 0.0f*/ )
  : AtomicComponent( context, name, parent )
  , mDatagramInput( "messageInput", *this, pml::EmptyParameterConfig() )
  , mPositionOutput( "positionOutput", *this, pml::EmptyParameterConfig() )
{
    mOffsetKinect = offsetKinect;
    mQw = qw;
    mQx = qx;
    mQy = qy;
    mQz = qz;
}

PositionDecoder::~PositionDecoder()
{
}

namespace // unnamed
{
/**
 * Parse a listener ID from a JSON representation
 * @param inputStream The test stream to be parsed.
 * @param pos [out] Object returning the parsed position data.
 */
void parseJSON(std::istream &  inputStream, pml::ListenerPosition & pos)
{
  namespace pt = boost::property_tree;

  pt::ptree tree;
  pt::read_json( inputStream, tree );

  pos.setTimeNs( tree.get<pml::ListenerPosition::TimeType>( "nTime" ) * 100 );
  pos.setFaceID( tree.get<pml::ListenerPosition::IdType>( "iFace" ) );
  pos.set( tree.get<pml::ListenerPosition::Coordinate>( "headJoint.X" ),
          tree.get<pml::ListenerPosition::Coordinate>( "headJoint.Y" ),
          tree.get<pml::ListenerPosition::Coordinate>( "headJoint.Z" ) );
  // TODO: Implement parsing of orientation
  pos.setOrientation( 0.0f, 0.0f, 0.0f );
}

}

void PositionDecoder::process()
{
  pml::ListenerPosition newPos;
  pml::ListenerPosition foundPos;
  pml::ListenerPosition::IdType smallestFaceId = std::numeric_limits<unsigned int>::max();
  pml::ListenerPosition::TimeType latestTimeStamp = 0;
  while( !mDatagramInput.empty() )
  {
    char const * nextMsg = mDatagramInput.front().str();
    std::stringstream msgStream( nextMsg );
    try
    {
      parseJSON( msgStream, newPos );
      // within each iteration, use only the position with the samllest timestamp (i.e., the face which has been within the view
      // of the tracker for the longest time.
      if( newPos.faceID() <= smallestFaceId )
      {
        // for a given face ID, update the position only if the timestamp is not older than the previously received timestamp.
        if( newPos.timeNs() >= latestTimeStamp )
        {
          foundPos = newPos;
          smallestFaceId = foundPos.faceID();
          latestTimeStamp = foundPos.timeNs();
        }
      }
    }
    catch( std::exception const & ex )
    {
      // Don't abort the program when receiving a corrupted message.
      std::cerr << "PositionDecoder: Error while decoding a position message: " << ex.what() << std::endl;
    }
    mDatagramInput.pop();
  }
  // Did we actually receive a valid message?
  if( smallestFaceId != std::numeric_limits<unsigned int>::max() )
  {
    mPositionOutput.data() = translatePosition( foundPos );
    mPositionOutput.swapBuffers();
  }
}

pml::ListenerPosition PositionDecoder::translatePosition( const pml::ListenerPosition &pos )
{
#if 0
  {
    std::array <float, 3> u{ { mQx, mQy, mQz } }; // the ijk components of the quaternion

    std::array <float, 3> v{ { pos.x(), pos.y(), pos.z() } }; //the vector to rotate

    // Extract the scalar part of the quaternion
    float s = mQw; //the quaternion w components

    // Do the math
    2.0f * dot(u, v) * u + (s*s - dot(u, u)) * v + 2.0f * s * cross(u, v);
  }
#endif

  return pml::ListenerPosition( -pos.z() + mOffsetKinect.x, -pos.x() + mOffsetKinect.y, pos.y() + mOffsetKinect.z );
}

} // namespace rcl
} // namespace visr


// Boost.Signals library

// Copyright Douglas Gregor 2001-2004. Use, modification and
// distribution is subject to the Boost Software License, Version
// 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// For more information, see http://www.boost.org

#include <boost/signal.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/random.hpp>
#include <map>
#include <set>
#include <stdlib.h>
#include <time.h>
#include <boost/test/minimal.hpp>

using namespace boost;
using namespace boost::signals;

struct signal_tag {
  typedef vertex_property_tag kind;
};

struct connection_tag {
  typedef edge_property_tag kind;
};

typedef signal4<void, int, int, double, int&> signal_type;
typedef adjacency_list<listS, listS, directedS,
                       // Vertex properties
                       property<signal_tag, signal_type*,
  //                       property<vertex_color_t, default_color_type,
                       property<vertex_index_t, int> >,
                       // Edge properties
                       property<connection_tag, connection,
                       property<edge_weight_t, int> > >
  signal_graph_type;
typedef signal_graph_type::vertex_descriptor vertex_descriptor;
typedef signal_graph_type::edge_descriptor edge_descriptor;

// The signal graph
static signal_graph_type signal_graph;

// Mapping from a signal to its associated vertex descriptor
static std::map<signal_type*, vertex_descriptor> signal_to_descriptor;

// Mapping from a connection to its associated edge descriptor
static std::map<connection, edge_descriptor> connection_to_descriptor;

std::map<signal_type*, int> min_signal_propagate_distance;

void remove_disconnected_connections()
{
  // remove disconnected connections
  std::map<connection, edge_descriptor>::iterator i =
    connection_to_descriptor.begin();
  while (i != connection_to_descriptor.end()) {
    if (!i->first.connected()) {
      connection_to_descriptor.erase(i++);
    }
    else {
      ++i;
    }
  }
}

void remove_signal(signal_type* sig)
{
  clear_vertex(signal_to_descriptor[sig], signal_graph);
  remove_vertex(signal_to_descriptor[sig], signal_graph);
  delete sig;
  signal_to_descriptor.erase(sig);
  remove_disconnected_connections();
}

void random_remove_signal(minstd_rand& rand_gen);

struct tracking_bridge {
  tracking_bridge(const tracking_bridge& other)
    : sig(other.sig), rand_gen(other.rand_gen)
  { ++bridge_count; }

  tracking_bridge(signal_type* s, minstd_rand& rg) : sig(s), rand_gen(rg)
  { ++bridge_count; }

  ~tracking_bridge()
  { --bridge_count; }

  void operator()(int cur_dist, int max_dist, double deletion_prob,
                  int& deletions_left) const
  {
    if (signal_to_descriptor.find(sig) == signal_to_descriptor.end())
      return;

    ++cur_dist;

    // Update the directed Bacon distance
    if (min_signal_propagate_distance.find(sig) ==
          min_signal_propagate_distance.end()) {
      min_signal_propagate_distance[sig] = cur_dist;
    }
    else if (cur_dist < min_signal_propagate_distance[sig]) {
      min_signal_propagate_distance[sig] = cur_dist;
    }
    else if (deletion_prob == 0.0) {
      // don't bother calling because we've already found a better route here
      return;
    }

    // Maybe delete the signal
    if (uniform_01<minstd_rand>(rand_gen)() < deletion_prob &&
        deletions_left-- && signal_to_descriptor.size() > 1) {
      random_remove_signal(rand_gen);
    }
    // propagate the signal
    else if (cur_dist < max_dist) {
      (*sig)(cur_dist, max_dist, deletion_prob, deletions_left);
    }
  }

  signal_type* sig;
  minstd_rand& rand_gen;
  static int bridge_count;
};

int tracking_bridge::bridge_count = 0;

namespace boost {
  template<typename V>
  void visit_each(V& v, const tracking_bridge& t, int)
  {
    v(t);
    v(t.sig);
  }
}

signal_type* add_signal()
{
  signal_type* sig = new signal_type();
  vertex_descriptor v = add_vertex(signal_graph);
  signal_to_descriptor[sig] = v;
  put(signal_tag(), signal_graph, v, sig);

  return sig;
}

connection add_connection(signal_type* sig1, signal_type* sig2,
                          minstd_rand& rand_gen)
{
  std::cout << "  Adding connection: " << sig1 << " -> " << sig2 << std::endl;

  connection c = sig1->connect(tracking_bridge(sig2, rand_gen));
  edge_descriptor e =
    add_edge(signal_to_descriptor[sig1], signal_to_descriptor[sig2],
             signal_graph).first;
  connection_to_descriptor[c] = e;
  put(connection_tag(), signal_graph, e, c);
  put(edge_weight, signal_graph, e, 1);
  return c;
}

void remove_connection(connection c)
{
  signal_type* sig1 = get(signal_tag(), signal_graph,
                          source(connection_to_descriptor[c], signal_graph));
  signal_type* sig2 = get(signal_tag(), signal_graph,
                          target(connection_to_descriptor[c], signal_graph));
  std::cout << "  Removing connection: " << sig1 << " -> " << sig2
            << std::endl;
  c.disconnect();
  remove_edge(connection_to_descriptor[c], signal_graph);
  connection_to_descriptor.erase(c);
}

bool signal_connection_exists(signal_type* sig1, signal_type* sig2,
                              edge_descriptor& edge_desc)
{
  vertex_descriptor source_sig = signal_to_descriptor[sig1];
  vertex_descriptor target_sig = signal_to_descriptor[sig2];
  signal_graph_type::out_edge_iterator e;
  for (e = out_edges(source_sig, signal_graph).first;
       e != out_edges(source_sig, signal_graph).second; ++e) {
    if (target(*e, signal_graph) == target_sig) {
      edge_desc = *e;
      return true;
    }
  }
  return false;
}

bool signal_connection_exists(signal_type* sig1, signal_type* sig2)
{
  edge_descriptor e;
  return signal_connection_exists(sig1, sig2, e);
}

std::map<signal_type*, vertex_descriptor>::iterator
choose_random_signal(minstd_rand& rand_gen)
{
  int signal_idx
    = uniform_int<>(0, signal_to_descriptor.size() - 1)(rand_gen);
  std::map<signal_type*, vertex_descriptor>::iterator result =
    signal_to_descriptor.begin();
  for(; signal_idx; --signal_idx)
    ++result;

  return result;
}

void random_remove_signal(minstd_rand& rand_gen)
{
  std::map<signal_type*, vertex_descriptor>::iterator victim =
    choose_random_signal(rand_gen);
  std::cout << "  Removing signal " << victim->first << std::endl;
  remove_signal(victim->first);
}

void random_add_connection(minstd_rand& rand_gen)
{
  std::map<signal_type*, vertex_descriptor>::iterator source;
  std::map<signal_type*, vertex_descriptor>::iterator target;
  do {
    source = choose_random_signal(rand_gen);
    target = choose_random_signal(rand_gen);
  } while (signal_connection_exists(source->first, target->first));

  add_connection(source->first, target->first, rand_gen);
}

void random_remove_connection(minstd_rand& rand_gen)
{
  int victim_idx =
    uniform_int<>(0, num_edges(signal_graph)-1)(rand_gen);
  signal_graph_type::edge_iterator e = edges(signal_graph).first;
  while (victim_idx--) {
    ++e;
  }

  remove_connection(get(connection_tag(), signal_graph, *e));
}

void random_bacon_test(minstd_rand& rand_gen)
{
  signal_type* kevin = choose_random_signal(rand_gen)->first;
  min_signal_propagate_distance.clear();
  min_signal_propagate_distance[kevin] = 0;

  const int horizon = 10; // only go to depth 10 at most

  std::cout << "  Bacon test: kevin is " << kevin
            << "\n    Propagating signal...";

  // Propagate the signal out to the horizon
  int deletions_left = 0;
  (*kevin)(0, horizon, 0.0, deletions_left);

  std::cout << "OK\n    Finding shortest paths...";

  // Initialize all colors to white
  {
    unsigned int num = 0;
    for (signal_graph_type::vertex_iterator v = vertices(signal_graph).first;
         v != vertices(signal_graph).second;
         ++v) {
      //      put(vertex_color, signal_graph, *v, white_color);
      put(vertex_index, signal_graph, *v, num++);
    }

    BOOST_CHECK(num == num_vertices(signal_graph));
  }

  // Perform a breadth-first search starting at kevin, and record the
  // distances from kevin to each reachable node.
  std::map<vertex_descriptor, int> bacon_distance_map;

#if 0
  bacon_distance_map[signal_to_descriptor[kevin]] = 0;
  breadth_first_visit(signal_graph, signal_to_descriptor[kevin],
                      visitor(
                        make_bfs_visitor(
                         record_distances(
                           make_assoc_property_map(bacon_distance_map),
                           on_examine_edge()))).
                      color_map(get(vertex_color, signal_graph)));
#endif

  dijkstra_shortest_paths(signal_graph, signal_to_descriptor[kevin],
                          distance_map(make_assoc_property_map(bacon_distance_map)));
  std::cout << "OK\n";
  // Make sure the bacon distances agree (prior to the horizon)
  {
    std::map<signal_type*, int>::iterator i;
    for (i = min_signal_propagate_distance.begin();
         i != min_signal_propagate_distance.end();
         ++i) {
      if (i->second != bacon_distance_map[signal_to_descriptor[i->first]]) {
        std::cout << "Signal distance to " << i->first << " was "
                  << i->second << std::endl;
        std::cout << "Graph distance was "
                  << bacon_distance_map[signal_to_descriptor[i->first]]
                  << std::endl;
      }
      BOOST_CHECK(i->second == bacon_distance_map[signal_to_descriptor[i->first]]);
    }
  }
}

void randomly_create_connections(minstd_rand& rand_gen, double edge_probability)
{
  // Randomly create connections
  uniform_01<minstd_rand> random(rand_gen);
  for (signal_graph_type::vertex_iterator v1 = vertices(signal_graph).first;
       v1 != vertices(signal_graph).second; ++v1) {
    for (signal_graph_type::vertex_iterator v2 = vertices(signal_graph).first;
         v2 != vertices(signal_graph).second; ++v2) {
      if (random() < edge_probability) {
        add_connection(get(signal_tag(), signal_graph, *v1),
                       get(signal_tag(), signal_graph, *v2),
                       rand_gen);
      }
    }
  }
}

void random_recursive_deletion(minstd_rand& rand_gen)
{
  signal_type* kevin = choose_random_signal(rand_gen)->first;
  min_signal_propagate_distance.clear();
  min_signal_propagate_distance[kevin] = 0;

  const int horizon = 4; // only go to depth "horizon" at most

  std::cout << "  Recursive deletion test: start is " << kevin << std::endl;

  // Propagate the signal out to the horizon
  int deletions_left = (int)(0.05*num_vertices(signal_graph));
  (*kevin)(0, horizon, 0.05, deletions_left);
}

int test_main(int argc, char* argv[])
{
  if (argc < 4) {
    std::cerr << "Usage: random_signal_system <# of initial signals> "
              << "<edge probability> <iterations>" << std::endl;
    return 1;
  }

  int number_of_initial_signals = atoi(argv[1]);
  double edge_probability = atof(argv[2]);
  int iterations = atoi(argv[3]);

  int seed;
  if (argc == 5)
    seed = atoi(argv[4]);
  else
    seed = time(0);

  std::cout << "Number of initial signals: " << number_of_initial_signals
            << std::endl;
  std::cout << "Edge probability: " << edge_probability << std::endl;
  std::cout << "Iterations: " << iterations << std::endl;
  std::cout << "Seed: " << seed << std::endl;

  // Initialize random number generator
  minstd_rand rand_gen;
  rand_gen.seed(seed);

  for (int iter = 0; iter < iterations; ++iter) {
    if (num_vertices(signal_graph) < 2) {
      for (int i = 0; i < number_of_initial_signals; ++i)
        add_signal();
    }

    while (num_edges(signal_graph) < 2) {
      randomly_create_connections(rand_gen, edge_probability);
    }

    std::cerr << "Iteration #" << (iter+1) << std::endl;

    uniform_int<> random_action(0, 7);
    switch (random_action(rand_gen)) {
    case 0:
      std::cout << "  Adding new signal: " << add_signal() << std::endl;
      break;

    case 1:
      random_remove_signal(rand_gen);
      break;

    case 2:
      if (num_edges(signal_graph) <
            num_vertices(signal_graph)*num_vertices(signal_graph)) {
        random_add_connection(rand_gen);
      }
      break;

    case 3:
      random_remove_connection(rand_gen);
      break;

    case 4:
    case 5:
    case 6:
      random_bacon_test(rand_gen);
      break;

    case 7:
      random_recursive_deletion(rand_gen);
      break;
    }
  }

  for (signal_graph_type::vertex_iterator v = vertices(signal_graph).first;
       v != vertices(signal_graph).second;
       ++v) {
    delete get(signal_tag(), signal_graph, *v);
  }

  BOOST_CHECK(tracking_bridge::bridge_count == 0);
  if (tracking_bridge::bridge_count != 0) {
    std::cerr << tracking_bridge::bridge_count << " connections remain.\n";
  }
  return 0;
}


//
// Copyright 2016 Pixar
//
// Licensed under the Apache License, Version 2.0 (the "Apache License")
// with the following modification; you may not use this file except in
// compliance with the Apache License and the following modification to it:
// Section 6. Trademarks. is deleted and replaced with:
//
// 6. Trademarks. This License does not grant permission to use the trade
//    names, trademarks, service marks, or product names of the Licensor
//    and its affiliates, except as required to comply with Section 4(c) of
//    the License and to reproduce the content of the NOTICE file.
//
// You may obtain a copy of the Apache License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the Apache License with the above modification is
// distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied. See the Apache License for the specific
// language governing permissions and limitations under the Apache License.
//

#include "pxr/pxr.h"
#include "pxr/usd/sdf/parserHelpers.h"
#include "pxr/usd/sdf/schema.h"
#include "pxr/base/gf/half.h"
#include "pxr/base/gf/matrix2d.h"
#include "pxr/base/gf/matrix3d.h"
#include "pxr/base/gf/matrix4d.h"
#include "pxr/base/gf/quatd.h"
#include "pxr/base/gf/quatf.h"
#include "pxr/base/gf/quath.h"
#include "pxr/base/gf/vec2d.h"
#include "pxr/base/gf/vec2f.h"
#include "pxr/base/gf/vec2h.h"
#include "pxr/base/gf/vec2i.h"
#include "pxr/base/gf/vec3d.h"
#include "pxr/base/gf/vec3f.h"
#include "pxr/base/gf/vec3h.h"
#include "pxr/base/gf/vec3i.h"
#include "pxr/base/gf/vec4d.h"
#include "pxr/base/gf/vec4f.h"
#include "pxr/base/gf/vec4h.h"
#include "pxr/base/gf/vec4i.h"
#include "pxr/base/tf/instantiateSingleton.h"
#include "pxr/base/tf/iterator.h"
#include "pxr/base/tf/staticData.h"
#include "pxr/base/plug/registry.h"
#include "pxr/base/vt/array.h"
#include "pxr/base/vt/value.h"

#include <boost/mpl/for_each.hpp>

#include <functional>
#include <type_traits>
#include <utility>

PXR_NAMESPACE_OPEN_SCOPE

namespace Sdf_ParserHelpers {

using std::pair;
using std::make_pair;

using boost::get;

// Check that there are enough values to parse so we don't overflow
#define CHECK_BOUNDS(count, name)                                          \
    if (index + count > vars.size()) {                                     \
        TF_CODING_ERROR("Not enough values to parse value of type %s",     \
                        name);                                             \
        throw boost::bad_get();                                            \
    }

inline void
MakeScalarValueImpl(string *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(1, "string");
    *out = vars[index++].Get<std::string>();
}

inline void
MakeScalarValueImpl(TfToken *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(1, "token");
    *out = TfToken(vars[index++].Get<std::string>());
}

inline void
MakeScalarValueImpl(double *out,
                                vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(1, "double");
    *out = vars[index++].Get<double>();
}

inline void
MakeScalarValueImpl(float *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(1, "float");
    *out = vars[index++].Get<float>();
}

inline void
MakeScalarValueImpl(GfHalf *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(1, "half");
    *out = GfHalf(vars[index++].Get<float>());
}

inline void
MakeScalarValueImpl(
    SdfTimeCode *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(1, "timecode");
    *out = SdfTimeCode(vars[index++].Get<double>());
}

template <class Int>
inline typename boost::enable_if<boost::is_integral<Int> >::type
MakeScalarValueImpl(Int *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(1, ArchGetDemangled<Int>().c_str());
    *out = vars[index++].Get<Int>();
}

inline void
MakeScalarValueImpl(GfVec2d *out,
                                vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(2, "Vec2d");
    (*out)[0] = vars[index++].Get<double>();
    (*out)[1] = vars[index++].Get<double>();
}

inline void
MakeScalarValueImpl(GfVec2f *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(2, "Vec2f");
    (*out)[0] = vars[index++].Get<float>();
    (*out)[1] = vars[index++].Get<float>();
}

inline void
MakeScalarValueImpl(GfVec2h *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(2, "Vec2h");
    (*out)[0] = GfHalf(vars[index++].Get<float>());
    (*out)[1] = GfHalf(vars[index++].Get<float>());
}

inline void
MakeScalarValueImpl(GfVec2i *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(2, "Vec2i");
    (*out)[0] = vars[index++].Get<int>();
    (*out)[1] = vars[index++].Get<int>();
}

inline void
MakeScalarValueImpl(GfVec3d *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(3, "Vec3d");
    (*out)[0] = vars[index++].Get<double>();
    (*out)[1] = vars[index++].Get<double>();
    (*out)[2] = vars[index++].Get<double>();
}

inline void
MakeScalarValueImpl(GfVec3f *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(3, "Vec3f");
    (*out)[0] = vars[index++].Get<float>();
    (*out)[1] = vars[index++].Get<float>();
    (*out)[2] = vars[index++].Get<float>();
}

inline void
MakeScalarValueImpl(GfVec3h *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(3, "Vec3h");
    (*out)[0] = GfHalf(vars[index++].Get<float>());
    (*out)[1] = GfHalf(vars[index++].Get<float>());
    (*out)[2] = GfHalf(vars[index++].Get<float>());
}

inline void
MakeScalarValueImpl(GfVec3i *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(3, "Vec3i");
    (*out)[0] = vars[index++].Get<int>();
    (*out)[1] = vars[index++].Get<int>();
    (*out)[2] = vars[index++].Get<int>();
}

inline void
MakeScalarValueImpl(GfVec4d *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(4, "Vec4d");
    (*out)[0] = vars[index++].Get<double>();
    (*out)[1] = vars[index++].Get<double>();
    (*out)[2] = vars[index++].Get<double>();
    (*out)[3] = vars[index++].Get<double>();
}

inline void
MakeScalarValueImpl(GfVec4f *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(4, "Vec4f");
    (*out)[0] = vars[index++].Get<float>();
    (*out)[1] = vars[index++].Get<float>();
    (*out)[2] = vars[index++].Get<float>();
    (*out)[3] = vars[index++].Get<float>();
}

inline void
MakeScalarValueImpl(GfVec4h *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(4, "Vec4h");
    (*out)[0] = GfHalf(vars[index++].Get<float>());
    (*out)[1] = GfHalf(vars[index++].Get<float>());
    (*out)[2] = GfHalf(vars[index++].Get<float>());
    (*out)[3] = GfHalf(vars[index++].Get<float>());
}

inline void
MakeScalarValueImpl(GfVec4i *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(4, "Vec4i");
    (*out)[0] = vars[index++].Get<int>();
    (*out)[1] = vars[index++].Get<int>();
    (*out)[2] = vars[index++].Get<int>();
    (*out)[3] = vars[index++].Get<int>();
}

inline void
MakeScalarValueImpl(GfMatrix2d *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(4, "Matrix2d");
    (*out)[0][0] = vars[index++].Get<double>();
    (*out)[0][1] = vars[index++].Get<double>();
    (*out)[1][0] = vars[index++].Get<double>();
    (*out)[1][1] = vars[index++].Get<double>();
}

inline void
MakeScalarValueImpl(GfMatrix3d *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(9, "Matrix3d");
    (*out)[0][0] = vars[index++].Get<double>();
    (*out)[0][1] = vars[index++].Get<double>();
    (*out)[0][2] = vars[index++].Get<double>();
    (*out)[1][0] = vars[index++].Get<double>();
    (*out)[1][1] = vars[index++].Get<double>();
    (*out)[1][2] = vars[index++].Get<double>();
    (*out)[2][0] = vars[index++].Get<double>();
    (*out)[2][1] = vars[index++].Get<double>();
    (*out)[2][2] = vars[index++].Get<double>();
}

inline void
MakeScalarValueImpl(GfMatrix4d *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(16, "Matrix4d");
    (*out)[0][0] = vars[index++].Get<double>();
    (*out)[0][1] = vars[index++].Get<double>();
    (*out)[0][2] = vars[index++].Get<double>();
    (*out)[0][3] = vars[index++].Get<double>();
    (*out)[1][0] = vars[index++].Get<double>();
    (*out)[1][1] = vars[index++].Get<double>();
    (*out)[1][2] = vars[index++].Get<double>();
    (*out)[1][3] = vars[index++].Get<double>();
    (*out)[2][0] = vars[index++].Get<double>();
    (*out)[2][1] = vars[index++].Get<double>();
    (*out)[2][2] = vars[index++].Get<double>();
    (*out)[2][3] = vars[index++].Get<double>();
    (*out)[3][0] = vars[index++].Get<double>();
    (*out)[3][1] = vars[index++].Get<double>();
    (*out)[3][2] = vars[index++].Get<double>();
    (*out)[3][3] = vars[index++].Get<double>();
}

inline void
MakeScalarValueImpl(GfQuatd *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(4, "Quatd");
    // Values in order are re, i, j, k.
    GfVec3d imag; double re;
    MakeScalarValueImpl(&re, vars, index);
    out->SetReal(re);
    MakeScalarValueImpl(&imag, vars, index);
    out->SetImaginary(imag);
}

inline void
MakeScalarValueImpl(GfQuatf *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(4, "Quatf");
    // Values in order are re, i, j, k.
    GfVec3f imag; float re;
    MakeScalarValueImpl(&re, vars, index);
    out->SetReal(re);
    MakeScalarValueImpl(&imag, vars, index);
    out->SetImaginary(imag);
}

inline void
MakeScalarValueImpl(GfQuath *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(4, "Quath");
    // Values in order are re, i, j, k.
    GfVec3h imag; GfHalf re;
    MakeScalarValueImpl(&re, vars, index);
    out->SetReal(re);
    MakeScalarValueImpl(&imag, vars, index);
    out->SetImaginary(imag);
}

inline void
MakeScalarValueImpl(
    SdfAssetPath *out, vector<Value> const &vars, size_t &index) {
    CHECK_BOUNDS(1, "asset");
    *out = vars[index++].Get<SdfAssetPath>();
}

template <typename T>
inline VtValue
MakeScalarValueTemplate(vector<unsigned int> const &,
                        vector<Value> const &vars, size_t &index,
                        string *errStrPtr) {
    T t;
    size_t origIndex = index;
    try {
        MakeScalarValueImpl(&t, vars, index);
    } catch (const boost::bad_get &) {
        *errStrPtr = TfStringPrintf("Failed to parse value (at sub-part %zd "
                                    "if there are multiple parts)",
                                    (index - origIndex) - 1);
        return VtValue();
    }
    return VtValue(t);
}

template <typename T>
inline VtValue
MakeShapedValueTemplate(vector<unsigned int> const &shape,
                        vector<Value> const &vars, size_t &index,
                        string *errStrPtr) {
    if (shape.empty())
        return VtValue(VtArray<T>());
//    TF_AXIOM(shape.size() == 1);
    unsigned int size = 1;
    TF_FOR_ALL(i, shape)
        size *= *i;

    VtArray<T> array(size);
    size_t shapeIndex = 0;
    size_t origIndex = index;
    try {
        TF_FOR_ALL(i, array) {
            MakeScalarValueImpl(&(*i), vars, index);
            shapeIndex++;
        }
    } catch (const boost::bad_get &) {
        *errStrPtr = TfStringPrintf("Failed to parse at element %zd "
                                    "(at sub-part %zd if there are "
                                    "multiple parts)", shapeIndex,
                                    (index - origIndex) - 1);
        return VtValue();
    }
    return VtValue(array);
}

typedef map<std::string, ValueFactory> _ValueFactoryMap;

// Walk through types and register factories.
struct _MakeFactoryMap {

    explicit _MakeFactoryMap(_ValueFactoryMap *factories) :
        _factories(factories) {}

    template <class CppType>
    void add(const SdfValueTypeName& scalar, const char* alias = NULL)
    {
        namespace ph = std::placeholders;

        static const bool isShaped = true;

        const SdfValueTypeName array = scalar.GetArrayType();

        const std::string scalarName =
            alias ? std::string(alias)        : scalar.GetAsToken().GetString();
        const std::string arrayName =
            alias ? std::string(alias) + "[]" : array.GetAsToken().GetString();

        _ValueFactoryMap &f = *_factories;
        f[scalarName] =
            ValueFactory(scalarName, scalar.GetDimensions(), !isShaped,
                         std::bind(MakeScalarValueTemplate<CppType>,
                                   ph::_1, ph::_2, ph::_3, ph::_4));
        f[arrayName] =
            ValueFactory(arrayName, array.GetDimensions(), isShaped,
                         std::bind(MakeShapedValueTemplate<CppType>,
                                   ph::_1, ph::_2, ph::_3, ph::_4));
    }

    _ValueFactoryMap *_factories;
};

TF_MAKE_STATIC_DATA(_ValueFactoryMap, _valueFactories) {
    _MakeFactoryMap builder(_valueFactories);
    // XXX: Would be better if SdfValueTypeName had a method to take
    //      a vector of VtValues and return a VtValue holding the
    //      appropriate C++ type (which mostly involves moving the
    //      MakeScalarValueImpl functions into the value type name
    //      registration code).  Then we could do this:
    //     for (const auto& typeName : SdfSchema::GetInstance().GetAllTypes()) {
    //        builder(typeName);
    //    }
    //            For symmetry (and I think it would actually be useful
    //            when converting usd into other formats) there should be
    //            a method to convert a VtValue holding the appropriate C++
    //            type into a vector of VtValues holding a primitive type.
    //            E.g. a VtValue holding a GfVec3f would return three
    //            VtValues each holding a float.
    builder.add<bool>(SdfValueTypeNames->Bool);
    builder.add<uint8_t>(SdfValueTypeNames->UChar);
    builder.add<int32_t>(SdfValueTypeNames->Int);
    builder.add<uint32_t>(SdfValueTypeNames->UInt);
    builder.add<int64_t>(SdfValueTypeNames->Int64);
    builder.add<uint64_t>(SdfValueTypeNames->UInt64);
    builder.add<GfHalf>(SdfValueTypeNames->Half);
    builder.add<float>(SdfValueTypeNames->Float);
    builder.add<double>(SdfValueTypeNames->Double);
    builder.add<SdfTimeCode>(SdfValueTypeNames->TimeCode);
    builder.add<std::string>(SdfValueTypeNames->String);
    builder.add<TfToken>(SdfValueTypeNames->Token);
    builder.add<SdfAssetPath>(SdfValueTypeNames->Asset);
    builder.add<GfVec2i>(SdfValueTypeNames->Int2);
    builder.add<GfVec2h>(SdfValueTypeNames->Half2);
    builder.add<GfVec2f>(SdfValueTypeNames->Float2);
    builder.add<GfVec2d>(SdfValueTypeNames->Double2);
    builder.add<GfVec3i>(SdfValueTypeNames->Int3);
    builder.add<GfVec3h>(SdfValueTypeNames->Half3);
    builder.add<GfVec3f>(SdfValueTypeNames->Float3);
    builder.add<GfVec3d>(SdfValueTypeNames->Double3);
    builder.add<GfVec4i>(SdfValueTypeNames->Int4);
    builder.add<GfVec4h>(SdfValueTypeNames->Half4);
    builder.add<GfVec4f>(SdfValueTypeNames->Float4);
    builder.add<GfVec4d>(SdfValueTypeNames->Double4);
    builder.add<GfVec3h>(SdfValueTypeNames->Point3h);
    builder.add<GfVec3f>(SdfValueTypeNames->Point3f);
    builder.add<GfVec3d>(SdfValueTypeNames->Point3d);
    builder.add<GfVec3h>(SdfValueTypeNames->Vector3h);
    builder.add<GfVec3f>(SdfValueTypeNames->Vector3f);
    builder.add<GfVec3d>(SdfValueTypeNames->Vector3d);
    builder.add<GfVec3h>(SdfValueTypeNames->Normal3h);
    builder.add<GfVec3f>(SdfValueTypeNames->Normal3f);
    builder.add<GfVec3d>(SdfValueTypeNames->Normal3d);
    builder.add<GfVec3h>(SdfValueTypeNames->Color3h);
    builder.add<GfVec3f>(SdfValueTypeNames->Color3f);
    builder.add<GfVec3d>(SdfValueTypeNames->Color3d);
    builder.add<GfVec4h>(SdfValueTypeNames->Color4h);
    builder.add<GfVec4f>(SdfValueTypeNames->Color4f);
    builder.add<GfVec4d>(SdfValueTypeNames->Color4d);
    builder.add<GfQuath>(SdfValueTypeNames->Quath);
    builder.add<GfQuatf>(SdfValueTypeNames->Quatf);
    builder.add<GfQuatd>(SdfValueTypeNames->Quatd);
    builder.add<GfMatrix2d>(SdfValueTypeNames->Matrix2d);
    builder.add<GfMatrix3d>(SdfValueTypeNames->Matrix3d);
    builder.add<GfMatrix4d>(SdfValueTypeNames->Matrix4d);
    builder.add<GfMatrix4d>(SdfValueTypeNames->Frame4d);
    builder.add<GfVec2f>(SdfValueTypeNames->TexCoord2f);
    builder.add<GfVec2d>(SdfValueTypeNames->TexCoord2d);
    builder.add<GfVec2h>(SdfValueTypeNames->TexCoord2h);
    builder.add<GfVec3f>(SdfValueTypeNames->TexCoord3f);
    builder.add<GfVec3d>(SdfValueTypeNames->TexCoord3d);
    builder.add<GfVec3h>(SdfValueTypeNames->TexCoord3h);

    // XXX: Backwards compatibility.  These should be removed when
    //      all assets are updated.  At the time of this writing
    //      under pxr only assets used by usdImaging need updating.
    //      Those assets must be moved anyway for open sourcing so
    //      I'm leaving this for now.  (Also note that at least one
    //      of those tests, testUsdImagingEmptyMesh, uses the prim
    //      type PxVolume which is not in pxr.)  Usd assets outside
    //      pxr must also be updated.
    builder.add<GfVec2i>(SdfValueTypeNames->Int2, "Vec2i");
    builder.add<GfVec2h>(SdfValueTypeNames->Half2, "Vec2h");
    builder.add<GfVec2f>(SdfValueTypeNames->Float2, "Vec2f");
    builder.add<GfVec2d>(SdfValueTypeNames->Double2, "Vec2d");
    builder.add<GfVec3i>(SdfValueTypeNames->Int3, "Vec3i");
    builder.add<GfVec3h>(SdfValueTypeNames->Half3, "Vec3h");
    builder.add<GfVec3f>(SdfValueTypeNames->Float3, "Vec3f");
    builder.add<GfVec3d>(SdfValueTypeNames->Double3, "Vec3d");
    builder.add<GfVec4i>(SdfValueTypeNames->Int4, "Vec4i");
    builder.add<GfVec4h>(SdfValueTypeNames->Half4, "Vec4h");
    builder.add<GfVec4f>(SdfValueTypeNames->Float4, "Vec4f");
    builder.add<GfVec4d>(SdfValueTypeNames->Double4, "Vec4d");
    builder.add<GfVec3f>(SdfValueTypeNames->Point3f, "PointFloat");
    builder.add<GfVec3d>(SdfValueTypeNames->Point3d, "Point");
    builder.add<GfVec3f>(SdfValueTypeNames->Vector3f, "NormalFloat");
    builder.add<GfVec3d>(SdfValueTypeNames->Vector3d, "Normal");
    builder.add<GfVec3f>(SdfValueTypeNames->Normal3f, "VectorFloat");
    builder.add<GfVec3d>(SdfValueTypeNames->Normal3d, "Vector");
    builder.add<GfVec3f>(SdfValueTypeNames->Color3f, "ColorFloat");
    builder.add<GfVec3d>(SdfValueTypeNames->Color3d, "Color");
    builder.add<GfQuath>(SdfValueTypeNames->Quath, "Quath");
    builder.add<GfQuatf>(SdfValueTypeNames->Quatf, "Quatf");
    builder.add<GfQuatd>(SdfValueTypeNames->Quatd, "Quatd");
    builder.add<GfMatrix2d>(SdfValueTypeNames->Matrix2d, "Matrix2d");
    builder.add<GfMatrix3d>(SdfValueTypeNames->Matrix3d, "Matrix3d");
    builder.add<GfMatrix4d>(SdfValueTypeNames->Matrix4d, "Matrix4d");
    builder.add<GfMatrix4d>(SdfValueTypeNames->Frame4d, "Frame");
    builder.add<GfMatrix4d>(SdfValueTypeNames->Matrix4d, "Transform");
    builder.add<int>(SdfValueTypeNames->Int, "PointIndex");
    builder.add<int>(SdfValueTypeNames->Int, "EdgeIndex");
    builder.add<int>(SdfValueTypeNames->Int, "FaceIndex");
    builder.add<TfToken>(SdfValueTypeNames->Token, "Schema");

    // Set up the special None factory.
    (*_valueFactories)[std::string("None")] = ValueFactory(
        std::string(""), SdfTupleDimensions(), false, NULL);

}

ValueFactory const &GetValueFactoryForMenvaName(std::string const &name,
                                                bool *found)
{
    _ValueFactoryMap::const_iterator it = _valueFactories->find(name);
    if (it != _valueFactories->end()) {
        *found = true;
        return it->second;
    }

    // No factory for given name.
    static ValueFactory const& none = (*_valueFactories)[std::string("None")];
    *found = false;
    return none;
}

};

std::string
Sdf_EvalQuotedString(const char* x, size_t n, size_t trimBothSides,
                     unsigned int* numLines)
{
    std::string ret;

    // Handle empty strings
    if (n <= 2 * trimBothSides)
        return ret;

    n -= 2 * trimBothSides;

    // Use local buf, or malloc one if not enough space.
    // (this is a little too much if there are escape chars in the string,
    // but we can live with it to avoid traversing the string twice)
    static const size_t LocalSize = 128;
    char localBuf[LocalSize];
    char *buf = n <= LocalSize ? localBuf : (char *)malloc(n);

    char *s = buf;
    for (const char *p = x + trimBothSides,
             *end = x + trimBothSides + n; p != end; ++p) {
        if (*p != '\\') {
            *s++ = *p;
        } else {
            TfEscapeStringReplaceChar(&p, &s);
        }
    }

    // Trim to final length.
    std::string(buf, s-buf).swap(ret);
    if (buf != localBuf)
        free(buf);

    if (numLines)
        *numLines = std::count(ret.begin(), ret.end(), '\n');

    return ret;
}

std::string
Sdf_EvalAssetPath(const char* x, size_t n, bool tripleDelimited)
{
    // See _StringFromAssetPath for the code that writes asset paths.

    // Asset paths are assumed to only contain printable characters and
    // no escape sequences except for the "@@@" delimiter.
    size_t numDelimiters = tripleDelimited ? 3 : 1;
    std::string ret(x + numDelimiters, n - (2 * numDelimiters));
    if (tripleDelimited) {
        ret = TfStringReplace(ret, "\\@@@", "@@@");
    }

    return ret;
}

PXR_NAMESPACE_CLOSE_SCOPE