/**
 * BSD 2-Clause License
 *
 * Copyright (c) 2017, Oscar Asterkrans
 * All rights reserved.
 */
#pragma once
#include <api/battlesnake.hpp>
#include <boost/graph/grid_graph.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <array>

// NOTE: This board is just a prototype. You probably want to add stuff to both Square and Board.
namespace board {

struct Square {
    // `avoid` is true if a snake occupies this square.
    bool avoid = false;
};

// Represents a fully connected grid layout graph.
using Full_grid = boost::grid_graph<2, Index>;

// Vertex descriptor representing a square on the game plan.
using Vertex = boost::array<Index, 2>; //Same as boost::graph_traits<Full_grid>::vertex_descriptor;

// Edge descriptor representing a directed connection between two squares on the game plan.
using Edge = std::pair<Vertex, Vertex>; // Same as boost::graph_traits<Full_grid>::edge_descriptor;

// Convert a Point to a vertex descriptor.
inline Vertex to_vertex(const Point& p) {
    return { {p.x, p.y} };
}

// Convert a vertex descriptor to a Point.
inline Point to_point(const Vertex& v) {
    return Point(v[0], v[1]);
}

class Board {
public:
    Board(const Index width, const Index height) :
            width(width),
            height(height),
            full_grid_graph(create_full_grid_graph(width, height)) {
        squares.resize(width * height);
    }

    Square& get_square(const Point& p) {
        return squares[p.x + p.y*width];
    }

    const Square& get_square(const Point& p) const {
        return squares[p.x + p.y*width];
    }

    // Only used by get_graph() below.
    const Full_grid& _get_full_grid() const {
        return full_grid_graph;
    }
private:
    // Create the full grid graph.
    Full_grid create_full_grid_graph(const Index width, const Index height) {
        boost::array<Index, 2> lengths = { {width, height} };
        return Full_grid(lengths);
    }

    const Index width;
    const Index height;
    // This graph have one vertex for each square on board. Neighbouring vertices have edges connecting them. (One edge in each direction.)
    Full_grid full_grid_graph;

    std::vector<Square> squares;
};

// Graph filter predicate to filter out non-valid moves.
struct Valid_move {
    // boost::filtered_graph requires default constructable predicate.
    Valid_move() : board() {
    }

    // Will keep a reference of board. Keep board available during the life-time of this object.
    Valid_move(const Board& board) :
            board(&board) {
    }

    template <typename EdgeT>
    bool operator()(const EdgeT& e) const {
        assert(board);
        const auto target = to_point(e.second);
        return !board->get_square(target).avoid;
    }

private:
    const Board* board;
};

// Filtered graph with non-valid moves removed.
using Graph = boost::filtered_graph<Full_grid, Valid_move>;

// Get a filtered graph representing valid moves on the board.
Graph get_graph(const Board& board) {
    Valid_move valid_move(board);
    Graph g(board._get_full_grid(), valid_move);
    return g;
}

// Get direction from an edge descriptor.
template <typename EdgeT, typename GraphT>
Direction getDirection(EdgeT e, GraphT g) {
    const auto source = boost::source(e, g);
    const auto target = boost::target(e, g);
    const int dx = static_cast<int>(target[0]) - source[0];
    const int dy = static_cast<int>(target[1]) - source[1];
    assert((dx == 0 || dy == 0) && std::abs(dx + dy) == 1); // one step in either direction.

    if (dy == 1) {
        return Direction::down;
    } else if (dy == -1) {
        return Direction::up;
    } else if (dx == 1) {
        return Direction::right;
    } else {
        return Direction::left;
    }
}

}


//////////////////////////////////////////////////////////////////////////////
//  monomials_horner::monomials.hpp                                         //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_MONOMIALS_HORNER_MONOMIALS_HPP_ER_2009
#define BOOST_MONOMIALS_HORNER_MONOMIALS_HPP_ER_2009
#include <vector>
#include <algorithm>
#include <iterator>
#include <boost/assert.hpp>
#include <boost/iterator/zip_iterator.hpp>
#include <boost/range.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/bind.hpp>
#include <boost/monomials_horner/monomials_properties.hpp>

namespace boost{
namespace monomials_horner{

    // Background:
    // \see http://mathworld.wolfram.com/Polynomial.html
    // A polynomial is a mathematical expression involving a sum of powers in
    // one or more variables  multiplied by coefficients. A polynomial in one
    // variable (i.e., a univariate polynomial) with constant coefficients is
    // given by
    //  a_nx^n+...+a_2x^2+a_1x+a_0. (1)
    // The individual summands with the coefficients (usually) included are
    // called monomials (Becker and Weispfenning 1993, p. 191),
    // the products of the form x_1^(alpha_1)...x_n^(alpha_n)
    // in the multivariate case,
    // i.e., with the coefficients omitted, are called terms
    // (Becker and Weispfenning 1993, p. 188).
    // The highest power  in a univariate polynomial is called its order,
    // or sometimes its degree

    // Example :
    // variables = (a,b,c)
    //
    // 1
    // >a >b >c
    // >a^2 ab ac >b^2 bc >c^2
    // >a^3 a^2b a^2 c ab^2 abc ac^2 >b^3 b^2c bc^2 > c^3
    //
    // each > above marks the begining of a "read range"
    // the kth write range on the nth line is obtained by
    // multiplying the kth read range on the (n-1)th line
    // by the kth variable in (a,b,c)
    // For example, (b^3, b^2c, bc^2) = b*(b^2, bc, c^2)

    template<typename Cont = std::vector<double> >
    class monomials{
        typedef typename Cont::size_type                size_type;
        typedef typename range_iterator<Cont>::type     it_;
        typedef std::vector<it_>                        it_s;
        typedef monomials_properties<>                  properties;
    public:
        typedef typename range_value<Cont>::type        value_type;
        typedef const Cont&                             result_type;
        monomials(){}
        monomials(monomials& that)
        :degree_(that.degree_),terms_(that.terms_){}
        monomials& operator=(const monomials& that){
            if(&that!=this){
                degree_ = that.degree_;
                terms_ = that.terms_;
            }
            return *this;
        }

        //degree (also called total degree) is |alpha|=alpha_0+...+alpha_{n-1}
        unsigned degree()const{return degree_;}
        result_type operator()()const{return terms_;}

        template<typename R>
        void operator()(const R& variables,unsigned the_degree){
            typedef std::vector<it_> it_s;
            static it_s read_bs;
            {
                size_type sz = properties::number_degree_less_than(
                    the_degree,(unsigned)(size(variables)));
                terms_.resize(sz);
            }
            it_ write_b = begin(terms_);

            *write_b++ = 1;
            read_bs.clear();
            std::fill_n(
                back_inserter(read_bs),
                size(variables),
                begin(terms_)
            );

            unsigned i = 1;
            unsigned n = the_degree + 1;
            while(i<n){
                BOOST_ASSERT(size(read_bs)==size(variables));
                write_b = for_each(//e=
                    make_zip_iterator(
                        make_tuple(
                            begin(read_bs),
                            begin(variables)
                        )
                    ),
                    make_zip_iterator(
                        make_tuple(
                            end(read_bs),
                            end(variables)
                        )
                    ),
                    zip_func<R>(write_b)
                ).write_b;

                ++i;
            }
            degree_ = the_degree;
        }

    private:
        // keeping terms_ inside spares the client the need to clear it
        // or keeping track of the number of terms.
        // using the same Cont for improved memory management
        unsigned degree_;
        Cont terms_;

        template<typename R>
        struct zip_func
        {
            typedef typename range_iterator<R>::type              it_1;
            typedef typename it_1::value_type                    value1_type;
            typedef const tuple<it_&,const value1_type&>& argument_type;
            typedef void                                          result_type;

            zip_func(it_ write_b_)
            :write_b(write_b_),read_e(write_b){}

            zip_func(const zip_func& that)
            :write_b(that.write_b),read_e(that.read_e){}

            zip_func& operator=(const zip_func& that){
                if(&that!=this){
                    write_b = that.write_b;
                    read_e = that.read_e;
                }
                return *this;
            }

            result_type operator()(argument_type t){
                typename it_1::value_type x = (t.template get<1>());

                it_ read_b = t.get<0>();
                t.template get<0>() = write_b;

                write_b =
                    transform(
                        read_b,
                        read_e,
                        write_b,
                        bind(
                            std::multiplies<value_type>(),
                            _1,
                            x
                        )
                    );
            }

            it_ write_b;
            it_ read_e;

        };
    };

}// monomials_horner
}// boost

#endif


#include <boost/graph/filtered_graph.hpp>

#include "graph.hpp"

#include "adaptive_units.hpp"
#include "generic_dijkstra.hpp"
#include "generic_constrained_label_creator.hpp"
#include "generic_label.hpp"
#include "generic_permanent.hpp"
#include "generic_tentative.hpp"
#include "generic_tracer.hpp"
#include "utils.hpp"

#include <optional>
#include <set>
#include <utility>

using namespace std;

template<typename Graph>
optional<cupath>
search(const Graph &g, const demand &d, const CU &cu)
{
  vertex src = d.first.first;
  vertex dst = d.first.second;
  // The number of contiguous units.
  int ncu = d.second;

  assert (src != dst);

  // The generic permanent solution type.
  using per_type = generic_permanent<Graph, COST, CU>;
  // The generic tentative solution type.
  using ten_type = generic_tentative<Graph, COST, CU>;

  // The permanent and tentative solutions.
  per_type P(boost::num_vertices(g));
  ten_type T(boost::num_vertices(g));
  // The label we start the search with.
  generic_label<Graph, COST, CU> l(0, CU(cu), edge(), src);
  // The creator of the labels.
  generic_constrained_label_creator<Graph, COST, CU> c(g, ncu);

  // Run the search.
  dijkstra(g, l, P, T, c, dst);

  // The tracer.
  generic_tracer<Graph, cupath, per_type, CU> t(g, ncu);
  // Get the path.
  auto op = trace(P, dst, l, t);

  if (op)
    {
      // The length of the path found.
      auto dist = get_path_length(g, op.value().second);
      // The path CU.
      auto &pcu = op.value().first;

      // Get the number of units required.
      int units = adaptive_units<COST>::units(ncu, dist);

      // First-fit spectrum allocation policy.
      pcu = CU(pcu.min(), pcu.min() + units);
    }

  return op;
}

std::optional<std::pair<cupath, cupath> >
ee(const graph &g, const demand &d, const CU &cu)
{
  std::optional<std::pair<cupath, cupath> > result;

  // We have to run the algorithm twice: first without filtering,
  // second with filtering.
  auto first = search(g, d, cu);

  if (first)
    {
      const auto &p1 = first.value();

      // Excluded edges.
      set<edge> ee;

      for(const auto &e: p1.second)
        ee.insert(e);

      // The filtered graph type.
      using fg_type =
        boost::filtered_graph<graph,
                              boost::is_not_in_subset<set<edge>>>;

      fg_type fg(g, ee);

      auto second = search(fg, d, cu);

      if (second)
        {
          const auto &p2 = second.value();

          // Let's make sure that the first path is not of a greater
          // cost than the second path.
          assert(get_cost(g, p1) <= get_cost(g, p2));

          // Let's make sure that the two paths have no mutual edges.
          for(const auto &e1: p1.second)
            for(const auto &e2: p2.second)
              assert(e1 != e2);

          return make_pair(p1, p2);
        }
    }

  return {};
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_COST_BASE_HPP_
#define CROCODDYL_CORE_COST_BASE_HPP_

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/state-base.hpp"
#include "crocoddyl/core/data-collector-base.hpp"
#include "crocoddyl/core/activation-base.hpp"
#include "crocoddyl/core/activations/quadratic.hpp"

namespace crocoddyl {

/**
 * @brief Abstract class for cost models
 *
 * In Crocoddyl, a cost model is defined by the scalar activation function \f$a(\cdot)\f$ and by the residual function
 * \f$\mathbf{r}(\cdot)\f$ as follows: \f[ cost = a(\mathbf{r}(\mathbf{x}, \mathbf{u})), \f] where
 * the residual function depends on the state point \f$\mathbf{x}\in\mathcal{X}\f$, which lies in the state manifold
 * described with a `nq`-tuple, its velocity \f$\dot{\mathbf{x}}\in T_{\mathbf{x}}\mathcal{X}\f$ that belongs to
 * the tangent space with `nv` dimension, and the control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$. The residual vector
 * is defined by \f$\mathbf{r}\in\mathbb{R}^{nr}\f$ where `nr` describes its dimension in the Euclidean space. On the
 * other hand, the activation function builds a cost value based on the definition of the residual vector. The residual
 * vector has to be specialized in a derived classes.
 *
 * The main computations are carring out in `calc` and `calcDiff` routines. `calc` computes the cost (and its residual)
 * and `calcDiff` computes the derivatives of the cost function (and its residual). Concretely speaking, `calcDiff`
 * builds a linear-quadratic approximation of the cost function with the form: \f$\mathbf{l_x}\in\mathbb{R}^{ndx}\f$,
 * \f$\mathbf{l_u}\in\mathbb{R}^{nu}\f$, \f$\mathbf{l_{xx}}\in\mathbb{R}^{ndx\times ndx}\f$,
 * \f$\mathbf{l_{xu}}\in\mathbb{R}^{ndx\times nu}\f$, \f$\mathbf{l_{uu}}\in\mathbb{R}^{nu\times nu}\f$ are the
 * Jacobians and Hessians, respectively.
 * Additionally, it is important remark that `calcDiff()` computes the derivates using the latest stored values by
 * `calc()`. Thus, we need to run first `calc()`.
 *
 * \sa `StateAbstractTpl`, `ActivationModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
 */
template <typename _Scalar>
class CostModelAbstractTpl {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef CostDataAbstractTpl<Scalar> CostDataAbstract;
  typedef StateAbstractTpl<Scalar> StateAbstract;
  typedef ActivationModelAbstractTpl<Scalar> ActivationModelAbstract;
  typedef ActivationModelQuadTpl<Scalar> ActivationModelQuad;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  /**
   * @brief Initialize the cost model
   *
   * @param[in] state       State of the multibody system
   * @param[in] activation  Activation model
   * @param[in] nu          Dimension of control vector
   */
  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, boost::shared_ptr<ActivationModelAbstract> activation,
                       const std::size_t nu);

  /**
   * @copybrief CostModelAbstractTpl()
   *
   * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
   *
   * @param[in] state       State of the multibody system
   * @param[in] activation  Activation model
   */
  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, boost::shared_ptr<ActivationModelAbstract> activation);

  /**
   * @copybrief CostModelAbstractTpl()
   *
   * We use `ActivationModelQuadTpl` as a default activation model (i.e. \f$a=\frac{1}{2}\|\mathbf{r}\|^2\f$)
   *
   * @param[in] state  State of the multibody system
   * @param[in] nr     Dimension of residual vector
   * @param[in] nu     Dimension of control vector
   */
  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr, const std::size_t nu);

  /**
   * @copybrief CostModelAbstractTpl()
   *
   * We use `ActivationModelQuadTpl` as a default activation model (i.e. \f$a=\frac{1}{2}\|\mathbf{r}\|^2\f$).
   * Furthermore, the default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
   *
   * @param[in] state  State of the multibody system
   * @param[in] nr     Dimension of residual vector
   * @param[in] nu     Dimension of control vector
   */
  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr);
  virtual ~CostModelAbstractTpl();

  /**
   * @brief Compute the cost value and its residual vector
   *
   * @param[in] data  Cost data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                    const Eigen::Ref<const VectorXs>& u) = 0;

  /**
   * @brief Compute the Jacobian and Hessian of cost and its residual vector
   *
   * It computes the Jacobian and Hessian of the cost function. It assumes that `calc()` has been run first.
   *
   * @param[in] data  Cost data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                        const Eigen::Ref<const VectorXs>& u) = 0;

  /**
   * @brief Create the cost data
   *
   * The default data contains objects to store the values of the cost, residual vector and their derivatives (first
   * and second order derivatives). However, it is possible to specialized this function is we need to create
   * additional data, for instance, to avoid dynamic memory allocation.
   *
   * @param data  Data collector
   * @return the cost data
   */
  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);

  /**
   * @copybrief calc()
   *
   * @param[in] data  Cost data
   * @param[in] x     State point
   */
  void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  /**
   * @copybrief calcDiff()
   *
   * @param[in] data  Cost data
   * @param[in] x     State point
   */
  void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief Return the state
   */
  const boost::shared_ptr<StateAbstract>& get_state() const;

  /**
   * @brief Return the activation model
   */
  const boost::shared_ptr<ActivationModelAbstract>& get_activation() const;

  /**
   * @brief Return the dimension of the control input
   */
  std::size_t get_nu() const;

  /**
   * @brief Modify the cost reference
   */
  template <class ReferenceType>
  void set_reference(ReferenceType ref);

  /**
   * @brief Return the cost reference
   */
  template <class ReferenceType>
  ReferenceType get_reference() const;

 protected:
  /**
   * @copybrief set_reference()
   */
  virtual void set_referenceImpl(const std::type_info&, const void*);

  /**
   * @copybrief get_reference()
   */
  virtual void get_referenceImpl(const std::type_info&, void*) const;

  boost::shared_ptr<StateAbstract> state_;                 //!< State description
  boost::shared_ptr<ActivationModelAbstract> activation_;  //!< Activation model
  std::size_t nu_;                                         //!< Control dimension
  VectorXs unone_;                                         //!< No control vector
};

template <typename _Scalar>
struct CostDataAbstractTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  template <template <typename Scalar> class Model>
  CostDataAbstractTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
      : shared(data),
        activation(model->get_activation()->createData()),
        cost(Scalar(0.)),
        Lx(model->get_state()->get_ndx()),
        Lu(model->get_nu()),
        Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
        Lxu(model->get_state()->get_ndx(), model->get_nu()),
        Luu(model->get_nu(), model->get_nu()),
        r(model->get_activation()->get_nr()),
        Rx(model->get_activation()->get_nr(), model->get_state()->get_ndx()),
        Ru(model->get_activation()->get_nr(), model->get_nu()) {
    Lx.setZero();
    Lu.setZero();
    Lxx.setZero();
    Lxu.setZero();
    Luu.setZero();
    r.setZero();
    Rx.setZero();
    Ru.setZero();
  }
  virtual ~CostDataAbstractTpl() {}

  DataCollectorAbstract* shared;
  boost::shared_ptr<ActivationDataAbstract> activation;
  Scalar cost;
  VectorXs Lx;
  VectorXs Lu;
  MatrixXs Lxx;
  MatrixXs Lxu;
  MatrixXs Luu;
  VectorXs r;
  MatrixXs Rx;
  MatrixXs Ru;
};

}  // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/cost-base.hxx"

#endif  // CROCODDYL_CORE_COST_BASE_HPP_


#include "Tunneling1D.h"
#include <cmath>
#include <iostream>
// #include <gsl/gsl_errno.h>
// #include <gsl/gsl_min.h>
// #include <gsl/gsl_roots.h>
#include "GSL_Wraper.h"
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/bessel.hpp>

using namespace std;
using namespace boost::math;

// typedef double (*root_func)(double,void*);
// double find_root_gsl_wraper(root_func func, void *params, double x_max, double x_min)
// {
//     int status;
//     int iter = 0, max_iter = 100;
//     const gsl_root_fsolver_type *T = gsl_root_fsolver_brent;
//     gsl_root_fsolver *s = gsl_root_fsolver_alloc(T);
//     double r;
//     double r_min = x_min;
//     double r_max = x_max;

//     gsl_function F;
//     // struct param_initialConditions params = {this, phi0, dV0, d2V0, phi_absMin, delta_phi_cutoff};

//     F.function = func;
//     F.params = params;

//     gsl_root_fsolver_set(s, &F, r_min, r_max);

//     do
//     {
//         iter++;
//         status = gsl_root_fsolver_iterate(s);
//         r      = gsl_root_fsolver_root(s);
//         r_min  = gsl_root_fsolver_x_lower(s);
//         r_max  = gsl_root_fsolver_x_upper(s);
//         status = gsl_root_test_interval(r_min,r_max,1e-8,1e-10);
//     } while (status == GSL_CONTINUE && iter < max_iter);

//     gsl_root_fsolver_free(s);
//     return r;
// }

// * function used by RungeKutta
VD func_for_rkqc(double r, VD y, void *param)
{
    Tunneling1D *mod = (Tunneling1D*) param;
    return mod->equationOfMotion(r,y);
}
Tunneling1D::Tunneling1D(double absMin, double metaMin, ScalarFunction V_, dScalarFunction dV_, HM d2V_, double dim, double phi_eps_rel_)
{
    V = V_;
    dV = dV_;
    d2V = d2V_;

    _rk_calculator.SetDOF(2);
    _rk_calculator.SetODE(func_for_rkqc);
    _rk_calculator.SetParams(this);

    phi_absMin = absMin;
    phi_metaMin = metaMin;
    phi_bar = findBarrierLocation();
    // cout<<"Barrier at "<<phi_bar<<endl;

    rscale = findRScale();
    // cout<<"r-scale: "<<rscale<<endl;

    Spatial_Dim = dim;
    alpha = dim-1;

    phi_eps_rel = phi_eps_rel_;
    phi_eps_abs = phi_eps_rel*abs(phi_absMin-phi_metaMin);
}
void Tunneling1D::SetMinima(double absMin, double metaMin)
{
    phi_absMin = absMin;
    phi_metaMin = metaMin;
}
void Tunneling1D::SetPotential(ScalarFunction potential)
{
    V = potential;
}
void Tunneling1D::SetdPotential(dScalarFunction dpotential)
{
    dV = dpotential;
}
void Tunneling1D::SetHM(HM d2potential)
{
    d2V = d2potential;
}
void Tunneling1D::SetPhiAtBarrier(double phibar)
{
    phi_bar = phibar;
}
void Tunneling1D::SetSpatialDim(double dim)
{
    Spatial_Dim = dim;
    alpha = Spatial_Dim - 1;
}
void Tunneling1D::SetPrecision(double eps_rel)
{
    phi_eps_rel = eps_rel;
    // if (isnan(phi_absMin) || isnan(phi_metaMin))
    // {
    //     phi_eps_abs = NAN;
    //     cout<<"Please Set the Location of the Minimum First"<<endl;
    // }
    // else
    // {
    //     phi_eps_abs = phi_eps_rel * abs(phi_absMin - phi_metaMin);
    // }
}
double Tunneling1D::dV_from_absMin(double delta_phi)
{
    double phi = phi_absMin + delta_phi;
    double T = 0;
    double dV_f = dV({phi},&T)[0];

    double dV_d = d2V({phi},&T)[0][0] * delta_phi;

    double blend_factor = exp(-pow(delta_phi/phi_eps_abs,2));

    return dV_f*(1-blend_factor) + dV_d*blend_factor;
}
double Tunneling1D::findBarrierLocation()
{
    double phi_tol = abs(phi_absMin-phi_metaMin)*1e-12;
    double T = 0;
    double V_meta = V({phi_metaMin},&T);
    double phiH = phi_metaMin;
    double phiL = phi_absMin;
    double phiM = (phiH + phiL)/2;

    double V0;
    while (abs(phiH-phiL) > phi_tol)
    {
        V0 = V({phiM},&T);
        if (V0 > V_meta)
        {
            phiH = phiM;
        }
        else
        {
            phiL = phiM;
        }
        phiM = (phiH + phiL)/2;
    }
    return phiM;
}
double func_for_findRScale(double x, void *params)
{
    Tunneling1D * mod = (Tunneling1D*)params;
    return -mod->VvalatX(x);
}
double Tunneling1D::findRScale()
{
    if (std::isnan(phi_bar)) findBarrierLocation();
    double phi_tol = abs(phi_bar - phi_metaMin)*1e-6;
    double x1 = min(phi_bar,phi_metaMin);
    double x2 = max(phi_bar,phi_metaMin);

    double phi_bar_top = find_min_arg_gsl_wraper(func_for_findRScale,this,x2,x1,phi_tol);

    if (phi_bar_top + phi_tol > x2 || phi_bar_top - phi_tol < x1)
    {
        cout<<"In findRScale: No Barrier for the potential: can't find the top position."<<endl;
    }

    double Vtop = VvalatX(phi_bar_top) - VvalatX(phi_metaMin);
    double xtop = phi_bar_top - phi_metaMin;

    if (Vtop <= 0)
    {
        cout<<"In findRScale: No Barrier for the potential: non-positive barrier height."<<endl;
    }
    // This is the `time'-scale for a harmonic oscillation (Approximating the potential by a quadratic potential).
    return abs(xtop)/sqrt(abs(2*Vtop));
}
tuple<double, double> Tunneling1D::exactSolution(double r, double phi0, double dV_, double d2V_)
{
    // ! Find phi(r) (and dphi(r)) given phi(0) assuming a quadractic potential
    double beta = sqrt(abs(d2V_));
    double beta_r = beta*r;
    double nu = (alpha - 1.0)/2.0;

    double phi = 0;
    double dphi = 0;
    double tmp;
    if (beta_r < 1e-2)
    {
        // Using the expansion to approximate the Bessel function
        double s = d2V_>0?1:-1;
        for (int k = 1; k < 4; k++)
        {
            tmp = pow(beta_r/2,2*k-2)*pow(s,k)/(tgamma((k+1)*1.0)*tgamma(k+1+nu));
            phi += tmp;
            dphi += tmp*(2*k);
        }
        phi *= tgamma(nu+1)*r*r*dV_*s/4;
        dphi *= tgamma(nu+1)*r*dV_*s/4;
        phi += phi0;
    }
    else if (d2V_ > 0)
    {
        // cout<<"beta_r: "<<beta_r<<endl;
        try
        {
            phi = (tgamma(nu+1)/pow(beta_r/2,nu)*cyl_bessel_i(nu,beta_r)-1)*dV_/d2V_;
            dphi = -nu/r/pow(beta_r/2,nu)*cyl_bessel_i(nu,beta_r);
            dphi += beta/2/pow(beta_r/2,nu)*(cyl_bessel_i(nu-1,beta_r)+cyl_bessel_i(nu+1,beta_r));
            dphi *= tgamma(nu+1)*dV_/d2V_;
            phi += phi0;
        }
        catch(const std::overflow_error& e)
        {
            // std::cerr << e.what() << '\n';
            // just ignore the overflow
            phi = INFINITY;
            dphi = INFINITY;
        }
    }
    else
    {
        phi = (tgamma(nu+1)/pow(beta_r/2,nu)*cyl_bessel_j(nu,beta_r)-1)*dV_/d2V_;
        dphi = -nu/r/pow(beta_r/2,nu)*cyl_bessel_j(nu,beta_r);
        dphi += beta/2/pow(beta_r/2,nu)*(cyl_bessel_j(nu-1,beta_r)-cyl_bessel_j(nu+1,beta_r));
        dphi *= tgamma(nu+1)*dV_/d2V_;
        phi += phi0;
    }
    return make_tuple(phi,dphi);
}
struct param_initialConditions
{
    Tunneling1D *tun;
    double phi0;
    double dV0;
    double d2V0;

    double phi_absMin;
    double delta_phi_cutoff;
};
double func_for_initialConditions(double r, void *params)
{
    param_initialConditions *mod = (param_initialConditions*)params;
    double phi0 = mod->phi0;
    double dV0 = mod->dV0;
    double d2V0 = mod->d2V0;
    double phi_absMin = mod->phi_absMin;
    double delta_phi_cutoff = mod->delta_phi_cutoff;
    double phir,dphir;
    std::tie(phir,dphir) = (mod->tun)->exactSolution(r,phi0,dV0,d2V0);
    return abs(phir - phi_absMin) - abs(delta_phi_cutoff);
}
tuple<double, double, double> Tunneling1D::initialConditions(double delta_phi0, double rmin, double delta_phi_cutoff)
{
    /*
    * Find the initial conditions for the ODE integration.
    *
    * The instanton equations of motion are singular at `r=0`,
    * so we need to start the integration at some larger radius.
    * This function finds the value `r0` such that `phi(r0) = phi_cutoff`.
    * If there is no such value, it returns the intial conditions at `rmin`.
    */

    double T = 0;
    double phi0 = phi_absMin + delta_phi0;
    double dV0 = dV_from_absMin(delta_phi0);
    double d2V0 = d2V({phi0},&T)[0][0];

    double phi_rmin, dphi_rmin;
    std::tie(phi_rmin, dphi_rmin) = exactSolution(rmin, phi0, dV0, d2V0);
    if (abs(phi_rmin - phi_absMin) > abs(delta_phi_cutoff))
    {
        return make_tuple(rmin, phi_rmin, dphi_rmin);
    }
    if (sign(dphi_rmin) != sign(delta_phi0))
    {
        return make_tuple(rmin, phi_rmin, dphi_rmin);
    }

    double r_cur = rmin;
    double r_last = rmin;

    double phi, dphi;
    r_last = r_cur;
    r_cur *= 10;
    while (std::isfinite(r_cur))
    {
        std::tie(phi, dphi) = exactSolution(r_cur, phi0, dV0, d2V0);
        if (!std::isfinite(phi))
        {
            r_cur = (r_last + r_cur)/2.0;
            continue;
        }
        if (abs(phi - phi_absMin) > abs(delta_phi_cutoff))
        {
            break;
        }
        r_last = r_cur;
        r_cur *= 10;
    }
    struct param_initialConditions params = {this, phi0, dV0, d2V0, phi_absMin, delta_phi_cutoff};
    // cout<<"Before root finding"<<endl;
    // cout<<"r_cur="<<r_cur<<" r_last="<<r_last<<endl;
    double r = find_root_gsl_wraper(&func_for_initialConditions,&params,r_cur,r_last);
    // cout<<"After root finding"<<endl;

    std::tie(phi,dphi) = exactSolution(r,phi0,dV0,d2V0);
    return make_tuple(r,phi,dphi);
}
VD Tunneling1D::equationOfMotion(double r, VD y)
{
    VD res(2);
    double T = 0;
    res[0] = y[1];
    res[1] = dV({y[0]},&T)[0]-alpha*y[1]/r;
    return res;
}
struct cubic_param
{
    double y0;
    double dy0;
    double y1;
    double dy1;
    double diff;
};

double cubicInterpolation(double x, void *param)
{
    cubic_param* mod = (cubic_param*)param;
    double mt = 1-x;
    double c3 = mod->y1;
    double c2 = mod->y1 - mod->dy1/3.0;
    double c1 = mod->y0 + mod->dy0/3.0;
    double c0 = mod->y0;
    return c0*pow(mt,3) + 3*c1*mt*mt*x + 3*c2*mt*x*x + c3*pow(x,3) - mod->diff;
}
tuple<double, VD, CONVERGENCETYPE> Tunneling1D::integrateProfile(double r0, VD y0, double dr0, double epsfrac, double epsabs, double drmin, double rmax)
{
    VD y_final_value = {phi_metaMin,0};
    VD y_diff;
    double dr_guess = dr0;
    double dr_did,dr_next;
    double r = r0;
    VD y = y0;
    VD dydr = equationOfMotion(r,y);
    double r_cache;
    VD y_cache;
    VD dydr_cache;
    VD y_scale;
    VD y_inter(2);
    int ysign = sign(y0[0]-phi_metaMin);
    rmax += r0;

    CONVERGENCETYPE convergQ = NONE;
    cubic_param inter_param;
    double x;
    while (true)
    {
        y_scale = abs(y)+abs(dydr*dr_guess);
        r_cache = r;
        y_cache = y;
        dydr_cache = dydr;
        // cout<<"\t\t----"<<endl;
        // cout<<"\t\t"<<r_cache<<"  "<<y_cache[0]<<"  "<<y_cache[1]<<"  "<<dydr_cache[0]<<"  "<<dydr_cache[1]<<"  "<<dr_guess<<"  "<<epsabs<<endl;
        _rk_calculator._RKQC_SingleStep(r_cache,y_cache,dydr_cache,dr_guess,epsabs,y_scale,dr_did,dr_next);
        dydr_cache = equationOfMotion(r_cache,y_cache);

        y_diff = abs(y_cache-y_final_value);
        // cout<<"\t\t"<<r_cache<<"  "<<y_cache[0]<<"  "<<y_cache[1]<<"  "<<ysign<<"  "<<dr_did<<"  "<<dr_next<<endl;
        // cout<<"\t\t\t"<<y_diff[0]<<"/"<<epsabs<<"  "<<y_diff[1]<<"/"<<epsabs<<endl;
        if ( y_diff[0] < epsabs && y_diff[1] < epsabs)
        {
            r = r_cache;
            y = y_cache;
            convergQ = CONVERGED;
            break;
        }

        if (y_cache[1]*ysign > 0)
        {
            // This means the `ball` is heading back, so it will never reach the desired point.
            convergQ = UNDERSHOOT;
            inter_param = {y[1],dydr[1]*dr_did,y_cache[1],dydr_cache[1]*dr_did,0};
            x = find_root_gsl_wraper(&cubicInterpolation,&inter_param,1,0);
            r += dr_did*x;
            y_inter[1] = cubicInterpolation(x,&inter_param);
            inter_param = {y[0],dydr[0]*dr_did,y_cache[0],dydr_cache[0]*dr_did,0};
            y_inter[0] = cubicInterpolation(x,&inter_param);
            y = y_inter;
            break;
        }

        if ((y_cache[0]-phi_metaMin)*ysign<0)
        {
            // Already passing the desired ending point
            convergQ = OVERSHOOT;
            inter_param = {y[0],dydr[0]*dr_did,y_cache[0],dydr_cache[0]*dr_did,phi_metaMin};
            x = find_root_gsl_wraper(&cubicInterpolation,&inter_param,1,0);
            r += dr_did*x;
            inter_param = {y[1],dydr[1]*dr_did,y_cache[1],dydr_cache[1]*dr_did,0};
            y_inter[1] = cubicInterpolation(x,&inter_param);
            inter_param = {y[0],dydr[0]*dr_did,y_cache[0],dydr_cache[0]*dr_did,0};
            y_inter[0] = cubicInterpolation(x,&inter_param);
            y = y_inter;
            break;
        }

        r = r_cache;
        y = y_cache;
        dydr = dydr_cache;
        dr_guess = dr_next;
    }
    y_diff = abs(y-y_final_value);
    if ( y_diff[0] < epsabs && y_diff[1] < epsabs)
    {
        convergQ = CONVERGED;
    }
    return make_tuple(r,y,convergQ);
}

tuple<VD, VD, VD, double> Tunneling1D::integrateAndSaveProfile(VD R, VD y0, double dr, double epsfrac, double epsabs, double drmin)
{
    int N = R.size();
    double r0 = R[0];
    VVD Yout(y0.size(),VD(N,0));
    Yout[0][0] = y0[0];
    Yout[1][0] = y0[1];
    VD dydr0 = equationOfMotion(r0,y0);
    double Rerr = NAN;

    int i = 1;
    double r = r0;
    VD y = y0;
    VD dydr = dydr0;
    double r_cache;
    VD y_cache;
    VD dydr_cache;
    double dr_guess = dr;
    double dr_did,dr_next;
    VD y_scale(2);
    cubic_param inter_param;
    while (i<N)
    {
        y_scale = abs(y)+abs(dydr*dr_guess);
        r_cache = r;
        y_cache = y;
        dydr_cache = dydr;
        _rk_calculator._RKQC_SingleStep(r_cache,y_cache,dydr_cache,dr_guess,epsabs,y_scale,dr_did,dr_next);
        if (dr_did < drmin)
        {
            y_cache = y + (y_cache-y)*drmin/dr_did;
            dr_did = drmin;
            dr_next = drmin;
            r_cache = r + dr_did;
            if (!(std::isnan(Rerr)))
            {
                Rerr = r_cache;
            }
        }
        dydr_cache = equationOfMotion(r_cache,y_cache);
        if (r < R[i] && R[i] <= r_cache)
        {
            while (i < N && r < R[i] && R[i] <= r_cache)
            {
                double x = (R[i]-r)/dr_did;
                inter_param = {y[0], dr_did*dydr[0], y_cache[0], dr_did*dydr_cache[0], 0};
                Yout[0][i] = cubicInterpolation(x, &inter_param);
                inter_param = {y[1], dr_did*dydr[1], y_cache[1], dr_did*dydr_cache[1], 0};
                Yout[1][i] = cubicInterpolation(x, &inter_param);
                i += 1;
            }
        }

        r = r_cache;
        y = y_cache;
        dydr = dydr_cache;
        dr_guess = dr_next;
    }

    return make_tuple(R,Yout[0],Yout[1],Rerr);
}
tuple<VD,VD,VD,double> Tunneling1D::findProfile(double xguess,double xtol,double phitol,double thinCutoff,int npoints,double rmin, double rmax, int max_interior_pts)
{
    double xmin = xtol*10;
    double xmax = INFINITY;
    double x;
    if (!std::isnan(xguess))
    {
        x = xguess;
    }
    else
    {
        x = - log(abs((phi_bar-phi_absMin)/(phi_metaMin-phi_absMin)));
    }
    // cout<<"Starting point: x = "<<x<<endl;
    double xincrease = 5.0;

    rmin *= rscale;
    double dr0 = rmin;
    double drmin = rmin*1e-2;
    rmax *= rscale;

    double delta_phi = phi_metaMin - phi_absMin;
    double epsabs = abs(delta_phi*phitol);
    double epsfrac = phitol;
    double delta_phi_cutoff = thinCutoff*delta_phi;

    double rf = NAN;
    double delta_phi0;
    double r0_,phi0,dphi0;
    double r0;
    VD y0;
    VD yf;
    CONVERGENCETYPE ctype;
    // cout<<"Starting of the shooting: "<<endl;
    while (true)
    {
        // cout<<"--------"<<endl;
        delta_phi0 = exp(-x)*delta_phi;
        // cout<<"\tdelta_phi0="<<delta_phi0<<endl;
        std::tie(r0_,phi0,dphi0) = initialConditions(delta_phi0,rmin,delta_phi_cutoff);
        // cout<<"\tInitial condition: r0="<<r0_<<"  phi0="<<phi0<<" dphi0="<<dphi0<<endl;
        if ( !std::isfinite(r0_) || !std::isfinite(x))
        {
            if (std::isnan(rf))
            {
                cerr<<"Failed to retrieve initial conditions on the first try"<<endl;
            }
            break;
        }
        r0 = r0_;
        y0 = {phi0,dphi0};
        std::tie(rf,yf,ctype) = integrateProfile(r0,y0,dr0,epsfrac,epsabs,drmin,rmax);
        if (ctype == CONVERGED)
        {
            break;
        }
        else if (ctype == UNDERSHOOT)
        {
            xmin = x;
            x = std::isfinite(xmax)?(xmin+xmax)/2:x*xincrease;
        }
        else if (ctype == OVERSHOOT)
        {
            xmax = x;
            x = (xmin+xmax)/2;
        }

        if (xmax-xmin < xtol)
        {
            break;
        }
    }

    VD R(npoints);
    for (size_t i = 0; i < npoints; i++)
    {
        R[i] = r0 + i*(rf-r0)/(npoints-1);
    }
    VD Phi_ex;
    VD dPhi_ex;
    double Rerr;
    std::tie(R,Phi_ex,dPhi_ex,Rerr)=integrateAndSaveProfile(R,y0,dr0,epsfrac,epsabs,drmin);

    VD R_int;
    if (max_interior_pts < 0)
    {
        max_interior_pts = R.size()/2;
    }
    if (max_interior_pts > 0)
    {
        double dx0 = R[1]-R[0];
        if (R[0]/dx0 <= max_interior_pts)
        {
            int n = ceil(R[0]/dx0);
            for (size_t i = 0; i < n; i++)
            {
                R_int.push_back(0+i*(R[0])/(n));
            }
        }
        else
        {
            int n = max_interior_pts;
            double a = (R[0]/dx0 - n)*2/(n*(n+1));
            for (size_t i = 0; i < n; i++)
            {
                int k = n-i;
                R_int.push_back(R[0]-dx0*(k + a*k*(k+1)/2));
            }
            R_int[0] = 0.0;
        }
    }
    VD Phi_int(R_int.size(),0);
    VD dPhi_int(R_int.size(),0);
    Phi_int[0] = phi_absMin + delta_phi0;
    dPhi_int[0] = 0.0;
    double dV_ = dV_from_absMin(delta_phi0);
    double d2V_ = d2V({Phi_int[0]},0)[0][0];
    for (size_t i = 1; i < R_int.size(); i++)
    {
        std::tie(Phi_int[i],dPhi_int[i]) = exactSolution(R_int[i],Phi_int[0],dV_,d2V_);
    }
    VD R_final(R_int);
    VD Phi_final(Phi_int);
    VD dPhi_final(dPhi_int);

    R_final.insert(R_final.end(),R.begin(),R.end());
    Phi_final.insert(Phi_final.end(),Phi_ex.begin(),Phi_ex.end());
    dPhi_final.insert(dPhi_final.end(),dPhi_ex.begin(),dPhi_ex.end());

    return make_tuple(R_final,Phi_final,dPhi_final,Rerr);

}
double Tunneling1D::findAction(VD R, VD Phi, VD dPhi)
{
    int N = R.size();
    double Sphere_area = pow(M_PI,Spatial_Dim/2.0)/tgamma(Spatial_Dim/2.0);
    VD area = pow(R,alpha)*Sphere_area;
    VD integrand(N);
    double T = 0;
    for (size_t i = 0; i < N; i++)
    {
        integrand[i] = (pow(dPhi[i],2)/2 + V({Phi[i]},&T) - V({phi_metaMin},&T))*area[i];
    }
    double S = Simpson(R,integrand);

    // For the bulk inside the bubble interior
    double volume = pow(R[0],Spatial_Dim)*pow(M_PI,Spatial_Dim/2.0)/tgamma(Spatial_Dim/2.0+1.0);
    S += volume*(V({Phi[0]},&T)-V({phi_metaMin},&T));
    return S;
}
std::tuple<VD, VD> Tunneling1D::evenlySpacedPhi(VD phi, VD dphi, int npoint, int k, bool fixAbs)
{
    if (fixAbs)
    {
        phi.insert(phi.begin(),phi_absMin);
        phi.insert(phi.end(),phi_metaMin);
        dphi.insert(dphi.begin(),0.0);
        dphi.insert(dphi.end(),0.0);
    }
    else
    {
        phi.insert(phi.end(),phi_metaMin);
        dphi.insert(dphi.end(),0.0);
    }

    // Sort phi in increasing order
    VVD fullPhi = transpose({phi,dphi});
    // cout<<"fullPhi dim: ("<<fullPhi.size()<<","<<fullPhi[0].size()<<")"<<endl;
    sort(fullPhi.begin(),fullPhi.end(),[](VD x1, VD x2){return x1[0]<x2[0];});
    VVD::iterator iter = unique(fullPhi.begin(),fullPhi.end(),[](VD x1, VD x2){return x1[0]==x2[0];});
    fullPhi.resize(distance(fullPhi.begin(),iter));
    fullPhi=transpose(fullPhi);
    // cout<<fullPhi[0]<<endl;

    GSL_Spline_Inter inter;
    inter.SetData(&fullPhi[1],&fullPhi[0]);

    VD p;
    if (fixAbs)
    {
        p = linspace(phi_absMin,phi_metaMin,npoint);
    }
    else
    {
        p = linspace(phi[0],phi_metaMin,npoint);
    }

    return make_tuple(p,inter.valAt(p));
}


//
//  cal_fe_global_plastic_vary.hpp
//  hybrid_fem_bie
//
//  Created by Max on 3/21/18.
//
//

#ifndef cal_fe_global_plastic_vary_hpp
#define cal_fe_global_plastic_vary_hpp

#include <stdio.h>
#include <Eigen/Eigen>
#include "mapglobal.hpp"
#include "maplocal.hpp"
#include "cal_fe_int_el.hpp"
using namespace Eigen;

void cal_fe_global_plastic_vary(int n_el, MatrixXi &index_store,std::vector<MatrixXd> &ke,std::vector<std::vector<MatrixXd>> &B_mat,
                                std::vector<double> &detJ, double E, double nu, double q, VectorXd &u_n, VectorXd &v_n, int Ndofn, VectorXd &fe_global, std::vector<MatrixXd> &strain_n_store,std::vector<MatrixXd> &stress_n_store,double sxx_initial,double syy_initial, double sxy_initial, double cohes, double blkfric,std::vector<double> &eq_ep_out,double &E_p);
#endif /* cal_fe_global_plastic_vary_hpp */


#ifndef ASLAM_CAMERAS_RT_DISTORTION_HPP
#define ASLAM_CAMERAS_RT_DISTORTION_HPP

#include <Eigen/Dense>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/version.hpp>
#include <sm/boost/serialization.hpp>
#include "StaticAssert.hpp"

namespace sm {
class PropertyTree;
}  // namespace sm

namespace aslam {
namespace cameras {

/**
 * \class RadialTangentialDistortion
 * \brief An implementation of the standard, four parameter distortion model for pinhole cameras.
 *
 * \todo outline the math here and provide a reference. What is the original reference?
 *
 *
 * The usual model of a pinhole camera follows these steps:
 *   - Transformation: Transform the point into a coordinate frame associated with the camera
 *   - Normalization:  Project the point onto the normalized image plane: \f$\mathbf y := \left[ x/z,y/z\right] \f$
 *   - Distortion:     apply a nonlinear transformation to \f$y\f$ to account for radial and tangential distortion of
 * the lens
 *   - Projection:     Project the point into the image using a standard \f$3 \time 3\f$ projection matrix
 *
 * This class represents a standard implementation of the distortion block. The function "distort" applies this
 * nonlinear transformation. The function "undistort" applies the inverse transformation. Note that the inverse
 * transformation in this case is not avaialable in closed form and so it is computed iteratively.
 *
 */
class RadialTangentialDistortion {
  public:
    enum { IntrinsicsDimension = 5 };
    enum { DesignVariableDimension = IntrinsicsDimension };

    /// \brief The default constructor sets all values to zero.
    RadialTangentialDistortion();

    /// \brief A constructor that initializes all values.
    RadialTangentialDistortion(double k1, double k2, double p1, double p2, double k3 = 0.0);

    RadialTangentialDistortion(const sm::PropertyTree& config);

    virtual ~RadialTangentialDistortion();

    /**
     * \brief Apply distortion to a point in the normalized image plane
     *
     * @param y The point in the normalized image plane. After the function, this point is distorted.
     */
    template <typename DERIVED_Y>
    void distort(const Eigen::MatrixBase<DERIVED_Y>& y) const;

    /**
     *
     * \brief Apply distortion to a point in the normalized image plane
     *
     * @param y The point in the normalized image plane. After the function, this point is distorted.
     * @param outJy The Jacobian of the distortion function with respect to small changes in the input point.
     */
    template <typename DERIVED_Y, typename DERIVED_JY>
    void distort(const Eigen::MatrixBase<DERIVED_Y>& y, const Eigen::MatrixBase<DERIVED_JY>& outJy) const;

    /**
     * \brief Apply undistortion to recover a point in the normalized image plane.
     *
     * @param y The distorted point. After the function, this point is in the normalized image plane.
     */
    template <typename DERIVED>
    void undistort(const Eigen::MatrixBase<DERIVED>& y) const;

    /**
     * \brief Apply undistortion to recover a point in the normalized image plane.
     *
     * @param y The distorted point. After the function, this point is in the normalized image plane.
     * @param outJy The Jacobian of the undistortion function with respect to small changes in the input point.
     */
    template <typename DERIVED, typename DERIVED_JY>
    void undistort(const Eigen::MatrixBase<DERIVED>& y, const Eigen::MatrixBase<DERIVED_JY>& outJy) const;

    /**
     * \brief Apply distortion to the point and provide the Jacobian of the distortion with respect to small changes in
     * the distortion parameters
     *
     * @param imageY the point in the normalized image plane.
     * @param outJd  the Jacobian of the distortion with respect to small changes in the distortion parameters.
     */
    template <typename DERIVED_Y, typename DERIVED_JD>
    void distortParameterJacobian(const Eigen::MatrixBase<DERIVED_Y>& imageY,
                                  const Eigen::MatrixBase<DERIVED_JD>& outJd) const;

    /**
     * \brief A function for compatibility with the aslam backend. This implements an update of the distortion
     * parameter.
     *
     * @param v A double array representing the update vector.
     */
    void update(const double* v);

    /**
     * \brief A function for compatibility with the aslam backend.
     *
     * @param v The number of parameters expected by the update equation. This should also define the number of columns
     * in the matrix returned by distortParameterJacobian.
     */
    int minimalDimensions() const;

    /**
     * \brief A function for compatibility with the aslam backend.
     *
     * @param P This matrix is resized and filled with parameters representing the full state of the distortion.
     */
    void getParameters(Eigen::MatrixXd& P) const;

    /**
     * \brief A function for compatibility with the aslam backend.
     *
     * @param P The full state of the distortion class is set from the matrix of parameters.
     */
    void setParameters(const Eigen::MatrixXd& P);

    Eigen::Vector2i parameterSize() const;

    /// \brief the first radial distortion parameter
    double k1() { return _k1; }
    /// \brief the second radial distortion parameter
    double k2() { return _k2; }
    /// \brief the first tangential distortion parameter
    double p1() { return _p1; }
    /// \brief the second tangential distortion parameter
    double p2() { return _p2; }
    /// \brief the third radial distortion parameter
    double k3() { return _k3; }

    void clear() {
        _k1 = 0.0;
        _k2 = 0.0;
        _p1 = 0.0;
        _p2 = 0.0;
        _k3 = 0.0;
    }

    /// \brief Compatibility with boost::serialization.
    enum { CLASS_SERIALIZATION_VERSION = 0 };
    BOOST_SERIALIZATION_SPLIT_MEMBER();
    template <class Archive>
    void load(Archive& ar, const unsigned int version);
    template <class Archive>
    void save(Archive& ar, const unsigned int version) const;

    bool isBinaryEqual(const RadialTangentialDistortion& rhs) const;

    static RadialTangentialDistortion getTestDistortion();

    /// \brief the first radial distortion parameter
    double _k1;
    /// \brief the second radial distortion parameter
    double _k2;
    /// \brief the first tangential distortion parameter
    double _p1;
    /// \brief the second tangential distortion parameter
    double _p2;
    // the third radial distortion parameter
    double _k3;
};

}  // namespace cameras
}  // namespace aslam

#include "implementation/RadialTangentialDistortion.hpp"

SM_BOOST_CLASS_VERSION(aslam::cameras::RadialTangentialDistortion);

#endif /* ASLAM_CAMERAS_DISTORTION_HPP */


#ifndef CUAUV_SIM_PID_H
#define CUAUV_SIM_PID_H

#include <Eigen/Geometry>

namespace cuauv {
namespace fishbowl {

class pid {
public:
    pid(double p, double i, double d, double r);

    /**
     * Steps the PID controller and returns a new output value.
     *
     * @param dt The time in seconds since this method was last called.
     * @param desired The desired value.
     * @param actual The actual value.
     */
    double step(double dt, double desired, double actual);

    double p;
    double i;
    double d;
    double r;

    double ei = 0; //!< Integrated error.
    double ep = 0; //!< Error in the previous timestep.
    bool epb = false; //!< Whether or not to use ep. Changed to true after first step.
};

/**
 * Euler angle PID controller.
 */
class epid {
public:
    epid(double p, double i, double d, double r);

    double step(double dt, double desired, double actual);

    double p;
    double i;
    double d;
    double r;

    double ei = 0; //!< Integrated error.
    double ep = 0; //!< Error in the previous timestep.
    bool epb = false;
};

} // namespace fishbowl
} // namespace cuauv

#endif // CUAUV_SIM_PID_H


/*
 * Copyright (C) 2019 Swift Navigation Inc.
 * Contact: Swift Navigation <dev@swiftnav.com>
 *
 * This source is subject to the license found in the file 'LICENSE' which must
 * be distributed together with this source. All other rights reserved.
 *
 * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
 * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
 */

#include <gtest/gtest.h>

#include <Eigen/Cholesky>

#include <albatross/GP>
#include <albatross/utils/RandomUtils>

#include "test_utils.h"

namespace albatross {

struct BlockExample {
  BlockDiagonal block;
  Eigen::MatrixXd dense;
};

BlockExample block_example() {
  Eigen::Index block_size = 3;
  Eigen::Index block_count = 2;
  Eigen::Index n = block_size * block_count;

  BlockDiagonal block_diag;
  Eigen::MatrixXd dense = Eigen::MatrixXd::Zero(n, n);
  for (Eigen::Index i = 0; i < block_count; ++i) {
    Eigen::MatrixXd m = Eigen::MatrixXd::Random(block_size, block_size);
    m = m.transpose() * m;
    block_diag.blocks.push_back(m);
    dense.block(i * block_size, i * block_size, block_size, block_size) = m;
  }
  return {block_diag, dense};
}

TEST(test_block_utils, test_to_dense) {

  auto example = block_example();
  auto block_diag = example.block;
  auto dense = example.dense;

  const auto block_result = block_diag.toDense();
  EXPECT_LE((block_result - dense).norm(), 1e-6);
}

TEST(test_block_utils, test_block_ldlt) {
  const auto example = block_example();
  const BlockDiagonalLDLT ldlt = example.block.ldlt();

  const Eigen::MatrixXd expect_identity = ldlt.solve(example.dense);
  const Eigen::MatrixXd identity =
      Eigen::MatrixXd::Identity(example.dense.rows(), example.dense.cols());
  EXPECT_LT((expect_identity - identity).norm(), 1e-8);

  EXPECT_NEAR(example.dense.determinant(),
              exp(example.block.ldlt().log_determinant()), 1e-8);
}

TEST(test_block_utils, test_diagonal) {

  auto example = block_example();
  auto block_diag = example.block;
  auto dense = example.dense;

  const auto block_result = block_diag.diagonal();
  EXPECT_LE((block_result - dense.diagonal()).norm(), 1e-6);
}

TEST(test_block_utils, test_dot_product) {

  auto example = block_example();
  auto block_diag = example.block;
  auto dense = example.dense;

  Eigen::MatrixXd rhs = Eigen::MatrixXd::Random(dense.cols(), 3);

  const Eigen::MatrixXd block_result = block_diag * rhs;
  const Eigen::MatrixXd dense_result = dense * rhs;

  EXPECT_LE((block_result - dense_result).norm(), 1e-6);
}

TEST(test_block_utils, test_solve) {

  auto example = block_example();
  auto block_diag = example.block;
  auto dense = example.dense;

  Eigen::VectorXd rhs = Eigen::VectorXd::Random(dense.cols());
  const auto block_result = block_diag.ldlt().solve(rhs);

  const Eigen::VectorXd dense_result = dense.ldlt().solve(rhs);

  EXPECT_LE((block_result - dense_result).norm(), 1e-6);
}

TEST(test_block_utils, test_sqrt_methods) {

  auto example = block_example();
  auto block_diag = example.block;
  auto dense = example.dense;

  Eigen::MatrixXd rhs = Eigen::MatrixXd::Random(dense.cols(), 3);
  const auto block_ldlt = block_diag.ldlt();

  const Eigen::MatrixXd block_sqrt = block_ldlt.sqrt_solve(rhs);
  const Eigen::MatrixXd block_result = block_sqrt.transpose() * block_sqrt;

  const Eigen::SerializableLDLT dense_ldlt = dense.ldlt();
  const Eigen::MatrixXd dense_sqrt = dense_ldlt.sqrt_solve(rhs);
  const Eigen::MatrixXd dense_result = dense_sqrt.transpose() * dense_sqrt;

  EXPECT_LE((block_result - dense_result).norm(), 1e-6);
}

TEST(test_block_utils, test_block_symmetric) {

  std::default_random_engine gen(2012);
  const auto X = random_covariance_matrix(5, gen);

  const Eigen::MatrixXd rhs = Eigen::MatrixXd::Random(X.cols(), 3);
  const Eigen::MatrixXd expected = X.ldlt().solve(rhs);

  const Eigen::MatrixXd A = X.topLeftCorner(3, 3);
  const Eigen::MatrixXd B = X.topRightCorner(3, 2);
  const Eigen::MatrixXd C = X.bottomRightCorner(2, 2);

  // Test when constructing from the actual blocks.
  const auto block = build_block_symmetric(A.ldlt(), B, C);
  const Eigen::MatrixXd actual = block.solve(rhs);
  EXPECT_TRUE(actual.isApprox(expected));

  // And again using a pre computed S
  const Eigen::MatrixXd S = C - B.transpose() * A.ldlt().solve(B);
  const auto block_direct = build_block_symmetric(A.ldlt(), B, S.ldlt());
  const Eigen::MatrixXd actual_direct = block_direct.solve(rhs);
  EXPECT_TRUE(actual_direct.isApprox(expected));
}

} // namespace albatross


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2010-2012 Barend Gehrels, Amsterdam, the Netherlands.

// 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)

// Tests with-strategies, especially point-in-polygon

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/covered_by.hpp>
#include <boost/geometry/algorithms/within.hpp>

#include <boost/geometry/strategies/cartesian/point_in_poly_franklin.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_crossings_multiply.hpp>
#include <boost/geometry/strategies/agnostic/point_in_poly_winding.hpp>
#include <boost/geometry/strategies/cartesian/point_in_box.hpp>
#include <boost/geometry/strategies/cartesian/box_in_box.hpp>
#include <boost/geometry/strategies/agnostic/point_in_box_by_side.hpp>

#include <boost/geometry/strategies/cartesian/side_by_triangle.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>

template <typename Point, typename Polygon, typename Strategy>
void test_point_in_polygon(std::string const& case_id,
            Point const& point, Polygon const& polygon,
            Strategy const& strategy, std::string const& strategy_id,
            bool expected)
{
    BOOST_CONCEPT_ASSERT( (bg::concept::WithinStrategyPolygonal<Strategy>) );
    bool detected = bg::within(point, polygon, strategy);

    BOOST_CHECK_MESSAGE(detected == expected,
            "within: " << case_id
            << " strategy: " << strategy_id // typeid(strategy).name() is too long
            << " output expected: " << int(expected)
            << " detected: " << int(detected)
            );
}

template <typename Point, typename Polygon>
void test_geometry(std::string const& case_id, std::string const& wkt_point
            , std::string const& wkt_polygon
            , bool expected
            , std::string const& deviations = ""
            )
{
    Point point;
    Polygon polygon;
    bg::read_wkt(wkt_point, point);
    bg::read_wkt(wkt_polygon, polygon);

    namespace sw = bg::strategy::within;
    test_point_in_polygon(case_id, point, polygon, sw::winding<Point>(),
        "winding", expected);
    test_point_in_polygon(case_id, point, polygon, sw::franklin<Point>(),
        "franklin", boost::contains(deviations, "f") ? !expected : expected);
    test_point_in_polygon(case_id, point, polygon, sw::crossings_multiply<Point>(),
        "cross.mult", boost::contains(deviations, "c") ? !expected : expected);
}

template <typename Point>
void test_box_of(std::string const& wkt_point, std::string const& wkt_box,
              bool expected_within, bool expected_covered_by)
{
    typedef bg::model::box<Point> box_type;

    Point point;
    box_type box;
    bg::read_wkt(wkt_point, point);
    bg::read_wkt(wkt_box, box);

    bool detected_within = bg::within(point, box);
    bool detected_covered_by = bg::covered_by(point, box);
    BOOST_CHECK_EQUAL(detected_within, expected_within);
    BOOST_CHECK_EQUAL(detected_covered_by, expected_covered_by);

    // Also test with the non-default agnostic side version
    namespace wi = bg::strategy::within;
    wi::point_in_box_by_side<Point, box_type> within_strategy;
    wi::point_in_box_by_side<Point, box_type, wi::decide_covered_by> covered_by_strategy;

    detected_within = bg::within(point, box, within_strategy);
    detected_covered_by = bg::covered_by(point, box, covered_by_strategy);
    BOOST_CHECK_EQUAL(detected_within, expected_within);
    BOOST_CHECK_EQUAL(detected_covered_by, expected_covered_by);

    // We might exchange strategies between within/covered by.
    // So the lines below might seem confusing, but are as intended
    detected_within = bg::covered_by(point, box, within_strategy);
    detected_covered_by = bg::within(point, box, covered_by_strategy);
    BOOST_CHECK_EQUAL(detected_within, expected_within);
    BOOST_CHECK_EQUAL(detected_covered_by, expected_covered_by);

    // Finally we call the strategies directly
    detected_within = within_strategy.apply(point, box);
    detected_covered_by = covered_by_strategy.apply(point, box);
    BOOST_CHECK_EQUAL(detected_within, expected_within);
    BOOST_CHECK_EQUAL(detected_covered_by, expected_covered_by);
}

template <typename Point>
void test_box()
{
    test_box_of<Point>("POINT(1 1)", "BOX(0 0,2 2)", true, true);
    test_box_of<Point>("POINT(0 0)", "BOX(0 0,2 2)", false, true);
    test_box_of<Point>("POINT(2 2)", "BOX(0 0,2 2)", false, true);
    test_box_of<Point>("POINT(0 1)", "BOX(0 0,2 2)", false, true);
    test_box_of<Point>("POINT(1 0)", "BOX(0 0,2 2)", false, true);
    test_box_of<Point>("POINT(3 3)", "BOX(0 0,2 2)", false, false);
}

template <typename Point>
void test_all()
{
    test_box<Point>();

    typedef bg::model::polygon<Point> polygon;

    std::string const box = "POLYGON((0 0,0 2,2 2,2 0,0 0))";
    std::string const triangle = "POLYGON((0 0,0 4,6 0,0 0))";
    std::string const with_hole = "POLYGON((0 0,0 3,3 3,3 0,0 0),(1 1,2 1,2 2,1 2,1 1))";

    test_geometry<Point, polygon>("b1", "POINT(1 1)", box, true);
    test_geometry<Point, polygon>("b2", "POINT(3 3)", box, false);

    // Test ALL corners (officialy false but some strategies might answer true)
    test_geometry<Point, polygon>("b3a", "POINT(0 0)", box, false, "f");
    test_geometry<Point, polygon>("b3b", "POINT(0 2)", box, false);
    test_geometry<Point, polygon>("b3c", "POINT(2 2)", box, false);
    test_geometry<Point, polygon>("b3d", "POINT(2 0)", box, false);

    // Test ALL sides (officialy false but some strategies might answer true)
    test_geometry<Point, polygon>("b4a", "POINT(0 1)", box, false, "f");
    test_geometry<Point, polygon>("b4b", "POINT(1 2)", box, false, "c");
    test_geometry<Point, polygon>("b4c", "POINT(2 1)", box, false);
    test_geometry<Point, polygon>("b4d", "POINT(1 0)", box, false, "f");

    test_geometry<Point, polygon>("t1", "POINT(1 1)", triangle, true);
    test_geometry<Point, polygon>("t2", "POINT(3 3)", triangle, false);

    test_geometry<Point, polygon>("t3a", "POINT(0 0)", triangle, false, "f");
    test_geometry<Point, polygon>("t3b", "POINT(0 4)", triangle, false, "c");
    test_geometry<Point, polygon>("t3c", "POINT(5 0)", triangle, false, "f");

    test_geometry<Point, polygon>("t4a", "POINT(0 2)", triangle, false, "f");
    test_geometry<Point, polygon>("t4b", "POINT(3 2)", triangle, false);
    test_geometry<Point, polygon>("t4c", "POINT(2 0)", triangle, false, "f");

    test_geometry<Point, polygon>("h1", "POINT(0.5 0.5)", with_hole, true);
    test_geometry<Point, polygon>("h2a", "POINT(1.5 1.5)", with_hole, false);
    test_geometry<Point, polygon>("h2b", "POINT(5 5)", with_hole, false);

    test_geometry<Point, polygon>("h3a", "POINT(1 1)", with_hole, false, "c");
    test_geometry<Point, polygon>("h3b", "POINT(2 2)", with_hole, false, "f");
    test_geometry<Point, polygon>("h3c", "POINT(0 0)", with_hole, false, "f");

    test_geometry<Point, polygon>("h4a", "POINT(1 1.5)", with_hole, false);
    test_geometry<Point, polygon>("h4b", "POINT(1.5 2)", with_hole, false, "f");

    // Lying ON (one of the sides of) interior ring
    test_geometry<Point, polygon>("#77-1", "POINT(6 3.5)", "POLYGON((5 3,5 4,4 4,4 5,3 5,3 6,5 6,5 5,7 5,7 6,8 6,8 5,9 5,9 2,8 2,8 1,7 1,7 2,5 2,5 3),(6 3,8 3,8 4,6 4,6 3))", false);
}

int test_main(int, char* [])
{
    test_all<bg::model::point<float, 2, bg::cs::cartesian> >();
    test_all<bg::model::point<double, 2, bg::cs::cartesian> >();

#if defined(HAVE_TTMATH)
    test_all<bg::model::point<ttmath_big, 2, bg::cs::cartesian> >();
#endif

    return 0;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_DIVFLOOR_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_DIVFLOOR_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
//#include <boost/simd/sdk/meta/is_upgradable.hpp>
#include <boost/simd/function/simd/divides.hpp>
#include <boost/simd/function/simd/floor.hpp>
#include <boost/simd/function/simd/groups.hpp>
#include <boost/simd/function/simd/split.hpp>
#include <boost/simd/function/simd/tofloat.hpp>
#include <boost/simd/function/simd/toints.hpp>
#include <boost/simd/function/simd/touints.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(divfloor_
                             , (typename A0, typename X)
                             , bd::cpu_
                             , bs::pack_<bd::int_<A0>, X>
                             , bs::pack_<bd::int_<A0>, X>
                             )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        using ivtype = bd::upgrade_t<A0>;
        ivtype a0l, a0h, a1l, a1h;
        bs::split(a0, a0l, a0h );
        bs::split(a1, a1l, a1h );
        ivtype d0 = toints(divfloor(tofloat(a0l), tofloat(a1l)));
        ivtype d1 = toints(divfloor(tofloat(a0h), tofloat(a1h)));
        return groups(d0, d1);
      }
   };

   BOOST_DISPATCH_OVERLOAD(divfloor_
                             , (typename A0, typename X)
                             , bd::cpu_
                             , bs::pack_<bd::uint_<A0>, X>
                             , bs::pack_<bd::uint_<A0>, X>
                             )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        using ivtype = bd::upgrade_t<A0>;
        ivtype a0l, a0h, a1l, a1h;
        bs::split(a0, a0l, a0h );
        bs::split(a1, a1l, a1h );
        ivtype d0 = touints(divfloor(tofloat(a0l), tofloat(a1l)));
        ivtype d1 = touints(divfloor(tofloat(a0h), tofloat(a1h)));
        return groups(d0, d1);
      }
   };

   BOOST_DISPATCH_OVERLOAD(divfloor_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::ints8_<A0>, X>
                          , bs::pack_<bd::ints8_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        using ivtype = bd::upgrade_t<A0>;
        ivtype a0l, a0h, a1l, a1h;
        bs::split(a0, a0l, a0h);
        bs::split(a1, a1l, a1h);
        ivtype d0 = divfloor(a0l, a1l);
        ivtype d1 = divfloor(a0h, a1h);
        return groups(d0, d1);
      }
   };

   BOOST_DISPATCH_OVERLOAD(divfloor_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        return bs::floor(a0/a1);
      }
   };

} } }

#endif


// Copyright (c) 2014-2016 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "chainparams.h"
#include "net.h"
#include "validation.h"

#include "test/test_bitcoin.h"

#include "signals-cpp/signal.h"
#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(main_tests, TestingSetup)

static void TestBlockRewards(const Consensus::Params &consensusParams) {
  const int maxYears = 4;
  const int nInitialReward = consensusParams.nInitialMiningRewardInCoins;
  const int nBlocksPerYear = consensusParams.nBlocksPerYear;
  Amount nInitialSubsidy = nInitialReward * COIN;

  for (int n = 0; n < maxYears; n++) {
    int nHeight = n * nBlocksPerYear;
    Amount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
    if (n < 2)
      BOOST_CHECK(nSubsidy >= nInitialSubsidy);
    else
      BOOST_CHECK(nSubsidy <= 2 * nInitialReward * COIN);
  }
  // Peak is 1.5 years out. Check here
  BOOST_CHECK_EQUAL(GetBlockSubsidy(1.5*nBlocksPerYear, consensusParams), 2 * nInitialReward * COIN);
  // Check next block reward drops
  BOOST_CHECK(GetBlockSubsidy((1.5*nBlocksPerYear)+1, consensusParams) <  2 * nInitialReward * COIN);
}

BOOST_AUTO_TEST_CASE(block_subsidy_test) {
  const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
  TestBlockRewards(chainParams->GetConsensus()); // As in main
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test) {
  const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
  const int nInitialReward = chainParams->GetConsensus().nInitialMiningRewardInCoins;
  Amount nSum = Amount::zero();
  for (int nHeight = 0; nHeight < 14000000; nHeight += 100000) {
    Amount nSubsidy = GetBlockSubsidy(nHeight, chainParams->GetConsensus());
    BOOST_CHECK(nSubsidy <= 2 * nInitialReward * COIN);
    nSum += nSubsidy;
    BOOST_CHECK(MoneyRange(nSum));
  }
  BOOST_CHECK_EQUAL(nSum, int64_t(1742400000000LL) * SATOSHI);
}
BOOST_AUTO_TEST_SUITE_END()


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file gaussian_filter_linear_vs_unscented_kalman_filter_test.cpp
 * \date July 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include <fl/util/meta.hpp>
#include <fl/filter/gaussian/gaussian_filter.hpp>

#include "../typecast.hpp"
#include "gaussian_filter_linear_vs_x_test_suite.hpp"

template <
    int StateDimension,
    int InputDimension,
    int ObsrvDimension,
    int ModelKey,
    template <typename> class TransitionNoiseType,
    template <typename> class SensorNoiseType
>
struct UkfTestConfig
{
    enum : signed int
    {
        StateDim = StateDimension,
        InputDim = InputDimension,
        ObsrvDim = ObsrvDimension
    };

    enum : signed int { SelectedModel = ModelKey };

    template <typename Transition, typename Sensor>
    struct FilterDefinition
    {
        typedef fl::GaussianFilter<
                    Transition,
                    Sensor,
                    fl::UnscentedQuadrature,
                    fl::SigmaPointPredictPolicy<
                        fl::UnscentedQuadrature,
                        typename fl::UseAs<
                            TransitionNoiseType<Transition>
                        >::Type>,
                    fl::SigmaPointUpdatePolicy<
                        fl::UnscentedQuadrature,
                        typename fl::UseAs<
                            SensorNoiseType<Sensor>
                        >::Type>
                > Type;
    };

    template <typename F, typename H>
    static typename FilterDefinition<F, H>::Type create_filter(F&& f, H&& h)
    {
        return typename FilterDefinition<F, H>::Type(
            f, h, fl::UnscentedQuadrature());
    }
};


/**
 * Copyright 2014 Facebook
 * @author Misha Denil (mdenil@fb.com)
 */

#include <lua.hpp>
#include <luaT.h>

#include <vector>
#include <queue>
#include <utility>
#include <stdexcept>

// Turn off range checking for multi_array indexing
#define BOOST_DISABLE_ASSERTS
#include <boost/multi_array.hpp>

#include "torch/fb/fbcunn/layers/LuaUtils.h"
#include "torch/fb/fbcunn/layers/Tensor.h"

namespace facebook { namespace deeplearning { namespace torch {

namespace {

template<typename T>
struct index_value : public std::pair<long, T> {
  index_value(long const& i, T const& v): std::pair<long, T>(i, v) {}
  long const& index() const { return this->first; }
  T const& value() const { return this->second; }
};

template<typename T>
inline index_value<T> make_index_value(long const& i, T const& v) {
  return index_value<T>(i, v);
}

template<typename T>
struct value_order {
  bool operator()(index_value<T> const& a, index_value<T> const& b) {
    return a.value() > b.value();
  }
};

template<typename T>
struct index_order {
  bool operator()(index_value<T> const& a, index_value<T> const& b) {
    return a.index() < b.index();
  }
};

template<typename T>
boost::multi_array_ref<T, 2> tensor_array_view(Tensor<T>& tensor) {
  return boost::multi_array_ref<T, 2>(
    tensor.data(),
    boost::extents[tensor.size(0)][tensor.size(1)]);
}

template<typename T>
boost::const_multi_array_ref<T, 2> tensor_array_view(Tensor<T> const& tensor) {
  return boost::const_multi_array_ref<T, 2>(
    tensor.data(),
    boost::extents[tensor.size(0)][tensor.size(1)]);
}

template<typename T>
boost::multi_array_ref<T, 1> tensor_vector_view(Tensor<T>& tensor) {
  return boost::multi_array_ref<T, 1>(
    tensor.data(),
    boost::extents[tensor.size(0)]);
}

template<typename T>
boost::const_multi_array_ref<T, 1> tensor_vector_view(Tensor<T> const& tensor) {
  return boost::const_multi_array_ref<T, 1>(
    tensor.data(),
    boost::extents[tensor.size(0)]);
}

template<typename T>
using value_priority_queue = std::priority_queue<
  index_value<T>,
  std::vector<index_value<T>>,
  value_order<T>>;

// Forward pass for a single example
template<typename T>
void updateOutput_single(
  Tensor<T> const& input_tensor,
  Tensor<T> const& norms_tensor,
  Tensor<long>& switches_tensor,
  Tensor<T>& output_tensor,
  long k) {

  auto input_data = tensor_array_view(input_tensor);
  auto norms_data = tensor_vector_view(norms_tensor);
  auto output_data = tensor_array_view(output_tensor);
  auto switches_data = tensor_vector_view(switches_tensor);

  // compute the k-max norms
  value_priority_queue<T> kmax;
  for (int row = 0; row < norms_tensor.size(0); ++row) {
    T value = norms_data[row];

    if (kmax.size() < k || value > kmax.top().value()) {
      kmax.push(make_index_value(row, value));

      if (kmax.size() > k) {
        kmax.pop();
      }
    }
  }

  // re-order k-max into input order
  std::vector<index_value<T>> kmax_sorted;
  while (!kmax.empty()) {
    kmax_sorted.push_back(kmax.top());
    kmax.pop();
  }
  std::sort(
    kmax_sorted.begin(),
    kmax_sorted.end(),
    index_order<T>());

  // copy the k-max indices into switches
  std::transform(
    kmax_sorted.begin(),
    kmax_sorted.end(),
    switches_data.begin(),
    [](index_value<T> const& v) -> long {
      return v.index();
    });

  // copy each k-max row to the output
  for (int row = 0; row < kmax_sorted.size(); ++row) {
    long input_row = switches_data[row];

    for (int col = 0; col < input_tensor.size(1); ++col) {
      output_data[row][col] = input_data[input_row][col];
    }
  }

}

// Forward pass
template <class T>
int updateOutput(lua_State* L) {
  auto const input_tensor = luaGetTensorChecked<T>(L, 2);
  auto const norms_tensor = luaGetTensorChecked<T>(L, 3);
  auto switches_tensor = luaGetFieldIfTensorChecked<long>(L, 1, "switches");
  auto k = luaGetFieldIfNumberChecked<long>(L, 1, "k");
  auto k_dynamic = luaGetFieldIfNumberChecked<double>(L, 1, "k_dynamic");
  auto output_tensor = luaGetFieldIfTensorChecked<T>(L, 1, "output");
  int outputIdx = lua_gettop(L);

  luaL_argcheck(L,
    input_tensor.ndims() == 2 || input_tensor.ndims() == 3,
    1,
    "input must have 2 or 3 dimensions.");

  // Set k for dynamic k-max pooling
  if (k_dynamic > 0) {
    int sentence_dim = input_tensor.ndims() - 2;
    k = std::max(k, static_cast<long>(k_dynamic * input_tensor.size(sentence_dim)));
  }

  if (input_tensor.ndims() == 2) {
    output_tensor.resize(LongStorage{k, input_tensor.size(1)});
    switches_tensor.resize(LongStorage{k});
    output_tensor.fill(0.0);
    switches_tensor.fill(0);

    updateOutput_single(
      input_tensor,
      norms_tensor,
      switches_tensor,
      output_tensor,
      k);
  }
  else {
    output_tensor.resize(
      LongStorage{input_tensor.size(0), k, input_tensor.size(2)});
    switches_tensor.resize(
      LongStorage{input_tensor.size(0), k});
    output_tensor.fill(0.0);
    switches_tensor.fill(0);

    #pragma omp parallel for
    for (int i = 0; i < input_tensor.size(0); ++i) {
      // Copy meta-data but not real-data
      Tensor<T> input_tensor_example = input_tensor;
      Tensor<T> norms_tensor_example = norms_tensor;
      Tensor<long> switches_tensor_example = switches_tensor;
      Tensor<T> output_tensor_example = output_tensor;

      // Narrow to the current example
      input_tensor_example.select(0, i);
      norms_tensor_example.select(0, i);
      switches_tensor_example.select(0, i);
      output_tensor_example.select(0, i);

      updateOutput_single(
        input_tensor_example,
        norms_tensor_example,
        switches_tensor_example,
        output_tensor_example,
        k);
    }
  }

  lua_pushvalue(L, outputIdx);
  return 1;
}

// Backprop for a single example
template<typename T>
void updateGradInput_singe(
  Tensor<T> const& gradOutput_tensor,
  Tensor<long> const& switches_tensor,
  Tensor<T>& gradInput_tensor) {

  auto gradInput_data = tensor_array_view(gradInput_tensor);
  auto const gradOutput_data = tensor_array_view(gradOutput_tensor);
  auto const switches_data = tensor_vector_view(switches_tensor);

  for (int row = 0; row < gradOutput_tensor.size(0); ++row) {
    long input_row = switches_data[row];

    for (int col = 0; col < gradOutput_tensor.size(1); ++col) {
      gradInput_data[input_row][col] = gradOutput_data[row][col];
    }
  }
}

// Backprop
template <class T>
int updateGradInput(lua_State* L) {
  auto const input_tensor = luaGetTensorChecked<T>(L, 2);
  auto const gradOutput_tensor = luaGetTensorChecked<T>(L, 3);
  auto const switches_tensor =
    luaGetFieldIfTensorChecked<long>(L, 1, "switches");
  auto gradInput_tensor = luaGetFieldIfTensorChecked<T>(L, 1, "gradInput");
  int gradInputIdx = lua_gettop(L);

  luaL_argcheck(L,
    gradOutput_tensor.ndims() == 2 || gradOutput_tensor.ndims() == 3,
    1,
    "gradOutput must have exactly 2 or 3 dimensions.");

  gradInput_tensor.resizeAs(input_tensor);
  gradInput_tensor.fill(0.0);

  if (gradOutput_tensor.ndims() == 2) {
    updateGradInput_singe(
      gradOutput_tensor,
      switches_tensor,
      gradInput_tensor);
  }
  else {

    #pragma omp parallel for
    for (int i = 0; i < gradOutput_tensor.size(0); ++i) {
      // Copy meta-data but not real-data
      Tensor<T> gradOutput_tensor_example = gradOutput_tensor;
      Tensor<long> switches_tensor_example = switches_tensor;
      Tensor<T> gradInput_tensor_example = gradInput_tensor;

      // Narrow to the current example
      gradOutput_tensor_example.select(0, i);
      switches_tensor_example.select(0, i);
      gradInput_tensor_example.select(0, i);

      updateGradInput_singe(
        gradOutput_tensor_example,
        switches_tensor_example,
        gradInput_tensor_example);
    }
  }

  lua_pushvalue(L, gradInputIdx);
  return 1;
}

template <class T>
class Registerer {
 private:
  static const luaL_Reg functions_[];

 public:
  static void registerFunctions(lua_State* L);
};

template <class T>
const luaL_Reg Registerer<T>::functions_[] = {
  {"GroupKMaxPooling_updateOutput", updateOutput<T>},
  {"GroupKMaxPooling_updateGradInput", updateGradInput<T>},
  {nullptr, nullptr},
};

template <class T>
void Registerer<T>::registerFunctions(lua_State* L) {
  luaT_pushmetatable(L, Tensor<T>::kLuaTypeName);
  luaT_registeratname(L, functions_, "nn");
  lua_pop(L, 1);
}

}  // namespace

void initGroupKMaxPooling(lua_State* L) {
  Registerer<float>::registerFunctions(L);
  Registerer<double>::registerFunctions(L);
}

}}}  // namespaces


/*
 * Eigen - an OCaml interface to C++ Eigen library
 * Copyright (c) 2016-2017 Liang Wang <liang.wang@cl.cam.ac.uk>
 */

#include <iostream>
#include <Eigen/Core>
#include <Eigen/CXX11/Tensor>
#include <Eigen/NeuralNetwork/eigen_spatial_convolutions.h>
#include <Eigen/NeuralNetwork/eigen_backward_spatial_convolutions.h>
#include <Eigen/NeuralNetwork/eigen_cuboid_convolution.h>
#include <Eigen/NeuralNetwork/eigen_backward_cuboid_convolutions.h>
#include <Eigen/NeuralNetwork/eigen_pooling.h>
#include "eigen_tensor.h"

#define C_PADDING_SAME 0
#define C_PADDING_VALID 1

using namespace Eigen;

#include "eigen_tensor_s.cpp"
#include "eigen_tensor_d.cpp"


/*==============================================================================
    Copyright (c) 2005-2010 Joel de Guzman
    Copyright (c) 2010 Thomas Heller

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/

#include <boost/phoenix/phoenix.hpp>
#include <boost/proto/proto.hpp>
#include <boost/proto/debug.hpp>

namespace phoenix = boost::phoenix;
namespace proto = boost::proto;

struct invert_actions
{
    template <typename Rule>
    struct when
        : proto::_
    {};
};

using phoenix::evaluator;

#ifdef _MSC_VER
// redifining evaluator, this is because MSVC chokes on function types like:
// F(G(...))
#define evaluator(A0, A1) proto::call<phoenix::evaluator(A0, A1)>
#endif

template <>
struct invert_actions::when<phoenix::rule::plus>
    : proto::call<
        phoenix::functional::make_minus(
            evaluator(proto::_left, phoenix::_context)
          , evaluator(proto::_right, phoenix::_context)
        )
    >
{};

template <>
struct invert_actions::when<phoenix::rule::minus>
    : proto::call<
        phoenix::functional::make_plus(
            evaluator(proto::_left, phoenix::_context)
          , evaluator(proto::_right, phoenix::_context)
        )
    >
{};

template <>
struct invert_actions::when<phoenix::rule::multiplies>
    : proto::call<
        phoenix::functional::make_divides(
            evaluator(proto::_left, phoenix::_context)
          , evaluator(proto::_right, phoenix::_context)
        )
    >
{};

template <>
struct invert_actions::when<phoenix::rule::divides>
    : proto::call<
        phoenix::functional::make_multiplies(
            evaluator(proto::_left, phoenix::_context)
          , evaluator(proto::_right, phoenix::_context)
        )
    >
{};

#ifdef _MSC_VER
#undef evaluator
#endif

template <typename Expr>
void print_expr(Expr const & expr)
{
    std::cout << "before inversion:\n";
    proto::display_expr(expr);
    std::cout << "after inversion:\n";
    proto::display_expr(
        phoenix::eval(
            expr
          , phoenix::context(
                phoenix::nothing
              , invert_actions()
            )
        )
    );
    std::cout << "\n";
}

template <typename Expr>
typename
    boost::phoenix::result_of::eval<
        Expr const&
      , phoenix::result_of::make_context<
            phoenix::result_of::make_env<>::type
          , invert_actions
        >::type
    >::type
invert(Expr const & expr)
{
    return
        phoenix::eval(
            expr
          , phoenix::make_context(
                phoenix::make_env()
              , invert_actions()
            )
        );
}

int main()
{
    using phoenix::placeholders::_1;
    using phoenix::placeholders::_2;
    using phoenix::placeholders::_3;
    using phoenix::placeholders::_4;

    print_expr(_1);
    print_expr(_1 + _2);
    print_expr(_1 + _2 - _3);
    print_expr(_1 * _2);
    print_expr(_1 * _2 / _3);
    print_expr(_1 * _2 + _3);
    print_expr(_1 * _2 - _3);
    print_expr(if_(_1 * _4)[_2 - _3]);

    print_expr(_1 * invert(_2 - _3));
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/function/raw.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/constant/halfeps.hpp>
#include <scalar_test.hpp>

namespace bs = boost::simd;

STF_CASE_TPL("Check basic behavior of raw_(frexp)", STF_IEEE_TYPES)
{
  STF_EXPR_IS( (bs::raw_(bs::frexp)(T(0))), (std::pair<T,T>) );

  auto p = bs::raw_(bs::frexp)(T(1));
  STF_EQUAL(p.first  , T(0.5));
  STF_EQUAL(p.second , T(1));
}

STF_CASE_TPL("Check behavior of raw_(frexp) on Valmax", STF_IEEE_TYPES)
{
  auto r = bs::raw_(bs::frexp)(bs::Valmax<T>());

  STF_ULP_EQUAL (r.first , T(1)-bs::Halfeps<T>(), 1);
  STF_EQUAL     (r.second, bs::Limitexponent<T>());
  STF_EQUAL     (ldexp(r.first,bs::toint(r.second)),bs::Valmax<T>());
}


#include "Fracture.h"
#include "MeshIO.h"

#include <deque>
#include <fstream>
#include <boost/geometry.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

namespace bg = boost::geometry;
typedef bg::model::d2::point_xy<double> BPoint;
typedef bg::model::polygon<BPoint, false> BPolygon;
using bg::get;

//one hole case for now.

void FracturePolygon::DividePolygon(TPolys &Polygons, TPolys::iterator it) {
	if (it->InnerRings.size() == 0)return;

	TRing &OuterRing = it->OuterRing;
	TRings &InnerRings = it->InnerRings;

	if (InnerRings.size() > 1) {
		cout << "Error - number of holes larger than one" << endl;
		exit(0);
	}

	int nOuter = OuterRing.size();
	int nInner = InnerRings[0].size();
	int InnerRingNum = 0;

	int StartOuter = -1;
	int TargetOuter = -1;

	int StartInner = -1;
	int TargetInner = -1;

	float dmin = 0;
	for (int i = 0; i < nOuter; i++) {
		float3 p1 = OuterRing[i];
		for (int j = 0; j < nInner; j++) {
			float3 p2 = InnerRings[InnerRingNum][j];
			float d = p1.Distance(p2);
			if (d == 0)continue;
			d = 1 / d;
			if (d > dmin) {
				dmin = d;
				StartOuter = i;
				TargetInner = j;
			}
		}
	}

	if (StartOuter < 0 || TargetInner < 0){
		cout << "Error! No points found" << endl;
		exit(0);
	}

	dmin = 0;
	float3 p1 = OuterRing[StartOuter];
	for (int i = 0; i < nOuter; i++) {
		if (i == StartOuter)continue;
		float3 p2 = OuterRing[i];
		float d = p1.Distance(p2);
		if (d == 0)continue;
		if (d > dmin) {
			dmin = d;
			TargetOuter = i;
		}
	}

	dmin = 0;
	p1 = OuterRing[TargetOuter];

	for (int i = 0; i < nInner; i++) {
		if (i == TargetInner)continue;
		float3 p2 = InnerRings[InnerRingNum][i];
		float d = p1.Distance(p2);
		if (d == 0)continue;
		d = 1 / d;
		if (d > dmin) {
			dmin = d;
			StartInner = i;
		}
	}

	Polygon P0, P1;

	if (StartOuter < TargetOuter) {
		for (int i = StartOuter; i <= TargetOuter; i++)
			P0.p.push_back(OuterRing[i%nOuter]);
		for (int i = TargetOuter; i <= StartOuter + nOuter; i++)
			P1.p.push_back(OuterRing[i%nOuter]);
	}
	else {
		for (int i = StartOuter; i <= TargetOuter + nOuter; i++)
			P0.p.push_back(OuterRing[i%nOuter]);
		for (int i = TargetOuter; i <= StartOuter; i++)
			P1.p.push_back(OuterRing[i%nOuter]);
	}

	if (StartInner < TargetInner) {
		for (int i = StartInner; i <= TargetInner; i++)
			P0.p.push_back(InnerRings[InnerRingNum][i%nInner]);
		for (int i = TargetInner; i <= StartInner+nInner; i++)
			P1.p.push_back(InnerRings[InnerRingNum][i%nInner]);
	}
	else {
		for (int i = StartInner; i <= TargetInner+nInner; i++)
			P0.p.push_back(InnerRings[InnerRingNum][i%nInner]);
		for (int i = TargetInner; i <= StartInner; i++)
			P1.p.push_back(InnerRings[InnerRingNum][i%nInner]);
	}

	InnerRings.clear();
	OuterRing = P0.p;

	TPolygon PP;
	PP.OuterRing = P1.p;

	Polygons.push_back(PP);

}

void FracturePolygon::Dissolve() {
	if (OutMesh.NumFaces() < 2)return;
	list<BPolygon> Polygons;

	for (auto p : OutMesh.Faces()) {
		BPolygon P;

		for (int i = 0; i < p.NumVertices()+1; i++) {
			auto v = InPolygon.MapTo2D(p.Vertex(i%p.NumVertices()));
			bg::append(P.outer(), BPoint(v.x, v.y));
		}

		Polygons.push_back(P);
	}

	for (auto it = Polygons.begin(); it != Polygons.end(); it++) {
		for (auto it2 = Polygons.begin(); it2 != it;) {
			vector<BPolygon> PPs;
			bg::union_(*it, *it2, PPs);
			auto it3 = it2++;
			if (PPs.size() != 1)continue;
			*it = PPs[0];
			Polygons.erase(it3);
		}
	}

	//for now we have a good polygon with hole. I need to find:
	//	1. Way to erase additional points from outer edges
	//	2. method to convert complex polygons (with inner rings) into non-weak simple polygons
	//	(without inner rings, but also without self-touching edges)

	TPolys Polys;

	for (BPolygon p : Polygons) {
		TRing OuterPoints;
		for (BPoint pp : p.outer()) {
			double x = get<0>(pp);
			double y = get<1>(pp);
			float3 Point = InPolygon.MapFrom2D(float2(x, y));
			OuterPoints.push_back(Point);
		}
		OuterPoints.pop_back();
		TRings InnerRings;
		for (auto Inner : p.inners()) {
			TRing InnerPoints;
			for (BPoint pp : Inner) {
				double x = get<0>(pp);
				double y = get<1>(pp);
				float3 Point = InPolygon.MapFrom2D(float2(x, y));
				InnerPoints.push_back(Point);
			}
			InnerPoints.pop_back();
			InnerRings.push_back(InnerPoints);
		}
		TPolygon Poly;
		Poly.OuterRing = OuterPoints;
		Poly.InnerRings = InnerRings;
		Polys.push_back(Poly);
	}

	GetMeshFromRings(Polys);

}

void FracturePolygon::GetMeshFromRings(TPolys Polygons) {
	OutMesh = Polyhedron();
	for (auto &Poly : Polygons) CleanRings(Poly);

	for (auto it = Polygons.begin(); it != Polygons.end(); it++) {
		DividePolygon(Polygons, it); //dividing as long as InnerRings size > 0
		Polygon OutPolygon;
		for (auto v : it->OuterRing) OutPolygon.p.push_back(v);
		AddPolygonToMesh(OutPolygon, OutMesh);
	}

}

void FracturePolygon::CleanRings(TPolygon &Poly) {

	TRing &OuterPoints = Poly.OuterRing;
	TRings &InnerRings = Poly.InnerRings;

	TRing NewOuter;
	for (int i = 0; i < OuterPoints.size(); i++) {
		float3 Point = OuterPoints[i];
		bool flag = false;
		for (float3 p : InPolygon.p) {
			float d = p.Distance(Point);
			if (d < 1e-4) {
				NewOuter.push_back(Point);
				flag = true;
				break;
			}
		}
		if (flag)continue; //point is exactly the same as one of InPolygon points

		for (Polygon &P : FracturingPolygons) {
			float d = P.Distance(Point);
			if (d < 1e-4) {
				NewOuter.push_back(Point);
				break;
			}
		}
	}

	OuterPoints = NewOuter;
}


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_CoherentScatteringDistribution.cpp
//! \author Luke Kersting
//! \brief  The coherent photon scattering distribution definition.
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <limits>

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// FRENSIE Includes
#include "MonteCarlo_CoherentScatteringDistribution.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_ContractException.hpp"

namespace MonteCarlo{

// Constructor
CoherentScatteringDistribution::CoherentScatteringDistribution(
		    const Teuchos::RCP<const Utility::TabularOneDDistribution>&
		    form_factor_function_squared )
  : PhotonScatteringDistribution(),
    d_form_factor_function_squared( form_factor_function_squared )
{
  // Make sure the array is valid
  testPrecondition( !form_factor_function_squared.is_null() );
}

// Evaluate the distribution
/*! The cross section (b) differential in the scattering angle cosine is
 * returned from this function.
 */
double CoherentScatteringDistribution::evaluate(
			           const double incoming_energy,
			           const double scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  // Make sure the scattering angle cosine is valid
  testPrecondition( scattering_angle_cosine >= -1.0 );
  testPrecondition( scattering_angle_cosine <= 1.0 );

  const double mult = Utility::PhysicalConstants::pi*
    Utility::PhysicalConstants::classical_electron_radius*
    Utility::PhysicalConstants::classical_electron_radius;

  const double form_factor_squared =
    this->evaluateFormFactorSquared( incoming_energy, scattering_angle_cosine);

  return mult*1e24*(1.0 + scattering_angle_cosine*scattering_angle_cosine)*
    form_factor_squared;
}

// Evaluate the PDF
double CoherentScatteringDistribution::evaluatePDF(
				   const double incoming_energy,
				   const double scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  // Make sure the scattering angle cosine is valid
  testPrecondition( scattering_angle_cosine >= -1.0 );
  testPrecondition( scattering_angle_cosine <= 1.0 );

  return this->evaluate( incoming_energy, scattering_angle_cosine )/
    this->evaluateIntegratedCrossSection( incoming_energy, 1e-3 );
}

// Evaluate the integrated cross section (b)
double CoherentScatteringDistribution::evaluateIntegratedCrossSection(
					         const double incoming_energy,
					         const double precision ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );

  // Evaluate the integrated cross section
  boost::function<double (double x)> diff_cs_wrapper =
    boost::bind<double>( &CoherentScatteringDistribution::evaluate,
			 boost::cref( *this ),
			 incoming_energy,
			 _1 );

  double abs_error, integrated_cs;

  Utility::GaussKronrodIntegrator quadrature_gkq_set( precision );

  quadrature_gkq_set.integrateAdaptively<15>( diff_cs_wrapper,
					     -1.0,
					     1.0,
					     integrated_cs,
					     abs_error );

  // Make sure the integrated cross section is valid
  testPostcondition( integrated_cs > 0.0 );

  return integrated_cs;
}

// Sample an outgoing energy and direction from the distribution
void CoherentScatteringDistribution::sample(
				     const double incoming_energy,
				     double& outgoing_energy,
				     double& scattering_angle_cosine ) const
{
  // The outgoing energy is always equal to the incoming energy
  outgoing_energy = incoming_energy;

  unsigned trial_dummy;

  // Sample an outgoing direction
  this->sampleAndRecordTrialsImpl( incoming_energy,
				   scattering_angle_cosine,
				   trial_dummy );
}

// Sample an outgoing energy and direction and record the number of trials
void CoherentScatteringDistribution::sampleAndRecordTrials(
					    const double incoming_energy,
					    double& outgoing_energy,
					    double& scattering_angle_cosine,
					    unsigned& trials ) const
{
  // The outgoing energy is always equal to the incoming energy
  outgoing_energy = incoming_energy;

  // Sample an outgoing direction
  this->sampleAndRecordTrialsImpl( incoming_energy,
				   scattering_angle_cosine,
				   trials );
}

// Randomly scatter the photon
void CoherentScatteringDistribution::scatterPhoton(
				     PhotonState& photon,
				     ParticleBank& bank,
				     SubshellType& shell_of_interaction ) const
{
  double scattering_angle_cosine;

  unsigned trial_dummy;

  // Sample an outgoing direction
  this->sampleAndRecordTrialsImpl( photon.getEnergy(),
				   scattering_angle_cosine,
				   trial_dummy );

  shell_of_interaction = UNKNOWN_SUBSHELL;

  // Set the new direction
  photon.rotateDirection( scattering_angle_cosine,
			  this->sampleAzimuthalAngle() );
}

// Randomly scatter the adjoint photon
void CoherentScatteringDistribution::scatterAdjointPhoton(
				     AdjointPhotonState& adjoint_photon,
				     ParticleBank& bank,
				     SubshellType& shell_of_interaction ) const
{
  double scattering_angle_cosine;

  unsigned trial_dummy;

  // Sample an outgoing direction
  this->sampleAndRecordTrialsImpl( adjoint_photon.getEnergy(),
				   scattering_angle_cosine,
				   trial_dummy );

  shell_of_interaction = UNKNOWN_SUBSHELL;

  // Set the new direction
  adjoint_photon.rotateDirection( scattering_angle_cosine,
				  this->sampleAzimuthalAngle() );
}

// Evaluate the form factor squared
double CoherentScatteringDistribution::evaluateFormFactorSquared(
				  const double incoming_energy,
				  const double scattering_angle_cosine ) const
{
  // The inverse wavelength of the photon (1/cm)
  const double inverse_wavelength = incoming_energy/
    (Utility::PhysicalConstants::planck_constant*
     Utility::PhysicalConstants::speed_of_light);

  // The squared form factor argument
  const double form_factor_arg_squared = ((1.0 - scattering_angle_cosine)/2.0)*
    inverse_wavelength*inverse_wavelength;

  // Make sure the squared form factor argument is valid
  testPostcondition( form_factor_arg_squared >= 0.0 );

  return d_form_factor_function_squared->evaluate( form_factor_arg_squared );
}

// Basic sampling implementation
void CoherentScatteringDistribution::sampleAndRecordTrialsBasicImpl(
					    const double incoming_energy,
					    double& scattering_angle_cosine,
					    unsigned& trials ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );

  // Increment the number of trials
  ++trials;

  // Use the probability mixing technique to sample an outgoing angle
  double random_number_1 =
    Utility::RandomNumberGenerator::getRandomNumber<double>();
  double random_number_2 =
    Utility::RandomNumberGenerator::getRandomNumber<double>();

  if( random_number_1 <= 0.75 )
    scattering_angle_cosine = 2*random_number_2 - 1.0;
  else
  {
    scattering_angle_cosine = pow( fabs(2*random_number_2 - 1.0), 1.0/3.0 );

    if( random_number_2 < 0.5 )
      scattering_angle_cosine *= -1.0;
  }

  // Check for roundoff error
  if( fabs( scattering_angle_cosine ) > 1.0 )
    scattering_angle_cosine = copysign( 1.0, scattering_angle_cosine );

  // Make sure the scattering angle cosine is valid
  testPostcondition( scattering_angle_cosine >= -1.0 );
  testPostcondition( scattering_angle_cosine <= 1.0 );
}

// Return the form factor function squared distribution
const Teuchos::RCP<const Utility::TabularOneDDistribution>&
CoherentScatteringDistribution::getFormFactorSquaredDistribution() const
{
  return d_form_factor_function_squared;
}

} // end MonteCarlo namespace

//---------------------------------------------------------------------------//
// end MonteCarlo_IncoherentScatteringDistribution.cpp
//---------------------------------------------------------------------------//