///////////////////////////////////////////////////////////////////////////////
//  Copyright 2014 Anton Bikineev
//  Copyright 2014 Christopher Kormanyos
//  Copyright 2014 John Maddock
//  Copyright 2014 Paul Bristow
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_HYPERGEOMETRIC_1F1_HPP
#define BOOST_MATH_HYPERGEOMETRIC_1F1_HPP

#include <boost/config.hpp>

#if defined(BOOST_NO_CXX11_AUTO_DECLARATIONS) || defined(BOOST_NO_CXX11_LAMBDAS) || defined(BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX)
# error "hypergeometric_1F1 requires a C++11 compiler"
#endif

#include <boost/math/policies/policy.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/detail/hypergeometric_series.hpp>
#include <boost/math/special_functions/detail/hypergeometric_asym.hpp>
#include <boost/math/special_functions/detail/hypergeometric_rational.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_recurrence.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_by_ratios.hpp>
#include <boost/math/special_functions/detail/hypergeometric_pade.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_bessel.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_scaled_series.hpp>
#include <boost/math/special_functions/detail/hypergeometric_pFq_checked_series.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_addition_theorems_on_z.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_large_abz.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_small_a_negative_b_by_ratio.hpp>
#include <boost/math/special_functions/detail/hypergeometric_1F1_negative_b_regions.hpp>

namespace boost { namespace math { namespace detail {

   // check when 1F1 series can't decay to polynom
   template <class T>
   inline bool check_hypergeometric_1F1_parameters(const T& a, const T& b)
   {
      BOOST_MATH_STD_USING

         if ((b <= 0) && (b == floor(b)))
         {
            if ((a >= 0) || (a < b) || (a != floor(a)))
               return false;
         }

      return true;
   }

   template <class T, class Policy>
   T hypergeometric_1F1_divergent_fallback(const T& a, const T& b, const T& z, const Policy& pol, int& log_scaling)
   {
      BOOST_MATH_STD_USING
      const char* function = "hypergeometric_1F1_divergent_fallback<%1%>(%1%,%1%,%1%)";
      //
      // We get here if either:
      // 1) We decide up front that Tricomi's method won't work, or:
      // 2) We've called Tricomi's method and it's failed.
      //
      if (b > 0)
      {
         // Commented out since recurrence seems to always be better?
#if 0
         if ((z < b) && (a > -50))
            // Might as well use a recurrence in preference to z-recurrence:
            return hypergeometric_1F1_backward_recurrence_for_negative_a(a, b, z, pol, function, log_scaling);
         T z_limit = fabs((2 * a - b) / (sqrt(fabs(a))));
         int k = 1 + itrunc(z - z_limit);
         // If k is too large we destroy all the digits in the result:
         T convergence_at_50 = (b - a + 50) * k / (z * 50);
         if ((k > 0) && (k < 50) && (fabs(convergence_at_50) < 1) && (z > z_limit))
         {
            return boost::math::detail::hypergeometric_1f1_recurrence_on_z_minus_zero(a, b, T(z - k), k, pol, log_scaling);
         }
#endif
         if (z < b)
            return hypergeometric_1F1_backward_recurrence_for_negative_a(a, b, z, pol, function, log_scaling);
         else
            return hypergeometric_1F1_backwards_recursion_on_b_for_negative_a(a, b, z, pol, function, log_scaling);
      }
      else  // b < 0
      {
         if (a < 0)
         {
            if ((b < a) && (z < -b / 4))
               return hypergeometric_1F1_from_function_ratio_negative_ab(a, b, z, pol, log_scaling);
            else
            {
               //
               // Solve (a+n)z/((b+n)n) == 1 for n, the number of iterations till the series starts to converge.
               // If this is well away from the origin then it's probably better to use the series to evaluate this.
               // Note that if sqr is negative then we have no solution, so assign an arbitrarily large value to the
               // number of iterations.
               //
               bool can_use_recursion = (z - b + 100 < boost::math::policies::get_max_series_iterations<Policy>()) && (100 - a < boost::math::policies::get_max_series_iterations<Policy>());
               T sqr = 4 * a * z + b * b - 2 * b * z + z * z;
               T iterations_to_convergence = sqr > 0 ? T(0.5f * (-sqrt(sqr) - b + z)) : T(-a - b);
               if(can_use_recursion && ((std::max)(a, b) + iterations_to_convergence > -300))
                  return hypergeometric_1F1_backwards_recursion_on_b_for_negative_a(a, b, z, pol, function, log_scaling);
               //
               // When a < b and if we fall through to the series, then we get divergent behaviour when b crosses the origin
               // so ideally we would pick another method.  Otherwise the terms immediately after b crosses the origin may
               // suffer catestrophic cancellation....
               //
               if((a < b) && can_use_recursion)
                  return hypergeometric_1F1_backwards_recursion_on_b_for_negative_a(a, b, z, pol, function, log_scaling);
            }
         }
         else
         {
            //
            // Start by getting the domain of the recurrence relations, we get either:
            //   -1     Backwards recursion is stable and the CF will converge to double precision.
            //   +1     Forwards recursion is satble and the CF will converge to double precision.
            //    0     No man's land, we're not far enough away from the crossover point to get double precision from either CF.
            //
            // At higher than double precision we need to be further away from the crossover location to
            // get full converge, but it's not clear how much further - indeed at quad precision it's
            // basically impossible to ever get forwards iteration to work.  Backwards seems to work
            // OK as long as a > 1 whatever the precision tbough.
            //
            int domain = hypergeometric_1F1_negative_b_recurrence_region(a, b, z);
            if ((domain < 0) && ((a > 1) || (boost::math::policies::digits<T, Policy>() <= 64)))
               return hypergeometric_1F1_from_function_ratio_negative_b(a, b, z, pol, log_scaling);
            else if (domain > 0)
            {
               if (boost::math::policies::digits<T, Policy>() <= 64)
                  return hypergeometric_1F1_from_function_ratio_negative_b_forwards(a, b, z, pol, log_scaling);
               try
               {
                  return hypergeometric_1F1_checked_series_impl(a, b, z, pol, log_scaling);
               }
               catch (const evaluation_error&)
               {
                  //
                  // The series failed, try the recursions instead and hope we get at least double precision:
                  //
                  return hypergeometric_1F1_from_function_ratio_negative_b_forwards(a, b, z, pol, log_scaling);
               }
            }
            //
            // We could fall back to Tricomi's approximation if we're in the transition zone
            // betweeen the above two regions.  However, I've been unable to find any examples
            // where this is better than the series, and there are many cases where it leads to
            // quite grievous errors.
            /*
            else if (allow_tricomi)
            {
               T aa = a < 1 ? T(1) : a;
               if (z < fabs((2 * aa - b) / (sqrt(fabs(aa * b)))))
                  return hypergeometric_1F1_AS_13_3_7_tricomi(a, b, z, pol, log_scaling);
            }
            */
         }
      }

      // If we get here, then we've run out of methods to try, use the checked series which will
      // raise an error if the result is garbage:
      return hypergeometric_1F1_checked_series_impl(a, b, z, pol, log_scaling);
   }

   template <class T>
   bool is_convergent_negative_z_series(const T& a, const T& b, const T& z, const T& b_minus_a)
   {
      BOOST_MATH_STD_USING
      //
      // Filter out some cases we don't want first:
      //
      if((b_minus_a > 0) && (b > 0))
      {
         if (a < 0)
            return false;
      }
      //
      // Generic check: we have small initial divergence and are convergent after 10 terms:
      //
      if ((fabs(z * a / b) < 2) && (fabs(z * (a + 10) / ((b + 10) * 10)) < 1))
      {
         // Double check for divergence when we cross the origin on a and b:
         if (a < 0)
         {
            T n = 300 - floor(a);
            if (fabs((a + n) * z / ((b + n) * n)) < 1)
            {
               if (b < 0)
               {
                  T m = 3 - floor(b);
                  if (fabs((a + m) * z / ((b + m) * m)) < 1)
                     return true;
               }
               else
                  return true;
            }
         }
         else if (b < 0)
         {
            T n = 3 - floor(b);
            if (fabs((a + n) * z / ((b + n) * n)) < 1)
               return true;
         }
      }
      if ((b > 0) && (a < 0))
      {
         //
         // For a and z both negative, we're OK with some initial divergence as long as
         // it occurs before we hit the origin, as to start with all the terms have the
         // same sign.
         //
         // https://www.wolframalpha.com/input/?i=solve+(a%2Bn)z+%2F+((b%2Bn)n)+%3D%3D+1+for+n
         //
         T sqr = 4 * a * z + b * b - 2 * b * z + z * z;
         T iterations_to_convergence = sqr > 0 ? T(0.5f * (-sqrt(sqr) - b + z)) : T(-a + b);
         if (iterations_to_convergence < 0)
            iterations_to_convergence = 0.5f * (sqrt(sqr) - b + z);
         if (a + iterations_to_convergence < -50)
         {
            // Need to check for divergence when we cross the origin on a:
            if (a > -1)
               return true;
            T n = 300 - floor(a);
            if(fabs((a + n) * z / ((b + n) * n)) < 1)
               return true;
         }
      }
      return false;
   }

   template <class T>
   inline T cyl_bessel_i_shrinkage_rate(const T& z)
   {
      // Approximately the ratio I_10.5(z/2) / I_9.5(z/2), this gives us an idea of how quickly
      // the Bessel terms in A&S 13.6.4 are converging:
      if (z < -160)
         return 1;
      if (z < -40)
         return 0.75f;
      if (z < -20)
         return 0.5f;
      if (z < -7)
         return 0.25f;
      if (z < -2)
         return 0.1f;
      return 0.05f;
   }

   template <class T>
   inline bool hypergeometric_1F1_is_13_3_6_region(const T& a, const T& b, const T& z)
   {
      BOOST_MATH_STD_USING
      if(fabs(a) == 0.5)
         return false;
      if ((z < 0) && (fabs(10 * a / b) < 1) && (fabs(a) < 50))
      {
         T shrinkage = cyl_bessel_i_shrinkage_rate(z);
         // We want the first term not too divergent, and convergence by term 10:
         if ((fabs((2 * a - 1) * (2 * a - b) / b) < 2) && (fabs(shrinkage * (2 * a + 9) * (2 * a - b + 10) / (10 * (b + 10))) < 0.75))
            return true;
      }
      return false;
   }

   template <class T>
   inline bool hypergeometric_1F1_need_kummer_reflection(const T& a, const T& b, const T& z)
   {
      BOOST_MATH_STD_USING
      //
      // Check to see if we should apply Kummer's relation or not:
      //
      if (z > 0)
         return false;
      if (z < -1)
         return true;
      //
      // When z is small and negative, things get more complex.
      // More often than not we do not need apply Kummer's relation and the
      // series is convergent as is, but we do need to check:
      //
      if (a > 0)
      {
         if (b > 0)
         {
            return fabs((a + 10) * z / (10 * (b + 10))) < 1;  // Is the 10'th term convergent?
         }
         else
         {
            return true;  // Likely to be divergent as b crosses the origin
         }
      }
      else // a < 0
      {
         if (b > 0)
         {
            return false;  // Terms start off all positive and then by the time a crosses the origin we *must* be convergent.
         }
         else
         {
            return true;  // Likely to be divergent as b crosses the origin, but hard to rationalise about!
         }
      }
   }

   template <class T, class Policy>
   T hypergeometric_1F1_imp(const T& a, const T& b, const T& z, const Policy& pol, int& log_scaling)
   {
      BOOST_MATH_STD_USING // exp, fabs, sqrt

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

      if ((z == 0) || (a == 0))
         return T(1);

      // undefined result:
      if (!detail::check_hypergeometric_1F1_parameters(a, b))
         return policies::raise_domain_error<T>(
            function,
            "Function is indeterminate for negative integer b = %1%.",
            b,
            pol);

      // other checks:
      if (a == -1)
         return 1 - (z / b);

      const T b_minus_a = b - a;

      // 0f0 a == b case;
      if (b_minus_a == 0)
      {
         int scale = itrunc(z, pol);
         log_scaling += scale;
         return exp(z - scale);
      }
      // Special case for b-a = -1, we don't use for small a as it throws the digits of a away and leads to large errors:
      if ((b_minus_a == -1) && (fabs(a) > 0.5))
      {
         // for negative small integer a it is reasonable to use truncated series - polynomial
         if ((a < 0) && (a == ceil(a)) && (a > -50))
            return detail::hypergeometric_1F1_generic_series(a, b, z, pol, log_scaling, function);

         return (b + z) * exp(z) / b;
      }

      if ((a == 1) && (b == 2))
         return boost::math::expm1(z, pol) / z;

      if ((b - a == b) && (fabs(z / b) < policies::get_epsilon<T, Policy>()))
         return 1;
      //
      // Special case for A&S 13.3.6:
      //
      if (z < 0)
      {
         if (hypergeometric_1F1_is_13_3_6_region(a, b, z))
         {
            // a is tiny compared to b, and z < 0
            // 13.3.6 appears to be the most efficient and often the most accurate method.
            T r = boost::math::detail::hypergeometric_1F1_AS_13_3_6(b_minus_a, b, T(-z), a, pol, log_scaling);
            int scale = itrunc(z, pol);
            log_scaling += scale;
            return r * exp(z - scale);
         }
         if ((b < 0) && (fabs(a) < 1e-2))
         {
            //
            // This is a tricky area, potentially we have no good method at all:
            //
            if (b - ceil(b) == a)
            {
               // Fractional parts of a and b are genuinely equal, we might as well
               // apply Kummer's relation and get a truncated series:
               int scaling = itrunc(z);
               T r = exp(z - scaling) * detail::hypergeometric_1F1_imp<T>(b_minus_a, b, -z, pol, log_scaling);
               log_scaling += scaling;
               return r;
            }
            if ((b < -1) && (max_b_for_1F1_small_a_negative_b_by_ratio(z) < b))
               return hypergeometric_1F1_small_a_negative_b_by_ratio(a, b, z, pol, log_scaling);
            if ((b > -1) && (b < -0.5f))
            {
               // Recursion is meta-stable:
               T first = hypergeometric_1F1_imp(a, T(b + 2), z, pol);
               T second = hypergeometric_1F1_imp(a, T(b + 1), z, pol);
               return tools::apply_recurrence_relation_backward(hypergeometric_1F1_recurrence_small_b_coefficients<T>(a, b, z, 1), 1, first, second);
            }
            //
            // We've got nothing left but 13.3.6, even though it may be initially divergent:
            //
            T r = boost::math::detail::hypergeometric_1F1_AS_13_3_6(b_minus_a, b, T(-z), a, pol, log_scaling);
            int scale = itrunc(z, pol);
            log_scaling += scale;
            return r * exp(z - scale);
         }
      }
      //
      // Asymptotic expansion for large z
      // TODO: check region for higher precision types.
      // Use recurrence relations to move to this region when a and b are also large.
      //
      if (detail::hypergeometric_1F1_asym_region(a, b, z, pol))
      {
         int saved_scale = log_scaling;
         try
         {
            return hypergeometric_1F1_asym_large_z_series(a, b, z, pol, log_scaling);
         }
         catch (const evaluation_error&)
         {
         }
         //
         // Very occationally our convergence criteria don't quite go to full precision
         // and we have to try another method:
         //
         log_scaling = saved_scale;
      }

      if ((fabs(a * z / b) < 3.5) && (fabs(z * 100) < fabs(b)) && ((fabs(a) > 1e-2) || (b < -5)))
         return detail::hypergeometric_1F1_rational(a, b, z, pol);

      if (hypergeometric_1F1_need_kummer_reflection(a, b, z))
      {
         if (a == 1)
            return detail::hypergeometric_1F1_pade(b, z, pol);
         if (is_convergent_negative_z_series(a, b, z, b_minus_a))
         {
            if ((boost::math::sign(b_minus_a) == boost::math::sign(b)) && ((b > 0) || (b < -200)))
            {
               // Series is close enough to convergent that we should be OK,
               // In this domain b - a ~ b and since 1F1[a, a, z] = e^z 1F1[b-a, b, -z]
               // and 1F1[a, a, -z] = e^-z the result must necessarily be somewhere near unity.
               // We have to rule out b small and negative becuase if b crosses the origin early
               // in the series (before we're pretty much converged) then all bets are off.
               // Note that this can go badly wrong when b and z are both large and negative,
               // in that situation the series goes in waves of large and small values which
               // may or may not cancel out.  Likewise the initial part of the series may or may
               // not converge, and even if it does may or may not give a correct answer!
               // For example 1F1[-small, -1252.5, -1043.7] can loose up to ~800 digits due to
               // cancellation and is basically incalculable via this method.
               return hypergeometric_1F1_checked_series_impl(a, b, z, pol, log_scaling);
            }
         }
         // Let's otherwise make z positive (almost always)
         // by Kummer's transformation
         // (we also don't transform if z belongs to [-1,0])
         int scaling = itrunc(z);
         T r = exp(z - scaling) * detail::hypergeometric_1F1_imp<T>(b_minus_a, b, -z, pol, log_scaling);
         log_scaling += scaling;
         return r;
      }
      //
      // Check for initial divergence:
      //
      bool series_is_divergent = (a + 1) * z / (b + 1) < -1;
      if (series_is_divergent && (a < 0) && (b < 0) && (a > -1))
         series_is_divergent = false;   // Best off taking the series in this situation
      //
      // If series starts off non-divergent, and becomes divergent later
      // then it's because both a and b are negative, so check for later
      // divergence as well:
      //
      if (!series_is_divergent && (a < 0) && (b < 0) && (b > a))
      {
         //
         // We need to exclude situations where we're over the initial "hump"
         // in the series terms (ie series has already converged by the time
         // b crosses the origin:
         //
         //T fa = fabs(a);
         //T fb = fabs(b);
         T convergence_point = sqrt((a - 1) * (a - b)) - a;
         if (-b < convergence_point)
         {
            T n = -floor(b);
            series_is_divergent = (a + n) * z / ((b + n) * n) < -1;
         }
      }
      else if (!series_is_divergent && (b < 0) && (a > 0))
      {
         // Series almost always become divergent as b crosses the origin:
         series_is_divergent = true;
      }
      if (series_is_divergent && (b < -1) && (b > -5) && (a > b))
         series_is_divergent = false;  // don't bother with divergence, series will be OK

      //
      // Test for alternating series due to negative a,
      // in particular, see if the series is initially divergent
      // If so use the recurrence relation on a:
      //
      if (series_is_divergent)
      {
         if((a < 0) && (floor(a) == a) && (-a < policies::get_max_series_iterations<Policy>()))
            // This works amazingly well for negative integer a:
            return hypergeometric_1F1_backward_recurrence_for_negative_a(a, b, z, pol, function, log_scaling);
         //
         // In what follows we have to set limits on how large z can be otherwise
         // the Bessel series become large and divergent and all the digits cancel out.
         // The criteria are distinctly empiracle rather than based on a firm analysis
         // of the terms in the series.
         //
         if (b > 0)
         {
            T z_limit = fabs((2 * a - b) / (sqrt(fabs(a))));
            if ((z < z_limit) && hypergeometric_1F1_is_tricomi_viable_positive_b(a, b, z))
               return detail::hypergeometric_1F1_AS_13_3_7_tricomi(a, b, z, pol, log_scaling);
         }
         else  // b < 0
         {
            if (a < 0)
            {
               T z_limit = fabs((2 * a - b) / (sqrt(fabs(a))));
               //
               // I hate these hard limits, but they're about the best we can do to try and avoid
               // Bessel function internal failures: these will be caught and handled
               // but up the expense of this function call:
               //
               if (((z < z_limit) || (a > -500)) && ((b > -500) || (b - 2 * a > 0)) && (z < -a))
               {
                  //
                  // Outside this domain we will probably get better accuracy from the recursive methods.
                  //
                  if(!(((a < b) && (z > -b)) || (z > z_limit)))
                     return detail::hypergeometric_1F1_AS_13_3_7_tricomi(a, b, z, pol, log_scaling);
                  //
                  // When b and z are both very small, we get large errors from the recurrence methods
                  // in the fallbacks.  Tricomi seems to work well here, as does direct series evaluation
                  // at least some of the time.  Picking the right method is not easy, and sometimes this
                  // is much worse than the fallback.  Overall though, it's a reasonable choice that keeps
                  // the very worst errors under control.
                  //
                  if(b > -1)
                     return detail::hypergeometric_1F1_AS_13_3_7_tricomi(a, b, z, pol, log_scaling);
               }
            }
            //
            // We previosuly used Tricomi here, but it appears to be worse than
            // the recurrence-based algorithms in hypergeometric_1F1_divergent_fallback.
            /*
            else
            {
               T aa = a < 1 ? T(1) : a;
               if (z < fabs((2 * aa - b) / (sqrt(fabs(aa * b)))))
                  return detail::hypergeometric_1F1_AS_13_3_7_tricomi(a, b, z, pol, log_scaling);
            }*/
         }

         return hypergeometric_1F1_divergent_fallback(a, b, z, pol, log_scaling);
      }

      if (hypergeometric_1F1_is_13_3_6_region(b_minus_a, b, T(-z)))
      {
         // b_minus_a is tiny compared to b, and -z < 0
         // 13.3.6 appears to be the most efficient and often the most accurate method.
         return boost::math::detail::hypergeometric_1F1_AS_13_3_6(a, b, z, b_minus_a, pol, log_scaling);
      }
#if 0
      if ((a > 0) && (b > 0) && (a * z / b > 2))
      {
         //
         // Series is initially divergent and slow to converge, see if applying
         // Kummer's relation can improve things:
         //
         if (is_convergent_negative_z_series(b_minus_a, b, T(-z), b_minus_a))
         {
            int scaling = itrunc(z);
            T r = exp(z - scaling) * detail::hypergeometric_1F1_checked_series_impl(b_minus_a, b, T(-z), pol, log_scaling);
            log_scaling += scaling;
            return r;
         }

      }
#endif
      if ((a > 0) && (b > 0) && (a * z > 50))
         return detail::hypergeometric_1F1_large_abz(a, b, z, pol, log_scaling);

      if (b < 0)
         return detail::hypergeometric_1F1_checked_series_impl(a, b, z, pol, log_scaling);

      return detail::hypergeometric_1F1_generic_series(a, b, z, pol, log_scaling, function);
   }

   template <class T, class Policy>
   inline T hypergeometric_1F1_imp(const T& a, const T& b, const T& z, const Policy& pol)
   {
      BOOST_MATH_STD_USING // exp, fabs, sqrt
      int log_scaling = 0;
      T result = hypergeometric_1F1_imp(a, b, z, pol, log_scaling);
      //
      // Actual result will be result * e^log_scaling.
      //
#ifndef BOOST_NO_CXX11_THREAD_LOCAL
    static const thread_local int max_scaling = itrunc(boost::math::tools::log_max_value<T>()) - 2;
    static const thread_local T max_scale_factor = exp(T(max_scaling));
#else
    int max_scaling = itrunc(boost::math::tools::log_max_value<T>()) - 2;
      T max_scale_factor = exp(T(max_scaling));
#endif

      while (log_scaling > max_scaling)
      {
         result *= max_scale_factor;
         log_scaling -= max_scaling;
      }
      while (log_scaling < -max_scaling)
      {
         result /= max_scale_factor;
         log_scaling += max_scaling;
      }
      if (log_scaling)
         result *= exp(T(log_scaling));
      return result;
   }

   template <class T, class Policy>
   inline T log_hypergeometric_1F1_imp(const T& a, const T& b, const T& z, int* sign, const Policy& pol)
   {
      BOOST_MATH_STD_USING // exp, fabs, sqrt
      int log_scaling = 0;
      T result = hypergeometric_1F1_imp(a, b, z, pol, log_scaling);
      if (sign)
      *sign = result < 0 ? -1 : 1;
     result = log(fabs(result)) + log_scaling;
      return result;
   }

   template <class T, class Policy>
   inline T hypergeometric_1F1_regularized_imp(const T& a, const T& b, const T& z, const Policy& pol)
   {
      BOOST_MATH_STD_USING // exp, fabs, sqrt
      int log_scaling = 0;
      T result = hypergeometric_1F1_imp(a, b, z, pol, log_scaling);
      //
      // Actual result will be result * e^log_scaling / tgamma(b).
      //
    int result_sign = 1;
    T scale = log_scaling - boost::math::lgamma(b, &result_sign, pol);
#ifndef BOOST_NO_CXX11_THREAD_LOCAL
      static const thread_local T max_scaling = boost::math::tools::log_max_value<T>() - 2;
    static const thread_local T max_scale_factor = exp(max_scaling);
#else
    T max_scaling = boost::math::tools::log_max_value<T>() - 2;
    T max_scale_factor = exp(max_scaling);
#endif

      while (scale > max_scaling)
      {
         result *= max_scale_factor;
         scale -= max_scaling;
      }
      while (scale < -max_scaling)
      {
         result /= max_scale_factor;
     scale += max_scaling;
      }
      if (scale != 0)
         result *= exp(scale);
      return result * result_sign;
   }

} // namespace detail

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type hypergeometric_1F1(T1 a, T2 b, T3 z, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
      typedef typename tools::promote_args<T1, T2, T3>::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, Policy>(
      detail::hypergeometric_1F1_imp<value_type>(
         static_cast<value_type>(a),
         static_cast<value_type>(b),
         static_cast<value_type>(z),
         forwarding_policy()),
      "boost::math::hypergeometric_1F1<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type hypergeometric_1F1(T1 a, T2 b, T3 z)
{
   return hypergeometric_1F1(a, b, z, policies::policy<>());
}

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type hypergeometric_1F1_regularized(T1 a, T2 b, T3 z, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
      typedef typename tools::promote_args<T1, T2, T3>::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, Policy>(
      detail::hypergeometric_1F1_regularized_imp<value_type>(
         static_cast<value_type>(a),
         static_cast<value_type>(b),
         static_cast<value_type>(z),
         forwarding_policy()),
      "boost::math::hypergeometric_1F1<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type hypergeometric_1F1_regularized(T1 a, T2 b, T3 z)
{
   return hypergeometric_1F1_regularized(a, b, z, policies::policy<>());
}

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type log_hypergeometric_1F1(T1 a, T2 b, T3 z, const Policy& /* pol */)
{
  BOOST_FPU_EXCEPTION_GUARD
    typedef typename tools::promote_args<T1, T2, T3>::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, Policy>(
    detail::log_hypergeometric_1F1_imp<value_type>(
      static_cast<value_type>(a),
      static_cast<value_type>(b),
      static_cast<value_type>(z),
      0,
      forwarding_policy()),
    "boost::math::hypergeometric_1F1<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type log_hypergeometric_1F1(T1 a, T2 b, T3 z)
{
  return log_hypergeometric_1F1(a, b, z, policies::policy<>());
}

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type log_hypergeometric_1F1(T1 a, T2 b, T3 z, int* sign, const Policy& /* pol */)
{
  BOOST_FPU_EXCEPTION_GUARD
    typedef typename tools::promote_args<T1, T2, T3>::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, Policy>(
    detail::log_hypergeometric_1F1_imp<value_type>(
      static_cast<value_type>(a),
      static_cast<value_type>(b),
      static_cast<value_type>(z),
      sign,
      forwarding_policy()),
    "boost::math::hypergeometric_1F1<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type log_hypergeometric_1F1(T1 a, T2 b, T3 z, int* sign)
{
  return log_hypergeometric_1F1(a, b, z, sign, policies::policy<>());
}

  } } // namespace boost::math

#endif // BOOST_MATH_HYPERGEOMETRIC_HPP


// [[Rcpp::plugins(openmp)]]
#include <omp.h>
#include <Rcpp.h>
#include <RcppEigen.h>
#include <Eigen/Core>
#include <random>
#include "distributions.h"
#include "concurrentqueue.h"

// [[Rcpp::depends(RcppEigen)]]
using namespace Rcpp;
using namespace RcppEigen;
using namespace Eigen;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::SparseVector;
using Eigen::LLT;
using Eigen::Lower;
using Eigen::Map;
using Eigen::Upper;
typedef Map<MatrixXd> MapMatd;

template<typename Scalar>
struct scalar_normal_dist_op
{
  static std::mt19937 rng;                        // The uniform pseudo-random algorithm
  mutable std::normal_distribution<Scalar> norm; // gaussian combinator

  EIGEN_EMPTY_STRUCT_CTOR(scalar_normal_dist_op)

    template<typename Index>
    inline const Scalar operator() (Index, Index = 0) const { return norm(rng); }
    inline void seed(const uint64_t &s) { rng.seed(s); }
};

template<typename Scalar>
std::mt19937 scalar_normal_dist_op<Scalar>::rng;

template<typename Scalar>
struct exponential_functor
{
  exponential_functor(){}

  const Scalar operator()(const Scalar& x) const{ return exp_rng(x); }

};

inline MatrixXd AtA(const MapMatd& A) {
  int n(A.cols());
  return MatrixXd(n,n).setZero().selfadjointView<Lower>()
                      .rankUpdate(A.adjoint());
}

template<typename Scalar>
struct inv_gamma_functor
{
  inv_gamma_functor(const Scalar& vd):m_a(vd){}

  const Scalar operator()(const Scalar& x) const{ return inv_gamma_rate_rng(0.5 + 0.5*m_a,x); }
  Scalar  m_a;
};

template<typename Scalar>
struct gamma_functor
{
  gamma_functor(const Scalar& vd):m_a(vd){}

  const Scalar operator()(const Scalar& x) const{ return gamma_rng(0.5 + 0.5*m_a,x); }
  Scalar  m_a;
};

template<typename Scalar>
struct inv_gamma_functor_init
{
  inv_gamma_functor_init(){}

  const Scalar operator()(const Scalar& x) const{ return inv_gamma_rate_rng(0.5,x); }

};

template<typename Scalar>
struct inv_gamma_functor_init_v
{
  inv_gamma_functor_init_v(const Scalar& vd):m_a(vd){}

  const Scalar operator()(const Scalar& x) const{ return inv_gamma_rate_rng(0.5*m_a,m_a*x); }
  Scalar  m_a;
};

/*
* Bayes R sampler
* outputFile- The file in which the samples aftare burnin will be stored
* seed- random seed
* max_iterations- total of number of samples taken.
* burn_in - integer leq than max_iterations, number of samples used for burn in, after which, al samples will  be stored in the outputFile
* thinning- thinning regime, not implemented
* X- matrix of snp markers, or covariates of interest
* Y- vector of response variates, must have the same number of rows as X
* sigma0- variance of the zero-centered normal prior over the intercept
* v0E- degrees of  freedom of the prior inverse scaled chi-squared distribution over residues variance
* s02E - scale parameter of the prior inverse scaled chi-squared distribution over residues variance
* v0G- degrees of freedom of the prior inverse scaled chi-squared distribution over genetic effects variance
* s02G- scale parameter of the prior inverse scaled chi-squared distribution over genetic effects variance
*/
// [[Rcpp::export]]
void HorseshoeR(std::string outputFile, int seed, int max_iterations, int burn_in, int thinning, Eigen::MatrixXd X, Eigen::VectorXd Y,double A, double v0E, double s02E, double vL, double vT,double c2,double vC,double sC) {
  int flag;
  moodycamel::ConcurrentQueue<Eigen::VectorXd> q;
  flag=0;
  int N(Y.size());
  int M(X.cols());
  VectorXd components(M);

  ////////////validate inputs

  if(max_iterations < burn_in || max_iterations<1 || burn_in<1) //validations related to mcmc burnin and iterations
  {
    std::cout<<"error: burn_in has to be a positive integer and smaller than the maximum number of iterations ";
    return;
  }

  Eigen::initParallel();
  Eigen::setNbThreads(10);
  double sum_beta_sqr;

#pragma omp parallel num_threads(2) shared(flag,q,M,N)
{
#pragma omp sections
{

  {

    //mean and residual variables
    double mu; // mean or intercept
    double sigmaG; //genetic variance
    double sigmaE; // residuals variance

    //component variables
    VectorXd lambda(M);
    VectorXd v(M);
    VectorXd phi(M);
    VectorXd chi(M);
    double tau;
    double eta;
    //linear model variables
    MatrixXd beta(M,1); // effect sizes
    VectorXd y_tilde(N); // variable containing the adjusted residuals to exclude the effects of a given marker
    VectorXd epsilon(N); // variable containing the residuals

    //sampler variables
    VectorXd sample(2*M+4+N); // varible containg a sambple of all variables in the model, M marker effects, M component assigned to markers, sigmaE, sigmaG, mu, iteration number and Explained variance
    std::vector<int> markerI;
    for (int i=0; i<M; ++i) {
      markerI.push_back(i);
    }

    int marker;
    double acum;

    y_tilde.setZero();

    beta.setZero();
    tau=beta_rng(1,1);

    mu=0;

    v=(v.setOnes().array()).unaryExpr(inv_gamma_functor<double>(0));
    v.setOnes();
    //std::cout<< "initial v" << eta;
    lambda=v.unaryExpr(inv_gamma_functor_init_v<double>(vL));
    lambda.setOnes();

    std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();
    epsilon= Y.array() - mu - (X*beta).array();
    sigmaE=epsilon.squaredNorm()/N*0.5;

    eta=inv_gamma_rate_rng(0.5,1/(sigmaE*pow(A,2)));
    //eta=0.00001;
    std::cout<< "initial eta " << eta<<"\n";
    tau=(1.0/eta)*inv_gamma_rate_rng(0.5*vT,vT);

    // tau=1/A;
    std::cout<< "initial tau " << tau<<"\n";

    for(int iteration=0; iteration < max_iterations; iteration++){

      if(iteration>0)
        if( iteration % (int)std::ceil(max_iterations/10) ==0)
        {
          std::cout << "iteration: "<<iteration <<"\n";
          std::cout<< " tau " << tau<<"\n";
          std::cout<< " eta " << eta<<"\n";
          std::cout<< "sigmaE" << sigmaE<<"\n";
        }

        epsilon= epsilon.array()+mu;//  we substract previous value
        mu = norm_rng(epsilon.sum()/(double)N, sigmaE/(double)N); //update mu
        epsilon= epsilon.array()-mu;// we substract again now epsilon =Y-mu-X*beta

        std::random_shuffle(markerI.begin(), markerI.end());

        eta = inv_gamma_rate_rng(0.5+0.5*vT,(1.0/(sigmaE*A*A))+vT/tau);
        v=(vL/(lambda).array()+1.0).unaryExpr(inv_gamma_functor<double>(vL));
        for(int j=0; j < M; j++){

          marker= markerI[j];

          y_tilde= epsilon.array()+(X.col(marker)*beta(marker,0)).array();//now y_tilde= Y-mu-X*beta+ X.col(marker)*beta(marker)_old

          // std::cout<< muk;
          //we compute the denominator in the variance expression to save computations
          //denom=X.col(marker).squaredNorm()+(sigmaE/(tau*c2*lambda[marker]/(tau*lambda[marker]+c2)));
          //muk for the other components is computed according to equaitons
          //muk= (X.col(marker).cwiseProduct(y_tilde)).sum()/denom;
          //beta(marker,0)=norm_rng(muk,sigmaE/denom);
          beta(marker,0)=(X.col(marker).cwiseProduct(y_tilde)).sum()/(X.col(marker).squaredNorm()+(sigmaE/(tau*c2*lambda[marker]/(tau*lambda[marker]+c2))))+sqrt(sigmaE/(X.col(marker).squaredNorm()+(sigmaE/(tau*c2*lambda[marker]/(tau*lambda[marker]+c2)))))*norm_rng(0,1);

          epsilon=y_tilde-X.col(marker)*beta(marker,0);//now epsilon contains Y-mu - X*beta+ X.col(marker)*beta(marker)_old- X.col(marker)*beta(marker)_new

        }

        lambda=(vL*v.cwiseInverse()+(0.5*beta.cwiseProduct(beta)*(1.0/tau))).unaryExpr(inv_gamma_functor<double>(vL));
        //  if(iteration==0)
        //  std::cout<< " lambda " << lambda<<"\n";
        tau= inv_gamma_rate_rng(0.5*(M+vT),vT/eta+((0.5)*((beta.array().pow(2))/lambda.array()).sum()));

        //tau=A;
         c2=inv_gamma_rate_rng(0.5*vC+0.5*M,vC*sC*0.5+0.5*beta.squaredNorm());
        //  c2=sC;

        sigmaE=inv_scaled_chisq_rng(v0E+N,((epsilon).squaredNorm()+v0E*s02E)/(v0E+N));

        if(iteration >= burn_in)
        {
          if(iteration % thinning == 0){
            sample<< iteration,mu,beta,sigmaE,tau,lambda,epsilon;
            q.enqueue(sample);
          }

        }

    }

    std::chrono::high_resolution_clock::time_point t2 = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::seconds>( t2 - t1 ).count();
    std::cout << "duration: "<<duration << "s\n";
    flag=1;
  }
#pragma omp section
{
  bool queueFull;
  queueFull=0;
  std::ofstream outFile;
  outFile.open(outputFile);
  VectorXd sampleq(2*M+4+N);
  IOFormat CommaInitFmt(StreamPrecision, DontAlignCols, ", ", ", ", "", "", "", "");
  outFile<< "iteration,"<<"mu,";
  for(unsigned int i = 0; i < M; ++i){
    outFile << "beta[" << (i+1) << "],";

  }
  outFile<<"sigmaE,"<<"tau,";
  for(unsigned int i = 0; i < M; ++i){
    outFile << "lambda[" << (i+1) << "],";
  }
  for(unsigned int i = 0; i < N; ++i){
    outFile << "epsilon[" << (i+1) << "],";
  }
  outFile<<"\n";

  while(!flag ){
    if(q.try_dequeue(sampleq))
      outFile<< sampleq.transpose().format(CommaInitFmt) << "\n";
  }
}

}
}

}

/*** R
M=3000
N=2000
MT=200
B=matrix(rnorm(M,sd=sqrt(0.5/MT)),ncol=1)
  B[sample(1:M,M-MT),1]=0
X <- matrix(rnorm(M*N), N, M); var(X[,1])
  G <- X%*%B; var(G)
    Y=X%*%B+rnorm(N,sd=sqrt((1-var(G)))); var(Y)
    Y=scale(Y)
    X=scale(X)
    vT=1
    vL=1
    A=1500
    A=(1/sqrt(N))*A/(M-A)
    c2=1.0
    v0E=0.001
    s02E=0.001
    vC=10
    sC=10.0

    HorseshoeR("./test2.csv",1, 20000,10000 ,10,X, Y, A,  v0E, s02E,  vL,  vT, c2,vC,sC)
      library(readr)
     library(data.table)
      tmp <- fread("./test2.csv")
      tmp<-as.matrix(tmp)
      plot(B,colMeans(tmp[,grep("beta",colnames(tmp))]))
      summary(lm(B~colMeans(tmp[,grep("beta",colnames(tmp))])))
      lines(B,B)
      abline(h=0)
      G <- X%*%B;
    var(G)
      tmp<-as.data.frame(tmp)

      mean(tmp$sigmaE)
      plot(tmp$sigmaE)
      plot(tmp$mu)
      plot(tmp$tau)
      hist(as.matrix(tmp[,grep("beta",names(tmp))]))

      */


#include "matchers/matcher.h"
#include "opencv2/opencv.hpp"
#include "camera/CameraModel.h"
#include "types/Frame.h"
//#include "extractors/FASTextractor.h"
#include "extractors/ORBextractor.h"
#include "types/Map.h"
#include "types/MapPoint.h"
#include "mapping/LocalBA.h"
#include "Viewer.h"
#include <memory>
#include <Eigen/Geometry>
#include <thread>
#include <sophus/se3.hpp>
#include <iostream>
#include <stdint.h>
#include <unordered_set>
#include "g2o/stuff/sampler.h"
#include<suitesparse/cholmod.h>

using namespace TRACKING_BENCH;

// simple direct
using namespace std;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// Camera intrinsics
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// baseline
double baseline = 0.573;
// paths
string left_file = "/home/lyc/share/slambook2-master/slambook2-master/ch8/left.png";
string disparity_file = "/home/lyc/code/slam_bench/trackingBench-SLAM/data/disparity.png";

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

/// class for accumulator jacobians in parallel
class JacobianAccumulator {
public:
    JacobianAccumulator(
            const cv::Mat &img1_,
            const cv::Mat &img2_,
            const VecVector2d &px_ref_,
            const vector<double> depth_ref_,
            Sophus::SE3d &T21_) :
            img1(img1_), img2(img2_), px_ref(px_ref_), depth_ref(depth_ref_), T21(T21_) {
        projection = VecVector2d(px_ref.size(), Eigen::Vector2d(0, 0));
    }

    /// accumulate jacobians in a range
    void accumulate_jacobian(const cv::Range &range);

    /// get hessian matrix
    Matrix6d hessian() const { return H; }

    /// get bias
    Vector6d bias() const { return b; }

    /// get total cost
    double cost_func() const { return cost; }

    /// get projected points
    VecVector2d projected_points() const { return projection; }

    /// reset h, b, cost to zero
    void reset() {
        H = Matrix6d::Zero();
        b = Vector6d::Zero();
        cost = 0;
    }

private:
    const cv::Mat &img1;
    const cv::Mat &img2;
    const VecVector2d &px_ref;
    const vector<double> depth_ref;
    Sophus::SE3d &T21;
    VecVector2d projection; // projected points

    std::mutex hessian_mutex;
    Matrix6d H = Matrix6d::Zero();
    Vector6d b = Vector6d::Zero();
    double cost = 0;
};

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationMultiLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3d &T21
);

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3d &T21
);

// bilinear interpolation
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    // boundary check
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols) x = img.cols - 1;
    if (y >= img.rows) y = img.rows - 1;
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
//    std::cout<<"f1: "<<(1 - xx) * (1 - yy)<<" x1: "<<(int)data[0]<<std::endl;
//    std::cout<<"f2: "<<xx * (1 - yy)<<" x2: "<<(int)data[1]<<std::endl;
//    std::cout<<"f3: "<<(1 - xx) * yy<<" x3: "<<(int)data[img.step]<<std::endl;
//    std::cout<<"f4: "<<xx * yy<<" x4: "<<(int)data[img.step + 1]<<std::endl;
    return float(
            (1 - xx) * (1 - yy) * data[0] +
            xx * (1 - yy) * data[1] +
            (1 - xx) * yy * data[img.step] +
            xx * yy * data[img.step + 1]
    );
}

void DirectPoseEstimationSingleLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,//new
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3d &T21) {

    const int iterations = 10;
    double cost = 0, lastCost = 0;
    auto t1 = chrono::steady_clock::now();
    JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);

    for (int iter = 0; iter < iterations; iter++) {
        jaco_accu.reset();
        cv::parallel_for_(cv::Range(0, px_ref.size()),
                          std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, std::placeholders::_1));
        Matrix6d H = jaco_accu.hessian();
        Vector6d b = jaco_accu.bias();

        // solve update and put it into estimation
        Vector6d update = H.ldlt().solve(b);
        T21 = Sophus::SE3d::exp(-update) * T21;
        cost = jaco_accu.cost_func();

        if (std::isnan(update[0])) {
            // sometimes occurred when we have a black or white patch and H is irreversible
            cout << "update is nan" << endl;
            break;
        }
        if (iter > 0 && cost > lastCost) {
            cout << "cost increased: " << cost << ", " << lastCost << endl;
            break;
        }
        if (update.norm() < 1e-3) {
            // converge
            break;
        }

        lastCost = cost;
        cout << "iteration: " << iter << ", cost: " << cost << endl;
    }

    cout << "T21 = \n" << T21.matrix() << endl;
    auto t2 = chrono::steady_clock::now();
    auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "direct method for single layer: " << time_used.count() << endl;

    // plot the projected pixels here
    cv::Mat img2_show;
    cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
    VecVector2d projection = jaco_accu.projected_points();
    for (size_t i = 0; i < px_ref.size(); ++i) {
        auto p_ref = px_ref[i];
        auto p_cur = projection[i];
        if (p_cur[0] > 0 && p_cur[1] > 0) {
            cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
                     cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("current", img2_show);
    cv::waitKey(0);
}

void JacobianAccumulator::accumulate_jacobian(const cv::Range &range) {

    // parameters
    const int half_patch_size = 2;
    int cnt_good = 0;
    Matrix6d hessian = Matrix6d::Zero();
    Vector6d bias = Vector6d::Zero();
    double cost_tmp = 0;

    for (size_t i = range.start; i < range.end; i++) {

        // compute the projection in the second image
        Eigen::Vector3d point_ref =
                depth_ref[i] * Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
        Eigen::Vector3d point_cur = T21 * point_ref;
        if (point_cur[2] < 0)   // depth invalid
            continue;

        float u = fx * point_cur[0] / point_cur[2] + cx, v = fy * point_cur[1] / point_cur[2] + cy;
        if (u < half_patch_size || u > img2.cols - half_patch_size || v < half_patch_size ||
            v > img2.rows - half_patch_size)
            continue;

        projection[i] = Eigen::Vector2d(u, v);
        double X = point_ref[0], Y = point_ref[1], Z = point_ref[2],
                Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;
        cnt_good++;

        // and compute error and jacobian
        for (int y = -half_patch_size; y < half_patch_size; y++)
            for (int x = -half_patch_size; x < half_patch_size; x++) {

                double error = GetPixelValue(img2, u + x, v + y) -
                               GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y);

                Matrix26d J_pixel_xi;
                Eigen::Vector2d J_img_pixel;

                J_pixel_xi(0, 0) = fx * Z_inv;
                J_pixel_xi(0, 1) = 0;
                J_pixel_xi(0, 2) = -fx * X * Z2_inv;
                J_pixel_xi(0, 3) = -fx * X * Y * Z2_inv;
                J_pixel_xi(0, 4) = fx + fx * X * X * Z2_inv;
                J_pixel_xi(0, 5) = -fx * Y * Z_inv;

                J_pixel_xi(1, 0) = 0;
                J_pixel_xi(1, 1) = fy * Z_inv;
                J_pixel_xi(1, 2) = -fy * Y * Z2_inv;
                J_pixel_xi(1, 3) = -fy - fy * Y * Y * Z2_inv;
                J_pixel_xi(1, 4) = fy * X * Y * Z2_inv;
                J_pixel_xi(1, 5) = fy * X * Z_inv;

                J_img_pixel = Eigen::Vector2d(
                        0.5 * (GetPixelValue(img1, px_ref[i][0] + 1 + x, px_ref[i][1] + y) - GetPixelValue(img1, px_ref[i][0] - 1 + x, px_ref[i][1] + y)),
                        0.5 * (GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + 1 + y) - GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] - 1 + y))
                );

                // total jacobian
                Vector6d J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose();

                hessian += J * J.transpose();
                bias += -error * J;
                cost_tmp += error * error;
                std::cout<<" H "<<J * J.transpose()<<std::endl;
                std::cout<<" b "<<-error * J<<std::endl;
            }
        std::cout<<"H: "<<hessian<<std::endl;
        std::cout<<"bias: "<<bias<<std::endl;
    }

    if (cnt_good) {
        // set hessian, bias and cost
        unique_lock<mutex> lck(hessian_mutex);
        H += hessian;
        b += bias;
        cost += cost_tmp / cnt_good;
    }
    //std::cout<<" mean cost: "<<std::endl;
}

void DirectPoseEstimationMultiLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3d &T21)
{
    // parameters
    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    // create pyramids
    vector<cv::Mat> pyr1, pyr2; // image pyramids
    for (int i = 0; i < pyramids; i++) {
        if (i == 0) {
            pyr1.push_back(img1);
            pyr2.push_back(img2);
        } else {
            cv::Mat img1_pyr, img2_pyr;
            cv::resize(pyr1[i - 1], img1_pyr,
                       cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
            cv::resize(pyr2[i - 1], img2_pyr,
                       cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
            pyr1.push_back(img1_pyr);
            pyr2.push_back(img2_pyr);
        }
    }

    double fxG = fx, fyG = fy, cxG = cx, cyG = cy;  // backup the old values
    for (int level = pyramids - 1; level >= 0; level--) {
        VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
        for (auto &px: px_ref) {
            px_ref_pyr.push_back(scales[level] * px);
        }

        // scale fx, fy, cx, cy in different pyramid levels
        fx = fxG * scales[level];
        fy = fyG * scales[level];
        cx = cxG * scales[level];
        cy = cyG * scales[level];
        DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
    }

}

void LoadImages(const string &strPathToSequence, vector<string> &vstrImageLeft,
                vector<string> &vstrImageRight, vector<double> &vTimestamps)
{
    ifstream fTimes;
    string strPathTimeFile = strPathToSequence + "/times.txt";
    fTimes.open(strPathTimeFile.c_str());
    while(!fTimes.eof())
    {
        string s;
        getline(fTimes,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            double t;
            ss >> t;
            vTimestamps.push_back(t);
        }
    }

    string strPrefixLeft = strPathToSequence + "/image_0/";
    string strPrefixRight = strPathToSequence + "/image_1/";

    const int nTimes = vTimestamps.size();
    vstrImageLeft.resize(nTimes);
    vstrImageRight.resize(nTimes);

    for(int i=0; i<nTimes; i++)
    {
        stringstream ss;
        ss << setfill('0') << setw(6) << i;
        vstrImageLeft[i] = strPrefixLeft + ss.str() + ".png";
        vstrImageRight[i] = strPrefixRight + ss.str() + ".png";
    }
}

void LoadKittiGroundTruth(const string& file, vector<Eigen::Matrix3d>& R, vector<Eigen::Vector3d>& t)
{
    ifstream fTimes;
    fTimes.open(file.c_str());
    R.reserve(1000);
    t.reserve(1000);

    while(!fTimes.eof())
    {
        Eigen::Vector3d t0;
        Eigen::Matrix3d R0;

        string s;
        for (int i = 0; i < 3; i ++)
        {
            for (int j = 0;j < 3; j ++)
            {
                getline(fTimes,s,' ');
                R0(i, j) =  std::atof(s.c_str());
            }
            if (i == 2)
                getline(fTimes,s);
            else
                getline(fTimes,s,' ');
            t0(i) = std::atof(s.c_str());
        }
        R.emplace_back(R0.transpose());
        t.emplace_back(- R0.transpose() * t0);
        //getline(fTimes,s);
    }
}

void test_projection()
{

    // Load Images Paths
    std::vector<string> imageLeft;
    std::vector<string> imageRight;
    std::vector<double> timeStamp;
    LoadImages( "/media/lyc/ 其他/dataset/sequences/00",
                imageLeft, imageRight, timeStamp);
    string left_file = "/home/lyc/code/slam_bench/trackingBench-SLAM/data/left.png";
    string right_file = "/home/lyc/code/slam_bench/trackingBench-SLAM/data/right.png";
    string disparity_file = "/home/lyc/code/slam_bench/trackingBench-SLAM/data/disparity.png";
    // Load ground truth
    cv::Mat disparity = cv::imread(disparity_file, 0);
    std::vector<Eigen::Matrix3d> gtR;
    std::vector<Eigen::Vector3d> gtT;
    LoadKittiGroundTruth("/home/lyc/share/data_odometry_poses/dataset/poses/00.txt",
                         gtR, gtT);

    //
    // 内参
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 间距
    double d = 0.573;

    auto extractor_ptr = std::make_shared<ORBExtractor>();
    auto local_ba = std::make_shared<LocalBA>();

    std::shared_ptr<Frame> key_frame, last_frame;
    auto matcher_ptr = std::make_shared<Matcher>();
    auto map_ptr = std::make_shared<TRACKING_BENCH::Map>();

    // viewer
    auto viewer = std::make_shared<Viewer>();
    viewer->SetCameraPos(Eigen::Matrix4f::Identity());
    viewer->Run();

    // view gt pos
    std::vector<cv::KeyPoint> keyPoints_ref;
    std::vector<Eigen::Matrix4f> kfs_pos;

    // for images
    for (int i = 0;i < 2;i +=1)
    {
        cv::Mat imLeft, imRight, show;
        std::vector<cv::KeyPoint> keyPoints_r, keyPoints_l;
        cv::Mat descriptors_r, descriptors_l;
        // 0. read image to frame
        imLeft = cv::imread(imageLeft[i], CV_LOAD_IMAGE_UNCHANGED);
        static auto camera_ptr = std::make_shared<PinholeCamera>(imLeft.cols, imLeft.rows,fx, fy, cx, cy);
        cv::Mat out;

        auto cur_frame_ptr = std::make_shared<Frame>(imLeft, 0, 5, 0.6, camera_ptr);
        if(last_frame != nullptr)
        {
            cur_frame_ptr->SetPose(last_frame->GetPose());
        }
        else
        {
            Eigen::Matrix4f pos = Eigen::Matrix4f::Identity();
            pos.block<3, 3>(0, 0) = gtR.at(i).cast<float>();
            pos.block<3, 1>(0 ,3) = gtT.at(i).cast<float>();
            cur_frame_ptr->SetPose(pos);
            viewer->SetCameraPos(cur_frame_ptr->GetPoseInverse());
        }

        // 1. match points
        if (last_frame != nullptr)
        {
            // match with last frame
//            auto matches = matcher_ptr->searchByBow(cur_frame_ptr, cur_frame_ptr, true);
//            auto matches = matcher_ptr->searchByBF(cur_frame_ptr, key_frame, 0, 5, 10, 30);
//            auto matches = matcher_ptr->searchByViolence(cur_frame_ptr, key_frame, 0, 5, 50);
            // matches by optical flow
//            std::vector<cv::Point2f> pts;
//            std::vector<cv::KeyPoint> kps;
//            auto matches = matcher_ptr->searchByOPFlow(cur_frame_ptr, last_frame, pts, true, true);
//            kps.reserve(pts.size());
//            for (auto& pt:pts)
//            {
//                cv::KeyPoint kp;
//                kp.pt = pt;
//                kps.emplace_back(kp);
//            }
//            cur_frame_ptr->SetKeys(kps, cur_frame_ptr);
            // matches by projection frame

//            extractor_ptr->operator()(cur_frame_ptr->GetImagePyramid(),
//                                      cur_frame_ptr->GetScaleFactors(),
//                                      2000,
//                                      80,
//                                      30,
//                                      keyPoints_l,
//                                      descriptors_l);
//
//            cur_frame_ptr->SetKeys(keyPoints_l, cur_frame_ptr, descriptors_l);
//            cur_frame_ptr->AssignFeaturesToGrid();

            Eigen::Matrix4f pos = Eigen::Matrix4f::Identity();
            pos.block<3, 3>(0, 0) = gtR.at(i).cast<float>();
            pos.block<3, 1>(0 ,3) = gtT.at(i).cast<float>();
            pos(2, 3) = -0.85;
            cur_frame_ptr->SetPose(last_frame->GetPose());//pos);//last_frame->GetPose());

//            matcher_ptr->setProjectionParam(30, 50, 30, true, 30);
//            auto matches = matcher_ptr->searchByProjection(cur_frame_ptr, key_frame);

            // matches by projection map
//            matcher_ptr->setProjectionParam(30, 50, 30, true, 20);
//            auto matches = matcher_ptr->searchByProjection(map_ptr, cur_frame_ptr, 0.6);
//
//            // assign map point
//            for (auto &item:matches)
//            {
//                // last frame
////                shared_ptr<MapPoint> mp = last_frame->GetMapPoint(item.trainIdx);
//                // key frame
////                shared_ptr<MapPoint> mp = key_frame->GetMapPoint(item.trainIdx);
//                // map
//                shared_ptr<MapPoint> mp = map_ptr->GetAllMapPoints().at(item.trainIdx);
//                if (mp != nullptr)
//                    cur_frame_ptr->AddMapPoint(mp, item.queryIdx);
//            }
//
//            for (auto& m:matches)
//            {
//                m.trainIdx =  map_ptr->GetAllMapPoints().at(m.trainIdx)->GetReferenceFeature()->idxF;
//            }

            // direct
            matcher_ptr->setDirectParam(4, 0, 20, 20, 0.01);
            auto matches = matcher_ptr->searchByDirect(map_ptr, cur_frame_ptr, last_frame);
            // pnp
            cur_frame_ptr->SetPose(Eigen::Matrix4f::Identity());
            local_ba->PoseOptimization(cur_frame_ptr);
            std::cout<<"current pose Twc: "<<cur_frame_ptr->GetPoseInverse()<<std::endl;
            // feature align
            cv::Mat show2 = cur_frame_ptr->GetImagePyramid()[0].clone();
            cv::cvtColor(show2,show2,CV_GRAY2BGR);
            for (const auto& f:cur_frame_ptr->GetKeys())
            {
                cv::line(show2,
                         cv::Point((int)f->px.x(), (int)f->px.y()),
                         cv::Point((int)f->point->GetReferenceFeature()->px.x(), (int)f->point->GetReferenceFeature()->px.y()),
                         cv::Scalar(255,0,0), 2);
            }
            cv::imshow("image align", show2);
            cv::waitKey(0);

//            cv::drawMatches(imLeft,
//                            keyPoints_l,
//                            key_frame->GetImagePyramid().at(0),
//                            keyPoints_ref, matches, show);
//            cv::imshow("match", show);

            // 2. pose optimization
//            local_ba->PoseOptimization(cur_frame_ptr);

            std::cout<<"current pose Twc: "<<cur_frame_ptr->GetPoseInverse()<<std::endl;
            std::cout<<"ground truth pose Twc: "<<pos.inverse()<<std::endl;
        }

        kfs_pos.emplace_back(cur_frame_ptr->GetPoseInverse());
        viewer->SetKeyFrames(kfs_pos);
        if(i % 10 == 0)
        {// 3. key frame

            extractor_ptr->operator()(cur_frame_ptr->GetImagePyramid(),
                                      cur_frame_ptr->GetScaleFactors(),
                                      2000,
                                      80,
                                      30,
                                      keyPoints_l,
                                      descriptors_l);

            cur_frame_ptr->SetKeys(keyPoints_l, cur_frame_ptr, descriptors_l);
            cur_frame_ptr->AssignFeaturesToGrid();
            imRight = cv::imread(imageRight[i], CV_LOAD_IMAGE_UNCHANGED);
//            cv::imshow("right", imRight);
            static auto right_camera_ptr = std::make_shared<PinholeCamera>(imLeft.cols, imLeft.rows, fx, fy, cx, cy);
//            right_camera_ptr->UndistortImage(imRight, imRight);
            // 4. add map points by stereo
            auto right_frame_ptr = std::make_shared<Frame>(imRight, 0, 5, 0.6, right_camera_ptr);
//            extractor_ptr->operator()(cur_frame_ptr->GetImagePyramid(),
//                                      cur_frame_ptr->GetScaleFactors(),
//                                      2000,
//                                      80,
//                                      30,
//                                      keyPoints_r,
//                                      descriptors_r);
//            right_frame_ptr->SetKeys(keyPoints_r, right_frame_ptr, descriptors_r);

            auto depth = local_ba->AddMapPointsByStereo(cur_frame_ptr, right_frame_ptr, d*fx, fx);
            float sum_err = 0, cnt = 0;
            for (size_t j = 0; j < depth.size(); j ++)
            {
                if(depth[j] > 0)
                {

                    int u = (int)cur_frame_ptr->GetKey(j)->kp.pt.x;
                    int v = (int)cur_frame_ptr->GetKey(j)->kp.pt.y;

                    double disp = disparity.at<uchar>(v, u);
                    disp = (disp)/fx;
                    float err = abs(depth[j]/5 - d/disp);
                    sum_err += err;
                    cnt ++;

                    depth[j] = d/disp;

                    Eigen::Vector3f norm;
                    norm[0] = (u-cx)/fx;
                    norm[1] = (v-cy)/fy;
                    norm[2] = 1;

//                    const auto& pt = cur_frame_ptr->GetKey(j);
//                    norm = camera_ptr->Cam2World(pt->px);

                    const Eigen::Matrix3f& R = cur_frame_ptr->GetRotation();
                    Eigen::Vector3f t = cur_frame_ptr->GetTranslation();
//                    t.x() += 0.5;
                    auto mp = std::make_shared<MapPoint>(R * norm * depth[j] + t, map_ptr, cur_frame_ptr, cur_frame_ptr->GetKey(j), cur_frame_ptr->GetDescriptor(j));

                    cur_frame_ptr->AddMapPoint(mp, j);
                    map_ptr->AddMapPoint(mp);
                    map_ptr->AddKeyFrame(cur_frame_ptr);
                }
            }
            std::cout<<" error: "<<sum_err<<" mean: "<<sum_err/cnt<<" cnt: "<<cnt<<std::endl;
            viewer->SetMapPoints(map_ptr->GetAllMapPoints(), cur_frame_ptr->GetMapPointMatches());
            keyPoints_ref = keyPoints_l;
            key_frame = cur_frame_ptr;
        }
        last_frame = cur_frame_ptr;
        // visualization
        cv::waitKey(2);
    }
    for(int i = 0;i < gtR.size();i += 1)
    {
        Eigen::Matrix4f pos = Eigen::Matrix4f::Identity();
        pos.block<3, 3>(0, 0) = gtR.at(i).cast<float>().transpose();
        pos.block<3, 1>(0 ,3) = -  pos.block<3, 3>(0, 0) * gtT.at(i).cast<float>();
        kfs_pos.emplace_back(pos);
    }
    cv::waitKey(0);
    viewer->RequestFinish();
}

int test_direct()
{
    std::vector<string> imageLeft;
    std::vector<string> imageRight;
    std::vector<double> timeStamp;

    LoadImages( "/media/lyc/ 其他/dataset/sequences/00",
                imageLeft, imageRight, timeStamp);

    cv::Mat left_img = cv::imread(imageLeft[0], 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng;
    int nPoints = 1;
    int boarder = 2;
    VecVector2d pixels_ref;
    vector<double> depth_ref;
    for (int i = 0; i < nPoints; i++)
    {
        int x = 130;//rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = 73;//rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }
    // estimates 01~05.png's pose using this information
    Sophus::SE3d T_cur_ref;

    //for (int i = 1; i < 6; i++) {  // 1~10
    cv::Mat img = cv::imread(imageLeft[1], 0);
    // try single layer by uncomment this line
    // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
    DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
    //}
    return 0;
}

void test_direct2()
{
    std::vector<string> imageLeft;
    std::vector<string> imageRight;
    std::vector<double> timeStamp;

    LoadImages( "/media/lyc/ 其他/dataset/sequences/00",
                imageLeft, imageRight, timeStamp);

    auto matcher_ptr = std::make_shared<Matcher>();

    cv::Mat left_img = cv::imread(imageLeft[0], 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng;
    int nPoints = 2000;
    int boarder = 20;
    VecVector2d pixels_ref;
    vector<double> depth_ref;
    auto map_ptr = std::make_shared<TRACKING_BENCH::Map>();

    auto camera_ptr = std::make_shared<PinholeCamera>(left_img.cols, left_img.rows,fx, fy, cx, cy);
    cv::Mat img = cv::imread(imageLeft[1], 0);

    auto ref_frame_ptr = std::make_shared<Frame>(left_img, 0, 4, 0.5, camera_ptr);
    auto cur_frame_ptr = std::make_shared<Frame>(img, 0, 4, 0.5, camera_ptr);

    std::vector<cv::KeyPoint> kps;
    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++)
    {
        int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
        kps.emplace_back(cv::Point2f((float)x,(float)y),0);
    }
    ref_frame_ptr->SetKeys(kps, ref_frame_ptr);

    for (int i = 0;i < nPoints; i++)
    {
        Eigen::Vector3f norm;
        norm[0] = (pixels_ref.at(i).x() - cx)/fx;
        norm[1] = (pixels_ref.at(i).y() - cy)/fy;
        norm[2] = 1;
        auto mp = std::make_shared<MapPoint>(norm * depth_ref[i], map_ptr, ref_frame_ptr, ref_frame_ptr->GetKey(i), cv::Mat());

        ref_frame_ptr->AddMapPoint(mp, i);
        map_ptr->AddMapPoint(mp);
    }
    ref_frame_ptr->SetPose(Eigen::Matrix4f::Identity());
    Eigen::Matrix4f init_gauss = Eigen::Matrix4f::Identity();
    cur_frame_ptr->SetPose(init_gauss);

    matcher_ptr->setDirectParam(3, 0, 10, 5, 0.0001);
    auto T_cur_ref = matcher_ptr->SparseImageAlign(cur_frame_ptr, ref_frame_ptr);
    std::cout<<"T: "<<T_cur_ref<<std::endl;

}

void test_image_align()
{
    std::vector<string> imageLeft;
    std::vector<string> imageRight;
    std::vector<double> timeStamp;

    LoadImages( "/media/lyc/ 其他/dataset/sequences/00",
                imageLeft, imageRight, timeStamp);

    auto matcher_ptr = std::make_shared<Matcher>();

    cv::Mat left_img = cv::imread(imageLeft[0], 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng;
    int nPoints = 1;
    int boarder = 20;
    VecVector2d pixels_ref;
    vector<double> depth_ref;
    auto map_ptr = std::make_shared<TRACKING_BENCH::Map>();

    auto camera_ptr = std::make_shared<PinholeCamera>(left_img.cols, left_img.rows,fx, fy, cx, cy);
    cv::Mat img = cv::imread(imageLeft[1], 0);

    auto ref_frame_ptr = std::make_shared<Frame>(left_img, 0, 4, 0.5, camera_ptr);
    auto cur_frame_ptr = std::make_shared<Frame>(img, 0, 4, 0.5, camera_ptr);

    std::vector<cv::KeyPoint> kps;
    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++)
    {
        int x = 130;//rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = 73;//rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
        kps.emplace_back(cv::Point2f((float)x,(float)y),0, 0, 0, 1);
    }
    ref_frame_ptr->SetKeys(kps, ref_frame_ptr);

    ref_frame_ptr->SetPose(Eigen::Matrix4f::Identity());
    Eigen::Matrix4f init_gauss = Eigen::Matrix4f::Identity();
    cur_frame_ptr->SetPose(init_gauss);
    for (int i = 0;i < nPoints; i++)
    {
        Eigen::Vector3f norm;
        norm[0] = (pixels_ref.at(i).x() - cx)/fx;
        norm[1] = (pixels_ref.at(i).y() - cy)/fy;
        norm[2] = 1;
        auto mp = std::make_shared<MapPoint>(norm * depth_ref[i], map_ptr, ref_frame_ptr, ref_frame_ptr->GetKey(i), cv::Mat());

        ref_frame_ptr->AddMapPoint(mp, i);
        map_ptr->AddMapPoint(mp);

        Eigen::Vector2f px_cur(130, 73);

        cv::Mat show1 = cur_frame_ptr->GetImagePyramid()[0].clone();
        cv::Mat show2 = ref_frame_ptr->GetImagePyramid()[0].clone();
        cv::cvtColor(show1, show1, CV_GRAY2BGR);
        cv::cvtColor(show2, show2, CV_GRAY2BGR);
        cv::circle(show2, cv::Point((int)px_cur[0], (int)px_cur[1]), 5, cv::Scalar(255, 0, 0), 5);

        std::cout<<"before "<<std::endl<<px_cur<<std::endl;
        matcher_ptr->FindMatchDirect(mp, cur_frame_ptr, px_cur);
        std::cout<<"after "<<std::endl<<px_cur<<std::endl;

        cv::circle(show1, cv::Point((int)px_cur[0], (int)px_cur[1]), 5, cv::Scalar(255, 0, 0), 5);

        cv::imshow("before", show2);
        cv::imshow("after", show1);
        cv::waitKey(0);
    }

    ref_frame_ptr->SetPose(Eigen::Matrix4f::Identity());
    cur_frame_ptr->SetPose(Eigen::Matrix4f::Identity());

//    const cv::Mat& cur_img, uint8_t* ref_patch_with_border, uint8_t* ref_patch, const int n_iter, Eigen::Vector2f& cur_ps_estimate, bool no_simd = false
    int half_patch_size = 4;
    uint8_t patch[half_patch_size*half_patch_size*4] __attribute__((aligned(16)));
    uint8_t patch_with_border[(half_patch_size+1)*(half_patch_size+1)*4] __attribute__((aligned(16)));

    Eigen::Vector2f px_scaled(65, 36.5);
    const int cur_step = ref_frame_ptr->GetImagePyramid()[1].step.p[0];

    for(int y=0; y<(half_patch_size+1)*2; ++y)
    {
        uint8_t *it = (uint8_t *) ref_frame_ptr->GetImagePyramid()[1].data + ((int)px_scaled[1] + y - half_patch_size-1) * cur_step + (int)px_scaled[0] - half_patch_size-1;
        for (int x = 0; x < (half_patch_size+1)*2; ++x, ++it)
        {
            int id = x + y*(half_patch_size+1)*2;
            patch_with_border[id] = *it;
        }
    }

    uint8_t* patch_ptr = patch;
    for(int y = 1; y < half_patch_size*2 + 1; ++y, patch_ptr += half_patch_size*2)
    {
        uint8_t* y_ptr = patch_with_border + y * (half_patch_size*2 + 2) + 1;
        for(int x = 0; x < half_patch_size*2; ++x)
            patch_ptr[x] = y_ptr[x];
    }

    cv::Mat show1 = cur_frame_ptr->GetImagePyramid()[1].clone();
    cv::Mat show2 = ref_frame_ptr->GetImagePyramid()[1].clone();
    cv::cvtColor(show1, show1, CV_GRAY2BGR);
    cv::cvtColor(show2, show2, CV_GRAY2BGR);

    cv::circle(show2, cv::Point((int)px_scaled[0], (int)px_scaled[1]), 5, cv::Scalar(255, 0, 0), 5);

    std::cout<<" before: "<<px_scaled<<std::endl;
    matcher_ptr->Align2D(cur_frame_ptr->GetImagePyramid()[1], patch_with_border, patch, 40, px_scaled);
    std::cout<<" align: "<<px_scaled<<std::endl;
    cv::circle(show1, cv::Point((int)px_scaled[0], (int)px_scaled[1]), 5, cv::Scalar(255, 0, 0), 5);

    cv::imshow("before", show2);
    cv::imshow("after", show1);
    cv::waitKey(0);
}

int main()
{
    //test_direct();
    //test_direct2();
    test_projection();
//    test_image_align();
    return 0;
}


/*

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

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

This file is part of Chaste.

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

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

*/

#ifndef _RUNGEKUTTAFEHLBERGIVPODESOLVER_HPP_
#define _RUNGEKUTTAFEHLBERGIVPODESOLVER_HPP_

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

#include "AbstractOneStepIvpOdeSolver.hpp"

/**
 * A concrete one step ODE solver class that employs the Runge Kutta
 * Fehlberg adaptive solver (RKF45).
 *
 * This solver is good for problems where you need to be able to
 * guarantee the accuracy of the answer as it is specified via the
 * tolerance parameter.
 *
 * The solver should also be reasonably fast as it increases the
 * timestep when the solutions are changing slowly, whilst maintaining
 * accuracy.
 */
class RungeKuttaFehlbergIvpOdeSolver : public AbstractIvpOdeSolver
{
friend class TestRungeKuttaFehlbergIvpOdeSolver;

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

    /*
     * All these are here for more efficient memory allocation, rather than
     * because they need to be member variables.
     */

    double m1932o2197;  /**< Working memory: numerical value for the fraction 1932/2197.  */
    double m7200o2197;  /**< Working memory: numerical value for the fraction 7200/2197.  */
    double m7296o2197;  /**< Working memory: numerical value for the fraction 7296/2197.  */
    double m12o13;      /**< Working memory: numerical value for the fraction 12/13.      */
    double m439o216;    /**< Working memory: numerical value for the fraction 439/216.    */
    double m3680o513;   /**< Working memory: numerical value for the fraction 3680/513.   */
    double m845o4104;   /**< Working memory: numerical value for the fraction 845/4104.   */
    double m8o27;       /**< Working memory: numerical value for the fraction 8/27.       */
    double m3544o2565;  /**< Working memory: numerical value for the fraction 3544/2565.  */
    double m1859o4104;  /**< Working memory: numerical value for the fraction 1859/4104.  */
    double m1o360;      /**< Working memory: numerical value for the fraction 1/360.      */
    double m128o4275;   /**< Working memory: numerical value for the fraction 128/4275.   */
    double m2197o75240; /**< Working memory: numerical value for the fraction 2197/75240. */
    double m2o55;       /**< Working memory: numerical value for the fraction 2/55.       */
    double m25o216;     /**< Working memory: numerical value for the fraction 25/216.     */
    double m1408o2565;  /**< Working memory: numerical value for the fraction 1408/2565.  */
    double m2197o4104;  /**< Working memory: numerical value for the fraction 2197/4104.  */

    std::vector<double> mError; /**< Error expression, used to adjust the timestep in the RKF45 method. */

    std::vector<double> mk1;  /**< Working memory: expression k1 in the RKF45 method.  */
    std::vector<double> mk2;  /**< Working memory: expression k2 in the RKF45 method.  */
    std::vector<double> mk3;  /**< Working memory: expression k3 in the RKF45 method.  */
    std::vector<double> mk4;  /**< Working memory: expression k4 in the RKF45 method.  */
    std::vector<double> mk5;  /**< Working memory: expression k5 in the RKF45 method.  */
    std::vector<double> mk6;  /**< Working memory: expression k6 in the RKF45 method.  */
    std::vector<double> myk2; /**< Working memory: expression yk2 in the RKF45 method. */
    std::vector<double> myk3; /**< Working memory: expression yk3 in the RKF45 method. */
    std::vector<double> myk4; /**< Working memory: expression yk4 in the RKF45 method. */
    std::vector<double> myk5; /**< Working memory: expression yk5 in the RKF45 method. */
    std::vector<double> myk6; /**< Working memory: expression yk6 in the RKF45 method. */

protected:

    /**
     * Method that actually performs the solving on behalf of the public Solve methods.
     *
     * @param rSolution  an ODE solution to input data into if requited
     * @param pAbstractOdeSystem  the ODE system to solve
     * @param rCurrentYValues  the current (initial) state; results will also be returned in here
     * @param rWorkingMemory  working memory; same size as rCurrentYValues
     * @param startTime  initial time
     * @param endTime  time to solve to
     * @param maxTimeStep  the maximum size of timestep allowable
     * @param minTimeStep  the maximum size of timestep allowable (to prevent huge loops)
     * @param tolerance  how accurate the numerical solution must be
     * @param outputSolution whether to output into rSolution (or save time by not doing)
     */
    void InternalSolve(OdeSolution& rSolution,
                       AbstractOdeSystem* pAbstractOdeSystem,
                       std::vector<double>& rCurrentYValues,
                       std::vector<double>& rWorkingMemory,
                       double startTime,
                       double endTime,
                       double maxTimeStep,
                       double minTimeStep,
                       double tolerance,
                       bool outputSolution);

    /**
     * Calculate the solution to the ODE system at the next timestep.
     * Updates the mError vector with current error.
     *
     * @param pAbstractOdeSystem  the ODE system to solve
     * @param timeStep  dt
     * @param time  the current time
     * @param rCurrentYValues  the current (initial) state
     * @param rNextYValues  the state at the next timestep
     */
    void CalculateNextYValue(AbstractOdeSystem* pAbstractOdeSystem,
                             double timeStep,
                             double time,
                             std::vector<double>& rCurrentYValues,
                             std::vector<double>& rNextYValues);

    /**
     * Use the error approximation of the last call to the CalculateNextYValue()
     * method to change the time step appropriately.
     *
     * @param rCurrentStepSize  the current step size being used (returns answer via this reference)
     * @param rError  the error in the approximation at this time step
     * @param rTolerance  the tolerance required
     * @param rMaxTimeStep  the maximum timestep to be used
     * @param rMinTimeStep  the minimum timestep to be used (to prevent huge loops)
     */
    void AdjustStepSize(double& rCurrentStepSize,
                        const double& rError,
                        const double& rTolerance,
                        const double& rMaxTimeStep,
                        const double& rMinTimeStep);

public:

    /**
     * Constructor.
     */
    RungeKuttaFehlbergIvpOdeSolver();

    /**
     * Solves a system of ODEs using a specified one-step ODE solver and returns
     * the solution as an OdeSolution object.
     *
     * @param pAbstractOdeSystem  pointer to the concrete ODE system to be solved
     * @param rYValues  a standard vector specifying the intial condition of each
     *                  solution variable in the system (this can be the initial
     *                  conditions vector stored in the ODE system)
     * @param startTime  the time at which the initial conditions are specified
     * @param endTime  the time to which the system should be solved and the solution
     *                 returned
     * @param timeStep  the time interval to be used by the solver
     * @param ignoredSamplingTime  the interval at which to sample the solution to the ODE system
     *                             (ignored in this class as the timestep is variable)
     *
     * @return OdeSolution is an object containing an integer of the number of
     * equations, a stdAbstractOdeSystem::vector of times and a std::vector of std::vectors where
     * each of those vectors contains the solution for one variable of the ODE
     * system at those times.
     */
    OdeSolution Solve(AbstractOdeSystem* pAbstractOdeSystem,
                      std::vector<double>& rYValues,
                      double startTime,
                      double endTime,
                      double timeStep,
                      double ignoredSamplingTime);

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

};

#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(RungeKuttaFehlbergIvpOdeSolver)

#endif //_RUNGEKUTTAFEHLBERGIVPODESOLVER_HPP_