//---------------------------------------------------------------------------//
// Copyright (c) 2020 Mikhail Komarov <nemo@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef BOOST_MULTIPRECISION_INVERSE_HPP
#define BOOST_MULTIPRECISION_INVERSE_HPP

#include <boost/container/vector.hpp>

#include <boost/type_traits/is_integral.hpp>

#include <nil/crypto3/multiprecision/cpp_int.hpp>
#include <nil/crypto3/multiprecision/cpp_int/cpp_int_config.hpp>
#include <nil/crypto3/multiprecision/modular/modular_adaptor.hpp>
#include <nil/crypto3/multiprecision/modular/inverse.hpp>

namespace nil {
    namespace crypto3 {
        namespace multiprecision {

            template<typename Backend, expression_template_option ExpressionTemplates>
            constexpr number<Backend, ExpressionTemplates>
                inverse_extended_euclidean_algorithm(const number<Backend, ExpressionTemplates>& n,
                                                     const number<Backend, ExpressionTemplates>& mod) {
                return number<Backend, ExpressionTemplates>(
                    backends::eval_inverse_extended_euclidean_algorithm(n.backend(), mod.backend()));
            }

            template<typename Backend, expression_template_option ExpressionTemplates>
            constexpr number<modular_adaptor<Backend>, ExpressionTemplates> inverse_extended_euclidean_algorithm(
                const number<modular_adaptor<Backend>, ExpressionTemplates>& modular) {
                number<Backend, ExpressionTemplates> new_base, res;
                number<modular_adaptor<Backend>, ExpressionTemplates> res_mod;

                modular.backend().mod_data().adjust_regular(new_base.backend(), modular.backend().base_data());
                res = backends::eval_inverse_extended_euclidean_algorithm(
                    new_base.backend(), modular.backend().mod_data().get_mod().backend());
                assign_components(res_mod.backend(), res.backend(), modular.backend().mod_data().get_mod().backend());

                return res_mod;
            }

            template<typename Backend, expression_template_option ExpressionTemplates>
            constexpr number<Backend, ExpressionTemplates>
                monty_inverse(const number<Backend, ExpressionTemplates>& a,
                              const number<Backend, ExpressionTemplates>& p,
                              const number<Backend, ExpressionTemplates>& k) {
                number<Backend, ExpressionTemplates> res;
                backends::eval_monty_inverse(res.backend(), a.backend(), p.backend(), k.backend());
                return res;
            }

            /*
            template <typename IntegerType, typename = typename boost::enable_if<typename
            is_trivial_cpp_int<IntegerType>::value>::type> IntegerType monty_inverse(const IntegerType& a)
            {
               return eval_monty_inverse(a);
            }
             */

            /*
            template <typename IntegerType, typename = typename boost::enable_if<!typename
            is_trivial_cpp_int<IntegerType>::value>::type> IntegerType monty_inverse(const IntegerType& a)
            {
               IntegerType res;
               eval_monty_inverse(res.backend(), a.backend());
               return res;
            }
             */

        }    // namespace multiprecision
    }        // namespace crypto3
}    // namespace nil

#endif


//
// Copyright 2013 Christian Henning
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#ifndef BOOST_GIL_TEST_EXTENSION_IO_MANDEL_VIEW_HPP
#define BOOST_GIL_TEST_EXTENSION_IO_MANDEL_VIEW_HPP

#include <boost/gil.hpp>

#include <cmath>
#include <cstdint>

namespace gil = boost::gil;

// Models a Unary Function
template <typename P> // Models PixelValueConcept
struct mandelbrot_fn {
  using point_t = gil::point_t;
  using const_t = mandelbrot_fn;
  using value_type = P;
  using reference = value_type;
  using const_reference = value_type;
  using argument_type = point_t;
  using result_type = reference;
  static constexpr bool is_mutable = false;

  value_type in_color_;
  value_type out_color_;
  point_t img_size_;
  static const int MAX_ITER = 100; // max number of iterations

  mandelbrot_fn() = default;
  mandelbrot_fn(gil::point_t const &sz, value_type const &in_color,
                value_type const &out_color)
      : in_color_(in_color), out_color_(out_color), img_size_(sz) {}

  std::ptrdiff_t width() { return img_size_.x; }
  std::ptrdiff_t height() { return img_size_.y; }

  result_type operator()(gil::point_t const &p) const {
    // normalize the coords to (-2..1, -1.5..1.5)
    // (actually make y -1.0..2 so it is asymmetric, so we can verify some view
    // factory methods)
    gil::point<double> const n{
        static_cast<double>(p.x) / static_cast<double>(img_size_.x) * 3 - 2,
        static_cast<double>(p.y) / static_cast<double>(img_size_.y) * 3 -
            1.0f}; // 1.5f}
    double t = get_num_iter(n);
    t = std::pow(t, 0.2);

    value_type ret;
    for (std::size_t k = 0; k < gil::num_channels<P>::value; ++k)
      ret[k] = (typename gil::channel_type<P>::type)(in_color_[k] * t +
                                                     out_color_[k] * (1 - t));
    return ret;
  }

private:
  double get_num_iter(boost::gil::point<double> const &p) const {
    gil::point<double> z(0, 0);
    for (int i = 0; i < MAX_ITER; ++i) {
      z = gil::point<double>(z.x * z.x - z.y * z.y + p.x, 2 * z.x * z.y + p.y);
      if (z.x * z.x + z.y * z.y > 4)
        return i / (double)MAX_ITER;
    }
    return 0;
  }
};

template <typename Pixel> struct mandel_view {
  using deref_t = mandelbrot_fn<Pixel>;
  using locator_t = gil::virtual_2d_locator<deref_t, false>;
  using my_virt_view_t = gil::image_view<locator_t>;
  using type = my_virt_view_t;
};

template <typename Pixel>
auto create_mandel_view(unsigned int width, unsigned int height,
                        Pixel const &in, Pixel const &out) ->
    typename mandel_view<Pixel>::type {
  using view_t = typename mandel_view<Pixel>::type;
  using deref_t = typename mandel_view<Pixel>::deref_t;
  using locator_t = typename mandel_view<Pixel>::locator_t;

  gil::point_t dims(width, height);
  return view_t(dims, locator_t(gil::point_t(0, 0), gil::point_t(1, 1),
                                deref_t(dims, in, out)));
}

#endif // BOOST_GIL_TEST_EXTENSION_IO_MANDEL_VIEW_HPP


//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::test::functions.h                                  //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef LIBS_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_TEST_FUNCTIONS_H_ER_2009
#define LIBS_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_TEST_FUNCTIONS_H_ER_2009
#include <ostream>
#include <boost/concept/assert.hpp>

#include <boost/statistics/detail/distribution_toolkit/distributions/chi_squared/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/exponential/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/gamma/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/normal/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/students_t/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/uniform/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/include.hpp>

#include <boost/statistics/detail/distribution_toolkit/map_pdf/inverse_pdf/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/map_pdf/product_pdf/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/map_pdf/ratio_pdf/include.hpp>

#include <boost/statistics/detail/distribution_common/concept/unary/description.hpp>
#include <boost/statistics/detail/distribution_common/concept/unary/mean.hpp>
#include <boost/statistics/detail/distribution_common/concept/unary/variance.hpp>
#include <boost/statistics/detail/distribution_common/concept/unary/quantile.hpp>
#include <boost/statistics/detail/distribution_common/concept/unary/cdf.hpp>
#include <boost/statistics/detail/distribution_common/concept/unary/pdf.hpp>
#include <boost/statistics/detail/distribution_common/concept/unary/log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_common/concept/unary/derivative_log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_common/concept/unary/os.hpp>

#include <boost/statistics/detail/distribution_common/distributions/reference/include.hpp>
#include <boost/statistics/detail/distribution_common/meta/wrapper/distribution.hpp>
#include <boost/statistics/detail/distribution_common/meta/wrapper/variable.hpp>
#include <boost/statistics/detail/distribution_common/meta/wrapper/probability.hpp>

template <typename T>
void test_functions(std::ostream& os)
{
    os << "test_functions - >";
    using namespace boost;
    using namespace math;

    namespace dist = boost::statistics::detail::distribution;

    typedef dist::meta::wrapper_variable<T> x_;
    typedef dist::meta::wrapper_probability<T,x_> p_x_;

    {
        typedef math::chi_squared_distribution<T>                   d_;
        typedef dist::meta::wrapper_distribution<d_,p_x_>          w_;
        BOOST_CONCEPT_ASSERT((
        	dist::concept::Os<
        		dist::concept::Description<
            		dist::concept::DerivativeLogUnnormalizedPdf<
                		dist::concept::LogUnnormalizedPdf<
                    		dist::concept::Pdf<
                        		dist::concept::Quantile<
                            		dist::concept::Cdf<
                                		dist::concept::Mean<
                                    		dist::concept::Variance<w_>
                                		>
                            		>
                        		>
                    		>
                		>
            		>
            	>
            >
        ));
    }
    {
        typedef boost::math::exponential_distribution<T>     d_;
        typedef dist::meta::wrapper_distribution<d_,p_x_>    w_;
        BOOST_CONCEPT_ASSERT((
        	dist::concept::Os<
        		dist::concept::Description<
            		dist::concept::DerivativeLogUnnormalizedPdf<
                		dist::concept::LogUnnormalizedPdf<
                    		dist::concept::Pdf<
                        		dist::concept::Quantile<
                            		dist::concept::Cdf<
                                		dist::concept::Mean<
                                    		dist::concept::Variance<w_>
                                		>
                            		>
                        		>
                    		>
                		>
            		>
            	>
            >
        ));
    }
    {
        typedef math::gamma_distribution<T>                d_;
        typedef dist::meta::wrapper_distribution<d_,p_x_>  w_;
        BOOST_CONCEPT_ASSERT((
        	dist::concept::Os<
        		dist::concept::Description<
            		dist::concept::DerivativeLogUnnormalizedPdf<
                		dist::concept::LogUnnormalizedPdf<
                    		dist::concept::Pdf<
                        		dist::concept::Quantile<
                            		dist::concept::Cdf<
                                		dist::concept::Mean<
                                    		dist::concept::Variance<w_>
                                		>
                            		>
                        		>
                    		>
                		>
            		>
            	>
            >
        ));
    }
    {
        typedef math::normal_distribution<T>                        d_;
        typedef dist::meta::wrapper_distribution<d_,p_x_>          w_;
        BOOST_CONCEPT_ASSERT((
        	dist::concept::Os<
        		dist::concept::Description<
            		dist::concept::DerivativeLogUnnormalizedPdf<
                		dist::concept::LogUnnormalizedPdf<
                    		dist::concept::Pdf<
                        		dist::concept::Quantile<
                            		dist::concept::Cdf<
                                		dist::concept::Mean<
                                    		dist::concept::Variance<w_>
                                		>
                            		>
                        		>
                    		>
                		>
            		>
            	>
            >
        ));
    }
    {
        typedef math::students_t_distribution<T>                   d_;
        typedef dist::meta::wrapper_distribution<d_,p_x_>          w_;

        BOOST_CONCEPT_ASSERT((
        	dist::concept::Os<
        		dist::concept::Description<
            		dist::concept::DerivativeLogUnnormalizedPdf<
                		dist::concept::LogUnnormalizedPdf<
                    		dist::concept::Pdf<
                        		dist::concept::Quantile<
                            		dist::concept::Cdf<
                                		dist::concept::Mean<
                                    		dist::concept::Variance<w_>
                                		>
                            		>
                        		>
                    		>
                		>
            		>
            	>
            >
        ));
    }
    {
        typedef boost::math::uniform_distribution<T>                d_;
        typedef dist::meta::wrapper_distribution<d_,p_x_>          	w_;

        BOOST_CONCEPT_ASSERT((
        	dist::concept::Description<
            	dist::concept::DerivativeLogUnnormalizedPdf<
                	dist::concept::LogUnnormalizedPdf<
                    	dist::concept::Pdf<
                        	dist::concept::Quantile<
                            	dist::concept::Cdf<
                                	dist::concept::Mean<
                                    	dist::concept::Variance<w_>
                                	>
                            	>
                        	>
                    	>
                	>
            	>
            >
        ));
    }
    {
        typedef math::normal_distribution<T>                        d_;
        typedef dist::meta::wrapper_distribution<d_,p_x_>          w_;

        BOOST_CONCEPT_ASSERT((
        	dist::concept::Os<
        		dist::concept::Description<
            		dist::concept::DerivativeLogUnnormalizedPdf<
                		dist::concept::LogUnnormalizedPdf<
                    		dist::concept::Pdf<
                        		dist::concept::Quantile<
                            		dist::concept::Cdf<
                                		dist::concept::Mean<
                                    		dist::concept::Variance<w_>
                                		>
                            		>
                        		>
                    		>
                		>
            		>
            	>
            >
        ));
    }
    {
        typedef math::normal_distribution<T>                        z_;
        typedef dist::toolkit::location_scale_distribution< z_ >    d_;
        typedef dist::meta::wrapper_distribution<d_,p_x_>          w_;

        BOOST_CONCEPT_ASSERT((
        	dist::concept::Os<
        		dist::concept::Description<
            		dist::concept::DerivativeLogUnnormalizedPdf<
                		dist::concept::LogUnnormalizedPdf<
                    		dist::concept::Pdf<
                        		dist::concept::Quantile<
                            		dist::concept::Cdf<
                                		dist::concept::Mean<
                                    		dist::concept::Variance<w_>
                                		>
                            		>
                        		>
                    		>
                		>
            		>
            	>
            >
        ));
    }
    {
        typedef math::normal_distribution<T>                d0_;
        typedef dist::reference_wrapper< const d0_& >       d_;
        typedef dist::meta::wrapper_distribution<d_,p_x_>   w_;

        BOOST_CONCEPT_ASSERT((
        	dist::concept::Os<
        		dist::concept::Description<
            		dist::concept::DerivativeLogUnnormalizedPdf<
                		dist::concept::LogUnnormalizedPdf<
                    		dist::concept::Pdf<
                        		dist::concept::Quantile<
                            		dist::concept::Cdf<
                                		dist::concept::Mean<
                                    		dist::concept::Variance<w_>
                                		>
                            		>
                        		>
                    		>
                		>
            		>
            	>
            >
        ));
    }
    {
        typedef math::normal_distribution<T>                        d0_;
        typedef dist::toolkit::inverse_pdf< d0_ >                  	d_;
        typedef dist::meta::wrapper_distribution<d_,p_x_>          	w_;

        BOOST_CONCEPT_ASSERT((
        	dist::concept::Os<
        		dist::concept::Description<
        			dist::concept::LogUnnormalizedPdf<w_>
                >
            >
        ));
    }
    {
        typedef math::normal_distribution<T>                        d0_;
        typedef math::chi_squared_distribution<T>                   d1_;
        typedef dist::toolkit::product_pdf< d0_, d1_ >              d_;
        typedef dist::meta::wrapper_distribution<d_,p_x_>  			w_;

        BOOST_CONCEPT_ASSERT((
        	dist::concept::Description<
            	dist::concept::LogUnnormalizedPdf<w_>
            >
        ));
    }
    {
        typedef math::normal_distribution<T>                        		d0_;
        typedef math::chi_squared_distribution<T>                   		d1_;
        typedef typename dist::toolkit::meta_ratio_pdf< d0_, d1_ >::type    d_;
        typedef dist::meta::wrapper_distribution<d_,p_x_>  w_;

        BOOST_CONCEPT_ASSERT((
        	dist::concept::Description<
	        	dist::concept::LogUnnormalizedPdf<w_>
            >
        ));
    }

    os << " <- " << std::endl;
}

#endif


#pragma once

/**
 * @file transforms.hpp
 * @author Zakary Littlefield
 * @brief  [INCOMPLETE] Some handy aliases for commonly used Eigen declarations.
 * */

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

namespace prx
{
	/** @brief A column vector of dimension N. */
	template <int N>
	using n_vector_t = Eigen::Matrix<double, N, 1>;
	/** @brief A matrix of dimension N x N. */
	template <int N>
	using n_matrix_t = Eigen::Matrix<double, N, N>;
	/** @brief A column vector of dimension 3. */
	using vector_t = n_vector_t<3>;
	/** @brief A 3x3 matrix. */
	using matrix_t = n_matrix_t<3>;
	/** @brief A quaternion. */
	using quaternion_t = Eigen::Quaternion<double>;
	using axis_angle_t = Eigen::AngleAxis<double>;
	using transform_t = Eigen::Transform<double, 3, Eigen::AffineCompact>;
}


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SINPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SINPI_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing sinpi capabilities

    sine of angle in \f$\pi\f$ multiples: \f$\sin(\pi x)\f$.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = sinpi(x);
    @endcode

    @see sine, sincospi, sin, sind

  **/
  Value sinpi(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/sinpi.hpp>
#include <boost/simd/function/simd/sinpi.hpp>

#endif


//! Merger, derived from Work.
/*!
Merges intra-user states into inter-user states
*/

#pragma once

#include <Core/VisualChangeClassifier.hpp>
#include <Core/Task.hpp>
#include <Data/InterUserState.hpp>
#include <Eigen/Dense>

namespace stage
{
	namespace merging
	{
		/////////////////////////////////////////////////
		/// Merger
		/////////////////////////////////////////////////

		// Clusterer class
		class Merger : public core::Work<data::InterUserStateContainer, core::PrintReport>
		{
		public:

			// Internal parser phases
			enum class Phase { InitSimiliarityMatrix, Merging, Finalize };

			// Constructor
			Merger(
				VD(std::shared_ptr<core::visual_debug::Dump> sp_dump, )
				std::shared_ptr<const core::VisualChangeClassifier> sp_classifier,
				std::string id,
				std::shared_ptr<data::IntraUserStates_const> sp_intras);

		protected:

			// Returns shared pointer to product if complete, otherwise nullptr
			virtual std::shared_ptr<ProductType> internal_step();

			// Report the progess of work
			virtual void internal_report(ReportType& r_report);

		private:

			// Initialize similarity matrix
			bool init_similarity_matrix();

			// Merge intra-user states into inter-user states
			bool merge();

			// Finalize inter-user states
			bool finalize();

			// Product of work
			std::shared_ptr<ProductType> _sp_container = nullptr; // inter-user state container

			// Members
			std::shared_ptr<const core::VisualChangeClassifier> _sp_classifier = nullptr;
			std::shared_ptr<data::IntraUserStates_const> _sp_intras;
			Phase _phase = Phase::InitSimiliarityMatrix;
			Eigen::Matrix<core::long64, Eigen::Dynamic, Eigen::Dynamic> _similarity_matrix;
			std::shared_ptr<data::InterUserStates> _sp_inters = nullptr;
			core::long64 _last_min_merged_similarity = std::numeric_limits<core::long64>::max();

		};
	}
}


//==============================================================================
//          Copyright 2015 J.T. Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EULER_FUNCTIONS_SCALAR_ERFCX_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SCALAR_ERFCX_HPP_INCLUDED

#include <nt2/euler/functions/erfcx.hpp>
#include <nt2/euler/functions/details/erf_kernel.hpp>
#include <nt2/include/functions/scalar/erfc.hpp>
#include <nt2/include/functions/scalar/expx2.hpp>
#include <nt2/include/functions/scalar/sqr.hpp>
#include <nt2/include/functions/scalar/sqrt.hpp>
#include <nt2/include/functions/scalar/oneplus.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/six.hpp>
#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#endif

#include <boost/simd/sdk/config.hpp>
#include <iostream>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( erfcx_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
#ifndef BOOST_SIMD_NO_INFINITIES
      if(a0 == Inf<A0>()) return Zero<A0>();
      if(nt2::is_nan(a0)) return a0;
#endif
      if(a0 < 0.65)
      {
        return expx2(a0)*erfc(a0);
      }
      else if (a0 < 2.2)
      {
        return details::erf_kernel<A0>::erfc2(a0);
      }
      else if(a0< A0(6))
      {
        return details::erf_kernel<A0>::erfc3(a0);
      }
      else
      {
        return details::erf_kernel<A0>::erfc4(rec(a0));
      }
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( erfcx_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
#ifndef BOOST_SIMD_NO_INFINITIES
      if(a0 == Inf<A0>()) return Zero<A0>();
      if(nt2::is_nan(a0)) return a0;
#endif
      if(a0 < Twothird<A0>())
      {
        return expx2(a0)*erfc(a0);
      }
      else
      {
        A0 z =  a0/oneplus(a0)- 0.4f;
        return details::erf_kernel<A0>::erfc2(z);
      }
    }
  };
} }

#endif


static char help[] = "main file launching the whole computation - kind of setup script";

#include <petscdmda.h>
#include <petsctime.h>
#include <petsclog.h>

#include <vector>
#include <iostream>
#include <algorithm>
#include <iterator>
#include <Eigen/Dense>
#include <cassert>

#include "Geometry.hpp"
#include "Grid.hpp"
#include "Initializer.hpp"
#include "Interface.hpp"
#include "FmmWrapper.hpp"
#include "TriangleElement.hpp"
#include "tictoc.hpp"
#include "BINWritter.hpp"

#undef __FUNCT__
#define __FUNCT__ "main"
int main(int argc,char **argv) {

	PetscErrorCode ierr;
	ierr = PetscInitialize(&argc, &argv, (char*)0, help); CHKERRQ(ierr);

    int init_event, loadData_event, solve_event;

    bool write_out = true;

    Eigen::Array3i M;            // number of nodes in each dimension

    char           fname[120];
    char           oname[120];
    PetscBool      flg;
    PetscLogDouble t0, t1, t2, t3, t4, t5, fmm_time, init_time, load_time;
    int rank, size;

    MPI_Comm_rank(PETSC_COMM_WORLD, &rank);
    MPI_Comm_size(PETSC_COMM_WORLD, &size);

    ///////////////////
    /// GET OPTIONS ///
    ///////////////////

    PetscOptionsGetString(PETSC_NULL, "-f", fname, 120, &flg);
    if (!flg) {
        // no input filename
        PetscSynchronizedPrintf(PETSC_COMM_WORLD, "C'mon gimme something!\n");
        PetscSynchronizedFlush(PETSC_COMM_WORLD);
        ierr = PetscFinalize();
        return 1;
    }

    PetscOptionsGetString(PETSC_NULL, "-o", oname, 120, &flg);
    if (!flg) {
        // no output filename default will be used
        write_out = false;
    }

    int tmpM, tmpN, tmpP;
    int M[3], NP[3];

    PetscOptionsGetInt(PETSC_NULL,"-m", &M[0], &flg);
    if (!flg) {
        // no input size
        PetscSynchronizedPrintf(PETSC_COMM_WORLD, "At least one dimension must be given!\n");
        PetscSynchronizedFlush(PETSC_COMM_WORLD);
        ierr = PetscFinalize();
        return 1;
    }

    PetscOptionsGetInt(PETSC_NULL,"-n", &M[1], &flg);
    if (!flg) {
        M[1] = M[0];
        // no worry, everything is ok
    }

    PetscOptionsGetInt(PETSC_NULL,"-p", &M[2], &flg);
    if (!flg) {
        M[2] = M[0];
        // no worry, everything is ok
    }

    PetscOptionsGetInt(PETSC_NULL,"-nm", &NP[0], &flg);
    if (!flg) {
        NP[0] = PETSC_DECIDE;
        // no worry, everything is ok
    }

    PetscOptionsGetInt(PETSC_NULL,"-nn", &NP[1], &flg);
    if (!flg) {
        NP[1] = PETSC_DECIDE;
        // no worry, everything is ok
    }

    PetscOptionsGetInt(PETSC_NULL,"-np", &NP[2], &flg);
    if (!flg) {
        NP[2] = PETSC_DECIDE;
        // no worry, everything is ok
    }

    int numberOfGroups = 1;
    PetscOptionsGetInt(PETSC_NULL,"-g", &numberOfGroups, &flg);
    if (!flg) {
        // no worry, everything is ok
        numberOfGroups = 1;
    }

    double growCoef = 1.01;
    PetscOptionsGetReal(PETSC_NULL,"-grow", &growCoef, &flg);
    if (!flg) {
        // no worry, everything is ok
        growCoef = 1.01;
    }

    int initAll = 0;
	PetscOptionsGetInt(PETSC_NULL,"-all", &initAll, &flg);
	if (!flg) {
		// no worry, everything is ok
		initAll = 0;
	}

    ////////////////////////////
    /// END SETUP PARAMETERS ///
    ////////////////////////////

    int procsPerGroup  = ceil( double(size) / double(numberOfGroups) );
    int groupID        = rank / procsPerGroup;

    Eigen::Array3d min, max;

    min << -0.5, -0.5, -0.5;
    max <<  0.5,  0.5,  0.5;

    PetscLogEventRegister("loadData", 0, &loadData_event);
    PetscLogEventBegin(loadData_event, 0, 0, 0, 0);

    Geometry geom = Geometry::fromFile(fname, growCoef);
    geom.initSearchAccelerator();

    PetscLogEventEnd(loadData_event, 0, 0, 0, 0);

    // SphereInitializer init( Eigen::Vector3d(0,0,0), 0.25 );
    Initializer init;
    Grid<double, 3>  gr(geom.aabb.bl(), geom.aabb.tr(), M, NP);
    // Grid<double, 3>  gr(min, max, tmpM);

    geom.preSortElements(gr.getNodeSpan(), gr.getDx(0), numberOfGroups, groupID);
    // std::cout << "presort done" << std::endl;

    Interface<double, 3>* interface = new Interface<double, 3>(gr);

    PetscLogEventRegister("initData", 0, &init_event);
	PetscLogEventBegin(init_event, 0, 0, 0, 0);

    init(geom, *interface, groupID, numberOfGroups);
    // init(*interface, initAll);

    PetscLogEventEnd(init_event, 0, 0, 0, 0);

    double shift = gr.getMaxDist();

    PetscLogEventRegister("solve", 0, &solve_event);
	PetscLogEventBegin(solve_event, 0, 0, 0, 0);

	if (!initAll) {
		std::cout << "FMM solver" << std::endl;
		FmmWrapper::solveEikonalEquation<3>(gr, interface, shift);
	}

	PetscLogEventEnd(solve_event, 0, 0, 0, 0);

    if (write_out) {
        // std::cout << "Writting out" << std::endl;
        BINWritter::writeData<double,3>(*interface, oname);
        // std::cout << "write" << std::endl;
    }

//  delete interface;
//  delete gr;

    ierr = PetscFinalize();
    return 0;

}


#include "arc_utilities/first_order_deformation.h"

#include <queue>
#include <Eigen/Core>

using namespace arc_utilities;
using namespace FirstOrderDeformation;

static double ConfigTypeDistance(const ConfigType& c1, const ConfigType& c2)
{
    return Eigen::Vector2d((double)(c1.first - c2.first), (double)(c1.second - c2.second)).norm();
}

static std::vector<ConfigType> GetNeighbours(const ConfigType& config, const ssize_t rows, const ssize_t cols, const ValidityCheckFnType& validity_check_fn)
{
    std::vector<ConfigType> neighbours;
    neighbours.reserve(8);

    const ssize_t row_min = std::max(0L, config.first - 1);
    const ssize_t row_max = std::min(rows - 1, config.first + 1);

    const ssize_t col_min = std::max(0L, config.second - 1);
    const ssize_t col_max = std::min(cols - 1, config.second + 1);

    for (ssize_t col = col_min; col <= col_max; col++)
    {
        for (ssize_t row = row_min; row <= row_max; row++)
        {
            if (!(row == config. first && col == config.second) && validity_check_fn(row, col) == true)
            {
                neighbours.push_back(ConfigType(row, col));
            }
        }
    }

    return neighbours;
}

bool FirstOrderDeformation::CheckFirstOrderDeformation(const ssize_t rows, const ssize_t cols, const ValidityCheckFnType& validity_check_fn)
{
    struct BestFirstSearchComparator
    {
        public:
            // Defines a "less" operation"; by using "greater" the smallest element will appear at the top of the priority queue
            bool operator()(const ConfigAndDistType& c1, const ConfigAndDistType& c2) const
            {
                // We want to explore the one with the smaller expected distance
                return (c1.second > c2.second);
            }
    };

    assert(rows > 0 && cols > 0);
    typedef Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> ArrayXb;

    // Setup the strucutres needed to keep track of the search
    std::priority_queue<ConfigAndDistType, std::vector<ConfigAndDistType>, BestFirstSearchComparator> frontier;
    ArrayXb explored = ArrayXb::Constant(rows, cols, false);

    // Setup the start and goal
    const ConfigType start(0, 0), goal(rows - 1, cols - 1);
    const double start_heuristic_distance = ConfigTypeDistance(start, goal);
    frontier.push(ConfigAndDistType(start, start_heuristic_distance));

    // Perform the best first search
    bool path_found = false;
    while (!path_found && frontier.size() > 0)
    {
        const ConfigAndDistType current = frontier.top();
        frontier.pop();
        const ConfigType& current_config = current.first;

        if (current_config.first == goal.first && current_config.second == goal.second)
        {
            path_found = true;
        }
        // Double check if we've already explored this node:
        //    a single node can be inserted into the frontier multiple times at the same or different priorities
        //    so we want to avoid the expense of re-exploring it, and just discard this one once we pop it
        else if (explored(current_config.first, current_config.second) == false)
        {
            explored(current_config.first, current_config.second) = true;

            // Expand the node to find all neighbours, adding them to the frontier if we have not already explored them
            const auto neighbours = GetNeighbours(current_config, rows, cols, validity_check_fn);
            for (const auto neighbour : neighbours)
            {
                // Check if we've already explored this neighbour to avoid re-adding it to the frontier
                if (explored(neighbour.first, neighbour.second) == false)
                {
                    // We only need the heuristic distance as we are doing a best first search
                    const double neighbour_heuristic_distance = ConfigTypeDistance(neighbour, goal);
                    frontier.push(ConfigAndDistType(neighbour, neighbour_heuristic_distance));
                }
            }
        }
    }

    return path_found;
}


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

#ifndef BOOST_HASH_DETAIL_PRIMES_HPP
#define BOOST_HASH_DETAIL_PRIMES_HPP

#include <boost/integer.hpp>

namespace boost {
namespace hashes {
namespace detail {

template <int Bits>
struct all_ones {
    typedef typename uint_t<Bits>::least type;
    static type const value = (type(all_ones<Bits-1>::value) << 1) | 1;
};
template <>
struct all_ones<0> {
    typedef uint_t<0>::least type;
    static type const value = 0;
};

template <int Bits>
struct largest_prime;

#define BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(B, D) \
    template <> \
    struct largest_prime<B> { \
        static uint_t<B>::least const value = all_ones<B>::value - D; \
    }

// http://primes.utm.edu/lists/2small/0bit.html or
// http://www.research.att.com/~njas/sequences/A013603
// Though those offets are from 2**b; This code is offsets from 2**b-1
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET( 2, 0);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET( 3, 0);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET( 4, 2);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET( 5, 0);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET( 6, 2);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET( 7, 0);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET( 8, 4);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET( 9, 2);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(10, 2);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(11, 8);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(12, 2);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(13, 0);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(14, 2);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(15, 18);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(16, 14);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(17, 0);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(18, 4);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(19, 0);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(20, 2);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(21, 8);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(22, 2);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(23, 14);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(24, 2);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(25, 38);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(26, 4);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(27, 38);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(28, 56);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(29, 2);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(30, 34);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(31, 0);
BOOST_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(32, 4);

} // namespace detail
} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_DETAIL_PRIMES_HPP


#include <layer/layer.h>
#include <layer/math.h>
#include <layer/hg.h>
#include <layer/microfacet.h>
#include <layer/fresnel.h>
#include <layer/spline.h>
#include <layer/log.h>
#include <filesystem/path.h>
#include <Eigen/SparseLU>
#include <Eigen/Geometry>
#include <tbb/tbb.h>

#if defined(HAVE_FFTW)
    #include <fftw3.h>
#endif

NAMESPACE_BEGIN(layer)

namespace {
    template <typename VectorType> MatrixS sparseDiagonal(const VectorType &vec) {
        MatrixS result(vec.size(), vec.size());
        for (MatrixS::Index i = 0; i < vec.size(); ++i)
            result.insert(i, i) = vec[i];
        result.makeCompressed();
        return result;
    }

    void sparsify(const MatrixX &dense, MatrixS &sparse) {
        sparse.setZero();

        for (MatrixX::Index j = 0; j < dense.cols(); ++j) {
            for (MatrixX::Index i = 0; i < dense.rows(); ++i) {
                Float value = dense.coeff(i, j);
                if (value != 0)
                    sparse.insert(i, j) = value;
            }
        }
        sparse.makeCompressed();
    }

    void scaleColumns(LayerMode &mode, const VectorX &d) {
        if ((size_t) d.size() != mode.resolution())
            throw std::runtime_error("scaleColumns(): size mismatch!");
        MatrixS scale = sparseDiagonal(d.head(d.size()/2));
        mode.transmissionBottomTop = mode.transmissionBottomTop * scale;
        mode.reflectionBottom = mode.reflectionBottom * scale;
        scale = sparseDiagonal(d.tail(d.size()/2));
        mode.transmissionTopBottom = mode.transmissionTopBottom * scale;
        mode.reflectionTop = mode.reflectionTop * scale;
    }

    void applySurfaceIntegrationWeights(Layer &layer) {
        for (size_t l=0; l<layer.fourierOrders(); ++l)
            scaleColumns(layer[l], layer.weights().cwiseProduct(
                                       layer.nodes().cwiseAbs()) *
                                       math::Pi * (l == 0 ? 2 : 1));
    }

    void applyMediumIntegrationWeights(Layer &layer) {
        for (size_t l=0; l<layer.fourierOrders(); ++l)
            scaleColumns(layer[l], layer.weights() * math::Pi * (l == 0 ? 2 : 1));
    }
};

Layer::Layer(const VectorX &nodes, const VectorX &weights, size_t nFourierOrders)
    : m_modes(nFourierOrders, LayerMode(nodes.size())), m_nodes(nodes), m_weights(weights) {
    if (nodes.size() < 2)
        throw std::runtime_error("Need at least 2 integration nodes!");
    else if (nodes.size() % 2 == 1)
        throw std::runtime_error("The number of integration nodes must be even!");
    for (int i=0; i<nodes.size(); ++i)
        if (nodes[i] == 0)
            throw std::runtime_error("The set of integrations includes mu=0 -- this is not allowed.");

    if (nodes[0] < nodes[1]) {
        size_t n = (size_t) nodes.size();
        /* Order integration weights so that they are usable for adding-doubling */
        m_weights.head(n/2).reverseInPlace();
        m_nodes.head(n/2).reverseInPlace();
    }
}

void Layer::setQuartets(const std::vector<Quartet> &quartets) {
    std::vector<std::vector<Eigen::Triplet<Float>>>
        tripletsTbt(fourierOrders()),
        tripletsTtb(fourierOrders()),
        tripletsRb(fourierOrders()),
        tripletsRt(fourierOrders());

    size_t approxSize = quartets.size() / (4 * fourierOrders());

    for (size_t i=0; i<fourierOrders(); ++i) {
        tripletsTbt[i].reserve(approxSize);
        tripletsTtb[i].reserve(approxSize);
        tripletsRb[i].reserve(approxSize);
        tripletsRt[i].reserve(approxSize);
    }

    size_t n = resolution() / 2;
    for (auto const &quartet: quartets) {
        typedef MatrixS::Index Index;
        if (quartet.o < n && quartet.i < n)
            tripletsTbt[quartet.l].emplace_back(Index(quartet.o), Index(quartet.i), quartet.value);
        else if (quartet.o >= n && quartet.i >= n)
            tripletsTtb[quartet.l].emplace_back(Index(quartet.o-n), Index(quartet.i-n), quartet.value);
        else if (quartet.o <n && quartet.i >= n)
            tripletsRt[quartet.l].emplace_back(Index(quartet.o), Index(quartet.i-n), quartet.value);
        else if (quartet.o >=n && quartet.i < n)
            tripletsRb[quartet.l].emplace_back(Index(quartet.o-n), Index(quartet.i), quartet.value);
        else
            throw std::runtime_error("Layer::setFromQuartets(): internal error!");
    }

    tbb::parallel_for(
        tbb::blocked_range<size_t>(0, fourierOrders(), 1),
        [&](const tbb::blocked_range<size_t> &range) {
            for (size_t l = range.begin(); l < range.end(); ++l) {
                m_modes[l].reflectionTop        .setFromTriplets(tripletsRt [l].begin(), tripletsRt [l].end());
                m_modes[l].reflectionBottom     .setFromTriplets(tripletsRb [l].begin(), tripletsRb [l].end());
                m_modes[l].transmissionTopBottom.setFromTriplets(tripletsTtb[l].begin(), tripletsTtb[l].end());
                m_modes[l].transmissionBottomTop.setFromTriplets(tripletsTbt[l].begin(), tripletsTbt[l].end());
            }
        }
    );
}

void Layer::setHenyeyGreenstein(Float albedo, Float g) {
    std::vector<Quartet> quartets;
    tbb::spin_mutex mutex;

    tbb::parallel_for(
        tbb::blocked_range<size_t>(0, resolution()),
        [&](const tbb::blocked_range<size_t> &range) {
            std::vector<Quartet> quartetsLocal;
            quartetsLocal.reserve(fourierOrders() * resolution());
            std::vector<Float> result;
            for (size_t i = range.begin(); i < range.end(); ++i) {
                for (size_t o = 0; o <= i; ++o) {
                    hgFourierSeries(m_nodes[o], m_nodes[i], g, (int) fourierOrders(),
                                    ERROR_GOAL, result);
                    for (size_t l=0; l<std::min(fourierOrders(), result.size()); ++l) {
                        quartetsLocal.emplace_back(l, o, i, result[l] * albedo);
                        if (i != o)
                            quartetsLocal.emplace_back(l, i, o, result[l] * albedo);
                    }
                }
                tbb::spin_mutex::scoped_lock lock(mutex);
                quartets.insert(quartets.end(), quartetsLocal.begin(), quartetsLocal.end());
            }
        }
    );
    setQuartets(quartets);
    applyMediumIntegrationWeights(*this);
}

void Layer::setVonMisesFisher(Float albedo, Float kappa) {
    std::vector<Quartet> quartets;
    tbb::spin_mutex mutex;

    Float scale;
    if (kappa == 0)
        scale = albedo / (4 * math::Pi);
    else
        scale = albedo * kappa / (4 * math::Pi * std::sinh(kappa));

    tbb::parallel_for(
        tbb::blocked_range<size_t>(0, resolution()),
        [&](const tbb::blocked_range<size_t> &range) {
            std::vector<Quartet> quartetsLocal;
            quartetsLocal.reserve(fourierOrders() * resolution());
            std::vector<Float> result;
            for (size_t i = range.begin(); i < range.end(); ++i) {
                Float mu_i = m_nodes[i];
                for (size_t o = 0; o <= i; ++o) {
                    Float mu_o = m_nodes[o];
                    Float A = kappa * mu_i * mu_o;
                    Float B = kappa * math::safe_sqrt((1 - mu_i * mu_i) *
                                                      (1 - mu_o * mu_o));
                    expCosFourierSeries(A, B, ERROR_GOAL, result);

                    for (size_t l=0; l<std::min(fourierOrders(), result.size()); ++l) {
                        quartetsLocal.emplace_back(l, o, i, result[l] * scale);
                        if (i != o)
                            quartetsLocal.emplace_back(l, i, o, result[l] * scale);
                    }
                }
                tbb::spin_mutex::scoped_lock lock(mutex);
                quartets.insert(quartets.end(), quartetsLocal.begin(), quartetsLocal.end());
            }
        }
    );

    setQuartets(quartets);
    applyMediumIntegrationWeights(*this);
}

void Layer::setDiffuse(Float albedo) {
    std::vector<Quartet> quartets;
    quartets.reserve(resolution() * resolution() / 2);

    size_t n = resolution(), h = n/2;
    for (size_t i=0; i<n; ++i) {
        for (size_t o=0; o<n; ++o) {
            if ((i < h && o >= h) || (o < h && i >= h))
                quartets.emplace_back(0, o, i, albedo * math::InvPi);
        }
    }

    setQuartets(quartets);
    applySurfaceIntegrationWeights(*this);
}

void Layer::setIsotropic(Float albedo) {
    std::vector<Quartet> quartets;
    quartets.reserve(resolution() * resolution());

    size_t n = resolution();
    for (size_t i=0; i<n; ++i)
        for (size_t o=0; o<n; ++o)
            quartets.emplace_back(0, o, i, albedo * math::InvFourPi);

    setQuartets(quartets);
    applyMediumIntegrationWeights(*this);
}

void Layer::setMicrofacet(std::complex<Float> eta, Float alpha, bool conserveEnergy,
                          size_t fourierOrdersTarget) {
    size_t n = resolution(), h = n/2;
    std::vector<Quartet> quartets;
    tbb::spin_mutex mutex;

    fourierOrdersTarget = std::max(fourierOrdersTarget, fourierOrders());

    tbb::parallel_for(
        tbb::blocked_range<size_t>(0, resolution()),
        [&](const tbb::blocked_range<size_t> &range) {
            std::vector<Quartet> quartetsLocal;
            quartetsLocal.reserve(fourierOrdersTarget * resolution());
            std::vector<Float> result;
            for (size_t i = range.begin(); i < range.end(); ++i) {
                for (size_t o=0; o<n; ++o) {
                    /* Sign flip due to different convention (depth values
                     * increase opposite to the normal direction) */
                    microfacetFourierSeries(-m_nodes[o], -m_nodes[i], eta,
                                            alpha, fourierOrdersTarget,
                                            ERROR_GOAL, result);

                    for (size_t l=0; l<std::min(fourierOrders(), result.size()); ++l)
                        quartetsLocal.emplace_back(l, o, i, result[l]);
                }
            }
            tbb::spin_mutex::scoped_lock lock(mutex);
            quartets.insert(quartets.end(), quartetsLocal.begin(), quartetsLocal.end());
        }
    );

    setQuartets(quartets);

    /* Add a pseudo-diffuse term to capture lost energy */
    if (conserveEnergy && eta.imag() == 0) {
        /* Case 1: Dielectrics */
        VectorX W = m_weights.tail(h).cwiseProduct(m_nodes.tail(h)) * 2 * math::Pi;
        LayerMode &l = m_modes[0];

        VectorX Mb  = (W.asDiagonal() * MatrixX(l.reflectionBottom)).colwise().sum();
        VectorX Mt  = (W.asDiagonal() * MatrixX(l.reflectionTop)).colwise().sum();
        VectorX Mtb = (W.asDiagonal() * MatrixX(l.transmissionTopBottom)).colwise().sum();
        VectorX Mbt = (W.asDiagonal() * MatrixX(l.transmissionBottomTop)).colwise().sum();

        /* Determine how much energy we'd like to put into the transmission component
           (proportional to the current reflection/reflaction split) */
        VectorX Atb = (VectorX::Ones(h) - Mt - Mtb).cwiseProduct(Mtb.cwiseQuotient(Mt + Mtb));
        VectorX Abt = (VectorX::Ones(h) - Mb - Mbt).cwiseProduct(Mbt.cwiseQuotient(Mb + Mbt));
        Atb = Atb.cwiseMax(VectorX::Zero(h));
        Abt = Abt.cwiseMax(VectorX::Zero(h));

        /* Create a correction matrix which contains as much of the desired
           energy as possible, while maintaining symmetry and energy conservation */
        MatrixX Ctb = Abt*Atb.transpose() / std::max(W.dot(Abt), W.dot(Atb) / (eta.real()*eta.real()));
        MatrixX Cbt = Ctb.transpose() / (eta.real()*eta.real());

        sparsify(MatrixX(l.transmissionTopBottom) + Ctb, l.transmissionTopBottom);
        sparsify(MatrixX(l.transmissionBottomTop) + Cbt, l.transmissionBottomTop);

        /* Update missing energy terms */
        Mtb = (W.asDiagonal() * MatrixX(l.transmissionTopBottom)).colwise().sum();
        Mbt = (W.asDiagonal() * MatrixX(l.transmissionBottomTop)).colwise().sum();

        /* Put the rest of the missing energy into the reflection component */
        VectorX At = VectorX::Ones(h) - Mt - Mtb;
        VectorX Ab = VectorX::Ones(h) - Mb - Mbt;
        At = At.cwiseMax(VectorX::Zero(h));
        Ab = Ab.cwiseMax(VectorX::Zero(h));

        sparsify(MatrixX(l.reflectionTop)    + At*At.transpose() / W.dot(At), l.reflectionTop);
        sparsify(MatrixX(l.reflectionBottom) + Ab*Ab.transpose() / W.dot(Ab), l.reflectionBottom);
    } else if (conserveEnergy && eta.imag() != 0) {
        /* Case 2: Conductors */
        VectorX W = m_weights.tail(h).cwiseProduct(m_nodes.tail(h)) * 2 * math::Pi;

        /* Compute a reference matrix for a material *without* Fresnel effects */
        MatrixX refMatrix(n, n);

        tbb::parallel_for(
            tbb::blocked_range<size_t>(0, resolution()),
            [&](const tbb::blocked_range<size_t> &range) {
                std::vector<Float> result;
                for (size_t i = range.begin(); i < range.end(); ++i) {
                    /* Parallel loop over 'i' */
                    for (size_t o = 0; o < n; ++o) {
                        microfacetFourierSeries(
                            -m_nodes[o], -m_nodes[i], std::complex<Float>(0.0f, 1.0f), alpha,
                            fourierOrdersTarget, ERROR_GOAL, result);
                        refMatrix(o, i) = result.size() > 0 ? result[0] : 0.0f;
                    }
                }
            }
        );

        MatrixX reflectionTopRef = MatrixX(refMatrix).block(0, h, h, h);

        VectorX Mt = VectorX::Ones(h) - (W.asDiagonal() * reflectionTopRef).colwise().sum().transpose();
        Mt = Mt.cwiseMax(VectorX::Zero(h));

        Float F = fresnelConductorIntegral(eta);
        Float E = 1 - W.dot(Mt) * math::InvPi;

        Float factor = F*E / (1-F*(1-E));

        MatrixX C = Mt * Mt.transpose() * (factor/ W.dot(Mt));

        sparsify(MatrixX(m_modes[0].reflectionTop) + C, m_modes[0].reflectionTop);
        sparsify(MatrixX(m_modes[0].reflectionBottom) + C, m_modes[0].reflectionBottom);
    }

    applySurfaceIntegrationWeights(*this);
}

void Layer::add(const Layer &layer1, const Layer &layer2, Layer &output, bool homogeneous) {
    if (output.resolution() != layer1.resolution() &&
        output.resolution() != layer2.resolution() &&
        output.fourierOrders() != layer1.fourierOrders() &&
        output.fourierOrders() != layer2.fourierOrders())
        throw std::runtime_error("Layer::addLayer(): incompatible sizes!");

    size_t n = output.resolution() / 2;
    MatrixS I((int) n, (int) n);
    I.setIdentity();

    /* Special case: it is possible to save quite a bit of computation when we
       know that both layers are homogeneous and of the same type */
    if (homogeneous) {
        tbb::parallel_for(
            tbb::blocked_range<size_t>(0, layer1.fourierOrders(), 1),
            [&](const tbb::blocked_range<size_t> &range) {
                MatrixS Rb, Ttb;
                for (size_t i = range.begin(); i < range.end(); ++i) {
                    const LayerMode &l1 = layer1[i], &l2 = layer2[i];
                    LayerMode &lo = output[i];

                    /* Gain for downward radiation */
                    Eigen::SparseLU<MatrixS, Eigen::AMDOrdering<int>> G_tb;
                    G_tb.compute(I - l1.reflectionBottom * l2.reflectionTop);

                    /* Transmission at the bottom due to illumination at the top */
                    MatrixS result = G_tb.solve(l1.transmissionTopBottom);
                    Ttb = l2.transmissionTopBottom * result;

                    /* Reflection at the bottom */
                    MatrixS temp = l1.reflectionBottom * l2.transmissionBottomTop;
                    result = G_tb.solve(temp);
                    Rb = l2.reflectionBottom + l2.transmissionTopBottom * result;

                    #if defined(DROP_THRESHOLD)
                        Ttb.prune((Float) 1, (Float) DROP_THRESHOLD);
                        Rb.prune((Float) 1, (Float) DROP_THRESHOLD);
                    #endif

                    lo.transmissionTopBottom = Ttb;
                    lo.transmissionBottomTop = Ttb;
                    lo.reflectionTop = Rb;
                    lo.reflectionBottom = Rb;
                }
            }
        );
    } else {
        tbb::parallel_for(
            tbb::blocked_range<size_t>(0, layer1.fourierOrders(), 1),
            [&](const tbb::blocked_range<size_t> &range) {
                for (size_t i = range.begin(); i < range.end(); ++i) {
                    const LayerMode &l1 = layer1[i], &l2 = layer2[i];
                    LayerMode &lo = output[i];

                    /* Gain for downward radiation */
                    Eigen::SparseLU<MatrixS, Eigen::AMDOrdering<int>> G_tb;
                    G_tb.compute(I - l1.reflectionBottom * l2.reflectionTop);

                    /* Gain for upward radiation */
                    Eigen::SparseLU<MatrixS, Eigen::AMDOrdering<int>> G_bt;
                    G_bt.compute(I - l2.reflectionTop * l1.reflectionBottom);

                    /* Transmission at the bottom due to illumination at the top */
                    MatrixS result = G_tb.solve(l1.transmissionTopBottom);
                    MatrixS Ttb = l2.transmissionTopBottom * result;

                    /* Reflection at the bottom */
                    MatrixS temp = l1.reflectionBottom * l2.transmissionBottomTop;
                    result = G_tb.solve(temp);
                    MatrixS Rb = l2.reflectionBottom + l2.transmissionTopBottom * result;

                    /* Transmission at the top due to illumination at the bottom */
                    result = G_bt.solve(l2.transmissionBottomTop);
                    MatrixS Tbt = l1.transmissionBottomTop * result;

                    /* Reflection at the top */
                    temp = l2.reflectionTop * l1.transmissionTopBottom;
                    result = G_bt.solve(temp);
                    MatrixS Rt = l1.reflectionTop + l1.transmissionBottomTop * result;

                    #if defined(DROP_THRESHOLD)
                        Ttb.prune((Float) 1, (Float) DROP_THRESHOLD);
                        Tbt.prune((Float) 1, (Float) DROP_THRESHOLD);
                        Rb.prune((Float) 1, (Float) DROP_THRESHOLD);
                        Rt.prune((Float) 1, (Float) DROP_THRESHOLD);
                    #endif

                    lo.transmissionTopBottom = Ttb;
                    lo.transmissionBottomTop = Tbt;
                    lo.reflectionTop = Rt;
                    lo.reflectionBottom = Rb;
                }
            }
        );
    }
}

#if !defined(HAVE_FFTW)
void Layer::setMatusik(const fs::path &, int, int) {
    throw std::runtime_error("setMatusik(): You need to recompile with support for FFTW!");
}
#else
void Layer::setMatusik(const fs::path &path, int ch, int order) {
    if (ch < 0 || ch >= 3)
        throw std::runtime_error("Channel must be between 1 and 3");
    double scale[3] = { 1.0/1500.0, 1.15/1500.0, 1.66/1500.0 };

    FILE *f = fopen(path.str().c_str(), "rb");
    if (f == nullptr)
        throw std::runtime_error("I/O error: could not open file " + path.str());

    struct {
        int res_theta_h;
        int res_theta_d;
        int res_phi_d;
    } header;

    if (fread(&header, sizeof(int), 3, f) != 3)
        throw std::runtime_error("I/O error while loading header");

    Log("Loading Matusik-style BRDF data file \"%s\" (%ix%ix%i)",
        path.str(), header.res_theta_h, header.res_theta_d, header.res_phi_d);

    size_t nValues = 3 * header.res_theta_h * header.res_theta_d * header.res_phi_d;

    double *storage = new double[nValues];
    if (fread(storage, sizeof(double), nValues, f) != nValues)
        throw std::runtime_error("I/O error while loading file contents");

    fftw_plan_with_nthreads(1);

    order = std::max(order, (int) fourierOrders());
    int fftSize = order * 4;
    fftw_plan plan = fftw_plan_r2r_1d(fftSize, NULL, NULL, FFTW_REDFT00, FFTW_ESTIMATE);

    size_t n = resolution(), nEntries = n*n;
    tbb::spin_mutex mutex;
    std::vector<Quartet> quartets;

    tbb::parallel_for(
        tbb::blocked_range<size_t>(0, nEntries, 1),
        [&](const tbb::blocked_range<size_t> &range) {
            for (size_t entry = range.begin(); entry < range.end(); ++entry) {
                int i = entry / n, o = entry % n;

                Float cosThetaI = m_nodes[i],
                      sinThetaI = std::sqrt(1-cosThetaI*cosThetaI),
                      cosThetaO = m_nodes[o],
                      sinThetaO = std::sqrt(1-cosThetaO*cosThetaO);

                Vector wi(sinThetaI, 0, cosThetaI);

                if (cosThetaI * cosThetaO > 0 || cosThetaI < 0)
                    continue;

                double *data = (double *) fftw_malloc(fftSize * sizeof(double));

                for (int j=0; j<fftSize; ++j) {
                    Float phi_d = M_PI * j/(Float) (fftSize-1),
                          cosPhi = std::cos(phi_d),
                          sinPhi = std::sin(phi_d);
                    Vector wo(-sinThetaO*cosPhi, -sinThetaO*sinPhi, -cosThetaO);

                    Vector half = (wi + wo).normalized();

                    Float theta_half = std::acos(half.z());
                    Float phi_half = std::atan2(half.y(), half.x());

                    Vector diff =
                          Eigen::AngleAxis<Float>(-theta_half, Vector(0, 1, 0)) *
                         (Eigen::AngleAxis<Float>(-phi_half, Vector(0, 0, 1)) * wi);

                    int theta_half_idx = std::min(std::max(0, (int) std::sqrt(
                        ((theta_half / (M_PI/2.0))*header.res_theta_h) * header.res_theta_h)), header.res_theta_h-1);

                    Float theta_diff = std::acos(diff.z());
                    Float phi_diff = std::atan2(diff.y(), diff.x());

                    if (phi_diff < 0)
                        phi_diff += M_PI;

                    int phi_diff_idx = std::min(std::max(0, int(phi_diff / M_PI * header.res_phi_d)), header.res_phi_d - 1);

                    int theta_diff_idx = std::min(std::max(0, int(theta_diff / (M_PI * 0.5) * header.res_theta_d)),
                        header.res_theta_d - 1);

                    int ind = phi_diff_idx +
                        theta_diff_idx * header.res_phi_d +
                        theta_half_idx * header.res_phi_d * header.res_theta_d;

                    data[j] = storage[ind+ch*header.res_theta_h*header.res_theta_d*header.res_phi_d] * scale[ch] * 2; /// XXX too dark?
                }
                double *spectrum = (double *) fftw_malloc(fftSize * sizeof(double));
                fftw_execute_r2r(plan, data, spectrum);

                for (int j=0; j<fftSize; ++j)
                    spectrum[j] /= (double) (fftSize-1);
                spectrum[0] /= 2;
                spectrum[fftSize-1] /= 2;

                double ref = std::abs(spectrum[0]);
                size_t sparseSize = 0;
                double partialSum = 0;
                if (ref != 0) {
                    sparseSize = fourierOrders();
                    for (size_t j= fourierOrders()-1; j>=1; --j) {
                        double value = (float) spectrum[j];
                        partialSum += std::abs(value);
                        if (partialSum <= ref * ERROR_GOAL)
                            sparseSize = j;
                    }
                }

                tbb::spin_mutex::scoped_lock lock(mutex);
                for (size_t l=0; l<sparseSize; ++l)
                    quartets.push_back(Quartet(l, o, i, (Float) spectrum[l]));
                fftw_free(spectrum);
                fftw_free(data);
            }
        }
    );

    fftw_destroy_plan(plan);
    delete[] storage;

    setQuartets(quartets);
    applySurfaceIntegrationWeights(*this);
}
#endif

void Layer::reverse() {
    for (auto &m: m_modes)
        m.reverse();
}

void Layer::clear() {
    for (auto &m: m_modes)
        m.clear();
}

void Layer::expand(Float target_tau) {
    /* Heuristic for choosing the initial width of a layer based on
       "Discrete Space Theory of Radiative Transfer" by Grant and Hunt
       Proc. R. Soc. London 1969 */
    Float tau = std::min(m_nodes.cwiseAbs().minCoeff() * 2, (Float) std::pow(2.0, -15.0));

    size_t doublings = (size_t) std::ceil(std::log(target_tau / tau) / std::log(2.0));
    tau = target_tau * std::pow((Float) 2.0f, -(Float) doublings);

    size_t n = resolution() / 2;

    MatrixS I((int) n, (int) n);
    I.setIdentity();

    MatrixS rowScale = sparseDiagonal(m_nodes.tail(n).cwiseInverse() * tau);

    tbb::parallel_for(
        tbb::blocked_range<size_t>(0, fourierOrders(), 1),
        [&](const tbb::blocked_range<size_t> &range) {
            for (size_t i = range.begin(); i < range.end(); ++i) {
                LayerMode &mode = m_modes[i];
                MatrixS Rt = rowScale * mode.reflectionTop;
                MatrixS Ttb = I + rowScale * (mode.transmissionTopBottom - I);

                mode.reflectionTop = Rt;
                mode.reflectionBottom = Rt;
                mode.transmissionTopBottom = Ttb;
                mode.transmissionBottomTop = Ttb;
            }
        }
    );

    for (size_t i = 0; i < (size_t) doublings; ++i)
        addToTop(*this, true);
}

std::string Layer::toString() const {
    size_t nz = 0, nz_max = resolution() * resolution() * fourierOrders();
    for (auto const &mode: m_modes)
        nz += mode.nonZeros();
    std::ostringstream oss;
    oss.precision(2);
    oss << "Layer[resolution=" << resolution() << "x" << resolution()
        << ", fourierOrders=" << fourierOrders() << ", nonZeros=" << nz << "/"
        << nz_max << " (" << ((Float) nz / (Float) nz_max * 100) << "%)]";
    return oss.str();
}

MatrixX Layer::matrix(size_t l) const {
    if (l >= m_modes.size())
        throw std::runtime_error("Layer::matrix(): out of bounds!");
    size_t n = resolution(), h = n / 2;
    MatrixX M(n, n);

    M.topLeftCorner(h, h)     = m_modes[l].transmissionBottomTop;
    M.bottomRightCorner(h, h) = m_modes[l].transmissionTopBottom;
    M.topRightCorner(h, h)    = m_modes[l].reflectionTop;
    M.bottomLeftCorner(h, h)  = m_modes[l].reflectionBottom;

    for (size_t i = 0; i < n; ++i)
        M.col(i) /= m_weights[i] * std::abs(m_nodes[i]) * (l == 0 ? 2.f : 1.f) * math::Pi;

    M.block(0, 0, h, n) = M.block(0, 0, h, n).colwise().reverse().eval();
    M.block(0, 0, h, n) = M.block(0, 0, n, h).rowwise().reverse().eval();

    return M;
}

Float Layer::eval(Float mu_o, Float mu_i, Float phi_d) const {
    int n = m_nodes.size(), h = n / 2;
    ssize_t offset_o, offset_i;
    Float weights_o[4], weights_i[4];

    if (mu_o < 0 || (mu_o == 0 && mu_i > 0)) {
        spline::evalSplineWeights(m_nodes.data() + h, h, -mu_o, offset_o, weights_o, true);
    } else {
        spline::evalSplineWeights(m_nodes.data() + h, h, mu_o, offset_o, weights_o, true);
        offset_o += h;
    }

    if (mu_i < 0 || (mu_i == 0 && mu_o > 0)) {
        spline::evalSplineWeights(m_nodes.data() + h, h, -mu_i, offset_i, weights_i, true);
    } else {
        spline::evalSplineWeights(m_nodes.data() + h, h, mu_i, offset_i, weights_i, true);
        offset_i += h;
    }

    Float result = 0;
    for (size_t l=0; l<fourierOrders(); ++l) {
        Float sum = 0;
        for (int o = 0; o < 4; ++o) {
            for (int i = 0; i < 4; ++i) {
                Float weight = weights_o[o] * weights_i[i];
                if (weight == 0)
                    continue;
                weight /=
                    std::abs(m_nodes[offset_i + i]) *
                    m_weights[offset_i + i] *
                    (l == 0 ? 2 : 1);

                sum += m_modes[l].coeff(offset_o + o, offset_i + i) * weight;
            }
        }
        result += sum * std::cos(phi_d * l);
    }

    return std::max((Float) 0.f, result / math::Pi);
}

std::string LayerMode::toString() const {
    size_t nz = nonZeros(), nz_max = resolution() * resolution();
    std::ostringstream oss;
    oss.precision(2);
    oss << "LayerMode[resolution=" << resolution() << "x" << resolution()
        << ", nonZeros=" << nz << "/" << nz_max << " ("
        << ((Float) nz / (Float) nz_max * 100) << "%)]";
    return oss.str();
}

std::pair<int, int> parameterHeuristicMicrofacet(Float alpha, std::complex<Float> &eta) {
    alpha = std::min(alpha, (Float) 1);
    if (eta.real() < 1 && eta.imag() == 0)
        eta = std::complex<Float>(1.f) / eta;

    static const Float c[][9] = {
        /* IOR    A_n      B_n     C_n       D_n      A_m      B_m      C_m      D_m                                 */
        {  0.0, 35.275,  14.136,  29.287,  1.8765,   39.814,  88.992, -98.998,  39.261  },  /* Generic conductor     */
        {  1.1, 256.47, -73.180,  99.807,  37.383,  110.782,  57.576,  94.725,  14.001  },  /* Dielectric, eta = 1.1 */
        {  1.3, 100.264, 28.187,  64.425,  14.850,   45.809,  17.785, -7.8543,  12.892  },  /* Dielectric, eta = 1.3 */
        {  1.5, 74.176,  27.470,  42.454,  9.6437,   31.700,  44.896, -45.016,  19.643  },  /* Dielectric, eta = 1.5 */
        {  1.7, 80.098,  17.016,  50.656,  7.2798,   46.549,  58.592, -73.585,  25.473  },  /* Dielectric, eta = 1.7 */
    };

    int i0 = 0, i1 = 0;

    if (eta.imag() == 0) { /* Dielectric case */
        for (int i=1; i<4; ++i) {
            if (eta.real() >= c[i][0] && eta.real() <= c[i+1][0]) {
                if (std::abs(eta.real()-c[i][0]) < 0.05f) {
                    i1 = i0 = i;
                } else if (std::abs(eta-c[i+1][0]) < 0.05f) {
                    i0 = i1 = i+1;
                } else {
                    i0 = i; i1 = i+1;
                }
            }
        }

        if (!i0)
            throw std::runtime_error("Index of refraction is out of bounds (must be between 1.1 and 1.7)!");
    }

    Float n0 = std::max(c[i0][1] + c[i0][2]*std::pow(std::log(alpha), (Float) 4)*alpha, c[i0][3]+c[i0][4]*std::pow(alpha, (Float) -1.2f));
    Float n1 = std::max(c[i1][1] + c[i1][2]*std::pow(std::log(alpha), (Float) 4)*alpha, c[i1][3]+c[i1][4]*std::pow(alpha, (Float) -1.2f));
    Float m0 = std::max(c[i0][5] + c[i0][6]*std::pow(std::log(alpha), (Float) 4)*alpha, c[i0][7]+c[i0][8]*std::pow(alpha, (Float) -1.2f));
    Float m1 = std::max(c[i1][5] + c[i1][6]*std::pow(std::log(alpha), (Float) 4)*alpha, c[i1][7]+c[i1][8]*std::pow(alpha, (Float) -1.2f));

    int n_i = (int) std::ceil(std::max(n0, n1));
    int m_i = (int) std::ceil(std::max(m0, m1));

    if (n_i % 2 == 1)
        n_i += 1;

    return std::make_pair(n_i, m_i);
}

std::pair<int, int> parameterHeuristicHG(Float g) {
    g = std::abs(g);
    Float m = 5.4f/(1.0f - g) - 1.3f;
    Float n = 8.6f/(1.0f - g) - 0.2f;
    int n_i = (int) std::ceil(n), m_i = (int) std::ceil(m);
    if (n_i % 2 == 1)
        n_i += 1;
    return std::make_pair(n_i, m_i);
}

NAMESPACE_END(layer)


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

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

*/

#ifndef PIC_FILTERING_FILTER_WLS_HPP
#define PIC_FILTERING_FILTER_WLS_HPP

#include "../filtering/filter.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Sparse"
    #include "../externals/Eigen/src/SparseCore/SparseMatrix.h"
#else
    #include <Eigen/Sparse>
    #include <Eigen/src/SparseCore/SparseMatrix.h>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

class FilterWLS: public Filter
{
protected:
    /**
     * @brief singleChannel applies WLS smoothing filter for gray-scale images.
     * @param imgIn
     * @param imgOut
     * @return
     */
    Image *singleChannel(ImageVec imgIn, Image *imgOut)
    {
        Image *L = imgIn[0];

        int width  = L->width;
        int height = L->height;
        int tot    = height * width;

        Eigen::VectorXd b, x;
        b = Eigen::VectorXd::Zero(tot);

        #ifdef PIC_DEBUG
            printf("Init matrix...");
        #endif

        std::vector< Eigen::Triplet< double > > tL;

        for(int i = 0; i < height; i++) {
            int tmpInd = i * width;

            for(int j = 0; j < width; j++) {

                float Ltmp, tmp;
                int indJ;
                int indI = tmpInd + j;
                float Lref = L->data[indI];

                b[indI] = Lref;

                float sum = 0.0f;

                if((i - 1) >= 0) {
                    indJ = indI - width;
                    Ltmp = L->data[indJ];
                    tmp  = -lambda / (powf(fabsf(Ltmp - Lref), alpha) + epsilon);
                    tL.push_back(Eigen::Triplet< double > (indI, indJ, tmp));
                    sum += tmp;
                }

                if((i + 1) < height) {
                    indJ = indI + width;
                    Ltmp = L->data[indJ];
                    tmp  = -lambda / (powf(fabsf(Ltmp - Lref), alpha) + epsilon);
                    tL.push_back(Eigen::Triplet< double > (indI, indJ, tmp));
                    sum += tmp;
                }

                if((j - 1) >= 0) {
                    indJ = indI - 1;
                    Ltmp = L->data[indJ];
                    tmp  = -lambda / (powf(fabsf(Ltmp - Lref), alpha) + epsilon);
                    tL.push_back(Eigen::Triplet< double > (indI, indJ, tmp));
                    sum += tmp;
                }

                if((j + 1) < width) {
                    indJ = indI + 1;
                    Ltmp = L->data[indJ];
                    tmp  = -lambda / (powf(fabsf(Ltmp - Lref), alpha) + epsilon);
                    tL.push_back(Eigen::Triplet< double > (indI, indJ, tmp));
                    sum += tmp;
                }

                tL.push_back(Eigen::Triplet< double > (indI, indI, 1.0f - sum));
            }
        }

        #ifdef PIC_DEBUG
            printf("Ok\n");
        #endif

        Eigen::SparseMatrix<double> A = Eigen::SparseMatrix<double>(tot, tot);
        A.setFromTriplets(tL.begin(), tL.end());

        Eigen::SimplicialCholesky<Eigen::SparseMatrix<double> > solver(A);
        x = solver.solve(b);

        if(solver.info() != Eigen::Success) {
            #ifdef PIC_DEBUG
                printf("SOLVER FAILED!\n");
            #endif
            return NULL;
        }

        #ifdef PIC_DEBUG
            printf("SOLVER SUCCESS!\n");
        #endif

        #pragma omp parallel for

        for(int i = 0; i < tot; i++) {
            imgOut->data[i] = float(x(i));
        }

        return imgOut;
    }

    /**
     * @brief multiChannel applies WLS filter for color images.
     * @param imgIn
     * @param imgOut
     * @return
     */
    Image *multiChannel(ImageVec imgIn, Image *imgOut)
    {
        Image *img = imgIn[0];

        int width  = img->width;
        int height = img->height;
        int tot    = height * width;

        alpha /= 2.0f;

        int stridex = width * img->channels;

        #ifdef PIC_DEBUG
            printf("Init matrix...");
        #endif

        std::vector< Eigen::Triplet< double > > tL;

        for(int i = 0; i < height; i++) {
            int tmpInd = i * width;

            for(int j = 0; j < width; j++) {

                float sum = 0.0f;
                float tmp;
                int indJ;
                int indI = tmpInd + j;
                int indImg = indI * img->channels;

                if((i - 1) >= 0) {
                    indJ = indImg - stridex;
                    float diff = 0.0f;

                    for(int p = 0; p < img->channels; p++) {
                        float tmpDiff = img->data[indJ + p] - img->data[indImg + p];
                        diff += tmpDiff * tmpDiff;
                    }

                    tmp  = -lambda / (powf(diff, alpha) + epsilon);

                    tL.push_back(Eigen::Triplet< double > (indI, indI - width , tmp));

                    sum += tmp;
                }

                if((i + 1) < height) {
                    indJ = indImg + stridex;
                    float diff = 0.0f;

                    for(int p = 0; p < img->channels; p++) {
                        float tmpDiff = img->data[indJ + p] - img->data[indImg + p];
                        diff += tmpDiff * tmpDiff;
                    }

                    tmp  = -lambda / (powf(diff, alpha) + epsilon);
                    tL.push_back(Eigen::Triplet< double > (indI, indI + width , tmp));
                    sum += tmp;
                }

                if((j - 1) >= 0) {
                    indJ = indImg - img->channels;
                    float diff = 0.0f;

                    for(int p = 0; p < img->channels; p++) {
                        float tmpDiff = img->data[indJ + p] - img->data[indImg + p];
                        diff += tmpDiff * tmpDiff;
                    }

                    tmp  = -lambda / (powf(diff, alpha) + epsilon);
                    tL.push_back(Eigen::Triplet< double > (indI, indI - 1 , tmp));
                    sum += tmp;
                }

                if((j + 1) < width) {
                    indJ = indImg + img->channels;
                    float diff = 0.0f;

                    for(int p = 0; p < img->channels; p++) {
                        float tmpDiff = img->data[indJ + p] - img->data[indImg + p];
                        diff += tmpDiff * tmpDiff;
                    }

                    tmp  = -lambda / (powf(diff, alpha) + epsilon);

                    tL.push_back(Eigen::Triplet< double > (indI, indI + 1 , tmp));
                    sum += tmp;
                }

                tL.push_back(Eigen::Triplet< double > (indI, indI, 1.0f - sum));
            }
        }

        #ifdef PIC_DEBUG
            printf("Ok\n");
        #endif

        Eigen::SparseMatrix<double> A = Eigen::SparseMatrix<double>(tot, tot);

        A.setFromTriplets(tL.begin(), tL.end());

        Eigen::SimplicialCholesky< Eigen::SparseMatrix< double > > solver(A);

        for(int i = 0; i < imgOut->channels; i++) {
            Eigen::VectorXd b, x;

            b = Eigen::VectorXd::Zero(tot);
            #pragma omp parallel for

            for(int j = 0; j < tot; j++) {
                b[j] = img->data[j * img->channels + i];
            }

            x = solver.solve(b);

            if(solver.info() == Eigen::Success) {

                #ifdef PIC_DEBUG
                    printf("SOLVER SUCCESS!\n");
                #endif

                #pragma omp parallel for

                for(int j = 0; j < tot; j++) {
                    imgOut->data[j * imgOut->channels + i] = float(x(j));
                }
            } else {
                #ifdef PIC_DEBUG
                    printf("SOLVER FAILED!\n");
                #endif
            }

        }

        return imgOut;
    }

    float alpha, lambda, epsilon;

public:

    /**
     * @brief FilterWLS
     */
    FilterWLS() : Filter()
    {
        update(1.2f, 1.0f);
    }

    /**
     * @brief FilterWLS
     * @param alpha
     * @param lambda
     */
    FilterWLS(float alpha, float lambda) : Filter()
    {
        update(alpha, lambda);
    }

    /**
     * @brief update
     * @param alpha
     * @param lambda
     */
    void update(float alpha, float lambda)
    {
        epsilon = 0.0001f;

        if(alpha <= 0.0f) {
            alpha = 1.2f;
        }

        if(lambda <= 0.0f) {
            lambda = 1.0f;
        }

        this->alpha = alpha;
        this->lambda = lambda;
    }

    /**
     * @brief Process
     * @param imgIn
     * @param imgOut
     * @return
     */
    Image *Process(ImageVec imgIn, Image *imgOut)
    {
        if(imgIn.empty()){
            return imgOut;
        }

        if(imgIn[0] == NULL) {
            return imgOut;
        }

        imgOut = setupAux(imgIn, imgOut);

        if(imgOut == NULL) {
            return imgOut;
        }

        //convolution
        if(imgIn[0]->channels == 1) {
            return singleChannel(imgIn, imgOut);
        } else {
            return multiChannel(imgIn, imgOut);
        }
    }

    /**
     * @brief main
     * @param argc
     * @param argv
     * @return
     */
    static int main(int argc, char* argv[])
    {
        if(argc < 4) {
            printf("Usage: name_input alpha lambad\n");
            return 0;
        }

        std::string nameIn = argv[1];
        std::string name = removeExtension(nameIn);
        std::string ext = getExtension(nameIn);

        float alpha = float(atof(argv[2]));
        float lambda = float(atof(argv[3]));

        std::string nameOut = name + "_wls." + ext;

        Image img(nameIn);

        FilterWLS *filter = new FilterWLS(alpha, lambda);

        filter->Process(Single(&img), NULL)->Write(nameOut);

        return 0;
    }
};
#endif

} // end namespace pic

#endif /* PIC_FILTERING_FILTER_WLS_HPP */


#include "stdio.h"
#include "stdlib.h"
#include "string.h"
#include <iostream>
#include <torch/torch.h>
#include <boost/circular_buffer.hpp>
#include <vector>

using namespace torch;
using Experience = std::tuple<torch::Tensor, torch::Tensor, torch::Tensor, torch::Tensor, torch::Tensor>;   //this is how we are going to save expirience in buffer

/*              Critic definition               */
struct CriticImpl: nn::Module
{
    CriticImpl(int state_size, int action_size, int layer1,int layer2, int layer3, int out):
    CLayer_state(nn::Linear(state_size,state_size)),
    CLayer_action(nn::Linear(action_size,action_size)),
    CLayer1(nn::Linear(state_size+action_size,layer1)),
    //Cbatch_norm1(layer1),
    CLayer2(nn::Linear(layer1,layer2)),
    //Cbatch_norm2(layer2),
    CLayer3(nn::Linear(layer2,layer3)),
    //Cbatch_norm3(layer3),
    CLayer4(nn::Linear(layer3,out))
    {
        register_module("CLayer_state",CLayer_state);
        register_module("CLayer_action",CLayer_action);
        register_module("CLayer1",CLayer1);
        register_module("CLayer2",CLayer2);
        register_module("CLayer3",CLayer3);
        register_module("CLayer4",CLayer4);
        //register_module("Cbatch_norm1",Cbatch_norm1);
        //register_module("Cbatch_norm2",Cbatch_norm2);
        //register_module("Cbatch_norm3",Cbatch_norm3);
    }

    torch::Tensor forward(torch::Tensor state , torch::Tensor action)
    {
        if (state.dim() == 1)
            state = torch::unsqueeze(state, 0);//Returns a new tensor with a dimension of size one inserted at the specified position.

        if (action.dim() == 1)
            action = torch::unsqueeze(action,0);
        state = torch::tanh(CLayer_state(state));
        action = torch::tanh(CLayer_action(action));
        auto xa = torch::cat({state,action}, /*dim=*/1);
        xa=torch::tanh(CLayer1(xa));
        xa=torch::tanh(CLayer2(xa));
        xa=torch::tanh(CLayer3(xa));
        xa=CLayer4(xa);
        return xa;
    }
    void print_params()
    {
        for (const auto& p : this->parameters())
        {
            std::cout << p << std::endl;
        }
    }

    torch::nn::Linear CLayer_state,CLayer_action,CLayer1,CLayer2,CLayer3,CLayer4;
    //torch::nn::BatchNorm1d Cbatch_norm1, Cbatch_norm2, Cbatch_norm3;
};
TORCH_MODULE(Critic);       //refer to https://pytorch.org/tutorials/advanced/cpp_frontend.html

/*              Actor definition            */
struct ActorImpl: nn::Module
{
    ActorImpl(int state_size, int layer1,int layer2, int layer3, int layer4, int out):
    //ALayer1(nn::Linear(nn::LinearOptions(state_size, layer1).bias(false))),
    ALayer1(nn::Linear(state_size,layer1)),
    ALayer2(nn::Linear(layer1,layer2)),
    ALayer3(nn::Linear(layer2,layer3)),
    ALayer4(nn::Linear(layer3,layer4)),
    ALayer5(nn::Linear(layer4,out))
    {
        register_module("ALayer1",ALayer1);
        register_module("ALayer2",ALayer2);
        register_module("ALayer3",ALayer3);
        register_module("ALayer4",ALayer4);
        register_module("ALayer5",ALayer5);
    }

    torch::Tensor forward(torch::Tensor x, float action_scale)
    {
        x=torch::tanh(ALayer1(x));
        x=torch::tanh(ALayer2(x));
        x=torch::tanh(ALayer3(x));
        x=torch::tanh(ALayer4(x));
        x=action_scale*torch::tanh(ALayer5(x));
        return x;
    }
    void print_params()
    {
        for (const auto& p : this->parameters())
        {
            std::cout << p << std::endl;
        }
    }
    torch::nn::Linear ALayer1,ALayer2,ALayer3,ALayer4,ALayer5;
};
TORCH_MODULE(Actor);

class ReplayBuffer
{
        public:
            ReplayBuffer() {}

            void addExperienceState(torch::Tensor state, torch::Tensor action, torch::Tensor reward, torch::Tensor next_state, torch::Tensor done)// add the 5 info to buffer
            {
                addExperienceState(std::make_tuple(state, action, reward, next_state, done));//but first convert them to a tuple
            }

            void addExperienceState(Experience experience) {
             circular_buffer.push_back(experience);//finally add them
            }

         Experience sample(int num_agent)//make a batch of size = num_Agent(rows) x 5(colums)// we need to transpose for batches
         {
             Experience experiences;
             Experience exp = sample();
             experiences = exp;

                for (short i = 0; i < num_agent-1; i++) {
                    exp=sample();
                    std::get<0>(experiences) = torch::cat({std::get<0>(experiences),std::get<0>(exp)}, /*dim=*/0);
                    std::get<1>(experiences) = torch::cat({std::get<1>(experiences),std::get<1>(exp)}, /*dim=*/0);
                    std::get<2>(experiences) = torch::cat({std::get<2>(experiences),std::get<2>(exp)}, /*dim=*/0);
                    std::get<3>(experiences) = torch::cat({std::get<3>(experiences),std::get<3>(exp)}, /*dim=*/0);
                    std::get<4>(experiences) = torch::cat({std::get<4>(experiences),std::get<4>(exp)}, /*dim=*/0);

                 }
             return experiences;
          }

          Experience sample() {
                  Experience exp = circular_buffer.at(static_cast<size_t>(rand() % static_cast<int>(circular_buffer.size())));//sample a data point randomly
                  //for(size_t i=0; i<5; i++)
                        std::get<0>(exp) = torch::unsqueeze(std::get<0>(exp), 0);
                        std::get<1>(exp) = torch::unsqueeze(std::get<1>(exp), 0);
                        std::get<2>(exp) = torch::unsqueeze(std::get<2>(exp), 0);
                        std::get<3>(exp) = torch::unsqueeze(std::get<3>(exp), 0);
                        std::get<4>(exp) = torch::unsqueeze(std::get<4>(exp), 0);
                   return exp;
          }

          size_t getLength() {
              return circular_buffer.size();//size of the buffer
          }

          boost::circular_buffer<Experience> circular_buffer{1000000};//max size of buffer = 10000
};      //refer to https://github.com/EmmiOcean/DDPG_LibTorch/blob/master/replayBuffer.h

class OUNoise {
//"""Ornstein-Uhlenbeck process."""
    private:
        size_t size;
        std::vector<double> mu;
        std::vector<double> state;
        double theta=0.15;
        double sigma=0.1;

    public:
        OUNoise (size_t size_in) {
            size = size_in;
            mu = std::vector<double>(size, 0);
            reset();
        }

        void reset() {
            state = mu;
        }

        std::vector<double> sample(std::vector<double> action) {
        //"""Update internal state and return it as a noise sample."""
            for (size_t i = 0; i < state.size(); i++) {
                auto random = ((double) rand() / (RAND_MAX));
                double dx = theta * (mu[i] - state[i]) + sigma * random;
                state[i] = state[i] + dx;
                action[i] += state[i];
            }
            return action;
    }
};


//
//  Copyright (C) 2020 Brian P. Kelley
//
//   @@ All Rights Reserved @@
//  This file is part of the RDKit.
//  The contents are covered by the terms of the BSD license
//  which is included in the file license.txt, found at the root
//  of the RDKit source tree.
//
#ifdef RDK_HAS_EIGEN3
#include "BCUT.h"
#include "Crippen.h"
#include <Eigen/Dense>
#include <GraphMol/RDKitBase.h>
#include "GraphMol/PartialCharges/GasteigerCharges.h"
#include "GraphMol/PartialCharges/GasteigerParams.h"
#include <RDGeneral/types.h>

namespace RDKit {
namespace Descriptors {
  // diagonal elements are a property (atomic num, charge, etc)
  // off diagonal are 1/sqrt(bond_order)
  //  Original burden matrix was .1, .2, .3, .15 for single,double,triple or aromatic
  //  all other elements are .001
namespace {
std::unique_ptr<Eigen::MatrixXd>  make_burden(const ROMol &m) {
  auto num_atoms = m.getNumAtoms();
  std::unique_ptr<Eigen::MatrixXd> burden(
	 new Eigen::MatrixXd(num_atoms, num_atoms));

  for(unsigned int i=0;i<num_atoms; ++i) {
    for(unsigned int j=0;j<num_atoms; ++j) {
      (*burden)(i,j) = (*burden)(j,i) = 0.001;
    }
  }

  for(auto &bond : m.bonds()) {
    unsigned int i = bond->getBeginAtomIdx();
    unsigned int j = bond->getEndAtomIdx();
    double score = 0.0;
    switch(bond->getBondType()) {
    case Bond::AROMATIC:
      // score = 0.15; orig burden
      score = 0.8164965809277261; // 1/sqrt(1.5)
      break;
    case Bond::SINGLE:
      // score = 0.1;
      score = 1.0; // 1/sqrt(1.0)
      break;
    case Bond::DOUBLE:
      // score = 0.2;
      score = 0.7071067811865475; // 1/sqrt(2.0)
      break;
    case Bond::TRIPLE:
      // score = 0.3;
      score = 0.5773502691896258; // 1/sqrt(3);
      break;
    default:
      CHECK_INVARIANT(0, "Bond order must be Single, Double, Triple or Aromatic");
    }
    (*burden)(i,j) = (*burden)(j,i) = score;
  }
  return burden;
}

std::pair<double,double> BCUT2D(std::unique_ptr<Eigen::MatrixXd> &burden,
				const std::vector<double> &atom_props) {
  for(unsigned int i=0; i<atom_props.size(); ++i) {
    (*burden)(i,i) = atom_props[i];
  }
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(*burden);
  auto eivals = es.eigenvalues();
  double lowest = eivals(0);
  double highest = eivals(atom_props.size()-1);
  return std::pair<double,double>(highest,lowest);
}
}

std::pair<double,double> BCUT2D(const ROMol &m, const std::vector<double> &atom_props) {
  unsigned int num_atoms = m.getNumAtoms();
  PRECONDITION(atom_props.size() == num_atoms, "Number of atom props not equal to number of atoms");

  if (num_atoms == 0) {
    return std::pair<double,double>(0,0);
  }
  auto burden = make_burden(m);
  return BCUT2D(burden, atom_props);
}

std::pair<double,double> BCUT2D(const ROMol &m, const std::string &atom_double_prop) {
  std::vector<double> props;
  props.reserve(m.getNumAtoms());
  for(auto &atom : m.atoms()) {
    props.push_back(atom->getProp<double>(atom_double_prop));
  }
  return BCUT2D(m, props);
}

std::vector<double> BCUT2D(const ROMol &m) {
  std::unique_ptr<ROMol> mol(MolOps::removeHs(m));
  std::vector<double> masses;
  std::vector<double> charges;
  unsigned int num_atoms = mol->getNumAtoms();
  masses.reserve(num_atoms);
  charges.reserve(num_atoms);

  RDKit::computeGasteigerCharges(*mol, 12, true);
  for(auto &atom: mol->atoms()) {
    masses.push_back(atom->getMass());
    charges.push_back(atom->getProp<double>(common_properties::_GasteigerCharge));
  }

  std::vector<double> slogp(num_atoms, 0.0);
  std::vector<double> cmr(num_atoms, 0.0);
  getCrippenAtomContribs(*mol, slogp, cmr);

  // polarizability? - need model
  // slogp?  sasa?
  auto burden = make_burden(m);
  auto atom_bcut = BCUT2D(burden, masses);
  auto gasteiger = BCUT2D(burden, charges);
  auto logp = BCUT2D(burden, slogp);
  auto mr = BCUT2D(burden, cmr);
  std::vector<double> res = {atom_bcut.first, atom_bcut.second,
			     gasteiger.first, gasteiger.second,
			     logp.first, logp.second,
			     mr.first, mr.second
  };
  return res;
}
}
}

#endif


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

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_REM_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REM_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-operator

    This function object computes the remainder of the two parameters of the
    same type with or without option.

    @par Header <boost/simd/function/rem.hpp>

    @par Notes

    For any value @c a and @c b of same type `rem({option, }a, b)`
    returns the quotient of @c a by @c b respective to the option specified.

    - Options

      With no option this function is equivalent to `x-div(fix, x, y)*y`

      `option` may be @ref ceil, @ref floor, @ref fix, @ref round, @ref nearbyint (in the
      namespace booost::simd).
      Each option provides the same result as `x-div(option, x, y)*y`.

    - Supported types

      unsigned types are not supported but for the option @c fix, as in other cases results
      can be negative

    - Limiting values for floating entries:

       - if x is +/-inf , Nan is returned
       - if x is +/-0 and y is not 0, 0 is returned (if the sign of x matters the pedantic_
         decorated version returns x)
       - if y is +/-0, Nan is returned
       - if either argument is NaN, Nan is returned

    @par Decorators

      - pedantic_ insure better limiting cases (see above note)

      - std_ allow direct calls to some libc++ function:

        - option fix        calls std::fmod
        - option nearbyint  calls std::remainder
        - The other options have no standard equivalent

    @see div

    @par Example:

      @snippet rem.cpp rem

    @par Possible output:

      @snippet rem.txt rem

  **/
  Value rem(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/rem.hpp>
#include <boost/simd/function/simd/rem.hpp>

#endif


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_FACESFROMTETS_HPP
#define MCL_FACESFROMTETS_HPP 1

#include <Eigen/Dense>
#include <set>

namespace mcl
{

// Given a tet mesh T, compute surface triangles F.
// True on success
template <typename DerivedT, typename DerivedF>
static inline bool faces_from_tets(
	const Eigen::MatrixBase<DerivedT> &T,
	Eigen::PlainObjectBase<DerivedF> &F)
{
	using namespace Eigen;
	struct FaceKey
	{
		FaceKey() : f(Vector3i::Zero()), f_sorted(Vector3i::Zero()) {}
		FaceKey(int f0, int f1, int f2)
		{
			f = Vector3i(f0, f1, f2);
			f_sorted = f;
			mcl::sort3(f_sorted[0], f_sorted[1], f_sorted[2]);
		}
		Vector3i f;
		Vector3i f_sorted;
		bool operator<(const FaceKey& other) const
		{
			for (int i=0; i<3; ++i)
			{
				if (f_sorted[i] < other.f_sorted[i]) { return true; }
				if (f_sorted[i] > other.f_sorted[i]) { return false; }
			}
			return false;
		}
	};

	if (T.rows()==0 || T.cols()!=4) {
		return false;
	}

	std::set<FaceKey> faces;
	std::set<FaceKey> faces_seen_twice;
	int n_tets = T.rows();
	int total_faces = 0;
	for (int t=0; t<n_tets; ++t)
	{
		total_faces += 4;
		int p0 = T(t,0);
		int p1 = T(t,1);
		int p2 = T(t,2);
		int p3 = T(t,3);
		FaceKey curr_faces[4] = {
			FaceKey(p0, p1, p3),
			FaceKey(p0, p2, p1),
			FaceKey(p0, p3, p2),
			FaceKey(p1, p2, p3) };

		for (int f=0; f<4; ++f)
		{
			const FaceKey &curr_face = curr_faces[f];
			// We've already seen the face at least twice.
			if (faces_seen_twice.count(curr_face)>0) { continue; }
			// Check that we don't already have the face
			typename std::set<FaceKey>::iterator it = faces.find(curr_face);
			if (it != faces.end())
			{
				faces.erase(it);
				faces_seen_twice.insert(curr_face);
				continue;
			}
			// Otherwise, add it to faces
			faces.insert(curr_face);
		} // end loop faces

	} // end loop tets

	int nf = faces.size();
	F.resize(nf, 3);
	typename std::set<FaceKey>::const_iterator fit = faces.begin();
	for (int f_idx=0; fit != faces.end(); ++fit, ++f_idx) {
		F.row(f_idx) = fit->f;
	}

	return true;
}

} // ns mcl

#endif


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/combine.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;

STF_CASE_TPL("combine scalar into a pack<T,2>", STF_NUMERIC_TYPES)
{
  bs::pack<T,2> res = bs::combine(T(13), T(37));
  STF_EQUAL(res[0], T(13));
  STF_EQUAL(res[1], T(37));
}

template<typename T, std::size_t N, typename Env>
void test( Env& $ )
{
  std::array<T,2*N> ref;
  for(std::size_t i=0;i<2*N;++i) ref[i] = T(1+i);

  bs::pack<T,N> first (&ref[0]  , &ref[0]+N   );
  bs::pack<T,N> second(&ref[0]+N, &ref[0]+2*N );

  STF_EXPR_IS( bs::combine(first,second), (bs::pack<T,2*N>));
  STF_ALL_EQUAL( bs::combine(first,second), ref);
}

STF_CASE_TPL("combine pack<T,N> into pack<T,2*N>", STF_NUMERIC_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  test<T,N>($);
  test<T,N/2>($);
  test<T,N*2>($);
}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file libs/numeric/ublasx/eye.cpp
 *
 * \brief Test suite for the \c eye operation.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2009, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublasx/operation/eye.hpp>
#include <cmath>
#include <complex>
#include <cstddef>
#include <iostream>
#include "libs/numeric/ublasx/test/utils.hpp"

namespace ublas = boost::numeric::ublas;
namespace ublasx = boost::numeric::ublasx;

BOOST_UBLASX_TEST_DEF( default_square_matrix )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Default Square Matrix" );

    const std::size_t n = 3;

    ublas::identity_matrix<> res = ublasx::eye(n);
    ublas::identity_matrix<> expect_res(n);

    BOOST_UBLASX_DEBUG_TRACE( "res = " << res );
    BOOST_UBLASX_TEST_CHECK_MATRIX_EQ( res, expect_res, n, n );
}

BOOST_UBLASX_TEST_DEF( real_square_matrix )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real Square Matrix" );

    typedef double value_type;

    const std::size_t n = 3;

    ublas::identity_matrix<value_type> res = ublasx::eye<value_type>(n);
    ublas::identity_matrix<value_type> expect_res(n);

    BOOST_UBLASX_DEBUG_TRACE( "res = " << res );
    BOOST_UBLASX_TEST_CHECK_MATRIX_EQ( res, expect_res, n, n );
}

BOOST_UBLASX_TEST_DEF( complex_square_matrix )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex Square Matrix" );

    typedef std::complex<double> value_type;

    const std::size_t n = 3;

    ublas::identity_matrix<value_type> res = ublasx::eye<value_type>(n);
    ublas::identity_matrix<value_type> expect_res(n);

    BOOST_UBLASX_DEBUG_TRACE( "res = " << res );
    BOOST_UBLASX_TEST_CHECK_MATRIX_EQ( res, expect_res, n, n );
}

BOOST_UBLASX_TEST_DEF( default_rect_matrix )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Default Rectangular Matrix" );

    const std::size_t nr = 3;
    const std::size_t nc = 5;

    ublas::identity_matrix<> res = ublasx::eye(nr, nc);
    ublas::identity_matrix<> expect_res(nr, nc);

    BOOST_UBLASX_DEBUG_TRACE( "res = " << res );
    BOOST_UBLASX_TEST_CHECK_MATRIX_EQ( res, expect_res, nr, nc );
}

BOOST_UBLASX_TEST_DEF( real_rect_matrix )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real Rectangular Matrix" );

    typedef double value_type;

    const std::size_t nr = 3;
    const std::size_t nc = 5;

    ublas::identity_matrix<value_type> res = ublasx::eye<value_type>(nr, nc);
    ublas::identity_matrix<value_type> expect_res(nr, nc);

    BOOST_UBLASX_DEBUG_TRACE( "res = " << res );
    BOOST_UBLASX_TEST_CHECK_MATRIX_EQ( res, expect_res, nr, nc );
}

BOOST_UBLASX_TEST_DEF( complex_rect_matrix )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex Rectangular Matrix" );

    typedef std::complex<double> value_type;

    const std::size_t nr = 3;
    const std::size_t nc = 5;

    ublas::identity_matrix<value_type> res = ublasx::eye<value_type>(nr, nc);
    ublas::identity_matrix<value_type> expect_res(nr, nc);

    BOOST_UBLASX_DEBUG_TRACE( "res = " << res );
    BOOST_UBLASX_TEST_CHECK_MATRIX_EQ( res, expect_res, nr, nc );
}

int main()
{
    BOOST_UBLASX_DEBUG_TRACE("Test Suite: 'eye' operations");

    BOOST_UBLASX_TEST_BEGIN();

    BOOST_UBLASX_TEST_DO( default_square_matrix );
    BOOST_UBLASX_TEST_DO( real_square_matrix );
    BOOST_UBLASX_TEST_DO( complex_square_matrix );
    BOOST_UBLASX_TEST_DO( default_rect_matrix );
    BOOST_UBLASX_TEST_DO( real_rect_matrix );
    BOOST_UBLASX_TEST_DO( complex_rect_matrix );

    BOOST_UBLASX_TEST_END();
}