//
// Copyright 2021 Prathamesh Tagore <prathameshtagore@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_GIL_IMAGE_PROCESSING_MORPHOLOGY_HPP
#define BOOST_GIL_IMAGE_PROCESSING_MORPHOLOGY_HPP
#include <boost/gil/extension/numeric/kernel.hpp>
#include <boost/gil/gray.hpp>
#include <boost/gil/image_processing/threshold.hpp>

namespace boost
{
namespace gil
{
namespace detail
{
enum class morphological_operation
{
    dilation,
    erosion,
};
/// \addtogroup ImageProcessing
/// @{

/// \brief Implements morphological operations at pixel level.This function
/// compares neighbouring pixel values according to the kernel and choose
/// minimum/mamximum neighbouring pixel value and assigns it to the pixel under
/// consideration.
/// \param src_view - Source/Input image view.
/// \param dst_view - View which stores the final result of operations performed by this function.
/// \param kernel - Kernel matrix/structuring element containing 0's and 1's
/// which will be used for applying the required morphological operation.
/// \param identifier - Indicates the type of morphological operation to be applied.
/// \tparam SrcView type of source image.
/// \tparam DstView type of output image.
/// \tparam Kernel type of structuring element.
template <typename SrcView, typename DstView, typename Kernel>
void morph_impl(SrcView const& src_view, DstView const& dst_view, Kernel const& kernel,
                morphological_operation identifier)
{
    std::ptrdiff_t flip_ker_row, flip_ker_col, row_boundary, col_boundary;
    typename channel_type<typename SrcView::value_type>::type target_element;
    for (std::ptrdiff_t view_row = 0; view_row < src_view.height(); ++view_row)
    {
        for (std::ptrdiff_t view_col = 0; view_col < src_view.width(); ++view_col)
        {
            target_element = src_view(view_col, view_row);
            for (std::size_t kernel_row = 0; kernel_row < kernel.size(); ++kernel_row)
            {
                flip_ker_row = kernel.size() - 1 - kernel_row; // row index of flipped kernel

                for (std::size_t kernel_col = 0; kernel_col < kernel.size(); ++kernel_col)
                {
                    flip_ker_col = kernel.size() - 1 - kernel_col; // column index of flipped kernel

                    // We ensure that we consider only those pixels which are overlapped
                    // on a non-zero kernel_element as
                    if (kernel.at(flip_ker_row, flip_ker_col) == 0)
                    {
                        continue;
                    }
                    // index of input signal, used for checking boundary
                    row_boundary = view_row + (kernel.center_y() - flip_ker_row);
                    col_boundary = view_col + (kernel.center_x() - flip_ker_col);

                    // ignore input samples which are out of bound
                    if (row_boundary >= 0 && row_boundary < src_view.height() &&
                        col_boundary >= 0 && col_boundary < src_view.width())
                    {

                        if (identifier == morphological_operation::dilation)
                        {
                            target_element =
                                (std::max)(src_view(col_boundary, row_boundary)[0], target_element);
                        }
                        else if (identifier == morphological_operation::erosion)
                        {
                            target_element =
                                (std::min)(src_view(col_boundary, row_boundary)[0], target_element);
                        }
                    }
                }
            }
            dst_view(view_col, view_row) = target_element;
        }
    }
}

/// \brief Checks feasibility of the desired operation and passes parameter
/// values to the function morph_impl alongwith individual channel views of the
/// input image.
/// \param src_view - Source/Input image view.
/// \param dst_view - View which stores the final result of operations performed by this function.
/// \param kernel - Kernel matrix/structuring element containing 0's and 1's
/// which will be used for applying the required morphological operation.
/// \param identifier - Indicates the type of morphological operation to be applied.
/// \tparam SrcView type of source image.
/// \tparam DstView type of output image.
/// \tparam Kernel type of structuring element.
template <typename SrcView, typename DstView, typename Kernel>
void morph(SrcView const& src_view, DstView const& dst_view, Kernel const& ker_mat,
           morphological_operation identifier)
{
    BOOST_ASSERT(ker_mat.size() != 0 && src_view.dimensions() == dst_view.dimensions());
    gil_function_requires<ImageViewConcept<SrcView>>();
    gil_function_requires<MutableImageViewConcept<DstView>>();

    gil_function_requires<ColorSpacesCompatibleConcept<typename color_space_type<SrcView>::type,
                                                       typename color_space_type<DstView>::type>>();

    gil::image<typename DstView::value_type> intermediate_img(src_view.dimensions());

    for (std::size_t i = 0; i < src_view.num_channels(); i++)
    {
        morph_impl(nth_channel_view(src_view, i), nth_channel_view(view(intermediate_img), i),
                   ker_mat, identifier);
    }
    copy_pixels(view(intermediate_img), dst_view);
}

/// \brief Calculates the difference between pixel values of first image_view
/// and second image_view.
/// \param src_view1 - First parameter for subtraction of views.
/// \param src_view2 - Second parameter for subtraction of views.
/// \param diff_view - View containing result of the subtraction of second view from
/// the first view.
/// \tparam SrcView type of source/Input images used for subtraction.
/// \tparam DiffView type of image view containing the result of subtraction.
template <typename SrcView, typename DiffView>
void difference_impl(SrcView const& src_view1, SrcView const& src_view2, DiffView const& diff_view)
{
    for (std::ptrdiff_t view_row = 0; view_row < src_view1.height(); ++view_row)
        for (std::ptrdiff_t view_col = 0; view_col < src_view1.width(); ++view_col)
            diff_view(view_col, view_row) =
                src_view1(view_col, view_row) - src_view2(view_col, view_row);
}

/// \brief Passes parameter values to the function 'difference_impl' alongwith
/// individual channel views of input images.
/// \param src_view1 - First parameter for subtraction of views.
/// \param src_view2 - Second parameter for subtraction of views.
/// \param diff_view - View containing result of the subtraction of second view from the first view.
/// \tparam SrcView type of source/Input images used for subtraction.
/// \tparam DiffView type of image view containing the result of subtraction.
template <typename SrcView, typename DiffView>
void difference(SrcView const& src_view1, SrcView const& src_view2, DiffView const& diff_view)
{
    gil_function_requires<ImageViewConcept<SrcView>>();
    gil_function_requires<MutableImageViewConcept<DiffView>>();

    gil_function_requires<ColorSpacesCompatibleConcept<
        typename color_space_type<SrcView>::type, typename color_space_type<DiffView>::type>>();

    for (std::size_t i = 0; i < src_view1.num_channels(); i++)
    {
        difference_impl(nth_channel_view(src_view1, i), nth_channel_view(src_view2, i),
                        nth_channel_view(diff_view, i));
    }
}
} // namespace detail

/// \brief Applies morphological dilation on the input image view using given
/// structuring element. It gives the maximum overlapped value to the pixel
/// overlapping with the center element of structuring element. \param src_view
/// - Source/input image view.
/// \param int_op_view - view for writing output and performing intermediate operations.
/// \param ker_mat - Kernel matrix/structuring element containing 0's and 1's which will be used for
/// applying dilation.
/// \param iterations - Specifies the number of times dilation is to be applied on the input image
/// view.
/// \tparam SrcView type of source image, models gil::ImageViewConcept.
/// \tparam IntOpView type of output image, models gil::MutableImageViewConcept.
/// \tparam Kernel type of structuring element.
template <typename SrcView, typename IntOpView, typename Kernel>
void dilate(SrcView const& src_view, IntOpView const& int_op_view, Kernel const& ker_mat,
            int iterations)
{
    copy_pixels(src_view, int_op_view);
    for (int i = 0; i < iterations; ++i)
        morph(int_op_view, int_op_view, ker_mat, detail::morphological_operation::dilation);
}

/// \brief Applies morphological erosion on the input image view using given
/// structuring element. It gives the minimum overlapped value to the pixel
/// overlapping with the center element of structuring element.
/// \param src_view - Source/input image view.
/// \param int_op_view - view for writing output and performing intermediate operations.
/// \param ker_mat - Kernel matrix/structuring element containing 0's and 1's which will be used for
/// applying erosion.
/// \param iterations - Specifies the number of times erosion is to be applied on the input
/// image view.
/// \tparam SrcView type of source image, models gil::ImageViewConcept.
/// \tparam IntOpView type of output image, models gil::MutableImageViewConcept.
/// \tparam Kernel type of structuring element.
template <typename SrcView, typename IntOpView, typename Kernel>
void erode(SrcView const& src_view, IntOpView const& int_op_view, Kernel const& ker_mat,
           int iterations)
{
    copy_pixels(src_view, int_op_view);
    for (int i = 0; i < iterations; ++i)
        morph(int_op_view, int_op_view, ker_mat, detail::morphological_operation::erosion);
}

/// \brief Performs erosion and then dilation on the input image view . This
/// operation is utilized for removing noise from images.
/// \param src_view - Source/input image view.
/// \param int_op_view - view for writing output and performing intermediate operations.
/// \param ker_mat - Kernel matrix/structuring element containing 0's and 1's which will be used for
/// applying the opening operation.
/// \tparam SrcView type of source image, models gil::ImageViewConcept.
/// \tparam IntOpView type of output image, models gil::MutableImageViewConcept.
/// \tparam Kernel type of structuring element.
template <typename SrcView, typename IntOpView, typename Kernel>
void opening(SrcView const& src_view, IntOpView const& int_op_view, Kernel const& ker_mat)
{
    erode(src_view, int_op_view, ker_mat, 1);
    dilate(int_op_view, int_op_view, ker_mat, 1);
}

/// \brief Performs dilation and then erosion on the input image view which is
/// exactly opposite to the opening operation . Closing operation can be
/// utilized for closing small holes inside foreground objects.
/// \param src_view - Source/input image view.
/// \param int_op_view - view for writing output and performing intermediate operations.
/// \param ker_mat - Kernel matrix/structuring element containing 0's and 1's which will be used for
/// applying the closing operation.
/// \tparam SrcView type of source image, models gil::ImageViewConcept.
/// \tparam IntOpView type of output image, models gil::MutableImageViewConcept.
/// \tparam Kernel type of structuring element.
template <typename SrcView, typename IntOpView, typename Kernel>
void closing(SrcView const& src_view, IntOpView const& int_op_view, Kernel const& ker_mat)
{
    dilate(src_view, int_op_view, ker_mat, 1);
    erode(int_op_view, int_op_view, ker_mat, 1);
}

/// \brief Calculates the difference between image views generated after
/// applying dilation dilation and erosion on an image . The resultant image
/// will look like the outline of the object(s) present in the image.
/// \param src_view - Source/input image view.
/// \param dst_view - Destination view which will store the final result of morphological
/// gradient operation.
/// \param ker_mat - Kernel matrix/structuring element containing 0's and 1's which
/// will be used for applying the morphological gradient operation.
/// \tparam SrcView type of source image, models gil::ImageViewConcept.
/// \tparam DstView type of output image, models gil::MutableImageViewConcept.
/// \tparam Kernel type of structuring element.
template <typename SrcView, typename DstView, typename Kernel>
void morphological_gradient(SrcView const& src_view, DstView const& dst_view, Kernel const& ker_mat)
{
    using namespace boost::gil;
    gil::image<typename DstView::value_type> int_dilate(src_view.dimensions()),
        int_erode(src_view.dimensions());
    dilate(src_view, view(int_dilate), ker_mat, 1);
    erode(src_view, view(int_erode), ker_mat, 1);
    difference(view(int_dilate), view(int_erode), dst_view);
}

/// \brief Calculates the difference between input image view and the view
/// generated by opening operation on the input image view.
/// \param src_view - Source/input image view.
/// \param dst_view - Destination view which will store the final result of top hat operation.
/// \param ker_mat - Kernel matrix/structuring element containing 0's and 1's which will be used for
/// applying the top hat operation.
/// \tparam SrcView type of source image, models gil::ImageViewConcept.
/// \tparam DstView type of output image, models gil::MutableImageViewConcept.
/// \tparam Kernel type of structuring element.
template <typename SrcView, typename DstView, typename Kernel>
void top_hat(SrcView const& src_view, DstView const& dst_view, Kernel const& ker_mat)
{
    using namespace boost::gil;
    gil::image<typename DstView::value_type> int_opening(src_view.dimensions());
    opening(src_view, view(int_opening), ker_mat);
    difference(src_view, view(int_opening), dst_view);
}

/// \brief Calculates the difference between closing of the input image and
/// input image.
/// \param src_view - Source/input image view.
/// \param dst_view - Destination view which will store the final result of black hat operation.
/// \param ker_mat - Kernel matrix/structuring element containing 0's and 1's
/// which will be used for applying the black hat operation.
/// \tparam SrcView type of source image, models gil::ImageViewConcept.
/// \tparam DstView type of output image, models gil::MutableImageViewConcept.
/// \tparam Kernel type of structuring element.
template <typename SrcView, typename DstView, typename Kernel>
void black_hat(SrcView const& src_view, DstView const& dst_view, Kernel const& ker_mat)
{
    using namespace boost::gil;
    gil::image<typename DstView::value_type> int_closing(src_view.dimensions());
    closing(src_view, view(int_closing), ker_mat);
    difference(view(int_closing), src_view, dst_view);
}
/// @}
}}     // namespace boost::gil
#endif // BOOST_GIL_IMAGE_PROCESSING_MORPHOLOGY_HPP


#ifndef _MODEL_HPP_
#define _MODEL_HPP_

#include "Trajectory.h"
#include <vector>
#include "Policy.h"
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

/*
This class builds a model of an MDPs with tabular states and actions, and a finite  horizon, L.
*/

class Model {
public:
	// Adding a defauly constructor so that I can extend it
	Model();

	/*
	Take historical data in trajs and build a model. numState and numActions are the total numbers of possible states and actions
	*/
	Model(const vector<Trajectory*> & trajs, int numStates, int numActions, int L, bool JiangStyle);

	/*
	Use the model to directly estimate the value of a policy. This uses value iteration.
	*/
	//double getPolicyValue(const Policy & pi) const;

	// V and Q predictions - can be loaded for a specific evaluation policy
	void loadEvalPolicy(const Policy & pi, const int & L); // int L = Gridworld::getMaxTrajLen();
	vector<VectorXd> actionProbabilities; // [s][a]
	vector<VectorXd> V; // [t](s) - t in [0,L].
	vector<MatrixXd> Q; // [t](s,a)
	//vector<MatrixXd> Rsa; // [t](s,a) - Prediction of R_t given that S_0 =s and A_0=a
	//vector<vector<MatrixXd>> Rsas; // [t][s](a,sPrime) - Prediction of R_t given that S_0 =s and A_0=a, and S_1 = sPrime
	//MatrixXd Rs; // (s,t)
	double evalPolicyValue;

	// Generate trajectories from the provided policy
	vector<Trajectory> generateTrajectories(const Policy & pi, int N, mt19937_64 & generator) const;

	// Estimate value of policy under model using Monte Carlo returns
  double evalMonteCarlo(const Policy & pi, int N, mt19937_64 & generator) const;

	// We exposed these variables in order to do DRv2 (version 2 implementations) without having to compute Rsa and Rsas tables above (compute intensive)
	vector<vector<vector<double>>> R;				// R[s][a][s']. Size = [numStates][numActions][numStates+1]
	vector<vector<vector<double>>> P;				// P[s][a][s']. Size = [numStates][numActions][numStates+1]

//private:
	int N;
	int L;
	int numStates;
	int numActions;
	vector<double> d0;								// d0[s] = Pr(S_0=s). Size = [numStates]

	// How many times was each (s), (s,a), and (s,a,s') tuple seen?
	vector<vector<int>> stateActionCounts;
	vector<vector<int>> stateActionCounts_includingHorizon;				 // Includes transitions to terminal absorbing state due to time horizon
	vector<vector<vector<int>>> stateActionStateCounts;
	vector<vector<vector<int>>> stateActionStateCounts_includingHorizon; // Includes transitions to terminal absorbing state due to time horizon
};

#endif


#ifdef STAN_OPENCL

#include <stan/math/opencl/kernel_generator.hpp>
#include <stan/math/opencl/matrix_cl.hpp>
#include <stan/math/opencl/copy.hpp>
#include <stan/math/opencl/multiply.hpp>
#include <test/unit/math/opencl/kernel_generator/reference_kernel.hpp>
#include <Eigen/Dense>
#include <gtest/gtest.h>
#include <algorithm>
#include <string>

using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::MatrixXi;
using stan::math::matrix_cl;

#define EXPECT_MATRIX_NEAR(A, B, DELTA) \
  for (int i = 0; i < A.size(); i++)    \
    EXPECT_NEAR(A(i), B(i), DELTA);

TEST(KernelGenerator, addition_test) {
  std::string kernel_filename = "binary_operation_addition.cl";
  MatrixXd m1(3, 3);
  m1 << 1, 2.5, 3, 4, 5, 6.3, 7, -8, -9.5;
  MatrixXd m2(3, 3);
  m2 << 10, 100, 1000, 0, -10, -12, 2, 4, 8;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<double> m2_cl(m2);

  auto tmp = m1_cl + m2_cl;

  matrix_cl<double> res_cl;
  std::string kernel_src = tmp.get_kernel_source_for_evaluating_into(res_cl);
  stan::test::store_reference_kernel_if_needed(kernel_filename, kernel_src);
  std::string expected_kernel_src
      = stan::test::load_reference_kernel(kernel_filename);
  EXPECT_EQ(expected_kernel_src, kernel_src);

  res_cl = tmp;
  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  MatrixXd correct = m1 + m2.cast<double>();
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

#define BINARY_OPERATION_TEST(test_name, operation, res_type)          \
  TEST(KernelGenerator, test_name) {                                   \
    MatrixXd m1(3, 3);                                                 \
    m1 << 1, 2.5, 3, 4, 5, 6.3, 7, -8, -9.5;                           \
    MatrixXi m2(3, 3);                                                 \
    m2 << 1, 100, 1000, 0, -10, -12, 2, -8, 8;                         \
                                                                       \
    matrix_cl<double> m1_cl(m1);                                       \
    matrix_cl<int> m2_cl(m2);                                          \
                                                                       \
    auto tmp = m1_cl operation m2_cl;                                  \
    matrix_cl<res_type> res_cl = tmp;                                  \
    Matrix<res_type, -1, -1> res = stan::math::from_matrix_cl(res_cl); \
                                                                       \
    Matrix<res_type, -1, -1> correct                                   \
        = m1.array() operation m2.cast<double>().array();              \
    EXPECT_MATRIX_NEAR(res, correct, 1e-9);                            \
  }

BINARY_OPERATION_TEST(subtraction_test, -, double);

TEST(KernelGenerator, elt_multiply_test) {
  MatrixXd m1(3, 3);
  m1 << 1, 2.5, 3, 4, 5, 6.3, 7, -8, -9.5;
  MatrixXi m2(3, 3);
  m2 << 10, 100, 1000, 0, -10, -12, 2, 4, 8;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<int> m2_cl(m2);

  auto tmp = elt_multiply(m1_cl, m2_cl);
  matrix_cl<double> res_cl = tmp;
  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  MatrixXd correct = m1.array() * m2.cast<double>().array();
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

TEST(KernelGenerator, elt_divide_test) {
  MatrixXd m1(3, 3);
  m1 << 1, 2.5, 3, 4, 5, 6.3, 7, -8, -9.5;
  MatrixXi m2(3, 3);
  m2 << 10, 100, 1000, 1, -10, -12, 2, 4, 8;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<int> m2_cl(m2);
  auto tmp = elt_divide(m1_cl, m2_cl);
  matrix_cl<double> res_cl = tmp;

  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  MatrixXd correct = m1.array() / m2.cast<double>().array();
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

BINARY_OPERATION_TEST(less_than_test, <, bool);
BINARY_OPERATION_TEST(less_than_or_equal_test, <=, bool);
BINARY_OPERATION_TEST(greater_than_test, >, bool);
BINARY_OPERATION_TEST(greater_than_or_equal_test, >=, bool);
BINARY_OPERATION_TEST(equals_test, ==, bool);
BINARY_OPERATION_TEST(not_equals_test, !=, bool);

TEST(KernelGenerator, logical_or_test) {
  Matrix<bool, -1, -1> m1(3, 3);
  m1 << true, true, true, false, false, true, true, false, false;
  Matrix<bool, -1, -1> m2(3, 3);
  m2 << true, false, false, true, false, true, false, true, false;

  matrix_cl<bool> m1_cl(m1);
  matrix_cl<bool> m2_cl(m2);

  auto tmp = m1_cl || m2_cl;
  matrix_cl<bool> res_cl = tmp;
  Matrix<bool, -1, -1> res = stan::math::from_matrix_cl(res_cl);

  Matrix<bool, -1, -1> correct = m1 || m2;
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

TEST(KernelGenerator, logical_and_test) {
  Matrix<bool, -1, -1> m1(3, 3);
  m1 << true, true, true, false, false, true, true, false, false;
  Matrix<bool, -1, -1> m2(3, 3);
  m2 << true, false, false, true, false, true, false, true, false;

  matrix_cl<bool> m1_cl(m1);
  matrix_cl<bool> m2_cl(m2);

  auto tmp = m1_cl && m2_cl;
  matrix_cl<bool> res_cl = tmp;
  Matrix<bool, -1, -1> res = stan::math::from_matrix_cl(res_cl);

  Matrix<bool, -1, -1> correct = m1 && m2;
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

TEST(KernelGenerator, binary_operation_multiple_operations) {
  MatrixXd m1(3, 3);
  m1 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  MatrixXi m2(3, 3);
  m2 << 10, 100, 1000, 0, -10, -12, 2, 4, 8;
  MatrixXi m3(3, 3);
  m3 << 1, 10, 1100, -40, -14, -1, 2, 4, 8;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<int> m2_cl(m2);
  matrix_cl<int> m3_cl(m3);
  auto tmp = m1_cl * 2. - (m2_cl + m3_cl) * 4;
  matrix_cl<double> res_cl = tmp;

  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  MatrixXd correct = m1 * 2. - ((m2 + m3) * 4).cast<double>();
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

TEST(KernelGenerator, binary_operation_multiple_operations_accepts_lvalue) {
  MatrixXd m1(3, 3);
  m1 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  MatrixXi m2(3, 3);
  m2 << 10, 100, 1000, 0, -10, -12, 2, 4, 8;
  MatrixXi m3(3, 3);
  m3 << 1, 10, 1100, -40, -14, -1, 2, 4, 8;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<int> m2_cl(m2);
  matrix_cl<int> m3_cl(m3);
  auto tmp = (m2_cl + m3_cl) * 4;
  auto tmp2 = m1_cl * 2. - tmp;
  matrix_cl<double> res_cl = tmp2;

  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  MatrixXd correct = m1 * 2. - ((m2 + m3) * 4).cast<double>();
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

TEST(KernelGenerator, multiplication_with_scalar_test) {
  MatrixXd m1(3, 3);
  m1 << 1, 2.5, 3, 4, 5, 6.3, 7, -8, -9.5;
  MatrixXd m2(3, 3);
  m2 << 10, 100, 1000, 0, -10, -12, 2, 4, 8;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<double> m2_cl(m2);

  auto tmp = m1_cl * 2;
  matrix_cl<double> res_cl = tmp;
  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  EXPECT_MATRIX_NEAR(res, (m1.array() * 2).matrix().eval(), 1e-9);

  auto tmp2 = 2 * m1_cl;
  matrix_cl<double> res2_cl = tmp2;
  MatrixXd res2 = stan::math::from_matrix_cl(res2_cl);

  MatrixXd correct = m1.array() * 2;
  EXPECT_MATRIX_NEAR(res2, correct, 1e-9);
}

TEST(KernelGenerator, matrix_multiplication_in_expression_test) {
  MatrixXd m1(3, 3);
  m1 << 1, 2.5, 3, 4, 5, 6.3, 7, -8, -9.5;
  MatrixXd m2(3, 3);
  m2 << 10, 100, 1000, 0, -10, -12, 2, 4, 8;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<double> m2_cl(m2);

  auto tmp = ((m1_cl - 2) * (m2_cl + 3)) - 1;
  matrix_cl<double> res_cl = tmp;
  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  MatrixXd correct
      = ((m1.array() - 2.).matrix() * (m2.array() + 3.).matrix()).array() - 1.;
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

TEST(KernelGenerator, reuse_expression_simple) {
  MatrixXd m1(3, 3);
  m1 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  MatrixXd m2(3, 3);
  m2 << 10, 100, 1000, 0.1, -10, -12, 2, 4, 8;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<double> m2_cl(m2);
  auto tmp = stan::math::elt_divide(m1_cl, m2_cl);
  auto tmp2 = stan::math::elt_multiply(tmp, tmp);
  matrix_cl<double> res_cl;
  std::string kernel_src = tmp2.get_kernel_source_for_evaluating_into(res_cl);
  // if the expression is correctly reused, division will only occur once in the
  // kernel
  EXPECT_EQ(1, std::count(kernel_src.begin(), kernel_src.end(), '/'));
  res_cl = tmp2;

  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  auto tmp_eig = m1.array() / m2.array();
  MatrixXd res_eig = tmp_eig * tmp_eig;

  EXPECT_MATRIX_NEAR(res_eig, res, 1e-9);
}

// Shows subexpressions tmp and tmp2 are reused in the kernel
TEST(KernelGenerator, reuse_expression_complicated) {
  std::string kernel_filename = "binary_operation_reuse_expression.cl";
  MatrixXd m1(3, 3);
  m1 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  MatrixXd m2(3, 3);
  m2 << 10, 100, 1000, 0, -10, -12, 2, 4, 8;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<double> m2_cl(m2);
  auto tmp = m1_cl + m2_cl;
  auto tmp2 = stan::math::elt_divide(stan::math::elt_multiply(tmp, tmp), m1_cl);
  auto tmp3 = stan::math::elt_multiply(stan::math::elt_divide(tmp, tmp2), tmp2);
  matrix_cl<double> res_cl;
  std::string kernel_src = tmp3.get_kernel_source_for_evaluating_into(res_cl);
  stan::test::store_reference_kernel_if_needed(kernel_filename, kernel_src);
  std::string expected_kernel_src
      = stan::test::load_reference_kernel(kernel_filename);
  EXPECT_EQ(expected_kernel_src, kernel_src);

  res_cl = tmp3;

  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  auto tmp_eig = m1.array() + m2.array();
  MatrixXd tmp2_eig = (tmp_eig * tmp_eig).array() / m1.array();
  MatrixXd tmp3_eig
      = (tmp_eig.array() / tmp2_eig.array()).array() * tmp2_eig.array();

  EXPECT_MATRIX_NEAR(tmp3_eig, res, 1e-9);
}

#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2011 Gael Guennebaud <g.gael@free.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#define EIGEN_NO_DEBUG_SMALL_PRODUCT_BLOCKS
#include "sparse_solver.h"

#include <Eigen/CholmodSupport>

template<typename T> void test_cholmod_T()
{
  CholmodDecomposition<SparseMatrix<T>, Lower> g_chol_colmajor_lower; g_chol_colmajor_lower.setMode(CholmodSupernodalLLt);
  CholmodDecomposition<SparseMatrix<T>, Upper> g_chol_colmajor_upper; g_chol_colmajor_upper.setMode(CholmodSupernodalLLt);
  CholmodDecomposition<SparseMatrix<T>, Lower> g_llt_colmajor_lower;  g_llt_colmajor_lower.setMode(CholmodSimplicialLLt);
  CholmodDecomposition<SparseMatrix<T>, Upper> g_llt_colmajor_upper;  g_llt_colmajor_upper.setMode(CholmodSimplicialLLt);
  CholmodDecomposition<SparseMatrix<T>, Lower> g_ldlt_colmajor_lower; g_ldlt_colmajor_lower.setMode(CholmodLDLt);
  CholmodDecomposition<SparseMatrix<T>, Upper> g_ldlt_colmajor_upper; g_ldlt_colmajor_upper.setMode(CholmodLDLt);

  CholmodSupernodalLLT<SparseMatrix<T>, Lower> chol_colmajor_lower;
  CholmodSupernodalLLT<SparseMatrix<T>, Upper> chol_colmajor_upper;
  CholmodSimplicialLLT<SparseMatrix<T>, Lower> llt_colmajor_lower;
  CholmodSimplicialLLT<SparseMatrix<T>, Upper> llt_colmajor_upper;
  CholmodSimplicialLDLT<SparseMatrix<T>, Lower> ldlt_colmajor_lower;
  CholmodSimplicialLDLT<SparseMatrix<T>, Upper> ldlt_colmajor_upper;

  check_sparse_spd_solving(g_chol_colmajor_lower);
  check_sparse_spd_solving(g_chol_colmajor_upper);
  check_sparse_spd_solving(g_llt_colmajor_lower);
  check_sparse_spd_solving(g_llt_colmajor_upper);
  check_sparse_spd_solving(g_ldlt_colmajor_lower);
  check_sparse_spd_solving(g_ldlt_colmajor_upper);

  check_sparse_spd_solving(chol_colmajor_lower);
  check_sparse_spd_solving(chol_colmajor_upper);
  check_sparse_spd_solving(llt_colmajor_lower);
  check_sparse_spd_solving(llt_colmajor_upper);
  check_sparse_spd_solving(ldlt_colmajor_lower);
  check_sparse_spd_solving(ldlt_colmajor_upper);

  check_sparse_spd_determinant(chol_colmajor_lower);
  check_sparse_spd_determinant(chol_colmajor_upper);
  check_sparse_spd_determinant(llt_colmajor_lower);
  check_sparse_spd_determinant(llt_colmajor_upper);
  check_sparse_spd_determinant(ldlt_colmajor_lower);
  check_sparse_spd_determinant(ldlt_colmajor_upper);
}

void test_cholmod_support()
{
  CALL_SUBTEST_1(test_cholmod_T<double>());
  CALL_SUBTEST_2(test_cholmod_T<std::complex<double> >());
}


//---------------------------------------------------------------------------//
// Copyright (c) 2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_MATH_EXPRESSION_MATH_HPP
#define CRYPTO3_MATH_EXPRESSION_MATH_HPP

#ifndef CRYPTO3_MATH_EXPRESSION_HPP
#error "math.hpp must not be included directly!"
#endif

#include <boost/math/constants/constants.hpp>

#include <cmath>

namespace nil {
    namespace crypto3 {
        namespace math {
            namespace expressions {
                namespace detail {
                    namespace math {

                        /// @brief Sign function
                        template <typename T>
                        T sgn(T x) {
                            return (T{0} < x) - (x < T{0});
                        }

                        /// @brief isnan function with adjusted return type
                        template <typename T>
                        T isnan(T x) {
                            return std::isnan(x);
                        }

                        /// @brief isinf function with adjusted return type
                        template <typename T>
                        T isinf(T x) {
                            return std::isinf(x);
                        }

                        /// @brief Convert radians to degrees
                        template <typename T>
                        T deg(T x) {
                            return x * boost::math::constants::radian<T>();
                        }

                        /// @brief Convert degrees to radians
                        template <typename T>
                        T rad(T x) {
                            return x * boost::math::constants::degree<T>();
                        }

                        /// @brief unary plus
                        template <typename T>
                        T plus(T x) {
                            return x;
                        }

                        /// @brief binary plus
                        template <typename T>
                        T plus(T x, T y) {
                            return x + y;
                        }

                        /// @brief unary minus
                        template <typename T>
                        T minus(T x) {
                            return -x;
                        }

                        /// @brief binary minus
                        template <typename T>
                        T minus(T x, T y) {
                            return x - y;
                        }

                        /// @brief multiply
                        template <typename T>
                        T multiplies(T x, T y) {
                            return x * y;
                        }

                        /// @brief divide
                        template <typename T>
                        T divides(T x, T y) {
                            return x / y;
                        }

                        /// @brief unary not
                        template <typename T>
                        T unary_not(T x) {
                            return !x;
                        }

                        /// @brief logical and
                        template <typename T>
                        T logical_and(T x, T y) {
                            return x && y;
                        }

                        /// @brief logical or
                        template <typename T>
                        T logical_or(T x, T y) {
                            return x || y;
                        }

                        /// @brief less
                        template <typename T>
                        T less(T x, T y) {
                            return x < y;
                        }

                        /// @brief less equals
                        template <typename T>
                        T less_equals(T x, T y) {
                            return x <= y;
                        }

                        /// @brief greater
                        template <typename T>
                        T greater(T x, T y) {
                            return x > y;
                        }

                        /// @brief greater equals
                        template <typename T>
                        T greater_equals(T x, T y) {
                            return x >= y;
                        }

                        /// @brief equals
                        template <typename T>
                        T equals(T x, T y) {
                            return x == y;
                        }

                        /// @brief not equals
                        template <typename T>
                        T not_equals(T x, T y) {
                            return x != y;
                        }

                    } // namespace math
                }    // namespace detail
            }    // namespace expressions
        }    // namespace math
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_MATH_EXPRESSION_MATH_HPP


#pragma once

#include <iostream>

#include <Eigen/Dense>

//The geometry of the grid
enum Geometry
{
    Triangular = 0,
    Orthogonal = 1,
    Hexagonal = 2
};

std::ostream& operator<<(std::ostream& os, const Geometry& g);

//The orientation of the grid
enum Orientation
{
    Horizontal = 0,
    Vertical = 1
};

std::ostream& operator<<(std::ostream& os, const Orientation& o);

//this is a flat mesh
//i will principaly use it to set lens array and checkerboard (if needed ;) )
template<int Dimension>
struct PolygonMesh
{
    Geometry geometry;
    Orientation orientation;
    std::array<int,2> dimensions;
    double edge_length;  //the mean distance between two vertices

    PolygonMesh(const Geometry g = Triangular
              , const Orientation o = Horizontal
              , const std::array<int,2>& d = std::array<int,2>{0,0}
              , const double e = 0.0
        )
    : geometry(g), dimensions(d), edge_length(e)
    {}

    ~PolygonMesh(){};

    Eigen::Matrix<double,Dimension,1> vertex(const int col, const int row) const;
    // Eigen::Matrix<double,Dimension,1> vertex(const int i) const;

    // return edge parameters linking two nodes (note by indices)
    // edge(const int i1, const int i2) const;
};

using PolygonMesh2D = PolygonMesh<2>;
using PolygonMesh3D = PolygonMesh<3>;

template<int Dimension>
Eigen::Matrix<double,Dimension,1> PolygonMesh<Dimension>::vertex(const int col, const int row) const
{
    // if ( col < 0 or col >= width )
    //     throw(std::out_of_range( boost::str(boost::format("PolygonMesh<Dimension>::vertex: wrong col index (%1%)")%col)) );
    // if ( row < 0 or row >= height )
    //     throw(std::out_of_range( boost::str(boost::format("PolygonMesh<Dimension>::vertex: wrong row index (%1%)")%row)) );

    Eigen::Matrix<double,Dimension,1> p = Eigen::Matrix<double,Dimension,1>::Zero();

    if (geometry == Triangular)
    {
        std::cerr << "Error: PolygonMesh::vertex: Not implemented." << std::endl;
        if (orientation == Horizontal)
        {
            return p;
        }
        else if (orientation == Vertical)
        {
            return p;
        }
        else
            std::cerr << "Error: PolygonMesh::vertex: wrong orientation value (" << orientation << ")."<< std::endl;

    }
    else if (geometry == Orthogonal)
    {
        p.x() = edge_length * (double(col) + double(row));
        p.y() = edge_length * (double(row) + double(col));
    }
    else if (geometry == Hexagonal)
    {
        double val = 0.0;
        if (orientation == Horizontal)
        {
            if (row%2 == 0)
                val = 0.5;

            p.x() =  (double(col) + val) * edge_length;
            p.y() =  double(row) * edge_length * std::sin(M_PI / 3.0);

            return p;

        }
        if (orientation == Vertical)
        {
            if (col%2 == 0)
                val = 0.5;

            p.x() = double(col) * edge_length * std::sin(M_PI / 3.0);
            p.y() = (double(row) + val) * edge_length;

            return p;
        }
        else
            std::cerr << "Error: PolygonMesh::vertex: wrong orientation value (" << orientation << ")."<< std::endl;
    }
    else
        std::cerr << "Error: PolygonMesh::vertex: wrong geometry value (" << geometry << ")."<< std::endl;

    return {};
}

template<int Dimension>
std::ostream& operator<<(std::ostream& os, const PolygonMesh<Dimension>& m)
{
    os << "geometry: " << m.geometry << "\n";
    os << "orientation: " << m.orientation << "\n";
    os << "dimensions: [" << m.dimensions[0] << ", " << m.dimensions[1] << "]\n";
    os << "edges length: " << m.edge_length;

    return os;
}


/*
 * A header-only version of RedSVD
 *
 * Copyright (c) 2014 Nicolas Tessore
 *
 * based on RedSVD
 *
 * Copyright (c) 2010 Daisuke Okanohara
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above Copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 2. 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.
 *
 * 3. Neither the name of the authors nor the names of its contributors
 *    may be uses to endorse or promote products derived from this
 *    software without specific prior written permission.
 *
 *    Minor modifications by Luca Formaggia.
 */

#ifndef REDSVD_MODULE_H
#define REDSVD_MODULE_H

#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include <cstdlib>
#include <cmath>
#include <random>

namespace RedSVD
{
  //! It generates a (pseudo) x and y extracted from a (pseudo) random Gaussian distribution
  /*!
   * It uses the Box-Muller transform, see
   * [this Wikipedia page](https://en.wikipedia.org/wiki/Box%E2%80%93Muller_transform),
   * to generate a couple of variables from a normally distribution
   * with zero mean and unitary variance.
   *
   * This version uses rand() to generate the uniform distribution from
   * which the Gaussian one is derived.  It may be changed to implement
   * the new random distribution.
   *
   * \note Compared with the original version, it is no more used to generate
   * random Gaussian matrices, since we use the random number generator
   * of the standard library.
   */
  template<typename Scalar>
  inline void sample_gaussian(Scalar& x, Scalar& y)
  {
    using std::sqrt;
    using std::log;
    using std::cos;
    using std::sin;

    constexpr Scalar PI(3.1415926535897932384626433832795028841971693993751);

    Scalar v1 = (Scalar)(std::rand() + Scalar(1)) / ((Scalar)RAND_MAX+Scalar(2));
    Scalar v2 = (Scalar)(std::rand() + Scalar(1)) / ((Scalar)RAND_MAX+Scalar(2));
    Scalar len = sqrt(Scalar(-2) * log(v1));
    x = len * cos(Scalar(2) * PI * v2);
    y = len * sin(Scalar(2) * PI * v2);
  }

  //! Generates a random Gaussian matrix
  /*! A matrix whose elements are drawn from Normal distribution with
   * zero mean and unitary variance.
   *
   * \note Modified from the original code by Luca Formaggia: using standard library random number distribution.
  */
  template<typename MatrixType>
  inline void sample_gaussian(MatrixType& mat)
  {
    // generate random seed
    std::random_device rd{};
    std::mt19937 gen{rd()};
    // define the distribution
    typedef typename MatrixType::Scalar Scalar;
    std::normal_distribution<Scalar> d;

    typedef typename MatrixType::Index Index;

    for(Index i = 0; i < mat.rows(); ++i)
      for(Index j = 0; j < mat.cols(); ++j)
        mat(i, j)=d(gen);
  }
  //! Performs Gram Schmidt on the matrix columns
  /*!
   * \tparam MatrixType An Eigen Dense Matrix
   * \param mat The matrix on which to operate. The orthonormalisation is performed in-place
   * \param EPS tolerance to discard small columns. Defaulted to 1e-6
   */
  template<typename MatrixType>
  inline void gram_schmidt(MatrixType& mat,
                           typename MatrixType::Scalar const EPS=1.E-6)
  {
    typedef typename MatrixType::Scalar Scalar;
    typedef typename MatrixType::Index Index;

    for(Index i = 0; i < mat.cols(); ++i)
      {
        // c_i = c_i - sum_{j<i} (c_i * c_j)c_j
        for(Index j = 0; j = i.
        if(norm < EPS)
          {
            for(Index k = i; k < mat.cols(); ++k)
              mat.col(k).setZero();
            return;
          }
        // Orhonormalization
        mat.col(i) /= norm;
      }
  }

  //! Performs reduced Singular Value Decomposition
  /*!
   * Given a nxm matrix \f$A\f$ it computes matrices \f$U\f$ (nxk, orthogonal), \f$\Sigma\f$ (kxk, diagonal) and
   * \f$V\f$ (mxk, orthogonal), so that \f$A^*=U\Sigma V^T\f$ is a rank-k matrix approximation of \f$A\f$.
   * Matrix \f$\Sigma\f$ is returned as vector of size k containing the diagonal elements, which are an approximation
   * of the first singular values of \f$A\f$.
   *
   * This algorithm uses is a probabilistic algorithm. More details on this type of algorithms may be found in
   * Finding structures with randomness:..., Halko, N. and Martinsson, P. G. and Tropp, J. A. (2009)
   */
  template<typename My_MatrixType>
  class RedSVD
  {
  public:
    typedef My_MatrixType MatrixType;
    typedef typename MatrixType::Scalar Scalar;
    typedef typename MatrixType::Index Index;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> DenseMatrix;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ScalarVector;
    //! Default constructor
    RedSVD()=default;

    //! A may be a dense or sparse Eigen matrix.
    /*!
      Here rank is taken as min between n. rows and n. columns
      No great memory savings.
    */
    RedSVD(const MatrixType& A)
    {
      int r = (A.rows() < A.cols()) ? A.rows() : A.cols();
      compute(A, r);
    }
    //! Specifying the rank
    /*!
      \param A a sparse or dense Eigen matrix
      \param rank the desired rank for the reduced decomposition
    */
    RedSVD(const MatrixType& A, const Index rank)
    {
      compute(A, rank);
    }
    /*!
      \brief Computes the actual reduced SVD
      It is automatically called by constructors that take a matrix in input.
    */
    void compute(const MatrixType& A, const Index rank)
    {
      if(A.cols() == 0 || A.rows() == 0)
        return;

      Index r = (rank < A.cols()) ? rank : A.cols();

      r = (r < A.rows()) ? r : A.rows();

      // Gaussian Random Matrix for A^T
      DenseMatrix O(A.rows(), r);
      sample_gaussian(O);

      // Compute Sample Matrix of A^T
      DenseMatrix Y = A.transpose() * O;

      // Orthonormalize Y
      gram_schmidt(Y);

      // Range(B) = Range(A^T)
      DenseMatrix B = A * Y;

      // Gaussian Random Matrix
      DenseMatrix P(B.cols(), r);
      sample_gaussian(P);

      // Compute Sample Matrix of B
      DenseMatrix Z = B * P;

      // Orthonormalize Z
      gram_schmidt(Z);

      // Range(C) = Range(B)
      DenseMatrix C = Z.transpose() * B;
      // Thin SVD.
      Eigen::JacobiSVD<DenseMatrix> svdOfC(C, Eigen::ComputeThinU | Eigen::ComputeThinV);

      // C = USV^T
      // A^* = (Z * U) * S * (Y * V)^T (low rank approx of A).
      m_matrixU = Z * svdOfC.matrixU();
      m_vectorS = svdOfC.singularValues();
      m_matrixV = Y * svdOfC.matrixV();
    }

    //! Returns matrix U
    DenseMatrix matrixU() const
    {
      return m_matrixU;
    }
    //! Returns vector with first k singular values
    ScalarVector singularValues() const
    {
      return m_vectorS;
    }
    //! Returns matrix V
    DenseMatrix matrixV() const
    {
      return m_matrixV;
    }

  private:
    DenseMatrix m_matrixU;
    ScalarVector m_vectorS;
    DenseMatrix m_matrixV;
  };

  //! Like RedSVD but for symmetric matrices
  /*!
   * In this case, \f$U=V\f$ is the matrix with the first (approximate) eigenvectors of \f$A\f$ and \f$\Sigma\f$ contains
   * the (approximate) first k eigenvalues.
   */
  template<typename _MatrixType>
  class RedSymEigen
  {
  public:
    typedef _MatrixType MatrixType;
    typedef typename MatrixType::Scalar Scalar;
    typedef typename MatrixType::Index Index;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> DenseMatrix;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ScalarVector;

    RedSymEigen()=default;

    RedSymEigen(const MatrixType& A)
    {
      int r = (A.rows() < A.cols()) ? A.rows() : A.cols();
      compute(A, r);
    }

    RedSymEigen(const MatrixType& A, const Index rank)
    {
      compute(A, rank);
    }

    void compute(const MatrixType& A, const Index rank)
    {
      if(A.cols() == 0 || A.rows() == 0)
        return;

      Index r = (rank < A.cols()) ? rank : A.cols();

      r = (r < A.rows()) ? r : A.rows();

      // Gaussian Random Matrix
      DenseMatrix O(A.rows(), r);
      sample_gaussian(O);

      // Compute Sample Matrix of A
      DenseMatrix Y = A.transpose() * O;

      // Orthonormalize Y
      gram_schmidt(Y);

      DenseMatrix B = Y.transpose() * A * Y;
      Eigen::SelfAdjointEigenSolver<DenseMatrix> eigenOfB(B);

      m_eigenvalues = eigenOfB.eigenvalues();
      m_eigenvectors = Y * eigenOfB.eigenvectors();
    }
    //! Returns a vector with the first k approximate eigenvalues
    /*!
      They are the diagonal elements of \f$\Sigma\f$.
     */
    ScalarVector eigenvalues() const
    {
      return m_eigenvalues;
    }
    //! Returns a nxk orthogonal matrix whose columns are the first k approximate eigenvectors
    DenseMatrix eigenvectors() const
    {
      return m_eigenvectors;
    }

  private:
    ScalarVector m_eigenvalues;
    DenseMatrix m_eigenvectors;
  };

  //! Performs the principal component analysis
  /*!
   * It works by contructing a RedSVD object to perform an low-rank SVD decomposition and then
   * it returns the nxk matrix \f$V\f$ that represent the components of the PCA and the nxk matrix
   * \f$U\Sigma\f$, which are the scores.
   */
  template<typename _MatrixType>
  class RedPCA
  {
  public:
    typedef _MatrixType MatrixType;
    typedef typename MatrixType::Scalar Scalar;
    typedef typename MatrixType::Index Index;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> DenseMatrix;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ScalarVector;

    RedPCA() {}

    RedPCA(const MatrixType& A)
    {
      int r = (A.rows() < A.cols()) ? A.rows() : A.cols();
      compute(A, r);
    }

    RedPCA(const MatrixType& A, const Index rank)
    {
      compute(A, rank);
    }

    void compute(const DenseMatrix& A, const Index rank)
    {
      RedSVD<MatrixType> redsvd(A, rank);

      ScalarVector S = redsvd.singularValues();

      m_components = redsvd.matrixV();
      m_scores = redsvd.matrixU() * S.asDiagonal();
    }
    //! Returns the components
    DenseMatrix components() const
    {
      return m_components;
    }
    //! Returns the scores
    DenseMatrix scores() const
    {
      return m_scores;
    }

  private:
    DenseMatrix m_components;
    DenseMatrix m_scores;
  };
}

#endif


#include "datatypes/heap.h"
#include "segmentation/isf.h"

#include <Eigen/Dense>

#include <queue>

using namespace segm;

ISF &ISF::operator=(const ISF &isf)
{
    ForestingTransform::operator=(isf);
    alpha = isf.getAlpha();
    beta = isf.getBeta();
    return (*this);
}

void ISF::run(int n_superpixels, int iterations)
{
    Image<int> markers(w, h);
    for (int ite = 0; ite < iterations; ite++)
    {
        if (ite == 0)
            markers = sample(n_superpixels);
        else
            markers = computeCentroids();

        ForestingTransform::run(markers);
    }
}

void ISF::conquer(int x, int y, int adj_x, int adj_y)
{
    if (valid(adj_x, adj_y) && !heap.is(adj_x, adj_y, heap.black))
    {
        int p = index(x, y);
        int q = index(adj_x, adj_y);
        float arc_weight = cost(p) + powf(alpha * l2norm(root(p), q), beta) + 1.0f;
        if (arc_weight < cost(q))
        {
            cost(q)  = arc_weight;
            root(q)  = root(p);
            pred(q)  = p;
            label(q) = label(p);

            if (heap.is(q, heap.gray))
                heap.goUp(adj_x, adj_y);
            else
                heap.insert(q);
        }
    }
}

Image<int> ISF::sample(int sample_size)
{
    auto step_x = static_cast<int>(w / round(sqrt(sample_size)));
    auto step_y = static_cast<int>(h / round(sqrt(sample_size)));

    if (step_x < 1.0f || step_y < 1.0f)
        std::runtime_error("Sample size is too big., ISF::gridSample");

    Image<int> samples(w, h);
    samples.fill(-1);

    int label = 0;
    for (int x = step_x / 2; x < w; x += step_x) {
        for (int y = step_y / 2; y < h; y += step_y) {
            samples(x, y) = label;
            label++;
        }
    }

    return samples;
}

Image<int> ISF::computeCentroids()
{
    int n_sup = label.max() + 1;

    /* 0 = x-axis, 1 = y-axis, 2 = count */
    Eigen::MatrixXi centroids = Eigen::MatrixXi::Constant(n_sup, 3, 0);

    for (int y = 0; y < h; y++) {
        for (int x = 0; x < w; x++) {
            int lb = label(x, y);
            centroids(lb, 0) += x;
            centroids(lb, 1) += y;
            centroids(lb, 2) += 1;
        }
    }

    Image<int> samples(w, h);
    samples.fill(-1);
    for (int lb = 0; lb < centroids.rows(); lb++)
    {
        int x = centroids(lb, 0) /= centroids(lb, 2);
        int y = centroids(lb, 1) /= centroids(lb, 2);
        if (label(x, y) == lb)
            samples(x, y) = lb;
        else {
            Pixel p = findNearest(x, y, lb);
            samples(p.x, p.y) = lb;
        }
    }

    return samples;
}

ISF::Pixel ISF::findNearest(int x, int y, int _label)
{
    Pixel p(x, y);
    std::queue<Pixel> Q;
    while (label(p.x, p.y) != _label)
    {
        if (valid(p.x + 1, p.y))
            Q.push(Pixel(p.x + 1, p.y));

        if (valid(p.x, p.y + 1))
            Q.push(Pixel(p.x, p.y + 1 ));

        if (valid(p.x - 1, p.y))
            Q.push(Pixel(p.x - 1, p.y));

        if (valid(p.x, p.y - 1))
            Q.push(Pixel(p.x, p.y - 1));

        p = Q.front();
        Q.pop();
    }

    return p;
}


#pragma once

#include <cmath>
#include <vector>
#include <Eigen/Dense>

/* NOTE: The following field layout:
   +-------+ (fW/2,fH/2) <- Enemy goal keeper
   |       |
   | (0,0) |
   +---+---+ (fw/2,0)
   |       |
   |       |
   +-------+ (fw/2,-fH/2) <- Our goal keeper */

    static const int NUM_OF_IMU_DATA = 6;
    static const double FIRST_TIME = 0.01;
    static const double DT = 0.01;
    static const double GRAV_ACC = 9.80665;
    static const double TO_DEG = 57.2958;
    static const double IMU_LPF = 0.07;
    static const std::vector<double> coordinateModifyFactor{ 1.0, -1.0, 1.0, -1.0, 1.0, -1.0 };
    static const std::vector<double> BIAS{ 0, 0, 0, 0, 0, 0 };

    static const std::vector<double> INIT_P{ 0.0001, 0.0001, 0.0001, 0.0001 };
    static const std::vector<double> INIT_Q{ 0.01, 0.01, 0.01, 0.01 };
    static const std::vector<double> INIT_R{ 1.0, 1.0, 10.0 };
    static const std::vector<double> INIT_X{ 0.0, 0.0, 0.0, 1.0 };
    static const int SIZE_VECTOR_X = 4;
    static const int SIZE_VECTOR_Y = 3;

/**
 * localization.hh
 *
 * Allow the player to update and track localization related information.
 **/
class Localization{
  private:
    Eigen::Matrix<double, SIZE_VECTOR_X, 1> xHat;
    Eigen::Matrix<double, SIZE_VECTOR_X, 1> xHatMinus;
    Eigen::Matrix<double, SIZE_VECTOR_Y, 1> y;
    Eigen::Matrix<double, SIZE_VECTOR_X, 1> f;
    Eigen::Matrix<double, SIZE_VECTOR_Y, 1> h;
    Eigen::Matrix<double, SIZE_VECTOR_X, SIZE_VECTOR_X> A;
    Eigen::Matrix<double, SIZE_VECTOR_X, SIZE_VECTOR_X> AT;
    Eigen::Matrix<double, SIZE_VECTOR_X, SIZE_VECTOR_Y> C;
    Eigen::Matrix<double, SIZE_VECTOR_Y, SIZE_VECTOR_X> CT;
    Eigen::Matrix<double, SIZE_VECTOR_X, SIZE_VECTOR_Y> g;
    Eigen::Matrix<double, SIZE_VECTOR_X, SIZE_VECTOR_X> P;
    Eigen::Matrix<double, SIZE_VECTOR_X, SIZE_VECTOR_X> PMinus;
    Eigen::Matrix<double, SIZE_VECTOR_X, SIZE_VECTOR_X> Q;
    Eigen::Matrix<double, SIZE_VECTOR_Y, SIZE_VECTOR_Y> R;
    Eigen::Matrix<double, SIZE_VECTOR_X, SIZE_VECTOR_X> I;
    std::vector<double> lpf;

  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    /**
     * Localization()
     *
     * Initialize the localization system.
     **/
    Localization();

    /**
     * ~Localization()
     *
     * De-initialize the localization system and free up resources.
     **/
    ~Localization();

    void update();

  private:
    void prediction(std::vector<double>& imu);

    void filter(std::vector<double>& imu);

    void normXHat();

    static Eigen::Matrix<double, 3, 3> getDCM(Eigen::Matrix<double, SIZE_VECTOR_X, 1>& x);

    static Eigen::Matrix<double, 3, 1> getEuler(Eigen::Matrix<double, 3, 3>& dcm);

    static Eigen::Matrix<double, 3, 1> getEulerFromAccData(std::vector<double>& imu);
};


#ifndef LIBRARY_INCLUDE_H
#define LIBRARY_INCLUDE_H
/// \file
/// \brief Include libraries that are commonly used in the package

// ROS
#include "ros/ros.h"

// Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

// Sophus
#include "sophus/se3.hpp"
#include "sophus/so3.hpp"

typedef Sophus::SE3d SE3;
typedef Sophus::SO3d SO3;

// OpenCV
#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>

#endif


/* =========================================================================
   Copyright (c) 2012-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
                             -----------------
               ViennaFEM - The Vienna Finite Element Method Library
                             -----------------

   Author:     Karl Rupp                          rupp@iue.tuwien.ac.at

   License:    MIT (X11), see file LICENSE in the ViennaFEM base directory
============================================================================ */

// include necessary system headers
#include <iostream>

// ViennaFEM includes:
#include "viennafem/fem.hpp"
#include "viennafem/io/vtk_writer.hpp"

// ViennaGrid includes:
#include "viennagrid/forwards.hpp"
#include "viennagrid/config/default_configs.hpp"
#include "viennagrid/io/netgen_reader.hpp"

// ViennaData includes:
#include "viennadata/api.hpp"

// ViennaMath includes:
#include "viennamath/expression.hpp"

// Boost.uBLAS includes:
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

//ViennaCL includes:
#ifndef VIENNACL_HAVE_UBLAS
 #define VIENNACL_HAVE_UBLAS
#endif

#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/prod.hpp"

/** @brief A tag class used for storing the permittivity (i.e. a cell quantity) with ViennaData */
struct permittivity_key
{
  // Operator< is required for compatibility with std::map
  bool operator<(permittivity_key const & /*other*/) const { return false; }
};

int main()
{
  typedef viennagrid::triangular_2d_mesh                                                  DomainType;
  typedef viennagrid::result_of::segmentation<DomainType>::type                           SegmentationType;
  typedef SegmentationType::iterator                                                      SegmentationIteratorType;
  typedef viennagrid::result_of::segment_handle<SegmentationType>::type                   SegmentType;
  typedef viennagrid::result_of::cell_tag<DomainType>::type                               CellTagType;
  typedef viennagrid::result_of::element<DomainType, viennagrid::vertex_tag>::type        VertexType;
  typedef viennagrid::result_of::element<DomainType, CellTagType>::type                   CellType;

  typedef viennagrid::result_of::element_range<DomainType, viennagrid::vertex_tag>::type  VertexContainerType;
  typedef viennagrid::result_of::iterator<VertexContainerType>::type                      VertexIteratorType;
  typedef viennagrid::result_of::element_range<SegmentType, CellTagType>::type            CellOnSegmentContainerType;
  typedef viennagrid::result_of::iterator<CellOnSegmentContainerType>::type               CellOnSegmentIteratorType;

  typedef boost::numeric::ublas::compressed_matrix<viennafem::numeric_type>  MatrixType;
  typedef boost::numeric::ublas::vector<viennafem::numeric_type>             VectorType;

  typedef viennamath::function_symbol   FunctionSymbol;
  typedef viennamath::equation          Equation;

  //
  // Create a domain from file
  //
  DomainType my_domain;
  SegmentationType segments(my_domain);

  //
  // Create a storage object
  //
  typedef viennadata::storage<> StorageType;
  StorageType   storage;

  try
  {
    viennagrid::io::netgen_reader my_netgen_reader;
    my_netgen_reader(my_domain, segments, "../examples/data/square224.mesh");
  }
  catch (...)
  {
    std::cerr << "File-Reader failed. Aborting program..." << std::endl;
    return EXIT_FAILURE;
  }

  //
  // Specify Poisson equation with inhomogeneous permittivity:
  //
  FunctionSymbol u(0, viennamath::unknown_tag<>());   //an unknown function used for PDE specification
  FunctionSymbol v(0, viennamath::test_tag<>());   //an unknown function used for PDE specification
  viennafem::cell_quan<CellType, viennamath::expr::interface_type>  permittivity; permittivity.wrap_constant( storage, permittivity_key() );

  //the strong form (not yet functional because of ViennaMath limitations)
  //Equation poisson_equ = viennamath::make_equation( viennamath::div(permittivity * viennamath::grad(u)), 0);

  //the weak form:
  Equation poisson_equ = viennamath::make_equation(
                          viennamath::integral(viennamath::symbolic_interval(),
                                               permittivity * (viennamath::grad(u) * viennamath::grad(v)) ),
                          0);

  MatrixType system_matrix;
  VectorType load_vector;

  //
  // Setting boundary information on domain (this should come from device specification)
  //
  //setting some boundary flags:
  VertexContainerType vertices = viennagrid::elements<VertexType>(my_domain);
  for (VertexIteratorType vit = vertices.begin();
      vit != vertices.end();
      ++vit)
  {
    // Boundary condition: 0 at left boundary, 1 at right boundary
    if ( viennagrid::point(my_domain, *vit)[0] == 0.0)
      viennafem::set_dirichlet_boundary(storage, *vit, 0.0);
    else if ( viennagrid::point(my_domain, *vit)[0] == 1.0)
      viennafem::set_dirichlet_boundary(storage, *vit, 1.0);

  }

  //
  // Create PDE solver functors: (discussion about proper interface required)
  //
  viennafem::pde_assembler<StorageType> fem_assembler(storage);

  //
  // Solve system and write solution vector to pde_result:
  // (discussion about proper interface required. Introduce a pde_result class?)
  //
  std::size_t si = 0;
  for(SegmentationIteratorType sit = segments.begin(); sit != segments.end(); sit++)
  {
    //set permittivity:
    CellOnSegmentContainerType cells = viennagrid::elements<CellType>(*sit);
    for (CellOnSegmentIteratorType cit  = cells.begin();
                                   cit != cells.end();
                                 ++cit)
    {
      if (si == 0) //Si
        viennadata::access<permittivity_key, double>(storage, permittivity_key(), *cit) = 3.9;
      else //SiO2
        viennadata::access<permittivity_key, double>(storage, permittivity_key(), *cit) = 11.9;
    }

    fem_assembler(viennafem::make_linear_pde_system(poisson_equ,
                                                    u,
                                                    viennafem::make_linear_pde_options(0,
                                                                                viennafem::lagrange_tag<1>(),
                                                                                viennafem::lagrange_tag<1>())
                                                  ),
                  *sit,
                  system_matrix,
                  load_vector
                );
  }

  VectorType pde_result = viennacl::linalg::solve(system_matrix, load_vector, viennacl::linalg::cg_tag());
  std::cout << "* solve(): Residual: " << norm_2(prod(system_matrix, pde_result) - load_vector) << std::endl;

  //
  // Writing solution back to domain (discussion about proper way of returning a solution required...)
  //
  viennafem::io::write_solution_to_VTK_file(pde_result, "poisson_cellquan_2d", my_domain, segments, storage, 0);

  std::cout << "*****************************************" << std::endl;
  std::cout << "* Poisson solver finished successfully! *" << std::endl;
  std::cout << "*****************************************" << std::endl;
  return EXIT_SUCCESS;
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Siargey Kachanovich
 *
 *    Copyright (C) 2019 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "manifold_tracing"
#include <boost/test/unit_test.hpp>
#include <gudhi/Unitary_tests_utils.h>

#include <iostream>

#include <gudhi/Coxeter_triangulation.h>
#include <gudhi/Functions/Function_Sm_in_Rd.h>
#include <gudhi/Implicit_manifold_intersection_oracle.h>
#include <gudhi/Manifold_tracing.h>

using namespace Gudhi::coxeter_triangulation;

BOOST_AUTO_TEST_CASE(manifold_tracing) {
  // manifold without boundary
  Function_Sm_in_Rd fun_sph(5.1111, 2);
  auto oracle = make_oracle(fun_sph);
  Coxeter_triangulation<> cox_tr(oracle.amb_d());
  // cox_tr.change_offset(Eigen::VectorXd::Random(oracle.amb_d()));

  using MT = Manifold_tracing<Coxeter_triangulation<> >;
  Eigen::VectorXd seed = fun_sph.seed();
  std::vector<Eigen::VectorXd> seed_points(1, seed);
  typename MT::Out_simplex_map out_simplex_map;
  manifold_tracing_algorithm(seed_points, cox_tr, oracle, out_simplex_map);

  for (auto si_pair : out_simplex_map) {
    BOOST_CHECK(si_pair.first.dimension() == oracle.function().cod_d());
    BOOST_CHECK(si_pair.second.size() == (long int)oracle.function().amb_d());
  }
  std::clog << "out_simplex_map.size() = " << out_simplex_map.size() << "\n";
  BOOST_CHECK(out_simplex_map.size() == 1118);

  // manifold with boundary
  Function_Sm_in_Rd fun_boundary(3.0, 2, fun_sph.seed());
  auto oracle_with_boundary = make_oracle(fun_sph, fun_boundary);
  typename MT::Out_simplex_map interior_simplex_map, boundary_simplex_map;
  manifold_tracing_algorithm(seed_points, cox_tr, oracle_with_boundary, interior_simplex_map, boundary_simplex_map);
  for (auto si_pair : interior_simplex_map) {
    BOOST_CHECK(si_pair.first.dimension() == oracle.function().cod_d());
    BOOST_CHECK(si_pair.second.size() == (long int)oracle.function().amb_d());
  }
  std::clog << "interior_simplex_map.size() = " << interior_simplex_map.size() << "\n";
  BOOST_CHECK(interior_simplex_map.size() == 96);
  for (auto si_pair : boundary_simplex_map) {
    BOOST_CHECK(si_pair.first.dimension() == oracle.function().cod_d() + 1);
    BOOST_CHECK(si_pair.second.size() == (long int)oracle.function().amb_d());
  }
  std::clog << "boundary_simplex_map.size() = " << boundary_simplex_map.size() << "\n";
  BOOST_CHECK(boundary_simplex_map.size() == 54);
}


#include <celero/Celero.h>

#include <Eigen/Core>
#include <cpzlib.hh>
#include <random>

CELERO_MAIN

// TODO: Add test setup for matrices that definitely need regularization
class BasicFixture : public celero::TestFixture {
 public:
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> generators;
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> center;
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> exponents;
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> constraints;
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> constraint_generators;
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> constraint_exponents;
  void setUp(const celero::TestFixture::ExperimentValue&) override {
    std::mt19937 gen(0);
    std::uniform_int_distribution<> dist(1, 50);
    const int dim             = dist(gen);
    const int num_gens        = dist(gen);
    const int num_coeffs      = dist(gen);
    const int num_constrs     = dist(gen);
    const int num_constr_gens = dist(gen);

    center     = Eigen::Matrix<float, Eigen::Dynamic, 1>::Random(dim);
    generators = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>::Random(dim, num_gens);
    exponents = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>::Random(num_coeffs, num_gens);
    constraints = Eigen::Matrix<float, Eigen::Dynamic, 1>::Random(num_constrs);
    constraint_generators =
    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>::Random(num_constrs, num_constr_gens);
    constraint_exponents =
    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>::Random(num_coeffs, num_constr_gens);
  }
};

BASELINE_F(Regularization, Baseline, BasicFixture, 1000, 0) {
  cpz::ConstrainedPolynomialZonotope<> z(
  center, generators, exponents, constraints, constraint_generators, constraint_exponents);
}


#include <functional>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseLU>
#include <iostream>
#include <fstream>
#include <assert.h>
#include <stdio.h>

#include "tree.h"
#include "util.h"
#include "mmio.hpp"

using namespace Eigen;
using namespace std;

int main(int argc, char* argv[]) {
  vector<int> dims  = {2,  2,  2,   3, 3,  3 };
  vector<int> sizes = {5,  16, 64,  5, 10, 15};
  int ntests = dims.size();
  if (argc > 1) {
      ntests = atoi(argv[1]);
      printf("Running max %d different tests problems\n", ntests);
  }
  matrix_hash<VectorXd> hash;
  vector<size_t> allhashes;
  for(int test = 0; test < ntests; test++) {
    int s = sizes[test];
    int d = dims[test];
    stringstream ss;
    ss << "../mats/neglapl_" << d << "_" << s << ".mm";
    string file = ss.str();
    cout << file << endl;
    SpMat Asymm = mmio::sp_mmread<double,int>(file);
    default_random_engine gen;
    uniform_real_distribution<double> dist(0.5,1.0);
    double a = dist(gen);
    SpMat Aunsymm = a * Asymm.triangularView<Lower>() + (1 - a) * Asymm.triangularView<Upper>();
    SpMat* A = nullptr;
    int N = Asymm.rows();
    int nlevelsmin = N < 1000 ? 1 : 8;
    for(int nlevels = nlevelsmin; nlevels < nlevelsmin+3; nlevels++) {
      vector<double> tols = {0.0, 1e-2, 1.0, 10.0};
      for(double tol : tols) {
        for(int skip = 0; skip < 3; skip++) {
          for(int pres = 0; pres < 2; pres++) {
            for(int symm = 0; symm < 2; symm++) {
              for(int geo = 0; geo < 2; geo++) {
                for(int scale = 0; scale < 2; scale++) {
                  for(int ortho = 0; ortho < 2; ortho++) {
                    // Only valid combinations
                    if(ortho && (!scale))   continue;
                    if(pres  && ((!ortho) || (!symm)) ) continue;
                    Tree t = Tree(nlevels);
                    t.set_verb(false);
                    t.set_tol(tol);
                    t.set_skip(skip);
                    t.set_scale(scale);
                    t.set_ortho(ortho);
                    t.set_preserve(pres);
                    t.set_use_geo(geo);
                    t.set_symmetry(symm);
                    MatrixXd phi = MatrixXd::Ones(N,1);
                    MatrixXd X = linspace_nd(s, d);
                    t.set_phi(&phi);
                    t.set_Xcoo(&X);
                    if(symm) {
                        A = &Asymm;
                    } else {
                        A = &Aunsymm;
                    }
                    t.partition(*A);
                    t.assemble(*A);
                    int errors = t.factorize();
                    assert(errors == 0);
                    VectorXd b = VectorXd::Random(N);
                    auto x = b;
                    t.solve(x);
                    double res = ((*A)*x-b).norm() / b.norm();
                    SparseLU<SpMat> lu((*A));
                    VectorXd xref;
                    xref = lu.solve(b);
                    double err = (xref - x).norm() / xref.norm();
                    auto h = hash(x);
                    allhashes.push_back(h);
                    printf("%6d %4d %d] %3d %3.2e %2d %d %d %d %d %d %3.2e %3.2e | %lu\n", N, s, d, nlevels, tol, skip, pres, symm, geo, scale, ortho, res, err, hash(x));
                  }
                }
              }
            }
          }
        }
      }
    }
  }
  size_t h = hashv(allhashes);
  cout << "Overall hash: " << h << endl;
  ofstream f;
  f.open("allhashes.log");
  for(auto v : allhashes) {
    f << v << "\n";
  }
  f.close();
  return 1;
}


/**
 * Copyright (c) 2015 Carnegie Mellon University, Daniel Maturana <dimatura@cmu.edu>
 *
 * For License information please see the LICENSE file in the root directory.
 *
 */

#ifndef SCROLLGRID2_HPP_YPBBYE5Q
#define SCROLLGRID2_HPP_YPBBYE5Q

#include <math.h>
#include <stdint.h>

#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>

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

#include <pcl_util/point_types.hpp>
#include <geom_cast/geom_cast.hpp>

#include "scrollgrid/mod_wrap.hpp"
#include "scrollgrid/grid_types.hpp"
#include "scrollgrid/box.hpp"

namespace ca
{

template<class Scalar>
class ScrollGrid2 {
public:
  typedef Eigen::Matrix<Scalar, 2, 1> Vec2;

  typedef boost::shared_ptr<ScrollGrid2> Ptr;
  typedef boost::shared_ptr<const ScrollGrid2> ConstPtr;

public:
  ScrollGrid2() :
      box_(),
      origin_(0, 0),
      min_world_corner_ij_(0, 0),
      dimension_(0, 0),
      num_cells_(0),
      strides_(0, 0),
      scroll_offset_(0, 0),
      last_ij_(0, 0),
      wrap_ij_min_(0, 0),
      wrap_ij_max_(0, 0),
      resolution_(0)
  { }

  ScrollGrid2(const Vec2& center,
              const Vec2Ix& dimension,
              Scalar resolution,
              bool x_fastest=false) :
      box_(center-(dimension.cast<Scalar>()*resolution)/2,
           center+(dimension.cast<Scalar>()*resolution)/2),
      origin_(center-box_.radius()),
      dimension_(dimension),
      num_cells_(dimension.prod()),
      strides_(dimension[1], 1),
      scroll_offset_(0, 0, 0),
      last_ij_(scroll_offset_ + dimension_),
      resolution_(resolution)
  {

    Vec2 m;
    m[0] = -static_cast<Scalar>(std::numeric_limits<uint16_t>::max()/2)*resolution_;
    m[1] = -static_cast<Scalar>(std::numeric_limits<uint16_t>::max()/2)*resolution_;
    min_world_corner_ij_ = this->world_to_grid(m);

    if (x_fastest) {
      strides_ = Vec2Ix(1, dimension[0]);
    }

    this->update_wrap_ij();
  }

  virtual ~ScrollGrid2() { }

  ScrollGrid2(const ScrollGrid2& other) :
      box_(other.box_),
      origin_(other.origin_),
      min_world_corner_ij_(other.min_world_corner_ij_),
      dimension_(other.dimension_),
      num_cells_(other.num_cells_),
      strides_(other.strides_),
      scroll_offset_(other.scroll_offset_),
      last_ij_(other.last_ij_),
      wrap_ij_min_(other.wrap_ij_min_),
      wrap_ij_max_(other.wrap_ij_max_),
      resolution_(other.resolution_)
  {
  }

  ScrollGrid2& operator=(const ScrollGrid2& other) {
    if (*this==other) { return *this; }
    box_ = other.box_;
    origin_ = other.origin_;
    min_world_corner_ij_ = other.min_world_corner_ij_;
    dimension_ = other.dimension_;
    num_cells_ = other.num_cells_;
    strides_ = other.strides_;
    scroll_offset_ = other.scroll_offset_;
    last_ij_ = other.last_ij_;
    wrap_ij_min_ = other.wrap_ij_min_;
    wrap_ij_max_ = other.wrap_ij_max_;
    resolution_ = other.resolution_;
    return *this;
  }

public:

  void reset(const Vec2& center,
             const Vec2Ix& dimension,
             Scalar resolution,
             bool x_fastest=false) {
    box_.set_center(center);
    box_.set_radius((dimension.cast<Scalar>()*resolution)/2);
    origin_ = center - box_.radius();

    dimension_ = dimension;
    num_cells_ = dimension.prod();
    if (x_fastest) {
      strides_ = Vec2Ix(1, dimension[0]);
    } else {
      strides_ = Vec2Ix(dimension[1], 1);
    }
    scroll_offset_.setZero();
    last_ij_ = scroll_offset_ + dimension_;

    this->update_wrap_ij();

    resolution_ = resolution;

    Vec2 m;
    m[0] = -static_cast<Scalar>(std::numeric_limits<uint16_t>::max()/2)*resolution_;
    m[1] = -static_cast<Scalar>(std::numeric_limits<uint16_t>::max()/2)*resolution_;
    min_world_corner_ij_ = this->world_to_grid(m);

  }

  /**
   * Is inside 3D box containing grid?
   * @param pt point in same frame as center (probably world_view)
   */
  bool is_inside_box(const Vec2& pt) const {
    return box_.contains(pt);
  }

  template<class PointT>
  bool is_inside_box(const PointT& pt) const {
    return box_.contains(ca::point_cast<Vec2>(pt));
  }

  /**
   * is i, j, k inside the grid limits?
   */
  bool is_inside_grid(const Vec2Ix& grid_ix) const {
    return ((grid_ix.array() >= scroll_offset_.array()).all() &&
            (grid_ix.array() < (scroll_offset_+dimension_).array()).all());
  }

  bool is_inside_grid(grid_ix_t i, grid_ix_t j) const {
    return this->is_inside_grid(Vec2Ix(i, j));
  }

  /**
   * scroll grid.
   * updates bounding box and offset_cells.
   * @param offset_cells. how much to scroll. offset_cells is a signed integral.
   *
   */
  void scroll(const Vec2Ix& offset_cells) {
    Vec2Ix new_offset = scroll_offset_ + offset_cells;
    box_.translate((offset_cells.cast<Scalar>()*resolution_));
    scroll_offset_ = new_offset;
    last_ij_ = scroll_offset_ + dimension_;

    this->update_wrap_ij();
  }

  /**
   * get boxes to clear if scrolling by offset_cells.
   * call this *before* scroll().
   * @param clear_i_min min corner of obsolete region in grid
   * @param clear_i_max max corner of obsolete region in grid
   * same for j and k
   * Note that boxes may overlap.
   */
  void get_clear_boxes(const Vec2Ix& offset_cells,
                       Vec2Ix& clear_i_min, Vec2Ix& clear_i_max,
                       Vec2Ix& clear_j_min, Vec2Ix& clear_j_max) {

    Vec2Ix new_offset = scroll_offset_ + offset_cells;

    clear_i_min.setZero();
    clear_j_min.setZero();
    clear_i_max.setZero();
    clear_j_max.setZero();

    // X axis
    if (offset_cells[0] > 0) {
      clear_i_min = scroll_offset_;
      clear_i_max = Vec2Ix(new_offset[0],
                           scroll_offset_[1]+dimension_[1]);
    } else if (offset_cells[0] < 0) {
      clear_i_min = Vec2Ix(scroll_offset_[0]+dimension_[0]+offset_cells[0],
                           scroll_offset_[1]);
      clear_i_max = scroll_offset_ + dimension_;
    }

    // Y axis
    if (offset_cells[1] > 0) {
      clear_j_min = scroll_offset_;
      clear_j_max = Vec2Ix(scroll_offset_[0]+dimension_[0],
                           new_offset[1]);
    } else if (offset_cells[1] < 0) {
      clear_j_min = Vec2Ix(scroll_offset_[0],
                           scroll_offset_[1]+dimension_[1]+offset_cells[1]);
      clear_j_max = scroll_offset_ + dimension_;
    }

  }

  /**
   * Given position in world coordinates, return grid coordinates.
   * (grid coordinates are not wrapped to be inside grid!)
   * Note: does not check if point is inside grid.
   */
  Vec2Ix world_to_grid(const Vec2& xy) const {
    Vec2 tmp = ((xy - origin_).array() - 0.5*resolution_)/resolution_;
    //ROS_INFO_STREAM("tmp = " << tmp);
    //return tmp.cast<grid_ix_t>();
    return Vec2Ix(round(tmp.x()), round(tmp.y()));
  }

  Vec2Ix world_to_grid(Scalar x, Scalar y) const {
    return this->world_to_grid(Vec2(x, y));
  }

  Vec2 grid_to_world(const Vec2Ix& grid_ix) const {
    Vec2 w((grid_ix.cast<Scalar>()*resolution_ + origin_).array() + 0.5*resolution_);
    return w;
  }

  Vec2 grid_to_world(grid_ix_t i, grid_ix_t j) const {
    return this->grid_to_world(Vec2Ix(i, j));
  }

  /**
   * Translate grid indices to an address in linear memory.
   * Does not check if grid_ix is inside current grid box.
   * Assumes C-order, x the slowest and z the fastest.
   */
  mem_ix_t grid_to_mem(const Vec2Ix& grid_ix) const {
    Vec2Ix grid_ix2(ca::mod_wrap(grid_ix[0], dimension_[0]),
                    ca::mod_wrap(grid_ix[1], dimension_[1]));
    return strides_.dot(grid_ix2);
  }

  /**
   * This is faster than grid_to_mem, as it avoids modulo.
   * But it only works if the grid_ix are inside the bounding box.
   * Hopefully branch prediction kicks in
   */
  mem_ix_t grid_to_mem2(const Vec2Ix& grid_ix) const {
    Vec2Ix grid_ix2(grid_ix);
    if (grid_ix2[0] >= wrap_ij_max_[0]) { grid_ix2[0] -= wrap_ij_max_[0]; } else { grid_ix2[0] -= wrap_ij_min_[0]; }
    if (grid_ix2[1] >= wrap_ij_max_[1]) { grid_ix2[1] -= wrap_ij_max_[1]; } else { grid_ix2[1] -= wrap_ij_min_[1]; }
    mem_ix_t mem_ix2 = strides_.dot(grid_ix2);
    return mem_ix2;
  }

  mem_ix_t grid_to_mem(grid_ix_t i, grid_ix_t j) const {
    return this->grid_to_mem(Vec2Ix(i, j));
  }

  mem_ix_t grid_to_mem2(grid_ix_t i, grid_ix_t j) const {
    return grid_to_mem2(Vec2Ix(i, j));
  }

  uint64_t grid_to_hash(const Vec2Ix& grid_ix) const {
    // grid2 should be all positive
    Vec2Ix grid2(grid_ix - min_world_corner_ij_);
    uint64_t hi = static_cast<uint64_t>(grid2[0]);
    uint64_t hj = static_cast<uint64_t>(grid2[1]);
    uint64_t h = (hi << 48) | (hj << 32);
    return h;
  }

  Vec2Ix hash_to_grid(uint64_t hix) const {
    uint64_t hi = (hix & 0xffff000000000000) >> 48;
    uint64_t hj = (hix & 0x0000ffff00000000) >> 32;
    Vec2Ix grid_ix(hi, hj);
    grid_ix += min_world_corner_ij_;
    return grid_ix;
  }

  /**
   * Note that no bound check is performed!
   */
  mem_ix_t world_to_mem(const Vec2& xy) const {
    Vec2 tmp(((xy - origin_).array() - 0.5*resolution_)/resolution_);
    Vec2Ix gix(round(tmp.x()), round(tmp.y()), round(tmp.z()));
    ca::inplace_mod_wrap(gix[0], dimension_[0]);
    ca::inplace_mod_wrap(gix[1], dimension_[1]);
    return strides_.dot(gix);
  }

  mem_ix_t world_to_mem2(const Vec2& xy) const {
    Vec2Ix gix(this->world_to_grid(xy));
    return this->world_to_mem2(gix);
  }

  Vec2Ix mem_to_grid(grid_ix_t mem_ix) const {
    // TODO does this work for x-fastest strides?
    grid_ix_t i = mem_ix/strides_[0];
    mem_ix -= i*strides_[0];
    grid_ix_t j = mem_ix/strides_[1];
    mem_ix -= j*strides_[1];

    // undo wrapping
    grid_ix_t ax = floor(static_cast<Scalar>(scroll_offset_[0])/dimension_[0])*dimension_[0];
    grid_ix_t ay = floor(static_cast<Scalar>(scroll_offset_[1])/dimension_[1])*dimension_[1];

    Vec2Ix fixed_ij;
    fixed_ij[0] = i + ax + (i<(scroll_offset_[0]-ax))*dimension_[0];
    fixed_ij[1] = j + ay + (j<(scroll_offset_[1]-ay))*dimension_[1];

    return fixed_ij;
  }

 public:

  grid_ix_t dim_i() const { return dimension_[0]; }
  grid_ix_t dim_j() const { return dimension_[1]; }
  grid_ix_t first_i() const { return scroll_offset_[0]; }
  grid_ix_t first_j() const { return scroll_offset_[1]; }
  grid_ix_t last_i() const { return last_ij_[0]; }
  grid_ix_t last_j() const { return last_ij_[1]; }
  const Vec2Ix& dimension() const { return dimension_; }
  const Vec2& radius() const { return box_.radius(); }
  const Vec2& origin() const { return origin_; }
  Vec2 min_pt() const { return box_.min_pt(); }
  Vec2 max_pt() const { return box_.max_pt(); }
  const Vec2& center() const { return box_.center(); }
  Scalar resolution() const { return resolution_; }
  const ca::scrollgrid::Box<Scalar, 2>& box() const { return box_; }
  grid_ix_t num_cells() const { return num_cells_; }
  Vec2Ix scroll_offset() const { return scroll_offset_; }

 private:

  void update_wrap_ij() {
    wrap_ij_min_[0] = floor(static_cast<float>(scroll_offset_[0])/dimension_[0])*dimension_[0];
    wrap_ij_min_[1] = floor(static_cast<float>(scroll_offset_[1])/dimension_[1])*dimension_[1];

    wrap_ij_max_[0] = floor(static_cast<float>(scroll_offset_[0]+dimension_[0])/dimension_[0])*dimension_[0];
    wrap_ij_max_[1] = floor(static_cast<float>(scroll_offset_[1]+dimension_[1])/dimension_[1])*dimension_[1];
  }

 private:
  // 2d box enclosing grid. In whatever coordinates were given (probably
  // world_view)
  ca::scrollgrid::Box<Scalar, 2> box_;

  // static origin of the grid coordinate system. does not move when scrolling
  // it's center - box.radius
  Vec2 origin_;

  // minimum world corner in ij. used for hash
  Vec2Ix min_world_corner_ij_;

  // number of grid cells along each axis
  Vec2Ix dimension_;

  // number of cells
  grid_ix_t num_cells_;

  // grid strides to translate from linear to 3D layout.
  // C-ordering, ie x slowest, z fastest.
  Vec2Ix strides_;

  // to keep track of scrolling along z.
  Vec2Ix scroll_offset_;

  // redundant but actually seems to have a performance benefit
  // should always be dimension + offset
  Vec2Ix last_ij_;

  // for grid_to_mem2. the points where the grid crosses modulo boundaries.
  Vec2Ix wrap_ij_min_;
  Vec2Ix wrap_ij_max_;

  // size of grid cells
  Scalar resolution_;

};

typedef ScrollGrid2<float> ScrollGrid2f;
typedef ScrollGrid2<double> ScrollGrid2d;

} /* ca */

#endif /* end of include guard: SCROLLGRID2_HPP_YPBBYE5Q */


#pragma once
#include <csapex/model/node.h>

#include <csapex_opencv/roi.h>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <opencv2/opencv.hpp>
#include <sstream>

namespace csapex
{
namespace vision
{
class ROISizeStatistics : public csapex::Node
{
    template <typename T>
    using Accumulator = boost::accumulators::accumulator_set<
        T, boost::accumulators::stats<boost::accumulators::tag::min, boost::accumulators::tag::max, boost::accumulators::tag::mean, boost::accumulators::tag::variance>>;

    template <typename T>
    static std::string formatAccumulator(const Accumulator<T>& accu)
    {
        std::ostringstream os;
        os << boost::accumulators::mean(accu);
        os << " (std.dev.: " << std::sqrt(boost::accumulators::variance(accu)) << ")";
        os << " (min: " << boost::accumulators::min(accu);
        os << ", max: " << boost::accumulators::max(accu) << ")";
        return os.str();
    }

    struct Statistics
    {
        void reset();
        void update(const Roi& roi);
        std::string format() const;

        std::size_t count;
        Accumulator<int> height;
        Accumulator<int> width;
        Accumulator<int> area;
        Accumulator<float> ratio;
    };

    enum class BinType
    {
        HEIGHT,
        WIDTH,
        AREA
    };

public:
    ROISizeStatistics();
    void setupParameters(csapex::Parameterizable& parameters) override;
    void setup(csapex::NodeModifier& node_modifier) override;
    void process() override;

private:
    void resetStats();
    void saveStats();
    void updateStats(const Roi& roi);
    std::string formatStats() const;

private:
    Input* in_rois_;
    Output* out_info_;
    Output* out_histogram_;

    int max_width_;
    int max_height_;
    int histogram_bin_size_;
    std::string output_path_;
    int bin_count_;
    BinType bin_type_;

    Statistics global_stats_;
    cv::Mat global_histogram_;
    std::vector<Statistics> bin_stats_;
};

}  // namespace vision
}  // namespace csapex


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

#ifndef BOOST_MATH_SF_BINOMIAL_HPP
#define BOOST_MATH_SF_BINOMIAL_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/policies/error_handling.hpp>

namespace boost{ namespace math{

template <class T, class Policy>
T binomial_coefficient(unsigned n, unsigned k, const Policy& pol)
{
   BOOST_STATIC_ASSERT(!boost::is_integral<T>::value);
   BOOST_MATH_STD_USING
   static const char* function = "boost::math::binomial_coefficient<%1%>(unsigned, unsigned)";
   if(k > n)
      return policies::raise_domain_error<T>(
         function,
         "The binomial coefficient is undefined for k > n, but got k = %1%.",
         static_cast<T>(k), pol);
   T result;
   if((k == 0) || (k == n))
      return static_cast<T>(1);
   if((k == 1) || (k == n-1))
      return static_cast<T>(n);

   if(n <= max_factorial<T>::value)
   {
      // Use fast table lookup:
      result = unchecked_factorial<T>(n);
      result /= unchecked_factorial<T>(n-k);
      result /= unchecked_factorial<T>(k);
   }
   else
   {
      // Use the beta function:
      if(k < n - k)
         result = k * beta(static_cast<T>(k), static_cast<T>(n-k+1), pol);
      else
         result = (n - k) * beta(static_cast<T>(k+1), static_cast<T>(n-k), pol);
      if(result == 0)
         return policies::raise_overflow_error<T>(function, 0, pol);
      result = 1 / result;
   }
   // convert to nearest integer:
   return ceil(result - 0.5f);
}
//
// Type float can only store the first 35 factorials, in order to
// increase the chance that we can use a table driven implementation
// we'll promote to double:
//
template <>
inline float binomial_coefficient<float, policies::policy<> >(unsigned n, unsigned k, const policies::policy<>& pol)
{
   return policies::checked_narrowing_cast<float, policies::policy<> >(binomial_coefficient<double>(n, k, pol), "boost::math::binomial_coefficient<%1%>(unsigned,unsigned)");
}

template <class T>
inline T binomial_coefficient(unsigned n, unsigned k)
{
   return binomial_coefficient<T>(n, k, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SF_BINOMIAL_HPP