#pragma once

#include <crab/cfg/cfg.hpp>
#include <crab/cfg/var_factory.hpp>
#include <crab/iterators/fwd_fixpoint_iterators.hpp>
#include <crab/analysis/fwd_analyzer.hpp>
#include <crab/analysis/abs_transformer.hpp>
#include <crab/analysis/dataflow/liveness.hpp>

#include <boost/shared_ptr.hpp>
#include <boost/unordered_map.hpp>
#include <boost/noncopyable.hpp>
#include <boost/range/iterator_range.hpp>

namespace crab {

  namespace analyzer {

    /**
     * Compute necessary preconditions by computing the abstract
     * least fixpoint of the reversed CFG. At each step the
     * precondition is refined with an invariant computed by a
     * forward analysis.
     **/
    template <typename NodeName, typename CFG, typename AbsDom>
    class necessary_preconditions_fixpoint_iterator:
      public ikos::interleaved_fwd_fixpoint_iterator
                   <NodeName, cfg::cfg_rev<CFG>, AbsDom> {

      typedef ikos::interleaved_fwd_fixpoint_iterator
      <NodeName, cfg::cfg_rev<CFG>, AbsDom> fixpoint_iterator_t;

      typedef intra_abs_transformer<AbsDom> abs_fwd_tr_t;
      typedef typename CFG::basic_block_label_t bb_label_t;
      typedef typename CFG::statement_t stmt_t;
      typedef boost::unordered_map<bb_label_t, AbsDom> bb_abstract_map_t;
      typedef boost::unordered_map<stmt_t*, AbsDom> pp_abstract_map_t;

      CFG m_cfg;
      // Postcondition (i.e, final states) that we want to propagate backwards
      AbsDom m_postcond;
      // necessary preconditions
      bb_abstract_map_t m_preconditions;
      // invariants
      bb_abstract_map_t m_invariants;

      /**
       * Compute necessary preconditions for a basic block
       **/
      virtual void analyze(bb_label_t node, AbsDom &precond) override {
	typedef intra_necessary_preconditions_abs_transformer
	  <AbsDom, pp_abstract_map_t> abs_bwd_tr_t;

	auto &bb = m_cfg.get_node (node);

	CRAB_LOG("backward-fixpoint",
		 crab::outs () << "Post at " << cfg_impl::get_label_str(node) << ": "
		               << precond << "\n");

	// invariants that hold at the entry of the block
	AbsDom invariant = m_invariants [node];
	// rebuild local invariants that hold at each program point.
	abs_fwd_tr_t F(&invariant);
	pp_abstract_map_t pp_invariants;
	for(auto &s: boost::make_iterator_range(bb.begin(),bb.end())) {
	  pp_invariants.insert (std::make_pair(&s, F.inv()));
	  CRAB_LOG("backward-fixpoint",
		   crab::outs () << "\tRebuilding at statement " << s << " inv="
		                 << F.inv() << "\n");
	  s.accept (&F);
	}

	CRAB_LOG("backward-fixpoint",
		 crab::outs ()
		 << "Done forward propagation at each program point \n"
		 << "Starting backward propagation ... \n");

	// compute precondition at the entry of the block
	abs_bwd_tr_t B(&precond, pp_invariants);
	for(auto &s: boost::make_iterator_range(bb.rbegin(),bb.rend()))
	  s.accept (&B);
	CRAB_LOG("backward-fixpoint",
		 crab::outs () << "Pre at " << cfg_impl::get_label_str(node) << ": "
		               << precond << "\n");

      }

      virtual void process_pre(bb_label_t /*node*/,
			       AbsDom /*postcond*/) override { }

      /**
       *  Store necessary preconditions
       **/
      virtual void process_post (bb_label_t node,
				 AbsDom precond) override {
	m_preconditions.insert(std::make_pair (node, precond));
      }

    public:

      typedef typename fixpoint_iterator_t::wto_t wto_t;
      typedef bb_abstract_map_t precond_map_t;
      typedef typename precond_map_t::iterator iterator;
      typedef typename precond_map_t::const_iterator const_iterator;

      necessary_preconditions_fixpoint_iterator
      (CFG cfg, const wto_t *wto, AbsDom postcond,
       /* fixpoint parameters */
       unsigned int widening_delay,
       unsigned int descending_iterations,
       size_t jump_set_size)
	: fixpoint_iterator_t(cfg::cfg_rev<CFG> (cfg), wto,
			      widening_delay, descending_iterations, jump_set_size),
	  m_cfg (cfg),
	  m_postcond (postcond) { }

      template <typename Range>
      void Run (Range invariants) {
	m_invariants = bb_abstract_map_t(invariants.begin(), invariants.end());
	this->run (m_postcond);
      }

      iterator begin () { return m_preconditions.begin(); }

      iterator end () { return m_preconditions.end();   }

      const_iterator begin () const { return m_preconditions.begin(); }

      const_iterator end () const { return m_preconditions.end();   }

      AbsDom operator[](bb_label_t node) const {
	auto it = m_preconditions.find (node);
	if (it != m_preconditions.end())
	  return it->second;
	else
	  return AbsDom::top();
      }

      const wto_t& get_WTO() const {
	return this->get_wto();
      }

    };

    /**
     *  A forward-backward analyzer to compute necessary
     *  preconditions based on Cousot&Cousot's JLP'92
     *
     * The API of this class is such that from outside it looks pretty
     * much like a forward analyzer.
     **/
    template<typename CFG, typename AbsDom>
    class intra_forward_backward_analyzer: public boost::noncopyable {
    public:

      typedef CFG cfg_t;
      typedef typename CFG::basic_block_label_t bb_label_t;
      typedef typename CFG::varname_t varname_t;
      typedef typename CFG::number_t number_t;
      // used for checkers
      typedef AbsDom abs_dom_t;
      typedef boost::shared_ptr<intra_abs_transformer<AbsDom> > abs_tr_ptr;

    private:

      typedef intra_fwd_analyzer<CFG, AbsDom> fwd_analyzer_t;
      typedef necessary_preconditions_fixpoint_iterator<bb_label_t, CFG, AbsDom>
      bwd_fixpoint_iterator_t;
      typedef boost::unordered_map<bb_label_t, AbsDom> invariant_map_t;
      typedef typename bwd_fixpoint_iterator_t::precond_map_t precond_map_t;
      typedef typename bwd_fixpoint_iterator_t::wto_t bwd_wto_t;
      typedef liveness<CFG> liveness_t;

    public:

      typedef typename fwd_analyzer_t::assumption_map_t assumption_map_t;
      // bwd_wto_t and wto_t are different types because bwd_wto_t is
      // over the reversed CFG.
      typedef typename fwd_analyzer_t::wto_t wto_t;

    private:

      CFG m_cfg;
      // We keep the two wto's (from forward and reversed CFGs) to
      // avoid recompute them during the below iterative process.
      // Only the forward wto is exposed to outside clients.
      const wto_t* m_wto;
      const bwd_wto_t* m_b_wto;

      // to keep the results of the last iteration
      invariant_map_t m_pre_invariants;
      invariant_map_t m_post_invariants;
      precond_map_t m_preconditions;

      // if a checker uses this analyzer to prove a property it will
      // need an abstract transformer to rebuild local invariants.
      abs_tr_ptr m_checker_abs_tr;

      void store_forward_analysis_results (fwd_analyzer_t &f) {
	m_pre_invariants = invariant_map_t(f.pre_begin(), f.pre_end());
	m_post_invariants = invariant_map_t(f.post_begin(), f.post_end());
      }

      void store_analysis_results (fwd_analyzer_t &f, bwd_fixpoint_iterator_t &b) {
	store_forward_analysis_results(f);
	m_preconditions = precond_map_t(b.begin(), b.end());
      }

    public:

      typedef typename bwd_fixpoint_iterator_t::iterator iterator;
      typedef typename bwd_fixpoint_iterator_t::const_iterator const_iterator;

      intra_forward_backward_analyzer (CFG cfg)
	: m_cfg(cfg),
	  m_wto(nullptr),
	  m_b_wto(nullptr),
	  m_checker_abs_tr(new intra_abs_transformer<AbsDom> (nullptr))
      {}

      ~intra_forward_backward_analyzer () {
	if (m_wto) delete m_wto;
	if (m_b_wto) delete m_b_wto;
      }

      /**
       * Perform the refining forward-backward loop.
       **/
      void run (AbsDom init_states, AbsDom final_states,
		// behaves as a standard forward analysis
		bool only_forward,
		// assumptions
		assumption_map_t &assumptions,
		// liveness information
		const liveness_t* live,
		// parameters for each forward or backward analysis
		unsigned int widening_delay=1,
		unsigned int descending_iters=UINT_MAX,
		size_t jump_set_size=0) {

	CRAB_LOG ("backward",
		  crab::outs() << "Initial states=" << init_states << "\n";
		  crab::outs() << "Final states=" << final_states << "\n");

	crab::CrabStats::count ("CombinedForwardBackward.invocations");
	unsigned iters = 0;
	while (true) {
	  iters++;
          crab::CrabStats::count ("CombinedForwardBackward.iterations");
	  CRAB_VERBOSE_IF(1, crab::outs() << "Iteration " << iters << "\n"
			                  << "Started forward analysis.\n";);

	  crab::CrabStats::resume ("CombinedForwardBackward.ForwardPass");
	  // run forward analysis computing invariants
	  fwd_analyzer_t F (m_cfg, m_wto, init_states, live,
			    widening_delay, descending_iters, jump_set_size);
	  F.run (assumptions);
	  crab::CrabStats::stop ("CombinedForwardBackward.ForwardPass");

	  // reuse wto for next iteration
	  if (iters == 1) m_wto = new wto_t(F.get_wto ());

	  CRAB_VERBOSE_IF(1, crab::outs () << "Finished forward analysis.\n";);

	  CRAB_LOG("backward",
		   for (auto &kv: boost::make_iterator_range (F.pre_begin(),
							      F.pre_end ())) {
		     crab::outs () << cfg_impl::get_label_str (kv.first)
				   << ":\n" << kv.second << "\n";
		   });

	  if (only_forward) {
	    store_forward_analysis_results(F);
	    CRAB_VERBOSE_IF(1, crab::outs () << "\nSkipped backward pass.\n";);
	    break;
	  }

	  CRAB_VERBOSE_IF(1, crab::outs () << "Started backward analysis.\n";);

	  crab::CrabStats::resume ("CombinedForwardBackward.BackwardPass");
	  // run backward analysis computing necessary preconditions
	  // refined with invariants
	  bwd_fixpoint_iterator_t B (m_cfg, m_b_wto, final_states,
				     widening_delay, descending_iters, jump_set_size);
	  B.Run (boost::make_iterator_range(F.pre_begin(), F.pre_end()));
	  crab::CrabStats::stop ("CombinedForwardBackward.BackwardPass");

	  CRAB_VERBOSE_IF(1, crab::outs () << "Finished backward analysis.\n";);

	  CRAB_LOG("backward",
		   for (auto &kv: boost::make_iterator_range (B.begin(),
							      B.end ())) {
		     crab::outs () << cfg_impl::get_label_str (kv.first)
				   << ":\n" << kv.second << "\n";
		   }
		   crab::outs () << "\n");

	  AbsDom new_init_states = B[m_cfg.entry ()];
	  if (init_states <= new_init_states) {
            //CRAB_LOG ("backward",
	    //           crab::outs() << "Cannot refine more initial states.\n");
	    store_analysis_results(F,B);
	    break;
	  } else {
            CRAB_LOG ("backward",
		      crab::outs() << "Initial state refined: \n"
		                   << init_states << " ==> ");

	    if (iters > descending_iters) {
	      // ensure termination by using narrowing
	      init_states = init_states && new_init_states;
	    } else {
	      init_states = init_states & new_init_states;
	    }

            CRAB_LOG ("backward",
		      crab::outs() << init_states << "\n");

	  }

	  // reuse wto for next iteration
	  if (iters == 1) m_b_wto = new bwd_wto_t(B.get_WTO ());

	} // end while true

	CRAB_VERBOSE_IF(1, crab::outs() << "Combined forward+backward analysis done after "
			                << iters << " iterations.\n";);
      }

      // Return the invariants that hold at the entry of b
      AbsDom operator[] (bb_label_t b) const {
        return get_pre (b);
      }

      // Return the invariants that hold at the entry of b
      AbsDom get_pre (bb_label_t b) const {
        auto it = m_pre_invariants.find (b);
        if (it == m_pre_invariants.end ())
          return abs_dom_t::top ();
        else
          return it->second;
      }

      // Return the invariants that hold at the exit of b
      AbsDom get_post (bb_label_t b) const {
        auto it = m_post_invariants.find (b);
        if (it == m_post_invariants.end ())
          return abs_dom_t::top ();
        else
          return it->second;
      }

      // Return the necessary preconditions for basic block b
      AbsDom get_preconditions(bb_label_t b) const {
	auto it = m_preconditions.find (b);
	if (it != m_preconditions.end())
	  return it->second;
	else
	  return AbsDom::top();
      }

      // Return the wto of the cfg
      const wto_t& get_wto () const {
	assert (m_wto);
	return *m_wto;
      }

      // API for checkers
      CFG get_cfg (void) { return m_cfg; }

      // API for checkers
      abs_tr_ptr get_abs_transformer (AbsDom &inv) {
	m_checker_abs_tr->set (inv);
	return m_checker_abs_tr;
      }

    };

  } // end namespace
} // end namespace


#include <stan/math/mix/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <test/unit/math/rev/scal/fun/util.hpp>
#include <test/unit/math/mix/scal/fun/nan_util.hpp>

TEST(AgradFwdGammaP, FvarVar_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(0.5, 1.0);
  fvar<var> z(1.0, 1.0);
  fvar<var> a = stan::math::gamma_p(x, z);

  EXPECT_FLOAT_EQ(boost::math::gamma_p(0.5, 1.0), a.val_.val());
  EXPECT_FLOAT_EQ(-0.18228334, a.d_.val());

  AVEC y = createAVEC(x.val_, z.val_);
  VEC g;
  a.val_.grad(y, g);
  EXPECT_FLOAT_EQ(-0.38983709, g[0]);
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5, 1.0), g[1]);
}
TEST(AgradFwdGammaP, Double_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  double x(0.5);
  fvar<var> z(1.0, 1.0);
  fvar<var> a = stan::math::gamma_p(x, z);

  EXPECT_FLOAT_EQ(boost::math::gamma_p(0.5, 1.0), a.val_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val());

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.val_.grad(y, g);
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5, 1.0), g[0]);
}
TEST(AgradFwdGammaP, FvarVar_Double_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(0.5, 1.0);
  double z(1.0);
  fvar<var> a = stan::math::gamma_p(x, z);

  EXPECT_FLOAT_EQ(boost::math::gamma_p(0.5, 1.0), a.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.val_.grad(y, g);
  EXPECT_FLOAT_EQ(-0.38983709, g[0]);
}
TEST(AgradFwdGammaP, FvarVar_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(0.5, 1.0);
  fvar<var> z(1.0, 1.0);
  fvar<var> a = stan::math::gamma_p(x, z);

  EXPECT_FLOAT_EQ(boost::math::gamma_p(0.5, 1.0), a.val_.val());
  EXPECT_FLOAT_EQ(-0.18228334, a.d_.val());

  AVEC y = createAVEC(x.val_, z.val_);
  VEC g;
  a.d_.grad(y, g);
  EXPECT_FLOAT_EQ(0.19403456, g[0]);
  EXPECT_FLOAT_EQ(0.096204743, g[1]);
}
TEST(AgradFwdGammaP, Double_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  double x(0.5);
  fvar<var> z(1.0, 1.0);
  fvar<var> a = stan::math::gamma_p(x, z);

  EXPECT_FLOAT_EQ(boost::math::gamma_p(0.5, 1.0), a.val_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val());

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.d_.grad(y, g);
  EXPECT_FLOAT_EQ(-0.31133062, g[0]);
}
TEST(AgradFwdGammaP, FvarVar_Double_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(0.5, 1.0);
  double z(1.0);
  fvar<var> a = stan::math::gamma_p(x, z);

  EXPECT_FLOAT_EQ(boost::math::gamma_p(0.5, 1.0), a.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.d_.grad(y, g);
  EXPECT_FLOAT_EQ(-0.21349931, g[0]);
}

TEST(AgradFwdGammaP, FvarFvarVar_FvarFvarVar_1stDeriv) {
  using boost::math::gamma_p;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_p(x, y);

  EXPECT_FLOAT_EQ(gamma_p(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0.40753385, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.val_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(-0.38983709, g[0]);
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5, 1.0), g[1]);
}
TEST(AgradFwdGammaP, Double_FvarFvarVar_1stDeriv) {
  using boost::math::gamma_p;
  using stan::math::fvar;
  using stan::math::var;

  double x(0.5);

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_p(x, y);

  EXPECT_FLOAT_EQ(gamma_p(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.val_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5, 1.0), g[0]);
}
TEST(AgradFwdGammaP, FvarFvarVar_Double_1stDeriv) {
  using boost::math::gamma_p;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  double y(1.0);

  fvar<fvar<var> > a = gamma_p(x, y);

  EXPECT_FLOAT_EQ(gamma_p(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(-0.38983709, g[0]);
}

TEST(AgradFwdGammaP, FvarFvarVar_FvarFvarVar_2ndDeriv_x) {
  using boost::math::gamma_p;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_p(x, y);

  EXPECT_FLOAT_EQ(gamma_p(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0.40753385, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.val_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(-0.21349931, g[0]);
  EXPECT_FLOAT_EQ(0.40753537, g[1]);
}
TEST(AgradFwdGammaP, FvarFvarVar_FvarFvarVar_2ndDeriv_y) {
  using boost::math::gamma_p;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_p(x, y);

  EXPECT_FLOAT_EQ(gamma_p(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0.40753385, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.d_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(0.40753385, g[0]);
  EXPECT_FLOAT_EQ(-0.31133062, g[1]);
}
TEST(AgradFwdGammaP, Double_FvarFvarVar_2ndDeriv) {
  using boost::math::gamma_p;
  using stan::math::fvar;
  using stan::math::var;

  double x(0.5);

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_p(x, y);

  EXPECT_FLOAT_EQ(gamma_p(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(-0.31133062, g[0]);
}
TEST(AgradFwdGammaP, FvarFvarVar_Double_2ndDeriv) {
  using boost::math::gamma_p;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  double y(1.0);

  fvar<fvar<var> > a = gamma_p(x, y);

  EXPECT_FLOAT_EQ(gamma_p(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(-0.21349931, g[0]);
}

TEST(AgradFwdGammaP, FvarFvarVar_FvarFvarVar_3rdDeriv) {
  using boost::math::gamma_p;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_p(x, y);

  EXPECT_FLOAT_EQ(gamma_p(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0.40753385, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.d_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(-0.22403987, g[0]);
  EXPECT_FLOAT_EQ(-0.40374705, g[1]);
}
TEST(AgradFwdGammaP, Double_FvarFvarVar_3rdDeriv) {
  using boost::math::gamma_p;
  using stan::math::fvar;
  using stan::math::var;

  double x(0.5);

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;
  y.val_.d_ = 1.0;

  fvar<fvar<var> > a = gamma_p(x, y);

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(0.57077283, g[0]);
}
TEST(AgradFwdGammaP, FvarFvarVar_Double_3rdDeriv) {
  using boost::math::gamma_p;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;
  x.d_.val_ = 1.0;

  double y(1.0);

  fvar<fvar<var> > a = gamma_p(x, y);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.d_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(0.5462361, g[0]);
}

struct gamma_p_fun {
  template <typename T0, typename T1>
  inline typename boost::math::tools::promote_args<T0, T1>::type operator()(
      const T0 arg1, const T1 arg2) const {
    return gamma_p(arg1, arg2);
  }
};

TEST(AgradFwdGammaP, nan) {
  gamma_p_fun gamma_p_;
  test_nan_mix(gamma_p_, 3.0, 5.0, false);
}


// Boost.Geometry

// Copyright (c) 2018 Yaghyavardhan Singh Khangarot, Hyderabad, India.
// Contributed and/or modified by Yaghyavardhan Singh Khangarot,
//   as part of Google Summer of Code 2018 program.

// This file was modified by Oracle on 2018-2021.
// Modifications copyright (c) 2018-2021 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DISCRETE_FRECHET_DISTANCE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DISCRETE_FRECHET_DISTANCE_HPP

#include <algorithm>

#ifdef BOOST_GEOMETRY_DEBUG_FRECHET_DISTANCE
#include <iostream>
#endif

#include <iterator>
#include <utility>
#include <vector>
#include <limits>

#include <boost/geometry/algorithms/detail/dummy_geometries.hpp>
#include <boost/geometry/algorithms/detail/throw_on_empty_input.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/discrete_distance/cartesian.hpp>
#include <boost/geometry/strategies/discrete_distance/geographic.hpp>
#include <boost/geometry/strategies/discrete_distance/spherical.hpp>
#include <boost/geometry/strategies/distance_result.hpp>
#include <boost/geometry/util/range.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace discrete_frechet_distance
{

// TODO: The implementation should calculate comparable distances

template <typename SizeType1, typename SizeType2, typename ResultType>
class coup_mat
{
public:
    coup_mat(SizeType1 w, SizeType2 h)
        : m_data(w * h,-1), m_width(w), m_height(h)
    {}

    ResultType & operator()(SizeType1 i, SizeType2 j)
    {
        BOOST_GEOMETRY_ASSERT(i < m_width && j < m_height);
        return m_data[j * m_width + i];
    }

private:
    std::vector<ResultType> m_data;
    SizeType1 m_width;
    SizeType2 m_height;
};

struct linestring_linestring
{
    template <typename Linestring1, typename Linestring2, typename Strategies>
    static inline auto apply(Linestring1 const& ls1, Linestring2 const& ls2,
                             Strategies const& strategies)
    {
        typedef typename distance_result
            <
                typename point_type<Linestring1>::type,
                typename point_type<Linestring2>::type,
                Strategies
            >::type result_type;
        typedef typename boost::range_size<Linestring1>::type size_type1;
        typedef typename boost::range_size<Linestring2>::type size_type2;

        boost::geometry::detail::throw_on_empty_input(ls1);
        boost::geometry::detail::throw_on_empty_input(ls2);

        // We can assume the inputs are not empty
        auto const strategy = strategies.distance(dummy_point(), dummy_point());

        size_type1 const a = boost::size(ls1);
        size_type2 const b = boost::size(ls2);

        //Coupling Matrix CoupMat(a,b,-1);
        coup_mat<size_type1, size_type2, result_type> coup_matrix(a, b);

        result_type const not_feasible = -100;
        //findin the Coupling Measure
        for (size_type1 i = 0 ; i < a ; i++ )
        {
            for (size_type2 j = 0 ; j < b ; j++ )
            {
                result_type dis = strategy.apply(range::at(ls1,i), range::at(ls2,j));
                if(i==0 && j==0)
                    coup_matrix(i,j) = dis;
                else if(i==0 && j>0)
                    coup_matrix(i,j) =
                        (std::max)(coup_matrix(i,j-1), dis);
                else if(i>0 && j==0)
                    coup_matrix(i,j) =
                        (std::max)(coup_matrix(i-1,j), dis);
                else if(i>0 && j>0)
                    coup_matrix(i,j) =
                        (std::max)((std::min)(coup_matrix(i,j-1),
                                              (std::min)(coup_matrix(i-1,j),
                                                         coup_matrix(i-1,j-1))),
                                   dis);
                else
                    coup_matrix(i,j) = not_feasible;
            }
        }

        #ifdef BOOST_GEOMETRY_DEBUG_FRECHET_DISTANCE
        //Print CoupLing Matrix
        for(size_type i = 0; i <a; i++)
        {
            for(size_type j = 0; j <b; j++)
            std::cout << coup_matrix(i,j) << " ";
            std::cout << std::endl;
        }
        #endif

        return coup_matrix(a-1,b-1);
    }
};

}} // namespace detail::frechet_distance
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{
template
<
    typename Geometry1,
    typename Geometry2,
    typename Tag1 = typename tag<Geometry1>::type,
    typename Tag2 = typename tag<Geometry2>::type
>
struct discrete_frechet_distance : not_implemented<Tag1, Tag2>
{};

template <typename Linestring1, typename Linestring2>
struct discrete_frechet_distance
    <
        Linestring1,
        Linestring2,
        linestring_tag,
        linestring_tag
    >
    : detail::discrete_frechet_distance::linestring_linestring
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

namespace resolve_strategy {

template
<
    typename Strategies,
    bool IsUmbrella = strategies::detail::is_umbrella_strategy<Strategies>::value
>
struct discrete_frechet_distance
{
    template <typename Geometry1, typename Geometry2>
    static inline auto apply(Geometry1 const& geometry1, Geometry2 const& geometry2,
                             Strategies const& strategies)
    {
        return dispatch::discrete_frechet_distance
            <
                Geometry1, Geometry2
            >::apply(geometry1, geometry2, strategies);
    }
};

template <typename Strategy>
struct discrete_frechet_distance<Strategy, false>
{
    template <typename Geometry1, typename Geometry2>
    static inline auto apply(Geometry1 const& geometry1, Geometry2 const& geometry2,
                             Strategy const& strategy)
    {
        using strategies::discrete_distance::services::strategy_converter;
        return dispatch::discrete_frechet_distance
            <
                Geometry1, Geometry2
            >::apply(geometry1, geometry2,
                     strategy_converter<Strategy>::get(strategy));
    }
};

template <>
struct discrete_frechet_distance<default_strategy, false>
{
    template <typename Geometry1, typename Geometry2>
    static inline auto apply(Geometry1 const& geometry1, Geometry2 const& geometry2,
                             default_strategy const&)
    {
        typedef typename strategies::discrete_distance::services::default_strategy
            <
                Geometry1, Geometry2
            >::type strategies_type;

        return dispatch::discrete_frechet_distance
            <
                Geometry1, Geometry2
            >::apply(geometry1, geometry2, strategies_type());
    }
};

} // namespace resolve_strategy

/*!
\brief Calculate discrete Frechet distance between two geometries (currently
       works for LineString-LineString) using specified strategy.
\ingroup discrete_frechet_distance
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam Strategy A type fulfilling a DistanceStrategy concept
\param geometry1 Input geometry
\param geometry2 Input geometry
\param strategy Distance strategy to be used to calculate Pt-Pt distance

\qbk{distinguish,with strategy}
\qbk{[include reference/algorithms/discrete_frechet_distance.qbk]}

\qbk{
[heading Available Strategies]
\* [link geometry.reference.strategies.strategy_distance_pythagoras Pythagoras (cartesian)]
\* [link geometry.reference.strategies.strategy_distance_haversine Haversine (spherical)]
[/ \* more (currently extensions): Vincenty\, Andoyer (geographic) ]

[heading Example]
[discrete_frechet_distance_strategy]
[discrete_frechet_distance_strategy_output]
}
*/
template <typename Geometry1, typename Geometry2, typename Strategy>
inline auto discrete_frechet_distance(Geometry1 const& geometry1,
                                      Geometry2 const& geometry2,
                                      Strategy const& strategy)
{
    return resolve_strategy::discrete_frechet_distance
            <
                Strategy
            >::apply(geometry1, geometry2, strategy);
}

// Algorithm overload using default Pt-Pt distance strategy

/*!
\brief Calculate discrete Frechet distance between two geometries (currently
       work for LineString-LineString).
\ingroup discrete_frechet_distance
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\param geometry1 Input geometry
\param geometry2 Input geometry

\qbk{[include reference/algorithms/discrete_frechet_distance.qbk]}

\qbk{
[heading Example]
[discrete_frechet_distance]
[discrete_frechet_distance_output]
}
*/
template <typename Geometry1, typename Geometry2>
inline auto discrete_frechet_distance(Geometry1 const& geometry1,
                                      Geometry2 const& geometry2)
{
    return resolve_strategy::discrete_frechet_distance
            <
                default_strategy
            >::apply(geometry1, geometry2, default_strategy());
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DISCRETE_FRECHET_DISTANCE_HPP


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Copyright (c) 2004   Gerald I. Evenden
// Copyright (c) 2012   Martin Raspaud

// See also (section 4.4.3.2):
//   http://www.eumetsat.int/en/area4/msg/news/us_doc/cgms_03_26.pdf

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_GEOS_HPP
#define BOOST_GEOMETRY_PROJECTIONS_GEOS_HPP

#include <boost/math/special_functions/hypot.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct geos {}; // Geostationary Satellite View

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace geos
    {
            template <typename T>
            struct par_geos
            {
                T           h;
                T           radius_p;
                T           radius_p2;
                T           radius_p_inv2;
                T           radius_g;
                T           radius_g_1;
                T           C;
                int         flip_axis;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_geos_ellipsoid
                : public base_t_fi<base_geos_ellipsoid<T, Parameters>, T, Parameters>
            {
                par_geos<T> m_proj_parm;

                inline base_geos_ellipsoid(const Parameters& par)
                    : base_t_fi<base_geos_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    T r, Vx, Vy, Vz, tmp;

                    /* Calculation of geocentric latitude. */
                    lp_lat = atan (this->m_proj_parm.radius_p2 * tan (lp_lat));

                    /* Calculation of the three components of the vector from satellite to
                    ** position on earth surface (lon,lat).*/
                    r = (this->m_proj_parm.radius_p) / boost::math::hypot(this->m_proj_parm.radius_p * cos (lp_lat), sin (lp_lat));
                    Vx = r * cos (lp_lon) * cos (lp_lat);
                    Vy = r * sin (lp_lon) * cos (lp_lat);
                    Vz = r * sin (lp_lat);

                    /* Check visibility. */
                    if (((this->m_proj_parm.radius_g - Vx) * Vx - Vy * Vy - Vz * Vz * this->m_proj_parm.radius_p_inv2) < 0.) {
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    }

                    /* Calculation based on view angles from satellite. */
                    tmp = this->m_proj_parm.radius_g - Vx;

                    if(this->m_proj_parm.flip_axis) {
                        xy_x = this->m_proj_parm.radius_g_1 * atan (Vy / boost::math::hypot (Vz, tmp));
                        xy_y = this->m_proj_parm.radius_g_1 * atan (Vz / tmp);
                    } else {
                        xy_x = this->m_proj_parm.radius_g_1 * atan (Vy / tmp);
                        xy_y = this->m_proj_parm.radius_g_1 * atan (Vz / boost::math::hypot (Vy, tmp));
                    }
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    T Vx, Vy, Vz, a, b, det, k;

                    /* Setting three components of vector from satellite to position.*/
                    Vx = -1.0;

                    if(this->m_proj_parm.flip_axis) {
                        Vz = tan (xy_y / this->m_proj_parm.radius_g_1);
                        Vy = tan (xy_x / this->m_proj_parm.radius_g_1) * boost::math::hypot(1.0, Vz);
                    } else {
                        Vy = tan (xy_x / this->m_proj_parm.radius_g_1);
                        Vz = tan (xy_y / this->m_proj_parm.radius_g_1) * boost::math::hypot(1.0, Vy);
                    }

                    /* Calculation of terms in cubic equation and determinant.*/
                    a = Vz / this->m_proj_parm.radius_p;
                    a   = Vy * Vy + a * a + Vx * Vx;
                    b   = 2 * this->m_proj_parm.radius_g * Vx;
                    if ((det = (b * b) - 4 * a * this->m_proj_parm.C) < 0.) {
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    }

                    /* Calculation of three components of vector from satellite to position.*/
                    k  = (-b - sqrt(det)) / (2. * a);
                    Vx = this->m_proj_parm.radius_g + k * Vx;
                    Vy *= k;
                    Vz *= k;

                    /* Calculation of longitude and latitude.*/
                    lp_lon = atan2 (Vy, Vx);
                    lp_lat = atan (Vz * cos (lp_lon) / Vx);
                    lp_lat = atan (this->m_proj_parm.radius_p_inv2 * tan (lp_lat));
                }

                static inline std::string get_name()
                {
                    return "geos_ellipsoid";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_geos_spheroid
                : public base_t_fi<base_geos_spheroid<T, Parameters>, T, Parameters>
            {
                par_geos<T> m_proj_parm;

                inline base_geos_spheroid(const Parameters& par)
                    : base_t_fi<base_geos_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    T Vx, Vy, Vz, tmp;

                    /* Calculation of the three components of the vector from satellite to
                    ** position on earth surface (lon,lat).*/
                    tmp = cos(lp_lat);
                    Vx = cos (lp_lon) * tmp;
                    Vy = sin (lp_lon) * tmp;
                    Vz = sin (lp_lat);

                    /* Check visibility.*/
                    // TODO: in proj4 5.0.0 this check is not present
                    if (((this->m_proj_parm.radius_g - Vx) * Vx - Vy * Vy - Vz * Vz) < 0.)
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );

                    /* Calculation based on view angles from satellite.*/
                    tmp = this->m_proj_parm.radius_g - Vx;

                    if(this->m_proj_parm.flip_axis) {
                        xy_x = this->m_proj_parm.radius_g_1 * atan(Vy / boost::math::hypot(Vz, tmp));
                        xy_y = this->m_proj_parm.radius_g_1 * atan(Vz / tmp);
                    } else {
                        xy_x = this->m_proj_parm.radius_g_1 * atan(Vy / tmp);
                        xy_y = this->m_proj_parm.radius_g_1 * atan(Vz / boost::math::hypot(Vy, tmp));
                    }
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    T Vx, Vy, Vz, a, b, det, k;

                    /* Setting three components of vector from satellite to position.*/
                    Vx = -1.0;
                    if(this->m_proj_parm.flip_axis) {
                        Vz = tan (xy_y / (this->m_proj_parm.radius_g - 1.0));
                        Vy = tan (xy_x / (this->m_proj_parm.radius_g - 1.0)) * sqrt (1.0 + Vz * Vz);
                    } else {
                        Vy = tan (xy_x / (this->m_proj_parm.radius_g - 1.0));
                        Vz = tan (xy_y / (this->m_proj_parm.radius_g - 1.0)) * sqrt (1.0 + Vy * Vy);
                    }

                    /* Calculation of terms in cubic equation and determinant.*/
                    a   = Vy * Vy + Vz * Vz + Vx * Vx;
                    b   = 2 * this->m_proj_parm.radius_g * Vx;
                    if ((det = (b * b) - 4 * a * this->m_proj_parm.C) < 0.) {
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    }

                    /* Calculation of three components of vector from satellite to position.*/
                    k  = (-b - sqrt(det)) / (2 * a);
                    Vx = this->m_proj_parm.radius_g + k * Vx;
                    Vy *= k;
                    Vz *= k;

                    /* Calculation of longitude and latitude.*/
                    lp_lon = atan2 (Vy, Vx);
                    lp_lat = atan (Vz * cos (lp_lon) / Vx);
                }

                static inline std::string get_name()
                {
                    return "geos_spheroid";
                }

            };

            // Geostationary Satellite View
            template <typename Parameters, typename T>
            inline void setup_geos(Parameters& par, par_geos<T>& proj_parm)
            {
                std::string sweep_axis;

                if ((proj_parm.h = pj_get_param_f(par.params, "h")) <= 0.)
                    BOOST_THROW_EXCEPTION( projection_exception(error_h_less_than_zero) );

                if (par.phi0 != 0.0)
                    BOOST_THROW_EXCEPTION( projection_exception(error_unknown_prime_meridian) );

                sweep_axis = pj_get_param_s(par.params, "sweep");
                if (sweep_axis.empty())
                    proj_parm.flip_axis = 0;
                else {
                    if (sweep_axis[1] != '\0' || (sweep_axis[0] != 'x' && sweep_axis[0] != 'y'))
                        BOOST_THROW_EXCEPTION( projection_exception(error_invalid_sweep_axis) );

                    if (sweep_axis[0] == 'x')
                        proj_parm.flip_axis = 1;
                    else
                        proj_parm.flip_axis = 0;
                }

                proj_parm.radius_g_1 = proj_parm.h / par.a;
                proj_parm.radius_g = 1. + proj_parm.radius_g_1;
                proj_parm.C  = proj_parm.radius_g * proj_parm.radius_g - 1.0;
                if (par.es != 0.0) {
                    proj_parm.radius_p      = sqrt (par.one_es);
                    proj_parm.radius_p2     = par.one_es;
                    proj_parm.radius_p_inv2 = par.rone_es;
                } else {
                    proj_parm.radius_p = proj_parm.radius_p2 = proj_parm.radius_p_inv2 = 1.0;
                }
            }

    }} // namespace detail::geos
    #endif // doxygen

    /*!
        \brief Geostationary Satellite View projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - h: Height (real)
         - sweep: Sweep axis ('x' or 'y') (string)
        \par Example
        \image html ex_geos.gif
    */
    template <typename T, typename Parameters>
    struct geos_ellipsoid : public detail::geos::base_geos_ellipsoid<T, Parameters>
    {
        inline geos_ellipsoid(const Parameters& par) : detail::geos::base_geos_ellipsoid<T, Parameters>(par)
        {
            detail::geos::setup_geos(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Geostationary Satellite View projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - h: Height (real)
         - sweep: Sweep axis ('x' or 'y') (string)
        \par Example
        \image html ex_geos.gif
    */
    template <typename T, typename Parameters>
    struct geos_spheroid : public detail::geos::base_geos_spheroid<T, Parameters>
    {
        inline geos_spheroid(const Parameters& par) : detail::geos::base_geos_spheroid<T, Parameters>(par)
        {
            detail::geos::setup_geos(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::geos, geos_spheroid, geos_ellipsoid)

        // Factory entry(s)
        template <typename T, typename Parameters>
        class geos_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    if (par.es)
                        return new base_v_fi<geos_ellipsoid<T, Parameters>, T, Parameters>(par);
                    else
                        return new base_v_fi<geos_spheroid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        inline void geos_init(detail::base_factory<T, Parameters>& factory)
        {
            factory.add_to_factory("geos", new geos_entry<T, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_GEOS_HPP


/*
Copyright 2017. All rights reserved.
Computer Vision Group, Visual Computing Institute
RWTH Aachen University, Germany

This file is part of the rwth_mot framework.
Authors: Aljosa Osep (osep -at- vision.rwth-aachen.de)

rwth_mot framework is free software; you can redistribute it and/or modify it under the
terms of the GNU General Public License as published by the Free Software
Foundation; either version 3 of the License, or any later version.

rwth_mot framework is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with
rwth_mot framework; if not, write to the Free Software Foundation, Inc., 51 Franklin
Street, Fifth Floor, Boston, MA 02110-1301, USA
*/

#include "camera.h"

// eigen
#include <Eigen/LU>

namespace SUN {
    namespace utils {

        Camera::Camera() {
            this->P_.setIdentity();
            this->Rt_.setIdentity();
            this->Rt_inv_.setIdentity();
            this->K_.setIdentity();
            this->K_inv_.setIdentity();
            reference_pose_.setZero();
        }

        bool Camera::ComputeMeasurementCovariance3d(const Eigen::Vector3d &pos_3d, Eigen::Matrix2d c2d_mat, const Eigen::Matrix<double, 3, 4>& P_left_cam, const Eigen::Matrix<double, 3, 4>& P_right_cam,  Eigen::Matrix3d &covariance_out) {
            //! Compute inverse of pixel-measurement covariance matrix.
            auto inv_tmp = c2d_mat.inverse().eval();
            c2d_mat = inv_tmp;

            //! Compute Jacobians of projection matrices of both cameras.
            double wl = P_left_cam(2,0)*pos_3d[0] + P_left_cam(2,1)*pos_3d[1] + P_left_cam(2,2)*pos_3d[2] + P_left_cam(2,3);
            double wl2 = wl * wl;

            double wr = P_right_cam(2,0)*pos_3d[0] + P_right_cam(2,1)*pos_3d[1] + P_right_cam(2,2)*pos_3d[2] + P_right_cam(2,3);
            double wr2 = wr * wr;

            Eigen::Matrix<double, 2, 3> FL;
            FL.setZero();

            Eigen::Matrix<double, 2, 3> FR;
            FR.setZero();

            for (int i = 0; i < 2; i++) {
                FL(i,0) = P_left_cam(i,0) / wl - (P_left_cam(i,0) * pos_3d[0] + P_left_cam(i,1) * pos_3d[1] + P_left_cam(i,2) * pos_3d[2] + P_left_cam(i,3)) * P_left_cam(2,0) / wl2;
                FL(i,1) = P_left_cam(i,1) / wl - (P_left_cam(i,0) * pos_3d[0] + P_left_cam(i,1) * pos_3d[1] + P_left_cam(i,2) * pos_3d[2] + P_left_cam(i,3)) * P_left_cam(2,1) / wl2;
                FL(i,2) = P_left_cam(i,2) / wl - (P_left_cam(i,0) * pos_3d[0] + P_left_cam(i,1) * pos_3d[1] + P_left_cam(i,2) * pos_3d[2] + P_left_cam(i,3)) * P_left_cam(2,2) / wl2;

                FR(i,0) = P_right_cam(i,0) / wr - (P_right_cam(i,0) * pos_3d[0] + P_right_cam(i,1)  * pos_3d[1] + P_right_cam(i,2) * pos_3d[2] + P_right_cam(i,3)) * P_right_cam(2,0) / wr2;
                FR(i,1) = P_right_cam(i,1) / wr - (P_right_cam(i,0) * pos_3d[0] + P_right_cam(i,1)  * pos_3d[1] + P_right_cam(i,2) * pos_3d[2] + P_right_cam(i,3)) * P_right_cam(2,1) / wr2;
                FR(i,2) = P_right_cam(i,2) / wr - (P_right_cam(i,0) * pos_3d[0] + P_right_cam(i,1)  * pos_3d[1] + P_right_cam(i,2) * pos_3d[2] + P_right_cam(i,3)) * P_right_cam(2,2) / wr2;
            }

            //! Sum left and right part and compute the inverse.
            Eigen::Matrix3d FLTFL = (FL.transpose()*c2d_mat*FL);
            Eigen::Matrix3d FRTFR = (FR.transpose()*c2d_mat*FR);
            Eigen::Matrix3d sum = FLTFL+FRTFR;

            bool invertible;
            double determinant;
            sum.computeInverseAndDetWithCheck(covariance_out, determinant, invertible);
            return invertible;
        }

        bool Camera::ComputeMeasurementCovariance3d(const Eigen::Vector3d &pos_3d, double c_2d, const Eigen::Matrix<double, 3, 4>& P_left_cam, const Eigen::Matrix<double, 3, 4>& P_right_cam,  Eigen::Matrix3d &covariance_out) {
            // Compute pixel cov. mat.
            Eigen::Matrix2d c2d_mat;
            c2d_mat.setIdentity();
            c2d_mat *= c_2d;

            bool invertible = ComputeMeasurementCovariance3d(pos_3d, c2d_mat, P_left_cam, P_right_cam, covariance_out);
            assert(invertible);
            return invertible;
        }

        void Camera::init(const Eigen::Matrix<double,3,4> &reference_projection_matrix, const Eigen::Matrix4d &odometry_matrix, int width, int height) {
            /// Set up K, Rt matrices and their inverses
            this->K_ = reference_projection_matrix.block<3,3>(0,0);
            this->K_inv_ = K_.inverse();

            /// Little kitti hack, set t part to 0
            /// because t is specified relative to camera 0
            // TODO (Aljosa) sort this out!
            auto ref_proj_mat_copy = reference_projection_matrix;
            ref_proj_mat_copy.col(3).head<3>() = Eigen::Vector3d(0.0,0.0,0.0);

            /// Compute reference pose (pose in the first frame)
            reference_pose_(2,0) = ref_proj_mat_copy(2,3);
            reference_pose_(0,0) = (ref_proj_mat_copy(0,3) - ref_proj_mat_copy(0,2)*reference_pose_(2,0)) / ref_proj_mat_copy(0,0);
            reference_pose_(1,0) = (ref_proj_mat_copy(1,3) - ref_proj_mat_copy(1,2)*reference_pose_(2,0)) / ref_proj_mat_copy(1,1);
            reference_pose_(3,0) = 1.0;

            this->ApplyPoseTransform(odometry_matrix);

            this->P_.setIdentity();
            this->P_.block(0,0,3,4) = reference_projection_matrix;

            /// width, height
            width_ = width;
            height_ = height;
        }

        void Camera::init(const Eigen::Matrix3d &K, const Eigen::Matrix4d &Rt, int width, int height) {
            /// Set up K, Rt matrices and their inverses
            this->Rt_ = Rt;
            this->Rt_inv_ = Rt.inverse();
            this->K_ = K;
            this->K_inv_ = K_.inverse();

            /// Compute projection matrix
            this->ApplyPoseTransform(Rt);

            /// width, height
            width_ = width;
            height_ = height;
        }

        Eigen::Vector4d Camera::CameraToWorld(const Eigen::Vector4d &p_camera) const {
            return Rt_*p_camera;
        }

        Eigen::Vector4d Camera::WorldToCamera(const Eigen::Vector4d &p_world) const {
            return Rt_inv_*p_world;
        }

        Eigen::Vector3i Camera::CameraToImage(const Eigen::Vector4d &p_camera) const {
            Eigen::Vector3d proj_point = K_*p_camera.head<3>();
            proj_point /= proj_point[2]; // Normalize - last value must be 1.
            return proj_point.cast<int>();
        }

        Eigen::Vector3i Camera::WorldToImage(const Eigen::Vector4d &p_world) const {
            Eigen::Vector4d p_camera = this->WorldToCamera(p_world);
            Eigen::Vector3d p_camera_3 = p_camera.head(3);
            Eigen::Vector3d proj_point = K_*p_camera_3;

            proj_point /= proj_point[2]; // Normalize - last value must be 1.
            return proj_point.cast<int>();
        }

        Eigen::Vector4d Camera::GetCameraPose() const {
            return Rt_*reference_pose_;
        }

        Ray Camera::GetRayCameraSpace(int u, int v) const {
            Ray ray;
            ray.origin = Eigen::Vector3d(0.0,0.0,0.0);
            ray.direction = (K_inv_*Eigen::Vector3d(u,v,1.0)).normalized();
            return ray;
        }

        bool Camera::IsPointInFrontOfCamera(const Eigen::Vector4d& point) const {
            Eigen::Vector4d camera_pose = GetCameraPose();
            Eigen::Vector3d camera_plane_normal = R().col(2); //row(2);
            double camera_plane_d = 0;
            camera_plane_d -= camera_pose[0]*camera_plane_normal[0];
            camera_plane_d -= camera_pose[1]*camera_plane_normal[1];
            camera_plane_d -= camera_pose[2]*camera_plane_normal[2];

            if((camera_plane_normal.dot(point.head<3>()) + camera_plane_d) > 0)
                return true;
            else
                return false;
        }

        // Setters / Getters
        const Eigen::Matrix3d& Camera::K() const {
            return K_;
        }

        const Eigen::Matrix3d& Camera::K_inv() const {
            return K_inv_;
        }

        Eigen::Matrix3d Camera::R() const {
            return Rt_.block<3,3>(0,0);
        }

        const Eigen::Matrix4d& Camera::Rt() const {
            return Rt_;
        }

        const Eigen::Matrix4d& Camera::Rt_inv() const {
            return Rt_inv_;
        }

        const Eigen::Matrix4d& Camera::P() const {
            return P_;
        }

        int Camera::width() const {
            return width_;
        }

        int Camera::height() const {
            return height_;
        }

        void Camera::set_ground_model(const std::shared_ptr<GroundModel> ground_model) {
            ground_model_ = ground_model;
        }

        const std::shared_ptr<GroundModel> Camera::ground_model() const {
            return ground_model_;
        }

        Eigen::Vector4d Camera::PlaneCameraToWorld(const Eigen::Vector4d &plane_in_camera_space) const {
            Eigen::Vector3d t_cam = this->Rt().col(3).head<3>();
            Eigen::Vector3d plane_n_world = this->R()*plane_in_camera_space.head<3>();
            double plane_d_world = plane_in_camera_space[3] - plane_n_world.dot(t_cam);
            plane_n_world.normalize();

            Eigen::Vector4d plane_in_world_space;
            plane_in_world_space.head<3>() = plane_n_world;
            plane_in_world_space[3] = plane_d_world;
            return plane_in_world_space;
        }

        double Camera::ComputeGroundPlaneHeightInWorldSpace(double pose_plane_x, double pose_plane_z, const Eigen::Vector4d &plane_camera_space) const {
            Eigen::Vector4d plane_world = this->PlaneCameraToWorld(plane_camera_space);
            const double plane_d_world = plane_world[3];
            double a = plane_world[0];
            double b = plane_world[1];
            double c = plane_world[2];

            // Simply solve for height.
            double height_world = (-plane_d_world - a * pose_plane_x - c * pose_plane_z) / b;
            return height_world;
        }

        Eigen::Vector3d Camera::ProjectPointToGroundInWorldSpace(const Eigen::Vector3d &p_world, const Eigen::Vector4d &plane_camera_space) const {
            Eigen::Vector4d plane_world = this->PlaneCameraToWorld(plane_camera_space);
            const double plane_d_world = plane_world[3];
            double a = plane_world[0];
            double b = plane_world[1];
            double c = plane_world[2];
            double d = plane_world[3];

            Eigen::Vector3d p_proj = p_world;
            double dist = DistancePointToGroundInWorldSpace(p_world, plane_camera_space); // This is correct (bud bad design)
            p_proj -= dist*Eigen::Vector3d(a,b,c); // Substract plane_normal*dist_to_plane, and we have a projected point!

            return p_proj;
        }

        double Camera::DistancePointToGroundInWorldSpace(const Eigen::Vector3d &p_world, const Eigen::Vector4d &plane_camera_space) const {
            Eigen::Vector4d plane_world = this->PlaneCameraToWorld(plane_camera_space);
            const double plane_d_world = plane_world[3];
            double a = plane_world[0];
            double b = plane_world[1];
            double c = plane_world[2];
            double d = plane_world[3];
            Eigen::Vector3d p_proj = p_world;

            // --------------
            double x = p_world[0];
            double y = p_world[1];
            double z = p_world[2];
            double dist = a * x + b * y + c * z + d; //(std::abs(a * x + b * y + c * z + d)) /
            // --------------

            return dist;
        }

        const double Camera::f_u() const {
            return K_(0,0);
        }

        const double Camera::f_v() const {
            return K_(1,1);
        }

        const double Camera::c_u() const {
            return K_(0,2);
        }

        const double Camera::c_v() const {
            return K_(1,2);
        }

        void Camera::ApplyPoseTransform(const Eigen::Matrix4d &Rt) {
            this->Rt_ = Rt;
            this->Rt_inv_ = Rt.inverse();
        }
    }
}


//          Copyright Carl Philipp Reh 2006 - 2019.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <sge/opengl/texture/pp_dims.hpp>
#include <sge/opengl/texture/mipmap/get_levels.hpp>
#include <sge/renderer/basic_dim.hpp>
#include <sge/renderer/texture/mipmap/all_levels_rep_fwd.hpp>
#include <sge/renderer/texture/mipmap/level_count.hpp>
#include <sge/renderer/texture/mipmap/levels_rep.hpp>
#include <sge/renderer/texture/mipmap/object.hpp>
#include <sge/renderer/texture/mipmap/off_rep_fwd.hpp>
#include <sge/renderer/texture/mipmap/variant.hpp>
#include <fcppt/strong_typedef_construct_cast.hpp>
#include <fcppt/cast/size_fun.hpp>
#include <fcppt/math/log2.hpp>
#include <fcppt/math/size_type.hpp>
#include <fcppt/math/to_array.hpp>
#include <fcppt/variant/match.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/preprocessor/seq/for_each.hpp>
#include <boost/range/algorithm/max_element.hpp>
#include <fcppt/config/external_end.hpp>

template <fcppt::math::size_type Size>
sge::renderer::texture::mipmap::level_count sge::opengl::texture::mipmap::get_levels(
    sge::renderer::texture::mipmap::object const &_mipmap,
    sge::renderer::basic_dim<Size> const &_dim)
{
  return fcppt::variant::match(
      _mipmap.variant(),
      [](sge::renderer::texture::mipmap::off_rep const &)
      { return sge::renderer::texture::mipmap::level_count(1U); },
      [&_dim](sge::renderer::texture::mipmap::all_levels_rep const &)
      {
        return fcppt::strong_typedef_construct_cast<
            sge::renderer::texture::mipmap::level_count,
            fcppt::cast::size_fun>(
            1U + fcppt::math::log2(
                     // TODO(philipp)!
                     *boost::range::max_element(fcppt::math::to_array(_dim))));
      },
      [](sge::renderer::texture::mipmap::levels_rep const &_levels) { return _levels.value(); });
}

#define SGE_OPENGL_TEXTURE_MIPMAP_INSTANTIATE_GET_LEVELS(seq, _, dimension) \
  template sge::renderer::texture::mipmap::level_count \
  sge::opengl::texture::mipmap::get_levels<dimension>( \
      sge::renderer::texture::mipmap::object const &, \
      sge::renderer::basic_dim<dimension> const &);

BOOST_PP_SEQ_FOR_EACH(
    SGE_OPENGL_TEXTURE_MIPMAP_INSTANTIATE_GET_LEVELS, _, SGE_OPENGL_TEXTURE_PP_DIMS)


#ifndef _HOMOMORPHINE_HELIB_BACKED_H_
#define _HOMOMORPHINE_HELIB_BACKED_H_

#include <iostream>
#include <utility>
#include <sstream>
#include <string>
#include <boost/log/trivial.hpp>
#include <boost/log/utility/setup.hpp>
#include <helib/FHE.h>
#include <helib/EncryptedArray.h>

#include "util.hpp"
#include "arithmetic_backend.hpp"
#include "backend.hpp"
#include "constants.hpp"

using namespace NTL;
using namespace std;
using namespace boost::archive::iterators;

namespace homomorphine
{
  //! HELib algorithms - there is only one used
  enum HELibAlgorithm {
    DEFAULT
  };

  /*! /brief HELib backend
   *
   * This class is an implementation of the SEAL backend.
   */
  class HELibBackend : public ArithmeticBackend
  {
    private:
      FHEcontext* context = nullptr;   /*!< HELib context object */
      FHEPubKey* public_key = nullptr; /*!< HELib public key */
      FHESecKey* secret_key = nullptr; /*!< HELib secret */
      ZZX polynomial;                  /*!< NTL polynomial, used internally by HELib */
      Ctxt* cipher = nullptr;          /*!< cipher */
      HELibAlgorithm algorithm;        /*!< HELib algorithm type */

      unsigned long hamming_weight;    /*!< Hamming weight - used for encrypting the value(s) */

    public:

      /*!
       * HELib backend cleanup
       */
      ~HELibBackend();

      /*!
       * Sets the specific HELib algorithm implementation
       * that backend provides (currently, only default one)
       *
       * \param algorithm homomorphic encryption algorithm
       */
      void setAlgorithm(string algorithm);

      /*!
       * Initializes the HELib backend
       */
      void init();

      /*!
       * Generates the public/secret key pair
       */
      void generateKeys();

      /*!
       * Returns the UUEncoded public key
       *
       * \return public key
       */
      string getPublicKey();

      /*!
       * Writes the public key to a stream
       *
       * \param stream public key stream
       */
      void writePublicKeyToStream(ostream& stream);

      /*!
       * Returns the UUEncoded secret key
       *
       * \return secret key
       */
      string getSecretKey();

      /*!
       * Writes the secret key to a stream
       *
       * \param stream secret key stream
       */
      void writeSecretKeyToStream(ostream& stream);

      /*!
       * Returns the pair of UUEncoded public and secret keys
       *
       * \return pair of public and secret keys
       */
      pair<string, string> getKeys();

      /*!
       * Sets the public key
       *
       * \param public_key UUEncoded public key
       */
      void setPublicKey(string public_key);

      /*!
       * Sets the public key from stream
       *
       * \param stream public key binary stream
       */
      void readPublicKeyFromStream(istream &stream);

      /*!
       * Sets the secret key
       *
       * \param secret_key UUEncoded secret key
       */
      void setSecretKey(string secret_key);

      /*!
       * Sets the secret key from stream
       *
       * \param stream secret key binary stream
       */
      void readSecretKeyFromStream(istream &stream);

      /*!
       * Sets the both public and secret keys
       *
       * \param public_key UUEncoded public key
       * \param secret_key UUEncoded secret key
       */
      void setKeys(string public_key, string secret_key);

      /*!
       * Returns the UUEncoded cipher containing ecrypted value, or vector of values
       *
       * \return UUEncoded cipher
       */
      string getCipher();

      /*!
       * Writes the cipher to output stream
       *
       * \param stream output stream
       */
      void writeCipherToStream(ostream& stream);

      /*!
       * Sets the UUEncoded cipher containing ecrypted value, or vector of values
       *
       * \param cipher UUEncoded cipher
       */
      void setCipher(string cipher);

      /*!
       * Reads the cipher from input stream
       *
       * \param stream cipher stream
       */
      void readCipherFromStream(istream &stream);

      /*!
       * Encrypts the vector of values using the public key
       *
       * \param values vector of values
       * \return UUEncoded cipher
       */
      void encrypt(vector<long> values);

      /*!
       * Encrypts the single value using the public key
       *
       * \param value value
       * \return UUEncoded cipher
       */
      void encrypt(long value);

      /*!
       * Decrypts the vector of values using the secret key
       *
       * \return vector of decrypted values
       */
      vector<long> decryptValues();

      /*!
       * Decrypts the single value using the secret key
       *
       * \return decrypted value
       */
      long decrypt();

      /*!
       * Adds the vector of values to encrypted vector of values
       *
       * \param values vector of values
       */
      void add(vector<long> values);

      /*!
       * Adds the value to encrypted value
       *
       * \param value value
       */
      void add(long value);

      /*!
       * Negates a single encrypted value, or a vector of encrypted values
       */
      void negate();

      /*!
       * Multiplies the vector of values with the encrypted vector of values
       *
       * \param values vector of values
       */
      void multiply(vector<long> values);

      /*!
       * Multiplies the value with the encrypted value
       *
       * \param value value
       */
      void multiply(long value);
  };
}

#endif


#pragma once
#include <algorithm>
#include <atomic>
#include <chrono>
#include <cmath>
#include <thread>
#include <boost/test/unit_test.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include "snapshot.hpp"

namespace bacc = boost::accumulators;

inline void test_snapshot(const std::vector<int>& vec, const metrics::snapshot& snap){
	int min = *std::min_element(vec.begin(),vec.end());
	int max = *std::max_element(vec.begin(),vec.end());

	BOOST_CHECK_EQUAL(min,snap.min());
	BOOST_CHECK_EQUAL(max,snap.max());

	double total = std::accumulate(vec.begin(),vec.end(),0.0);
	BOOST_CHECK_CLOSE(total/vec.size(),snap.mean(),0.0001);

	bacc::accumulator_set<double, bacc::stats<bacc::tag::variance> > acc;
  	for_each(vec.begin(), vec.end(), std::bind<void>(std::ref(acc), std::placeholders::_1));

  	BOOST_CHECK_CLOSE(bacc::mean(acc),snap.mean(),1e-6);
  	// TODO : Evaluate how to allow sample stats in std dev nicely
  	if(vec.size() > 1){
  		BOOST_CHECK_CLOSE(sqrt((bacc::variance(acc)) * vec.size() / (vec.size() - 1)),snap.std_dev(),1e-6);
  	} else {
  		BOOST_CHECK_CLOSE(0.0,snap.std_dev(),1e-6);
  	}
}

inline void wait_for(const std::function<bool(void)>& func){
	do {
		std::this_thread::yield();
	} while (!func());
}

namespace mock {
  class clock {
  public:
    typedef std::chrono::seconds duration;
    typedef std::chrono::time_point<clock> time_point;
    static time_point now();
    static void set_time(const int& val);
    static int time();
  };
}


/*
 * StokesM2L.cpp
 *
 *  Created on: Oct 12, 2016
 *      Author: wyan
 */

#include "SVD_pvfmm.hpp"

#include <Eigen/Dense>

#include <algorithm>
#include <iomanip>
#include <iostream>
#include <limits>
#include <numeric>

#define DIRECTLAYER 2

namespace StokesPVel3D3D {

using EVec3 = Eigen::Vector3d;
using EVec4 = Eigen::Vector4d;
using EMat3 = Eigen::Matrix3d;
using EMat4 = Eigen::Matrix4d;
using EMat64 = Eigen::Matrix<double, 6, 4>;
constexpr double eps = std::numeric_limits<double>::epsilon();

/**
 * \brief Returns the coordinates of points on the surface of a cube.
 * \param[in] p Number of points on an edge of the cube is (n+1)
 * \param[in] c Coordinates to the centre of the cube (3D array).
 * \param[in] alpha Scaling factor for the size of the cube.
 * \param[in] depth Depth of the cube in the octree.
 * \return Vector with coordinates of points on the surface of the cube in the
 * format [x0 y0 z0 x1 y1 z1 .... ].
 */
template <class Real_t>
std::vector<Real_t> surface(int p, Real_t *c, Real_t alpha, int depth) {
    size_t n_ = (6 * (p - 1) * (p - 1) + 2); // Total number of points.

    std::vector<Real_t> coord(n_ * 3);
    coord[0] = coord[1] = coord[2] = -1.0;
    size_t cnt = 1;
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = -1.0;
            coord[cnt * 3 + 1] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = (2.0 * j - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * i - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = -1.0;
            coord[cnt * 3 + 2] = (2.0 * (j + 1) - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = (2.0 * j - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = -1.0;
            cnt++;
        }
    for (size_t i = 0; i < (n_ / 2) * 3; i++)
        coord[cnt * 3 + i] = -coord[i];

    Real_t r = 0.5 * pow(0.5, depth);
    Real_t b = alpha * r;
    for (size_t i = 0; i < n_; i++) {
        coord[i * 3 + 0] = (coord[i * 3 + 0] + 1.0) * b + c[0];
        coord[i * 3 + 1] = (coord[i * 3 + 1] + 1.0) * b + c[1];
        coord[i * 3 + 2] = (coord[i * 3 + 2] + 1.0) * b + c[2];
    }
    return coord;
}

/**********************************************
 *   Ewald for Stokeslet
 * ********************************************/
inline Eigen::Matrix3d AEW(const double xi, const Eigen::Vector3d &rvec) {
    const double r = rvec.norm();
    Eigen::Matrix3d A = 2 * (xi * exp(-(xi * xi) * (r * r)) / (sqrt(M_PI) * r * r) + erfc(xi * r) / (2 * r * r * r)) *
                            (r * r * Eigen::Matrix3d::Identity() + (rvec * rvec.transpose())) -
                        4 * xi / sqrt(M_PI) * exp(-(xi * xi) * (r * r)) * Eigen::Matrix3d::Identity();
    return A;
}

inline Eigen::Matrix3d BEW(const double xi, const Eigen::Vector3d &kvec) {
    const double k = kvec.norm();
    Eigen::Matrix3d B = 8 * M_PI * (1 + k * k / (4 * (xi * xi))) *
                        ((k * k) * Eigen::Matrix3d::Identity() - (kvec * kvec.transpose())) / (k * k * k * k);
    B *= exp(-k * k / (4 * xi * xi));
    return B;
}

inline void GkernelEwald(const Eigen::Vector3d &rvec_, Eigen::Matrix3d &Gsum) {
    const double xi = 2;
    EVec3 rvec = rvec_;
    rvec[0] = rvec[0] - floor(rvec[0]);
    rvec[1] = rvec[1] - floor(rvec[1]);
    rvec[2] = rvec[2] - floor(rvec[2]);
    const double r = rvec.norm();
    Eigen::Matrix3d real = Eigen::Matrix3d::Zero();
    const int N = 5;
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            for (int k = -N; k < N + 1; k++) {
                if (r < eps && i == 0 && j == 0 && k == 0)
                    continue;
                real = real + AEW(xi, rvec + Eigen::Vector3d(i, j, k));
            }
        }
    }

    EMat3 Gself = -4 * xi / sqrt(M_PI) * Eigen::Matrix3d::Identity(); // the self term
    if (r < eps) {
        real += Gself;
    }

    Eigen::Matrix3d wave = Eigen::Matrix3d::Zero();
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            for (int k = -N; k < N + 1; k++) {
                Eigen::Vector3d kvec(i, j, k);
                kvec *= (2 * M_PI);
                if (i == 0 && j == 0 && k == 0) {
                    continue;
                }
                wave += BEW(xi, kvec) * cos(kvec.dot(rvec));
            }
        }
    }
    Gsum = real + wave;
}

/*********************************************************
 * Ewald for Laplace potential grad
 * *******************************************************/
inline double freal(double xi, double r) { return std::erfc(xi * r) / r; }

inline double frealp(double xi, double r) {
    return -(2. * exp(-r * r * (xi * xi)) * xi) / (sqrt(M_PI) * r) - std::erfc(r * xi) / (r * r);
}

inline void realSum(double xi, const EVec3 &target, const EVec3 &source, EVec3 &v) {

    EVec3 rvec = target - source;
    double rnorm = rvec.norm();
    if (rnorm < eps) {
        v.setZero();
    } else {
        v = (frealp(xi, rnorm) / rnorm) * rvec;
    }
}

inline void Lkernel(const EVec3 &target, const EVec3 &source, EVec3 &answer) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    if (rnorm < eps) {
        answer.setZero();
        return;
    }
    double rnorm3 = rnorm * rnorm * rnorm;
    answer = -rst / rnorm3;
}

// grad of Laplace potential, without 1/4pi prefactor, periodic of -r_k/r^3
inline void LkernelEwald(const EVec3 &target_, const EVec3 &source_, EVec3 &answer) {
    EVec3 target = target_;
    EVec3 source = source_;
    target[0] = target[0] - floor(target[0]); // periodic BC
    target[1] = target[1] - floor(target[1]);
    target[2] = target[2] - floor(target[2]);
    source[0] = source[0] - floor(source[0]);
    source[1] = source[1] - floor(source[1]);
    source[2] = source[2] - floor(source[2]);

    double xi = 0.54;

    // real sum
    int rLim = 10;
    EVec3 Kreal = EVec3::Zero();
    for (int i = -rLim; i < rLim + 1; i++) {
        for (int j = -rLim; j < rLim + 1; j++) {
            for (int k = -rLim; k < rLim + 1; k++) {
                EVec3 v = EVec3::Zero();
                realSum(xi, target, source + EVec3(i, j, k), v);
                Kreal += v;
            }
        }
    }

    // wave sum
    int wLim = 10;
    EVec3 rmn = target - source;
    double xi2 = xi * xi;
    EVec3 Kwave(0., 0., 0.);
    for (int i = -wLim; i < wLim + 1; i++) {
        for (int j = -wLim; j < wLim + 1; j++) {
            for (int k = -wLim; k < wLim + 1; k++) {
                if (i == 0 && j == 0 && k == 0)
                    continue;
                EVec3 kvec = EVec3(i, j, k) * (2 * M_PI);
                double k2 = kvec.dot(kvec);
                double knorm = kvec.norm();
                Kwave += -kvec * (sin(kvec.dot(rmn)) * exp(-k2 / (4 * xi2)) / k2);
            }
        }
    }

    answer = Kreal + Kwave;
}

/***************************************************************
 * Combination of Laplace grad and Stokeslet
 * *************************************************************/
// fx,fy,fz,trD -> p, vx,vy,vz
inline void Wkernel(const EVec3 &target, const EVec3 &source, EMat4 &answer) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    if (rnorm < eps) {
        answer.setZero();
        return;
    }
    double rnorm3 = rnorm * rnorm * rnorm;

    answer.setZero();
    answer.block<3, 3>(1, 0) = EMat3::Identity() / rnorm;
    answer.block<3, 3>(1, 0) += rst * rst.transpose() / rnorm3;
    answer(0, 0) = rst[0] / rnorm3;
    answer(0, 1) = rst[1] / rnorm3;
    answer(0, 2) = rst[2] / rnorm3;
    answer(0, 3) = 0;
    answer(1, 3) = -rst[0] / rnorm3;
    answer(2, 3) = -rst[1] / rnorm3;
    answer(3, 3) = -rst[2] / rnorm3;
    answer.row(0) *= (1 / (4 * M_PI));
    answer.block<3, 4>(1, 0) *= (1 / (8 * M_PI));
}

inline void WkernelEwald(const EVec3 &target, const EVec3 &source, EMat4 &answer) {

    /************************************
     * Verification for this part is done in testEwald()
     * **********************************/
    answer.setZero();
    EMat3 G = EMat3::Zero();
    GkernelEwald(target - source, G);
    EVec3 L = EVec3::Zero();
    LkernelEwald(target, source, L);
    answer.block<3, 3>(1, 0) = G;
    answer(0, 0) = -L[0];
    answer(0, 1) = -L[1];
    answer(0, 2) = -L[2];
    answer(0, 3) = 0;
    answer(1, 3) = L[0];
    answer(2, 3) = L[1];
    answer(3, 3) = L[2];
    answer.row(0) *= (1 / (4 * M_PI));
    answer.block<3, 4>(1, 0) *= (1 / (8 * M_PI));

    /************************************
     * Verification needed for this part
     * **********************************/
    // pressure gradient balancing net force
    answer(0, 0) += (target[0] - 0.5);
    answer(0, 1) += (target[1] - 0.5);
    answer(0, 2) += (target[2] - 0.5);

    /************************************
     * Verification for this part is done in calcFlux()
     * **********************************/
    // dipole balancing net flux
    EVec3 y = source;
    y[0] = y[0] - floor(y[0]);
    y[1] = y[1] - floor(y[1]);
    y[2] = y[2] - floor(y[2]);
    answer(1, 3) += 0.5 * y[0];
    answer(2, 3) += 0.5 * y[1];
    answer(3, 3) += 0.5 * y[2];
}

inline EMat4 WkernelNF(const EVec3 &target, const EVec3 &source, const int N = DIRECTLAYER) {
    EMat4 WNF = EMat4::Zero();
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            for (int k = -N; k < N + 1; k++) {
                EMat4 W = EMat4::Zero();
                Wkernel(target, source + EVec3(i, j, k), W);
                WNF += W;
            }
        }
    }
    return WNF;
}

inline void WkernelFF(const EVec3 &target, const EVec3 &source, EMat4 &answer) {
    EMat4 WEwald = EMat4::Zero();
    WkernelEwald(target, source, WEwald);
    answer = WEwald - WkernelNF(target, source);
}

/**
 * @brief calculate the flux across xy, xz, yz surface
 *
 * @param val
 * @param pos
 */
void calcFlux(const std::vector<EVec4, Eigen::aligned_allocator<EVec4>> &val,
              const std::vector<EVec3, Eigen::aligned_allocator<EVec3>> &pos) {
    constexpr int NX = 32;
    std::vector<double> meshPos(NX * NX * 2, 0.0);
    std::vector<double> meshVal(NX * NX, 0.0);
    const double dx = 1.0 / NX;
    for (int i = 0; i < NX; i++) {
        for (int j = 0; j < NX; j++) {
            meshPos[2 * (i * NX + j) + 0] = dx * i;
            meshPos[2 * (i * NX + j) + 1] = dx * j;
        }
    }

    const int nsrc = pos.size();
    const int ni = NX * NX;
    for (int ax = 0; ax < 3; ax++) {
        meshVal.clear();
        meshVal.resize(ni, 0);
#pragma omp parallel for
        for (int i = 0; i < ni; i++) {
            double a = meshPos[2 * i];
            double b = meshPos[2 * i + 1];
            EVec3 trg;
            trg[ax] = 0;
            trg[(ax + 1) % 3] = a;
            trg[(ax + 2) % 3] = b;
            double f = 0;
            for (int j = 0; j < nsrc; j++) {
                EMat4 WE = EMat4::Zero();
                WkernelEwald(trg, pos[j], WE);
                EVec4 pvel = WE * val[j];
                f += pvel[ax + 1];
            }
            meshVal[i] = f;
        }
        double flux = std::accumulate(meshVal.begin(), meshVal.end(), 0.0);
        std::cout << "flux along axis " << ax << " = " << flux * dx * dx << std::endl;
    }
}

void testEwald() {
    // test Ewald kernel with absolute convergent source configuration
    {
        const EVec3 center(0.5, 0.5, 0.5);
        const double rad = 0.2;
        const int nPts = 8;

        std::vector<EVec3, Eigen::aligned_allocator<EVec3>> forcePoint(nPts);
        std::vector<EVec4, Eigen::aligned_allocator<EVec4>> forceValue(nPts);

        for (int i = 0; i < nPts; i++) {
            const double alpha = (2 * M_PI * i) / nPts;
            forcePoint[i] = rad * EVec3(cos(alpha), sin(alpha), 0) + center;
            forceValue[i] = EVec4(cos(alpha), sin(alpha), 0, 1) * cos(i * M_PI);
        }

        {
            // check src settings
            EVec4 fnetS;
            EVec3 dnetS;
            fnetS.setZero();
            dnetS.setZero();
            for (int i = 0; i < nPts; i++) {
                fnetS += forceValue[i];
                dnetS += forceValue[i][3] * forcePoint[i];
            }
            std::cout << "From S: " << std::endl;
            std::cout << "Source Sum: " << fnetS.transpose() << std::endl;
            std::cout << "TrD Dipole Sum: " << dnetS.transpose() << std::endl;
        }

        calcFlux(forceValue, forcePoint);

#pragma omp parallel for
        for (int s = 0; s < 200; s++) {
            const EVec3 samplePoint(0.2, 0.3, 0.01 * s - 0.5);
            EVec4 pvelDirect = EVec4::Zero();
            EVec4 pvelEwald = EVec4::Zero();

            for (int p = 0; p < nPts; p++) {
                EMat4 W = EMat4::Zero();
                WkernelEwald(samplePoint, forcePoint[p], W);
                pvelEwald += W * forceValue[p];
            }

            for (int p = 0; p < nPts; p++) {
                EMat4 WNF = EMat4::Zero();
                const int N = 40;
                for (int i = -N; i < N + 1; i++) {
                    for (int j = -N; j < N + 1; j++) {
                        for (int k = -N; k < N + 1; k++) {
                            EMat4 W = EMat4::Zero();
                            Wkernel(samplePoint, forcePoint[p] + EVec3(i, j, k), W);
                            WNF += W;
                        }
                    }
                }
                pvelDirect += WNF * forceValue[p];
            }

#pragma omp critical
            {

                std::cout << samplePoint.transpose() << std::endl;
                std::cout << "x,p " << samplePoint[2] << " " << pvelEwald[0] << std::endl;
                std::cout << "Ewald " << pvelEwald.transpose() << std::endl;
                std::cout << "Direct " << pvelDirect.transpose() << std::endl;
                std::cout << "Error " << (pvelDirect - pvelEwald).transpose() << std::endl;
            }
        }
    }

    std::exit(0);
}

/***
 * Explain: this kernel WEwald does not satisfy translational invariance.
 *
 * **/
void testTranslation() {
    const int nPts = 2;
    std::vector<EVec3, Eigen::aligned_allocator<EVec3>> forcePoint(nPts);
    std::vector<EVec4, Eigen::aligned_allocator<EVec4>> forceValue(nPts);
    forcePoint[0] = EVec3(0.3, 0.1, 0.1);
    forcePoint[1] = EVec3(0.7, 0.1, 0.1);
    forceValue[0] = EVec4(0, 0, 0, 1);
    forceValue[1] = EVec4(0, 0, 0, -1);

    for (int i = 0; i < nPts; i++) {
        std::cout << "src loc " << forcePoint[i].transpose() << " val " << forceValue[i].transpose() << std::endl;
    }

    EVec3 samplePoint(0.1, 0.2, 0.3);

    for (int i = 0; i < 5; i++) {
        EVec3 shift(i * 0.2, 0, 0);
        auto shiftPoint = forcePoint;
        shiftPoint[0] += shift;
        shiftPoint[1] += shift;
        EVec3 shiftSample = samplePoint + shift;
        // reset to [0,1]
        for (int j = 0; j < 3; j++) {
            shiftPoint[0][j] = shiftPoint[0][j] - floor(shiftPoint[0][j]);
            shiftPoint[1][j] = shiftPoint[1][j] - floor(shiftPoint[1][j]);
            shiftSample[j] = shiftSample[j] - floor(shiftSample[j]);
        }
        EVec4 pvel = EVec4::Zero();
        for (int j = 0; j < nPts; j++) {
            EMat4 W;
            WkernelEwald(shiftSample, shiftPoint[j], W);
            pvel += W * forceValue[j];
        }
        std::cout << "pvel " << pvel.transpose() << std::endl;
        calcFlux(forceValue, shiftPoint);
    }
}

int main(int argc, char **argv) {
    Eigen::initParallel();
    Eigen::setNbThreads(1);

    std::cout << std::scientific << std::setprecision(18);

    const int pEquiv = atoi(argv[1]); // (8-1)^2*6 + 2 points
    const int pCheck = atoi(argv[1]);
    const double scaleEquiv = 1.05;
    const double scaleCheck = 2.95;
    const double pCenterEquiv[3] = {-(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2};
    const double pCenterCheck[3] = {-(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2};
    auto pointMEquiv = surface(pEquiv, (double *)&(pCenterEquiv[0]), scaleEquiv, 0);
    // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointMCheck = surface(pCheck, (double *)&(pCenterCheck[0]), scaleCheck, 0);
    // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    auto pointLEquiv = surface(pEquiv, (double *)&(pCenterCheck[0]), scaleCheck, 0);
    // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointLCheck = surface(pCheck, (double *)&(pCenterEquiv[0]), scaleEquiv, 0);
    // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    // calculate the operator M2L with least square
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointLCheck.size() / 3;

    Eigen::MatrixXd A(4 * checkN, 4 * equivN);
#pragma omp parallel for
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1], pointLCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointLEquiv[3 * l], pointLEquiv[3 * l + 1], pointLEquiv[3 * l + 2]);
            EMat4 W = EMat4::Zero();
            Wkernel(Cpoint, Lpoint, W);
            A.block<4, 4>(4 * k, 4 * l) = W;
        }
    }
    Eigen::MatrixXd ApinvU(A.cols(), A.rows());
    Eigen::MatrixXd ApinvVT(A.cols(), A.rows());
    pinv(A, ApinvU, ApinvVT);

    Eigen::MatrixXd M2L(4 * equivN, 4 * equivN);
    M2L.setZero();
#pragma omp parallel for
    for (int i = 0; i < equivN; i++) {
        const Eigen::Vector3d Mpoint(pointMEquiv[3 * i], pointMEquiv[3 * i + 1], pointMEquiv[3 * i + 2]);
        const EVec3 npoint(0.5, 0.5, 0.5);
        Eigen::MatrixXd f(4 * checkN, 4);
        for (int k = 0; k < checkN; k++) {
            Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1], pointLCheck[3 * k + 2]);
            EMat4 val = EMat4::Zero();
            EMat4 neu = EMat4::Zero();
            WkernelFF(Cpoint, Mpoint, val);
            WkernelFF(Cpoint, npoint, neu);
            EMat4 temp = val;
            // EMat4 temp = val - neu;
            // std::cout << temp << std::endl;
            // temp.row(0).setZero();
            temp.col(3) = val.col(3) - neu.col(3);
            // temp.row(0) = val.row(0) - neu.row(0);
            // std::cout << temp << std::endl;
            f.block<4, 4>(4 * k, 0) = temp;
        }
        // std::cout << f.col(0) << std::endl;
        M2L.block(0, 4 * i, 4 * equivN, 4) = (ApinvU.transpose() * (ApinvVT.transpose() * f));
        // std::cout << "M2L\n" << M2L.block(0, 4 * i, 4 * equivN, 4) << std::endl;
    }

    // dump M2L
    for (int i = 0; i < 4 * equivN; i++) {
        for (int j = 0; j < 4 * equivN; j++) {
            std::cout << i << " " << j << " " << std::scientific << std::setprecision(18) << M2L(i, j) << std::endl;
        }
    }

    // A operator
    A.resize(4 * checkN, 4 * equivN);
    ApinvU.resize(A.cols(), A.rows());
    ApinvVT.resize(A.cols(), A.rows());
#pragma omp parallel for
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1], pointMCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            Eigen::Vector3d Mpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1], pointMEquiv[3 * l + 2]);
            EMat4 W = EMat4::Zero();
            Wkernel(Cpoint, Mpoint, W);
            A.block<4, 4>(4 * k, 4 * l) = W;
        }
    }
    pinv(A, ApinvU, ApinvVT);

    // Test
    // Sum of force and trD must be zero
    const int nPts = 5;
    std::vector<EVec3, Eigen::aligned_allocator<EVec3>> forcePoint(nPts);
    std::vector<EVec4, Eigen::aligned_allocator<EVec4>> forceValue(nPts);
    double dsum = 0;
    for (int i = 0; i < nPts; i++) {
        forcePoint[i] = EVec3::Random() * 0.1 + EVec3(0.5, 0.5, 0.5);
        forceValue[i] = EVec4::Random();
        dsum += forceValue[i][3];
    }
    for (auto &v : forceValue) {
        v[3] -= dsum / nPts;
    }

    for (int i = 0; i < nPts; i++) {
        std::cout << "src loc " << forcePoint[i].transpose() << " val " << forceValue[i].transpose() << std::endl;
    }

    // check src settings
    EVec4 fnetS;
    EVec3 dnetS;
    fnetS.setZero();
    dnetS.setZero();
    for (int i = 0; i < nPts; i++) {
        fnetS += forceValue[i];
        dnetS += forceValue[i][3] * forcePoint[i];
    }
    std::cout << "From S: " << std::endl;
    std::cout << "Source Sum: " << fnetS.transpose() << std::endl;
    std::cout << "TrD Dipole Sum: " << dnetS.transpose() << std::endl;

    calcFlux(forceValue, forcePoint);

    // solve M
    Eigen::VectorXd f(4 * checkN);
#pragma omp parallel for
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1], pointMCheck[3 * k + 2]);
        EVec4 temp = EVec4::Zero();
        for (int p = 0; p < forceValue.size(); p++) {
            EMat4 W = EMat4::Zero();
            Wkernel(Cpoint, forcePoint[p], W);
            temp += W * (forceValue[p]);
        }
        f.block<4, 1>(4 * k, 0) = temp;
    }
    Eigen::VectorXd Msource = (ApinvU.transpose() * (ApinvVT.transpose() * f));

    EVec4 fnetM;
    EVec3 dnetM;
    fnetM.setZero();
    dnetM.setZero();
    for (int i = 0; i < equivN; i++) {
        fnetM += EVec4(Msource[4 * i + 0], Msource[4 * i + 1], Msource[4 * i + 2], Msource[4 * i + 3]);
        dnetM[0] += Msource[4 * i + 3] * pointMEquiv[3 * i];
        dnetM[1] += Msource[4 * i + 3] * pointMEquiv[3 * i + 1];
        dnetM[2] += Msource[4 * i + 3] * pointMEquiv[3 * i + 2];
    }

    std::cout << "From M: " << std::endl;
    std::cout << "Source Sum: " << fnetM.transpose() << std::endl;
    std::cout << "TrD Dipole Sum: " << dnetM.transpose() << std::endl;
    EVec4 fnetError = fnetM - fnetS;
    std::cout << "Source MS Error: " << fnetError.transpose() << std::endl;
    EVec3 dnetError = dnetM - dnetS;
    std::cout << "Dipole MS Error: " << dnetError.transpose() << std::endl;

    EVec3 dnetMP;
    dnetMP.setZero();
    auto pointMP = pointMEquiv;
    for (int i = 0; i < 3 * equivN; i++) {
        pointMP[i] = pointMP[i] - floor(pointMP[i]);
    }
    for (int i = 0; i < equivN; i++) {
        dnetMP[0] += Msource[4 * i + 3] * pointMP[3 * i];
        dnetMP[1] += Msource[4 * i + 3] * pointMP[3 * i + 1];
        dnetMP[2] += Msource[4 * i + 3] * pointMP[3 * i + 2];
    }

    std::cout << "From MP: " << std::endl;
    std::cout << "TrD Dipole Sum: " << dnetMP.transpose() << std::endl;
    std::cout << "Dipole MPS Error: " << (dnetMP - dnetS).transpose() << std::endl;

    Eigen::VectorXd M2Lsource = M2L * (Msource);

    for (int i = 0; i < 6; i++) {
        // EVec3 samplePoint = EVec3(0.2, 0.2, 0.2) * i + EVec3(0., 0., 0.);
        EVec3 samplePoint = EVec3::Random() * 0.5 + EVec3(0.5, 0.5, 0.5);

        // Compute: WFF from L, WFF from WkernelFF
        EVec4 WFFL = EVec4::Zero();
        EVec4 WFFM = EVec4::Zero();
        EVec4 WFFS = EVec4::Zero();
        EVec4 WS = EVec4::Zero();
        std::cout << "----------------------------" << std::endl;
        std::cout << "sample point: " << samplePoint.transpose() << std::endl;

        // S2T
        for (int k = 0; k < forceValue.size(); k++) {
            EMat4 W = EMat4::Zero();
            WkernelFF(samplePoint, forcePoint[k], W);
            WFFS += W * forceValue[k];
            W.setZero();
            WkernelEwald(samplePoint, forcePoint[k], W);
            WS += W * forceValue[k];
        }
        std::cout << "WS: " << WS.transpose() << std::endl;
        std::cout << "WFF S2T: " << WFFS.transpose() << std::endl;

        // L2T
        for (int k = 0; k < equivN; k++) {
            EVec3 Lpoint(pointLEquiv[3 * k], pointLEquiv[3 * k + 1], pointLEquiv[3 * k + 2]);
            EMat4 W = EMat4::Zero();
            Wkernel(samplePoint, Lpoint, W);
            WFFL += W * M2Lsource.block<4, 1>(4 * k, 0);
        }
        WFFL[1] += (dnetM[0] - dnetMP[0]) * 0.5;
        WFFL[2] += (dnetM[1] - dnetMP[1]) * 0.5;
        WFFL[3] += (dnetM[2] - dnetMP[2]) * 0.5;
        std::cout << "WFF L2T: " << WFFL.transpose() << std::endl;
        std::cout << "WFF Error LS: " << (WFFL - WFFS).transpose() << std::endl;

        // M2T
        for (int k = 0; k < equivN; k++) {
            EVec3 Mpoint(pointMEquiv[3 * k], pointMEquiv[3 * k + 1], pointMEquiv[3 * k + 2]);
            EMat4 W = EMat4::Zero();
            WkernelFF(samplePoint, Mpoint, W);
            WFFM += W * Msource.block<4, 1>(4 * k, 0);
        }
        WFFM[1] += (dnetM[0] - dnetMP[0]) * 0.5;
        WFFM[2] += (dnetM[1] - dnetMP[1]) * 0.5;
        WFFM[3] += (dnetM[2] - dnetMP[2]) * 0.5;
        std::cout << "WFF M2T: " << WFFM.transpose() << std::endl;
        std::cout << "WFF Error MS: " << (WFFM - WFFS).transpose() << std::endl;
    }

    return 0;
}

} // namespace StokesPVel3D3D

#undef DIRECTLAYER


#define BOOST_TEST_MODULE MyTest
#include <boost/test/included/unit_test.hpp>

using namespace boost::unit_test;

BOOST_AUTO_TEST_CASE(MyTestCase)
{
	float x = 9.5f;
	BOOST_CHECK(x != 0.0f);
	BOOST_CHECK_EQUAL((int)x, 9);
	BOOST_CHECK_CLOSE(x, 9.5f, 0.0001f); // Checks differ no more then 0.0001%
}