#ifndef STAN_MATH_PRIM_SCAL_FUN_LOG_SUM_EXP_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LOG_SUM_EXP_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/fun/log1p_exp.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <boost/math/tools/promotion.hpp>

namespace stan {
namespace math {

/**
 * Calculates the log sum of exponetials without overflow.
 *
 * \f$\log (\exp(a) + \exp(b)) = m + \log(\exp(a-m) + \exp(b-m))\f$,
 *
 * where \f$m = max(a, b)\f$.
 *
 *
   \f[
   \mbox{log\_sum\_exp}(x, y) =
   \begin{cases}
     \ln(\exp(x)+\exp(y)) & \mbox{if } -\infty\leq x, y \leq \infty \\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN or } y = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{log\_sum\_exp}(x, y)}{\partial x} =
   \begin{cases}
     \frac{\exp(x)}{\exp(x)+\exp(y)} & \mbox{if } -\infty\leq x, y \leq \infty
 \\[6pt] \textrm{NaN} & \mbox{if } x = \textrm{NaN or } y = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{log\_sum\_exp}(x, y)}{\partial y} =
   \begin{cases}
     \frac{\exp(y)}{\exp(x)+\exp(y)} & \mbox{if } -\infty\leq x, y \leq \infty
 \\[6pt] \textrm{NaN} & \mbox{if } x = \textrm{NaN or } y = \textrm{NaN}
   \end{cases}
   \f]
 *
 * @param a the first variable
 * @param b the second variable
 */
template <typename T1, typename T2>
inline return_type_t<T1, T2> log_sum_exp(const T2& a, const T1& b) {
  if (a == NEGATIVE_INFTY)
    return b;
  if (a == INFTY && b == INFTY)
    return INFTY;
  if (a > b)
    return a + log1p_exp(b - a);
  return b + log1p_exp(a - b);
}

}  // namespace math
}  // namespace stan

#endif


//
// Created by Hamza El-Kebir on 5/9/21.
//

#ifndef LODESTAR_ALGEBRAICRICCATIEQUATION_HPP
#define LODESTAR_ALGEBRAICRICCATIEQUATION_HPP

#include "Lodestar/systems/StateSpace.hpp"
#include <Eigen/Eigenvalues>

namespace ls {
    namespace synthesis {
        class AlgebraicRiccatiEquation {
        public:
            static Eigen::MatrixXd
            solveDARE(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &Q,
                      const Eigen::MatrixXd &R);

            static Eigen::MatrixXd
            solveDARE(const systems::StateSpace<> &sys, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R);
        };
    }
}

#endif //LODESTAR_ALGEBRAICRICCATIEQUATION_HPP


// Depth filling based on the paper
//  "Colorization Using Optimization" by A. Levin, D. Lischinski, Y. Weiss
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
//  based on an implementation by Benedikt Waldvogel

#include <depth_filler/depth_filler.h>

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/LU>

#ifdef EIGEN_UMFPACK_SUPPORT
#include <Eigen/UmfPackSupport>
#endif

#ifdef KLU_SUPPORT
#include "eigen_klu.h"
#endif

#include <unsupported/Eigen/SparseExtra>

#include <chrono>

namespace depth_filler
{

constexpr bool DUMP_MATRICES = false;
constexpr bool PROFILE_SOLVING = true;

typedef decltype(std::chrono::high_resolution_clock::now()) Time;

// Choose best available solver
#ifdef KLU_SUPPORT
// KLU can only solve in double precision
typedef double SolveType;
#elif defined(EIGEN_UMFPACK_SUPPORT)
#warning Using slower UMFPACK solver. Install KLU for faster solving.
// UMFPACK can only solve in double precision
typedef double SolveType;
#else
#warning Using very slow built-in Eigen BiCGSTAB solver. Install KLU!
typedef float SolveType;
#endif

namespace
{
	class ScopeTimer
	{
	public:
		explicit ScopeTimer(const std::string& name)
		: m_name(name)
		{
			if(PROFILE_SOLVING)
				m_start = std::chrono::high_resolution_clock::now();
		}

		~ScopeTimer()
		{
			if(PROFILE_SOLVING)
			{
				Time stop = std::chrono::high_resolution_clock::now();
				std::cout << m_name << " took " << std::chrono::duration_cast<std::chrono::milliseconds>(stop - m_start).count() << "ms\n";
			}
		}
	private:
		std::string m_name;
		Time m_start;
	};
}

class DepthFillerPrivate
{
public:
	void solve();

	int rows;
	int cols;

	cv::Mat_<uint8_t> mask;
	cv::Mat_<float> depth;
	cv::Mat_<cv::Vec3b> rgb;
	unsigned int numUnknown;

	cv::Mat_<int> mapImgToIdx;

	std::vector<int> outerIndices;
	std::vector<int> innerIndices;
	std::vector<SolveType> values;

	Eigen::Matrix<SolveType, Eigen::Dynamic, 1> b;
	Eigen::Matrix<SolveType, Eigen::Dynamic, 1> newDepth;

#if defined(KLU_SUPPORT)
	Eigen::KLU<Eigen::MappedSparseMatrix<SolveType, Eigen::RowMajor>> solver;
#elif defined(EIGEN_UMFPACK_SUPPORT)
	Eigen::UmfPackLU<Eigen::SparseMatrix<SolveType>> solver;
#else
	Eigen::BiCGSTAB<Eigen::MappedSparseMatrix<SolveType, Eigen::RowMajor>> solver;
#endif

	DepthFiller::ColorDistance distance = DepthFiller::CD_GRAYSCALE;
	bool normalizeWithVariance = true;
	float distExponent = 1.2f;
	float distScale = 1.0f;
};

void DepthFillerPrivate::solve()
{
	const int winRad = 1;
	const int winPixel = (2 * winRad + 1)*(2 * winRad + 1);

	outerIndices.clear();
	innerIndices.clear();
	values.clear();

	const int GUESS_NONZERO = 9 * numUnknown;
	outerIndices.reserve(numUnknown);
	innerIndices.reserve(GUESS_NONZERO);
	values.reserve(GUESS_NONZERO);

	Eigen::VectorXi cols(winPixel);
	Eigen::VectorXf winDepth(winPixel);
	Eigen::VectorXf gvals = Eigen::VectorXf::Zero(winPixel);
	std::vector<Eigen::Vector3f> rgbWin(winPixel);

	b.resize(numUnknown);
	b.setZero();

	mapImgToIdx = cv::Mat_<int>(depth.rows, depth.cols);
	{
		unsigned int idx = 0;
		for(int y = 0; y < depth.rows; ++y)
		{
			float* ptr = depth[y];
			int* idxPtr = mapImgToIdx[y];

			for(int x = 0; x < depth.cols; ++x)
			{
				if(ptr[x] < 0 && (!mask.rows || mask(y,x)))
					idxPtr[x] = idx++;
				else
					idxPtr[x] = -1;
			}
		}
	}

	{
		ScopeTimer timer("formulate system matrix");

		cv::Mat_<uint8_t> gray;
		cv::cvtColor(rgb, gray, CV_BGR2GRAY);

		int absIdx = 0;

		for(int y = 0; y < depth.rows; ++y)
		{
			for(int x = 0; x < depth.cols; ++x)
			{
				// For pixels r without depth, we need to put the equation
				//  U(r) - sum(w_rs * U(s)) = 0
				// into matrix form (sum over neighborhood of r).
				// For pixels r with depth, we have
				//  U(r) = depth.

				if(depth(y,x) >= 0 || (mask.rows && !mask(y,x)))
					continue; // this pixel is known

				outerIndices.push_back(innerIndices.size());

				int nWin = 0;

				for(int wy = std::max(0, y - winRad); wy < std::min(y + winRad + 1, depth.rows); ++wy)
				{
					for(int wx = std::max(0, x - winRad); wx < std::min(x + winRad + 1, depth.cols); ++wx)
					{
#if REDUCED_NEIGHBORHOOD
						if(std::abs(wx-x) == 1 && std::abs(wy-y) == 1)
							continue;
#endif

						if(wx == x && wy == y)
							continue;

						cols[nWin] = mapImgToIdx(wy, wx);
						gvals[nWin] = gray(wy, wx);
						winDepth[nWin] = depth(wy,wx);

						auto& color = rgb.at<cv::Vec3b>(y,x);
						rgbWin[nWin] = Eigen::Vector3f((float)color[0] / 255, (float)color[1]/255, (float)color[2]/255);

						nWin++;
					}
				}

				// center pixel
				float curVal = gray(y, x);
				gvals[nWin] = curVal;

				auto& color = rgb.at<cv::Vec3b>(y,x);
				auto ownColor = Eigen::Vector3f((float)color[0] / 255, (float)color[1]/255, (float)color[2]/255);

				// Calculate variance
				float mean = gvals.mean();
				Eigen::VectorXf dev = gvals - Eigen::VectorXf::Constant(gvals.rows(), mean);
				float c_var = (dev.array()*dev.array()).mean();

				float csig = normalizeWithVariance ? std::max(c_var, 0.0000002f) : 1.0f;

				Eigen::ArrayXf tmp(nWin);
				if(distance == DepthFiller::CD_GRAYSCALE)
				{
					tmp = gvals.head(nWin).array() - curVal;
					tmp = (tmp * tmp).pow(distExponent);
				}
				else if(distance == DepthFiller::CD_RGB)
				{
					// RGB distance
					for(int i = 0; i < nWin; ++i)
					{
						tmp[i] = std::pow((ownColor - rgbWin[i]).squaredNorm(), distExponent);
					}
				}
				tmp *= distScale;

				gvals.head(nWin) = (- tmp / csig).exp().matrix();

				float s = gvals.head(nWin).sum();
				if(s > 0)
				{
					gvals.head(nWin) /= s;
				}

				bool self = false;
				for(int i = 0; i < nWin; ++i)
				{
					if(!self && cols[i] > absIdx)
					{
						// Now the self-reference U(r) (along the diagonal).
						innerIndices.push_back(absIdx);
						values.push_back(1.0);
						self = true;
					}

					if(cols[i] >= 0)
					{
						// relation to other unknown pixel...
						innerIndices.push_back(cols[i]);
						values.push_back(-gvals[i]);
					}
					else
					{
						// relation to fixed pixel => put on rhs
						b[absIdx] += gvals[i] * winDepth[i];
					}
				}
				if(!self)
				{
					innerIndices.push_back(absIdx);
					values.push_back(1.0);
				}

				absIdx++;
			}
		}
	}

	outerIndices.push_back(innerIndices.size());

	// Create Eigen matrix from A
	Eigen::MappedSparseMatrix<SolveType, Eigen::RowMajor> A(
		numUnknown, numUnknown,
		values.size(),
		outerIndices.data(), innerIndices.data(),
		values.data()
	);
	{
		ScopeTimer timer("sparse matrix");

		// Check CSR format
		if(outerIndices.size() != numUnknown+1)
			throw std::runtime_error("outer indices has invalid size");

		printf("numUnknown: %u, non-zero: %lu\n", numUnknown, values.size());

		for(unsigned int i = 0; i < numUnknown; ++i)
		{
			int rowStart = outerIndices[i];
			int nextRowStart = outerIndices[i+1];

			if(rowStart > nextRowStart)
				throw std::runtime_error("outer indices are non-monotonic");

			if(rowStart >= (int)values.size() || rowStart < 0)
				throw std::runtime_error("invalid start idx");

			if(nextRowStart > (int)values.size() || nextRowStart < 0)
				throw std::runtime_error("invalid end idx");

			int lastOne = -1;
			for(int j = rowStart; j < nextRowStart; ++j)
			{
				int col = innerIndices[j];
				if(col < 0 || col >= (int)numUnknown)
					throw std::runtime_error("invalid inner index");

				if(col <= lastOne)
					throw std::runtime_error("non-monotonic inner index");

				lastOne = col;
			}
		}

		if(DUMP_MATRICES)
		{
			Eigen::saveMarket(A, "/tmp/filler_A.mtx");
			Eigen::saveMarketVector(b, "/tmp/filler_A_b.mtx");

			// Dump upper left corner of matrix as image
			const int IMG_SIZE = std::min<int>(5000, A.rows());
			cv::Mat_<uint8_t> vis(IMG_SIZE, IMG_SIZE, 255);
			Eigen::MatrixXf reduced(IMG_SIZE, IMG_SIZE);
			reduced.fill(0);

			for (int k=0; k<A.outerSize(); ++k)
			{
				for (Eigen::MappedSparseMatrix<SolveType, Eigen::RowMajor>::InnerIterator it(A,k); it; ++it)
				{
					if(it.row() < 0 || it.col() < 0)
					{
						fprintf(stderr, "Invalid idx!\n");
					}
					if(it.row() < IMG_SIZE && it.col() < IMG_SIZE)
					{
						vis(it.row(), it.col()) = 0;
						reduced(it.row(), it.col()) = it.value();
					}
				}
			}

			cv::imwrite("/tmp/filler_A.png", vis);

			Eigen::MatrixXf inverse = reduced.inverse();
			cv::Mat_<uint8_t> inverseVis(IMG_SIZE, IMG_SIZE);
			for(int i = 0; i < IMG_SIZE; ++i)
			{
				for(int j = 0; j < IMG_SIZE; ++j)
				{
					inverseVis(i,j) = (std::abs(inverse(i,j)) > 1e-6) ? 0 : 255;
				}
			}

			cv::imwrite("/tmp/filler_Ainv.png", inverseVis);
		}
	}

	{
		ScopeTimer timer("solving");

#if KLU_SUPPORT
// 		if(sameStructure)
// 			solver.refactorize(A);
// 		else
			solver.compute(A);
#else
		solver.compute(Eigen::SparseMatrix<SolveType>(A));
#endif
		if(solver.info() != Eigen::Success)
		{
			fprintf(stderr, "Failed to solve system!\n");
		}
		newDepth = solver.solve(b);
	}
}

DepthFiller::DepthFiller()
 : m_d(new DepthFillerPrivate)
{
}

DepthFiller::~DepthFiller()
{
}

void DepthFiller::setColorDistance(ColorDistance dist)
{
	m_d->distance = dist;
}

void DepthFiller::setNormalizeWithVariance(bool on)
{
	m_d->normalizeWithVariance = on;
}

void DepthFiller::setDistanceExponent(float exp)
{
	m_d->distExponent = exp;
}

void DepthFiller::setDistanceScale(float scale)
{
	m_d->distScale = scale;
}

cv::Mat DepthFiller::fillDepth(const cv::Mat& input, const cv::Mat& rgb, bool sameStructure, const cv::Mat_<uint8_t>& mask)
{
	m_d->rows = input.rows;
	m_d->cols = input.cols;

	// Normalize depth image to [0, 1]
	m_d->depth = cv::Mat_<float>(input.size());
	float oldMin = std::numeric_limits<float>::infinity();
	float oldMax = -std::numeric_limits<float>::infinity();

	m_d->numUnknown = 0;

	{
		ScopeTimer timer("normalize depth image");

		for(int y = 0; y < input.rows; ++y)
		{
			for(int x = 0; x < input.cols; ++x)
			{
				float ival = input.at<float>(y,x);
				if(!std::isfinite(ival) || ival == 0)
					continue;

				oldMin = std::min<float>(oldMin, ival);
				oldMax = std::max<float>(oldMax, ival);
			}
		}

		for(int y = 0; y < input.rows; ++y)
		{
			for(int x = 0; x < input.cols; ++x)
			{
				float ival = input.at<float>(y,x);
				if(!std::isfinite(ival) || ival == 0)
				{
					m_d->depth(y,x) = -1;

					if(!mask.rows || mask(y,x))
						m_d->numUnknown++;
				}
				else
					m_d->depth(y,x) = ((float)(ival - oldMin)) / (oldMax - oldMin);
			}
		}
	}
	printf("depth: %d by %d, numUnknown: %u\n", m_d->depth.cols, m_d->depth.rows, m_d->numUnknown);

	m_d->rgb = rgb;
	m_d->mask = mask;

	m_d->solve();

	cv::Mat_<float> ret(input.size());
	int idx = 0;
	for(int y = 0; y < ret.rows; ++y)
	{
		for(int x = 0; x < ret.cols; ++x)
		{
			float ival = input.at<float>(y,x);
			if(std::isfinite(ival) || (mask.rows && !mask(y,x)))
				ret(y,x) = ival;
			else
			{
				ret(y,x) = oldMin + (oldMax - oldMin) * m_d->newDepth[idx];
				idx++;
			}
		}
	}

	return ret;
}

void DepthFiller::erodeDepth(const cv::Mat_<float>& input, cv::Mat_<float>& output, int kernelSize)
{
	cv::Mat_<uint8_t> valid(input.rows, input.cols);
	for(int y = 0; y < input.rows; ++y)
	{
		for(int x = 0; x < input.cols; ++x)
		{
			valid(y,x) = std::isfinite(input(y,x));
		}
	}

	cv::Mat_<uint8_t> erodedValid;
	cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(2*kernelSize+1, 2*kernelSize+1), cv::Point(kernelSize, kernelSize));
	cv::erode(valid, erodedValid, kernel);

	for(int y = 0; y < valid.rows; ++y)
	{
		for(int x = 0; x < valid.cols; ++x)
		{
			if(!erodedValid(y,x))
				output(y,x) = NAN;
		}
	}
}

cv::Mat_<float> DepthFiller::prefill(const cv::Mat_<float>& input)
{
	cv::Mat_<float> ret;
	input.copyTo(ret);

	Eigen::Matrix<float, 9, 1> values;
	cv::Mat_<bool> valid(3,3);

	for(int y = 1; y < input.rows-1; ++y)
	{
		for(int x = 1; x < input.cols-1; ++x)
		{
			if(std::isfinite(input(y,x)))
			{
				ret(y,x) = input(y,x);
				continue;
			}

			valid = false;

			int numValid = 0;

			for(int dy = -1; dy <= 1; ++dy)
			{
				for(int dx = -1; dx <= 1; ++dx)
				{
					if(std::isfinite(input(y+dy,x+dx)))
					{
						valid[dy+1][dx+1] = true;

						values[numValid] = input(y+dy,x+dx);
						numValid++;
					}
				}
			}

			bool corners[4] = {
				valid[0][0] || valid[0][1] || valid[1][0],
				valid[2][0] || valid[1][0] || valid[2][1],
				valid[0][2] || valid[1][2] || valid[0][1],
				valid[2][2] || valid[1][2] || valid[2][1]
			};

			if(corners[0] && corners[1] && corners[2] && corners[3]
				&& values.head(numValid).maxCoeff() - values.head(numValid).minCoeff() < 0.05)
			{
				ret(y,x) = values.head(numValid).mean();
			}
			else
				ret(y,x) = NAN;
		}
	}

	return ret;
}

void DepthFiller::dumpSystem(const std::string& prefix)
{
	Eigen::MappedSparseMatrix<SolveType, Eigen::RowMajor> A(
		m_d->numUnknown, m_d->numUnknown,
		m_d->values.size(),
		m_d->outerIndices.data(), m_d->innerIndices.data(),
		m_d->values.data()
	);

	Eigen::saveMarket(A, prefix + ".mtx");
	Eigen::saveMarketVector(m_d->b, prefix + "_b.mtx");
}

}


/*!
 * Copyright (C) tkornuta, IBM Corporation 2015-2019
 *
 * 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.
 */
/*!
 * \file Matrix.hpp
 * \brief
 * \author tkornut
 * \date Mar 7, 2016
 */

#ifndef SRC_TYPES_MATRIX_HPP_
#define SRC_TYPES_MATRIX_HPP_

#include <Eigen/Dense>
#include <random>
#include <memory> // std::shared_ptr

#include <boost/serialization/serialization.hpp>
// include this header to serialize vectors
#include <boost/serialization/vector.hpp>
// include this header to serialize arrays
#include <boost/serialization/array.hpp>
#include <boost/serialization/version.hpp>

// Forward declaration of class boost::serialization::access
namespace boost {
namespace serialization {
class access;
}//: serialization
}//: access

namespace mic {
namespace types {

// Forward declaration of a class Tensor.
template<typename T>
class Tensor;

// Forward declaration of a class Vector.
//template<typename T>
//class Vector;

/*!
 * \brief Template-typed Matrix of dynamic size.
 * Uses OpenBLAS if found by CMAKE - overloaded, specializations of * operator for types: float, double.
 *
 * \tparam T Template parameter denoting elementary type of data used (int, float, double etc.)
 * \date Mar 7, 2016
 * \author tkornuta
 */
template<typename T>
class Matrix : public Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> {
public:

	/*!
	 * Constructor. Calls default Eigen::MatrixXf constructor.
	 */
	EIGEN_STRONG_INLINE
	Matrix() :
		Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>() {
	}

	/*!
	 * Constructor. Calls default Eigen::MatrixXf constructor.
	 * @param Rows_ Number of rows.
	 * @param Cols_ Number of columns.
	 */
	EIGEN_STRONG_INLINE
	Matrix(int Rows_, int Cols_) :
		Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>(Rows_, Cols_) {
		this->setZero();
	}

	/*!
	 * Copying constructor on the basis of a vector. Sets dimensions to rows = size(), cols = 1.
	 * @param vector_ Vector
	 */
	EIGEN_STRONG_INLINE
	Matrix(const Eigen::Matrix<T, Eigen::Dynamic, 1>& vector_) :
		Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>(vector_.size(), 1)
	{
		// Copy the whole vector block.
		memcpy(this->data(), vector_.data(), vector_.size() * sizeof(T));
	}

	/*!
	 * Copying constructor on the basis of another matrix.
	 * @param matrix_ Matrix to be copied.
	 */
	EIGEN_STRONG_INLINE
	Matrix(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& matrix_) :
		Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>(matrix_.rows(), matrix_.cols())
	{
		// Copy the whole vector block.
		memcpy(this->data(), matrix_.data(), matrix_.size() * sizeof(T));
	}

	/*!
	 * Copying constructor on the basis of a tensor. Copies dimensions and data.
	 * Note: tensor must be 2D.
	 * @param tensor_ Tensor
	 */
	EIGEN_STRONG_INLINE
	Matrix(mic::types::Tensor<T>& tensor_) :
		Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>(tensor_.dim(1), tensor_.dim(0))
	{
		// Tensor must be 2D!
		assert(tensor_.dims().size() == 2);
		// Copy the whole block.
		memcpy(this->data(), tensor_.data(), tensor_.size() * sizeof(T));
	}

	/*
	 * Overloaded assignment operator - calls base operator.
	 * @param mat_ Input matrix
	 * @return An exact copy of the input matrix.
	 */
/*	EIGEN_STRONG_INLINE
	const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& operator =(const mic::types::Matrix<T>& mat_) {
		// Using base EIGEN operator =
		return Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::operator=(mat_);
	}*/

/*	EIGEN_STRONG_INLINE
	const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& operator =(const Eigen::Matrix<T, Eigen::Dynamic, 1>& vector_) {
		// Using base EIGEN operator =
		return Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::operator=(vector_);
	}*/

	/*!
	 * Overloaded assignment operator - calls base operator.
	 * @param mat_ Input matrix
	 * @return An exact copy of the input matrix.
	 */
	EIGEN_STRONG_INLINE
	Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& operator =(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& mat_) {
		// Using base EIGEN operator =
		Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::operator=(mat_);
		return *this;
	}

	/*!
	 * Overloaded multiplication operator.
	 * @param mat_ Input matrix
	 * @return Resulting matrix - multiplication of this and input mat_.
	 */
	EIGEN_STRONG_INLINE
	Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> operator *(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& mat_) {
		// Calling base EIGEN operator *
		//printf("Calling base EIGEN operator *\n");
		return Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::operator*(mat_);
	}

	/*!
	 * Sets values of all element to the value given as parameter.
	 * @param value_ The value to be set.
	 */
	void setValue(T value_) {
		// Get access to data.
		T* data_ptr = this->data();

#pragma omp parallel for
		for (size_t i = 0; i < (size_t) (this->rows() * this->cols()); i++)
			data_ptr[i] = value_;
	}

	/*!
	 * Enumerates - sets values of elements to their indices.
	 */
	void enumerate() {
		// Get access to data.
		T* data_ptr = this->data();

#pragma omp parallel for
		for (size_t i = 0; i < (size_t)this->size(); i++)
			data_ptr[i] = i;
	}

	/*!
	 * Set values of all matrix elements to random with a normal distribution.
	 * @param mean Mean
	 * @param stddev Variance
	 */
	void randn(T mean = 0, T stddev = 1) {

		// Initialize random number generator with normal distribution.
		std::random_device rd;
		std::mt19937 mt(rd());
		std::normal_distribution<T> dist(mean, stddev);

		// Get access to data.
		T* data_ptr = this->data();

#pragma omp parallel for
		for (size_t i = 0; i < (size_t) (this->rows() * this->cols()); i++) {
			data_ptr[i] = (T)dist(mt);
		}
	}

	/*!
	 * Set values of all matrix elements to random numbers from range <min, max> - uniform distribution.
	 * @param min Min value.
	 * @param max Max value.
	 * @return Random real value.
	 */
	void rand(T min = 0, T max = 1) {

		// Initialize random number generator with normal distribution.
		std::random_device rd;
		std::mt19937 mt(rd());
		std::uniform_real_distribution<T> dist(min, max);

		// Get access to data.
		T* data_ptr = this->data();

#pragma omp parallel for
		for (size_t i = 0; i < (size_t) (this->rows() * this->cols()); i++) {
			data_ptr[i] = (T)dist(rd);
		}
	}

	/*!
	 * Applies elementwise function to all matrix elements.
	 * @param func Function to be applied.
	 */
	void elementwiseFunction(T (*func)(T)) {

		// Get access to data.
		T* data_ptr = this->data();

		// Apply function to all elements.
#pragma omp parallel for
		for (size_t i = 0; i < (size_t) (this->rows() * this->cols()); i++) {
			data_ptr[i] = (*func)(data_ptr[i]);
		} //: for i
	}

	/*!
	 * Applies elementwise function to all matrix elements passing scalar as function argument.
	 * @param func Function to be applied.
	 * @param scalar_ Scalar passed to function as argument.
	 */
	void elementwiseFunctionScalar(T (*func)(T, T), T scalar_) {

		// Get access to data.
		T* data_ptr = this->data();

		// Apply function to all elements.
#pragma omp parallel for
		for (size_t i = 0; i < (size_t) (this->rows() * this->cols()); i++) {
			data_ptr[i] = (*func)(data_ptr[i], scalar_);
		} //: for i
	}

	/*!
	 * Applies elementwise function to all matrix elements and uses additional Matrix mat_ data as function parameter.
	 * @param func Function to be applied.
	 * @param mat_ Matrix passed to function as argument.
	 */
	void elementwiseFunctionMatrix(T (*func)(T, T), Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> & mat_) {

		// Check dimensions.
		if ((this->rows() != mat_.rows()) || (this->cols() != mat_.cols()))
			printf("elementwiseFunctionMatrix: dimensions mismatch!\n");

		// Get access to data.
		T* data_ptr = this->data();
		T* m_data_ptr = mat_.data();

		// Apply function to all elements.
#pragma omp parallel for
		for (size_t i = 0; i < (size_t) (this->rows() * this->cols()); i++) {
			data_ptr[i] = (*func)(data_ptr[i], m_data_ptr[i]);
		}//: for i

	}

	/*!
	 * Applies function to all matrix elements and uses additional vector data as function parameter - columnwise.
	 * @param func Used function
	 * @param v_ Vector passed to function
	 */
	void matrixColumnVectorFunction(T (*func)(T, T),
			Eigen::Matrix<T, Eigen::Dynamic, 1>& v_) {

		if (this->rows() != v_.cols())
			printf("matrixColumnVectorFunction: dimensions mismatch\n");

		// Get access to data.
		//float* data_ptr = data();
		//int rows = this->rows();
		//int cols = this->cols();
		//float* vector_data_ptr = v_.data();

#pragma omp parallel for
		for (size_t x = 0; x < (size_t)this->cols(); x++) {
			for (size_t y = 0; y < (size_t)this->rows(); y++) {
				//data_ptr[x + y*cols] = (*func)(data_ptr[x + y*cols], vector_data_ptr[x]);
				(*this)(y, x) = (*func)((*this)(y, x), v_(y));
			}//: for y
		}//: for x
	}

	/*!
	 * Applies function to all matrix elements and uses additional vector data as function parameter - rowwise.
	 * @param func Used function
	 * @param v_ Vector passed to function
	 */
	void matrixRowVectorFunction(T (*func)(T, T), Eigen::Matrix<T, Eigen::Dynamic, 1>& v_) {

		if (this->cols() != v_.cols())
			printf("matrixRowVectorFunction: dimensions mismatch\n");

		// Get access to data.
		/*float* data_ptr = data();
		 int rows = this->rows();
		 int cols = this->cols();
		 float* vector_data_ptr = v_.data();*/

#pragma omp parallel for
		for (size_t x = 0; x < (size_t)this->cols(); x++) {
			for (size_t y = 0; y < (size_t)this->rows(); y++) {
				//h(y,x) += c(x);
				//(*this)(y,x) += v_(x);
				(*this)(y, x) = (*func)((*this)(y, x), v_(x));
				//data_ptr[x + y*cols] = (*func)(data_ptr[x + y*cols], vector_data_ptr[y]);
			}//: for y
		}//: for x
	}

	/*!
	 * Sets the consecutive columns to be equal to given vector.
	 * @param in Input vector, that will be "cloned".
	 */
	void repeatVector(Eigen::Matrix<T, Eigen::Dynamic, 1> &in) {
#pragma omp parallel for
		for (size_t x = 0; x < (size_t)this->cols(); x++) {
			for (size_t y = 0; y < (size_t)this->rows(); y++) {

				(*this)(y, x) = in(y);
			}//: y
		}//: x
	}

	/*!
	 * Returns a vector of indices indicating maximal elements in consecutive matrix columns (colwise).
	 * @return Vector of indices.
	 */
	Eigen::Matrix<T, Eigen::Dynamic, 1> colwiseReturnMaxIndices() {

		Eigen::Matrix<T, Eigen::Dynamic, 1> indices((*this).cols());

		for (size_t i = 0; i < (size_t)(*this).cols(); i++) {

			T current_max_val;
			T index;

			for (size_t j = 0; j < (size_t)(*this).rows(); j++) {

				if (j == 0 || (*this)(j, i) > current_max_val) {

					index = j;
					current_max_val = (*this)(j, i);
				}

				indices(i) = index;

			}
		}

		return indices;
	}

	/*!
	 * Calculates the cross entropy as measure of how accurate given matrix (treated as prediction) fits to the desired (target) matrix.
	 * @param targets_ Desired results (targets) in the form of a matrix of answers.
	 * @return
	 */
	T calculateCrossEntropy(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& targets_) {

		T ce = 0.0;
		Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> error(this->rows(), this->cols());

		//check what has happened and get information content for that event
		error.array() = - (this->unaryExpr(std::ptr_fun(::logf)).array() * targets_.array());

		// Sum the errors.
		ce = error.sum();

		return ce;
	}

	/****************** ARMADILLO COMPATIBILITY *********************************/

	/*!
	 * Set zeros.
	 */
	void zeros (){
		this->setZero();
	}

    T& operator [](int idx) {
        return this->data()[idx];
    }

    T operator [](int idx) const {
        return this->data()[idx];
    }

private:

	// Friend class - required for using boost serialization.
    friend class boost::serialization::access;

    /*!
     * Serialization save - saves the matrix object to archive.
     * @param ar Used archive.
     * @param version Version of the matrix class.
     */
	template<class Archive>
	void save(Archive & ar, const unsigned int version) const {
    	size_t rows, cols;
    	rows = this->rows();
    	cols = this->cols();
		ar & rows;
		ar & cols;
        // Save elements.
        size_t elements = (size_t)(this->rows() * this->cols());
		T* data_ptr = (T*)this->data();
        ar & boost::serialization::make_array<T>(data_ptr, elements);
     }

    /*!
     * Serialization load - loads the matrix object to archive.
     * @param ar Used archive.
     * @param version Version of the matrix class.
     */
     template<class Archive>
     void load(Archive & ar, const unsigned int version) {
    	size_t rows, cols;
		ar & rows;
		ar & cols;
		// Allocate memory - resize.
		this->resize(rows, cols);
		// Load elements
        size_t elements = (size_t)(this->rows() * this->cols());
		T* data_ptr = this->data();
		ar & boost::serialization::make_array<T>(data_ptr, elements);
     }

     // The serialization must be splited as load requires to allocate the memory.
     BOOST_SERIALIZATION_SPLIT_MEMBER()

};

/*!
 * \brief Typedef for a shared pointer to template-typed dynamic matrices.
 * \author tkornuta
 */
template<typename T>
using MatrixPtr = typename std::shared_ptr< mic::types::Matrix<T> >;

}//: namespace types
}//: namespace mic

// Just in the case that something important will change in the matrix class - set version.
BOOST_CLASS_VERSION(mic::types::Matrix<bool>, 1)
BOOST_CLASS_VERSION(mic::types::Matrix<short>, 1)
BOOST_CLASS_VERSION(mic::types::Matrix<int>, 1)
BOOST_CLASS_VERSION(mic::types::Matrix<long>, 1)
BOOST_CLASS_VERSION(mic::types::Matrix<float>, 1)
BOOST_CLASS_VERSION(mic::types::Matrix<double>, 1)

#endif /* SRC_TYPES_MATRIX_HPP_ */


#include "gaussianXd.h"
#include <Eigen/Dense>
#include <iostream>
#include <vector>

using namespace Eigen;

int main(){
    VectorXd mean(2, 1);
    mean << 2,
            17;
    MatrixXd covariance(2, 2);
    covariance << 10.0, 0,
                0.0, 4.0;
    filters::MultivariateGaussian dist(mean, covariance);

    sciplot::Vec x = sciplot::linspace(-10, 15, 0.2);
    sciplot::Vec y = sciplot::linspace(5, 25, 0.2);
    std::vector<double> z;
    for(auto x_: x){
        for(auto y_: y){
            VectorXd vec(2, 1);
            vec << x_,
                   y_;

            double z_ = dist.probability(vec);
            z.push_back(z_);
        }
    }
    for(auto val: z){
        std::cout << val << std::endl;
    }
}


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

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


#include "bio/math.h"
#include "bio/iterator.h"
USING_BIO_NS

#include <boost/test/unit_test.hpp>
#include <boost/test/parameterized_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/assign/list_of.hpp>
using namespace boost;
using namespace boost::assign;
using boost::unit_test::test_suite;

#include <iostream>
using namespace std;

#include <gsl/gsl_sf_exp.h>

//#define VERBOSE_CHECKING

typedef vector< unsigned > obs_vec_t;

const std::vector< double > alpha(4, 1.0);
const vector< obs_vec_t > obs_vec = list_of< vector< unsigned > >
	(list_of< unsigned > (0) (0) (0) (0))
	(list_of< unsigned > (0) (1) (1) (0))
	(list_of< unsigned > (0) (2) (0) (0))
	(list_of< unsigned > (1) (1) (1) (1))
	(list_of< unsigned > (4) (4) (4) (4))
	(list_of< unsigned > (0) (0) (0) (4))
	(list_of< unsigned > (0) (0) (0) (8))
	(list_of< unsigned > (1) (1) (1) (13))
	(list_of< unsigned > (100) (97) (103) (101))
	;

const vector< unsigned > total_vec = list_of< unsigned >
	(1)
	(2)
	(3)
	(10)
	(30)
	;

void
check_calc_ln_gamma_factor_total(const unsigned total)
{
	cout << "******* check_calc_ln_gamma_factor(): total = " << total << "\n";

	obs_vec_t obs(4);

	double uniform = 0.0;
	double dirichlet = 0.0;

	for (obs[0] = 0; obs[0] != total + 1; ++obs[0])
	{
		for (obs[1] = 0; obs[0] + obs[1] != total + 1; ++obs[1])
		{
			for (obs[2] = 0; obs[0] + obs[1] + obs[2] != total + 1; ++obs[2])
			{
				obs[3] = total - obs[0] - obs[1] - obs[2];

				BOOST_ASSERT(unsigned(std::accumulate(obs.begin(), obs.end(), 0)) == total);

				const double ln_uniform = calc_multinomial_ln_likelihood_uniform_dist(obs.begin(), obs.end());
				double p = 0.25;
				BOOST_CHECK_CLOSE(
					ln_uniform,
					calc_multinomial_ln_likelihood(obs.begin(), obs.end(), single_value_iterator< double >( p )),
					0.001);

				uniform += BIO_GSL_EXP(ln_uniform);
				dirichlet += BIO_GSL_EXP(calc_multinomial_ln_likelihood_dirichlet_prior(obs.begin(), obs.end(), alpha.begin()));
			}
		}
	}

	BOOST_CHECK_CLOSE(uniform, 1.0, 0.001);
	BOOST_CHECK_CLOSE(dirichlet, 1.0, 0.001);
}

void
check_calc_ln_gamma_factor(const obs_vec_t & obs)
{
	cout << "******* check_calc_ln_gamma_factor()" << endl;

	boost::io::ios_base_all_saver ias(cout);

	const double n_choose = gsl_sf_exp(calc_ln_n_choose(obs.begin(), obs.end()));
	const double uniform_ll = calc_multinomial_ln_likelihood_uniform_dist(obs.begin(), obs.end());
	const double dirichlet_ll = calc_multinomial_ln_likelihood_dirichlet_prior(obs.begin(), obs.end(), alpha.begin());

	cout.setf(ios::left, ios::adjustfield);
	cout.setf(ios::fixed, ios::floatfield);

#ifdef VERBOSE_CHECKING
	copy(obs.begin(), obs.end(), ostream_iterator< unsigned >(cout , ","));
	cout
		<< " " << setw(6) << uniform_ll
		<< " " << setw(6) << dirichlet_ll
		<< " " << setw(6) << uniform_ll - dirichlet_ll
		<< "\n";
#endif

}

void
register_math_tests(boost::unit_test::test_suite * test)
{
	test->add( BOOST_PARAM_TEST_CASE( &check_calc_ln_gamma_factor, obs_vec.begin(), obs_vec.end() ), 0);
	test->add( BOOST_PARAM_TEST_CASE( &check_calc_ln_gamma_factor_total, total_vec.begin(), total_vec.end() ), 0);
}


#ifndef TVMTL_MANIFOLD_SPHERE_HPP
#define TVMTL_MANIFOLD_SPHERE_HPP

#include <cmath>
#include <complex>
#include <iostream>

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

#include "enumerators.hpp"

namespace tvmtl {

// Specialization SPHERE
template < int N>
struct Manifold< SPHERE, N> {

    public:
	static const MANIFOLD_TYPE MyType;
	static const int manifold_dim ;
	static const int value_dim; // TODO: maybe rename to embedding_dim

	static const bool non_isometric_embedding;

	// Scalar type of manifold
	//typedef double scalar_type;
	typedef double scalar_type;
	typedef double dist_type;
	typedef std::complex<double> complex_type;
	typedef std::vector<double> weight_list;

	// Value Typedef
	typedef Eigen::Matrix< scalar_type, N, 1>				value_type;
	typedef value_type&							ref_type;
	typedef const value_type&						cref_type;
	typedef std::vector<value_type, Eigen::aligned_allocator<value_type> >	value_list;

	// Tangent space typedefs
	typedef Eigen::Matrix < scalar_type, N, N-1> tm_base_type;
	typedef tm_base_type& tm_base_ref_type;

	// Derivative Typedefs
	typedef value_type			     deriv1_type;
	typedef deriv1_type&			     deriv1_ref_type;

	typedef Eigen::Matrix<scalar_type, N, N>     deriv2_type;
	typedef deriv2_type&			     deriv2_ref_type;
	typedef	Eigen::Matrix<scalar_type, N-1, N-1> restricted_deriv2_type;

	// Manifold distance functions (for IRLS)
	inline static dist_type dist_squared(cref_type x, cref_type y);
	inline static void deriv1x_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);
	inline static void deriv1y_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);

	inline static void deriv2xx_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2xy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2yy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);

	// Manifold exponentials und logarithms ( for Proximal point)
	template <typename DerivedX, typename DerivedY>
	inline static void exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedX>& result);
	inline static void log(cref_type x, cref_type y, ref_type result);

	inline static void convex_combination(cref_type x, cref_type y, double t, ref_type result);

	// Implementations of the Karcher mean
	// Slow list version
	inline static void karcher_mean(ref_type x, const value_list& v, double tol=1e-10, int maxit=15);
	inline static void weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol=1e-10, int maxit=15);
	// Variadic templated version
	template <typename V, class... Args>
	inline static void karcher_mean(V& x, const Args&... args);
	template <typename V>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y);
	template <typename V, class... Args>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y1, const Args&... args);

	// Basis transformation for restriction to tangent space
	inline static void tangent_plane_base(cref_type x, tm_base_ref_type result);

	// Projection
	inline static void projector(ref_type x);

	// Interpolation pre- and postprocessing
	inline static void interpolation_preprocessing(ref_type x) {};
	inline static void interpolation_postprocessing(ref_type x) {};

};

/*-----IMPLEMENTATION SPHERE----------*/

// Static constants, Outside definition to avoid linker error

template <int N>
const MANIFOLD_TYPE Manifold < SPHERE, N>::MyType = SPHERE;

template <int N>
const int Manifold < SPHERE, N>::manifold_dim = N-1;

template <int N>
const int Manifold < SPHERE, N>::value_dim = N;

template <int N>
const bool Manifold < SPHERE, N>::non_isometric_embedding = false;

// Squared Sphere distance function
template <int N>
inline typename Manifold < SPHERE, N>::dist_type Manifold < SPHERE, N>::dist_squared( cref_type x, cref_type y ){
    scalar_type xdoty = x.dot(y);
    if (xdoty < - 1.0) xdoty = -1.0;
    if (xdoty >  1.0) xdoty = 1.0;
    dist_type d = std::acos(xdoty);
    #ifdef TVMTL_MANIFOLD_DEBUG
	    if(!std::isfinite(d)){
	    std::cout << "\nDX Non-Series: " << std::endl;
	    std::cout << "x "<< x  << std::endl;
	    std::cout << "y "<< y << std::endl;
	    std::cout << "x^Ty "<< x.dot(y) << std::endl;
	    std::cout << "xdoty "<< xdoty << std::endl;
	    std::cout << "result" << d << std::endl;
	    }
	#endif
    return d*d;
}

// Derivative of Squared Sphere distance w.r.t. first argument
// TODO: Extende implementation of series to 1.0-eps
template <int N>
inline void Manifold < SPHERE, N>::deriv1x_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    scalar_type xdoty = x.dot(y);
    bool useSeries = false;

    if (xdoty < - 1.0) xdoty = -1.0;
    if (xdoty >= 1.0) { xdoty = 1.0; useSeries = true; }

    if(!useSeries){
	scalar_type acos = std::acos(xdoty);
	result =  -2.0 * acos / std::sqrt(1.0 - xdoty * xdoty) * y;
	#ifdef TVMTL_MANIFOLD_DEBUG
	    if(!std::isfinite(result(0))){
	    std::cout << "\nDX Non-Series: " << std::endl;
	    std::cout << "x "<< x  << std::endl;
	    std::cout << "y "<< y << std::endl;
	    std::cout << "x^Ty "<< x.dot(y) << std::endl;
	    std::cout << "xdoty "<< xdoty << std::endl;
	    std::cout << "acos(x) " << acos << std::endl;
	    std::cout << "sqrt(1-x^2)" << std::sqrt(1.0 - xdoty * xdoty) << std::endl;
	    std::cout << "result" << result << std::endl;
	    }
	#endif
    }
    else{
	result = -2.0 * y;
	#ifdef TVMTL_MANIFOLD_DEBUG
	    if(!std::isfinite(result(0)))
		std::cout << "DX Series:" << result << std::endl;
	#endif
    }
}
// Derivative of Squared Sphere distance w.r.t. second argument
template <int N>
inline void Manifold < SPHERE, N>::deriv1y_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    scalar_type xdoty = x.dot(y);
    bool useSeries = false;

    if (xdoty < - 1.0) xdoty = -1.0;
    if (xdoty >=  1.0) { xdoty = 1.0; useSeries = true; }

    if(!useSeries){
	scalar_type acos = std::acos(xdoty);
	result =  -2.0 * acos / std::sqrt(1.0 - xdoty * xdoty) * x;
    }
    else
	result = -2.0 * x;
}

// Second Derivative of Squared Sphere distance w.r.t first argument
template <int N>
inline void Manifold < SPHERE, N>::deriv2xx_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    scalar_type xdoty = x.dot(y);
    bool useSeries = false;

    if (xdoty < - 1.0) xdoty = -1.0;
    if (xdoty >=  1.0) { xdoty = 1.0; useSeries = true; }

    if(!useSeries){
	scalar_type onemx2 = 1.0 - xdoty * xdoty;
	scalar_type acos = std::acos(xdoty);
	scalar_type da =  -2.0 * acos / std::sqrt(onemx2);
	result = (2.0 + da * xdoty) / onemx2 * y * y.transpose() - da * xdoty * deriv2_type::Identity();
    }
    else
	result = 2.0/3.0 * y * y.transpose() + 2.0 * xdoty * deriv2_type::Identity();

}
// Second Derivative of Squared Sphere distance w.r.t first and second argument
template <int N>
inline void Manifold < SPHERE, N>::deriv2xy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    scalar_type xdoty = x.dot(y);
    bool useSeries = false;

    if (xdoty < - 1.0) xdoty = -1.0;
    if (xdoty >=  1.0) { xdoty = 1.0; useSeries = true; }

    if(!useSeries){
	scalar_type onemx2 = 1.0 - xdoty * xdoty;
	scalar_type acos = std::acos(xdoty);
	scalar_type da =  -2.0 * acos / std::sqrt(onemx2);
	result = (2.0 + da * xdoty) / onemx2 * y * x.transpose() + da * deriv2_type::Identity();
    }
    else
	result = 2.0/3.0 * y * x.transpose() - 2.0 * deriv2_type::Identity();
}
// Second Derivative of Squared Sphere distance w.r.t second argument
template <int N>
inline void Manifold < SPHERE, N>::deriv2yy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    scalar_type xdoty = x.dot(y);
    bool useSeries = false;

    if (xdoty < - 1.0) xdoty = -1.0;
    if (xdoty >=  1.0) { xdoty = 1.0; useSeries = true; }

    if(!useSeries){
	scalar_type onemx2 = 1.0 - xdoty * xdoty;
	scalar_type acos = std::acos(xdoty);
	scalar_type da =  -2.0 * acos / std::sqrt(onemx2);
	result = (2.0 + da * xdoty) / onemx2 * x * x.transpose() - da * xdoty * deriv2_type::Identity();

    }
    else
	result = 2.0/3.0 * x * x.transpose() + 2.0 * xdoty * deriv2_type::Identity();

}

// Exponential and Logarithm Map
template <int N>
template <typename DerivedX, typename DerivedY>
inline void Manifold <SPHERE, N>::exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedX>& result){
    //result=(x+y).normalized();
    scalar_type n = y.norm();
    if(n!=0)
	result = std::cos(n) * x + std::sin(n) * y / n;
    else
	result = x;
}

template <int N>
inline void Manifold <SPHERE, N>::log(cref_type x, cref_type y, ref_type result){
    //result = (y-x).normalized();
    scalar_type xdoty = x.dot(y);
    bool useSeries = false;

    if (xdoty < - 1.0) xdoty = -1.0;
    if (xdoty >=  1.0) { xdoty = 1.0; useSeries = true; }

    if(!useSeries){
	scalar_type fac = std::acos(xdoty) / std::sqrt(1.0 - xdoty * xdoty);
	result = fac * (y - xdoty * x);
    }
    else
	result =  y - xdoty * x;
}

// Tangent Plane restriction
// TODO: Implement general QR Composition here
template <int N>
inline void Manifold <SPHERE, N>::tangent_plane_base(cref_type x, tm_base_ref_type result){
    result = tm_base_type::Identity();
}

template <> // Special Version for S^2 utilizing the cross product
inline void Manifold <SPHERE, 3>::tangent_plane_base(cref_type x, tm_base_ref_type result){
    //int c = static_cast<int>(std::abs(x.coeff(0)) > 0.5);
    int c = static_cast<int>(std::abs(x.coeff(2)) > 0.5 || std::abs(x.coeff(1)) > 0.5);
    result.col(0) = value_type(0, x.coeff(2), -x.coeff(1)) * c + value_type(x.coeff(2), 0, -x.coeff(0)) * (1-c);
    result.col(0).normalize();
    result.col(1) = x.cross(result.col(0)).normalized();

    if(x.norm()==0)
	result = tm_base_type::Zero();

    #ifdef TVMTL_MANIFOLD_DEBUG
	    if(!std::isfinite(result(0,0))){
	    std::cout << "\n\nx "<< x  << std::endl;
	    std::cout << "col0 V1 "<<  value_type(0, x.coeff(2), -x.coeff(1)) << std::endl;
	    std::cout << "col0 V2 "<<  value_type(x.coeff(2), 0, -x.coeff(0)) << std::endl;
	    std::cout << "col0 norm " << result.col(0) << std::endl;
	    std::cout << "cross prod" << x.cross(result.col(0)) << std::endl;
	    std::cout << "cross prod norm" << result.col(1) << std::endl;
	    }
    #endif
}

// Convex geodesic combinations
template <int N>
inline void Manifold <SPHERE, N>::convex_combination(cref_type x, cref_type y, double t, ref_type result){
   if(t == 0.5){
	result = x + y;
	projector(result);
   }
   else{
	value_type l;
	log(x, y, l);
	exp(x, l * t, result);
   }
}

// Karcher mean implementations
template <int N>
inline void Manifold<SPHERE, N>::karcher_mean(ref_type x, const value_list& v, double tol, int maxit){
    value_type L, temp;

    int k = 0;
    double error = 0.0;
    do{
	scalar_type m1 = x.sum();
	L = value_type::Zero();
	for(int i = 0; i < v.size(); ++i){
	    log(x, v[i], temp);
	    L += temp;
	}
	exp(x, 1.0 / v.size() * L, temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);

}

template <int N>
inline void Manifold<SPHERE, N>::weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol, int maxit){
    value_type L, temp;

    int k = 0;
    double error = 0.0;
    do{
	scalar_type m1 = x.sum();
	L = value_type::Zero();
	for(int i = 0; i < v.size(); ++i){
	    log(x, v[i], temp);
	    L += w[i] * temp;
	}
	exp(x, 1.0 / v.size() * L, temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);

}

template <int N>
template <typename V, class... Args>
inline void Manifold<SPHERE, N>::karcher_mean(V& x, const Args&... args){
    V temp, sum;

    int numArgs = sizeof...(args);
    int k = 0;
    double error = 0.0;
    double tol = 1e-10;
    int maxit = 15;
    do{
	scalar_type m1 = x.sum();
	sum = x;
	variadic_karcher_mean_gradient(sum, args...);
	exp(x, 1.0 / numArgs * sum, temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);
}

template <int N>
template <typename V>
inline void Manifold<SPHERE, N>::variadic_karcher_mean_gradient(V& x, const V& y){
    V temp;
    log(x, y, temp);
    x = temp;
}

template <int N>
template <typename V, class... Args>
inline void Manifold<SPHERE, N>::variadic_karcher_mean_gradient(V& x, const V& y1, const Args& ... args){
    V temp1, temp2;
    temp2 = x;

    log(x, y1, temp1);

    variadic_karcher_mean_gradient(temp2, args...);
    temp1 += temp2;
    x = temp1;
}

template <int N>
inline void Manifold <SPHERE, N>::projector(ref_type x){

    #ifdef TVMTL_MANIFOLD_DEBUG
	if(!std::isfinite(x(0))) std::cout << "Projector recieved nan" << std::endl;
    #endif

    scalar_type norm = x.norm();
    if(norm!=0.0) x.normalize();
    else x.setConstant(1.0 / 256.0).normalize();

    #ifdef TVMTL_MANIFOLD_DEBUG
	if(!std::isfinite(x(0))) std::cout << "Projector returns nan" << std::endl;
    #endif
}

} // end namespace tvmtl

#endif


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Constant \f$\tan3\frac\pi{8} = \sqrt2 + 1\f$.

    @par Semantic:

    For type T:

    @code
    T r = Tan_3pio_8<T>();
    @endcode

    is similar to:

    @code
    T r = 1.0+sqrt(2.0);
    @endcode

    @return a value of type T

**/
  template<typename T> T Tan_3pio_8();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Constant \f$\tan3\frac\pi{8} = \sqrt2 + 1\f$.

      Generate the  constant tan_3pio_8.

      @return The Tan_3pio_8 constant for the proper type
    **/
    const boost::dispatch::functor<tag::tan_3pio_8_> tan_3pio_8 = {};
  }
} }
#endif

#include <boost/simd/constant/definition/tan_3pio_8.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include <MathExtensions.h>
#include <Pars.h>

#include <boost/math/special_functions/factorials.hpp>

using namespace std;
using namespace boost::math;
using namespace cppqedutils;

int main(int argc, char* argv[])
{
  parameters::Table p;
  dcomp         & alpha=p.add("alpha","",dcomp(-1,2));
  unsigned long & max  =p.add("max"  ,"",200ul      );

  update(p,argc,argv,"--");

  cerr<<max_factorial<double>::value<<" "<<max_factorial<long double>::value<<endl;
  for (unsigned long n=1; n<max; ++n) {
    const dcomp
      straight(n<max_factorial<double>::value ? pow(alpha,n)/sqrt(factorial<double>(n)) : dcomp()),
      stirling(n ? pow(2*n*PI,-.25)*pow(alpha/sqrt(n/EULER),n) : dcomp(1.));
    cout<<n<<" "<<straight.real()<<" "<<straight.imag()<<" "<<stirling.real()<<" "<<stirling.imag()<<" "<<abs((straight-stirling)/(straight+stirling))<<endl;
  }
}


#pragma once

#include "types.hh"

#include <Eigen/Dense>

namespace sonder {

Eigen::AngleAxisf vector3f_to_angleaxis(const Eigen::Vector3f &vec);

// Construct a rotation between two vectors (Non-unique)
//
//
Eigen::Quaternionf create_rotation_to(const Eigen::Vector3f &from, const Eigen::Vector3f &to);

// Construct a rotation into a frame defined by two vectors
//
//
Eigen::Quaternionf rotation_from_xy(const Eigen::Vector3f &frame_x, const Eigen::Vector3f &frame_y);

Eigen::Vector3f any_perpendicular(const Eigen::Vector3f &direction);
}


/*
 * Copyright 2015 Christoph Jud (christoph.jud@unibas.ch)
 *
 * 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.
 *
 */

#include <iostream>
#include <memory>
#include <ctime>
#include <cmath>

#include <boost/random.hpp>

#include "GaussianProcess.h"
#include "Kernel.h"

using namespace gpr;

typedef double ScalarType;
typedef Kernel<ScalarType>  KernelType;
typedef std::shared_ptr<KernelType> KernelTypePointer;
typedef GaussianProcess<ScalarType> GaussianProcessType;
typedef std::shared_ptr<GaussianProcessType> GaussianProcessTypePointer;

typedef typename GaussianProcessType::VectorType VectorType;
typedef typename GaussianProcessType::MatrixType MatrixType;

void Test1(){
    /*
     * Test 1: derivative test of kernels
     */
    std::cout << "Test 1.1: gaussian kernel derivative... " << std::flush;

    VectorType x = VectorType::Zero(2);
    x(0) = 0.1; x(1) = 0.5;

    VectorType y = VectorType::Zero(2);
    y(0) = -0.1; y(1) = 0.8;

    // typedefs
    typedef GaussianKernel<ScalarType>    GaussianKernelType;
    typedef std::shared_ptr<GaussianKernelType>                 GaussianKernelTypePointer;

    double h = 0.001;
    VectorType err = VectorType::Zero(2);
    unsigned counter = 0;
    for(double sigma=0.1; sigma<10; sigma+=0.4){
        for(double scale=0.1; scale<3; scale+=0.8){
            // analytical derivative
            GaussianKernelTypePointer gk(new GaussianKernelType(sigma, scale));
            VectorType D = gk->GetDerivative(x,y);

            // scale central difference
            GaussianKernelTypePointer gk1_scale(new GaussianKernelType(sigma, scale+h/2));
            GaussianKernelTypePointer gk2_scale(new GaussianKernelType(sigma, scale-h/2));
            double cd_scale = (*gk1_scale)(x,y) - (*gk2_scale)(x,y);
            cd_scale/=h;

            // sigma central difference
            GaussianKernelTypePointer gk1_sigma(new GaussianKernelType(sigma+h/2, scale));
            GaussianKernelTypePointer gk2_sigma(new GaussianKernelType(sigma-h/2, scale));
            double cd_sigma = (*gk1_sigma)(x,y) - (*gk2_sigma)(x,y);
            cd_sigma/=h;

            err[0] += std::fabs(cd_sigma-D[0]);
            err[1] += std::fabs(cd_scale-D[1]);

            counter++;
        }
    }

    if(err[0]/counter < 1e-5 && err[1]/counter < 1e-12){
        std::cout << "\t[passed]." << std::endl;
    }
    else{
        std::stringstream ss; ss<<"(sigma) " << err[0]/counter << " and (scale) " << err[1]/counter; throw ss.str();
    }
    return;

}

void Test1_2(){
    /*
     * Test 1.2: derivative test of kernels
     */
    std::cout << "Test 1.2: gaussian kernel derivative with exponentiated parameters... " << std::flush;

    VectorType x = VectorType::Zero(2);
    x(0) = 0.1; x(1) = 0.5;

    VectorType y = VectorType::Zero(2);
    y(0) = -0.1; y(1) = 0.8;

    // typedefs
    typedef GaussianExpKernel<ScalarType>           GaussianExpKernelType;
    typedef std::shared_ptr<GaussianExpKernelType>  GaussianExpKernelTypePointer;

    double h = 0.001;
    VectorType err = VectorType::Zero(2);
    unsigned counter = 0;
    for(double sigma=0.1; sigma<10; sigma+=0.4){
        for(double scale=0.1; scale<3; scale+=0.8){
            // analytical derivative
            GaussianExpKernelTypePointer gk(new GaussianExpKernelType(sigma, scale));
            VectorType D = gk->GetDerivative(x,y);

            // scale central difference
            GaussianExpKernelTypePointer gk1_scale(new GaussianExpKernelType(sigma, scale+h/2));
            GaussianExpKernelTypePointer gk2_scale(new GaussianExpKernelType(sigma, scale-h/2));
            double cd_scale = (*gk1_scale)(x,y) - (*gk2_scale)(x,y);
            cd_scale/=h;

            // sigma central difference
            GaussianExpKernelTypePointer gk1_sigma(new GaussianExpKernelType(sigma+h/2, scale));
            GaussianExpKernelTypePointer gk2_sigma(new GaussianExpKernelType(sigma-h/2, scale));
            double cd_sigma = (*gk1_sigma)(x,y) - (*gk2_sigma)(x,y);
            cd_sigma/=h;

            err[0] += std::fabs(cd_sigma-D[0]);
            err[1] += std::fabs(cd_scale-D[1]);

            counter++;
        }
    }

    if(err[0]/counter < 1e-6 && err[1]/counter < 1e-3){
        std::cout << "\t[passed]." << std::endl;
    }
    else{
        std::stringstream ss; ss<<"(sigma) " << err[0]/counter << " and (scale) " << err[1]/counter; throw ss.str();
    }
    return;
}

void Test2(){
    /*
     * Test 2: derivative test of kernels
     */
    std::cout << "Test 2: white kernel derivative... " << std::flush;

    VectorType x = VectorType::Zero(2);
    x(0) = 0.1; x(1) = 0.5;

    VectorType y = VectorType::Zero(2);
    y(0) = -0.1; y(1) = 0.8;

    // typedefs
    typedef WhiteKernel<ScalarType>              KernelType;
    typedef std::shared_ptr<KernelType>          KernelTypePointer;

    double h = 0.1;
    VectorType err1 = VectorType::Zero(1);
    VectorType err2 = VectorType::Zero(1);
    unsigned counter = 0;

    for(double scale=0.1; scale<3; scale+=0.8){
        // analytical derivative
        KernelTypePointer k(new KernelType(scale));
        VectorType D1 = k->GetDerivative(x,y);
        VectorType D2 = k->GetDerivative(x,x);

        // scale central difference
        KernelTypePointer k1_scale(new KernelType(scale+h/2));
        KernelTypePointer k2_scale(new KernelType(scale-h/2));
        double cd_scale1 = (*k1_scale)(x,y) - (*k2_scale)(x,y);
        cd_scale1/=h;
        double cd_scale2 = (*k1_scale)(x,x) - (*k2_scale)(x,x);
        cd_scale2/=h;

        err1[0] += std::fabs(cd_scale1-D1[0]);
        err2[0] += std::fabs(cd_scale2-D2[0]);
        counter++;
    }

    if(err1[0]/counter == 0 && err2[0]/counter < 1e-13){
        std::cout << "\t[passed]." << std::endl;
    }
    else{
        std::stringstream ss; ss<<"scale 1: " << err1[0]/counter << ", scale 2: "<<err2[0]/counter; throw ss.str();
    }
}

void Test3(){
    /*
     * Test 3: derivative test of kernels
     */
    std::cout << "Test 3: rational quadratic kernel derivative... " << std::flush;

    VectorType x = VectorType::Zero(2);
    x(0) = 0.1; x(1) = 0.5;

    VectorType y = VectorType::Zero(2);
    y(0) = -0.1; y(1) = 0.8;

    // typedefs
    typedef RationalQuadraticKernel<ScalarType>              KernelType;
    typedef std::shared_ptr<KernelType>          KernelTypePointer;

    double h = 0.01;
    VectorType err = VectorType::Zero(3);
    unsigned counter = 0;

    for(double scale=0.1; scale<3; scale+=0.8){
        for(double sigma=0.2; sigma<10; sigma+=0.6){
            for(double alpha=0.1; alpha<6; alpha+=0.6){
                // analytical derivative
                KernelTypePointer k(new KernelType(scale, sigma, alpha));
                VectorType D = k->GetDerivative(x,y);

                // scale central difference
                KernelTypePointer k1_scale(new KernelType(scale+h/2, sigma, alpha));
                KernelTypePointer k2_scale(new KernelType(scale-h/2, sigma, alpha));
                double cd_scale = (*k1_scale)(x,y) - (*k2_scale)(x,y);
                cd_scale/=h;

                // sigma central difference
                KernelTypePointer k1_sigma(new KernelType(scale, sigma+h/2, alpha));
                KernelTypePointer k2_sigma(new KernelType(scale, sigma-h/2, alpha));
                double cd_sigma = (*k1_sigma)(x,y) - (*k2_sigma)(x,y);
                cd_sigma/=h;

                // alpha central difference
                KernelTypePointer k1_alpha(new KernelType(scale, sigma, alpha+h/2));
                KernelTypePointer k2_alpha(new KernelType(scale, sigma, alpha-h/2));
                double cd_alpha = (*k1_alpha)(x,y) - (*k2_alpha)(x,y);
                cd_alpha/=h;

                err[0] += std::fabs(cd_scale-D[0]);
                err[1] += std::fabs(cd_sigma-D[1]);
                err[2] += std::fabs(cd_alpha-D[2]);
                counter++;
            }
        }
    }

    // sigma and alpha are not that stable with central difference

    if(err[0]/counter < 1e-13 && err[1]/counter < 0.001 && err[2]/counter < 1e-4){
        std::cout << "\t\t[passed]." << std::endl;
    }
    else{
        std::stringstream ss; ss<<"scale: " << err[0]/counter << ", sigma: "<<err[1]/counter << ", alpha: "<<err[2]/counter; throw ss.str();
    }
}

void Test4(){
    /*
     * Test 4: derivative test of kernels
     */
    std::cout << "Test 4: periodic kernel derivative... " << std::flush;

    VectorType x = VectorType::Zero(2);
    x(0) = 0.1; x(1) = 0.5;

    VectorType y = VectorType::Zero(2);
    y(0) = -0.1; y(1) = 0.8;

    // typedefs
    typedef PeriodicKernel<ScalarType>              KernelType;
    typedef std::shared_ptr<KernelType>          KernelTypePointer;

    double h = 0.01;
    VectorType err = VectorType::Zero(3);
    unsigned counter = 0;

    for(double scale=0.1; scale<3; scale+=0.8){
        for(double b=0.1; b<5*M_PI; b+=0.3){
            for(double sigma=0.1; sigma<4; sigma+=0.1){
                // analytical derivative
                KernelTypePointer k(new KernelType(scale, b, sigma));
                VectorType D = k->GetDerivative(x,y);

                // scale central difference
                KernelTypePointer k1_scale(new KernelType(scale+h/2, b, sigma));
                KernelTypePointer k2_scale(new KernelType(scale-h/2, b, sigma));
                double cd_scale = (*k1_scale)(x,y) - (*k2_scale)(x,y);
                cd_scale/=h;

                // sigma central difference
                KernelTypePointer k1_b(new KernelType(scale, b+h/2, sigma));
                KernelTypePointer k2_b(new KernelType(scale, b-h/2, sigma));
                double cd_b = (*k1_b)(x,y) - (*k2_b)(x,y);
                cd_b/=h;

                // alpha central difference
                KernelTypePointer k1_sigma(new KernelType(scale, b, sigma+h/2));
                KernelTypePointer k2_sigma(new KernelType(scale, b, sigma-h/2));
                double cd_sigma = (*k1_sigma)(x,y) - (*k2_sigma)(x,y);
                cd_sigma/=h;

                err[0] += std::fabs(cd_scale-D[0]);
                err[1] += std::fabs(cd_b-D[1]);
                err[2] += std::fabs(cd_sigma-D[2]);
                counter++;
            }
        }
    }

    if(err[0]/counter < 1e-13 && err[1]/counter < 1e-5 && err[2]/counter < 0.001){
        std::cout << "\t[passed]." << std::endl;
    }
    else{
        std::stringstream ss; ss<<"scale: "<<err[0]/counter << ", b: "<<err[1]/counter << ", sigma: "<<err[2]/counter; throw ss.str();
    }
}

void Test5(){
    /*
     * Test 5: derivative test of kernels
     */
    std::cout << "Test 5: sum of gaussian and periodic kernel derivative... " << std::flush;

    VectorType x = VectorType::Zero(2);
    x(0) = 0.1; x(1) = 0.5;

    VectorType y = VectorType::Zero(2);
    y(0) = -0.1; y(1) = 0.8;

    // typedefs
    typedef SumKernel<ScalarType>                ProductKernelType;
    typedef std::shared_ptr<ProductKernelType>       ProductKernelTypePointer;
    typedef GaussianKernel<ScalarType>           GaussianKernelType;
    typedef std::shared_ptr<GaussianKernelType>  GaussianKernelTypePointer;
    typedef PeriodicKernel<ScalarType>           PeriodicKernelType;
    typedef std::shared_ptr<PeriodicKernelType>  PeriodicKernelTypePointer;

    double h = 0.01;
    VectorType err = VectorType::Zero(5);
    unsigned counter = 0;

    for(double gscale=0.1; gscale<5; gscale+=0.8){
        for(double gsigma=0.1; gsigma<6; gsigma+=0.4){
            for(double pscale=0.1; pscale<4; pscale+=0.8){
                for(double b=0.1; b<5*M_PI; b+=0.4){
                    for(double psigma=0.2; psigma<6; psigma+=0.3){

                        // analytical derivative
                        GaussianKernelTypePointer gk(new GaussianKernelType(gsigma, gscale));
                        PeriodicKernelTypePointer pk(new PeriodicKernelType(pscale, b, psigma));
                        ProductKernelTypePointer k(new ProductKernelType(gk,pk));
                        VectorType D = k->GetDerivative(x,y);

                        // gaussian scale central difference
                        GaussianKernelTypePointer gk1_gscale(new GaussianKernelType(gsigma, gscale+h/2));
                        GaussianKernelTypePointer gk2_gscale(new GaussianKernelType(gsigma, gscale-h/2));
                        ProductKernelTypePointer k1_gscale(new ProductKernelType(gk1_gscale, pk));
                        ProductKernelTypePointer k2_gscale(new ProductKernelType(gk2_gscale, pk));
                        double cd_gscale = (*k1_gscale)(x,y) - (*k2_gscale)(x,y);
                        cd_gscale/=h;

                        // gaussian scale central difference
                        GaussianKernelTypePointer gk1_gsigma(new GaussianKernelType(gsigma+h/2, gscale));
                        GaussianKernelTypePointer gk2_gsigma(new GaussianKernelType(gsigma-h/2, gscale));
                        ProductKernelTypePointer k1_gsigma(new ProductKernelType(gk1_gsigma, pk));
                        ProductKernelTypePointer k2_gsigma(new ProductKernelType(gk2_gsigma, pk));
                        double cd_gsigma = (*k1_gsigma)(x,y) - (*k2_gsigma)(x,y);
                        cd_gsigma/=h;

                        // periodic scale central difference
                        PeriodicKernelTypePointer pk1_pscale(new PeriodicKernelType(pscale+h/2, b, psigma));
                        PeriodicKernelTypePointer pk2_pscale(new PeriodicKernelType(pscale-h/2, b, psigma));
                        ProductKernelTypePointer k1_pscale(new ProductKernelType(gk, pk1_pscale));
                        ProductKernelTypePointer k2_pscale(new ProductKernelType(gk, pk2_pscale));
                        double cd_pscale = (*k1_pscale)(x,y) - (*k2_pscale)(x,y);
                        cd_pscale/=h;

                        // periodic period length central difference
                        PeriodicKernelTypePointer pk1_pb(new PeriodicKernelType(pscale, b+h/2, psigma));
                        PeriodicKernelTypePointer pk2_pb(new PeriodicKernelType(pscale, b-h/2, psigma));
                        ProductKernelTypePointer k1_pb(new ProductKernelType(gk, pk1_pb));
                        ProductKernelTypePointer k2_pb(new ProductKernelType(gk, pk2_pb));
                        double cd_pb = (*k1_pb)(x,y) - (*k2_pb)(x,y);
                        cd_pb/=h;

                        // periodic sigma central difference
                        PeriodicKernelTypePointer pk1_psigma(new PeriodicKernelType(pscale, b, psigma+h/2));
                        PeriodicKernelTypePointer pk2_psigma(new PeriodicKernelType(pscale, b, psigma-h/2));
                        ProductKernelTypePointer k1_psigma(new ProductKernelType(gk, pk1_psigma));
                        ProductKernelTypePointer k2_psigma(new ProductKernelType(gk, pk2_psigma));
                        double cd_psigma = (*k1_psigma)(x,y) - (*k2_psigma)(x,y);
                        cd_psigma/=h;

                        err[0] += std::fabs(cd_gsigma-D[0]);
                        err[1] += std::fabs(cd_gscale-D[1]);
                        err[2] += std::fabs(cd_pscale-D[2]);
                        err[3] += std::fabs(cd_pb-D[3]);
                        err[4] += std::fabs(cd_psigma-D[4]);
                        counter++;
                    }
                }
            }
        }
    }

    if(err[0]/counter < 0.005 &&
            err[1]/counter < 1e-11 &&
            err[2]/counter < 1e-11 &&
            err[3]/counter < 1e-6 &&
            err[4]/counter < 1e-4){
        std::cout << "\t[passed]." << std::endl;
    }
    else{
        std::stringstream ss; ss<<"gsigma: "<<err[0]/counter << ", gscale: "<<err[1]/counter;
        ss<<", pscale: " << err[2]/counter << ", pb: " << err[3]/counter << ", psigma: " << err[4]/counter;
        throw ss.str();
    }
}

void Test6(){
    /*
     * Test 6: derivative test of kernels
     */
    std::cout << "Test 6: product of gaussian and periodic kernel derivative... " << std::flush;

    VectorType x = VectorType::Zero(2);
    x(0) = 0.1; x(1) = 0.5;

    VectorType y = VectorType::Zero(2);
    y(0) = -0.1; y(1) = 0.8;

    // typedefs
    typedef ProductKernel<ScalarType>            ProductKernelType;
    typedef std::shared_ptr<ProductKernelType>   ProductKernelTypePointer;
    typedef GaussianKernel<ScalarType>           GaussianKernelType;
    typedef std::shared_ptr<GaussianKernelType>  GaussianKernelTypePointer;
    typedef PeriodicKernel<ScalarType>           PeriodicKernelType;
    typedef std::shared_ptr<PeriodicKernelType>  PeriodicKernelTypePointer;

    double h = 0.01;
    VectorType err = VectorType::Zero(5);
    unsigned counter = 0;

    for(double gscale=0.4; gscale<4; gscale+=0.8){
        for(double gsigma=0.1; gsigma<5; gsigma+=0.4){
            for(double pscale=0.1; pscale<4; pscale+=0.8){
                for(double b=0.1; b<4*M_PI; b+=0.4){
                    for(double psigma=0.4; psigma<5; psigma+=0.3){

                        // analytical derivative
                        GaussianKernelTypePointer gk(new GaussianKernelType(gsigma, gscale));
                        PeriodicKernelTypePointer pk(new PeriodicKernelType(pscale, b, psigma));
                        ProductKernelTypePointer k(new ProductKernelType(gk,pk));
                        VectorType D = k->GetDerivative(x,y);

                        // gaussian scale central difference
                        GaussianKernelTypePointer gk1_gscale(new GaussianKernelType(gsigma, gscale+h/2));
                        GaussianKernelTypePointer gk2_gscale(new GaussianKernelType(gsigma, gscale-h/2));
                        ProductKernelTypePointer k1_gscale(new ProductKernelType(gk1_gscale, pk));
                        ProductKernelTypePointer k2_gscale(new ProductKernelType(gk2_gscale, pk));
                        double cd_gscale = (*k1_gscale)(x,y) - (*k2_gscale)(x,y);
                        cd_gscale/=h;

                        // gaussian scale central difference
                        GaussianKernelTypePointer gk1_gsigma(new GaussianKernelType(gsigma+h/2, gscale));
                        GaussianKernelTypePointer gk2_gsigma(new GaussianKernelType(gsigma-h/2, gscale));
                        ProductKernelTypePointer k1_gsigma(new ProductKernelType(gk1_gsigma, pk));
                        ProductKernelTypePointer k2_gsigma(new ProductKernelType(gk2_gsigma, pk));
                        double cd_gsigma = (*k1_gsigma)(x,y) - (*k2_gsigma)(x,y);
                        cd_gsigma/=h;

                        // periodic scale central difference
                        PeriodicKernelTypePointer pk1_pscale(new PeriodicKernelType(pscale+h/2, b, psigma));
                        PeriodicKernelTypePointer pk2_pscale(new PeriodicKernelType(pscale-h/2, b, psigma));
                        ProductKernelTypePointer k1_pscale(new ProductKernelType(gk, pk1_pscale));
                        ProductKernelTypePointer k2_pscale(new ProductKernelType(gk, pk2_pscale));
                        double cd_pscale = (*k1_pscale)(x,y) - (*k2_pscale)(x,y);
                        cd_pscale/=h;

                        // periodic period length central difference
                        PeriodicKernelTypePointer pk1_pb(new PeriodicKernelType(pscale, b+h/2, psigma));
                        PeriodicKernelTypePointer pk2_pb(new PeriodicKernelType(pscale, b-h/2, psigma));
                        ProductKernelTypePointer k1_pb(new ProductKernelType(gk, pk1_pb));
                        ProductKernelTypePointer k2_pb(new ProductKernelType(gk, pk2_pb));
                        double cd_pb = (*k1_pb)(x,y) - (*k2_pb)(x,y);
                        cd_pb/=h;

                        // periodic sigma central difference
                        PeriodicKernelTypePointer pk1_psigma(new PeriodicKernelType(pscale, b, psigma+h/2));
                        PeriodicKernelTypePointer pk2_psigma(new PeriodicKernelType(pscale, b, psigma-h/2));
                        ProductKernelTypePointer k1_psigma(new ProductKernelType(gk, pk1_psigma));
                        ProductKernelTypePointer k2_psigma(new ProductKernelType(gk, pk2_psigma));
                        double cd_psigma = (*k1_psigma)(x,y) - (*k2_psigma)(x,y);
                        cd_psigma/=h;

                        err[0] += std::fabs(cd_gsigma-D[0]);
                        err[1] += std::fabs(cd_gscale-D[1]);
                        err[2] += std::fabs(cd_pscale-D[2]);
                        err[3] += std::fabs(cd_pb-D[3]);
                        err[4] += std::fabs(cd_psigma-D[4]);
                        counter++;
                    }
                }
            }
        }
    }

    if(err[0]/counter < 0.009 &&
            err[1]/counter < 1e-11 &&
            err[2]/counter < 1e-11 &&
            err[3]/counter < 1e-5 &&
            err[4]/counter < 0.001){
        std::cout << "\t[passed]." << std::endl;
    }
    else{
        std::stringstream ss; ss<<"gsigma: "<<err[0]/counter << ", gscale: "<<err[1]/counter;
        ss<<", pscale: " << err[2]/counter << ", pb: " << err[3]/counter << ", psigma: " << err[4]/counter;
        throw ss.str();
    }
}

int main (int argc, char *argv[]){
    std::cout << "Product kernel test: " << std::endl;
    try{
        Test1();
        Test1_2();
        Test2();
        Test3();
        Test4();
        Test5();
        Test6();
    }
    catch(std::string& s){
        std::cout << "[failed] Error: " << s << std::endl;
        return -1;
    }

    return 0;
}


#ifndef MATH_UTILITY
#define MATH_UTILITY

#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <optional>
#include <type_traits>

namespace math {
namespace utility {

/**
 * @brief 適切に2πの倍数を加えることで角度を[-π, π]の範囲にラップする
 *
 * @param  x  ラップ対象の角度
 * @retval w  ラップ後の角度
 */
template <typename Float> constexpr inline Float wrap_pi(Float x) {
  static_assert(std::is_floating_point_v<Float>,
                "only makes sence for floating point types.");
  namespace bmc = boost::math::constants;
  constexpr Float y = x + bmc::pi<Float>();
  constexpr Float z =
      y - std::floor(y * bmc::half_pi<Float>()) * bmc::two_pi<Float>();
  constexpr Float w = z - bmc::pi<Float>();
  return w;
}

} // namespace utility
} // namespace math

#endif


#include <iostream>
#include <fstream>
#include<iomanip>

#include <cstdlib>
#include <stdio.h>
#include <string>
#include <vector>

#include <time.h>
#include <sys/time.h>

#include "mkl.h"
#include <omp.h>
//#include "mkl_service.h"
#include <fftw3.h>
#include <armadillo>
using namespace std;
using namespace arma;

double gettime(){
    struct timeval tv;
    gettimeofday(&tv,NULL);
    return tv.tv_sec*1000+tv.tv_usec/1000.0;
};

double *** al(int &n1,int &n2,int &n3){
    double ***p;
    p=new double**[n1];
    for (int i=0;i<n1;i++){
        p[i] = new double *[n2];
        for (int j=0;j<n2;j++) {
            p[i][j] = new double[n3];
        }
    }
    return p;
}

//template <typename T>
void del(double *** p, int &n1,int &n2){
    for(int i=0; i<n1; i++) {
        for (int j = 0; j < n2; j++) {
            delete [] p[i][j];
        }
        delete [] p[i];
    }
    delete  [] p;
    p=NULL;
}

double *** loadfile(int &n1, int &n2, int &n3, char *path){

    int count = 0;
    FILE* fp;
    char str[100];

    double ***v = al(n1,n2,n3);

    fp = fopen(path,"r");

    string tmp;
    while (fscanf(fp, "%s", str) != EOF)
    {
        int NUM=count;
        int k=NUM%(n1*n2);
        k=(NUM-k)/(n1*n2);
        int j=(NUM-k*n1*n2)%n2;
        int i=(NUM-k*n1*n2)/n2;
        tmp=str;
        v[i][j][k]=(double)atof(tmp.c_str());
        count++;
        if(count==n1*n2*n3){break;}
    }

    fclose(fp);

    return v;
}

void display(double *** v, int &n1, int &n2, int &n3){

    cout << "Tensor: " << endl;

    for (int k = 0; k < n3; k++) {
        for (int i = 0; i < n1; i++) {
            for (int j = 0; j < n2; j++) {
                cout << v[i][j][k] << " ";
            }
            cout << endl;
        }
        cout << endl;
    }
}

int main()
{
    double t0, t1, t2, t3, t4;
    int turn=2000;
    int n1 = turn, n2 = turn, n3 = 110;
    int N0 = floor(n3/2.0) + 1;

    char path[1000] = "/home/jcfei/Documents/MATLAB/data/a2000.txt";

    double ***M = loadfile(n1, n2, n3, path);
    cout << "loadfile" << endl;

    double ***v_t = al(n1, n2, N0);
    double ***v_t1 = al(n1, n2, N0);

    t0 = gettime();
    fftw_complex out[N0]; //fftw_alloc_real()
    double *in = fftw_alloc_real(n3);

    fftw_plan p_fft;
    p_fft = fftw_plan_dft_r2c_1d(n3, in, out, FFTW_ESTIMATE);
//    p=fftw_plan_dft_1d(n3,in,out,FFTW_FORWARD,FFTW_MEASURE);

//#pragma omp parallel for num_threads(8)
    for (int i = 0; i < n1; i++) {
        for (int j = 0; j < n2; j++) {
            in = M[i][j];
            fftw_execute_dft_r2c(p_fft, in, out);
            for (int k = 0; k < N0; k++) {
                v_t[i][j][k] = out[k][0];
                v_t1[i][j][k] = out[k][1];
            }
        }
    }
    del(M,n1,n2);
//    t1=gettime();
//    cout<<"fft time: "<<t1-t0<<endl;

    double ***uf = al(n1, n1, N0);
    double ***uf1 = al(n1, n1, N0);
    double ***theta = al(n1, n2, N0);
    double ***vf = al(n2, n2, N0);
    double ***vf1 = al(n2, n2, N0);

    cx_mat TMP = zeros<cx_mat>(n1, n2);
    cx_mat TMPU = zeros<cx_mat>(n1, n1);
    cx_mat TMPV = zeros<cx_mat>(n2, n2);
    colvec TMPT;

//    t2=gettime();
//    cout<<"alloc space: "<<t2-t1<<endl;

//#pragma omp parallel for num_threads(8)
    for (int k = 0; k < N0; k++) {
        for (int i = 0; i < n1; i++) {
            for (int j = 0; j < n2; j++) {
                TMP(i, j).real(v_t[i][j][k]);
                TMP(i, j).imag(v_t1[i][j][k]);
            }
        }
        svd(TMPU, TMPT, TMPV, TMP, "dc");
//        svd(TMPU,TMPT,TMPV,TMP,"std");

        for (int i = 0; i < n1; i++) {
            for (int j = 0; j < n1; j++) {
                uf[i][j][k] = TMPU(i, j).real();
                uf1[i][j][k] = TMPU(i, j).imag();
            }
        }
        for (int i = 0; i < n2; i++) {
            for (int j = 0; j < n2; j++) {
                vf[i][j][k] = TMPV(i, j).real();
                vf1[i][j][k] = TMPV(i, j).imag();
            }
        }
        if (n1 <= n2) {
            for (int i = 0; i < n1; i++) {
                theta[i][i][k] = TMPT(i);
            }
        } else {
            for (int i = 0; i < n2; i++) {
                theta[i][i][k] = TMPT(i);
            }
        }
    }

    del(v_t, n1, n2);
    del(v_t1, n1, n2);

    fftw_complex out1[N0]; //fftw_alloc_real()
    double *in1 = fftw_alloc_real(n3);
    p_fft = fftw_plan_dft_c2r_1d(n3, out1, in1, FFTW_ESTIMATE);

//    #pragma omp parallel for num_threads(8)
//    #pragma omp parallel for num_threads(2)

    double ***U = al(n1, n1, n3);

    for (int i = 0; i < n1; i++) {
        for (int j = 0; j < n1; j++) {
            for (int k = 0; k < N0; k++) {
                out1[k][0] = uf[i][j][k];
                out1[k][1] = uf1[i][j][k];
            }
            fftw_execute_dft_c2r(p_fft, out1, in1);

            for (int k = 0; k < n3; k++) {
                U[i][j][k] = 1.0 / n3 * in1[k];
            }
        }
    }

    del(uf, n1, n1);
    del(uf1, n1, n1);

    double ***V = al(n2, n2, n3);
    for (int i = 0; i < n2; i++) {
        for (int j = 0; j < n2; j++) {
            for (int k = 0; k < N0; k++) {
                out1[k][0] = vf[i][j][k];
                out1[k][1] = vf1[i][j][k];
            }
            fftw_execute_dft_c2r(p_fft, out1, in1);
            for (int k = 0; k < n3; k++) {
                V[i][j][k] = 1.0 / n3 * in1[k];
            }
        }
    }
    del(vf, n2, n2);
    del(vf1, n2, n2);

    double ***Theta = al(n1, n2, n3);
    for (int i = 0; i < n1; i++) {
        for (int j = 0; j < n2; j++) {
            for (int k = 0; k < N0; k++) {
                out1[k][0] = theta[i][j][k];
                out1[k][1] = 0;
            }
            fftw_execute_dft_c2r(p_fft, out1, in1);
            for (int k = 0; k < n3; k++) {
                Theta[i][j][k] = 1.0 / n3 * in1[k];
            }
        }
    }
    del(theta, n1, n2);

    fftw_destroy_plan(p_fft);

    t4 = gettime();
//    cout<<"ifft time: "<<t4-t3<<endl;
    cout << "Total time: " << t4 - t0 << endl;

    //fft transform result write to txt
//    ofstream a;
//    a.open("/home/jcfei/Desktop/TensorC++/txtToarray/Theta-video.txt");
//    for (int k=0; k<n3;k++){
//        for (int i=0; i<n1;i++){
//            for(int j=0; j<n2;j++){
//                a<<setiosflags(ios::right)<<setw(15)<<Theta[i][j][k]<<"  ";
//            }
//            a<<endl;
//        }
//    }
//    a.close();
//
//    ofstream b;
//    b.open("/home/jcfei/Desktop/TensorC++/txtToarray/U-video.txt");
//    for (int k=0; k<n3;k++){
//        for (int i=0; i<n1;i++){
//            for(int j=0; j<n1;j++){
//                b<<setiosflags(ios::right)<<setw(10)<<U[i][j][k]<<"  ";
//            }
//            b<<endl;
//        }
//    }
//    b.close();
//
//    ofstream c;
//    c.open("/home/jcfei/Desktop/TensorC++/txtToarray/V-video.txt");
//    for (int k=0; k<n3;k++){
//        for (int i=0; i<n2;i++){
//            for(int j=0; j<n2;j++){
//                c<<setiosflags(ios::right)<<setw(10)<<V[i][j][k]<<"  ";
//            }
//            c<<endl;
//        }
//    }
//    c.close();
    return 0;
}


/*!
 * @file diffusion_problem_ms.hpp
 * @brief Contains implementation of the main object for multiscale FEM.
 * @author Konrad Simon
 * @date August 2019
 */

#ifndef INCLUDE_DIFFUSION_PROBLEM_MS_HPP_
#define INCLUDE_DIFFUSION_PROBLEM_MS_HPP_

// Deal.ii
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/work_stream.h>
#include <deal.II/base/multithread_info.h>
#include <deal.II/base/timer.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/grid_generator.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>

#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>

// STL
#include <cmath>
#include <fstream>
#include <iostream>
#include <vector>

// My Headers
#include "matrix_coeff.hpp"
#include "right_hand_side.hpp"
#include "neumann_bc.hpp"
#include "dirichlet_bc.hpp"
#include "diffusion_problem_basis.hpp"

/*!
 * @namespace DiffusionProblem
 * @brief Contains implementation of the main object
 * and all functions to solve a
 * Dirichlet-Neumann problem on a unit square.
 */
namespace DiffusionProblem
{
using namespace dealii;

/*!
 * @class DiffusionProblemMultiscale
 * @brief Main class to solve
 * Dirichlet-Neumann problem on a unit square with
 * multiscale FEM.
 */
template <int dim>
class DiffusionProblemMultiscale
{
public:
	DiffusionProblemMultiscale (unsigned int n_refine, unsigned int n_refine_local);
	void run ();

private:
	// *********************************************
	// This is for threading the basis computation
	struct BasisScratchData
	{
		BasisScratchData () {}; // No implementation
		BasisScratchData (const BasisScratchData& /*scratch_data*/) {}; // No implementation
	};

	struct BasisCopyData
	{

	};

	void compute_local_basis(const typename std::vector<DiffusionProblemBasis<dim>>::iterator &it_basis,
								BasisScratchData	&scratch_data,
								BasisCopyData	&copy_data);
	void output_local_solution(const typename std::vector<DiffusionProblemBasis<dim>>::iterator &it_basis,
								BasisScratchData	&scratch_data,
								BasisCopyData	&copy_data);
	void fake_copy (const BasisCopyData&) {}; // No implementation
	// *********************************************

	void make_grid ();
	void initialize_basis_problem ();
	void compute_basis ();
	void setup_system ();
	void assemble_system ();
	void solve_iterative ();

	void send_global_weights_to_cell ();

	void output_global_coarse () const;
	void output_global_fine ();

	Triangulation<dim>   			triangulation;
	FE_Q<dim>            			fe;
	DoFHandler<dim>      			dof_handler;

	AffineConstraints<double> 		constraints;

	SparsityPattern      			sparsity_pattern;
	SparseMatrix<double> 			system_matrix;

	/*!
	 * Solution vector containing weights at the dofs.
	 */
	Vector<double>       			solution;

	/*!
	 * Contains all parts of the right-hand side needed to
	 * solve the linear system.
	 */
	Vector<double>       			system_rhs;

	/*!
	 * Number of global refinements.
	 */
	const unsigned int n_refine;

	/*!
	 * Number of local refinements.
	 */
	const unsigned int n_refine_local;

	/*!
	 * STL Vector holding basis functions for each coarse cell.
	 */
	std::vector<DiffusionProblemBasis<dim>> 	cell_basis_vector;
};

/*!
 * Constructor.
 */
template <int dim>
DiffusionProblemMultiscale<dim>::DiffusionProblemMultiscale (unsigned int n_refine, unsigned int n_refine_local) :
  fe (1),
  dof_handler (triangulation),
  n_refine(n_refine),
  n_refine_local(n_refine_local),
  cell_basis_vector(std::pow(2,dim*n_refine))
{}

/*!
 * Set all relevant data to local basis object and initialize the basis fully.
 */
template <int dim>
void DiffusionProblemMultiscale<dim>::initialize_basis_problem ()
{
	// First set up all cell problems serially
	typename Triangulation<dim>::active_cell_iterator
									cell = dof_handler.begin_active(),
									endc = dof_handler.end();
	unsigned int cell_number = 0;
	for (; cell!=endc; ++cell)
	{
		cell_basis_vector[cell_number].set_n_local_refinements (n_refine_local);
		cell_basis_vector[cell_number].set_cell_data (cell, cell_number);
		cell_basis_vector[cell_number].set_basis_data ();

		if (cell_number==0)
			cell_basis_vector[cell_number].set_output_flag (true);

		++cell_number;
	}
}

/*!
 * @brief Function pre-computes basis functions.
 */
template <int dim>
void DiffusionProblemMultiscale<dim>::compute_basis ()
{
	// Now run them in threads
	typename std::vector<DiffusionProblemBasis<dim>>::iterator
										it_basis = cell_basis_vector.begin (),
										it_endbasis = cell_basis_vector.end ();
	WorkStream::run(it_basis,
					it_endbasis,
					*this,
					&DiffusionProblemMultiscale<dim>::compute_local_basis,
					&DiffusionProblemMultiscale<dim>::fake_copy,
					BasisScratchData(),
					BasisCopyData());
}

/*!
 * Pre-compute the local basis. This function
 * is only used for threading.
 */
template <int dim>
void
DiffusionProblemMultiscale<dim>::compute_local_basis(const typename
														std::vector<DiffusionProblemBasis<dim>>::iterator &it_basis,
														BasisScratchData&,
														BasisCopyData&)
{
	it_basis->run ();
}

/*!
 * @brief Set up the grid with a certain number of refinements.
 *
 * Generate a triangulation of \f$[0,1]^{\rm{dim}}\f$ with edges/faces
 * numbered form \f$1,\dots,2\rm{dim}\f$.
 */
template <int dim>
void DiffusionProblemMultiscale<dim>::make_grid ()
{
	GridGenerator::hyper_cube (triangulation, 0, 1, /* colorize */ true);

	triangulation.refine_global (n_refine);

	std::cout << "Number of active cells: "
			<< triangulation.n_active_cells()
			<< std::endl;
}

/*!
 * @brief Setup sparsity pattern and system matrix.
 *
 * Compute sparsity pattern and reserve memory for the sparse system matrix
 * and a number of right-hand side vectors. Also build a constraint object
 * to take care of Dirichlet boundary conditions.
 */
template <int dim>
void DiffusionProblemMultiscale<dim>::setup_system ()
{
	dof_handler.distribute_dofs (fe);

	std::cout << std::endl
			<< "Number of active global cells:   "
			<< triangulation.n_active_cells()
			<< std::endl
			<< "Number of degrees of freedom:   " << dof_handler.n_dofs()
			<< std::endl
			<< std::endl;

	constraints.clear();
	DoFTools::make_hanging_node_constraints(dof_handler, constraints);

	/*
	 * Set up Dirichlet boundary conditions.
	 */
	const Coefficients::DirichletBC<dim> dirichlet_bc;
	for (unsigned int i = 0; i<dim; ++i)
	{
		VectorTools::interpolate_boundary_values(dof_handler,
													/*boundary id*/ 2*i, // only even boundary id
													dirichlet_bc,
													constraints);
	}

	constraints.close();

	DynamicSparsityPattern dsp(dof_handler.n_dofs());
	DoFTools::make_sparsity_pattern (dof_handler,
									dsp,
									constraints,
									/*keep_constrained_dofs =*/ true); // for time stepping this is essential to be true

	sparsity_pattern.copy_from(dsp);

	system_matrix.reinit (sparsity_pattern);

	solution.reinit (dof_handler.n_dofs());
	system_rhs.reinit (dof_handler.n_dofs());
}

/*!
 * @brief Assemble the system matrix and the static right hand side.
 *
 * Assembly routine to build the time-independent (static) part.
 * Neumann boundary conditions will be put on edges/faces
 * with odd number. Constraints are not applied here yet.
 */
template <int dim>
void DiffusionProblemMultiscale<dim>::assemble_system ()
{
	QGauss<dim - 1> face_quadrature_formula(fe.degree + 1);

	FEFaceValues<dim> 	fe_face_values(fe,
										face_quadrature_formula,
										update_values | update_quadrature_points |
										update_normal_vectors |
										update_JxW_values);

	const unsigned int   	dofs_per_cell = fe.dofs_per_cell;
	const unsigned int 		n_face_q_points = face_quadrature_formula.size();

	FullMatrix<double>   cell_matrix (dofs_per_cell, dofs_per_cell);
	Vector<double>       cell_rhs (dofs_per_cell);

	std::vector<types::global_dof_index> local_dof_indices (dofs_per_cell);

	/*
	 * Neumann BCs and vector to store the values.
	 */
	const Coefficients::NeumannBC<dim> 	neumann_bc;
	std::vector<double>  	neumann_values(n_face_q_points);

	// initialize basis iterator
	typename std::vector<DiffusionProblemBasis<dim>>::iterator
									it_basis = cell_basis_vector.begin();

	/*
	 * Integration over cells.
	 */
	for (const auto &cell: dof_handler.active_cell_iterators())
	{
		cell_matrix = 0;
		cell_rhs = 0;

		cell_matrix = it_basis->get_global_element_matrix ();
		cell_rhs = it_basis->get_global_element_rhs ();

		/*
		 * Boundary integral for Neumann values for odd boundary_id.
		 */
		for (unsigned int face_number = 0;
			 face_number < GeometryInfo<dim>::faces_per_cell;
			 ++face_number)
		{
			if (cell->face(face_number)->at_boundary() &&
					(
						(cell->face(face_number)->boundary_id() == 1) ||
						(cell->face(face_number)->boundary_id() == 3) ||
						(cell->face(face_number)->boundary_id() == 5)
					)
				)
			{
				fe_face_values.reinit(cell, face_number);

				// Fill in values at this particular face.
				neumann_bc.value_list(fe_face_values.get_quadrature_points(),
										   neumann_values);

				for (unsigned int q_face_point = 0; q_face_point < n_face_q_points; ++q_face_point)
				{
					for (unsigned int i = 0; i < dofs_per_cell; ++i)
					{
						cell_rhs(i) += neumann_values[q_face_point] // g(x_q)
										* fe_face_values.shape_value(i, q_face_point) // phi_i(x_q)
										* fe_face_values.JxW(q_face_point); // dS
					} // end ++i
				} // end ++q_face_point
			} // end if
		} // end ++face_number

		// get global indices
		cell->get_dof_indices (local_dof_indices);
		/*
		 * Now add the cell matrix and rhs to the right spots
		 * in the global matrix and global rhs. Constraints will
		 * be taken care of later.
		 */
		for (unsigned int i = 0; i < dofs_per_cell; ++i)
		{
			for (unsigned int j = 0; j < dofs_per_cell; ++j)
			{
				system_matrix.add(local_dof_indices[i],
							local_dof_indices[j],
							cell_matrix(i, j));
			}
			system_rhs(local_dof_indices[i]) += cell_rhs(i);
		}
	} // end ++cell
}

/*!
 * @brief Iterative solver.
 *
 * CG-based solver with SSOR-preconditioning.
 */
template <int dim>
void
DiffusionProblemMultiscale<dim>::solve_iterative ()
{
	SolverControl           solver_control (1000, 1e-12);
	SolverCG<>              solver (solver_control);

	PreconditionSSOR<> preconditioner;
	preconditioner.initialize(system_matrix, 1.2);

	solver.solve (system_matrix,
				solution,
				system_rhs,
				preconditioner);

	constraints.distribute (solution);

	std::cout << "   "
			<< "(global problem)   "
			<< solver_control.last_step()
			<< " coarse CG iterations needed to obtain convergence."
			<< std::endl;
}

/*!
 * @brief Send coarse weights to corresponding local cell.
 *
 * After the coarse (global) weights have been computed they
 * must be set to the local basis object and stored there.
 * This is necessary to write the local multiscale solution.
 */
template <int dim>
void
DiffusionProblemMultiscale<dim>::send_global_weights_to_cell ()
{
	// For each cell we get dofs_per_cell values
	const unsigned int   dofs_per_cell   = fe.dofs_per_cell;
	std::vector<types::global_dof_index> 	local_dof_indices (dofs_per_cell);

	// active cell iterator
	typename DoFHandler<dim>::active_cell_iterator
								cell = dof_handler.begin_active (),
								endc = dof_handler.end ();
	// initialize basis iterator
	typename std::vector<DiffusionProblemBasis<dim>>::iterator
									it_basis = cell_basis_vector.begin ();
	for (; cell!=endc; ++cell)
	{
		// Get local
		cell->get_dof_indices (local_dof_indices);

		std::vector<double> extracted_weights (dofs_per_cell, 0);
		solution.extract_subvector_to (local_dof_indices, extracted_weights);
		it_basis->set_global_weights (extracted_weights);

		// increase syncronously
		++it_basis;
	}
}

/*!
 * @brief Write coarse solution to disk.
 *
 * Write results for coarse solution to disk in vtu-format.
 */
template <int dim>
void
DiffusionProblemMultiscale<dim>::output_global_coarse () const
{
	DataOut<dim> data_out;
	data_out.attach_dof_handler (dof_handler);
	data_out.add_data_vector (solution, "solution");
	data_out.build_patches ();

	std::ofstream output (dim == 2 ?
					"solution-ms_coarse-2d.vtu" :
					"solution-ms_coarse-3d.vtu");

	data_out.write_vtu (output);
}

/*!
 * Output function to write local multiscale solution. Only used for threading output.
 */
template <int dim>
void
DiffusionProblemMultiscale<dim>::output_local_solution (
		const typename std::vector<DiffusionProblemBasis<dim>>::iterator &it_basis,
		BasisScratchData&,
		BasisCopyData&)
{
	it_basis->output_global_solution_in_cell ();
}

/*!
 * Write all local multiscale solution (threaded) and
 * a global pvtu-record.
 */
template <int dim>
void
DiffusionProblemMultiscale<dim>::output_global_fine ()
{
	// List of filenames of local outputs for master file
	std::vector<std::string> filenames_on_cell;

	// Now run them in threads
	typename std::vector<DiffusionProblemBasis<dim>>::iterator
											it_basis = cell_basis_vector.begin (),
											it_endbasis = cell_basis_vector.end ();
	WorkStream::run(it_basis,
					it_endbasis,
					*this,
					&DiffusionProblemMultiscale<dim>::output_local_solution,
					&DiffusionProblemMultiscale<dim>::fake_copy,
					BasisScratchData(),
					BasisCopyData());

	// Active cell iterator
	typename DoFHandler<dim>::active_cell_iterator
								cell = dof_handler.begin_active (),
								endc = dof_handler.end ();
	// Initialize const basis iterator again
	it_basis = cell_basis_vector.begin ();

	for (; cell!=endc; ++cell)
	{
		// Get the global file name
		filenames_on_cell.push_back ( it_basis->get_filename_global () );

		++it_basis;
	}

	// Build a*.pvtu file that points to all output files
	DataOut<dim> data_out;
	data_out.attach_dof_handler (dof_handler);

	// Names of solution components
	data_out.add_data_vector (solution, "solution");

	std::string filename_master = (dim == 2 ?
			"solution-ms_fine-2d" :
			"solution-ms_fine-3d");
	filename_master += ".pvtu";

	std::ofstream master_output (filename_master.c_str ());

	data_out.write_pvtu_record (master_output, filenames_on_cell);
}

/*!
 * @brief Run function of the object.
 *
 * Run the computation after object is built.
 */
template <int dim>
void DiffusionProblemMultiscale<dim>::run ()
{
	std::cout << std::endl
			<< "==========================================="
			<< std::endl;

	std::cout << "Solving problem in "
			<< dim << " space dimensions."
			<< std::endl;

	make_grid ();

	setup_system ();

	initialize_basis_problem ();
	compute_basis ();

	assemble_system ();

	// Now solve
	constraints.condense(system_matrix, system_rhs);
	solve_iterative ();

	send_global_weights_to_cell ();

	output_global_coarse ();
	output_global_fine ();

	std::cout << std::endl
			<< "==========================================="
			<< std::endl;
}

} // end namespace DiffusionProblem

#endif /* INCLUDE_DIFFUSION_PROBLEM_MS_HPP_ */