#ifndef STAN_MATH_PRIM_SCAL_FUN_DIGAMMA_HPP
#define STAN_MATH_PRIM_SCAL_FUN_DIGAMMA_HPP

#include <stan/math/prim/scal/fun/boost_policy.hpp>
#include <boost/math/special_functions/digamma.hpp>

namespace stan {
namespace math {

/**
 * Return the derivative of the log gamma function
 * at the specified value.
 *
   \f[
   \mbox{digamma}(x) =
   \begin{cases}
     \textrm{error} & \mbox{if } x\in \{\dots, -3, -2, -1, 0\}\\
     \Psi(x) & \mbox{if } x\not\in \{\dots, -3, -2, -1, 0\}\\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{digamma}(x)}{\partial x} =
   \begin{cases}
     \textrm{error} & \mbox{if } x\in \{\dots, -3, -2, -1, 0\}\\
     \frac{\partial\, \Psi(x)}{\partial x} & \mbox{if } x\not\in \{\dots, -3,
 -2, -1, 0\}\\[6pt] \textrm{NaN} & \mbox{if } x = \textrm{NaN} \end{cases} \f]

   \f[
   \Psi(x)=\frac{\Gamma'(x)}{\Gamma(x)}
   \f]

   \f[
   \frac{\partial \, \Psi(x)}{\partial x} =
 \frac{\Gamma''(x)\Gamma(x)-(\Gamma'(x))^2}{\Gamma^2(x)} \f]

 *
 * The design follows the standard C++ library in returning NaN
 * rather than throwing exceptions.
 *
 * @param[in] x argument
 * @return derivative of log gamma function at argument
 */
inline double digamma(double x) {
  return boost::math::digamma(x, boost_policy_t());
}

}  // namespace math
}  // namespace stan
#endif


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

#define BOOST_TEST_MODULE kronecker_substitution_test

#include <vector>
#include <cstdint>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <nil/crypto3/algebra/fields/bls12/base_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/bls12.hpp>

#include <nil/crypto3/math/polynomial/basic_operations.hpp>
#include <nil/crypto3/math/kronecker_substitution.hpp>

using namespace nil::crypto3::algebra;
using namespace nil::crypto3::math;

typedef fields::bls12<381> FieldType;

BOOST_AUTO_TEST_SUITE(kronecker_substitution_test_suite)

BOOST_AUTO_TEST_CASE(standard_polynomial_multiplication) {

    std::vector<typename FieldType::value_type> a = {1, 2, 3, 1};
    std::vector<typename FieldType::value_type> b = {1, 2, 1, 1};
    std::vector<typename FieldType::value_type> c(1, FieldType::value_type::zero());

    _polynomial_multiplication_on_kronecker<FieldType>(c, a, b);

    std::vector<typename FieldType::value_type> c_answer(1, FieldType::value_type::zero());
    _polynomial_multiplication<FieldType>(c_answer, a, b);

    for (std::size_t i = 0; i < c_answer.size(); i++) {
        BOOST_CHECK_EQUAL(c_answer[i].data, c[i].data);
    }
}

BOOST_AUTO_TEST_CASE(squared_polynomial_multiplication) {

    std::vector<typename FieldType::value_type> a = {1, 2, 3, 1};
    std::vector<typename FieldType::value_type> b = a;
    std::vector<typename FieldType::value_type> c(1, FieldType::value_type::zero());

    _polynomial_multiplication_on_kronecker<FieldType>(c, a, b);

    std::vector<typename FieldType::value_type> c_answer(1, FieldType::value_type::zero());
    _polynomial_multiplication<FieldType>(c_answer, a, b);

    for (std::size_t i = 0; i < c_answer.size(); i++) {
        BOOST_CHECK_EQUAL(c_answer[i].data, c[i].data);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include "catch2/catch.hpp"
#include <boost/config.hpp>
#include <boost/random/random_device.hpp>
#include <boost/uniform_int_distribution.hpp>

static const std::string provider = "/dev/urandom";

boost::random_device device(provider);
boost::random::uniform_int distribution<unsigned short> random(1000);

TEST_CASE("Test random return between 1000 and 65535","[thousand]"){
    auto test = random(device);
    REQUIRE( test == 1000);
}


#include "rtff/fft/fftw/fftw_fft.h"

#include <iostream>
#include <vector>
#include "fftw3.h"

#include <Eigen/Core>

namespace rtff {

class FFTWFft::Impl {
 public:
  Impl() : real_to_complex_(nullptr), complex_to_real_(nullptr) {}
  ~Impl() { Cleanup(); }

  void Init(uint32_t nfft) {
    real_data_.resize(nfft);
    complex_data_.resize(nfft / 2 + 1);

    // Initialize the fftw_plan
    auto complex_data_ptr = complex_data_.data();
    auto fftw_complex_data_ptr =
        reinterpret_cast<fftwf_complex*>(complex_data_ptr);
    auto real_data_ptr = real_data_.data();
    auto fftw_flags = FFTW_ESTIMATE;

#ifdef RTFF_FFTW_USE_WISDOM
    fftw_flags = FFTW_EXHAUSTIVE;
    // make the wisdom filename
    std::string wisdom_filename("rtff_" + std::to_string(nfft) + ".fftw");
    auto export_wisdom = true;

    // if the wisdom file exists
    FILE* wisdom = fopen(wisdom_filename.c_str(), "r");
    if (wisdom) {
      export_wisdom = false;
      fftw_flags = FFTW_WISDOM_ONLY | FFTW_EXHAUSTIVE;
      fftwf_import_wisdom_from_file(wisdom);
      fclose(wisdom);
    }
#endif  // RTFF_FFTW_USE_WISDOM

    // create the plan
    real_to_complex_ = fftwf_plan_dft_r2c_1d(
        nfft, real_data_ptr, fftw_complex_data_ptr, fftw_flags);
    complex_to_real_ = fftwf_plan_dft_c2r_1d(nfft, fftw_complex_data_ptr,
                                             real_data_ptr, fftw_flags);

#ifdef RTFF_FFTW_USE_WISDOM
    // export the wisdom if it didn't exist
    if (export_wisdom) {
      fftwf_export_wisdom_to_filename(wisdom_filename.c_str());
    }
#endif  // RTFF_FFTW_USE_WISDOM
  }

  void Forward(const float* in, std::complex<float>* out) {
    auto complex_data_ptr = complex_data_.data();
    auto real_data_ptr = real_data_.data();

    std::copy(in, in + get_nfft(), real_data_ptr);
    fftwf_execute(real_to_complex_);
    std::copy(complex_data_ptr, complex_data_ptr + get_nfft() / 2 + 1, out);
  }

  void Backward(const std::complex<float>* in, float* out) {
    auto complex_data_ptr = complex_data_.data();
    auto real_data_ptr = real_data_.data();

    std::copy(in, in + get_nfft() / 2 + 1, complex_data_ptr);
    // TODO also rebuild the complex conjugate in the second part
    fftwf_execute(complex_to_real_);
    std::copy(real_data_ptr, real_data_ptr + get_nfft(), out);

    // we need to devide the output by nfft
    Eigen::Map<Eigen::MatrixXf> out_matrix(out, get_nfft(), 1);
    out_matrix /= get_nfft();
  }

 private:
  uint32_t get_nfft() const { return real_data_.size(); }

  void Cleanup() {
    if (real_to_complex_) {
      fftwf_destroy_plan(real_to_complex_);
    }
    if (complex_to_real_) {
      fftwf_destroy_plan(complex_to_real_);
    }
  }

  std::vector<float> real_data_;
  std::vector<std::complex<float>> complex_data_;
  fftwf_plan real_to_complex_;
  fftwf_plan complex_to_real_;
};

FFTWFft::FFTWFft() : impl_(std::make_shared<FFTWFft::Impl>()) {}

void FFTWFft::Init(uint32_t nfft, std::error_code& err) { impl_->Init(nfft); }

void FFTWFft::Forward(const float* in, std::complex<float>* out) {
  impl_->Forward(in, out);
}

void FFTWFft::Backward(const std::complex<float>* in, float* out) {
  impl_->Backward(in, out);
}

}  // namespace rtff


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2012 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef ALGORITHM_3_ERROR_ESTIMATOR_HH
#define ALGORITHM_3_ERROR_ESTIMATOR_HH

#include <algorithm>
#include <iostream>
#include <utility>
#include <vector>

#include <boost/timer/timer.hpp>
#include <boost/fusion/include/at_c.hpp>

#include "algorithm/newton_bridge.hh"
#include "linalg/jacobiPreconditioner.hh"
#include "fem/hierarchicErrorEstimator.hh"
#include "fem/variables.hh"
#include "linalg/direct.hh"
#include "linalg/blockDiagonalSchurPreconditioner.hh"
#include "linalg/tcg.hh"
#include "utilities/enums.hh"

// forward declarations
namespace Dune
{
  struct InverseOperatorResult;
  template <class,int> class FieldVector;
}

namespace Kaskade
{
  template <template <class,class,class,bool> class Functional, class VariableSetDescription, class ExtensionVariableSetDescription, class ExtensionSpace,
            class NormFunctional, class LinearSolverLA, class LinearSolverHA, class LinearSolverLM=LinearSolverLA, class LinearSolverHM=LinearSolverHA,
            bool lumpM=false>
  class HierarchicalBasisErrorEstimator2 : public AbstractHierarchicalErrorEstimator
  {
    static constexpr int noOfVariables = ExtensionVariableSetDescription::noOfVariables;
//    typedef typename LF::AnsatzVars LVars;
    static constexpr int yId = 1, uId = 0, pId = 2;
  public:
    typedef Functional<VariableSetDescription,VariableSetDescription,VariableSetDescription,lumpM>                             FLL;
    typedef Functional<ExtensionVariableSetDescription,VariableSetDescription,ExtensionVariableSetDescription,lumpM>         FHL;
    typedef Functional<VariableSetDescription,ExtensionVariableSetDescription,VariableSetDescription,lumpM>                    FLH;
    typedef Functional<ExtensionVariableSetDescription,ExtensionVariableSetDescription,ExtensionVariableSetDescription,lumpM>  FHH;

    typedef HierarchicErrorEstimator<LinearizationAt<FLL>,ExtensionVariableSetDescription,ExtensionVariableSetDescription,HierarchicErrorEstimatorDetail::TakeAllD2<LinearizationAt<FLL> > > ErrorEstimator;
    typedef VariationalFunctionalAssembler<ErrorEstimator> Assembler;

    typedef VariationalFunctionalAssembler<LinearizationAt<FLL> >                                 Assll;
    typedef VariationalFunctionalAssembler<LinearizationAt<FLH> >                                 Asslh;
    typedef VariationalFunctionalAssembler<LinearizationAt<FHL> >                                 Asshl;
    typedef VariationalFunctionalAssembler<LinearizationAt<FHH> >                                 Asshh;

    typedef AssembledGalerkinOperator<Asshh,pId,pId+1,yId,yId+1>                                              Ahh;
    typedef AssembledGalerkinOperator<Assll,pId,pId+1,yId,yId+1>                                              All;
    typedef AssembledGalerkinOperator<Asslh,pId,pId+1,yId,yId+1>                                              Alh;
    typedef AssembledGalerkinOperator<Asshl,pId,pId+1,yId,yId+1>                                              Ahl;
    typedef AssembledGalerkinOperator<Asshh,yId,yId+1,yId,yId+1>                                              Myyhh;

    typedef AssembledGalerkinOperator<Assll,pId,pId+1,uId,uId+1>                                              Bll;
    typedef AssembledGalerkinOperator<Asslh,pId,pId+1,uId,uId+1>                                              Blh;
    typedef AssembledGalerkinOperator<Asshl,pId,pId+1,uId,uId+1>                                              Bhl;
    typedef AssembledGalerkinOperator<Asshh,pId,pId+1,uId,uId+1>                                              Bhh;

    typedef AssembledGalerkinOperator<Assll,uId,uId+1,uId,uId+1>                                             Muull;
    typedef AssembledGalerkinOperator<Asslh,uId,uId+1,uId,uId+1>                                             Muulh;
    typedef AssembledGalerkinOperator<Asshl,uId,uId+1,uId,uId+1>                                             Muuhl;
    typedef AssembledGalerkinOperator<Asshh,uId,uId+1,uId,uId+1>                                             Muuhh;

    typedef typename FLL::Scalar Scalar;
    static constexpr int dim = VariableSetDescription::Grid::dimension;
    typedef VariableSet<VariableSetDescription> VarSet;
    typedef VariableSet<ExtensionVariableSetDescription> ExtVarSet;
    typedef typename VariableSetDescription::template CoefficientVectorRepresentation<> CVL;
    typedef typename VariableSetDescription::template CoefficientVectorRepresentation<yId,yId+1> CVLY;
    typedef typename VariableSetDescription::template CoefficientVectorRepresentation<uId,uId+1> CVLU;
    typedef typename VariableSetDescription::template CoefficientVectorRepresentation<pId,pId+1> CVLP;
    typedef typename CVL::type CoefficientVectorL;
    typedef typename CVLY::type CoefficientVectorLY;
    typedef typename CVLU::type CoefficientVectorLU;
    typedef typename CVLP::type CoefficientVectorLP;
    typedef typename ExtensionVariableSetDescription::template CoefficientVectorRepresentation<0,1>::type CoefficientVectorH01;
    typedef typename ExtensionVariableSetDescription::template CoefficientVectorRepresentation<0,2>::type CoefficientVectorH02;
    typedef typename ExtensionVariableSetDescription::template CoefficientVectorRepresentation<2,3> CVH23;
    typedef typename ExtensionVariableSetDescription::template CoefficientVectorRepresentation<2,3>::type CoefficientVectorH23;
    typedef typename ExtensionVariableSetDescription::template CoefficientVectorRepresentation<> CVH;
    typedef typename ExtensionVariableSetDescription::template CoefficientVectorRepresentation<yId,yId+1> CVHY;
    typedef typename ExtensionVariableSetDescription::template CoefficientVectorRepresentation<uId,uId+1> CVHU;
    typedef typename ExtensionVariableSetDescription::template CoefficientVectorRepresentation<pId,pId+1> CVHP;
    typedef typename CVH::type CoefficientVectorH;
    typedef typename CVHY::type CoefficientVectorHY;
    typedef typename CVHU::type CoefficientVectorHU;
    typedef typename CVHP::type CoefficientVectorHP;
    typedef typename ExtensionVariableSetDescription::GridView::template Codim<0>::Iterator CellIterator;

    typedef MatrixAsTriplet<Scalar> Matrix;

    //    HierarchicalBasisErrorEstimator(Functional& f_, LF& lf_, LVars& lvars, ExtensionVariableSetDescription& extensionVariableSetDescription_, ExtensionSpace& extensionSpace_, Scalar fraction=0.7, bool verbose_=false)
    //    : f(f_), lf(lf_), normFunctional(f_), extensionVariableSetDescription(extensionVariableSetDescription_), extensionSpace(extensionSpace_), assembler(extensionVariableSetDescription.spaces),
    //    squaredFraction(fraction*fraction), verbose(verbose_)
    //    {}

    HierarchicalBasisErrorEstimator2(NormFunctional& normFunctional_, VariableSetDescription& variableSetDescription_, ExtensionVariableSetDescription& extensionVariableSetDescription_,
    ExtensionSpace& extensionSpace_, Scalar fraction=0.7, bool verbose_=false)
    : /*lf(lf_), lvars(lvars_),*/ normFunctional(normFunctional_), variableSetDescription(variableSetDescription_), extensionVariableSetDescription(extensionVariableSetDescription_),
      extensionSpace(extensionSpace_), assembler(extensionVariableSetDescription.spaces),
      squaredFraction(fraction*fraction), verbose(verbose_)
    {
      initAssemblers();
    }

    virtual ~HierarchicalBasisErrorEstimator2(){}

    void operator()(AbstractVector const& x_, AbstractVector const& dx_, int step, AbstractVector const&)
    {
      using boost::fusion::at_c;
      if(verbose) std::cout << "ERROR ESTIMATOR: Start." << std::endl;
      Bridge::Vector<VarSet> const& xl = dynamic_cast<const Bridge::Vector<VarSet>&>(x_);
      Bridge::Vector<ExtVarSet> xe(extensionVariableSetDescription);
      assembleAt(xl,xe);
      Bridge::Vector<VarSet> const& dxl = dynamic_cast<const Bridge::Vector<VarSet>&>(dx_);

      // estimate error in dual variable
      typename ExtensionVariableSetDescription::VariableSet tmpRep(extensionVariableSetDescription), mySol(extensionVariableSetDescription);
      typename VariableSetDescription::VariableSet tmpRepL(variableSetDescription);

      All all(*assll,false);
      Ahh ahh(*asshh,false);
      Alh alh(*asslh,false);

      Matrix mAll = all.template get<Matrix>();
      mAll.transpose();
      Matrix mAhh = ahh.template get<Matrix>();
      mAhh.transpose();
      Matrix mAlh = alh.template get<Matrix>();
      mAlh.transpose();

      tmpRep *= 0;
      at_c<yId>(tmpRep.data) = at_c<0>(rhs0.data);
//      //MyH1SemiNorm myNorm;
//      //std::cout << "||rhs||=" << myNorm(tmpRep) << std::endl;
//      LinearSolverHA(stateOp,DirectType::MUMPS,MatrixProperties::GENERAL).apply(sol0,rhs0);
//      //directInverseOperator(stateOp,DirectType::MUMPS,MatrixProperties::GENERAL).apply(rhs0,sol0);
//      tmpRep *= 0;
      at_c<pId>(tmpRep.data) = at_c<0>(sol0.data);
      at_c<pId>(mySol.data) = at_c<0>(sol0.data);
      //std::cout << "||sol||=" << myNorm(tmpRep) << std::endl;

      // estimate error in control variable
      // rhs vectors
      CoefficientVectorLU rhs1L(asshl->template rhs<uId,uId+1>());
      CoefficientVectorHU rhs1H(asshh->template rhs<uId,uId+1>());
      Bhl bhl(*asshl,false);
      Blh blh(*asslh,false);
      Bll bll(*assll,false);
      Bhh bhh(*asshh,false);

      MatrixAsTriplet<Scalar> tmpmat = blh.template get<MatrixAsTriplet<Scalar> >();
      std::vector<Scalar> tmpx, tmpy;
      IstlInterfaceDetail::toVector(sol0,tmpy);
      IstlInterfaceDetail::toVector(rhs1L,tmpx);
      tmpmat.usmtv(-1.0,tmpy,tmpx);
      IstlInterfaceDetail::fromVector(tmpx,rhs1L);
      //std::cout << "rhs1L=" << at_c<0>(rhs1L.data)[0] << std::endl;

      tmpx.clear(); tmpy.clear();
      IstlInterfaceDetail::toVector(sol0,tmpy);
      IstlInterfaceDetail::toVector(rhs1H,tmpx);
      tmpmat = bhh.template get<MatrixAsTriplet<Scalar> >();
      tmpmat.usmtv(-1.0,tmpy,tmpx);
      IstlInterfaceDetail::fromVector(tmpx,rhs1H);
      //        bophh.applyscaleadd(-1.0,sol0,rhs1H);
      //std::cout << "rhs1H=" << at_c<0>(rhs1H.data)[0] << std::endl;

      Muuhh muuhh(*asshh,true);
      Muull muull(*assll,true);
      Muulh muulh(*asslh,true);
      CoefficientVectorHU solhu(CVHU::init(extensionVariableSetDescription));
      CoefficientVectorLU sollu(CVLU::init(variableSetDescription));
      LinearSolverLM(muull,DirectType::MUMPS,MatrixProperties::GENERAL).apply(sollu,rhs1L);
      //directInverseOperator(muull,DirectType::MUMPS,MatrixProperties::GENERAL).apply(rhs1L,sollu);
      muulh.applyscaleadd(-1.0,sollu,rhs1H);
      LinearSolverHM(muuhh,DirectType::MUMPS,MatrixProperties::GENERAL).apply(solhu,rhs1H);
      //      directInverseOperator(muuhh,DirectType::MUMPS,MatrixProperties::GENERAL).apply(rhs1H,solhu);

      //std::cout << "sollu=" << at_c<0>(sollu.data)[0] << std::endl;
      //std::cout << "solhu=" << at_c<0>(solhu.data)[0] << std::endl;
      at_c<uId>(mySol.data) = at_c<0>(solhu.data);

      // estimate error in state variable
      CoefficientVectorLY solly(CVLP::init(variableSetDescription));
      CoefficientVectorHY solhy(CVHP::init(extensionVariableSetDescription));
      CoefficientVectorLP rhslp(assll->template rhs<pId,pId+1>());
      CoefficientVectorHP rhshp(asshh->template rhs<pId,pId+1>());

      bll.applyscaleadd(-1.0,sollu,rhslp);
      blh.applyscaleadd(-1.0,sollu,rhshp);
      bhl.applyscaleadd(-1.0,solhu,rhslp);
      bhh.applyscaleadd(-1.0,solhu,rhshp);

      LinearSolverLA(all,DirectType::MUMPS,MatrixProperties::GENERAL).apply(solly,rhslp);
      //directInverseOperator(all,DirectType::MUMPS,MatrixProperties::GENERAL).apply(rhslp,solly);
      alh.applyscaleadd(-1.0,solly,rhshp);
      LinearSolverHA(ahh,DirectType::MUMPS,MatrixProperties::GENERAL).apply(solhy,rhshp);
      //      directInverseOperator(ahh,DirectType::MUMPS,MatrixProperties::GENERAL).apply(rhshp,solhy);
      at_c<yId>(mySol.data) = at_c<0>(solhy.data);

      // Transfer error indicators to cells.
      auto const& is = extensionSpace.gridManager().grid().leafIndexSet();
      errorDistribution.clear();
      errorDistribution.resize(is.size(0),std::make_pair(0.0,0));
      Scalar errLevel(0.0), minRefine(0.2);

      typedef ErrorDistribution<NormFunctional,ExtensionVariableSetDescription> EnergyError;
      EnergyError energyError(normFunctional,xl.get(),mySol);
      typedef VariationalFunctionalAssembler<LinearizationAt<EnergyError> > EnergyErrorAssembler;
      EnergyErrorAssembler eeAssembler(extensionSpace.gridManager(), energyError.getSpaces());

      eeAssembler.assemble(linearization(energyError,xl.get()), EnergyErrorAssembler::RHS);
      typename EnergyError::ErrorVector distError( eeAssembler.rhs() );
      typename EnergyError::AnsatzVars::VariableSet mde( energyError.getVariableSetDescription() );
      mde = distError;
      //std::cout << "||mde||=" << myNorm(mde) << std::endl;

      if(verbose)
      {
        std::string name = "errorDistribution_";
        name += boost::lexical_cast<std::string>(step);
        writeVTKFile(mde.descriptions.gridView,mde.descriptions,mde,name);
      }
      // Fehler bzgl L_xx : ( sum_i estSol_i * (L_xx * estSol)_i )^{1/2}  (nur die x - Komponente, nicht p)
      // Lokalisierung: einfachste Idee: v_i = estSol_i * (L_xx * estSol)_i
      // Aufteilen von v_i auf die einzelnen Zellen
      CellIterator cend = extensionVariableSetDescription.gridView.template end<0>();
      for (CellIterator ci=extensionVariableSetDescription.gridView.template begin<0>(); ci!=cend; ++ci)
        errorDistribution[is.index(*ci)] = std::make_pair( fabs(boost::fusion::at_c<0>(mde.data).value(*ci,Dune::FieldVector<Scalar,dim>(0.3))) , is.index(*ci));

      totalErrorSquared = std::accumulate(errorDistribution.begin(), errorDistribution.end(), 0.0, HierarchicErrorEstimator_Detail::add);
    }

    void refineGrid()
    {
//      int lastIndexForDoubleRefinement = -1;
      std::sort(errorDistribution.begin(), errorDistribution.end(), HierarchicErrorEstimator_Detail::biggerThanAbs);

      Scalar bulkCriterionTolerance = squaredFraction*totalErrorSquared;
      if(verbose)
      {
        std::cout << "ERROR ESTIMATOR: totalErrorSquared: " << totalErrorSquared << std::endl;
        std::cout << "ERROR ESTIMATOR: bulkCriterionTolerance: " << bulkCriterionTolerance << std::endl;
      }
      Scalar bulkErrorSquared = 0;

      std::vector<std::pair<Scalar,size_t> > bulkErrorDistribution;
      auto const& is = extensionSpace.gridManager().grid().leafIndexSet();
      CellIterator cend = extensionVariableSetDescription.gridView.template end<0>();

      while(bulkErrorSquared < bulkCriterionTolerance)
      {
        bulkErrorDistribution.push_back(errorDistribution[bulkErrorDistribution.size()]);
        bulkErrorSquared += bulkErrorDistribution.back().first;
      }

      if(verbose) std::cout << "ERROR ESTIMATOR: number of candidates for refinement: " << bulkErrorDistribution.size() << std::endl;

//      size_t lastIndex = bulkErrorDistribution.size()-1;
//      size_t firstIndex = 0;

      /*while(firstIndex+3 < lastIndex)
      {
        if(bulkErrorDistribution.size() > 4)
        {
          Scalar firstContribution = bulkErrorDistribution[firstIndex].first;
          firstContribution *= 15.0/256.0;

          Scalar lastContributions = bulkErrorDistribution[lastIndex--].first;
          lastContributions += bulkErrorDistribution[lastIndex--].first;
          lastContributions += bulkErrorDistribution[lastIndex].first;
          lastContributions *= 15.0/16.0;

          if(lastContributions < firstContribution)
          {
            ++lastIndexForDoubleRefinement;
            ++firstIndex;
            --lastIndex;
            bulkErrorDistribution.erase(bulkErrorDistribution.end()-3, bulkErrorDistribution.end());
          }
          else break;
        }
      }*/

      // Refine mesh.
      for (CellIterator ci=extensionVariableSetDescription.gridView.template begin<0>(); ci!=cend; ++ci) // iterate over cells
      {
        for(int i=0; i<bulkErrorDistribution.size(); ++i) // iterate over chosen part of the error distribution
          if(is.index(*ci) == bulkErrorDistribution[i].second)
          {
//            if(i <= lastIndexForDoubleRefinement) {
//              extensionSpace.gridManager().mark(2,*ci);
//            }
//            else{
              extensionSpace.gridManager().mark(1,*ci);
//            }
          }
      }

      extensionSpace.gridManager().adaptAtOnce();
    }

    double estimatedAbsoluteError() const final
    {
      return sqrt(fabs(totalErrorSquared));
    }

    size_t gridSize() const final
    {
      return extensionSpace.gridManager().grid().size(0);
    }

    template <typename... Args>
    void initFunctionals(const Args&... args)
    {
      Fll.reset(new FLL(args...));
      Flh.reset(new FLH(args...));
      Fhl.reset(new FHL(args...));
      Fhh.reset(new FHH(args...));
    }

  private:
    void initAssemblers()
    {
      assll.reset(new Assll(extensionVariableSetDescription.spaces));
      asslh.reset(new Asslh(extensionVariableSetDescription.spaces));
      asshl.reset(new Asshl(extensionVariableSetDescription.spaces));
      asshh.reset(new Asshh(extensionVariableSetDescription.spaces));
    }

    void assembleAt(const Bridge::Vector<VarSet>& xl, const Bridge::Vector<ExtVarSet>& xe)
    {
      assll->assemble(linearization(*Fll,xl.get()));
      asslh->assemble(linearization(*Flh,xl.get()));
      asshl->assemble(linearization(*Fhl,xe.get()));
      asshh->assemble(linearization(*Fhh,xe.get()));
    }

    std::unique_ptr<FLL> Fll;
    std::unique_ptr<FLH> Flh;
    std::unique_ptr<FHL> Fhl;
    std::unique_ptr<FHH> Fhh;
    std::unique_ptr<Assll> assll;
    std::unique_ptr<Asslh> asslh;
    std::unique_ptr<Asshl> asshl;
    std::unique_ptr<Asshh> asshh;
//    LF& lf;
//    LVars& lvars;
    NormFunctional& normFunctional;
    VariableSetDescription& variableSetDescription;
    ExtensionVariableSetDescription& extensionVariableSetDescription;
    ExtensionSpace& extensionSpace;
    Assembler assembler;
    Scalar squaredFraction;
    Scalar totalErrorSquared;
    std::vector<std::pair<double,int> > errorDistribution;
    bool verbose;
   // AdjustRHS<CoefficientVector> adjustRHS;
  };
}

#endif


#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>


// Copyright (c) 2020 fortiss GmbH
//
// Authors: Julian Bernhard, Klemens Esterle, Patrick Hart and
// Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef BARK_GEOMETRY_POLYGON_HPP_
#define BARK_GEOMETRY_POLYGON_HPP_

#include <memory>
#include <vector>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include "bark/geometry/line.hpp"
#include "commons.hpp"

namespace bark {
namespace geometry {

//! templated polygon class with a boost polygon as a member function
template <typename T>
struct Polygon_t : public Shape<bg::model::polygon<T>, T> {
  Polygon_t();
  virtual ~Polygon_t() {}
  Polygon_t(const Pose& center, const std::vector<T>& points);
  Polygon_t(
      const Pose& center,
      const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& points);
  Polygon_t(const Pose& center,
            const Line_t<T>&
                line);  //! create a polygon from a line enclosing the polygon
  Polygon_t(const Line_t<T>& left_line, const Line_t<T>& right_line);
  virtual Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> ToArray() const;
  virtual double CalculateArea() const;

  virtual std::shared_ptr<Shape<bg::model::polygon<T>, T>> Clone() const;

  void UpdateDistancesToCenter();

  virtual std::shared_ptr<Shape<bg::model::polygon<T>, T>> Scale(const double& x_dir, const double& y_dir) const override;

  virtual std::shared_ptr<Shape<bg::model::polygon<T>, T>> Inflate(const double& x_dir, const double& y_dir) const override;

  void ConcatenatePolygons(Polygon_t<T> poly) {
    std::vector<boost::geometry::model::polygon<Point2d>> merged_polygon;
    boost::geometry::correct(this->obj_);
    boost::geometry::correct(poly.obj_);
    boost::geometry::union_(this->obj_, poly.obj_, merged_polygon);
    if (merged_polygon.size() > 0) {
      Shape<bg::model::polygon<T>, T>::obj_ = merged_polygon[0];
    }
  }

  void ClearInners() {
    this->obj_.inners().clear();
  }

  void SetPrecision(int precision) {
    const double scale = pow(10, precision);
    for (auto it = this->obj_.outer().begin(); it != this->obj_.outer().end();
         ++it) {
      boost::geometry::set<0>(
          *it, round(boost::geometry::get<0>(*it) * scale) / scale);
      boost::geometry::set<1>(
          *it, round(boost::geometry::get<1>(*it) * scale) / scale);
    }
  }

  double rear_dist_;
  double front_dist_;
  double left_dist_;
  double right_dist_;
};

template <typename T>
inline Polygon_t<T>::Polygon_t()
    : Shape<bg::model::polygon<T>, T>(Pose(0, 0, 0), std::vector<T>(), 0),
      rear_dist_(0.0),
      front_dist_(0.0),
      left_dist_(0.0),
      right_dist_(0.0) {}

template <typename T>
inline Polygon_t<T>::Polygon_t(const Pose& center, const std::vector<T>& points)
    : Shape<bg::model::polygon<T>, T>(center, points, 0),
      rear_dist_(0.0),
      front_dist_(0.0),
      left_dist_(0.0),
      right_dist_(0.0) {
  boost::geometry::correct(Shape<bg::model::polygon<T>, T>::obj_);
  UpdateDistancesToCenter();
}

template <typename T>
inline Polygon_t<T>::Polygon_t(
    const Pose& center,
    const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& points)
    : Shape<bg::model::polygon<T>, T>(center, points, 0),
      rear_dist_(0.0),
      front_dist_(0.0),
      left_dist_(0.0),
      right_dist_(0.0) {
  boost::geometry::correct(Shape<bg::model::polygon<T>, T>::obj_);
  UpdateDistancesToCenter();
}

template <typename T>
inline Polygon_t<T>::Polygon_t(const Pose& center, const Line_t<T>& line)
    : Shape<bg::model::polygon<T>, T>(center, std::vector<T>(), 0),
      rear_dist_(0.0),
      front_dist_(0.0),
      left_dist_(0.0),
      right_dist_(0.0) {
  for (const T& next_pt : line.obj_) {
    Shape<bg::model::polygon<T>, T>::AddPoint(next_pt);
  }
  boost::geometry::correct(Shape<bg::model::polygon<T>, T>::obj_);
  UpdateDistancesToCenter();
}

/**
 * @brief Template function to construct a Polygon_t object from two parallel
 * lines. Lines have to be defined in the same direction.
 */
template <typename T>
inline Polygon_t<T>::Polygon_t(const Line_t<T>& line1, const Line_t<T>& line2)
    : Polygon_t() {
  for (const T& next_pt : line1.obj_) {
    Shape<bg::model::polygon<T>, T>::AddPoint(next_pt);
  }
  auto reversed_outer = line2;
  reversed_outer.Reverse();
  for (const T& next_pt : reversed_outer.obj_) {
    Shape<bg::model::polygon<T>, T>::AddPoint(next_pt);
  }
  // Polygons need to be closed!
  Shape<bg::model::polygon<T>, T>::AddPoint(*(line1.begin()));
  boost::geometry::correct(Shape<bg::model::polygon<T>, T>::obj_);
  UpdateDistancesToCenter();
}

template <typename T>
void Polygon_t<T>::UpdateDistancesToCenter() {
  boost::geometry::model::box<T> box;
  boost::geometry::envelope(Shape<bg::model::polygon<T>, T>::obj_, box);

  boost::geometry::correct(box);
  double center_x = Shape<bg::model::polygon<T>, T>::center_[0];
  double center_y = Shape<bg::model::polygon<T>, T>::center_[1];

  rear_dist_ = std::abs(bg::get<bg::min_corner, 0>(box) - center_x);
  front_dist_ = std::abs(bg::get<bg::max_corner, 0>(box) - center_x);
  left_dist_ = std::abs(bg::get<bg::min_corner, 1>(box) - center_y);
  right_dist_ = std::abs(bg::get<bg::max_corner, 1>(box) - center_y);
}

template <typename T>
inline std::shared_ptr<Shape<bg::model::polygon<T>, T>> Polygon_t<T>::Clone()
    const {
  std::shared_ptr<Polygon_t<T>> new_poly =
      std::make_shared<Polygon_t<T>>(*this);
  return new_poly;
}

template <typename T>
std::shared_ptr<Shape<bg::model::polygon<T>, T>> Polygon_t<T>::Scale(const double& x_dir, const double& y_dir) const {
  auto scaled = Shape<bg::model::polygon<T>, T>::Scale(x_dir, y_dir);
  std::dynamic_pointer_cast<Polygon_t<T>>(scaled)->UpdateDistancesToCenter();
  return scaled;
}

template <typename T>
std::shared_ptr<Shape<bg::model::polygon<T>, T>> Polygon_t<T>::Inflate(const double& x_dir, const double& y_dir) const {
  auto inflated = Shape<bg::model::polygon<T>, T>::Inflate(x_dir, y_dir);
  std::dynamic_pointer_cast<Polygon_t<T>>(inflated)->UpdateDistancesToCenter();
  return inflated;
}

//! for better usage simple float defines
using PolygonPoint = Point2d;  // for internal stores of collision checkers
using Polygon = Polygon_t<PolygonPoint>;

template <>
inline Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Polygon::ToArray()
    const {
  std::vector<Point2d> points = obj_.outer();
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> mat(points.size(), 2);
  for (std::vector<Point2d>::size_type i = 0; i < points.size(); ++i) {
    mat.row(i) << bg::get<0>(points[i]), bg::get<1>(points[i]);
  }
  return mat;
}

template <>
inline double Polygon::CalculateArea() const {
  return bg::area(obj_);
}

inline bool Equals(const Polygon& poly1, const Polygon& poly2) {
  return bg::equals(poly1.obj_, poly2.obj_);
}

inline double Distance(const Polygon& poly, const Point2d& p) {
  return bg::distance(poly.obj_, p);
}

inline double Distance(const Polygon& poly, const Line& l) {
  return bg::distance(poly.obj_, l.obj_);
}

inline double Distance(const Polygon& poly1, const Polygon& poly2) {
  return bg::distance(poly1.obj_, poly2.obj_);
}

//! Polygon - Point collision checker using boost::covered_by
//! As opposed to boost::whithin, also considers points on the boundary lines
inline bool Collide(const Polygon& poly, const PolygonPoint& p) {
  return bg::covered_by(p, poly.obj_);
}

//! Point - Polygon collision checker using boost::within
inline bool Collide(const PolygonPoint& p, const Polygon& poly) {
  return Collide(poly, p);
}

//! Polygon within polygon check, true if g1 is completely contained within g2,
//! else false
inline bool Within(const Polygon& g1, const Polygon& g2) {
  return bg::within(g1.obj_, g2.obj_);
}

//! Point2d within polygon check, true if g1 is completely contained within g2,
//! else false
inline bool Within(const Point2d& g1, const Polygon& g2) {
  return bg::within(g1, g2.obj_);
}

//! Line2d within polygon check, true if g1 is completely contained within g2,
//! else false
inline bool Within(const Line& l, const Polygon& poly) {
  return bg::within(l.obj_, poly.obj_);
}

//! Polygon - Line collision checker using boost::intersection
//! @note we only check the shape intersection(s) and no line RHS/LHS line
//! crossing!
inline bool Collide(const Polygon& poly, const Line& l) {
  return bg::intersects(poly.obj_, l.obj_);
}

//! Line - Polygon collision checker using boost::intersection
inline bool Collide(const Line& line, const Polygon& poly) {
  return Collide(poly, line);
}

//! Polygon - Polygon collision checker using boost::intersects
inline bool Collide(const Polygon& poly1, const Polygon& poly2) {
  return bg::intersects(poly1.obj_, poly2.obj_);
}

inline bool BufferPolygon(const Polygon& polygon, const double distance,
                          Polygon* buffered_polygon) {
  namespace bg = boost::geometry;
  namespace bbuf = bg::strategy::buffer;

  bbuf::distance_symmetric<double> distance_strategy(distance);
  bbuf::side_straight side_strategy;
  bbuf::join_miter join_strategy;
  bbuf::end_flat end_strategy;
  bbuf::point_circle point_strategy;
  bg::model::multi_polygon<bg::model::polygon<geometry::Point2d>>
      buffered_polygons;
  Polygon copied_polygon = polygon;
  bg::correct(copied_polygon.obj_);
  bg::buffer(copied_polygon.obj_, buffered_polygons, distance_strategy,
             side_strategy, join_strategy, end_strategy, point_strategy);
  bg::correct(buffered_polygons);
  if (buffered_polygons.size() != 1) {
    // Shrinking the polygon turns it into two disjointed polygons
    return false;
  }
  for (auto const& point : boost::make_iterator_range(
           bg::exterior_ring(buffered_polygons.front()))) {
    buffered_polygon->AddPoint(point);
  }
  if (!buffered_polygon->Valid()) {
    LOG(INFO) << "Buffered polygon is not valid.";
  }
  return true;
}

inline Polygon CalculateBoundingBoxPolygon(const Polygon& polygon) {
  auto box = polygon.BoundingBox();
  double xmin = bg::get<0>(box.first);
  double xmax = bg::get<0>(box.second);
  double ymin = bg::get<1>(box.first);
  double ymax = bg::get<1>(box.second);
  Polygon bbpoly =
      Polygon(polygon.center_,
              std::vector<Point2d>{Point2d(xmin, ymin), Point2d(xmin, ymax),
                                   Point2d(xmax, ymax), Point2d(xmax, ymin),
                                   Point2d(xmin, ymin)});
  return bbpoly;
}

}  // namespace geometry
}  // namespace bark

#endif  // BARK_GEOMETRY_POLYGON_HPP_


/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 *************************************************************************/

#ifndef MFMG_LAPLACE_MATRIX_FREE_HPP
#define MFMG_LAPLACE_MATRIX_FREE_HPP

#include <deal.II/base/index_set.h>
#include <deal.II/distributed/tria.h>
#include <deal.II/dofs/dof_renumbering.h>
#include <deal.II/grid/filtered_iterator.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_tools.h>
#include <deal.II/lac/la_parallel_vector.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/matrix_free/fe_evaluation.h>
#include <deal.II/matrix_free/matrix_free.h>
#include <deal.II/matrix_free/operators.h>
#include <deal.II/numerics/vector_tools.h>

#include <boost/property_tree/ptree.hpp>

template <int dim, int fe_degree, typename ScalarType>
class LaplaceOperator
    : public dealii::MatrixFreeOperators::Base<
          dim, dealii::LinearAlgebra::distributed::Vector<ScalarType>>
{
public:
  typedef ScalarType value_type;

  LaplaceOperator();

  virtual ~LaplaceOperator() override = default;

  virtual void compute_diagonal() override final;

  template <typename MaterialPropertyType>
  void evaluate_coefficient(MaterialPropertyType const &material_property);

  // private:
  virtual void
  apply_add(dealii::LinearAlgebra::distributed::Vector<ScalarType> &dst,
            dealii::LinearAlgebra::distributed::Vector<ScalarType> const &src)
      const override final;

  void
  local_apply(dealii::MatrixFree<dim, ScalarType> const &matrix_free_data,
              dealii::LinearAlgebra::distributed::Vector<ScalarType> &dst,
              dealii::LinearAlgebra::distributed::Vector<ScalarType> const &src,
              std::pair<unsigned int, unsigned int> const &cell_range) const;

  void local_compute_diagonal(
      dealii::MatrixFree<dim, ScalarType> const &matrix_free_data,
      dealii::LinearAlgebra::distributed::Vector<ScalarType> &dst,
      unsigned int const &dummy,
      std::pair<unsigned int, unsigned int> const &cell_range) const;

  dealii::Table<2, dealii::VectorizedArray<ScalarType>> _coefficient;
};

template <int dim, int fe_degree, typename ScalarType>
LaplaceOperator<dim, fe_degree, ScalarType>::LaplaceOperator()
    : dealii::MatrixFreeOperators::Base<
          dim, dealii::LinearAlgebra::distributed::Vector<ScalarType>>()
{
}

template <int dim, int fe_degree, typename ScalarType>
void LaplaceOperator<dim, fe_degree, ScalarType>::compute_diagonal()
{
  this->diagonal_entries.reset(
      new dealii::DiagonalMatrix<
          dealii::LinearAlgebra::distributed::Vector<ScalarType>>());
  dealii::LinearAlgebra::distributed::Vector<ScalarType> &diagonal =
      this->diagonal_entries->get_vector();
  this->data->initialize_dof_vector(diagonal);
  unsigned int const dummy = 0;
  this->data->cell_loop(&LaplaceOperator::local_compute_diagonal, this,
                        diagonal, dummy);

  this->set_constrained_entries_to_one(diagonal);

  this->inverse_diagonal_entries.reset(
      new dealii::DiagonalMatrix<
          dealii::LinearAlgebra::distributed::Vector<ScalarType>>());
  dealii::LinearAlgebra::distributed::Vector<ScalarType> &inverse_diagonal =
      this->inverse_diagonal_entries->get_vector();
  this->data->initialize_dof_vector(inverse_diagonal);
  for (unsigned int i = 0; i < inverse_diagonal.local_size(); ++i)
    inverse_diagonal.local_element(i) = 1. / diagonal.local_element(i);
}

template <int dim, int fe_degree, typename ScalarType>
template <typename MaterialPropertyType>
void LaplaceOperator<dim, fe_degree, ScalarType>::evaluate_coefficient(
    MaterialPropertyType const &material_property)
{
  int constexpr n_q_points = fe_degree + 1;
  int constexpr n_components = 1;
  unsigned int const n_cells = this->data->n_macro_cells();
  dealii::FEEvaluation<dim, fe_degree, n_q_points, n_components, ScalarType>
      fe_eval(*this->data);

  _coefficient.reinit(n_cells, fe_eval.n_q_points);
  for (unsigned int cell = 0; cell < n_cells; ++cell)
  {
    fe_eval.reinit(cell);
    for (unsigned int q = 0; q < fe_eval.n_q_points; ++q)
      _coefficient(cell, q) =
          material_property.value(fe_eval.quadrature_point(q));
  }
}

template <int dim, int fe_degree, typename ScalarType>
void LaplaceOperator<dim, fe_degree, ScalarType>::apply_add(
    dealii::LinearAlgebra::distributed::Vector<ScalarType> &dst,
    dealii::LinearAlgebra::distributed::Vector<ScalarType> const &src) const
{
  this->data->cell_loop(&LaplaceOperator::local_apply, this, dst, src);
}

template <int dim, int fe_degree, typename ScalarType>
void LaplaceOperator<dim, fe_degree, ScalarType>::local_apply(
    dealii::MatrixFree<dim, ScalarType> const &matrix_free_data,
    dealii::LinearAlgebra::distributed::Vector<ScalarType> &dst,
    dealii::LinearAlgebra::distributed::Vector<ScalarType> const &src,
    std::pair<unsigned int, unsigned int> const &cell_range) const
{
  int constexpr n_q_points = fe_degree + 1;
  int constexpr n_components = 1;
  dealii::FEEvaluation<dim, fe_degree, n_q_points, n_components, ScalarType>
      fe_eval(matrix_free_data);

  bool const evaluate_values = false;
  bool const evaluate_gradients = true;
  bool const integrate_values = false;
  bool const integrate_gradients = true;
  for (unsigned int cell = cell_range.first; cell < cell_range.second; ++cell)
  {
    fe_eval.reinit(cell);
    fe_eval.read_dof_values(src);
    fe_eval.evaluate(evaluate_values, evaluate_gradients);
    for (unsigned int q = 0; q < fe_eval.n_q_points; ++q)
      fe_eval.submit_gradient(_coefficient(cell, q) * fe_eval.get_gradient(q),
                              q);
    fe_eval.integrate(integrate_values, integrate_gradients);
    fe_eval.distribute_local_to_global(dst);
  }
}

template <int dim, int fe_degree, typename ScalarType>
void LaplaceOperator<dim, fe_degree, ScalarType>::local_compute_diagonal(
    dealii::MatrixFree<dim, ScalarType> const &matrix_free_data,
    dealii::LinearAlgebra::distributed::Vector<ScalarType> &dst,
    unsigned int const &,
    std::pair<unsigned int, unsigned int> const &cell_range) const
{
  int constexpr n_q_points = fe_degree + 1;
  int constexpr n_components = 1;
  dealii::FEEvaluation<dim, fe_degree, n_q_points, n_components, ScalarType>
      fe_eval(matrix_free_data);

  dealii::AlignedVector<dealii::VectorizedArray<ScalarType>> diagonal(
      fe_eval.dofs_per_cell);

  bool const evaluate_values = false;
  bool const evaluate_gradients = true;
  bool const integrate_values = false;
  bool const integrate_gradients = true;
  for (unsigned int cell = cell_range.first; cell < cell_range.second; ++cell)
  {
    fe_eval.reinit(cell);
    for (unsigned int i = 0; i < fe_eval.dofs_per_cell; ++i)
    {
      for (unsigned int j = 0; j < fe_eval.dofs_per_cell; ++j)
        fe_eval.submit_dof_value(dealii::VectorizedArray<ScalarType>(), j);
      fe_eval.submit_dof_value(dealii::make_vectorized_array<ScalarType>(1.),
                               i);

      fe_eval.evaluate(evaluate_values, evaluate_gradients);
      for (unsigned int q = 0; q < fe_eval.n_q_points; ++q)
        fe_eval.submit_gradient(_coefficient(cell, q) * fe_eval.get_gradient(q),
                                q);

      fe_eval.integrate(integrate_values, integrate_gradients);
      diagonal[i] = fe_eval.get_dof_value(i);
    }
    for (unsigned int i = 0; i < fe_eval.dofs_per_cell; ++i)
      fe_eval.submit_dof_value(diagonal[i], i);
    fe_eval.distribute_local_to_global(dst);
  }
}

template <int dim, int fe_degree, typename ScalarType>
class LaplaceMatrixFree
{
public:
  LaplaceMatrixFree(MPI_Comm const &comm);

  template <typename MaterialPropertyType>
  void setup_system(boost::property_tree::ptree const &ptree,
                    MaterialPropertyType const &material_property);

  template <typename SourceType>
  void assemble_rhs(SourceType const &source);

  template <typename PreconditionerType>
  void solve(PreconditionerType &preconditioner);

  double compute_error(dealii::Function<dim> const &exact_solution);

  // The following variable should be private but there are public for
  // simplicity
  MPI_Comm _comm;
  dealii::parallel::distributed::Triangulation<dim> _triangulation;
  dealii::FE_Q<dim> _fe;
  dealii::DoFHandler<dim> _dof_handler;
  dealii::IndexSet _locally_owned_dofs;
  dealii::IndexSet _locally_relevant_dofs;
  dealii::AffineConstraints<double> _constraints;
  LaplaceOperator<dim, fe_degree, ScalarType> _laplace_operator;
  dealii::LinearAlgebra::distributed::Vector<ScalarType> _solution;
  dealii::LinearAlgebra::distributed::Vector<ScalarType> _system_rhs;
};

template <int dim, int fe_degree, typename ScalarType>
LaplaceMatrixFree<dim, fe_degree, ScalarType>::LaplaceMatrixFree(
    MPI_Comm const &comm)
    : _comm(comm), _triangulation(_comm), _fe(fe_degree),
      _dof_handler(_triangulation)
{
}

template <int dim, int fe_degree, typename ScalarType>
template <typename MaterialPropertyType>
void LaplaceMatrixFree<dim, fe_degree, ScalarType>::setup_system(
    boost::property_tree::ptree const &ptree,
    MaterialPropertyType const &material_property)
{
  std::string const mesh = ptree.get("mesh", "hyper_cube");
  if (mesh == "hyper_ball")
    dealii::GridGenerator::hyper_ball(_triangulation);
  else
    dealii::GridGenerator::hyper_cube(_triangulation);

  _triangulation.refine_global(ptree.get("n_refinements", 3));

  // Set the boundary id to one
  auto boundary_cells =
      dealii::filter_iterators(_triangulation.active_cell_iterators(),
                               dealii::IteratorFilters::AtBoundary());
  for (auto &cell : boundary_cells)
  {
    for (unsigned int f = 0; f < dealii::GeometryInfo<dim>::faces_per_cell; ++f)
      if (cell->face(f)->at_boundary())
        cell->face(f)->set_boundary_id(1);
  }

  if (ptree.get("distort_random", false))
    dealii::GridTools::distort_random(0.1, _triangulation);

  _dof_handler.distribute_dofs(_fe);

  std::string const reordering = ptree.get("reordering", "None");
  if (reordering == "Reverse Cuthill-McKee")
    dealii::DoFRenumbering::Cuthill_McKee(_dof_handler, true);
  else if (reordering == "King")
    dealii::DoFRenumbering::boost::king_ordering(_dof_handler);
  else if (reordering == "Reverse minimum degree")
    dealii::DoFRenumbering::boost::minimum_degree(_dof_handler, true);
  else if (reordering == "Hierarchical")
    dealii::DoFRenumbering::hierarchical(_dof_handler);

  // Get the IndexSets
  _locally_owned_dofs = _dof_handler.locally_owned_dofs();
  dealii::DoFTools::extract_locally_relevant_dofs(_dof_handler,
                                                  _locally_relevant_dofs);

  // Compute the constraints
  _constraints.clear();
  _constraints.reinit(_locally_relevant_dofs);
  dealii::DoFTools::make_hanging_node_constraints(_dof_handler, _constraints);
  dealii::VectorTools::interpolate_boundary_values(
      _dof_handler, 1, dealii::Functions::ZeroFunction<dim>(), _constraints);
  _constraints.close();

  // Initialize the MatrixFree object
  typename dealii::MatrixFree<dim, ScalarType>::AdditionalData additional_data;
  additional_data.tasks_parallel_scheme =
      dealii::MatrixFree<dim, ScalarType>::AdditionalData::none;
  additional_data.mapping_update_flags = dealii::update_gradients |
                                         dealii::update_JxW_values |
                                         dealii::update_quadrature_points;
  std::shared_ptr<dealii::MatrixFree<dim, ScalarType>> mf_storage(
      new dealii::MatrixFree<dim, ScalarType>());
  mf_storage->reinit(_dof_handler, _constraints,
                     dealii::QGauss<1>(fe_degree + 1), additional_data);
  _laplace_operator.initialize(mf_storage);

  // Resize the vectors
  _laplace_operator.initialize_dof_vector(_solution);
  _laplace_operator.initialize_dof_vector(_system_rhs);

  _laplace_operator.evaluate_coefficient(material_property);
  _laplace_operator.compute_diagonal();
}

template <int dim, int fe_degree, typename ScalarType>
template <typename SourceType>
void LaplaceMatrixFree<dim, fe_degree, ScalarType>::assemble_rhs(
    SourceType const &source)
{
  _system_rhs = 0.;
  dealii::FEEvaluation<dim, fe_degree> fe_eval(
      *_laplace_operator.get_matrix_free());
  unsigned int const n_macro_cells =
      _laplace_operator.get_matrix_free()->n_macro_cells();
  bool constexpr integrate_values = true;
  bool constexpr integrate_gradients = false;
  for (unsigned int cell = 0; cell < n_macro_cells; ++cell)
  {
    fe_eval.reinit(cell);
    for (unsigned int q = 0; q < fe_eval.n_q_points; ++q)
      fe_eval.submit_value(source.value(fe_eval.quadrature_point(q)), q);
    fe_eval.integrate(integrate_values, integrate_gradients);
    fe_eval.distribute_local_to_global(_system_rhs);
  }
  _system_rhs.compress(dealii::VectorOperation::add);
}

template <int dim, int fe_degree, typename ScalarType>
template <typename PreconditionerType>
void LaplaceMatrixFree<dim, fe_degree, ScalarType>::solve(
    PreconditionerType &preconditioner)
{
  dealii::SolverControl solver_control(_dof_handler.n_dofs(),
                                       1e-12 * _system_rhs.l2_norm());
  dealii::SolverCG<dealii::LinearAlgebra::distributed::Vector<double>> cg(
      solver_control);
  _constraints.set_zero(_solution);

  cg.solve(_laplace_operator, _solution, _system_rhs, preconditioner);

  if (dealii::Utilities::MPI::this_mpi_process(_comm) == 0)
    std::cout << "Solved in " << solver_control.last_step() << " iterations."
              << std::endl;

  _constraints.distribute(_solution);
}

template <int dim, int fe_degree, typename ScalarType>
double LaplaceMatrixFree<dim, fe_degree, ScalarType>::compute_error(
    dealii::Function<dim> const &exact_solution)
{
  dealii::QGauss<dim> const quadrature(fe_degree + 1);

  // We need a regular ghosted vector but for some reason we need to first get a
  // non ghosted vector and then, we can get a ghosted vector.
  dealii::LinearAlgebra::distributed::Vector<ScalarType>
      locally_relevant_solution(_locally_owned_dofs, _locally_relevant_dofs,
                                _comm);
  dealii::LinearAlgebra::distributed::Vector<ScalarType> sol(
      _locally_owned_dofs, _comm);
  sol = _solution;
  locally_relevant_solution = sol;

  dealii::Vector<double> difference(
      _dof_handler.get_triangulation().n_active_cells());
  dealii::VectorTools::integrate_difference(
      _dof_handler, locally_relevant_solution, exact_solution, difference,
      quadrature, dealii::VectorTools::L2_norm);

  return dealii::VectorTools::compute_global_error(
      _dof_handler.get_triangulation(), difference,
      dealii::VectorTools::L2_norm);
}

#endif


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/function/cyl_bessel_in.hpp>
#include <eve/function/diff/cyl_bessel_in.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/platform.hpp>
#include <cmath>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>

//==================================================================================================
//== Types tests
//==================================================================================================
EVE_TEST_TYPES( "Check return types of cyl_bessel_in"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
 using i_t = eve::as_integer_t<v_t>;
 using I_t = eve::wide<i_t, eve::cardinal_t<T>>;
  TTS_EXPR_IS( eve::cyl_bessel_in(T(), T())  ,  T);
  TTS_EXPR_IS( eve::cyl_bessel_in(v_t(),v_t()), v_t);
  TTS_EXPR_IS( eve::cyl_bessel_in(i_t(),T()),   T);
  TTS_EXPR_IS( eve::cyl_bessel_in(I_t(),T()),   T);
  TTS_EXPR_IS( eve::cyl_bessel_in(i_t(),v_t()), v_t);
  TTS_EXPR_IS( eve::cyl_bessel_in(I_t(),v_t()), T);
};

//==================================================================================================
//== integral orders
//==================================================================================================
EVE_TEST( "Check behavior of cyl_bessel_in on wide with integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::ramp(0),
                              eve::test::randoms(0.0, 10.0))
        )
  <typename T>(T n , T a0)
{
  using v_t = eve::element_type_t<T>;
  auto eve__cyl_bessel_in =  [](auto n, auto x) { return eve::cyl_bessel_in(n, x); };
  auto std__cyl_bessel_in =  [](auto n, auto x)->v_t { return boost::math::cyl_bessel_i(n, x); };
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_in(0, eve::minf(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(2, eve::inf(eve::as<v_t>())), eve::inf(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(3, eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
  }
  //scalar large x
  TTS_ULP_EQUAL(eve__cyl_bessel_in(3, v_t(1500)), eve::inf(eve::as<v_t>()), 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(2, v_t(50)), std__cyl_bessel_in(2, v_t(50)), 5.0);
  //scalar forward
  TTS_ULP_EQUAL(eve__cyl_bessel_in(0, v_t(10)), std__cyl_bessel_in(0, v_t(10))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(1, v_t(5)),  std__cyl_bessel_in(1, v_t(5))   , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(2, v_t(10)), std__cyl_bessel_in(2, v_t(10))  , 35.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(3, v_t(5)),  std__cyl_bessel_in(3, v_t(5))   , 35.0);
  //scalar small
  TTS_ULP_EQUAL(eve__cyl_bessel_in(0, v_t(0.1)), std__cyl_bessel_in(0, v_t(0.1))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(1, v_t(0.2)),  std__cyl_bessel_in(1, v_t(0.2))   , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(2, v_t(0.1)), std__cyl_bessel_in(2, v_t(0.1))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(3, v_t(0.2)),  std__cyl_bessel_in(3, v_t(0.2))   , 5.0);
  //scalar medium
  TTS_ULP_EQUAL(eve__cyl_bessel_in(10, v_t(8)), std__cyl_bessel_in(10, v_t(8))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(20, v_t(8)), std__cyl_bessel_in(20, v_t(8))   , 5.0);

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_in(0, eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(2, eve::inf(eve::as<T>())), eve::inf(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(3, eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  //simd large x
  TTS_ULP_EQUAL(eve__cyl_bessel_in(3, T(1500)),  eve::inf(eve::as<T>()),  5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(2, T(50)),   T(std__cyl_bessel_in(2, v_t(50))),   5.0);
  //simd forward
  TTS_ULP_EQUAL(eve__cyl_bessel_in(2, T(10)),    T(std__cyl_bessel_in(2, v_t(10)))   , 35.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(3, T(5)),     T(std__cyl_bessel_in(3, v_t(5)))    , 35.0);
  //simd small
  TTS_ULP_EQUAL(eve__cyl_bessel_in(0, T(0.1)),   T(std__cyl_bessel_in(0, v_t(0.1)))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(1, T(0.2)),   T(std__cyl_bessel_in(1, v_t(0.2)))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(2, T(0.1)),   T(std__cyl_bessel_in(2, v_t(0.1)))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(3, T(0.2)),   T(std__cyl_bessel_in(3, v_t(0.2)))  , 5.0);
  //simd medium
  TTS_ULP_EQUAL(eve__cyl_bessel_in(10, T(8)),   T(std__cyl_bessel_in(10, v_t(8)))   , 7.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(20, T(8)),   T(std__cyl_bessel_in(20, v_t(8)))   , 5.0);

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_in(T(0), eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(T(2), eve::inf(eve::as<T>())), eve::inf(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(T(3), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(3), T(1500)),  eve::inf(eve::as<T>()),  5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(2), T(50)),   T(std__cyl_bessel_in(2, v_t(50))),   5.0);
  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(2), T(10)),    T(std__cyl_bessel_in(2, v_t(10)))   , 35.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(3), T(5)),     T(std__cyl_bessel_in(3, v_t(5)))    , 35.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(2), T(0.1)),   T(std__cyl_bessel_in(2, v_t(0.1)))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(3), T(0.2)),   T(std__cyl_bessel_in(3, v_t(0.2)))  , 5.0);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(10), T(8)),   T(std__cyl_bessel_in(10, v_t(8)))   , 7.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(20), T(8)),   T(std__cyl_bessel_in(20, v_t(8)))   , 5.0);

  using i_t = eve::as_integer_t<v_t>;
  using I_t = eve::wide<i_t, eve::cardinal_t<T>>;

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_in(I_t(0), eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(I_t(2), eve::inf(eve::as<T>())), eve::inf(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(I_t(3), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_in(I_t(3), T(1500)),  eve::inf(eve::as<T>()),  5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(I_t(2), T(50)),   T(std__cyl_bessel_in(2, v_t(50))),   5.0);
  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_in(I_t(2), T(10)),    T(std__cyl_bessel_in(2, v_t(10)))   , 35.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(I_t(3), T(5)),     T(std__cyl_bessel_in(3, v_t(5)))    , 35.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_in(I_t(2), T(0.1)),   T(std__cyl_bessel_in(2, v_t(0.1)))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(I_t(3), T(0.2)),   T(std__cyl_bessel_in(3, v_t(0.2)))  , 5.0);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_in(I_t(10), T(8)),   T(std__cyl_bessel_in(10, v_t(8)))   , 7.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(I_t(20), T(8)),   T(std__cyl_bessel_in(20, v_t(8)))   , 5.0);

  TTS_ULP_EQUAL(map(eve__cyl_bessel_in, n, a0),   map(std__cyl_bessel_in, n, a0)   , 500.0);
  TTS_ULP_EQUAL(map(eve__cyl_bessel_in, -n, a0),   map(std__cyl_bessel_in, -n, a0)   , 500.0);
};

//==================================================================================================
//== non integral orders
//==================================================================================================
EVE_TEST( "Check behavior of cyl_bessel_in on wide with non integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 10.0)
                             , eve::test::randoms(0.0, 10.0))
        )
  <typename T>(T n, T a0 )
{
  using v_t = eve::element_type_t<T>;

  auto eve__cyl_bessel_in =  [](auto n, auto x) { return eve::cyl_bessel_in(n, x); };
  auto std__cyl_bessel_in =  [](auto n, auto x)->v_t { return boost::math::cyl_bessel_i(double(n), double(x)); };

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_in(T(0.5), eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(T(2.5), eve::inf(eve::as<T>())), eve::inf(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(T(3.5), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(3.5), v_t(1500)),  eve::inf(eve::as<v_t>()),  10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(2.5), v_t(50)),   std__cyl_bessel_in(v_t(2.5), v_t(50)),   10.0);
  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(2.5), v_t(10)),    std__cyl_bessel_in(v_t(2.5), v_t(10))   , 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(3.5), v_t(5)),     std__cyl_bessel_in(v_t(3.5), v_t(5))    , 10.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(2.5), v_t(0.1)),   std__cyl_bessel_in(v_t(2.5), v_t(0.1))  , 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(3.5), v_t(0.2)),   std__cyl_bessel_in(v_t(3.5), v_t(0.2))  , 10.0);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(10.5), v_t(8)),    std__cyl_bessel_in(v_t(10.5), v_t(8))   , 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(10.5), v_t(8)),    std__cyl_bessel_in(v_t(10.5), v_t(8))   , 10.0);

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_in(T(0.5), eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(T(2.5), eve::inf(eve::as<T>())), eve::inf(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(T(3.5), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(3.5), T(700)),  T(std__cyl_bessel_in(v_t(3.5), v_t(700))),  10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(2.5), T(50)),   T(std__cyl_bessel_in(v_t(2.5), v_t(50))),   10.0);
  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(2.5), T(10)),    T(std__cyl_bessel_in(v_t(2.5), v_t(10)))   , 310.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(3.5), T(5)),     T(std__cyl_bessel_in(v_t(3.5), v_t(5)))    , 310.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(2.5), T(0.1)),   T(std__cyl_bessel_in(v_t(2.5), v_t(0.1)))  , 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(3.5), T(0.2)),   T(std__cyl_bessel_in(v_t(3.5), v_t(0.2)))  , 10.0);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(10.5), T(8)),   T(std__cyl_bessel_in(v_t(10.5), v_t(8)))   , 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(10.5), T(8)),   T(std__cyl_bessel_in(v_t(10.5), v_t(8)))   , 10.0);

  TTS_RELATIVE_EQUAL(eve__cyl_bessel_in(n, a0),   map(std__cyl_bessel_in, n, a0)   , 1.0e-3);
};

EVE_TEST( "Check behavior of cyl_bessel_in on wide with negative non integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 10.0)
                             , eve::test::randoms(0.0, 10.0))
        )
  <typename T>(T n, T a0 )
{
  using v_t = eve::element_type_t<T>;

  auto eve__cyl_bessel_in =  [](auto n, auto x) { return eve::cyl_bessel_in(n, x); };
  auto std__cyl_bessel_in =  [](auto n, auto x)->v_t { return boost::math::cyl_bessel_i(double(n), double(x)); };

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_in(T(-0.5), eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(T(-2.5), eve::inf(eve::as<T>())), eve::inf(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(T(-3.5), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(-3.5), v_t(1500)),  eve::inf(eve::as<v_t>()),  10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(-2.5), v_t(60)),   std__cyl_bessel_in(v_t(-2.5), v_t(60)),   10.0);
  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(-2.5), v_t(10)),    std__cyl_bessel_in(v_t(-2.5), v_t(10))   , 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(-3.5), v_t(5)),     std__cyl_bessel_in(v_t(-3.5), v_t(5))    , 10.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(-2.5), v_t(0.1)),   std__cyl_bessel_in(v_t(-2.5), v_t(0.1))  , 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(-3.5), v_t(0.2)),   std__cyl_bessel_in(v_t(-3.5), v_t(0.2))  , 10.0);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(-10.5), v_t(8)),    std__cyl_bessel_in(v_t(-10.5), v_t(8))   , 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(v_t(-10.5), v_t(8)),    std__cyl_bessel_in(v_t(-10.5), v_t(8))   , 10.0);

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_in(T(-0.5), eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(T(-2.5), eve::inf(eve::as<T>())), eve::inf(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_in(T(-3.5), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(-3.5), T(1500)),  eve::inf(eve::as<T>()),  10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(-2.5), T(50)),   T(std__cyl_bessel_in(v_t(-2.5), v_t(50))),   10.0);
  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(-2.5), T(10)),    T(std__cyl_bessel_in(v_t(-2.5), v_t(10)))   , 310.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(-3.5), T(5)),     T(std__cyl_bessel_in(v_t(-3.5), v_t(5)))    , 310.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(-2.5), T(0.1)),   T(std__cyl_bessel_in(v_t(-2.5), v_t(0.1)))  , 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(-3.5), T(0.2)),   T(std__cyl_bessel_in(v_t(-3.5), v_t(0.2)))  , 10.0);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(-10.5), T(8)),   T(std__cyl_bessel_in(v_t(-10.5), v_t(8)))   , 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_in(T(-10.5), T(8)),   T(std__cyl_bessel_in(v_t(-10.5), v_t(8)))   , 10.0);

  TTS_RELATIVE_EQUAL(eve__cyl_bessel_in(-n, a0),   map(std__cyl_bessel_in, -n, a0)   , 1.0e-3);
};

EVE_TEST( "Check behavior of diff of cyl_bessel_in on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 10.0)
                             , eve::test::randoms(0.0, 10.0))
        )
  <typename T>(T n, T a0 )
{
  using v_t = eve::element_type_t<T>;
  auto eve__diff_bessel_in =  [](auto n, auto x) { return eve::diff(eve::cyl_bessel_in)(n, x); };
  auto std__diff_bessel_in =  [](auto n, auto x)->v_t { return boost::math::cyl_bessel_i_prime(double(n), double(x)); };
  TTS_RELATIVE_EQUAL(eve__diff_bessel_in(n, a0),   map(std__diff_bessel_in, n, a0)   , 1.0e-3);

};


#define BOOST_TEST_MODULE SolutionTest

#include "solution.hpp"

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(SolutionSuite)

BOOST_AUTO_TEST_CASE(PlainTest)
{
    vector<int> num1{2, 4, 3};
    ListNode* list1 = stringToListNode(num1);
    vector<int> num2{5, 6, 4};
    ListNode* list2 = stringToListNode(num2);

    ListNode* results = Solution().addTwoNumbers(list1, list2);
    string result_str = listNodeToString(results);

    string expected = "[7, 0, 8]";

    BOOST_CHECK_EQUAL(result_str, expected);
}
BOOST_AUTO_TEST_SUITE_END()


#include "mtf/SSM//SSMEstimator.h"
#include "mtf/SSM/internal/cv2_q_sort.h"
#include "opencv2/core/core_c.h"
//#if CV_MAJOR_VERSION < 3
//#include "opencv2/core/internal.hpp"
//#else
////#include "mtf/SM//opencv2/core/internal.hpp"
//#endif
#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>

_MTF_BEGIN_NAMESPACE

SSMEstimator::SSMEstimator(int _modelPoints, CvSize _modelSize,
int _maxBasicSolutions, bool _use_boost_rng):
modelPoints(_modelPoints),
modelSize(_modelSize),
maxBasicSolutions(_maxBasicSolutions),
checkPartialSubsets(true),
use_boost_rng(_use_boost_rng){
	boost::random_device seed_gen;
	boost_rng = BoostRNG(seed_gen());
	cv_rng = cvRNG(seed_gen());
}

SSMEstimator::~SSMEstimator() {}

void SSMEstimator::setSeed(int64 seed) {
	cv_rng = cvRNG(seed);
	boost_rng.seed(static_cast<const uint32_t>(seed));
}

int SSMEstimator::findInliers(const CvMat* m1, const CvMat* m2,
	const CvMat* model, CvMat* _err,
	CvMat* _mask, double threshold) {
	int i, count = _err->rows * _err->cols, goodCount = 0;
	const float* err = _err->data.fl;
	uchar* mask = _mask->data.ptr;

	computeReprojError(m1, m2, model, _err);
	threshold *= threshold;
	for(i = 0; i < count; i++)
		goodCount += mask[i] = err[i] <= threshold;
	return goodCount;
}

int cvRANSACUpdateNumIters(double p, double ep,
	int model_points, int max_iters) {
	if(model_points <= 0)
		CV_Error(CV_StsOutOfRange, "the number of model points should be positive");

	p = MAX(p, 0.);
	p = MIN(p, 1.);
	ep = MAX(ep, 0.);
	ep = MIN(ep, 1.);

	// avoid inf's & nan's
	double num = MAX(1. - p, DBL_MIN);
	double denom = 1. - pow(1. - ep, model_points);
	if(denom < DBL_MIN)
		return 0;

	num = log(num);
	denom = log(denom);

	return denom >= 0 || -num >= max_iters * (-denom) ?
	max_iters : cvRound(num / denom);
}

bool SSMEstimator::runRANSAC(const CvMat* m1, const CvMat* m2, CvMat* model,
	CvMat* mask0, double reprojThreshold,double confidence, int maxIters,
	int maxAttempts) {
	bool result = false;
	cv::Ptr<CvMat> mask = cvCloneMat(mask0);
	cv::Ptr<CvMat> models, err, tmask;
	cv::Ptr<CvMat> ms1, ms2;

	int iter, niters = maxIters;
	int count = m1->rows * m1->cols, maxGoodCount = 0;
	CV_Assert(CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask));

	if(count < modelPoints)
		return false;

	models = cvCreateMat(modelSize.height * maxBasicSolutions, modelSize.width, CV_64FC1);
	err = cvCreateMat(1, count, CV_32FC1);
	tmask = cvCreateMat(1, count, CV_8UC1);

	if(count > modelPoints) {
		ms1 = cvCreateMat(1, modelPoints, m1->type);
		ms2 = cvCreateMat(1, modelPoints, m2->type);
	} else {
		niters = 1;
		ms1 = cvCloneMat(m1);
		ms2 = cvCloneMat(m2);
	}

	for(iter = 0; iter < niters; iter++) {
		int i, goodCount, nmodels;
		if(count > modelPoints) {
			bool found = getSubset(m1, m2, ms1, ms2, maxAttempts);
			if(!found) {
				if(iter == 0)
					return false;
				break;
			}
		}

		nmodels = runKernel(ms1, ms2, models);
		if(nmodels <= 0)
			continue;
		for(i = 0; i < nmodels; i++) {
			CvMat model_i;
			cvGetRows(models, &model_i, i * modelSize.height, (i + 1)*modelSize.height);
			goodCount = findInliers(m1, m2, &model_i, err, tmask, reprojThreshold);

			if(goodCount > MAX(maxGoodCount, modelPoints - 1)) {
				std::swap(tmask, mask);
				cvCopy(&model_i, model);
				maxGoodCount = goodCount;
				niters = cvRANSACUpdateNumIters(confidence,
					(double)(count - goodCount) / count, modelPoints, niters);
			}
		}
	}

	//printf("SSMEstimator::runRANSAC :: iter: %d\n", iter);

	if(maxGoodCount > 0) {
		if(mask != mask0)
			cvCopy(mask, mask0);
		result = true;
	}

	return result;
}

static CV_IMPLEMENT_QSORT(icvSortDistances, int, CV_LT)
bool SSMEstimator::runLMeDS(const CvMat* m1, const CvMat* m2, CvMat* model,
CvMat* mask, double confidence, int maxIters, int maxAttempts) {
	const double outlierRatio = 0.45;
	bool result = false;
	cv::Ptr<CvMat> models;
	cv::Ptr<CvMat> ms1, ms2;
	cv::Ptr<CvMat> err;

	int iter, niters = maxIters;
	int count = m1->rows * m1->cols;
	double minMedian = DBL_MAX, sigma;

	CV_Assert(CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask));

	if(count < modelPoints)
		return false;

	models = cvCreateMat(modelSize.height * maxBasicSolutions, modelSize.width, CV_64FC1);
	err = cvCreateMat(1, count, CV_32FC1);

	if(count > modelPoints) {
		ms1 = cvCreateMat(1, modelPoints, m1->type);
		ms2 = cvCreateMat(1, modelPoints, m2->type);
	} else {
		niters = 1;
		ms1 = cvCloneMat(m1);
		ms2 = cvCloneMat(m2);
	}

	niters = cvRound(log(1 - confidence) / log(1 - pow(1 - outlierRatio, (double)modelPoints)));
	niters = MIN(MAX(niters, 3), maxIters);

	for(iter = 0; iter < niters; iter++) {
		int i, nmodels;
		if(count > modelPoints) {
			bool found = getSubset(m1, m2, ms1, ms2, maxAttempts);
			if(!found) {
				if(iter == 0)
					return false;
				break;
			}
		}

		nmodels = runKernel(ms1, ms2, models);
		if(nmodels <= 0)
			continue;
		for(i = 0; i < nmodels; i++) {
			CvMat model_i;
			cvGetRows(models, &model_i, i * modelSize.height, (i + 1)*modelSize.height);
			computeReprojError(m1, m2, &model_i, err);
			icvSortDistances(err->data.i, count, 0);

			double median = count % 2 != 0 ?
				err->data.fl[count / 2] : (err->data.fl[count / 2 - 1] + err->data.fl[count / 2]) * 0.5;

			if(median < minMedian) {
				minMedian = median;
				cvCopy(&model_i, model);
			}
		}
	}

	//printf("SSMEstimator::runLMeDS :: iter: %d\n", iter);

	if(minMedian < DBL_MAX) {
		sigma = 2.5 * 1.4826 * (1 + 5. / (count - modelPoints)) * sqrt(minMedian);
		sigma = MAX(sigma, 0.001);

		count = findInliers(m1, m2, model, err, mask, sigma);
		result = count >= modelPoints;
	}

	return result;
}

bool SSMEstimator::getSubset(const CvMat* m1, const CvMat* m2,
	CvMat* ms1, CvMat* ms2, int maxAttempts) {
	cv::AutoBuffer<int> _idx(modelPoints);
	int* idx = _idx;
	int i = 0, j, k, idx_i, iters = 0;
	int type = CV_MAT_TYPE(m1->type), elemSize = CV_ELEM_SIZE(type);
	const int *m1ptr = m1->data.i, *m2ptr = m2->data.i;
	int *ms1ptr = ms1->data.i, *ms2ptr = ms2->data.i;
	int count = m1->cols * m1->rows;

	assert(CV_IS_MAT_CONT(m1->type & m2->type) && (elemSize % sizeof(int) == 0));
	elemSize /= sizeof(int);

	boost::random::uniform_int_distribution<int> uni(0, count-1);

	for(; iters < maxAttempts; iters++) {
		for(i = 0; i < modelPoints && iters < maxAttempts;) {
			idx[i] = idx_i = use_boost_rng ? uni(boost_rng) : cvRandInt(&cv_rng) % count;
			for(j = 0; j < i; j++)
				if(idx_i == idx[j])
					break;
			if(j < i)
				continue;
			for(k = 0; k < elemSize; k++) {
				ms1ptr[i * elemSize + k] = m1ptr[idx_i * elemSize + k];
				ms2ptr[i * elemSize + k] = m2ptr[idx_i * elemSize + k];
			}
			if(checkPartialSubsets && (!checkSubset(ms1, i + 1) || !checkSubset(ms2, i + 1))) {
				iters++;
				continue;
			}
			i++;
		}
		if(!checkPartialSubsets && i == modelPoints &&
			(!checkSubset(ms1, i) || !checkSubset(ms2, i)))
			continue;
		break;
	}

	return i == modelPoints && iters < maxAttempts;
}

bool SSMEstimator::checkSubset(const CvMat* m, int count) {
	if(count <= 2)
		return true;

	int j, k, i, i0, i1;
	CvPoint2D64f* ptr = (CvPoint2D64f*)m->data.ptr;

	assert(CV_MAT_TYPE(m->type) == CV_64FC2);

	if(checkPartialSubsets)
		i0 = i1 = count - 1;
	else
		i0 = 0, i1 = count - 1;

	for(i = i0; i <= i1; i++) {
		// check that the i-th selected point does not belong
		// to a line connecting some previously selected points
		for(j = 0; j < i; j++) {
			double dx1 = ptr[j].x - ptr[i].x;
			double dy1 = ptr[j].y - ptr[i].y;
			for(k = 0; k < j; k++) {
				double dx2 = ptr[k].x - ptr[i].x;
				double dy2 = ptr[k].y - ptr[i].y;
				if(fabs(dx2 * dy1 - dy2 * dx1) <= FLT_EPSILON * (fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2)))
					break;
			}
			if(k < j)
				break;
		}
		if(j < i)
			break;
	}

	return i > i1;
}

LevMarq::LevMarq() {
	mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = cv::Ptr<CvMat>();
	lambdaLg10 = 0;
	state = DONE;
	criteria = cvTermCriteria(0, 0, 0);
	iters = 0;
	completeSymmFlag = false;
}

LevMarq::LevMarq(int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag) {
	mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = cv::Ptr<CvMat>();
	init(nparams, nerrs, criteria0, _completeSymmFlag);
}

void LevMarq::clear() {
	mask.release();
	prevParam.release();
	param.release();
	J.release();
	err.release();
	JtJ.release();
	JtJN.release();
	JtErr.release();
	JtJV.release();
	JtJW.release();
}

LevMarq::~LevMarq() {
	clear();
}

void LevMarq::init(int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag) {
	if(!param || param->rows != nparams || nerrs != (err ? err->rows : 0))
		clear();
	mask = cvCreateMat(nparams, 1, CV_8U);
	cvSet(mask, cvScalarAll(1));
	prevParam = cvCreateMat(nparams, 1, CV_64F);
	param = cvCreateMat(nparams, 1, CV_64F);
	JtJ = cvCreateMat(nparams, nparams, CV_64F);
	JtJN = cvCreateMat(nparams, nparams, CV_64F);
	JtJV = cvCreateMat(nparams, nparams, CV_64F);
	JtJW = cvCreateMat(nparams, 1, CV_64F);
	JtErr = cvCreateMat(nparams, 1, CV_64F);
	if(nerrs > 0) {
		J = cvCreateMat(nerrs, nparams, CV_64F);
		err = cvCreateMat(nerrs, 1, CV_64F);
	}
	prevErrNorm = DBL_MAX;
	lambdaLg10 = -3;
	criteria = criteria0;
	if(criteria.type & CV_TERMCRIT_ITER)
		criteria.max_iter = MIN(MAX(criteria.max_iter, 1), 1000);
	else
		criteria.max_iter = 30;
	if(criteria.type & CV_TERMCRIT_EPS)
		criteria.epsilon = MAX(criteria.epsilon, 0);
	else
		criteria.epsilon = DBL_EPSILON;
	state = STARTED;
	iters = 0;
	completeSymmFlag = _completeSymmFlag;
}

bool LevMarq::update(const CvMat*& _param, CvMat*& matJ, CvMat*& _err) {
	double change;

	matJ = _err = 0;

	assert(!err.empty());
	if(state == DONE) {
		_param = param;
		return false;
	}

	if(state == STARTED) {
		_param = param;
		cvZero(J);
		cvZero(err);
		matJ = J;
		_err = err;
		state = CALC_J;
		return true;
	}

	if(state == CALC_J) {
		cvMulTransposed(J, JtJ, 1);
		cvGEMM(J, err, 1, 0, 0, JtErr, CV_GEMM_A_T);
		cvCopy(param, prevParam);
		step();
		if(iters == 0)
			prevErrNorm = cvNorm(err, 0, CV_L2);
		_param = param;
		cvZero(err);
		_err = err;
		state = CHECK_ERR;
		return true;
	}

	assert(state == CHECK_ERR);
	errNorm = cvNorm(err, 0, CV_L2);
	if(errNorm > prevErrNorm) {
		if(++lambdaLg10 <= 16) {
			step();
			_param = param;
			cvZero(err);
			_err = err;
			state = CHECK_ERR;
			return true;
		}
	}

	lambdaLg10 = MAX(lambdaLg10 - 1, -16);
	if(++iters >= criteria.max_iter ||
		(change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon) {
		_param = param;
		state = DONE;
		return true;
	}

	prevErrNorm = errNorm;
	_param = param;
	cvZero(J);
	matJ = J;
	_err = err;
	state = CALC_J;
	return true;
}

bool LevMarq::updateAlt(const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm) {
	double change;

	CV_Assert(err.empty());
	if(state == DONE) {
		_param = param;
		return false;
	}

	if(state == STARTED) {
		_param = param;
		cvZero(JtJ);
		cvZero(JtErr);
		errNorm = 0;
		_JtJ = JtJ;
		_JtErr = JtErr;
		_errNorm = &errNorm;
		state = CALC_J;
		return true;
	}

	if(state == CALC_J) {
		cvCopy(param, prevParam);
		step();
		_param = param;
		prevErrNorm = errNorm;
		errNorm = 0;
		_errNorm = &errNorm;
		state = CHECK_ERR;
		return true;
	}

	assert(state == CHECK_ERR);
	if(errNorm > prevErrNorm) {
		if(++lambdaLg10 <= 16) {
			step();
			_param = param;
			errNorm = 0;
			_errNorm = &errNorm;
			state = CHECK_ERR;
			return true;
		}
	}

	lambdaLg10 = MAX(lambdaLg10 - 1, -16);
	if(++iters >= criteria.max_iter ||
		(change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon) {
		_param = param;
		state = DONE;
		return false;
	}

	prevErrNorm = errNorm;
	cvZero(JtJ);
	cvZero(JtErr);
	_param = param;
	_JtJ = JtJ;
	_JtErr = JtErr;
	state = CALC_J;
	return true;
}

void LevMarq::step() {
	const double LOG10 = log(10.);
	double lambda = exp(lambdaLg10 * LOG10);
	int i, j, nparams = param->rows;

	for(i = 0; i < nparams; i++)
		if(mask->data.ptr[i] == 0) {
			double *row = JtJ->data.db + i * nparams, *col = JtJ->data.db + i;
			for(j = 0; j < nparams; j++)
				row[j] = col[j * nparams] = 0;
			JtErr->data.db[i] = 0;
		}

	if(!err)
		cvCompleteSymm(JtJ, completeSymmFlag);
#if 1
	cvCopy(JtJ, JtJN);
	for(i = 0; i < nparams; i++)
		JtJN->data.db[(nparams + 1)*i] *= 1. + lambda;
#else
	cvSetIdentity(JtJN, cvRealScalar(lambda));
	cvAdd(JtJ, JtJN, JtJN);
#endif
	cvSVD(JtJN, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T);
	cvSVBkSb(JtJW, JtJV, JtJV, JtErr, param, CV_SVD_U_T + CV_SVD_V_T);
	for(i = 0; i < nparams; i++)
		param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? param->data.db[i] : 0);
}

_MTF_END_NAMESPACE


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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
//==============================================================================
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/meta/float.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>

////////////////////////////////////////////////////////////////////////////////
// Test value of real_c constants for every base real types
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE( single_ )
{
  using boost::simd::meta::single_;

  single_<0x3F800000UL>          one_fc;
  float  one_f = 1.f;

  NT2_TEST_EQUAL( one_f, float(one_fc)  );
}


/**
* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
*
* Copyright (c) 2016 LIBSCAPI (http://crypto.biu.ac.il/SCAPI)
* This file is part of the SCAPI project.
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
* to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
* and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
* We request that any publication and/or code referring to and/or based on SCAPI contain an appropriate citation to SCAPI, including a reference to
* http://crypto.biu.ac.il/SCAPI.
*
* Libscapi uses several open source libraries. Please see these projects for any further licensing issues.
* For more information , See https://github.com/cryptobiu/libscapi/blob/master/LICENSE.MD
*
* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
*
*/

#pragma once

#define WIN32_LEAN_AND_MEAN

#include <boost/random.hpp>
#ifdef _WIN32
#include <boost/multiprecision/cpp_int.hpp>
#else
#include <boost/multiprecision/gmp.hpp>
#endif
#include <boost/algorithm/hex.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/timer/timer.hpp>
#include <math.h> /* pow */
#include <random>
#include <memory>
#include <iostream>
#include <fstream>
#include <unordered_map>
#include <chrono>

#include <boost/serialization/serialization.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/string.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/export.hpp>

using namespace std;

class IllegalStateException : public logic_error
{
public:
	IllegalStateException(const string & msg) : logic_error(msg) {};
};

class NotImplementedException : public logic_error
{
public:
	NotImplementedException(const string & msg) : logic_error(msg) {};
};

class InvalidKeyException : public logic_error
{
public:
	InvalidKeyException(const string & msg) : logic_error(msg) {};
};

class KeyException : public logic_error
{
public:
	KeyException(const string & msg) : logic_error(msg) {};
};

class UnsupportedOperationException : public logic_error
{
public:
	UnsupportedOperationException(const string & msg) : logic_error(msg) {};
};

class SecurityLevelException : public logic_error
{
public:
	SecurityLevelException(const string & msg) : logic_error(msg) {};
};

class CheatAttemptException : public logic_error
{
public:
	CheatAttemptException(const string & msg) : logic_error(msg) {};
};

// using boost::multiprecision:cpp_int - Arbitrary precision integer type.
namespace mp = boost::multiprecision;     // reduce the typing a bit later...

#ifdef _WIN32
using biginteger = boost::multiprecision::cpp_int;
#else
using biginteger = boost::multiprecision::mpz_int;
#endif

typedef unsigned char byte;		// put in global namespace to avoid ambiguity with other byte typedefs

int find_log2_floor(biginteger);
int NumberOfBits(const biginteger & bi);

/*
* Retruns the number of bytes needed to represent a biginteger
* Notice that due to the sign number of byte can exceed log(value)
*/
size_t bytesCount(const biginteger & value);

class PrgFromOpenSSLAES;

shared_ptr<PrgFromOpenSSLAES> get_seeded_prg();

void copy_byte_vector_to_byte_array(const vector<byte> &source_vector, byte * dest, int beginIndex);
void copy_byte_array_to_byte_vector(const byte* src, int src_len, vector<byte>& target_vector, int beginIndex);

/*
* Converting big integer to a byte array. Array must be allocated already
* Number can be postive or negative - the sign will be preserved in the encoding
* Use byteCount(biginteger) method to calculate the number of bytes needed.
*/
void encodeBigInteger(const biginteger & value, byte* output, size_t length);

void fastEncodeBigInteger(const biginteger & value, byte* output, size_t length);

/*
* Decodoing big integer from byte array back to a biginteger object
*/
biginteger decodeBigInteger(const byte* input, size_t length);

biginteger fastDecodeBigInteger(const byte* input, size_t length);

biginteger convert_hex_to_biginteger(const string & hex);

/*
* Returns a random biginteger uniformly distributed in [min, max]
*/
biginteger getRandomInRange(const biginteger & min, const biginteger & max, PrgFromOpenSSLAES* random);

biginteger fastGetRandomInRange(const biginteger & max, PrgFromOpenSSLAES* random, int length);
/*
* Returns a random prime number with the given number of bytes.
*/
biginteger getRandomPrime(int numBytes, int certainty, PrgFromOpenSSLAES* random);
const vector<string> explode(const string& s, const char& c);

bool isPrime(const biginteger & bi, int certainty = 40);

/********************/
/* Debugging Methods*/
/********************/

void print_elapsed_ms(std::chrono::time_point<std::chrono::system_clock> start, string message);
void print_elapsed_micros(std::chrono::time_point<std::chrono::system_clock> start, string message);
std::chrono::time_point<std::chrono::system_clock> scapi_now();
string hexStr(vector<byte> const & data);
void print_byte_array(byte * arr, int len, string message);
void gen_random_bytes_vector(vector<byte> &v, const int len, PrgFromOpenSSLAES* random);

/**
* Abstract marker interface that allow serialization and deserialization from byte array and size
*/
class NetworkSerialized {
public:
	virtual string toString() = 0;
	virtual void initFromString(const string & raw) = 0;
	virtual void initFromByteVector(const vector<byte> & byteVector) {
		const byte * uc = &(byteVector[0]);
		std::string s(reinterpret_cast<char const*>(uc), byteVector.size());
		initFromString(s);
	}
};


// MTL4 example: minimize complexity of traversal

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace mtl;

template <typename Matrix>
void f(Matrix& A)
{
    using traits::range_generator; using traits::range::min;

    // Set values in diagonal
    A= 7.0;

    // Choose between row and column traversal
    typedef typename min<range_generator<tag::row, Matrix>, range_generator<tag::col, Matrix> >::type range_type;
    range_type                                                      my_range;

    // Type of outer cursor
    typedef typename range_type::type                               c_type;
    // Type of inner cursor
    typedef typename traits::range_generator<tag::nz, c_type>::type ic_type;

    // Define the property maps
    typename traits::row<Matrix>::type                              row(A);
    typename traits::col<Matrix>::type                              col(A);
    typename traits::const_value<Matrix>::type                      value(A);

    // Now iterate over the matrix
    for (c_type cursor(my_range.begin(A)), cend(my_range.end(A)); cursor != cend; ++cursor)
       for (ic_type icursor(begin<tag::nz>(cursor)), icend(end<tag::nz>(cursor)); icursor != icend; ++icursor)
	   std::cout << "matrix[" << row(*icursor) << ", " << col(*icursor) << "] = " << value(*icursor) << '\n';
}

int main(int, char**)
{
    // Define a CRS matrix
    compressed2D<double>                                  A(3, 3);
    // And a CCS matrix
    compressed2D<double, mat::parameters<col_major> >  B(3, 3);

    f(A);
    f(B);

    return 0;
}


// Testbed_EigenRedSVD.cpp : Defines the entry point for the console application.
//
#include "stdafx.h"

#include <iostream>
#include <fstream>
#include <cmath>
#include <ctime>
#include <chrono>
#include <omp.h>

#define EIGEN_RUNTIME_NO_MALLOC
#define EIGEN_NO_AUTOMATIC_RESIZING

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

#include "RedSVD.h"

class Stopwatch {
public:
	void Start() {
		_start = _clock::now();
	};

	double Stop() const {
		return std::chrono::duration_cast<_seconds>(_clock::now() - _start).count();
	};

public:
	Stopwatch() {};

private:
	Stopwatch& operator=(const Stopwatch& other_) = delete;
	Stopwatch(const Stopwatch& other_) = delete;

private:
	typedef std::chrono::steady_clock _clock;
	typedef std::chrono::duration<double, std::ratio<1>> _seconds;
	std::chrono::time_point<_clock> _start;
};

class Sumulation {
public:
	static void TimeIterations(int numberOfIterations_, int numberOfRows_, int numberOfColumns_) {
		Stopwatch timer;
		double totalTime = 0.0;
		for (int iteration = 0; iteration < numberOfIterations_; ++iteration) {
			timer.Start();
			RunIteration(numberOfRows_, numberOfColumns_);
			double timeTaken = timer.Stop();
			totalTime = timeTaken;
			std::cout << "Time take for run: " << timeTaken << "seconds" << std::endl;
		}
		double averageTime = totalTime / numberOfIterations_;
		std::cout << "Average time take for " << numberOfIterations_ << " runs:" << std::endl;
		std::cout << averageTime << "seconds" << std::endl;
		std::cout << std::endl << std::endl;
	}

private:
	static void RunIteration(int numberOfRows_, int numberOfColumns_) {
		std::cout << "------------- Eigen Library Test -------------" << std::endl;
		std::cout << "Number Of Rows " << numberOfRows_ << "\tNumber of Columns " << numberOfColumns_ << std::endl;

		std::cout << "Making U & V" << std::endl;
		Eigen::MatrixXf U = Eigen::MatrixXf::Random(numberOfRows_, numberOfColumns_);
		Eigen::MatrixXf V = Eigen::MatrixXf::Random(numberOfRows_, numberOfColumns_);

		std::cout << "Diagonalising S" << std::endl;
		Eigen::VectorXf S = Eigen::VectorXf::Random(numberOfColumns_);
		Eigen::MatrixXf sDiagonal = S.asDiagonal();

		std::cout << "Transposing V" << std::endl;
		Eigen::MatrixXf vTransposed = V.transpose();

		std::cout << "Building A" << std::endl;
		Eigen::MatrixXf A;

		std::cout << "...using A = S(daigonal) * U * V(transposed)" << std::endl;
		A.noalias() = U * sDiagonal * vTransposed;

		/*std::cout << "Calculating SVD of U.S.V'" << std::endl;
		Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);*/
	};

private:
	Sumulation() = delete;
	Sumulation& operator=(const Sumulation& other_) = delete;
	Sumulation(const Sumulation& other_) = delete;
};

int main() {
	const int numberOfRows = 11344;
	const int numberOfColumns = 513;
	Stopwatch timer;

	int ompMaxThreads = omp_get_max_threads();
	std::cout << "OMP Max number of threads: " << ompMaxThreads << std::endl;
	omp_set_num_threads(ompMaxThreads);
	Eigen::setNbThreads(ompMaxThreads);
	std::cout << "Eigen number of threads: " << Eigen::nbThreads() << std::endl;

	Eigen::internal::set_is_malloc_allowed(false);

	Sumulation::TimeIterations(100, 100, 100);
	Sumulation::TimeIterations(100, 1000, 100);
	Sumulation::TimeIterations(100, 100, 1000);

	std::cout << "Press any key to quit!" << std::endl;
	std::cin.get();

	return 0;
}


// Filthy awful hack to test.
#include "gtest/gtest.h"
#include <Eigen/Dense>
#include <memory>
#define private public
#include "velocity_verlet.h"
#include "energies.h"

TEST(VerletTest, StationaryEnergy)
{
    Eigen::VectorXd rail(2);
    rail << 1.0, 0.0;
    auto bond_ptr = std::make_unique<HarmonicBond>(1.0, 1.0, 1, 0, rail);
    std::vector<std::unique_ptr<Bond>> bonds_vec;
    bonds_vec.push_back(std::move(bond_ptr));

    Eigen::MatrixXd positions(2, 2);
    positions << 0.0, 0.0,
                 1.0, 0.0;
    auto forces = calculate_forces(bonds_vec, positions);
    for (int row = 0; row < forces.rows(); ++row)  {
        for (int col = 0; col < forces.cols(); ++col)  {
            ASSERT_FLOAT_EQ(forces(row, col), 0.0);
        }
    }
}

TEST(VerletTest, ConservationOfEnergy)
{
    Eigen::VectorXd rail(2);
    rail << 1.0, 0.0;
    auto bond_ptr = std::make_unique<HarmonicBond>(1.0, 1.0, 1, 0, rail);
    std::vector<std::unique_ptr<Bond>> bonds_vec;
    bonds_vec.push_back(std::move(bond_ptr));

    Eigen::MatrixXd positions(2, 2);
    positions << 0.0, 0.0,
                 1.0, 0.0;

    Eigen::MatrixXd velocities(2, 2);
    positions << 0.0, 0.0,
                 0.0, 0.0;

    Eigen::MatrixXd accelerations(2, 2);
    positions << 0.0, 0.0,
                 0.0, 0.0;

    Eigen::VectorXd masses(2);
    masses << 1.0, 1.0;

    excite_bond(bonds_vec[0], positions, masses, 1.5);
    const auto initial_energy = calculate_kinetic_energy(velocities, masses) + calculate_potential_energy(bonds_vec, positions);

    constexpr int num_steps = 10000;
    constexpr double timestep = 0.01;
    constexpr double error_threshold = 1e-10;
    for (int i = 0; i < num_steps; ++i) {
        velocity_verlet(positions, velocities,
                        accelerations, masses,
                        timestep, bonds_vec);
        const auto total_energy = calculate_kinetic_energy(velocities, masses) + calculate_potential_energy(bonds_vec, positions);
        ASSERT_NEAR(initial_energy, total_energy, error_threshold);
    }
}