// boost\math\distributions\non_central_beta.hpp

// Copyright John Maddock 2008.

// 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_SPECIAL_NON_CENTRAL_BETA_HPP
#define BOOST_MATH_SPECIAL_NON_CENTRAL_BETA_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/beta.hpp> // for incomplete gamma. gamma_q
#include <boost/math/distributions/complement.hpp> // complements
#include <boost/math/distributions/beta.hpp> // central distribution
#include <boost/math/distributions/detail/generic_mode.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp> // error checks
#include <boost/math/special_functions/fpclassify.hpp> // isnan.
#include <boost/math/tools/roots.hpp> // for root finding.
#include <boost/math/tools/series.hpp>

namespace boost
{
   namespace math
   {

      template <class RealType, class Policy>
      class non_central_beta_distribution;

      namespace detail{

         template <class T, class Policy>
         T non_central_beta_p(T a, T b, T lam, T x, T y, const Policy& pol, T init_val = 0)
         {
            BOOST_MATH_STD_USING
               using namespace boost::math;
            //
            // Variables come first:
            //
            boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
            T errtol = boost::math::policies::get_epsilon<T, Policy>();
            T l2 = lam / 2;
            //
            // k is the starting point for iteration, and is the
            // maximum of the poisson weighting term,
            // note that unlike other similar code, we do not set
            // k to zero, when l2 is small, as forward iteration
            // is unstable:
            //
            int k = itrunc(l2);
            if(k == 0)
               k = 1;
            T pois;
            if(k == 0)
            {
               // Starting Poisson weight:
               pois = exp(-l2);
            }
            else
            {
               // Starting Poisson weight:
               pois = gamma_p_derivative(T(k+1), l2, pol);
            }
            if(pois == 0)
               return init_val;
            // recurance term:
            T xterm;
            // Starting beta term:
            T beta = x < y
               ? detail::ibeta_imp(T(a + k), b, x, pol, false, true, &xterm)
               : detail::ibeta_imp(b, T(a + k), y, pol, true, true, &xterm);

            xterm *= y / (a + b + k - 1);
            T poisf(pois), betaf(beta), xtermf(xterm);
            T sum = init_val;

            if((beta == 0) && (xterm == 0))
               return init_val;

            //
            // Backwards recursion first, this is the stable
            // direction for recursion:
            //
            T last_term = 0;
            boost::uintmax_t count = k;
            for(int i = k; i >= 0; --i)
            {
               T term = beta * pois;
               sum += term;
               if(((fabs(term/sum) < errtol) && (last_term >= term)) || (term == 0))
               {
                  count = k - i;
                  break;
               }
               pois *= i / l2;
               beta += xterm;
               xterm *= (a + i - 1) / (x * (a + b + i - 2));
               last_term = term;
            }
            for(int i = k + 1; ; ++i)
            {
               poisf *= l2 / i;
               xtermf *= (x * (a + b + i - 2)) / (a + i - 1);
               betaf -= xtermf;

               T term = poisf * betaf;
               sum += term;
               if((fabs(term/sum) < errtol) || (term == 0))
               {
                  break;
               }
               if(static_cast<boost::uintmax_t>(count + i - k) > max_iter)
               {
                  return policies::raise_evaluation_error(
                     "cdf(non_central_beta_distribution<%1%>, %1%)",
                     "Series did not converge, closest value was %1%", sum, pol);
               }
            }
            return sum;
         }

         template <class T, class Policy>
         T non_central_beta_q(T a, T b, T lam, T x, T y, const Policy& pol, T init_val = 0)
         {
            BOOST_MATH_STD_USING
               using namespace boost::math;
            //
            // Variables come first:
            //
            boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
            T errtol = boost::math::policies::get_epsilon<T, Policy>();
            T l2 = lam / 2;
            //
            // k is the starting point for iteration, and is the
            // maximum of the poisson weighting term:
            //
            int k = itrunc(l2);
            T pois;
            if(k <= 30)
            {
               //
               // Might as well start at 0 since we'll likely have this number of terms anyway:
               //
               if(a + b > 1)
                  k = 0;
               else if(k == 0)
                  k = 1;
            }
            if(k == 0)
            {
               // Starting Poisson weight:
               pois = exp(-l2);
            }
            else
            {
               // Starting Poisson weight:
               pois = gamma_p_derivative(T(k+1), l2, pol);
            }
            if(pois == 0)
               return init_val;
            // recurance term:
            T xterm;
            // Starting beta term:
            T beta = x < y
               ? detail::ibeta_imp(T(a + k), b, x, pol, true, true, &xterm)
               : detail::ibeta_imp(b, T(a + k), y, pol, false, true, &xterm);

            xterm *= y / (a + b + k - 1);
            T poisf(pois), betaf(beta), xtermf(xterm);
            T sum = init_val;
            if((beta == 0) && (xterm == 0))
               return init_val;
            //
            // Forwards recursion first, this is the stable
            // direction for recursion, and the location
            // of the bulk of the sum:
            //
            T last_term = 0;
            boost::uintmax_t count = 0;
            for(int i = k + 1; ; ++i)
            {
               poisf *= l2 / i;
               xtermf *= (x * (a + b + i - 2)) / (a + i - 1);
               betaf += xtermf;

               T term = poisf * betaf;
               sum += term;
               if((fabs(term/sum) < errtol) && (last_term >= term))
               {
                  count = i - k;
                  break;
               }
               if(static_cast<boost::uintmax_t>(i - k) > max_iter)
               {
                  return policies::raise_evaluation_error(
                     "cdf(non_central_beta_distribution<%1%>, %1%)",
                     "Series did not converge, closest value was %1%", sum, pol);
               }
               last_term = term;
            }
            for(int i = k; i >= 0; --i)
            {
               T term = beta * pois;
               sum += term;
               if(fabs(term/sum) < errtol)
               {
                  break;
               }
               if(static_cast<boost::uintmax_t>(count + k - i) > max_iter)
               {
                  return policies::raise_evaluation_error(
                     "cdf(non_central_beta_distribution<%1%>, %1%)",
                     "Series did not converge, closest value was %1%", sum, pol);
               }
               pois *= i / l2;
               beta -= xterm;
               xterm *= (a + i - 1) / (x * (a + b + i - 2));
            }
            return sum;
         }

         template <class RealType, class Policy>
         inline RealType non_central_beta_cdf(RealType x, RealType y, RealType a, RealType b, RealType l, bool invert, const Policy&)
         {
            typedef typename policies::evaluation<RealType, Policy>::type value_type;
            typedef typename policies::normalise<
               Policy,
               policies::promote_float<false>,
               policies::promote_double<false>,
               policies::discrete_quantile<>,
               policies::assert_undefined<> >::type forwarding_policy;

            BOOST_MATH_STD_USING

            if(x == 0)
               return invert ? 1.0f : 0.0f;
            if(y == 0)
               return invert ? 0.0f : 1.0f;
            value_type result;
            value_type c = a + b + l / 2;
            value_type cross = 1 - (b / c) * (1 + l / (2 * c * c));
            if(l == 0)
               result = cdf(boost::math::beta_distribution<RealType, Policy>(a, b), x);
            else if(x > cross)
            {
               // Complement is the smaller of the two:
               result = detail::non_central_beta_q(
                  static_cast<value_type>(a),
                  static_cast<value_type>(b),
                  static_cast<value_type>(l),
                  static_cast<value_type>(x),
                  static_cast<value_type>(y),
                  forwarding_policy(),
                  static_cast<value_type>(invert ? 0 : -1));
               invert = !invert;
            }
            else
            {
               result = detail::non_central_beta_p(
                  static_cast<value_type>(a),
                  static_cast<value_type>(b),
                  static_cast<value_type>(l),
                  static_cast<value_type>(x),
                  static_cast<value_type>(y),
                  forwarding_policy(),
                  static_cast<value_type>(invert ? -1 : 0));
            }
            if(invert)
               result = -result;
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               result,
               "boost::math::non_central_beta_cdf<%1%>(%1%, %1%, %1%)");
         }

         template <class T, class Policy>
         struct nc_beta_quantile_functor
         {
            nc_beta_quantile_functor(const non_central_beta_distribution<T,Policy>& d, T t, bool c)
               : dist(d), target(t), comp(c) {}

            T operator()(const T& x)
            {
               return comp ?
                  T(target - cdf(complement(dist, x)))
                  : T(cdf(dist, x) - target);
            }

         private:
            non_central_beta_distribution<T,Policy> dist;
            T target;
            bool comp;
         };

         //
         // This is more or less a copy of bracket_and_solve_root, but
         // modified to search only the interval [0,1] using similar
         // heuristics.
         //
         template <class F, class T, class Tol, class Policy>
         std::pair<T, T> bracket_and_solve_root_01(F f, const T& guess, T factor, bool rising, Tol tol, boost::uintmax_t& max_iter, const Policy& pol)
         {
            BOOST_MATH_STD_USING
               static const char* function = "boost::math::tools::bracket_and_solve_root_01<%1%>";
            //
            // Set up inital brackets:
            //
            T a = guess;
            T b = a;
            T fa = f(a);
            T fb = fa;
            //
            // Set up invocation count:
            //
            boost::uintmax_t count = max_iter - 1;

            if((fa < 0) == (guess < 0 ? !rising : rising))
            {
               //
               // Zero is to the right of b, so walk upwards
               // until we find it:
               //
               while((boost::math::sign)(fb) == (boost::math::sign)(fa))
               {
                  if(count == 0)
                  {
                     b = policies::raise_evaluation_error(function, "Unable to bracket root, last nearest value was %1%", b, pol);
                     return std::make_pair(a, b);
                  }
                  //
                  // Heuristic: every 20 iterations we double the growth factor in case the
                  // initial guess was *really* bad !
                  //
                  if((max_iter - count) % 20 == 0)
                     factor *= 2;
                  //
                  // Now go ahead and move are guess by "factor",
                  // we do this by reducing 1-guess by factor:
                  //
                  a = b;
                  fa = fb;
                  b = 1 - ((1 - b) / factor);
                  fb = f(b);
                  --count;
                  BOOST_MATH_INSTRUMENT_CODE("a = " << a << " b = " << b << " fa = " << fa << " fb = " << fb << " count = " << count);
               }
            }
            else
            {
               //
               // Zero is to the left of a, so walk downwards
               // until we find it:
               //
               while((boost::math::sign)(fb) == (boost::math::sign)(fa))
               {
                  if(fabs(a) < tools::min_value<T>())
                  {
                     // Escape route just in case the answer is zero!
                     max_iter -= count;
                     max_iter += 1;
                     return a > 0 ? std::make_pair(T(0), T(a)) : std::make_pair(T(a), T(0));
                  }
                  if(count == 0)
                  {
                     a = policies::raise_evaluation_error(function, "Unable to bracket root, last nearest value was %1%", a, pol);
                     return std::make_pair(a, b);
                  }
                  //
                  // Heuristic: every 20 iterations we double the growth factor in case the
                  // initial guess was *really* bad !
                  //
                  if((max_iter - count) % 20 == 0)
                     factor *= 2;
                  //
                  // Now go ahead and move are guess by "factor":
                  //
                  b = a;
                  fb = fa;
                  a /= factor;
                  fa = f(a);
                  --count;
                  BOOST_MATH_INSTRUMENT_CODE("a = " << a << " b = " << b << " fa = " << fa << " fb = " << fb << " count = " << count);
               }
            }
            max_iter -= count;
            max_iter += 1;
            std::pair<T, T> r = toms748_solve(
               f,
               (a < 0 ? b : a),
               (a < 0 ? a : b),
               (a < 0 ? fb : fa),
               (a < 0 ? fa : fb),
               tol,
               count,
               pol);
            max_iter += count;
            BOOST_MATH_INSTRUMENT_CODE("max_iter = " << max_iter << " count = " << count);
            return r;
         }

         template <class RealType, class Policy>
         RealType nc_beta_quantile(const non_central_beta_distribution<RealType, Policy>& dist, const RealType& p, bool comp)
         {
            static const char* function = "quantile(non_central_beta_distribution<%1%>, %1%)";
            typedef typename policies::evaluation<RealType, Policy>::type value_type;
            typedef typename policies::normalise<
               Policy,
               policies::promote_float<false>,
               policies::promote_double<false>,
               policies::discrete_quantile<>,
               policies::assert_undefined<> >::type forwarding_policy;

            value_type a = dist.alpha();
            value_type b = dist.beta();
            value_type l = dist.non_centrality();
            value_type r;
            if(!beta_detail::check_alpha(
               function,
               a, &r, Policy())
               ||
            !beta_detail::check_beta(
               function,
               b, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy())
               ||
            !detail::check_probability(
               function,
               static_cast<value_type>(p),
               &r,
               Policy()))
                  return (RealType)r;
            //
            // Special cases first:
            //
            if(p == 0)
               return comp
               ? 1.0f
               : 0.0f;
            if(p == 1)
               return !comp
               ? 1.0f
               : 0.0f;

            value_type c = a + b + l / 2;
            value_type mean = 1 - (b / c) * (1 + l / (2 * c * c));
            /*
            //
            // Calculate a normal approximation to the quantile,
            // uses mean and variance approximations from:
            // Algorithm AS 310:
            // Computing the Non-Central Beta Distribution Function
            // R. Chattamvelli; R. Shanmugam
            // Applied Statistics, Vol. 46, No. 1. (1997), pp. 146-156.
            //
            // Unfortunately, when this is wrong it tends to be *very*
            // wrong, so it's disabled for now, even though it often
            // gets the initial guess quite close.  Probably we could
            // do much better by factoring in the skewness if only
            // we could calculate it....
            //
            value_type delta = l / 2;
            value_type delta2 = delta * delta;
            value_type delta3 = delta * delta2;
            value_type delta4 = delta2 * delta2;
            value_type G = c * (c + 1) + delta;
            value_type alpha = a + b;
            value_type alpha2 = alpha * alpha;
            value_type eta = (2 * alpha + 1) * (2 * alpha + 1) + 1;
            value_type H = 3 * alpha2 + 5 * alpha + 2;
            value_type F = alpha2 * (alpha + 1) + H * delta
               + (2 * alpha + 4) * delta2 + delta3;
            value_type P = (3 * alpha + 1) * (9 * alpha + 17)
               + 2 * alpha * (3 * alpha + 2) * (3 * alpha + 4) + 15;
            value_type Q = 54 * alpha2 + 162 * alpha + 130;
            value_type R = 6 * (6 * alpha + 11);
            value_type D = delta
               * (H * H + 2 * P * delta + Q * delta2 + R * delta3 + 9 * delta4);
            value_type variance = (b / G)
               * (1 + delta * (l * l + 3 * l + eta) / (G * G))
               - (b * b / F) * (1 + D / (F * F));
            value_type sd = sqrt(variance);

            value_type guess = comp
               ? quantile(complement(normal_distribution<RealType, Policy>(static_cast<RealType>(mean), static_cast<RealType>(sd)), p))
               : quantile(normal_distribution<RealType, Policy>(static_cast<RealType>(mean), static_cast<RealType>(sd)), p);

            if(guess >= 1)
               guess = mean;
            if(guess <= tools::min_value<value_type>())
               guess = mean;
            */
            value_type guess = mean;
            detail::nc_beta_quantile_functor<value_type, Policy>
               f(non_central_beta_distribution<value_type, Policy>(a, b, l), p, comp);
            tools::eps_tolerance<value_type> tol(policies::digits<RealType, Policy>());
            boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();

            std::pair<value_type, value_type> ir
               = bracket_and_solve_root_01(
                  f, guess, value_type(2.5), true, tol,
                  max_iter, Policy());
            value_type result = ir.first + (ir.second - ir.first) / 2;

            if(max_iter >= policies::get_max_root_iterations<Policy>())
            {
               return policies::raise_evaluation_error<RealType>(function, "Unable to locate solution in a reasonable time:"
                  " either there is no answer to quantile of the non central beta distribution"
                  " or the answer is infinite.  Current best guess is %1%",
                  policies::checked_narrowing_cast<RealType, forwarding_policy>(
                     result,
                     function), Policy());
            }
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               result,
               function);
         }

         template <class T, class Policy>
         T non_central_beta_pdf(T a, T b, T lam, T x, T y, const Policy& pol)
         {
            BOOST_MATH_STD_USING
               using namespace boost::math;
            //
            // Variables come first:
            //
            boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
            T errtol = boost::math::policies::get_epsilon<T, Policy>();
            T l2 = lam / 2;
            //
            // k is the starting point for iteration, and is the
            // maximum of the poisson weighting term:
            //
            int k = itrunc(l2);
            // Starting Poisson weight:
            T pois = gamma_p_derivative(T(k+1), l2, pol);
            // Starting beta term:
            T beta = x < y ?
               ibeta_derivative(a + k, b, x, pol)
               : ibeta_derivative(b, a + k, y, pol);
            T sum = 0;
            T poisf(pois);
            T betaf(beta);

            //
            // Stable backwards recursion first:
            //
            boost::uintmax_t count = k;
            for(int i = k; i >= 0; --i)
            {
               T term = beta * pois;
               sum += term;
               if((fabs(term/sum) < errtol) || (term == 0))
               {
                  count = k - i;
                  break;
               }
               pois *= i / l2;
               beta *= (a + i - 1) / (x * (a + i + b - 1));
            }
            for(int i = k + 1; ; ++i)
            {
               poisf *= l2 / i;
               betaf *= x * (a + b + i - 1) / (a + i - 1);

               T term = poisf * betaf;
               sum += term;
               if((fabs(term/sum) < errtol) || (term == 0))
               {
                  break;
               }
               if(static_cast<boost::uintmax_t>(count + i - k) > max_iter)
               {
                  return policies::raise_evaluation_error(
                     "pdf(non_central_beta_distribution<%1%>, %1%)",
                     "Series did not converge, closest value was %1%", sum, pol);
               }
            }
            return sum;
         }

         template <class RealType, class Policy>
         RealType nc_beta_pdf(const non_central_beta_distribution<RealType, Policy>& dist, const RealType& x)
         {
            BOOST_MATH_STD_USING
            static const char* function = "pdf(non_central_beta_distribution<%1%>, %1%)";
            typedef typename policies::evaluation<RealType, Policy>::type value_type;
            typedef typename policies::normalise<
               Policy,
               policies::promote_float<false>,
               policies::promote_double<false>,
               policies::discrete_quantile<>,
               policies::assert_undefined<> >::type forwarding_policy;

            value_type a = dist.alpha();
            value_type b = dist.beta();
            value_type l = dist.non_centrality();
            value_type r;
            if(!beta_detail::check_alpha(
               function,
               a, &r, Policy())
               ||
            !beta_detail::check_beta(
               function,
               b, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy())
               ||
            !beta_detail::check_x(
               function,
               static_cast<value_type>(x),
               &r,
               Policy()))
                  return (RealType)r;

            if(l == 0)
               return pdf(boost::math::beta_distribution<RealType, Policy>(dist.alpha(), dist.beta()), x);
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               non_central_beta_pdf(a, b, l, static_cast<value_type>(x), value_type(1 - static_cast<value_type>(x)), forwarding_policy()),
               "function");
         }

         template <class T>
         struct hypergeometric_2F2_sum
         {
            typedef T result_type;
            hypergeometric_2F2_sum(T a1_, T a2_, T b1_, T b2_, T z_) : a1(a1_), a2(a2_), b1(b1_), b2(b2_), z(z_), term(1), k(0) {}
            T operator()()
            {
               T result = term;
               term *= a1 * a2 / (b1 * b2);
               a1 += 1;
               a2 += 1;
               b1 += 1;
               b2 += 1;
               k += 1;
               term /= k;
               term *= z;
               return result;
            }
            T a1, a2, b1, b2, z, term, k;
         };

         template <class T, class Policy>
         T hypergeometric_2F2(T a1, T a2, T b1, T b2, T z, const Policy& pol)
         {
            typedef typename policies::evaluation<T, Policy>::type value_type;

            const char* function = "boost::math::detail::hypergeometric_2F2<%1%>(%1%,%1%,%1%,%1%,%1%)";

            hypergeometric_2F2_sum<value_type> s(a1, a2, b1, b2, z);
            boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
            value_type zero = 0;
            value_type result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<value_type, Policy>(), max_iter, zero);
#else
            value_type result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<value_type, Policy>(), max_iter);
#endif
            policies::check_series_iterations<T>(function, max_iter, pol);
            return policies::checked_narrowing_cast<T, Policy>(result, function);
         }

      } // namespace detail

      template <class RealType = double, class Policy = policies::policy<> >
      class non_central_beta_distribution
      {
      public:
         typedef RealType value_type;
         typedef Policy policy_type;

         non_central_beta_distribution(RealType a_, RealType b_, RealType lambda) : a(a_), b(b_), ncp(lambda)
         {
            const char* function = "boost::math::non_central_beta_distribution<%1%>::non_central_beta_distribution(%1%,%1%)";
            RealType r;
            beta_detail::check_alpha(
               function,
               a, &r, Policy());
            beta_detail::check_beta(
               function,
               b, &r, Policy());
            detail::check_non_centrality(
               function,
               lambda,
               &r,
               Policy());
         } // non_central_beta_distribution constructor.

         RealType alpha() const
         { // Private data getter function.
            return a;
         }
         RealType beta() const
         { // Private data getter function.
            return b;
         }
         RealType non_centrality() const
         { // Private data getter function.
            return ncp;
         }
      private:
         // Data member, initialized by constructor.
         RealType a;   // alpha.
         RealType b;   // beta.
         RealType ncp; // non-centrality parameter
      }; // template <class RealType, class Policy> class non_central_beta_distribution

      typedef non_central_beta_distribution<double> non_central_beta; // Reserved name of type double.

      // Non-member functions to give properties of the distribution.

      template <class RealType, class Policy>
      inline const std::pair<RealType, RealType> range(const non_central_beta_distribution<RealType, Policy>& /* dist */)
      { // Range of permissible values for random variable k.
         using boost::math::tools::max_value;
         return std::pair<RealType, RealType>(static_cast<RealType>(0), static_cast<RealType>(1));
      }

      template <class RealType, class Policy>
      inline const std::pair<RealType, RealType> support(const non_central_beta_distribution<RealType, Policy>& /* dist */)
      { // Range of supported values for random variable k.
         // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
         using boost::math::tools::max_value;
         return std::pair<RealType, RealType>(static_cast<RealType>(0), static_cast<RealType>(1));
      }

      template <class RealType, class Policy>
      inline RealType mode(const non_central_beta_distribution<RealType, Policy>& dist)
      { // mode.
         static const char* function = "mode(non_central_beta_distribution<%1%> const&)";

         RealType a = dist.alpha();
         RealType b = dist.beta();
         RealType l = dist.non_centrality();
         RealType r;
         if(!beta_detail::check_alpha(
               function,
               a, &r, Policy())
               ||
            !beta_detail::check_beta(
               function,
               b, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy()))
                  return (RealType)r;
         RealType c = a + b + l / 2;
         RealType mean = 1 - (b / c) * (1 + l / (2 * c * c));
         return detail::generic_find_mode_01(
            dist,
            mean,
            function);
      }

      //
      // We don't have the necessary information to implement
      // these at present.  These are just disabled for now,
      // prototypes retained so we can fill in the blanks
      // later:
      //
      template <class RealType, class Policy>
      inline RealType mean(const non_central_beta_distribution<RealType, Policy>& dist)
      {
         BOOST_MATH_STD_USING
         RealType a = dist.alpha();
         RealType b = dist.beta();
         RealType d = dist.non_centrality();
         RealType apb = a + b;
         return exp(-d / 2) * a * detail::hypergeometric_2F2<RealType, Policy>(1 + a, apb, a, 1 + apb, d / 2, Policy()) / apb;
      } // mean

      template <class RealType, class Policy>
      inline RealType variance(const non_central_beta_distribution<RealType, Policy>& dist)
      {
         //
         // Relative error of this function may be arbitarily large... absolute
         // error will be small however... that's the best we can do for now.
         //
         BOOST_MATH_STD_USING
         RealType a = dist.alpha();
         RealType b = dist.beta();
         RealType d = dist.non_centrality();
         RealType apb = a + b;
         RealType result = detail::hypergeometric_2F2(RealType(1 + a), apb, a, RealType(1 + apb), RealType(d / 2), Policy());
         result *= result * -exp(-d) * a * a / (apb * apb);
         result += exp(-d / 2) * a * (1 + a) * detail::hypergeometric_2F2(RealType(2 + a), apb, a, RealType(2 + apb), RealType(d / 2), Policy()) / (apb * (1 + apb));
         return result;
      }

      // RealType standard_deviation(const non_central_beta_distribution<RealType, Policy>& dist)
      // standard_deviation provided by derived accessors.
      template <class RealType, class Policy>
      inline RealType skewness(const non_central_beta_distribution<RealType, Policy>& /*dist*/)
      { // skewness = sqrt(l).
         const char* function = "boost::math::non_central_beta_distribution<%1%>::skewness()";
         typedef typename Policy::assert_undefined_type assert_type;
         BOOST_STATIC_ASSERT(assert_type::value == 0);

         return policies::raise_evaluation_error<RealType>(
            function,
            "This function is not yet implemented, the only sensible result is %1%.",
            std::numeric_limits<RealType>::quiet_NaN(), Policy()); // infinity?
      }

      template <class RealType, class Policy>
      inline RealType kurtosis_excess(const non_central_beta_distribution<RealType, Policy>& /*dist*/)
      {
         const char* function = "boost::math::non_central_beta_distribution<%1%>::kurtosis_excess()";
         typedef typename Policy::assert_undefined_type assert_type;
         BOOST_STATIC_ASSERT(assert_type::value == 0);

         return policies::raise_evaluation_error<RealType>(
            function,
            "This function is not yet implemented, the only sensible result is %1%.",
            std::numeric_limits<RealType>::quiet_NaN(), Policy()); // infinity?
      } // kurtosis_excess

      template <class RealType, class Policy>
      inline RealType kurtosis(const non_central_beta_distribution<RealType, Policy>& dist)
      {
         return kurtosis_excess(dist) + 3;
      }

      template <class RealType, class Policy>
      inline RealType pdf(const non_central_beta_distribution<RealType, Policy>& dist, const RealType& x)
      { // Probability Density/Mass Function.
         return detail::nc_beta_pdf(dist, x);
      } // pdf

      template <class RealType, class Policy>
      RealType cdf(const non_central_beta_distribution<RealType, Policy>& dist, const RealType& x)
      {
         const char* function = "boost::math::non_central_beta_distribution<%1%>::cdf(%1%)";
            RealType a = dist.alpha();
            RealType b = dist.beta();
            RealType l = dist.non_centrality();
            RealType r;
            if(!beta_detail::check_alpha(
               function,
               a, &r, Policy())
               ||
            !beta_detail::check_beta(
               function,
               b, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy())
               ||
            !beta_detail::check_x(
               function,
               x,
               &r,
               Policy()))
                  return (RealType)r;

         if(l == 0)
            return cdf(beta_distribution<RealType, Policy>(a, b), x);

         return detail::non_central_beta_cdf(x, RealType(1 - x), a, b, l, false, Policy());
      } // cdf

      template <class RealType, class Policy>
      RealType cdf(const complemented2_type<non_central_beta_distribution<RealType, Policy>, RealType>& c)
      { // Complemented Cumulative Distribution Function
         const char* function = "boost::math::non_central_beta_distribution<%1%>::cdf(%1%)";
         non_central_beta_distribution<RealType, Policy> const& dist = c.dist;
            RealType a = dist.alpha();
            RealType b = dist.beta();
            RealType l = dist.non_centrality();
            RealType x = c.param;
            RealType r;
            if(!beta_detail::check_alpha(
               function,
               a, &r, Policy())
               ||
            !beta_detail::check_beta(
               function,
               b, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy())
               ||
            !beta_detail::check_x(
               function,
               x,
               &r,
               Policy()))
                  return (RealType)r;

         if(l == 0)
            return cdf(complement(beta_distribution<RealType, Policy>(a, b), x));

         return detail::non_central_beta_cdf(x, RealType(1 - x), a, b, l, true, Policy());
      } // ccdf

      template <class RealType, class Policy>
      inline RealType quantile(const non_central_beta_distribution<RealType, Policy>& dist, const RealType& p)
      { // Quantile (or Percent Point) function.
         return detail::nc_beta_quantile(dist, p, false);
      } // quantile

      template <class RealType, class Policy>
      inline RealType quantile(const complemented2_type<non_central_beta_distribution<RealType, Policy>, RealType>& c)
      { // Quantile (or Percent Point) function.
         return detail::nc_beta_quantile(c.dist, c.param, true);
      } // quantile complement.

   } // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_MATH_SPECIAL_NON_CENTRAL_BETA_HPP


#pragma once

#include <boost/endian/conversion.hpp>
#include <boost/multiprecision/cpp_int.hpp>

#include <koinos/binary.hpp>

namespace koinos {

using int128_t  = boost::multiprecision::int128_t;
using uint128_t = boost::multiprecision::uint128_t;
using int256_t  = boost::multiprecision::int256_t;
using uint256_t = boost::multiprecision::uint256_t;

using int160_t = boost::multiprecision::number<
   boost::multiprecision::cpp_int_backend<
      160,
      160,
      boost::multiprecision::signed_magnitude,
      boost::multiprecision::unchecked, void
   >
>;

using uint160_t = boost::multiprecision::number<
   boost::multiprecision::cpp_int_backend<
      160,
      160,
      boost::multiprecision::unsigned_magnitude,
      boost::multiprecision::unchecked, void
   >
>;

template<>
void to_binary< int128_t >( std::ostream& s, const int128_t& v );

template<>
void from_binary< int128_t >( std::istream& s, int128_t& v );

template<>
void to_binary< uint128_t >( std::ostream& s, const uint128_t& v );

template<>
void from_binary< uint128_t >( std::istream& s, uint128_t& v );

template<>
void to_binary< int160_t >( std::ostream& s, const int160_t& v );

template<>
void from_binary< int160_t >( std::istream& s, int160_t& v );

template<>
void to_binary< uint160_t >( std::ostream& s, const uint160_t& v );

template<>
void from_binary< uint160_t >( std::istream& s, uint160_t& v );

template<>
void to_binary< int256_t >( std::ostream& s, const int256_t& v );

template<>
void from_binary< int256_t >( std::istream& s, int256_t& v );

template<>
void to_binary< uint256_t >( std::ostream& s, const uint256_t& v );

template<>
void from_binary< uint256_t >( std::istream& s, uint256_t& v );

} // koinos


//#include <fdeep/fdeep.hpp>

#include <stdarg.h>
#include <stdio.h>      /* vsnprintf */

#include "Enclave.h"
#include "Enclave_t.h"  /* print_string */

#include "sgx_trts.h"
#include "sgx_tcrypto.h"

//#include "sgxdnn_main.hpp"
#include "tensor_types.h"
//#include "Crypto.h"

#include <Eigen/Dense>
#include <algorithm>
#include "layer.h"
#include "layer/conv.h"
#include "layer/fully_connected.h"
#include "layer/ave_pooling.h"
#include "layer/max_pooling.h"
#include "layer/relu.h"
#include "layer/sigmoid.h"
#include "layer/softmax.h"
#include "loss.h"
#include "loss/mse_loss.h"
#include "loss/cross_entropy_loss.h"
#include "mnist.h"
#include "network.h"
#include "optimizer.h"
#include "optimizer/sgd.h"

/*
 * Invokes OCALL to display the enclave buffer to the terminal.
 */
void printf(const char *fmt, ...){
	char buf[BUFSIZ] = {'\0'};
	va_list ap;
	va_start(ap, fmt);
	vsnprintf(buf, BUFSIZ, fmt, ap);
	va_end(ap);
	ocall_print_string(buf);
}

/*
 * Invokes OCALL to start the clock.
 */
void start_clock() {
	ocall_start_clock();
}

/*
 * Invokes OCALL to end the clock.
 */
void end_clock(const char* str) {
	ocall_end_clock(str);
}

typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<float, Eigen::Dynamic, 1> Vector;
typedef Eigen::Array<float, 1, Eigen::Dynamic> RowVector;

class RandData {
public:
    Matrix train_data;
    Matrix train_labels;
    Matrix test_data;
    Matrix test_labels;

    RandData(int size) {
        train_data = Matrix::Random(28 * 28, size);
        train_labels = Matrix::Ones(1, size);
        test_data = Matrix::Random(28 * 28, size);
        test_labels = Matrix::Ones(1, size);
    }
};


#define BOOST_TEST_MODULE "test_directional_contact_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/local/DirectionalContactInteraction.hpp>
#include <mjolnir/forcefield/local/CosinePotential.hpp>
#include <mjolnir/forcefield/local/GoContactPotential.hpp>
#include <mjolnir/util/make_unique.hpp>
#include <mjolnir/math/constants.hpp>
#include <mjolnir/math/Matrix.hpp>

#include <tuple>
#include <random>

BOOST_AUTO_TEST_CASE(DirectionalContactInteraction_numerical_diff)
{
    mjolnir::LoggerManager::set_default_logger("test_directional_contact_interaction.log");
    using traits_type              = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type                = traits_type::real_type;
    using coord_type               = traits_type::coordinate_type;
    using boundary_type            = traits_type::boundary_type;
    using system_type              = mjolnir::System<traits_type>;
    using angle_potential_type     = mjolnir::CosinePotential<real_type>;
    using contact_potential_type   = mjolnir::GoContactPotential<real_type>;
    using directional_contact_type = mjolnir::DirectionalContactInteraction<
        traits_type, angle_potential_type, angle_potential_type, contact_potential_type>;

    constexpr real_type pi = mjolnir::math::constants<real_type>::pi();
    const     real_type k_angle(1e0);
    const     real_type k_contact(1e0);
    const     real_type angle1_native(pi);
    const     real_type angle2_native(pi);
    const     real_type contact_native(2.0);
    const auto angle1_potential    = angle_potential_type{k_angle, 1, angle1_native};
    const auto angle2_potential    = angle_potential_type{k_angle, 1, angle2_native};
    const auto contact_potential   = contact_potential_type{k_contact, contact_native};
    directional_contact_type interaction("none",
        {{std::make_tuple(std::array<std::size_t, 4>{{0,1,2,3}},
        angle1_potential, angle2_potential, contact_potential)
        }});

    system_type sys(4, boundary_type{});
    sys.mass(0) = 1.0;
    sys.mass(1) = 1.0;
    sys.mass(2) = 1.0;
    sys.mass(3) = 1.0;
    sys.position(0) = mjolnir::math::make_coordinate<coord_type>(0.0, 0.0, 0.0);
    sys.position(1) = mjolnir::math::make_coordinate<coord_type>(0.0, 0.0, 0.0);
    sys.position(2) = mjolnir::math::make_coordinate<coord_type>(0.0, 0.0, 0.0);
    sys.position(3) = mjolnir::math::make_coordinate<coord_type>(0.0, 0.0, 0.0);
    interaction.initialize(sys);

    const int angle_step_num           = 108;
    const std::size_t contact_step_num = 10;
    const real_type dtheta             = pi / real_type(angle_step_num);
    const real_type contact_cutoff     = contact_potential.cutoff();
    const real_type test_contact_range = 1.1 * contact_cutoff;
    const real_type dr                 = test_contact_range / real_type(contact_step_num);
    const real_type tol                = 1e-3;
    const real_type dx                 = 1e-6;
    const real_type ignore_tolerance   = 1e-12;
    std::mt19937 mt(123456789);
    std::uniform_real_distribution<real_type> uni(-1.0, 1.0);

    for(std::size_t i = 0; i < angle_step_num; ++i)
    {
        for(std::size_t j = 0; j < angle_step_num; ++j)
        {
            for(std::size_t k = 1; k <= contact_step_num; ++k)
            {
                const real_type theta1 = i * dtheta;
                const real_type theta2 = j * dtheta;
                if(std::sin(theta1) < 1e-2 || std::sin(theta2) < 1e-2)
                {
                    // In this case, numerical error become big and difficult to
                    // implement appropriate test. So skip.
                    continue;
                }

                const real_type r = k * dr;
                BOOST_TEST_MESSAGE("theta1 = " << theta1 << ", theta2 = " << pi + theta2
                                   << ", r = " << r);

                sys.position(0) = coord_type(std::cos(theta1), std::sin(theta1), 0e0);
                sys.position(1) = coord_type(0e0, 0e0, 0e0);
                sys.position(2) = coord_type(r, 0e0, 0e0);
                sys.position(3) = coord_type(r - std::cos(theta2), std::sin(theta2), 0e0);
                sys.velocity(0) = coord_type(0.0, 0.0, 0.0);
                sys.velocity(1) = coord_type(0.0, 0.0, 0.0);
                sys.velocity(2) = coord_type(0.0, 0.0, 0.0);
                sys.velocity(3) = coord_type(0.0, 0.0, 0.0);
                sys.force   (0) = coord_type(0.0, 0.0, 0.0);
                sys.force   (1) = coord_type(0.0, 0.0, 0.0);
                sys.force   (2) = coord_type(0.0, 0.0, 0.0);
                sys.force   (3) = coord_type(0.0, 0.0, 0.0);

                // rotate randomly system
                using matrix33_type = typename traits_type::matrix33_type;

                const auto rot_x = uni(mt) * pi;
                const auto rot_y = uni(mt) * pi;
                const auto rot_z = uni(mt) * pi;

                const matrix33_type rotm_x(1.0,             0.0,              0.0,
                                           0.0, std::cos(rot_x), -std::sin(rot_x),
                                           0.0, std::sin(rot_x),  std::cos(rot_x));
                const matrix33_type rotm_y(std::cos(rot_y), 0.0,  std::sin(rot_y),
                                           0.0,             1.0,              0.0,
                                          -std::sin(rot_y), 0.0,  std::cos(rot_y));
                const matrix33_type rotm_z(std::cos(rot_z), -std::sin(rot_z), 0.0,
                                           std::sin(rot_z),  std::cos(rot_z), 0.0,
                                           0.0,              0.0,             1.0);

                const matrix33_type rotm = rotm_x * rotm_y * rotm_z;
                for(std::size_t idx=0; idx<sys.size(); ++idx)
                {
                    sys.position(idx) = rotm * sys.position(idx);
                }

                const auto init = sys;
                interaction.update(sys);

                for(std::size_t idx=0; idx<4; ++idx)
                {
                    BOOST_TEST_MESSAGE("idx = " << idx);
                    {
                        sys = init;
                        interaction.calc_force(sys);
                        const auto total_f = sys.force(0) + sys.force(1) + sys.force(2) + sys.force(3);
                        BOOST_TEST_REQUIRE(mjolnir::math::X(total_f) == 0e0,
                                           boost::test_tools::tolerance(tol));
                        BOOST_TEST_REQUIRE(mjolnir::math::Y(total_f) == 0e0,
                                           boost::test_tools::tolerance(tol));
                        BOOST_TEST_REQUIRE(mjolnir::math::Z(total_f) == 0e0,
                                           boost::test_tools::tolerance(tol));
                    }
                    { // test for X coordinate
                        // reset positions
                        sys = init;

                        // calc F(x)
                        interaction.calc_force(sys);

                        // calcu U(x-dx)
                        mjolnir::math::X(sys.position(idx)) -= dx;
                        const auto E0 = interaction.calc_energy(sys);

                        // calc U(x+dx)
                        mjolnir::math::X(sys.position(idx)) += 2.0 * dx;
                        const auto E1 = interaction.calc_energy(sys);

                        // central difference
                        const auto dE = (E1 - E0) * 0.5;
                        if(std::abs(dE) < ignore_tolerance)
                        {
                            BOOST_TEST(dE == 0.0, boost::test_tools::tolerance(tol));
                        }
                        else
                        {
                            BOOST_TEST(-dE == dx * mjolnir::math::X(sys.force(idx)),
                                       boost::test_tools::tolerance(tol));
                        }
                    }
                    { // test for Y coordinate
                        // reset positions
                        sys = init;

                        // calc F(x)
                        interaction.calc_force(sys);

                        // calc U(x-dx)
                        mjolnir::math::Y(sys.position(idx)) -= dx;
                        const auto E0 = interaction.calc_energy(sys);

                        // calc U(x+dx)
                        mjolnir::math::Y(sys.position(idx)) += 2.0 * dx;
                        const auto E1 = interaction.calc_energy(sys);

                        //central difference
                        const auto dE = (E1 - E0) * 0.5;
                        if(std::abs(dE) < ignore_tolerance)
                        {
                            BOOST_TEST(dE == 0.0, boost::test_tools::tolerance(tol));
                        }
                        else
                        {
                            BOOST_TEST(-dE == dx * mjolnir::math::Y(sys.force(idx)),
                                       boost::test_tools::tolerance(tol));
                        }
                    }
                    { // test for Z coordinate
                        // reset positions
                        sys = init;

                        // calc F(x)
                        interaction.calc_force(sys);

                        // calc U(x-dx)
                        mjolnir::math::Z(sys.position(idx)) -= dx;
                        const auto E0 = interaction.calc_energy(sys);

                        // calc U(x+dx)
                        mjolnir::math::Z(sys.position(idx)) += 2.0 * dx;
                        const auto E1 = interaction.calc_energy(sys);

                        //central difference
                        const auto dE = (E1 - E0) * 0.5;

                        if(std::abs(dE) < ignore_tolerance)
                        {
                            BOOST_TEST(dE == 0.0, boost::test_tools::tolerance(tol));
                        }
                        else
                        {
                            BOOST_TEST(-dE == dx * mjolnir::math::Z(sys.force(idx)),
                                       boost::test_tools::tolerance(tol));
                        }
                    }
                }
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(DirectionalContactInteraction_calc_force_and_energy)
{
    mjolnir::LoggerManager::set_default_logger("test_directional_contact_interaction.log");
    using traits_type              = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type                = traits_type::real_type;
    using coord_type               = traits_type::coordinate_type;
    using boundary_type            = traits_type::boundary_type;
    using system_type              = mjolnir::System<traits_type>;
    using angle_potential_type     = mjolnir::CosinePotential<real_type>;
    using contact_potential_type   = mjolnir::GoContactPotential<real_type>;
    using directional_contact_type = mjolnir::DirectionalContactInteraction<
        traits_type, angle_potential_type, angle_potential_type, contact_potential_type>;

    constexpr real_type pi = mjolnir::math::constants<real_type>::pi();
    const     real_type k_angle(1e0);
    const     real_type k_contact(1e0);
    const     real_type angle1_native(pi);
    const     real_type angle2_native(pi);
    const     real_type contact_native(2.0);
    const auto angle1_potential    = angle_potential_type{k_angle, 1, angle1_native};
    const auto angle2_potential    = angle_potential_type{k_angle, 1, angle2_native};
    const auto contact_potential   = contact_potential_type{k_contact, contact_native};
    directional_contact_type interaction("none",
        {{std::make_tuple(std::array<std::size_t, 4>{{0,1,2,3}},
        angle1_potential, angle2_potential, contact_potential)
        }});

    system_type sys(4, boundary_type{});
    sys.mass(0) = 1.0;
    sys.mass(1) = 1.0;
    sys.mass(2) = 1.0;
    sys.mass(3) = 1.0;
    sys.position(0) = mjolnir::math::make_coordinate<coord_type>(0.0, 0.0, 0.0);
    sys.position(1) = mjolnir::math::make_coordinate<coord_type>(0.0, 0.0, 0.0);
    sys.position(2) = mjolnir::math::make_coordinate<coord_type>(0.0, 0.0, 0.0);
    sys.position(3) = mjolnir::math::make_coordinate<coord_type>(0.0, 0.0, 0.0);
    interaction.initialize(sys);

    const int angle_step_num           = 108;
    const std::size_t contact_step_num = 10;
    const real_type dtheta             = pi / real_type(angle_step_num);
    const real_type contact_cutoff     = contact_potential.cutoff();
    const real_type test_contact_range = 1.1 * contact_cutoff;
    const real_type dr                 = test_contact_range / real_type(contact_step_num);
    std::mt19937 mt(123456789);
    std::uniform_real_distribution<real_type> uni(-1.0, 1.0);

    for(std::size_t i = 0; i < angle_step_num; ++i)
    {
        for(std::size_t j = 0; j < angle_step_num; ++j)
        {
            for(std::size_t k = 1; k <= contact_step_num; ++k)
            {
                const real_type theta1 = i * dtheta;
                const real_type theta2 = j * dtheta;
                if(std::sin(theta1) < 1e-2 || std::sin(theta2) < 1e-2)
                {
                    // In this case, numerical error become big and difficult to
                    // implement appropriate test. So skip.
                    continue;
                }

                const real_type r = k * dr;
                BOOST_TEST_MESSAGE("theta1 = " << theta1 << ", theta2 = " << pi + theta2
                                   << ", r = " << r);

                sys.position(0) = coord_type(std::cos(theta1), std::sin(theta1), 0e0);
                sys.position(1) = coord_type(0e0, 0e0, 0e0);
                sys.position(2) = coord_type(r, 0e0, 0e0);
                sys.position(3) = coord_type(r - std::cos(theta2), std::sin(theta2), 0e0);
                sys.velocity(0) = coord_type(0.0, 0.0, 0.0);
                sys.velocity(1) = coord_type(0.0, 0.0, 0.0);
                sys.velocity(2) = coord_type(0.0, 0.0, 0.0);
                sys.velocity(3) = coord_type(0.0, 0.0, 0.0);
                sys.force   (0) = coord_type(0.0, 0.0, 0.0);
                sys.force   (1) = coord_type(0.0, 0.0, 0.0);
                sys.force   (2) = coord_type(0.0, 0.0, 0.0);
                sys.force   (3) = coord_type(0.0, 0.0, 0.0);

                // rotate randomly system
                using matrix33_type = typename traits_type::matrix33_type;

                const auto rot_x = uni(mt) * pi;
                const auto rot_y = uni(mt) * pi;
                const auto rot_z = uni(mt) * pi;

                const matrix33_type rotm_x(1.0,             0.0,              0.0,
                                           0.0, std::cos(rot_x), -std::sin(rot_x),
                                           0.0, std::sin(rot_x),  std::cos(rot_x));
                const matrix33_type rotm_y(std::cos(rot_y), 0.0,  std::sin(rot_y),
                                           0.0,             1.0,              0.0,
                                          -std::sin(rot_y), 0.0,  std::cos(rot_y));
                const matrix33_type rotm_z(std::cos(rot_z), -std::sin(rot_z), 0.0,
                                           std::sin(rot_z),  std::cos(rot_z), 0.0,
                                           0.0,              0.0,             1.0);

                const matrix33_type rotm = rotm_x * rotm_y * rotm_z;
                for(std::size_t idx=0; idx<sys.size(); ++idx)
                {
                    sys.position(idx) = rotm * sys.position(idx);
                }

                interaction.update(sys);
                constexpr real_type tol = 1e-4;
                auto ref_sys = sys;

                const auto energy = interaction.calc_force_and_energy(sys);
                const auto ref_energy = interaction.calc_energy(ref_sys);
                interaction.calc_force(ref_sys);
                BOOST_TEST(ref_energy == energy, boost::test_tools::tolerance(tol));

                for(std::size_t idx=0; idx<sys.size(); ++idx)
                {
                    BOOST_TEST(mjolnir::math::X(sys.force(idx)) == mjolnir::math::X(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
                    BOOST_TEST(mjolnir::math::Y(sys.force(idx)) == mjolnir::math::Y(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
                    BOOST_TEST(mjolnir::math::Z(sys.force(idx)) == mjolnir::math::Z(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
                }
            }
        }
    }
}


/*-----------------------------------------------------------------------------+
Copyright (c) 2010-2010: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_TYPE_TRAITS_ADDS_INVERSELY_HPP_JOFA_100829
#define BOOST_ICL_TYPE_TRAITS_ADDS_INVERSELY_HPP_JOFA_100829

#include <boost/icl/type_traits/has_inverse.hpp>
#include <boost/icl/functors.hpp>

namespace boost{ namespace icl
{

template<class Type, class Combiner>
struct adds_inversely
{
    typedef adds_inversely type;
    BOOST_STATIC_CONSTANT(bool,
        value = (mpl::and_<has_inverse<Type>, is_negative<Combiner> >::value));
};

}} // namespace boost icl

#endif // BOOST_ICL_TYPE_TRAITS_ADDS_INVERSELY_HPP_JOFA_100829


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-operator
    This function object computes the difference of two parameters of the same type.

    Infix notation can be used with operator '-',

    @par Header <boost/simd/function/minus.hpp>

    @par Decorators

     - `saturated_` (See @ref group-decorator) computes the saturated difference.

    @see plus, divides, multiplies

    @par Example:

      @snippet minus.cpp minus

    @par Possible output:

      @snippet minus.txt minus

  **/
  Value minus(Value const& x, Value const& y);
} }
#endif

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

#endif


#include "focal_grid.h"

#include <string>
#include <stdint.h>
#include <iostream>
#include <iomanip>
#include <math.h>
#include <chrono>
#include <vector>
#include <map>
#include <unordered_map>
#include <iterator>
#include <future>
#include <thread>
#include <algorithm>
#include <dirent.h>
#include <armadillo>

using namespace std;

FocalGrid::FocalGrid() {
	// empty
}

bool FocalGrid::LoadCalibration(session_param &ses_par, vox_grid &vox, frames &frame) {

	bool grid_loaded = true;

	try {

		vox.image_size.clear();
		vox.calib_mat.clear();
		vox.X_xyz.clear();
		vox.X_uv.clear();
		vox.uv_offset.clear();

		image_size.clear();
		calib_mat.clear();
		X_xyz.clear();
		X_uv.clear();
		uv_offset.clear();

		string calib_file = ses_par.session_loc + "/" + ses_par.cal_loc + "/" + ses_par.cal_name;

		arma::Mat<double> CalibMatrix;

		CalibMatrix.load(calib_file);

		for (int i=0; i<ses_par.N_cam; i++) {
			vox.image_size.push_back(frame.image_size[i]);
			vox.calib_mat.push_back(CalibMatrix.col(i));
			vox.X_xyz.push_back(FocalGrid::Camera2WorldMatrix(CalibMatrix.col(i)));
			vox.X_uv.push_back(FocalGrid::World2CameraMatrix(CalibMatrix.col(i)));
			arma::Col<double> uv_off_i = {CalibMatrix(10,i)/2.0-CalibMatrix(12,i)/2.0, CalibMatrix(9,i)/2.0-CalibMatrix(11,i)/2.0, 0.0};
			vox.uv_offset.push_back(uv_off_i);

			image_size.push_back(frame.image_size[i]);
			calib_mat.push_back(CalibMatrix.col(i));
			X_xyz.push_back(FocalGrid::Camera2WorldMatrix(CalibMatrix.col(i)));
			X_uv.push_back(FocalGrid::World2CameraMatrix(CalibMatrix.col(i)));
			uv_offset.push_back(uv_off_i);
		}

		// Populate internal parameters

		N_cam = vox.N_cam;
		N_threads = vox.N_threads;
		nx = vox.nx;
		ny = vox.ny;
		nz = vox.nz;
		ds = vox.ds;
		x0 = vox.x0;
		y0 = vox.y0;
		z0 = vox.z0;

	}
	catch (...) {
		grid_loaded = false;
	}

	return grid_loaded;
}

bool FocalGrid::ConstructFocalGrid(vox_grid &vox) {

	bool grid_build = true;

	int voxel_ind = 0;

	try {

		vox.pix2vox.clear();

		vox.vox2pix.clear();

		for (int k=0; k<nz; k++) {
			cout << k << endl;
			for (int j=0; j<ny; j++) {
				for (int i=0; i<nx; i++) {
					vector<int> uv_voxel = FocalGrid::CheckVoxel(i, j, k);
					if (uv_voxel.size()==N_cam) {
						voxel_ind = k*nx*ny+j*nx+i;
						vox.pix2vox.insert(pair<int,int>(uv_voxel[0],voxel_ind));
						vox.vox2pix.insert(pair<int,vector<int>>(voxel_ind,uv_voxel));
					}
				}
			}
		}

	}
	catch (...) {
		grid_build = false;
	}

	return grid_build;
}

vector<int> FocalGrid::CheckVoxel(int i, int j, int k) {

	vector<int> uv_out;

	int uv_ind = -1;

	arma::Col<double> xyz(4);

	xyz = {x0-((nx-1)/2.0)*ds+i*ds,
		y0-((ny-1)/2.0)*ds+j*ds,
		z0-((nz-1)/2.0)*ds+k*ds,
		1.0};

	arma::Col<double> uv(3);

	int n=0;
	bool uv_out_of_range = false;

	while (n<N_cam && uv_out_of_range==false) {

		uv = X_uv[n]*xyz-uv_offset[n];

		if (uv(0)>=0 && uv(0)<(get<1>(image_size[n]))) {
			if (uv(1)>=0 && uv(1)<(get<0>(image_size[n]))) {
				uv_ind = ((int) uv(1))*get<1>(image_size[n])+((int) uv(0));
				uv_out.push_back(uv_ind);
			}
			else {
				uv_out.clear();
				uv_out_of_range = true;
			}
		}
		else {
			uv_out.clear();
			uv_out_of_range = true;
		}
		n++;
	}

	return uv_out;
}

arma::Col<double> FocalGrid::PointCloudMatching(arma::Mat<double> &dest_pcl, arma::Mat<double> &src_pcl, double search_radius) {

	// Create a multimap of the two pointclouds and try to find dest-src pairs in the x, y and z direction:

	multimap<int,double> xy_map;
	multimap<int,double> xz_map;
	multimap<int,double> yz_map;

	int N_dest = dest_pcl.n_cols;

	cout << "N_dest" << endl;
	cout << N_dest << endl;

	for (int i=0; i<N_dest; i++) {

		int i_x = (int) (dest_pcl(1,i)-x0)/ds+((nx-1)/2.0);
		int i_y = (int) (dest_pcl(2,i)-y0)/ds+((ny-1)/2.0);
		int i_z = (int) (dest_pcl(3,i)-z0)/ds+((nz-1)/2.0);

		xy_map.insert(pair<int,double>(i_y*nx+i_x,dest_pcl(1,i)));
		xz_map.insert(pair<int,double>(i_x*nz+i_z,dest_pcl(2,i)));
		yz_map.insert(pair<int,double>(i_z*ny+i_y,dest_pcl(3,i)));

	}

	int N_src = src_pcl.n_cols;

	cout << "N_src" << endl;
	cout << N_src << endl;

	arma::Col<double> src_cg;
	src_cg.zeros(3);

	vector<tuple<double,double,double>> delta_x_vec;
	vector<tuple<double,double,double>> delta_y_vec;
	vector<tuple<double,double,double>> delta_z_vec;

	for (int j=0; j<N_src; j++) {

		src_cg(0) += src_pcl(1,j)/(N_src*1.0);
		src_cg(1) += src_pcl(2,j)/(N_src*1.0);
		src_cg(2) += src_pcl(3,j)/(N_src*1.0);

		int j_x = (int) (src_pcl(1,j)-x0)/ds+((nx-1)/2.0);
		int j_y = (int) (src_pcl(2,j)-y0)/ds+((ny-1)/2.0);
		int j_z = (int) (src_pcl(3,j)-z0)/ds+((nz-1)/2.0);

		// delta z
		pair<multimap<int,double>::iterator, multimap<int,double>::iterator> dest_z_voxels;
		dest_z_voxels = xy_map.equal_range(j_y*nx+j_x);
		double delta_z = 100000.0;
		double delta_z_new;
		double z_val;

		for (multimap<int,double>::iterator it_z=dest_z_voxels.first; it_z != dest_z_voxels.second; ++it_z) {
			z_val = it_z->second;
			delta_z_new = z_val-src_pcl(3,j);
			if (abs(delta_z_new)<abs(delta_z)) {
				delta_z = delta_z_new;
			}
		}

		if (abs(delta_z)<search_radius) {
			delta_z_vec.push_back(make_tuple(delta_z,src_pcl(1,j),src_pcl(2,j)));
		}

		// delta y
		pair<multimap<int,double>::iterator, multimap<int,double>::iterator> dest_y_voxels;
		dest_y_voxels = xz_map.equal_range(j_x*nz+j_z);
		double delta_y = 100000.0;
		double delta_y_new;
		double y_val;

		for (multimap<int,double>::iterator it_y=dest_y_voxels.first; it_y != dest_y_voxels.second; ++it_y) {
			y_val = it_y->second;
			delta_y_new = y_val-src_pcl(2,j);
			if (abs(delta_y_new)<abs(delta_y)) {
				delta_y = delta_y_new;
			}
		}

		if (abs(delta_y)<search_radius) {
			delta_y_vec.push_back(make_tuple(delta_y,src_pcl(1,j),src_pcl(3,j)));
		}

		// delta x
		pair<multimap<int,double>::iterator, multimap<int,double>::iterator> dest_x_voxels;
		dest_x_voxels = yz_map.equal_range(j_z*ny+j_y);
		double delta_x = 100000.0;
		double delta_x_new;
		double x_val;

		for (multimap<int,double>::iterator it_x=dest_x_voxels.first; it_x != dest_x_voxels.second; ++it_x) {
			x_val = it_x->second;
			delta_x_new = x_val-src_pcl(1,j);
			if (abs(delta_x_new)<abs(delta_x)) {
				delta_x = delta_x_new;
			}
		}

		if (abs(delta_x)<search_radius) {
			delta_x_vec.push_back(make_tuple(delta_x,src_pcl(2,j),src_pcl(3,j)));
		}

	}

	double theta_x = 0.0;
	double trans_x = 0.0;
	double theta_y = 0.0;
	double trans_y = 0.0;
	double theta_z = 0.0;
	double trans_z = 0.0;

	int N_dx = delta_x_vec.size();
	int N_dy = delta_y_vec.size();
	int N_dz = delta_z_vec.size();

	cout << "delta x size" << endl;
	cout << N_dx << endl;
	cout << "delta y size" << endl;
	cout << N_dy << endl;
	cout << "delta z size" << endl;
	cout << N_dz << endl;

	double mse_x = 0.0;
	double mse_y = 0.0;
	double mse_z = 0.0;

	double theta_x_update;
	double theta_y_update;
	double theta_z_update;
	double trans_x_update;
	double trans_y_update;
	double trans_z_update;

	int N_theta_x = 0;
	int N_theta_y = 0;
	int N_theta_z = 0;
	int N_trans_x = 0;
	int N_trans_y = 0;
	int N_trans_z = 0;

	for (int k=0; k<N_dx; k++) {

		trans_x_update = get<0>(delta_x_vec[k]);
		theta_y_update = atan2(get<0>(delta_x_vec[k]),get<2>(delta_x_vec[k])-src_cg(2));
		theta_z_update = atan2(get<0>(delta_x_vec[k]),get<1>(delta_x_vec[k])-src_cg(1));

		if (isfinite(trans_x_update)==true) {
			trans_x += trans_x_update;
			mse_x += pow(get<0>(delta_x_vec[k]),2);
			N_trans_x++;
		}
		if (isfinite(theta_y_update)==true) {
			theta_y += theta_y_update;
			N_theta_y++;
		}
		if (isfinite(theta_z_update)==true) {
			theta_z -= theta_z_update;
			N_theta_z++;
		}
	}

	for (int k=0; k<N_dy; k++) {

		trans_y_update = get<0>(delta_y_vec[k]);
		theta_x_update = atan2(get<0>(delta_y_vec[k]),get<2>(delta_y_vec[k])-src_cg(2));
		theta_z_update = atan2(get<0>(delta_y_vec[k]),get<2>(delta_y_vec[k])-src_cg(0));

		if (isfinite(trans_y_update)==true) {
			trans_y += trans_y_update;
			mse_y += pow(get<0>(delta_y_vec[k]),2);
			N_trans_y++;
		}
		if (isfinite(theta_x_update)==true) {
			theta_x -= theta_x_update;
			N_theta_x++;
		}
		if (isfinite(theta_z_update)==true) {
			theta_z += theta_z_update;
			N_theta_z++;
		}
	}

	for (int k=0; k<N_dz; k++) {

		trans_z_update = get<0>(delta_z_vec[k]);
		theta_x_update = atan2(get<0>(delta_z_vec[k]),get<2>(delta_z_vec[k])-src_cg(1));
		theta_y_update = atan2(get<0>(delta_z_vec[k]),get<1>(delta_z_vec[k])-src_cg(0));

		if (isfinite(trans_z_update)==true) {
			trans_z += trans_z_update;
			mse_z += pow(get<0>(delta_z_vec[k]),2);
			N_trans_z++;
		}
		if (isfinite(theta_x_update)==true) {
			theta_x += theta_x_update;
			N_theta_x++;
		}
		if (isfinite(theta_y_update)==true) {
			theta_y -= theta_y_update;
			N_theta_y++;
		}
	}

	arma::Col<double> state_update(9);

	if (N_theta_x>3) {
		state_update(0) = theta_x/(N_theta_x*1.0);
	}
	else {
		state_update(0) = 0.0;
	}
	if (N_theta_y>3) {
		state_update(1) = theta_y/(N_theta_y*1.0);
	}
	else {
		state_update(1) = 0.0;
	}
	if (N_theta_z>3) {
		state_update(2) = theta_z/(N_theta_z*1.0);
	}
	else {
		state_update(2) = 0.0;
	}
	if (N_trans_x>3) {
		state_update(3) = trans_x/(N_trans_x*1.0);
		state_update(6) = mse_x/(N_trans_x*1.0);
	}
	else {
		state_update(3) = 0.0;
		state_update(6) = 1.0;
	}
	if (N_trans_y>3) {
		state_update(4) = trans_y/(N_trans_y*1.0);
		state_update(7) = mse_y/(N_trans_y*1.0);
	}
	else {
		state_update(4) = 0.0;
		state_update(7) = 1.0;
	}
	if (N_trans_z>3) {
		state_update(5) = trans_z/(N_trans_z*1.0);
		state_update(8) = mse_z/(N_trans_z*1.0);
	}
	else {
		state_update(5) = 0.0;
		state_update(8) = 1.0;
	}

	cout << "state update" << endl;
	cout << state_update << endl;

	return state_update;
}

vector<tuple<int,double,double,double,double,double,double>> FocalGrid::ProjectImage2Cloud(vector<arma::Col<int>> &frame_in, vox_grid &vox) {

	//vector<tuple<double,double,double,int>> pcl_now;

	vector<tuple<int,double,double,double,double,double,double>> pcl_now;

	unordered_map<int,int> pcl_voxels;

	int N_row = get<0>(vox.image_size[0]);
	int N_col = get<1>(vox.image_size[0]);

	pair<multimap<int,int>::iterator, multimap<int,int>::iterator> voxels_i;

	int vox_now;
	vector<int> uv_now;

	int n = 0;
	bool is_voxel = true;

	int frame_val_0 = 0;
	int frame_val_n = 0;

	int code_now = 0;

	int count = 0;

	// Insert voxels into an unordered map and give them a segment code:
	for (int i=0; i<(N_row*N_col); i++) {
		frame_val_0 = frame_in[0](i);
		if (frame_val_0>0) {
			voxels_i = vox.pix2vox.equal_range(i);
			for (multimap<int,int>::iterator it=voxels_i.first; it != voxels_i.second; ++it) {
				vox_now = it->second;
				uv_now = vox.vox2pix[vox_now];
				n = 1;
				is_voxel = true;
				code_now = frame_val_0;
				while (is_voxel==true && n < N_cam) {
					frame_val_n = frame_in[n](uv_now[n]);
					if (frame_val_n>0) {
						code_now = code_now+pow(max_n_seg,n)*frame_val_n;
					}
					else {
						is_voxel = false;
					}
					n++;
				}
				if (is_voxel==true) {
					count++;
					pcl_voxels.insert(pair<int,int>(vox_now,code_now));
				}
			}
		}
	}

	// For each voxel in the pcl_voxels map:
	// -> find the neighboring voxels
	// -> calculate normal
	// -> if a normal could be calculated, add the voxel_id, xyz_pos and normal to pcl_now

	unordered_map<int,int>::iterator nb_it;

	arma::Col<int> neighbors(27);

	arma::Col<double> nb_vec(27);

	arma::Col<double> normal(3);

	arma::Col<double> xyz_pos(3);

	int nb_vox_id;
	int nb_vox_code;

	for (nb_it = pcl_voxels.begin(); nb_it != pcl_voxels.end(); nb_it++) {
		vox_now = nb_it->first;
		code_now = nb_it->second;
		neighbors = FocalGrid::FindNeighbors(vox_now);
		if (neighbors(13)>0) {
			for (int m=0; m<27; m++) {
				if (pcl_voxels.find(neighbors(m)) != pcl_voxels.end()) {
					nb_vec(m) = 0.0;
				}
				else {
					nb_vec(m) = 1.0;
				}
			}
			normal = FocalGrid::CalculateNormal(nb_vec);
			if (arma::norm(normal,2)>0.5) {
				xyz_pos = FocalGrid::CalculatePosition(vox_now);
				pcl_now.push_back(make_tuple(code_now,xyz_pos(0),xyz_pos(1),xyz_pos(2),normal(0),normal(1),normal(2)));
			}
		}
	}

	if (pcl_now.size() <= 0) {
		pcl_now.push_back(make_tuple(0,0.0,0.0,0.0,0.0,0.0,0.0));
	}
	return pcl_now;
}

arma::Col<int> FocalGrid::FindNeighbors(int vox_ind) {

	// find the neighboring voxels:

	arma::Col<int> vox_int_mat(27);

	int i = vox_ind % nx;
	int j = ((vox_ind-i)/nx) % ny;
	int k = (vox_ind-i-j*nx)/(nx*ny);

	if ((i>0 && i<(nx-1)) && (j>0 && j<(ny-1)) && (k>0 && k<(nz-1))) {

		vox_int_mat(0) = vox_ind-nx*ny-nx-1;
		vox_int_mat(1) = vox_ind-nx*ny-nx;
		vox_int_mat(2) = vox_ind-nx*ny-nx+1;
		vox_int_mat(3) = vox_ind-nx*ny-1;
		vox_int_mat(4) = vox_ind-nx*ny;
		vox_int_mat(5) = vox_ind-nx*ny+1;
		vox_int_mat(6) = vox_ind-nx*ny+nx-1;
		vox_int_mat(7) = vox_ind-nx*ny+nx;
		vox_int_mat(8) = vox_ind-nx*ny+nx+1;
		vox_int_mat(9) = vox_ind-nx-1;
		vox_int_mat(10) = vox_ind-nx;
		vox_int_mat(11) = vox_ind-nx+1;
		vox_int_mat(12) = vox_ind-1;
		vox_int_mat(13) = vox_ind;
		vox_int_mat(14) = vox_ind+1;
		vox_int_mat(15) = vox_ind+nx-1;
		vox_int_mat(16) = vox_ind+nx;
		vox_int_mat(17) = vox_ind+nx+1;
		vox_int_mat(18) = vox_ind+nx*ny-nx-1;
		vox_int_mat(19) = vox_ind+nx*ny-nx;
		vox_int_mat(20) = vox_ind+nx*ny-nx+1;
		vox_int_mat(21) = vox_ind+nx*ny-1;
		vox_int_mat(22) = vox_ind+nx*ny;
		vox_int_mat(23) = vox_ind+nx*ny+1;
		vox_int_mat(24) = vox_ind+nx*ny+nx-1;
		vox_int_mat(25) = vox_ind+nx*ny+nx;
		vox_int_mat(26) = vox_ind+nx*ny+nx+1;

	}
	else {
		vox_int_mat.zeros();
	}

	return vox_int_mat;
}

arma::Col<double> FocalGrid::CalculatePosition(int vox_ind) {

	int i = vox_ind % nx;
	int j = ((vox_ind-i)/nx) % ny;
	int k = (vox_ind-i-j*nx)/(nx*ny);

	arma::Col<double> xyz_pos(3);

	xyz_pos(0) = x0-((nx-1)/2.0)*ds+i*ds;
	xyz_pos(1) = y0-((ny-1)/2.0)*ds+j*ds;
	xyz_pos(2) = z0-((nz-1)/2.0)*ds+k*ds;

	return xyz_pos;
}

arma::Col<double> FocalGrid::CalculateNormal(arma::Col<double> neighbor_vector) {

	arma::Col<double> normal;

	normal.zeros(3);

	if (arma::sum(neighbor_vector)>1.0 && arma::sum(neighbor_vector)<20.0) {
		normal = normal_mat.t()*neighbor_vector;
		if (arma::norm(normal,2)>0.5){
			normal = arma::normalise(normal);
		}
		else {
			normal.zeros(3);
		}
	}

	return normal;
}

vector<arma::Col<int>> FocalGrid::ProjectCloud2Image(vector<tuple<double,double,double,int>> &cloud_in) {

	vector<arma::Col<int>> frame_now;

	int N_vox = cloud_in.size();

	for (int n=0; n<N_cam; n++) {
		arma::Col<int> frame_n;
		frame_n.zeros(get<0>(image_size[n])*get<1>(image_size[n]));
		frame_now.push_back(frame_n);
	}

	arma::Col<double> uv;
	arma::Col<double> xyz;
	int u = 0;
	int v = 0;

	for (int i=0; i<N_vox; i++) {
		for (int j=0; j<N_cam; j++) {
			xyz = {get<0>(cloud_in[i]),get<1>(cloud_in[i]),get<2>(cloud_in[i]),1.0};
			uv = X_uv[j]*xyz-uv_offset[j];
			if (uv(0)>=0 && uv(0)<(get<1>(image_size[j]))) {
				if (uv(1)>=0 && uv(1)<(get<0>(image_size[j]))) {
					u = (int) uv(0);
					v = (int) uv(1);
					if (frame_now[j](get<1>(image_size[j])*v+u)==0) {
						frame_now[j](get<1>(image_size[j])*v+u) = get<3>(cloud_in[i]);
					}
					else {
						if (frame_now[j](get<1>(image_size[j])*v+u)>get<3>(cloud_in[i])) {
							frame_now[j](get<1>(image_size[j])*v+u) = get<3>(cloud_in[i]);
						}
					}
				}
			}
		}
	}
	return frame_now;
}

arma::Mat<int> FocalGrid::TransformXYZ2UV(arma::Col<double> xyz_pos) {

	arma::Mat<int> uv_mat(2,N_cam);

	for (int n=0; n<N_cam; n++) {
		arma::Col<double> uv = X_uv[n]*xyz_pos-uv_offset[n];
		uv_mat(0,n) = int(uv(0));
		uv_mat(1,n) = int(uv(1));
	}

	return uv_mat;
}

tuple<arma::Mat<int>, arma::Col<double>> FocalGrid::RayCasting(int cam_nr, arma::Col<double> xyz_pos_prev, arma::Col<double> uv_pos_prev, arma::Col<double> uv_pos_now) {

	// Calculate the 3D translation vector:
	arma::Col<double> xyz_uv_prev = X_xyz[cam_nr]*(uv_pos_prev+uv_offset[cam_nr]);
	arma::Col<double> xyz_uv_now = X_xyz[cam_nr]*(uv_pos_now+uv_offset[cam_nr]);
	arma::Col<double> trans_vec = xyz_uv_now-xyz_uv_prev;
	trans_vec(3) = 0.0;

	// Add the translation to the xyz position:
	arma::Col<double> xyz_pos_now = xyz_pos_prev + trans_vec;

	// Project the new position back to the camera views:

	arma::Mat<int> uv_mat(3,N_cam);

	for (int n=0; n<N_cam; n++) {
		arma::Col<double> uv = X_uv[n]*xyz_pos_now-uv_offset[n];
		if (uv(0)>=0 && uv(0)<(get<1>(image_size[n])) && uv(1)>=0 && uv(1)<(get<0>(image_size[n]))) {
			uv_mat(0,n) = int(uv(0));
			uv_mat(1,n) = int(uv(1));
			uv_mat(2,n) = int(uv(2));
		}
		else {
			arma::Col<double> uv_old = X_uv[n]*xyz_pos_prev-uv_offset[n];
			uv_mat(0,n) = int(uv_old(0));
			uv_mat(1,n) = int(uv_old(1));
			uv_mat(2,n) = int(uv_old(2));
		}
	}

	return make_tuple(uv_mat,xyz_pos_now);
}

arma::Mat<double> FocalGrid::Camera2WorldMatrix(arma::Col<double> calib_param) {

	// return the world to camera projection matrix

	arma::Mat<double> C = {{calib_param(0), calib_param(2), 0, 0},
		 			{0, calib_param(1), 0, 0},
		 			{0, 0, 0, 1}};

	double theta = sqrt(pow(calib_param(3),2)+pow(calib_param(4),2)+pow(calib_param(5),2));

	arma::Mat<double> omega = {{0, -calib_param(5), calib_param(4)},
			 			{calib_param(5), 0, -calib_param(3)},
			 			{-calib_param(4), calib_param(3), 0}};

	arma::Mat<double> R(3,3); R.eye();

	R = R+(sin(theta)/theta)*omega+((1-cos(theta))/pow(theta,2))*(omega*omega);

	arma::Col<double> T = {calib_param(6), calib_param(7), calib_param(8)};

	arma::Mat<double> K = {{R(0,0), R(0,1), R(0,2), T(0)},
		 				{R(1,0), R(1,1), R(1,2), T(1)},
		 				{R(2,0), R(2,1), R(2,2), T(2)},
		 				{0, 0, 0, 1}};

	return arma::inv(K)*arma::pinv(C);

}

arma::Mat<double> FocalGrid::World2CameraMatrix(arma::Col<double> calib_param) {

	// return the world to camera projection matrix

	arma::Mat<double> C = {{calib_param(0), calib_param(2), 0, 0},
		 			{0, calib_param(1), 0, 0},
		 			{0, 0, 0, 1}};

	double theta = sqrt(pow(calib_param(3),2)+pow(calib_param(4),2)+pow(calib_param(5),2));

	arma::Mat<double> omega = {{0, -calib_param(5), calib_param(4)},
			 			{calib_param(5), 0, -calib_param(3)},
			 			{-calib_param(4), calib_param(3), 0}};

	arma::Mat<double> R(3,3); R.eye();

	R = R+(sin(theta)/theta)*omega+((1-cos(theta))/pow(theta,2))*(omega*omega);

	arma::Col<double> T = {calib_param(6), calib_param(7), calib_param(8)};

	arma::Mat<double> K = {{R(0,0), R(0,1), R(0,2), T(0)},
		 				{R(1,0), R(1,1), R(1,2), T(1)},
		 				{R(2,0), R(2,1), R(2,2), T(2)},
		 				{0, 0, 0, 1}};

	return C*K;

}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef VACUUM_HPP
#define VACUUM_HPP

#include <cmath>
#include <iosfwd>
#include <vector>

#include "Config.hpp"

#include <Eigen/Core>

class Element;

#include "DerivativeTypes.hpp"
#include "DerivativeUtils.hpp"
#include "bi_operators/IntegratorForward.hpp"
#include "GreensFunction.hpp"

/*! \file Vacuum.hpp
 *  \class Vacuum
 *  \brief Green's function for vacuum.
 *  \author Luca Frediani and Roberto Di Remigio
 *  \date 2012-2014
 *  \tparam DerivativeTraits evaluation strategy for the function and its derivatives
 *  \tparam IntegratorPolicy policy for the calculation of the matrix represenation of S and D
 */

template <typename DerivativeTraits = AD_directional,
          typename IntegratorPolicy = CollocationIntegrator>
class Vacuum __final : public GreensFunction<DerivativeTraits, IntegratorPolicy, Uniform,
                                     Vacuum<DerivativeTraits, IntegratorPolicy> >
{
public:
    Vacuum() : GreensFunction<DerivativeTraits, IntegratorPolicy, Uniform,
                              Vacuum<DerivativeTraits, IntegratorPolicy> >()
    {
        this->profile_ = Uniform(1.0);
    }
    Vacuum(double f) : GreensFunction<DerivativeTraits, IntegratorPolicy, Uniform,
                              Vacuum<DerivativeTraits, IntegratorPolicy> >(f)
    {
        this->profile_ = Uniform(1.0);
    }
    virtual ~Vacuum() {}

    /*! Calculates the matrix representation of the S operator
     *  \param[in] e list of finite elements
     */
    virtual Eigen::MatrixXd singleLayer(const std::vector<Element> & e) const __override
    {
        return this->integrator_.singleLayer(*this, e);
    }
    /*! Calculates the matrix representation of the D operator
     *  \param[in] e list of finite elements
     */
    virtual Eigen::MatrixXd doubleLayer(const std::vector<Element> & e) const __override
    {
        return this->integrator_.doubleLayer(*this, e);
    }

    friend std::ostream & operator<<(std::ostream & os, Vacuum & gf) {
        return gf.printObject(os);
    }
private:
    /*! Evaluates the Green's function given a pair of points
     *  \param[in] sp the source point
     *  \param[in] pp the probe point
     */
    virtual DerivativeTraits operator()(DerivativeTraits * sp, DerivativeTraits * pp) const __override
    {
        return (1 / distance(sp, pp));
    }
    /*! Returns value of the kernel of the \f$\mathcal{D}\f$ integral operator
     *  for the pair of points p1, p2:
     *  \f$ [\boldsymbol{\varepsilon}\nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)]\cdot \mathbf{n}_{\mathbf{p}_2}\f$
     *  To obtain the kernel of the \f$\mathcal{D}^\dagger\f$ operator call this methods with \f$\mathbf{p}_1\f$
     *  and \f$\mathbf{p}_2\f$ exchanged and with \f$\mathbf{n}_{\mathbf{p}_2} = \mathbf{n}_{\mathbf{p}_1}\f$
     *  \param[in] direction the direction
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    virtual double kernelD_impl(const Eigen::Vector3d & direction,
                              const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const __override
    {
        return this->derivativeProbe(direction, p1, p2);
    }
    virtual KernelS exportKernelS_impl() const __override {
      return pcm::bind(&Vacuum<DerivativeTraits, IntegratorPolicy>::kernelS, *this, pcm::_1, pcm::_2);
    }
    virtual KernelD exportKernelD_impl() const __override {
      return pcm::bind(&Vacuum<DerivativeTraits, IntegratorPolicy>::kernelD, *this, pcm::_1, pcm::_2, pcm::_3);
    }
    virtual std::ostream & printObject(std::ostream & os) __override
    {
        os << "Green's function type: vacuum";
        return os;
    }
};

#endif // VACUUM_HPP


// Copyright (C) 2015 The Regents of the University of California (Regents)
// and Google, Inc. 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 Regents or University of California, Google,
//       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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/two_point_pose_partial_rotation.h"

#include <math.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "theia/math/closed_form_polynomial_solver.h"

namespace theia {
using Eigen::Map;
using Eigen::Vector3d;
using Eigen::Quaterniond;

namespace {

// Adds a specific pose solution corresponding to the ray lengths.
inline void AddPoseSolution(const Vector3d& axis,
                            const Vector3d& model_point_1,
                            const Vector3d& model_point_2,
                            const Vector3d& image_ray_1,
                            const Vector3d& image_ray_2,
                            const double ray_length_1,
                            const double ray_length_2,
                            Quaterniond* rotation,
                            Vector3d* translation) {
  // Scales the image_rays by the calculated ray lengths, computing the points
  // in image space.
  const Vector3d point_in_image_space_1 = ray_length_1 * image_ray_1;
  const Vector3d point_in_image_space_2 = ray_length_2 * image_ray_2;
  const Vector3d image_points_diff =
      point_in_image_space_1 - point_in_image_space_2;

  const Vector3d model_points_diff = model_point_1 - model_point_2;

  // Computes two basis vectors that lie on the plane orthogonal to the axis.
  const Vector3d basis_vector_2 = axis.cross(model_points_diff).normalized();
  const Vector3d basis_vector_1 = basis_vector_2.cross(axis).normalized();

  assert(0.0 < basis_vector_1.dot(basis_vector_1));

  // Finds the projection of the image_points_diff vector in this basis.
  const double dp_1 = basis_vector_1.dot(image_points_diff);
  const double dp_2 = basis_vector_2.dot(image_points_diff);

  // Finds the angle around the axis.
  const double angle = atan2(dp_2, dp_1);
  *rotation = Quaterniond(Eigen::AngleAxisd(angle, axis));

  // Calculates the translation, after taking into account the rotation.
  *translation = point_in_image_space_1 - *rotation * model_point_1;
}

int TwoPointPoseCore(const Vector3d& axis,
                     const Vector3d& model_point_1,
                     const Vector3d& model_point_2,
                     const Vector3d& image_ray_1,
                     const Vector3d& image_ray_2,
                     Quaterniond soln_rotations[2],
                     Vector3d soln_translations[2]) {
  // Let the points in the camera coordinate system be
  // y * image_ray_1, x * image_ray_2, where x, y are the lengths of the
  // rays. Since there is only rotation about the passed axis, the difference
  // between the values in the model and camera coordinate systems, projected
  // on the axis, should be the same. So:
  //
  // DotProd(axis, y * image_ray_1 - x * image_ray_2)  =
  //     DotProd(axis, model_points[0] - model_points[1])
  //
  // This allows x to be expressed in terms of y:
  //
  // x = m + n * y
  const double ray_1_axis_dp = image_ray_1.dot(axis);
  const double ray_2_axis_dp = image_ray_2.dot(axis);
  const double model_diff_axis_dp = (model_point_1 - model_point_2).dot(axis);

  const double m = model_diff_axis_dp / ray_1_axis_dp;
  const double n = ray_2_axis_dp / ray_1_axis_dp;

  // Next, the distance between the model points and the image points should
  // be the same:
  //
  // |y * image_ray_1 - x * image_ray_2| =
  //     |model_points[0] - model_points[1]|
  //
  // Using this and the substitution for x above we can create a quadratic
  // equation in y.

  // Computes the coefficients of the quadratic equation ay^2 + by + c = 0.0.
  const double ray_dp = image_ray_1.dot(image_ray_2);

  const long double a = n * (n - 2.0 * ray_dp) + 1.0;
  const long double b = 2.0 * m * (n - ray_dp);
  const long double c = m * m - (model_point_1 - model_point_2).squaredNorm();

  double roots[2] = { 0.0, 0.0 };
  const int number_of_roots = SolveQuadraticReals(a, b, c, roots);

  int num_solutions = 0;
  for (int i = 0; i < number_of_roots; ++i) {
    // Only accept positive ray distances.
    if (roots[i] > 0) {
      // Computes the other ray distance.
      const double ray_distance = m + n * roots[i];
      if (ray_distance > 0) {
        AddPoseSolution(axis, model_point_1, model_point_2, image_ray_1,
                        image_ray_2, ray_distance, roots[i],
                        &soln_rotations[num_solutions],
                        &soln_translations[num_solutions]);
        num_solutions++;
      }
    }
  }
  return num_solutions;
}

}  // namespace

int TwoPointPosePartialRotation(const Vector3d& axis,
                                const Vector3d& model_point_1,
                                const Vector3d& model_point_2,
                                const Vector3d& image_ray_1,
                                const Vector3d& image_ray_2,
                                Quaterniond soln_rotations[2],
                                Vector3d soln_translations[2]) {
  static const double kEpsilon = 1e-9;
  assert(fabs(image_ray_1.squaredNorm() - 1.0) < kEpsilon);
  assert(fabs(image_ray_2.squaredNorm() - 1.0) < kEpsilon);

  if (fabs(image_ray_1.dot(axis)) < kEpsilon) {
    if (fabs(image_ray_2.dot(axis)) > kEpsilon) {
      // If image_ray_1.y() == 0 then the function above doesn't work because
      // the calculate m and n will have a divide by 0.
      // However if image_ray_2.y() is not equal to zero then we can swap
      // and the points and call the above function with the swapped points.
      // TODO(cmsweeney): Maybe always swap to improve the conditioning?
      return TwoPointPoseCore(axis, model_point_2, model_point_1, image_ray_2,
                              image_ray_1, soln_rotations, soln_translations);
    }
    return 0;
  } else {
    return TwoPointPoseCore(axis, model_point_1, model_point_2, image_ray_1,
                            image_ray_2, soln_rotations, soln_translations);
  }
}

}  // namespace theia


#define BOOST_TEST_MODULE "C++ Unit Tests for metaLBM"
#include <boost/test/included/unit_test.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
namespace tt = boost::test_tools;

#include <iostream>

#define NPROCS 1
#include "metaLBM/Lattice.h"
typedef double valueType;
typedef lbm::Lattice<valueType, lbm::LatticeType::D1Q3> L;
#include "metaLBM/Force.h"
#include "metaLBM/MathVector.h"

using namespace lbm;

BOOST_AUTO_TEST_SUITE(TestForce)

BOOST_AUTO_TEST_CASE(TestConstant) {
  constexpr ForceType forceType = ForceType::Constant;
  constexpr MathVector<valueType, 3> amplitude{{(valueType) 1}};
  constexpr MathVector<unsigned int, 3> iP{{1}};

  Force<valueType, forceType> force(amplitude, amplitude);
  force.setForce(iP);

  BOOST_TEST(force.getForce()[d::X] == (valueType) 1,
             tt::tolerance(1e-15));
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/geometry/formulas/authalic_radius_sqr.hpp>