/*

	pclub03.cpp

	pclub01.cppから派生。01は以下のもの。
	@Programming Club, Imaplus, Nov 16, 2019
	Instant Test for kdatasettest00.cpp

	Written by Koji Yamamoto
	Copyright (C) 2019-2020 Koji Yamamoto

	TODO:
	度数分布表をつくる。kstatを見て。
	　別に、連続変数用の機能をつける。
	　　start/end, width, bin を指定する方式。
	　　自動で、スタージェスの公式を使う方式。
	　　階級の端点の表を与える方式。
	ヒストグラムを描く。
	SVGにする。

*/

/* ********** Preprocessor Directives ********** */

#include <k09/kdataset01.cpp>
#include <k09/kstat02.cpp>
#include <k09/koutputfile00.cpp>
#include <iostream>
#include <iomanip>
#include <algorithm>

#include <boost/algorithm/string.hpp>

/* ********** Namespace Declarations/Directives ********** */

using namespace std;

/* ********** Class Declarations ********** */

/* ********** Enum Definitions ********** */

/* ********** Function Declarations ********** */

int main( int, char *[]);

void drawHistogramToSvg(
	const std::string &,
	const std::vector <double> &, const std::vector <double> &,
	const std::vector <int> &,
	bool = false
);

std::vector <double>
getGridPoints( double, double, int = 4, bool = true, bool = true);

/* ********** Class Definitions ********** */

/* ********** Global Variables ********** */

/* ********** Definitions of Static Member Variables ********** */

/* ********** Function Definitions ********** */

int main( int, char *[])
{

	vector <double> dvec;
	vector <double> dvecclean;

	{
		Dataset ds;
		bool b;

		cout << "Reading data...";
		b = ds.readCsvFile( "jhpsmerged_191029_v403.csv");
		if ( b == false){
			return 0;
		}
		cout << "Done." << endl;

		cout << "Fixing variable types...";
		int nnum, nmis;
		ds.fixVariableType( nnum, nmis);
		cout << "Done." << endl;

		cout << "Getting numeric vector before specifying missing...";
		b = ds.getNumericVectorWithoutMissing( dvec, "v403");
		if ( b == false){
			return 0;
		}
		cout << "Done." << endl;

		cout << "Specifying missing cases...";
		ds.specifyValid(
			"v403",
			[]( double v)->bool{ return ( v < 99999.0);}
		);
		cout << "Done." << endl;

		cout << "Getting numeric vector excl. missing...";
		b = ds.getNumericVectorWithoutMissing( dvecclean, "v403");
		if ( b == false){
			return 0;
		}
		cout << "Done." << endl;

	}

	cout << endl;
	cout << "***************************************************" << endl;
	cout << "JHPS 2009 Household Income incl. Tax" << endl;
	cout << "Calculated by mean() and median()" << endl;
	cout << "Mean:   " << setprecision( 15) << mean( dvecclean)   << " (Ten Thousand Yen)" << endl;
	cout << "Median: " << median( dvecclean) << " (Ten Thousand Yen)" << endl;
	cout << "***************************************************" << endl;
	cout << "FYI: Mean from \"dirty\" data: " << setprecision( 15) << mean( dvec) << endl;

	// 度数分布表

	cout << endl;
	cout << "Number of unique values: " << countUniqueValues( dvecclean) << endl;
	cout << "FYI Number of unique values in \"dirty\" vector: " << countUniqueValues( dvec) << endl << endl;

	RecodeTable <double, int> rt;
	rt.setAutoTableFromContVar( dvecclean);

	cout << "RecodeTable:" << endl;
	rt.print( cout, ",");
	cout << endl;

	FreqType <int, int> ft;
	ft.setFreqFromRecodeTable( dvecclean, rt);

	ft.printPadding( cout);

	// ヒストグラムをつくりたい。

	vector <int> codes;
	vector <int> counts;
	vector <double> leftvec;
	vector <double> rightvec;
	ft.getVectors( codes, counts);
	ft.getRangeVectors( leftvec, rightvec);

	drawHistogramToSvg( "pclub03out01.svg", leftvec, rightvec, counts);
	drawHistogramToSvg( "pclub03out02.svg", leftvec, rightvec, counts, true); // アニメバージョン

	return 0;

	// 今のRecodeTableには、左端・右端がない（無限大）という指定ができない。

	// FreqTypeにはすごく小さい機能だけを持たせることにして、
	// 別にFreqTableTypeか何かをつくって、そこに、RecodeTableを持たせたり、
	// それをもとにしたFreqを作らせたりしてもよいかも。

	/*
	度数分布表をつくる。kstatを見て。
	　別に、連続変数用の機能をつける。
	　　start/end, width, bin を指定する方式。
	　　自動で、スタージェスの公式を使う方式？
	　　※Stataでは、min{ sqrt(N), 10*ln(N)/ln(10)}らしいので、それでいく。
	　　階級の端点の表を与える方式。
	*/

/*
	// ちょうどいい間隔と基準点の実験。
	{
		vector <double> gridpoints = getGridPoints( -12.34, 567.8);
		for ( auto d : gridpoints){
			cout << d << endl;
		}
		cout << endl;
	}

	{
		vector <double> gridpoints = getGridPoints( -12.34, 567.8, 4, false, false);
		for ( auto d : gridpoints){
			cout << d << endl;
		}
		cout << endl;
	}

	{
		vector <double> gridpoints = getGridPoints( -1234.5, 567.8);
		for ( auto d : gridpoints){
			cout << d << endl;
		}
		cout << endl;
	}

	{
		vector <double> gridpoints = getGridPoints( -1234.5, 567.8, 5);
		for ( auto d : gridpoints){
			cout << d << endl;
		}
		cout << endl;
	}

	{
		vector <double> gridpoints = getGridPoints( 123.5, 5678.9);
		for ( auto d : gridpoints){
			cout << d << endl;
		}
		cout << endl;
	}

	{
		vector <double> gridpoints = getGridPoints( -80001.0, -299.9);
		for ( auto d : gridpoints){
			cout << d << endl;
		}
		cout << endl;
	}

	{
		vector <double> gridpoints = getGridPoints( -80001.0, -299.9, 5);
		for ( auto d : gridpoints){
			cout << d << endl;
		}
		cout << endl;
	}
*/
}

void drawHistogramToSvg(
	const std::string &fn,
	const std::vector <double> &leftvec, const std::vector <double> &rightvec,
	const std::vector <int> &counts,
	bool animated /*= false*/
)
{

	using namespace std;

	// SVGのviewBoxについて：アスペクト比が違っているとわかりにくい。
	// （強制的に余白がつくられたりするか、強制的に拡大縮小して円が歪んだりする）ので、
	// svgタグのサイズとviewBoxのサイズを合わせたい。

	struct Cambus;
	struct Point;

	struct Point {

		double x, y;

		Point( void)
		 : x( std::numeric_limits<double>::quiet_NaN()),
		   y( std::numeric_limits<double>::quiet_NaN())
		{}

		Point( double x0, double y0) : x( x0), y( y0)
		{}

	};

	struct Cambus {

		// 実際の座標系では、y座標は大きいほど「下」の位置を示す。
		// 論理座標系では、y座標は大きいほど「上」の位置を示す。

		double actuXMin, actuYMin, actuXMax, actuYMax; // 実際の座標系での、枠の範囲
		double actuWidth, actuHeight; // 同上

		double theoXMin, theoYMin, theoXMax, theoYMax; // 論理座標系での、枠の範囲
		double theoWidth, theoHeight; // 同上

		void setTheoretical( double xmin0, double ymin0, double xmax0, double ymax0)
		{
			theoXMin = xmin0; theoYMin = ymin0; theoXMax = xmax0; theoYMax = ymax0;
			theoWidth = xmax0 - xmin0; theoHeight = ymax0 - ymin0;
		}

		void setActual( double xmin0, double ymin0, double xmax0, double ymax0)
		{
			actuXMin = xmin0; actuYMin = ymin0; actuXMax = xmax0; actuYMax = ymax0;
			actuWidth = xmax0 - xmin0; actuHeight = ymax0 - ymin0;
		}

		// 論理座標系表現から実際の座標系表現を作成。
		Point getActualFromTheoretical( const Point &poi0)
		{

			double x0 = poi0.x;
			double y0 = poi0.y;
			Point ret;
			ret.x = ( x0 - theoXMin) / theoWidth  * actuWidth  + actuXMin;
			ret.y = ( theoYMax - y0) / theoHeight * actuHeight + actuYMin;
			return ret;

		}

		// x座標のみを算出→論理座標系表現から実際の座標系表現を作成。
		double getXActualFromTheoretical( double x0)
		{

			double retx = ( x0 - theoXMin) / theoWidth  * actuWidth  + actuXMin;
			return retx;

		}

		// y座標のみを算出→論理座標系表現から実際の座標系表現を作成。
		double getYActualFromTheoretical( double y0)
		{

			double rety = ( theoYMax - y0) / theoHeight * actuHeight + actuYMin;
			return rety;

		}

		// 実際の座標系での中点のxを返す。
		double getActualMidX( void)
		{

			return ( actuXMin + actuWidth / 2);

		}

		// 実際の座標系での中点のyを返す。
		double getActualMidY( void)
		{

			return ( actuYMin + actuHeight / 2);

		}

	};

	vector <string> svglines;
	Cambus cam;

	// SVG領域の大きさと、座標系のある領域の大きさを指定することで、それらしく計算してほしい。

	// ちょうどいい間隔のグリッド線の点と、範囲を得る。

	// x軸
	double xminval = leftvec.front();
	double xmaxval = rightvec.back();
	vector <double> xgridpoints = getGridPoints( xminval, xmaxval);
	for ( auto d : xgridpoints){
		cout << d << endl;
	}
	cout << endl;
	// y軸
	double ymaxval = *( max_element( counts.begin(), counts.end()));
	vector <double> ygridpoints = getGridPoints( 0, ymaxval);
	for ( auto d : ygridpoints){
		cout << d << endl;
	}
	cout << endl;

	// 描画範囲は、Gridpointsのさらに5%外側にする。
	double theoWidthTemp = xgridpoints.back() - xgridpoints.front();
	double theoXMin = xgridpoints.front() - 0.05 * theoWidthTemp;
	double theoXMax = xgridpoints.back() + 0.05 * theoWidthTemp;

	double theoHeightTemp = ygridpoints.back() - ygridpoints.front();
	double theoYMin = ygridpoints.front() - 0.05 * theoHeightTemp;
	double theoYMax = ygridpoints.back() + 0.05 * theoHeightTemp;

	// SVGファイル化の開始

	cam.setActual( 50, 50, 450, 450);
	cam.setTheoretical( theoXMin, theoYMin, theoXMax, theoYMax);

	svglines.push_back( R"(<?xml version="1.0" encoding="UTF-8" ?>)"); // This should be exactly in the first line.
	svglines.push_back( R"(<svg width="500px" height="500px" viewBox="0 0 500 500" xmlns="http://www.w3.org/2000/svg">)");
	svglines.push_back( R"(<rect x="0" y="0" width="500" height="500" fill="whitesmoke" stroke-width="0" />)");

	// 背景の描画開始

	// 背景色だけ塗る。
	svglines.push_back( R"(  <rect x="50" y="50" width="400" height="400" fill="gainsboro" stroke-width="0" />)");

	// x軸の目盛を示すグリッド線
	// <g>で属性一括指定：開始
	{
		stringstream ss;
		ss << "  "
		   << R"(<g)"
		   << " "
		   << R"(stroke=")" << "silver" << R"(")"
		   << " "
		   << R"(stroke-width=")" << 1 << R"(")"
		   << R"(>)";
		svglines.push_back( ss.str());
	}
	for ( auto v : xgridpoints){

		Point theoP1( v, theoYMax); // top
		Point theoP2( v, theoYMin); // bottom
		Point actuP1 = cam.getActualFromTheoretical( theoP1);
		Point actuP2 = cam.getActualFromTheoretical( theoP2);

		stringstream ss;
		ss << "    "
		<< R"(<line)"
		<< " "
		<< R"(x1=")" << actuP1.x << R"(")"
		<< " "
		<< R"(y1=")" << actuP1.y << R"(")"
		<< " "
		<< R"(x2=")" << actuP2.x << R"(")"
		<< " "
		<< R"(y2=")" << actuP2.y << R"(")"
		<< " "
		<< R"(/>)";

		svglines.push_back( ss.str());

	}
	// <g>で属性一括指定：終了
	{
		svglines.push_back( "  </g>");
	}

	// y軸の目盛を示すグリッド線
	// <g>で属性一括指定：開始
	{
		stringstream ss;
		ss << "  "
		   << R"(<g)"
		   << " "
		   << R"(stroke=")" << "silver" << R"(")"
		   << " "
		   << R"(stroke-width=")" << 1 << R"(")"
		   << R"(>)";
		svglines.push_back( ss.str());
	}
	for ( auto v : ygridpoints){

		Point theoP1( theoXMin, v); // left
		Point theoP2( theoXMax, v); // right
		Point actuP1 = cam.getActualFromTheoretical( theoP1);
		Point actuP2 = cam.getActualFromTheoretical( theoP2);

		stringstream ss;
		ss << "    "
		<< R"(<line)"
		<< " "
		<< R"(x1=")" << actuP1.x << R"(")"
		<< " "
		<< R"(y1=")" << actuP1.y << R"(")"
		<< " "
		<< R"(x2=")" << actuP2.x << R"(")"
		<< " "
		<< R"(y2=")" << actuP2.y << R"(")"
		<< " "
		<< R"(/>)";

		svglines.push_back( ss.str());

	}
	// <g>で属性一括指定：終了
	{
		svglines.push_back( "  </g>");
	}

	// 背景の描画終了

	// メインの情報の描画開始

	// 度数を示すバー。
	// 注：これを目盛グリッド線よりもあとに描くべし。グリッド線を「上書き」してほしいから。
	// <g>で属性一括指定：開始
	{
		stringstream ss;
		ss << "  "
		   << R"(<g)"
		   << " "
		   << R"(stroke=")" << "Gray" << R"(")"
		   << " "
		   << R"(fill=")" << "Gray" << R"(")"
		   << R"(>)";
		svglines.push_back( ss.str());
	}
	for ( int i = 0; i < counts.size(); i++){

		Point theoP1( leftvec[ i], counts[ i]); // left-top
		Point theoP2( rightvec[ i], 0); // right-bottom
		Point actuP1 = cam.getActualFromTheoretical( theoP1);
		Point actuP2 = cam.getActualFromTheoretical( theoP2);

		if ( animated == true){

			// 以下はアニメ用
			// SVGアニメをパワポに貼っても動かないらしい。

			stringstream ss;

			ss << "    "
			<< R"(<rect)"
			<< " "
			<< R"(x=")" << actuP1.x << R"(")"
			<< " "
			<< R"(y=")" << actuP1.y << R"(")"
			<< " "
			<< R"(width=")" << ( actuP2.x - actuP1.x) << R"(")"
			<< " "
			<< R"(height=")" << ( actuP2.y - actuP1.y) << R"(")"
			<< " "
			<< R"(>)";
			svglines.push_back( ss.str());

			ss.str( "");
			ss << R"(      <animate attributeName="height" begin="0s" dur="1s" from="0" to=")" << ( actuP2.y - actuP1.y) << R"(" repeatCount="1"/>)";
			svglines.push_back( ss.str());

			ss.str( "");
			ss << R"(      <animate attributeName="y" begin="0s" dur="1s" from=")" << actuP2.y << R"(" to=")" << actuP1.y << R"(" repeatCount="1"/>)";
			svglines.push_back( ss.str());

			svglines.push_back( R"(</rect>)");

		} else {

			stringstream ss;
			ss << "    "
			<< R"(<rect)"
			<< " "
			<< R"(x=")" << actuP1.x << R"(")"
			<< " "
			<< R"(y=")" << actuP1.y << R"(")"
			<< " "
			<< R"(width=")" << ( actuP2.x - actuP1.x) << R"(")"
			<< " "
			<< R"(height=")" << ( actuP2.y - actuP1.y) << R"(")"
			<< " "
			<< R"(/>)";
			svglines.push_back( ss.str());

		}

	}
	// <g>で属性一括指定：終了
	{
		svglines.push_back( "  </g>");
	}

	// メインの情報の描画終了

	// 周辺情報記載の開始

	// TODO: フォントサイズを自動調整→優先順位が低い。フォントサイズ固定でもいい。

	// x軸の目盛のヒゲ
	// <g>で属性一括指定：開始
	{
		stringstream ss;
		ss << "  "
		   << R"(<g)"
		   << " "
		   << R"(stroke=")" << "Black" << R"(")"
		   << " "
		   << R"(stroke-width=")" << 1 << R"(")"
		   << R"(>)";
		svglines.push_back( ss.str());
	}
	for ( auto v : xgridpoints){

		double actuX = cam.getXActualFromTheoretical( v);

		double tickheight = 5; // とりあえずの値。

		stringstream ss;

		ss << "    "
		<< R"(<line)"
		<< " "
		<< R"(x1=")" << actuX << R"(")"
		<< " "
		<< R"(y1=")" << cam.actuYMax << R"(")"
		<< " "
		<< R"(x2=")" << actuX << R"(")"
		<< " "
		<< R"(y2=")" << ( cam.actuYMax + tickheight) << R"(")"
		<< " "
		<< R"(/>)";

		svglines.push_back( ss.str());

	}
	// <g>で属性一括指定：終了
	{
		svglines.push_back( "  </g>");
	}

	// TODO: 軸の単位の記載→優先順位は低い。

	// textタグで、IEやWordはdominant-baselineが効かないらしい。
	// （指定してもdominant-baseline="alphabetic"扱いになる。）

	// x軸の目盛のラベル
	double xlabelfontsize = 14; // とりあえずの値。
	// <g>で属性一括指定：開始
	{
		stringstream ss;
		ss << "  "
		   << R"(<g)"
		   << " "
		   << R"(font-family=")" << "Arial,san-serif" << R"(")"
		   << " "
		   << R"(font-size=")" << xlabelfontsize << R"(")"
		   << " "
		   << R"(text-anchor="middle")"
		   << " "
		   << R"(dominant-baseline="alphabetic")" // こうしないとIEやWordで崩れる。。
		   << R"(>)";
		svglines.push_back( ss.str());
	}
	for ( auto v : xgridpoints){

		Point theoP( v, 0); // 本当はy軸の座標は要らないのだが。。

		Point actuP = cam.getActualFromTheoretical( theoP);

		double ticklabelmargin = 10; // とりあえずの値。

		stringstream ss;

		ss << "    "
		<< R"(<text)"
		<< " "
		<< R"(x=")" << actuP.x << R"(")" // 左右方向に中央揃えをする前提で座標を指定。
		<< " "
		// 描画領域の下端からmarginだけ離す。
		// alphabeticの基線は、このフォントの場合、本当のフォント下端より20%上なので、その分をずらしている。
		<< R"(y=")" << ( std::round( cam.actuYMax + ticklabelmargin + xlabelfontsize * 0.8)) << R"(")"
		<< ">"
		<< v // 桁数はどうなるのか。。
		<< R"(</text>)";

		svglines.push_back( ss.str());

	}
	// <g>で属性一括指定：終了
	{
		svglines.push_back( "  </g>");
	}

	// y軸の目盛のヒゲ
	// <g>で属性一括指定：開始
	{
		stringstream ss;
		ss << "  "
		   << R"(<g)"
		   << " "
		   << R"(stroke=")" << "Black" << R"(")"
		   << " "
		   << R"(stroke-width=")" << 1 << R"(")"
		   << R"(>)";
		svglines.push_back( ss.str());
	}
	for ( auto v : ygridpoints){

		double actuY = cam.getYActualFromTheoretical( v);

/*		Point theoP( 0, v); // 本当はy軸の座標は要らないのだが。。

		Point actuP = cam.getActualFromTheoretical( theoP);
*/
		double tickwidth = 5; // とりあえずの値。

		stringstream ss;

		ss << "    "
		<< R"(<line)"
		<< " "
		<< R"(x1=")" << cam.actuXMin << R"(")"
		<< " "
		<< R"(y1=")" << actuY /*actuP.y*/ << R"(")"
		<< " "
		<< R"(x2=")" << ( cam.actuXMin - tickwidth) << R"(")"
		<< " "
		<< R"(y2=")" << actuY /*actuP.y*/ << R"(")"
		<< " "
		<< R"(/>)";

		svglines.push_back( ss.str());

	}
	// <g>で属性一括指定：終了
	{
		svglines.push_back( "  </g>");
	}

	// y軸の目盛ラベル
	/*
	文字列を回転させる方法を探った→svgtest04.svgとsvgtest05.svg
	　svgtest04.svgで2つの方法を試したが、もっとシンプルにしたかった。
	　svgtest05.svgで、transform属性を使えばよいことがわかった。
	*/
	// y軸の目盛のラベル
	double ylabelfontsize = 14; // とりあえずの値。
	// <g>で属性一括指定：開始
	{
		stringstream ss;
		ss << "  "
		   << R"(<g)"
		   << " "
		   << R"(font-family=")" << "Arial,san-serif" << R"(")"
		   << " "
		   << R"(font-size=")" << ylabelfontsize << R"(")"
		   << " "
		   << R"(text-anchor="middle")" // 文字列の左右方向の中心で位置決めする。
		   << " "
		   << R"(dominant-baseline="alphabetic")"
		   << R"(>)";
		svglines.push_back( ss.str());
	}
	for ( auto v : ygridpoints){

		double actuY = cam.getYActualFromTheoretical( v);

		double ticklabelmargin = 10; // とりあえずの値。

		// alphabetic基線に合わせるために20%ずらしている。
		double xplace = std::round( cam.actuXMin - ticklabelmargin - ylabelfontsize * 0.2);

		stringstream ss;

		ss << "    "
		<< R"(<text)"
		<< " "
		<< R"(x=")" << xplace << R"(")" // 描画領域の左端からmarginだけ離す。
		<< " "
		<< R"(y=")" << actuY << R"(")" // 上下方向に中央揃えをする前提で座標を指定。
		<< " "
		<< R"(transform="rotate(270 )" << xplace << " " << actuY << ")" << R"(")" // 回転の中心が各点で異なるので、一括指定できない。
		<< ">"
		<< v // 桁数はどうなるのか。。
		<< R"(</text>)";

		svglines.push_back( ss.str());

	}
	// <g>で属性一括指定：終了
	{
		svglines.push_back( "  </g>");
	}

	// Title

	// グラフタイトル
	string title = "Frequency from pclub03.cpp"s;
	{
		stringstream ss;
		double fontsize = std::floor( cam.actuWidth * 0.7 / title.size() * 2.0); // 描画領域の幅のうち、7割を占めるぐらいのサイズ
		if ( fontsize >= cam.actuYMin * 0.7){ // 余白の高さの70%より大きいのはダメ
			fontsize = cam.actuYMin * 0.7;
		}
		ss << "  "
		<< R"(<text)"
		<< " "
		<< R"(x=")" << ( std::round( cam.getActualMidX())) << R"(")" // 中央揃えをするので。
		<< " "
		<< R"(y=")" << ( std::round( cam.actuYMin * 0.9)) << R"(")" // 余白のうち10%浮かせる。
		<< " "
		<< R"(font-family=")" << "Arial,san-serif" << R"(")"
		<< " "
		<< R"(font-size=")" << fontsize << R"(")"
		<< " "
		<< R"(text-anchor="middle")"
		<< " "
		<< R"(dominant-baseline="text-after-edge")"
		<< ">"
		<< title
		<< R"(</text>)";

		svglines.push_back( ss.str());
	}

	// x軸タイトルを書く。
	string xaxislabel = "Household Income";
	{

		double fontsize = 20; // とりあえずの値。
		double xaxislabelmargin = 30; // とりあえずの値。

		stringstream ss;

		ss << "  "
		<< R"(<text)"
		<< " "
		<< R"(x=")" << ( std::round( cam.getActualMidX())) << R"(")" // 中央揃えをするので。
		<< " "
		<< R"(y=")" << ( std::round( cam.actuYMax + xaxislabelmargin + fontsize)) << R"(")" // 描画領域の下端からmarginだけ離す。
		<< " "
		<< R"(font-family=")" << "Arial,san-serif" << R"(")"
		<< " "
		<< R"(font-size=")" << fontsize << R"(")"
		<< " "
		<< R"(text-anchor="middle")"
		<< " "
		<< R"(dominant-baseline="text-after-edge")" // これでないとIEやWordで崩れる。
		<< ">"
		<< xaxislabel
		<< R"(</text>)";

		svglines.push_back( ss.str());

	}

	// y軸タイトルを書く。
	string yaxislabel = "#Cases";
	{

		double fontsize = 20; // とりあえずの値。
		double yaxislabelmargin = 30; // とりあえずの値。

		stringstream ss;
		double x = std::round( cam.actuXMin - yaxislabelmargin);
		double y = std::round( cam.getActualMidY());

		ss << "  "
		<< R"(<text)"
		<< " "
		<< R"(x=")" << x << R"(")" // 描画領域の左端からmarginだけ離す。
		<< " "
		<< R"(y=")" << y << R"(")" // 中央揃えをするので。
		<< " "
		<< R"(font-family=")" << "Arial,san-serif" << R"(")"
		<< " "
		<< R"(font-size=")" << fontsize << R"(")"
		<< " "
		<< R"(text-anchor="middle")"
		<< " "
		<< R"(dominant-baseline="text-after-edge")"
		<< " "
		<< R"(transform="rotate(270 )" << x << " " << y << ")" << R"(")"
		<< ">"
		<< yaxislabel
		<< R"(</text>)";

		svglines.push_back( ss.str());

	}

	// 周辺情報記載の終了

	// 枠線を描く。最後にすべき。
	// fillは透過させる。
	svglines.push_back( R"(  <rect x="50" y="50" width="400" height="400" fill-opacity="0" stroke="Black" stroke-width="1" />)");

	svglines.push_back( R"(</svg>)");

	string detector = "<!-- "
	                  u8"\u6587\u5B57\u30B3\u30FC\u30C9\u8B58\u5225\u7528" // 「文字コード識別用」というUTF-8文字列
	                  " -->";
	svglines.push_back( detector);

	koutputfile outsvg( fn);
	outsvg.open( false, false, true);
	outsvg.writeLines( svglines);
	outsvg.close();

}

// [min0, max0]に、いい感じの間隔で点をとる。
// k0個以上で最小の点を返す。
// newminがtrueのとき、得られた間隔に乗る新しいminも返す。
// newmaxがtrueのとき、得られた間隔に乗る新しいmaxも返す。
std::vector <double>
getGridPoints( double min0, double max0, int k0 /*= 4*/, bool newmin /*= true*/, bool newmax /*= true*/)
{

	using namespace std;

	vector <double> ret;

	// この数以上の最小の点を返すようにする。
	int minnpoints = k0;

	// error
	if ( min0 >= max0){
		alert( "getGripPoints()");
		return ret;
	}

	double max0ab = abs( max0);
	double min0ab = abs( min0);

	// max0abとmin0abのうち大きい方は何桁？（その値マイナス1）
	double digits_m1 = floor( log10( max( max0ab, min0ab)));

	// 基準となる10のべき乗値
	double base10val = pow( 10.0, digits_m1);

	// 候補となる、intervalの先頭の桁の値
	vector <double> headcands = { 5.0, 2.5, 2.0, 1.0};

	double interval;

	bool loop = true;
	while ( loop){

		for ( auto h : headcands){

			ret.clear();
			interval = base10val * h;

			// setting startpoint; to avoid startpoint being "-0", we do a little trick.
			double startpoint = ceil( min0 / interval);
			if ( startpoint > -1.0 && startpoint < 1.0){
				startpoint = 0.0;
			}
			startpoint *= interval;

			for ( double p = startpoint; p <= max0; p += interval){
				ret.push_back( p);
			}
			if ( ret.size() >= minnpoints){
				loop = false;
				break;
			}

		}

		base10val /= 10.0;

	}

	if ( newmin == true){
		double oldmin = ret.front();
		if ( oldmin == min0){
			// if the first point already obtained is equal to min0
			// do nothing
		} else {
			ret.insert( ret.begin(), oldmin - interval);
		}
	}

	if ( newmax == true){
		double oldmax = ret.back();
		if ( oldmax == max0){
			// if the last point already obtained is equal to max0
			// do nothing
		} else {
			ret.push_back( oldmax + interval);
		}
	}

	return ret;

}

/* ********** Definitions of Member Functions ********** */


/*
simplexSizeGenerator.cpp - This file is part of the Bayesembler (v1.1.1)

The MIT License (MIT)

Copyright (c) 2014 Lasse Maretty and Jonas Andreas Sibbesen

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.
*/

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/lexical_cast.hpp>
#include <simplexSizeGenerator.h>

typedef boost::random::gamma_distribution<> gamma_distribution_t;
typedef boost::random::variate_generator<boost::random::mt19937*, boost::random::gamma_distribution<> > gamma_sampler_t;
typedef boost::random::mt19937* mt_rng_pt_t;
typedef boost::random::uniform_01<boost::random::mt19937*> uniform_01_sampler_t;

int SimplexSizeGenerator::sampleSimplexSize(double pi, double gamma, int count_plus_size) {

	assert(count_plus_size <= num_transcripts);
	assert(pi >= double_underflow);
	assert(pi <= double_almost_one);
	assert(gamma > double_underflow);

	// Init binomial distribution over simplex sizes
	vector <double> simplex_prob_vector;
    simplex_prob_vector.reserve(num_transcripts - count_plus_size + 1);

	// Cardinality of equivalence class of size |s+| is one
	double cardinal_eq_z_log = 0;

	// Calculate probability of member of equivalence class
	double prob_z_log = count_plus_size*log(pi) + (num_transcripts-count_plus_size)*log(1-pi);

	// Probability of assignment given the binary vector
	double prob_t_log = boost::math::lgamma(count_plus_size*gamma) - boost::math::lgamma(num_fragments + count_plus_size*gamma);

	// Full probability of the binary vector
	double prob_eq_z_log = cardinal_eq_z_log + prob_z_log + prob_t_log;
	double row_sum = prob_eq_z_log;

	simplex_prob_vector.push_back(row_sum);

	for (int i = count_plus_size + 1; i < num_transcripts + 1; i++) {

		// Calculate cardinality of equivalence class
		cardinal_eq_z_log = boost::math::lgamma(num_transcripts-count_plus_size+1)-(boost::math::lgamma(i-count_plus_size+1)+boost::math::lgamma(num_transcripts - i + 1));

		// Calculate probability of member of equivalence class
		prob_z_log = i*log(pi) + (num_transcripts-i)*log(1-pi);

		// Probability of assignment given the binary vector
		prob_t_log = boost::math::lgamma(i*gamma) - boost::math::lgamma(num_fragments + i*gamma);

		// Full probability of the binary vector
		prob_eq_z_log = cardinal_eq_z_log + prob_z_log + prob_t_log;

		row_sum += log(1 + exp(prob_eq_z_log - row_sum));
		simplex_prob_vector.push_back(row_sum);

    	if (double_compare(simplex_prob_vector.back(), *(simplex_prob_vector.rbegin() + 1))) {

            break;
        }
	}

	// Row-normalise and transform back from log-space
	for (int i = 0; i < simplex_prob_vector.size(); i++) {

	    simplex_prob_vector[i] = exp(simplex_prob_vector[i] - row_sum);

	}

    assert (simplex_prob_vector.back() > double_almost_one);

	uniform_01_sampler_t sample_uniform_01(mt_rng_pt);

	int b = int(upper_bound(simplex_prob_vector.begin(), simplex_prob_vector.end(), sample_uniform_01()) - simplex_prob_vector.begin()) + count_plus_size;

	return b;
}

bool SimplexSizeGenerator::isFixed() {

	return is_fixed;

}

/* SIMPLEX-SIZE GENERATOR CLASS */
FixedBinomialFixedGammaSimplexSizeGenerator::FixedBinomialFixedGammaSimplexSizeGenerator(double pi_in, double gamma_in, int num_transcripts_in, mt_rng_pt_t mt_rng_pt_in, int num_fragments_in) {

    // Set members
    pi = pi_in;
    gamma = gamma_in;
    num_transcripts = num_transcripts_in;
    is_fixed = true;
    mt_rng_pt = mt_rng_pt_in;
    num_fragments = num_fragments_in;

    simplex_prob_matrix = vector<vector<double> >(num_transcripts, vector<double>());

    // Loop over count plus sizes
    for (int i=0; i < num_transcripts; i++) {

        int count_plus_size = i + 1;

    	// Init binomial distribution over simplex sizes
    	vector < double> simplex_prob_vector;

    	// Cardinality of equivalence class of size |s+| is zero
    	double cardinal_eq_z_log = 0;

    	// Calculate probability of member of equivalence class
    	double prob_z_log = count_plus_size*log(pi) + (num_transcripts-count_plus_size)*log(1-pi);

    	// Probability of assignment given the binary vector
    	double prob_t_log = boost::math::lgamma(count_plus_size*gamma) - boost::math::lgamma(num_fragments + count_plus_size*gamma);

    	// Full probability of the binary vector
    	double prob_eq_z_log = cardinal_eq_z_log + prob_z_log + prob_t_log;
    	double row_sum = prob_eq_z_log;

    	simplex_prob_vector.push_back(row_sum);

    	for (int j = count_plus_size + 1; j < num_transcripts + 1; j++) {

    		// Calculate cardinality of equivalence class
    		cardinal_eq_z_log = boost::math::lgamma(num_transcripts-count_plus_size+1)-(boost::math::lgamma(j-count_plus_size+1)+boost::math::lgamma(num_transcripts - j + 1));

    		// Calculate probability of member of equivalence class
    		prob_z_log = j*log(pi) + (num_transcripts-j)*log(1-pi);

    		// Probability of assignment given the binary vector
    		prob_t_log = boost::math::lgamma(j*gamma) - boost::math::lgamma(num_fragments + j*gamma);

    		// Full probability of the binary std::vector<char> v;
    		prob_eq_z_log = cardinal_eq_z_log + prob_z_log + prob_t_log;

            row_sum += log(1 + exp(prob_eq_z_log - row_sum));
    		simplex_prob_vector.push_back(row_sum);

            if (double_compare(simplex_prob_vector.back(), *(simplex_prob_vector.rbegin() + 1))) {

                break;
            }
    	}

    	// Row-normalise and transform back from log-space
    	for (int j = 0; j < simplex_prob_vector.size(); j++) {

    	    simplex_prob_vector[j] = exp(simplex_prob_vector[j] - row_sum);

    	}

        assert (simplex_prob_vector.back() > double_almost_one);

        simplex_prob_matrix[i] = simplex_prob_vector;

    }
}

string FixedBinomialFixedGammaSimplexSizeGenerator::getParameterString(){

    string parameter_str;
    parameter_str += "pi";
    parameter_str += boost::lexical_cast<string>(pi);
    parameter_str += "gamma";
    parameter_str += boost::lexical_cast<string>(gamma);

    return parameter_str;
}

// Initialise simplex probability matrix
pair<int, double> FixedBinomialFixedGammaSimplexSizeGenerator::initSimplexSize(int count_plus_size, double gamma) {

    int b = sampleSimplexSize(pi, gamma, count_plus_size);

    return pair<int, double>(b, pi);
}

// Samples a simplex size from the simplex size probability matrix
pair<int, double> FixedBinomialFixedGammaSimplexSizeGenerator::generateSimplexSize(int expression_plus_size, int count_plus_size, double gamma) {

    int b = sampleSimplexSize(pi, gamma, count_plus_size);

    return pair<int, double>(b, pi);
}

int FixedBinomialFixedGammaSimplexSizeGenerator::sampleSimplexSize(double pi, double gamma, int count_plus_size) {

    uniform_01_sampler_t sample_uniform_01(mt_rng_pt);

    int b = int(upper_bound(simplex_prob_matrix[count_plus_size-1].begin(), simplex_prob_matrix[count_plus_size-1].end(), sample_uniform_01()) - simplex_prob_matrix[count_plus_size-1].begin()) + count_plus_size;

    return b;

}

FixedBinomialSimplexSizeGenerator::FixedBinomialSimplexSizeGenerator(double pi_in, int num_transcripts_in, mt_rng_pt_t mt_rng_pt_in, int num_fragments_in) {

	// Set members
	pi = pi_in;
	num_transcripts = num_transcripts_in;
	is_fixed = true;
	mt_rng_pt = mt_rng_pt_in;
	num_fragments = num_fragments_in;
}

string FixedBinomialSimplexSizeGenerator::getParameterString(){

    string parameter_str;
	parameter_str += "pi";
    parameter_str += boost::lexical_cast<string>(pi);

    return parameter_str;
}

// Initialise simplex probability matrix
pair<int, double> FixedBinomialSimplexSizeGenerator::initSimplexSize(int count_plus_size, double gamma) {

	int b = sampleSimplexSize(pi, gamma, count_plus_size);

	return pair<int, double>(b, pi);
}

// Samples a simplex size from the simplex size probability matrix
pair<int, double> FixedBinomialSimplexSizeGenerator::generateSimplexSize(int expression_plus_size, int count_plus_size, double gamma) {

	int b = sampleSimplexSize(pi, gamma, count_plus_size);

	return pair<int, double>(b, pi);
}

BetaBinomialSimplexSizeGenerator::BetaBinomialSimplexSizeGenerator(double alpha_in, double beta_in, int num_transcripts_in, mt_rng_pt_t mt_rng_pt_in, int num_fragments_in, int slice_iterations_in, double slice_window_size_in) {

	alpha = alpha_in;
	beta = beta_in;
	num_transcripts = num_transcripts_in;
	is_fixed = false;
	mt_rng_pt = mt_rng_pt_in;
	num_fragments = num_fragments_in;
	slice_iterations = slice_iterations_in;
    slice_window_size = slice_window_size_in;

}

string BetaBinomialSimplexSizeGenerator::getParameterString(){

	string parameter_str;
	parameter_str += "alpha";
    parameter_str += boost::lexical_cast<string>(alpha);
	parameter_str += "_";
	parameter_str += "beta";
    parameter_str += boost::lexical_cast<string>(beta);

    return parameter_str;
}

pair<int, double> BetaBinomialSimplexSizeGenerator::initSimplexSize(int count_plus_size, double gamma) {

	// Sample pi from beta-prior
	gamma_distribution_t gamma_dist_alpha(alpha,1);
	gamma_distribution_t gamma_dist_beta(beta,1);

	gamma_sampler_t sample_gamma_alpha(mt_rng_pt, gamma_dist_alpha);
	gamma_sampler_t sample_gamma_beta(mt_rng_pt, gamma_dist_beta);

	double sample_alpha = sample_gamma_alpha();
	double sample_beta = sample_gamma_beta();

	double pi = sample_alpha / (sample_alpha + sample_beta);

	if (pi > double_almost_one) {

		pi = double_almost_one;
	}

	if (pi < double_precision) {

		pi = double_precision;
	}

	int b = sampleSimplexSize(pi, gamma, count_plus_size);

	return pair<int, double>(b, pi);

}

pair<int, double> BetaBinomialSimplexSizeGenerator::generateSimplexSize(int expression_plus_size, int count_plus_size, double gamma) {

	double pi = samplePi(expression_plus_size);

	if (pi > double_almost_one) {

		pi = double_almost_one;
	}

	if (pi < double_precision) {

		pi = double_precision;
	}

	int b = sampleSimplexSize(pi, gamma, count_plus_size);

	return pair<int, double>(b, pi);
}

double BetaBinomialSimplexSizeGenerator::posteriorPiLogDensity(int expression_plus_size, double pi) {

	return ((alpha + expression_plus_size - 1)*log(pi) + (beta + num_transcripts - expression_plus_size - 1)*log(1-pi) - log(1 - exp(num_transcripts*log(1-pi))));

}

double BetaBinomialSimplexSizeGenerator::samplePi(int expression_plus_size) {

	// Init gamma and uniform sampler
	uniform_01_sampler_t sample_uniform_01(mt_rng_pt);

	double pi_current = sample_uniform_01();
	double pi = pi_current;

	if (pi_current < double_precision) {

		pi_current = double_precision;

	}

	if (pi_current > double_almost_one) {

		pi_current = double_almost_one;

	}

	// Output all samples for convergence assessment
	// ofstream pi_slice_out("pi_slice_out.txt", ios::app);

	for (int i=0; i < slice_iterations; i++) {

		// Sample height
		double y = posteriorPiLogDensity(expression_plus_size, pi_current) + log(1-sample_uniform_01());

		// Find slice by "step-out"
		double left = pi_current - sample_uniform_01() * slice_window_size;
		double right = left + slice_window_size;

		int j = 1;
		int k = 1;

		// Truncate distribution at zero
		if (left < double_precision) {
			left = double_precision;
			j = 0;
		}

		// Truncate distribution at one
		if (right > double_almost_one) {
			right = double_almost_one;
			k = 0;
		}

		// Expand window to the left
		while (j == 1 && y < (posteriorPiLogDensity(expression_plus_size, left))) {
			left = left - slice_window_size;

			if (left < double_precision) {
				left = double_precision;
				break;
			}
		}

		// Expand window to the right
		while (k == 1 && y < (posteriorPiLogDensity(expression_plus_size, right))) {
			right = right + slice_window_size;

			if (right > double_almost_one) {
				right = double_almost_one;
				break;
			}
		}

		// Sample from the window and step-in window boundaries until in slice
		pi = sample_uniform_01()*(right-left) + left;

		while ( y >= (posteriorPiLogDensity(expression_plus_size, pi))) {

            if (pi < pi_current) {

				left = pi;
				pi = sample_uniform_01()*(right-left) + left;

			} else {

				right = pi;
				pi = sample_uniform_01()*(right-left) + left;
			}
		}

		pi_current = pi;
		// pi_slice_out << pi << endl;
	}

	// pi_slice_out.close();

	return pi;
}


#include <sequential-line-search/gaussianprocessregressor.h>
#include <sequential-line-search/utils.h>
#include <iostream>
#include <cmath>
#include <Eigen/LU>
#include <nlopt-util.hpp>

//#define NOISELESS

using Eigen::MatrixXd;
using Eigen::VectorXd;

namespace
{
    using namespace sequential_line_search;

    const bool   useLogNormalPrior     = true;
    const double a_prior_mu            = std::log(0.500);
    const double a_prior_sigma_squared = 0.10;
#ifdef NOISELESS
    const double b_fixed               = 1e-06;
#else
    const double b_prior_mu            = std::log(0.001);
    const double b_prior_sigma_squared = 0.10;
#endif
    const double r_prior_mu            = std::log(0.500);
    const double r_prior_sigma_squared = 0.10;

    double calc_grad_a_prior(const double a)
    {
        return (a_prior_mu - a_prior_sigma_squared - std::log(a)) / (a_prior_sigma_squared * a);
    }

#ifndef NOISELESS
    double calc_grad_b_prior(const double b)
    {
        return (b_prior_mu - b_prior_sigma_squared - std::log(b)) / (b_prior_sigma_squared * b);
    }
#endif

    double calc_grad_r_i_prior(const Eigen::VectorXd &r, const int index)
    {
        return (r_prior_mu - r_prior_sigma_squared - std::log(r(index))) / (r_prior_sigma_squared * r(index));
    }

    double calc_a_prior(const double a)
    {
        return std::log(utils::log_normal(a, a_prior_mu, a_prior_sigma_squared));
    }

#ifndef NOISELESS
    double calc_b_prior(const double b)
    {
        return std::log(utils::log_normal(b, b_prior_mu, b_prior_sigma_squared));
    }
#endif

    double calc_r_i_prior(const Eigen::VectorXd &r, const int index)
    {
        return std::log(utils::log_normal(r(index), r_prior_mu, r_prior_sigma_squared));
    }

    double calc_grad_a(const MatrixXd& X, const MatrixXd& C_inv, const VectorXd& y, const double a, const double b, const VectorXd& r)
    {
        const MatrixXd C_grad_a = Regressor::calc_C_grad_a(X, a, b, r);
        const double term1 = + 0.5 * y.transpose() * C_inv * C_grad_a * C_inv * y;
        const double term2 = - 0.5 * (C_inv * C_grad_a).trace();
        return term1 + term2 + (useLogNormalPrior ? calc_grad_a_prior(a) : 0.0);
    }

#ifndef NOISELESS
    double calc_grad_b(const MatrixXd& X, const MatrixXd& C_inv, const VectorXd& y, const double a, const double b, const VectorXd& r)
    {
        const MatrixXd C_grad_b = Regressor::calc_C_grad_b(X, a, b, r);
        const double term1 = + 0.5 * y.transpose() * C_inv * C_grad_b * C_inv * y;
        const double term2 = - 0.5 * (C_inv * C_grad_b).trace();
        return term1 + term2 + (useLogNormalPrior ? calc_grad_b_prior(b) : 0.0);
    }
#endif

    double calc_grad_r_i(const MatrixXd& X, const MatrixXd& C_inv, const VectorXd& y, const double a, const double b, const VectorXd& r, const int index)
    {
        const MatrixXd C_grad_r_i = Regressor::calc_C_grad_r_i(X, a, b, r, index);
        const double term1 = + 0.5 * y.transpose() * C_inv * C_grad_r_i * C_inv * y;
        const double term2 = - 0.5 * (C_inv * C_grad_r_i).trace();
        return term1 + term2 + (useLogNormalPrior ? calc_grad_r_i_prior(r, index) : 0.0);
    }

    VectorXd calc_grad(const MatrixXd& X, const MatrixXd& C_inv, const VectorXd& y, const double a, const double b, const VectorXd& r)
    {
        const unsigned D = X.rows();

        VectorXd grad(D + 2);
        grad(0) = calc_grad_a(X, C_inv, y, a, b, r);
#ifdef NOISELESS
        grad(1) = 0.0;
#else
        grad(1) = calc_grad_b(X, C_inv, y, a, b, r);
#endif

        for (unsigned i = 2; i < D + 2; ++ i)
        {
            const unsigned index = i - 2;
            grad(i) = calc_grad_r_i(X, C_inv, y, a, b, r, index);
        }

        return grad;
    }

    struct Data
    {
        const MatrixXd X;
        const VectorXd y;
    };

    // For counting the number of function evaluations
    unsigned count;

    // Log likelihood that will be maximized
    double objective(const std::vector<double> &x, std::vector<double>& grad, void* data)
    {
        // For counting the number of function evaluations
        ++ count;

        const MatrixXd& X = static_cast<const Data*>(data)->X;
        const VectorXd& y = static_cast<const Data*>(data)->y;

        const unsigned N = X.cols();

        const double   a = x[0];
#ifdef NOISELESS
        const double   b = b_fixed;
#else
        const double   b = x[1];
#endif
        const VectorXd r = Eigen::Map<const VectorXd>(&x[2], x.size() - 2);

        const MatrixXd C     = Regressor::calc_C(X, a, b, r);
        const MatrixXd C_inv = C.inverse();

        // When the algorithm is gradient-based, compute the gradient vector
        if (grad.size() == x.size())
        {
            const VectorXd g = calc_grad(X, C_inv, y, a, b, r);
            for (unsigned i = 0; i < g.rows(); ++ i) grad[i] = g(i);
        }

        const double term1 = - 0.5 * y.transpose() * C_inv * y;
        const double term2 = - 0.5 * std::log(C.determinant());
        const double term3 = - 0.5 * N * std::log(2.0 * M_PI);

        // Computing the regularization terms from a prior assumptions
        const double a_prior = calc_a_prior(a);
#ifdef NOISELESS
        const double b_prior = 1.0;
#else
        const double b_prior = calc_b_prior(b);
#endif
        const double r_prior = [&r]()
        {
            double sum = 0.0;
            for (unsigned i = 0; i < r.rows(); ++ i) sum += calc_r_i_prior(r, i);
            return sum;
        }();
        const double regularization = useLogNormalPrior ? (a_prior + b_prior + r_prior) : 0.0;

        return term1 + term2 + term3 + regularization;
    }
}

namespace sequential_line_search
{
    GaussianProcessRegressor::GaussianProcessRegressor(const MatrixXd& X, const VectorXd& y)
    {
        this->X = X;
        this->y = y;

        if (X.rows() == 0) return;

        compute_MAP();

        C     = calc_C(X, a, b, r);
        C_inv = C.inverse();
    }

    GaussianProcessRegressor::GaussianProcessRegressor(const Eigen::MatrixXd &X, const Eigen::VectorXd &y, double a, double b, const Eigen::VectorXd &r)
    {
        this->X = X;
        this->y = y;
        this->a = a;
        this->b = b;
        this->r = r;

        C     = calc_C(X, a, b, r);
        C_inv = C.inverse();
    }

    double GaussianProcessRegressor::estimate_y(const VectorXd &x) const
    {
        const VectorXd k = calc_k(x, X, a, b, r);
        return k.transpose() * C_inv * y;
    }

    double GaussianProcessRegressor::estimate_s(const VectorXd &x) const
    {
        const VectorXd k = calc_k(x, X, a, b, r);
        return std::sqrt(a + b - k.transpose() * C_inv * k);
    }

    void GaussianProcessRegressor::compute_MAP()
    {
        const unsigned D = X.rows();

        Data data{ X, y };

        const VectorXd x_ini = VectorXd::Constant(D + 2, 1e+00);
        const VectorXd upper = VectorXd::Constant(D + 2, 5e+01);
        const VectorXd lower = VectorXd::Constant(D + 2, 1e-08);

        const VectorXd x_glo = nloptutil::solve(x_ini, upper, lower, objective, nlopt::GN_DIRECT, &data, 300);
        const VectorXd x_loc = nloptutil::solve(x_glo, upper, lower, objective, nlopt::LD_TNEWTON, &data, 1000);

        a = x_loc(0);
        b = x_loc(1);
        r = x_loc.block(2, 0, D, 1);

#ifdef NOISELESS
        b = b_fixed;
#endif
    }
}


/**
 * \file LinkwitzRileyFilter.cpp
 */

#include <boost/math/tools/polynomial.hpp>

#include "LinkwitzRileyFilter.h"
#include "IIRFilter.h"

namespace ATK
{
  template<typename DataType>
  LinkwitzRileyLowPassCoefficients<DataType>::LinkwitzRileyLowPassCoefficients(int nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void LinkwitzRileyLowPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType omega = boost::math::constants::pi<DataType>() * cut_frequency;
    DataType kappa = omega / std::tan(omega / input_sampling_rate);
    DataType delta = kappa * kappa + omega * omega + 2 * kappa * omega;

    coefficients_in[2] = omega * omega / delta;
    coefficients_in[1] = 2 * omega * omega / delta;
    coefficients_in[0] = omega * omega / delta;
    coefficients_out[1] = 2 * (kappa * kappa - omega * omega) / delta;
    coefficients_out[0] = -(omega * omega + kappa * kappa - 2 * kappa * omega) / delta;
  }

  template<typename DataType>
  LinkwitzRileyHighPassCoefficients<DataType>::LinkwitzRileyHighPassCoefficients(int nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void LinkwitzRileyHighPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType omega = boost::math::constants::pi<DataType>() * cut_frequency;
    DataType kappa = omega / std::tan(omega / input_sampling_rate);
    DataType delta = kappa * kappa + omega * omega + 2 * kappa * omega;

    coefficients_in[2] = kappa * kappa / delta;
    coefficients_in[1] = - 2 * kappa * kappa / delta;
    coefficients_in[0] = kappa * kappa / delta;
    coefficients_out[1] = 2 * (kappa * kappa - omega * omega) / delta;
    coefficients_out[0] = -(omega * omega + kappa * kappa - 2 * kappa * omega) / delta;
  }

  template class LinkwitzRileyLowPassCoefficients<float>;
  template class LinkwitzRileyLowPassCoefficients<double>;
  template class LinkwitzRileyHighPassCoefficients<float>;
  template class LinkwitzRileyHighPassCoefficients<double>;

  template class IIRFilter<LinkwitzRileyLowPassCoefficients<float> >;
  template class IIRFilter<LinkwitzRileyLowPassCoefficients<double> >;
  template class IIRFilter<LinkwitzRileyHighPassCoefficients<float> >;
  template class IIRFilter<LinkwitzRileyHighPassCoefficients<double> >;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EULER_FUNCTIONS_SCALAR_GAMMA_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SCALAR_GAMMA_HPP_INCLUDED
#include <nt2/euler/functions/gamma.hpp>
#include <nt2/euler/functions/details/gamma_kernel.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/copysign.hpp>
#include <nt2/include/functions/scalar/floor.hpp>
#include <nt2/include/functions/scalar/is_eqz.hpp>
#include <nt2/include/functions/scalar/is_even.hpp>
#include <nt2/include/functions/scalar/is_ltz.hpp>
#include <nt2/include/functions/scalar/sinpi.hpp>
#include <nt2/include/functions/scalar/stirling.hpp>

#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/functions/scalar/is_nan.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( gamma_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      if (is_eqz(a0)) return copysign(Inf<A0>(), a0);
      #ifndef BOOST_SIMD_NO_INVALIDS
      if( nt2::is_nan(a0) || (a0 == nt2::Minf<A0>()) ) return nt2::Nan<A0>();
      if (a0 == nt2::Inf<A0>()) return a0;
      #endif

      A0 x = a0;
      A0 q = nt2::abs(x);
      if(x < A0(-33.0))
      {
        A0 st = nt2::stirling(q);
        A0 p =  nt2::floor(q);
        bool iseven =  nt2::is_even((int32_t)p);
        if (p == q) return nt2::Nan<A0>();
        A0 z = q - p;
        if( z > nt2::Half<A0>() )
        {
          p += nt2::One<A0>();
          z = q - p;
        }
        z = q*nt2::sinpi(z);
        if( nt2::is_eqz(z) ) return nt2::Nan<A0>();
        st = nt2::Pi<A0>()/(nt2::abs(z)*st);
        return iseven  ? -st : st;
      }
      A0 z = nt2::One<A0>();
      while( x >= nt2::Three<A0>() )
      {
        x -= nt2::One<A0>();
        z *= x;
      }
      while( nt2::is_ltz(x) )
      {
        z /= x;
        x += nt2::One<A0>();
      }
      while( x < nt2::Two<A0>() )
      {
        if( nt2::is_eqz(x)) return nt2::Nan<A0>();
        z /= x;
        x +=  nt2::One<A0>();
      }
      if( x == nt2::Two<A0>() ) return(z);
      x -= nt2::Two<A0>();
      return z*details::gamma_kernel<A0>::gamma1(x);
    }
  };
} }

#endif


#include <vector>
#include <boost/math/distributions/beta.hpp>
#include "beta_dist.h"

stochastic::BetaDistribution::BetaDistribution(double alpha, double beta)
  : Distribution(),
    alpha_{alpha},
    beta_{beta},
    distribution_{alpha, beta_}
{}

std::vector<double> stochastic::BetaDistribution::cumulative_dist_func(
    const std::vector<double>& locations) const {
  std::vector<double> evaluations(locations.size());

  for (unsigned int i = 0; i < locations.size(); ++i) {
    evaluations[i] = cdf(distribution_, locations[i]);
  }

  return evaluations;
}

std::vector<double> stochastic::BetaDistribution::inv_cumulative_dist_func(
    const std::vector<double>& probabilities) const {
  std::vector<double> evaluations(probabilities.size());

  for (unsigned int i = 0; i < probabilities.size(); ++i) {
    evaluations[i] = quantile(distribution_, probabilities[i]);
  }

  return evaluations;
}


// Boost.Geometry

// Copyright (c) 2016 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP
#define BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/algorithms/detail/flattening.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief The intersection of two geodesics as proposed by Sjoberg.
\author See
    - [Sjoberg02] Lars E. Sjoberg, Intersections on the sphere and ellipsoid, 2002
      http://link.springer.com/article/10.1007/s00190-001-0230-9
    - [Sjoberg07] Lars E. Sjoberg, Geodetic intersection on the ellipsoid, 2007
      http://link.springer.com/article/10.1007/s00190-007-0204-7
*/
template
<
    typename CT,
    template <typename, bool, bool, bool, bool, bool> class Inverse,
    unsigned int Order = 4
>
class sjoberg_intersection
{
    typedef Inverse<CT, false, true, false, false, false> inverse_type;
    typedef typename inverse_type::result_type inverse_result;

public:
    template <typename T1, typename T2, typename Spheroid>
    static inline bool apply(T1 const& lona1, T1 const& lata1,
                             T1 const& lona2, T1 const& lata2,
                             T2 const& lonb1, T2 const& latb1,
                             T2 const& lonb2, T2 const& latb2,
                             CT & lon, CT & lat,
                             Spheroid const& spheroid)
    {
        CT const lon_a1 = lona1;
        CT const lat_a1 = lata1;
        CT const lon_a2 = lona2;
        CT const lat_a2 = lata2;
        CT const lon_b1 = lonb1;
        CT const lat_b1 = latb1;
        CT const lon_b2 = lonb2;
        CT const lat_b2 = latb2;

        CT const alpha1 = inverse_type::apply(lon_a1, lat_a1, lon_a2, lat_a2, spheroid).azimuth;
        CT const alpha2 = inverse_type::apply(lon_b1, lat_b1, lon_b2, lat_b2, spheroid).azimuth;

        return apply(lon_a1, lat_a1, alpha1, lon_b1, lat_b1, alpha2, lon, lat, spheroid);
    }

    template <typename Spheroid>
    static inline bool apply(CT const& lon1, CT const& lat1, CT const& alpha1,
                             CT const& lon2, CT const& lat2, CT const& alpha2,
                             CT & lon, CT & lat,
                             Spheroid const& spheroid)
    {
        // coordinates in radians

        // TODO - handle special cases like degenerated segments, equator, poles, etc.

        CT const c0 = 0;
        CT const c1 = 1;
        CT const c2 = 2;

        CT const pi = math::pi<CT>();
        CT const pi_half = pi / c2;
        CT const f = detail::flattening<CT>(spheroid);
        CT const one_minus_f = c1 - f;
        CT const e_sqr = f * (c2 - f);

        CT const sin_alpha1 = sin(alpha1);
        CT const sin_alpha2 = sin(alpha2);

        CT const tan_beta1 = one_minus_f * tan(lat1);
        CT const tan_beta2 = one_minus_f * tan(lat2);
        CT const beta1 = atan(tan_beta1);
        CT const beta2 = atan(tan_beta2);
        CT const cos_beta1 = cos(beta1);
        CT const cos_beta2 = cos(beta2);
        CT const sin_beta1 = sin(beta1);
        CT const sin_beta2 = sin(beta2);

        // Clairaut constants (lower-case in the paper)
        int const sign_C1 = math::abs(alpha1) <= pi_half ? 1 : -1;
        int const sign_C2 = math::abs(alpha2) <= pi_half ? 1 : -1;
        // Cj = 1 if on equator
        CT const C1 = sign_C1 * cos_beta1 * sin_alpha1;
        CT const C2 = sign_C2 * cos_beta2 * sin_alpha2;

        CT const sqrt_1_C1_sqr = math::sqrt(c1 - math::sqr(C1));
        CT const sqrt_1_C2_sqr = math::sqrt(c1 - math::sqr(C2));

        // handle special case: segments on the equator
        bool const on_equator1 = math::equals(sqrt_1_C1_sqr, c0);
        bool const on_equator2 = math::equals(sqrt_1_C2_sqr, c0);
        if (on_equator1 && on_equator2)
        {
            return false;
        }
        else if (on_equator1)
        {
            CT const dL2 = d_lambda_e_sqr(sin_beta2, c0, C2, sqrt_1_C2_sqr, e_sqr);
            CT const asin_t2_t02 = asin(C2 * tan_beta2 / sqrt_1_C2_sqr);
            lat = c0;
            lon = lon2 - asin_t2_t02 + dL2;
            return true;
        }
        else if (on_equator2)
        {
            CT const dL1 = d_lambda_e_sqr(sin_beta1, c0, C1, sqrt_1_C1_sqr, e_sqr);
            CT const asin_t1_t01 = asin(C1 * tan_beta1 / sqrt_1_C1_sqr);
            lat = c0;
            lon = lon1 - asin_t1_t01 + dL1;
            return true;
        }

        CT const t01 = sqrt_1_C1_sqr / C1;
        CT const t02 = sqrt_1_C2_sqr / C2;

        CT const asin_t1_t01 = asin(tan_beta1 / t01);
        CT const asin_t2_t02 = asin(tan_beta2 / t02);
        CT const t01_t02 = t01 * t02;
        CT const t01_t02_2 = c2 * t01_t02;
        CT const sqr_t01_sqr_t02 = math::sqr(t01) + math::sqr(t02);

        CT t = tan_beta1;
        int t_id = 0;

        // find the initial t using simplified spherical solution
        // though not entirely since the reduced latitudes and azimuths are spheroidal
        // [Sjoberg07]
        CT const k_base = lon1 - lon2 + asin_t2_t02 - asin_t1_t01;

        {
            CT const K = sin(k_base);
            CT const d1 = sqr_t01_sqr_t02;
            //CT const d2 = t01_t02_2 * math::sqrt(c1 - math::sqr(K));
            CT const d2 = t01_t02_2 * cos(k_base);
            CT const D1 = math::sqrt(d1 - d2);
            CT const D2 = math::sqrt(d1 + d2);
            CT const K_t01_t02 = K * t01_t02;

            CT const T1 = K_t01_t02 / D1;
            CT const T2 = K_t01_t02 / D2;
            CT asin_T1_t01 = 0;
            CT asin_T1_t02 = 0;
            CT asin_T2_t01 = 0;
            CT asin_T2_t02 = 0;

            // test 4 possible results
            CT l1 = 0, l2 = 0, dl = 0;
            bool found = check_t<0>( T1,
                                    lon1,  asin_T1_t01 = asin(T1 / t01), asin_t1_t01,
                                    lon2,  asin_T1_t02 = asin(T1 / t02), asin_t2_t02,
                                    t, l1, l2, dl, t_id)
                      || check_t<1>(-T1,
                                    lon1, -asin_T1_t01                 , asin_t1_t01,
                                    lon2, -asin_T1_t02                 , asin_t2_t02,
                                    t, l1, l2, dl, t_id)
                      || check_t<2>( T2,
                                    lon1,  asin_T2_t01 = asin(T2 / t01), asin_t1_t01,
                                    lon2,  asin_T2_t02 = asin(T2 / t02), asin_t2_t02,
                                    t, l1, l2, dl, t_id)
                      || check_t<3>(-T2,
                                    lon1, -asin_T2_t01                 , asin_t1_t01,
                                    lon2, -asin_T2_t02                 , asin_t2_t02,
                                    t, l1, l2, dl, t_id);

            boost::ignore_unused(found);
        }

        // [Sjoberg07]
        //int const d2_sign = t_id < 2 ? -1 : 1;
        int const t_sign = (t_id % 2) ? -1 : 1;
        // [Sjoberg02]
        CT const C1_sqr = math::sqr(C1);
        CT const C2_sqr = math::sqr(C2);

        CT beta = atan(t);
        CT dL1 = 0, dL2 = 0;
        CT asin_t_t01 = 0;
        CT asin_t_t02 = 0;

        for (int i = 0; i < 10; ++i)
        {
            CT const sin_beta = sin(beta);

            // integrals approximation
            dL1 = d_lambda_e_sqr(sin_beta1, sin_beta, C1, sqrt_1_C1_sqr, e_sqr);
            dL2 = d_lambda_e_sqr(sin_beta2, sin_beta, C2, sqrt_1_C2_sqr, e_sqr);

            // [Sjoberg07]
            /*CT const k = k_base + dL1 - dL2;
            CT const K = sin(k);
            CT const d1 = sqr_t01_sqr_t02;
            //CT const d2 = t01_t02_2 * math::sqrt(c1 - math::sqr(K));
            CT const d2 = t01_t02_2 * cos(k);
            CT const D = math::sqrt(d1 + d2_sign * d2);
            CT const t_new = t_sign * K * t01_t02 / D;
            CT const dt = math::abs(t_new - t);
            t = t_new;
            CT const new_beta = atan(t);
            CT const dbeta = math::abs(new_beta - beta);
            beta = new_beta;*/

            // [Sjoberg02] - it converges faster
            // Newton�Raphson method
            asin_t_t01 = asin(t / t01);
            asin_t_t02 = asin(t / t02);
            CT const R1 = asin_t_t01 + dL1;
            CT const R2 = asin_t_t02 + dL2;
            CT const cos_beta = cos(beta);
            CT const cos_beta_sqr = math::sqr(cos_beta);
            CT const G = c1 - e_sqr * cos_beta_sqr;
            CT const f1 = C1 / cos_beta * math::sqrt(G / (cos_beta_sqr - C1_sqr));
            CT const f2 = C2 / cos_beta * math::sqrt(G / (cos_beta_sqr - C2_sqr));
            CT const abs_f1 = math::abs(f1);
            CT const abs_f2 = math::abs(f2);
            CT const dbeta = t_sign * (k_base - R2 + R1) / (abs_f1 + abs_f2);

            if (math::equals(dbeta, CT(0)))
            {
                break;
            }

            beta = beta - dbeta;
            t = tan(beta);
        }

        // t = tan(beta) = (1-f)tan(lat)
        lat = atan(t / one_minus_f);

        CT const l1 = lon1 + asin_t_t01 - asin_t1_t01 + dL1;
        //CT const l2 = lon2 + asin_t_t02 - asin_t2_t02 + dL2;
        lon = l1;

        return true;
    }

private:
    /*! Approximation of dLambda_j [Sjoberg07], expanded into taylor series in e^2
        Maxima script:
        dLI_j(c_j, sinB_j, sinB) := integrate(1 / (sqrt(1 - c_j ^ 2 - x ^ 2)*(1 + sqrt(1 - e2*(1 - x ^ 2)))), x, sinB_j, sinB);
        dL_j(c_j, B_j, B) := -e2 * c_j * dLI_j(c_j, B_j, B);
        S: taylor(dLI_j(c_j, sinB_j, sinB), e2, 0, 3);
        assume(c_j < 1);
        assume(c_j > 0);
        L1: factor(integrate(sqrt(-x ^ 2 - c_j ^ 2 + 1) / (x ^ 2 + c_j ^ 2 - 1), x));
        L2: factor(integrate(((x ^ 2 - 1)*sqrt(-x ^ 2 - c_j ^ 2 + 1)) / (x ^ 2 + c_j ^ 2 - 1), x));
        L3: factor(integrate(((x ^ 4 - 2 * x ^ 2 + 1)*sqrt(-x ^ 2 - c_j ^ 2 + 1)) / (x ^ 2 + c_j ^ 2 - 1), x));
        L4: factor(integrate(((x ^ 6 - 3 * x ^ 4 + 3 * x ^ 2 - 1)*sqrt(-x ^ 2 - c_j ^ 2 + 1)) / (x ^ 2 + c_j ^ 2 - 1), x));
    */
    static inline CT d_lambda_e_sqr(CT const& sin_betaj, CT const& sin_beta,
                                    CT const& Cj, CT const& sqrt_1_Cj_sqr,
                                    CT const& e_sqr)
    {
        if (Order == 0)
        {
            return 0;
        }

        CT const c2 = 2;

        CT const asin_B = asin(sin_beta / sqrt_1_Cj_sqr);
        CT const asin_Bj = asin(sin_betaj / sqrt_1_Cj_sqr);
        CT const L0 = (asin_B - asin_Bj) / c2;

        if (Order == 1)
        {
            return -Cj * e_sqr * L0;
        }

        CT const c1 = 1;
        CT const c16 = 16;

        CT const X = sin_beta;
        CT const Xj = sin_betaj;
        CT const Cj_sqr = math::sqr(Cj);
        CT const Cj_sqr_plus_one = Cj_sqr + c1;
        CT const one_minus_Cj_sqr = c1 - Cj_sqr;
        CT const sqrt_Y = math::sqrt(-math::sqr(X) + one_minus_Cj_sqr);
        CT const sqrt_Yj = math::sqrt(-math::sqr(Xj) + one_minus_Cj_sqr);
        CT const L1 = (Cj_sqr_plus_one * (asin_B - asin_Bj) + X * sqrt_Y - Xj * sqrt_Yj) / c16;

        if (Order == 2)
        {
            return -Cj * e_sqr * (L0 + e_sqr * L1);
        }

        CT const c3 = 3;
        CT const c5 = 5;
        CT const c128 = 128;

        CT const E = Cj_sqr * (c3 * Cj_sqr + c2) + c3;
        CT const X_sqr = math::sqr(X);
        CT const Xj_sqr = math::sqr(Xj);
        CT const F = X * (-c2 * X_sqr + c3 * Cj_sqr + c5);
        CT const Fj = Xj * (-c2 * Xj_sqr + c3 * Cj_sqr + c5);
        CT const L2 = (E * (asin_B - asin_Bj) + F * sqrt_Y - Fj * sqrt_Yj) / c128;

        if (Order == 3)
        {
            return -Cj * e_sqr * (L0 + e_sqr * (L1 + e_sqr * L2));
        }

        CT const c8 = 8;
        CT const c9 = 9;
        CT const c10 = 10;
        CT const c15 = 15;
        CT const c24 = 24;
        CT const c26 = 26;
        CT const c33 = 33;
        CT const c6144 = 6144;

        CT const G = Cj_sqr * (Cj_sqr * (Cj_sqr * c15 + c9) + c9) + c15;
        CT const H = -c10 * Cj_sqr - c26;
        CT const I = Cj_sqr * (Cj_sqr * c15 + c24) + c33;
        CT const J = X_sqr * (X * (c8 * X_sqr + H)) + X * I;
        CT const Jj = Xj_sqr * (Xj * (c8 * Xj_sqr + H)) + Xj * I;
        CT const L3 = (G * (asin_B - asin_Bj) + J * sqrt_Y - Jj * sqrt_Yj) / c6144;

        // Order 4 and higher
        return -Cj * e_sqr * (L0 + e_sqr * (L1 + e_sqr * (L2 + e_sqr * L3)));
    }

    static inline CT fj(CT const& cos_beta, CT const& cos2_beta, CT const& Cj, CT const& e_sqr)
    {
        CT const c1 = 1;
        CT const Cj_sqr = math::sqr(Cj);
        return Cj / cos_beta * math::sqrt((c1 - e_sqr * cos2_beta) / (cos2_beta - Cj_sqr));
    }

    template <int TId>
    static inline bool check_t(CT const& t,
                               CT const& lon_a1, CT const& asin_t_t01, CT const& asin_t1_t01,
                               CT const& lon_b1, CT const& asin_t_t02, CT const& asin_t2_t02,
                               CT & current_t, CT & current_lon1, CT & current_lon2, CT & current_dlon,
                               int & t_id)
    {
        CT const lon1 = lon_a1 + asin_t_t01 - asin_t1_t01;
        CT const lon2 = lon_b1 + asin_t_t02 - asin_t2_t02;

        // TODO - true angle difference
        CT const dlon = math::abs(lon2 - lon1);

        bool are_equal = math::equals(dlon, CT(0));

        if ((TId == 0) || are_equal || dlon < current_dlon)
        {
            current_t = t;
            current_lon1 = lon1;
            current_lon2 = lon2;
            current_dlon = dlon;
            t_id = TId;
        }

        return are_equal;
    }
};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP


/*
*  @file 		ex7.cpp
*  @details  	This file is the solution to exercise 7.
*  @author    	Alexander Rettkowski
*  @date      	28.06.2017
*/
#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/fusion/include/io.hpp>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/property_map/property_map.hpp>

#include <boost/timer/timer.hpp>
#include <boost/chrono.hpp>

#include <iostream>
#include <string>
#include <complex>
#include <fstream>
#include <queue>

using namespace boost;

namespace exercise5
{
	namespace qi = boost::spirit::qi;
	namespace ascii = boost::spirit::ascii;
	struct edge
	{
		int startNode;
		int endNode;
		int length;
	};
}

BOOST_FUSION_ADAPT_STRUCT(
	exercise5::edge,
	(int, startNode)
	(int, endNode)
	(int, length)
)

namespace exercise5
{
	template <typename Iterator>
	struct line_parser : qi::grammar<Iterator, edge()>
	{
		line_parser() : line_parser::base_type(start)
		{
			using qi::int_;
			start %= int_ >> ' ' >> int_ >> ' ' >> int_;
		}

		qi::rule<Iterator, edge()> start;
	};
}

/**
* The function that calculates the longest shortest path from node 0.
* @param numberOfNodes Number of nodes in the graph.
* @param graph The graph represented as a vector auf edge-vectors.
*/
std::pair<int, int> dijkstra( int numberOfNodes, std::vector< std::vector< std::pair<int, int> > > graph)
{
	std::vector<int> distanceTo(numberOfNodes, INT_MAX);
	std::priority_queue< std::pair<int, int>, std::vector< std::pair<int, int> >, std::greater< std::pair<int, int> > > queue;
	queue.push(std::pair<int, int>(0, 0));
	distanceTo[0] = 0;

	int currentNode, compareNode, compareNodeDistance, currentNodeDistance;

	while (!queue.empty()) {
		currentNode = queue.top().first;
		currentNodeDistance = queue.top().second;
		queue.pop();

		if (distanceTo[currentNode] < currentNodeDistance) continue;

		for (int i = 0; i < graph[currentNode].size(); i++) {
			compareNode = graph[currentNode][i].first;
			compareNodeDistance = graph[currentNode][i].second;
			if (distanceTo[compareNode] > distanceTo[currentNode] + compareNodeDistance) {
				distanceTo[compareNode] = distanceTo[currentNode] + compareNodeDistance;
				queue.push(std::pair<int, int>(compareNode, distanceTo[compareNode]));
			}
		}
	}

	std::pair<int, int> solution(-1, -1);
	for (int i = 0; i < numberOfNodes; i++)
	{
		if (distanceTo[i] > solution.second)
		{
			solution.second = distanceTo[i];
			solution.first = i;
		}
		if ((distanceTo[i] == solution.second) && (i > solution.first))
		{
			solution.first = i;
		}
	}

	return solution;
}

/**
* The main function that reads in a file and processes it.
* @param argc Number of command line arguments.
* @param *argv a pointer to the array of command line arguments.
*/
int main(int argc, char *argv[])
{
	timer::cpu_timer boostTimer;

	using boost::spirit::ascii::space;
	typedef std::string::const_iterator iterator_type;
	typedef exercise5::line_parser<iterator_type> line_parser;
	line_parser parser;
	std::string currentLine;
	std::ifstream file(argv[1]);

	// get number of nodes
	char delimiter = ' ';
	getline(file, currentLine, delimiter);
	const int numberOfNodes = std::stoi(currentLine);
	getline(file, currentLine);
	int constSub = 1;

	std::vector<std::vector<std::pair<int, int>>> edges(numberOfNodes);
	while (getline(file, currentLine))
	{
		exercise5::edge parsedLine;
		std::string::const_iterator currentPosition = currentLine.begin();
		std::string::const_iterator lineEnd = currentLine.end();
		bool parsingSucceeded = phrase_parse(currentPosition, lineEnd, parser, space, parsedLine);

		if (parsingSucceeded && currentPosition == lineEnd)
		{
			std::pair<int, int> *tempEdge = new std::pair<int, int>();
			tempEdge->first = parsedLine.endNode - constSub;
			tempEdge->second = parsedLine.length;
			edges[parsedLine.startNode - constSub].push_back(*tempEdge);

			std::pair<int, int> *reverseEdge = new std::pair<int, int>();
			reverseEdge->first = parsedLine.startNode - constSub;
			reverseEdge->second = parsedLine.length;
			edges[parsedLine.endNode - constSub].push_back(*reverseEdge);
		}
	}

	file.close();

	std::pair<int, int> solution = dijkstra(numberOfNodes, edges);
	int vertex = solution.first, distance = solution.second;

	std::cout << "RESULT VERTEX " << vertex << std::endl;
	std::cout << "RESULT DIST " << distance << std::endl;

	timer::cpu_times time = boostTimer.elapsed();
	std::cout << "CPU TIME: " << (time.user + time.system) / 1e9 << "s\n";
	std::cout << "WALL CLOCK TIME: " << time.wall / 1e9 << "s\n";

	return 0;
}


#ifndef _SIAR_ARM_HPP_
#define _SIAR_ARM_HPP_

#include "math.h"
#include <functions/linear_interpolator.hpp>
#include <string>
#include <queue>
#include <vector>
#include <boost/concept_check.hpp>

#define L1 0.0475
#define L2 0.215
#define L3 0.155
#define L4 0.080
#include "siar_functions.hpp"

class SiarArm{

  public:

    // Linear interpolators data
  int n_motors;
  std::vector<functions::LinearInterpolator *> pos_mot_interpol_, mot_pos_interpol_;
  std::vector<double> length;

  SiarArm(const std::string &mot_arm_file, const std::string &pos_arm_file) {
    load_data(mot_arm_file, pos_arm_file);
  }

  bool load_data(const std::string &mot_arm_file, const std::string &pos_arm_file) {
    n_motors = 5;
    for (int i = 0; i < n_motors; i++) {
      std::ostringstream mot_arm, pos_arm;
      mot_arm << mot_arm_file << i;
      pos_arm << pos_arm_file << i;
      pos_mot_interpol_.push_back(new functions::LinearInterpolator(mot_arm.str(), pos_arm.str()));
    }
    length.push_back(0.035); // TODO: read it from file?
    length.push_back(0.186);
    length.push_back(0.140);
    length.push_back(0.0651);
    length.push_back(0.04297);

  }

  ~SiarArm() {
    for(int i = 0; i < pos_mot_interpol_; i++) {
      delete pos_mot_interpol_[i];
    }
    for(int i = 0; i < mot_pos_interpol_; i++) {
      delete mot_pos_interpol_[i];
    }
  }

  bool inverseKinematics(double x, double y, double z, std::vector <double> &result)
  {
    bool coordenadas_correctas = true;

    // a1 = atan2(Z,X)

// a3 = acos(Z^2+X^2+(Y-L1)^2-L2^2-L3^2)

// d = sqrt(L2^2 + L3^2 -2*L2*L3*cos(a3))

// a2 = acos((y-L1)/d)

// donde L1 = 35.06; L2 = 186; L3 =140.05

// a1,2,3 son los ángulos de la primera, segunda y tercera articulación.

    result[0] = atan2(z, x);
    result[2] = acos(z*z+ x*x + pow(y-length[0],2.0) - length[2]*length[2] - length[3]*length[3]);
    double d = sqrt(length[1]*length[1] + length[2]*length[2] - 2.0 * length[1] * length[2] * cos(result[2]);
    result[1] = acos((y - length[0])/d;
    //ROS_INFO("x: %f; y: %f;z: %f",x,y,z);
    //ROS_INFO("q1: %f, q2: %f, q3: %f, q4: %f, q5:%f",q1,q2,q3,q4,q5);
  }

  void rad2motor(const std::vector<double> &angles, std::vector<int> &commands) {
    commands.resize(n_motors);
    for (int i = 0; i < n_motors; i++) {
      functions::LinearInterpolator &interpol = *pos_mot_interpol_[i];
      commands[i] = interpol(angles[i]);
    }
  }

  void motor2rad(const std::vector<int> &commands, std::vector<double> &angles) {
    angles.resize(n_motors);
    for (int i = 0; i < n_motors; i++) {
      functions::LinearInterpolator &interpol = *mot_pos_interpol_[i];
      angles[i] = interpol(commands[i]);
    }
  }

  void forwardKinematics(const std::vector  <int> &joint_values, double &x, double &y, double &z)
  {

//       X = cos(a1) *(L2*sin(a2)+L3*sin(a2+a3))
// Y = L1 + (L2*cos(a2)+L3*cos(a2+a3))
// Z = -1 * sin(a1) *(L2*sin(a2)+L3*sin(a2+a3))
// Donde Y es positiva desde la base en dirección a la primera articulación, X es ortogonal  a Y en el plano proyectado por el dibujo y Z va desde el plano hacia dentro.

    std::vector<double> angles;
    motor2rad(joint_values, angles);
    double a1 = joint_values[0];
    double a2 = joint_values[1];
    double a3 = joint_values[2];
    double L1 = length[0];
    double L2 = length[1];
    double L3 = length[2];
    x = cos(a1) * (L2*sin(a2) + L3*sin(a2 + a3));
    y = L1 + L2*cos(a2) + L3*cos(a2 + a3);
    z = -sin(a1) * (L2*sin(a2) + L3*sin(a2 + a3));
  }

  std::vector<std::vector<int> > straightInterpol(double x, double y, double z, uint8_t n_points, const std::vector<int> &curr_pos)
  {
    doble a_x, a_y, a_z;
    forwardKinematics(curr_pos, a_x, a_y, a_z);
    doble i_x, i_y, i_z;
    i_x = (x - a_x)/(n_points+1);
    i_y = (y - a_y)/(n_points+1);
    i_z = (z - a_z)/(n_points+1);

    std::vector<std::vector<int> > ret;

    std::vector<double> angles;
    std::vector<int> commands;
    for( int i = 0; i < n_points+1; i++)
    {
      a_x += i_x;
      a_y += i_y;
      a_z += i_z;

      inverseKinematics(a_x, a_y, a_z, angles);
      rad2motor(angles, commands);
      ret.push_back(commands);
    }

  }

  bool checkJointLimits(const boost::array<int16_t, 5> joint_values)
  {
    bool ret_val = true;

    for (int i = 0; i < n_motors && ret_val; i++) {
      double max, min;
      functions::LinearInterpolator &curr_inter = *mot_pos_interpol_[i];
      min = curr_inter.upper_bound(0);
      max = curr_inter.lower_bound(2000); // Usually the commands are in the [0, 2000] range

      ret_val &= joint_values[i] > min && joint_values[i] < max;

    }

    return ret_val;
  }

  bool checkTemperatureAndStatus(const boost::array<uint8_t,5> &herculex_temperature, const boost::array<uint8_t,5> &herculex_status) {
    bool ret_val = true;
    for(int i = 0; i < 5; i++)
    {
      if (herculex_temperature[i]<0 && herculex_temperature[i]>50)
      {
        ROS_ERROR("TEMPERATURE OF THE %d LINK IS OUT OF RANGE: %d", i, herculex_temperature[i]);
        ret_val = false;
      }
//       if (herculex_status[i]!=1)    // TODO: Check this!!
//       {
//         ROS_ERROR("%d LINK STATUS: %d", i, herculex_status[i]);
//         ret_val = false;
//       }
    }
    return ret_val;
  }

};
#endif /* _SIAR_ARM_H_ */


#pragma once

// deal.II includes ------------------------------------------------------------
#include <deal.II/base/numbers.h>
#include <deal.II/base/tensor.h>
//#include <deal.II/base/quadrature_lib.h>
// system includes -------------------------------------------------------------
#include <map>
// own includes ----------------------------------------------------------------
#include <quadrature/trig_int.hpp>

namespace boltzmann {

namespace local_ {
// ----------------------------------------------------------------------
inline bool
is_zero(const dealii::Tensor<2, 2, double>& t2)
{
  const double tol = 1e-16;
  return std::abs(t2[0][0]) < tol && std::abs(t2[0][1]) < tol && std::abs(t2[1][0]) < tol &&
         std::abs(t2[1][1]) < tol;
}

// ----------------------------------------------------------------------
inline bool
is_zero(const dealii::Tensor<1, 2, double>& t1)
{
  const double tol = 1e-16;
  return std::abs(t1[0]) < tol && std::abs(t1[1]) < tol;
}

}  // end namespace local_

template <int DIM>
class VelocityAngularIntegrator;

/**
 * @brief helper class for system matrix assembly
 *
 */
template <>
class VelocityAngularIntegrator<2>
{
 public:
  typedef double numeric_t;
  typedef dealii::Tensor<2, 2, numeric_t> T2_t;
  typedef dealii::Tensor<1, 2, numeric_t> T1_t;
  typedef std::pair<unsigned int, unsigned int> key_t;

 private:
  typedef std::map<key_t, numeric_t> map_S0_t;
  typedef std::map<key_t, T1_t> map_T1_t;
  typedef std::map<key_t, T2_t> map_T2_t;

 public:
  VelocityAngularIntegrator() { /* empty */}
  template <typename BASIS>
  void init(const BASIS& angular_basis);

  map_S0_t::const_iterator begin_s0() const;
  map_S0_t::const_iterator end_s0() const;
  map_T1_t::const_iterator begin_t1() const;
  map_T1_t::const_iterator end_t1() const;
  map_T2_t::const_iterator begin_t2() const;
  map_T2_t::const_iterator end_t2() const;

  const map_S0_t& get_s0() const { return ms0; }
  const map_T1_t& get_s1() const { return mt1; }
  const map_T1_t& get_t1() const { return mt1; }
  const map_T2_t& get_t2() const { return mt2; }

  //  void write_to_file(std::string fname) const;
 private:
  map_S0_t ms0;
  map_T1_t mt1;
  map_T2_t mt2;
};

// ---------------------------------------------------------------------------
template <typename BASIS>
void
VelocityAngularIntegrator<2>::init(const BASIS& angular_basis)
{
  auto make_t1 = [](int l1, int t1, int l2, int t2) {
    T1_t m1;
    m1[0] = trig_int(COS, 1, {(TRIG)t1, (TRIG)t2}, {l1, l2});
    m1[1] = trig_int(SIN, 1, {(TRIG)t1, (TRIG)t2}, {l1, l2});
    return m1;
  };

  auto make_t2 = [](int l1, int t1, int l2, int t2) {
    T2_t m2;
    for (int tp = 0; tp < 2; ++tp) {
      for (int t = 0; t < 2; ++t) {
        m2[tp][t] = trig_int((TRIG)tp, 1, {(TRIG)t, (TRIG)t1, (TRIG)t2}, {1, l1, l2});
      }
    }
    return m2;
  };

  for (auto it1 = angular_basis.begin(); it1 != angular_basis.end(); ++it1) {
    int l1 = it1->get_id().l;
    int t1 = it1->get_id().t;
    unsigned int ix1 = it1 - angular_basis.begin();
    for (auto it2 = angular_basis.begin(); it2 != angular_basis.end(); ++it2) {
      int l2 = it2->get_id().l;
      int t2 = it2->get_id().t;
      unsigned int ix2 = it2 - angular_basis.begin();
      // S0
      double s0 = trig_int((TRIG)t1, l1, {(TRIG)t2}, {l2});
      if (std::abs(s0) > 1e-16) ms0[std::make_pair(ix1, ix2)] = s0;
      // T1
      T1_t m1 = make_t1(l1, t1, l2, t2);
      if (!local_::is_zero(m1)) mt1[std::make_pair(ix1, ix2)] = m1;
      // T2
      T2_t m2 = make_t2(l1, t1, l2, t2);
      if (!local_::is_zero(m2)) mt2[std::make_pair(ix1, ix2)] = m2;
    }
  }
}

}  // end namespace boltzmann


/**
 * @file   main_advection_dir.cpp
 * @author Simon Pintarelli <simon@thinkpadX1>
 * @date   Wed Mar 30 18:30:47 2016
 *
 * @brief  solve advection equation separately for each direction (quad points)
 *
 *
 */

// system includes -----------------------------------------------
#include <boost/math/constants/constants.hpp>
#include <boost/program_options.hpp>
#include <iostream>
// own includes --------------------------------------------------
#include <base/eigen2hdf.hpp>
#include <base/init.hpp>
#include <base/timer.hpp>
#include <fft/fft2.hpp>
#include <fft/fft2_r2c.hpp>
#include <ridgelet/init_fftw.hpp>
#include <ridgelet/ridgelet_cell_array.hpp>
#include <ridgelet/ridgelet_frame.hpp>
#include <ridgelet/rt.hpp>

#include <omp.h>
#include <operators/operators.hpp>
#include <solver/cg.hpp>
#include <solver/ridgelet_solver.hpp>
#include <spectral/quadrature/gauss_hermite_roots.hpp>

using namespace std;
typedef FFTr2c<PlannerR2C> fft_t;
// TODO: check if RT coeffs are real valued in this case
typedef RT<double, RidgeletFrame, fft_t> RT_t;
typedef RT_t::array_t array_t;
typedef RT_t::complex_array_t complex_array_t;
typedef RT_t::rt_coeff_t rt_coeff_t;

int main(int argc, char* argv[])
{
  SOURCE_INFO();

  namespace po = boost::program_options;

  unsigned int Jx, Jy, rho_x, rho_y;
  double dt;
  unsigned int f;
  int K;

  unsigned int cg_maxit;
  double cg_reltol;

  po::options_description options("options");
  options.add_options()("help", "produce help message")
      ("Jx,i", po::value<unsigned int>(&Jx)->default_value(3), "Jx")
      ("Jy,j", po::value<unsigned int>(&Jy)->default_value(3), "Jy")
      ("rx,x", po::value<unsigned int>(&rho_x)->default_value(1), "rho_x")
      ("ry,y", po::value<unsigned int>(&rho_y)->default_value(1), "rho_x")
      ("dt,t", po::value<double>(&dt)->default_value(0.1), "dt")
      ("deg,K", po::value<int>(&K)->default_value(10), "poly. deg.")
      ("f", po::value<unsigned int>(&f)->default_value(2), "grid out factor")
      ("maxiter", po::value<unsigned int>(&cg_maxit)->default_value(40), "cg::maxiter")
      ("reltol", po::value<double>(&cg_reltol)->default_value(1e-4), "cg::reltol");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << options << "\n";
    return 0;
  }

  cout << "CMD::";
  for (int i = 0; i < argc; ++i) {
    cout << argv[i] << " ";
  }
  cout << "\n";

  cout << setw(20) << "Jx"
       << ": " << Jx << "\n"
       << setw(20) << "Jy"
       << ": " << Jy << "\n"
       << setw(20) << "rho_x"
       << ": " << rho_x << "\n"
       << setw(20) << "rho_y"
       << ": " << rho_y << "\n"
       << setw(20) << "K"
       << ": " << K << "\n"
       << setw(20) << "dt"
       << ": " << dt << "\n";

  RidgeletFrame rf(Jx, Jy, rho_x, rho_y);
  const unsigned int Nx = rf.Nx();  // #cols
  const unsigned int Ny = rf.Ny();  // #rows
  cout << "Nx: " << Nx << "\n";
  cout << "Ny: " << Ny << "\n";

  fft_t fft;
  init_fftw(fft, FFTW_MEASURE, rf);
  fft.get_plan().create_and_get_plan(f * Ny, f * Nx, PlannerR2C::INV);
  fft.get_plan().create_and_get_plan(f * Ny, f * Nx, PlannerR2C::FWD);

  std::vector<double> vi(K);
  boltzmann::gauss_hermite_roots(vi, K);

  Eigen::ArrayXd xi = Eigen::ArrayXd::LinSpaced(Nx + 1, 0, 1).segment(0, Nx);
  Eigen::ArrayXd yi = Eigen::ArrayXd::LinSpaced(Ny + 1, 0, 1).segment(0, Ny);

  array_t F =
      (xi.transpose().replicate(Ny, 1)).binaryExpr(yi.replicate(1, Nx), [](double x, double y) {
        return std::exp(-300 * (std::pow(x - 0.5, 2) + std::pow(y - 0.5, 2)));
      });

  RT_t rt(rf);

  complex_array_t Fh(Ny, Nx);
  fft.ft(Fh, F, false);
  typedef RidgeletCellArray<rt_coeff_t> rca_t;
  rca_t rt_cell_array(rf);
  auto& rt_coeffs = rt_cell_array.coeffs();
  rt.rt(rt_coeffs, Fh);

  hid_t file = H5Fcreate("advection_dir.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  cout << "write results to `advection_dir.h5`"
       << "\n";
  cout << "max thread: " << omp_get_max_threads() << "\n";
  cout << "RT_SOLVER::tol  : " << cg_reltol << "\n";
  cout << "RT_SOLVER::maxit: " << cg_maxit << "\n";

  eigen2hdf::save(file, "F", F);
  eigen2hdf::save(file, "Fh", Fh);
  cout << "dt:" << dt << "\n";
  const double Lx = 1;
  const double Ly = 1;

  Eigen::MatrixXi ITER_MAT(K, K);
  Eigen::MatrixXd RELRES_MAT(K, K);  // rel. residual
  Eigen::MatrixXd TIME_MAT(K, K);    // [GCycle]

#pragma omp parallel for
  for (int qx = 0; qx < K; ++qx) {
    for (int qy = 0; qy < K; ++qy) {
      const double vx = vi[qx];
      const double vy = vi[qy];
      AhAOp AhA(vx, vy, Lx, Ly, Nx, Ny, dt);
      // preconditioned operator
      RDTSCTimer timer;

      PTransportOp<RT_t> A(rt, AhA, vx, vy);
      TransportOperator T(vx, vy, Lx, Ly, Nx, Ny, dt);  // required for rhs
      // T'*Fh
      complex_array_t Bh(Ny, Nx);
      T.apply(Bh, Fh, true /* hermitian transpose */);
      rca_t b(rf);
      rt.rt(b.coeffs(), Bh);
      RidgeletSolver<rt_coeff_t> rt_solver(rf, vx, vy);
      rca_t x(rf);
      x.resize(rt_cell_array);
      x = rt_cell_array;
      timer.start();
      rt_solver.solve(x, A, b, cg_reltol, cg_maxit);
      auto nc_solve = timer.stop();

      RELRES_MAT(qx, qy) = rt_solver.relres();
      ITER_MAT(qx, qy) = rt_solver.iter();
      TIME_MAT(qx, qy) = nc_solve / 1e9;

      // #pragma omp critical
      //       {
      //         cout << "vx: " << vx << "\n";
      //         cout << "vy: " << vy << "\n";
      //         cout << "RidgeletSolver took: " << nc_solve / 1e9 << " Gcycles\n";
      //         cout << "cg::relres: " << rt_solver.relres() << "\n";
      //         cout << "cg::iter: " << rt_solver.iter() << "\n\n";
      //       }
      // write arrays to hdf5
      complex_array_t tmp(Ny, Nx);
      rt.irt(tmp, x.coeffs());
      array_t sol(f * Ny, f * Nx);
      complex_array_t solh(f * Ny, f * Nx);
      solh.setZero();
      ftcut(solh, Ny / 2, Nx / 2) = ftcut(tmp, Ny / 2, Nx / 2);
      // ftcut(solh, Ny, Nx) = tmp;
      fft.ift(sol, solh);
      char buf[256];
      std::sprintf(buf, "%d_%d", qx, qy);
#pragma omp critical
      {
        hid_t group = H5Gcreate(file, buf, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
        eigen2hdf::save(group, "sol", sol);
        eigen2hdf::save(group, "solh_ftcut", solh);
        eigen2hdf::save(group, "solh", tmp);
        H5Gclose(group);
      }
    }
  }

  eigen2hdf::save(file, "cg_relres", RELRES_MAT);
  eigen2hdf::save(file, "cg_iter", ITER_MAT);
  eigen2hdf::save(file, "cg_time", TIME_MAT);

  H5Fclose(file);

  return 0;
}