#include "benchmark.h"

#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <fstream>
#include <chrono>
#include <iostream>
#include <cstdlib>

namespace
{
	class Gaussian
	{
		static const double log2pi;

		size_t n_;
		Eigen::VectorXd mean_;
		Eigen::MatrixXd covariance_;
		Eigen::MatrixXd invCovariance_;
		double logdet_;
		Eigen::LLT<Eigen::MatrixXd> cholesky_;

		Eigen::VectorXd tmp_;

		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	public:
		Gaussian(const Eigen::VectorXd& mean, const Eigen::MatrixXd& cov)
			: n_(mean.size()), mean_(mean), covariance_(cov), logdet_(0)
		{
			invCovariance_.resizeLike(covariance_);
			computeDetInv();
		}
		void computeDetInv()
		{
			bool invertible = true;
			double det;
			if (n_ == 1) {
				covariance_.block<1, 1>(0, 0).computeInverseAndDetWithCheck(invCovariance_, det, invertible);
				logdet_ = std::log(det);
			}
			else if (n_ == 2) {
				covariance_.block<2, 2>(0, 0).computeInverseAndDetWithCheck(invCovariance_, det, invertible);
				logdet_ = std::log(det);
			}
			else if (n_ == 3) {
				covariance_.block<3, 3>(0, 0).computeInverseAndDetWithCheck(invCovariance_, det, invertible);
				logdet_ = std::log(det);
			}
			else if (n_ == 4) {
				covariance_.block<4, 4>(0, 0).computeInverseAndDetWithCheck(invCovariance_, det, invertible);
				logdet_ = std::log(det);
			}
			else {
				cholesky_.compute(covariance_);
				invertible = cholesky_.info() == Eigen::Success;
				//https://gist.github.com/redpony/fc8a0db6b20f7b1a3f23#gistcomment-2277286
				logdet_ = cholesky_.matrixL().toDenseMatrix().diagonal().array().log().sum();
			}
			if (!invertible)
			{
				std::cerr << "Covariance is singular, re-initialize with the identity matrix" << std::endl;
				covariance_.setIdentity(n_, n_);
				invCovariance_.setIdentity(n_, n_);
				logdet_ = 0; // log(det(In))=log(1)=0
			}
		}
		//Returns the log-probability of x in this gaussian
		double logP(const Eigen::VectorXd& x)
		{
			tmp_ = x - mean_;
			double alpha;
			if (n_ <= 4)
				alpha = tmp_.dot(invCovariance_*tmp_);
			else
				alpha = tmp_.dot(cholesky_.solve(tmp_));
			return -0.5 * (alpha + n_*log2pi + logdet_);
		}
		//getter+setter
		const Eigen::VectorXd& mean() const { return mean_; }
		Eigen::VectorXd& mean() { return mean_; }
		const Eigen::MatrixXd& cov() const { return covariance_; }
		Eigen::MatrixXd& cov() { return covariance_; }

		friend std::ostream& operator<<(std::ostream& o, const Gaussian& g)
		{
			o << "Mean: " << g.mean().transpose() << "\n"
				<< "Cov:\n" << g.cov();
			return o;
		}
	};
	const double Gaussian::log2pi = 1.8378770664093454835606594728112f;

	template<typename Derived, typename Scalar = typename Derived::Scalar>
	Scalar LSE(const Eigen::MatrixBase<Derived>& vec)
	{
		Scalar m = vec.maxCoeff();
		return m + std::log((vec.array() - m).exp().sum());
	}

}

void benchmark_Eigen(
	const std::string& pointsFile,
	const std::string& settingsFile,
	int numIterations,
    Json::Object& returnValues)
{
    //load settings and points
	int dimension, components, numPoints;
	Eigen::MatrixXd points;
	std::vector<double> logWeights;
	std::vector<Gaussian> gaussians;
	{ //points
		std::ifstream in(pointsFile);
		in >> dimension >> components >> numPoints;
		points.resize(dimension, numPoints);
		float dummy; //skip ground truth
		for (int i = 0; i < components * (1 + dimension + dimension * dimension); ++i) in >> dummy;
		for (int i = 0; i < numPoints; ++i) { //read points
			Eigen::VectorXd p(dimension);
			for (int d = 0; d < dimension; ++d) in >> p[d];
			points.col(i) = p;
		}
	}
	{ //initial settings
		std::ifstream in(settingsFile);
		int dummy;
		in >> dimension >> components >> dummy;
		logWeights.resize(components);
		gaussians.reserve(components);
		Eigen::VectorXd mean(dimension);
		Eigen::MatrixXd cov(dimension, dimension);
		for (int i=0; i<components; ++i)
		{
			double weight;
			in >> weight;
			logWeights[i] = std::log(weight);
			for (int x = 0; x < dimension; ++x) in >> mean[x];
			for (int x = 0; x < dimension; ++x)
				for (int y = 0; y < dimension; ++y)
					in >> cov(x, y);
			gaussians.emplace_back(mean, cov);
		}
	}

#ifndef NDEBUG
	for (int k = 0; k < components; ++k)
	{
		std::cout << "\nComponent " << k
			<< "\nWeight: " << std::exp(logWeights[k])
			<< "\n" << gaussians[k] << std::endl;
	}
#endif

	//temporary memory
	Eigen::MatrixXd logW(numPoints, components);

	//run EM (fixed number of iterations)
	std::cout << "Run EM algorithm" << std::endl;
	std::chrono::time_point<std::chrono::steady_clock> start;
	double logLikeliehoodAccum;
	for (int iter=0; iter < numIterations; ++iter)
	{
		//half of the iterations for warm up
		if (iter==numIterations/2) start = std::chrono::steady_clock::now();

#ifndef NDEBUG
		std::cout << "    Iteration " << iter << std::endl;
#endif

		//Precomputation
#pragma omp parallel for
		for (int i = 0; i < components; ++i)
			gaussians[i].computeDetInv();

		//E-Step
		logLikeliehoodAccum = 0;
		for (int i=0; i<numPoints; ++i)
		{
			//compute membership weight w_ik
			for (int k=0; k<components; ++k)
			{
				double lw = gaussians[k].logP(points.col(i)) + logWeights[k];
				logW(i, k) = lw;
			}
			double lse = LSE(logW.row(i));
			logW.row(i) -= Eigen::RowVectorXd::Constant(components, lse);
			logLikeliehoodAccum += lse;

#ifndef NDEBUG
			std::cout << "Point " << i << ":";
			for (int k = 0; k < components; ++k)
				std::cout << " " << std::exp(logW(i, k));
			std::cout << std::endl;
#endif
		}

		//M-Step
		for (int k=0; k<components; ++k)
		{
#ifndef NDEBUG
			std::cout << "\nComponent " << k << " pre-update:"
				<< "\nWeight: " << std::exp(logWeights[k])
				<< "\n" << gaussians[k] << std::endl;
#endif

			double logNk = LSE(logW.col(k));
			double divNk = 1.0f / std::exp(logNk);
			logWeights[k] = logNk - std::log(numPoints);
			//Eigen does not support batches, so I have to write it explicitly as a loop here
			//(and broadcasting is very ugly)
			gaussians[k].mean().setZero();
			gaussians[k].cov().setZero();
			for (int i = 0; i < numPoints; ++i)
				gaussians[k].mean() += std::exp(logW(i, k)) * points.col(i);
			gaussians[k].mean() *= divNk;
			for (int i = 0; i < numPoints; ++i)
				gaussians[k].cov() += std::exp(logW(i, k)) *
					(points.col(i) - gaussians[k].mean()) * (points.col(i) - gaussians[k].mean()).transpose();
			gaussians[k].cov() = gaussians[k].cov()*divNk
			+ Eigen::MatrixXd::Identity(dimension, dimension) * 1e-1;

#ifndef NDEBUG
			std::cout << "Component " << k << " post-update:"
				<< "\nWeight: " << std::exp(logWeights[k])
				<< "\n" << gaussians[k] << std::endl;
#endif
		}

#ifndef NDEBUG
		std::cout << " -> log-likelihood: " << logLikeliehoodAccum << "\n";
#endif
	}
#ifdef NDEBUG
	std::cout << " -> log-likelihood: " << logLikeliehoodAccum << "\n";
#endif

	auto finish = std::chrono::steady_clock::now();
	double elapsed = std::chrono::duration_cast<
		std::chrono::duration<double>>(finish - start).count() * 1000 * 2; //*2 for warm up
	std::cout << "    Done in " << elapsed << "ms" << std::endl;

	//save results
	returnValues.Insert(std::make_pair("Time", elapsed));
	returnValues.Insert(std::make_pair("LogLikelihood", logLikeliehoodAccum));
	Json::Array comData;
	for (int k=0; k<components; ++k)
	{
		Json::Object com;
		com.Insert(std::make_pair("Weight", std::exp(logWeights[k])));
		Json::Array m, c;
		for (int i = 0; i < dimension; ++i) m.PushBack(gaussians[k].mean()[i]);
		for (int i = 0; i < dimension; ++i)
			for (int j = 0; j < dimension; ++j)
				c.PushBack(gaussians[k].cov()(i, j));
		com.Insert(std::make_pair("Mean", m));
		com.Insert(std::make_pair("Cov", c));
		comData.PushBack(com);
	}
	returnValues.Insert(std::make_pair("Components", comData));
}


/*! \file mabinogi_roulette_mc.cpp
    \brief マビノギのルーレットビンゴをモンテカルロ・シミュレーションする

    Copyright © 2015-2017 @dc1394 All Rights Reserved.
    This software is released under the BSD 2-Clause License.
*/

#include "../checkpoint/checkpoint.h"
#include "goexit/goexit.h"
#ifdef HAVE_SSE2
	#include "myrandom/myrandsfmt.h"
#else
	#include "myrandom/myrand.h"
#endif
#include <algorithm>                            // for std::shuffle
#include <cstdint>                              // for std::int32_t
#include <cmath>                                // for std::sqrt
#ifdef _MSC_VER
	#include <format>                           // for std::format
#endif
#include <fstream>                              // for std::ofstream
#include <iostream>                             // for std::cout
#include <iterator>                             // for std::begin, std::ostream_iterator
#include <map>                                  // for std::map
#include <random>                               // for std::mt19937
#include <unordered_map>                        // for std::unordered_map
#include <utility>                              // for std::make_pair, std::move
#include <vector>                               // for std::vector
#include <valarray>                             // for std::valarray
#include <boost/algorithm/cxx11/iota.hpp>       // for boost::algorithm::iota
#ifndef _MSC_VER
	#include <boost/format.hpp>                 // for boost::format
#endif
#include <boost/range/algorithm.hpp>            // for boost::find, boost::max_element, boost::transform
#include <tbb/concurrent_vector.h>              // for tbb::concurrent_vector
#include <tbb/parallel_for.h>                   // for tbb::parallel_for

namespace {
    //! A global variable (constant expression).
    /*!
        列のサイズ
    */
    static auto constexpr COLUMN = 5ULL;

    //! A global variable (constant expression).
    /*!
        行のサイズ
    */
    static auto constexpr ROW = 5ULL;

    //! A global variable (constant expression).
    /*!
        ビンゴボードのマス数
    */
    static auto constexpr BOARDSIZE = ROW * COLUMN;

    //! A global variable (constant expression).
    /*!
        モンテカルロシミュレーションの試行回数
    */
    static auto constexpr MCMAX = 1000000U;

    //! A global variable (constant expression).
    /*!
        行・列の総数
    */
    static auto constexpr ROWCOLUMN = ROW + COLUMN;

    //! A typedef.
    /*!
        そのマスに書かれてある番号と、そのマスが当たったかどうかを示すフラグのstd::pair
    */
    using mypair = std::pair<std::int32_t, bool>;

    //! A typedef.
    /*!
        数字と数字のstd::pair
    */
    using mypair2 = std::pair<std::int32_t, std::int32_t>;

    //! A typedef.
    /*!
        (n + 1)個目の行・列が埋まったときの分布を格納するためのmapの型
    */
    using mymap = std::map<std::int32_t, std::int32_t>;

	//! A function.
	/*!
		(n + 1)個目の行・列またはマスが埋まったときの平均試行回数、埋まっているマスまたは行・列の平均個数を求める
		\param mcresult モンテカルロ・シミュレーションの結果が格納された二次元可変長配列
		\param size 行・列またはマスの総数
		\return (n + 1)個目の行・列が埋まったときの平均試行回数、埋まっているマスの平均個数が格納された可変長配列のstd::pair
	*/
	std::pair< std::valarray<double>, std::valarray<double> > eval_average(tbb::concurrent_vector< std::vector<mypair2> > const & mcresult, std::size_t size);

	//! A function.
	/*!
		(n + 1)個目の行・列が埋まったときの中央値を求める
		\param (n + 1)個目の数値n
		\param mcresult モンテカルロ・シミュレーションの結果が格納された二次元可変長配列
		\return (n + 1)個目の行・列が埋まったときの中央値
	*/
	std::int32_t eval_median(tbb::concurrent_vector< std::vector<mypair2> > const & mcresult, std::int32_t n);

	//! A function.
	/*!
		(n + 1)個目の行・列が埋まったときの最頻値と分布を求める
		\param (n + 1)個目の数値n
		\param mcresult モンテカルロ・シミュレーションの結果が格納された二次元可変長配列
		\return (n + 1)個目の行・列が埋まったときの最頻値と分布のstd::pair
	*/
	std::pair<std::int32_t, std::map<std::int32_t, std::int32_t> > eval_mode(tbb::concurrent_vector< std::vector<mypair2> > const & mcresult, std::int32_t n);

	//! A function.
	/*!
		(n + 1)個目の行・列が埋まったときの標準偏差を求める
		\param avgten (n + 1)個目の行・列が埋まったときの平均試行回数
		\param (n + 1)個目の数値n
		\param mcresult モンテカルロ・シミュレーションの結果が格納された二次元可変長配列
		\return (n + 1)個目の行・列が埋まったときの標準偏差
	*/
	double eval_std_deviation(double avg, tbb::concurrent_vector< std::vector<mypair2> > const & mcresult, std::int32_t n);

    //! A function.
    /*!
        ビンゴボードを生成する
        \return ビンゴボードが格納された可変長配列
    */
    auto makeboard();

#ifdef _CHECK_PARALELL_PERFORM
    //! A function.
    /*!
        モンテカルロ・シミュレーションを行う
        \return モンテカルロ・シミュレーションの結果が格納された二次元可変長配列
    */
	std::pair<std::vector< std::vector<mypair2> >, std::vector< std::vector<mypair2> > > montecarlo();
#endif

    //! A function.
    /*!
        モンテカルロ・シミュレーションの実装
        \param mr 自作乱数クラスのオブジェクト
        \return モンテカルロ法の結果が格納された可変長配列
    */
	template <typename MyRandom>
	std::pair<std::vector<mypair2>, std::vector<mypair2> > montecarloImpl(MyRandom & mr);

    //! A function.
    /*!
        モンテカルロ・シミュレーションをTBBで並列化して行う
        \return モンテカルロ・シミュレーションの結果が格納された二次元可変長配列
    */
	std::pair<tbb::concurrent_vector< std::vector<mypair2> >, tbb::concurrent_vector< std::vector<mypair2> > > montecarloTBB();

    //! A function.
    /*!
        (n + 1)個目の行・列が埋まったときの分布をcsvファイルに出力する
		\param distmap (n + 1)個目の行・列が埋まったときの分布
		\param filename ファイル名
    */
    void outputcsv(mymap const & distmap, std::string const & filename);
}

int main()
{
    checkpoint::CheckPoint cp;

    cp.checkpoint("処理開始", __LINE__);

#ifdef _CHECK_PARALELL_PERFORM
    // モンテカルロ・シミュレーションの結果を代入
    auto const mcresult(montecarlo());

    cp.checkpoint("並列化無効", __LINE__);
#endif

    // TBBで並列化したモンテカルロ・シミュレーションの結果を代入
    auto const mcresult2(montecarloTBB());

    cp.checkpoint("並列化有効", __LINE__);

    auto const [trialavg, fillavg] = eval_average(mcresult2.first, ROWCOLUMN);

    for (auto n = 0U; n < ROWCOLUMN; n++) {
		auto const [mode, distmap] = eval_mode(mcresult2.first, n);
#ifdef _MSC_VER
		outputcsv(distmap, std::format("result/distribution_{:d}個目.csv", n + 1));

        std::cout
			<< std::format("ビンゴ{:d}個目に必要な平均試行回数：{:.1f}回, 効率：{:.1f}(回/個), ", n + 1, trialavg[n], trialavg[n] / static_cast<double>(n + 1))
			<< std::format("中央値：{:d}回, 最頻値：{:d}回, 標準偏差：{:.1f}, ", eval_median(mcresult2.first, n), mode, eval_std_deviation(trialavg[n], mcresult2.first, n))
			<< std::format("埋まっているマスの平均個数：{:.1f}個\n", fillavg[n]);
#else
        outputcsv(distmap, (boost::format("result/distribution_%d個目.csv") % (n + 1)).str());

        std::cout
            << boost::format("ビンゴ%d個目に必要な平均試行回数：%.1f回, 効率：%.1f(回/個), ")
            % (n + 1)
            % trialavg[n]
            % (trialavg[n] / static_cast<double>(n + 1))
            << boost::format("中央値：%d回, 最頻値：%d回, 標準偏差：%.1f, ")
            % eval_median(mcresult2.first, n)
            % mode
            % eval_std_deviation(trialavg[n], mcresult2.first, n)
            << boost::format("埋まっているマスの平均個数：%.1f個\n")
            % fillavg[n];
#endif
    }

	auto const [trialavg2, fillavg2] = eval_average(mcresult2.second, BOARDSIZE);

	for (auto n = 0U; n < BOARDSIZE; n++) {
		auto const [mode, distmap] = eval_mode(mcresult2.second, n);
#ifdef _MSC_VER
        outputcsv(distmap, std::format("result/distribution2_{:d}個目.csv", n + 1));

        std::cout
            << std::format("{:d}個目のマスに必要な平均試行回数：{:.1f}回, 効率：{:.1f}(回/個), ", n + 1, trialavg2[n], trialavg2[n] / static_cast<double>(n + 1))
            << std::format("中央値：{:d}回, 最頻値：{:d}回, 標準偏差：{:.1f}, ", eval_median(mcresult2.second, n), mode, eval_std_deviation(trialavg2[n], mcresult2.second, n))
            << std::format("埋まっている行・列の平均個数：{:.1f}個\n", fillavg2[n]);
#else
		outputcsv(distmap, (boost::format("result/distribution2_%d個目.csv") % (n + 1)).str());

		std::cout
			<< boost::format("%d個目のマスに必要な平均試行回数：%.1f回, 効率：%.1f(回/個), ")
			% (n + 1)
			% trialavg2[n]
			% (trialavg2[n] / static_cast<double>(n + 1))
			<< boost::format("中央値：%d回, 最頻値：%d回, 標準偏差：%.1f, ")
			% eval_median(mcresult2.second, n)
			% mode
			% eval_std_deviation(trialavg2[n], mcresult2.second, n)
			<< boost::format("埋まっている行・列の平均個数：%.1f個\n")
			% fillavg2[n];
#endif
	}

    cp.checkpoint("それ以外の処理", __LINE__);

    cp.checkpoint_print();

	goexit::goexit();

    return 0;
}

namespace {
    std::pair< std::valarray<double>, std::valarray<double> > eval_average(tbb::concurrent_vector< std::vector<mypair2> > const & mcresult, std::size_t size)
    {
        // モンテカルロ・シミュレーションの平均試行回数の結果を格納した可変長配列
        std::valarray<double> trialavg(size);

        // モンテカルロ・シミュレーションのn回目の試行で、埋まっているマスの数を格納した可変長配列
        std::valarray<double> fillavg(size);

        // 行・列の総数分繰り返す
        for (auto n = 0U; n < size; n++) {
            // 総和を0で初期化
            auto trialsum = 0;
            auto fillsum = 0;

            // 試行回数分繰り返す
            for (auto j = 0U; j < MCMAX; j++) {
                // j回目の結果を加える
                trialsum += mcresult[j][n].first;
                fillsum += mcresult[j][n].second;
            }

            // 平均を算出してn行・列目のtrialavg、fillavgに代入
            trialavg[n] = static_cast<double>(trialsum) / static_cast<double>(MCMAX);
            fillavg[n] = static_cast<double>(fillsum) / static_cast<double>(MCMAX);
        }

        return std::make_pair(std::move(trialavg), std::move(fillavg));
    }

	std::int32_t eval_median(tbb::concurrent_vector< std::vector<mypair2> > const & mcresult, std::int32_t n)
	{
		// 中央値を求めるために必要な可変長配列
		std::vector<std::int32_t> medtmp(MCMAX);

		// 中央値を求めるために必要な可変長配列を、モンテカルロ法の結果から生成
		boost::transform(
			mcresult,
			medtmp.begin(),
			[n](auto const & res) { return res[n].first; });

		// 中央値を求めるためにソートする
		boost::sort(medtmp);

		// 中央値を求める
		if constexpr (MCMAX % 2) {
			// 要素が奇数個なら中央の要素を返す
			return medtmp[(MCMAX - 1) / 2];
		}
		else {
			// 要素が偶数個なら中央二つの平均を返す
			return (medtmp[(MCMAX / 2) - 1] + medtmp[MCMAX / 2]) / 2;
		}
	}

	std::pair<std::int32_t, mymap> eval_mode(tbb::concurrent_vector< std::vector<mypair2> > const & mcresult, std::int32_t n)
	{
		// (n + 1)個目の行・列が埋まったときの分布
		std::unordered_map<std::int32_t, std::int32_t> distmap;

		// distmapを埋める
		for (auto const & res : mcresult) {
			// (n + 1)個目の行・列が埋まったときの回数をkeyとする
			auto const key = res[n].first;

			// keyが存在するかどうか
			auto itr = distmap.find(key);
			if (itr == distmap.end()) {
				// keyが存在しなかったので、そのキーでハッシュを拡張（値1）
				distmap.emplace(key, 1);
			}
			else {
				// keyが指す値を更新
				itr->second++;
			}
		}

		// 最頻値を探索
		auto const mode = boost::max_element(
			distmap,
			[](auto const & p1, auto const & p2) { return p1.second < p2.second; })->first;

		// 最頻値と(n + 1)個目の行・列が埋まったときの分布をpairにして返す
		return std::make_pair(mode, mymap(distmap.begin(), distmap.end()));
	}

	double eval_std_deviation(double avg, tbb::concurrent_vector< std::vector<mypair2> > const & mcresult, std::int32_t n)
	{
		// 標準偏差を求めるために必要な可変長配列
		std::valarray<double> devtmp(MCMAX);

		// 標準偏差の計算
		boost::transform(
			mcresult,
			std::begin(devtmp),
			[avg, n](auto const & res) {
			auto const val = static_cast<double>(res[n].first);
			return (val - avg) * (val - avg);
		});

		// 標準偏差を求める
		return std::sqrt(devtmp.sum() / static_cast<double>(MCMAX));
	}

    auto makeboard()
    {
        // 仮のビンゴボードを生成
        std::vector<std::int32_t> boardtmp(BOARDSIZE);

        // 仮のビンゴボードに1～25の数字を代入
        boost::algorithm::iota(boardtmp, 1);

        // 仮のビンゴボードの数字をシャッフル
        std::shuffle(boardtmp.begin(), boardtmp.end(), std::mt19937());

        // ビンゴボードを生成
        std::vector<mypair> board(BOARDSIZE);

        // 仮のビンゴボードからビンゴボードを生成する
        boost::transform(
            boardtmp,
            board.begin(),
            [](auto n) { return std::make_pair(n, false); });

        // ビンゴボードを返す
        return board;
    }

#ifdef _CHECK_PARALELL_PERFORM
	std::pair<std::vector< std::vector<mypair2> >, std::vector< std::vector<mypair2> > > montecarlo()
    {
        // モンテカルロ・シミュレーションの結果を格納するための二次元可変長配列
		std::pair<std::vector< std::vector<mypair2> >, std::vector< std::vector<mypair2> > > mcresult;

		// MCMAX個の容量を確保
		mcresult.first.reserve(MCMAX);
		mcresult.second.reserve(MCMAX);

#ifdef HAVE_SSE2
		// 自作乱数クラスを初期化
		myrandom::MyRandSfmt mr(1, BOARDSIZE);
#else
		// 自作乱数クラスを初期化
		myrandom::MyRand mr(1, BOARDSIZE);
#endif
        // 試行回数分繰り返す
        for (auto n = 0U; n < MCMAX; n++) {
			// モンテカルロ・シミュレーションの結果を代入
			auto const [resf, ress] = montecarloImpl(mr);
			mcresult.first.emplace_back(resf);
			mcresult.second.emplace_back(ress);
        }

        // モンテカルロ・シミュレーションの結果を返す
        return mcresult;
    }
#endif

	template <typename MyRandom>
	std::pair<std::vector<mypair2>, std::vector<mypair2> > montecarloImpl(MyRandom & mr)
    {
        // ビンゴボードを生成
        auto board(makeboard());

        // その行・列が既に埋まっているかどうかを格納する可変長配列
        // ROWCOLUMN個の要素をfalseで初期化
        std::vector<bool> rcfill(ROWCOLUMN, false);

        // 行・列が埋まるまでに要した回数と、その時点で埋まったマスを格納した
        // 可変長配列
        std::vector<mypair2> fillnum;

		// (n + 1)個目のマスが埋まったときの回数と、その時点で埋まった行・列を格納した
        // 可変長配列
		std::vector<mypair2> fillnum2;

        // ROWCOLUMN個の容量を確保
        fillnum.reserve(ROWCOLUMN);

        // その時点で埋まっているマスを計算するためのラムダ式
        auto const sum = [](auto const & vec) {
            auto cnt = 0;
            for (auto & e : vec) {
                if (e.second) {
                    cnt++;
                }
            }

            return cnt;
        };

        // 無限ループ
        for (auto n = 1; ; n++) {
            // 乱数で得た数字で、かつまだ当たってないマスを検索
            auto itr = boost::find(board, std::make_pair(mr.myrand(), false));

            // そのようなマスがあった
            if (itr != board.end()) {
                // そのマスは当たったとし、フラグをtrueにする
                itr->second = true;
            }
            // そのようなマスがなかった
            else {
                //ループ続行
                continue;
            }

            // 各行・列が埋まったかどうかをチェック
            for (auto j = 0U; j < ROW; j++) {
                // 各行が埋まったかどうかのフラグ
                auto rowflag = true;

                // 各行が埋まったかどうかをチェック
                for (auto k = 0U; k < COLUMN; k++) {
                    rowflag &= board[COLUMN * j + k].second;
                }

                // 行の処理
                if (rowflag &&
                    // その行は既に埋まっているかどうか
                    !rcfill[j]) {
                    // その行は埋まったとして、フラグをtrueにする
                    rcfill[j] = true;

                    // 要した試行回数と、その時点で埋まったマスの数を格納
                    fillnum.emplace_back(n, sum(board));
                }

                // 各列が埋まったかどうかのフラグ
                auto columnflag = true;

                // 各列が埋まったかどうかをチェック
                for (auto k = 0U; k < ROW; k++) {
                    columnflag &= board[j + COLUMN * k].second;
                }

                // 列の処理
                if (columnflag &&
                    // その列は既に埋まっているかどうか
                    !rcfill[j + ROW]) {

                    // その列は埋まったとして、フラグをtrueにする
                    rcfill[j + ROW] = true;

                    // 要した試行回数と、その時点で埋まったマスの数を格納
                    fillnum.emplace_back(n, sum(board));
                }
            }

			// 要した試行回数と、その時点で埋まっている行・列の数を格納
			fillnum2.emplace_back(n, static_cast<std::int32_t>(fillnum.size()));

            // 全ての行・列が埋まったかどうか
            if (fillnum.size() == ROWCOLUMN) {
                // 埋まったのでループ脱出
                break;
            }
        }

        // 要した試行関数の可変長配列を返す
        return std::make_pair(std::move(fillnum), std::move(fillnum2));
    }

    std::pair<tbb::concurrent_vector< std::vector<mypair2> >, tbb::concurrent_vector< std::vector<mypair2> > > montecarloTBB()
    {
        // モンテカルロ・シミュレーションの結果を格納するための二次元可変長配列
        // 複数のスレッドが同時にアクセスする可能性があるためtbb::concurrent_vectorを使う
        std::pair<tbb::concurrent_vector< std::vector<mypair2> >, tbb::concurrent_vector< std::vector<mypair2> > > mcresult;

        // MCMAX個の容量を確保
        mcresult.first.reserve(MCMAX);
		mcresult.second.reserve(MCMAX);

        // MCMAX回のループを並列化して実行
        tbb::parallel_for(
            0U,
            MCMAX,
            1U,
            [&mcresult](auto) {

#ifdef HAVE_SSE2
			// 自作乱数クラスを初期化
			myrandom::MyRandSfmt mr(1, BOARDSIZE);
#else
			// 自作乱数クラスを初期化
			myrandom::MyRand mr(1, BOARDSIZE);
#endif

            // モンテカルロ・シミュレーションの結果を代入
			auto const [resf, ress] = montecarloImpl(mr);
            mcresult.first.emplace_back(resf);
			mcresult.second.emplace_back(ress);
        });

        // モンテカルロ・シミュレーションの結果を返す
        return mcresult;
    }

    void outputcsv(mymap const & distmap, std::string const & filename)
    {
        std::ofstream ofs(filename);

        boost::transform(
            distmap,
            std::ostream_iterator<std::string>(ofs, "\n"),
#ifdef _MSC_VER
            [](auto const& p) { return std::format("{:d},{:d}", p.first, p.second); });
#else
            [](auto const & p) { return (boost::format("%d,%d") % p.first % p.second).str(); });
#endif
    }
}


////////////////////////////////////////////////////////////////////////////////
// MIT License
//
// Copyright (c) 2017 Jonas Spenger
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
////////////////////////////////////////////////////////////////////////////////
//
// This is a modified (see below) version of the SAX algorithm proposed in:
//  Lin, Jessica, et al. "A symbolic representation of time series, with
//  implications for streaming algorithms." Proceedings of the 8th ACM SIGMOD
//  workshop on Research issues in data mining and knowledge discovery.  ACM,
//  2003.
//
// This implementation (MSAX) differs from the original SAX in two aspects.
//  (1) The time series is normalized by normalizing each point to the
//      neighbouring #windowsize points. That is, for each point we subtract
//      the moving average and divide by the moving standard deviation (with
//      a moving window of size windowsize). In the original SAX algorithm,
//      each subsequence is normalized.
//  (2) Because the entire time series is normalized, this implementation
//      outputs just one symbolic sequence, that represents the entire time
//      series. In comparison to the original SAX, which outputs a symbolic
//      sequence for every subsequence of the time series (of size windowsize),
//      resulting in more than one symbolic sequences.
//
//  To use this include file and run msax::run() with following parameters:
//    timeSeries: the time series (a container / vector)
//    alphabetSize: the size of the alphabet
//    frameSize: the size of a frame that gets transformed to one symbol, the
//               dimensionality reduction so to speak
//    windowSize: the size of the moving window used for the normalization
//
////////////////////////////////////////////////////////////////////////////////

#pragma once

#include <boost/math/distributions/normal.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>
#include <boost/accumulators/statistics/rolling_variance.hpp>
#include <vector>

namespace msax {

template <typename T>
std::vector<int> run(const T& timeSeries,
                     size_t alphabetSize,
                     size_t frameSize,
                     size_t windowSize) {

  //////////////////////////////////////////////////////////////////////////////
  // Normalize the time series
  //////////////////////////////////////////////////////////////////////////////

  T normalizedTimeSeries(timeSeries.size());

  // accumulator for calculating moving variance and moving average
  boost::accumulators::accumulator_set
    <double,
    boost::accumulators::stats<boost::accumulators::tag::rolling_mean,
    boost::accumulators::tag::rolling_variance> >
    accumulator(boost::accumulators::tag::rolling_window::window_size = windowSize);

  // calculate the mean and variance for first windowsize elements
  for (size_t i = 0; i < windowSize; ++i) {
      accumulator(timeSeries[i]);
  }
  // calculate the normalized values for the first windowsize / 2 elements
  for (size_t i = 0; i < windowSize / 2; ++i) {
    normalizedTimeSeries[i] = (timeSeries[i] - boost::accumulators::rolling_mean(accumulator))
      / std::sqrt(boost::accumulators::rolling_variance(accumulator));
  }
  // calculate normalized values for windowsize/2 to timeSeries.size - (windowSize + 1) / 2 values
  for (size_t i = windowSize; i < timeSeries.size(); ++i) {
    accumulator(timeSeries[i]);
    size_t j = i - (windowSize + 1) / 2;
    normalizedTimeSeries[j] = (timeSeries[j] - boost::accumulators::rolling_mean(accumulator))
      / std::sqrt(boost::accumulators::rolling_variance(accumulator));
  }
  // calculate normalized values for the last (windowSize + 1) / 2 values
  for (size_t i = timeSeries.size() - (windowSize + 1) / 2; i < timeSeries.size(); ++i) {
    normalizedTimeSeries[i] = (timeSeries[i] - boost::accumulators::rolling_mean(accumulator))
      / std::sqrt(boost::accumulators::rolling_variance(accumulator));
  }

  //////////////////////////////////////////////////////////////////////////////
  // Perform PAA (Piecewise Aggregate Approximation)
  //////////////////////////////////////////////////////////////////////////////

  T paaTimeSeries((normalizedTimeSeries.size() - 1) / frameSize + 1);

  for (size_t i = 0; i < paaTimeSeries.size(); ++i) {
    double mean = 0.0;

    if (i == paaTimeSeries.size() - 1) {
      for (size_t j = i * frameSize; j < normalizedTimeSeries.size(); ++j) {
        mean += normalizedTimeSeries[j];
      }
      mean = mean / ((double)(normalizedTimeSeries.size() - i * frameSize));
    } else {
      for (size_t j = i * frameSize; j < (i+1) * frameSize; ++j) {
        mean += normalizedTimeSeries[j];
      }
      mean = mean / ((double)frameSize);
    }
    paaTimeSeries[i] = mean;
  }

  //////////////////////////////////////////////////////////////////////////////
  // Calculate breakpoints
  //////////////////////////////////////////////////////////////////////////////

  std::vector<double> breakpoints(alphabetSize - 1);
  boost::math::normal stdNormDist(0.0, 1.0); // standard normal distribution
  for (size_t i = 1; i < alphabetSize; ++i) {
    double q = quantile(stdNormDist, ((double) i) / ((double) alphabetSize));
    breakpoints[i-1] = q;
  }

  //////////////////////////////////////////////////////////////////////////////
  // Discretize
  //////////////////////////////////////////////////////////////////////////////

  std::vector<int> output(paaTimeSeries.size());
  // for each element of paaTimeSeries, map it to the corresponding symbol
  // as defined by the breakpoints, if breakpoints[i] < value < breakpoints[i+1],
  // then map it to symbol nom i+1
  for (size_t i = 0; i < output.size(); ++i) {
    for (size_t j = 0; j < breakpoints.size(); ++j) {
      output[i] = breakpoints.size();
      if (paaTimeSeries[i] < breakpoints[j]) {
        output[i] = j;
        break;
      }
    }
  }

  return output;
} // run()

} // namespace msax


/*
 * statistics_ext.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <boost/math/distributions/normal.hpp>
#include <dials/array_family/scitbx_shared_and_versa.h>
#include <dials/algorithms/statistics/kolmogorov_smirnov_one_sided_distribution.h>
#include <dials/algorithms/statistics/kolmogorov_smirnov_two_sided_distribution.h>
#include <dials/algorithms/statistics/kolmogorov_smirnov_test.h>
#include <dials/algorithms/statistics/poisson_test.h>
#include <dials/algorithms/statistics/correlation.h>
#include <dials/algorithms/statistics/binned_gmm.h>

namespace dials { namespace algorithms { namespace boost_python {

  using namespace boost::python;

  template <typename RealType>
  RealType kolmogorov_smirnov_one_sided_cdf(std::size_t n, RealType x) {
    return cdf(kolmogorov_smirnov_one_sided_distribution<RealType>(n), x);
  }

  template <typename RealType>
  RealType kolmogorov_smirnov_two_sided_cdf(std::size_t n, RealType x) {
    return cdf(kolmogorov_smirnov_two_sided_distribution<RealType>(),
               x * std::sqrt((double)n));
  }

  // template <typename RealType>
  // RealType kolmogorov_smirnov_one_sided_pdf(std::size_t n, RealType x) {
  // return pdf(kolmogorov_smirnov_one_sided_distribution<RealType>(n), x);
  //}

  template <typename RealType>
  boost::python::tuple kolmogorov_smirnov_test_standard_normal(
    const af::const_ref<RealType> &data,
    std::string type) {
    // Get the enumeration
    KSType etype = TwoSided;
    if (type.compare("less") == 0) {
      etype = Less;
    } else if (type.compare("greater") == 0) {
      etype = Greater;
    } else {
      DIALS_ASSERT(type.compare("two_sided") == 0);
    }

    // Perform the test
    std::pair<RealType, RealType> result =
      kolmogorov_smirnov_test(boost::math::normal_distribution<RealType>(0, 1),
                              data.begin(),
                              data.end(),
                              etype);
    return boost::python::make_tuple(result.first, result.second);
  }

  BOOST_PYTHON_MODULE(dials_algorithms_statistics_ext) {
    def("kolmogorov_smirnov_one_sided_cdf", &kolmogorov_smirnov_one_sided_cdf<double>);
    def("kolmogorov_smirnov_two_sided_cdf", &kolmogorov_smirnov_two_sided_cdf<double>);
    // def("kolmogorov_smirnov_one_sided_pdf",
    //    &kolmogorov_smirnov_one_sided_pdf<double>);
    def("kolmogorov_smirnov_test_standard_normal",
        &kolmogorov_smirnov_test_standard_normal<double>,
        (arg("data"), arg("type") = "two_sided"));

    def("poisson_expected_max_counts", &poisson_expected_max_counts);

    def("spearman_correlation_coefficient", &spearman_correlation_coefficient<double>);
    def("pearson_correlation_coefficient", &pearson_correlation_coefficient<double>);

    class_<BinnedGMMSingle1DFixedMean>("BinnedGMMSingle1DFixedMean", no_init)
      .def(init<const af::const_ref<double> &,
                const af::const_ref<double> &,
                const af::const_ref<double> &,
                double,
                double,
                double,
                std::size_t>())
      .def("max_iter", &BinnedGMMSingle1DFixedMean::max_iter)
      .def("num_iter", &BinnedGMMSingle1DFixedMean::num_iter)
      .def("epsilon", &BinnedGMMSingle1DFixedMean::epsilon)
      .def("mu", &BinnedGMMSingle1DFixedMean::mu)
      .def("sigma", &BinnedGMMSingle1DFixedMean::sigma);

    class_<BinnedGMMSingle1D>("BinnedGMMSingle1D", no_init)
      .def(init<const af::const_ref<double> &,
                const af::const_ref<double> &,
                const af::const_ref<double> &,
                double,
                double,
                double,
                std::size_t>())
      .def("max_iter", &BinnedGMMSingle1D::max_iter)
      .def("num_iter", &BinnedGMMSingle1D::num_iter)
      .def("epsilon", &BinnedGMMSingle1D::epsilon)
      .def("mu", &BinnedGMMSingle1D::mu)
      .def("sigma", &BinnedGMMSingle1D::sigma);
  }

}}}  // namespace dials::algorithms::boost_python


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

int main()
{
  arma::mat output;
  arma::mat input;

  input = arma::zeros(16, 1);
  input(0) = 1.5;
  input(1) = 2.0;
  input(2) = 2.3;
  input(3) = 2.2;
  input(4) = 1.7;
  input(5) = 2.1;
  input(6) = 1.9;
  input(7) = 2.1;
  input(8) = 1.4;
  input(9) = 1.8;
  input(10) = 1.5;
  input(11) = 1.6;
  input(12) = 1.3;
  input(13) = 1.6;
  input(14) = 1.4;
  input(15) = 1.7;

  //input = arma::mat("1.5 2.0 2.3 2.2 1.7 2.1 1.9 2.1 1.4 1.8 1.5 1.6 1.3 1.6 1.4 1.7").t();

  cout << "-------------------------------------" << endl;
  cout << "INPUT : " << endl;
  cout << "Input shape : "<< input.n_rows << " " << input.n_cols << endl;
  cout << "Input (Transposed view) : " << input.t() << endl;
  cout << "-------------------------------------" << endl;

  const size_t kernelWidth = 2;
  const size_t kernelHeight = 2;
  const size_t strideWidth = 2;
  const size_t strideHeight = 2;
  const bool floor = true;
  size_t inSize = 0;
  size_t outSize = 0;
  bool reset = false;
  size_t inputWidth = 0;
  size_t inputHeight = 0;
  size_t outputWidth = 0;
  size_t outputHeight = 0;
  bool deterministic = false;
  size_t offset = 0;
  size_t batchSize = 0;

  inputHeight = 4;
  inputWidth = 4;
  batchSize = input.n_cols;
  inSize = input.n_elem / (inputWidth * inputHeight * batchSize);

  // cube(ptr_aux_mem, n_rows, n_cols, n_slices, copy_aux_mem = true, strict = false)
  arma::cube inputTemp = arma::cube(const_cast<arma::mat &>(input).memptr(),
      inputWidth, inputHeight, batchSize * inSize, false, false);

  cout << "inputTemp BEFORE POOLING: " << endl;
  cout << "inputTemp shape : "<< inputTemp.n_rows << " " << inputTemp.n_cols << endl;
  cout << "Num slices   : " << inputTemp.n_slices << endl;
  cout << inputTemp << endl;
  cout << "-------------------------------------" << endl;

  outputWidth = std::floor(((inputWidth - (double) kernelWidth) / (double) strideWidth) + 1);
  outputHeight = std::floor(((inputHeight - (double) kernelHeight) / (double) strideHeight) + 1);
  offset = 0;

  arma::cube outputTemp = arma::zeros<arma::cube>(outputWidth, outputHeight, batchSize * inSize);

  cout << "outputTemp BEFORE POOLING: " << endl;
  cout << "outputTemp shape : "<< outputTemp.n_rows << " " << outputTemp.n_cols << endl;
  cout << outputTemp << endl;
  cout << "-------------------------------------" << endl;

  std::vector<arma::cube> poolingIndices;
  poolingIndices.push_back(outputTemp);

  arma::Mat<size_t> indices;
  arma::Col<size_t> indicesCol;

  size_t elements = inputWidth * inputHeight;
  indicesCol = arma::linspace<arma::Col<size_t> >(0, (elements - 1), elements);
  indices = arma::Mat<size_t>(indicesCol.memptr(), inputWidth, inputHeight);
  reset = true;

  cout << "All parameters of layer : " << endl;
  cout << "kernelWidth   : " << kernelWidth << endl;
  cout << "kernelHeight  : " << kernelHeight << endl;
  cout << "strideWidth   : " << strideWidth << endl;
  cout << "strideHeight  : " << strideHeight << endl;
  cout << "inputWidth    : " << inputWidth << endl;
  cout << "inputHeight   : " << inputHeight << endl;
  cout << "outputWidth   : " << outputWidth << endl;
  cout << "outputHeight  : " << outputHeight << endl;
  cout << "batchSize     : " << batchSize << endl;
  cout << "inSize        : " << inSize << endl;
  cout << "outSize       : " << batchSize * inSize << endl;
  cout << "offset        : " << offset << endl;
  cout << "-------------------------------------" << endl;

  cout << "Calculations done inside (reset) conditional statement : " << endl;
  cout << "indicesCol : " << endl;
  cout << "indicesCol shape : "<< indicesCol.n_rows << " " << indicesCol.n_cols << endl;
  cout << "indicesCol (Transposed view) : " << indicesCol.t() << endl;
  cout << "indices : " << endl;
  cout << "indices shape : "<< indices.n_rows << " " << indices.n_cols << endl;
  cout << indices << endl;
  cout << "-------------------------------------" << endl;

  for (size_t s = 0; s < inputTemp.n_slices; s++)
  {
    // PoolingOperation(inputTemp.slice(s), outputTemp.slice(s),poolingIndices.back().slice(s));

    const arma::mat& in = inputTemp.slice(s);
    arma::mat& out = outputTemp.slice(s);
    arma::mat& poolIndexes = poolingIndices.back().slice(s);

    cout << "poolingIndices FOR SLICE " << s << " BEFORE POOLING: " << endl;
    cout << poolingIndices.back().slice(s)<< endl;
    cout << "-------------------------------------" << endl;

    cout << "POOLING OPERATIONS START NOW." << endl;
    cout << "-------------------------------------" << endl;

    size_t x = 0;
    for (size_t j = 0, colidx = 0; j < out.n_cols; ++j, colidx += strideHeight)
    {
      size_t y = 0;
      for (size_t i = 0, rowidx = 0; i < out.n_rows; ++i, rowidx += strideWidth)
      {

        arma::mat subInput = in(
            arma::span(rowidx, rowidx + kernelWidth - 1 - offset),
            arma::span(colidx, colidx + kernelHeight - 1 - offset)
        );

        // const size_t idx = pooling.Pooling(subInput);
        const size_t idx = arma::as_scalar(arma::find(subInput.max() == subInput, 1));
        out(i, j) = subInput(idx);

        arma::Mat<size_t> subIndices = indices(
            arma::span(rowidx, rowidx + kernelWidth - 1 - offset),
            arma::span(colidx, colidx + kernelHeight - 1 - offset)
        );

        poolIndexes(i, j) = subIndices(idx);

        cout << "COUNTER (" << x << ", " << y << ") CALCULATIONS : " << endl;
        cout << "subInput : " << endl;
        cout << subInput << endl;
        cout << "subIndices : " << endl;
        cout << subIndices << endl;
        cout << "idx : " << idx << endl;
        cout << "subInput(idx) : " << endl;
        cout << subInput(idx) << endl;
        cout << "subIndices(idx) : " << endl;
        cout << subIndices(idx) << endl;
        cout << "-------------------------------------" << endl;
        y++;
      }
      x++;
    }

  cout << "POOLING OPERATIONS END NOW." << endl;
  cout << "-------------------------------------" << endl;

  cout << "poolingIndices FOR SLICE " << s << " POST POOLING: " << endl;
  cout << poolingIndices.back().slice(s)<< endl;
  cout << "-------------------------------------" << endl;

  }

  cout << "inputTemp AFTER POOLING: " << endl;
  cout << "inputTemp shape : "<< inputTemp.n_rows << " " << inputTemp.n_cols << endl;
  cout << "Num slices   : " << inputTemp.n_slices << endl;
  cout << inputTemp << endl;
  cout << "-------------------------------------" << endl;

  cout << "outputTemp AFTER POOLING: " << endl;
  cout << "outputTemp shape : "<< outputTemp.n_rows << " " << outputTemp.n_cols << endl;
  cout << outputTemp << endl;
  cout << "-------------------------------------" << endl;

  output = arma::mat(outputTemp.memptr(), outputTemp.n_elem / batchSize, batchSize);
  outputWidth = outputTemp.n_rows;
  outputHeight = outputTemp.n_cols;
  outSize = batchSize * inSize;

  cout << "OUTPUT : " << endl;
  cout << "Output shape : "<< output.n_rows << " " << output.n_cols << endl;
  cout << "Output (Transposed view) : " << output.t() << endl;
  cout << "-------------------------------------" << endl;

  cout << "-------------------------------------" << endl;
  cout << "PSEUDO BACKWARD : " << endl;
  cout << "-------------------------------------" << endl;

  arma::cube gTemp;
  gTemp = arma::zeros<arma::cube>(inputTemp.n_rows, inputTemp.n_cols, inputTemp.n_slices);

  arma::cube mappedError;
  mappedError = arma::ones<arma::cube>(outputWidth, outputHeight, outSize);

  for (size_t s = 0; s < mappedError.n_slices; s++)
  {
    //Unpooling(mappedError.slice(s), gTemp.slice(s), poolingIndices.back().slice(s));

    const arma::mat& error = mappedError.slice(s);
    arma::mat& out = gTemp.slice(s);
    arma::mat& poIdx = poolingIndices.back().slice(s);

    for (size_t i = 0; i < poIdx.n_elem; ++i)
    {
      out(poIdx(i)) += error(i);
    }

    cout << "OUTPUT : " << endl;
    cout << "Output shape : "<< out.n_rows << " " << out.n_cols << endl;
    cout << "Output (Transposed view) : " << endl << out << endl;
    cout << "-------------------------------------" << endl;
  }

  cout << "-------------------------------------" << endl;
  cout << "UNPOOLING : " << endl;
  cout << "-------------------------------------" << endl;

  // Output of MaxPool is the input for UnPooling.
  input = arma::zeros(4, 1);
  input(0) = 2.1;
  input(1) = 2.3;
  input(2) = 1.8;
  input(3) = 1.7;

  cout << "-------------------------------------" << endl;
  cout << "INPUT FOR UNPOOLING: " << endl;
  cout << "Input shape : "<< input.n_rows << " " << input.n_cols << endl;
  cout << "Input (Transposed view) : " << input.t() << endl;
  cout << "-------------------------------------" << endl;

  inSize = 0;
  outSize = 0;
  reset = false;
  inputWidth = 0;
  inputHeight = 0;
  outputWidth = 0;
  outputHeight = 0;
  batchSize = 0;

  inputHeight = 2;
  inputWidth = 2;
  batchSize = input.n_cols;
  inSize = input.n_elem / (inputWidth * inputHeight * batchSize);

  // cube(ptr_aux_mem, n_rows, n_cols, n_slices, copy_aux_mem = true, strict = false)
  inputTemp = arma::cube(const_cast<arma::mat &>(input).memptr(),
      inputWidth, inputHeight, batchSize * inSize, false, false);

  cout << "inputTemp BEFORE UNPOOLING: " << endl;
  cout << "inputTemp shape : "<< inputTemp.n_rows << " " << inputTemp.n_cols << endl;
  cout << "Num slices   : " << inputTemp.n_slices << endl;
  cout << inputTemp << endl;
  cout << "-------------------------------------" << endl;

  outputWidth = (inputWidth - 1) * strideWidth + kernelWidth;
  outputHeight = (inputHeight - 1) * strideHeight + kernelHeight;
  outSize = batchSize * inSize;
  outputTemp = arma::zeros<arma::cube>(outputWidth, outputHeight,
      outSize);

  cout << "outputTemp BEFORE UNPOOLING: " << endl;
  cout << "outputTemp shape : "<< outputTemp.n_rows << " " << outputTemp.n_cols << endl;
  cout << outputTemp << endl;
  cout << "-------------------------------------" << endl;

  for (size_t s = 0; s < inputTemp.n_slices; s++)
  {
    // Unpooling(inputTemp.slice(s), outputTemp.slice(s), poolingIndices.back().slice(s));
    const arma::mat& IN = inputTemp.slice(s);
    arma::mat& OUT = outputTemp.slice(s);
    arma::mat& INDICES = poolingIndices.back().slice(s);

    for (size_t i = 0; i < INDICES.n_elem; ++i)
    {
      OUT(INDICES(i)) += IN(i);
    }

    cout << "OUTPUT : " << endl;
    cout << "Output shape : "<< OUT.n_rows << " " << OUT.n_cols << endl;
    cout << "Output : " << endl << OUT << endl;
    cout << "-------------------------------------" << endl;
  }

  cout << "inputTemp AFTER UNPOOLING: " << endl;
  cout << "inputTemp shape : "<< inputTemp.n_rows << " " << inputTemp.n_cols << endl;
  cout << "Num slices   : " << inputTemp.n_slices << endl;
  cout << inputTemp << endl;
  cout << "-------------------------------------" << endl;

  cout << "outputTemp AFTER UNPOOLING: " << endl;
  cout << "outputTemp shape : "<< outputTemp.n_rows << " " << outputTemp.n_cols << endl;
  cout << outputTemp << endl;
  cout << "-------------------------------------" << endl;

  cout << "-------------------------------------" << endl;
  cout << "UNPOOLING BACKWARD (PROOF OF CONCEPT): " << endl;
  cout << "-------------------------------------" << endl;

  arma::mat L;
  L = arma::zeros(16, 1);
  L(0) = 1;
  L(1) = 2;
  L(2) = 3;
  L(3) = 4;
  L(4) = 5;
  L(5) = 6;
  L(6) = 7;
  L(7) = 8;
  L(8) = 9;
  L(9) = 10;
  L(10) = 11;
  L(11) = 12;
  L(12) = 13;
  L(13) = 14;
  L(14) = 15;
  L(15) = 16;

  mappedError = arma::cube(const_cast<arma::mat &>(L).memptr(), outputWidth, outputHeight, outSize, false, false);
  gTemp = arma::zeros<arma::cube>(inputWidth, inputHeight, outSize);

  for (size_t s = 0; s < gTemp.n_slices; s++)
  {
    arma::mat& gySlice = mappedError.slice(s);
    arma::mat& gSlice = gTemp.slice(s);
    arma::mat& idxs = poolingIndices.back().slice(s);

    cout << "gySlice: " << endl;
    cout << "gy shape : "<< gySlice.n_rows << " " << gySlice.n_cols << endl;
    cout << gySlice << endl;
    cout << "-------------------------------------" << endl;

    cout << "gTempSlice BEFORE: " << endl;
    cout << "g shape : "<< gSlice.n_rows << " " << gSlice.n_cols << endl;
    cout << gSlice << endl;
    cout << "-------------------------------------" << endl;

    for (size_t i = 0, j = 0; i < idxs.n_elem; ++i, ++j)
    {
      gSlice(j) = gySlice(idxs(i));
    }

    cout << "gTempSlice AFTER: " << endl;
    cout << "g shape : "<< gSlice.n_rows << " " << gSlice.n_cols << endl;
    cout << gSlice << endl;
    cout << "-------------------------------------" << endl;
  }

  arma::mat g = arma::mat(gTemp.memptr(), gTemp.n_elem / batchSize, batchSize);
  cout << "g: " << endl;
  cout << "g shape : "<< g.n_rows << " " << g.n_cols << endl;
  cout << g << endl;
  cout << "-------------------------------------" << endl;

  return 0;
}


#include "itkImage.h"
#include "itkImageFileReader.h"
#include "itkImageFileWriter.h"
#include "vnl/vnl_matrix_ref.h"
#include "vnl/vnl_matrix.h"
#include "itkNumericTraits.h"

#include "tkdCmdParser.h"

#include <iostream>
#include <utility>
#include <fstream>

#include <boost/utility.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/betweenness_centrality.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_matrix.hpp>

/**
 * (Relative) Betweenness centrality.
 */
template< class DirectedProperty = boost::undirectedS >
class BC
{
public:

	typedef double PixelType;

	typedef boost::property< boost::vertex_index_t, unsigned int, boost::property< boost::vertex_centrality_t, PixelType > > VertexPropertyType;
	typedef boost::property< boost::edge_weight_t, PixelType, boost::property< boost::edge_centrality_t, PixelType > > EdgePropertyType;
	typedef boost::adjacency_matrix< DirectedProperty, VertexPropertyType, EdgePropertyType > GraphMatrixType;

	typedef itk::Image< PixelType, 2 > ImageType;
	typedef vnl_matrix_ref< PixelType > DataMatrixType;
	typedef vnl_vector< PixelType > VectorType;

	/**
	 * Run.
	 */
	void Run( const std::string& filename, PixelType lowerThreshold, PixelType upperThreshold, const std::string& outputFileName,
			bool relative, bool dominance )
	{
		typedef itk::ImageFileReader< ImageType > ReaderType;
		typename ReaderType::Pointer reader = ReaderType::New();
		reader->SetFileName( filename.c_str() );
		reader->Update();

		typename ImageType::Pointer image = reader->GetOutput();
		reader = 0;

		PixelType* buffer = image->GetPixelContainer()->GetBufferPointer();
		typename ImageType::RegionType region = image->GetLargestPossibleRegion();
		typename ImageType::SizeType size = region.GetSize();
		int rows = size[0];
		int cols = size[1];

		DataMatrixType data( rows, cols, buffer );

		typename ImageType::Pointer output = ImageType::New();
		output->CopyInformation( image );
		output->SetRegions( image->GetLargestPossibleRegion() );
		output->Allocate();
		output->FillBuffer( 0 );

		DataMatrixType distance( rows, cols, output->GetPixelContainer()->GetBufferPointer() );

		GraphMatrixType g( rows );

		/* TEST List -> weighted version works, and indeed lower values are shorter paths,
		GraphMatrixType g( 5 );
		boost::add_edge( 0, 1, EdgePropertyType( 1.0 ), g );
		boost::add_edge( 1, 4, EdgePropertyType( 0.2 ), g );
		boost::add_edge( 4, 3, EdgePropertyType( 0.3 ), g );
		boost::add_edge( 0, 2, EdgePropertyType( 1.0 ), g );
		boost::add_edge( 2, 3, EdgePropertyType( 1.0 ), g );
		*/

		const bool isUndirected = typeid(DirectedProperty) == typeid(boost::undirectedS);
		for ( int i = 0; i < rows; ++i )
		{
			int start = isUndirected ? i + 1 : 0;
			for ( int j = start; j < cols; ++j )
			{
				if ( i == j )
				{
					continue;
				}

				PixelType weight = data( i, j );

				if ( weight <= upperThreshold && weight > 0 && weight > lowerThreshold )
				{
					// bc makes use of fwsp, so invert weight (correlation coefficients ...)
					boost::add_edge( i, j, EdgePropertyType( 1.0 / weight ), g );
				}
			}
		}

		VectorType centrality( boost::num_vertices( g ) );
		typename boost::graph_traits< GraphMatrixType >::vertex_iterator vi, vi_end;

		boost::brandes_betweenness_centrality( g,
				centrality_map( boost::get( boost::vertex_centrality, g ) ).
				edge_centrality_map( boost::get( boost::edge_centrality, g ) ).
				weight_map( boost::get(
						boost::edge_weight, g ) ) );

		if ( !relative )
		{
			typename boost::property_map< GraphMatrixType, boost::vertex_centrality_t >::type b = boost::get( boost::vertex_centrality, g );

			for ( tie( vi, vi_end ) = vertices( g ); vi != vi_end; ++vi )
				centrality[*vi] = b[*vi];
		}
		else
		{
			boost::relative_betweenness_centrality( g, boost::get( boost::vertex_centrality, g ) );
			typename boost::property_map< GraphMatrixType, boost::vertex_centrality_t >::type r = boost::get( boost::vertex_centrality, g );

			for ( boost::tie( vi, vi_end ) = vertices( g ); vi != vi_end; ++vi )
				centrality[*vi] = r[*vi];

			// Central point dominance is correct if and only if betweenness centrality is relative!
			if ( dominance )
			{
				double dominance = boost::central_point_dominance( g, boost::get( boost::vertex_centrality, g ) );
				std::cout << "Dominance," << dominance << std::endl;
			}
		}

		WriteVectorToFile( outputFileName, centrality );
	}

	/**
	 * Write vector to filename.
	 */
	void WriteVectorToFile( const std::string& outputFileName, const VectorType& V )
	{
		std::ofstream out( outputFileName.c_str() );

		if ( out.fail() )
		{
			std::cerr << "*** ERROR ***: Not able to write to: " << outputFileName << "!" << std::endl;
			exit( EXIT_FAILURE );
		}

		for ( unsigned int i = 0; i < V.size(); i++ )
			out << V[i] << std::endl;

		out.close();
	}
};

/**
 * Betweenness centrality.
 */
int main( int argc, char ** argv )
{
	tkd::CmdParser p( "betweenness centrality", "Calculate betweenness centrality measure for all vertices" );

	std::string inputFileName;
	std::string outputFileName;

	float lowerThreshold = 0;
	float upperThreshold = 1.0;
	bool isUndirected = true;
	bool relative = true;
	bool dominance = false;

	p.AddArgument( inputFileName, "input" ) ->AddAlias( "i" ) ->SetDescription( "Input image: 2D adjacency matrix" ) ->SetRequired( true );

	p.AddArgument( upperThreshold, "threshold" ) ->AddAlias( "thu" ) ->AddAlias( "t" ) ->SetDescription(
			"Threshold; only include paths with weight in (0, threshold] (default: 1.0)" );

	p.AddArgument( lowerThreshold, "threshold-lower" ) ->AddAlias( "thl" ) ->SetDescription(
			"Lower threshold; only include paths with weight > threshold (default: 0)" );

	p.AddArgument( outputFileName, "output" ) ->AddAlias( "o" ) ->SetDescription(
			"Output image: List with (relative) betweenness centralities per vertex" ) ->SetRequired( true );

	p.AddArgument( isUndirected, "undirected" ) ->AddAlias( "u" ) ->SetDescription(
			"Input is a symmetric matrix representing an undirected graph [default: true]" );

	p.AddArgument( relative, "relative" ) ->AddAlias( "r" ) ->SetDescription( "Relative betweenness centrality algorithm [default: true]" );

	p.AddArgument( dominance, "dominance" ) ->AddAlias( "d" ) ->SetDescription( "Print central point dominance [relative only, default: false]" );

	if ( !p.Parse( argc, argv ) )
	{
		p.PrintUsage( std::cout );
		return -1;
	}

	if ( isUndirected )
	{
		BC< boost::undirectedS > bc;
		bc.Run( inputFileName, lowerThreshold, upperThreshold, outputFileName, relative, dominance );
	} else
	{
		BC< boost::directedS > bc;
		bc.Run( inputFileName, lowerThreshold, upperThreshold, outputFileName, relative, dominance );
	}

	return EXIT_SUCCESS;
}


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

// This file is part of OpenMVG, an Open Multiple View Geometry C++ library.

// Copyright (c) 2012, 2013 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "openMVG/numeric/numeric.h"

#include <Eigen/Geometry>

#include <fstream>

namespace openMVG {

Mat3 CrossProductMatrix(const Vec3 &x) {
  Mat3 X;
  X << 0, -x(2),  x(1),
    x(2),     0, -x(0),
   -x(1),  x(0),     0;
  return X;
}

Mat3 RotationAroundX(double angle) {
  return Eigen::AngleAxisd(angle, Vec3::UnitX()).toRotationMatrix();
}

Mat3 RotationAroundY(double angle) {
  return Eigen::AngleAxisd(angle, Vec3::UnitY()).toRotationMatrix();
}

Mat3 RotationAroundZ(double angle) {
  return Eigen::AngleAxisd(angle, Vec3::UnitZ()).toRotationMatrix();
}

double getRotationMagnitude(const Mat3 & R2) {
  const Mat3 R1 = Mat3::Identity();
  double cos_theta = (R1.array() * R2.array()).sum() / 3.0;
  cos_theta = clamp(cos_theta, -1.0, 1.0);
  return std::acos(cos_theta);
}

Mat3 LookAt(const Vec3 &center, const Vec3 & up) {
  Vec3 zc = center.normalized();
  Vec3 xc = up.cross(zc).normalized();
  Vec3 yc = zc.cross(xc);
  Mat3 R;
  R.row(0) = xc;
  R.row(1) = yc;
  R.row(2) = zc;
  return R;
}

//eyePosition3D is a XYZ position. This is where you are (your eye is).
//center3D is the XYZ position where you want to look at.
//upVector3D is a XYZ normalized vector. Quite often 0.0, 1.0, 0.0

Mat3 LookAt2(const Vec3 &eyePosition3D,
  const Vec3 &center3D,
  const Vec3 &upVector3D )
{
  Vec3 forward, side, up;
  Mat3 matrix2, resultMatrix;
  //------------------
  forward = center3D - eyePosition3D;
  forward.normalize();
  //------------------
  //Side = forward x up
  //ComputeNormalOfPlane(side, forward, upVector3D);
  side[0]=(forward[1]*upVector3D[2])-(forward[2]*upVector3D[1]);
  side[1]=(forward[2]*upVector3D[0])-(forward[0]*upVector3D[2]);
  side[2]=(forward[0]*upVector3D[1])-(forward[1]*upVector3D[0]);
  side.normalize();
  //------------------
  //Recompute up as: up = side x forward
  //ComputeNormalOfPlane(up, side, forward);
  up[0]=(side[1]*forward[2])-(side[2]*forward[1]);
  up[1]=(side[2]*forward[0])-(side[0]*forward[2]);
  up[2]=(side[0]*forward[1])-(side[1]*forward[0]);

  //------------------
  matrix2(0) = side[0];
  matrix2(1) = side[1];
  matrix2(2) = side[2];
  //------------------
  matrix2(3) = up[0];
  matrix2(4) = up[1];
  matrix2(5) = up[2];
  //------------------
  matrix2(6) = -forward[0];
  matrix2(7) = -forward[1];
  matrix2(8) = -forward[2];

  return matrix2;
}

void MeanAndVarianceAlongRows(const Mat &A,
  Vec *mean_pointer,
  Vec *variance_pointer) {
  const Mat::Index n = A.rows();
  const double m = static_cast<double>(A.cols());
  (*mean_pointer) = Vec::Zero(n);
  (*variance_pointer) = Vec::Zero(n);

  for (Mat::Index i = 0; i < n; ++i) {
    (*mean_pointer)(i) += A.row(i).array().sum();
    (*variance_pointer)(i) += (A.row(i).array() * A.row(i).array()).array().sum();
  }
  (*mean_pointer) /= m;
  for (Mat::Index i = 0; i < n; ++i) {
    (*variance_pointer)(i) = (*variance_pointer)(i) / m - Square((*mean_pointer)(i));
  }
}

bool exportMatToTextFile(const Mat & mat, const std::string & filename,
  const std::string & sPrefix)
{
  bool bOk = false;
  std::ofstream outfile;
  outfile.open(filename.c_str(), std::ios_base::out);
  if (outfile.is_open()) {
    outfile << sPrefix << "=[" << std::endl;
    for (int j=0; j < mat.rows(); ++j)  {
      for (int i=0; i < mat.cols(); ++i)  {
        outfile << mat(j,i) << " ";
      }
      outfile << ";\n";
    }
    outfile << "];";
    bOk = true;
  }
  outfile.close();
  return bOk;
}

}  // namespace openMVG


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

  [begin_description]
  tba.
  [end_description]

  Copyright 2009-2012 Karsten Ahnert
  Copyright 2009-2012 Mario Mulansky

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

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_SYMPLECTIC_RKN_SB3A_M4_MCLACHLAN_HPP_DEFINED
#define BOOST_NUMERIC_ODEINT_STEPPER_SYMPLECTIC_RKN_SB3A_M4_MCLACHLAN_HPP_DEFINED

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

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

namespace boost {
namespace numeric {
namespace odeint {

#ifndef DOXYGEN_SKIP
namespace detail {
namespace symplectic_rkn_sb3a_m4_mclachlan {

    /*
      exp( a1 t A ) exp( b1 t B )
      exp( a2 t A ) exp( b2 t B )
      exp( a3 t A )
      exp( b2 t B ) exp( a2 t A )
      exp( b1 t B ) exp( a1 t A )
    */

    template< class Value >
    struct coef_a_type : public boost::array< Value , 5 >
    {
        coef_a_type( void )
        {
            using std::sqrt;

            Value z = sqrt( static_cast< Value >( 7 ) / static_cast< Value >( 8 ) ) / static_cast< Value >( 3 );
            (*this)[0] = static_cast< Value >( 1 ) / static_cast< Value >( 2 ) - z ;
            (*this)[1] = static_cast< Value >( -1 ) / static_cast< Value >( 3 ) + z ;
            (*this)[2] = static_cast< Value >( 2 ) / static_cast< Value >( 3 );
            (*this)[3] = (*this)[1];
            (*this)[4] = (*this)[0];
        }
    };

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

} // namespace symplectic_rkn_sb3a_m4_mclachlan
} // namespace detail
#endif // DOXYGEN_SKIP

template<
    class Coor ,
    class Momentum = Coor ,
    class Value = double ,
    class CoorDeriv = Coor ,
    class MomentumDeriv = Coor ,
    class Time = Value ,
    class Algebra = typename algebra_dispatcher< Coor >::algebra_type ,
    class Operations = typename operations_dispatcher< Coor >::operations_type ,
    class Resizer = initially_resizer
    >
#ifndef DOXYGEN_SKIP
class symplectic_rkn_sb3a_m4_mclachlan :
        public symplectic_nystroem_stepper_base
<
    5 , 4 ,
    Coor , Momentum , Value , CoorDeriv , MomentumDeriv , Time , Algebra , Operations , Resizer
    >
#else
class symplectic_rkn_sb3a_m4_mclachlan : public symplectic_nystroem_stepper_base
#endif
{
public:
#ifndef DOXYGEN_SKIP
    typedef symplectic_nystroem_stepper_base
    <
    5 , 4 ,
    Coor , Momentum , Value , CoorDeriv , MomentumDeriv , Time , Algebra , Operations , Resizer
    > stepper_base_type;
#endif
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::value_type value_type;

    symplectic_rkn_sb3a_m4_mclachlan( const algebra_type &algebra = algebra_type() )
        : stepper_base_type(
            detail::symplectic_rkn_sb3a_m4_mclachlan::coef_a_type< value_type >() ,
            detail::symplectic_rkn_sb3a_m4_mclachlan::coef_b_type< value_type >() ,
            algebra )
    { }
};

/***************** DOXYGEN ***************/

/**
 * \class symplectic_rkn_sb3a_m4_mclachlan
 * \brief Implementation of the symmetric B3A Runge-Kutta Nystroem method of fifth order.
 *
 * The method is of fourth order and has five stages. It is described HERE. This method can be used
 * with multiprecision types since the coefficients are defined analytically.
 *
 * ToDo: add reference to paper.
 *
 * \tparam Order The order of the stepper.
 * \tparam Coor The type representing the coordinates q.
 * \tparam Momentum The type representing the coordinates p.
 * \tparam Value The basic value type. Should be something like float, double or a high-precision type.
 * \tparam CoorDeriv The type representing the time derivative of the coordinate dq/dt.
 * \tparam MomemtnumDeriv The type representing the time derivative of the momentum dp/dt.
 * \tparam Time The type representing the time t.
 * \tparam Algebra The algebra.
 * \tparam Operations The operations.
 * \tparam Resizer The resizer policy.
 */

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

} // namespace odeint
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_SYMPLECTIC_RKN_SB3A_M4_MCLACHLAN_HPP_DEFINED


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

#pragma once
#include <iostream>
#include <stdint.h>
#include <vector>
#include <Eigen/Dense>

#include <boost/shared_ptr.hpp>

#include <dpMM/dpMM.hpp>
#include <dpMM/cat.hpp>
#include <dpMM/dir.hpp>
#include <dpMM/niw.hpp>
#include <dpMM/sampler.hpp>
#include <dpMM/basemeasure.hpp>

using namespace Eigen;
using std::cout;
using std::endl;
using boost::shared_ptr;
using std::vector;

template<typename T=double>
class DirNaiveBayes : public DpMM<T>{

public:
  DirNaiveBayes(const Dir<Cat<T>, T>& alpha, const boost::shared_ptr<BaseMeasure<T> >& theta);
  DirNaiveBayes(const Dir<Cat<T>, T>& alpha, const vector<boost::shared_ptr<BaseMeasure<T> > >&thetas);
  virtual ~DirNaiveBayes();

  virtual void initialize(const vector< Matrix<T,Dynamic,Dynamic> >&x);
  virtual void initialize(const boost::shared_ptr<ClGMMData<T> >&cld)
    {cout<<"not supported"<<endl; assert(false);};

  virtual void sampleLabels();
  virtual void sampleParameters();

  virtual T logJoint(bool verbose=false);
  virtual const VectorXu& labels(){return z_;};
  virtual const VectorXu& getLabels(){return z_;};
  virtual uint32_t getK() const { return K_;};

//  virtual MatrixXu mostLikelyInds(uint32_t n);
  virtual MatrixXu mostLikelyInds(uint32_t n, Matrix<T,Dynamic,Dynamic>& logLikes);

  Matrix<T,Dynamic,1> getCounts();

  virtual void inferAll(uint32_t nIter, bool verbose=false);
  virtual void dump(std::ofstream& fOutMeans, std::ofstream& fOutCovs);

protected:
  uint32_t K_;
  Dir<Cat<T>, T> dir_;
  Cat<T> pi_;
#ifdef CUDA
  SamplerGpu<T>* sampler_;
#else
  Sampler<T>* sampler_;
#endif
  Matrix<T,Dynamic,Dynamic> pdfs_;
//  Cat cat_;
  vector<boost::shared_ptr<BaseMeasure<T> > > thetas_;

  vector<Matrix<T,Dynamic,Dynamic> > x_;
  VectorXu z_;
};

// --------------------------------------- impl -------------------------------

template<typename T>
DirNaiveBayes<T>::DirNaiveBayes(const Dir<Cat<T>,T>& alpha, const boost::shared_ptr<BaseMeasure<T> >& theta) :
  K_(alpha.K_), dir_(alpha), pi_(dir_.sample()) //cat_(dir_.sample()),
{
// init the parameters
    cout<<"[DirNaiveBayes::DirNaiveBayes] creating thetas (you only gave me one)"<<endl;
    for (uint32_t k=0; k<K_; ++k)
      thetas_.push_back(boost::shared_ptr<BaseMeasure<T> >(theta->copy()));
};

template<typename T>
DirNaiveBayes<T>::DirNaiveBayes(const Dir<Cat<T>,T>& alpha,
    const vector<boost::shared_ptr<BaseMeasure<T> > >& thetas) :
  K_(alpha.K_), dir_(alpha), pi_(dir_.sample()), //cat_(dir_.sample()),
  thetas_(thetas)
{};

template<typename T>
DirNaiveBayes<T>::~DirNaiveBayes()
{
  if (sampler_ != NULL) delete sampler_;
};

template <typename T>
Matrix<T,Dynamic,1> DirNaiveBayes<T>::getCounts()
{
  return counts<T,uint32_t>(z_,K_);
};

template<typename T>
void DirNaiveBayes<T>::initialize(const vector< Matrix<T,Dynamic,Dynamic> > &x)
{
  cout<<"init"<<endl;
  x_ = x;
  // randomly init labels from prior
  z_.setZero(x.size());
  cout<<"sample pi"<<endl;
  pi_ = dir_.sample();
  cout<<"init pi="<<pi_.pdf().transpose()<<endl;
  pi_.sample(z_);

  pdfs_.setZero(x.size(),K_);
#ifdef CUDA
  sampler_ = new SamplerGpu<T>(uint32_t(x.size()),K_,dir_.pRndGen_);
#else
  sampler_ = new Sampler<T>(dir_.pRndGen_);
#endif

#pragma omp parallel for
  for(int32_t k=0; k<int32_t(K_); ++k)
    thetas_[k]->posterior(x_,z_,k);
};

template<typename T>
void DirNaiveBayes<T>::sampleLabels()
{
  // obtain posterior categorical under labels
  pi_ = dir_.posterior(z_).sample();
//  cout<<pi_.pdf().transpose()<<endl;

#pragma omp parallel for
  for(int32_t i=0; i<z_.size(); ++i)
  {
    //TODO: could buffer this better
    // compute categorical distribution over label z_i
    VectorXd logPdf_z = pi_.pdf().array().log();
    for(uint32_t k=0; k<K_; ++k)
    {
//      cout<<thetas_[k].logLikelihood(x_.col(i))<<" ";
		for(uint32_t w=0; w<x_[i].cols(); ++w)
		{
			logPdf_z[k] += thetas_[k]->logLikelihood(x_[i],w);
		}
    }
//    cout<<endl;
    // make pdf sum to 1. and exponentiate
    pdfs_.row(i) = (logPdf_z.array()-logSumExp(logPdf_z)).exp().matrix().transpose();
//    cout<<pi_.pdf().transpose()<<endl;
//    cout<<pdf.transpose()<<" |.|="<<pdf.sum();
//    cout<<" z_i="<<z_[i]<<endl;
  }
  // sample z_i
  sampler_->sampleDiscPdf(pdfs_,z_);
};

template<typename T>
void DirNaiveBayes<T>::sampleParameters()
{
#pragma omp parallel for
  for(int32_t k=0; k<int32_t(K_); ++k)
  {
    thetas_[k]->posterior(x_,z_,k);
//    cout<<"k:"<<k<<endl;
//    thetas_[k]->print();
  }
};

template<typename T>
T DirNaiveBayes<T>::logJoint(bool verbose)
{
  T logJoint = dir_.logPdf(pi_);
  if(verbose)
  	cout<<"log p(pi)="<<logJoint<<" -> ";

#pragma omp parallel for reduction(+:logJoint)
  for (int32_t k=0; k<int32_t(K_); ++k)
    logJoint = logJoint + thetas_[k]->logPdfUnderPrior();
	if(verbose)
		cout<<"log p(pi)*p(theta)="<<logJoint<<" -> ";

#pragma omp parallel for reduction(+:logJoint)
  for (int32_t i=0; i<z_.size(); ++i)
	  for(int32_t w=0; w<x_[i].cols(); ++w)
		logJoint = logJoint + thetas_[z_[i]]->logLikelihood(x_[i],w);
  if(verbose)
  	cout<<"log p(phi)*p(theta)*p(x|z,theta)="<<logJoint<<"]"<<endl;

  return logJoint;
};

template<typename T>
MatrixXu DirNaiveBayes<T>::mostLikelyInds(uint32_t n, Matrix<T,Dynamic,Dynamic>& logLikes)
{
  MatrixXu inds = MatrixXu::Zero(n,K_);
  logLikes = Matrix<T,Dynamic,Dynamic>::Ones(n,K_);
  logLikes *= -99999.0;

#pragma omp parallel for
  for (int32_t k=0; k<int32_t(K_); ++k)
  {
    for (uint32_t i=0; i<z_.size(); ++i)
      if(z_(i) == k)
      {
        T logLike = thetas_[z_[i]]->logLikelihood(x_[i]);
        for (uint32_t j=0; j<n; ++j)
          if(logLikes(j,k) < logLike)
          {
            for(uint32_t l=n-1; l>j; --l)
            {
              logLikes(l,k) = logLikes(l-1,k);
              inds(l,k) = inds(l-1,k);
            }
            logLikes(j,k) = logLike;
            inds(j,k) = i;
//            cout<<"after update "<<logLike<<endl;
//            Matrix<T,Dynamic,Dynamic> out(n,K_*2);
//            out<<logLikes.cast<T>(),inds.cast<T>();
//            cout<<out<<endl;
            break;
          }
      }
  }
  cout<<"::mostLikelyInds: logLikes"<<endl;
  cout<<logLikes<<endl;
  cout<<"::mostLikelyInds: inds"<<endl;
  cout<<inds<<endl;
  return inds;
};

template<typename T>
void DirNaiveBayes<T>::inferAll(uint32_t nIter, bool verbose)
{
  if(verbose){
  	cout<<"[DirNaiveBayes::inferALL] ------ inferingALL (nIter=" << nIter << ") ------"<<endl;
  	cout <<"initial labels:"<< endl;
  	cout<<this->labels().transpose()<<endl;
  }
  for(uint32_t t=0; t<nIter; ++t)
  {
    this->sampleLabels();
    this->sampleParameters();
    if(verbose){
		cout << "[" << std::setw(3)<< std::setfill('0')  << t <<"] label: "
    	<< this->labels().transpose()
      	<< " [joint= " << std::setw(6) << this->logJoint(false) << "]"<< endl;
    }
  }
}

template <typename T>
void DirNaiveBayes<T>::dump(std::ofstream& fOutMeans, std::ofstream& fOutCovs)
{
	cout << "dumping naiveBayes" << endl;
	cout << "doc index: " << endl;
	cout << this->labels().transpose() << endl;

	cout << "printing cluster params: " << endl;
	cout << K_ << endl;
	for(uint32_t k=0; k<K_; ++k)
	{
		cout << "theta: " << k << endl;
		thetas_[k]->print();
	}

	cout << "printing mixture params: " << endl;
	pi_.print();

}


/*! \file demo_2d_uncertainty.cpp
    \brief Demonstration of some 2D plot features.
    \details including showing values with uncertainty information as 'plus minus', confidence intervals and degrees of freedom estimates.
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A. Bristow 2007, 2008, 2009, 2021

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

//[demo_2d_uncertainty_1

/*`First we need some includes to use Boost.SVG_Plot, and C++ Standard Library:
*/

#include <boost/svg_plot/svg_2d_plot.hpp>
// using namespace boost::svg;

//#include <boost/svg_plot/show_2d_settings.hpp> // Only needed for showing ALL the many settings used.
//using boost::svg::show_2d_plot_settings;
//void boost::svg::show_2d_plot_settings(svg_2d_plot&);

#include <boost/svg_plot/detail/pair.hpp>
// using boost::svg::detail::pair; operator<<

#include <boost/quan/unc.hpp>
//#include <boost/quan/unc_init.hpp>

#include <boost/quan/meas.hpp>

#include <algorithm>
//  using std::copy;
//#include <functional>

#include <map>
  //using std::map;
  //using std::multimap;

#include <utility>
  //using std::pair;
  //using std::make_pair;

#include <vector>
//  using std::vector;

#include <cmath>
//   using std::sqrt;

#include <iostream>
   //using std::cout;
   //using std::endl;
   //using std::scientific;
   //using std::hex;
   //using std::ios;
   //using std::boolalpha;
#include <iomanip>
// Using std::setw;
// using std::setprecision;

#include <iterator>
//   using std::ostream_iterator;

//] [/demo_2d_uncertainty_1]

  enum side
  { //! \enum side Where axis labels go.
    left_side = -1,
    on_axis = 0,
    right_side = +1,
    bottom_side = -1,
    top_side = +1,
  };

int main()
{
  using std::pair;
  using std::make_pair;
  using std::vector;
  using std::map;
  using std::multimap;
  using std::cout;
  using std::endl;
  using std::scientific;
  using std::hex;
  using std::ios;
  using std::boolalpha;
  using std::sqrt;

  using namespace boost::svg;
  using boost::svg::detail::operator<<;

  try
  {
//[demo_2d_uncertainty_2

/*`STL map is used as the container for our two data-series,
and pairs of values and their uncertainty information
(approximately standard deviation and degrees of freedom)
are inserted using push_back. Since this is a 2-D plot
the order of data values is important.
*/
  typedef unc<false> uncun; // Uncertain Uncorrelated (the normal case).

  using boost::svg::detail::operator<<;
  using std::ostream_iterator;

  setUncDefaults(std::cout);  // Initialize for uncertain types.

  //setPlusminusSds(2); // cout.iword(plusminusSdsIndex);

  // Create pair for fundamental type double that is implicitly exact with no uncertainty information.
  pair<double, double> double_pair; // double X and Y pair.
  double_pair = make_pair(double(-2.234), double(-8.76)); // Construct and then echo the data values thus:
  std::cout << double_pair.first << ", " << double_pair.second << std::endl; //  make_pair(double(-2.234), double(-8.76)) = -2.234, -8.76
  // But it is more convenient to use a specialization of operator<< for `std::pair` to show both:
  std::cout << "make_pair(double(-2.234), double(-8.76)) = " << double_pair << std::endl;
  // make_pair(double(-2.234), double(-8.76)) = -2.234, -8.76

  // Or, more informatively, use an uncertain type `uncun` that holds explicit uncertainty information
  // (standard-deviation, degrees of freedom and so can compute confidence internals).

  uncun ux(2.0, 0.5F, 7); // For an X-value.
  //uncun ux(1.03, 0.56F, 7); // For an X-value.
  // Using the  `uncun operator<<` provided we can output all the details of the uncertain value.
  std::cout << scientific << plusminus << addlimits << adddegfree << std::setw(20)  << std::left
    << "ux = " << ux << std::endl; // 1.2 +/-0.56 <0.82, 1.64> (7)

  uncun uy(4.0, 0.5F, 9); // For a Y-value.
//  uncun uy(3.45, 0.67F, 9); // For a Y-value.
  std::cout << "uy = " << uy << std::endl; // 3.5 +/-0.67 <3.01, 3.89> (9)

  // Now we make a pair of X and Y uncertain values:
  pair<uncun, uncun > mp1 = make_pair(ux, uy); // X & Y pair of two uncertain values.
  // Echo both X and Y-values and add all the uncertainty information, standard deviation and degrees of freedom :
  std::cout << "mp1 = "<< mp1 << std::endl; // 1.2 +/-0.56 <0.82, 1.64> (7), 3.5 +/-0.67 <3.01, 3.89> (9)

  std::map<uncun, uncun > data1; // Container for X & Y pairs of data-point values.
  data1.insert(mp1); // Insert 1st pair of X & Y.
  //data1.insert(make_pair(uncun(3.9, 0.01F, 8), uncun(1.1, 0.1F, 18))); // and add another X&Y-pair
  data1.insert(make_pair(uncun(3.9, 0.01F, 8), uncun(1.1, 0.2F, 18))); // and add another X&Y-pair
  data1.insert(make_pair(uncun(-2.234, 0.1F, 7), uncun(-5.76, 0.4F, 9))); // and a third pair.

 /*
`Make very sure you don't forget either uncun(...) like this
`data1.insert(make_pair((-2.234, 0.12F, 7),(-8.76, 0.56F, 9)));`
because, by the bizarre operation of the comma operator, the result will be an integer!
So you will astonished to find that the values will be the *pair of degrees of freedom, (7, 9)*
and the other parts of uncun will be undefined!

Echo the values input, correctly rounded using the uncertainy and degrees of freedom:
  */
  std::cout << data1.size() << " XY data pairs:" << std::endl;
  std::copy(data1.begin(), data1.end(), ostream_iterator<pair<uncun, uncun> >(std::cout, "\n"));
  std::cout << std::endl;

  /*
  3 XY data pairs:
   -2.23 +/-0.030 <-2.26, -2.21> (7), -8.8 +/-0.90 <-9.35, -8.17> (9)
   1.2 +/-0.56 <0.82, 1.64> (7), 3.5 +/-0.67 <3.01, 3.89> (9)
   4.1 +/-0.40 <3.82, 4.38> (8), 3.1 +/-0.30 <2.96, 3.24> (18)
  */

  svg_2d_plot my_plot;  // Construct an empty plot.

  /*`If you can be confident that the data set(s) only contains normal, valid data,
  so none are 'at limits' - too big or too small to be meaningful, infinite or NaN (NotANumber),
  then these checks can be skipped (for speed).
  An instrument or operator input might be known to provide only normal data.
  For this example, we know this is true, so override the default autoscale_check_limits(true).
  */
  my_plot.autoscale_check_limits(false);
  /*`The default is autoscale_plusminus(3.) so that confidence ellipses
  at 1, 2 and 3 (uncertainty nominally standard deviations)
  are all within the plot window,
  but if you are less interested in seeing the 2 and 3 ellipses,
  you could risk the outer edges spilling over the borders
  by reducing autoscale_plusminus, for example, to 1.5, down to zero.
  */
  //my_plot.autoscale_plusminus(1.5); // default is 3.
  // my_plot.confidence(0.01);  // Change from default 0.05 to 0.01 for 99% confidence.

  /*`Use data set `data` to autoscale. (You can use a different data set to scale from the one you chose to plot).
  */
  //my_plot.xy_autoscale(data1); // But may display data point value labels illegibly outside the plot frame or image.
  // Specifying the X- and Y-axes explicitly is usually best.

  //my_plot.confidence(0.01);  // Optionally use an alpha of 0.01 (99%) rather than default of 0.05 (95%) for culculation of confidence interval.

  //my_plot.plusminus_sds(2.); // Optionally display uncertainty multiplied by a factor of two instead of default one.

  my_plot
    // X values settings:
    .x_label("times (sec)")
    .x_range(-3, +10)
    //.x_values_on(true) // Show X-values next to each point. Triggers a warning if also write .xy_values_on(true)
    // SVG_plot warning : x_values_on has overwritten xy_values_on!

     //! \note Essential use of Unicode space &\#x00A0; in all strings - ANSI space has no effect!
  //.x_decor("t ", ", ", "sec") // Keep all on one line using X-separator does NOT start with a newline.
   .x_decor("g ", "\n", "sec") // Split X and Y onto two lines because X-separator *does* start with newline.
  // .x_decor("after t ", "\ntravels", "sec") // Split X and Y onto two lines because X-separator *does* start with newline,
    // and add some descriptive text too.
   // .x_values_rotation(backdown) // Not good.
   // .x_values_rotation(downward) //
   .x_values_rotation(uphill)
   //.x_values_rotation(steepup) //
   // .x_values_rotation(horizontal) // Lines up vertically, but shifted too high???
   // .x_values_rotation(upward) //
   // .x_values_rotation(rightward) //
    .x_values_font_size(10) // Bigger than default.
    .x_values_font_family("Times New Roman") // Serif font just to show difference from sans-serif used for Y value-labels.
    .x_major_grid_on(true)
    .y_major_grid_on(true)
    .x_major_interval(1)
    .y_major_interval(1)

    .x_plusminus_color(red) // Show plus/minus +/- uncertainty data-point value-label in red.
    .x_plusminus_on(true) // Show plus/minus +/- uncertainty with data-point value-labels, for example "2.1 +/- 0.001"

    // Y values settings:
    .y_label("distance (km)")
    .y_range(-10., +10.) // But may be over-written by x and or  y-autoscale below.
    //.y_values_on(true) // Show Y values next to each point.
    // SVG_plot warning : xy_values_on has overwritten y_values_on!

   // .y_values_rotation(downhill) // is ignored if both X and Y-values are shown on line below.
    .y_values_font_family("Arial") // Sans serif different from X-values font just to show effect.
    .y_values_font_size(8) // Smaller than default.
    .y_decor("y=", "", " km")
 //   .y_decor("           d  ", " ", " km")
//    .y_decor("   time = ", " ", " sec")
     // Note: a few padding spaces are used to get Y-value-labels to lie more nearly under X-value-labels.
     // This is only necessary when value-labels are not horizontal.
     // y_prefix "   "   3 spaces.
     // y_separator " time ="  space before word time.
     // y_suffix " sec" space before word sec.

    .y_plusminus_on(true) // Show plus/minus +/- uncertainty with data-point value-labels, for example "2.1 +/- 0.001"
    .y_plusminus_color(magenta) // Show plus/minus +/- uncertainty in magenta.

    .y_addlimits_on(true)  // Show plus/minus +/- confidence limits for data-point value-labels.
    .y_addlimits_color(darkgreen) // Show +/- in darkgreen, for example: "+/- 0.03".

    .y_df_on(true) // Show degrees of freedom (usually observations -1) for data-points.
    .y_df_color(green) // Show degrees of freedom in green, for examples: "11").

    .xy_values_on(true) // Show both X-values and Y-values next to each point.
     // IS needed if both .x_values_on(true) and .y_values_on(true) specified.
     // .xy_autoscale(data1) // may result in value-labels running off the plot and image.
   // .y_values_alignment(align_style::left_align)
    //.x_values_alignment(align_style::right_align) // By default, set by code #L2881 switch statement depending on rotation.

  /*`The default uncertainty ellipse colors (that apply to both X and Y axes)
  can be changed thus:
  */
    .one_sd_color(lightblue) // Color of ellipse for one standard deviation (about 66% probability).
    .two_sd_color(svg_color(200, 230, 255)) // Color of ellipse for two standard deviation (~95%).
    .three_sd_color(svg_color(230, 240, 255)) // Color of ellipse for two standard deviation (~99%).
   ; // my_plot

//  my_plot.plot(data1, "data1").shape(unc_ellipse).fill_color(lightyellow).stroke_color(magenta);
  my_plot.plot(data1, "data1").shape(unc_ellipse).fill_color(blue).stroke_color(magenta);
  // TODO the uncertainty ellipses are now not showing anything useful for 2D but OK for 1D??? :-(

  my_plot.write("./demo_2d_uncertainty");

  std::cout << "confidence alpha " << my_plot.confidence() <<", plusminus sd multiplier = " << my_plot.plusminus_sds() << std::endl;

  // show_2d_plot_settings(my_plot); // Needs #include <boost/svg_plot/show_2d_settings.hpp>

 //] [/demo_2d_uncertainty_2]

  }
  catch (const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
} // int main()

/*

Output :

   ------ Build started: Project: demo_2d_uncertainty, Configuration: Debug Win32 ------
  unc.cpp
  unc_print.cpp
  unc_read.cpp
  demo_2d_uncertainty.cpp
  Generating Code...
  demo_2d_uncertainty.vcxproj -> J:\Cpp\SVG\Debug\demo_2d_uncertainty.exe
  make_pair(double(-2.234), double(-8.76)) = -2.234, -8.76
   make_pair(double(-2.234), double(-8.76)) = -2.234, -8.76
  ux = 1.2
  1.2, 3.5
  3 XY data pairs:
  -2.23, -8.8
  1.2, 3.5
  4.10, 3.100

  No limits checks: x_min = -2.234, x_max = 4.1, y_min = -8.76, y_max = 3.45

  axes_on true
  background_border_width 2
  background_border_color RGB(255,255,0)
  background_color RGB(255,255,255)
  image_border_margin() 3
  image_border_width() 2
  coord_precision 3
  copyright_date
  copyright_holder
  description
  document_title ""
  x_size 500
  image y_size 400
  image_filename
  legend_on false
  legend_place 2
  legend_top_left -1, -1, legend_bottom_right -1, -1
  legend_background_color blank
  legend_border_color RGB(255,255,0)
  legend_color blank
  legend_title ""
  legend_title_font_size 14
  legend_font_weight
  legend_width 0
  legend_lines true
  limit points stroke color RGB(119,136,153)
  limit points fill color RGB(250,235,215)
  license_on false
  license_reproduction permits
  license_distribution permits
  license_attribution requires
  license_commercialuse permits
  plot_background_color RGB(255,255,255)
  plot_border_color RGB(119,136,153)
  plot_border_width 2
  plot_window_on true
  plot_window_x 92.6, 474
  plot_window_x_left 92.6
  plot_window_x_right 474
  plot_window_y 8, 341
  plot_window_y_top 8
  plot_window_y_bottom 341
  title_on false
  title ""
  title_color blank
  title_font_alignment 2
  title_font_decoration
  title_font_family Verdana
  title_font_rotation 0
  title_font_size 18
  title_font_stretch
  title_font_style
  title_font_weight
  x_values_on false
  x_values_font_size 16
  x_values_font_family Times New Roman
  x_values_precision 3
  x_values_ioflags 200 IOS format flags (0x200) dec.
  y_values_precision 3
  y_values_font_size() 3
  y_values_ioflags 200 IOS format flags (0x200) dec.
  y_values_color blank
  y_values_font_family() Arial
  y_values_font_size() 12
  x_max 5
  x_min -3
  x_autoscale true
  y_autoscale true
  xy_autoscale true
  x_autoscale_check_limits false
  x_axis_on true
  x_axis_color() RGB(0,0,0)
  x_axis_label_color blank
  x_values_color blank
  x_axis_width 1
  x_label_on true
  x_label "times (sec)"
  x_label_color blank
  x_label_font_family Verdana
  x_label_font_size 14
  x_label_units
  x_label_units_on false
  x_major_labels_side left
  x_major_label_rotation 0
  x_major_grid_color RGB(200,220,255)
  x_major_grid_on false
  x_major_grid_width 1
  x_major_interval 1
  x_major_tick 1
  x_major_tick_color RGB(0,0,0)
  x_major_tick_length 5
  x_major_tick_width 2
  x_minor_interval 0
  x_minor_tick_color RGB(0,0,0)
  x_minor_tick_length 2
  x_minor_tick_width 1
  x_minor_grid_on false
  x_minor_grid_color RGB(200,220,255)
  x_minor_grid_width 0.5
  x_range() -3, 5
  x_num_minor_ticks 4
  x_ticks_down_on true
  x_ticks_up_on false
  x_ticks_on_window_or_axis bottom
  y_axis_position y_axis_position intersects X axis (X range includes zero)
  x_axis_position x_axis_position intersects Y axis (Y range includes zero)
  x_plusminus_on true
  x_plusminus_color RGB(0,255,255)
  x_df_on true
  x_df_color RGB(255,0,255)
  x_prefix
  x_separator
  x_suffix
  xy_values_on true
  y_label_on "true"
  y_label_axis distance (km)
  y_axis_color RGB(0,0,0)
  y_axis_label_color blank
  y_axis_on true
  axes_on true
  y_axis_value_color RGB(0,0,0)
  y_axis_width 1
  y_label distance (km)
  y_label_color blank
  y_label_font_family Verdana
  y_label_font_size 14
  y_label_on true
  y_label_units
  y_label_units_on false
  y_label_width 0
  y_major_grid_on false
  y_major_grid_color RGB(200,220,255)
  y_major_grid_width 1
  y_major_interval 2.5
  y_major_labels_side bottom
  y_major_label_rotation 0
  y_major_tick_color RGB(0,0,0)
  y_major_tick_length  5
  y_major_tick_width  2
  y_minor_grid_on false
  y_minor_grid_color  RGB(200,220,255)
  y_minor_grid_width 0.5
  y_minor_interval 0
  y_minor_tick_color RGB(0,0,0)
  y_minor_tick_length 2
  y_minor_tick_width 1
  y_range() -10, 5
  y_num_minor_ticks
  y_ticks_left_on true
  y_ticks_right_on false
  y_ticks_on_window_or_axis left
  y_max 5
  y_min -10
  y_values_on false
  y_plusminus_on true
  y_plusminus_color RGB(255,0,0)
  y_df_on true
  y_df_color RGB(0,128,0)
  y_prefix "   "
  y_separator "time = "
  y_suffix " sec"
  data lines width 2
========== Build: 1 succeeded, 0 failed, 0 up-to-date, 0 skipped ==========

*/


#include "xpbd/neohookean_elasticity_constraint.h"

#include <Eigen/Dense>
#include <eigen/SVD>

namespace xpbd {

neohookean_elasticity_constraint_t::neohookean_elasticity_constraint_t(
    std::initializer_list<index_type> indices,
    positions_type const& p,
    scalar_type young_modulus,
    scalar_type poisson_ratio,
    scalar_type const alpha)
    : base_type(indices, alpha), V0_{0.}, DmInv_{}, mu_{}, lambda_{}
{
    assert(indices.size() == 4u);

    auto const v1 = this->indices().at(0);
    auto const v2 = this->indices().at(1);
    auto const v3 = this->indices().at(2);
    auto const v4 = this->indices().at(3);

    auto const p1 = p.row(v1);
    auto const p2 = p.row(v2);
    auto const p3 = p.row(v3);
    auto const p4 = p.row(v4);

    Eigen::Matrix3d Dm;
    Dm.col(0) = (p1 - p4).transpose();
    Dm.col(1) = (p2 - p4).transpose();
    Dm.col(2) = (p3 - p4).transpose();

    V0_     = (1. / 6.) * Dm.determinant();
    DmInv_  = Dm.inverse();
    mu_     = (young_modulus) / (2. * (1 + poisson_ratio));
    lambda_ = (young_modulus * poisson_ratio) / ((1 + poisson_ratio) * (1 - 2 * poisson_ratio));
}

void neohookean_elasticity_constraint_t::project(
    positions_type& p,
    masses_type const& m,
    scalar_type& lagrange,
    scalar_type const dt) const
{
    auto const v1 = this->indices().at(0);
    auto const v2 = this->indices().at(1);
    auto const v3 = this->indices().at(2);
    auto const v4 = this->indices().at(3);

    auto const p1 = p.row(v1);
    auto const p2 = p.row(v2);
    auto const p3 = p.row(v3);
    auto const p4 = p.row(v4);

    auto const w1 = 1. / m(v1);
    auto const w2 = 1. / m(v2);
    auto const w3 = 1. / m(v3);
    auto const w4 = 1. / m(v4);

    auto const Vsigned        = signed_volume(p);
    bool const is_V_positive  = Vsigned >= 0.;
    bool const is_V0_positive = V0_ >= 0.;
    bool const is_tet_inverted =
        (is_V_positive && !is_V0_positive) || (!is_V_positive && is_V0_positive);

    Eigen::Matrix3d Ds;
    Ds.col(0) = (p1 - p4).transpose();
    Ds.col(1) = (p2 - p4).transpose();
    Ds.col(2) = (p3 - p4).transpose();

    Eigen::Matrix3d const F = Ds * DmInv_;
    Eigen::Matrix3d const I = Eigen::Matrix3d::Identity();

    scalar_type constexpr epsilon = 1e-20;

    Eigen::Matrix3d Piola;
    scalar_type psi{};

    // TODO: Implement correct inversion handling described in
    // Irving, Geoffrey, Joseph Teran, and Ronald Fedkiw. "Invertible finite elements for robust
    // simulation of large deformation." Proceedings of the 2004 ACM SIGGRAPH/Eurographics symposium
    // on Computer animation. 2004.
    if (is_tet_inverted)
    {
        Eigen::JacobiSVD<Eigen::Matrix3d> UFhatV(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::Vector3d const Fsigma = UFhatV.singularValues();
        Eigen::Matrix3d Fhat;
        Fhat.setZero();
        Fhat(0, 0) = Fsigma(0);
        Fhat(1, 1) = Fsigma(1);
        Fhat(2, 2) = Fsigma(2);

        Eigen::Matrix3d U       = UFhatV.matrixU();
        Eigen::Matrix3d const V = UFhatV.matrixV();

        auto smallest_element_idx = 0;
        if (Fsigma(0) < Fsigma(1) && Fsigma(0) < Fsigma(2))
            smallest_element_idx = 0;
        if (Fsigma(1) < Fsigma(0) && Fsigma(1) < Fsigma(2))
            smallest_element_idx = 1;
        if (Fsigma(2) < Fsigma(0) && Fsigma(2) < Fsigma(1))
            smallest_element_idx = 2;

        Fhat(smallest_element_idx, smallest_element_idx) =
            -Fhat(smallest_element_idx, smallest_element_idx);
        U.col(smallest_element_idx) = -U.col(smallest_element_idx);

        // stress reaches maximum at 58% compression
        scalar_type constexpr min_singular_value = 0.577;
        Fhat(0, 0)                               = std::min(Fhat(0, 0), min_singular_value);
        Fhat(1, 1)                               = std::min(Fhat(1, 1), min_singular_value);
        Fhat(2, 2)                               = std::min(Fhat(2, 2), min_singular_value);

        Eigen::Matrix3d const Fprime = U * Fhat * V.transpose();
        Eigen::Matrix3d const F2     = Fprime.transpose() * Fprime;
        Eigen::Matrix3d const Finv   = Fprime.inverse();
        Eigen::Matrix3d const FinvT  = Finv.transpose();
        scalar_type const I1         = F2.trace();
        scalar_type const J          = Fprime.determinant();

        scalar_type const logJ = std::log(J);
        // psi(I1, J) = (mu/2)*(I1 - 3) - mu*log(J) + (lambda/2)*log^2(J)
        psi = static_cast<scalar_type>(0.5) * mu_ * (I1 - static_cast<scalar_type>(3.)) -
              mu_ * logJ + static_cast<scalar_type>(0.5) * lambda_ * logJ * logJ;
        // P(F) = mu*(F - mu*F^-T) + lambda*log(J)*F^-T
        Piola = mu_ * (Fprime - mu_ * FinvT) + lambda_ * logJ * FinvT;
    }
    else
    {
        Eigen::Matrix3d const F2    = F.transpose() * F;
        Eigen::Matrix3d const Finv  = F.inverse();
        Eigen::Matrix3d const FinvT = Finv.transpose();
        scalar_type const I1        = F2.trace();
        scalar_type const J         = F.determinant();

        scalar_type const logJ = std::log(J);
        // psi(I1, J) = (mu/2)*(I1 - 3) - mu*log(J) + (lambda/2)*log^2(J)
        psi = static_cast<scalar_type>(0.5) * mu_ * (I1 - static_cast<scalar_type>(3.)) -
              mu_ * logJ + static_cast<scalar_type>(0.5) * lambda_ * logJ * logJ;
        // P(F) = mu*(F - mu*F^-T) + lambda*log(J)*F^-T
        Piola = mu_ * (F - mu_ * FinvT) + lambda_ * logJ * FinvT;
    }

    // H is the negative gradient of the elastic potential
    scalar_type const V0     = std::abs(V0_);
    Eigen::Matrix3d const H  = -V0 * Piola * DmInv_.transpose();
    Eigen::Vector3d const f1 = H.col(0);
    Eigen::Vector3d const f2 = H.col(1);
    Eigen::Vector3d const f3 = H.col(2);
    Eigen::Vector3d const f4 = -(f1 + f2 + f3);

    // clang-format off
     auto const weighted_sum_of_gradients =
        w1 * f1.squaredNorm() +
        w2 * f2.squaredNorm() +
        w3 * f3.squaredNorm() +
        w4 * f4.squaredNorm();
    // clang-format on

    if (weighted_sum_of_gradients < epsilon)
        return;

    scalar_type const C           = V0 * psi;
    scalar_type const alpha_tilde = alpha_ / (dt * dt);
    scalar_type const delta_lagrange =
        -(C + alpha_tilde * lagrange) / (weighted_sum_of_gradients + alpha_tilde);

    lagrange += delta_lagrange;
    // because f = - grad(potential), then grad(potential) = -f and thus grad(C) = -f
    p.row(v1) += w1 * -f1 * delta_lagrange;
    p.row(v2) += w2 * -f2 * delta_lagrange;
    p.row(v3) += w3 * -f3 * delta_lagrange;
    p.row(v4) += w4 * -f4 * delta_lagrange;
}

neohookean_elasticity_constraint_t::scalar_type
neohookean_elasticity_constraint_t::signed_volume(positions_type const& V) const
{
    Eigen::RowVector3d const p1 = V.row(indices()[0]);
    Eigen::RowVector3d const p2 = V.row(indices()[1]);
    Eigen::RowVector3d const p3 = V.row(indices()[2]);
    Eigen::RowVector3d const p4 = V.row(indices()[3]);

    Eigen::Matrix3d Ds;
    Ds.col(0)      = (p1 - p4).transpose();
    Ds.col(1)      = (p2 - p4).transpose();
    Ds.col(2)      = (p3 - p4).transpose();
    auto const vol = (1. / 6.) * Ds.determinant();
    return vol;
}

} // namespace xpbd