/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>, Randi Cabezas <rcabezas@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>

#include <dpMM/basemeasure.hpp>
#include <dpMM/niw.hpp>

/*
 * NIW base measure; integrates over Normal distribution parameters
 */
template<typename T>
class NiwMarginalized : public BaseMeasure<T>
{
public:
  NIW<T> niw0_;
  NIW<T> niw_;

  NiwMarginalized(const NIW<T>& niw);
  ~NiwMarginalized();

  virtual baseMeasureType getBaseMeasureType() const {return(NIW_MARGINALIZED); }

  virtual BaseMeasure<T>* copy();

  T logLikelihood(const Matrix<T,Dynamic,1>& x) const;

  void posterior(const Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z,
      uint32_t k);

  T logPdfUnderPrior() const;

  void print() const ;
  virtual uint32_t getDim() const {return(uint32_t(niw_.D_));};
};

/*
 * NIW base measure; samples Normal distribution parameters
 */
template<typename T>
class NiwSampled : public BaseMeasure<T>
{
public:
  NIW<T> niw0_;
  Normal<T> normal_;

  NiwSampled(const NIW<T>& niw);
  NiwSampled(const NIW<T>& niw, const Normal<T> &normal);
  ~NiwSampled();

  virtual baseMeasureType getBaseMeasureType() const {return(NIW_SAMPLED); }

  virtual BaseMeasure<T>* copy();
  virtual NiwSampled<T>* copyNative();

  T logLikelihood(const Matrix<T,Dynamic,1>& x) const;
  T logLikelihood(const Matrix<T,Dynamic,Dynamic>& x, uint32_t i) const
    {return logLikelihood(x.col(i));};
  // assumes vector [N, sum(x), flatten(sum(outer(x,x)))]
  T logLikelihoodFromSS(const Matrix<T,Dynamic,1>& x) const;
  void posterior(const Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z,
    uint32_t k);
  void posterior(const vector<Matrix<T,Dynamic,Dynamic> >&x, const
      VectorXu& z, uint32_t k);
  void posteriorFromSS(const vector<Matrix<T,Dynamic,1> >&x, const
      VectorXu& z, uint32_t k);
  void posteriorFromSS(const Matrix<T,Dynamic,1> &x);
  void sample();

  T logPdfUnderPrior() const;
  T logPdfUnderPriorMarginalized() const;
  T logPdfUnderPriorMarginalizedMerged(const shared_ptr<NiwSampled<T> >& other) const;

  T logPdfUnderPriorMarginalized(const Matrix<T,Dynamic,1>& x) {return 0.;};

  virtual NiwSampled<T>* merge(const NiwSampled<T>& other);
  void fromMerge(const NiwSampled<T>& niwA, const NiwSampled<T>& niwB);

  void print() const;
  virtual uint32_t getDim() const {return(uint32_t(normal_.D_));};

  const Matrix<T,Dynamic,Dynamic>& scatter() const {return niw0_.scatter();};
  const Matrix<T,Dynamic,1>& mean() const {return niw0_.mean();};
  T count() const {return niw0_.count();};
//  T& count() {return niw0_.count_;};
  const Matrix<T,Dynamic,1>& getMean() const {return normal_.mu_;};

  const Matrix<T,Dynamic,Dynamic>& Sigma() const {return normal_.Sigma();};
private:

};

typedef NiwSampled<double> NiwSampledd;
typedef NiwSampled<float> NiwSampledf;

// ---------------------------------------------------------------------------

template<typename T>
NiwSampled<T>::NiwSampled(const NIW<T>& niw)
  : niw0_(niw), normal_(niw0_.sample())
{};

template<typename T>
NiwSampled<T>::NiwSampled(const NIW<T>& niw, const Normal<T> &normal)
 : niw0_(niw), normal_(normal)
{};

template<typename T>
NiwSampled<T>::~NiwSampled()
{};

template<typename T>
BaseMeasure<T>* NiwSampled<T>::copy()
{
  NiwSampled<T>* niwSampled = new NiwSampled<T>(niw0_);
  niwSampled->normal_ = normal_;
  return niwSampled;
};

template<typename T>
NiwSampled<T>* NiwSampled<T>::copyNative()
{
  NiwSampled<T>* niwSampled = new NiwSampled<T>(niw0_);
  niwSampled->normal_ = normal_;
  return niwSampled;
};

template<typename T>
T NiwSampled<T>::logLikelihood(const Matrix<T,Dynamic,1>& x) const
{
//  normal_.print();
  T logLike = normal_.logPdf(x);
//  cout<<x.transpose()<<" -> " <<logLike<<endl;
//  cout<<x.transpose()<<" -> " <<normal_.logPdfSlower(x)<<endl;
  return logLike;
};

// assumes vector [N, sum(x), flatten(sum(outer(x,x)))]
template<typename T>
T NiwSampled<T>::logLikelihoodFromSS(const Matrix<T,Dynamic,1>& x) const
{
//  normal_.print();
  uint32_t D = niw0_.D_;
  T count = x(0);
  Matrix<T,Dynamic,1> mean(D);
  if(count>0)
	  mean = x.middleRows(1,D)/count;
  else
	  mean = Matrix<T,Dynamic,1>::Zero(D); //this should not matter since everything gets multiplied by 0 counts

  //NOTE: Eigen::Map does not like const data, so this cast is needed to strip const data from input
  //alternatively the input could be changed to non-const, but this is cleaner from the outside
  T* datPtr = const_cast<T*>(&(x.data()[(D+1)]));
  Matrix<T,Dynamic,Dynamic> scatter =
    Map<Matrix<T,Dynamic,Dynamic> >(datPtr,D,D);
  scatter -= (mean*mean.transpose())*count;

  T logLike = normal_.logPdf(scatter,mean,count);
  //  cout<<x.transpose()<<" -> " <<logLike<<endl;
  //  cout<<x.transpose()<<" -> " <<normal_.logPdfSlower(x)<<endl;
  return logLike;
};

template<typename T>
void NiwSampled<T>::posterior(const Matrix<T,Dynamic,Dynamic>& x,
    const VectorXu& z, uint32_t k)
{
  normal_ = niw0_.posterior(x,z,k).sample();
};

template<typename T>
void NiwSampled<T>::posterior(const vector<Matrix<T,Dynamic,Dynamic> >&x,
	const VectorXu& z, uint32_t k)
{
	normal_ = niw0_.posterior(x,z,k).sample();
}

template<typename T>
void NiwSampled<T>::posteriorFromSS(const vector<Matrix<T,Dynamic,1> > &x, const VectorXu& z, uint32_t k)
{
	normal_ = niw0_.posteriorFromSS(x,z,k).sample();
}

template<typename T>
void NiwSampled<T>::posteriorFromSS(const Matrix<T,Dynamic,1> &x)
{
	normal_ = niw0_.posteriorFromSS(x).sample();
}

template<typename T>
T NiwSampled<T>::logPdfUnderPrior() const
{
  return niw0_.logPdf(normal_);
};

template<typename T>
T NiwSampled<T>::logPdfUnderPriorMarginalized() const
{
  // evaluates log pdf of sufficient statistics stored within niw0_
//  niw0_.print();
  return niw0_.logPdfMarginalized();
};

template<typename T>
T NiwSampled<T>::logPdfUnderPriorMarginalizedMerged(
    const shared_ptr<NiwSampled<T> >& other) const
{
  return niw0_.logPdfUnderPriorMarginalizedMerged(other->niw0_);
};

template<typename T>
void NiwSampled<T>::fromMerge(const NiwSampled<T>& niwA,
  const NiwSampled<T>& niwB)
{
  niw0_.fromMerge(niwA.niw0_,niwB.niw0_);
  normal_ = niw0_.posterior().sample();
};

template<typename T>
NiwSampled<T>* NiwSampled<T>::merge(const NiwSampled<T>& other)
{
  NiwSampled<T>* newNiw = this->copyNative();
  newNiw->niw0_.fromMerge(niw0_,other.niw0_);
  newNiw->sample();
  return newNiw;
};

template<typename T>
void NiwSampled<T>::sample()
{
  normal_ = niw0_.posterior().sample();
};

template<typename T>
void NiwSampled<T>::print() const
{
  niw0_.posterior().print();
  normal_.print();
};

// ----------------------------------------------------------------------------

template<typename T>
NiwMarginalized<T>::NiwMarginalized(const NIW<T>& niw)
  : niw0_(niw),niw_(niw)
{};

template<typename T>
NiwMarginalized<T>::~NiwMarginalized()
{};

template<typename T>
BaseMeasure<T>* NiwMarginalized<T>::copy()
{
  return new NiwMarginalized<T>(niw0_);
};

template<typename T>
T NiwMarginalized<T>::logLikelihood(const Matrix<T,Dynamic,1>& x) const
{
  return niw_.logProb(x);
};

template<typename T>
void NiwMarginalized<T>::posterior(const Matrix<T,Dynamic,Dynamic>& x,
    const VectorXu& z, uint32_t k)
{
  niw_ = niw0_.posterior(x,z,k);
};

template<typename T>
T NiwMarginalized<T>::logPdfUnderPrior() const
{
  // 0 since we integrate over parameters -> there is no parameters for which
  // we would want to eval a pdf
  return 0.0;
};

template<typename T>
void NiwMarginalized<T>::print() const
{
  niw0_.posterior().print();
};


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

  @copyright 2016 NumScale SAS

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

/*!
  @ingroup group-constant
  @defgroup constant-Log2_e Log2_e (function template)

  Generates the constant \f$\log_2(e)\f$

  @headerref{<boost/simd/constant/log2_e.hpp>}

  @par Description

  1.  @code
      template<typename T> T Log2_e();
      @endcode

  2.  @code
      template<typename T> T Log2_e( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T that evaluates to \f$\log_2(e)\f$.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to `T(1.4426950408889634073599246810019)`

  @par Requirements
  - **T** models IEEEValue
**/

#include <boost/simd/constant/scalar/log2_e.hpp>
#include <boost/simd/constant/simd/log2_e.hpp>

#endif


#ifndef SHAPES_SPHERE_HPP
#define SHAPES_SPHERE_HPP

#include <math/point3d.hpp>

#include <boost/optional.hpp>

namespace math { struct ray3d; }

namespace shapes
{

struct intersection_info;

struct sphere
{
  math::point3d center;
  float radius;
};

inline bool operator==(sphere lhs, sphere rhs)
{
  return std::tie(lhs.center, lhs.radius) == std::tie(rhs.center, rhs.radius);
}

inline bool operator!=(sphere lhs, sphere rhs)
{
  return !(lhs == rhs);
}

// Check for ray-sphere intersection using geometric test.
//
// Note:
//  If ray origin is on the surface of the sphere and ray direction points
//  to the outside, no intersection is found.
bool intersects(shapes::sphere shape, math::ray3d ray);

// Returns the closest intersection point between ray and sphere.
//
// Note:
//  If ray origin is on the surface of the sphere and ray direction points
//  to the outside, no intersection is found.
boost::optional<shapes::intersection_info> closest_intersection(shapes::sphere shape, math::ray3d ray);

}

#endif


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

#ifndef BOOST_MATH_CCMATH_HYPOT_HPP
#define BOOST_MATH_CCMATH_HYPOT_HPP

#include <cmath>
#include <array>
#include <limits>
#include <type_traits>
#include <boost/math/tools/config.hpp>
#include <boost/math/tools/is_constant_evaluated.hpp>
#include <boost/math/ccmath/sqrt.hpp>
#include <boost/math/ccmath/abs.hpp>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/detail/swap.hpp>

namespace boost::math::ccmath {

namespace detail {

template <typename T>
inline constexpr T hypot_impl(T x, T y) noexcept
{
    x = boost::math::ccmath::abs(x);
    y = boost::math::ccmath::abs(y);

    if (y > x)
    {
        boost::math::ccmath::detail::swap(x, y);
    }

    if(x * std::numeric_limits<T>::epsilon() >= y)
    {
        return x;
    }

    T rat = y / x;
    return x * boost::math::ccmath::sqrt(1 + rat * rat);
}

} // Namespace detail

template <typename Real, std::enable_if_t<!std::is_integral_v<Real>, bool> = true>
inline constexpr Real hypot(Real x, Real y) noexcept
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(x))
    {
        return boost::math::ccmath::abs(x) == Real(0) ? boost::math::ccmath::abs(y) :
               boost::math::ccmath::abs(y) == Real(0) ? boost::math::ccmath::abs(x) :
               boost::math::ccmath::isinf(x) ? std::numeric_limits<Real>::infinity() :
               boost::math::ccmath::isinf(y) ? std::numeric_limits<Real>::infinity() :
               boost::math::ccmath::isnan(x) ? std::numeric_limits<Real>::quiet_NaN() :
               boost::math::ccmath::isnan(y) ? std::numeric_limits<Real>::quiet_NaN() :
               boost::math::ccmath::detail::hypot_impl(x, y);
    }
    else
    {
        using std::hypot;
        return hypot(x, y);
    }
}

template <typename T1, typename T2>
inline constexpr auto hypot(T1 x, T2 y) noexcept
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(x))
    {
        // If the type is an integer (e.g. epsilon == 0) then set the epsilon value to 1 so that type is at a minimum
        // cast to double
        constexpr auto T1p = std::numeric_limits<T1>::epsilon() > 0 ? std::numeric_limits<T1>::epsilon() : 1;
        constexpr auto T2p = std::numeric_limits<T2>::epsilon() > 0 ? std::numeric_limits<T2>::epsilon() : 1;

        using promoted_type =
                              #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
                              std::conditional_t<T1p <= LDBL_EPSILON && T1p <= T2p, T1,
                              std::conditional_t<T2p <= LDBL_EPSILON && T2p <= T1p, T2,
                              #endif
                              std::conditional_t<T1p <= DBL_EPSILON && T1p <= T2p, T1,
                              std::conditional_t<T2p <= DBL_EPSILON && T2p <= T1p, T2, double
                              #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
                              >>>>;
                              #else
                              >>;
                              #endif

        return boost::math::ccmath::hypot(promoted_type(x), promoted_type(y));
    }
    else
    {
        using std::hypot;
        return hypot(x, y);
    }
}

inline constexpr float hypotf(float x, float y) noexcept
{
    return boost::math::ccmath::hypot(x, y);
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline constexpr long double hypotl(long double x, long double y) noexcept
{
    return boost::math::ccmath::hypot(x, y);
}
#endif

} // Namespaces

#endif // BOOST_MATH_CCMATH_HYPOT_HPP


/*
 * This file is part of bogus, a C++ sparse block matrix library.
 *
 * Copyright 2013 Gilles Daviet <gdaviet@gmail.com>
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/

#ifndef BOGUS_EIGEN_LINEAR_SOLVERS
#define BOGUS_EIGEN_LINEAR_SOLVERS

#include "../Utils/LinearSolverBase.hpp"

#include <Eigen/LU>
#include <Eigen/Cholesky>

namespace bogus {

template<typename Decomposition, typename RhsType>
struct EigenSolveResult
{
#if EIGEN_VERSION_AT_LEAST(3,2,90)
	  typedef const Eigen::Solve< Decomposition, RhsType > Type ;
#else
	  typedef Eigen::internal::solve_retval< Decomposition, RhsType > Type ;
#endif
};

template < typename Derived >
struct LinearSolverTraits< LU< Eigen::MatrixBase< Derived > > >
{
	typedef typename Derived::PlainObject MatrixType ;
	typedef Eigen::FullPivLU< MatrixType > FactType ;

	template < typename RhsT > struct Result {
		typedef typename EigenSolveResult< FactType, RhsT >::Type Type ;
	} ;
	template < typename RhsT >
	struct Result< Eigen::MatrixBase< RhsT > > {
		typedef typename Result< RhsT >::Type Type ;
	} ;
} ;

template < typename Derived >
struct LU< Eigen::MatrixBase< Derived > >
		: public LinearSolverBase< LU< Eigen::MatrixBase< Derived > > >
{
	typedef Eigen::MatrixBase< Derived > MatrixType ;
	typedef LinearSolverTraits< LU< MatrixType > > Traits ;

	LU() {}
	template< typename OtherDerived >
	explicit LU ( const Eigen::MatrixBase< OtherDerived >& mat )
		: m_fact( mat )
	{}

	template< typename OtherDerived >
	LU< Eigen::MatrixBase< Derived > >& compute ( const Eigen::MatrixBase< OtherDerived >& mat )
	{
		m_fact.compute( mat ) ;
		return *this ;
	}

	template < typename RhsT, typename ResT >
	void solve( const Eigen::MatrixBase< RhsT >& rhs, ResT& res ) const
	{
		res = m_fact.solve( rhs ) ;
	}

	template < typename RhsT >
	typename Traits::template Result< Eigen::MatrixBase< RhsT > >::Type
	solve( const Eigen::MatrixBase< RhsT >& rhs ) const
	{
		return m_fact.solve( rhs ) ;
	}

  private:
	typename Traits::FactType m_fact ;
} ;

template < typename Scalar, int Rows, int Cols = Rows, int Options = 0 >
struct DenseLU : public LU< Eigen::MatrixBase< Eigen::Matrix< Scalar, Rows, Cols, Options > > >
{
	DenseLU() {}
	template< typename OtherDerived >
	explicit DenseLU ( const Eigen::MatrixBase< OtherDerived >& mat )
		: LU< Eigen::MatrixBase< Eigen::Matrix< Scalar, Rows, Cols, Options > > >( mat )
	{}
} ;

template < typename Derived >
struct LinearSolverTraits< LDLT< Eigen::MatrixBase< Derived > > >
{
	typedef typename Derived::PlainObject MatrixType ;
	typedef Eigen::LDLT< MatrixType > FactType ;

	template < typename RhsT > struct Result {
		typedef typename EigenSolveResult< FactType, RhsT >::Type Type ;
	} ;
	template < typename RhsT >
	struct Result< Eigen::MatrixBase< RhsT > > {
		typedef typename Result< RhsT >::Type Type ;
	} ;
} ;

template < typename Derived >
struct LDLT< Eigen::MatrixBase< Derived > >
		: public LinearSolverBase< LDLT< Eigen::MatrixBase< Derived > > >
{
	typedef Eigen::MatrixBase< Derived > MatrixType ;
	typedef LinearSolverTraits< LDLT< MatrixType > > Traits ;

	LDLT() {}
	template< typename OtherDerived >
	explicit LDLT ( const Eigen::MatrixBase< OtherDerived >& mat )
		: m_fact( mat )
	{}

	template< typename OtherDerived >
	LDLT< Eigen::MatrixBase< Derived > >& compute ( const Eigen::MatrixBase< OtherDerived >& mat )
	{
		m_fact.compute( mat ) ;
		return *this ;
	}

	template < typename RhsT, typename ResT >
	void solve( const Eigen::MatrixBase< RhsT >& rhs, ResT& res ) const
	{
		res = m_fact.solve( rhs ) ;
	}

	template < typename RhsT >
	typename Traits::template Result< Eigen::MatrixBase< RhsT > >::Type
	solve( const Eigen::MatrixBase< RhsT >& rhs ) const
	{
		return m_fact.solve( rhs ) ;
	}

  private:
	typename Traits::FactType m_fact ;
} ;

template < typename Scalar, int Rows, int Options = 0 >
struct DenseLDLT : public LDLT< Eigen::MatrixBase< Eigen::Matrix< Scalar, Rows, Rows, Options > > >
{
	DenseLDLT() {}
	template< typename OtherDerived >
	explicit DenseLDLT ( const Eigen::MatrixBase< OtherDerived >& mat )
		: LDLT< Eigen::MatrixBase< Eigen::Matrix< Scalar, Rows, Rows, Options > > >( mat )
	{}
} ;

template < typename Derived, typename RhsT >
typename LinearSolverTraits< Derived >::template Result< Eigen::MatrixBase< RhsT > >::Type operator*
	( const LinearSolverBase< Derived >& solver, const Eigen::MatrixBase< RhsT >& rhs )
{
  return solver.solve( rhs ) ;
}

} //namespace bogus

#endif


#include "penalized_gd_solver.h"
#include "../../common.h"
#include "../../logging/easylogging++.h"
#include <armadillo>

using arma::mat;
using arma::pow;
using arma::sum;

PenalizedGDSolver::PenalizedGDSolver(const arma::mat &L,
                                     const double lr,
                                     const int max_iter,
                                     const double termination_threshold,
                                     const double penalty)
    : GDSolver(L, lr, max_iter, termination_threshold)
{
    m_penalty = penalty;
};

mat PenalizedGDSolver::objective(mat estimate, mat expected)
{
    arma::uvec negative_indices = arma::find(m_x < 0);
    arma::mat x_negative_only = arma::abs(m_x.cols(negative_indices));
    arma::mat penalty = m_penalty * arma::sum(x_negative_only, 1);
    arma::mat rmse_value = rmse(estimate, expected);
    arma::mat objective_value = rmse_value + penalty;
    LOG(DEBUG) << "Round " << m_round;
    LOG(DEBUG) << "Solution " << m_x;
    LOG(DEBUG) << "Penalty " << penalty;
    LOG(DEBUG) << "RMSE " << rmse_value;
    LOG(DEBUG) << "Objective " << objective_value;
    return objective_value;
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include <eve/function/ellint_rc.hpp>

#include <boost/math/special_functions/ellint_rc.hpp>
#include <eve/wide.hpp>

TTS_CASE_TPL("Check eve::ellint_rc behavior", EVE_TYPE)
{
  using elt_t = eve::element_type_t<T>;
  TTS_ULP_EQUAL(eve::ellint_rc(T(0.2), T(0.4)),  T(boost::math::ellint_rc(elt_t(0.2), elt_t(0.4))),   1.0);
  TTS_ULP_EQUAL(eve::ellint_rc(T(1.5), T(1)),T(boost::math::ellint_rc(elt_t(1.5), elt_t(1))), 1.0);
  TTS_ULP_EQUAL(eve::ellint_rc(T(0), T(5)),  T(boost::math::ellint_rc(elt_t(0), elt_t(5))),   1.0);
  TTS_ULP_EQUAL(eve::ellint_rc(T(2), T(5)),  T(boost::math::ellint_rc(elt_t(2), elt_t(5))),   1.0);
  TTS_ULP_EQUAL(eve::ellint_rc(T(2), T(2)),  T(boost::math::ellint_rc(elt_t(2), elt_t(2))),   1.0);
  TTS_ULP_EQUAL(eve::ellint_rc(T(2), T(0.1)),  T(boost::math::ellint_rc(elt_t(2), elt_t(0.1))),   1.0);
  TTS_ULP_EQUAL(eve::ellint_rc(T(1.0e38), T(0.1)),  T(boost::math::ellint_rc(elt_t(1.0e38), elt_t(0.1))),   1.0);
  TTS_ULP_EQUAL(eve::ellint_rc(T(1.0), T(1.0e38)),  T(boost::math::ellint_rc(elt_t(1.0),elt_t(1.0e38))),   1.0);
  TTS_ULP_EQUAL(eve::ellint_rc(T(1.0e38), T(1.0e38)),  T(boost::math::ellint_rc(elt_t(1.0e38),elt_t(1.0e38))),   1.0);
}


// test_linear_systems_of_equations.cpp
// (c) Tivole

#include <boost/test/unit_test.hpp>
#include "../src/numerary.hpp"

namespace numerary
{

    BOOST_AUTO_TEST_SUITE(TestLinearSystemsOfEquations)

    BOOST_AUTO_TEST_CASE(test_gauss_elimination)
    {
        double **a = new double*[3];
        double *y = new double[3];
        double *x = new double[3];
        double *expected_x = new double[3];
        short int result;

        for (int i = 0; i < 3; i ++)
            a[i] = new double[3];

        expected_x[0] = 5.0/2.0;
        expected_x[1] = 2.0/3.0;
        expected_x[2] = 2.0/9.0;

        a[0][0] = 2;
        a[0][1] = -1;
        a[0][2] = 3;

        a[1][0] = 2;
        a[1][1] = 2;
        a[1][2] = 3;

        a[2][0] = -2;
        a[2][1] = 3;
        a[2][2] = 0;

        y[0] = 5;
        y[1] = 7;
        y[2] = -3;

        result = Numerary::linear_systems_of_equations(a, y, x, 3, "gauss");

        for (int i = 0; i < 3; i ++) BOOST_CHECK(fabs(expected_x[i] - x[i]) < 1.e-7);

        for (int i = 0; i < 3; i++) delete[] a[i];

        delete[] a;
        delete[] x;
        delete[] y;
        delete[] expected_x;

    }

    BOOST_AUTO_TEST_SUITE_END()
}


/*
 * lorenz.hpp
 *
 * Copyright 2011 Mario Mulansky
 * Copyright 2012 Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef LORENZ_HPP_
#define LORENZ_HPP_

#include <boost/array.hpp>

struct lorenz
{
    template< class state_type >
    void inline operator()( const state_type &x , state_type &dxdt , const double t ) const
    {
        const double sigma = 10.0;
        const double R = 28.0;
        const double b = 8.0 / 3.0;
        dxdt[0] = sigma * ( x[1] - x[0] );
        dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
        dxdt[2] = x[0]*x[1] - b * x[2];
    }
};

#endif /* LORENZ_HPP_ */


// SPDX-License-Identifier: Apache-2.0
// Copyright 2021 - 2021, the Anboto author and contributors
#include <Core/Core.h>
#include <Functions4U/Functions4U.h>
#include <Eigen/Eigen.h>
#include "Permutations.h"

namespace Upp {

using namespace Eigen;

Buffer<Buffer<int>> PermutationsWithRepetition(int nVals, int nOptionsVal) {
	Buffer<Buffer<int>> list;

	int num = int(pow(nOptionsVal, nVals));
	list.Alloc(nVals);
	for (int ip = 0; ip < nVals; ++ip) {
		list[ip].Alloc(num);
		int nrep = int(pow(nOptionsVal, ip));
		int val = 0, irep = 0;
		for (int i = 0; i < num; ++i) {
			list[ip][i] = val;
			irep++;
			if (irep >= nrep) {
				irep = 0;
				val++;
				if (val >= nOptionsVal)
					val = 0;
			}
		}
	}
	return list;
}

}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/containers/t4mesh/t4mesh.h>
#include <OpenTissue/core/geometry/geometry_compute_inscribed_circumscribed_radius_quality_measure.h>
#include <OpenTissue/core/geometry/geometry_compute_volume_length_quality_measure.h>
#include <OpenTissue/core/geometry/geometry_compute_inscribed_radius_length_quality_measure.h>
#include <OpenTissue/core/containers/t4mesh/util/t4mesh_compute_mesh_quality.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_t4mesh_compute_mesh_quality);

BOOST_AUTO_TEST_CASE(quality_testing)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t>   math_types;
  typedef OpenTissue::t4mesh::T4Mesh< math_types >           mesh_type;
  typedef math_types::vector3_type                           vector3_type;
  typedef vector3_type::value_type                           real_type;

  std::vector<vector3_type> c;
  c.resize( 6 );
  c[0] = vector3_type(0,0,0);
  c[1] = vector3_type(0,0,1);
  c[2] = vector3_type(0,1,0);
  c[3] = vector3_type(0,1,1);
  c[4] = vector3_type(1,0,0);
  c[5] = vector3_type(1,0,1);

  mesh_type M;
  M.insert( c[0] );
  M.insert( c[1] );
  M.insert( c[2] );
  M.insert( c[3] );
  M.insert( c[4] );
  M.insert( c[5] );
  M.insert(0,1,2,3);
  M.insert(0,1,2,4);

  {
    using namespace OpenTissue::geometry;
    std::vector<real_type> Q;
    OpenTissue::t4mesh::compute_mesh_quality( M, c, Q, &(compute_inscribed_circumscribed_radius_quality_measure<vector3_type>)  );

    BOOST_CHECK(Q.size() == 2);
  }
  {
    using namespace OpenTissue::geometry;
    std::vector<real_type> Q;
    OpenTissue::t4mesh::compute_mesh_quality( M,Q, &(compute_inscribed_circumscribed_radius_quality_measure<vector3_type>)  );

    BOOST_CHECK(Q.size() == 2);
  }
  {
    using namespace OpenTissue::geometry;
    std::vector<real_type> Q;
    OpenTissue::t4mesh::compute_mesh_quality( M,c, Q, &(compute_volume_length_quality_measure<vector3_type>)  );

    BOOST_CHECK(Q.size() == 2);
  }
  {
    using namespace OpenTissue::geometry;
    std::vector<real_type> Q;
    OpenTissue::t4mesh::compute_mesh_quality( M,Q, &(compute_volume_length_quality_measure<vector3_type>)  );

    BOOST_CHECK(Q.size() == 2);
  }
  {
    using namespace OpenTissue::geometry;
    std::vector<real_type> Q;
    OpenTissue::t4mesh::compute_mesh_quality( M, c, Q, &(compute_inscribed_radius_length_quality_measure<vector3_type>)  );

    BOOST_CHECK(Q.size() == 2);
  }
  {
    using namespace OpenTissue::geometry;
    std::vector<real_type> Q;
    OpenTissue::t4mesh::compute_mesh_quality( M, Q, &(compute_inscribed_radius_length_quality_measure<vector3_type>)  );
    BOOST_CHECK(Q.size() == 2);
  }

}

BOOST_AUTO_TEST_SUITE_END();


// ConsoleApplication1.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

#include <iostream>
#include <boost/math/differentiation/autodiff.hpp>
#include "Instrument.h"
#include "Interpolator.h"
#include "Holiday.h"
#include "Utility.h"
#include "YearFraction.h"

using namespace boost::math::differentiation;
typedef boost::gregorian::date date;

//class FixedCashFlow {
//private:
//    double _flow;
//
//public:
//    double accural_yf;
//    double coupon_rate;
//    double payment_date;
//    double notional;
//
//    FixedCashFlow() {
//        accural_yf = 1.0;
//        coupon_rate = 0.02;
//        notional = 1000000.0;
//
//    }
//
//    double get_flow() {
//        _flow = notional * accural_yf * coupon_rate;
//        return _flow;
//    }
//
//};
//
//double get_dsc() {
//    return 0.9;
//}
//
//template <typename T>
//T plus(T const& x, T const& y) {
//
//    return x + y;
//}
//
//static class FixedFlowCalculator {
//
//private:
//    const static unsigned Order = 2;                  // Highest order derivative to be calculated.
//
//    template <typename T>
//    static T _price(double const& flow, T const& DF) {
//        return flow * DF;
//    }
//
//public:
//    auto static price(FixedCashFlow& cf) {
//        auto const df = make_fvar<double, Order>(get_dsc());
//        auto const pv = _price(cf.get_flow(), df);
//        return pv;
//    }
//};

//int main()
//{
//    deriv2 deriv_obj = deriv2(2.0);
//    deriv_obj.show_value();
//    std::cout << "Hello World!\n";
//
//    FixedCashFlow a_cash = FixedCashFlow();
//    std::cout << "my flow is: " << a_cash.get_flow() << std::endl;
//    auto pv = FixedFlowCalculator::price(a_cash);
//
//    std::cout << "PV = " << pv.derivative(0) << std::endl;
//    std::cout << "dPV/dDF = " << pv.derivative(1) << std::endl;
//
//    auto pv2 = plus(pv,pv);
//    std::cout << "PV = " << pv2.derivative(0) << std::endl;
//    std::cout << "dPV/dDF = " << pv2.derivative(1) << std::endl;
//
//    //using namespace boost::math::differentiation;
//
//    //constexpr unsigned Order = 5;                  // Highest order derivative to be calculated.
//    //auto const x = make_fvar<double, Order>(2.0);  // Find derivatives at x=2.
//    //auto const y = fourth_power(x);
//    //for (unsigned i = 0; i <= Order; ++i)
//    //    std::cout << "y.derivative(" << i << ") = " << y.derivative(i) << std::endl;
//
//}

// Equations and function/variable names are from
// https://en.wikipedia.org/wiki/Greeks_(finance)#Formulas_for_European_option_Greeks
// Standard normal probability density function
template<typename X>
X phi(const X& x)
{
    return boost::math::constants::one_div_root_two_pi<double>() * exp(-0.5 * x * x);
}
// Standard normal cumulative distribution function
template<typename X>
X Phi(const X& x)
{
    return 0.5 * erfc(-boost::math::constants::one_div_root_two<double>() * x);
}
enum CP { call, put };
// Assume zero annual dividend yield (q=0).
using namespace boost::math::differentiation;

template<typename Price, typename Sigma, typename Tau, typename Rate>
promote<Price, Sigma, Tau, Rate>
black_scholes_option_price(CP cp, double K, const Price& S, const Sigma& sigma, const Tau& tau, const Rate& r)
{
    using namespace std;
    const auto d1 = (log(S / K) + (r + sigma * sigma / 2) * tau) / (sigma * sqrt(tau));
    const auto d2 = (log(S / K) + (r - sigma * sigma / 2) * tau) / (sigma * sqrt(tau));
    if (cp == call)
        return S * Phi(d1) - exp(-r * tau) * K * Phi(d2);
    else
        return exp(-r * tau) * K * Phi(-d2) - S * Phi(-d1);
}

int main()
{
    const double K = 100.0; // Strike price.
    auto const S = make_fvar<double, 3>(105); // Stock price.
    auto const sigma = make_fvar<double, 0, 3>(5); // Volatility.
    auto const tau = make_fvar<double, 0, 0, 1>(30.0 / 365);  // Time to expiration in years. (30 days).
    auto const r = make_fvar<double, 0, 0, 0, 1>(1.25 / 100); // Interest rate.

    const auto call_price = black_scholes_option_price(call, K, S, sigma, tau, r);
    const auto put_price = black_scholes_option_price(put, K, S, sigma, tau, r);
    // Compare automatically calculated greeks by autodiff with formulas for greeks.
    // https://en.wikipedia.org/wiki/Greeks_(finance)#Formulas_for_European_option_Greeks

    const double d1 = static_cast<double>((log(S / K) + (r + sigma * sigma / 2) * tau) / (sigma * sqrt(tau)));
    const double d2 = static_cast<double>((log(S / K) + (r - sigma * sigma / 2) * tau) / (sigma * sqrt(tau)));
    const double formula_call_delta = +Phi(+d1);
    const double formula_put_delta = -Phi(-d1);
    const double formula_vega = static_cast<double>(S * phi(d1) * sqrt(tau));
    const double formula_call_theta = static_cast<double>(-S * phi(d1) * sigma / (2 * sqrt(tau)) - r * K * exp(-r * tau) * Phi(+d2));
    const double formula_put_theta = static_cast<double>(-S * phi(d1) * sigma / (2 * sqrt(tau)) + r * K * exp(-r * tau) * Phi(-d2));
    const double formula_call_rho = static_cast<double>(+K * tau * exp(-r * tau) * Phi(+d2));
    const double formula_put_rho = static_cast<double>(-K * tau * exp(-r * tau) * Phi(-d2));
    const double formula_gamma = static_cast<double>(phi(d1) / (S * sigma * sqrt(tau)));
    const double formula_vanna = static_cast<double>(-phi(d1) * d2 / sigma);
    const double formula_charm = static_cast<double>(phi(d1) * (d2 * sigma * sqrt(tau) - 2 * r * tau) / (2 * tau * sigma * sqrt(tau)));
    const double formula_vomma = static_cast<double>(S * phi(d1) * sqrt(tau) * d1 * d2 / sigma);
    const double formula_veta = static_cast<double>(-S * phi(d1) * sqrt(tau) * (r * d1 / (sigma * sqrt(tau)) - (1 + d1 * d2) / (2 * tau)));
    const double formula_speed = static_cast<double>(-phi(d1) * (d1 / (sigma * sqrt(tau)) + 1) / (S * S * sigma * sqrt(tau)));
    const double formula_zomma = static_cast<double>(phi(d1) * (d1 * d2 - 1) / (S * sigma * sigma * sqrt(tau)));
    const double formula_color =
        static_cast<double>(-phi(d1) / (2 * S * tau * sigma * sqrt(tau)) * (1 + (2 * r * tau - d2 * sigma * sqrt(tau)) * d1 / (sigma * sqrt(tau))));
    const double formula_ultima = -formula_vega * static_cast<double>((d1 * d2 * (1 - d1 * d2) + d1 * d1 + d2 * d2) / (sigma * sigma));
    std::cout << std::setprecision(std::numeric_limits<double>::digits10)
        << "autodiff black-scholes call price = " << call_price.derivative(0, 0, 0, 0) << '\n'
        << "autodiff black-scholes put  price = " << put_price.derivative(0, 0, 0, 0) << '\n'
        << "\n## First-order Greeks\n"
        << "autodiff call delta = " << call_price.derivative(1, 0, 0, 0) << '\n'
        << " formula call delta = " << formula_call_delta << '\n'
        << "autodiff call vega  = " << call_price.derivative(0, 1, 0, 0) << '\n'
        << " formula call vega  = " << formula_vega << '\n'
        << "autodiff call theta = " << -call_price.derivative(0, 0, 1, 0) << '\n' // minus sign due to tau = T-time
        << " formula call theta = " << formula_call_theta << '\n'
        << "autodiff call rho   = " << call_price.derivative(0, 0, 0, 1) << '\n'
        << " formula call rho   = " << formula_call_rho << '\n'
        << '\n'
        << "autodiff put delta = " << put_price.derivative(1, 0, 0, 0) << '\n'
        << " formula put delta = " << formula_put_delta << '\n'
        << "autodiff put vega  = " << put_price.derivative(0, 1, 0, 0) << '\n'
        << " formula put vega  = " << formula_vega << '\n'
        << "autodiff put theta = " << -put_price.derivative(0, 0, 1, 0) << '\n'
        << " formula put theta = " << formula_put_theta << '\n'
        << "autodiff put rho   = " << put_price.derivative(0, 0, 0, 1) << '\n'
        << " formula put rho   = " << formula_put_rho << '\n'
        << "\n## Second-order Greeks\n"
        << "autodiff call gamma = " << call_price.derivative(2, 0, 0, 0) << '\n'
        << "autodiff put  gamma = " << put_price.derivative(2, 0, 0, 0) << '\n'
        << "      formula gamma = " << formula_gamma << '\n'
        << "autodiff call vanna = " << call_price.derivative(1, 1, 0, 0) << '\n'
        << "autodiff put  vanna = " << put_price.derivative(1, 1, 0, 0) << '\n'
        << "      formula vanna = " << formula_vanna << '\n'
        << "autodiff call charm = " << -call_price.derivative(1, 0, 1, 0) << '\n'
        << "autodiff put  charm = " << -put_price.derivative(1, 0, 1, 0) << '\n'
        << "      formula charm = " << formula_charm << '\n'
        << "autodiff call vomma = " << call_price.derivative(0, 2, 0, 0) << '\n'
        << "autodiff put  vomma = " << put_price.derivative(0, 2, 0, 0) << '\n'
        << "      formula vomma = " << formula_vomma << '\n'
        << "autodiff call veta = " << call_price.derivative(0, 1, 1, 0) << '\n'
        << "autodiff put  veta = " << put_price.derivative(0, 1, 1, 0) << '\n'
        << "      formula veta = " << formula_veta << '\n'
        << "\n## Third-order Greeks\n"
        << "autodiff call speed = " << call_price.derivative(3, 0, 0, 0) << '\n'
        << "autodiff put  speed = " << put_price.derivative(3, 0, 0, 0) << '\n'
        << "      formula speed = " << formula_speed << '\n'
        << "autodiff call zomma = " << call_price.derivative(2, 1, 0, 0) << '\n'
        << "autodiff put  zomma = " << put_price.derivative(2, 1, 0, 0) << '\n'
        << "      formula zomma = " << formula_zomma << '\n'
        << "autodiff call color = " << call_price.derivative(2, 0, 1, 0) << '\n'
        << "autodiff put  color = " << put_price.derivative(2, 0, 1, 0) << '\n'
        << "      formula color = " << formula_color << '\n'
        << "autodiff call ultima = " << call_price.derivative(0, 3, 0, 0) << '\n'
        << "autodiff put  ultima = " << put_price.derivative(0, 3, 0, 0) << '\n'
        << "      formula ultima = " << formula_ultima << '\n'
        ;

    auto v = std::vector<double>{ 1.0, 2.0, 5.0, 10.0 };
    auto v2 = std::vector<double>{ 10.0, 20.0, 50.0, 100.0 };
    auto interp_1 = LinearInterpolator<double, double>(v, v2);
    std::cout << "interp function: " << interp_1.interp(2.5) << std::endl;
    std::cout << "interp function: " << interp_1.interp(12.5) << std::endl;
    std::cout << "interp function: " << interp_1.interp(0.5) << std::endl;
    Holiday us_holiday;
    date day1{ 2021, 5, 31 };
    std::cout << "is " << day1 << " a holiday: " << us_holiday.is_holiday(day1) << std::endl;
    date day2 = bus_day_shift(day1, 30, us_holiday);
    std::cout << "So after adj current date is " << day2  << std::endl;

    std::string aa = "1y";
    std::string bb = "6m";

    std::cout << "test function 1y: " << tenor_to_year(aa) << std::endl;
    std::cout << "test function 6m: " << tenor_to_year(bb) << std::endl;

    FixedCashFlow cf(1000000.00, 0.02, day1, day2, DCC::A30360);
    CashFlowSchedule cfs(1000000.0, 0.02, day1, 2, "10Y");
    cfs.show();
    return 0;
}

// Run program: Ctrl + F5 or Debug > Start Without Debugging menu
// Debug program: F5 or Debug > Start Debugging menu

// Tips for Getting Started:
//   1. Use the Solution Explorer window to add/manage files
//   2. Use the Team Explorer window to connect to source control
//   3. Use the Output window to see build output and other messages
//   4. Use the Error List window to view errors
//   5. Go to Project > Add New Item to create new code files, or Project > Add Existing Item to add existing code files to the project
//   6. In the future, to open this project again, go to File > Open > Project and select the .sln file


#include <dlib/dnn.h>
#include <dlib/matrix.h>

#include <iostream>
#include <random>

using namespace dlib;

using NetworkType = loss_mean_squared<fc<1, input<matrix<double>>>>;
using SampleType = matrix<double, 1, 1>;
using KernelType = linear_kernel<SampleType>;

float func(float x) {
  return 4.f + 0.3f * x;  // line coeficients
}

void TrainAndSaveKRR(const std::vector<matrix<double>>& x,
                     const std::vector<float>& y) {
  krr_trainer<KernelType> trainer;
  trainer.set_kernel(KernelType());
  decision_function<KernelType> df = trainer.train(x, y);
  serialize("dlib-krr.dat") << df;
}

void LoadAndPredictKRR(const std::vector<matrix<double>>& x) {
  decision_function<KernelType> df;

  deserialize("dlib-krr.dat") >> df;

  // Predict

  std::cout << "KRR predictions \n";
  for (auto& v : x) {
    auto p = df(v);
    std::cout << static_cast<double>(p) << std::endl;
  }
}

void TrainAndSaveNetwork(const std::vector<matrix<double>>& x,
                         const std::vector<float>& y) {
  NetworkType network;
  sgd solver;
  dnn_trainer<NetworkType> trainer(network, solver);
  trainer.set_learning_rate(0.0001);
  trainer.set_mini_batch_size(50);
  trainer.set_max_num_epochs(300);
  trainer.be_verbose();
  trainer.train(x, y);
  network.clean();

  serialize("dlib-net.dat") << network;
  net_to_xml(network, "net.xml");
}

void LoadAndPredictNetwork(const std::vector<matrix<double>>& x) {
  NetworkType network;

  deserialize("dlib-net.dat") >> network;

  // Predict
  auto predictions = network(x);

  std::cout << "Net predictions \n";
  for (auto p : predictions) {
    std::cout << static_cast<double>(p) << std::endl;
  }
}

int main() {
  size_t n = 1000;
  std::vector<matrix<double>> x(n);
  std::vector<float> y(n);

  std::random_device rd;
  std::mt19937 re(rd());
  std::uniform_real_distribution<float> dist(-1.5, 1.5);

  // generate data
  for (size_t i = 0; i < n; ++i) {
    x[i].set_size(1, 1);
    x[i](0, 0) = i;

    y[i] = func(i) + dist(re);
  }

  // normalize data
  vector_normalizer<matrix<double>> normalizer_x;
  // let the normalizer learn the mean and standard deviation of the samples
  normalizer_x.train(x);
  // now normalize each sample
  for (size_t i = 0; i < x.size(); ++i) {
    x[i] = normalizer_x(x[i]);
  }

  TrainAndSaveNetwork(x, y);
  TrainAndSaveKRR(x, y);

  // Generate new data
  std::cout << "Target values \n";
  std::vector<matrix<double>> new_x(5);
  for (size_t i = 0; i < 5; ++i) {
    new_x[i].set_size(1, 1);
    new_x[i](0, 0) = i;
    new_x[i] = normalizer_x(new_x[i]);
    std::cout << func(i) << std::endl;
  }

  // Predict
  LoadAndPredictNetwork(new_x);
  LoadAndPredictKRR(new_x);

  return 0;
}


#ifndef UTIL_HPP
#define UTIL_HPP

#define SCREEN_WIDTH 1024
#define SCREEN_HEIGHT 550
#define SCALE 30.f
#define PI 3.1459
#define EXP 2.71828182845904523536
#define DELIM ","

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace ublas = boost::numeric::ublas;
typedef ublas::matrix<float> matrix;
typedef ublas::vector<float> vector;

inline float
random_float (float min, float max)
{
  float random = ((float) rand()) / (float) RAND_MAX;
  float range = max - min;
  return (random*range) + min;
}

inline vector
sigmoid(vector z)
{
  for (unsigned long i=0; i < z.size(); ++i)
  {
    z(i) = (1.f / (1.f + std::pow(EXP, -z(i))));
  }
  return z;
}

inline std::vector<matrix> split_theta(vector theta, int inputs, int hidden)
{
  std::vector<matrix> split_T;
  matrix Theta1(inputs,hidden);
  matrix Theta2(hidden+1,1);

  int w_index = 0;
  for (int i=0; i < hidden; ++i)
  {
    for(int k=0; k < inputs; ++k)
    {
      Theta1(k,i) = theta(w_index);
      ++w_index;
    }
  }

  for(int j=0; j < hidden; ++j)
  {
    Theta2(j,0) = theta(w_index);
    ++w_index;
  }

  split_T.push_back(Theta1);
  split_T.push_back(Theta2);

  return(split_T);
}

#endif


#include "ann.h"
#include <boost/range/algorithm/for_each.hpp>
#include <algorithm>

namespace ann {

float calculateLoss(const math::MatrixF &currentOutput, const math::MatrixF &expectedOutput)
{
    auto m = static_cast<float>(expectedOutput.getRowCount());
    auto hatLog = currentOutput.apply(std::logf);
    auto yml = expectedOutput.hadamardProduct(hatLog);
    auto LSum = math::sum(yml);
    auto L = -(1.0f / m) * LSum;
    return L;
}

float sigmoid(float value)
{
    return 1 / (1 + std::expf(value));
}

float sigmoidDerived(float value)
{
    const auto s = sigmoid(value);
    return s * (1.0f - s);
}

float leakyLRU(float value)
{
    return std::max<float>(0.01f * value, value);
}

float leakyLRUDerived(float value)
{
    return (value < 0.0f) ? 0.01f : 1.0f;
}

void ANN::configureNetwork(size_t inputSize)
{
    Layer::layerFx f = [](const math::MatrixF &m) {
        return m.apply(leakyLRU);
    };

    Layer::layerFx fd = [](const math::MatrixF &m) {
        return m.apply(leakyLRUDerived);
    };

    const size_t layerSize1 = 128;
    const size_t layerSize2 = 64;

    mLayers.emplace_back(inputSize, layerSize1, f, fd);
    mLayers.emplace_back(layerSize1, layerSize2, f, fd);
    mLayers.emplace_back(layerSize2, 10, math::columnWiseSoftMax<float>, fd);
}

float ANN::train(const math::MatrixF &input, const math::MatrixF &expectedOutput)
{
    auto inputData = input;
    boost::for_each(mLayers, [&inputData](Layer &layer){
        inputData = layer.feedForward(inputData);
    });

    auto loss = calculateLoss(mLayers.back().getResult(), expectedOutput);

    auto d = mLayers.back().beginBackPropagation(expectedOutput);
    std::for_each(mLayers.rbegin() + 1, mLayers.rend(), [&d](Layer &layer){
        d = layer.calculateGradients(d);
    });

    boost::for_each(mLayers, [](Layer &layer){
        layer.applyGradients(0.5f);
    });

    return loss;
}

const math::MatrixF& ANN::test(const math::MatrixF &input)
{
    auto inputData = input;
    boost::for_each(mLayers, [&inputData](Layer &layer) {
        inputData = layer.feedForward(inputData);
    });
    return mLayers.back().getResult();
}

IANN::trainData ANN::collectTrainData()
{
    trainData result;

    auto activationResult = mLayers.back().getResult();
    const uint32_t numberOfSamples = static_cast<uint32_t>(activationResult.getColumnCount());
    result.push_back(numberOfSamples);
    for (size_t i = 0; i < numberOfSamples; ++i)
    {
        const uint32_t predictionLabel = static_cast<uint32_t>(math::getMaxElementIndex(activationResult.columnBegin(i), activationResult.columnEnd(i)));
        result.push_back(predictionLabel);
    }
    return result;
}

}


#include "problemes.h"
#include "arithmetique.h"
#include "graphe.h"

#include <fstream>
#include <boost/algorithm/string.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;
typedef std::vector<vecteur> matrice;

typedef std::pair<nombre, nombre> paire;
typedef std::vector<paire> vecteur_paire;

ENREGISTRER_PROBLEME(83, "Path sum: four ways") {
    // In the 5 by 5 matrix below, the minimal path sum from the top left to the bottom right,
    // by moving left, right, up, and down, is indicated in bold red and is equal to 2297.
    //
    // Find the minimal path sum, in matrix.txt (right click and "Save Link/Target As..."), a 31K
    // text file containing a 80 by 80 matrix, from the top left to the bottom right by moving left,
    // right, up, and down.
    matrice m;
    std::ifstream ifs("data/p083_matrix.txt");
    std::string ligne;
    while (ifs >> ligne) {
        std::vector<std::string> v;
        boost::split(v, ligne, boost::is_any_of(","));
        vecteur l;
        for (const auto &s: v) {
            l.push_back(std::stoull(s));
        }
        m.push_back(std::move(l));
    }

    const nombre taille = m.size();
    graphe::Dijkstra::graphe graphe;
    for (nombre i = 0; i < taille; ++i)
        for (nombre j = 0; j < taille; ++j) {
            vecteur_paire v;
            const nombre poids = m[i][j];
            if (i > 0)
                v.emplace_back((i - 1) * taille + j, poids);
            if (j > 0)
                v.emplace_back(i * taille + j - 1, poids);
            if (i < taille - 1)
                v.emplace_back((i + 1) * taille + j, poids);
            if (j < taille - 1)
                v.emplace_back(i * taille + j + 1, poids);

            graphe[i * taille + j] = v;
        }

    graphe::Dijkstra dijkstra(graphe, 0, (taille - 1) * (taille + 1));
    nombre resultat = dijkstra.algorithme() + m[taille - 1][taille - 1];
    return std::to_string(resultat);
}


#include "RefinedDynamicalSystem.h"
#include <boost/timer.hpp>
#include <set>
#include <Eigen/QR>
#include <Eigen/Eigenvalues>

namespace abstract{

/// Constructs an empty buffer
template<class scalar>
CegarSystem<scalar>::CegarSystem(int dimension,int idimension) :
  DynamicalSystem<scalar>(dimension,idimension)
{}

/// Solves the Sylvester equation AX+XB=C for X
template<class scalar>
typename CegarSystem<scalar>::MatrixS CegarSystem<scalar>::solveSylvester(const MatrixS &A,const MatrixS &B,const MatrixS &C,bool BisDiagonal)
{
  MatrixS U,V;
  SolverMatrixType refA,refB;
  interToRef(refA,A);
  interToRef(refB,B);
  Eigen::RealSchur<SolverMatrixType> realSchur(refA.transpose());
  refToInter(U,realSchur.matrixU());//We want transposed so keep it this way
  refA=realSchur.matrixT().transpose();//Lower triangular
  if (BisDiagonal) {
    V=MatrixS::Identity(C.rows(),refB.rows());
  }
  else {
    interToRef(refB,B);
    realSchur.compute(refB);
    refToInter(V,realSchur.matrixU());
    refB=realSchur.matrixT();
  }
  MatrixS result=U*C*V;
  for (int row=0;row<result.rows();row++) {
    for (int col=0;col<result.cols();col++) {
      for (int i=0;i<row;i++) {
        result.coeffRef(row,col)-=refA.coeff(row,i)*result.coeff(i,col);
      }
      for (int i=0;i<col;i++) {
        result.coeffRef(row,col)-=result.coeff(row,i)*refB.coeff(i,col);
      }
      result.coeffRef(row,col)/=refA.coeff(row,row)+refB.coeff(col,col);
    }
  }
  return result;
}

/// Retrieves the characteristic polynomial coefficients of the dynamics
template <class scalar>
typename CegarSystem<scalar>::MatrixS CegarSystem<scalar>::getDynamicPolynomialCoefficients()
{
  MatrixC result=MatrixC::Zero(2,m_dimension+1);
  result.coeffRef(0,0)=ms_one;
  result.coeffRef(0,1)=-m_eigenValues.coeff(0,0);
  for (int i=1;i<m_dimension;i++) {
    result.row(1)=-result.row(0)*m_eigenValues.coeff(i,i);
    result.block(0,1,1,i+1)+=result.block(1,0,1,i+1);
  }
  return result.real();
}

/// Retrieves the reachability matrix [B AB A^2B ...A^{n-1}B]
template <class scalar>
typename CegarSystem<scalar>::MatrixS CegarSystem<scalar>::getReachabilityMatrix()
{
  MatrixS result(m_sensitivity.rows(),m_dimension*m_sensitivity.cols());
  result.block(0,0,m_sensitivity.rows(),m_sensitivity.cols())=m_sensitivity;
  MatrixS multiplier=m_dynamics;
  for (int i=1;i<m_dimension;i++)
  {
    result.block(0,i*m_sensitivity.cols(),m_sensitivity.rows(),m_sensitivity.cols())=multiplier*m_sensitivity;
    multiplier*=m_dynamics;
  }
  if (ms_trace_dynamics) ms_logger.logData(result,"Reachability Matrix");
  return result;
}

/// Retrieves the reachability matrix [1 a1 a2...a_{n-1};0 1 a1 ... a_{n-2}...]
template <class scalar>
typename CegarSystem<scalar>::MatrixS CegarSystem<scalar>::getCanonicalReachabilityMatrix()
{
  //TODO: not sure how this works on MIMO
  MatrixS result=MatrixS::Identity(m_sensitivity.rows(),m_dimension*m_sensitivity.cols());
  MatrixS coefficients=getDynamicPolynomialCoefficients();
  if (ms_trace_dynamics>=eTraceDynamics) {
    MatrixS matrix=coefficients.row(0);
    ms_logger.logData(matrix,"Coefficients");
  }
  for (int i=1;i<m_dimension;i++) result.block(i-1,i,1,m_dimension-i)=coefficients.block(0,1,1,m_dimension-i);
  if (ms_trace_dynamics>=eTraceDynamics) ms_logger.logData(result,"Canonical Reachability Matrix");
  return result;
}

/// Retrieves the transform matrix T : z=T^{-1}x turns A,B,C into controllable canonical form
template <class scalar>
typename CegarSystem<scalar>::MatrixS CegarSystem<scalar>::getReachableCanonicalTransformMatrix()
{
  MatrixS result=getReachabilityMatrix()*getCanonicalReachabilityMatrix();
  return result.inverse();
}

/// Retrieves the observability matrix [C CA CA^2 ...CA^{n-1}]^T
template <class scalar>
typename CegarSystem<scalar>::MatrixS CegarSystem<scalar>::getObservabilityMatrix()
{
  MatrixS result(m_dimension*m_outputSensitivity.rows(),m_outputSensitivity.cols());
  result.block(0,0,m_outputSensitivity.rows(),m_outputSensitivity.cols())=m_outputSensitivity;
  MatrixS multiplier=m_dynamics;
  for (int i=1;i<m_dimension;i++)
  {
    result.block(i*m_outputSensitivity.rows(),0,m_outputSensitivity.rows(),m_outputSensitivity.cols())=m_outputSensitivity*multiplier;
    multiplier*=m_dynamics;
  }
  return result;
}

/// Retrieves the observability matrix [1 0 ...;a1 1 0 ...;...;a_{n-1}...a1 1]^-1
template <class scalar>
typename CegarSystem<scalar>::MatrixS CegarSystem<scalar>::getInverseCanonicalObservabilityMatrix()
{
  //TODO: not sure how this works on MIMO
  MatrixS result=MatrixS::Identity(m_dimension*m_outputSensitivity.rows(),m_outputSensitivity.cols());
  MatrixS coefficients=getDynamicPolynomialCoefficients();
  for (int i=0;i<m_dimension;i++) {
    for (int j=0;j<i;j++) {
      result.coeffRef(i,j)=coefficients.coeff(0,m_dimension-i-j-1);
    }
  }
  return result;
}

/// Retrieves the transform matrix T : z=T^{-1}x turns A,B,C into observable canonical form
template <class scalar>
typename CegarSystem<scalar>::MatrixS CegarSystem<scalar>::getObservableCanonicalTransformMatrix()
{
  return getObservabilityMatrix().inverse()*getInverseCanonicalObservabilityMatrix();
}

/// Retrieves the reference gain
template <class scalar>
typename CegarSystem<scalar>::MatrixS CegarSystem<scalar>::getReferenceGain()
{
  return m_outputSensitivity*((m_dynamics-(m_sensitivity*m_feedback)).inverse()*m_sensitivity);
}

/// Retrieves the reach tube at the given iteration
template <class scalar>
/// Retrieves refined dynamics given a safety specification
AbstractPolyhedra<scalar>& CegarSystem<scalar>::getRefinedDynamics(int refinements,powerS iteration,int directions,inputType_t inputType)
{
  AbstractPolyhedra<scalar>&reachTube=getAbstractReachTube(iteration,2,directions,inputType);
  if (ms_trace_dynamics>=eTraceDynamics) {
    reachTube.logTableau("PreCegar:",true);
    reachTube.logVertices(true);
  }
  //if (m_safeReachTube.getPolyhedra().contains(reachTube)) return getAbstractDynamics(inputType);
  if (!m_safeReachTube.isEmpty()) {
    AbstractPolyhedra<scalar> bounds=synthesiseDynamicBounds(m_inputType,m_safeReachTube.getPolyhedra(eEigenSpace));
    if (ms_trace_dynamics>=eTraceDynamics) {
      bounds.logTableau("Projected Bounds: ",true);
    }
    for(int i=0;(i<refinements) && refineAbstractDynamics(bounds);i++);
    if (ms_trace_dynamics>=eTraceDynamics) {
      AbstractPolyhedra<scalar>& result=getRefinedAbstractReachTube(eNormalSpace);
      result.logTableau("PosCegar:",true);
      result.logVertices(true);
    }
  }
  return getAbstractDynamics(inputType);
}

/// Synthesises a bound on the dynamics given a known guard and eigenvectors.
template<class scalar>
AbstractPolyhedra<scalar> CegarSystem<scalar>::synthesiseDynamicBounds(inputType_t inputType,AbstractPolyhedra<scalar> &end)
{
  m_inputType=inputType;
  MatrixS vectors;
  int numVertices;
  getAbstractVertices(end.getDirections(),vectors,numVertices);
  vectors.transposeInPlace();
  MatrixS supports(vectors.rows(),1);
  MatrixS endSupports=end.getSupports();
  int perTemplate=supports.rows()/endSupports.rows();
  for (int i=0;i<endSupports.rows();i++)
  {
    for (int j=0;j<perTemplate;j++) {
      supports.coeffRef(i*perTemplate+j,0)=endSupports.coeff(i,0);
    }
  }
  if (m_inputType>eNoInputs) {
    MatrixS inSupports=m_accelVertices*end.getDirections();
    demergeAccelInSupports(supports,inSupports,endSupports.rows());
  }
  AbstractPolyhedra<scalar> bounds;
  bounds.load(vectors,supports);
  if (ms_trace_dynamics>=eTraceAll) {
    bounds.logTableau("Full Bounds",true);
  }
  bounds.removeRedundancies();
  return bounds;
}

/// Creates a model for the quantization noise as an input specification
template<class scalar>
AbstractPolyhedra<scalar> CegarSystem<scalar>::generateNoiseInput()
{
  int odimension=(m_odimension>0) ? m_odimension : m_dimension;
  int ndimension=1;
  AbstractPolyhedra<scalar> result(ndimension);
  int fbits=m_paramValues.coeff(eNumBits,1)-m_paramValues.coeff(eNumBits,2);
  if (fbits<=0) fbits=m_paramValues.coeff(eNumBits,0);
  if (fbits<=0) fbits=func::getDefaultPrec();
  scalar lsb=func::pow(this->ms_two,-fbits);
  MatrixS faces(2*ndimension,ndimension);
  MatrixS supports(2*ndimension,1);
  faces.block(0,0,ndimension,ndimension)=MatrixS::Identity(ndimension,ndimension);
  faces.block(ndimension,0,ndimension,ndimension)=-MatrixS::Identity(ndimension,ndimension);
  scalar fb_noise=m_feedback.cwiseAbs().sum()/*lpNorm<1>()*/+this->ms_one;//|Noise|<(|K|_1+1)lsb assuming q1,q2=1lsb,(q3:=q1->K) = (|K|_1q1+q2)
  supports.coeffRef(0,0)=lsb*(fb_noise);
  supports.coeffRef(1,0)=supports.coeff(0,0);
  result.load(faces,supports);
  return result;
}

/// Creates a model for the input of the closed loop
template<class scalar>
AbstractPolyhedra<scalar> CegarSystem<scalar>::generateFeedbackInput(int fdimension,bool makeNoise,MatrixS &sensitivity)
{
  AbstractPolyhedra<scalar> inputs(0);
  if (m_reference.getDimension()>0) {
    inputs.copy(m_reference);
    fdimension=m_reference.getDimension();
  }
  if (m_idimension>fdimension) {
    MatrixS inputFaces=m_inputs.getFaceDirections().rightCols(m_idimension-fdimension);
    MatrixS inSupports=m_inputs.getSupports();
    inputs.concatenate(inputFaces,inSupports);
  }
  if (makeNoise) {
    //m_closedLoop.setInputType(eVariableInputs);
    AbstractPolyhedra<scalar> noiseInputs=generateNoiseInput();
    inputs.concatenate(noiseInputs);
    sensitivity.conservativeResize(m_dimension,inputs.getDimension());
    sensitivity.block(0,inputs.getDimension()-noiseInputs.getDimension(),m_dimension,noiseInputs.getDimension())=MatrixS::Ones(m_dimension,noiseInputs.getDimension());
  }
  return inputs;
}

/// Retrieves a list of iterations whose reach set fails the specification
template<class scalar>
bool CegarSystem<scalar>::findCounterExampleIterations(powerList &iterations,AbstractPolyhedra<scalar> &bounds)
{
  AbstractPolyhedra<scalar>& dynamics=getAbstractDynamics(m_inputType);
  MatrixS &vertices=dynamics.getVertices(true);
  bool found=false;
  for (int i=0;i<vertices.rows();i++) {
    MatrixS point=vertices.row(i);
    if (!bounds.isInside(point)) {
      findIterations(point,iterations);
      found=true;
    }
  }
  return found;
}

/// Refines the abstraction in order to meet the safety specification
template<class scalar>
bool CegarSystem<scalar>::refineAbstractDynamics(AbstractPolyhedra<scalar> &bounds,powerList &iterations)
{
  AbstractPolyhedra<scalar>& dynamics=getAbstractDynamics(m_inputType);
  if (findCounterExampleIterations(iterations,bounds)) {
    typename powerList::iterator it;
    for (it=iterations.begin();it!=iterations.end();it++) {
      addSupportsAtIteration(dynamics,it->first,m_maxIterations);
    }
    return true;
  }
  return false;
}

/// Corrects the support set by the input offset
template <class scalar>
void CegarSystem<scalar>::demergeAccelInSupports(MatrixS &supports,MatrixS &inSupports,int numTemplates)
{
  if (!m_hasOnes || (m_inputType==eVariableInputs))  {
    for (int row=0;row<numTemplates;row++) {
      int pos=row*m_numVertices;
      supports.coeffRef(pos,0)-=inSupports.coeff(0,row);
      for (int point=1;point<m_numVertices;point++) {
        supports.coeffRef(pos+point,0)-=inSupports.coeff(point%inSupports.rows(),row);
      }
    }
  }
}

/// Retrieves the support set for the inputs
template <class scalar>
typename JordanMatrix<scalar>::MatrixS& CegarSystem<scalar>::getRefinedAccelInSupports()
{
  if (m_hasOnes && (m_inputType==eVariableInputs)) {
    AbstractPolyhedra<scalar>& inputDynamics=getAbstractDynamics(eParametricInputs);
    MatrixS supports;
    inputDynamics.maximiseAll(m_abstractInputVertices,supports);
    m_accelInSupports=supports.transpose();
    if (ms_trace_dynamics>=eTraceAbstraction) ms_logger.logData(m_accelInSupports,"Input Supports",true);
  }
  return m_accelInSupports;
}

/// Retrieves the reach tube at the given iteration
template <class scalar>
AbstractPolyhedra<scalar>& CegarSystem<scalar>::getRefinedAbstractReachTube(space_t space,bool guarded)
{
  boost::timer timer;
  AbstractPolyhedra<scalar>& init=m_initialState.getPolyhedra(eEigenSpace);
  AbstractPolyhedra<scalar>& dynamics=getAbstractDynamics(m_inputType);

  MatrixS& templates=getTemplates(eEigenSpace);
  if (ms_trace_time) ms_logger.logData(timer.elapsed()*1000,"Abstract Vertices: ",true);
  MatrixS supports;
  if (!dynamics.maximiseAll(m_abstractVertices,supports)) processError(dynamics.getName());

  if (m_inputType>eNoInputs) getRefinedAccelInSupports();
  if (ms_trace_dynamics>=eTraceAll) {
    traceSupports(templates,supports,dynamics,m_abstractVertices);
  }
  if (m_inputType>eNoInputs) {
    mergeAccelInSupports(supports,templates.cols());
    if (ms_trace_dynamics>=eTraceAll) {
      ms_logger.logData(m_abstractVertices,supports,"Combined",true);
    }
  }
  mergeAbstractSupports(templates,supports);
  MatrixS faces=templates.transpose();
  m_pAbstractReachTube->mergeLoad(init,faces,supports,eEigenSpace);
  AbstractPolyhedra<scalar>& result=m_pAbstractReachTube->getPolyhedra(space);
  if (guarded) getGuardedReachTube(result,space);
  if (ms_trace_dynamics>=eTraceAbstraction) result.logTableau();
  m_reachTime=timer.elapsed()*1000;
  result.setCalculationTime(m_reachTime);
  if (ms_trace_time) ms_logger.logData(m_reachTime,"Abstract Reach Time: ",true);
  return result;
}

#ifdef USE_LDOUBLE
  #ifdef USE_SINGLES
    template class CegarSystem<long double>;
  #endif
  #ifdef USE_INTERVALS
    template class CegarSystem<ldinterval>;
  #endif
#endif
#ifdef USE_MPREAL
  #ifdef USE_SINGLES
    template class CegarSystem<mpfr::mpreal>;
  #endif
  #ifdef USE_INTERVALS
    template class CegarSystem<mpinterval>;
  #endif
#endif

}


#include "config.h"
#include "Scene_points_with_normal_item.h"
#include "Scene_polygon_soup_item.h"
#include "Scene_polyhedron_item.h"
#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>
#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>

#include <CGAL/Random.h>

#include <CGAL/Shape_detection_3.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Alpha_shape_2.h>

#include <QObject>
#include <QAction>
#include <QMainWindow>
#include <QApplication>
#include <QtPlugin>
#include <QMessageBox>

#include <boost/foreach.hpp>

#include "ui_Point_set_shape_detection_plugin.h"

typedef CGAL::Exact_predicates_inexact_constructions_kernel Epic_kernel;
typedef Epic_kernel::Point_3 Point;
//typedef CGAL::Point_with_normal_3<Epic_kernel> Point_with_normal;
//typedef std::vector<Point_with_normal> Point_list;
//typedef CGAL::Identity_property_map<Point_with_normal> PointPMap;
//typedef CGAL::Normal_of_point_with_normal_pmap<Epic_kernel> NormalPMap;
using namespace CGAL::Three;
class Polyhedron_demo_point_set_shape_detection_plugin :
  public QObject,
  public Polyhedron_demo_plugin_helper
{
  Q_OBJECT
    Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
    Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")
    QAction* actionDetect;

public:
  void init(QMainWindow* mainWindow, CGAL::Three::Scene_interface* scene_interface) {
    actionDetect = new QAction(tr("Point Set Shape Detection"), mainWindow);
    actionDetect->setObjectName("actionDetect");

    Polyhedron_demo_plugin_helper::init(mainWindow, scene_interface);
  }

  bool applicable(QAction*) const {
    Scene_points_with_normal_item* item =
      qobject_cast<Scene_points_with_normal_item*>(scene->item(scene->mainSelectionIndex()));
    if (item && item->has_normals())
      return true;
    return false;
  }

  QList<QAction*> actions() const {
    return QList<QAction*>() << actionDetect;
  }

  public Q_SLOTS:
    void on_actionDetect_triggered();

private:

  typedef Kernel::Plane_3 Plane_3;

  void build_alpha_shape (Point_set& points, const Plane_3& plane,
                          Scene_polyhedron_item* item, double epsilon);

}; // end Polyhedron_demo_point_set_shape_detection_plugin

class Point_set_demo_point_set_shape_detection_dialog : public QDialog, private Ui::PointSetShapeDetectionDialog
{
  Q_OBJECT
public:
  Point_set_demo_point_set_shape_detection_dialog(QWidget * /*parent*/ = 0)
  {
    setupUi(this);
  }

  //QString shapeDetectionMethod() const { return m_shapeDetectionMethod->currentText(); }
  double cluster_epsilon() const { return m_cluster_epsilon_field->value(); }
  double epsilon() const { return m_epsilon_field->value(); }
  unsigned int min_points() const { return m_min_pts_field->value(); }
  double normal_tolerance() const { return m_normal_tolerance_field->value(); }
  double search_probability() const { return m_probability_field->value(); }
  double gridCellSize() const { return 1.0; }
  bool detect_plane() const { return planeCB->isChecked(); }
  bool detect_sphere() const { return sphereCB->isChecked(); }
  bool detect_cylinder() const { return cylinderCB->isChecked(); }
  bool detect_torus() const { return torusCB->isChecked(); }
  bool detect_cone() const { return coneCB->isChecked(); }
  bool generate_alpha() const { return m_generate_alpha->isChecked(); }
  bool generate_subset() const { return !(m_do_not_generate_subset->isChecked()); }
};

void Polyhedron_demo_point_set_shape_detection_plugin::on_actionDetect_triggered() {
  CGAL::Random rand(time(0));
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();

  Scene_points_with_normal_item* item =
    qobject_cast<Scene_points_with_normal_item*>(scene->item(index));

  Scene_points_with_normal_item::Bbox bb = item->bbox();

  double diam = bb.diagonal_length();

  if(item)
  {
    // Gets point set
    Point_set* points = item->point_set();

    if(points == NULL)
      return;

    //Epic_kernel::FT diag = sqrt(((points->bounding_box().max)() - (points->bounding_box().min)()).squared_length());

    // Gets options
    Point_set_demo_point_set_shape_detection_dialog dialog;
    if(!dialog.exec())
      return;

    QApplication::setOverrideCursor(Qt::WaitCursor);

    typedef CGAL::Identity_property_map<Point_set::Point_with_normal> PointPMap;
    typedef CGAL::Normal_of_point_with_normal_pmap<Point_set::Geom_traits> NormalPMap;

    typedef CGAL::Shape_detection_3::Efficient_RANSAC_traits<Epic_kernel, Point_set, PointPMap, NormalPMap> Traits;
    typedef CGAL::Shape_detection_3::Efficient_RANSAC<Traits> Shape_detection;

    Shape_detection shape_detection;
    shape_detection.set_input(*points);

    // Shapes to be searched for are registered by using the template Shape_factory
    if(dialog.detect_plane()){
        shape_detection.add_shape_factory<CGAL::Shape_detection_3::Plane<Traits> >();
      }
    if(dialog.detect_cylinder()){
      shape_detection.add_shape_factory<CGAL::Shape_detection_3::Cylinder<Traits> >();
    }
    if(dialog.detect_torus()){
       shape_detection.add_shape_factory< CGAL::Shape_detection_3::Torus<Traits> >();
    }
    if(dialog.detect_cone()){
      shape_detection.add_shape_factory< CGAL::Shape_detection_3::Cone<Traits> >();
    }
    if(dialog.detect_sphere()){
      shape_detection.add_shape_factory< CGAL::Shape_detection_3::Sphere<Traits> >();
    }

    // Parameterization of the shape detection using the Parameters structure.
    Shape_detection::Parameters op;
    op.probability = dialog.search_probability();       // probability to miss the largest primitive on each iteration.
    op.min_points = dialog.min_points();          // Only extract shapes with a minimum number of points.
    op.epsilon = dialog.epsilon();          // maximum euclidean distance between point and shape.
    op.cluster_epsilon = dialog.cluster_epsilon();    // maximum euclidean distance between points to be clustered.
    op.normal_threshold = dialog.normal_tolerance();   // normal_threshold < dot(surface_normal, point_normal); maximum normal deviation.

    // The actual shape detection.
    shape_detection.detect(op);

    std::cout << shape_detection.shapes().size() << " shapes found" << std::endl;
    //print_message(QString("%1 shapes found.").arg(shape_detection.number_of_shapes()));
    int index = 0;
    BOOST_FOREACH(boost::shared_ptr<Shape_detection::Shape> shape, shape_detection.shapes())
    {
      CGAL::Shape_detection_3::Cylinder<Traits> *cyl;
      cyl = dynamic_cast<CGAL::Shape_detection_3::Cylinder<Traits> *>(shape.get());
      if (cyl != NULL){
        if(cyl->radius() > diam){
          continue;
        }
      }

      Scene_points_with_normal_item *point_item = new Scene_points_with_normal_item;
      BOOST_FOREACH(std::size_t i, shape->indices_of_assigned_points())
        point_item->point_set()->push_back((*points)[i]);

      unsigned char r, g, b;
      r = static_cast<unsigned char>(64 + rand.get_int(0, 192));
      g = static_cast<unsigned char>(64 + rand.get_int(0, 192));
      b = static_cast<unsigned char>(64 + rand.get_int(0, 192));
      point_item->setRbgColor(r, g, b);

      // Providing a useful name consisting of the order of detection, name of type and number of inliers
      std::stringstream ss;
      if (dynamic_cast<CGAL::Shape_detection_3::Cylinder<Traits> *>(shape.get())){
        CGAL::Shape_detection_3::Cylinder<Traits> * cyl
          = dynamic_cast<CGAL::Shape_detection_3::Cylinder<Traits> *>(shape.get());
        ss << item->name().toStdString() << "_cylinder_" << cyl->radius() << "_";
      }
      else if (dynamic_cast<CGAL::Shape_detection_3::Plane<Traits> *>(shape.get()))
        {
          ss << item->name().toStdString() << "_plane_";

          if (dialog.generate_alpha ())
            {
              // If plane, build alpha shape
              Scene_polyhedron_item* poly_item = new Scene_polyhedron_item;

              Plane_3 plane = (Plane_3)(*(dynamic_cast<CGAL::Shape_detection_3::Plane<Traits>*>(shape.get ())));
              build_alpha_shape (*(point_item->point_set()), plane,
                                 poly_item, dialog.cluster_epsilon());

              poly_item->setRbgColor(r-32, g-32, b-32);
              poly_item->setName(QString("%1%2_alpha_shape").arg(QString::fromStdString(ss.str()))
                                 .arg (QString::number (shape->indices_of_assigned_points().size())));
              poly_item->setRenderingMode (Flat);
              scene->addItem(poly_item);
            }
        }
      else if (dynamic_cast<CGAL::Shape_detection_3::Cone<Traits> *>(shape.get()))
        ss << item->name().toStdString() << "_cone_";
      else if (dynamic_cast<CGAL::Shape_detection_3::Torus<Traits> *>(shape.get()))
        ss << item->name().toStdString() << "_torus_";
      else if (dynamic_cast<CGAL::Shape_detection_3::Sphere<Traits> *>(shape.get()))
        ss << item->name().toStdString() << "_sphere_";

      ss << shape->indices_of_assigned_points().size();

      //names[i] = ss.str(
      point_item->setName(QString::fromStdString(ss.str()));
      point_item->set_has_normals(true);
      point_item->setRenderingMode(item->renderingMode());
      if (dialog.generate_subset())
        scene->addItem(point_item);
      else
        delete point_item;

      ++index;
    }

    // Updates scene
    scene->itemChanged(index);

    QApplication::restoreOverrideCursor();

    //     Warn user, maybe choice of parameters is unsuitable
    //         if (nb_points_to_remove > 0)
    //         {
    //           QMessageBox::information(NULL,
    //                                    tr("Points selected for removal"),
    //                                    tr("%1 point(s) are selected for removal.\nYou may delete or reset the selection using the item context menu.")
    //                                    .arg(nb_points_to_remove));
    //         }
    item->setVisible(false);
  }
}

void Polyhedron_demo_point_set_shape_detection_plugin::build_alpha_shape
(Point_set& points, const Plane_3& plane, Scene_polyhedron_item* item, double epsilon)
{
  typedef Kernel::Point_2  Point_2;
  typedef CGAL::Alpha_shape_vertex_base_2<Kernel> Vb;
  typedef CGAL::Alpha_shape_face_base_2<Kernel>  Fb;
  typedef CGAL::Triangulation_data_structure_2<Vb,Fb> Tds;
  typedef CGAL::Delaunay_triangulation_2<Kernel,Tds> Triangulation_2;
  typedef CGAL::Alpha_shape_2<Triangulation_2>  Alpha_shape_2;

  std::vector<Point_2> projections;
  projections.reserve (points.size ());

  for (std::size_t i = 0; i < points.size (); ++ i)
    projections.push_back (plane.to_2d (points[i]));

  Alpha_shape_2 ashape (projections.begin (), projections.end (), epsilon);

  std::map<Alpha_shape_2::Vertex_handle, std::size_t> map_v2i;

  Scene_polygon_soup_item *soup_item = new Scene_polygon_soup_item;

  soup_item->init_polygon_soup(points.size(), ashape.number_of_faces ());
  std::size_t current_index = 0;

  for (Alpha_shape_2::Finite_faces_iterator it = ashape.finite_faces_begin ();
       it != ashape.finite_faces_end (); ++ it)
    {
      if (ashape.classify (it) != Alpha_shape_2::INTERIOR)
        continue;

      for (int i = 0; i < 3; ++ i)
        {
          if (map_v2i.find (it->vertex (i)) == map_v2i.end ())
            {
              map_v2i.insert (std::make_pair (it->vertex (i), current_index ++));
              Point p = plane.to_3d (it->vertex (i)->point ());
              soup_item->new_vertex (p.x (), p.y (), p.z ());
            }
        }
      soup_item->new_triangle (map_v2i[it->vertex (0)],
                               map_v2i[it->vertex (1)],
                               map_v2i[it->vertex (2)]);
    }

  soup_item->orient();
  soup_item->exportAsPolyhedron (item->polyhedron());

  delete soup_item;
}

#include <QtPlugin>

#include "Point_set_shape_detection_plugin.moc"


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//

#include <boost/gil/image.hpp>
#include <boost/gil/typedefs.hpp>
#include <boost/gil/extension/io/jpeg.hpp>

// Creates a synthetic image defining the Mandelbrot set.
// The example relies on a virtual_2d_locator to iterate over the pixels in the destination view.
// The pixels (of type rgb8_pixel_t) are generated programmatically, and the code shows how to access
// the colour channels and set them to arbitrary values.

using namespace boost::gil;

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

    value_type                    _in_color,_out_color;
    point_t                       _img_size;
    static const int MAX_ITER=100;        // max number of iterations

    mandelbrot_fn() {}
    mandelbrot_fn(const point_t& sz, const value_type& in_color, const value_type& out_color) : _in_color(in_color), _out_color(out_color), _img_size(sz) {}

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

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

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

int main()
{
    using deref_t = mandelbrot_fn<rgb8_pixel_t>;
    using point_t = deref_t::point_t;
    using locator_t = virtual_2d_locator<deref_t,false>;
    using my_virt_view_t = image_view<locator_t>;

    boost::function_requires<PixelLocatorConcept<locator_t>>();
    gil_function_requires<StepIteratorConcept<locator_t::x_iterator>>();

    point_t dims(200,200);
    my_virt_view_t mandel(dims, locator_t(point_t(0,0), point_t(1,1), deref_t(dims, rgb8_pixel_t(255,0,255), rgb8_pixel_t(0,255,0))));
    write_view("out-mandelbrot.jpg",mandel, jpeg_tag{});

    return 0;
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include <eve/function/ellint_rg.hpp>
#include <boost/math/special_functions/ellint_rg.hpp>
#include <eve/wide.hpp>

TTS_CASE_TPL("Check eve::ellint_rg behavior", EVE_TYPE)
{
  using elt_t = eve::element_type_t<T>;
  TTS_ULP_EQUAL(eve::ellint_rg(T(0.2), T(0.4), T(0)),  T(boost::math::ellint_rg(elt_t(0.2), elt_t(0.4), elt_t(0))),   1.0);
  TTS_ULP_EQUAL(eve::ellint_rg(T(1.5), T(1), T(7)),T(boost::math::ellint_rg(elt_t(1.5), elt_t(1), elt_t(7))), 1.0);
  TTS_ULP_EQUAL(eve::ellint_rg(T(2), T(0), T(7)),  T(boost::math::ellint_rg(elt_t(2), elt_t(0), elt_t(7))),   1.5);
  TTS_ULP_EQUAL(eve::ellint_rg(T(0), T(5), T(7)),  T(boost::math::ellint_rg(elt_t(0), elt_t(5), elt_t(7))),   1.0);
  TTS_ULP_EQUAL(eve::ellint_rg(T(2), T(5), T(7)),  T(boost::math::ellint_rg(elt_t(2), elt_t(5), elt_t(7))),   1.0);
                                                                        }


/* ------------------------------------------------------------------*\
                        ╦═╗╔═╗╔╦╗╔═╗╔═╗╔═╗
                        ╠╦╝║ ║║║║╚═╗║ ║║
                        ╩╚═╚═╝╩ ╩╚═╝╚═╝╚═╝
 Reduced Order Modelling, Simulation, Optimization of Coupled Systems
                            2017-2021

 Authors :
 Ashwin Nayak, Andres Prieto, Daniel Fernandez Comesana

 Disclaimer :
 In downloading this SOFTWARE you are deemed to have read and agreed to
 the following terms: This SOFTWARE has been designed with an exclusive
 focus on civil applications. It is not to be used for any illegal,
 deceptive, misleading or unethical purpose or in any military
 applications. This includes ANY APPLICATION WHERE THE USE OF THE
 SOFTWARE MAY RESULT IN DEATH, PERSONAL INJURY OR SEVERE PHYSICAL
 OR ENVIRONMENTAL DAMAGE. Any redistribution of the software must
 retain this disclaimer. BY INSTALLING, COPYING, OR OTHERWISE USING
 THE SOFTWARE, YOU AGREE TO THE TERMS ABOVE. IF YOU DO NOT AGREE TO
 THESE TERMS, DO NOT INSTALL OR USE THE SOFTWARE

 Acknowledgements:
 The ROMSOC project has received funding from the European Union’s
 Horizon 2020 research and innovation programme under the Marie
 Skłodowska-Curie Grant Agreement No. 765374.
\*-------------------------------------------------------------------*/

// Include necessary PYBIND files
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
namespace py = pybind11;

// Include necessary DOLFIN classes
#include <dolfin/function/Expression.h>
#include <dolfin/function/Constant.h>

// Include necessary BOOST classes
#include <boost/math/special_functions/legendre.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/hankel.hpp>

#include <complex>
#include <iostream>
class ScatteringExact_Re : public dolfin::Expression {
    public:
    double p0, k, a;
    ScatteringExact_Re() : dolfin::Expression(), p0(1.0), k(400.),  a(0.05){}

    void eval(Eigen::Ref<Eigen::VectorXd> values, Eigen::Ref<const Eigen::VectorXd> x) const {

        using namespace boost::math;

        std::complex<double> one_i(0.0,1.0);

        double r = x.norm();

        // Compute Scattered field
        double tol = 1e-10;
        int l=0;
        int N=50;
        std::complex<double> p_sc = 0., p_sc_add = 0.;

        auto J = [&] () -> std::complex<double> {
                        return ((double)l* sph_bessel(l,k*a) / (k*a) ) - sph_bessel(l+1,k*a);
                    };

        auto H = [&] () -> std::complex<double> {
                        return ((double)l* sph_hankel_1(l,k*a) / (k*a) ) - sph_hankel_1(l+1,k*a);
                    };

        p_sc_add = - p0 * J()* sph_hankel_1(l,k*r) * legendre_p(l,x[0]/r)/ H();
        p_sc += p_sc_add;

        while ((l<N) && (abs(p_sc_add/p_sc) > tol)) {
            ++l;
            std::complex<double>  A_l = (2*l+1.0)*std::pow(one_i,l)*J()/H();
            p_sc_add = -p0 * A_l * sph_hankel_1(l,k*r) * legendre_p(l,x[0]/r);
            p_sc += p_sc_add;
        }

        values[0] = cos(k*x[0]) + p_sc.real();
    }

};

PYBIND11_MODULE(SIGNATURE, m) {
    py::class_<ScatteringExact_Re, std::shared_ptr<ScatteringExact_Re>, dolfin::Expression>
    (m, "ScatteringExact_Re")
    .def(py::init<>())
    .def_readwrite("p0", &ScatteringExact_Re::p0)
    .def_readwrite("k", &ScatteringExact_Re::k)
    .def_readwrite("a", &ScatteringExact_Re::a)
    ;
}


// ism.cpp: test to validate that a matrix is an M matrix
//
// Copyright (C) 2017-2020 Stillwater Supercomputing, Inc.
//
// This file is part of the HPR-BLAS project, which is released under an MIT Open Source license.

#include <boost/numeric/mtl/mtl.hpp>
#include <hprblas>
#include <matpak/isa/ism.hpp>

// Selects posits or floats
#define USE_POSIT 0

template<typename Matrix>
bool isMatrixAnMMatrix(const Matrix& A) {
	return false;
}

int main(int argc, char** argv)
try {
	using namespace std;
	using namespace mtl;
	using namespace sw::unum;
	using namespace sw::hprblas;

#if USE_POSIT
	using Ty     = sw::unum::posit<8, 0>;
	using Matrix = mtl::dense2D< Ty >;
	using Vector = mtl::dense_vector< Ty >;
#else
	using Ty     = float;
	using Matrix = mtl::dense2D<float>;
	using Vector = mtl::dense_vector<float>;
#endif

	constexpr int n = 3; // Number of states

	mtl::dense2D<Ty> A(n, n); // System dynamics matrix

	A = 1;  // create identity matrix

	if (sw::hprblas::ism(A)) {
		cout << "A is an M-matrix\n" << A << '\n';
	} else {
		cout << "A is not an M-Matrix\n" << A << '\n';
	}

	return EXIT_SUCCESS;
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)

//! \file
//!\brief Tests for the hyperbolic arcsine function of (fixed_point) for a small digit range.

#include <cmath>

#define BOOST_TEST_MODULE test_negatable_func_hyperbolic_arcsine_small
#define BOOST_LIB_DIAGNOSTIC

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE(test_negatable_func_hyperbolic_arcsine_small)
{
  typedef boost::fixed_point::negatable<7, -24> fixed_point_type;
  typedef fixed_point_type::float_type          float_point_type;

  const fixed_point_type tol = ldexp(fixed_point_type(1), fixed_point_type::resolution + 4);

  using std::asinh;

  // Check positive arguments.
  for(int i = 0; i < 16; ++i)
  {
    const fixed_point_type x = asinh(fixed_point_type(i) / fixed_point_type(3.1415926535897932385L));
    const float_point_type y = asinh(float_point_type(i) / float_point_type(3.1415926535897932385L));

    BOOST_CHECK_CLOSE_FRACTION(x, fixed_point_type(y), tol);
  }

  // Check negative arguments.
  for(int i = 0; i < 16; ++i)
  {
    const fixed_point_type x = asinh(fixed_point_type(-i) / fixed_point_type(3.1415926535897932385L));
    const float_point_type y = asinh(float_point_type(-i) / float_point_type(3.1415926535897932385L));

    BOOST_CHECK_CLOSE_FRACTION(x, fixed_point_type(y), tol);
  }

  fixed_point_type x;
  float_point_type y;

  // Check the valid zero argument.
  x = asinh(0);
  y = float_point_type(0);

  BOOST_CHECK_CLOSE_FRACTION(x, fixed_point_type(y), tol);

  // Check a positive argument clost to 0.
  x = asinh(1 / (fixed_point_type(97) / 10));
  y = asinh(1 / (float_point_type(97) / 10));

  BOOST_CHECK_CLOSE_FRACTION(x, fixed_point_type(y), tol);

  // Check a negative argument clost to 0.
  x = asinh(-1 / (fixed_point_type(97) / 10));
  y = asinh(-1 / (float_point_type(97) / 10));

  BOOST_CHECK_CLOSE_FRACTION(x, fixed_point_type(y), tol);
}


#include <stan/math/prim/arr.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <vector>

TEST(MathFunctions, sumZeroSize) {
  std::vector<double> x;
  EXPECT_FLOAT_EQ(0.0, stan::math::sum(x));
}

TEST(MathFunctions, sum) {
  std::vector<double> x(3);

  x[0] = 1.0;
  x[1] = 2.0;
  x[2] = 3.0;

  EXPECT_FLOAT_EQ(6.0, stan::math::sum(x));
}

TEST(MathFunctions, sub_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();
  std::vector<double> x(3);

  x[0] = 1.0;
  x[1] = 2.0;
  x[2] = nan;

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::sum(x));
}
TEST(MathMatrix, sum_vector_int) {
  std::vector<int> x(3);
  EXPECT_EQ(0, stan::math::sum(x));
  x[0] = 1;
  x[1] = 2;
  x[2] = 3;
  EXPECT_EQ(6, stan::math::sum(x));
}


/*
 * This example is based on the MTL tutorial
 * http://www.simunova.com/docs/mtl4/html/using__solvers.html
 */
#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>
#include "idr_s.hpp"

using namespace mtl;
using namespace itl;

int main(int, char**)
{
    const int size = 40, N = size * size;
    typedef compressed2D<double>  matrix_type;
    size_t s = 2;

    // Set up a matrix 1,600 x 1,600 with 5-point-stencil
    matrix_type                   A(N, N);
    mat::laplacian_setup(A, size, size);

    // Create an ILU(0) preconditioner
    pc::ilu_0<matrix_type>        P(A);

    // Set b such that x == 1 is solution; start with x == 0
    dense_vector<double>          x(N, 1.0), b(N);
    b= A * x; x= 0;

    // Termination criterion: r < 1e-6 * b or N iterations
    noisy_iteration<double>       iter(b, 500, 1.e-6);

    // Solve Ax == b with left preconditioner P
    idr_s(A, x, b, P, iter, s);

    return 0;
}


#define BOOST_TEST_MODULE "test_eigen"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include <mill/math/Matrix.hpp>
#include <mill/math/Vector.hpp>
#include <mill/math/EigenSolver.hpp>

#include <random>
#include <iostream>
constexpr static unsigned int seed = 32479327;
constexpr static std::size_t N = 1000;

inline double
determinant(const mill::Matrix<double, 3, 3>& mat)
{
    return mat(0,0) * mat(1,1) * mat(2,2) +
           mat(1,0) * mat(2,1) * mat(0,2) +
           mat(2,0) * mat(0,1) * mat(1,2) -
           mat(0,0) * mat(2,1) * mat(1,2) -
           mat(2,0) * mat(1,1) * mat(0,2) -
           mat(1,0) * mat(0,1) * mat(2,2);
}

BOOST_AUTO_TEST_CASE(eigenvalues)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> uni(0., 1.0);
    mill::JacobiEigenSolver solver;
    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        mill::Matrix<double, 3, 3> mat;
        double det = 0.;
        while(det == 0.)
        {
            for(std::size_t i=0; i<3; ++i)
                for(std::size_t j=i; j<3; ++j)
                    if(i==j)
                        mat(i,j) = uni(mt);
                    else
                        mat(i,j) = mat(j,i) = uni(mt);
            det = determinant(mat);
        }

        auto ev = solver.solve(mat);

        for(std::size_t i=0; i<3; ++i)
        {
            mill::Matrix<double, 3, 3> M = mat;

            for(std::size_t j=0; j<3; ++j)
                M(j, j) -= ev.at(i).first;

            const mill::Vector<double, 3> vec = M * ev.at(i).second;
            BOOST_CHECK_SMALL(vec[0], 1e-6);
            BOOST_CHECK_SMALL(vec[1], 1e-6);
            BOOST_CHECK_SMALL(vec[2], 1e-6);
        }
    }
}

BOOST_AUTO_TEST_CASE(eigenvalues_dynamic)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> uni(0.0, 1.0);
    mill::JacobiEigenSolver solver;

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        mill::Matrix<double, mill::DYNAMIC, mill::DYNAMIC> mat({10u, 10u});
        for(std::size_t i=0; i<mat.col(); ++i)
        {
            for(std::size_t j=i; j<mat.row(); ++j)
            {
                if(i == j)
                {
                    mat(i, j) = uni(mt);
                }
                else
                {
                    mat(i, j) = mat(j, i) = uni(mt);
                }
            }
        }

        auto ev = solver.solve(mat);
        for(std::size_t i=0; i<mat.row(); ++i)
        {
            auto M = mat;

            for(std::size_t j=0; j<mat.row(); ++j)
            {
                M(j, j) -= ev.at(i).first;
            }

            const auto vec = M * ev.at(i).second;
            BOOST_CHECK(vec.len() == M.row());
            BOOST_CHECK(vec.row() == M.row());
            BOOST_CHECK(vec.col() == 1);
            for(std::size_t j=0; j<vec.len(); ++j)
            {
                BOOST_CHECK_SMALL(vec[j], 1e-5);
            }
        }
    }
}


#pragma once

#include <math.h>
#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <acado/acado_optimal_control.hpp>
#include <acado_toolkit.hpp>
#include <map>
// #include "log.h"
#include "mission_planner_types.hpp"
#include <trajectory_planner.hpp>

typedef trajectory_planner::state state;

//! MissionPlannerInspection class
/*!
 * Abstract base class for mission planner. It cannot be instantiated
 */

class MissionPlannerInspection : public trajectory_planner::TrajectoryPlanner{

 public:

  /**
   * @brief constructor of the class
   */
  MissionPlannerInspection(const trajectory_planner::parameters _param, const inspection_params _inspection_params);

  /**
   * @brief destructor of the class
   */
  virtual ~MissionPlannerInspection();

  /**
   * @brief changes the desired point to inspect
   *
   * @param _point point to inspect
   */
  void setPointToInspect(const Eigen::Vector3d &_point) {
    point_to_inspect_ = std::move(_point);
  }

  /**
   * @brief gives the desired point to inspect
   *
   * @return point to inspect
   */
  Eigen::Vector3d getPointToInspect() { return point_to_inspect_; }

  /**
   * @brief changes the desired distance to the inspection point
   *
   * @param _distance distance to the inspection point
   */
  void setDistanceToInspect(const float &_distance) {
    distance_to_inspect_point_ = _distance;
  }

  /**
   * @brief increases/decreases the desired distance to the inspection point
   *
   * @param _distance true if increase, false if decrease
   */
  void incDistanceToInspect(const bool &_distance) {
    if (_distance)  setDistanceToInspect(distance_to_inspect_point_ + inspection_params_.inc_distance);
    else            setDistanceToInspect(distance_to_inspect_point_ - inspection_params_.inc_distance);
  }

  /**
   * @brief gives the distance to the inspection point
   *
   * @return distance to the inspection point
   */
  float getDistanceToInspect() { return distance_to_inspect_point_; }

  /**
   * @brief changes the desired relative angle of the drones
   *
   * @param _angle angle
   */
  void setRelativeAngle(const float &_angle) { relative_angle_ = _angle; }

  /**
   * @brief gives the relative angle
   *
   * @return relative angle
   */
  float getRelativeAngle() { return relative_angle_; }

  /**
   * @brief gives the current formation angle of a UAV
   *
   * @return formation angle
   */
  float getFormationAngle(const int &_id) { return calculateFormationAngle(_id); }

  /**
   * @brief gives the current inspection distance of a UAV
   *
   * @return inspection distance
   */
  float getInspectionDistance(const int &_id) { return calculateInspectionDistance(_id); }

  /**
   * @brief gives the mission status
   *
   * @return mission status: true if activated, false if not activated
   */
  float getMissionStatus() { return mission_status_; }

  /**
   * @brief sets the current time (obtained from ROS::Time)
   */
  void setCurrentTime(float time_) {current_time_ = time_;}

  /**
   * @brief sets the mission status
   *
   * @param _status mission status
   */
  void setMissionStatus(const bool &_status) { mission_status_ = _status; }

  /**
   * @brief changes the flight mode of the formation
   *
   * @param _mode mode
   */
  void setFlightMode(const uint &_mode) { flight_mode_ = _mode; }

  /**
   * @brief increases/decreases the relative angle of the drones
   *
   * @param _angle true if increase, false if decrease
   */
  void incRelativeAngle(const bool &_angle) {
    if (_angle)  setRelativeAngle(relative_angle_ + inspection_params_.inc_angle);
    else         setRelativeAngle(relative_angle_ - inspection_params_.inc_angle);
  }

  /**
   * @brief fits a given point to the cylinder/circle where the drones are
   * moving around
   *
   * @param point desired point to fit on the cylinder/circle
   * @return point on the cylinder/circle
   */
  Eigen::Vector3d pointOnCircle(const Eigen::Vector3d point);

  /**
   * @brief gets the angle of a given point based on the inspection point
   *
   * @param point desired point to infer its angle
   * @return angle (between 0 and PI)
   */
  float getPointAngle(const Eigen::Vector3d &_point);

  /**
   * @brief function that returns if path to describe between two point is clockwise or anticlockwise
   *
   * @param _point1 point to start
   * @param _point2 point to finish
   *
   * @return true if clockwise
   * @return false if anticlockwise
   */
  bool isClockwise(const Eigen::Vector3d &_point1, const Eigen::Vector3d &_point2);

  /**
   * @brief sets a new bunch of waypoints
   */
  void setGoals(const std::vector<trajectory_planner::state> &_waypoints) {
    goals_.clear();
    for (auto &waypoint : _waypoints) {
      goals_.push_back(waypoint);
    }
  }

 protected:
  bool mission_status_ = false;
  state last_goal_;
  Eigen::Vector3d point_to_inspect_ = Eigen::Vector3d::Zero();
  float distance_to_inspect_point_ = 3;
  float relative_angle_ = 0.7;
  std::map<int, float> inspection_distance_;
  std::map<int, float> formation_angle_;
  inspection_params inspection_params_;

  /**
   * @brief refreshes the value of the goal points
   */
  void refreshGoals() {
    for (auto &goal : goals_) {
      goal.pos = pointOnCircle(goal.pos);
    }
  }

  /**
   * @brief gets the total angle to travel given an initial and a final angle
   *
   * @param _initial_angle initial angle
   * @param _final_angle final angle
   * @return total angle to travel
   */
  float getTotalAngle(const float &_initial_angle, const float &_final_angle);

  /**
   * @brief calculates the formation angle that the leader UAV has with the _id follower UAV
   *
   * @param _id id of the follower UAV
   * @return formation angle
   */
  float calculateFormationAngle(const int &_id);

  /**
   * @brief calculates the inspection distance of the _id UAV
   *
   * @param _id id of the follower UAV
   * @return inspection distance
   */
  float calculateInspectionDistance(const int &_id);

 private:
  /**
   * @brief returns an initial trajectory to inspect for the drone according to
   * the initial pose
   *
   * @param initial_pose initial pose of the drone
   * @return vector of states of the trajectory
   */
  virtual std::vector<state> initialTrajectory(
      const state &initial_pose) = 0;

  /**
   * @brief returns the inspection trajectory for the drone according to
   * the initial pose
   *
   * @param initial_pose initial pose of the drone
   * @return vector of states of the trajectory
   */
  virtual std::vector<state> inspectionTrajectory(
      const state &initial_pose) = 0;

  /**
   * @brief virtual function that makes the following checks
   *
   * @return true if all checks are passed
   * @return false if any ot the check is not passed
   */
  virtual bool checks() {
    std::cout << "check mission planner abstract" << std::endl;
  }

  /**
   * @brief check if the formation has to inspect
   *
   * @return true if the formation has to inspect
   * @return false if the formation does not have to inspect
   */
  virtual bool inspecting();

  /**
   * @brief gives an initial orientation according to a trajectory given
   *
   * @param traj trajectory
   */
  virtual void initialOrientation(std::vector<state> &traj);

};


#include <array>
#include <fstream>
#include <iostream>
#include <numeric>
#include <string_view>
#include <unordered_map>

#include <boost/range/irange.hpp>
#include <boost/range/istream_range.hpp>

enum class Cmd { on, off, toggle };

using uint64 = std::uint64_t;

struct Position {
	uint64 row, col;
};

struct EndPoints {
	std::string state;
	Position first, last;
};

auto& operator>>(std::istream& in, Position& pos) {
	return in >> pos.row >> pos.col;
}

auto& operator>>(std::istream& in, EndPoints& pos) {
	return in >> pos.state >> pos.first >> pos.last;
}

auto get_total_brightness(std::fstream& file) {

	constexpr auto gridlen = uint64{1000};
	constexpr auto numlights = (gridlen * gridlen);

	auto grid = std::array<uint64, numlights>{};

	const auto commands = std::unordered_map<std::string_view, Cmd>{
		{"on", Cmd::on},
		{"off", Cmd::off},
		{"toggle", Cmd::toggle}
	};

	const auto stream = boost::istream_range<EndPoints>(file);

	for(const auto& endpoints : stream) {

		const auto command = commands.find(endpoints.state)->second;

		const auto& first = endpoints.first;
		const auto& last  = endpoints.last;

		const auto begin_row = first.row;
		const auto end_row   = (last.row + 1);

		for(const auto row : boost::irange(begin_row, end_row)) {

			const auto begin_col = first.col;
			const auto end_col   = (last.col + 1);

			for(const auto col : boost::irange(begin_col, end_col)) {

				const auto pos = (row * gridlen + col);

				switch(command) {
					case Cmd::on:
						++grid[pos];
						break;
					case Cmd::off:
						grid[pos] -= (grid[pos] > 0);
						break;
					case Cmd::toggle:
						grid[pos] += 2;
						break;
					default:
						std::cerr << "WTF?!? Something went really wrong!" << std::endl;
				}
			}
		}
	}

	return std::accumulate(grid.begin(), grid.end(), uint64{});
}

int main() {

	const auto filename = std::string{"instructions.txt"};
	auto file = std::fstream{filename};

	if(file.is_open()) {

		std::cout << get_total_brightness(file) << std::endl;

	} else {
		std::cerr << "Error! Could not open \"" << filename << "\"!" << std::endl;
	}

	return 0;
}