//---------------------------------Spheral++----------------------------------//
// GeomSymmetricTensor -- the symmetric tensor class
//----------------------------------------------------------------------------//
#include <cmath>
#include <limits>
#include <float.h>
#include <vector>

#include "GeomSymmetricTensor.hh"
#include "EigenStruct.hh"
#include "buildEigenVector.hh"
#include "findEigenValues3.hh"
#include "Utilities/SpheralFunctions.hh"
#include "Utilities/rotationMatrix.hh"

#include "Jacobi2.hh"

#include <Eigen/Dense>

#include <cmath>
using std::min;
using std::max;
using std::abs;

namespace Spheral {

using std::abs;

//------------------------------------------------------------------------------
// Return the eigen values and eigen vectors of a symmetric tensor
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// 3-D.
template<>
EigenStruct<3>
GeomSymmetricTensor<3>::eigenVectors() const {

  // Some useful typedefs.
  typedef GeomVector<3> Vector;
  typedef GeomTensor<3> Tensor;
  typedef GeomSymmetricTensor<3> SymTensor;

  // Tolerances for fuzzy math.
  const double degenerate = 1.0e-20;
  const double tolerance = 5.0e-5;

  // Prepare the result.
  EigenStruct<3> result;

  // Create a scaled version of this tensor, with all elements in the range [-1,1].
  const double fscale = max(10.0*std::numeric_limits<double>::epsilon(), this->maxAbsElement());
  CHECK(fscale > 0.0);
  const double fscalei = 1.0/fscale;
  SymTensor A = (*this)*fscalei;

  // Check for any degenerate elements, and just zero 'em out.
  A.xx(abs(A.xx()) < degenerate ? 0.0 : A.xx());
  A.xy(abs(A.xy()) < degenerate ? 0.0 : A.xy());
  A.xz(abs(A.xz()) < degenerate ? 0.0 : A.xz());
  A.yy(abs(A.yy()) < degenerate ? 0.0 : A.yy());
  A.yz(abs(A.yz()) < degenerate ? 0.0 : A.yz());
  A.zz(abs(A.zz()) < degenerate ? 0.0 : A.zz());

// #ifdef USEJACOBI

//   // Use the Jacobi iterative diagonalization method to determine
//   // the eigen values/vectors.
//   const int nrot = jacobiDiagonalize<Dim<3> >(A,
//                                               result.eigenVectors,
//                                               result.eigenValues);
//   result.eigenValues *= fscale;

// #elif USEEIGEN

  // Use the Eigen library to determine the eigen values/vectors.
  {
    Eigen::Matrix3d B;
    B <<
      A.xx(), A.xy(), A.xz(),
      A.yx(), A.yy(), A.yz(),
      A.zx(), A.zy(), A.zz();
    const Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(B);
    const Eigen::Vector3d& Bvals = eigensolver.eigenvalues();
    const Eigen::Matrix3d& Bvecs = eigensolver.eigenvectors();
    result.eigenValues = Vector(Bvals(0), Bvals(1), Bvals(2)) * fscale;
    const double x1 = 1.0/std::sqrt(Bvecs(0,0)*Bvecs(0,0) + Bvecs(1,0)*Bvecs(1,0) + Bvecs(2,0)*Bvecs(2,0));
    const double x2 = 1.0/std::sqrt(Bvecs(0,1)*Bvecs(0,1) + Bvecs(1,1)*Bvecs(1,1) + Bvecs(2,1)*Bvecs(2,1));
    const double x3 = 1.0/std::sqrt(Bvecs(0,2)*Bvecs(0,2) + Bvecs(1,2)*Bvecs(1,2) + Bvecs(2,2)*Bvecs(2,2));
    result.eigenVectors = Tensor(Bvecs(0,0)*x1, Bvecs(0,1)*x2, Bvecs(0,2)*x3,
                                 Bvecs(1,0)*x1, Bvecs(1,1)*x2, Bvecs(1,2)*x3,
                                 Bvecs(2,0)*x1, Bvecs(2,1)*x2, Bvecs(2,2)*x3);
  }

// #else

//   // Compute the scaled eigen-values, and sort them.
//   Vector lambdaVec = A.eigenValues();
//   sort(lambdaVec.begin(), lambdaVec.end());
//   CHECK(lambdaVec.x() <= lambdaVec.y() and
//         lambdaVec.y() <= lambdaVec.z());

//   // Assign the true eigen-values in the result.
//   result.eigenValues = fscale*lambdaVec;
//   result.eigenVectors = SymTensor::one;

//   // If any of the eigen-values result in a tensor that is not positive-rank
//   // (all zero elements), we assume the eigen-values are equal and punt
//   // with the identity tensor for the eigen-vectors.
//   // We simultaneously compute the row containing the maximum absolute value
//   // element for each eigen-value.
//   bool punt = false;
//   double maxEVelement = -1.0;
//   Vector maxEVrow;
//   int iFirst = -1;
//   for (int ivalue = 0; ivalue != 3; ++ivalue) {
//     const SymTensor M = A - lambdaVec(ivalue)*SymTensor::one;
//     if (M.maxAbsElement() < degenerate) punt = true;
//     for (int irow = 0; irow != 3; ++irow) {
//       const Vector Mvec = M.getRow(irow);
//       const double thpt = Mvec.maxAbsElement();
//       if (thpt > maxEVelement) {
//         maxEVelement = thpt;
//         maxEVrow = Mvec;
//         iFirst = ivalue;
//       }
//     }
//   }

//   // If we found an all zero M (= A - lambda*I) matrix, we punt and accept the identity
//   // tensor as our eigen-vectors.  Otherwise, continue the compuation.
//   if (!punt) {
//     CHECK(iFirst >= 0 and iFirst < 3);

//     // Select the ordering we'll go through the eigen-values in, starting
//     // with the row with the largest absolute value element.
//     const int iSecond = (iFirst + 1) % 3;
//     const int iThird = (iSecond + 1) % 3;
//     CHECK(iFirst + iSecond + iThird == 3);

//     // We need two orthogonal unit vectors in the plane perpendicular to
//     // the maximum row selected previously.  We can do this by finding the
//     // rotational transformation wherein x' axis is aligned with this row, and
//     // taking our two vectors as the other two rows of this transform.
//     const Vector R = maxEVrow.unitVector();
//     const Tensor Tr = rotationMatrix(R);
//     const Vector U0 = Tr.getRow(1);
//     const Vector U1 = Tr.getRow(2);

//     // Now we can compute the eigen-vector corresponding the first eigen-value
//     // selected previously.
//     const Vector V0 = buildUniqueEigenVector(A,
//                                              lambdaVec(iFirst),
//                                              U0,
//                                              U1);
//     result.eigenVectors.setColumn(iFirst, V0);

//     // Now we know the remaining eigen-vectors are in the plane perpendicular to
//     // V0.  We know R is in that plane, and so is R x V0.  With that knowledge
//     // we can basically repeat the same procedure for the next eigen-vector.
//     Vector S = R.cross(V0);
//     CHECK(fuzzyEqual(S.magnitude2(), 1.0, tolerance));
//     const Vector V1 = buildUniqueEigenVector(A,
//                                              lambdaVec(iSecond),
//                                              R,
//                                              S);
//     result.eigenVectors.setColumn(iSecond, V1);

//     // The last eigen-vector is orthogonal to the first two, so we can find it
//     // simply by taking the cross-product of the previous eigen-vectors.
//     const Vector V2 = V0.cross(V1);
//     CHECK(fuzzyEqual(V2.magnitude2(), 1.0, tolerance));
//     CHECK(fuzzyEqual(((A - lambdaVec(iThird)*SymTensor::one)*V2).maxAbsElement(), 0.0, tolerance));
//     result.eigenVectors.setColumn(iThird, V2);
//   }

// #endif

  BEGIN_CONTRACT_SCOPE
  // Check the result.
  const double lambda1 = result.eigenValues.x();
  const double lambda2 = result.eigenValues.y();
  const double lambda3 = result.eigenValues.z();
  const Vector v1 = result.eigenVectors.getColumn(0);
  const Vector v2 = result.eigenVectors.getColumn(1);
  const Vector v3 = result.eigenVectors.getColumn(2);
  ENSURE2(fuzzyEqual(v1.dot(v2), 0.0, tolerance) and
          fuzzyEqual(v1.dot(v3), 0.0, tolerance) and
          fuzzyEqual(v2.dot(v3), 0.0, tolerance),
          v1 << " " << v2 << " " << v3 << " : " << *this);
  ENSURE2(fuzzyEqual(v1.magnitude2(), 1.0, tolerance) and
          fuzzyEqual(v2.magnitude2(), 1.0, tolerance) and
          fuzzyEqual(v3.magnitude2(), 1.0, tolerance),
          v1 << " " << v2 << " " << v3);
  const double tol = tolerance*max(1.0, this->maxAbsElement());
  ENSURE2(fuzzyEqual((SymTensor(xx() - lambda1, xy(), xz(),
                                yx(), yy() - lambda1, yz(),
                                zx(), zy(), zz() - lambda1)*v1).maxAbsElement(), 0.0, tol),
          *this << " " << A << " " << lambda1 << " " << v1 << " " << tol << " "
          << SymTensor(xx() - lambda1, xy(), xz(),
                       yx(), yy() - lambda1, yz(),
                       zx(), zy(), zz() - lambda1)*v1);
  ENSURE(fuzzyEqual((SymTensor(xx() - lambda2, xy(), xz(),
                               yx(), yy() - lambda2, yz(),
                               zx(), zy(), zz() - lambda2)*v2).maxAbsElement(), 0.0, tol));
  ENSURE(fuzzyEqual((SymTensor(xx() - lambda3, xy(), xz(),
                               yx(), yy() - lambda3, yz(),
                               zx(), zy(), zz() - lambda3)*v3).maxAbsElement(), 0.0, tol));
  ENSURE(fuzzyEqual(abs(result.eigenVectors.Determinant()), 1.0, tolerance));
  END_CONTRACT_SCOPE

  return result;
}

//------------------------------------------------------------------------------
// Explicit instantiation.
//------------------------------------------------------------------------------
template class GeomSymmetricTensor<1>;
template class GeomSymmetricTensor<2>;
template class GeomSymmetricTensor<3>;

//------------------------------------------------------------------------------
// Set the static variables.
//------------------------------------------------------------------------------
template<> const unsigned GeomSymmetricTensor<1>::nDimensions = 1;
template<> const unsigned GeomSymmetricTensor<1>::numElements = 1;
template<> const GeomSymmetricTensor<1> GeomSymmetricTensor<1>::zero = GeomSymmetricTensor<1>(0.0);
template<> const GeomSymmetricTensor<1> GeomSymmetricTensor<1>::one = GeomSymmetricTensor<1>(1.0);

template<> const unsigned GeomSymmetricTensor<2>::nDimensions = 2;
template<> const unsigned GeomSymmetricTensor<2>::numElements = 3;
template<> const GeomSymmetricTensor<2> GeomSymmetricTensor<2>::zero = GeomSymmetricTensor<2>(0.0, 0.0,
                                                                                0.0, 0.0);
template<> const GeomSymmetricTensor<2> GeomSymmetricTensor<2>::one = GeomSymmetricTensor<2>(1.0, 0.0,
                                                                                0.0, 1.0);

template<> const unsigned GeomSymmetricTensor<3>::nDimensions = 3;
template<> const unsigned GeomSymmetricTensor<3>::numElements = 6;
template<> const GeomSymmetricTensor<3> GeomSymmetricTensor<3>::zero = GeomSymmetricTensor<3>(0.0, 0.0, 0.0,
                                                                                0.0, 0.0, 0.0,
                                                                                0.0, 0.0, 0.0);
template<> const GeomSymmetricTensor<3> GeomSymmetricTensor<3>::one = GeomSymmetricTensor<3>(1.0, 0.0, 0.0,
                                                                                0.0, 1.0, 0.0,
                                                                                0.0, 0.0, 1.0);

template<> const double GeomSymmetricTensor<1>::onethird = 1.0/3.0;
template<> const double GeomSymmetricTensor<2>::onethird = 1.0/3.0;
template<> const double GeomSymmetricTensor<3>::onethird = 1.0/3.0;

template<> const double GeomSymmetricTensor<1>::sqrt3 = std::sqrt(3.0);
template<> const double GeomSymmetricTensor<2>::sqrt3 = std::sqrt(3.0);
template<> const double GeomSymmetricTensor<3>::sqrt3 = std::sqrt(3.0);

}


// Copyright Oleg Maximenko 2014.
// 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)
//
// See http://github.com/svgpp/svgpp for library home page.

#pragma once

#include <svgpp/definitions.hpp>
#include <boost/mpl/joint_view.hpp>
#include <boost/mpl/set.hpp>
#include <boost/ratio/ratio.hpp>

namespace svgpp { namespace traits
{

typedef boost::mpl::set5<
  tag::length_units::in,
  tag::length_units::cm,
  tag::length_units::mm,
  tag::length_units::pt,
  tag::length_units::pc
> absolute_length_units;

typedef boost::mpl::joint_view<
  absolute_length_units,
  boost::mpl::set5<
    tag::length_units::em,
    tag::length_units::ex,
    tag::length_units::px,
    tag::length_units::percent,
    tag::length_units::none
> > all_length_units;

template<class Src, class Dst>
struct absolute_length_conversion_coefficient
{
  typedef typename boost::ratio_divide<
    typename absolute_length_conversion_coefficient<tag::length_units::in, Dst>::ratio,
    typename absolute_length_conversion_coefficient<tag::length_units::in, Src>::ratio
  > ratio;
};

template<class Src>
struct absolute_length_conversion_coefficient<Src, Src>
{ typedef boost::ratio<1> ratio; };

template<>
struct absolute_length_conversion_coefficient<tag::length_units::in, tag::length_units::cm>
{ typedef boost::ratio<254, 100> ratio; };

template<>
struct absolute_length_conversion_coefficient<tag::length_units::in, tag::length_units::mm>
{ typedef boost::ratio<254, 10> ratio; };

template<>
struct absolute_length_conversion_coefficient<tag::length_units::in, tag::length_units::pt>
{ typedef boost::ratio<72> ratio; };

template<>
struct absolute_length_conversion_coefficient<tag::length_units::in, tag::length_units::pc>
{ typedef boost::ratio<6> ratio; };

}}


/**
 * \file dcs/math/random/linear_congruential.hpp
 *
 * \brief Linear Congruential Random Number Engine.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_RANDOM_LINEAR_CONGRUENTIAL_HPP
#define DCS_MATH_RANDOM_LINEAR_CONGRUENTIAL_HPP

#include <dcs/detail/config/boost.hpp>

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(101500) // 1.15
#	error "Required Boost libraries version >= 1.15."
#endif

#include <boost/random/linear_congruential.hpp>
#include <cstddef>
#include <dcs/math/random/base_generator.hpp>
#include <stdint.h>

namespace dcs { namespace math { namespace random {

//using ::std::size_t;

/*
template <typename IntT, IntT a, IntT c, IntT m>
class linear_congruential
{
	public: typedef IntT result_type;
	public: static const result_type min_value = (c == 0 ? 1 : 0);
	public: static const result_type max_value = m-1;
	public: static const result_type multiplier = a;
	public: static const result_type increment = c;
	public: static const result_type modulus = m;

	public: explicit linear_congruential(IntT x0=1)
	{
		seed(x0);
	}

	public: void seed(IntT x0=1)
	{
		x_ = ((modulus != 0) ? (x0 % modulus) : x0);

		// handle negative seeds
		if (x_ < 0)
		{
			x_ += modulus;
		}
		// adjust to the correct range
		if (increment == 0 && x_ == 0)
		{
			x_ = 1;
		}

		// post-conditions
		DCS_ASSERT(
			x_ >= min_value,
			throw std::domain_error("Seed less than min value")
		);
		DCS_ASSERT(
			x_ <= max_value,
			throw std::domain_error("Seed greater than max value")
		);
	}

	public: result_type operator()()
	{
		return next();
	}

	public: friend bool operator==(linear_congruential const& l1, linear_congruential const& l2)
	{
		return l1.x_ == l2.x_;
	}

	public: friend bool operator!=(linear_congruential const& l1, linear_congruential const& l2)
	{
		return !(l1 == l2);
	}

	// Perform: (a*x+c) mod m
	private: IntT next()
	{
		IntT max_d_a = std::numeric_limits<IntT>::max()/multiplier;

		if(modulus <= (max_d_a - increment/multiplier))   // i.e. a*m+c <= max
		{
			x_= (multiplier * x_ + increment) % modulus;
		}
		else
		{
			// increment and multiplier are big

			// Perform the product
			if (multiplier != 1)
			{
				if (modulus <= max_d_a) // i.e. a*m <= max
				{
					// small product
					x_ = (multiplier * x_) % modulus;
				}
				else if (std::numeric_limits<IntT>::is_signed && (modulus % multiplier) < (modulus / multiplier))
				{
					// Use the Schrage's Algorithm:
					//   an algorithm for multiplying two 32-bit integers modulo
					//   a 32-bit constant without using any intermediates
					//   larger than 32 bits.

					const IntT q = modulus / multiplier;
					const IntT r = modulus % multiplier;

					assert(r < q);        // check that overflow cannot happen

					x_ = multiplier*(x_%q) - r*(x_/q);
					while (x_ <= 0)
					{
						x_ += modulus;
					}
				}
				else
				{
//					DCS_ASSERT("multiplier is too large");
					x_ = 0;
				}
			}

			// Perform the sum
			if (increment != 0)
			{
				if (increment <= (std::numeric_limits<IntT>::max()-modulus)) // i.e. m+x < max
				{
					// small sum
					x_ += increment;
					if (x_ >= modulus)
					{
						x_ -= modulus;
					}
				}
				else if (std::numeric_limits<IntT>::is_signed)
				{
					IntT m_sub_c(modulus-increment);

					if (x_ < m_sub_c)
					{
						x_ += increment;
					}
					else
					{
						x_ -= m_sub_c;
					}
				}
				else
				{
//					DCS_ASSERT("increment is too large°);
					x_ = 0;
				}
			}
		}

		return x_;
	}

	private: IntT x_;
};
*/

/**
 * \brief Linear Congruential Random Number Engine.
 *
 * \tparam UIntT The type of randomly generated numbers.
 * \tparam a The multiplier.
 * \tparam c The increment.
 * \tparam m The modulus.
 *
 * The Linear Congruential random number generator has the following form:
 * \f[
 *   X_{i+1} = a X_{i} + c \quad (\operatorname{mod} m)
 * \f]
 * where:
 * - \f$a\f$ is the \e multiplier.
 * - \f$c\f$ is the \e increment.
 * - \f$m\f$ is the \e modulus.
 * .
 * This class implements the \c RandomNumberEngine concept.
 *
 * \see "Numerical Recipes in C: The Art of Scientific Computing" (William H. Press, Brian P. Flannery, Saul A. Teukolsky, William T. Vetterling).
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename UIntT, UIntT a, UIntT c, UIntT m>
class linear_congruential: public base_generator<UIntT>
{
	private: typedef base_generator<UIntT> base_type;
	public: typedef UIntT result_type;
	private: typedef ::boost::random::linear_congruential<UIntT,a,c,m,0> impl_type;
	public: typedef typename base_type::ulonglong_type ulonglong_type;
//	public: typedef long long ulonglong_type;

	public: static const result_type multiplier = a;
	public: static const result_type increment = c;
	public: static const result_type modulus = m;
	public: static const result_type default_seed = impl_type::default_seed;

	public: linear_congruential()
		: impl_()
	{
		// empty
	}

	public: explicit linear_congruential(result_type s)
		: impl_(s)
	{
		// empty
	}

//	public: template <typename ItT>
//		linear_congruential(ItT& first, ItT& last)
//		: impl_(first,last)
//	{
//		// empty
//	}

//	public: template <typename ItT>
//		void seed(ItT& first, ItT& last)
//	{
//		impl_.seed(first, last);
//	}

	public: static result_type min()
	{
		return (increment == 0u ? 1u : 0u);
	}

	public: static result_type max()
	{
		return (modulus - 1);
	}

	private: result_type do_min() const
	{
		return impl_.min();
	}

	private: result_type do_max() const
	{
		return impl_.max();
	}

	private: void do_seed()
	{
		impl_.seed();
	}

	private: void do_seed(result_type s)
	{
		impl_.seed(s);
	}

	private: result_type do_generate()
	{
		return impl_();
	}

	//FIXME: actually cannot use impl_.discard since it is defined only when
	//       BOOST_NO_LONG_LONG is undefined.
	private: void do_discard(ulonglong_type z)
	{
		for ( ; z != 0; --z)
		{
			this->operator()();
		}
//		impl_.discard(z);
	}

	public: friend bool operator==(linear_congruential const& l1, linear_congruential const& l2)
	{
		return l1.impl_ == l2.impl_;
	}

	public: friend bool operator!=(linear_congruential const& l1, linear_congruential const& l2)
	{
		return !(l1 == l2);
	}

	private: impl_type impl_;
};

// Some standard LCG (see http://random.mat.sbg.ac.at/~charly/server/node3.html)

typedef linear_congruential<int32_t, 16807, 0, 2147483647> minstd_rand0;
typedef linear_congruential<int32_t, 48271, 0, 2147483647> minstd_rand1;
typedef linear_congruential<int32_t, 69621, 0, 2147483647> minstd_rand2;
//FIXME: the declaration below requires a true 64 bit integer in order to
// store the two integral constants.
// For the moment use the boost::rand48 class (which emulates drand48
// without requiring true 64bit support) until we find a way the underlying
// architecture type.
#if true
typedef linear_congruential<uint64_t, 25214903917, 11, 281474976710656> rand48;
#else
class rand48
{
	public: typedef ::boost::rand48::result_type result_type;
	public: static const result_type default_seed = 1u;

	public: rand48()
		: impl_(default_seed)
	{
		// empty
	}

	public: explicit rand48(result_type s)
		: impl_(s)
	{
		// empty
	}

	public: template <typename ItT>
		rand48(ItT& first, ItT& last)
		: impl_(first,last)
	{
		// empty
	}

	public: void seed(result_type s=default_seed)
	{
		impl_.seed(s);
	}

	public: template <typename ItT>
		void seed(ItT& first, ItT& last)
	{
		impl_.seed(first, last);
	}

	public: static result_type min()
	{
		return 0;
	}

	public: static result_type max()
	{
		return ::std::numeric_limits<int32_t>::max();
	}

	public: result_type operator()()
	{
		return impl_();
	}

	public: void discard(size_t z)
	{
		for ( ; z != 0; --z)
		{
			operator()();
		}
	}

	public: friend bool operator==(rand48 const& lhs, rand48 const& rhs)
	{
		return lhs.impl_ == rhs.impl_;
	}

	public: friend bool operator!=(rand48 const& lhs, rand48 const& rhs)
	{
		return !(lhs == rhs);
	}

	private: ::boost::rand48 impl_;
};
#endif // false

}}} // Namespace dcs::math::random

#endif // DCS_MATH_RANDOM_LINEAR_CONGRUENTIAL_HPP


#pragma once

#include <stdlib.h>
#include <math.h>

#include "vec.h"
#include "gnilk/engine/core/Core.hpp"
#include "gnilk/engine/core/Edge.hpp"
#include "gnilk/engine/core/Triangle.hpp"
#include "gnilk/engine/core/FaceGroup.hpp"

#include <boost/unordered/unordered_map.hpp>

#include <vector>

namespace gnilk
{
	namespace engine
	{
		using namespace gnilk::engine::core;

		class FaceHelper
		{
		public:
			//
			// calculates vertex normals per group basis
			// vertex normals are stored directly in to the triangles
			// I did not want unsynchronized buffer sizes nor buffer dependencies on group
			// This is not optimal for rendering - that has to be solved when computing render buffers
			//
			static void CalculateVertexNormals(int vertices, std::vector<int> &faces, std::vector<Triangle *> &triangles) {
				std::vector<Normal> normals;
				normals.reserve(vertices);	// need intermediate storage
				// need to initialize array
				for(int i=0;i<vertices;i++) {
					vIni(normals[i].data,0,0,0);
				}
				// add up normals per vertex - note: normals should already be normalized!
				for(int i=0;i<faces.size();i++) {
					int idxTri = faces[i];
					Triangle *tri = triangles[idxTri];
					for(int j=0;j<3;j++) {
						vAdd(normals[tri->v[j]].data, normals[tri->v[j]].data, tri->normal.data);
					}
				}
				// normalize and assign
				for(int i=0;i<faces.size();i++) {
					Triangle *tri = triangles[faces[i]];
					for(int j=0;j<3;j++) {
						vNorm(tri->vn[j].data,normals[tri->v[j]].data);
					}
				}
			}

			static void CalculateVertexNormals(int vertices, std::vector<FaceGroup*> &groups, std::vector<Triangle *> &triangles)
			{
				for(int i=0;i<groups.size();i++)
				{
					printf("CVN, G: %d, T: %d\n",i, groups[i]->triangles.size());
					CalculateVertexNormals(vertices, groups[i]->triangles, triangles);
				}
			}

			static void CalculateFaceNormal(Normal *faceNormal, std::vector<Vertex3D> &coords, int v1, int v2, int v3) {
				float *pV1 = coords[v1].data;
				float *pV2 = coords[v2].data;
				float *pV3 = coords[v3].data;

				float vE1[3];
				float vE2[3];
				vSub(vE1, pV3, pV1);
				vSub(vE2, pV2, pV1);
				vCross(faceNormal->data, vE1, vE2);
				vNorm(faceNormal->data,faceNormal->data);
			}

			// Calculate normals for a list of faces and assign to face
			static void CalculateFaceNormals(std::vector<Triangle *> &triangles, std::vector<Vertex3D> &coords) {
				for(int i=0;i<triangles.size();i++) {
					Triangle *tri = triangles[i];
					if (!tri->HasNormal()) {
						CalculateFaceNormal(tri->normal.data, coords, tri->v[0], tri->v[1], tri->v[2]);
					}
				}
			}

			// Calculates a single face normal
			static void CalculateFaceNormal(float *normal, std::vector<Vertex3D> &coords, int v1, int v2, int v3) {
				float *pV1 = coords[v1].data;
				float *pV2 = coords[v2].data;
				float *pV3 = coords[v3].data;

				float vE1[3];
				float vE2[3];
				vSub(vE1, pV3, pV1);
				vSub(vE2, pV2, pV1);
				vCross(normal, vE2, vE1);
				vNorm(normal, normal);
			}

			// calculate list of normals for each face
			static void CalculateFaceNormals(std::vector<Normal> &normals, std::vector<Triangle *> &triangles, std::vector<Vertex3D> &coords) {
				normals.reserve(triangles.size());
				for(int i=0;i<triangles.size();i++) {
					CalculateFaceNormal(normals[i].data, coords, triangles[i]->v[0], triangles[i]->v[1], triangles[i]->v[2]);
				}
			}

			// Gets or adds (a new) group for face depending on the deviation between face normal and group normal
			// Not used..
			static FaceGroup *GetOrAddGroup(std::vector<FaceGroup *> &groups, Normal &normal, float threshold) {
				FaceGroup *group = NULL;
				float lastFac = threshold;

				// Find group (if any) which fulfills the the normal threshold
				for(size_t i=0;i<groups.size();i++) {
					float fac = 1.0 - vDot(normal.data, groups[i]->baseNormal.data);
					if ((fac > 0) && (fac < threshold)) {
						group = groups[i];
						lastFac = fac;
					}
				}
				if (group == NULL)
				{
					group = new FaceGroup();
					vDup(group->baseNormal.data, normal.data);
					groups.push_back(group);
				}
				return group;
			}

			// Recursive, assigns faces to groups - based on edges and if two faces have a similar normal they are allowed in the same group
			static void DoCalculateFaceGroups(FaceGroup *pGroup,
					int triangle,
					float threshold,
					std::vector<bool> &visited,
					std::vector<Edge *> &edges,
					std::vector<Triangle *> &triangles,
					std::vector<Normal> &faceNormals,
					int depth)
			{
				if (visited[triangle])
				{
					//printf("%d:%d -> .\n",depth,triangle);
					return;
				}

				pGroup->triangles.push_back(triangle);
				visited[triangle] = true;

				Triangle *pTri = triangles[triangle];
				for(int i=0;i<3;i++)
				{
					Edge *edge = triangles[triangle]->edges[i];
					if (edge == NULL)
					{
						printf("NULL edge %d for triangle %d\n",i, triangle);
						continue;
					}
					int tNext = -1;
					if ((edge->f1 != triangle) && (edge->f1 != -1)) tNext = edge->f1;
					else if ((edge->f2 != triangle) && (edge->f2 != -1)) tNext = edge->f2;

					if (tNext >= 0)
					{
						Triangle *pTriNext = triangles[tNext];
						float faceDiff = 1.0 - vDot(pTri->normal.data, pTriNext->normal.data);
						//float faceDiff = 1.0 - vDot(faceNormals[triangle].data, faceNormals[tNext].data);
						faceDiff = fabs(faceDiff);
						if ((faceDiff >= 0) && (faceDiff <= threshold))
						{
							//printf("%d:%d -> %d - %f\n",depth,triangle, tNext, faceDiff);
							DoCalculateFaceGroups(pGroup, tNext, threshold, visited, edges, triangles, faceNormals,depth+1);
						} else
						{
							// rejected because of threshold
							//printf("%d, r: %f\n",faceDiff);
						}
					}
				} // Foreach edge
				//printf("%d:%d -> x\n",depth,triangle);
			}

			// Automatic group of faces depending on
			static void CalculateFaceGroups(std::vector<FaceGroup *> &groups, std::vector<Edge *> &edges, std::vector<Triangle *> &triangles, std::vector<Normal> &faceNormals)
			{
				std::vector<bool> visited;
				visited.reserve(triangles.size());
				for(size_t i=0;i<triangles.size();i++)
				{
					visited.push_back(false);
				}
				for(size_t i=0;i<triangles.size();i++)
				{
					if (!visited[i])
					{
						FaceGroup *pGroup = new FaceGroup();
						DoCalculateFaceGroups(pGroup, i, 0.5, visited, edges, triangles, faceNormals,0);
						printf("Group %d, triangles: %d\n", groups.size(), pGroup->triangles.size());
						groups.push_back(pGroup);
					}
				}

				for(size_t i=0;i<groups.size();i++)
				{
					FaceGroup *group = groups[i];
					vMul(group->normal.data, group->normal.data, 1.0 / (double)group->triangles.size());
				}
				printf("Groups: %d\n",groups.size());
			}

			// Calculate edges
			static void CalculateEdges(std::vector<Edge *> &edges, std::vector<Triangle *> &triangles, const std::vector<Vertex3D> &vertices)
			{
				boost::unordered::unordered_map<int, Edge *> edgemap;
				for(size_t i=0;i<triangles.size();i++)
				{
					triangles[i]->Reset();
					GetOrAddEdgeMap(edgemap, triangles, vertices, i, 0, 1);
					GetOrAddEdgeMap(edgemap, triangles, vertices, i, 1, 2);
					GetOrAddEdgeMap(edgemap, triangles, vertices, i, 2, 0);
				}
				// I guess there is a better way to generate a vector instead of this...
				int count = 0;
				for (boost::unordered::unordered_map<int, Edge *>::iterator it = edgemap.begin(); it != edgemap.end(); ++it )
				{
					Edge *edge = it->second;
					if (edge->f1 >= 0) triangles[edge->f1]->AddEdge(edge);
					if (edge->f2 >= 0) triangles[edge->f2]->AddEdge(edge);
					edges.push_back(it->second);
					count++;
				}
			} // CalculateEdges

			// Convert faces to triangles - quads are split
			static void FacesToTriangles(
					std::vector<Triangle *> &triangles,
					std::vector<Face *> &faces) {

				for(size_t i=0;i<faces.size();i++) {
					Triangle *tri = new Triangle();
					tri->faceData = faces[i]->faceData;
					for(int j=0;j<3;j++) {
						tri->v[j] = faces[i]->v[j];
						//tri->vn[j] = faces[i]->vn[j];
						tri->uv[j] = faces[i]->uv[j];
					}
					triangles.push_back(tri);
					if (faces[i]->faceType == kFaceType_Quad) {
						tri = new Triangle();
						tri->faceData = faces[i]->faceData;
						for(int j=0;j<3;j++) {
							int idx = ((j+2)>3) ? 0 : j+2;
							tri->v[j] = faces[i]->v[idx];
							//tri->vn[j] = faces[i]->vn[idx];
							tri->uv[j] = faces[i]->uv[idx];
						}
						triangles.push_back(tri);
					}
				}
			} // FacesToTriangles

			static void CollapseEdge(int edge, std::vector<Triangle *> &triangles, std::vector<Edge *> &edges, std::vector<Vertex3D> &coords)
			{
				Edge *pEdge = edges[edge];
				int f1 = pEdge->f1;
				int f2 = pEdge->f2;
				int v1 = pEdge->v1;
				int v2 = pEdge->v2;
				// will this work?
				triangles.erase(triangles.begin()+f1);
				triangles.erase(triangles.begin()+f2);

				edges.erase((edges.begin()+edge));
			}

			static int CountDuplicateVertices(std::vector<Vertex3D> &coords) {
				int duplicates = 0;
				for(int i=0;i<coords.size();i++) {
					float temp[3];
					vNorm(temp,coords[i].data);
					for(int j=0;j<coords.size();j++) {
						if (i==j) continue;
						float temp2[3];
						vNorm(temp2, coords[j].data);
						float fac = vDot(temp,temp2);
						if (fac>0 && fac<0.01) {
							duplicates++;
						}
					}
				}
				return duplicates;
			}

			// Calculates bound sphere radius and mid point
			static double CalculateBoundingSphere(Vertex3D &mid, std::vector<Vertex3D> &coords) {
				Vertex3D cmax,cmin;
				float tmp[3];
				cmax = coords[0];
				cmin = coords[1];
				// Find max/min vertex
				for(int i=0;i<coords.size();i++) {
					cmax.data[0] = fmax(coords[i].data[0], cmax.data[0]);
					cmax.data[1] = fmax(coords[i].data[1], cmax.data[1]);
					cmax.data[2] = fmax(coords[i].data[2], cmax.data[2]);

					cmin.data[0] = fmin(coords[i].data[0], cmin.data[0]);
					cmin.data[1] = fmin(coords[i].data[1], cmin.data[1]);
					cmin.data[2] = fmin(coords[i].data[2], cmin.data[2]);
				}

				// Calculate radius and mid-point
				// Note: Why 2.5???
				double r = vAbs(vSub(tmp,cmax.data, cmin.data)) / 2.5;
				vAdd(tmp, cmax.data, cmin.data);
				vMul(mid.data, tmp, 0.5);
				return r;
			}

			static int SplitEdge(std::vector<Vertex3D> &coordOut, std::vector<Vertex3D> &coords, int v1, int v2) {
				Vertex3D mid;
				vMul(mid.data, vAdd(mid.data,coords[v1].data,coords[v2].data),0.5f);
				coordOut.push_back(mid);
				return (coordOut.size()-1);
			}

			/*      2
			 *     4 5
			 *    1 6 3
			 *
			 * Tris:
			 * 	1,4,6
			 * 	2,5,4
			 * 	3,6,5
			 * 	4,5,6
			 *
			*/
			static void SplitTriangle(std::vector<Triangle *> &triOut, std::vector<Vertex3D> &coordOut, Triangle *triangle, std::vector<Vertex3D> &coords) {
				int v1 = triangle->v[0];
				int v2 = triangle->v[1];
				int v3 = triangle->v[2];

				int idxStart = coordOut.size();
				coordOut.push_back(coords[v1]);
				coordOut.push_back(coords[v2]);
				coordOut.push_back(coords[v3]);
				SplitEdge(coordOut, coords, v1, v2);
				SplitEdge(coordOut, coords, v2, v3);
				SplitEdge(coordOut, coords, v3, v1);
				// Create 4 new triangles

				triOut.push_back(new Triangle(idxStart+0, idxStart+3, idxStart+5));
				triOut.push_back(new Triangle(idxStart+1, idxStart+4, idxStart+3));
				triOut.push_back(new Triangle(idxStart+2, idxStart+5, idxStart+4));
				triOut.push_back(new Triangle(idxStart+3, idxStart+4, idxStart+5));
			}

		private:
			static void GetOrAddEdgeMap(boost::unordered::unordered_map<int, Edge *> &edges,
				std::vector<Triangle *> &triangles,
				const std::vector<Vertex3D> &vertices,
				int face, int v1, int v2) {
				Edge *pEdge = NULL;

				int hashCode = Edge::HashCode(triangles[face]->v[v1], triangles[face]->v[v2]);
				//printf("hc: %d (%d, %d)\n",hashCode,faces[face]->v[v1], faces[face]->v[v2]);
				if (edges.find(hashCode) == edges.end()) {
					pEdge = new Edge(triangles[face]->v[v1], triangles[face]->v[v2], vertices);
					pEdge->AddFace(face);
					edges.insert(std::pair<int, Edge *>(hashCode, pEdge));
				} else {
					if (edges[hashCode]->AddFace(face) != true) {
						//printf("!!!! MULTIPLE FACES SHARING SAME EDGE!!!\n");
						// TODO: Create new face!
					}
				}
			} // GetOrAddEdgeMap
		}; // class FaceHelper
	}
}


// SPDX-License-Identifier: MIT
// Copyright (c) 2021 ETH Zurich, Luc Grosheintz-Laval

#include <iostream>
#include <string>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <filesystem>
#include <numeric>
#include <zisa/grid/grid.hpp>
#include <zisa/io/hdf5_serial_writer.hpp>
#include <zisa/math/cartesian.hpp>
#include <zisa/math/permutation.hpp>
#include <zisa/math/space_filling_curve.hpp>
#include <zisa/memory/array.hpp>
#include <zisa/memory/array_view.hpp>

namespace zisa {

void sanity_check(const array_const_view<int_t, 2> &vertex_indices,
                  const array_const_view<XYZ, 1> vertices) {

  auto v = [&vertex_indices, &vertices](int_t i, int_t k) {
    return vertices[vertex_indices(i, k)];
  };

  auto n_cells = vertex_indices.shape(0);
  auto volumes = array<double, 1>(n_cells);
  auto areas = array<double, 1>(n_cells);

  for (int_t i = 0; i < n_cells; ++i) {
    auto tet = Tetrahedron(v(i, 0), v(i, 1), v(i, 2), v(i, 3));
    volumes(i) = volume(tet);
    areas(i) = volume(face(tet, 0)) + volume(face(tet, 1))
               + volume(face(tet, 2)) + volume(face(tet, 3));
  }

  auto [vol_min, vol_max] = std::minmax_element(volumes.begin(), volumes.end());

  LOG_WARN_IF(*vol_max / *vol_min > 10.0, "Suspect cell found.");
  LOG_ERR_IF(*vol_max / *vol_min > 100.0, "Faulty cell found.");
}

void renumber_grid(const std::string &grid_file) {

  auto [vertices, vertex_indices, n_dims] = [&grid_file]() {
    auto reader = HDF5SerialReader(grid_file);

    auto vertices = array<XYZ, 1>::load(reader, "vertices");
    auto vertex_indices = array<int_t, 2>::load(reader, "vertex_indices");
    auto n_dims = reader.read_scalar<int_t>("n_dims");

    return std::tuple{
        std::move(vertices), std::move(vertex_indices), std::move(n_dims)};
  }();

  sanity_check(vertex_indices, vertices);

  auto n_cells = vertex_indices.shape(0);
  auto max_vertices = vertex_indices.shape(1);
  auto cell_centers = array<XYZ, 1>(n_cells);

  double x_min = std::numeric_limits<double>::max();
  double x_max = std::numeric_limits<double>::min();

  double y_min = std::numeric_limits<double>::max();
  double y_max = std::numeric_limits<double>::min();

  double z_min = std::numeric_limits<double>::max();
  double z_max = std::numeric_limits<double>::min();

  for (int_t i = 0; i < n_cells; ++i) {
    cell_centers[i] = vertices[vertex_indices(i, 0)];
    for (int_t k = 0; k < max_vertices; ++k) {
      cell_centers[i] += vertices[vertex_indices(i, k)];
    }

    cell_centers[i] /= max_vertices;

    x_min = zisa::min(cell_centers[i][0], x_min);
    x_max = zisa::max(cell_centers[i][0], x_max);

    y_min = zisa::min(cell_centers[i][1], y_min);
    y_max = zisa::max(cell_centers[i][1], y_max);

    z_min = zisa::min(cell_centers[i][2], z_min);
    z_max = zisa::max(cell_centers[i][2], z_max);
  }

  for (int_t i = 0; i < n_cells; ++i) {
    auto [x, y, z] = cell_centers[i];

    cell_centers[i][0] = (x - x_min) / (x_max - x_min + 1e-10 * x_max);
    cell_centers[i][1] = (y - y_min) / (y_max - y_min + 1e-10 * y_max);

    if (n_dims == 3) {
      cell_centers[i][2] = (z - z_min) / (z_max - z_min + 1e-10 * z_max);
    }
  }

  auto sfc_indices = array<int_t, 1>(n_cells);
  for (int_t i = 0; i < n_cells; ++i) {
    if (n_dims == 2) {
      auto [x, y, _] = cell_centers[i];
      sfc_indices[i]
          = integer_cast<int_t>(hilbert_index<64 / 2>(x, y).to_ullong());
    }

    if (n_dims == 3) {
      auto [x, y, z] = cell_centers[i];
      sfc_indices[i]
          = integer_cast<int_t>(hilbert_index<64 / 3>(x, y, z).to_ullong());
    }
  }

  auto sigma = array<int_t, 1>(n_cells);
  for (int_t i = 0; i < n_cells; ++i) {
    sigma[i] = i;
  }
  std::sort(sigma.begin(), sigma.end(), [&sfc_indices](int_t i, int_t j) {
    return sfc_indices[i] < sfc_indices[j];
  });

  apply_permutation(array_view(vertex_indices), factor_permutation(sigma));

  {
    auto writer = HDF5SerialWriter(grid_file + "_");
    save(writer, vertices, "vertices");
    save(writer, vertex_indices, "vertex_indices");
    writer.write_scalar(n_dims, "n_dims");
  }
  std::filesystem::rename(grid_file + "_", grid_file);
}

void generate_full_grid(const std::string &msh_h5_file) {
  auto grid = zisa::load_grid(msh_h5_file);

  auto filename = msh_h5_file.substr(0, msh_h5_file.size() - 7) + ".h5";
  auto hdf5_writer = HDF5SerialWriter(filename);
  save(hdf5_writer, *grid);
}

}

int main(int argc, char *argv[]) {
#if ZISA_HAS_MPI
  MPI_Init(&argc, &argv);
#endif

  po::variables_map options;

  // generic options
  po::options_description generic("Generic options");

  // clang-format off
  generic.add_options()
      ("help,h", "produce this message")
      ("grid", po::value<std::string>(), "Name of the .msh.h5 grid file, will be overwritten.")
      ;
  // clang-format on

  // first parse cmdline and check what config file to use
  po::store(po::parse_command_line(argc, argv, generic), options);

  if (options.count("help") != 0) {
    std::cout << generic << "\n";
    std::exit(EXIT_SUCCESS);
  }

  if (options.count("grid") == 0) {
    std::cout << "Missing argument `--grid GRID`.\n";
    std::exit(EXIT_FAILURE);
  }

  auto grid_file = options["grid"].as<std::string>();

  zisa::renumber_grid(grid_file);
  zisa::generate_full_grid(grid_file);

#if ZISA_HAS_MPI
  MPI_Finalize();
#endif
}


//
// Created by Vlad Argunov on 24/10/2021.
//

#include "Option.h"
#include <iostream>
#include <Eigen/Dense>
#include <string>
#include <algorithm>
#include <iomanip>

Option::Option(std::string type_option_, bool european_){
    type_option = type_option_;
    european = european_;
}

void Option::set_tn(double tn_) {
    tn = tn_;

}

void Option::set_s_max(double s_max_) {
    s_max = s_max_;
}

void Option::set_numdiff_t(int numdiff_t_) {
    numdiff_t = numdiff_t_;
}

void Option::set_numdiff_s(int numdiff_s_) {
    numdiff_s = numdiff_s_;
    }

void Option::set_volatility(double vol_, bool stochastic_vol_) {
    stochastic_vol = stochastic_vol_;
    if (stochastic_vol == 0){
        deterministic_vol = vol_;
    }
    // Code for implementation of stochastic volatility
}

void Option::set_interest_rate(double rate_, bool stochastic_rate_) {
    stochastic_rate = stochastic_rate_;
    if (stochastic_rate == 0){
        deterministic_rate = rate_;
    }
    // Code for implementation of stochastic interest rate
}

void Option::set_strike(double strike_) {
    strike = strike_;
}

void Option::set_stock_boundary_condition(std::string stock_boundary_condition_) {
    stock_boundary_condition = stock_boundary_condition_;
}

double Option::parameter_a(int n) {
    if (stochastic_vol == 0 && stochastic_rate == 0 && fixed_step_time == 1){
        return 0.5 * (deterministic_rate * n - deterministic_vol * deterministic_vol * n * n) * fixed_dt;
    }

}

double Option::parameter_b(int n) {
    if (stochastic_vol == 0 && stochastic_rate == 0 && fixed_step_time == 1) {
        return (deterministic_vol * deterministic_vol * n * n + deterministic_rate) * fixed_dt;
    }
}

double Option::parameter_c(int n) {
    if (stochastic_vol == 0 && stochastic_rate == 0 && fixed_step_time == 1) {
        return -0.5 * (deterministic_vol * deterministic_vol * n * n + deterministic_rate * n) * fixed_dt;
    }
}

Eigen::MatrixXd Option::implicit_matrix_vanilla() {
    double a;
    double b;
    double c;
    Eigen::MatrixXd computation_matrix;
    computation_matrix.resize(numdiff_s - 1, numdiff_s - 1);
    for (int row = 0; row < computation_matrix.rows(); ++row) {
        if (row == 0){
            b = parameter_b(row + 1);
            c = parameter_c(row + 1);
            computation_matrix(row,row) = 1 + b;
            computation_matrix(row,row + 1) = c;
        } else if (row == computation_matrix.rows() - 1) {
            b = parameter_b(row + 1);
            a = parameter_a(row + 1);
            computation_matrix(row, row) = 1 + b;
            computation_matrix(row, row - 1) = a;
        } else {
            a = parameter_a(row + 1);
            b = parameter_b(row + 1);
            c = parameter_c(row + 1);
            computation_matrix(row, row) = 1 + b;
            computation_matrix(row, row - 1) = a;
            computation_matrix(row, row + 1) = c;
        }
    }
    return computation_matrix;
}

void Option::fixed_difference_step(bool fixed_step_stock_, bool fixed_step_time_) {
    fixed_step_stock = fixed_step_stock_;
    fixed_step_time = fixed_step_time_;
    if (fixed_step_stock == 1 && fixed_step_time == 1){
        fixed_dt = tn / numdiff_t;
        fixed_ds = s_max / numdiff_s;
    }

    // Code of the step size is not fixed

}

void Option::compute_solution_grid(std::string method) {
    if (type_option == "Call"  || type_option == "Put" && european == 1){
        solution_grid = solution_grid_vanilla(method);
    }

}

void Option::create_boundary_condition_time(Eigen::VectorXd& boundary_vector) {

    if (fixed_step_time == 1 && fixed_step_stock == 1){
        for (int row=0; row < boundary_vector.rows(); ++row) {
            if (type_option == "Call")
                boundary_vector(row, 0) = std::max((row + 1) * fixed_ds - strike, 0.0);
            else if (type_option == "Put")
                boundary_vector(row, 0) = std::max(strike - (row + 1) * fixed_ds, 0.0);
        }
    }

}

Eigen::MatrixXd Option::solution_grid_vanilla(std::string method) {
    Eigen::MatrixXd solution_matrix;

    if (method == "Implicit"){
        // Implementation of the solution grid computation with Dirichlet boundary conditions
        if (stock_boundary_condition == "Dirichlet"){

            Eigen::MatrixXd implicit_matrix = implicit_matrix_vanilla().inverse();
            Eigen::VectorXd current_vt;
            Eigen::VectorXd previous_vt;

            Eigen::Index implicit_matrix_rows = implicit_matrix.rows();

            current_vt.resize(implicit_matrix_rows);
            create_boundary_condition_time(current_vt);
            previous_vt.resize(implicit_matrix_rows);

            Eigen::Index solution_matrix_cols = numdiff_t + 1;

            solution_matrix.resize(implicit_matrix_rows, solution_matrix_cols);

            Eigen::VectorXd adjustment_dirichlet = Eigen::VectorXd::Zero(implicit_matrix_rows);

            double a = parameter_a(1);
            double c = parameter_c(numdiff_s - 1);

            for (int t = 0; t < solution_matrix_cols; ++t) {
                // Add adjustment for Dirichlet
                if (type_option == "Call"){
                    adjustment_dirichlet(implicit_matrix_rows - 1,0) = c * (s_max - strike * exp( - deterministic_rate * (t - 1) * fixed_dt));
                    adjustment_dirichlet(0,0) = 0;
                } else if (type_option == "Put"){
                    adjustment_dirichlet(implicit_matrix_rows - 1,0) = 0;
                    adjustment_dirichlet(0,0) = a * strike * exp( - deterministic_rate * (t - 1) * fixed_dt);
                }

                // Perform the iteration
                if (t == 0) {
                    solution_matrix.col(solution_matrix_cols - 1) = current_vt;
                } else {
                    previous_vt = implicit_matrix * (current_vt - adjustment_dirichlet);
                    solution_matrix.col(solution_matrix_cols - t - 1) = previous_vt;
                    current_vt = previous_vt;
                }
            }

            Eigen::MatrixXd solution_matrix_adjusted;
            solution_matrix_adjusted.resize(implicit_matrix_rows + 2, solution_matrix_cols);
            Eigen::MatrixXd & ref_solution_matrix = solution_matrix;
            solution_matrix_adjusted.middleRows(1,implicit_matrix_rows) = ref_solution_matrix;

            for (int col = 0; col < solution_matrix_cols; ++col) {
                if (type_option == "Call"){
                    solution_matrix_adjusted(0,solution_matrix_cols - col - 1) = 0;
                    solution_matrix_adjusted(implicit_matrix_rows + 1, solution_matrix_cols - col - 1) = s_max - strike * exp( - deterministic_rate * col * fixed_dt);
                } else if (type_option == "Put") {
                    solution_matrix_adjusted(0,solution_matrix_cols - col - 1) = strike * exp( - deterministic_rate * col * fixed_dt);
                    solution_matrix_adjusted(implicit_matrix_rows + 1, solution_matrix_cols - col - 1) = 0;
                }
            }
            return solution_matrix_adjusted;
        }

        if (stock_boundary_condition == "Neumann"){

            double a = parameter_a(1);
            double c = parameter_c(numdiff_s - 1);

            Eigen::MatrixXd implicit_matrix = implicit_matrix_vanilla();
            Eigen::Index implicit_matrix_rows = implicit_matrix.rows();
            implicit_matrix(0,0) += 2 * a;
            implicit_matrix(0,1) -= a;

            implicit_matrix(implicit_matrix_rows - 1, implicit_matrix_rows - 1) += 2 * c;
            implicit_matrix(implicit_matrix_rows - 1, implicit_matrix_rows - 2) -= c;

            implicit_matrix = implicit_matrix.inverse();

            Eigen::VectorXd current_vt;
            current_vt.resize(implicit_matrix_rows);
            create_boundary_condition_time(current_vt);

            Eigen::VectorXd previous_vt;
            previous_vt.resize(implicit_matrix_rows);

            Eigen::Index solution_matrix_cols = numdiff_t + 1;

            solution_matrix.resize(implicit_matrix_rows, solution_matrix_cols);

            for (int t = 0; t < solution_matrix_cols; ++t) {
                // Perform the iteration
                if (t == 0) {
                    solution_matrix.col(solution_matrix_cols - 1) = current_vt;
                } else {
                    previous_vt = implicit_matrix * current_vt;
                    solution_matrix.col(solution_matrix_cols - t - 1) = previous_vt;
                    current_vt = previous_vt;
                }
            }

            Eigen::MatrixXd solution_matrix_adjusted;
            solution_matrix_adjusted.resize(implicit_matrix_rows + 2, solution_matrix_cols);
            Eigen::MatrixXd & ref_solution_matrix = solution_matrix;
            solution_matrix_adjusted.middleRows(1,implicit_matrix_rows) = ref_solution_matrix;

            for (int col = 0; col < solution_matrix_cols; ++col) {
                Eigen::Index column_number = solution_matrix_cols - col - 1;
                Eigen::Index last_row = implicit_matrix_rows + 1;
                solution_matrix_adjusted(0,column_number) = 2 * solution_matrix_adjusted(1,column_number)
                            - solution_matrix_adjusted(2,column_number);
                solution_matrix_adjusted(last_row, column_number) = 2 * solution_matrix_adjusted(last_row - 1, column_number)
                            - solution_matrix_adjusted(last_row - 2, column_number);

            }
            return solution_matrix_adjusted;
        }}

    if (method == "Crank-Nicholson"){
        if (stock_boundary_condition == "Dirichlet"){

            Eigen::MatrixXd implicit_matrix = implicit_matrix_vanilla();
            Eigen::MatrixXd implicit_matrix_cn = - implicit_matrix;
            Eigen::Index implicit_matrix_rows = implicit_matrix_cn.cols();
            Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(implicit_matrix_rows, implicit_matrix_rows);
            const Eigen::MatrixXd & ref_identity = identity;
            implicit_matrix_cn += 3 * ref_identity;
            implicit_matrix += ref_identity;
            implicit_matrix = implicit_matrix.inverse();

            Eigen::VectorXd current_vt;
            Eigen::VectorXd previous_vt;

            current_vt.resize(implicit_matrix_rows);
            create_boundary_condition_time(current_vt);
            previous_vt.resize(implicit_matrix_rows);

            Eigen::Index solution_matrix_cols = numdiff_t + 1;
            solution_matrix.resize(implicit_matrix_rows, solution_matrix_cols);

            Eigen::VectorXd adjustment_dirichlet = Eigen::VectorXd::Zero(implicit_matrix_rows);

            double a = parameter_a(1);
            double c = parameter_c(numdiff_s - 1);

            for (int t = 0; t < solution_matrix_cols; ++t) {
                // Add adjustment for Dirichlet
                if (type_option == "Call"){
                    adjustment_dirichlet(adjustment_dirichlet.rows() - 1,0) = c * (2 * s_max - strike * exp( - deterministic_rate * (t - 1) * fixed_dt)
                            - strike * exp( - deterministic_rate * std::max(t - 2,0) * fixed_dt));
                    adjustment_dirichlet(0,0) = 0;
                } else if (type_option == "Put"){
                    adjustment_dirichlet(adjustment_dirichlet.rows() - 1,0) = 0;
                    adjustment_dirichlet(0,0) = a * strike * ( exp( - deterministic_rate * (t - 1) * fixed_dt)
                            + exp( - deterministic_rate * std::max(t - 2,0) * fixed_dt));
                }

                // Perform the iteration
                if (t == 0) {
                    solution_matrix.col(solution_matrix_cols - 1) = current_vt;
                } else {
                    previous_vt = implicit_matrix * (implicit_matrix_cn * current_vt - adjustment_dirichlet);
                    solution_matrix.col(solution_matrix_cols - t - 1) = previous_vt;
                    current_vt = previous_vt;
                }
            }

            Eigen::MatrixXd solution_matrix_adjusted;
            solution_matrix_adjusted.resize(implicit_matrix_rows + 2, solution_matrix_cols);
            Eigen::MatrixXd & ref_solution_matrix = solution_matrix;
            solution_matrix_adjusted.middleRows(1,implicit_matrix_rows) = ref_solution_matrix;

            for (int col = 0; col < solution_matrix_cols; ++col) {
                if (type_option == "Call"){
                    solution_matrix_adjusted(0,solution_matrix_cols - col - 1) = 0;
                    solution_matrix_adjusted(implicit_matrix_rows + 1, solution_matrix_cols - col - 1) = s_max - strike * exp( - deterministic_rate * col * fixed_dt);
                } else if (type_option == "Put") {
                    solution_matrix_adjusted(0,solution_matrix_cols - col - 1) = strike * exp( - deterministic_rate * col * fixed_dt);
                    solution_matrix_adjusted(implicit_matrix_rows + 1, solution_matrix_cols - col - 1) = 0;
                }
            }
            return solution_matrix_adjusted;
        }
        if (stock_boundary_condition == "Neumann"){
            Eigen::MatrixXd implicit_matrix = implicit_matrix_vanilla();
            Eigen::MatrixXd implicit_matrix_cn = - implicit_matrix;
            Eigen::Index implicit_matrix_rows = implicit_matrix_cn.rows();
            Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(implicit_matrix_rows, implicit_matrix_rows);
            const Eigen::MatrixXd & ref_identity = identity;
            implicit_matrix_cn += 3 * ref_identity;
            implicit_matrix += ref_identity;

            double a = parameter_a(1);
            double c = parameter_c(numdiff_s - 1);

            implicit_matrix(0,0) += 2 * a;
            implicit_matrix(0,1) -= a;

            implicit_matrix(implicit_matrix.rows() - 1, implicit_matrix.cols() - 1) += 2 * c;
            implicit_matrix(implicit_matrix.rows() - 1, implicit_matrix.cols() - 2) -= c;

            implicit_matrix_cn(0,0) -= 2 * a;
            implicit_matrix_cn(0,1) += a;

            implicit_matrix_cn(implicit_matrix.rows() - 1, implicit_matrix.cols() - 1) -= 2 * c;
            implicit_matrix_cn(implicit_matrix.rows() - 1, implicit_matrix.cols() - 2) += c;

            implicit_matrix = implicit_matrix.inverse();

            Eigen::VectorXd current_vt;
            current_vt.resize(implicit_matrix_rows);
            create_boundary_condition_time(current_vt);

            Eigen::VectorXd previous_vt;
            previous_vt.resize(implicit_matrix_rows);

            Eigen::Index solution_matrix_cols = numdiff_t + 1;
            solution_matrix.resize(implicit_matrix_rows, solution_matrix_cols);

            for (int t = 0; t < solution_matrix_cols; ++t) {
                // Perform the iteration
                if (t == 0) {
                    solution_matrix.col(solution_matrix_cols - 1) = current_vt;
                } else {
                    previous_vt = implicit_matrix * (implicit_matrix_cn * current_vt);
                    solution_matrix.col(solution_matrix_cols - t - 1) = previous_vt;
                    current_vt = previous_vt;
                }
            }

            Eigen::MatrixXd solution_matrix_adjusted;

            solution_matrix_adjusted.resize(implicit_matrix_rows + 2, solution_matrix_cols);
            Eigen::MatrixXd & ref_solution_matrix = solution_matrix;
            solution_matrix_adjusted.middleRows(1,implicit_matrix_rows) = ref_solution_matrix;

            for (int col = 0; col < solution_matrix_cols; ++col) {
                Eigen::Index column_number = solution_matrix_cols - col - 1;
                Eigen::Index last_row = implicit_matrix_rows + 1;
                solution_matrix_adjusted(0,column_number) = 2 * solution_matrix_adjusted(1,column_number)
                                                            - solution_matrix_adjusted(2,column_number);
                solution_matrix_adjusted(last_row, column_number) = 2 * solution_matrix_adjusted(last_row - 1, column_number)
                                                                    - solution_matrix_adjusted(last_row - 2, column_number);
            }
            return solution_matrix_adjusted;
        }

    }

}

void Option::print_solution_grid(int precision) {
    std::cout << "Type of Option: " + type_option + "\n";
    std::cout << "Stock boundary condition: " + stock_boundary_condition + "\n\n";
    std::cout << "Legend of the matrix: s/t 1 2 . . T\n";
    std::cout << "                      0   x x x x x\n";
    std::cout << "                      1   x x x x x\n";
    std::cout << "                      .   x x x x x\n";
    std::cout << "                      .   x x x x x\n";
    std::cout << "                  S_max   x x x x x\n\n";
    std::cout << "Printing the solution grid:\n\n";

    Eigen::MatrixXd printing_matrix;
    printing_matrix.resize(solution_grid.rows() + 1, solution_grid.cols() + 1);
    printing_matrix.bottomRightCorner(solution_grid.rows(),solution_grid.cols()) = solution_grid;

    if (fixed_step_time == 1){
        for (int col = 1; col < printing_matrix.cols(); ++col) {
            printing_matrix(0, col) = fixed_dt * (col - 1);
        }

    }

    if (fixed_step_stock == 1){
        for (int row = 1; row < printing_matrix.rows(); ++row) {
            printing_matrix(row, 0) = fixed_ds * (row - 1);
        }
    }

    std::streamsize prec = std::cout.precision();
    std::cout << std::setprecision(precision) << printing_matrix << std::setprecision(prec) << "\n";
}


#include <doctest/doctest.h>

#define EIGEN_INITIALIZE_MATRICES_BY_ZERO

#include <Eigen/Core>
#include <ode/ode.hpp>

TEST_CASE("Adaptive Test")
{
  using method_type             = ode::explicit_method<ode::dormand_prince_5_tableau<float>>;
  using problem_type            = ode::initial_value_problem<float, Eigen::Vector3f>;
  using i_controller_iterator   = ode::adaptive_step_iterator<method_type, problem_type, ode::integral_controller                        <method_type, problem_type>>;
  using pi_controller_iterator  = ode::adaptive_step_iterator<method_type, problem_type, ode::proportional_integral_controller           <method_type, problem_type>>;
  using pid_controller_iterator = ode::adaptive_step_iterator<method_type, problem_type, ode::proportional_integral_derivative_controller<method_type, problem_type>>;

  constexpr auto sigma   = 10.0f;
  constexpr auto rho     = 28.0f;
  constexpr auto beta    = 8.0f / 3.0f;
  const     auto problem = problem_type
  {
    0.0f,                                         /* t0 */
    Eigen::Vector3f(16.0f, 16.0f, 16.0f),         /* y0 */
    [&] (const float t, const Eigen::Vector3f& y) /* y' = f(t, y) */
    {
      return Eigen::Vector3f(sigma * (y[1] - y[0]), y[0] * (rho - y[2]) - y[1], y[0] * y[1] - beta * y[2]); /* Lorenz system */
    }
  };

  auto iterator_1 = i_controller_iterator  (problem, 1.0f /* h */);
  for (auto i = 0; i < 10000; ++i)
    ++iterator_1;

  auto iterator_2 = pi_controller_iterator (problem, 1.0f /* h */);
  for (auto i = 0; i < 10000; ++i)
    ++iterator_2;

  auto iterator_3 = pid_controller_iterator(problem, 1.0f /* h */);
  for (auto i = 0; i < 10000; ++i)
    ++iterator_3;
}


#include<stdio.h>
#include<cstring>
#include<iostream>
#include "Eigen/Core"
#include "Eigen/Dense"
#include "Eigen/Sparse"
#include "Eigen/SVD"
#include <Eigen/Eigenvalues>
#include <unsupported/Eigen/SparseExtra>
#include <unsupported/Eigen/KroneckerProduct>

using namespace Eigen;
std :: string filename = "D:/sydney/first/data/tester_ (";
MatrixXf A,result;
const int nShape = 47;
const int nVerts = 11510;
const int nFaces = 11540;
const int test_num = 77;
const int iden_num = 10;
std::vector<int> mouse_edge[1000];
using namespace std;
//int cnt_vtx[nVerts];
void smooth_mesh(MatrixX3f &mesh, int iteration, std::vector<int> *mouse_edge) {
	while (iteration--)
	{
		MatrixX3f temp = mesh;

		for (int i_e = 0; mouse_edge[i_e].size() >0; i_e++) {
			if (mouse_edge[i_e].size() < 6) continue;
			int v = mouse_edge[i_e][0];
			mesh.block(v, 0, 1, 3).setZero();
			for (int j = 1; j < mouse_edge[i_e].size(); j++)
				mesh.row(v).array() += temp.row(mouse_edge[i_e][j]).array() / (mouse_edge[i_e].size() - 1);
		}
	}
}
void load_smooth_edge(std::vector<int> *mouse_edge) {
	puts("loading mouse edge...");
	FILE *fp;
	fopen_s(&fp, "D:\\sydney\\first\\code\\2017\\cal_coeffience_Q_M_u_e_3\\cal_coeffience_Q_M_u_e_3/mouse_point.txt", "r");
	int n;
	fscanf_s(fp, "%d", &n);
	for (int i = 0; i < n; i++) {
		mouse_edge[i].clear();
		int t, num;
		fscanf_s(fp, "%d%d", &t, &num);
		mouse_edge[i].push_back(t);
		for (int j = 0; j < num; j++) {
			fscanf_s(fp, "%d", &t);
			mouse_edge[i].push_back(t);
		}
	}
	fclose(fp);
}

int main() {
	A.resize(test_num, nShape*nVerts * 3);
	for (int i = 0; i < test_num; i++) {
		std::string name = filename + std::to_string(i + 1) + ")/Blendshape/shape.bs";
		std :: cout << name << std :: endl;
		FILE *fp;
		fopen_s(&fp,name.c_str(), "rb");
		int nShapes = 0, nVerts = 0, nFaces = 0;
		fread(&nShapes, sizeof(int), 1, fp);			// nShape = 46
		fread(&nVerts, sizeof(int), 1, fp);			// nVerts = 11510
		fread(&nFaces, sizeof(int), 1, fp);			// nFaces = 11540
		printf("%d %d %d\n", nShapes, nVerts, nFaces);

													// Load neutral expression B_0
		float temp;
		for (int j = 0; j < nVerts * 3; j++) {
			fread(&temp, sizeof(float), 1, fp);
			A(i, j) = temp;
		}
		//for (int j = 0; j < 10; j++)
		//	printf("%.10f ", A(i, j));
		//puts("");

		// Load other expressions B_i ( 1 <= i <= 46 )
		for (int exprId = 0; exprId < nShapes; exprId++)
			for (int j = 0; j < nVerts * 3; j++) {
				fread(&temp, sizeof(float), 1, fp);
				A(i, 3 * nVerts*(exprId + 1) + j) = temp;
			}

		fclose(fp);
	}
	puts("loading initial bldshps complete!...");
	load_smooth_edge(mouse_edge);
	for (int i_id=0;i_id< iden_num;i_id++)
		for (int i_exp = 0; i_exp < nShape; i_exp++) {
			printf("smoothing id:%d exp:%d\n",i_id,i_exp);
			Eigen::MatrixX3f temp(nVerts, 3);
			for (int i_v = 0; i_v < nVerts; i_v++)
				for (int axis = 0; axis < 3; axis++)
					temp(i_v, axis) = A(i_id, i_exp*nVerts*3 + i_v * 3 + axis);
			smooth_mesh(temp, 25, mouse_edge);
			for (int i_v = 0; i_v < nVerts; i_v++)
				for (int axis = 0; axis < 3; axis++)
					A(i_id, i_exp*nVerts * 3 + i_v * 3 + axis) = temp(i_v, axis);
		}

	puts("saving...");
	FILE *fp;
	fopen_s(&fp, "blendshape_ide_svd_77_ite25_bound.lv", "wb");
	for (int i = 0; i < iden_num; i++)
		for (int j = 0; j < 3 * nVerts*nShape; j++)
			fwrite(&A(i, j), sizeof(float), 1, fp);
	fclose(fp);

	//MatrixXf mean = A.rowwise().mean();
	//cout << mean.rows() << ' ' << mean.cols() << '\n';
	//cout << mean;
	//cout << A(32, 22301) << "\n";
	//cout << A(24, 11144) << "\n";
	//cout << A(53, 32333) << "\n";
	//cout << A(60, 523441) << "\n";
	//cout << A(8, 765201) << "\n";

	//system("pause");
	//VectorXf vmean = mean;
	//A.colwise() -= vmean;
	//BDCSVD<Eigen::MatrixXf> svd((A * A.transpose()).array()/ A.cols(), ComputeThinU);//*A.transpose()
	//puts("asd");
	////printf("%d %d %d %d\n", U.rows(), U.cols(), V.rows(), V.cols());
	////cout <<"U:\n"<< U << endl;
	////cout << "V:\n" << V << endl;
	//MatrixXf  S = svd.singularValues();
	////printf("%d %d\n",S.cols(),S.rows());
	//float tot = 0,temp=0;
	//for (int i = 0; i < test_num; i++) tot += sqrt(S(i));
	//for (int i = 0; i < test_num; i++, puts("")) {
	//	temp += sqrt(S(i));
	//	printf("%d %.10f %.2f", i, sqrt(S(i)), temp / tot * 100);
	//}
	//A.colwise() += vmean;
	//result = svd.matrixU().block(0, 0, test_num, iden_num).transpose()*A;
	//FILE *fp;
	//fopen_s(&fp, "blendshape_ide_svd_50.lv", "wb");
	//for (int i = 0; i < iden_num; i++)
	//	for (int j = 0; j < 3 * nVerts*nShape; j++)
	//		fwrite(&result(i,j),sizeof(float),1,fp);
	//fclose(fp);
	//fopen_s(&fp, "blendshape_ide_svd_value_sqrt_50.txt", "w");
	//for (int i = 0; i < iden_num; i++)
	//	fprintf(fp, "%.10f\n", sqrt(S(i)));
	//fclose(fp);

	system("pause");
	return 0;
}

/*
test svd

MatrixXf m = MatrixXf::Random(3, 2);
m.resize(3, 2);
cout << "Here is the matrix m:" << endl << m << endl;
JacobiSVD<MatrixXf> svd(m, ComputeThinU | ComputeThinV);
cout << "Its singular values are:" << endl << svd.singularValues() << endl;
cout << "Its left singular vectors are the columns of the thin U matrix:" << endl << svd.matrixU() << endl;
cout << "Its right singular vectors are the columns of the thin V matrix:" << endl << svd.matrixV() << endl;*/


// Author: Dai Wei (wdai@cs.cmu.edu), Pengtao Xie (pxie@cs.cmu.edu)
// Date: 2014.10.21

#include <ml/util/math_util.hpp>
#include <ml/util/fastapprox/fastapprox.hpp>
#include <glog/logging.h>
#include <cmath>
#include <sstream>
#include <Eigen/Dense>

namespace petuum {
namespace ml {

namespace {

const float kCutoff = 1e-15;

}  // anonymous namespace

float SafeLog(float x) {
  if (std::abs(x) < kCutoff) {
    x = kCutoff;
  }
  return fastlog(x);
}

float Sigmoid(float x) {
  return 1. / (1. + exp(-x));
}

float LogSum(float log_a, float log_b) {
  return (log_a < log_b) ? log_b + fastlog(1 + fastexp(log_a - log_b)) :
    log_a + fastlog(1 + fastexp(log_b-log_a));
}

float LogSumVec(const std::vector<float>& logvec) {
	float sum = 0.;
	sum = logvec[0];
	for (int i = 1; i < logvec.size(); ++i) {
		sum = LogSum(sum, logvec[i]);
	}
	return sum;
}

void Softmax(std::vector<float>* vec) {
  CHECK_NOTNULL(vec);
  // TODO(wdai): Figure out why this is necessary. Doubt it is.
	for (int i = 0; i < vec->size(); ++i) {
		if (std::abs((*vec)[i]) < kCutoff) {
			(*vec)[i] = kCutoff;
    }
	}
	double lsum = LogSumVec(*vec);
	for (int i = 0; i < vec->size(); ++i) {
		(*vec)[i] = fastexp((*vec)[i] - lsum);
		//(*vec)[i] = exp((*vec)[i] - lsum);
    (*vec)[i] = (*vec)[i] > 1 ? 1. : (*vec)[i];
  }
}

float DenseDenseFeatureDotProduct(const AbstractFeature<float>& f1,
    const AbstractFeature<float>& f2) {
  CHECK_EQ(f1.GetFeatureDim(), f2.GetFeatureDim());
  auto f1_dense_ptr = static_cast<const DenseFeature<float>*>(&f1);
  auto f2_dense_ptr = static_cast<const DenseFeature<float>*>(&f2);
  const std::vector<float>& v1 = f1_dense_ptr->GetVector();
  const std::vector<float>& v2 = f2_dense_ptr->GetVector();
  Eigen::Map<const Eigen::VectorXf> e1(v1.data(), v1.size());
  Eigen::Map<const Eigen::VectorXf> e2(v2.data(), v2.size());
  return e1.dot(e2);
}

float SparseDenseFeatureDotProduct(const AbstractFeature<float>& f1,
    const AbstractFeature<float>& f2) {
  CHECK_EQ(f1.GetFeatureDim(), f2.GetFeatureDim());
  float sum = 0.;
  for (int i = 0; i < f1.GetNumEntries(); ++i) {
    int32_t f1_fid = f1.GetFeatureId(i);
    sum += f1.GetFeatureVal(i) * f2[f1_fid];
  }
  return sum;
}

float DenseSparseFeatureDotProduct(const AbstractFeature<float>& f1,
    const AbstractFeature<float>& f2) {
  return SparseDenseFeatureDotProduct(f2, f1);
}

float SparseSparseFeatureDotProduct(const AbstractFeature<float>& f1,
    const AbstractFeature<float>& f2) {
  CHECK_EQ(f1.GetFeatureDim(), f2.GetFeatureDim());
  int j = 0;
  float sum = 0.;
  int f2_num_entries = f2.GetNumEntries();
  for (int i = 0; i < f1.GetNumEntries() && j < f2_num_entries; ++i) {
    int32_t f1_fid = f1.GetFeatureId(i);
    while (f2.GetFeatureId(j) < f1_fid && j < f2_num_entries) {
      ++j;
    }
    if (f1_fid == f2.GetFeatureId(j)) {
      sum += f1.GetFeatureVal(i) * f2.GetFeatureVal(j);
    }
  }
  return sum;
}

void FeatureScaleAndAdd(float alpha, const DenseFeature<float>& f1,
    DenseFeature<float>* f2) {
  CHECK_EQ(f1.GetFeatureDim(), f2->GetFeatureDim());
  const std::vector<float>& f1_vec = f1.GetVector();
  std::vector<float>& f2_vec = f2->GetVector();
  for (int i = 0; i < f1_vec.size(); ++i) {
    f2_vec[i] += alpha * f1_vec[i];
  }
}

// f1 sparse, f2 dense.
void FeatureScaleAndAdd(float alpha, const AbstractFeature<float>& f1,
    AbstractFeature<float>* f2) {
  CHECK_EQ(f1.GetFeatureDim(), f2->GetFeatureDim());
  for (int i = 0; i < f1.GetNumEntries(); ++i) {
    int32_t f1_fid = f1.GetFeatureId(i);
    f2->SetFeatureVal(f1_fid, alpha * f1.GetFeatureVal(i) + (*f2)[f1_fid]);
  }
}

}  // namespace ml
}  // namespace petuum


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* 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 ROKKO_UTILITY_HELMERT_MATRIX_HPP
#define ROKKO_UTILITY_HELMERT_MATRIX_HPP

#include <cmath>
#include <stdexcept>
#include <boost/throw_exception.hpp>
#include <rokko/config.h>
#include <rokko/localized_matrix.hpp>
#if defined(ROKKO_HAVE_PARALLEL_DENSE_SOLVER)
# include <rokko/distributed_matrix.hpp>
#endif

namespace rokko {

class helmert_matrix {
public:
  template<typename T, typename MATRIX_MAJOR>
  static void generate(rokko::localized_matrix<T, MATRIX_MAJOR>& mat) {
    if (mat.rows() != mat.cols())
      BOOST_THROW_EXCEPTION(std::invalid_argument("helmert_matrix::generate() : non-square matrix"));
    int n = mat.rows();
    mat.row(0).fill( 1 / sqrt(n) );
    for (int i=1; i < mat.rows(); ++i) {
      mat.row(i).head(i).fill( 1 / sqrt(static_cast<T>(i*(i+1))) );
      mat(i,i) = - sqrt(static_cast<T>(i)/(i+1));
    }
  }

  template<typename T, typename MATRIX_MAJOR>
  static void generate_for_given_eigenvalues(rokko::localized_matrix<T, MATRIX_MAJOR>& mat, rokko::localized_vector<T> const& diag) {
    if (mat.rows() != mat.cols())
      BOOST_THROW_EXCEPTION(std::invalid_argument("helmert_matrix::generate() : non-square matrix"));
    int n = mat.rows();
    for (int i=0; i<n; ++i) {
      double common_elem = diag(0) / n;
      for (int k=i+1; k<n; ++k)
	common_elem += diag(k) / (k*(k+1));  // Remark: i=max(i,j)
      mat(i, i) = common_elem + i * diag(i) / (i+1);
      double val = common_elem - diag(i) / (i+1);
      mat.row(i).head(i).setConstant(val);
      mat.col(i).head(i).setConstant(val);
    }
  }

#if defined(ROKKO_HAVE_PARALLEL_DENSE_SOLVER)
  template<typename T, typename MATRIX_MAJOR>
  static void generate(rokko::distributed_matrix<T, MATRIX_MAJOR>& mat) {
    if (mat.get_m_global() != mat.get_n_global())
      BOOST_THROW_EXCEPTION(std::invalid_argument("helmert_matrix::generate() : non-square matrix"));
    const int n = mat.get_m_global();
    int start_i;
    if (mat.is_gindex_myrow(0)) {
      T val = 1 / sqrt(n);
      for(int local_j = 0; local_j < mat.get_n_local(); ++local_j)
        mat.set_local(0, local_j, val);
      start_i = 1;
    }
    else start_i = 0;

    for(int local_i = start_i; local_i < mat.get_m_local(); ++local_i) {
      int global_i = mat.translate_l2g_row(local_i);
      T val = 1 / sqrt(static_cast<T>(global_i*(global_i+1)));
      for(int local_j = 0; local_j < mat.get_n_local(); ++local_j) {
        int global_j = mat.translate_l2g_col(local_j);
	if (global_j < global_i) mat.set_local(local_i, local_j, val);
	else if (global_j == global_i) mat.set_local(local_i, local_j, - sqrt(static_cast<T>(global_i)/(global_i+1)));
      }
    }
  }

  /*
  // another (slower) implementation using set_global function
  template<typename T, typename MATRIX_MAJOR>
  static void generate_global(rokko::distributed_matrix<MATRIX_MAJOR>& mat) {
    if (mat.m_global != mat.n_global)
      BOOST_THROW_EXCEPTION(std::invalid_argument("helmert_matrix::generate() : non-square matrix"));
    for(int global_i=0; global_i<mat.m_global; ++global_i) {
      for(int global_j=0; global_j<mat.n_global; ++global_j) {
        mat.set_global(global_i, global_j, mat.m_global - std::max(global_i, global_j) );
      }
    }
  }
  */

  template<typename T, typename MATRIX_MAJOR>
  static void generate_for_given_eigenvalues(rokko::distributed_matrix<T, MATRIX_MAJOR>& mat, rokko::localized_vector<T> const& diag) {
    if (mat.get_m_global() != mat.get_n_global())
      BOOST_THROW_EXCEPTION(std::invalid_argument("helmert_matrix::generate() : non-square matrix"));
    const int n = mat.get_m_global();

    for(int local_i = 0; local_i < mat.get_m_local(); ++local_i) {
      int global_i = mat.translate_l2g_row(local_i);
      double common_elem = diag(0) / n;
      for (int k=global_i+1; k<n; ++k)
	common_elem += diag(k) / (k*(k+1));  // Remark: i=max(i,j)
      T val = common_elem - diag(global_i) / (global_i+1);
      for(int local_j = 0; local_j < mat.get_n_local(); ++local_j) {
        int global_j = mat.translate_l2g_col(local_j);
	if (global_j < global_i) mat.set_local(local_i, local_j, val);
	else if (global_j == global_i) mat.set_local(local_i, local_j, common_elem + global_i * diag(global_i) / (global_i+1));
      }
    }

    for(int local_j = 0; local_j < mat.get_n_local(); ++local_j) {
      int global_j = mat.translate_l2g_col(local_j);
      double common_elem = diag(0) / n;
      for (int k=global_j+1; k<n; ++k)
	common_elem += diag(k) / (k*(k+1));  // Remark: i=max(i,j)
      T val = common_elem - diag(global_j) / (global_j+1);
      for(int local_i = 0; local_i < mat.get_m_local(); ++local_i) {
        int global_i = mat.translate_l2g_row(local_i);
	if (global_i < global_j) mat.set_local(local_i, local_j, val);
      }
    }
  }
#endif
};

} // namespace rokko

#endif // ROKKO_UTILITY_HELMERT_MATRIX_HPP


/*
 [auto_generated]
 boost/numeric/odeint/stepper/runge_kutta4.hpp

 [begin_description]
 Implementation of the classical Runge-Kutta stepper with the generic stepper.
 [end_description]

 Copyright 2011-2013 Mario Mulansky
 Copyright 2011-2013 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 BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA4_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA4_HPP_INCLUDED

#include <boost/fusion/container/vector.hpp>
#include <boost/fusion/container/generation/make_vector.hpp>

#include <boost/numeric/odeint/stepper/explicit_generic_rk.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>

#include <boost/array.hpp>

#include <boost/numeric/odeint/util/resizer.hpp>

namespace boost {
namespace numeric {
namespace odeint {

#ifndef DOXYGEN_SKIP
template< class Value = double >
struct rk4_coefficients_a1 : boost::array< Value , 1 >
{
    rk4_coefficients_a1( void )
    {
        (*this)[0] = static_cast< Value >( 1 ) / static_cast< Value >( 2 );
    }
};

template< class Value = double >
struct rk4_coefficients_a2 : boost::array< Value , 2 >
{
    rk4_coefficients_a2( void )
    {
        (*this)[0] = static_cast<Value>(0);
        (*this)[1] = static_cast< Value >( 1 ) / static_cast< Value >( 2 );
    }
};

template< class Value = double >
struct rk4_coefficients_a3 : boost::array< Value , 3 >
{
    rk4_coefficients_a3( void )
            {
        (*this)[0] = static_cast<Value>(0);
        (*this)[1] = static_cast<Value>(0);
        (*this)[2] = static_cast<Value>(1);
            }
};

template< class Value = double >
struct rk4_coefficients_b : boost::array< Value , 4 >
{
    rk4_coefficients_b( void )
    {
        (*this)[0] = static_cast<Value>(1)/static_cast<Value>(6);
        (*this)[1] = static_cast<Value>(1)/static_cast<Value>(3);
        (*this)[2] = static_cast<Value>(1)/static_cast<Value>(3);
        (*this)[3] = static_cast<Value>(1)/static_cast<Value>(6);
    }
};

template< class Value = double >
struct rk4_coefficients_c : boost::array< Value , 4 >
{
    rk4_coefficients_c( void )
    {
        (*this)[0] = static_cast<Value>(0);
        (*this)[1] = static_cast< Value >( 1 ) / static_cast< Value >( 2 );
        (*this)[2] = static_cast< Value >( 1 ) / static_cast< Value >( 2 );
        (*this)[3] = static_cast<Value>(1);
    }
};
#endif

template<
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = typename algebra_dispatcher< State >::algebra_type ,
class Operations = typename operations_dispatcher< State >::operations_type ,
class Resizer = initially_resizer
>
#ifndef DOXYGEN_SKIP
class runge_kutta4 : public explicit_generic_rk< 4 , 4 , State , Value , Deriv , Time ,
Algebra , Operations , Resizer >
#else
class runge_kutta4 : public explicit_generic_rk
#endif
{

public:

#ifndef DOXYGEN_SKIP
    typedef explicit_generic_rk< 4 , 4 , State , Value , Deriv , Time ,
            Algebra , Operations , Resizer > stepper_base_type;
#endif
    typedef typename stepper_base_type::state_type state_type;
    typedef typename stepper_base_type::value_type value_type;
    typedef typename stepper_base_type::deriv_type deriv_type;
    typedef typename stepper_base_type::time_type time_type;
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::operations_type operations_type;
    typedef typename stepper_base_type::resizer_type resizer_type;

    #ifndef DOXYGEN_SKIP
    typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
    typedef typename stepper_base_type::stepper_type stepper_type;
    #endif

    runge_kutta4( const algebra_type &algebra = algebra_type() ) : stepper_base_type(
            boost::fusion::make_vector( rk4_coefficients_a1<Value>() , rk4_coefficients_a2<Value>() , rk4_coefficients_a3<Value>() ) ,
            rk4_coefficients_b<Value>() , rk4_coefficients_c<Value>() , algebra )
    { }

};

/**
 * \class runge_kutta4
 * \brief The classical Runge-Kutta stepper of fourth order.
 *
 * The Runge-Kutta method of fourth order is one standard method for
 * solving ordinary differential equations and is widely used, see also
 * en.wikipedia.org/wiki/Runge-Kutta_methods
 * The method is  explicit and fulfills the Stepper concept. Step size control
 * or continuous output are not provided.
 *
 * This class derives from explicit_stepper_base and inherits its interface via CRTP (current recurring template pattern).
 * Furthermore, it derivs from explicit_generic_rk which is a generic Runge-Kutta algorithm. For more details see
 * explicit_stepper_base and explicit_generic_rk.
 *
 * \tparam State The state type.
 * \tparam Value The value type.
 * \tparam Deriv The type representing the time derivative of the state.
 * \tparam Time The time representing the independent variable - the time.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

/**
 * \fn runge_kutta4::runge_kutta4( const algebra_type &algebra = algebra_type() )
 * \brief Constructs the runge_kutta4 class. This constructor can be used as a default
 * constructor if the algebra has a default constructor.
 * \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
 */

}
}
}

#endif // BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA4_HPP_INCLUDED


#pragma once

#include "matrix/assembly/velocity_var_form.hpp"

#include <Eigen/Sparse>

namespace boltzmann {

// ----------------------------------------------------------------------
template <typename TEST_BASIS, typename TRIAL_BASIS>
void
make_mass_matrix(Eigen::SparseMatrix<double>& dst,
                 const TEST_BASIS& test_basis,
                 const TRIAL_BASIS& trial_basis,
                 const double beta = 2)
{
  VelocityVarForm<2> velocity_var_form;
  velocity_var_form.init(test_basis, trial_basis, beta);
  const auto& s0 = velocity_var_form.get_s0();

  for (auto it = s0.begin(); it != s0.end(); ++it) {
    int i = it->row;
    int j = it->col;
    double val = it->val;
    dst.insert(i, j) = val;
  }
}

template <typename TEST_BASIS, typename TRIAL_BASIS>
Eigen::SparseMatrix<double>
make_mass_matrix(const TEST_BASIS& test_basis, const TRIAL_BASIS& trial_basis)
{
  assert(test_basis.n_dofs() == trial_basis.n_dofs());
  int N = test_basis.n_dofs();

  Eigen::SparseMatrix<double> M(N, N);
  VelocityVarForm<2> velocity_var_form;
  velocity_var_form.init(test_basis, trial_basis, 2.0);
  const auto& s0 = velocity_var_form.get_s0();

  for (auto it = s0.begin(); it != s0.end(); ++it) {
    int i = it->row;
    int j = it->col;
    double val = it->val;
    M.insert(i, j) = val;
  }

  return M;
}

template <typename TRIAL_BASIS>
Eigen::VectorXd
make_mass_vdiag(const TRIAL_BASIS& basis)
{
  int N = basis.n_dofs();

  Eigen::VectorXd out(N);
  VelocityVarForm<2> velocity_var_form;
  velocity_var_form.init(basis, basis, 2.0);
  const auto& s0 = velocity_var_form.get_s0();

  for (auto it = s0.begin(); it != s0.end(); ++it) {
    int i = it->row;
    int j = it->col;

    if (i != j) throw std::runtime_error("make_mass_vdiag: not diagonal!");
    double val = it->val;
    out[i] = val;
  }

  return out;
}

}  // end namespace boltzmann


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// ProductTools.hpp
// This file is part of the Garamon Generator.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file ProductTools.hpp
/// \author Vincent Nozick, Stephane Breuils
/// \brief Geometric algebra "naive" implementation tools dedicated to build the optimized C++ products.

#ifndef GARAGEN_PRODUCTTOOLS_HPP
#define GARAGEN_PRODUCTTOOLS_HPP

#include <vector>
#include <array>
#include <map>
#include <utility>     // pairs
#include <Eigen/Dense> // use the metric which is dense
#include <Eigen/Sparse> // use the transformation matrices which are sparse
#include <iostream>
#if defined(_MSC_BUILD)
	#ifndef __builtin_popcount
		#define __builtin_popcount __popcnt
	#endif
#endif // __WINDOWS MSVC compiler__

/// In the computation of mv3 = mv1^mv2, this represents a quadruplet containing the indices of mv1,mv2,mv3 and a coefficient in a product between two blades.
/// e.g. suppose that a product is defined by mv3[2] += -3.0*mv1[3]*mv2[4]  then the first quadruplet associated with this product will be 3,4,1,-3.0
template<typename T>
struct productComponent{
    unsigned int indexOfMv1; // in the example above, indexOfMv1 would be 3
    unsigned int indexOfMv2; // in the example above, indexOfMv2 would be 4
    unsigned int indexOfMv3; // in the example above, indexOfMv2 would be 4
    T coefficient;           // in the example above, coefficient would be -1
};

template<typename C>
bool compareProductComponents(const C &a, const C &b)
//bool operator < (const productComponent &a, const productComponent &b) const
{
    // first compare on indexOfMv3
    if(a.indexOfMv3 > b.indexOfMv3) return false;
    if(a.indexOfMv3 < b.indexOfMv3) return true;

    // second compare on indexOfMv1
    if(a.indexOfMv1 > b.indexOfMv1) return false;
    if(a.indexOfMv1 < b.indexOfMv1) return true;

    // thrid compare on indexOfMv2
    if(a.indexOfMv2 > b.indexOfMv2) return false;
    if(a.indexOfMv2 < b.indexOfMv2) return true;

    return true;
}

class ProductTools {
public:

    /// constructor
    /// \param vectorSpaceDimension is the dimension of the vector space of the algebra
    ProductTools(const unsigned int vectorSpaceDimension);

    /// destructor
    ~ProductTools();

    /// \brief compute the Hamming weight of the xor index (blade) xorIndexMv (i.e the number of bits=1 in the binary value xorIndexMv)
    /// \param xorIndexMv is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    static unsigned int hammingWeight(const unsigned int xorIndexMv);

    /// \brief return whether the outer product between the two blades whose indices are xorIndexMv1 and xorIndexMv2 is a null blade
    /// \param xorIndexMv1 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    /// \param xorIndexMv2 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    static bool outerProductExists(const unsigned int xorIndexMv1, const unsigned int xorIndexMv2);

    /// \brief return the sign of the outer product between the two blades whose indices are xorIndexMv1 and xorIndexMv2
    /// \param xorIndexMv1 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    /// \param xorIndexMv2 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    static int outerProductSign(const unsigned int xorIndexMv1, const unsigned int xorIndexMv2);

    /// \brief return whether the inner product exists
    /// \param xorIndexMv1 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    /// \param xorIndexMv2 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    static bool innerProductExists(const unsigned int xorIndexMv1, const unsigned int xorIndexMv2);

    /// \brief Compute the coefficient required in the computation of the inner and geometric products of 2 multivectors.
    /// \param xorIndexMv1 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebr  in the algebra related orthogonal space
    /// \param xorIndexMv2 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra in the algebra related orthogonal space
    /// \param diagonalMetric defines the metric in the algebra related orthogonal space
    static double productCoefficientFromMetric(const unsigned int xorIndexMv1, const unsigned int xorIndexMv2,
                                               const Eigen::VectorXd &diagonalMetric);

    /// \brief Compute the position of the product between two XOR indices. Is common for the geometric, outer and inner product
    /// \param xorIndexMv1 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    /// \param xorIndexMv2 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    static unsigned int productResultXorIndex(const unsigned int xorIndexMv1, const unsigned int xorIndexMv2);

    /// \brief Generate all the components of the product mv1{gradeMv1} ^ mv2{gradeMv2}.
    /// \param xorIndexMv1 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebr  in the algebra related orthogonal space
    /// \param xorIndexMv2 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra in the algebra related orthogonal space
    /// \return For each component of mv3=mv1^mv2 with grade gradeMv3=gradeMv1+gradeMv2, we return a list of quadruplet (indexMv1,indexMv2,indexMv3,coefficient).
    std::list<productComponent<double>> generateExplicitOuterProductList(const unsigned int gradeMv1, const unsigned int gradeMv2) const;

    /// \brief Generate all the components of the inner product mv1<gradeMv1> . mv2<gradeMv2> in a Euclidean space with diagonal metric. In practice, we compute here the Hestenes inner product.
    /// \param gradeMv1 the grade of the homogeneous multivector Mv1 to be considered
    /// \param gradeMv2 the grade of the homogeneous multivector Mv2 to be considered
    /// \param diagonalMetric defines the metric in the algebra related orthogonal space
    /// \return for each component of the result mv3=mv1.mv2 with grade gradeMv3=|gradeMv1-gradeMv2|, we return a list of quadruplet (indexMv1,indexMv2,indexMv3,coefficient).
    std::list<productComponent<double>> generateExplicitInnerProductListEuclideanSpace(const unsigned int gradeMv1,
                                                                                const unsigned int gradeMv2,
                                                                                const Eigen::VectorXd &diagonalMetric) const;

    /// \brief Generate all the components of the inner product mv1<gradeMv1> . mv2<gradeMv2>. In practice, we compute here the Hestenes inner product.
    /// \param gradeMv1 the grade of the homogeneous multivector Mv1 to be considered
    /// \param gradeMv2 the grade of the homogeneous multivector Mv2 to be considered
    /// \param transformationMatrixMv1 is the matrix that put a k-vector of grade "gradeMv1" to the algebra related orthogonal space
    /// \param transformationMatrixMv2 is the matrix that put a k-vector of grade "gradeMv2" to the algebra related orthogonal space
    /// \param transformationMatrixMv3 is the matrix that put back a k-vector of grade "gradeMv3" from the algebra related orthogonal space to the original space
    /// \param diagonalMetric defines the metric in the algebra related orthogonal space
    /// \return for each component of the result mv3=mv1.mv2 with grade gradeMv3=|gradeMv1-gradeMv2|, we return a list of quadruplet (indexMv1,indexMv2,indexMv3,coefficient).
    std::list<productComponent<double>> generateExplicitInnerProductList(const unsigned int gradeMv1,
                                                                         const unsigned int gradeMv2,
                                                                         const Eigen::SparseMatrix<double, 0> &transformationMatrixMv1,
                                                                         const Eigen::SparseMatrix<double, 0> &transformationMatrixMv2,
                                                                         const Eigen::SparseMatrix<double, 0> &transformationMatrixMv3,
                                                                         const Eigen::VectorXd &diagonalMetric) const;

    /// \brief Generate all the components of the geometric product mv1<gradeMv1> mv2<gradeMv2>.
    /// \param gradeMv1 the grade of the homogeneous multivector Mv1 to be considered
    /// \param gradeMv2 the grade of the homogeneous multivector Mv2 to be considered
    /// \param transformationMatrixMv1 is the matrix that put a k-vector of grade "gradeMv1" to the algebra related orthogonal space
    /// \param transformationMatrixMv2 is the matrix that put a k-vector of grade "gradeMv2" to the algebra related orthogonal space
    /// \param transformationMatrixMv3 is the matrix that put back a k-vector of grade "gradeMv3" from the algebra related orthogonal space to the original space
    /// \param diagonalMetric defines the metric in the algebra related orthogonal space
    /// \return a vector of products (each cell of the vector contains its associated grade products). For each component of the result mv3=mv1 mv2, we return a list of quadruplet (indexMv1,indexMv2,indexMv3,coefficient).
    std::vector<std::list<productComponent<double>>> generateExplicitGeometricProductListEuclideanSpace(const unsigned int gradeMv1,const unsigned int gradeMv2,
                                                                                const Eigen::VectorXd& diagonalMetric) const;

    /// \brief Generate all the components of the geometric product mv1<gradeMv1> mv2<gradeMv2>.
    /// \param gradeMv1 the grade of the homogeneous multivector Mv1 to be considered
    /// \param gradeMv2 the grade of the homogeneous multivector Mv2 to be considered
    /// \param transformationMatrixMv1 is the matrix that put a k-vector of grade "gradeMv1" to the algebra related orthogonal space
    /// \param transformationMatrixMv2 is the matrix that put a k-vector of grade "gradeMv2" to the algebra related orthogonal space
    /// \param transformationMatrixMv3 is the matrix that put back a k-vector of grade "gradeMv3" from the algebra related orthogonal space to the original space
    /// \param diagonalMetric defines the metric in the algebra related orthogonal space
    /// \return a vector of products (each cell of the vector contains its associated grade products). For each component of the result mv3=mv1 mv2, we return a list of quadruplet (indexMv1,indexMv2,indexMv3,coefficient).
    std::vector<std::list<productComponent<double>>> generateExplicitGeometricProductList(const unsigned int gradeMv1,const unsigned int gradeMv2,
                                                                                Eigen::SparseMatrix<double, Eigen::ColMajor>& transformationMatrixMv1,
                                                                                Eigen::SparseMatrix<double, Eigen::ColMajor>& transformationMatrixMv2,
                                                                                std::vector<Eigen::SparseMatrix<double, Eigen::ColMajor> >& transformationMatricesMv3,
                                                                                const Eigen::VectorXd& diagonalMetric) const;

    // tool to debug
    void displayGradeToXorIndices() const;

    /// \brief getter: return the grade knowing the Xor index
    unsigned int getGrade(unsigned int xorIndex) const;

    /// \brief getter: return the homogeneous index knowing the Xor index
    unsigned int getHomogeneousIndex(unsigned int xorIndex) const;

    /// \brief getter: return the xor index knowing the grade and the homogeneous index
    unsigned int getXorIndex(unsigned int grade, unsigned int homogeneousIndex) const;

protected:
    const unsigned int dimension;

    /// order induced by the xor:                 scal, 1, 2, 12, 3, 13, 23, 123
    /// Mapping between
    /// Xor index:                                   0, 1, 2,  3, 4,  5,  6, 7
    /// and grade and position in the sequence:      0, 0, 1,  0, 2,  1,  2, 0

    // defines the one to one correspondence between a xor index (index in the multivector) and the grade and position in the homogeneous vector
    // grade and position to xor index
    /// xorIndex = gradePositionToXorIndex[grade][pos];
    std::vector<std::vector<unsigned int> > gradePositionToXorIndex;

    /// array that converts xor index to grade and position
    /// first: grade
    /// second: position
    std::vector<std::pair<unsigned int,unsigned int> > xorIndexToGradeAndPosition; // number of pairs is 2^dimension
};

/// \brief generate a scale containing the sign of the norm of the pseudo-inverse
double getScaleInversePseudoScalar(const Eigen::MatrixXd &metric);

#endif //GARAGEN_PRODUCTTOOLS_HPP


#ifndef STATIC_LINK_VW
#define BOOST_TEST_DYN_LINK
#endif

#include <boost/test/unit_test.hpp>
#include <boost/test/test_tools.hpp>

#include "fast_pow10.h"
#include "test_common.h"

bool are_same(float a, float b) { return std::abs(a - b) < std::numeric_limits<float>::epsilon(); }

BOOST_AUTO_TEST_CASE(pow10_tests)
{
  // In reality most of these tests would all evaluate to the same. The reason is that the epsilon is usually around 1.2e-7
  const float base = 10;
  BOOST_CHECK(are_same(VW::fast_pow10(-127), std::pow(base, -127)));
  BOOST_CHECK(are_same(VW::fast_pow10(-46), std::pow(base, -46)));
  BOOST_CHECK(are_same(VW::fast_pow10(-45), std::pow(base, -45)));
  BOOST_CHECK(are_same(VW::fast_pow10(-44), std::pow(base, -44)));
  BOOST_CHECK(are_same(VW::fast_pow10(-40), std::pow(base, -40)));
  BOOST_CHECK(are_same(VW::fast_pow10(-38), std::pow(base, -38)));
  BOOST_CHECK(are_same(VW::fast_pow10(-37), std::pow(base, -37)));
  BOOST_CHECK(are_same(VW::fast_pow10(-10), std::pow(base, -10)));
  BOOST_CHECK(are_same(VW::fast_pow10(-5), std::pow(base, -5)));
  BOOST_CHECK_CLOSE(VW::fast_pow10(0), 1, FLOAT_TOL);
  BOOST_CHECK_CLOSE(VW::fast_pow10(5), 1e5, FLOAT_TOL);
  BOOST_CHECK_CLOSE(VW::fast_pow10(10), 1e10, FLOAT_TOL);
  BOOST_CHECK_CLOSE(VW::fast_pow10(37), 1e37, FLOAT_TOL);
  BOOST_CHECK_CLOSE(VW::fast_pow10(38), 1e38, FLOAT_TOL);
  BOOST_CHECK(std::isinf(VW::fast_pow10(39)));
  BOOST_CHECK(std::isinf(VW::fast_pow10(127)));
}


/*
 * phase_chain.cpp
 *
 * Example of OMP parallelization with odeint
 *
 * Copyright 2013 Karsten Ahnert
 * Copyright 2013 Mario Mulansky
 * Copyright 2013 Pascal Germroth
 * 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 <iostream>
#include <vector>
#include <boost/random.hpp>
#include <boost/timer/timer.hpp>
//[phase_chain_openmp_header
#include <omp.h>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/openmp/openmp.hpp>
//]

using namespace std;
using namespace boost::numeric::odeint;
using boost::timer::cpu_timer;
using boost::math::double_constants::pi;

//[phase_chain_vector_state
typedef std::vector< double > state_type;
//]

//[phase_chain_rhs
struct phase_chain
{
    phase_chain( double gamma = 0.5 )
    : m_gamma( gamma ) { }

    void operator()( const state_type &x , state_type &dxdt , double /* t */ ) const
    {
        const size_t N = x.size();
        #pragma omp parallel for schedule(runtime)
        for(size_t i = 1 ; i < N - 1 ; ++i)
        {
            dxdt[i] = coupling_func( x[i+1] - x[i] ) +
                      coupling_func( x[i-1] - x[i] );
        }
        dxdt[0  ] = coupling_func( x[1  ] - x[0  ] );
        dxdt[N-1] = coupling_func( x[N-2] - x[N-1] );
    }

    double coupling_func( double x ) const
    {
        return sin( x ) - m_gamma * ( 1.0 - cos( x ) );
    }

    double m_gamma;
};
//]

int main( int argc , char **argv )
{
    //[phase_chain_init
    size_t N = 131101;
    state_type x( N );
    boost::random::uniform_real_distribution<double> distribution( 0.0 , 2.0*pi );
    boost::random::mt19937 engine( 0 );
    generate( x.begin() , x.end() , boost::bind( distribution , engine ) );
    //]

    //[phase_chain_stepper
    typedef runge_kutta4<
                      state_type , double ,
                      state_type , double ,
                      openmp_range_algebra
                    > stepper_type;
    //]

    //[phase_chain_scheduling
    int chunk_size = N/omp_get_max_threads();
    omp_set_schedule( omp_sched_static , chunk_size );
    //]

    cpu_timer timer;
    //[phase_chain_integrate
    integrate_n_steps( stepper_type() , phase_chain( 1.2 ) ,
                       x , 0.0 , 0.01 , 100 );
    //]
    double run_time = static_cast<double>(timer.elapsed().wall) * 1.0e-9;
    std::cerr << run_time << "s" << std::endl;
    // copy(x.begin(), x.end(), ostream_iterator<double>(cout, "\n"));

    return 0;
}