//  (C) Copyright John Maddock 2006.
//  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_BETA_HPP
#define BOOST_MATH_SPECIAL_BETA_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/static_assert.hpp>
#include <boost/config/no_tr1/cmath.hpp>

namespace boost{ namespace math{

namespace detail{

//
// Implementation of Beta(a,b) using the Lanczos approximation:
//
template <class T, class Lanczos, class Policy>
T beta_imp(T a, T b, const Lanczos&, const Policy& pol)
{
   BOOST_MATH_STD_USING  // for ADL of std names

   if(a <= 0)
      policies::raise_domain_error<T>("boost::math::beta<%1%>(%1%,%1%)", "The arguments to the beta function must be greater than zero (got a=%1%).", a, pol);
   if(b <= 0)
      policies::raise_domain_error<T>("boost::math::beta<%1%>(%1%,%1%)", "The arguments to the beta function must be greater than zero (got b=%1%).", b, pol);

   T result;

   T prefix = 1;
   T c = a + b;

   // Special cases:
   if((c == a) && (b < tools::epsilon<T>()))
      return boost::math::tgamma(b, pol);
   else if((c == b) && (a < tools::epsilon<T>()))
      return boost::math::tgamma(a, pol);
   if(b == 1)
      return 1/a;
   else if(a == 1)
      return 1/b;

   /*
   //
   // This code appears to be no longer necessary: it was
   // used to offset errors introduced from the Lanczos
   // approximation, but the current Lanczos approximations
   // are sufficiently accurate for all z that we can ditch
   // this.  It remains in the file for future reference...
   //
   // If a or b are less than 1, shift to greater than 1:
   if(a < 1)
   {
      prefix *= c / a;
      c += 1;
      a += 1;
   }
   if(b < 1)
   {
      prefix *= c / b;
      c += 1;
      b += 1;
   }
   */

   if(a < b)
      std::swap(a, b);

   // Lanczos calculation:
   T agh = a + Lanczos::g() - T(0.5);
   T bgh = b + Lanczos::g() - T(0.5);
   T cgh = c + Lanczos::g() - T(0.5);
   result = Lanczos::lanczos_sum_expG_scaled(a) * Lanczos::lanczos_sum_expG_scaled(b) / Lanczos::lanczos_sum_expG_scaled(c);
   T ambh = a - T(0.5) - b;
   if((fabs(b * ambh) < (cgh * 100)) && (a > 100))
   {
      // Special case where the base of the power term is close to 1
      // compute (1+x)^y instead:
      result *= exp(ambh * boost::math::log1p(-b / cgh, pol));
   }
   else
   {
      result *= pow(agh / cgh, a - T(0.5) - b);
   }
   if(cgh > 1e10f)
      // this avoids possible overflow, but appears to be marginally less accurate:
      result *= pow((agh / cgh) * (bgh / cgh), b);
   else
      result *= pow((agh * bgh) / (cgh * cgh), b);
   result *= sqrt(boost::math::constants::e<T>() / bgh);

   // If a and b were originally less than 1 we need to scale the result:
   result *= prefix;

   return result;
} // template <class T, class Lanczos> beta_imp(T a, T b, const Lanczos&)

//
// Generic implementation of Beta(a,b) without Lanczos approximation support
// (Caution this is slow!!!):
//
template <class T, class Policy>
T beta_imp(T a, T b, const lanczos::undefined_lanczos& /* l */, const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(a <= 0)
      policies::raise_domain_error<T>("boost::math::beta<%1%>(%1%,%1%)", "The arguments to the beta function must be greater than zero (got a=%1%).", a, pol);
   if(b <= 0)
      policies::raise_domain_error<T>("boost::math::beta<%1%>(%1%,%1%)", "The arguments to the beta function must be greater than zero (got b=%1%).", b, pol);

   T result;

   T prefix = 1;
   T c = a + b;

   // special cases:
   if((c == a) && (b < tools::epsilon<T>()))
      return boost::math::tgamma(b, pol);
   else if((c == b) && (a < tools::epsilon<T>()))
      return boost::math::tgamma(a, pol);
   if(b == 1)
      return 1/a;
   else if(a == 1)
      return 1/b;

   // shift to a and b > 1 if required:
   if(a < 1)
   {
      prefix *= c / a;
      c += 1;
      a += 1;
   }
   if(b < 1)
   {
      prefix *= c / b;
      c += 1;
      b += 1;
   }
   if(a < b)
      std::swap(a, b);

   // set integration limits:
   T la = (std::max)(T(10), a);
   T lb = (std::max)(T(10), b);
   T lc = (std::max)(T(10), T(a+b));

   // calculate the fraction parts:
   T sa = detail::lower_gamma_series(a, la, pol) / a;
   sa += detail::upper_gamma_fraction(a, la, ::boost::math::policies::get_epsilon<T, Policy>());
   T sb = detail::lower_gamma_series(b, lb, pol) / b;
   sb += detail::upper_gamma_fraction(b, lb, ::boost::math::policies::get_epsilon<T, Policy>());
   T sc = detail::lower_gamma_series(c, lc, pol) / c;
   sc += detail::upper_gamma_fraction(c, lc, ::boost::math::policies::get_epsilon<T, Policy>());

   // and the exponent part:
   result = exp(lc - la - lb) * pow(la/lc, a) * pow(lb/lc, b);

   // and combine:
   result *= sa * sb / sc;

   // if a and b were originally less than 1 we need to scale the result:
   result *= prefix;

   return result;
} // template <class T>T beta_imp(T a, T b, const lanczos::undefined_lanczos& l)

//
// Compute the leading power terms in the incomplete Beta:
//
// (x^a)(y^b)/Beta(a,b) when normalised, and
// (x^a)(y^b) otherwise.
//
// Almost all of the error in the incomplete beta comes from this
// function: particularly when a and b are large. Computing large
// powers are *hard* though, and using logarithms just leads to
// horrendous cancellation errors.
//
template <class T, class Lanczos, class Policy>
T ibeta_power_terms(T a,
                        T b,
                        T x,
                        T y,
                        const Lanczos&,
                        bool normalised,
                        const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(!normalised)
   {
      // can we do better here?
      return pow(x, a) * pow(y, b);
   }

   T result;

   T prefix = 1;
   T c = a + b;

   // combine power terms with Lanczos approximation:
   T agh = a + Lanczos::g() - T(0.5);
   T bgh = b + Lanczos::g() - T(0.5);
   T cgh = c + Lanczos::g() - T(0.5);
   result = Lanczos::lanczos_sum_expG_scaled(c) / (Lanczos::lanczos_sum_expG_scaled(a) * Lanczos::lanczos_sum_expG_scaled(b));

   // l1 and l2 are the base of the exponents minus one:
   T l1 = (x * b - y * agh) / agh;
   T l2 = (y * a - x * bgh) / bgh;
   if(((std::min)(fabs(l1), fabs(l2)) < 0.2))
   {
      // when the base of the exponent is very near 1 we get really
      // gross errors unless extra care is taken:
      if((l1 * l2 > 0) || ((std::min)(a, b) < 1))
      {
         //
         // This first branch handles the simple cases where either:
         //
         // * The two power terms both go in the same direction
         // (towards zero or towards infinity).  In this case if either
         // term overflows or underflows, then the product of the two must
         // do so also.
         // *Alternatively if one exponent is less than one, then we
         // can't productively use it to eliminate overflow or underflow
         // from the other term.  Problems with spurious overflow/underflow
         // can't be ruled out in this case, but it is *very* unlikely
         // since one of the power terms will evaluate to a number close to 1.
         //
         if(fabs(l1) < 0.1)
         {
            result *= exp(a * boost::math::log1p(l1, pol));
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
         else
         {
            result *= pow((x * cgh) / agh, a);
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
         if(fabs(l2) < 0.1)
         {
            result *= exp(b * boost::math::log1p(l2, pol));
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
         else
         {
            result *= pow((y * cgh) / bgh, b);
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
      }
      else if((std::max)(fabs(l1), fabs(l2)) < 0.5)
      {
         //
         // Both exponents are near one and both the exponents are
         // greater than one and further these two
         // power terms tend in opposite directions (one towards zero,
         // the other towards infinity), so we have to combine the terms
         // to avoid any risk of overflow or underflow.
         //
         // We do this by moving one power term inside the other, we have:
         //
         //    (1 + l1)^a * (1 + l2)^b
         //  = ((1 + l1)*(1 + l2)^(b/a))^a
         //  = (1 + l1 + l3 + l1*l3)^a   ;  l3 = (1 + l2)^(b/a) - 1
         //                                    = exp((b/a) * log(1 + l2)) - 1
         //
         // The tricky bit is deciding which term to move inside :-)
         // By preference we move the larger term inside, so that the
         // size of the largest exponent is reduced.  However, that can
         // only be done as long as l3 (see above) is also small.
         //
         bool small_a = a < b;
         T ratio = b / a;
         if((small_a && (ratio * l2 < 0.1)) || (!small_a && (l1 / ratio > 0.1)))
         {
            T l3 = boost::math::expm1(ratio * boost::math::log1p(l2, pol), pol);
            l3 = l1 + l3 + l3 * l1;
            l3 = a * boost::math::log1p(l3, pol);
            result *= exp(l3);
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
         else
         {
            T l3 = boost::math::expm1(boost::math::log1p(l1, pol) / ratio, pol);
            l3 = l2 + l3 + l3 * l2;
            l3 = b * boost::math::log1p(l3, pol);
            result *= exp(l3);
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
      }
      else if(fabs(l1) < fabs(l2))
      {
         // First base near 1 only:
         T l = a * boost::math::log1p(l1, pol)
            + b * log((y * cgh) / bgh);
         result *= exp(l);
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
      else
      {
         // Second base near 1 only:
         T l = b * boost::math::log1p(l2, pol)
            + a * log((x * cgh) / agh);
         result *= exp(l);
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
   }
   else
   {
      // general case:
      T b1 = (x * cgh) / agh;
      T b2 = (y * cgh) / bgh;
      l1 = a * log(b1);
      l2 = b * log(b2);
      BOOST_MATH_INSTRUMENT_VARIABLE(b1);
      BOOST_MATH_INSTRUMENT_VARIABLE(b2);
      BOOST_MATH_INSTRUMENT_VARIABLE(l1);
      BOOST_MATH_INSTRUMENT_VARIABLE(l2);
      if((l1 >= tools::log_max_value<T>())
         || (l1 <= tools::log_min_value<T>())
         || (l2 >= tools::log_max_value<T>())
         || (l2 <= tools::log_min_value<T>())
         )
      {
         // Oops, overflow, sidestep:
         if(a < b)
            result *= pow(pow(b2, b/a) * b1, a);
         else
            result *= pow(pow(b1, a/b) * b2, b);
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
      else
      {
         // finally the normal case:
         result *= pow(b1, a) * pow(b2, b);
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
   }
   // combine with the leftover terms from the Lanczos approximation:
   result *= sqrt(bgh / boost::math::constants::e<T>());
   result *= sqrt(agh / cgh);
   result *= prefix;

   BOOST_MATH_INSTRUMENT_VARIABLE(result);

   return result;
}
//
// Compute the leading power terms in the incomplete Beta:
//
// (x^a)(y^b)/Beta(a,b) when normalised, and
// (x^a)(y^b) otherwise.
//
// Almost all of the error in the incomplete beta comes from this
// function: particularly when a and b are large. Computing large
// powers are *hard* though, and using logarithms just leads to
// horrendous cancellation errors.
//
// This version is generic, slow, and does not use the Lanczos approximation.
//
template <class T, class Policy>
T ibeta_power_terms(T a,
                        T b,
                        T x,
                        T y,
                        const boost::math::lanczos::undefined_lanczos&,
                        bool normalised,
                        const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(!normalised)
   {
      return pow(x, a) * pow(y, b);
   }

   T result= 0; // assignment here silences warnings later

   T c = a + b;

   // integration limits for the gamma functions:
   //T la = (std::max)(T(10), a);
   //T lb = (std::max)(T(10), b);
   //T lc = (std::max)(T(10), a+b);
   T la = a + 5;
   T lb = b + 5;
   T lc = a + b + 5;
   // gamma function partials:
   T sa = detail::lower_gamma_series(a, la, pol) / a;
   sa += detail::upper_gamma_fraction(a, la, ::boost::math::policies::get_epsilon<T, Policy>());
   T sb = detail::lower_gamma_series(b, lb, pol) / b;
   sb += detail::upper_gamma_fraction(b, lb, ::boost::math::policies::get_epsilon<T, Policy>());
   T sc = detail::lower_gamma_series(c, lc, pol) / c;
   sc += detail::upper_gamma_fraction(c, lc, ::boost::math::policies::get_epsilon<T, Policy>());
   // gamma function powers combined with incomplete beta powers:

   T b1 = (x * lc) / la;
   T b2 = (y * lc) / lb;
   T e1 = lc - la - lb;
   T lb1 = a * log(b1);
   T lb2 = b * log(b2);

   if((lb1 >= tools::log_max_value<T>())
      || (lb1 <= tools::log_min_value<T>())
      || (lb2 >= tools::log_max_value<T>())
      || (lb2 <= tools::log_min_value<T>())
      || (e1 >= tools::log_max_value<T>())
      || (e1 <= tools::log_min_value<T>())
      )
   {
      result = exp(lb1 + lb2 - e1);
   }
   else
   {
      T p1, p2;
      if((fabs(b1 - 1) * a < 10) && (a > 1))
         p1 = exp(a * boost::math::log1p((x * b - y * la) / la, pol));
      else
         p1 = pow(b1, a);
      if((fabs(b2 - 1) * b < 10) && (b > 1))
         p2 = exp(b * boost::math::log1p((y * a - x * lb) / lb, pol));
      else
         p2 = pow(b2, b);
      T p3 = exp(e1);
      result = p1 * p2 / p3;
   }
   // and combine with the remaining gamma function components:
   result /= sa * sb / sc;

   return result;
}
//
// Series approximation to the incomplete beta:
//
template <class T>
struct ibeta_series_t
{
   typedef T result_type;
   ibeta_series_t(T a_, T b_, T x_, T mult) : result(mult), x(x_), apn(a_), poch(1-b_), n(1) {}
   T operator()()
   {
      T r = result / apn;
      apn += 1;
      result *= poch * x / n;
      ++n;
      poch += 1;
      return r;
   }
private:
   T result, x, apn, poch;
   int n;
};

template <class T, class Lanczos, class Policy>
T ibeta_series(T a, T b, T x, T s0, const Lanczos&, bool normalised, T* p_derivative, T y, const Policy& pol)
{
   BOOST_MATH_STD_USING

   T result;

   BOOST_ASSERT((p_derivative == 0) || normalised);

   if(normalised)
   {
      T c = a + b;

      // incomplete beta power term, combined with the Lanczos approximation:
      T agh = a + Lanczos::g() - T(0.5);
      T bgh = b + Lanczos::g() - T(0.5);
      T cgh = c + Lanczos::g() - T(0.5);
      result = Lanczos::lanczos_sum_expG_scaled(c) / (Lanczos::lanczos_sum_expG_scaled(a) * Lanczos::lanczos_sum_expG_scaled(b));
      if(a * b < bgh * 10)
         result *= exp((b - 0.5f) * boost::math::log1p(a / bgh, pol));
      else
         result *= pow(cgh / bgh, b - 0.5f);
      result *= pow(x * cgh / agh, a);
      result *= sqrt(agh / boost::math::constants::e<T>());

      if(p_derivative)
      {
         *p_derivative = result * pow(y, b);
         BOOST_ASSERT(*p_derivative >= 0);
      }
   }
   else
   {
      // Non-normalised, just compute the power:
      result = pow(x, a);
   }
   if(result < tools::min_value<T>())
      return s0; // Safeguard: series can't cope with denorms.
   ibeta_series_t<T> s(a, b, x, result);
   boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
   result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter, s0);
   policies::check_series_iterations<T>("boost::math::ibeta<%1%>(%1%, %1%, %1%) in ibeta_series (with lanczos)", max_iter, pol);
   return result;
}
//
// Incomplete Beta series again, this time without Lanczos support:
//
template <class T, class Policy>
T ibeta_series(T a, T b, T x, T s0, const boost::math::lanczos::undefined_lanczos&, bool normalised, T* p_derivative, T y, const Policy& pol)
{
   BOOST_MATH_STD_USING

   T result;
   BOOST_ASSERT((p_derivative == 0) || normalised);

   if(normalised)
   {
      T c = a + b;

      // figure out integration limits for the gamma function:
      //T la = (std::max)(T(10), a);
      //T lb = (std::max)(T(10), b);
      //T lc = (std::max)(T(10), a+b);
      T la = a + 5;
      T lb = b + 5;
      T lc = a + b + 5;

      // calculate the gamma parts:
      T sa = detail::lower_gamma_series(a, la, pol) / a;
      sa += detail::upper_gamma_fraction(a, la, ::boost::math::policies::get_epsilon<T, Policy>());
      T sb = detail::lower_gamma_series(b, lb, pol) / b;
      sb += detail::upper_gamma_fraction(b, lb, ::boost::math::policies::get_epsilon<T, Policy>());
      T sc = detail::lower_gamma_series(c, lc, pol) / c;
      sc += detail::upper_gamma_fraction(c, lc, ::boost::math::policies::get_epsilon<T, Policy>());

      // and their combined power-terms:
      T b1 = (x * lc) / la;
      T b2 = lc/lb;
      T e1 = lc - la - lb;
      T lb1 = a * log(b1);
      T lb2 = b * log(b2);

      if((lb1 >= tools::log_max_value<T>())
         || (lb1 <= tools::log_min_value<T>())
         || (lb2 >= tools::log_max_value<T>())
         || (lb2 <= tools::log_min_value<T>())
         || (e1 >= tools::log_max_value<T>())
         || (e1 <= tools::log_min_value<T>()) )
      {
         T p = lb1 + lb2 - e1;
         result = exp(p);
      }
      else
      {
         result = pow(b1, a);
         if(a * b < lb * 10)
            result *= exp(b * boost::math::log1p(a / lb, pol));
         else
            result *= pow(b2, b);
         result /= exp(e1);
      }
      // and combine the results:
      result /= sa * sb / sc;

      if(p_derivative)
      {
         *p_derivative = result * pow(y, b);
         BOOST_ASSERT(*p_derivative >= 0);
      }
   }
   else
   {
      // Non-normalised, just compute the power:
      result = pow(x, a);
   }
   if(result < tools::min_value<T>())
      return s0; // Safeguard: series can't cope with denorms.
   ibeta_series_t<T> s(a, b, x, result);
   boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
   result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter, s0);
   policies::check_series_iterations<T>("boost::math::ibeta<%1%>(%1%, %1%, %1%) in ibeta_series (without lanczos)", max_iter, pol);
   return result;
}

//
// Continued fraction for the incomplete beta:
//
template <class T>
struct ibeta_fraction2_t
{
   typedef std::pair<T, T> result_type;

   ibeta_fraction2_t(T a_, T b_, T x_, T y_) : a(a_), b(b_), x(x_), y(y_), m(0) {}

   result_type operator()()
   {
      T aN = (a + m - 1) * (a + b + m - 1) * m * (b - m) * x * x;
      T denom = (a + 2 * m - 1);
      aN /= denom * denom;

      T bN = m;
      bN += (m * (b - m) * x) / (a + 2*m - 1);
      bN += ((a + m) * (a * y - b * x + 1 + m *(2 - x))) / (a + 2*m + 1);

      ++m;

      return std::make_pair(aN, bN);
   }

private:
   T a, b, x, y;
   int m;
};
//
// Evaluate the incomplete beta via the continued fraction representation:
//
template <class T, class Policy>
inline T ibeta_fraction2(T a, T b, T x, T y, const Policy& pol, bool normalised, T* p_derivative)
{
   typedef typename lanczos::lanczos<T, Policy>::type lanczos_type;
   BOOST_MATH_STD_USING
   T result = ibeta_power_terms(a, b, x, y, lanczos_type(), normalised, pol);
   if(p_derivative)
   {
      *p_derivative = result;
      BOOST_ASSERT(*p_derivative >= 0);
   }
   if(result == 0)
      return result;

   ibeta_fraction2_t<T> f(a, b, x, y);
   T fract = boost::math::tools::continued_fraction_b(f, boost::math::policies::get_epsilon<T, Policy>());
   BOOST_MATH_INSTRUMENT_VARIABLE(fract);
   BOOST_MATH_INSTRUMENT_VARIABLE(result);
   return result / fract;
}
//
// Computes the difference between ibeta(a,b,x) and ibeta(a+k,b,x):
//
template <class T, class Policy>
T ibeta_a_step(T a, T b, T x, T y, int k, const Policy& pol, bool normalised, T* p_derivative)
{
   typedef typename lanczos::lanczos<T, Policy>::type lanczos_type;

   BOOST_MATH_INSTRUMENT_VARIABLE(k);

   T prefix = ibeta_power_terms(a, b, x, y, lanczos_type(), normalised, pol);
   if(p_derivative)
   {
      *p_derivative = prefix;
      BOOST_ASSERT(*p_derivative >= 0);
   }
   prefix /= a;
   if(prefix == 0)
      return prefix;
   T sum = 1;
   T term = 1;
   // series summation from 0 to k-1:
   for(int i = 0; i < k-1; ++i)
   {
      term *= (a+b+i) * x / (a+i+1);
      sum += term;
   }
   prefix *= sum;

   return prefix;
}
//
// This function is only needed for the non-regular incomplete beta,
// it computes the delta in:
// beta(a,b,x) = prefix + delta * beta(a+k,b,x)
// it is currently only called for small k.
//
template <class T>
inline T rising_factorial_ratio(T a, T b, int k)
{
   // calculate:
   // (a)(a+1)(a+2)...(a+k-1)
   // _______________________
   // (b)(b+1)(b+2)...(b+k-1)

   // This is only called with small k, for large k
   // it is grossly inefficient, do not use outside it's
   // intended purpose!!!
   BOOST_MATH_INSTRUMENT_VARIABLE(k);
   if(k == 0)
      return 1;
   T result = 1;
   for(int i = 0; i < k; ++i)
      result *= (a+i) / (b+i);
   return result;
}
//
// Routine for a > 15, b < 1
//
// Begin by figuring out how large our table of Pn's should be,
// quoted accuracies are "guestimates" based on empiracal observation.
// Note that the table size should never exceed the size of our
// tables of factorials.
//
template <class T>
struct Pn_size
{
   // This is likely to be enough for ~35-50 digit accuracy
   // but it's hard to quantify exactly:
   BOOST_STATIC_CONSTANT(unsigned, value = 50);
   BOOST_STATIC_ASSERT(::boost::math::max_factorial<T>::value >= 100);
};
template <>
struct Pn_size<float>
{
   BOOST_STATIC_CONSTANT(unsigned, value = 15); // ~8-15 digit accuracy
   BOOST_STATIC_ASSERT(::boost::math::max_factorial<float>::value >= 30);
};
template <>
struct Pn_size<double>
{
   BOOST_STATIC_CONSTANT(unsigned, value = 30); // 16-20 digit accuracy
   BOOST_STATIC_ASSERT(::boost::math::max_factorial<double>::value >= 60);
};
template <>
struct Pn_size<long double>
{
   BOOST_STATIC_CONSTANT(unsigned, value = 50); // ~35-50 digit accuracy
   BOOST_STATIC_ASSERT(::boost::math::max_factorial<long double>::value >= 100);
};

template <class T, class Policy>
T beta_small_b_large_a_series(T a, T b, T x, T y, T s0, T mult, const Policy& pol, bool normalised)
{
   typedef typename lanczos::lanczos<T, Policy>::type lanczos_type;
   BOOST_MATH_STD_USING
   //
   // This is DiDonato and Morris's BGRAT routine, see Eq's 9 through 9.6.
   //
   // Some values we'll need later, these are Eq 9.1:
   //
   T bm1 = b - 1;
   T t = a + bm1 / 2;
   T lx, u;
   if(y < 0.35)
      lx = boost::math::log1p(-y, pol);
   else
      lx = log(x);
   u = -t * lx;
   // and from from 9.2:
   T prefix;
   T h = regularised_gamma_prefix(b, u, pol, lanczos_type());
   if(h <= tools::min_value<T>())
      return s0;
   if(normalised)
   {
      prefix = h / boost::math::tgamma_delta_ratio(a, b, pol);
      prefix /= pow(t, b);
   }
   else
   {
      prefix = full_igamma_prefix(b, u, pol) / pow(t, b);
   }
   prefix *= mult;
   //
   // now we need the quantity Pn, unfortunatately this is computed
   // recursively, and requires a full history of all the previous values
   // so no choice but to declare a big table and hope it's big enough...
   //
   T p[ ::boost::math::detail::Pn_size<T>::value ] = { 1 };  // see 9.3.
   //
   // Now an initial value for J, see 9.6:
   //
   T j = boost::math::gamma_q(b, u, pol) / h;
   //
   // Now we can start to pull things together and evaluate the sum in Eq 9:
   //
   T sum = s0 + prefix * j;  // Value at N = 0
   // some variables we'll need:
   unsigned tnp1 = 1; // 2*N+1
   T lx2 = lx / 2;
   lx2 *= lx2;
   T lxp = 1;
   T t4 = 4 * t * t;
   T b2n = b;

   for(unsigned n = 1; n < sizeof(p)/sizeof(p[0]); ++n)
   {
      /*
      // debugging code, enable this if you want to determine whether
      // the table of Pn's is large enough...
      //
      static int max_count = 2;
      if(n > max_count)
      {
         max_count = n;
         std::cerr << "Max iterations in BGRAT was " << n << std::endl;
      }
      */
      //
      // begin by evaluating the next Pn from Eq 9.4:
      //
      tnp1 += 2;
      p[n] = 0;
      T mbn = b - n;
      unsigned tmp1 = 3;
      for(unsigned m = 1; m < n; ++m)
      {
         mbn = m * b - n;
         p[n] += mbn * p[n-m] / boost::math::unchecked_factorial<T>(tmp1);
         tmp1 += 2;
      }
      p[n] /= n;
      p[n] += bm1 / boost::math::unchecked_factorial<T>(tnp1);
      //
      // Now we want Jn from Jn-1 using Eq 9.6:
      //
      j = (b2n * (b2n + 1) * j + (u + b2n + 1) * lxp) / t4;
      lxp *= lx2;
      b2n += 2;
      //
      // pull it together with Eq 9:
      //
      T r = prefix * p[n] * j;
      sum += r;
      if(r > 1)
      {
         if(fabs(r) < fabs(tools::epsilon<T>() * sum))
            break;
      }
      else
      {
         if(fabs(r / tools::epsilon<T>()) < fabs(sum))
            break;
      }
   }
   return sum;
} // template <class T, class Lanczos>T beta_small_b_large_a_series(T a, T b, T x, T y, T s0, T mult, const Lanczos& l, bool normalised)

//
// For integer arguments we can relate the incomplete beta to the
// complement of the binomial distribution cdf and use this finite sum.
//
template <class T>
inline T binomial_ccdf(T n, T k, T x, T y)
{
   BOOST_MATH_STD_USING // ADL of std names
   T result = pow(x, n);
   T term = result;
   for(unsigned i = itrunc(T(n - 1)); i > k; --i)
   {
      term *= ((i + 1) * y) / ((n - i) * x) ;
      result += term;
   }

   return result;
}

//
// The incomplete beta function implementation:
// This is just a big bunch of spagetti code to divide up the
// input range and select the right implementation method for
// each domain:
//
template <class T, class Policy>
T ibeta_imp(T a, T b, T x, const Policy& pol, bool inv, bool normalised, T* p_derivative)
{
   static const char* function = "boost::math::ibeta<%1%>(%1%, %1%, %1%)";
   typedef typename lanczos::lanczos<T, Policy>::type lanczos_type;
   BOOST_MATH_STD_USING // for ADL of std math functions.

   BOOST_MATH_INSTRUMENT_VARIABLE(a);
   BOOST_MATH_INSTRUMENT_VARIABLE(b);
   BOOST_MATH_INSTRUMENT_VARIABLE(x);
   BOOST_MATH_INSTRUMENT_VARIABLE(inv);
   BOOST_MATH_INSTRUMENT_VARIABLE(normalised);

   bool invert = inv;
   T fract;
   T y = 1 - x;

   BOOST_ASSERT((p_derivative == 0) || normalised);

   if(p_derivative)
      *p_derivative = -1; // value not set.

   if((x < 0) || (x > 1))
      policies::raise_domain_error<T>(function, "Parameter x outside the range [0,1] in the incomplete beta function (got x=%1%).", x, pol);

   if(normalised)
   {
      if(a < 0)
         policies::raise_domain_error<T>(function, "The argument a to the incomplete beta function must be >= zero (got a=%1%).", a, pol);
      if(b < 0)
         policies::raise_domain_error<T>(function, "The argument b to the incomplete beta function must be >= zero (got b=%1%).", b, pol);
      // extend to a few very special cases:
      if(a == 0)
      {
         if(b == 0)
            policies::raise_domain_error<T>(function, "The arguments a and b to the incomplete beta function cannot both be zero, with x=%1%.", x, pol);
         if(b > 0)
            return inv ? 0 : 1;
      }
      else if(b == 0)
      {
         if(a > 0)
            return inv ? 1 : 0;
      }
   }
   else
   {
      if(a <= 0)
         policies::raise_domain_error<T>(function, "The argument a to the incomplete beta function must be greater than zero (got a=%1%).", a, pol);
      if(b <= 0)
         policies::raise_domain_error<T>(function, "The argument b to the incomplete beta function must be greater than zero (got b=%1%).", b, pol);
   }

   if(x == 0)
   {
      if(p_derivative)
      {
         *p_derivative = (a == 1) ? (T)1 : (a < 1) ? T(tools::max_value<T>() / 2) : T(tools::min_value<T>() * 2);
      }
      return (invert ? (normalised ? T(1) : boost::math::beta(a, b, pol)) : T(0));
   }
   if(x == 1)
   {
      if(p_derivative)
      {
         *p_derivative = (b == 1) ? T(1) : (b < 1) ? T(tools::max_value<T>() / 2) : T(tools::min_value<T>() * 2);
      }
      return (invert == 0 ? (normalised ? 1 : boost::math::beta(a, b, pol)) : 0);
   }

   if((std::min)(a, b) <= 1)
   {
      if(x > 0.5)
      {
         std::swap(a, b);
         std::swap(x, y);
         invert = !invert;
         BOOST_MATH_INSTRUMENT_VARIABLE(invert);
      }
      if((std::max)(a, b) <= 1)
      {
         // Both a,b < 1:
         if((a >= (std::min)(T(0.2), b)) || (pow(x, a) <= 0.9))
         {
            if(!invert)
            {
               fract = ibeta_series(a, b, x, T(0), lanczos_type(), normalised, p_derivative, y, pol);
               BOOST_MATH_INSTRUMENT_VARIABLE(fract);
            }
            else
            {
               fract = -(normalised ? 1 : boost::math::beta(a, b, pol));
               invert = false;
               fract = -ibeta_series(a, b, x, fract, lanczos_type(), normalised, p_derivative, y, pol);
               BOOST_MATH_INSTRUMENT_VARIABLE(fract);
            }
         }
         else
         {
            std::swap(a, b);
            std::swap(x, y);
            invert = !invert;
            if(y >= 0.3)
            {
               if(!invert)
               {
                  fract = ibeta_series(a, b, x, T(0), lanczos_type(), normalised, p_derivative, y, pol);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
               else
               {
                  fract = -(normalised ? 1 : boost::math::beta(a, b, pol));
                  invert = false;
                  fract = -ibeta_series(a, b, x, fract, lanczos_type(), normalised, p_derivative, y, pol);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
            }
            else
            {
               // Sidestep on a, and then use the series representation:
               T prefix;
               if(!normalised)
               {
                  prefix = rising_factorial_ratio(T(a+b), a, 20);
               }
               else
               {
                  prefix = 1;
               }
               fract = ibeta_a_step(a, b, x, y, 20, pol, normalised, p_derivative);
               if(!invert)
               {
                  fract = beta_small_b_large_a_series(T(a + 20), b, x, y, fract, prefix, pol, normalised);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
               else
               {
                  fract -= (normalised ? 1 : boost::math::beta(a, b, pol));
                  invert = false;
                  fract = -beta_small_b_large_a_series(T(a + 20), b, x, y, fract, prefix, pol, normalised);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
            }
         }
      }
      else
      {
         // One of a, b < 1 only:
         if((b <= 1) || ((x < 0.1) && (pow(b * x, a) <= 0.7)))
         {
            if(!invert)
            {
               fract = ibeta_series(a, b, x, T(0), lanczos_type(), normalised, p_derivative, y, pol);
               BOOST_MATH_INSTRUMENT_VARIABLE(fract);
            }
            else
            {
               fract = -(normalised ? 1 : boost::math::beta(a, b, pol));
               invert = false;
               fract = -ibeta_series(a, b, x, fract, lanczos_type(), normalised, p_derivative, y, pol);
               BOOST_MATH_INSTRUMENT_VARIABLE(fract);
            }
         }
         else
         {
            std::swap(a, b);
            std::swap(x, y);
            invert = !invert;

            if(y >= 0.3)
            {
               if(!invert)
               {
                  fract = ibeta_series(a, b, x, T(0), lanczos_type(), normalised, p_derivative, y, pol);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
               else
               {
                  fract = -(normalised ? 1 : boost::math::beta(a, b, pol));
                  invert = false;
                  fract = -ibeta_series(a, b, x, fract, lanczos_type(), normalised, p_derivative, y, pol);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
            }
            else if(a >= 15)
            {
               if(!invert)
               {
                  fract = beta_small_b_large_a_series(a, b, x, y, T(0), T(1), pol, normalised);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
               else
               {
                  fract = -(normalised ? 1 : boost::math::beta(a, b, pol));
                  invert = false;
                  fract = -beta_small_b_large_a_series(a, b, x, y, fract, T(1), pol, normalised);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
            }
            else
            {
               // Sidestep to improve errors:
               T prefix;
               if(!normalised)
               {
                  prefix = rising_factorial_ratio(T(a+b), a, 20);
               }
               else
               {
                  prefix = 1;
               }
               fract = ibeta_a_step(a, b, x, y, 20, pol, normalised, p_derivative);
               BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               if(!invert)
               {
                  fract = beta_small_b_large_a_series(T(a + 20), b, x, y, fract, prefix, pol, normalised);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
               else
               {
                  fract -= (normalised ? 1 : boost::math::beta(a, b, pol));
                  invert = false;
                  fract = -beta_small_b_large_a_series(T(a + 20), b, x, y, fract, prefix, pol, normalised);
                  BOOST_MATH_INSTRUMENT_VARIABLE(fract);
               }
            }
         }
      }
   }
   else
   {
      // Both a,b >= 1:
      T lambda;
      if(a < b)
      {
         lambda = a - (a + b) * x;
      }
      else
      {
         lambda = (a + b) * y - b;
      }
      if(lambda < 0)
      {
         std::swap(a, b);
         std::swap(x, y);
         invert = !invert;
         BOOST_MATH_INSTRUMENT_VARIABLE(invert);
      }

      if(b < 40)
      {
         if((floor(a) == a) && (floor(b) == b) && (a < (std::numeric_limits<int>::max)() - 100))
         {
            // relate to the binomial distribution and use a finite sum:
            T k = a - 1;
            T n = b + k;
            fract = binomial_ccdf(n, k, x, y);
            if(!normalised)
               fract *= boost::math::beta(a, b, pol);
            BOOST_MATH_INSTRUMENT_VARIABLE(fract);
         }
         else if(b * x <= 0.7)
         {
            if(!invert)
            {
               fract = ibeta_series(a, b, x, T(0), lanczos_type(), normalised, p_derivative, y, pol);
               BOOST_MATH_INSTRUMENT_VARIABLE(fract);
            }
            else
            {
               fract = -(normalised ? 1 : boost::math::beta(a, b, pol));
               invert = false;
               fract = -ibeta_series(a, b, x, fract, lanczos_type(), normalised, p_derivative, y, pol);
               BOOST_MATH_INSTRUMENT_VARIABLE(fract);
            }
         }
         else if(a > 15)
         {
            // sidestep so we can use the series representation:
            int n = itrunc(T(floor(b)), pol);
            if(n == b)
               --n;
            T bbar = b - n;
            T prefix;
            if(!normalised)
            {
               prefix = rising_factorial_ratio(T(a+bbar), bbar, n);
            }
            else
            {
               prefix = 1;
            }
            fract = ibeta_a_step(bbar, a, y, x, n, pol, normalised, static_cast<T*>(0));
            fract = beta_small_b_large_a_series(a,  bbar, x, y, fract, T(1), pol, normalised);
            fract /= prefix;
            BOOST_MATH_INSTRUMENT_VARIABLE(fract);
         }
         else if(normalised)
         {
            // the formula here for the non-normalised case is tricky to figure
            // out (for me!!), and requires two pochhammer calculations rather
            // than one, so leave it for now....
            int n = itrunc(T(floor(b)), pol);
            T bbar = b - n;
            if(bbar <= 0)
            {
               --n;
               bbar += 1;
            }
            fract = ibeta_a_step(bbar, a, y, x, n, pol, normalised, static_cast<T*>(0));
            fract += ibeta_a_step(a, bbar, x, y, 20, pol, normalised, static_cast<T*>(0));
            if(invert)
               fract -= (normalised ? 1 : boost::math::beta(a, b, pol));
            //fract = ibeta_series(a+20, bbar, x, fract, l, normalised, p_derivative, y);
            fract = beta_small_b_large_a_series(T(a+20),  bbar, x, y, fract, T(1), pol, normalised);
            if(invert)
            {
               fract = -fract;
               invert = false;
            }
            BOOST_MATH_INSTRUMENT_VARIABLE(fract);
         }
         else
         {
            fract = ibeta_fraction2(a, b, x, y, pol, normalised, p_derivative);
            BOOST_MATH_INSTRUMENT_VARIABLE(fract);
         }
      }
      else
      {
         fract = ibeta_fraction2(a, b, x, y, pol, normalised, p_derivative);
         BOOST_MATH_INSTRUMENT_VARIABLE(fract);
      }
   }
   if(p_derivative)
   {
      if(*p_derivative < 0)
      {
         *p_derivative = ibeta_power_terms(a, b, x, y, lanczos_type(), true, pol);
      }
      T div = y * x;

      if(*p_derivative != 0)
      {
         if((tools::max_value<T>() * div < *p_derivative))
         {
            // overflow, return an arbitarily large value:
            *p_derivative = tools::max_value<T>() / 2;
         }
         else
         {
            *p_derivative /= div;
         }
      }
   }
   return invert ? (normalised ? 1 : boost::math::beta(a, b, pol)) - fract : fract;
} // template <class T, class Lanczos>T ibeta_imp(T a, T b, T x, const Lanczos& l, bool inv, bool normalised)

template <class T, class Policy>
inline T ibeta_imp(T a, T b, T x, const Policy& pol, bool inv, bool normalised)
{
   return ibeta_imp(a, b, x, pol, inv, normalised, static_cast<T*>(0));
}

template <class T, class Policy>
T ibeta_derivative_imp(T a, T b, T x, const Policy& pol)
{
   static const char* function = "ibeta_derivative<%1%>(%1%,%1%,%1%)";
   //
   // start with the usual error checks:
   //
   if(a <= 0)
      policies::raise_domain_error<T>(function, "The argument a to the incomplete beta function must be greater than zero (got a=%1%).", a, pol);
   if(b <= 0)
      policies::raise_domain_error<T>(function, "The argument b to the incomplete beta function must be greater than zero (got b=%1%).", b, pol);
   if((x < 0) || (x > 1))
      policies::raise_domain_error<T>(function, "Parameter x outside the range [0,1] in the incomplete beta function (got x=%1%).", x, pol);
   //
   // Now the corner cases:
   //
   if(x == 0)
   {
      return (a > 1) ? 0 :
         (a == 1) ? 1 / boost::math::beta(a, b, pol) : policies::raise_overflow_error<T>(function, 0, pol);
   }
   else if(x == 1)
   {
      return (b > 1) ? 0 :
         (b == 1) ? 1 / boost::math::beta(a, b, pol) : policies::raise_overflow_error<T>(function, 0, pol);
   }
   //
   // Now the regular cases:
   //
   typedef typename lanczos::lanczos<T, Policy>::type lanczos_type;
   T f1 = ibeta_power_terms<T>(a, b, x, 1 - x, lanczos_type(), true, pol);
   T y = (1 - x) * x;

   if(f1 == 0)
      return 0;

   if((tools::max_value<T>() * y < f1))
   {
      // overflow:
      return policies::raise_overflow_error<T>(function, 0, pol);
   }

   f1 /= y;

   return f1;
}
//
// Some forwarding functions that dis-ambiguate the third argument type:
//
template <class RT1, class RT2, class Policy>
inline typename tools::promote_args<RT1, RT2>::type
   beta(RT1 a, RT2 b, const Policy&, const mpl::true_*)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<RT1, RT2>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename lanczos::lanczos<value_type, Policy>::type evaluation_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(detail::beta_imp(static_cast<value_type>(a), static_cast<value_type>(b), evaluation_type(), forwarding_policy()), "boost::math::beta<%1%>(%1%,%1%)");
}
template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   beta(RT1 a, RT2 b, RT3 x, const mpl::false_*)
{
   return boost::math::beta(a, b, x, policies::policy<>());
}
} // namespace detail

//
// The actual function entry-points now follow, these just figure out
// which Lanczos approximation to use
// and forward to the implementation functions:
//
template <class RT1, class RT2, class A>
inline typename tools::promote_args<RT1, RT2, A>::type
   beta(RT1 a, RT2 b, A arg)
{
   typedef typename policies::is_policy<A>::type tag;
   return boost::math::detail::beta(a, b, arg, static_cast<tag*>(0));
}

template <class RT1, class RT2>
inline typename tools::promote_args<RT1, RT2>::type
   beta(RT1 a, RT2 b)
{
   return boost::math::beta(a, b, policies::policy<>());
}

template <class RT1, class RT2, class RT3, class Policy>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   beta(RT1 a, RT2 b, RT3 x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename lanczos::lanczos<value_type, Policy>::type evaluation_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(detail::ibeta_imp(static_cast<value_type>(a), static_cast<value_type>(b), static_cast<value_type>(x), forwarding_policy(), false, false), "boost::math::beta<%1%>(%1%,%1%,%1%)");
}

template <class RT1, class RT2, class RT3, class Policy>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   betac(RT1 a, RT2 b, RT3 x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename lanczos::lanczos<value_type, Policy>::type evaluation_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(detail::ibeta_imp(static_cast<value_type>(a), static_cast<value_type>(b), static_cast<value_type>(x), forwarding_policy(), true, false), "boost::math::betac<%1%>(%1%,%1%,%1%)");
}
template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   betac(RT1 a, RT2 b, RT3 x)
{
   return boost::math::betac(a, b, x, policies::policy<>());
}

template <class RT1, class RT2, class RT3, class Policy>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   ibeta(RT1 a, RT2 b, RT3 x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, 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;

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(detail::ibeta_imp(static_cast<value_type>(a), static_cast<value_type>(b), static_cast<value_type>(x), forwarding_policy(), false, true), "boost::math::ibeta<%1%>(%1%,%1%,%1%)");
}
template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   ibeta(RT1 a, RT2 b, RT3 x)
{
   return boost::math::ibeta(a, b, x, policies::policy<>());
}

template <class RT1, class RT2, class RT3, class Policy>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   ibetac(RT1 a, RT2 b, RT3 x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, 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;

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(detail::ibeta_imp(static_cast<value_type>(a), static_cast<value_type>(b), static_cast<value_type>(x), forwarding_policy(), true, true), "boost::math::ibetac<%1%>(%1%,%1%,%1%)");
}
template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   ibetac(RT1 a, RT2 b, RT3 x)
{
   return boost::math::ibetac(a, b, x, policies::policy<>());
}

template <class RT1, class RT2, class RT3, class Policy>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   ibeta_derivative(RT1 a, RT2 b, RT3 x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, 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;

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(detail::ibeta_derivative_imp(static_cast<value_type>(a), static_cast<value_type>(b), static_cast<value_type>(x), forwarding_policy()), "boost::math::ibeta_derivative<%1%>(%1%,%1%,%1%)");
}
template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   ibeta_derivative(RT1 a, RT2 b, RT3 x)
{
   return boost::math::ibeta_derivative(a, b, x, policies::policy<>());
}

} // namespace math
} // namespace boost

#include <boost/math/special_functions/detail/ibeta_inverse.hpp>
#include <boost/math/special_functions/detail/ibeta_inv_ab.hpp>

#endif // BOOST_MATH_SPECIAL_BETA_HPP


//  Copyright (c) 2006 Xiaogang Zhang
//  Copyright (c) 2006 John Maddock
//  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)
//
//  History:
//  XZ wrote the original of this file as part of the Google
//  Summer of Code 2006.  JM modified it to fit into the
//  Boost.Math conceptual framework better, and to ensure
//  that the code continues to work no matter how many digits
//  type T has.

#ifndef BOOST_MATH_ELLINT_1_HPP
#define BOOST_MATH_ELLINT_1_HPP

#include <boost/math/special_functions/ellint_rf.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/tools/workaround.hpp>

// Elliptic integrals (complete and incomplete) of the first kind
// Carlson, Numerische Mathematik, vol 33, 1 (1979)

namespace boost { namespace math {

template <class T1, class T2, class Policy>
typename tools::promote_args<T1, T2>::type ellint_1(T1 k, T2 phi, const Policy& pol);

namespace detail{

template <typename T, typename Policy>
T ellint_k_imp(T k, const Policy& pol);

// Elliptic integral (Legendre form) of the first kind
template <typename T, typename Policy>
T ellint_f_imp(T phi, T k, const Policy& pol)
{
    BOOST_MATH_STD_USING
    using namespace boost::math::tools;
    using namespace boost::math::constants;

    static const char* function = "boost::math::ellint_f<%1%>(%1%,%1%)";
    BOOST_MATH_INSTRUMENT_VARIABLE(phi);
    BOOST_MATH_INSTRUMENT_VARIABLE(k);
    BOOST_MATH_INSTRUMENT_VARIABLE(function);

    if (abs(k) > 1)
    {
       return policies::raise_domain_error<T>(function,
            "Got k = %1%, function requires |k| <= 1", k, pol);
    }

    bool invert = false;
    if(phi < 0)
    {
       BOOST_MATH_INSTRUMENT_VARIABLE(phi);
       phi = fabs(phi);
       invert = true;
    }

    T result;

    if(phi >= tools::max_value<T>())
    {
       // Need to handle infinity as a special case:
       result = policies::raise_overflow_error<T>(function, 0, pol);
       BOOST_MATH_INSTRUMENT_VARIABLE(result);
    }
    else if(phi > 1 / tools::epsilon<T>())
    {
       // Phi is so large that phi%pi is necessarily zero (or garbage),
       // just return the second part of the duplication formula:
       result = 2 * phi * ellint_k_imp(k, pol) / constants::pi<T>();
       BOOST_MATH_INSTRUMENT_VARIABLE(result);
    }
    else
    {
       // Carlson's algorithm works only for |phi| <= pi/2,
       // use the integrand's periodicity to normalize phi
       //
       // Xiaogang's original code used a cast to long long here
       // but that fails if T has more digits than a long long,
       // so rewritten to use fmod instead:
       //
       BOOST_MATH_INSTRUMENT_CODE("pi/2 = " << constants::pi<T>() / 2);
       T rphi = boost::math::tools::fmod_workaround(phi, constants::pi<T>() / 2);
       BOOST_MATH_INSTRUMENT_VARIABLE(rphi);
       T m = 2 * (phi - rphi) / constants::pi<T>();
       BOOST_MATH_INSTRUMENT_VARIABLE(m);
       int s = 1;
       if(boost::math::tools::fmod_workaround(m, T(2)) > 0.5)
       {
          m += 1;
          s = -1;
          rphi = constants::pi<T>() / 2 - rphi;
          BOOST_MATH_INSTRUMENT_VARIABLE(rphi);
       }
       T sinp = sin(rphi);
       T cosp = cos(rphi);
       BOOST_MATH_INSTRUMENT_VARIABLE(sinp);
       BOOST_MATH_INSTRUMENT_VARIABLE(cosp);
       result = s * sinp * ellint_rf_imp(cosp * cosp, 1 - k * k * sinp * sinp, T(1), pol);
       BOOST_MATH_INSTRUMENT_VARIABLE(result);
       if(m != 0)
       {
          result += m * ellint_k_imp(k, pol);
          BOOST_MATH_INSTRUMENT_VARIABLE(result);
       }
    }
    return invert ? -result : result;
}

// Complete elliptic integral (Legendre form) of the first kind
template <typename T, typename Policy>
T ellint_k_imp(T k, const Policy& pol)
{
    BOOST_MATH_STD_USING
    using namespace boost::math::tools;

    static const char* function = "boost::math::ellint_k<%1%>(%1%)";

    if (abs(k) > 1)
    {
       return policies::raise_domain_error<T>(function,
            "Got k = %1%, function requires |k| <= 1", k, pol);
    }
    if (abs(k) == 1)
    {
       return policies::raise_overflow_error<T>(function, 0, pol);
    }

    T x = 0;
    T y = 1 - k * k;
    T z = 1;
    T value = ellint_rf_imp(x, y, z, pol);

    return value;
}

template <typename T, typename Policy>
inline typename tools::promote_args<T>::type ellint_1(T k, const Policy& pol, const mpl::true_&)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::ellint_k_imp(static_cast<value_type>(k), pol), "boost::math::ellint_1<%1%>(%1%)");
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type ellint_1(T1 k, T2 phi, const mpl::false_&)
{
   return boost::math::ellint_1(k, phi, policies::policy<>());
}

}

// Complete elliptic integral (Legendre form) of the first kind
template <typename T>
inline typename tools::promote_args<T>::type ellint_1(T k)
{
   return ellint_1(k, policies::policy<>());
}

// Elliptic integral (Legendre form) of the first kind
template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type ellint_1(T1 k, T2 phi, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::ellint_f_imp(static_cast<value_type>(phi), static_cast<value_type>(k), pol), "boost::math::ellint_1<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type ellint_1(T1 k, T2 phi)
{
   typedef typename policies::is_policy<T2>::type tag_type;
   return detail::ellint_1(k, phi, tag_type());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_1_HPP


// Copyright (C) 2021 Icey Chiu
// Using to translate the camera reference system to the arbitray one.

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>

#include <dirent.h>
#include <sys/types.h>

#include <opencv2/opencv.hpp>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

class Calibration
{
public:
    Calibration ( const std::string& setting_dir ) {
        /* load parameters */
        cv::FileStorage fs ( setting_dir, cv::FileStorage::READ );

        // camera params
        fx = fs["camera.fx"];
        fy = fs["camera.fy"];
        cx = fs["camera.cx"];
        cy = fs["camera.cy"];

        k1 = fs["camera.k1"];
        k2 = fs["camera.k2"];
        p1 = fs["camera.p1"];
        p2 = fs["camera.p2"];
        k3 = fs["camera.k3"];

        K = ( cv::Mat_<float> ( 3, 3 ) << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0 );
        dist = ( cv::Mat_<float> ( 1, 5 ) << k1, k2, p1, p2, k3 );
        eK << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0;

        // target params
        target_cols = fs["target.cols"];
        target_rows = fs["target.rows"];
        target_col_grid_size = fs["target.col_grid_size"];
        target_row_grid_size = fs["target.row_grid_size"];
        target_offset = fs["target.offset"];
    }

    bool calibrate ( cv::Mat& image ) {
        /* process the img with one time calibration */

        // extract corners
        std::vector<cv::Point2f> corners, undist_corners;
        bool found = cv::findChessboardCorners ( image, cv::Size ( 3, 7 ), corners );

        if ( found ) {

            // refine corners
            cv::cornerSubPix ( image, corners, cv::Size ( 11, 8 ), cv::Size ( 6 , 3 ),
                               cv::TermCriteria ( cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1 ) );

            // show corners
            cv::drawChessboardCorners ( image, cv::Size ( target_cols, target_rows ), corners, found );
            cv::resize ( image, image, cv::Size ( image.cols / 4,  image.rows/4 ) );
            cv::imshow ( "detect corners", image );
            cv::waitKey ( 1 );

            // undistort
            cv::undistortPoints ( corners, undist_corners, K, dist, cv::Mat(), K );
        }// if found target
        else {
            return false;
        }

        // get Tct, translation between camera and the target
        // get 3d mappoints refer to the target coordinate
        std::vector<cv::Point3f> pts;
        for ( int i = 0; i < target_rows; ++i )
            for ( int j = 0; j < target_cols; ++j ) {
// 				int yj = ( target_cols - j - 1);
// 				pts.push_back ( cv::Point3f ( yj*target_col_grid_size, i*target_row_grid_size, 0 ) );
				pts.push_back ( cv::Point3f ( i*target_row_grid_size, j*target_col_grid_size, 0 ) );
            }

        cv::Vec3d rvec, tvec;
        bool solve_ok =  cv::solvePnP ( pts, undist_corners, K, cv::Mat(), rvec, tvec );
        if ( !solve_ok ) {
            return false;
        }

        // convert to homo T
        Eigen::Matrix3d R;
        cv::Mat cvR;
        cv::Rodrigues ( rvec, cvR );
        R <<  cvR.at<double> ( 0, 0 ), cvR.at<double> ( 0, 1 ), cvR.at<double> ( 0, 2 ),
          cvR.at<double> ( 1, 0 ), cvR.at<double> ( 1, 1 ), cvR.at<double> ( 1, 2 ),
          cvR.at<double> ( 2, 0 ), cvR.at<double> ( 2, 1 ), cvR.at<double> ( 2, 2 );

        Eigen::Vector3d t;
        t << tvec[0], tvec[1], tvec[2];

        Eigen::Vector3d r1 = R.block ( 0, 0, 3, 1 );
        Eigen::Vector3d r2 = R.block ( 0, 1, 3, 1 );

        Eigen::Vector3d tb;
        tb << 0.0, 0.0, target_offset;

        Eigen::Matrix3d RT;
        RT.block ( 0,0, 3, 1 ) = r1;
        RT.block ( 0,1, 3, 1 ) = r2;
        RT.block ( 0,2, 3, 1 ) = R * tb + t;

        H = eK * RT; // homography
        std::cout << std::endl << H << std::endl << std::endl << std::endl;
        // Save result.
        return true;
    }

    void saveResult ( const std::string& result_dir ) {
        cv::FileStorage fs ( result_dir, cv::FileStorage::WRITE );

        // Save homography Matrix
        cv::Mat cvH = ( cv::Mat_<double> ( 3, 3 ) << H ( 0,0 ), H ( 0,1 ), H ( 0,2 ), H ( 1,0 ), H ( 1,1 ), H ( 1,2 ), H ( 2,0 ), H ( 2,1 ), H ( 2,2 ) );
        fs << "homograph_matrix" << cvH;
        fs.release();
    }

private:

    float fx;
    float fy;
    float cx;
    float cy;

    float k1;
    float k2;
    float p1;
    float p2;
    float k3;
    cv::Mat K;
    cv::Mat dist;
    Eigen::Matrix3d eK;

    int target_cols;
    int target_rows;
    float target_col_grid_size;
    float target_row_grid_size;
    float target_offset;

    Eigen::Matrix3d H;
};// class Calbration

int main ( int argc, char **argv )
{
    //if ( argc != 2 ) {
    //    std::cout << "Please input: setting file, homograph_matrix.yaml\n";
    //    return -1;
    //}

    /* load parameters */
    const std::string cfg_dir = "/home/icey/workspace/Aruco/intrinsic_calibrationfile.yml";
    const std::string calibration_dir = "/home/icey/workspace/Aruco/extrinsic_calibrationfile.yml";
    cv::Mat img = cv::imread("/home/icey/图片/calibration/666.jpg", 2 | 4);
    // Init Calbration class.
    Calibration calibration ( cfg_dir );

    // Calibration.
    calibration.calibrate ( img );
    calibration.saveResult ( calibration_dir );

    cv::waitKey( 0 );

    std::cout << "Complete" << std::endl;
    return 0;
}

/*
cv::Mat getRTMatrix ( const cv::Mat &R_,const cv::Mat &T_ ,int forceType ) {
   cv::Mat M;
   cv::Mat R,T;
   R_.copyTo ( R );
   T_.copyTo ( T );
   if ( R.type() ==CV_64F ) {
       assert ( T.type() ==CV_64F );
       cv::Mat Matrix=cv::Mat::eye ( 4,4,CV_64FC1 );
       cv::Mat R33=cv::Mat ( Matrix,cv::Rect ( 0,0,3,3 ) );
       if ( R.total() ==3 ) {
           cv::Rodrigues ( R,R33 );
       } else if ( R.total() ==9 ) {
           cv::Mat R64;
           R.convertTo ( R64,CV_64F );
           R.copyTo ( R33 );
       }
       for ( int i=0; i<3; i++ )
           Matrix.at<double> ( i,3 ) =T.ptr<double> ( 0 ) [i];
       M=Matrix;
   } else if ( R.depth() ==CV_32F ) {
       cv::Mat Matrix=cv::Mat::eye ( 4,4,CV_32FC1 );
       cv::Mat R33=cv::Mat ( Matrix,cv::Rect ( 0,0,3,3 ) );
       if ( R.total() ==3 ) {
           cv::Rodrigues ( R,R33 );
       } else if ( R.total() ==9 ) {
           cv::Mat R32;
           R.convertTo ( R32,CV_32F );
           R.copyTo ( R33 );
       }
       for ( int i=0; i<3; i++ )
           Matrix.at<float> ( i,3 ) =T.ptr<float> ( 0 ) [i];
       M=Matrix;
   }
   if ( forceType==-1 ) return M;
   else {
       cv::Mat MTyped;
       M.convertTo ( MTyped,forceType );
       return MTyped;
   }
}
*/


#include <iostream>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <Eigen/SparseCholesky>
#include <Eigen/Dense>
#include <chrono>

typedef Eigen::SparseMatrix<double> SpMat;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorXd;

#include "mesh_reader_eigen.hpp"
#include "constants_sparse.hpp"
#include "newton_raphson_sparse.hpp"

// Compile command:
// g++ -Wall -std=c++14 eigen_sparse.cpp -o sparse.o -I/usr/local/include/eigen3
// Run command:
// ./sparse.o filename T(in C) eta_u(in %) eta_v(in %) 0/1 (1 for quasi-newton, 0 for newton)

// u = Cu = concentration O2, v = Cv = concentration CO2

SpMat block(SpMat A,SpMat B,SpMat C, SpMat D)
{
  SpMat R(2*A.rows(),2*A.rows());
  int n = A.rows();
  for (int k=0; k<A.outerSize(); ++k)
  {
    for (SparseMatrix<double>::InnerIterator it(A,k); it; ++it)
    {
      R.insert(it.row(),it.col())  = it.value();
    }
  }
  for (int k=0; k<B.outerSize(); ++k)
  {
    for (SparseMatrix<double>::InnerIterator it(B,k); it; ++it)
    {
      R.insert(it.row(),n+it.col())  = it.value();
    }
  }
  for (int k=0; k<C.outerSize(); ++k)
  {
    for (SparseMatrix<double>::InnerIterator it(C,k); it; ++it)
    {
      R.insert(n+it.row(),it.col())  = it.value();
    }
  }
  for (int k=0; k<D.outerSize(); ++k)
  {
    for (SparseMatrix<double>::InnerIterator it(D,k); it; ++it)
    {
      R.insert(n+it.row(),n+it.col())  = it.value();
    }
  }
  return R;
}

class F
{
  /* Returns tuple containing two functors, one for the original expression and one for the Jacobian. */

  private:
    SpMat& Au_;
    SpMat& Av_;
    SpMat& B_;
    SpMat& C_;
    SpMat& D_;

  public:
    F(SpMat& Au, SpMat& Av,SpMat& B,SpMat& C,SpMat& D)
    :Au_(Au), Av_(Av),B_(B), C_(C),D_(D)
    {
    }

    VectorXd operator()(VectorXd& x)
    {
      int n = Au_.rows();
      VectorXd u = x.head(n).sparseView();
      VectorXd v = x.tail(n).sparseView();
      VectorXd func(2*n);
      VectorXd f1 = Au_*u + B_*Ru(u,v) + hu*(C_*u - D_*uamb);
      VectorXd f2 = Av_*v - B_*Rv(u,v) + hv*(C_*v - D_*vamb);
      func << f1,f2;
      return func;
    }
};

class J
{
  /* Returns tuple containing two functors, one for the original expression and one for the Jacobian. */

  private:
    SpMat& Au_;
    SpMat& Av_;
    SpMat& B_;
    SpMat& C_;
    SpMat& D_;
  public:
    J(SpMat& Au, SpMat& Av,SpMat& B,SpMat& C,SpMat& D)
    :Au_(Au), Av_(Av),B_(B), C_(C),D_(D)
    {
    }

    SpMat operator()(VectorXd& x)
    {
      int n = Au_.rows();
      VectorXd u = x.head(n);
      VectorXd v = x.tail(n);
      SpMat func(2*n,2*n);
      func = block(Au_ + B_*dRudu(u,v)+hu*C_,B_*dRudv(u,v),-B_*dRvdu(u,v),Av_ - (B_*dRvdv(u,v)) + hv*C_);
      return func;
    }
};

int main(int argc, char *argv[])
{
  std::cout << std::endl;
  std::cout << "Setting constants ..." << std::endl;
  int quasi = atoi(argv[5]);
  setConstants(atof(argv[2])+273.15,atof(argv[3])/100,atof(argv[4])/100);

  auto t1 = std::chrono::high_resolution_clock::now();
  std::string file_name = argv[1];
  std::string location = "../mesh/"+ file_name +".1";
  MatrixXd vertices = mesh::read_vertices(location+".node");
  MatrixXd triangles = mesh::read_triangles(vertices,location+".ele");
  MatrixXi boundaries = mesh::read_boundaries(vertices,location+".poly");
  auto t2 = std::chrono::high_resolution_clock::now();

  std::cout << std::endl;
  std::cout << "Input data successfully read:" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  std::cout << "Number of vertices :" << vertices.rows() << std::endl;
  std::cout << "Number of triangles:" << triangles.rows() <<std::endl;
  t1 = std::chrono::high_resolution_clock::now();

  /* Calculate coefficient matrix B related to the respiration kinetics,
  part of right hand side in system of nonlinear equations */

  SpMat B_init(vertices.rows(), vertices.rows());
  for (unsigned t = 0; t < triangles.rows(); ++t)
  {
    int a = triangles(t, 0);
    int b = triangles(t, 1);
    int c = triangles(t, 2);
    double area = triangles(t, 3);
    B_init.coeffRef(a, a) += area*(6.*vertices(a, 0) + 2.*vertices(b, 0) + 2.*vertices(c, 0));
    B_init.coeffRef(std::min(a,b),std::max(a,b)) += area*(2.*vertices(a, 0) + 2.*vertices(b, 0) + vertices(c, 0));
    B_init.coeffRef(std::min(c, a),std::max(c, a)) += area*(2.*vertices(a, 0) + vertices(b, 0) + 2.*vertices(c, 0));
    B_init.coeffRef(b, b) += area*(2.*vertices(a, 0) + 6.*vertices(b, 0) + 2.*vertices(c, 0));
    B_init.coeffRef(std::min(b, c),std::max(b, c)) += area*(vertices(a, 0) + 2.*vertices(b, 0) + 2.*vertices(c, 0));
    B_init.coeffRef(c, c) += area*(2.*vertices(a, 0) + 2.*vertices(b, 0) + 6.*vertices(c, 0));
  }
  B_init *= (1./60.);
  SpMat B = B_init.selfadjointView<Eigen::Upper>();
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout << "B matrix successfully assembled" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Calculate the first part of the stiffness matrix in the lefthand
  side of the the nonlinear system, A */

  t1 = std::chrono::high_resolution_clock::now();
  SpMat A_U_init(vertices.rows(), vertices.rows());
  SpMat A_V_init(vertices.rows(), vertices.rows());
  Eigen::Matrix<double,3,2> G;
  Eigen::Matrix<double,3,3> GGT_U;
  Eigen::Matrix<double,3,3> GGT_V;
  Eigen::Matrix<double,2,2> I_U;
  Eigen::Matrix<double,2,2> I_V;
  I_U(0,0) = DU_R;
  I_U(1,0) = 0;
  I_U(0,1) = 0;
  I_U(1,1) = DU_Z;
  I_V(0,0) = DV_R;
  I_V(1,1) = DV_Z;
  I_V(1,0) = 0;
  I_V(0,1) = 0;
  for (unsigned t = 0; t < triangles.rows(); ++t)
  {
    int a = triangles(t, 0);
    int b = triangles(t, 1);
    int c = triangles(t, 2);
    double area = triangles(t, 3);
    G(0, 0) = (vertices(b, 1) - vertices(c, 1));
    G(1, 0) = (vertices(c, 1) - vertices(a, 1));
    G(2, 0) = (vertices(a, 1) - vertices(b, 1));
    G(0, 1) = (vertices(c, 0) - vertices(b, 0));
    G(1, 1) = (vertices(a, 0) - vertices(c, 0));
    G(2, 1) = (vertices(b, 0) - vertices(a, 0));
    GGT_U = (1/(2*area))*((vertices(a, 0)+vertices(b, 0)+vertices(c, 0))/6)*(G*I_U*G.transpose());
    GGT_V = (1/(2*area))*((vertices(a, 0)+vertices(b, 0)+vertices(c, 0))/6)*(G*I_V*G.transpose());
    #ifdef DEBUG
    std::cout<<"GGT_U" << std::endl << GGT_U << std::endl;
    std::cout<<"GGT_V" << std::endl << GGT_V << std::endl;
    #endif
    A_U_init.coeffRef(a, a) += GGT_U(0, 0);
    A_U_init.coeffRef(std::min(b, a),std::max(b, a)) += GGT_U(1, 0);
    A_U_init.coeffRef(std::min(c, a),std::max(c, a)) += GGT_U(2, 0);
    A_U_init.coeffRef(b, b) += GGT_U(1, 1);
    A_U_init.coeffRef(std::min(b, c),std::max(b,c)) += GGT_U(1, 2);
    A_U_init.coeffRef(c, c) += GGT_U(2, 2);
    A_V_init.coeffRef(a, a) += GGT_V(0, 0);
    A_V_init.coeffRef(std::min(b, a),std::max(b, a)) += GGT_V(1, 0);
    A_V_init.coeffRef(std::min(c, a),std::max(c, a)) += GGT_V(2, 0);
    A_V_init.coeffRef(b, b) += GGT_V(1, 1);
    A_V_init.coeffRef(std::min(b, c),std::max(b,c)) += GGT_V(1, 2);
    A_V_init.coeffRef(c, c) += GGT_V(2, 2);
  }
  SpMat A_U = A_U_init.selfadjointView<Eigen::Upper>();
  SpMat A_V = A_V_init.selfadjointView<Eigen::Upper>();
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout << "A matrices assembled." << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Calculate the second part of the stiffness matrix (C) and the
  second part of the righthand side (D) */

  SpMat C(vertices.rows(), vertices.rows());
  SpMat D(vertices.rows(),1);
  for (unsigned b = 0; b < boundaries.rows(); ++b)
  {
    double len = sqrt(pow(vertices(boundaries(b, 0), 0) - vertices(boundaries(b, 1), 0), 2) +
      pow(vertices(boundaries(b, 0), 1) - vertices(boundaries(b, 1), 1), 2));
    C.coeffRef(boundaries(b, 0), boundaries(b, 0)) += len*(vertices(boundaries(b, 0), 0)/4 + vertices(boundaries(b, 1), 0)/12);
    C.coeffRef(boundaries(b, 0), boundaries(b, 1)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/12);
    C.coeffRef(boundaries(b, 1), boundaries(b, 0)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/12);
    C.coeffRef(boundaries(b, 1), boundaries(b, 1)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/4);
    D.coeffRef(boundaries(b,0),0) += len*(vertices(boundaries(b,0),0)/3.+vertices(boundaries(b,1),0)/6.);
    D.coeffRef(boundaries(b,1),0) += len*(vertices(boundaries(b,0),0)/6.+vertices(boundaries(b,1),0)/3.);
  }

  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout << "C matrix and D vector assembled." << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Solve linearized problem with ambient concentrations as input_field
  to obtain starting concentration values, then solve the nonlinear
  system with Newton-Raphson */

  t1 = std::chrono::high_resolution_clock::now();

  t1 = std::chrono::high_resolution_clock::now();
  F F_funct(A_U, A_V, B, C, D);
  J J_funct(A_U, A_V, B, C, D);
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout << "Functors are created" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  t1 = std::chrono::high_resolution_clock::now();
  SparseLU<SpMat> solver;
  VectorXd guess(vertices.rows()*2);
  SpMat T1 = A_U+(Vmu/Kmu)*B+hu*C;
  SpMat T2 = A_V+hv*C;

  solver.analyzePattern(T1);
  solver.factorize(T1);
  VectorXd u_0 = solver.solve(hu*D*uamb);
  solver.analyzePattern(T2);
  solver.factorize(T2);
  VectorXd v_0 = solver.solve(rq*(Vmu/Kmu)*B*u_0+hv*vamb*D);

  guess << u_0,v_0;
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout<< "Initial guess calculated" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  #ifdef DEBUG
  std::cout << "A_U =" << std::endl;
  std::cout << MatrixXd(A_U) << std::endl;
  std::cout << "A_V =" << std::endl;
  std::cout << MatrixXd(A_V) << std::endl;
  std::cout << "B =" << std::endl;
  std::cout << MatrixXd(B) << std::endl;
  std::cout << "C =" << std::endl;
  std::cout << MatrixXd(C) << std::endl;
  std::cout << "D =" << std::endl;
  std::cout << MatrixXd(D) << std::endl;
  std::cout << "u_0" << std::endl;
  std::cout << u_0 << std::endl;
  std::cout << "v_0" << std::endl;
  std::cout << v_0 <<std::endl;
  std::cout << "Initial function value = " << std::endl;
  std::cout << F_funct(guess) <<std::endl;
  std::cout << "Initial Jacobian = " << std::endl;
  std::cout << J_funct(guess);
  #endif

  std::cout << std::endl;
  std::cout<< "Calculating nonlinear system solution ..." << std::endl;
  t1 = std::chrono::high_resolution_clock::now();
  if (quasi==0)
  {
    newton_raphson(F_funct,J_funct,guess,pow(10,-17));
  }
  else
  {
    quasi_newton_raphson(F_funct,J_funct,guess,pow(10,-17));
  }
  t2 = std::chrono::high_resolution_clock::now();

  std::cout<< "Numerical solution nonlinear system calculated" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Write out the result for python matplotlib code */

  t1 = std::chrono::high_resolution_clock::now();
  int n = vertices.rows();
  VectorXd u = guess.head(n);
  VectorXd v = guess.tail(n);
  mesh::write_result(u,v);
  t2 = std::chrono::high_resolution_clock::now();

  std::cout << std::endl;
  std::cout<< "Results for u and v written out" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  std::cout << std::endl;

  return 0;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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.  See the license for more details.
*/

#include <ql/exercise.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/experimental/models/fxoptionhelper.hpp>
#include <ql/time/calendars/nullcalendar.hpp>

#include <boost/make_shared.hpp>

namespace QuantLib {

FxOptionHelper::FxOptionHelper(
    const Period &maturity, const Calendar &calendar, const Real strike,
    const Handle<Quote> fxSpot, const Handle<Quote> volatility,
    const Handle<YieldTermStructure> &domesticYield,
    const Handle<YieldTermStructure> &foreignYield,
    CalibrationHelper::CalibrationErrorType errorType)
    : CalibrationHelper(volatility, domesticYield, errorType),
      hasMaturity_(true), maturity_(maturity), calendar_(calendar),
      strike_(strike), fxSpot_(fxSpot), foreignYield_(foreignYield) {
    registerWith(fxSpot_);
    registerWith(foreignYield_);
}

FxOptionHelper::FxOptionHelper(
    const Date &exerciseDate, const Real strike, const Handle<Quote> fxSpot,
    const Handle<Quote> volatility,
    const Handle<YieldTermStructure> &domesticYield,
    const Handle<YieldTermStructure> &foreignYield,
    CalibrationHelper::CalibrationErrorType errorType)
    : CalibrationHelper(volatility, domesticYield, errorType),
      hasMaturity_(false), exerciseDate_(exerciseDate), strike_(strike),
      fxSpot_(fxSpot), foreignYield_(foreignYield) {
    registerWith(fxSpot_);
    registerWith(foreignYield_);
}

void FxOptionHelper::performCalculations() const {
    if (hasMaturity_)
        exerciseDate_ =
            calendar_.advance(termStructure_->referenceDate(), maturity_);
    tau_ = termStructure_->timeFromReference(exerciseDate_);
    atm_ = fxSpot_->value() * foreignYield_->discount(tau_) /
           termStructure_->discount(tau_);
    effStrike_ = strike_;
    if(effStrike_ == Null<Real>())
        effStrike_ = atm_;
    type_ = effStrike_ >= atm_ ? Option::Call : Option::Put;
    boost::shared_ptr<StrikedTypePayoff> payoff(
        new PlainVanillaPayoff(type_, effStrike_));
    boost::shared_ptr<Exercise> exercise =
        boost::make_shared<EuropeanExercise>(exerciseDate_);
    option_ =
        boost::shared_ptr<VanillaOption>(new VanillaOption(payoff, exercise));
    CalibrationHelper::performCalculations();
}

Real FxOptionHelper::modelValue() const {
    calculate();
    option_->setPricingEngine(engine_);
    return option_->NPV();
}

Real FxOptionHelper::blackPrice(Real volatility) const {
    calculate();
    const Real stdDev = volatility * std::sqrt(tau_);
    return blackFormula(type_, strike_, atm_, stdDev, termStructure_->discount(tau_));
}

} // namespace QuantLib


#include <SBGATPolyhedronGravityModel.hpp>
#include <SBGATPolyhedronGravityModelUQ.hpp>
#include <SBGATObjWriter.hpp>

#include <vtkCleanPolyData.h>
#include <vtkOBJReader.h>

#include <json.hpp>
#include <boost/progress.hpp>

#include <armadillo>

int main(){

	std::ifstream i("input_file.json");
	nlohmann::json input_data;
	i >> input_data;

	arma::arma_rng::set_seed(0);

	std::string PATH_SHAPE = input_data["PATH_SHAPE"];
	double CORRELATION_DISTANCE =  input_data["CORRELATION_DISTANCE"];

	double ERROR_STANDARD_DEV  = input_data["ERROR_STANDARD_DEV"];
	double DENSITY  = input_data["DENSITY"];
	double PERIOD_SD  = input_data["PERIOD_SD"];
	double PERIOD  = input_data["PERIOD"];

	bool UNIT_IN_METERS  = input_data["UNIT_IN_METERS"];
	bool HOLD_MASS_CONSTANT  = input_data["HOLD_MASS_CONSTANT"];

	int N_MONTE_CARLO = input_data["N_MONTE_CARLO"];
	std::vector<int> COV_REGION_CENTERS = input_data["COV_REGION_CENTERS"];

	std::string OUTPUT_DIR = input_data["OUTPUT_DIR"];
	std::string UNCERTAINTY_TYPE = input_data["UNCERTAINTY_TYPE"];

	std::vector<unsigned int > FACETS_TO_INVESTIGATE = input_data["FACETS_TO_INVESTIGATE"];

	std::cout << "- Path to shape: " << PATH_SHAPE << std::endl;
	std::cout << "- Uncertainty type: " << UNCERTAINTY_TYPE << std::endl;
	std::cout << "- Standard deviation on point coordinates (m) : " << ERROR_STANDARD_DEV << std::endl;
	std::cout << "- Correlation distance (m) : " << CORRELATION_DISTANCE << std::endl;
	std::cout << "- Standard deviation on rotation period (s) : " << PERIOD_SD << std::endl;
	std::cout << "- Density (kg/m^3) : " << DENSITY << std::endl;
	std::cout << "- Rotation period (s) : " << PERIOD << std::endl;
	std::cout << "- Covariance region centers:\n" ;
	for(auto center : COV_REGION_CENTERS){
		std::cout << "\t" << center << std::endl;
	}
	std::cout << "- Facets to investigate:\n" ;
	for(auto facet : FACETS_TO_INVESTIGATE){
		std::cout << "\t" << facet << std::endl;
	}

	// Reading
	vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
	reader -> SetFileName(PATH_SHAPE.c_str());
	reader -> Update();

	// An instance of SBGATPolyhedronGravityModel is created to evaluate the PGM of
	// the considered polytdata
	vtkSmartPointer<SBGATPolyhedronGravityModel> pgm_filter = vtkSmartPointer<SBGATPolyhedronGravityModel>::New();
	pgm_filter -> SetInputConnection(reader -> GetOutputPort());
	pgm_filter -> SetDensity(DENSITY);

	if(UNIT_IN_METERS){
		pgm_filter -> SetScaleMeters();
	} else{
		pgm_filter -> SetScaleKiloMeters();
	}

	std::cout << "Building pgm ...\n";
	pgm_filter -> SetOmega(2 * arma::datum::pi / PERIOD * arma::vec({0,0,1}));
	pgm_filter -> Update();

	// An instance of SBGATPolyhedronGravityModelUQ is created to perform
	// uncertainty quantification from the PGM associated to the shape
	SBGATPolyhedronGravityModelUQ pgm_uq;
	pgm_uq.SetModel(pgm_filter);
	pgm_uq.SetPeriodErrorStandardDeviation(PERIOD_SD);
	pgm_uq.PrecomputeMassPropertiesPartials();

	// Saving baseline slices
	pgm_uq.TakeAndSaveSlice(0,OUTPUT_DIR + "baseline_slice_x.txt",0);
	pgm_uq.TakeAndSaveSlice(1,OUTPUT_DIR + "baseline_slice_y.txt",0);
	pgm_uq.TakeAndSaveSlice(2,OUTPUT_DIR + "baseline_slice_z.txt",0);

	std::cout << "Populating shape covariance ...\n";

	// Populate the shape vertices covariance

	if (UNCERTAINTY_TYPE == "radial"){
		for (auto region_center : COV_REGION_CENTERS){
			pgm_uq.AddRadialUncertaintyRegionToCovariance(region_center,ERROR_STANDARD_DEV,CORRELATION_DISTANCE);

		}
	}
	else if (UNCERTAINTY_TYPE == "normal"){
		for (auto region_center : COV_REGION_CENTERS){

			pgm_uq.AddRadialUncertaintyRegionToCovariance(region_center,ERROR_STANDARD_DEV,CORRELATION_DISTANCE);

		}
	}
	else if (UNCERTAINTY_TYPE == "global"){
		pgm_uq.ComputeVerticesCovarianceGlobal(ERROR_STANDARD_DEV,CORRELATION_DISTANCE);
	}
	else{
		throw(std::runtime_error("Got unknown uncertainty direction type: " + UNCERTAINTY_TYPE));
	}

	arma::mat P_CC = pgm_uq.GetVerticesCovariance();
	arma::mat C_CC = pgm_uq.GetCovarianceSquareRoot();

	P_CC.save(OUTPUT_DIR + "full_covariance.txt",arma::raw_ascii);
	C_CC.save(OUTPUT_DIR + "full_covariance_sqrt.txt",arma::raw_ascii);

	// Regularizing the covariance
	int regularized_eigen_values = pgm_uq.RegularizeCovariance();

	std::cout << regularized_eigen_values << " eigenvalues were regularized\n";
	C_CC = pgm_uq.GetCovarianceSquareRoot();

	P_CC = pgm_uq.GetVerticesCovariance();
	P_CC.save(OUTPUT_DIR + "full_covariance_regularized.txt",arma::raw_ascii);
	C_CC.save(OUTPUT_DIR + "full_covariance_sqrt_regularized.txt",arma::raw_ascii);

	regularized_eigen_values = pgm_uq.RegularizeCovariance();

	std::cout << regularized_eigen_values << " eigenvalues were regularized\n";

	std::cout << "Maximum absolute error in covariance square root: " << arma::abs(P_CC - C_CC * C_CC.t()).max() << std::endl;

	std::cout << "Saving non-zero partition of shape covariance ...\n";

	// Analytical UQ
	std::vector<double> analytical_variances_slopes;

	std::cout << "Computing analytical uncertainties ... ";
	auto start = std::chrono::system_clock::now();

	pgm_uq.GetVarianceSlopes(analytical_variances_slopes,FACETS_TO_INVESTIGATE,HOLD_MASS_CONSTANT);

	auto end = std::chrono::system_clock::now();

	std::chrono::duration<double> elapsed_seconds = end-start;
	std::cout << "Done computing analytical uncertainties in " << elapsed_seconds.count() << " s\n";

	// Running a Monte Carlo to compare against
	std::vector<arma::vec> deviations;
	std::vector<double> period_errors;
	std::vector<double> densities;

	std::vector < std::vector<double> > all_slopes;

	std::cout << "Running MC ... ";

	start = std::chrono::system_clock::now();
	SBGATPolyhedronGravityModelUQ::RunMCUQSlopes(PATH_SHAPE,
		DENSITY,
		pgm_filter -> GetOmega() ,
		UNIT_IN_METERS,
		HOLD_MASS_CONSTANT,
		C_CC,
		PERIOD_SD,
		N_MONTE_CARLO,
		FACETS_TO_INVESTIGATE,
		OUTPUT_DIR,
		std::min(30,N_MONTE_CARLO),
		deviations,
		densities,
		period_errors,
		all_slopes);

	end = std::chrono::system_clock::now();

	elapsed_seconds = end-start;

	std::cout << "Done running MC in " << elapsed_seconds.count() << " s\n";

	std::vector<double> mc_variances_slopes(FACETS_TO_INVESTIGATE.size());

	std::cout << "Computing MC dispersions...\n";

	for (int e = 0; e < mc_variances_slopes.size(); ++e){
		arma::vec slopes_mc(N_MONTE_CARLO);
		for (int sample = 0; sample < N_MONTE_CARLO; ++sample){
			slopes_mc(sample) = all_slopes[sample][e];
		}
		mc_variances_slopes[e] = arma::var(slopes_mc);

		slopes_mc.save(OUTPUT_DIR + "/slope_distribution_facet_" + std::to_string(FACETS_TO_INVESTIGATE[e]) + ".txt",arma::raw_ascii);
	}

	std::cout << "\t After " << N_MONTE_CARLO << " MC outcomes:\n";

	for (int e = 0; e < FACETS_TO_INVESTIGATE.size(); ++e){
		std::cout << "\tAt facet " << FACETS_TO_INVESTIGATE[e] << "\n";
		std::cout << "\t\tSlope (rad): " << pgm_filter -> GetSlope(FACETS_TO_INVESTIGATE[e]) << std::endl;
		std::cout << "\t\tMC variance in slope: " << mc_variances_slopes[e] << std::endl;
		std::cout << "\t\tAnalytical variance in slope: " << analytical_variances_slopes[e] << std::endl;
		std::cout << "\t\tError (%): " << (mc_variances_slopes[e] - analytical_variances_slopes[e])/analytical_variances_slopes[e] * 100 << std::endl;

	}

	return 0;
}


/*
 * @file SURF_FlannMatcher
 * @brief SURF detector + descriptor + FLANN Matcher
 * @author A. Huaman
 */

#include <fstream>
#include <iostream>
#include <stdio.h>
#include <iomanip>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>

#include <Eigen/Dense>
#include <opengv/amm.hpp>
#include <opengv/relative_pose/methods.hpp>
#include <opengv/relative_pose/NoncentralRelativeAdapter.hpp>
#include <opengv/optimization_tools/objective_function_tools/GlobalPnPFunctionInfo.hpp>
#include <opengv/optimization_tools/objective_function_tools/SquaredFunctionNoIterationsInfo.hpp>
#include <opengv/optimization_tools/solver_tools/SolverToolsNoncentralRelativePose.hpp>

#include "opencv2/core.hpp"
#include "opencv2/features2d.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/xfeatures2d.hpp"

#include "random_generators.hpp"
#include "experiment_helpers.hpp"
#include "time_measurement.hpp"

using namespace std;
using namespace cv;
using namespace cv::xfeatures2d;
//void readme();
/*
 * @function main
 * @brief Main function
 */
void returnCorrespondences(const Mat & img1, const Mat & img2, std::vector<KeyPoint> & k1, std::vector<KeyPoint> & k2);

int main( int argc, char** argv )
{
  if( argc != 5 )
  {
    std::cout << "argc: " << argc << std::endl;
    //readme();
    return -1;
  }
  Mat img_left_1 = imread( argv[1], IMREAD_GRAYSCALE );
  Mat img_left_2 = imread( argv[2], IMREAD_GRAYSCALE );

  Mat img_right_1 = imread( argv[3], IMREAD_GRAYSCALE );
  Mat img_right_2 = imread( argv[4], IMREAD_GRAYSCALE );
  if( !img_left_1.data || !img_left_2.data || !img_right_1.data || !img_right_2.data)
  {
    std::cout<< " --(!) Error reading images " << std::endl;
    return -1;
  }
  std::vector<KeyPoint> k1_left;
  std::vector<KeyPoint> k2_left;
  returnCorrespondences(img_left_1, img_left_2, k1_left, k2_left);

  std::vector<KeyPoint> k1_right;
  std::vector<KeyPoint> k2_right;
  returnCorrespondences(img_right_1, img_right_2, k1_right, k2_right);

  /*std::cout << "Left: " << std::endl;
  for(int i=0; i < k1_left.size(); ++i){
    std::cout << "(" << k1_left[i].pt.x << ", " << k1_left[i].pt.y << ")  (" << k2_left[i].pt.x << ", " << k2_left[i].pt.y << ")" <<  std::endl;
  }

  std::cout << "Right: " << std::endl;
  for(int i=0; i < k1_right.size(); ++i){
    std::cout << "(" << k1_right[i].pt.x << ", " << k1_right[i].pt.y << ")  (" << k2_right[i].pt.x << ", " << k2_right[i].pt.y << ")" <<  std::endl;
    }*/

  std::cout << "Cal matrices: " << std::endl;
  Eigen::Matrix3d K_left = Eigen::Matrix3d::Identity(3,3);
  K_left(0,0) = 9.597910e+02; K_left(0,1) =  0.000000e+00; K_left(0,2) = 6.960217e+02;
  K_left(1,0) = 0.000000e+00; K_left(1,1) =  9.569251e+02; K_left(1,2) = 2.241806e+02;
  K_left(2,0) = 0.000000e+00; K_left(2,1) =  0.000000e+00; K_left(2,2) = 1.000000e+00;

  Eigen::Matrix3d K_right = Eigen::Matrix3d::Identity(3,3);
  K_right(0,0) = 9.037596e+02; K_right(0,1) =  0.000000e+00; K_right(0,2) = 6.957519e+02;
  K_right(1,0) = 0.000000e+00; K_right(1,1) =  9.019653e+02; K_right(1,2) = 2.242509e+02;
  K_right(2,0) = 0.000000e+00; K_right(2,1) =  0.000000e+00; K_right(2,2) = 1.000000e+00;

  //We have the calibration matrices and the vector points now it is necessary to build the bearing vectors and convert them in the reference frame of the centra

  //Pose for viewpoint left
  opengv::translation_t position_left = Eigen::MatrixXd::Zero(3,1);
  position_left(0,0) = -0.27;
  opengv::rotation_t rotation_left    = Eigen::MatrixXd::Identity(3,3);

  //Pose for viewpoint right
  opengv::translation_t position_right = Eigen::MatrixXd::Zero(3,1);
  position_right(0,0) = 0.27;
  opengv::rotation_t rotation_right    = Eigen::MatrixXd::Identity(3,3);

  //Create a fake central camera

  opengv::bearingVectors_t bearingVectors1;
  opengv::bearingVectors_t bearingVectors2;
  Eigen::MatrixXd aux = Eigen::MatrixXd::Zero(3,1);
  for(int j = 0; j < k1_left.size(); ++j){
    aux(0,0) = k1_left[j].pt.x; aux(1,0) = k1_left[j].pt.y; aux(2,0) = 1;
    aux = K_left.inverse() * aux;
    bearingVectors1.push_back(aux);
  }

  for(int j = 0; j < k1_right.size(); ++j){
    aux(0,0) = k1_right[j].pt.x; aux(1,0) = k1_right[j].pt.y; aux(2,0) = 1;
    aux = K_right.inverse() * aux;
    bearingVectors1.push_back(aux);
  }

  for(int j = 0; j < k2_left.size(); ++j){
    aux(0,0) = k2_left[j].pt.x; aux(1,0) = k2_left[j].pt.y; aux(2,0) = 1;
    aux = K_left.inverse() * aux;
    bearingVectors2.push_back(aux);
  }

  for(int j = 0; j < k2_right.size(); ++j){
    aux(0,0) = k2_right[j].pt.x; aux(1,0) = k2_right[j].pt.y; aux(2,0) = 1;
    aux = K_right.inverse() * aux;
    bearingVectors2.push_back(aux);
  }
  std::cout << "bearing vector1: " << bearingVectors1.size() << std::endl;
  std::cout << "bearing vector2: " << bearingVectors2.size() << std::endl;
  //To use the methods it will be necessary to build the central adapter
  /*opengv::bearingVectors_t v1;
  opengv::bearingVectors_t v2;
  int cols = (int)good_matches.size();
  for( int i = 0; i < cols; i++ )
    {
      std::cout << "Start cycle: " << i << std::endl;
      Eigen::MatrixXd v_left = Eigen::MatrixXd::Zero(3,1);
      v_left(0,0) = k1[i].pt.x;
      v_left(1,0) = k1[i].pt.y;
      v_left(2,0) = 1;
      //std::cout << "v1: " << std::endl << v1 << std::endl;

      Eigen::MatrixXd v_right = Eigen::MatrixXd::Zero(3,1);
      v_right(0,0) = k2[i].pt.x;
      v_right(1,0) = k2[i].pt.y;
      v_right(2,0) = 1;
      //std::cout << "v2: " << std::endl << v2 <<std::endl;

      v_left = K_left.inverse() * v_left;
      double norm_left = v_left.norm();
      v_left = v_left / norm_left;

      v_right = K_right.inverse() * v_right;
      double norm_right = v_right.norm();
      v_right = v_right /norm_right;

      v1.push_back(v_left);
      v2.push_back(v_right);
    }
  opengv::rotations_t camRotations;
  opengv::rotation_t rotation1 = Eigen::MatrixXd::Identity(3,3);
  opengv::rotation_t rotation2 = Eigen::MatrixXd::Identity(3,3);
  opengv::translation_t position1 = Eigen::MatrixXd::Zero(3,1);
  opengv::translation_t position2 = Eigen::MatrixXd::Zero(3,1);
  position2(0,0) = 0.54;
  camRotations.push_back(rotation1);
  camRotations.push_back(rotation2);

  opengv::translations_t camOffsets;
  camOffsets.push_back(position1);
  camOffsets.push_back(position2);
  std::vector<int> camCorrespondences1;
  std::vector<int> camCorrespondences2;
  int camCorrespondence = 0;
  for(int i = 0; i < good_matches.size(); ++i){
    camCorrespondences1.push_back(camCorrespondence);
    camCorrespondences2.push_back(camCorrespondence++);
  }

  //Extract the relative pose
  opengv::translation_t position; opengv::rotation_t rotation;
  opengv::extractRelativePose(position1, position2, rotation1, rotation2, position, rotation, false );

  //create non-central relative adapter
  opengv::relative_pose::NoncentralRelativeAdapter adapter(
						       v1,
						       v2,
						       camCorrespondences1,
						       camCorrespondences2,
						       camOffsets,
						       camRotations,
						       position,
						       rotation);
  return 0;*/
}

/*
 * @function readme
 */
/*void readme(){
  std::cout << " Usage: ./SURF_FlannMatcher <img1> <img2>" << std::endl;
  }*/

void returnCorrespondences(const Mat & img1, const Mat & img2, std::vector<KeyPoint> & k1, std::vector<KeyPoint> & k2){
  //-- Step 1: Detect the keypoints using SURF Detector, compute the descriptors
  int minHessian = 400;
  Ptr<SURF> detector = SURF::create();
  detector->setHessianThreshold(minHessian);

  std::vector<KeyPoint> keypoints_1, keypoints_2;
  Mat descriptors_1, descriptors_2;
  detector->detectAndCompute( img1, Mat(), keypoints_1, descriptors_1 );
  detector->detectAndCompute( img2, Mat(), keypoints_2, descriptors_2 );
  //-- Step 2: Matching descriptor vectors using FLANN matcher
  FlannBasedMatcher matcher;
  std::vector< DMatch > matches;
  matcher.match( descriptors_1, descriptors_2, matches );
  double max_dist = 0; double min_dist = 100;
  //-- Quick calculation of max and min distances between keypoints
  for( int i = 0; i < descriptors_1.rows; i++ )
  {
    double dist = matches[i].distance;
    if( dist < min_dist ) min_dist = dist;
    if( dist > max_dist ) max_dist = dist;
  }
  printf("-- Max dist : %f \n", max_dist );
  printf("-- Min dist : %f \n", min_dist );
  //-- Draw only "good" matches (i.e. whose distance is less than 2*min_dist,
  //-- or a small arbitary value ( 0.02 ) in the event that min_dist is very
  //-- small)
  //-- PS.- radiusMatch can also be used here.
  std::vector< DMatch > good_matches;
  for( int i = 0; i < descriptors_1.rows; i++ )
  {
    if( matches[i].distance <= max(2*min_dist, 0.02) ){
      good_matches.push_back( matches[i]);
    }
  }
  //-- Draw only "good" matches
  k1.clear();
  k2.clear();
  for( int i = 0; i < (int)good_matches.size(); i++ )
  {
    //printf( "-- Good Match [%d] Keypoint 1: %d  -- Keypoint 2: %d  \n", i, good_matches[i].queryIdx, good_matches[i].trainIdx );
    std::cout << ( "-- Good Match [%d] Keypoint 1: %d  -- Keypoint 2: %d  \n", i, good_matches[i].queryIdx, good_matches[i].trainIdx ) << std::endl;
    k1.push_back(keypoints_1[good_matches[i].queryIdx]);
    k2.push_back(keypoints_2[good_matches[i].trainIdx]);
  }

}


#include <iostream>
#include <math.h>
#include <fstream>
#include <time.h>
#include <boost/range/numeric.hpp>
#include <boost/range/adaptor/map.hpp>
#include <unordered_map>
#include <algorithm>
#include <vector>
#include <iterator>
#define h  800
#define w  800
#define output_file "output.raw"

unsigned char* readImage(const char *image, int height, int width, int bpp)
{

	unsigned char* Imagedata = new unsigned char [height*width*bpp];

	FILE *file;
	if (!(file=fopen(image,"rb"))) {
		std::cout << "Cannot open file: " << image << std::endl;
		exit(1);
	}
	fread(Imagedata, sizeof(unsigned char), height*width*bpp, file);
	fclose(file);

	return Imagedata;
}

void saveImage(const char* image, unsigned char* Imagedata, int height, int width, int bpp)
{
	FILE *file;

	if (!(file=fopen(image,"wb"))) {
		std::cout << "Cannot open file: " << image << std::endl;
		exit(1);
	}
	fwrite(Imagedata, sizeof(unsigned char), height*width*bpp, file);
	fclose(file);

	std::cout << "Filed Saved Succesfully" << std::endl;
}

int getEuclideanDist(int pixel, int mean)
{
	return abs(pixel - mean);
}

int getMeanOfCluster(std::unordered_map<int, int> cluster)
{
	std::unordered_map<int, int>::iterator itr;
	int sum(0);

	// for(itr = cluster.begin(); itr != cluster.end(); itr++)
	// {
	// 	sum += itr->second;
	// 	// std::cout << sum << std::endl;
	// }

	sum = boost::accumulate(cluster | boost::adaptors::map_values, 0);

	return sum/cluster.size();
}

unsigned char* getFinalOutput(unsigned char* Image1DOutput, std::unordered_map<int, int> cluster,
	int height, int width, int mean)
{
	std::unordered_map<int, int>::iterator itr;

	for(itr = cluster.begin(); itr != cluster.end(); itr++)
	{
		Image1DOutput[itr->first] = mean;
	}

	return Image1DOutput;
}

unsigned char* getKMeansClustered(unsigned char* Image1D, int height, int width)
{
	int iters(30);
	int mean1(0), mean2(85), mean3(170), mean4(255), pixel;

	unsigned char* Image1DOutput = new unsigned char[height*width];
	std::unordered_map<int, int> cluster1, cluster2, cluster3, cluster4;
	std::vector<int> minDist(4, 0);

	while(iters>0)
	{
		cluster1.clear();
		cluster2.clear();
		cluster3.clear();
		cluster4.clear();
		for (int i = 0; i < height; ++i)
		{
			for (int j = 0; j < width; ++j)
			{
				pixel = Image1D[i*width + j];

				minDist[0] = (int(abs(pixel - mean1)));

				minDist[1] = (int(abs(pixel - mean2)));

				minDist[2] = (int(abs(pixel - mean3)));

				minDist[3] = (int(abs(pixel - mean4)));

				int min_pos = std::distance(minDist.begin(), std::min_element(minDist.begin(),minDist.end()));

				switch(min_pos)
				{
					case 0:
					cluster1[i*width + j] = pixel;
					break;

					case 1:
					cluster2[i*width + j] = pixel;
					break;

					case 2:
					cluster3[i*width + j] = pixel;
					break;

					case 3:
					cluster4[i*width + j] = pixel;
					break;
				}
			}
		}

		iters--;
		mean1 = getMeanOfCluster(cluster1);
		mean2 = getMeanOfCluster(cluster2);
		mean3 = getMeanOfCluster(cluster3);
		mean4 = getMeanOfCluster(cluster4);

		// std::cout << "Means are: " << mean1 << ", " << mean2 << ", " << mean3 << ", " << mean4 << ", " << "; Iter: " << iters << std::endl;
	}

	Image1DOutput = getFinalOutput(Image1DOutput, cluster1, h, w, mean1);
	Image1DOutput = getFinalOutput(Image1DOutput, cluster2, h, w, mean2);
	Image1DOutput = getFinalOutput(Image1DOutput, cluster3, h, w, mean3);
	Image1DOutput = getFinalOutput(Image1DOutput, cluster4, h, w, mean4);

	return Image1DOutput;

}

int main(int argc, char const *argv[])
{
	struct timespec start, stop;
	unsigned char* Image1D = readImage(argv[1], h, w, 1);
	std::cout << "Image read" << std::endl;

	if(clock_gettime(CLOCK_REALTIME, &start) == -1)
		perror("clock gettime");

	unsigned char* Image1DOutput = getKMeansClustered(Image1D, h, w);

	if(clock_gettime( CLOCK_REALTIME, &stop) == -1 )
		perror("clock gettime");

	double time = (stop.tv_sec - start.tv_sec)+ (double)(stop.tv_nsec - start.tv_nsec)/1e9;
	std::cout << "time taken: " << time << std::endl;

	saveImage(output_file, Image1DOutput, h, w, 1);
	delete [] Image1D;
	delete [] Image1DOutput;

	return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_FUNCTIONS_SCALAR_SINC_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_FUNCTIONS_SCALAR_SINC_HPP_INCLUDED
#include <nt2/trigonometric/functions/sinc.hpp>
#include <nt2/include/functions/scalar/sin.hpp>
#include <nt2/include/constants/one.hpp>
#include <boost/simd/sdk/config.hpp>

#if !defined(BOOST_SIMD_NO_INFINITIES)
#include <nt2/include/functions/scalar/is_inf.hpp>
#include <nt2/include/constants/zero.hpp>
#endif

#if !defined(BOOST_SIMD_NO_DENORMALS)
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/constants/eps.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( sinc_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      #if !defined(BOOST_SIMD_NO_INFINITIES)
      if(nt2::is_inf(a0)) return nt2::Zero<result_type>();
      #endif

      #if !defined(BOOST_SIMD_NO_DENORMALS)
      return (nt2::abs(a0) < nt2::Eps<result_type>()) ? nt2::One<result_type>()
                                                      : nt2::sin(a0)/a0;
      #else
      return a0 ? nt2::sin(a0)/a0 : nt2::One<result_type>();
      #endif
    }
  };
} }

#endif