//  Copyright John Maddock 2007.
//  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)

#ifndef BOOST_MATH_POLICY_HPP
#define BOOST_MATH_POLICY_HPP

#include <boost/mpl/list.hpp>
#include <boost/mpl/contains.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/find_if.hpp>
#include <boost/mpl/remove_if.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/comparison.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/static_assert.hpp>
#include <boost/assert.hpp>
#include <boost/math/tools/config.hpp>
#include <limits>
// Sadly we do need the .h versions of these to be sure of getting
// FLT_MANT_DIG etc.
#include <limits.h>
#include <stdlib.h>
#include <stddef.h>
#include <math.h>

namespace boost{ namespace math{

namespace tools{

template <class T>
int digits(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE(T));
template <class T>
T epsilon(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE(T));

}

namespace policies{

//
// Define macros for our default policies, if they're not defined already:
//
#ifndef BOOST_MATH_DOMAIN_ERROR_POLICY
#define BOOST_MATH_DOMAIN_ERROR_POLICY throw_on_error
#endif
#ifndef BOOST_MATH_POLE_ERROR_POLICY
#define BOOST_MATH_POLE_ERROR_POLICY throw_on_error
#endif
#ifndef BOOST_MATH_OVERFLOW_ERROR_POLICY
#define BOOST_MATH_OVERFLOW_ERROR_POLICY throw_on_error
#endif
#ifndef BOOST_MATH_EVALUATION_ERROR_POLICY
#define BOOST_MATH_EVALUATION_ERROR_POLICY throw_on_error
#endif
#ifndef BOOST_MATH_ROUNDING_ERROR_POLICY
#define BOOST_MATH_ROUNDING_ERROR_POLICY throw_on_error
#endif
#ifndef BOOST_MATH_UNDERFLOW_ERROR_POLICY
#define BOOST_MATH_UNDERFLOW_ERROR_POLICY ignore_error
#endif
#ifndef BOOST_MATH_DENORM_ERROR_POLICY
#define BOOST_MATH_DENORM_ERROR_POLICY ignore_error
#endif
#ifndef BOOST_MATH_INDETERMINATE_RESULT_ERROR_POLICY
#define BOOST_MATH_INDETERMINATE_RESULT_ERROR_POLICY ignore_error
#endif
#ifndef BOOST_MATH_DIGITS10_POLICY
#define BOOST_MATH_DIGITS10_POLICY 0
#endif
#ifndef BOOST_MATH_PROMOTE_FLOAT_POLICY
#define BOOST_MATH_PROMOTE_FLOAT_POLICY true
#endif
#ifndef BOOST_MATH_PROMOTE_DOUBLE_POLICY
#ifdef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
#define BOOST_MATH_PROMOTE_DOUBLE_POLICY false
#else
#define BOOST_MATH_PROMOTE_DOUBLE_POLICY true
#endif
#endif
#ifndef BOOST_MATH_DISCRETE_QUANTILE_POLICY
#define BOOST_MATH_DISCRETE_QUANTILE_POLICY integer_round_outwards
#endif
#ifndef BOOST_MATH_ASSERT_UNDEFINED_POLICY
#define BOOST_MATH_ASSERT_UNDEFINED_POLICY true
#endif
#ifndef BOOST_MATH_MAX_SERIES_ITERATION_POLICY
#define BOOST_MATH_MAX_SERIES_ITERATION_POLICY 1000000
#endif
#ifndef BOOST_MATH_MAX_ROOT_ITERATION_POLICY
#define BOOST_MATH_MAX_ROOT_ITERATION_POLICY 200
#endif

#if !defined(__BORLANDC__)
#define BOOST_MATH_META_INT(type, name, Default)\
   template <type N = Default> struct name : public boost::mpl::int_<N>{};\
   namespace detail{\
   template <type N>\
   char test_is_valid_arg(const name<N>*);\
   char test_is_default_arg(const name<Default>*);\
   template <class T> struct is_##name##_imp\
   {\
      template <type N> static char test(const name<N>*);\
      static double test(...);\
      BOOST_STATIC_CONSTANT(bool, value = sizeof(test(static_cast<T*>(0))) == 1);\
   };\
   }\
   template <class T> struct is_##name : public boost::mpl::bool_< ::boost::math::policies::detail::is_##name##_imp<T>::value>{};

#define BOOST_MATH_META_BOOL(name, Default)\
   template <bool N = Default> struct name : public boost::mpl::bool_<N>{};\
   namespace detail{\
   template <bool N>\
   char test_is_valid_arg(const name<N>*);\
   char test_is_default_arg(const name<Default>*);\
   template <class T> struct is_##name##_imp\
   {\
      template <bool N> static char test(const name<N>*);\
      static double test(...);\
      BOOST_STATIC_CONSTANT(bool, value = sizeof(test(static_cast<T*>(0))) == 1);\
   };\
   }\
   template <class T> struct is_##name : public boost::mpl::bool_< ::boost::math::policies::detail::is_##name##_imp<T>::value>{};
#else
#define BOOST_MATH_META_INT(Type, name, Default)\
   template <Type N = Default> struct name : public boost::mpl::int_<N>{};\
   namespace detail{\
   template <Type N>\
   char test_is_valid_arg(const name<N>*);\
   char test_is_default_arg(const name<Default>*);\
   template <class T> struct is_##name##_tester\
   {\
      template <Type N> static char test(const name<N>&);\
      static double test(...);\
   };\
   template <class T> struct is_##name##_imp\
   {\
      static T inst;\
      BOOST_STATIC_CONSTANT(bool, value = sizeof( ::boost::math::policies::detail::is_##name##_tester<T>::test(inst)) == 1);\
   };\
   }\
   template <class T> struct is_##name : public boost::mpl::bool_< ::boost::math::policies::detail::is_##name##_imp<T>::value>\
   {\
      template <class U> struct apply{ typedef is_##name<U> type; };\
   };

#define BOOST_MATH_META_BOOL(name, Default)\
   template <bool N = Default> struct name : public boost::mpl::bool_<N>{};\
   namespace detail{\
   template <bool N>\
   char test_is_valid_arg(const name<N>*);\
   char test_is_default_arg(const name<Default>*);\
   template <class T> struct is_##name##_tester\
   {\
      template <bool N> static char test(const name<N>&);\
      static double test(...);\
   };\
   template <class T> struct is_##name##_imp\
   {\
      static T inst;\
      BOOST_STATIC_CONSTANT(bool, value = sizeof( ::boost::math::policies::detail::is_##name##_tester<T>::test(inst)) == 1);\
   };\
   }\
   template <class T> struct is_##name : public boost::mpl::bool_< ::boost::math::policies::detail::is_##name##_imp<T>::value>\
   {\
      template <class U> struct apply{ typedef is_##name<U> type;  };\
   };
#endif
//
// Begin by defining policy types for error handling:
//
enum error_policy_type
{
   throw_on_error = 0,
   errno_on_error = 1,
   ignore_error = 2,
   user_error = 3
};

BOOST_MATH_META_INT(error_policy_type, domain_error, BOOST_MATH_DOMAIN_ERROR_POLICY)
BOOST_MATH_META_INT(error_policy_type, pole_error, BOOST_MATH_POLE_ERROR_POLICY)
BOOST_MATH_META_INT(error_policy_type, overflow_error, BOOST_MATH_OVERFLOW_ERROR_POLICY)
BOOST_MATH_META_INT(error_policy_type, underflow_error, BOOST_MATH_UNDERFLOW_ERROR_POLICY)
BOOST_MATH_META_INT(error_policy_type, denorm_error, BOOST_MATH_DENORM_ERROR_POLICY)
BOOST_MATH_META_INT(error_policy_type, evaluation_error, BOOST_MATH_EVALUATION_ERROR_POLICY)
BOOST_MATH_META_INT(error_policy_type, rounding_error, BOOST_MATH_ROUNDING_ERROR_POLICY)
BOOST_MATH_META_INT(error_policy_type, indeterminate_result_error, BOOST_MATH_INDETERMINATE_RESULT_ERROR_POLICY)

//
// Policy types for internal promotion:
//
BOOST_MATH_META_BOOL(promote_float, BOOST_MATH_PROMOTE_FLOAT_POLICY)
BOOST_MATH_META_BOOL(promote_double, BOOST_MATH_PROMOTE_DOUBLE_POLICY)
BOOST_MATH_META_BOOL(assert_undefined, BOOST_MATH_ASSERT_UNDEFINED_POLICY)
//
// Policy types for discrete quantiles:
//
enum discrete_quantile_policy_type
{
   real,
   integer_round_outwards,
   integer_round_inwards,
   integer_round_down,
   integer_round_up,
   integer_round_nearest
};

BOOST_MATH_META_INT(discrete_quantile_policy_type, discrete_quantile, BOOST_MATH_DISCRETE_QUANTILE_POLICY)
//
// Precision:
//
BOOST_MATH_META_INT(int, digits10, BOOST_MATH_DIGITS10_POLICY)
BOOST_MATH_META_INT(int, digits2, 0)
//
// Iterations:
//
BOOST_MATH_META_INT(unsigned long, max_series_iterations, BOOST_MATH_MAX_SERIES_ITERATION_POLICY)
BOOST_MATH_META_INT(unsigned long, max_root_iterations, BOOST_MATH_MAX_ROOT_ITERATION_POLICY)
//
// Define the names for each possible policy:
//
#define BOOST_MATH_PARAMETER(name)\
   BOOST_PARAMETER_TEMPLATE_KEYWORD(name##_name)\
   BOOST_PARAMETER_NAME(name##_name)

struct default_policy{};

namespace detail{
//
// Trait to work out bits precision from digits10 and digits2:
//
template <class Digits10, class Digits2>
struct precision
{
   //
   // Now work out the precision:
   //
   typedef typename mpl::if_c<
      (Digits10::value == 0),
      digits2<0>,
      digits2<((Digits10::value + 1) * 1000L) / 301L>
   >::type digits2_type;
public:
#ifdef __BORLANDC__
   typedef typename mpl::if_c<
      (Digits2::value > ::boost::math::policies::detail::precision<Digits10,Digits2>::digits2_type::value),
      Digits2, digits2_type>::type type;
#else
   typedef typename mpl::if_c<
      (Digits2::value > digits2_type::value),
      Digits2, digits2_type>::type type;
#endif
};

template <class A, class B, bool b>
struct select_result
{
   typedef A type;
};
template <class A, class B>
struct select_result<A, B, false>
{
   typedef typename mpl::deref<B>::type type;
};

template <class Seq, class Pred, class DefaultType>
struct find_arg
{
private:
   typedef typename mpl::find_if<Seq, Pred>::type iter;
   typedef typename mpl::end<Seq>::type end_type;
public:
   typedef typename select_result<
      DefaultType, iter,
      ::boost::is_same<iter, end_type>::value>::type type;
};

double test_is_valid_arg(...);
double test_is_default_arg(...);
char test_is_valid_arg(const default_policy*);
char test_is_default_arg(const default_policy*);

template <class T>
struct is_valid_policy_imp
{
   BOOST_STATIC_CONSTANT(bool, value = sizeof(::boost::math::policies::detail::test_is_valid_arg(static_cast<T*>(0))) == 1);
};

template <class T>
struct is_default_policy_imp
{
   BOOST_STATIC_CONSTANT(bool, value = sizeof(::boost::math::policies::detail::test_is_default_arg(static_cast<T*>(0))) == 1);
};

template <class T> struct is_valid_policy
: public mpl::bool_<
   ::boost::math::policies::detail::is_valid_policy_imp<T>::value>
{};

template <class T> struct is_default_policy
: public mpl::bool_<
   ::boost::math::policies::detail::is_default_policy_imp<T>::value>
{
   template <class U>
   struct apply
   {
      typedef is_default_policy<U> type;
   };
};

template <class Seq, class T, int N>
struct append_N
{
   typedef typename mpl::push_back<Seq, T>::type new_seq;
   typedef typename append_N<new_seq, T, N-1>::type type;
};

template <class Seq, class T>
struct append_N<Seq, T, 0>
{
   typedef Seq type;
};

//
// Traits class to work out what template parameters our default
// policy<> class will have when modified for forwarding:
//
template <bool f, bool d>
struct default_args
{
   typedef promote_float<false> arg1;
   typedef promote_double<false> arg2;
};

template <>
struct default_args<false, false>
{
   typedef default_policy arg1;
   typedef default_policy arg2;
};

template <>
struct default_args<true, false>
{
   typedef promote_float<false> arg1;
   typedef default_policy arg2;
};

template <>
struct default_args<false, true>
{
   typedef promote_double<false> arg1;
   typedef default_policy arg2;
};

typedef default_args<BOOST_MATH_PROMOTE_FLOAT_POLICY, BOOST_MATH_PROMOTE_DOUBLE_POLICY>::arg1 forwarding_arg1;
typedef default_args<BOOST_MATH_PROMOTE_FLOAT_POLICY, BOOST_MATH_PROMOTE_DOUBLE_POLICY>::arg2 forwarding_arg2;

} // detail
//
// Now define the policy type with enough arguments to handle all
// the policies:
//
template <class A1 = default_policy,
          class A2 = default_policy,
          class A3 = default_policy,
          class A4 = default_policy,
          class A5 = default_policy,
          class A6 = default_policy,
          class A7 = default_policy,
          class A8 = default_policy,
          class A9 = default_policy,
          class A10 = default_policy,
          class A11 = default_policy,
          class A12 = default_policy,
          class A13 = default_policy>
struct policy
{
private:
   //
   // Validate all our arguments:
   //
   BOOST_STATIC_ASSERT(::boost::math::policies::detail::is_valid_policy<A1>::value);
   BOOST_STATIC_ASSERT(::boost::math::policies::detail::is_valid_policy<A2>::value);
   BOOST_STATIC_ASSERT(::boost::math::policies::detail::is_valid_policy<A3>::value);
   BOOST_STATIC_ASSERT(::boost::math::policies::detail::is_valid_policy<A4>::value);
   BOOST_STATIC_ASSERT(::boost::math::policies::detail::is_valid_policy<A5>::value);
   BOOST_STATIC_ASSERT(::boost::math::policies::detail::is_valid_policy<A6>::value);
   BOOST_STATIC_ASSERT(::boost::math::policies::detail::is_valid_policy<A7>::value);
   BOOST_STATIC_ASSERT(::boost::math::policies::detail::is_valid_policy<A8>::value);
   BOOST_STATIC_ASSERT(::boost::math::policies::detail::is_valid_policy<A9>::value);
   BOOST_STATIC_ASSERT(::boost::math::policies::detail::is_valid_policy<A10>::value);
   BOOST_STATIC_ASSERT(::boost::math::policies::detail::is_valid_policy<A11>::value);
   BOOST_STATIC_ASSERT(::boost::math::policies::detail::is_valid_policy<A12>::value);
   BOOST_STATIC_ASSERT(::boost::math::policies::detail::is_valid_policy<A13>::value);
   //
   // Typelist of the arguments:
   //
   typedef mpl::list<A1,A2,A3,A4,A5,A6,A7,A8,A9,A10,A11,A12,A13> arg_list;

public:
   typedef typename detail::find_arg<arg_list, is_domain_error<mpl::_1>, domain_error<> >::type domain_error_type;
   typedef typename detail::find_arg<arg_list, is_pole_error<mpl::_1>, pole_error<> >::type pole_error_type;
   typedef typename detail::find_arg<arg_list, is_overflow_error<mpl::_1>, overflow_error<> >::type overflow_error_type;
   typedef typename detail::find_arg<arg_list, is_underflow_error<mpl::_1>, underflow_error<> >::type underflow_error_type;
   typedef typename detail::find_arg<arg_list, is_denorm_error<mpl::_1>, denorm_error<> >::type denorm_error_type;
   typedef typename detail::find_arg<arg_list, is_evaluation_error<mpl::_1>, evaluation_error<> >::type evaluation_error_type;
   typedef typename detail::find_arg<arg_list, is_rounding_error<mpl::_1>, rounding_error<> >::type rounding_error_type;
   typedef typename detail::find_arg<arg_list, is_indeterminate_result_error<mpl::_1>, indeterminate_result_error<> >::type indeterminate_result_error_type;
private:
   //
   // Now work out the precision:
   //
   typedef typename detail::find_arg<arg_list, is_digits10<mpl::_1>, digits10<> >::type digits10_type;
   typedef typename detail::find_arg<arg_list, is_digits2<mpl::_1>, digits2<> >::type bits_precision_type;
public:
   typedef typename detail::precision<digits10_type, bits_precision_type>::type precision_type;
   //
   // Internal promotion:
   //
   typedef typename detail::find_arg<arg_list, is_promote_float<mpl::_1>, promote_float<> >::type promote_float_type;
   typedef typename detail::find_arg<arg_list, is_promote_double<mpl::_1>, promote_double<> >::type promote_double_type;
   //
   // Discrete quantiles:
   //
   typedef typename detail::find_arg<arg_list, is_discrete_quantile<mpl::_1>, discrete_quantile<> >::type discrete_quantile_type;
   //
   // Mathematically undefined properties:
   //
   typedef typename detail::find_arg<arg_list, is_assert_undefined<mpl::_1>, assert_undefined<> >::type assert_undefined_type;
   //
   // Max iterations:
   //
   typedef typename detail::find_arg<arg_list, is_max_series_iterations<mpl::_1>, max_series_iterations<> >::type max_series_iterations_type;
   typedef typename detail::find_arg<arg_list, is_max_root_iterations<mpl::_1>, max_root_iterations<> >::type max_root_iterations_type;
};
//
// These full specializations are defined to reduce the amount of
// template instantiations that have to take place when using the default
// policies, they have quite a large impact on compile times:
//
template <>
struct policy<default_policy, default_policy, default_policy, default_policy, default_policy, default_policy, default_policy, default_policy, default_policy, default_policy, default_policy>
{
public:
   typedef domain_error<> domain_error_type;
   typedef pole_error<> pole_error_type;
   typedef overflow_error<> overflow_error_type;
   typedef underflow_error<> underflow_error_type;
   typedef denorm_error<> denorm_error_type;
   typedef evaluation_error<> evaluation_error_type;
   typedef rounding_error<> rounding_error_type;
   typedef indeterminate_result_error<> indeterminate_result_error_type;
#if BOOST_MATH_DIGITS10_POLICY == 0
   typedef digits2<> precision_type;
#else
   typedef detail::precision<digits10<>, digits2<> >::type precision_type;
#endif
   typedef promote_float<> promote_float_type;
   typedef promote_double<> promote_double_type;
   typedef discrete_quantile<> discrete_quantile_type;
   typedef assert_undefined<> assert_undefined_type;
   typedef max_series_iterations<> max_series_iterations_type;
   typedef max_root_iterations<> max_root_iterations_type;
};

template <>
struct policy<detail::forwarding_arg1, detail::forwarding_arg2, default_policy, default_policy, default_policy, default_policy, default_policy, default_policy, default_policy, default_policy, default_policy>
{
public:
   typedef domain_error<> domain_error_type;
   typedef pole_error<> pole_error_type;
   typedef overflow_error<> overflow_error_type;
   typedef underflow_error<> underflow_error_type;
   typedef denorm_error<> denorm_error_type;
   typedef evaluation_error<> evaluation_error_type;
   typedef rounding_error<> rounding_error_type;
   typedef indeterminate_result_error<> indeterminate_result_error_type;
#if BOOST_MATH_DIGITS10_POLICY == 0
   typedef digits2<> precision_type;
#else
   typedef detail::precision<digits10<>, digits2<> >::type precision_type;
#endif
   typedef promote_float<false> promote_float_type;
   typedef promote_double<false> promote_double_type;
   typedef discrete_quantile<> discrete_quantile_type;
   typedef assert_undefined<> assert_undefined_type;
   typedef max_series_iterations<> max_series_iterations_type;
   typedef max_root_iterations<> max_root_iterations_type;
};

template <class Policy,
          class A1 = default_policy,
          class A2 = default_policy,
          class A3 = default_policy,
          class A4 = default_policy,
          class A5 = default_policy,
          class A6 = default_policy,
          class A7 = default_policy,
          class A8 = default_policy,
          class A9 = default_policy,
          class A10 = default_policy,
          class A11 = default_policy,
          class A12 = default_policy,
          class A13 = default_policy>
struct normalise
{
private:
   typedef mpl::list<A1,A2,A3,A4,A5,A6,A7,A8,A9,A10,A11,A12,A13> arg_list;
   typedef typename detail::find_arg<arg_list, is_domain_error<mpl::_1>, typename Policy::domain_error_type >::type domain_error_type;
   typedef typename detail::find_arg<arg_list, is_pole_error<mpl::_1>, typename Policy::pole_error_type >::type pole_error_type;
   typedef typename detail::find_arg<arg_list, is_overflow_error<mpl::_1>, typename Policy::overflow_error_type >::type overflow_error_type;
   typedef typename detail::find_arg<arg_list, is_underflow_error<mpl::_1>, typename Policy::underflow_error_type >::type underflow_error_type;
   typedef typename detail::find_arg<arg_list, is_denorm_error<mpl::_1>, typename Policy::denorm_error_type >::type denorm_error_type;
   typedef typename detail::find_arg<arg_list, is_evaluation_error<mpl::_1>, typename Policy::evaluation_error_type >::type evaluation_error_type;
   typedef typename detail::find_arg<arg_list, is_rounding_error<mpl::_1>, typename Policy::rounding_error_type >::type rounding_error_type;
   typedef typename detail::find_arg<arg_list, is_indeterminate_result_error<mpl::_1>, typename Policy::indeterminate_result_error_type >::type indeterminate_result_error_type;
   //
   // Now work out the precision:
   //
   typedef typename detail::find_arg<arg_list, is_digits10<mpl::_1>, digits10<> >::type digits10_type;
   typedef typename detail::find_arg<arg_list, is_digits2<mpl::_1>, typename Policy::precision_type >::type bits_precision_type;
   typedef typename detail::precision<digits10_type, bits_precision_type>::type precision_type;
   //
   // Internal promotion:
   //
   typedef typename detail::find_arg<arg_list, is_promote_float<mpl::_1>, typename Policy::promote_float_type >::type promote_float_type;
   typedef typename detail::find_arg<arg_list, is_promote_double<mpl::_1>, typename Policy::promote_double_type >::type promote_double_type;
   //
   // Discrete quantiles:
   //
   typedef typename detail::find_arg<arg_list, is_discrete_quantile<mpl::_1>, typename Policy::discrete_quantile_type >::type discrete_quantile_type;
   //
   // Mathematically undefined properties:
   //
   typedef typename detail::find_arg<arg_list, is_assert_undefined<mpl::_1>, typename Policy::assert_undefined_type >::type assert_undefined_type;
   //
   // Max iterations:
   //
   typedef typename detail::find_arg<arg_list, is_max_series_iterations<mpl::_1>, typename Policy::max_series_iterations_type>::type max_series_iterations_type;
   typedef typename detail::find_arg<arg_list, is_max_root_iterations<mpl::_1>, typename Policy::max_root_iterations_type>::type max_root_iterations_type;
   //
   // Define a typelist of the policies:
   //
   typedef mpl::vector<
      domain_error_type,
      pole_error_type,
      overflow_error_type,
      underflow_error_type,
      denorm_error_type,
      evaluation_error_type,
      rounding_error_type,
      indeterminate_result_error_type,
      precision_type,
      promote_float_type,
      promote_double_type,
      discrete_quantile_type,
      assert_undefined_type,
      max_series_iterations_type,
      max_root_iterations_type> result_list;
   //
   // Remove all the policies that are the same as the default:
   //
   typedef typename mpl::remove_if<result_list, detail::is_default_policy<mpl::_> >::type reduced_list;
   //
   // Pad out the list with defaults:
   //
   typedef typename detail::append_N<reduced_list, default_policy, (14 - ::boost::mpl::size<reduced_list>::value)>::type result_type;
public:
   typedef policy<
      typename mpl::at<result_type, mpl::int_<0> >::type,
      typename mpl::at<result_type, mpl::int_<1> >::type,
      typename mpl::at<result_type, mpl::int_<2> >::type,
      typename mpl::at<result_type, mpl::int_<3> >::type,
      typename mpl::at<result_type, mpl::int_<4> >::type,
      typename mpl::at<result_type, mpl::int_<5> >::type,
      typename mpl::at<result_type, mpl::int_<6> >::type,
      typename mpl::at<result_type, mpl::int_<7> >::type,
      typename mpl::at<result_type, mpl::int_<8> >::type,
      typename mpl::at<result_type, mpl::int_<9> >::type,
      typename mpl::at<result_type, mpl::int_<10> >::type,
      typename mpl::at<result_type, mpl::int_<11> >::type,
      typename mpl::at<result_type, mpl::int_<12> >::type > type;
};
//
// Full specialisation to speed up compilation of the common case:
//
template <>
struct normalise<policy<>,
          promote_float<false>,
          promote_double<false>,
          discrete_quantile<>,
          assert_undefined<>,
          default_policy,
          default_policy,
          default_policy,
          default_policy,
          default_policy,
          default_policy,
          default_policy>
{
   typedef policy<detail::forwarding_arg1, detail::forwarding_arg2> type;
};

template <>
struct normalise<policy<detail::forwarding_arg1, detail::forwarding_arg2>,
          promote_float<false>,
          promote_double<false>,
          discrete_quantile<>,
          assert_undefined<>,
          default_policy,
          default_policy,
          default_policy,
          default_policy,
          default_policy,
          default_policy,
          default_policy>
{
   typedef policy<detail::forwarding_arg1, detail::forwarding_arg2> type;
};

inline policy<> make_policy()
{ return policy<>(); }

template <class A1>
inline typename normalise<policy<>, A1>::type make_policy(const A1&)
{
   typedef typename normalise<policy<>, A1>::type result_type;
   return result_type();
}

template <class A1, class A2>
inline typename normalise<policy<>, A1, A2>::type make_policy(const A1&, const A2&)
{
   typedef typename normalise<policy<>, A1, A2>::type result_type;
   return result_type();
}

template <class A1, class A2, class A3>
inline typename normalise<policy<>, A1, A2, A3>::type make_policy(const A1&, const A2&, const A3&)
{
   typedef typename normalise<policy<>, A1, A2, A3>::type result_type;
   return result_type();
}

template <class A1, class A2, class A3, class A4>
inline typename normalise<policy<>, A1, A2, A3, A4>::type make_policy(const A1&, const A2&, const A3&, const A4&)
{
   typedef typename normalise<policy<>, A1, A2, A3, A4>::type result_type;
   return result_type();
}

template <class A1, class A2, class A3, class A4, class A5>
inline typename normalise<policy<>, A1, A2, A3, A4, A5>::type make_policy(const A1&, const A2&, const A3&, const A4&, const A5&)
{
   typedef typename normalise<policy<>, A1, A2, A3, A4, A5>::type result_type;
   return result_type();
}

template <class A1, class A2, class A3, class A4, class A5, class A6>
inline typename normalise<policy<>, A1, A2, A3, A4, A5, A6>::type make_policy(const A1&, const A2&, const A3&, const A4&, const A5&, const A6&)
{
   typedef typename normalise<policy<>, A1, A2, A3, A4, A5, A6>::type result_type;
   return result_type();
}

template <class A1, class A2, class A3, class A4, class A5, class A6, class A7>
inline typename normalise<policy<>, A1, A2, A3, A4, A5, A6, A7>::type make_policy(const A1&, const A2&, const A3&, const A4&, const A5&, const A6&, const A7&)
{
   typedef typename normalise<policy<>, A1, A2, A3, A4, A5, A6, A7>::type result_type;
   return result_type();
}

template <class A1, class A2, class A3, class A4, class A5, class A6, class A7, class A8>
inline typename normalise<policy<>, A1, A2, A3, A4, A5, A6, A7, A8>::type make_policy(const A1&, const A2&, const A3&, const A4&, const A5&, const A6&, const A7&, const A8&)
{
   typedef typename normalise<policy<>, A1, A2, A3, A4, A5, A6, A7, A8>::type result_type;
   return result_type();
}

template <class A1, class A2, class A3, class A4, class A5, class A6, class A7, class A8, class A9>
inline typename normalise<policy<>, A1, A2, A3, A4, A5, A6, A7, A8, A9>::type make_policy(const A1&, const A2&, const A3&, const A4&, const A5&, const A6&, const A7&, const A8&, const A9&)
{
   typedef typename normalise<policy<>, A1, A2, A3, A4, A5, A6, A7, A8, A9>::type result_type;
   return result_type();
}

template <class A1, class A2, class A3, class A4, class A5, class A6, class A7, class A8, class A9, class A10>
inline typename normalise<policy<>, A1, A2, A3, A4, A5, A6, A7, A8, A9, A10>::type make_policy(const A1&, const A2&, const A3&, const A4&, const A5&, const A6&, const A7&, const A8&, const A9&, const A10&)
{
   typedef typename normalise<policy<>, A1, A2, A3, A4, A5, A6, A7, A8, A9, A10>::type result_type;
   return result_type();
}

template <class A1, class A2, class A3, class A4, class A5, class A6, class A7, class A8, class A9, class A10, class A11>
inline typename normalise<policy<>, A1, A2, A3, A4, A5, A6, A7, A8, A9, A10, A11>::type make_policy(const A1&, const A2&, const A3&, const A4&, const A5&, const A6&, const A7&, const A8&, const A9&, const A10&, const A11&)
{
   typedef typename normalise<policy<>, A1, A2, A3, A4, A5, A6, A7, A8, A9, A10, A11>::type result_type;
   return result_type();
}

//
// Traits class to handle internal promotion:
//
template <class Real, class Policy>
struct evaluation
{
   typedef Real type;
};

template <class Policy>
struct evaluation<float, Policy>
{
   typedef typename mpl::if_<typename Policy::promote_float_type, double, float>::type type;
};

template <class Policy>
struct evaluation<double, Policy>
{
   typedef typename mpl::if_<typename Policy::promote_double_type, long double, double>::type type;
};

#ifdef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS

template <class Real>
struct basic_digits : public mpl::int_<0>{ };
template <>
struct basic_digits<float> : public mpl::int_<FLT_MANT_DIG>{ };
template <>
struct basic_digits<double> : public mpl::int_<DBL_MANT_DIG>{ };
template <>
struct basic_digits<long double> : public mpl::int_<LDBL_MANT_DIG>{ };

template <class Real, class Policy>
struct precision
{
   BOOST_STATIC_ASSERT( ::std::numeric_limits<Real>::radix == 2);
   typedef typename Policy::precision_type precision_type;
   typedef basic_digits<Real> digits_t;
   typedef typename mpl::if_<
      mpl::equal_to<digits_t, mpl::int_<0> >,
      // Possibly unknown precision:
      precision_type,
      typename mpl::if_<
         mpl::or_<mpl::less_equal<digits_t, precision_type>, mpl::less_equal<precision_type, mpl::int_<0> > >,
         // Default case, full precision for RealType:
         digits2< ::std::numeric_limits<Real>::digits>,
         // User customised precision:
         precision_type
      >::type
   >::type type;
};

template <class Policy>
struct precision<float, Policy>
{
   typedef digits2<FLT_MANT_DIG> type;
};
template <class Policy>
struct precision<double, Policy>
{
   typedef digits2<DBL_MANT_DIG> type;
};
template <class Policy>
struct precision<long double, Policy>
{
   typedef digits2<LDBL_MANT_DIG> type;
};

#else

template <class Real, class Policy>
struct precision
{
   BOOST_STATIC_ASSERT((::std::numeric_limits<Real>::radix == 2) || ((::std::numeric_limits<Real>::is_specialized == 0) || (::std::numeric_limits<Real>::digits == 0)));
#ifndef __BORLANDC__
   typedef typename Policy::precision_type precision_type;
   typedef typename mpl::if_c<
      ((::std::numeric_limits<Real>::is_specialized == 0) || (::std::numeric_limits<Real>::digits == 0)),
      // Possibly unknown precision:
      precision_type,
      typename mpl::if_c<
         ((::std::numeric_limits<Real>::digits <= precision_type::value)
         || (Policy::precision_type::value <= 0)),
         // Default case, full precision for RealType:
         digits2< ::std::numeric_limits<Real>::digits>,
         // User customised precision:
         precision_type
      >::type
   >::type type;
#else
   typedef typename Policy::precision_type precision_type;
   typedef mpl::int_< ::std::numeric_limits<Real>::digits> digits_t;
   typedef mpl::bool_< ::std::numeric_limits<Real>::is_specialized> spec_t;
   typedef typename mpl::if_<
      mpl::or_<mpl::equal_to<spec_t, mpl::false_>, mpl::equal_to<digits_t, mpl::int_<0> > >,
      // Possibly unknown precision:
      precision_type,
      typename mpl::if_<
         mpl::or_<mpl::less_equal<digits_t, precision_type>, mpl::less_equal<precision_type, mpl::int_<0> > >,
         // Default case, full precision for RealType:
         digits2< ::std::numeric_limits<Real>::digits>,
         // User customised precision:
         precision_type
      >::type
   >::type type;
#endif
};

#endif

#ifdef BOOST_MATH_USE_FLOAT128

template <class Policy>
struct precision<__float128, Policy>
{
   typedef mpl::int_<113> type;
};

#endif

namespace detail{

template <class T, class Policy>
inline int digits_imp(mpl::true_ const&)
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   BOOST_STATIC_ASSERT( ::std::numeric_limits<T>::is_specialized);
#else
   BOOST_ASSERT(::std::numeric_limits<T>::is_specialized);
#endif
   typedef typename boost::math::policies::precision<T, Policy>::type p_t;
   return p_t::value;
}

template <class T, class Policy>
inline int digits_imp(mpl::false_ const&)
{
   return tools::digits<T>();
}

} // namespace detail

template <class T, class Policy>
inline int digits(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE(T))
{
   typedef mpl::bool_< std::numeric_limits<T>::is_specialized > tag_type;
   return detail::digits_imp<T, Policy>(tag_type());
}

template <class Policy>
inline unsigned long get_max_series_iterations()
{
   typedef typename Policy::max_series_iterations_type iter_type;
   return iter_type::value;
}

template <class Policy>
inline unsigned long get_max_root_iterations()
{
   typedef typename Policy::max_root_iterations_type iter_type;
   return iter_type::value;
}

namespace detail{

template <class T, class Digits, class Small, class Default>
struct series_factor_calc
{
   static T get()
   {
      return ldexp(T(1.0), 1 - Digits::value);
   }
};

template <class T, class Digits>
struct series_factor_calc<T, Digits, mpl::true_, mpl::true_>
{
   static T get()
   {
      return boost::math::tools::epsilon<T>();
   }
};
template <class T, class Digits>
struct series_factor_calc<T, Digits, mpl::true_, mpl::false_>
{
   static T get()
   {
      static const boost::uintmax_t v = static_cast<boost::uintmax_t>(1u) << (Digits::value - 1);
      return 1 / static_cast<T>(v);
   }
};
template <class T, class Digits>
struct series_factor_calc<T, Digits, mpl::false_, mpl::true_>
{
   static T get()
   {
      return boost::math::tools::epsilon<T>();
   }
};

template <class T, class Policy>
inline T get_epsilon_imp(mpl::true_ const&)
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   BOOST_STATIC_ASSERT( ::std::numeric_limits<T>::is_specialized);
   BOOST_STATIC_ASSERT( ::std::numeric_limits<T>::radix == 2);
#else
   BOOST_ASSERT(::std::numeric_limits<T>::is_specialized);
   BOOST_ASSERT(::std::numeric_limits<T>::radix == 2);
#endif
   typedef typename boost::math::policies::precision<T, Policy>::type p_t;
   typedef mpl::bool_<p_t::value <= std::numeric_limits<boost::uintmax_t>::digits> is_small_int;
   typedef mpl::bool_<p_t::value >= std::numeric_limits<T>::digits> is_default_value;
   return series_factor_calc<T, p_t, is_small_int, is_default_value>::get();
}

template <class T, class Policy>
inline T get_epsilon_imp(mpl::false_ const&)
{
   return tools::epsilon<T>();
}

} // namespace detail

template <class T, class Policy>
inline T get_epsilon(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE(T))
{
   typedef mpl::bool_< (std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::radix == 2)) > tag_type;
   return detail::get_epsilon_imp<T, Policy>(tag_type());
}

namespace detail{

template <class A1,
          class A2,
          class A3,
          class A4,
          class A5,
          class A6,
          class A7,
          class A8,
          class A9,
          class A10,
          class A11>
char test_is_policy(const policy<A1,A2,A3,A4,A5,A6,A7,A8,A9,A10,A11>*);
double test_is_policy(...);

template <class P>
struct is_policy_imp
{
   BOOST_STATIC_CONSTANT(bool, value = (sizeof(::boost::math::policies::detail::test_is_policy(static_cast<P*>(0))) == 1));
};

}

template <class P>
struct is_policy : public mpl::bool_< ::boost::math::policies::detail::is_policy_imp<P>::value> {};

//
// Helper traits class for distribution error handling:
//
template <class Policy>
struct constructor_error_check
{
   typedef typename Policy::domain_error_type domain_error_type;
   typedef typename mpl::if_c<
      (domain_error_type::value == throw_on_error) || (domain_error_type::value == user_error),
      mpl::true_,
      mpl::false_>::type type;
};

template <class Policy>
struct method_error_check
{
   typedef typename Policy::domain_error_type domain_error_type;
   typedef typename mpl::if_c<
      (domain_error_type::value == throw_on_error) && (domain_error_type::value != user_error),
      mpl::false_,
      mpl::true_>::type type;
};

}}} // namespaces

#endif // BOOST_MATH_POLICY_HPP


/*
 * Copyright (c) 2011, Mattia Penati <mattia.penati@gmail.com>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright notice,
 *       this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice,
 *       this list of conditions and the following disclaimer in the documentation
 *       and/or other materials provided with the distribution.
 *     * Neither the name of the Politecnico di Milano nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef AMA_TENSOR_DETAIL_TENSOR_BASE_HPP
#define AMA_TENSOR_DETAIL_TENSOR_BASE_HPP 1

#include <boost/mpl/plus.hpp>

namespace ama
{
  namespace tensor_
  {

    /* this class is needed to retrieve the information of DERIVED type */
    template <typename T> struct tensor_traits;

    /*
     * This class is the base for every class that represent a tensor, it uses
     * the CRTP to implement the static polymorphism. The previous traits need
     * to access to the property of tensor:
     *   - value_type, the scalar type used to represent the numbers
     *   - dimension, the dimension of tensor
     *   - order, the order (or rank) of tensor
     *
     * Moreover this class is only for internal purpose, the developers can use
     * that class to implement new tensors. The end user can ignore this class.
     */
    template <typename DERIVED>
    class tensor_base
    {
    public:
      /* the type retrieved through the tensor_traits */
      typedef typename tensor_traits<DERIVED>::value_type value_type;

      typedef typename tensor_traits<DERIVED>::dimension_type dimension_type;

      typedef typename tensor_traits<DERIVED>::controvariant_type controvariant_type;
      typedef typename tensor_traits<DERIVED>::covariant_type covariant_type;
      typedef typename ::boost::mpl::plus<controvariant_type,covariant_type>::type order_type;

      typedef typename tensor_traits<DERIVED>::is_assignable is_assignable;
      typedef typename tensor_traits<DERIVED>::is_temporary is_temporary;

    private:
      /* the foundamental types for CRTP */
      typedef tensor_base<DERIVED> base_type;
      typedef DERIVED derived_type;

    public:
      /* cast the object to the base class */
      base_type & base() { return *static_cast<base_type *>(this); }
      base_type const & base() const { return *static_cast<base_type const *>(this); }

    public:
      /* cast the object to the derived class */
      derived_type & derived() { return *static_cast<derived_type *>(this); }
      derived_type const & derived() const { return *static_cast<derived_type const *>(this); }
    };

  }
}

#endif /* AMA_TENSOR_DETAIL_TENSOR_BASE_HPP */


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

Copyright (c) 2000-2007, Stanford University,
Rensselaer Polytechnic Institute, Kenneth E. Jansen,
Charles A. Taylor (see SimVascular Acknowledgements file
for additional contributors to the source code).

All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:

Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
Neither the name of the Stanford University or Rensselaer Polytechnic
Institute nor the names of its contributors may be used to endorse or
promote products derived from this software without specific prior
written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
DAMAGE.

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

#include "cmd.h"
#include "cvSolverIO.h"

#ifdef USE_ZLIB
#include "zlib.h"
#else
#include <stdlib.h>
#define gzopen fopen
#define gzprintf fprintf
#define gzFile FILE*
#define gzclose fclose
#define gzgets fgets
#define gzeof feof
#endif
#include <time.h>
#include <fstream>
using namespace std;

// added std::vector
#include <vector>
#include <unordered_map>
#include <unordered_set>
#include <algorithm>
#include <array>
#include <boost/config.hpp>

// =========
//   Cross
// =========
inline
void Cross(double ax, double ay, double az,
double bx, double by, double bz,
double *prodx, double *prody, double *prodz)
{
    (*prodx) = ay * bz - az * by;
    (*prody) = -(ax * bz - az * bx);
    (*prodz) = ax * by - ay * bx;
    return;
}

// =======
//   Dot
// =======

inline
double Dot(double ax, double ay, double az,
double bx, double by, double bz)
{
    double product;

    product = ax * bx + ay * by + az * bz;
    return product;
}

//
// some helpful global variables
//
extern const char* oformat;

extern int numNodes_;
extern int numElements_;
extern int numMeshEdges_;
extern int numMeshFaces_;
extern int numSolnVars_;
extern int numBoundaryFaces_;
extern int** boundaryElements_;
extern double* nodes_;
extern int* elements_;
extern int* boundaryElementsIds_;
extern std::unordered_map<int, std::vector<int>> boundaryElementIdToIndicesMap_;
extern int* xadj_;
extern int xadjSize_;
extern int* adjncy_;
extern int adjncySize_;
extern int* iBC_;
extern int* iBCB_;
extern double* BCB_;
extern int   DisplacementNumElements_;
extern int*  DisplacementConn_[3];
extern int   DisplacementNumNodes_;
extern int*  DisplacementNodeMap_;
extern int* ndsurfg_;

int writeGEOMBCDAT(char* filename);

gzFile fp_ = NULL;
char buffer_[MAXCMDLINELENGTH];

int NWopenFile(char* filename) {

    if (fp_ != NULL) {
        fprintf(stderr, "ERROR: Apparently a file is already open! (opening %s)\n", filename);
        return CV_ERROR;
    }

    fp_ = NULL;
    fp_ = gzopen(filename, "rb");
    if (fp_ == NULL) {
        fprintf(stderr, "ERROR: could not open file (%s)\n", filename);
        return CV_ERROR;
    }

    return CV_OK;
}

int NWcloseFile() {
    if (fp_ == NULL) {
        fprintf(stderr, "Closing a file already closed!\n");
        return CV_OK;
    }
    gzclose(fp_);
    fp_ = NULL;
    return CV_OK;
}

int NWgetNextLine(int *eof) {
    for (int i = 0; i < MAXCMDLINELENGTH; i++) {
        buffer_[i] = '\0';
    }
#ifdef USE_ZLIB
    gzgets(fp_,buffer_,MAXCMDLINELENGTH);
#else
    fgets(buffer_, MAXCMDLINELENGTH, fp_);
#endif

    *eof = gzeof(fp_);

    return CV_OK;
}

int NWgetNextNonBlankLine(int *eof) {
    NWgetNextLine(eof);
    for (int i = 0; i < MAXCMDLINELENGTH; i++) {
        if (buffer_[i] == '\0') break;
        if (buffer_[i] == '\n') break;
        if (buffer_[i] != ' ') return CV_OK;
    }
    return CV_ERROR;
}

int parseCmdStr(char *cmd, char *mystr) {
    // parse command string
    int n = 0;
    int end = 0;
    char ignored[MAXSTRINGLENGTH];
    ignored[0] = '\0';
    cmd_token_get(&n, cmd, ignored, &end);
    mystr[0] = '\0';
    cmd_token_get(&n, cmd, mystr, &end);
    return CV_OK;
}

int parseNum(char *cmd, int *num) {

    // parse command string
    char mystr[MAXSTRINGLENGTH];
    parseCmdStr(cmd, mystr);

    // do work
    *num = 0;
    if (sscanf(mystr, "%i", num) != 1) {
        fprintf(stderr, "error parsing num!\n");
        return CV_ERROR;
    }

    return CV_OK;

}

int parseNum2(char *cmd, int *num) {

    // parse command string
    int n = 0;
    int end = 0;
    char ignored[MAXSTRINGLENGTH];
    ignored[0] = '\0';
    cmd_token_get(&n, cmd, ignored, &end);
    char infile[MAXPATHLEN];
    infile[0] = '\0';
    cmd_token_get(&n, cmd, infile, &end);
    char surfidstr[MAXSTRINGLENGTH];
    surfidstr[0] = '\0';
    cmd_token_get(&n, cmd, surfidstr, &end);

    // do work

    *num = 0;

    int surfID = 0;
    if (sscanf(surfidstr, "%i", &surfID) != 1) {
        fprintf(stderr, "error parsing num!\n");
        return CV_ERROR;
    }

    *num = surfID;

    return CV_OK;

}

int parseDouble(char *cmd, double *num) {

    // parse command string
    char mystr[MAXSTRINGLENGTH];
    parseCmdStr(cmd, mystr);

    // do work
    *num = 0.0;
    if (sscanf(mystr, "%lf", num) != 1) {
        fprintf(stderr, "error parsing double!\n");
        return CV_ERROR;
    }

    return CV_OK;

}

int parseDouble2(char *cmd, double *num) {

    // parse command string

    int n = 0;
    int end = 0;
    char ignored[MAXSTRINGLENGTH];
    ignored[0] = '\0';
    cmd_token_get(&n, cmd, ignored, &end);
    ignored[0] = '\0';
    cmd_token_get(&n, cmd, ignored, &end);

    char mystr[MAXSTRINGLENGTH];
    mystr[0] = '\0';
    cmd_token_get(&n, cmd, mystr, &end);

    // do work
    *num = 0.0;
    if (sscanf(mystr, "%lf", num) != 1) {
        fprintf(stderr, "error parsing double!\n");
        return CV_ERROR;
    }

    return CV_OK;

}

int parseDouble3(char *cmd, double *v1, double *v2, double *v3) {

    // parse command string
    int n = 0;
    int end = 0;
    char ignored[MAXSTRINGLENGTH];
    ignored[0] = '\0';
    cmd_token_get(&n, cmd, ignored, &end);
    char str1[MAXPATHLEN];
    str1[0] = '\0';
    cmd_token_get(&n, cmd, str1, &end);
    char str2[MAXSTRINGLENGTH];
    str2[0] = '\0';
    cmd_token_get(&n, cmd, str2, &end);
    char str3[MAXSTRINGLENGTH];
    str3[0] = '\0';
    cmd_token_get(&n, cmd, str3, &end);

    // do work
    *v1 = 0.0;
    if (sscanf(str1, "%lf", v1) != 1) {
        fprintf(stderr, "error parsing double!\n");
        return CV_ERROR;
    }
    *v2 = 0.0;
    if (sscanf(str2, "%lf", v2) != 1) {
        fprintf(stderr, "error parsing double!\n");
        return CV_ERROR;
    }
    *v3 = 0.0;
    if (sscanf(str3, "%lf", v3) != 1) {
        fprintf(stderr, "error parsing double!\n");
        return CV_ERROR;
    }

    return CV_OK;

}

int parseFile(char *cmd) {

    // parse command string
    char infile[MAXPATHLEN];
    parseCmdStr(cmd, infile);

    // do work
    return NWopenFile(infile);

}

int setNodesWithCode(char *cmd, int val) {

    // enter
    debugprint(stddbg, "Entering setNodesWithCode.\n");

    // do work
    if (numNodes_ == 0) {
        fprintf(stderr, "ERROR:  Must specify number of nodes before you read them in!\n");
        return CV_ERROR;
    }

    if (parseFile(cmd) == CV_ERROR) {
        return CV_ERROR;
    }

    if (iBC_ == NULL) {
        iBC_ = new int[numNodes_]();
    }

    int eof = 0;
    int nodeId = 0;

    while (NWgetNextNonBlankLine(&eof) == CV_OK) {
        if (sscanf(buffer_, "%i", &nodeId) != 1) {
            fprintf(stderr, "WARNING:  line not of correct format (%s)\n", buffer_);
        }
        else {
            // this should be a bit set instead of an int!!
            iBC_[nodeId - 1] = val;
        }
    }
    if (eof == 0) return CV_ERROR;
    NWcloseFile();

    // cleanup
    debugprint(stddbg, "Exiting setNodesWithCode.\n");
    return CV_OK;

}

int check_node_order(int n0, int n1, int n2, int n3, int elementId,
    int *k0, int *k1, int *k2, int *k3) {

    int i, j0, j1, j2, j3;

    if (n3 >= 0) {

        if (phasta_node_order_ == 0) {
            // Not necessary if using phasta-conn
            // flip the middle to vertexes because we used a value of 1
            // for node ordering in meshsim and scorec used the value of 0
            j0 = n0;
            j1 = n2;
            j2 = n1;
            j3 = n3;
        }
        else {
            j0 = n0;
            j1 = n1;
            j2 = n2;
            j3 = n3;
        }

    }
    else {

        j0 = n0;
        j1 = n1;
        j2 = n2;
        j3 = -1;

        for (i = 0; i < 4; i++) {
            if (elements_[i*numElements_ + (elementId - 1)] != j0 &&
                elements_[i*numElements_ + (elementId - 1)] != j1 &&
                elements_[i*numElements_ + (elementId - 1)] != j2) {
                j3 = elements_[i*numElements_ + (elementId - 1)];
                break;
            }
        }
        if (j3 < 0) {
            //gzclose(fp);
            fprintf(stderr, "ERROR:  could not find nodes in element (%i %i %i %i)\n",
                elementId, n0, n1, n2);
            return CV_ERROR;
        }

    }

    double a[3];
    double b[3];
    double c[3];
    double norm0, norm1, norm2;

    a[0] = nodes_[0 * numNodes_ + j1 - 1] - nodes_[0 * numNodes_ + j0 - 1];
    a[1] = nodes_[1 * numNodes_ + j1 - 1] - nodes_[1 * numNodes_ + j0 - 1];
    a[2] = nodes_[2 * numNodes_ + j1 - 1] - nodes_[2 * numNodes_ + j0 - 1];
    b[0] = nodes_[0 * numNodes_ + j2 - 1] - nodes_[0 * numNodes_ + j0 - 1];
    b[1] = nodes_[1 * numNodes_ + j2 - 1] - nodes_[1 * numNodes_ + j0 - 1];
    b[2] = nodes_[2 * numNodes_ + j2 - 1] - nodes_[2 * numNodes_ + j0 - 1];
    c[0] = nodes_[0 * numNodes_ + j3 - 1] - nodes_[0 * numNodes_ + j0 - 1];
    c[1] = nodes_[1 * numNodes_ + j3 - 1] - nodes_[1 * numNodes_ + j0 - 1];
    c[2] = nodes_[2 * numNodes_ + j3 - 1] - nodes_[2 * numNodes_ + j0 - 1];

    Cross(a[0], a[1], a[2], b[0], b[1], b[2], &norm0, &norm1, &norm2);
    double mydot = Dot(norm0, norm1, norm2, c[0], c[1], c[2]);

    if (mydot > 0) {
        int tmpj = j0;
        j0 = j2;
        j2 = j1;
        j1 = tmpj;
        debugprint(stdout, "elementId %i : %i %i %i %i   (flipped) %lf\n", elementId, j0, j1, j2, j3, mydot);
    }
    else {
        //debugprint(stdout,"elementId %i : %i %i %i %i  %lf\n",elementId,j0,j1,j2,j3,mydot);
    }

    *k0 = j0; *k1 = j1; *k2 = j2; *k3 = j3;

    return CV_OK;
}

int setBoundaryFacesWithCode(char* cmd, int setSurfID, int surfID, int setCode, int code, double value)
{

    // enter
    debugprint(stddbg, "Entering setBoundaryFacesWithCode.\n");

    int i;

    // parse command string
    if (parseFile(cmd) == CV_ERROR) {
        return CV_ERROR;
    }

    if (iBCB_ == NULL) {
        iBCB_ = new int[2 * numBoundaryFaces_]();
        BCB_ = new double[numBoundaryFaces_ * 6]();
    }

    if (ndsurfg_ == NULL) {
        ndsurfg_ = new int[numNodes_]();
    }

    int elementId, matId;

    int eof = 0;

    vector<vector<int>> elem_edge; // vector of element edges - stored as node pairs [n1,n2] *** added by KDL 07/02/14
    vector<vector<int>> surf_data; // vector of surface elements -stored as  [ids,n1,n2,n3]  *** added by KDL 07/02/14

    while (NWgetNextNonBlankLine(&eof) == CV_OK) {

        int n0, n1, n2;
        if (sscanf(buffer_, "%i %i %i %i %i", &elementId, &matId, &n0, &n1, &n2) != 5) {
            fprintf(stderr, "WARNING:  line not of correct format (%s)\n", buffer_);
            NWcloseFile();
            return CV_ERROR;
        }

        /*
         * store node pairings for each element *** added by KDL 07/02/14
         */
        /*
         if (setSurfID) {

         // arrange each edge's node pair - lowest index first
         vector <int> node_pair;

         // check and add n0 & n1
         if (n0 < n1) {
         node_pair.push_back(int(n0));
         node_pair.push_back(int(n1));
         } else {
         node_pair.push_back(int(n1));
         node_pair.push_back(int(n0));
         }

         elem_edge.push_back(node_pair);
         node_pair.clear();

         // check and add n1 & n2
         if (n1 < n2) {
         node_pair.push_back(int(n1));
         node_pair.push_back(int(n2));
         } else {
         node_pair.push_back(int(n2));
         node_pair.push_back(int(n1));
         }

         elem_edge.push_back(node_pair);
         node_pair.clear();

         // check and add n2 & n0
         if (n0 < n2) {
         node_pair.push_back(int(n0));
         node_pair.push_back(int(n2));
         } else {
         node_pair.push_back(int(n2));
         node_pair.push_back(int(n0));
         }

         elem_edge.push_back(node_pair);
         node_pair.clear();
         }
         */

        int j0 = n0;
        int j1 = n1;
        int j2 = n2;
        int j3 = -1;

        for (i = 0; i < 4; i++) {
            if (elements_[i * numElements_ + (elementId - 1)] != j0 && elements_[i * numElements_ + (elementId - 1)] != j1 &&
                elements_[i * numElements_ + (elementId - 1)] != j2) {
                j3 = elements_[i * numElements_ + (elementId - 1)];
                break;
            }
        }
        if (j3 < 0) {
            NWcloseFile();
            fprintf(stderr, "ERROR:  could not find nodes in element (%i %i %i %i)\n", elementId, n0, n1, n2);
            return CV_ERROR;
        }

        double a[3];
        double b[3];
        double c[3];
        double norm0, norm1, norm2;

        a[0] = nodes_[0 * numNodes_ + j1 - 1] - nodes_[0 * numNodes_ + j0 - 1];
        a[1] = nodes_[1 * numNodes_ + j1 - 1] - nodes_[1 * numNodes_ + j0 - 1];
        a[2] = nodes_[2 * numNodes_ + j1 - 1] - nodes_[2 * numNodes_ + j0 - 1];
        b[0] = nodes_[0 * numNodes_ + j2 - 1] - nodes_[0 * numNodes_ + j0 - 1];
        b[1] = nodes_[1 * numNodes_ + j2 - 1] - nodes_[1 * numNodes_ + j0 - 1];
        b[2] = nodes_[2 * numNodes_ + j2 - 1] - nodes_[2 * numNodes_ + j0 - 1];
        c[0] = nodes_[0 * numNodes_ + j3 - 1] - nodes_[0 * numNodes_ + j0 - 1];
        c[1] = nodes_[1 * numNodes_ + j3 - 1] - nodes_[1 * numNodes_ + j0 - 1];
        c[2] = nodes_[2 * numNodes_ + j3 - 1] - nodes_[2 * numNodes_ + j0 - 1];

        Cross(a[0], a[1], a[2], b[0], b[1], b[2], &norm0, &norm1, &norm2);
        double mydot = Dot(norm0, norm1, norm2, c[0], c[1], c[2]);

        if (mydot > 0) {
            std::swap(j0, j1);
            /*
            int tmpj = j0;
            j0 = j2;
            j2 = j1;
            j1 = tmpj;
            */
            fprintf(stdout, "elementId %i : %i %i %i %i   (flipped0) %lf\n", elementId, j0, j1, j2, j3, mydot);
        }
        else {
            // fprintf(stdout,"elementId %i : %i %i %i %i  %lf\n",elementId,j0,j1,j2,j3,mydot);
        }

        // find matching element already read in
        auto iter = boundaryElementIdToIndicesMap_.find(elementId - 1);

        if (iter == boundaryElementIdToIndicesMap_.end()) {
            fprintf(stderr, "ERROR: could not find pressure face in boundary faces!\n");
            return CV_ERROR;
        }

        for (int i : iter->second) {
            std::array<int, 4> elementNodeIds{
                { boundaryElements_[0][i], boundaryElements_[1][i], boundaryElements_[2][i], boundaryElements_[3][i] } };

            if (std::find(elementNodeIds.begin(), elementNodeIds.end(), j0) != elementNodeIds.end() &&
                std::find(elementNodeIds.begin(), elementNodeIds.end(), j1) != elementNodeIds.end() &&
                std::find(elementNodeIds.begin(), elementNodeIds.end(), j2) != elementNodeIds.end() && elementNodeIds[3] == j3) {
                if (setCode) {
                    iBCB_[i] = code;
                    BCB_[1 * numBoundaryFaces_ + i] = value;
                }
                if (setSurfID) {
                    iBCB_[numBoundaryFaces_ + i] = surfID;
                    //                    // add surf ID to global ndsurfl
                    //                    ndsurfg_[j0-1] = surfID;
                    //                    ndsurfg_[j1-1] = surfID;
                    //                    ndsurfg_[j2-1] = surfID;

                    // first time we hit a node it is 0
                    // if it then set to 1 it is left alone

                    if (ndsurfg_[j0 - 1] != 1) {
                        ndsurfg_[j0 - 1] = surfID;
                        // fprintf(stdout,"surfID %i\n",surfID);
                    }

                    if (ndsurfg_[j1 - 1] != 1) {
                        ndsurfg_[j1 - 1] = surfID;
                    }

                    if (ndsurfg_[j2 - 1] != 1) {
                        ndsurfg_[j2 - 1] = surfID;
                    }

                    if (surfID != 1) {

                        if (ndsurfg_[j0 - 1] != 0) {
                            ndsurfg_[j0 - 1] = surfID;
                            // fprintf(stdout,"surfID %i\n",surfID);
                        }
                        if (ndsurfg_[j1 - 1] != 0) {
                            ndsurfg_[j1 - 1] = surfID;
                        }

                        if (ndsurfg_[j2 - 1] != 0) {
                            ndsurfg_[j2 - 1] = surfID;
                        }
                    }

                    vector<int> elem_data;
                    elem_data.push_back(elementId);
                    elem_data.push_back(boundaryElements_[0][i]);
                    elem_data.push_back(boundaryElements_[1][i]);
                    elem_data.push_back(boundaryElements_[2][i]);
                    surf_data.push_back(elem_data);
                }
                break;
            }
        }
    }

    NWcloseFile();

    vector<vector<double>> edge_coords; // vector of edges node coordinates
    vector<vector<double>> surf_coords; // vector of surface node coordinates

    /*
     * Calculate radius to each node
     * Added by KDL 07/02/14
     */
    /*
    if (setSurfID) {

    int count[elem_edge.size()];
    vector<double> xyz;
    double x,y,z;

    // count frequency
    for (int j=0; j<elem_edge.size(); j++) {
    count[j] = 0;
    for (int k=0; k<elem_edge.size(); k++) {
    if (elem_edge.at(j).at(0) == elem_edge.at(k).at(0) &&
    elem_edge.at(j).at(1) == elem_edge.at(k).at(1) ) {
    count[j] += 1;
    }
    }
    }

    // if count == 1 it is unique
    for (int j=0; j<elem_edge.size(); j++) {
    if (count[j] == 1) {

    x = nodes_[0*numNodes_ + elem_edge.at(j).at(0) - 1 ];
    y = nodes_[1*numNodes_ + elem_edge.at(j).at(0) - 1 ];
    z = nodes_[2*numNodes_ + elem_edge.at(j).at(0) - 1 ];
    xyz.push_back(x);
    xyz.push_back(y);
    xyz.push_back(z);
    edge_coords.push_back(xyz);
    xyz.clear();

    x = nodes_[0*numNodes_ + elem_edge.at(j).at(1) - 1 ];
    y = nodes_[1*numNodes_ + elem_edge.at(j).at(1) - 1 ];
    z = nodes_[2*numNodes_ + elem_edge.at(j).at(1) - 1 ];
    xyz.push_back(x);
    xyz.push_back(y);
    xyz.push_back(z);
    edge_coords.push_back(xyz);
    xyz.clear();

    }
    }

    for (int j=0; j<surf_data.size(); j++) {
    int id,n[3];

    id = surf_data[j].at(0);
    n[0] = surf_data[j].at(1);
    n[1] = surf_data[j].at(2);
    n[2] = surf_data[j].at(3);

    printf("%8i %8i %8i %8i\n",id, n[0], n[1], n[2]);

    for (int l=0; l<3; l++) {
    double x = nodes_[0*numNodes_ + n[l] - 1 ];
    double y = nodes_[1*numNodes_ + n[l] - 1 ];
    double z = nodes_[2*numNodes_ + n[l] - 1 ];
    printf("%12.3e %12.3e %12.3e\n",x,y,z);
    }

    }

    }
    */

    // cleanup
    debugprint(stddbg, "Exiting setBoundaryFacesWithCode.\n");
    return CV_OK;
}

int fixFreeEdgeNodes(char *cmd) {

    // enter
    debugprint(stddbg, "Entering fixFreeEdgeNodes.\n");

    // parse command string
    if (parseFile(cmd) == CV_ERROR) {
        return CV_ERROR;
    }

    if (iBC_ == NULL) {
        iBC_ = new int[numNodes_]();
    }

    // count lines in file
    int eof = 0;
    int numLinesInFile = 0;
    while (NWgetNextNonBlankLine(&eof) == CV_OK) {
        numLinesInFile++;
    }
    NWcloseFile();

    debugprint(stddbg, "Number of lines in file: %i\n", numLinesInFile);

    if (numLinesInFile == 0) {
        return CV_ERROR;
    }

    struct EdgeHasher {
        std::size_t operator()(const std::array<int, 2>& e) const
        {
            return std::hash<int>{}(e[0]) * 31 + std::hash<int>{}(e[1]);
        }
    };

    std::unordered_set<std::array<int, 2>, EdgeHasher> edges;

    if (parseFile(cmd) == CV_ERROR) {
        return CV_ERROR;
    }

    eof = 0;

    while (NWgetNextNonBlankLine(&eof) == CV_OK) {

        int n0, n1, n2;
        int elementId, matId;
        if (sscanf(buffer_, "%i %i %i %i %i", &elementId, &matId, &n0, &n1, &n2) != 5) {
            fprintf(stderr, "WARNING:  line not of correct format (%s)\n", buffer_);
            NWcloseFile();
            return CV_ERROR;
        }

        auto add_or_remove_edge = [&edges](int n0, int n1)
        {
            #ifdef BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX
            std::array<int, 2> toInsert{{ std::min(n0, n1), std::max(n0, n1) }};
            auto iterBoolPair = edges.insert(toInsert);
            #else
            auto iterBoolPair = edges.insert({ std::min(n0, n1), std::max(n0, n1) });
            #endif
            if (!iterBoolPair.second) {
                // Remove duplicate if already present in set
                edges.erase(iterBoolPair.first);
            }
        };

        add_or_remove_edge(n0, n1);
        add_or_remove_edge(n1, n2);
        add_or_remove_edge(n0, n2);
    }

    NWcloseFile();

    for (const auto& edge : edges) {
        debugprint(stddbg, "  Fixing Node: %i\n", edge[0]);
        debugprint(stddbg, "  Fixing Node: %i\n", edge[1]);
        // no slip code
        // this should be a bit set instead of an int!!
        iBC_[edge[0] - 1] = 56;
        iBC_[edge[1] - 1] = 56;
    }

    // cleanup

    debugprint(stddbg, "Exiting fixFreeEdgeNodes.\n");
    return CV_OK;
}

int createMeshForDispCalc(char *cmd) {

    // enter
    debugprint(stddbg, "Entering createMeshForDispCalc.\n");

    // parse command string
    if (parseFile(cmd) == CV_ERROR) {
        return CV_ERROR;
    }

    // count lines in file
    int eof = 0;
    int numLinesInFile = 0;
    while (NWgetNextNonBlankLine(&eof) == CV_OK) {
        numLinesInFile++;
    }
    NWcloseFile();

    debugprint(stddbg, "Number of lines in file: %i\n", numLinesInFile);

    if (numLinesInFile == 0) {
        return CV_ERROR;
    }

    std::vector<std::array<int, 3>> ids;
    ids.reserve(numLinesInFile);

    if (parseFile(cmd) == CV_ERROR) {
        return CV_ERROR;
    }

    eof = 0;
    int numElements = 0;
    while (NWgetNextNonBlankLine(&eof) == CV_OK) {
        int n0, n1, n2;
        int elementId, matId;

        if (sscanf(buffer_, "%i %i %i %i %i", &elementId, &matId, &n0, &n1, &n2) != 5) {
            fprintf(stderr, "WARNING:  line not of correct format (%s)\n", buffer_);
            NWcloseFile();
            return CV_ERROR;
        }

        #ifdef BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX
        {
            std::array<int, 3> toPushBack{ { n0, numElements, 0 } };
            ids.push_back(toPushBack);
        }
        {
            std::array<int, 3> toPushBack{ { n1, numElements, 1 } };
            ids.push_back(toPushBack);
        }
        {
            std::array<int, 3> toPushBack{ { n2, numElements, 2 } };
            ids.push_back(toPushBack);
        }
        #else
        ids.push_back({ { n0, numElements, 0 } });
        ids.push_back({ { n1, numElements, 1 } });
        ids.push_back({ { n2, numElements, 2 } });
        #endif

        numElements++;
    }

    NWcloseFile();

    // sort node ids so we can find duplicates
    std::sort(ids.begin(), ids.end());

    // count the number of unique nodes
    int numUniqueNodes = 1;
    int index0 = ids[0][0];
    for (int i = 0; i < ids.size(); i++) {
        if (ids[i][0] != index0) {
            numUniqueNodes++;
            index0 = ids[i][0];
        }
    }
    debugprint(stddbg, "  Number of Unique Nodes Found: %i\n", numUniqueNodes);

    // create renumbered connectivity for initial disp. calc.

    int* conn[3];
    conn[0] = new int[numElements];
    conn[1] = new int[numElements];
    conn[2] = new int[numElements];

    int* map = new int[numUniqueNodes];

    index0 = -1;
    int j = 0;

    ofstream mapping("mapping.dat");
    mapping << "i\t\tmap[i]" << endl;
    for (int i = 0; i < ids.size(); i++) {
        if (ids[i][0] != index0) {
            map[j] = ids[i][0];
            //debugprint(stddbg,"  map[%i] %i\n",j,map[j]);
            mapping << j + 1 << "\t\t" << map[j] << endl;
            index0 = ids[i][0];
            j++;
        }
        conn[ids[i][2]][ids[i][1]] = j - 1;
    }
    mapping.close();
    debugprint(stddbg, "  Number of Unique Nodes Found: %i\n", j);

    // set global variables
    DisplacementNumElements_ = numElements;
    DisplacementConn_[0] = conn[0];
    DisplacementConn_[1] = conn[1];
    DisplacementConn_[2] = conn[2];
    DisplacementNumNodes_ = numUniqueNodes;
    DisplacementNodeMap_ = map;

    debugprint(stddbg, "Exiting createMeshForDispCalc.\n");
    return CV_OK;
    return 0;
}


// -*- C++ -*-
// Implementation of class xFOOx
//
// Copyright (C) 2009-2010 by John Weiss
// This program is free software; you can redistribute it and/or modify
// it under the terms of the Artistic License, included as the file
// "LICENSE" in the source code archive.
//
// This program 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.
//
// You should have received a copy of the file "LICENSE", containing
// the License John Weiss originally placed this program under.
//
static const char* const
xFOOx_cc__="RCS $Id: statistics.cc 1887 2009-10-13 20:05:57Z candide $";

// Includes
//
#include <iostream>
#include <sys/types.h>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include <vector>
#include <string>
#include <stdexcept>
#include <sstream>
#include <algorithm>
#include <boost/static_assert.hpp>

#include "jpw_nld.h"
#include "Matrix.h"
#include "Manips.h"
#include "MathTools.h"
#include "statistics.h"

#include "details/Matrix.tcc"

using std::cout;
using std::cerr;
using std::endl;
using std::flush;
using std::scientific;
using std::setprecision;
using std::vector;
using outManips::reset;
using jpw_nld::index_t;

using namespace jpw_math;
using namespace jpw_math::statistics;

//
// Static variables
//

namespace {
 static bool g__pRNG_Initialized(false);
};

//
// Typedefs
//

typedef unsigned short seed48v_t[3];

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

//
// Inline Template Function Definitions
//

template<unsigned SZ> inline void
l__remap_seed__(time_t, seed48v_t&)
{
    BOOST_STATIC_ASSERT(false && SZ);
}

// Specialization for 64-bit time_t
template<> inline void
l__remap_seed__<8>(time_t rawSeedVal, seed48v_t& elSeed)
{
    // 64-bit time_t:  Ignore the upper 2 bytes, which change far more slowly
    //                 than the rest.
    // Layout:
    //    ........ ........ ........ ........ ........ ........
    // 48       40       32       24       16        8        0
    //           5        4        3        2        1        0

    // Bytes 0 and 5
    elSeed[0] = ( ((rawSeedVal & 0xFF) << 8)
                  | ((rawSeedVal >> 40) & 0xFF) );
    // Bytes 2 and 3
    elSeed[1] = ( ((rawSeedVal >> 8) & 0xFF00)
                  | ((rawSeedVal >> 24) & 0xFF) );
    // Bytes 4 and 1
    elSeed[2] = ( ((rawSeedVal >> 24) & 0xFF00)
                  | ((rawSeedVal >> 8) & 0xFF) );
}

// Specialization for 32-bit time_t
template<> inline void
l__remap_seed__<4>(time_t rawSeedVal, seed48v_t& elSeed)
{
    // 32-bit time_t:  Split into two 6-bit segments, and four 5-bit
    //                 segments.
    //
    // Layout - The Input:
    //   ........ ........ ........ ........
    //  |     |      |     |    |     |    |
    // 32    26     20    15   10     5    0
    //          3        2        1        0
    //
    // Layout - The Output:
    //    ......00 0.....00 .....000 00.....0 000..... 00......
    // 48       40       32       24       16        8        0
    //           5        4        3        2        1        0

    // Bits 0-4 and 26-31
    // Map 26-31 to the LSB, and 0-4 to the bit-0 of the upper byte.
    elSeed[0] = ( ((rawSeedVal & 0x1F) << 8)
                  | ((rawSeedVal >> 26) & 0x3F) );
    // Bits 10-14 and 15-19
    // Map 15-19 to the bit-1 of the lower byte, and 10-14 to the bit-3 of
    // the upper byte.
    elSeed[1] = ( ((rawSeedVal << 1) & 0xF800)
                  | ((rawSeedVal >> 14) & 0x3E) );
    // Bits 20-25 and 5-9
    // Map 5-9 to the bit-2 of the lower byte, and 20-25 to the bit-2 of
    // the upper byte.
    elSeed[2] = ( ((rawSeedVal >> 10) & 0x7C00)
                  | ((rawSeedVal >> 3) & 0x7C) );
}

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

//
// namespace statistics Function Definitions
//

// REFACTOR:  Use template functions & inline-functors to define the pRNG
// functions.
// Hmmm... use classes.  Design so that we can refactor the implementation at
// a later stage (like, say, the very last!)
//
// When refactoring the actual generators [like gaussian()], do some more
// performance tests.  See how well they inline.

void jpw_math::statistics::parseSeed(std::string& seedStr,
                                     unsigned& seedLo, unsigned& seedHi)
{
    // FIXME:  This is now part of 'jpwTools::RandomDataSrc'.
    using namespace std;

    if(seedStr.empty()) {
        return;
    }

    // Make sure there's a leading "0x" prefix, or you'll get parsing errors.
    if( (seedStr[0] & 0x5F) == 'X' ) {
        seedStr.insert(0, 1, '0');
    } else if( (seedStr.size() > 1) &&
               !( (seedStr[0] == '0') && ((seedStr[1] & 0x5F) == 'X') )
               ) {
        seedStr.insert(0, "0x");
    }

    unsigned long long seedVal(0);
    istringstream hex_iss(seedStr);
    hex_iss >> hex >> seedVal;
    if(!hex_iss.good() && !hex_iss.eof()) {
        string err("Not a valid format hex string:  \"");
        err += seedStr;
        err += '"';
        seedStr.clear();
        throw runtime_error(err);
    }

    seedHi = static_cast<unsigned>(seedVal >> 32);
    seedLo = static_cast<unsigned>(seedVal & 0xFFFFFFFF);
}

void jpw_math::statistics::init_rand(bool verbose)
{
    seed48v_t elseed;
    // get time/date to use as a seed
    l__remap_seed__<sizeof(time_t)>(time(static_cast<time_t*>(0)), elseed);
    if (verbose) {
        cout << std::hex
             << "\t\t[Random seed=0x"
             << elseed[2] << elseed[1] << elseed[0]
             << "]" << endl;
    }
    seed48(elseed);
    g__pRNG_Initialized = true;
}

void jpw_math::statistics::init_rand_firstTimeOnly(bool verbose)
{
    if(!g__pRNG_Initialized) {
        init_rand(verbose);
    }
}

void jpw_math::statistics::init_rand(unsigned s1, unsigned s2, bool verbose)
{
    seed48v_t elseed;

    elseed[2] = static_cast<unsigned short>(s1 & 0xFFFF);
    elseed[1] = static_cast<unsigned short>((s2 & 0xFFFF0000) >> 16);
    elseed[0] = static_cast<unsigned short>(s2 & 0xFFFF);
    if (verbose) {
        cout << std::hex << "\t\t[Specified seed=0x"
             << elseed[2] << elseed[1] << elseed[0]
             << "]" << endl;
    }
    seed48(elseed);
    g__pRNG_Initialized = true;
}

double jpw_math::statistics::gaussrand(void)
{
    static int iset=1;
    register double u,v,rsq;
    double fac;
    static double gset=0.0;

    if(iset)
    {
        // Pick two uniform random #'s on the unit circle.
        do
        {
            u = 2.0*drand48() - 1.0;
            v = 2.0*drand48() - 1.0;
            rsq = u*u + v*v + jpw_math::TINY;
        }
        while(rsq >= 1.0);

        // Now calculate the pair of normal random #'s.
        fac = sqrt(-2.0*log(rsq)/rsq);
        gset = v*fac;
        iset = 0;

        return(u*fac);
    }
    else
    {
        iset = 1;
        return(gset);
    }
}

double jpw_math::statistics::std_dev(const dvector_t& values)
{
    double mean(0);
    double sum_sq(0);
    double total(values.size());

    dvector_t::const_iterator values_end = values.end();
    for(dvector_t::const_iterator
            vi = values.begin(); vi != values_end; ++vi) {
        mean += *vi;
    }
    mean /= total;

    for(dvector_t::const_iterator
            vi = values.begin(); vi != values_end; ++vi) {
        sum_sq += jpw_math::SQR((*vi) - mean);
    }

    return(sqrt(sum_sq/(total-1)));
}

double jpw_math::statistics::signif_del_chisq_tg(double& siglevel)
{
    static double sigvals[]={.5,
                             .68268949,
                             .8,
                             .9,
                             .95,
                             .95449974,
                             .96,
                             .98,
                             .99,
                             .99730020,
                             .9999};
    static double del_chisq_vals[]={0.45493642,
                                    1.0,
                                    1.6423744,
                                    2.7055435,
                                    3.8414588,
                                    4.0,
                                    4.2178846,
                                    5.4118944,
                                    6.6348966,
                                    9.0,
                                    15.136705};
    static index_t nsigs=sizeof(sigvals)/sizeof(double);  // Should be 11.
    index_t i, sigidx;

    // Assumes errors obey Gaussian statistics and uses theory for finding the
    // significance of a value of DeltaChi^2 for 1 parameter.
    if(siglevel < sigvals[0])
    {
        sigidx = 0;
    }
    else if(siglevel >= sigvals[nsigs-1])
    {
        sigidx = nsigs-1;
    }
    else
    {
        i=1;
        sigidx=-1;
        while(i<nsigs)
        {
            if( (sigvals[i-1] <= siglevel) && (siglevel < sigvals[i]) )
            {
                sigidx = i-1;
            }
            ++i;
        }
    }

    siglevel = sigvals[sigidx];
    return(del_chisq_vals[sigidx]);
}

double jpw_math::statistics::confidence_tg(double del_chisq_sig,
                                           const dmatrix_t& inv_curv_m,
                                           index_t parmidx, bool verbose)
{
    double inval;

    // At the minimum, we should ALWAYS be positive definite.
    inval = inv_curv_m[parmidx][parmidx];
    if(jpw_math::NEAR_ZERO(inval, -jpw_math::DBL_PREC, jpw_math::DBL_PREC))
    {
        inval = 0.0;
    }

    if(inval <= 0.0)
    {
        if( (inval == 0.0) && verbose )
        {
            cerr << "confidence_tg():  null value at param #" << parmidx
                 << endl;
        }
        else if(inval < 0.0)
        {
            cerr << scientific << setprecision(2)
                 << "confidence_tg():  negative value, inv["
                 << parmidx << "][" << parmidx
                 << "]=" << inval
                 << "  Minimization Failure?"
                 << reset << endl;
        }
        return(jpw_math::HUGE);
    }

    return(sqrt(del_chisq_sig*inval));
}

long jpw_math::statistics::nearest_multiple(double val,
                                            double nearfac,
                                            bool use_larger)
{
    double yy_l, yy_u, nufac_l, nufac_u;
    double iy_l, iy_u;
    double aval, udiff, ldiff;

    // Make sure our input values are nonzero integers.
    val = rint(val);
    aval = jpw_math::ABS(val);
    nearfac = rint(nearfac);
    if(jpw_math::ABS(nearfac) >= aval)
    {
        // Note that this includes the case val==0.0.
        return(static_cast<long>(nearfac));
    }
    else if(nearfac == 0.0)
    {
        return(1L); // 1 is the closest multiple to 0 of any number.
    }

    yy_l = val/nearfac;
    iy_l = rint(yy_l);
    // Is nearfac actually a multiple of val?
    if(yy_l == iy_l)
    {
        return(static_cast<long>(nearfac));
    }

    yy_u = yy_l;
    iy_u = iy_l;
    nufac_l = nufac_u = nearfac;
    // Find multiple just above nearfac.
    while( (yy_u != iy_u) && (jpw_math::ABS(nufac_u) < aval) )
    {
        ++nufac_u;
        yy_u = val/nufac_u;
        iy_u = rint(yy_u);
    }
    // Find multiple just below nearfac.
    while( (yy_l != iy_l) && (jpw_math::ABS(nufac_l) > 1.0) )
    {
        --nufac_l;
        yy_l = val/nufac_l;
        iy_l = rint(yy_l);
    }
    udiff = jpw_math::ABS(nufac_u-nearfac);
    ldiff = jpw_math::ABS(nearfac-nufac_l);

    // Now choose the one which is nearest to 'nearfac'.
    if(udiff > ldiff)
    {
        return(static_cast<long>(nufac_l));
    }
    else if(udiff < ldiff)
    {
        return(static_cast<long>(nufac_u));
    }
    else
    {
        // When it's equally spaced, flip up or down depending on the value of
        // the flag "use_larger"
        if(use_larger) {
            return(static_cast<long>(nufac_u));
        }
        else
        {
            return(static_cast<long>(nufac_l));
        }
    }
}

/////////////////////////
//
// End


/*

Copyright (c) 2005-2022, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef _ABSTRACTONESTEPIVPODESOLVER_HPP_
#define _ABSTRACTONESTEPIVPODESOLVER_HPP_

#include "ChasteSerialization.hpp"
#include "ClassIsAbstract.hpp"
#include <boost/serialization/base_object.hpp>

#include "AbstractIvpOdeSolver.hpp"

/**
 * Abstract one-step initial value problem ODE solver class. Sets up variables and functions
 * for all the ODE solvers that only have one timestep.
*/
class AbstractOneStepIvpOdeSolver : public AbstractIvpOdeSolver
{

private:

    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive the abstract IVP Solver, never used directly - boost uses this.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        // This calls serialize on the base class.
        archive & boost::serialization::base_object<AbstractIvpOdeSolver>(*this);
    }

    /**
     * Working memory
     */
    std::vector<double> mWorkingMemory;

protected:

    /**
     * Method that actually performs the solving on behalf of the public Solve methods.
     *
     * @param pAbstractOdeSystem  the ODE system to solve
     * @param rCurrentYValues  the current (initial) state; results will also be returned
     *                         in here
     * @param rWorkingMemory  working memory; same size as rCurrentYValues
     * @param startTime  initial time
     * @param endTime  time to solve to
     * @param timeStep  dt
     */
    virtual void InternalSolve(AbstractOdeSystem* pAbstractOdeSystem,
                               std::vector<double>& rCurrentYValues,
                               std::vector<double>& rWorkingMemory,
                               double startTime,
                               double endTime,
                               double timeStep);

    /**
     * Calculate the solution to the ODE system at the next timestep.
     * Concrete subclasses should provide this method.
     *
     * @param pAbstractOdeSystem  the ODE system to solve
     * @param timeStep  dt
     * @param time  the current time
     * @param rCurrentYValues  the current (initial) state
     * @param rNextYValues  the state at the next timestep
     */
    virtual void CalculateNextYValue(AbstractOdeSystem* pAbstractOdeSystem,
                                     double timeStep,
                                     double time,
                                     std::vector<double>& rCurrentYValues,
                                     std::vector<double>& rNextYValues)=0;

public:

    /**
     * Solves a system of ODEs using a specified one-step ODE solver and returns
     * the solution as an OdeSolution object.
     *
     * An example, which returns the solution every 0.1 seconds using dt=0.01:
     *
     *     MyOdeSystem ode;
     *     std::vector<double> init_cond = ode_system.GetInitialConditions();
     *     OdeSolution solution = solver.Solve(&ode, init_cond, 0, 1, 0.01, 0.1);
     *
     *
     * @param pAbstractOdeSystem  pointer to the concrete ODE system to be solved
     * @param rYValues  a standard vector specifying the intial condition of each
     *                  solution variable in the system (this can be the initial
     *                  conditions vector stored in the ODE system)
     * @param startTime  the time at which the initial conditions are specified
     * @param endTime  the time to which the system should be solved and the solution
     *                 returned
     * @param timeStep  the time interval to be used by the solver
     * @param timeSampling  the interval at which to sample the solution to the ODE system
     *
     * @return OdeSolution is an object containing an integer of the number of
     * equations, a stdAbstractOdeSystem::vector of times and a std::vector of std::vectors where
     * each of those vectors contains the solution for one variable of the ODE
     * system at those times.
     */
    virtual OdeSolution Solve(AbstractOdeSystem* pAbstractOdeSystem,
                              std::vector<double>& rYValues,
                              double startTime,
                              double endTime,
                              double timeStep,
                              double timeSampling);

    /**
     * Second version of Solve. Solves a system of ODEs using a specified one-step
     * ODE solver. This method does not return the solution and therefore does not
     * take in a sampling time. Instead, the mStateVariables component in the ODE
     * system object is updated.
     *
     * An example:
     *
     *     std::vector<double> init_cond = ode_system.GetInitialConditions();
     *     solver.Solve(&ode, init_cond, 0, 1, 0.01);
     *     state_variables = ode_system.rGetStateVariables(); // solution at t=1 found here
     *
     *
     * @param pAbstractOdeSystem  pointer to the concrete ODE system to be solved
     * @param rYValues  a standard vector specifying the intial condition of each
     *                  solution variable in the system (this can be the initial
     *                  conditions vector stored in the ODE system)
     * @param startTime  the time at which the initial conditions are specified
     * @param endTime  the time to which the system should be solved and the solution
     *                 returned
     * @param timeStep  the time interval to be used by the solver
     */
    virtual void Solve(AbstractOdeSystem* pAbstractOdeSystem,
                       std::vector<double>& rYValues,
                       double startTime,
                       double endTime,
                       double timeStep);

    /**
     * Virtual destructor since we have virtual methods.
     */
    virtual ~AbstractOneStepIvpOdeSolver()
    {}
};

CLASS_IS_ABSTRACT(AbstractOneStepIvpOdeSolver)

#endif //_ABSTRACTONESTEPIVPODESOLVER_HPP_


//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

//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/qvm/operations.hpp>

#include <boost/qvm/quat.hpp>

#include <boost/qvm/vec.hpp>

#include <boost/qvm/mat.hpp>

namespace

my_stuff

    {

    struct

    mat

        {

        float a[3][3];

        };

    struct

    vec

        {

        float a[3];

        };

    struct

    quat

        {

        float a[4];

        };

    }

namespace

boost

    {

    namespace

    qvm

        {

        template <>

        struct

        mat_traits<my_stuff::mat>

            {

            typedef float scalar_type;

            static int const rows=3;

            static int const cols=3;

            template <int R,int C>

            static

            scalar_type &

            write_element( my_stuff::mat & m )

                {

                BOOST_QVM_STATIC_ASSERT(R>=0);

                BOOST_QVM_STATIC_ASSERT(R<rows);

                BOOST_QVM_STATIC_ASSERT(C>=0);

                BOOST_QVM_STATIC_ASSERT(C<cols);

                return m.a[R][C];

                }

            template <int R,int C>

            static

            scalar_type

            read_element( my_stuff::mat const & m )

                {

                BOOST_QVM_STATIC_ASSERT(R>=0);

                BOOST_QVM_STATIC_ASSERT(R<rows);

                BOOST_QVM_STATIC_ASSERT(C>=0);

                BOOST_QVM_STATIC_ASSERT(C<cols);

                return m.a[R][C];

                }

            static

            inline

            scalar_type &

            write_element_idx( int r, int c, my_stuff::mat & m )

                {

                BOOST_QVM_ASSERT(r>=0);

                BOOST_QVM_ASSERT(r<rows);

                BOOST_QVM_ASSERT(c>=0);

                BOOST_QVM_ASSERT(c<cols);

                return m.a[r][c];

                }

            static

            inline

            scalar_type

            read_element_idx( int r, int c, my_stuff::mat const & m )

                {

                BOOST_QVM_ASSERT(r>=0);

                BOOST_QVM_ASSERT(r<rows);

                BOOST_QVM_ASSERT(c>=0);

                BOOST_QVM_ASSERT(c<cols);

                return m.a[r][c];

                }

            };

        template <>

        struct

        vec_traits<my_stuff::vec>

            {

            static int const dim=3;

            typedef float scalar_type;

            template <int I>

            static

            scalar_type &

            write_element( my_stuff::vec & m )

                {

                BOOST_QVM_STATIC_ASSERT(I>=0);

                BOOST_QVM_STATIC_ASSERT(I<dim);

                return m.a[I];

                }

            template <int I>

            static

            scalar_type

            read_element( my_stuff::vec const & m )

                {

                BOOST_QVM_STATIC_ASSERT(I>=0);

                BOOST_QVM_STATIC_ASSERT(I<dim);

                return m.a[I];

                }

            static

            inline

            scalar_type &

            write_element_idx( int i, my_stuff::vec & m )

                {

                BOOST_QVM_ASSERT(i>=0);

                BOOST_QVM_ASSERT(i<dim);

                return m.a[i];

                }

            static

            inline

            scalar_type

            read_element_idx( int i, my_stuff::vec const & m )

                {

                BOOST_QVM_ASSERT(i>=0);

                BOOST_QVM_ASSERT(i<dim);

                return m.a[i];

                }

            };

        template <>

        struct

        quat_traits<my_stuff::quat>

            {

            typedef float scalar_type;

            template <int I>

            static

            scalar_type &

            write_element( my_stuff::quat & m )

                {

                BOOST_QVM_STATIC_ASSERT(I>=0);

                BOOST_QVM_STATIC_ASSERT(I<4);

                return m.a[I];

                }

            template <int I>

            static

            scalar_type

            read_element( my_stuff::quat const & m )

                {

                BOOST_QVM_STATIC_ASSERT(I>=0);

                BOOST_QVM_STATIC_ASSERT(I<4);

                return m.a[I];

                }

            };

        }

    }

namespace

my_stuff

    {

    mat &

    operator/=( mat & x, float y )

        {

        return boost::qvm::operator/=(x,y);

        }

    vec &

    operator/=( vec & x, float y )

        {

        return boost::qvm::operator/=(x,y);

        }

    quat &

    operator/=( quat & x, float y )

        {

        return boost::qvm::operator/=(x,y);

        }

    mat &

    operator*=( mat & x, float y )

        {

        return boost::qvm::operator*=(x,y);

        }

    vec &

    operator*=( vec & x, float y )

        {

        return boost::qvm::operator*=(x,y);

        }

    quat &

    operator*=( quat & x, float y )

        {

        return boost::qvm::operator*=(x,y);

        }

    mat

    operator/( mat const & x, float y )

        {

        return boost::qvm::operator/(x,y);

        }

    vec

    operator/( vec const & x, float y )

        {

        return boost::qvm::operator/(x,y);

        }

    quat

    operator/( quat const & x, float y )

        {

        return boost::qvm::operator/(x,y);

        }

    mat

    operator*( mat const & x, float y )

        {

        return boost::qvm::operator*(x,y);

        }

    vec

    operator*( vec const & x, float y )

        {

        return boost::qvm::operator*(x,y);

        }

    quat

    operator*( quat const & x, float y )

        {

        return boost::qvm::operator*(x,y);

        }

    mat &

    operator*=( mat & x, mat const & y )

        {

        return boost::qvm::operator*=(x,y);

        }

    mat

    operator*=( mat const & x, mat const & y )

        {

        return boost::qvm::operator*(x,y);

        }

    vec

    operator*( mat const & x, vec const & y )

        {

        return boost::qvm::operator*(x,y);

        }

    vec

    operator*( quat const & x, vec const & y )

        {

        return boost::qvm::operator*(x,y);

        }

    vec

    operator*( vec const & x, mat const & y )

        {

        return boost::qvm::operator*(x,y);

        }

    bool

    operator==( mat const & x, mat const & y )

        {

        return boost::qvm::operator==(x,y);

        }

    bool

    operator!=( mat const & x, mat const & y )

        {

        return boost::qvm::operator!=(x,y);

        }

    bool

    operator==( vec const & x, vec const & y )

        {

        return boost::qvm::operator==(x,y);

        }

    bool

    operator!=( vec const & x, vec const & y )

        {

        return boost::qvm::operator!=(x,y);

        }

    bool

    operator==( quat const & x, quat const & y )

        {

        return boost::qvm::operator==(x,y);

        }

    bool

    operator!=( quat const & x, quat const & y )

        {

        return boost::qvm::operator!=(x,y);

        }

    }

int

main()

    {

    using namespace boost::qvm::sfinae;

    using namespace my_stuff;

    typedef boost::qvm::mat<double,3,3> mat2;

    typedef boost::qvm::vec<double,3> vec2;

    typedef boost::qvm::quat<double> quat2;

    mat ma1, mb1; set_zero(ma1); set_zero(mb1);

    vec va1, vb1; set_zero(va1); set_zero(vb1);

    quat qa1, qb1; set_zero(qa1); set_zero(qb1);

    mat2 ma2, mb2; set_zero(ma2); set_zero(mb2);

    vec2 va2, vb2; set_zero(va2); set_zero(vb2);

    quat2 qa2, qb2; set_zero(qa2); set_zero(qb2);

    set_zero(ma1);

    set_zero(mb1);

    set_zero(va1);

    set_zero(vb1);

    set_zero(qa1);

    set_zero(qb1);

    set_zero(ma2);

    set_zero(mb2);

    set_zero(va2);

    set_zero(vb2);

    set_zero(qa2);

    set_zero(qb2);

    ma1/=2;

    va1/=2;

    qa1/=2;

    ma2/=2;

    va2/=2;

    qa2/=2;

    ma1*=2;

    va1*=2;

    qa1*=2;

    ma2*=2;

    va2*=2;

    qa2*=2;

    mb1=ma1/2;

    vb1=va1/2;

    qb1=qb1/2;

    mb2=convert_to<mat2>(ma1/2);

    vb2=convert_to<vec2>(va1/2);

    qb2=convert_to<quat2>(qa1/2);

    mb1=scalar_cast<float>(ma2/2);

    vb1=scalar_cast<float>(va2/2);

    qb1=scalar_cast<float>(qa2/2);

    mb1=ma1*2;

    vb1=va1*2;

    qb1=qa1*2;

    mb2=convert_to<mat2>(ma1*2);

    vb2=convert_to<vec2>(va1*2);

    qb2=convert_to<quat2>(qa1*2);

    mb1=scalar_cast<float>(ma2*2);

    vb1=scalar_cast<float>(va2*2);

    qb1=scalar_cast<float>(qa2*2);

    ma1*=mb1;

    ma1*=scalar_cast<float>(ma2);

    ma2*=ma1;

    va1=ma1*va1;

    va1=qa1*va1;

    va1=scalar_cast<float>(ma2*va1);

    va1=scalar_cast<float>(ma1*va2);

    va1=scalar_cast<float>(ma2*va2);

    va1=scalar_cast<float>(qa2*va1);

    va1=scalar_cast<float>(qa1*va2);

    va1=scalar_cast<float>(qa2*va2);

    va2=convert_to<vec2>(ma1*va1);

    va2=convert_to<vec2>(qa1*va1);

    va2=ma2*va1;

    va2=ma1*va2;

    va2=ma2*va2;

    va2=qa2*va1;

    va2=qa1*va2;

    va2=qa2*va2;

    va1=va1*ma1;

    va1=scalar_cast<float>(va1*ma2);

    va1=scalar_cast<float>(va2*ma1);

    va1=scalar_cast<float>(va2*ma2);

    va2=convert_to<vec2>(va1*ma1);

    va2=va1*ma2;

    va2=va2*ma1;

    va2=va2*ma2;

    (void) (ma1==mb1);

    (void) (ma1==ma2);

    (void) (ma2==ma1);

    (void) (ma1!=mb1);

    (void) (ma1!=ma2);

    (void) (ma2!=ma1);

    (void) (va1==vb1);

    (void) (va1==va2);

    (void) (va2==va1);

    (void) (va1!=vb1);

    (void) (va1!=va2);

    (void) (va2!=va1);

    (void) (qa1==qb1);

    (void) (qa1==qa2);

    (void) (qa2==qa1);

    (void) (qa1!=qb1);

    (void) (qa1!=qa2);

    (void) (qa2!=qa1);

    return 0;

    }