#ifndef STAN_MATH_REV_MAT_FUN_MULTIPLY_HPP
#define STAN_MATH_REV_MAT_FUN_MULTIPLY_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/typedefs.hpp>
#include <stan/math/rev/scal/fun/value_of_rec.hpp>
#include <stan/math/rev/scal/fun/value_of.hpp>
#include <stan/math/rev/mat/fun/to_var.hpp>
#include <stan/math/rev/core.hpp>
#include <stan/math/prim/mat/fun/value_of_rec.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <boost/math/tools/promotion.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>

namespace stan {
  namespace math {

    /**
     * This is a subclass of the vari class for matrix
     * multiplication A * B where A is N by M and B
     * is M by K.
     *
     * The class stores the structure of each matrix,
     * the double values of A and B, and pointers to
     * the varis for A and B if A or B is a var. It
     * also instantiates and stores pointers to
     * varis for all elements of A * B.
     *
     * @tparam TA Scalar type for matrix A
     * @tparam RA Rows for matrix A
     * @tparam CA Columns for matrix A, Rows for matrix B
     * @tparam TB Scalar type for matrix B
     * @tparam CB Columns for matrix B
     */
    template <typename TA, int RA, int CA, typename TB, int CB>
    class multiply_mat_vari : public vari {
    public:
      int A_rows_, A_cols_, B_cols_, A_size_, B_size_;
      double* Ad_;
      double* Bd_;
      vari** variRefA_;
      vari** variRefB_;
      vari** variRefAB_;

      /**
       * Constructor for multiply_mat_vari.
       *
       * All memory allocated in
       * ChainableStack's stack_alloc arena.
       *
       * It is critical for the efficiency of this object
       * that the constructor create new varis that aren't
       * popped onto the var_stack_, but rather are
       * popped onto the var_nochain_stack_. This is
       * controlled to the second argument to
       * vari's constructor.
       *
       * @param A matrix
       * @param B matrix
       */
      multiply_mat_vari(const Eigen::Matrix<TA, RA, CA>& A,
                        const Eigen::Matrix<TB, CA, CB>& B)
        : vari(0.0),
          A_rows_(A.rows()), A_cols_(A.cols()),
          B_cols_(B.cols()), A_size_(A.size()), B_size_(B.size()),
          Ad_(ChainableStack::memalloc_.alloc_array<double>(A_size_)),
          Bd_(ChainableStack::memalloc_.alloc_array<double>(B_size_)),
          variRefA_(ChainableStack::memalloc_.alloc_array<vari*>(A_size_)),
          variRefB_(ChainableStack::memalloc_.alloc_array<vari*>(B_size_)),
          variRefAB_(ChainableStack::memalloc_.alloc_array<vari*>(A_rows_
                                                                  * B_cols_)) {
        using Eigen::Map;
        using Eigen::MatrixXd;
        for (size_type i = 0; i < A.size(); ++i) {
          variRefA_[i] = A.coeffRef(i).vi_;
          Ad_[i] = A.coeffRef(i).val();
        }
        for (size_type i = 0; i < B.size(); ++i) {
          variRefB_[i] = B.coeffRef(i).vi_;
          Bd_[i] = B.coeffRef(i).val();
        }
        MatrixXd AB
          = Map<MatrixXd>(Ad_, A_rows_, A_cols_)
          * Map<MatrixXd>(Bd_, A_cols_, B_cols_);
        for (size_type i = 0; i < AB.size(); ++i)
          variRefAB_[i] = new vari(AB.coeffRef(i), false);
      }

      virtual void chain() {
        using Eigen::MatrixXd;
        using Eigen::Map;
        MatrixXd adjAB(A_rows_, B_cols_);
        MatrixXd adjA(A_rows_, A_cols_);
        MatrixXd adjB(A_cols_, B_cols_);

        for (size_type i = 0; i < adjAB.size(); ++i)
          adjAB(i) = variRefAB_[i]->adj_;
        adjA = adjAB
          * Map<MatrixXd>(Bd_, A_cols_, B_cols_).transpose();
        adjB = Map<MatrixXd>(Ad_, A_rows_, A_cols_).transpose()
          * adjAB;
        for (size_type i = 0; i < A_size_; ++i)
          variRefA_[i]->adj_ += adjA(i);
        for (size_type i = 0; i < B_size_; ++i)
          variRefB_[i]->adj_ += adjB(i);
      }
    };

    /**
     * This is a subclass of the vari class for matrix
     * multiplication A * B where A is 1 by M and B
     * is M by 1.
     *
     * The class stores the structure of each matrix,
     * the double values of A and B, and pointers to
     * the varis for A and B if A or B is a var. It
     * also instantiates and stores pointers to
     * varis for all elements of A * B.
     *
     * @tparam TA Scalar type for matrix A
     * @tparam CA Columns for matrix A, Rows for matrix B
     * @tparam TB Scalar type for matrix B
     */
    template <typename TA, int CA, typename TB>
    class multiply_mat_vari<TA, 1, CA, TB, 1> : public vari {
    public:
      int size_;
      double* Ad_;
      double* Bd_;
      vari** variRefA_;
      vari** variRefB_;
      vari* variRefAB_;

      /**
       * Constructor for multiply_mat_vari.
       *
       * All memory allocated in
       * ChainableStack's stack_alloc arena.
       *
       * It is critical for the efficiency of this object
       * that the constructor create new varis that aren't
       * popped onto the var_stack_, but rather are
       * popped onto the var_nochain_stack_. This is
       * controlled to the second argument to
       * vari's constructor.
       *
       * @param A row vector
       * @param B vector
       */
      multiply_mat_vari(const Eigen::Matrix<TA, 1, CA>& A,
                        const Eigen::Matrix<TB, CA, 1>& B)
        : vari(0.0),
          size_(A.cols()),
          Ad_(ChainableStack::memalloc_.alloc_array<double>(size_)),
          Bd_(ChainableStack::memalloc_.alloc_array<double>(size_)),
          variRefA_(ChainableStack::memalloc_.alloc_array<vari*>(size_)),
          variRefB_(ChainableStack::memalloc_.alloc_array<vari*>(size_)) {
        using Eigen::Map;
        using Eigen::VectorXd;
        using Eigen::RowVectorXd;
        for (size_type i = 0; i < size_; ++i) {
          variRefA_[i] = A.coeffRef(i).vi_;
          Ad_[i] = A.coeffRef(i).val();
        }
        for (size_type i = 0; i < size_; ++i) {
          variRefB_[i] = B.coeffRef(i).vi_;
          Bd_[i] = B.coeffRef(i).val();
        }
        double AB = Map<RowVectorXd>(Ad_, 1, size_)
          * Map<VectorXd>(Bd_, size_, 1);
        variRefAB_ = new vari(AB, false);
      }

      virtual void chain() {
        using Eigen::VectorXd;
        using Eigen::RowVectorXd;
        using Eigen::Map;
        double adjAB;
        RowVectorXd adjA(size_);
        VectorXd adjB(size_);

        adjAB = variRefAB_->adj_;
        adjA = adjAB
          * Map<VectorXd>(Bd_, size_, 1).transpose();
        adjB = Map<RowVectorXd>(Ad_, 1, size_).transpose()
          * adjAB;
        for (size_type i = 0; i < size_; ++i)
          variRefA_[i]->adj_ += adjA(i);
        for (size_type i = 0; i < size_; ++i)
          variRefB_[i]->adj_ += adjB(i);
      }
    };

    /**
     * This is a subclass of the vari class for matrix
     * multiplication A * B where A is an N by M
     * matrix of double and B is M by K.
     *
     * The class stores the structure of each matrix,
     * the double values of A and B, and pointers to
     * the varis for A and B if A or B is a var. It
     * also instantiates and stores pointers to
     * varis for all elements of A * B.
     *
     * @tparam RA Rows for matrix A
     * @tparam CA Columns for matrix A, Rows for matrix B
     * @tparam TB Scalar type for matrix B
     * @tparam CB Columns for matrix B
     */
    template <int RA, int CA, typename TB, int CB>
    class multiply_mat_vari<double, RA, CA, TB, CB> : public vari {
    public:
      int A_rows_, A_cols_, B_cols_, A_size_, B_size_;
      double* Ad_;
      double* Bd_;
      vari** variRefB_;
      vari** variRefAB_;

      /**
       * Constructor for multiply_mat_vari.
       *
       * All memory allocated in
       * ChainableStack's stack_alloc arena.
       *
       * It is critical for the efficiency of this object
       * that the constructor create new varis that aren't
       * popped onto the var_stack_, but rather are
       * popped onto the var_nochain_stack_. This is
       * controlled to the second argument to
       * vari's constructor.
       *
       * @param A row vector
       * @param B vector
       */
      multiply_mat_vari(const Eigen::Matrix<double, RA, CA>& A,
                        const Eigen::Matrix<TB, CA, CB>& B)
        : vari(0.0),
          A_rows_(A.rows()), A_cols_(A.cols()),
          B_cols_(B.cols()), A_size_(A.size()), B_size_(B.size()),
          Ad_(ChainableStack::memalloc_.alloc_array<double>(A_size_)),
          Bd_(ChainableStack::memalloc_.alloc_array<double>(B_size_)),
          variRefB_(ChainableStack::memalloc_.alloc_array<vari*>(B_size_)),
          variRefAB_(ChainableStack::memalloc_.alloc_array<vari*>(A_rows_
                                                                  * B_cols_)) {
        using Eigen::MatrixXd;
        using Eigen::Map;
        for (size_type i = 0; i < A.size(); ++i)
          Ad_[i] = A.coeffRef(i);
        for (size_type i = 0; i < B.size(); ++i) {
          variRefB_[i] = B.coeffRef(i).vi_;
          Bd_[i] = B.coeffRef(i).val();
        }
        MatrixXd AB
          = Map<MatrixXd>(Ad_, A_rows_, A_cols_)
          * Map<MatrixXd>(Bd_, A_cols_, B_cols_);
        for (size_type i = 0; i < AB.size(); ++i)
          variRefAB_[i] = new vari(AB.coeffRef(i), false);
      }

      virtual void chain() {
        using Eigen::MatrixXd;
        using Eigen::Map;
        MatrixXd adjAB(A_rows_, B_cols_);
        MatrixXd adjB(A_cols_, B_cols_);

        for (size_type i = 0; i < adjAB.size(); ++i)
          adjAB(i) = variRefAB_[i]->adj_;
        adjB = Map<MatrixXd>(Ad_, A_rows_, A_cols_).transpose()
          * adjAB;
        for (size_type i = 0; i < B_size_; ++i)
          variRefB_[i]->adj_ += adjB(i);
      }
    };

    /**
     * This is a subclass of the vari class for matrix
     * multiplication A * B where A is a double
     * row vector of length M and B is a vector of
     * length M.
     *
     * The class stores the structure of each matrix,
     * the double values of A and B, and pointers to
     * the varis for A and B if A or B is a var. It
     * also instantiates and stores pointers to
     * varis for all elements of A * B.
     *
     * @tparam CA Columns for matrix A, Rows for matrix B
     * @tparam TB Scalar type for matrix B
     */
    template <int CA, typename TB>
    class multiply_mat_vari<double, 1, CA, TB, 1> : public vari {
    public:
      int size_;
      double* Ad_;
      double* Bd_;
      vari** variRefB_;
      vari* variRefAB_;

      /**
       * Constructor for multiply_mat_vari.
       *
       * All memory allocated in
       * ChainableStack's stack_alloc arena.
       *
       * It is critical for the efficiency of this object
       * that the constructor create new varis that aren't
       * popped onto the var_stack_, but rather are
       * popped onto the var_nochain_stack_. This is
       * controlled to the second argument to
       * vari's constructor.
       *
       * @param A row vector
       * @param B vector
       */
      multiply_mat_vari(const Eigen::Matrix<double, 1, CA>& A,
                        const Eigen::Matrix<TB, CA, 1>& B)
        : vari(0.0),
          size_(A.cols()),
          Ad_(ChainableStack::memalloc_.alloc_array<double>(size_)),
          Bd_(ChainableStack::memalloc_.alloc_array<double>(size_)),
          variRefB_(ChainableStack::memalloc_.alloc_array<vari*>(size_)) {
        using Eigen::Map;
        using Eigen::VectorXd;
        using Eigen::RowVectorXd;
        for (size_type i = 0; i < size_; ++i)
          Ad_[i] = A.coeffRef(i);
        for (size_type i = 0; i < size_; ++i) {
          variRefB_[i] = B.coeffRef(i).vi_;
          Bd_[i] = B.coeffRef(i).val();
        }
        double AB
          = Eigen::Map<RowVectorXd>(Ad_, 1, size_)
          * Eigen::Map<VectorXd>(Bd_, size_, 1);
        variRefAB_ = new vari(AB, false);
      }

      virtual void chain() {
        using Eigen::RowVectorXd;
        using Eigen::VectorXd;
        using Eigen::Map;
        double adjAB;
        VectorXd adjB(size_);

        adjAB = variRefAB_->adj_;
        adjB = Map<RowVectorXd>(Ad_, 1, size_).transpose()
          * adjAB;
        for (size_type i = 0; i < size_; ++i)
          variRefB_[i]->adj_ += adjB(i);
      }
    };

    /**
     * This is a subclass of the vari class for matrix
     * multiplication A * B where A is N by M and B
     * is an M by K matrix of doubles.
     *
     * The class stores the structure of each matrix,
     * the double values of A and B, and pointers to
     * the varis for A and B if A or B is a var. It
     * also instantiates and stores pointers to
     * varis for all elements of A * B.
     *
     * @tparam TA Scalar type for matrix A
     * @tparam RA Rows for matrix A
     * @tparam CA Columns for matrix A, Rows for matrix B
     * @tparam CB Columns for matrix B
     */
    template <typename TA, int RA, int CA, int CB>
    class multiply_mat_vari<TA, RA, CA, double, CB> : public vari {
    public:
      int A_rows_, A_cols_, B_cols_, A_size_, B_size_;
      double* Ad_;
      double* Bd_;
      vari** variRefA_;
      vari** variRefAB_;

      /**
       * Constructor for multiply_mat_vari.
       *
       * All memory allocated in
       * ChainableStack's stack_alloc arena.
       *
       * It is critical for the efficiency of this object
       * that the constructor create new varis that aren't
       * popped onto the var_stack_, but rather are
       * popped onto the var_nochain_stack_. This is
       * controlled to the second argument to
       * vari's constructor.
       *
       * @param A row vector
       * @param B vector
       */
      multiply_mat_vari(const Eigen::Matrix<TA, RA, CA>& A,
                        const Eigen::Matrix<double, CA, CB>& B)
        : vari(0.0),
          A_rows_(A.rows()), A_cols_(A.cols()),
          B_cols_(B.cols()), A_size_(A.size()), B_size_(B.size()),
          Ad_(ChainableStack::memalloc_.alloc_array<double>(A_size_)),
          Bd_(ChainableStack::memalloc_.alloc_array<double>(B_size_)),
          variRefA_(ChainableStack::memalloc_.alloc_array<vari*>(A_size_)),
          variRefAB_(ChainableStack::memalloc_.alloc_array<vari*>(A_rows_
                                                                  * B_cols_)) {
        using Eigen::Map;
        using Eigen::MatrixXd;
        for (size_type i = 0; i < A_size_; ++i) {
          variRefA_[i] = A.coeffRef(i).vi_;
          Ad_[i] = A.coeffRef(i).val();
        }
        for (size_type i = 0; i < B_size_; ++i) {
          Bd_[i] = B.coeffRef(i);
        }
        MatrixXd AB
          = Map<MatrixXd>(Ad_, A_rows_, A_cols_)
          * Map<MatrixXd>(Bd_, A_cols_, B_cols_);
        for (size_type i = 0; i < AB.size(); ++i)
          variRefAB_[i] = new vari(AB.coeffRef(i), false);
      }

      virtual void chain() {
        using Eigen::MatrixXd;
        using Eigen::Map;
        MatrixXd adjAB(A_rows_, B_cols_);
        MatrixXd adjA(A_rows_, A_cols_);

        for (size_type i = 0; i < adjAB.size(); ++i)
          adjAB(i) = variRefAB_[i]->adj_;
        adjA = adjAB * Map<MatrixXd>(Bd_, A_cols_, B_cols_).transpose();
        for (size_type i = 0; i < A_size_; ++i)
          variRefA_[i]->adj_ += adjA(i);
      }
    };

    /**
     * This is a subclass of the vari class for matrix
     * multiplication A * B where A is a row
     * vector of length M and B is a vector of length M
     * of doubles.
     *
     * The class stores the structure of each matrix,
     * the double values of A and B, and pointers to
     * the varis for A and B if A or B is a var. It
     * also instantiates and stores pointers to
     * varis for all elements of A * B.
     *
     * @tparam TA Scalar type for matrix A
     * @tparam RA Rows for matrix A
     * @tparam CA Columns for matrix A, Rows for matrix B
     * @tparam TB Scalar type for matrix B
     * @tparam CB Columns for matrix B
     */
    template <typename TA, int CA>
    class multiply_mat_vari<TA, 1, CA, double, 1> : public vari {
    public:
      int size_;
      double* Ad_;
      double* Bd_;
      vari** variRefA_;
      vari* variRefAB_;

      /**
       * Constructor for multiply_mat_vari.
       *
       * All memory allocated in
       * ChainableStack's stack_alloc arena.
       *
       * It is critical for the efficiency of this object
       * that the constructor create new varis that aren't
       * popped onto the var_stack_, but rather are
       * popped onto the var_nochain_stack_. This is
       * controlled to the second argument to
       * vari's constructor.
       *
       * @param A row vector
       * @param B vector
       */
      multiply_mat_vari(const Eigen::Matrix<TA, 1, CA>& A,
                        const Eigen::Matrix<double, CA, 1>& B)
        : vari(0.0),
          size_(A.cols()),
          Ad_(ChainableStack::memalloc_.alloc_array<double>(size_)),
          Bd_(ChainableStack::memalloc_.alloc_array<double>(size_)),
          variRefA_(ChainableStack::memalloc_.alloc_array<vari*>(size_)) {
        using Eigen::Map;
        using Eigen::VectorXd;
        using Eigen::RowVectorXd;
        for (size_type i = 0; i < size_; ++i) {
          variRefA_[i] = A.coeffRef(i).vi_;
          Ad_[i] = A.coeffRef(i).val();
        }
        for (size_type i = 0; i < size_; ++i)
          Bd_[i] = B.coeffRef(i);
        double AB
          = Map<RowVectorXd>(Ad_, 1, size_)
          * Map<VectorXd>(Bd_, size_, 1);
        variRefAB_ = new vari(AB, false);
      }

      virtual void chain() {
        using Eigen::Map;
        using Eigen::VectorXd;
        using Eigen::RowVectorXd;
        double adjAB;
        RowVectorXd adjA(size_);

        adjAB = variRefAB_->adj_;
        adjA = adjAB
          * Map<VectorXd>(Bd_, size_, 1).transpose();
        for (size_type i = 0; i < size_; ++i)
          variRefA_[i]->adj_ += adjA(i);
      }
    };

    /**
     * Return the product of two scalars.
     * @tparam T1 scalar type of v
     * @tparam T2 scalar type of c
     * @param[in] v First scalar
     * @param[in] c Specified scalar
     * @return Product of scalars
     */
    template <typename T1, typename T2>
    inline typename
    boost::enable_if_c<
      (boost::is_scalar<T1>::value || boost::is_same<T1, var>::value)
      && (boost::is_scalar<T2>::value || boost::is_same<T2, var>::value),
      typename boost::math::tools::promote_args<T1, T2>::type>::type
    multiply(const T1& v, const T2& c) {
      return v * c;
    }

    /**
     * Return the product of scalar and matrix.
     * @tparam T1 scalar type v
     * @tparam T2 scalar type matrix m
     * @tparam R2 Rows matrix m
     * @tparam C2 Columns matrix m
     * @param[in] c Specified scalar
     * @param[in] m Matrix
     * @return Product of scalar and matrix
     */
    template<typename T1, typename T2, int R2, int C2>
    inline Eigen::Matrix<var, R2, C2>
    multiply(const T1& c, const Eigen::Matrix<T2, R2, C2>& m) {
      // TODO(trangucci) pull out to eliminate overpromotion of one side
      // move to matrix.hpp w. promotion?
      return to_var(m) * to_var(c);
    }

    /**
     * Return the product of scalar and matrix.
     * @tparam T1 scalar type matrix m
     * @tparam T2 scalar type v
     * @tparam R1 Rows matrix m
     * @tparam C1 Columns matrix m
     * @param[in] c Specified scalar
     * @param[in] m Matrix
     * @return Product of scalar and matrix
     */
    template<typename T1, int R1, int C1, typename T2>
    inline Eigen::Matrix<var, R1, C1>
    multiply(const Eigen::Matrix<T1, R1, C1>& m, const T2& c) {
      // TODO(trangucci) pull out to eliminate overpromotion of one side
      // move to matrix.hpp w. promotion?
      return to_var(m) * to_var(c);
    }

    /**
     * Return the product of two matrices.
     * @tparam TA scalar type matrix A
     * @tparam RA Rows matrix A
     * @tparam CA Columns matrix A
     * @tparam TB scalar type matrix B
     * @tparam RB Rows matrix B
     * @tparam CB Columns matrix B
     * @param[in] A Matrix
     * @param[in] B Matrix
     * @return Product of scalar and matrix.
     */
    template <typename TA, int RA, int CA, typename TB, int CB>
    inline typename
    boost::enable_if_c<boost::is_same<TA, var>::value
                       || boost::is_same<TB, var>::value,
                       Eigen::Matrix<var, RA, CB> >::type
    multiply(const Eigen::Matrix<TA, RA, CA> &A,
             const Eigen::Matrix<TB, CA, CB> &B) {
      check_multiplicable("multiply", "A", A, "B", B);
      check_not_nan("multiply", "A", A);
      check_not_nan("multiply", "B", B);

      // Memory managed with the arena allocator.
      multiply_mat_vari<TA, RA, CA, TB, CB> *baseVari
        = new multiply_mat_vari<TA, RA, CA, TB, CB>(A, B);
      Eigen::Matrix<var, RA, CB> AB_v(A.rows(), B.cols());
      for (size_type i = 0; i < AB_v.size(); ++i) {
        AB_v.coeffRef(i).vi_ = baseVari->variRefAB_[i];
      }
      return AB_v;
    }

    /**
     * Return the scalar product of a row vector and
     * a vector.
     * @tparam TA scalar type row vector A
     * @tparam CA Columns matrix A
     * @tparam TB scalar type vector B
     * @param[in] A Row vector
     * @param[in] B Column vector
     * @return Scalar product of row vector and vector
     */
    template <typename TA, int CA, typename TB>
    inline typename
    boost::enable_if_c<boost::is_same<TA, var>::value
                       || boost::is_same<TB, var>::value, var>::type
    multiply(const Eigen::Matrix<TA, 1, CA> &A,
             const Eigen::Matrix<TB, CA, 1> &B) {
      check_multiplicable("multiply", "A", A, "B", B);
      check_not_nan("multiply", "A", A);
      check_not_nan("multiply", "B", B);

      // Memory managed with the arena allocator.
      multiply_mat_vari<TA, 1, CA, TB, 1> *baseVari
        = new multiply_mat_vari<TA, 1, CA, TB, 1>(A, B);
      var AB_v;
      AB_v.vi_ = baseVari->variRefAB_;
      return AB_v;
    }
  }
}
#endif


#include <cmath>
#include <algorithm>

#include "boost/geometry/geometry.hpp"
#include <boost/container_hash/hash.hpp>
#include <memory>

#include "lsst/pex/exceptions.h"
#include "lsst/geom/Extent.h"
#include "lsst/afw/geom/polygon/Polygon.h"

#include "lsst/afw/table/io/OutputArchive.h"
#include "lsst/afw/table/io/InputArchive.h"
#include "lsst/afw/table/io/CatalogVector.h"
#include "lsst/afw/table/aggregates.h"
#include "lsst/afw/table/io/Persistable.hpp"

typedef lsst::afw::geom::polygon::Polygon::Point LsstPoint;
typedef lsst::afw::geom::polygon::Polygon::Box LsstBox;
typedef std::vector<LsstPoint> LsstRing;
typedef boost::geometry::model::polygon<LsstPoint> BoostPolygon;
typedef boost::geometry::model::box<LsstPoint> BoostBox;
typedef boost::geometry::model::linestring<LsstPoint> BoostLineString;

namespace boost {
namespace geometry {
namespace traits {

// Setting up LsstPoint
template <>
struct tag<LsstPoint> {
    typedef point_tag type;
};
template <>
struct coordinate_type<LsstPoint> {
    typedef LsstPoint::Element type;
};
template <>
struct coordinate_system<LsstPoint> {
    typedef cs::cartesian type;
};
template <>
struct dimension<LsstPoint> : boost::mpl::int_<2> {};
template <std::size_t dim>
struct access<LsstPoint, dim> {
    static double get(LsstPoint const& p) { return p[dim]; }
    static void set(LsstPoint& p, LsstPoint::Element const& value) { p[dim] = value; }
};

// Setting up LsstBox
//
// No setters, because it's inefficient (can't set individual elements of lsst::geom::Box2D directly).
// For box outputs from boost::geometry we'll use BoostBox and then convert.
template <>
struct tag<LsstBox> {
    typedef box_tag type;
};
template <>
struct point_type<LsstBox> {
    typedef LsstPoint type;
};
template <>
struct indexed_access<LsstBox, 0, 0> {
    static double get(LsstBox const& box) { return box.getMinX(); }
};
template <>
struct indexed_access<LsstBox, 1, 0> {
    static double get(LsstBox const& box) { return box.getMaxX(); }
};
template <>
struct indexed_access<LsstBox, 0, 1> {
    static double get(LsstBox const& box) { return box.getMinY(); }
};
template <>
struct indexed_access<LsstBox, 1, 1> {
    static double get(LsstBox const& box) { return box.getMaxY(); }
};

// Setting up LsstRing
template <>
struct tag<LsstRing> {
    typedef ring_tag type;
};
// template<> struct range_value<LsstRing> { typedef LsstPoint type; };
}  // namespace traits
}  // namespace geometry
}  // namespace boost

namespace {

/// @internal Convert BoostBox to LsstBox
LsstBox boostBoxToLsst(BoostBox const& box) { return LsstBox(box.min_corner(), box.max_corner()); }

/// @internal Convert box to corners
std::vector<LsstPoint> boxToCorners(LsstBox const& box) {
    std::vector<LsstPoint> corners;
    corners.reserve(4);
    corners.push_back(box.getMin());
    corners.push_back(LsstPoint(box.getMaxX(), box.getMinY()));
    corners.push_back(box.getMax());
    corners.push_back(LsstPoint(box.getMinX(), box.getMaxY()));
    return corners;
}

/**
 * @internal Sub-sample a line
 *
 * Add `num` points to `vector` between `first` and `second`
 */
void addSubSampledEdge(std::vector<LsstPoint>& vertices,  // Vector of points to which to add
                       LsstPoint const& first,            // First vertex defining edge
                       LsstPoint const& second,           // Second vertex defining edge
                       size_t const num                   // Number of parts to divide edge into
) {
    lsst::geom::Extent2D const delta = (second - first) / num;
    vertices.push_back(first);
    for (size_t i = 1; i < num; ++i) {
        vertices.push_back(first + delta * i);
    }
}

/// @internal Calculate area of overlap between polygon and pixel
double pixelOverlap(BoostPolygon const& poly, int const x, int const y) {
    std::vector<BoostPolygon> overlap;  // Overlap between pixel and polygon
    LsstBox const pixel(lsst::geom::Point2D(x - 0.5, y - 0.5), lsst::geom::Point2D(x + 0.5, y + 0.5));
    boost::geometry::intersection(poly, pixel, overlap);
    double area = 0.0;
    for (std::vector<BoostPolygon>::const_iterator i = overlap.begin(); i != overlap.end(); ++i) {
        double const polyArea = boost::geometry::area(*i);
        area += std::min(polyArea, 1.0);  // remove any rounding error
    }
    return area;
}

/// @internal Set each pixel in a row to the amount of overlap with polygon
void pixelRowOverlap(std::shared_ptr<lsst::afw::image::Image<float>> const image, BoostPolygon const& poly,
                     int const xStart, int const xStop, int const y) {
    int x = xStart;
    for (lsst::afw::image::Image<float>::x_iterator i = image->x_at(x - image->getX0(), y - image->getY0());
         x <= xStop; ++i, ++x) {
        *i = pixelOverlap(poly, x, y);
    }
}

}  // anonymous namespace

namespace lsst {
namespace afw {

template std::shared_ptr<geom::polygon::Polygon> table::io::PersistableFacade<
        geom::polygon::Polygon>::dynamicCast(std::shared_ptr<table::io::Persistable> const&);

namespace geom {
namespace polygon {

/// @internal Stream vertices
std::ostream& operator<<(std::ostream& os, std::vector<LsstPoint> const& vertices) {
    os << "[";
    size_t num = vertices.size();
    for (size_t i = 0; i < num - 1; ++i) {
        os << vertices[i] << ",";
    }
    os << vertices[vertices.size() - 1] << "]";
    return os;
}

/// @internal Stream BoostPolygon
std::ostream& operator<<(std::ostream& os, BoostPolygon const& poly) {
    return os << "BoostPolygon(" << poly.outer() << ")";
}

std::ostream& operator<<(std::ostream& os, Polygon const& poly) {
    os << poly.toString();
    return os;
}

struct Polygon::Impl {
    Impl() : poly() {}
    explicit Impl(Polygon::Box const& box) : poly() {
        boost::geometry::assign(poly, box);
        // Assignment from a box is correctly handled by BoostPolygon, so doesn't need a "check()"
    }
    explicit Impl(std::vector<LsstPoint> const& vertices) : poly() {
        boost::geometry::assign(poly, vertices);
        check();  // because the vertices might not have the correct orientation (CW vs CCW) or be open
    }
    explicit Impl(BoostPolygon const& _poly) : poly(_poly) {}

    void check() { boost::geometry::correct(poly); }

    /// @internal Convert collection of Boost polygons to our own
    static std::vector<std::shared_ptr<Polygon>> convertBoostPolygons(
            std::vector<BoostPolygon> const& boostPolygons);

    template <class PolyT>
    bool overlaps(PolyT const& other) const {
        return !boost::geometry::disjoint(poly, other);
    }

    template <class PolyT>
    std::shared_ptr<Polygon> intersectionSingle(PolyT const& other) const;

    template <class PolyT>
    std::vector<std::shared_ptr<Polygon>> intersection(PolyT const& other) const;

    template <class PolyT>
    std::shared_ptr<Polygon> unionSingle(PolyT const& other) const;

    template <class PolyT>
    std::vector<std::shared_ptr<Polygon>> union_(PolyT const& other) const;

    template <class PolyT>
    std::vector<std::shared_ptr<Polygon>> symDifference(PolyT const& other) const;

    BoostPolygon poly;
};

std::vector<std::shared_ptr<Polygon>> Polygon::Impl::convertBoostPolygons(
        std::vector<BoostPolygon> const& boostPolygons) {
    std::vector<std::shared_ptr<Polygon>> lsstPolygons;
    lsstPolygons.reserve(boostPolygons.size());
    for (std::vector<BoostPolygon>::const_iterator i = boostPolygons.begin(); i != boostPolygons.end(); ++i) {
        std::shared_ptr<Polygon> tmp(new Polygon(std::shared_ptr<Polygon::Impl>(new Polygon::Impl(*i))));
        lsstPolygons.push_back(tmp);
    }
    return lsstPolygons;
}

template <class PolyT>
std::shared_ptr<Polygon> Polygon::Impl::intersectionSingle(PolyT const& other) const {
    std::vector<BoostPolygon> result;
    boost::geometry::intersection(poly, other, result);
    if (result.size() == 0) {
        throw LSST_EXCEPT(SinglePolygonException, "Polygons have no intersection");
    }
    if (result.size() > 1) {
        throw LSST_EXCEPT(
                SinglePolygonException,
                (boost::format("Multiple polygons (%d) created by intersection()") % result.size()).str());
    }
    return std::shared_ptr<Polygon>(new Polygon(std::shared_ptr<Impl>(new Impl(result[0]))));
}

template <class PolyT>
std::vector<std::shared_ptr<Polygon>> Polygon::Impl::intersection(PolyT const& other) const {
    std::vector<BoostPolygon> boostResult;
    boost::geometry::intersection(poly, other, boostResult);
    return convertBoostPolygons(boostResult);
}

template <class PolyT>
std::shared_ptr<Polygon> Polygon::Impl::unionSingle(PolyT const& other) const {
    std::vector<BoostPolygon> result;
    boost::geometry::union_(poly, other, result);
    if (result.size() != 1) {
        throw LSST_EXCEPT(
                SinglePolygonException,
                (boost::format("Multiple polygons (%d) created by union_()") % result.size()).str());
    }
    return std::shared_ptr<Polygon>(new Polygon(std::shared_ptr<Impl>(new Impl(result[0]))));
}

template <class PolyT>
std::vector<std::shared_ptr<Polygon>> Polygon::Impl::union_(PolyT const& other) const {
    std::vector<BoostPolygon> boostResult;
    boost::geometry::union_(poly, other, boostResult);
    return convertBoostPolygons(boostResult);
}

template <class PolyT>
std::vector<std::shared_ptr<Polygon>> Polygon::Impl::symDifference(PolyT const& other) const {
    std::vector<BoostPolygon> boostResult;
    boost::geometry::sym_difference(poly, other, boostResult);
    return convertBoostPolygons(boostResult);
}

Polygon::Polygon(Polygon const&) = default;
Polygon::Polygon(Polygon&&) = default;
Polygon& Polygon::operator=(Polygon const&) = default;
Polygon& Polygon::operator=(Polygon&&) = default;

Polygon::~Polygon() = default;

Polygon::Polygon(Polygon::Box const& box) : _impl(new Polygon::Impl(box)) {}

Polygon::Polygon(std::vector<LsstPoint> const& vertices) : _impl(new Polygon::Impl(vertices)) {}

Polygon::Polygon(Polygon::Box const& box, afw::geom::TransformPoint2ToPoint2 const& transform)
        : _impl(new Polygon::Impl()) {
    auto corners = transform.applyForward(boxToCorners(box));
    boost::geometry::assign(_impl->poly, corners);
    _impl->check();
}

Polygon::Polygon(Polygon::Box const& box, lsst::geom::AffineTransform const& transform)
        : _impl(new Polygon::Impl()) {
    std::vector<LsstPoint> corners = boxToCorners(box);
    for (std::vector<LsstPoint>::iterator p = corners.begin(); p != corners.end(); ++p) {
        *p = transform(*p);
    }
    boost::geometry::assign(_impl->poly, corners);
    _impl->check();
}

size_t Polygon::getNumEdges() const {
    // boost::geometry::models::polygon uses a "closed" polygon: the start/end point is included twice
    return boost::geometry::num_points(_impl->poly) - 1;
}

Polygon::Box Polygon::getBBox() const {
    return boostBoxToLsst(boost::geometry::return_envelope<BoostBox>(_impl->poly));
}

LsstPoint Polygon::calculateCenter() const {
    return boost::geometry::return_centroid<LsstPoint>(_impl->poly);
}

double Polygon::calculateArea() const { return boost::geometry::area(_impl->poly); }

double Polygon::calculatePerimeter() const { return boost::geometry::perimeter(_impl->poly); }

std::vector<std::pair<LsstPoint, LsstPoint>> Polygon::getEdges() const {
    std::vector<LsstPoint> const vertices = getVertices();
    std::vector<std::pair<LsstPoint, LsstPoint>> edges;
    edges.reserve(getNumEdges());
    for (std::vector<LsstPoint>::const_iterator i = vertices.begin(), j = vertices.begin() + 1;
         j != vertices.end(); ++i, ++j) {
        edges.push_back(std::make_pair(*i, *j));
    }
    return edges;
}

std::vector<LsstPoint> Polygon::getVertices() const { return _impl->poly.outer(); }

std::vector<LsstPoint>::const_iterator Polygon::begin() const { return _impl->poly.outer().begin(); }

std::vector<LsstPoint>::const_iterator Polygon::end() const {
    return _impl->poly.outer().end() - 1;  // Note removal of final "closed" point
}

bool Polygon::operator==(Polygon const& other) const {
    return boost::geometry::equals(_impl->poly, other._impl->poly);
}

std::size_t Polygon::hash_value() const noexcept {
    // boost::hash allows hash functions to throw, but the container hashes throw
    // only if the element [geom::Point] has a throwing hash
    static boost::hash<BoostPolygon::ring_type> polygonHash;
    return polygonHash(_impl->poly.outer());
}

bool Polygon::contains(LsstPoint const& point) const { return boost::geometry::within(point, _impl->poly); }

bool Polygon::overlaps(Polygon const& other) const { return _impl->overlaps(other._impl->poly); }

bool Polygon::overlaps(Box const& box) const { return _impl->overlaps(box); }

std::shared_ptr<Polygon> Polygon::intersectionSingle(Polygon const& other) const {
    return _impl->intersectionSingle(other._impl->poly);
}

std::shared_ptr<Polygon> Polygon::intersectionSingle(Box const& box) const {
    return _impl->intersectionSingle(box);
}

std::vector<std::shared_ptr<Polygon>> Polygon::intersection(Polygon const& other) const {
    return _impl->intersection(other._impl->poly);
}

std::vector<std::shared_ptr<Polygon>> Polygon::intersection(Box const& box) const {
    return _impl->intersection(box);
}

std::shared_ptr<Polygon> Polygon::unionSingle(Polygon const& other) const {
    return _impl->unionSingle(other._impl->poly);
}

std::shared_ptr<Polygon> Polygon::unionSingle(Box const& box) const { return _impl->unionSingle(box); }

std::vector<std::shared_ptr<Polygon>> Polygon::union_(Polygon const& other) const {
    return _impl->union_(other._impl->poly);
}

std::vector<std::shared_ptr<Polygon>> Polygon::union_(Box const& box) const { return _impl->union_(box); }

std::vector<std::shared_ptr<Polygon>> Polygon::symDifference(Polygon const& other) const {
    return _impl->symDifference(other._impl->poly);
}

std::vector<std::shared_ptr<Polygon>> Polygon::symDifference(Box const& box) const {
    return _impl->symDifference(box);
}

std::shared_ptr<Polygon> Polygon::simplify(double const distance) const {
    BoostPolygon result;
    boost::geometry::simplify(_impl->poly, result, distance);
    return std::shared_ptr<Polygon>(new Polygon(std::shared_ptr<Impl>(new Impl(result))));
}

std::shared_ptr<Polygon> Polygon::convexHull() const {
    BoostPolygon hull;
    boost::geometry::convex_hull(_impl->poly, hull);
    return std::shared_ptr<Polygon>(new Polygon(std::shared_ptr<Impl>(new Impl(hull))));
}

std::shared_ptr<Polygon> Polygon::transform(TransformPoint2ToPoint2 const& transform) const {
    auto newVertices = transform.applyForward(getVertices());
    return std::shared_ptr<Polygon>(new Polygon(std::shared_ptr<Impl>(new Impl(newVertices))));
}

std::shared_ptr<Polygon> Polygon::transform(lsst::geom::AffineTransform const& transform) const {
    std::vector<LsstPoint> vertices;  // New vertices
    vertices.reserve(getNumEdges());
    for (std::vector<LsstPoint>::const_iterator i = _impl->poly.outer().begin();
         i != _impl->poly.outer().end(); ++i) {
        vertices.push_back(transform(*i));
    }
    return std::shared_ptr<Polygon>(new Polygon(std::shared_ptr<Impl>(new Impl(vertices))));
}

std::shared_ptr<Polygon> Polygon::subSample(size_t num) const {
    std::vector<LsstPoint> vertices;  // New vertices
    vertices.reserve(getNumEdges() * num);
    std::vector<std::pair<Point, Point>> edges = getEdges();
    for (std::vector<std::pair<Point, Point>>::const_iterator i = edges.begin(); i != edges.end(); ++i) {
        addSubSampledEdge(vertices, i->first, i->second, num);
    }
    return std::shared_ptr<Polygon>(new Polygon(std::shared_ptr<Impl>(new Impl(vertices))));
}

std::shared_ptr<Polygon> Polygon::subSample(double maxLength) const {
    std::vector<LsstPoint> vertices;  // New vertices
    vertices.reserve(getNumEdges() + static_cast<size_t>(::ceil(calculatePerimeter() / maxLength)));
    std::vector<std::pair<Point, Point>> edges = getEdges();
    for (std::vector<std::pair<Point, Point>>::const_iterator i = edges.begin(); i != edges.end(); ++i) {
        Point const &p1 = i->first, p2 = i->second;
        double const dist = ::sqrt(p1.distanceSquared(p2));
        addSubSampledEdge(vertices, p1, p2, static_cast<size_t>(::ceil(dist / maxLength)));
    }
    return std::shared_ptr<Polygon>(new Polygon(std::shared_ptr<Impl>(new Impl(vertices))));
}

std::shared_ptr<afw::image::Image<float>> Polygon::createImage(lsst::geom::Box2I const& bbox) const {
    typedef afw::image::Image<float> Image;
    std::shared_ptr<Image> image = std::make_shared<Image>(bbox);
    image->setXY0(bbox.getMin());
    *image = 0.0;
    lsst::geom::Box2D bounds = getBBox();  // Polygon bounds
    int xMin = std::max(static_cast<int>(bounds.getMinX()), bbox.getMinX());
    int xMax = std::min(static_cast<int>(::ceil(bounds.getMaxX())), bbox.getMaxX());
    int yMin = std::max(static_cast<int>(bounds.getMinY()), bbox.getMinY());
    int yMax = std::min(static_cast<int>(::ceil(bounds.getMaxY())), bbox.getMaxY());
    for (int y = yMin; y <= yMax; ++y) {
        double const yPixelMin = (double)y - 0.5, yPixelMax = (double)y + 0.5;
        BoostPolygon row;  // A polygon of row y
        boost::geometry::assign(
                row, LsstBox(lsst::geom::Point2D(xMin, yPixelMin), lsst::geom::Point2D(xMax, yPixelMax)));
        std::vector<BoostPolygon> intersections;
        boost::geometry::intersection(_impl->poly, row, intersections);

        if (intersections.size() == 1 && boost::geometry::num_points(intersections[0]) == 5) {
            // This row is fairly tame, and should have a long run of pixels within the polygon
            BoostPolygon const& row = intersections[0];
            std::vector<double> top, bottom;
            top.reserve(2);
            bottom.reserve(2);
            bool failed = false;
            for (std::vector<Point>::const_iterator i = row.outer().begin(); i != row.outer().end() - 1;
                 ++i) {
                double const xCoord = i->getX(), yCoord = i->getY();
                if (yCoord == yPixelMin) {
                    bottom.push_back(xCoord);
                } else if (yCoord == yPixelMax) {
                    top.push_back(xCoord);
                } else {
                    failed = true;
                    break;
                }
            }
            if (!failed && top.size() == 2 && bottom.size() == 2) {
                std::sort(top.begin(), top.end());
                std::sort(bottom.begin(), bottom.end());
                int const xMin = std::min(top[0], bottom[0]);
                int const xStart = ::ceil(std::max(top[0], bottom[0])) + 1;
                int const xStop = std::min(top[1], bottom[1]) - 1;
                int const xMax = ::ceil(std::max(top[1], bottom[1]));
                pixelRowOverlap(image, _impl->poly, std::max(xMin, bbox.getMinX()),
                                std::min(xStart, bbox.getMaxX()), y);
                int x = xStart;
                for (Image::x_iterator i = image->x_at(std::max(xStart, bbox.getMinX()) - image->getX0(),
                                                       y - image->getY0());
                     x <= std::min(xStop, bbox.getMaxX()); ++i, ++x) {
                    *i = 1.0;
                }
                pixelRowOverlap(image, _impl->poly, std::max(xStop, bbox.getMinX()),
                                std::min(xMax, bbox.getMaxX()), y);
                continue;
            }
        }

        // Last resort: do each pixel independently...
        for (std::vector<BoostPolygon>::const_iterator p = intersections.begin(); p != intersections.end();
             ++p) {
            double xMinRow = xMax, xMaxRow = xMin;
            std::vector<LsstPoint> const vertices = p->outer();
            for (std::vector<LsstPoint>::const_iterator q = vertices.begin(); q != vertices.end(); ++q) {
                double const x = q->getX();
                if (x < xMinRow) xMinRow = x;
                if (x > xMaxRow) xMaxRow = x;
            }

            pixelRowOverlap(image, _impl->poly, std::max(static_cast<int>(xMinRow), bbox.getMinX()),
                            std::min(static_cast<int>(::ceil(xMaxRow)), bbox.getMaxX()), y);
        }
    }
    return image;
}

// -------------- Table-based Persistence -------------------------------------------------------------------

/*
 *
 */
namespace {

struct PolygonSchema {
    afw::table::Schema schema;
    afw::table::PointKey<double> vertices;

    static PolygonSchema const& get() {
        static PolygonSchema instance;
        return instance;
    }

    // No copying
    PolygonSchema(const PolygonSchema&) = delete;
    PolygonSchema& operator=(const PolygonSchema&) = delete;

    // No moving
    PolygonSchema(PolygonSchema&&) = delete;
    PolygonSchema& operator=(PolygonSchema&&) = delete;

private:
    PolygonSchema()
            : schema(),
              vertices(afw::table::PointKey<double>::addFields(schema, "vertices", "list of vertex points",
                                                               "")) {}
};

class PolygonFactory : public table::io::PersistableFactory {
public:
    explicit PolygonFactory(std::string const& name) : table::io::PersistableFactory(name) {}

    std::shared_ptr<table::io::Persistable> read(InputArchive const& archive,
                                                 CatalogVector const& catalogs) const override {
        static PolygonSchema const& keys = PolygonSchema::get();

        LSST_ARCHIVE_ASSERT(catalogs.size() == 1u);
        afw::table::BaseCatalog const& cat = catalogs.front();

        std::vector<LsstPoint> vertices;
        for (afw::table::BaseCatalog::const_iterator iter = cat.begin(); iter != cat.end(); ++iter) {
            vertices.push_back(iter->get(keys.vertices));
        }
        std::shared_ptr<Polygon> result(new Polygon(vertices));
        return result;
    }
};

std::string getPolygonPersistenceName() { return "Polygon"; }

PolygonFactory registration(getPolygonPersistenceName());

}  // anonymous namespace

std::string Polygon::getPersistenceName() const { return getPolygonPersistenceName(); }

void Polygon::write(OutputArchiveHandle& handle) const {
    static PolygonSchema const& keys = PolygonSchema::get();
    afw::table::BaseCatalog catalog = handle.makeCatalog(keys.schema);

    std::vector<LsstPoint> vertices = this->getVertices();
    for (std::vector<LsstPoint>::const_iterator i = vertices.begin(); i != vertices.end(); ++i) {
        std::shared_ptr<afw::table::BaseRecord> record = catalog.addNew();
        record->set(keys.vertices, *i);
    }

    handle.saveCatalog(catalog);
}

std::shared_ptr<typehandling::Storable> Polygon::cloneStorable() const {
    return std::make_unique<Polygon>(*this);
}

std::string Polygon::toString() const {
    std::stringstream buffer;
    buffer << "Polygon(" << this->getVertices() << ")";
    return buffer.str();
}

bool Polygon::equals(typehandling::Storable const& other) const noexcept {
    return singleClassEquals(*this, other);
}

}  // namespace polygon
}  // namespace geom
}  // namespace afw
}  // namespace lsst


// Copyright (c) 2019-2020 The Curvehash developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <kernel.h>
#include <chainparams.h>
#include <util.h>
#include <validation.h>
#include <streams.h>
#include <timedata.h>
#include <bignum.h>
#include <txdb.h>
#include <consensus/validation.h>
#include <random.h>
#include <script/interpreter.h>

#include <boost/assign/list_of.hpp>

using namespace std;

// Stake Modifier (hash modifier of proof-of-stake):
// The purpose of stake modifier is to prevent a txout (coin) owner from
// computing future proof-of-stake generated by this txout at the time
// of transaction confirmation. To meet kernel protocol, the txout
// must hash with a future stake modifier to generate the proof.
uint256 ComputeStakeModifier(const CBlockIndex* pindexPrev, const uint256& kernel)
{
    if (!pindexPrev)
        return uint256();  // genesis block's modifier is 0

    CDataStream ss(SER_GETHASH, 0);
    ss << kernel << pindexPrev->bnStakeModifier;
    return Hash(ss.begin(), ss.end());
}

// Curvehash kernel protocol
// coinstake must meet hash target according to the protocol:
// kernel (input 0) must meet the formula
//     hash(nStakeModifier + txPrev.block.nTime + txPrev.nTime + txPrev.vout.hash + txPrev.vout.n + nTime) < bnTarget * nWeight
// this ensures that the chance of getting a coinstake is proportional to the
// amount of coins one owns.
// The reason this hash is chosen is the following:
//   nStakeModifier: scrambles computation to make it very difficult to precompute
//                   future proof-of-stake
//   txPrev.block.nTime: prevent nodes from guessing a good timestamp to
//                       generate transaction for future advantage
//   txPrev.nTime: slightly scrambles computation
//   txPrev.vout.hash: hash of txPrev, to reduce the chance of nodes
//                     generating coinstake at the same time
//   txPrev.vout.n: output number of txPrev, to reduce the chance of nodes
//                  generating coinstake at the same time
//   nTime: current timestamp
//   block/tx hash should not be used here as they can be generated in vast
//   quantities so as to generate blocks faster, degrading the system back into
//   a proof-of-work situation.
//
bool CheckStakeKernelHash(unsigned int nBits, CBlockIndex* pindexPrev, const CBlockHeader& blockFrom, const CTransactionRef& txPrev, const COutPoint& prevout, unsigned int nTimeTx, uint256& hashProofOfStake, uint256& targetProofOfStake, bool fPrintProofOfStake)
{
    unsigned int nTimeBlockFrom = blockFrom.GetBlockTime();
    if (nTimeTx < txPrev->nTime)  // Transaction timestamp violation
        return error("CheckStakeKernelHash() : nTime violation");

    // Base target
    CBigNum bnTarget;
    bnTarget.SetCompact(nBits);

    // Weighted target
    int64_t nValueIn = txPrev->vout[prevout.n].nValue;
    CBigNum bnWeight = CBigNum(nValueIn);
    bnTarget *= bnWeight;

    targetProofOfStake = bnTarget.getuint256();

    uint256 bnStakeModifier = pindexPrev->bnStakeModifier;

    // Calculate hash
    CDataStream ss(SER_GETHASH, 0);
    ss << bnStakeModifier;
    ss << txPrev->nTime << prevout.hash << prevout.n << nTimeTx;
    hashProofOfStake = Hash(ss.begin(), ss.end());

    if (fPrintProofOfStake)
    {
        LogPrint(BCLog::ALL, "check modifier%s nTimeBlockFrom=%u nTimeTxPrev=%u nPrevout=%u nTimeTx=%u hashProof=%s\n",
                  bnStakeModifier.ToString(),
                  nTimeBlockFrom, txPrev->nTime, prevout.n, nTimeTx,
                  hashProofOfStake.ToString());
    }

    // Now check if proof-of-stake hash meets target protocol
    if (CBigNum(hashProofOfStake) > bnTarget) {
        return false;
    }

    if (!fPrintProofOfStake)
    {
        LogPrint(BCLog::ALL, "pass modifier=%s nTimeBlockFrom=%u nTimeTxPrev=%u nPrevout=%u nTimeTx=%u hashProof=%s\n",
                  bnStakeModifier.ToString(),
                  nTimeBlockFrom, txPrev->nTime, prevout.n, nTimeTx,
                  hashProofOfStake.ToString());
    }

    return true;
}

// Check kernel hash target and coinstake signature
bool CheckProofOfStake(CValidationState &state, CBlockIndex* pindexPrev, const CTransactionRef& tx, unsigned int nBits, uint256& hashProofOfStake, uint256& targetProofOfStake)
{
    if (!tx->IsCoinStake())
        return error("CheckProofOfStake() : called on non-coinstake %s", tx->GetHash().ToString());

    // Kernel (input 0) must match the stake hash target per coin age (nBits)
    const CTxIn& txin = tx->vin[0];

    // Transaction index is required to get to block header
    if (!fTxIndex)
        return error("CheckProofOfStake() : transaction index not available");

    // Get transaction index for the previous transaction
    CDiskTxPos postx;
    if (!pblocktree->ReadTxIndex(txin.prevout.hash, postx))
        return error("CheckProofOfStake() : tx index not found");  // tx index not found

    // Read txPrev and header of its block
    CBlockHeader header;
    CTransactionRef txPrev;
    CBlock blockKernel; // block containing stake kernel, GetTransaction should only fill the header.
    if (!GetTransaction(txin.prevout.hash, txPrev, Params().GetConsensus(), blockKernel)) {
        LogPrintf("ERROR: %s: prevout-not-in-chain\n", __func__);
        return error("prevout-not-in-chain");
    }
    int nDepth;
    if (IsConfirmedInNPrevBlocks(header.GetHash(), pindexPrev, Params().GetConsensus().nStakeMinConfirmations - 1, nDepth)) {
        return error("CheckProofOfStake() : tried to stake at depth %d", nDepth + 1);
    }

    if (txPrev->GetHash() != txin.prevout.hash)
        return error("%s() : txid mismatch in CheckProofOfStake()", __PRETTY_FUNCTION__);

    // Verify signature
    {
        int nIn = 0;
        const CTxOut& prevOut = txPrev->vout[tx->vin[nIn].prevout.n];
        TransactionSignatureChecker checker(&(*tx), nIn, prevOut.nValue, PrecomputedTransactionData(*tx));

        if (!VerifyScript(tx->vin[nIn].scriptSig, prevOut.scriptPubKey, &(tx->vin[nIn].scriptWitness), SCRIPT_VERIFY_P2SH, checker, nullptr))
            return state.DoS(100, false, REJECT_INVALID, "invalid-pos-script", false, strprintf("%s: VerifyScript failed on coinstake %s", __func__, tx->GetHash().ToString()));
    }

    if (!CheckStakeKernelHash(nBits, pindexPrev, header, txPrev, txin.prevout, tx->nTime, hashProofOfStake, targetProofOfStake, gArgs.GetBoolArg("-debug", false)))
        return state.DoS(1, error("CheckProofOfStake() : INFO: check kernel failed on coinstake %s, hashProof=%s", tx->GetHash().ToString(), hashProofOfStake.ToString())); // may occur during initial download or if behind on block chain sync

    return true;
}

// Check whether the coinstake timestamp meets protocol
bool CheckCoinStakeTimestamp(int64_t nTimeBlock, int64_t nTimeTx)
{
    return (nTimeBlock == nTimeTx);
}

// Used only when staking, not during validation
bool CheckKernel(unsigned int nBits, CBlockIndex *pindexPrev, const CBlockHeader& header, const CTransactionRef& txPrev, const COutPoint &prevoutStake, unsigned int nTime)
{
    uint256 hashProofOfStake, targetProofOfStake;
    int nDepth;

    if (IsConfirmedInNPrevBlocks(header.GetHash(), pindexPrev, Params().GetConsensus().nStakeMinConfirmations - 1, nDepth)) {
        return false;
    }

    return CheckStakeKernelHash(nBits, pindexPrev, header, txPrev, prevoutStake, nTime, hashProofOfStake, targetProofOfStake);
}


#pragma once

#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/include/find.hpp>
#include <boost/mpl/vector.hpp>
#include <libv/lma/ttt/mpl/cat.hpp>
#include <libv/lma/lma.hpp>
#include "nodes.hpp"

namespace lma
{

  struct Constraints {};
  struct Parameters {};

  template<typename, typename> struct Graph;

  template<typename ... C, typename ... P>
  struct Graph<Constraints(C...),Parameters(P...)>
  {
    struct DataGraph : boost::fusion::vector<C...,P...>
    {
      template<class T> operator T& () { return *boost::fusion::find<T>(*this); }
      template<class T> operator const T& () { return *boost::fusion::find<T>(*this); }
      template<class T> operator const T& () const { return *boost::fusion::find<T>(*this); }
    };

    DataGraph graph;
    DataGraph& node() { return graph;}

    struct ExtractFunctors
    {
      template<typename T> struct GetFunctors : T::functors {};
      using folded = boost::mpl::fold<boost::mpl::vector<C...>,boost::mpl::vector<>,ttt::Cat<boost::mpl::_1,GetFunctors<boost::mpl::_2>>>;
      using type = typename boost::fusion::result_of::as_vector<typename folded::type>::type;
    };

    struct ExtractParameters
    {
      template<typename V, typename T> struct GetParameter : boost::mpl::push_back<V,typename T::type> {};
      using type = typename boost::mpl::fold<boost::mpl::vector<P...>,boost::mpl::vector<>,GetParameter<boost::mpl::_1,boost::mpl::_2>>::type;
    };

    template<typename ...> struct CreateSolver;
    template<template <typename ...> typename V, typename ... T, typename ... Optional> struct CreateSolver<V<T...>,Optional...>
      { using type = lma::Solver<Optional...,T...>; };
    using Solver = typename CreateSolver<typename ExtractFunctors::type, typename ExtractParameters::type>::type;

    void cover(auto& node)
    {
      //node.cover(*this);
      _cover(*this,node);
    }

    void solve(auto ... options)
    {
      boost::fusion::for_each(graph,[this](auto& node){node.covered=false;});
      Solver solver;
      boost::fusion::for_each(graph,[this](auto& node){_cover(*this,node);});
      boost::fusion::for_each(graph,[this,&solver](auto& node){fill_solver(node,solver);});
      solver.solve(options...);
      boost::fusion::for_each(graph,[this](auto& node){post_solve(*this,node);});
	  }
  };

}


//=============================================================================================================
/**
 * @file     filterkernel.cpp
 * @author   Lorenz Esch <lesch@mgh.harvard.edu>;
 *           Ruben Doerfel <Ruben.Doerfel@tu-ilmenau.de>;
 *           Christoph Dinh <chdinh@nmr.mgh.harvard.edu>
 * @since    0.1.0
 * @date     February, 2014
 *
 * @section  LICENSE
 *
 * Copyright (C) 2014, Lorenz Esch, Ruben Doerfel, Christoph Dinh. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 * the following conditions are met:
 *     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 *       following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
 *       the following disclaimer in the documentation and/or other materials provided with the distribution.
 *     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
 *       to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * @brief    Contains all FilterKernel.
 *
 */

//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "filterkernel.h"

#include <utils/mnemath.h>

#include "parksmcclellan.h"
#include "cosinefilter.h"

#include <iostream>

//=============================================================================================================
// QT INCLUDES
//=============================================================================================================

#include <QDebug>

//=============================================================================================================
// EIGEN INCLUDES
//=============================================================================================================

#include <Eigen/SparseCore>
//#ifndef EIGEN_FFTW_DEFAULT
//#define EIGEN_FFTW_DEFAULT
//#endif
#include <unsupported/Eigen/FFT>

//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace RTPROCESSINGLIB;
using namespace Eigen;
using namespace UTILSLIB;

//=============================================================================================================
// INIT STATIC MEMBERS
//=============================================================================================================

QVector<RTPROCESSINGLIB::FilterParameter> FilterKernel::m_designMethods ({
    FilterParameter(QString("Cosine"), QString("A cosine filter")),
    FilterParameter(QString("Tschebyscheff"), QString("A tschebyscheff filter"))
//    FilterParameter(QString("External"), QString("An external filter"))
});
QVector<RTPROCESSINGLIB::FilterParameter> FilterKernel::m_filterTypes ({
    FilterParameter(QString("LPF"), QString("An LPF filter")),
    FilterParameter(QString("HPF"), QString("An HPF filter")),
    FilterParameter(QString("BPF"), QString("A BPF filter")),
    FilterParameter(QString("NOTCH"), QString("A NOTCH filter")),
    FilterParameter(QString("UNKNOWN"), QString("An UNKNOWN filter"))
});

//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

FilterKernel::FilterKernel()
: m_iFilterType(m_filterTypes.indexOf(FilterParameter("BPF")))
, m_iFilterOrder(80)
, m_sFilterName("Unknown")
, m_dParksWidth(0.1)
, m_iDesignMethod(m_designMethods.indexOf(FilterParameter("Cosine")))
, m_dCenterFreq(0.5)
, m_dBandwidth(0.1)
, m_sFreq(1000)
, m_dLowpassFreq(40)
, m_dHighpassFreq(4)
{
    designFilter();
}

//=============================================================================================================

FilterKernel::FilterKernel(const QString& sFilterName,
                           int iFilterType,
                           int iOrder,
                           double dCenterfreq,
                           double dBandwidth,
                           double dParkswidth,
                           double dSFreq,
                           int iDesignMethod)
: m_iDesignMethod(iDesignMethod)
, m_iFilterType(iFilterType)
, m_sFreq(dSFreq)
, m_dCenterFreq(dCenterfreq)
, m_dBandwidth(dBandwidth)
, m_dParksWidth(dParkswidth)
, m_iFilterOrder(iOrder)
, m_sFilterName(sFilterName)
{
    if(iOrder < 9) {
       qWarning() << "[FilterKernel::FilterKernel] Less than 9 taps were provided. Setting number of taps to 9.";
    }

    designFilter();
}

//=============================================================================================================

void FilterKernel::prepareFilter(int iDataSize)
{
    int iFftLength, exp;

    iFftLength = iDataSize + m_vecCoeff.cols();
    exp = ceil(MNEMath::log2(iFftLength));
    iFftLength = pow(2, exp);

    // Transform coefficients anew if needed
    if(m_vecCoeff.cols() != (iFftLength/2+1)) {
        fftTransformCoeffs(iFftLength);
    }
}

//=============================================================================================================

RowVectorXd FilterKernel::applyConvFilter(const RowVectorXd& vecData,
                                          bool bKeepOverhead) const
{
    //Do zero padding or mirroring depending on user input
    RowVectorXd vecDataZeroPad = RowVectorXd::Zero(2*m_vecCoeff.cols() + vecData.cols());
    RowVectorXd vecFilteredTime = RowVectorXd::Zero(2*m_vecCoeff.cols() + vecData.cols());

    vecDataZeroPad.segment(m_vecCoeff.cols(), vecData.cols()) = vecData;

    //Do the convolution
    for(int i = m_vecCoeff.cols(); i < vecFilteredTime.cols(); i++) {
        vecFilteredTime(i-m_vecCoeff.cols()) = vecDataZeroPad.segment(i-m_vecCoeff.cols(),m_vecCoeff.cols()) * m_vecCoeff.transpose();
    }

    //Return filtered data
    if(!bKeepOverhead) {
        return vecFilteredTime.segment(m_vecCoeff.cols()/2, vecData.cols());
    }

    return vecFilteredTime.head(vecData.cols()+m_vecCoeff.cols());
}

//=============================================================================================================

void FilterKernel::applyFftFilter(RowVectorXd& vecData,
                                  bool bKeepOverhead)
{
    #ifdef EIGEN_FFTW_DEFAULT
    fftw_make_planner_thread_safe();
    #endif

    // Make sure we always have the correct FFT length for the given input data and filter overlap
    int iFftLength = vecData.cols() + m_vecCoeff.cols();
    int exp = ceil(MNEMath::log2(iFftLength));
    iFftLength = pow(2, exp);

    // Transform coefficients anew if needed
    if(m_vecFftCoeff.cols() != (iFftLength/2+1)) {
        fftTransformCoeffs(iFftLength);
    }

    //generate fft object
    Eigen::FFT<double> fft;
    fft.SetFlag(fft.HalfSpectrum);

    // Zero padd if necessary. Please note: The zero padding in Eigen's FFT is only working for column vectors -> We have to zero pad manually here
    int iOriginalSize = vecData.cols();
    if (vecData.cols() < iFftLength) {
        int iResidual = iFftLength - vecData.cols();
        vecData.conservativeResize(iFftLength);
        vecData.tail(iResidual).setZero();
    }

    //fft-transform data sequence
    RowVectorXcd vecFreqData;
    fft.fwd(vecFreqData, vecData, iFftLength);

    //perform frequency-domain filtering
    vecFreqData = m_vecFftCoeff.array() * vecFreqData.array();

    //inverse-FFT
    fft.inv(vecData, vecFreqData);

    //Return filtered data
    if(!bKeepOverhead) {
        vecData = vecData.segment(m_vecCoeff.cols()/2, iOriginalSize).eval();
    } else {
        vecData = vecData.head(iOriginalSize + m_vecCoeff.cols()).eval();
    }
}

//=============================================================================================================

QString FilterKernel::getName() const
{
    return m_sFilterName;
}

//=============================================================================================================

void FilterKernel::setName(const QString& sFilterName)
{
    m_sFilterName = sFilterName;
}

//=============================================================================================================

double FilterKernel::getSamplingFrequency() const
{
    return m_sFreq;
}

//=============================================================================================================

void FilterKernel::setSamplingFrequency(double dSFreq)
{
    m_sFreq = dSFreq;
}

//=============================================================================================================

int FilterKernel::getFilterOrder() const
{
    return m_iFilterOrder;
}

//=============================================================================================================

void FilterKernel::setFilterOrder(int iOrder)
{
    m_iFilterOrder = iOrder;
}

//=============================================================================================================

double FilterKernel::getCenterFrequency() const
{
    return m_dCenterFreq;
}

//=============================================================================================================

void FilterKernel::setCenterFrequency(double dCenterFreq)
{
    m_dCenterFreq = dCenterFreq;
}

//=============================================================================================================

double FilterKernel::getBandwidth() const
{
    return m_dBandwidth;
}

//=============================================================================================================

void FilterKernel::setBandwidth(double dBandwidth)
{
    m_dBandwidth = dBandwidth;
}

//=============================================================================================================

double FilterKernel::getParksWidth() const
{
    return m_dParksWidth;
}

//=============================================================================================================

void FilterKernel::setParksWidth(double dParksWidth)
{
    m_dParksWidth = dParksWidth;
}

//=============================================================================================================

double FilterKernel::getHighpassFreq() const
{
    return m_dHighpassFreq;
}

//=============================================================================================================

void FilterKernel::setHighpassFreq(double dHighpassFreq)
{
    m_dHighpassFreq = dHighpassFreq;
}

//=============================================================================================================

double FilterKernel::getLowpassFreq() const
{
    return m_dLowpassFreq;
}

//=============================================================================================================

void FilterKernel::setLowpassFreq(double dLowpassFreq)
{
    m_dLowpassFreq = dLowpassFreq;
}

//=============================================================================================================

Eigen::RowVectorXd FilterKernel::getCoefficients() const
{
    return m_vecCoeff;
}

//=============================================================================================================

void FilterKernel::setCoefficients(const Eigen::RowVectorXd& vecCoeff)
{
    m_vecCoeff = vecCoeff;
}

//=============================================================================================================

Eigen::RowVectorXcd FilterKernel::getFftCoefficients() const
{
    return m_vecFftCoeff;
}

//=============================================================================================================

void FilterKernel::setFftCoefficients(const Eigen::RowVectorXcd& vecFftCoeff)
{
    m_vecFftCoeff = vecFftCoeff;
}

//=============================================================================================================

bool FilterKernel::fftTransformCoeffs(int iFftLength)
{
    #ifdef EIGEN_FFTW_DEFAULT
        fftw_make_planner_thread_safe();
    #endif

    if(m_vecCoeff.cols() > iFftLength) {
        std::cout <<"[FilterKernel::fftTransformCoeffs] The number of filter taps is bigger than the FFT length."<< std::endl;
        return false;
    }

    //generate fft object
    Eigen::FFT<double> fft;
    fft.SetFlag(fft.HalfSpectrum);

    // Zero padd if necessary. Please note: The zero padding in Eigen's FFT is only working for column vectors -> We have to zero pad manually here
    RowVectorXd vecInputFft;
    if (m_vecCoeff.cols() < iFftLength) {
        vecInputFft.setZero(iFftLength);
        vecInputFft.block(0,0,1,m_vecCoeff.cols()) = m_vecCoeff;
    } else {
        vecInputFft = m_vecCoeff;
    }

    //fft-transform filter coeffs
    RowVectorXcd vecFreqData;
    fft.fwd(vecFreqData, vecInputFft, iFftLength);
    m_vecFftCoeff = vecFreqData;;

    return true;
}

//=============================================================================================================

void FilterKernel::designFilter()
{
    // Make sure we only use a minimum needed FFT size
    int iFftLength = m_iFilterOrder;
    int exp = ceil(MNEMath::log2(iFftLength));
    iFftLength = pow(2, exp);

    switch(m_iDesignMethod) {
        case 1: {
            ParksMcClellan filter(m_iFilterOrder,
                                  m_dCenterFreq,
                                  m_dBandwidth,
                                  m_dParksWidth,
                                  static_cast<ParksMcClellan::TPassType>(m_iFilterType));
            m_vecCoeff = filter.FirCoeff;

            //fft-transform m_vecCoeff in order to be able to perform frequency-domain filtering
            fftTransformCoeffs(iFftLength);

            break;
        }

        case 0: {
            CosineFilter filtercos;

            switch(m_iFilterType) {
                case 0:
                    filtercos = CosineFilter(iFftLength,
                                             (m_dCenterFreq)*(m_sFreq/2.),
                                             m_dParksWidth*(m_sFreq/2),
                                             (m_dCenterFreq)*(m_sFreq/2),
                                             m_dParksWidth*(m_sFreq/2),
                                             m_sFreq,
                                             static_cast<CosineFilter::TPassType>(m_iFilterType));

                    break;

                case 1:
                    filtercos = CosineFilter(iFftLength,
                                             (m_dCenterFreq)*(m_sFreq/2),
                                             m_dParksWidth*(m_sFreq/2),
                                             (m_dCenterFreq)*(m_sFreq/2),
                                             m_dParksWidth*(m_sFreq/2),
                                             m_sFreq,
                                             static_cast<CosineFilter::TPassType>(m_iFilterType));

                    break;

                case 2:
                    filtercos = CosineFilter(iFftLength,
                                             (m_dCenterFreq + m_dBandwidth/2)*(m_sFreq/2),
                                             m_dParksWidth*(m_sFreq/2),
                                             (m_dCenterFreq - m_dBandwidth/2)*(m_sFreq/2),
                                             m_dParksWidth*(m_sFreq/2),
                                             m_sFreq,
                                             static_cast<CosineFilter::TPassType>(m_iFilterType));

                    break;
            }

            //This filter is designed in the frequency domain, hence the time domain impulse response need to be shortend by the users dependent number of taps
            m_vecCoeff.resize(m_iFilterOrder);

            m_vecCoeff.head(m_iFilterOrder/2) = filtercos.m_vecCoeff.tail(m_iFilterOrder/2);
            m_vecCoeff.tail(m_iFilterOrder/2) = filtercos.m_vecCoeff.head(m_iFilterOrder/2);

            //Now generate the fft version of the shortened impulse response
            fftTransformCoeffs(iFftLength);

            break;
        }
    }

    switch(m_iFilterType) {
        case 0:
            m_dLowpassFreq = 0;
            m_dHighpassFreq = m_dCenterFreq*(m_sFreq/2);
        break;

        case 1:
            m_dLowpassFreq = m_dCenterFreq*(m_sFreq/2);
            m_dHighpassFreq = 0;
        break;

        case 2:
            m_dLowpassFreq = (m_dCenterFreq + m_dBandwidth/2)*(m_sFreq/2);
            m_dHighpassFreq = (m_dCenterFreq - m_dBandwidth/2)*(m_sFreq/2);
        break;
    }
}

//=============================================================================================================

RTPROCESSINGLIB::FilterParameter FilterKernel::getDesignMethod() const
{
    if(m_iDesignMethod < 0){
        return m_designMethods.at(0);
    }
    return m_designMethods.at(m_iDesignMethod);
}

//=============================================================================================================

RTPROCESSINGLIB::FilterParameter FilterKernel::getFilterType() const
{
    if(m_iFilterType < 0){
        return m_filterTypes.at(0);
    }
    return m_filterTypes.at(m_iFilterType);
}

//=============================================================================================================

void FilterKernel::setDesignMethod(int iDesignMethod)
{
    if(iDesignMethod < 0){
        m_iDesignMethod = 0;
    } else {
        m_iDesignMethod = iDesignMethod;
    }
}

//=============================================================================================================

void FilterKernel::setFilterType(int iFilterType)
{
    if(iFilterType < 0){
        m_iFilterType = 0;
    } else {
        m_iFilterType = iFilterType;
    }
}

//=============================================================================================================

FilterParameter::FilterParameter()
:FilterParameter("Unknown", "")
{
}

//=============================================================================================================

FilterParameter::FilterParameter(QString sName)
:FilterParameter(sName,"")
{
}

//=============================================================================================================

FilterParameter::FilterParameter(QString sName,
                           QString sDescription)
: m_sName(sName)
, m_sDescription(sDescription)
{

}

//=============================================================================================================

QString FilterParameter::getName() const
{
    return m_sName;
}


#include <sbpl_perception/config_parser.h>
#include <sbpl_perception/object_model.h>

#include <perception_utils/vfh/vfh_pose_estimator.h>
#include <perception_utils/perception_utils.h>

#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transformation_estimation_2D.h>
#include <pcl/registration/transformation_estimation_lm.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/registration/warp_point_rigid_3d.h>

#include <ros/package.h>
#include <ros/ros.h>

#include <boost/filesystem.hpp>

#include <algorithm>
#include <chrono>

using namespace perception_utils;
using namespace std;
using namespace sbpl_perception;

typedef vector<double> VD;
typedef vector<VD> VVD;
typedef vector<int> VI;

const string kDebugDir = ros::package::getPath("sbpl_perception") +
                         "/visualization/";

// From Stanform ACM's handbook.
///////////////////////////////////////////////////////////////////////////
// Min cost bipartite matching via shortest augmenting paths
//
// This is an O(n^3) implementation of a shortest augmenting path
// algorithm for finding min cost perfect matchings in dense
// graphs.  In practice, it solves 1000x1000 problems in around 1
// second.
//
//   cost[i][j] = cost for pairing left node i with right node j
//   Lmate[i] = index of right node that left node i pairs with
//   Rmate[j] = index of left node that right node j pairs with
//
// The values in cost[i][j] may be positive or negative.  To perform
// maximization, simply negate the cost[][] matrix.
///////////////////////////////////////////////////////////////////////////
double MinCostMatching(const VVD &cost, VI &Lmate, VI &Rmate) {
  int n = int(cost.size());

  // construct dual feasible solution
  VD u(n);
  VD v(n);

  for (int i = 0; i < n; i++) {
    u[i] = cost[i][0];

    for (int j = 1; j < n; j++) {
      u[i] = min(u[i], cost[i][j]);
    }
  }

  for (int j = 0; j < n; j++) {
    v[j] = cost[0][j] - u[0];

    for (int i = 1; i < n; i++) {
      v[j] = min(v[j], cost[i][j] - u[i]);
    }
  }

  // construct primal solution satisfying complementary slackness
  Lmate = VI(n, -1);
  Rmate = VI(n, -1);
  int mated = 0;

  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      if (Rmate[j] != -1) {
        continue;
      }

      if (fabs(cost[i][j] - u[i] - v[j]) < 1e-10) {
        Lmate[i] = j;
        Rmate[j] = i;
        mated++;
        break;
      }
    }
  }

  VD dist(n);
  VI dad(n);
  VI seen(n);

  // repeat until primal solution is feasible
  while (mated < n) {

    // find an unmatched left node
    int s = 0;

    while (Lmate[s] != -1) {
      s++;
    }

    // initialize Dijkstra
    fill(dad.begin(), dad.end(), -1);
    fill(seen.begin(), seen.end(), 0);

    for (int k = 0; k < n; k++) {
      dist[k] = cost[s][k] - u[s] - v[k];
    }

    int j = 0;

    while (true) {

      // find closest
      j = -1;

      for (int k = 0; k < n; k++) {
        if (seen[k]) {
          continue;
        }

        if (j == -1 || dist[k] < dist[j]) {
          j = k;
        }
      }

      seen[j] = 1;

      // termination condition
      if (Rmate[j] == -1) {
        break;
      }

      // relax neighbors
      const int i = Rmate[j];

      for (int k = 0; k < n; k++) {
        if (seen[k]) {
          continue;
        }

        const double new_dist = dist[j] + cost[i][k] - u[i] - v[k];

        if (dist[k] > new_dist) {
          dist[k] = new_dist;
          dad[k] = j;
        }
      }
    }

    // update dual variables
    for (int k = 0; k < n; k++) {
      if (k == j || !seen[k]) {
        continue;
      }

      const int i = Rmate[k];
      v[k] += dist[k] - dist[j];
      u[i] -= dist[k] - dist[j];
    }

    u[s] += dist[j];

    // augment along path
    while (dad[j] >= 0) {
      const int d = dad[j];
      Rmate[j] = Rmate[d];
      Lmate[Rmate[j]] = j;
      j = d;
    }

    Rmate[j] = s;
    Lmate[s] = j;

    mated++;
  }

  double value = 0;

  for (int i = 0; i < n; i++) {
    value += cost[i][Lmate[i]];
  }

  return value;
}

Eigen::Affine3f GetICPAdjustedPose(const PointCloudPtr cloud_in,
                                   const PointCloudPtr target_cloud,
                                   PointCloudPtr cloud_out) {

  pcl::IterativeClosestPointNonLinear<PointT, PointT> icp;

  icp.setInputSource(cloud_in);
  icp.setInputTarget(target_cloud);

  pcl::registration::TransformationEstimation2D<PointT, PointT>::Ptr est;
  est.reset(new pcl::registration::TransformationEstimation2D<PointT, PointT>);
  icp.setTransformationEstimation(est);

  // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
  // const double kEnvRes = 0.1;
  const double kCorrespondenceThresh = 0.1 / 2;
  icp.setMaxCorrespondenceDistance (kCorrespondenceThresh); //TODO: properly
  icp.setMaximumIterations (50);
  icp.setEuclideanFitnessEpsilon (1e-5);
  icp.align(*cloud_out);

  Eigen::Affine3f transformation = Eigen::Affine3f::Identity();

  if (icp.hasConverged()) {
    double score = icp.getFitnessScore();
    transformation.matrix() = icp.getFinalTransformation();
  } else {
    std::cerr << "ICP did not converge!" << std::endl;
  }

  return transformation;
}

int main(int argc, char **argv) {

  // if (argc < 2) {
  //   cerr << "Usage: ./vfh_estimator <path_to_config_file>"
  //        << endl;
  //   return -1;
  //
  // }
  // string config_file = argv[1];

  if (argc < 4) {
    cerr << "Usage: ./vfh_estimator <path_to_config_dir> <path_output_file_poses> <path_output_state_file>"
         << endl;
    return -1;
  }

  boost::filesystem::path config_dir = argv[1];
  boost::filesystem::path output_file = argv[2];
  boost::filesystem::path output_file_stats = argv[3];

  ros::init(argc, argv, "ourcvfh_estimator");
  ros::NodeHandle private_nh("~");
  XmlRpc::XmlRpcValue model_bank_list;

  vector<ModelMetaData> model_bank;
  std::string param_key;
  if (private_nh.searchParam("model_bank", param_key)) {
    private_nh.getParam(param_key, model_bank_list);
  }

  ROS_ASSERT(model_bank_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
  printf("Model bank has %d models:\n", model_bank_list.size());
  model_bank.resize(model_bank_list.size());
  for (int ii = 0; ii < model_bank_list.size(); ++ii) {
    auto &object_data = model_bank_list[ii];
    ROS_ASSERT(object_data.getType() == XmlRpc::XmlRpcValue::TypeArray);
    ROS_ASSERT(object_data.size() == 4);
    ROS_ASSERT(object_data[0].getType() == XmlRpc::XmlRpcValue::TypeString);
    ROS_ASSERT(object_data[1].getType() == XmlRpc::XmlRpcValue::TypeString);
    ROS_ASSERT(object_data[2].getType() == XmlRpc::XmlRpcValue::TypeBoolean);
    ROS_ASSERT(object_data[3].getType() == XmlRpc::XmlRpcValue::TypeBoolean);

    ModelMetaData model_meta_data;
    SetModelMetaData(static_cast<string>(object_data[0]),
                     static_cast<string>(object_data[1]), static_cast<bool>(object_data[2]),
                     static_cast<bool>(object_data[3]), &model_meta_data);
    model_bank[ii] = model_meta_data;
    printf("%s: %s, %d, %d\n", model_meta_data.name.c_str(),
           model_meta_data.file.c_str(), model_meta_data.flipped,
           model_meta_data.symmetric);

  }

  if (!boost::filesystem::is_directory(config_dir)) {
    cerr << "Invalid config directory" << endl;
    return -1;
  }

  ofstream fs, fs_stats;
  fs.open (output_file.string().c_str());

  if (!fs.is_open () || fs.fail ()) {
    return (false);
  }

  fs_stats.open (output_file_stats.string().c_str(),
                 std::ofstream::out | std::ofstream::app);
  if (!fs_stats.is_open () || fs_stats.fail ()) {
    return (false);
  }

  boost::filesystem::directory_iterator dir_itr(config_dir), dir_end;

  for (dir_itr; dir_itr != dir_end; ++dir_itr) {

    if (dir_itr->path().extension().native().compare(".txt") != 0) {
      continue;
    }

    boost::filesystem::path config_file_path = config_dir /
                                               dir_itr->path().filename();
    string config_file = dir_itr->path().string();
    cout << config_file << endl;

    ConfigParser parser;
    parser.Parse(config_file);

    pcl::PointCloud<PointT>::Ptr cloud_in(new PointCloud);

    // Read the input PCD file from disk.
    if (pcl::io::loadPCDFile<PointT>(parser.pcd_file_path.c_str(),
                                     *cloud_in) != 0) {
      cerr << "Could not find input PCD file!" << endl;
      return -1;
    }

    vector<ObjectModel> test_obj_models, train_obj_models;

    for (size_t ii = 0; ii < parser.model_files.size(); ++ii) {
      ModelMetaData meta_data = sbpl_perception::GetMetaDataFromModelFilename(model_bank, parser.model_files[ii]);
      pcl::PolygonMesh mesh;
      pcl::io::loadPolygonFile (parser.model_files[ii].c_str(), mesh);
      ObjectModel test_obj_model(mesh, meta_data.name,
                                 meta_data.symmetric,
                                 meta_data.flipped);
      ObjectModel train_obj_model(mesh, parser.model_files[ii].c_str(),
                                  false,
                                  false);
      test_obj_models.push_back(test_obj_model);
      train_obj_models.push_back(train_obj_model);
    }

    // Start measuring computation time.
	  chrono::time_point<chrono::system_clock> start, end;
    start = chrono::system_clock::now();

    VFHPoseEstimator pose_estimator;
    float roll, pitch, yaw;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_colorless (new
                                                         pcl::PointCloud<pcl::PointXYZ>);
    copyPointCloud(*cloud_in, *cloud_colorless);

    vector<PointCloudPtr> cluster_clouds;
    vector<pcl::PointIndices> cluster_indices;
    DoEuclideanClustering(cloud_in, &cluster_clouds, &cluster_indices, -1);
    size_t num_clusters = cluster_clouds.size();

    if (num_clusters < parser.model_files.size()) {
      fs << config_file << endl;
      for (size_t ii = 0; ii < parser.model_files.size(); ++ii) {
        fs << "-1" << " " << "-1" << " " << "-1" << " " << "-1" << endl;
        fs_stats << "-1" << endl;
      }
      continue;
    }

    // Take only the k largest clusters if we have more than the number of
    // models.
    if(num_clusters > parser.model_files.size()) {
      cluster_clouds.resize(parser.model_files.size());
      num_clusters = cluster_clouds.size();
    }

    vector<string> models;

    for (size_t ii = 0; ii < parser.model_files.size(); ++ii) {
      const string &file = parser.model_files[ii];
      size_t start_pos = file.find_last_of("//") + 1;
      string name(file.begin() + start_pos, file.end() - 4);
      models.push_back(name);
      cout << models[ii] << endl;
    }

    // models.push_back("");
    Eigen::Affine3f cam_to_body;
    cam_to_body.matrix() << 0, 0, 1, 0,
                       -1, 0, 0, 0,
                       0, -1, 0, 0,
                       0, 0, 0, 1;
    PointCloudPtr depth_img_cloud(new PointCloud);
    Eigen::Affine3f world_to_cam_transform;
    world_to_cam_transform.matrix() = parser.camera_pose.matrix().cast<float>();
    world_to_cam_transform = cam_to_body.inverse() *
                             world_to_cam_transform.inverse();

    vector<vector<double>> cost_matrix(num_clusters);
    vector<vector<Eigen::Affine3f>> all_transforms(num_clusters);

    for (size_t cluster_idx = 0; cluster_idx < num_clusters; ++cluster_idx) {
      // cluster_clouds[cluster_idx] = DownsamplePointCloud(cluster_clouds[cluster_idx], 0.003);
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new
                                                 pcl::PointCloud<pcl::PointXYZ>);
      copyPointCloud(*cluster_clouds[cluster_idx], *cloud);

      transformPointCloud(*cloud, *cloud,
                          world_to_cam_transform);

      pcl::PCDWriter writer;
      stringstream ss;
      ss.precision(20);
      ss << kDebugDir + "cluster_" << cluster_idx << ".pcd";
      writer.writeBinary (ss.str()  , *cloud);

      // pose_estimator.getPose(cloud, roll, pitch, yaw, true);
      vector<double> distances;
      vector<Eigen::Affine3f> model_to_scene_transforms;
      const bool visualize = false;
      auto matched_clouds = pose_estimator.getPoseConstrained(cloud, visualize,
                                                              models, &distances, &model_to_scene_transforms);

      vector<double> normalized_distances(distances.size());
      auto min_element_it = std::min_element(distances.begin(), distances.end());
      double min_distance = *min_element_it;
      std::transform(distances.begin(), distances.end(),
      normalized_distances.begin(), [min_distance](double distance) {
        // return distance / std::max(1e-3, min_distance);
        return distance;
      });

      cost_matrix[cluster_idx] = normalized_distances;
      all_transforms[cluster_idx] = model_to_scene_transforms;

      // for (size_t model_idx = 0; model_idx < models.size(); ++model_idx) {
      //   Eigen::Affine3f final_transform;
      //   final_transform = transform.inverse() * model_to_scene_transforms[model_idx] * train_obj_models[model_idx].preprocessing_transform() * test_obj_models[model_idx].preprocessing_transform().inverse();
      //   float roll, pitch, yaw;
      //   float x, y, z;
      //   pcl::getTranslationAndEulerAngles(final_transform, x, y, z, roll, pitch, yaw);
      //   cout << "\nThe output Euler angles (using getEulerAngles function) are : "
      //        << std::endl;
      //   x = final_transform.translation()[0];
      //   y = final_transform.translation()[1];
      //   z = final_transform.translation()[2];
      //   cout << x << " " << y << " " << z << endl;
      //   cout << "roll : " << roll << " ,pitch : " << pitch << " ,yaw : " << yaw <<
      //        std::endl;
      //
      //   // fs << x << " " << y << " " << z << " " << yaw << endl;
      // }
    }

    vector<int> left_mates, right_mates;
    MinCostMatching(cost_matrix, left_mates, right_mates);

    // Stop measuring computation time.
		end = chrono::system_clock::now();
		chrono::duration<double> elapsed_seconds = end-start;
    fs_stats << config_file << endl;
    fs_stats << elapsed_seconds.count() << endl;

    PointCloudPtr composed_cloud(new PointCloud);

    fs << config_file << endl;
    for (int model_idx = 0; model_idx < models.size(); ++model_idx) {
      int cluster_idx = right_mates[model_idx];

      // double best_dist = std::numeric_limits<double>::max();
      // for (int jj = 0; jj < num_clusters; ++jj) {
      //   if (cost_matrix[jj][model_idx] < best_dist) {
      //     best_dist = cost_matrix[jj][model_idx];
      //     cluster_idx = jj;
      //   }
      // }

      Eigen::Affine3f final_transform;
      final_transform = world_to_cam_transform.inverse() *
                        all_transforms[cluster_idx][model_idx] *
                        train_obj_models[model_idx].preprocessing_transform() *
                        test_obj_models[model_idx].preprocessing_transform().inverse();

      float roll, pitch, yaw;
      float x, y, z;
      pcl::getTranslationAndEulerAngles(final_transform, x, y, z, roll, pitch, yaw);
      // Construct transform that has only yaw.
      Eigen::Matrix4f projected_transform;
      projected_transform <<
                          cos(yaw), -sin(yaw) , 0, x,
                              sin(yaw) , cos(yaw) , 0, y,
                              0, 0 , 1 , parser.table_height,
                              0, 0 , 0 , 1;
      final_transform.matrix() = projected_transform;

      auto transformed_mesh = test_obj_models[model_idx].GetTransformedMesh(
                                final_transform.matrix());
      pcl::PointCloud<PointT>::Ptr cloud (new
                                          pcl::PointCloud<PointT>);
      pcl::fromPCLPointCloud2(transformed_mesh->cloud, *cloud);

      // Now use ICP to align the two clouds
      Eigen::Affine3f icp_transform;
      icp_transform = GetICPAdjustedPose(cloud, cloud_in, cloud);
      final_transform = icp_transform * final_transform;

      pcl::getTranslationAndEulerAngles(final_transform, x, y, z, roll, pitch, yaw);
      cout << "\nThe output pose: " << std::endl;
      x = final_transform.translation()[0];
      y = final_transform.translation()[1];
      z = final_transform.translation()[2];
      cout << x << " " << y << " " << z << endl;
      cout << "roll : " << roll << " ,pitch : " << pitch << " ,yaw : " << yaw <<
           std::endl;

      *composed_cloud += *cloud;

      fs << x << " " << y << " " << z << " " << yaw << endl;
    }

    pcl::PCDWriter writer;
    std::stringstream ss;
    ss.precision(20);
    ss << kDebugDir + "composed_cloud_" << config_file_path.filename().string() <<
       ".pcd";
    writer.writeBinary (ss.str()  , *composed_cloud);
  }
  fs.close();
  fs_stats.close();

  return 0;
}


#include <iostream>
#include <fstream>

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

#include "glog/logging.h"

#include "dbscan_vp.h"

namespace po = boost::program_options;
using namespace clustering;

int main( int argc, char const* argv[] )
{
    std::string file_in;
    std::string file_out;
    std::string file_format;
    double eps = 0.1;
    size_t numpts = 3;

    po::options_description option_desc( "DBSCAN clusterizer" );

    option_desc.add_options()( "help", "Display help" )(
        "in,i", po::value< std::string >( &file_in )->required(), "Input file (CSV format)" )(
        "out,o", po::value< std::string >( &file_out )->required(), "Output file" )(
        "type,t", po::value< std::string >( &file_format )->default_value( "csv" ), "Output file format" )(
        "eps,e", po::value< double >( &eps )->default_value( 0.1 ), "Epsilon param" )(
        "numpts,n", po::value< size_t >( &numpts )->default_value( 3u ), "Min number of points in cluster" );

    po::variables_map options;

    po::store( po::command_line_parser( argc, argv ).options( option_desc ).run(), options );

    if ( options.empty() or options.count( "help" ) ) {
        std::cout << option_desc << std::endl;
        return 0;
    }

    po::notify( options );

    Dataset::Ptr dset = Dataset::create();
    LOG( INFO ) << "Loading dataset from " << file_in;
    dset->load_csv( file_in );

    DBSCAN_VP::Ptr dbs = boost::make_shared< DBSCAN_VP >( dset );

    dbs->fit();

    LOG( INFO ) << "Fit time " << dbs->get_fit_time() << " seconds";

    const uint32_t num_clusters = dbs->predict( eps, numpts );

    LOG( INFO ) << "Predict time " << dbs->get_predict_time() << " seconds";

    const DBSCAN_VP::Labels& l = dbs->get_labels();

    std::ofstream fl( file_out.c_str(), std::ios::out | std::ios::trunc );

    if ( file_format == "txt" ) {

        for ( ssize_t cl_id = 0; cl_id < num_clusters; ++cl_id ) {
            fl << "Cluster " << cl_id << std::endl;
            size_t cluster_total = 0;
            for ( size_t i = 0; i < l.size(); ++i ) {
                if ( l[i] == cl_id ) {
                    fl << "\t" << i << "\t" << dset->get_label( i ) << std::endl;
                    ++cluster_total;
                }
            }
            if ( cluster_total ) {
                fl << "Total " << cluster_total << std::endl;
            }
        }

        fl << "Outliers (-1)" << std::endl;

        size_t cluster_total = 0;
        for ( size_t i = 0; i < l.size(); ++i ) {
            if ( l[i] == -1 ) {
                fl << "\t" << i << "\t" << dset->get_label( i ) << std::endl;
                ++cluster_total;
            }
        }

        fl << "Total " << cluster_total << std::endl;
    } else if ( file_format == "csv" ) {
        for ( size_t i = 0; i < l.size(); ++i ) {
            fl << l[i] << std::endl;
        }
    }

    fl.close();

    // std::cout << "id,cluster_id" << std::endl;

    // for ( size_t i = 0; i < l.size(); ++i ) {
    //     std::cout << i << "," << l[i] << std::endl;
    // }

    LOG( INFO ) << "Num clusters " << num_clusters;

    return 0;
}