/*
	kstat
	Ver. k09.02

	Written by Koji Yamamoto
	Copyright (C) 2015-2020 Koji Yamamoto
	In using this, please read the document which states terms of use.

	Module for Statistical Computations

	k10になるときに以下の関数を消す。
	double unbiasedVar()　→kstatboostのものを推奨。
	double boostMean( const std::vector <double> &dv0)

	Note:
	As for FreqType, possibly in the future we should refer the following libraries:
	　GSL's Histogram

*/

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

#ifndef kstat_cpp_include_guard
#define kstat_cpp_include_guard

#include <memory>
#include <map>
#include <vector>
#include <limits>
#include <functional>
#include <cmath>

#include <k09/kutil02.cpp>
#include <k09/kalgo02.cpp>

/* ********** Using Directives ********** */

//using namespace std;

/* ********** Type Declarations: enum, class, etc. ********** */

// TCount is int by default
template <typename T, typename TCount = int>
class FreqType;

// TCode is int by default
template <typename T, typename TCode = int>
class RecodeTable;

/*
// This class is declared in RecodeType
template <typename T, typename TCode>
struct
RecodeTable ::
CodeType;
*/

template <typename T>
class JudgeEqual;

template <typename T>
class JudgeEqualTol;

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

template <typename T>
int countUniqueValues( const std::vector <T> &);

template <typename TOrigin, typename TCode, typename TCount>
void
createFreqFromRecodeTable(
	FreqType <TCode, TCount> &,
	const std::vector <TOrigin> &,
	const RecodeTable <TOrigin, TCode> &
);

template <typename T>
std::vector <T> omitNan( const std::vector <T> &);

double sum( const std::vector <double> &);
double mean( const std::vector <double> &);
double median( const std::vector <double> &);
double unbiasedVar( const std::vector <double> &); // 非推奨。deprecated.
double boostMean( const std::vector <double> &); // 非推奨。deprecated.
double sum( const double *, int);
double mean( const double *, int);
double unbiasedVar( const double *, int);

// このファイル内でよいのか？
template <typename T>
std::string toString( const T &);

/* ********** Type Definitions: enum, class, etc. ********** */

// frequency type
// T is type for key, and TCount is type for count
// This type is essentially for discrete variables,
// with Tolerance for equality judgment being very small.
// When applied to continuous variables,
// special methods should be used.
// 基本的に離散変数用。
// doubleなどに使う場合、等値判断のための
// toleranceは0か非常に小さい値である、
// という前提がある。そうでないとカテゴリ間の
// 重なりが生じてしまう。
// 連続変数に用いる場合には、特定の機能を使うべし。
// TCount is int by default
template <typename T, typename TCount /* = int */>
class FreqType {

private:

	map <T, TCount> freqmap;
	std::function < bool( const T &, const T &) > areEqual;
	TCount sumcount;

	// Note that we specify double as TOrigin for RecodeTable
	// RecodeTableのTOriginとして、doubleを指定してしまっている。
	// ここに柔軟性を持たせたいならば、rtablepがないクラスをつくり、
	// それを基底クラスにして、派生クラスでテンプレート引数を増やしてrtablepを持たせるのが、素直だろう。
	std::unique_ptr < RecodeTable<double, T> > rtablep;

public:

	FreqType( void);
	~FreqType( void);

	void clear( void);
	void setEqualExactly( void);
	void setEqualWithTol( const T &);
	bool addPossibleKey( const T &);
	bool addPossibleKeys( const std::vector <T> &);

	void increment( const T &);
	void addCount( const T &, const TCount &);
	void addFreqType( const FreqType <T, TCount> &);

	void clearCount( void);

	TCount getSumCount( void) const;
	void getVectors( std::vector <T> &, std::vector <TCount> &) const;
	double meanFromFreq( void) const;
	double medianFromFreq( void) const;
	void modeFromFreq( std::vector <T> &) const;

	void setFreqFromRecodeTable( const std::vector <double> &, const RecodeTable <double, T> &);

	void printPadding( std::ostream &) const;

	// Note that obtained vectors should be
	// parallel to those obtained by getVectors() above
	void getRangeVectors( std::vector <double> &, std::vector <double> &) const;

};

// TCode is int by default
template <typename TOrigin, typename TCode /* = int */>
class RecodeTable {

private:

	struct CodeType; // inner class; defined below

	// 1要素はRecodeTableの1行分
	std::vector <CodeType> codes;

	// "else"の場合の処理方法　（スコープを持つ列挙型）
	enum class ElseType { Copy, AssignValue};

	ElseType toDoForElse; // "else"の場合の処理方法
	TCode codeForElse; // "else"の場合に値を埋める場合の値（NaN以外）

public:

	RecodeTable( void){}
	~RecodeTable( void){}

	void setAutoTableFromContVar( const std::vector <TOrigin> &);
	std::vector <TCode> getPossibleCodeVec( void) const;
	TCode getCodeForValue( TOrigin) const;

	void getLeftRightForCode( TOrigin &, TOrigin &, TCode) const;

	std::string getRangeLabelForCode( TCode) const;
	void print( ostream &, string = ","s) const;

};

template <typename TOrigin, typename TCode>
struct
RecodeTable <TOrigin, TCode> ::
CodeType {

// Fields:

	TOrigin left; // the left-handside endpoint of the class
	bool leftIn; // whether the endpoint value is inclusive

	TOrigin right; // the right-handside endpoint of the class
	bool rightIn; // whether the endpoint value is inclusive

	TCode codeAssigned; // the code assigned to this class

// Methods:

	CodeType( TOrigin l0, bool li0, TOrigin r0, bool ri0, TCode ca0)
	: left( l0), leftIn( li0), right( r0), rightIn( ri0), codeAssigned( ca0)
	{}

	~CodeType( void){};

	bool correspond( TOrigin v) const
	{
		if ( left < v && v < right){
			return true;
		}
		if ( leftIn == true && left == v){
			return true;
		}
		if ( rightIn == true && right == v){
			return true;
		}
		return false;
	}

	string getRangeLabel( void) const
	{

		stringstream ss;
		ss << left << " ";
		if ( leftIn == true){
			ss << "<=";
		} else {
			ss << "<";
		}
		ss << " x ";
		if ( rightIn == true){
			ss << "<=";
		} else {
			ss << "<";
		}
		ss << " " << right;

		return ss.str();

	}

};

// 等値判断のためのファンクタクラス：==演算子で判断する。
template <typename T>
class JudgeEqual {

public:

	JudgeEqual( void){}

	~JudgeEqual( void){}

	bool operator()( const T &a, const T &b)
	{

		return ( a == b);

	}

};

// 等値判断のためのファンクタクラス：Toleranceを含んで判断する。
// class to judge equality with tolerance
// i.e. regard a and b equal when a < b + tol and a > b - tol
// thus if T is double and a = 1.0 and tol = 0.1,
// then b can be ( 0.9, 1.1)
template <typename T>
class JudgeEqualTol {

private:

	T tol; // this is assumed to be non-negative and very small

public:

	JudgeEqualTol( void){}

	JudgeEqualTol( const T &tol0) : tol( tol0){}

	~JudgeEqualTol( void){}

	void setTol( const T &tol0)
	{
		tol = tol0;
	}

	bool operator()( const T &a, const T &b)
	{
		return ( ( a < b + tol) && ( a > b - tol));
	}

};

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

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

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

// ==演算子を用いて、ユニークな値が何個あるかを返す。
// var0に欠損値は含まないと仮定して、ケース数を算出している。
// We assume var0 does not contain invalid values.
template <typename T>
int countUniqueValues( const std::vector <T> &vec0)
{

	if ( vec0.size() < 1){
		return 0;
	}

	std::vector <T> vec( vec0);

	std::sort( vec.begin(), vec.end());

	T prev = vec[ 0];
	int ret = 1;
	for ( auto v : vec){
		if ( v != prev){
			ret++;
			prev = v;
		}
	}

	return ret;

}

// This function is more flexible than FreqType::setFreqFromRecodeType()
// in the sense that this can allow <TOrigin> different from <double>.
// But this function does not make FreqType store rtable itself.
template <typename TOrigin, typename TCode, typename TCount>
void
createFreqFromRecodeTable(
	FreqType <TCode, TCount> &ret,
	const std::vector <TOrigin> &var,
	const RecodeTable <TOrigin, TCode> &rtable
)
{

	ret.clear();
	std::vector <TCode> codeVec = rtable.getPossibleCodeVec();
	ret.addPossibleKeys( codeVec);
	for ( auto v : var){
		if ( std::isnan( v)){
			// do nothing
		} else {
			TCode codeToAdd = rtable.getCodeForValue( v);
			ret.increment( codeToAdd);
		}
	}

}

template <typename T>
std::vector <T> omitNan( const std::vector <T> &vec0)
{

	std::vector <T> ret;
	ret.reserve( vec0.size());

	for ( const auto &v : vec0){
		if ( std::isnan( v)){
			// do nothing
		} else {
			ret.push_back( v);
		}
	}

	return ret;

}

// returns the sum of all the elements
double sum( const std::vector <double> &dv0)
{

	double s;
	s = 0.0;
	for ( auto v : dv0){
		s += v;
	}
	return s;

}

// returns the mean value
inline double mean( const std::vector <double> &dv0)
{

	double s, ret;
	s = sum( dv0);
	ret = s / ( double)( dv0.size());
	return ret;

}

// returns the median value
double median( const std::vector <double> &dv0)
{
	int n;
	std::vector <double> sorted;
	double ret;

	n = dv0.size();
	sorted = dv0;
	sort( sorted.begin(), sorted.end());

	if ( n % 2 == 0){
		// n is an even number
		ret = ( sorted[ n / 2 - 1] + sorted[ n / 2]) / 2.0;
	} else {
		// n is an odd number
		ret = sorted[ n / 2];
	}

	return ret;

}

// DEPRECATED 非推奨
// このアルゴリズムでは精度が落ちるときがあるらしい。
// kstatboostのものを推奨する。
// returns "unbiased" variance
inline double unbiasedVar( const std::vector <double> &v0)
{

	int n = v0.size();

	double m = mean( v0);

	double s2 = 0.0;
	for ( auto &d : v0){
		s2 += d * d;
	}

	return ( s2 / ( double)n - m * m) * ( double)n / ( double)( n - 1);

}

// DEPRECATED 非推奨
// boost version of mean calculation
//
// I would not prefer this series,
// because the algorithm for median value is just estimating it;
// not returining accurate values
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp> // for stats<> template, tag::mean
double boostMean( const std::vector <double> &dv0)
{

	using namespace boost::accumulators;

	accumulator_set< double, stats< tag::mean > > acc;

	acc = std::for_each( dv0.begin(), dv0.end(), acc);

	return extract::mean( acc);

}

// 長さnの配列へのポインタpを得て、合計を返す。
double sum( const double *p, int n)
{

	double s;
	s = 0.0;
	for ( int i = 0; i < n; i++){
		s += p[ i];
	}
	return s;

}

// 長さnの配列へのポインタpを得て、平均を返す。
inline double mean( const double *p, int n)
{

	return ( sum( p, n) / ( double)n);

}

// 長さnの配列へのポインタpを得て、不偏分散を返す。
inline double unbiasedVar( const double *p, int n)
{

	double m, s2;

	m = mean( p, n);

	s2 = 0.0;
	for ( int i = 0; i < n; i++){
		s2 += p[ i] * p[ i];
	}

	return ( s2 / ( double)n - m * m) * ( double)n / ( double)( n - 1);

}

// convert any type T into string
// using stringstream
template <typename T>
std::string toString( const T &origin)
{
	std::stringstream ss;
	ss.str( "");
	ss << origin;
	return ss.str();
}

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

/* ---------- class FreqType ---------- */

template <typename T, typename TCount>
FreqType <T, TCount> ::
FreqType( void)
: freqmap(),
  areEqual( JudgeEqual<T>()),
  sumcount( static_cast<TCount>( 0))
{
}

template <typename T, typename TCount>
FreqType <T, TCount> ::
~FreqType( void)
{
}

template <typename T, typename TCount>
void
FreqType <T, TCount> ::
clear( void)
{

	freqmap.clear();
	sumcount = static_cast<TCount>( 0);

}

template <typename T, typename TCount>
void
FreqType <T, TCount> ::
setEqualExactly( void)
{

	areEqual = JudgeEqual<T>();

}

template <typename T, typename TCount>
void
FreqType <T, TCount> ::
setEqualWithTol( const T &tol)
{

	areEqual = JudgeEqualTol<T>( tol);

}

// add v as a key with no freq when such a key does not exist.
// if the "same" key exists, this does nothing.
// returns true if such a key does not exist.
// returns false if such a key does exist.
// Note that the judgment about equivalence is made
// simply using "map::find()" function; not using areEqual()
template <typename T, typename TCount>
bool
FreqType <T, TCount> ::
addPossibleKey( const T &v)
{

	auto it = freqmap.find( v); // ここでfindを使う。areEqualを使っていない。
	if ( it == freqmap.end()){
		freqmap.insert( { v, 0});
		return true;
	}

	return false;

}

// adds values in vec as a key with no freq when such a key does not exist.
// if the same key exists, this does nothing.
// returns true if such a key does not exist.
// returns false if such a key does exist.
// We can use this function like below.
//   addPossibleKeys( { 1.0, 2.0, 3.0, 4.0, 5.0});
//   addPossibleKeys( vector <double> ( { 1.0, 2.0, 3.0, 4.0, 5.0}));
template <typename T, typename TCount>
bool
FreqType <T, TCount> ::
addPossibleKeys( const std::vector <T> &vec)
{

	bool ret = true;

	for ( const T &v : vec){
		bool b = addPossibleKey( v);
		if ( b == false){
			ret = false;
		}

	}

	return ret;

}

// increment the count for the key "v"
template <typename T, typename TCount>
void
FreqType <T, TCount> ::
increment( const T &v)
{

	bool exist = false;
	for ( auto &pair : freqmap){

		if ( areEqual( pair.first, v) == true){
			( pair.second)++;
			exist = true;
			break;
		}

	}

	if ( exist == false){
		freqmap.insert( { v, 1});
	}

	sumcount++;

}

template <typename T, typename TCount>
void
FreqType <T, TCount> ::
addCount( const T &key, const TCount &count)
{

	bool exist = false;
	for ( auto &pair : freqmap){

		if ( areEqual( pair.first, key) == true){
			pair.second += count;
			exist = true;
			break;
		}

	}

	if ( exist == false){
		freqmap.insert( { key, count});
	}

	sumcount += count;

}

template <typename T, typename TCount>
void
FreqType <T, TCount> ::
addFreqType( const FreqType <T, TCount> &freqobj)
{

	for ( const auto &pair : freqobj.freqmap){
		addCount( pair.first, pair.second);
	}

}

// Clears counts into zeros.
// Possible keys and setting for equality judgement are retained.
template <typename T, typename TCount>
void
FreqType <T, TCount> ::
clearCount( void)
{

	for ( auto &pair : freqmap){
		( pair.second) = static_cast<TCount>( 0);
	}

	sumcount = static_cast<TCount>( 0);

}

template <typename T, typename TCount>
TCount
FreqType <T, TCount> ::
getSumCount( void)
const
{

	return sumcount;

}

// returns vectors of keys and frequency counts
template <typename T, typename TCount>
void
FreqType <T, TCount> ::
getVectors( std::vector <T> &keyvec, std::vector <TCount> &frevec)
const
{

	keyvec.resize( freqmap.size());
	frevec.resize( freqmap.size());

	int i = 0;
	for ( const auto &pair : freqmap){
		keyvec[ i] = pair.first;
		frevec[ i] = pair.second;
		i++;
	}

}

template <typename T, typename TCount>
double
FreqType <T, TCount> ::
meanFromFreq( void)
const
{

	// if sumcount == 0, then return NaN
	if ( sumcount == static_cast<TCount>( 0)){
		alert( "FreqType :: meanFromFreq()");
		return numeric_limits<double>::signaling_NaN();
	}

	double sumval = 0.0;

	for ( const auto &pair : freqmap){
		sumval += static_cast<double>( pair.first)
		          * static_cast<double>( pair.second);
	}

	return ( sumval / static_cast<double>( sumcount));

}

template <typename T, typename TCount>
double
FreqType <T, TCount> ::
medianFromFreq( void)
const
{

	// we are using class "map" because it stores data sorted by key

	// if sumcount == 0, then return NaN
	if ( sumcount == static_cast<TCount>( 0)){
		alert( "FreqType :: medianFromFreq()");
		return numeric_limits<double>::signaling_NaN();
	}

	TCount half;
	half = static_cast<TCount>( floor( static_cast<double>( sumcount) / 2.0));
	bool even = false;
	if ( half * 2 == sumcount){
		even = true;
	}

	TCount cumcount = 0;
	double ret1;
	bool waitfornext = false;
	int i = 0;

	// in class "map" data are sorted by the key
	for ( const auto &pair : freqmap){

		cumcount += pair.second;

		if ( waitfornext == true){

			if ( pair.second > static_cast<TCount>( 0)){

				// 頻度総計が偶数で、「累積頻度50%」が2値の間になっており、
				// 大きい方を待っていた状態のとき

				return ( ( ret1 + ( double)pair.first) / 2.0);

			}

		} else {

			if ( even == true){

				if ( cumcount > half){
					return static_cast<double>( pair.first);
				} else if ( cumcount == half){
					ret1 = static_cast<double>( pair.first);
					waitfornext = true;
				}

			} else {

				if ( cumcount > half){
					return static_cast<double>( pair.first);
				}

			}

		}

	}

	alert( "FreqType :: medianFromFreq()");
	return numeric_limits<double>::signaling_NaN();

}

template <typename T, typename TCount>
void
FreqType <T, TCount> ::
modeFromFreq( std::vector <T> &ret)
const
{

	// if sumcount == 0, then return a vector of size 0
	if ( sumcount == static_cast<TCount>( 0)){
		alert( "FreqType :: modeFromFreq()");
		ret.clear();
		return;
	}

	std::vector <T> keyvec;
	std::vector <TCount> frevec;

	getVectors( keyvec, frevec);

	std::vector <int> idmax;
	getIndexOfMax( idmax, frevec);

	ret.clear();
	for ( const auto &id : idmax){
		ret.push_back( keyvec[ id]);
	}

}

// Note that rt0 should be of RecodeTable <double, T>
template <typename T, typename TCount>
void
FreqType <T, TCount> ::
setFreqFromRecodeTable( const std::vector <double> &vec0, const RecodeTable <double, T> &rt0)
{

	clear();
	rtablep.reset( new RecodeTable<double, T>( rt0));
	std::vector <T> codeVec = rtablep->getPossibleCodeVec();
	addPossibleKeys( codeVec);
	for ( auto v : vec0){
		if ( std::isnan( v)){
			// do nothing
		} else {
			T codeToAdd = rtablep->getCodeForValue( v);
			increment( codeToAdd);
		}
	}

}

// 本来は、Datasetに入れて表示したいが、また今度。
template <typename T, typename TCount>
void
FreqType <T, TCount> ::
printPadding( std::ostream &os)
const
{

	using namespace std;

	vector < vector <string> > strcols;
	vector <int> width;
	int ncol;

	if ( freqmap.size() < 1){
		alert( "FreqType :: printPadding()");
	}

	// preparation

	ncol = 2;
	if ( bool( rtablep) == true){
		// we will add range label column
		ncol = 3;
	}
	strcols.resize( ncol);

	for ( auto &col : strcols){
		col.reserve( freqmap.size() + 1);
	}

	{
		int j = 0;
		strcols[ j].push_back( "Code");
		j++;
		if ( bool( rtablep) == true){
			strcols[ j].push_back( "Label");
			j++;
		}
		strcols[ j].push_back( "Freq");
	}

	for ( auto pair : freqmap){

		const T &key = pair.first;
		const TCount &fre = pair.second;

		int j_col = 0;

		// Code
		strcols[ j_col].push_back( toString( key));
		j_col++;

		// Range Label (optional)
		if ( bool( rtablep) == true){
			string labelstr = rtablep->getRangeLabelForCode( pair.first);
			strcols[ j_col].push_back( labelstr);
			j_col++;
		}

		// Freq
		strcols[ j_col].push_back( toString( fre));

	}

	width.resize( strcols.size());
	for ( int i = 0; i < strcols.size(); i++){
		int w0 = 0;
		for ( const auto &s : strcols[ i]){
			if ( w0 < s.size()){
				w0 = s.size();
			}
		}
		width[ i] = w0;
	}

	// display

	os << setfill( ' ');
	for ( int i_row = 0; i_row < strcols[ 0].size(); i_row++){
		for ( int j_col = 0; j_col < strcols.size(); j_col++){
			os << setw( width[ j_col]);
			os << strcols[ j_col][ i_row];
			os << " ";
		}
		os << endl;
	}

}

// 各階級の左端と右端のVectorを得る。
// RecodeTypeがない場合には空のVectorが返る。
// Note that obtained vectors should be
// parallel to those obtained by getVectors() above
// これも、上記のprintPadding()と同様に、
// 本来はDatasetに入れて処理したいが、また今度。
template <typename T, typename TCount>
void
FreqType <T, TCount> ::
getRangeVectors( std::vector <double> &lvec, std::vector <double> &rvec)
const
{

	lvec.clear();
	rvec.clear();

	if ( bool( rtablep) == false){
		return;
	}

	lvec.reserve( freqmap.size());
	rvec.reserve( freqmap.size());

	for ( const auto &pair : freqmap){

		const T &key = pair.first;

		double left, right;
		rtablep->getLeftRightForCode( left, right, key);
		lvec.push_back( left);
		rvec.push_back( right);

	}

}

/* ---------- class RecodeTable ---------- */

// 自動で階級を作成する。
// Nから階級の数を設定する。Stataの方式で。
// 最小値と最大値の幅を出して、そこから階級幅を出す。
// 各階級は、左端点を含み、右端点を含まない。
// 有効ケース数が1未満のとき、エラー。
// 有効ケース数が1のとき、階級数は1。
// 最大値と最小値の差がゼロの場合、次のように最大・最小を修正。
// 　値がゼロなら、最小値を-1に、最大値を1にする。
// 　値が正なら、最小値を0に、最大値をそのままにする。
// 　値が負なら、最大値を0に、最小値をそのままにする。
template <typename TOrigin, typename TCode>
void
RecodeTable <TOrigin, TCode> ::
setAutoTableFromContVar( const std::vector <TOrigin> &var0)
{

	codes.clear();

	std::vector <TOrigin> vec = omitNan( var0);
	int nobs = vec.size();
	if ( nobs < 1){
		alert( "RecodeTable :: setAutoTableFromContVar()");
		return;
	}

	std::sort( vec.begin(), vec.end());
	TOrigin truemin = vec.front();
	TOrigin truemax = vec.back();

	// This way to determine the number of classes is
	// from Stata's histogram command.
	long double log10n = log10( nobs);
	int nclasses = std::round( std::min( ( long double)( std::sqrt( nobs)), 10.0 * log10n));
	if ( nclasses < 1){ // This will occur when nobs==1
		nclasses = 1;
	}

	// Manipulation for the case where width is zero
	if ( ( long double)truemax - ( long double)truemin <= 0.0){
		nclasses = 1;
		if ( ( long double)truemin == 0.0){
			truemin = static_cast<TOrigin>( -1.0);
			truemax = static_cast<TOrigin>(  1.0);
		} else {
			if ( truemin > 0){
				truemin = static_cast<TOrigin>(  0.0);
			} else {
				truemax = static_cast<TOrigin>(  0.0);
			}
		}
	}

	// Adjustment to make sure all cases would fall inside one of ranges,
	// even if rounding errors occur
	// adjmin and adjmax are rounded into values with 3-digits accuracy
	TOrigin truewidth = static_cast<TOrigin>(
		( ( long double)truemax - (long double)truemin) / ( long double)nclasses
	);
	int log10truewidth = std::floor( log10( truewidth));
	long double base = std::pow( 10.0, log10truewidth - 3);
	long double adjmin = std::floor( ( long double)truemin / base - 0.5) * base;
	long double adjmax = std::ceil( ( long double)truemax / base + 0.5) * base;
	long double adjwidth = ( adjmax - adjmin) / ( long double)nclasses;

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

		TOrigin left  = adjmin + adjwidth * ( long double)i;
		TOrigin right = adjmin + adjwidth * ( long double)( i + 1);

		CodeType ct =
			CodeType (
				left, true,
				right, false,
				i + 1
			);

		codes.push_back( ct);

	}

	toDoForElse = ElseType :: AssignValue;
	codeForElse = std::numeric_limits<TCode>::quiet_NaN();

}

// codeになりうる値のvectorを返す。
// ソートして返す。
// ただし、Elseの場合のcodeは含めない。
template <typename TOrigin, typename TCode>
std::vector <TCode>
RecodeTable <TOrigin, TCode> ::
getPossibleCodeVec( void) const
{

	std::vector <TCode> ret;
	ret.clear();

	for ( auto c : codes){
		ret.push_back( c.codeAssigned);
	}
	std::sort( ret.begin(), ret.end());

	return ret;

}

// vに対応するコードを返す。
template <typename TOrigin, typename TCode>
TCode
RecodeTable <TOrigin, TCode> ::
getCodeForValue( TOrigin v)
const
{

	for ( auto c : codes){
		if ( c.correspond( v) == true){
			return c.codeAssigned;
		}
	}

	TCode ret;

	switch( toDoForElse){
		case ElseType :: Copy:
			ret = v;
			break;
		case ElseType :: AssignValue:
			ret = codeForElse;
			break;
	}

	return ret;

}

// code0に対応するleftとrightを返す。
// code0に対応する範囲が登録されていなければ、nanを返す。
template <typename TOrigin, typename TCode>
void
RecodeTable <TOrigin, TCode> ::
getLeftRightForCode( TOrigin &lret, TOrigin &rret, TCode code0)
const
{

	for ( const auto &c : codes){
		if ( c.codeAssigned == code0){
			lret = c.left;
			rret = c.right;
			return;
		}
	}

	lret = std::numeric_limits<TOrigin>::quiet_NaN();
	rret = std::numeric_limits<TOrigin>::quiet_NaN();

}

// code0に対応するラベルを返す。
// code0に対応する範囲が登録されていなければ、空のstringを返す。
template <typename TOrigin, typename TCode>
std::string
RecodeTable <TOrigin, TCode> ::
getRangeLabelForCode( TCode code0)
const
{

	for ( const auto &c : codes){
		if ( c.codeAssigned == code0){
			return c.getRangeLabel();
		}
	}

	return string( "");

}

// ストリームに出力する。
// スペースでパディングはしない。sepで区切る。
template <typename TOrigin, typename TCode>
void
RecodeTable <TOrigin, TCode> ::
print(
	ostream &os,
	string sep // = ","s
)
const
{

	os << "Code" << sep << "Range" << std::endl;
	for ( auto c : codes){
		os << c.codeAssigned;
		os << sep;
		os << c.getRangeLabel();
		os << endl;
	}

}

#endif /* kstat_cpp_include_guard */


#include "llr.h"

#include "threads.h"
#include <Eigen/SVD>
#include <random>

Cx4 llr(Cx4 const &x, float const l, Index const p)
{
  Index const K = x.dimension(0);
  Log::Print(FMT_STRING("LLR regularization patch size {} lamdba {}"), p, l);
  Cx4 lr(x.dimensions());
  lr.setZero();

  auto zTask = [&](Index const lo, Index const hi) {
    for (Index iz = lo; iz < hi; iz++) {
      for (Index iy = 0; iy < x.dimension(2) - p; iy++) {
        for (Index ix = 0; ix < x.dimension(1) - p; ix++) {
          Cx4 px = x.slice(Sz4{0, ix, iy, iz}, Sz4{K, p, p, p});
          Eigen::Map<Eigen::MatrixXcf> patch(px.data(), K, p * p * p);
          auto const svd = patch.transpose().bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
          // Soft-threhold svals
          Eigen::ArrayXf s = svd.singularValues();
          float const sl = s.sum() * l;
          s = s * (s.abs() - sl) / s.abs();
          s = (s > sl).select(s, 0.f);
          patch.transpose() = svd.matrixU() * s.matrix().asDiagonal() * svd.matrixV().adjoint();
          lr.chip<3>(iz + p / 2).chip<2>(iy + p / 2).chip<1>(ix + p / 2) =
            px.chip<3>(p / 2).chip<2>(p / 2).chip<1>(p / 2);
        }
      }
    }
  };
  auto const now = Log::Now();
  Threads::RangeFor(zTask, 0, x.dimension(3) - p);
  Log::Print(FMT_STRING("LLR Regularization took {}"), Log::ToNow(now));
  return lr;
}

Cx4 llr_patch(Cx4 const &x, float const l, Index const p)
{
  std::array<Index, 3> nP, shift;
  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_int_distribution<> int_dist(0, p - 1);
  for (Index ii = 0; ii < 3; ii++) {
    if (x.dimension(ii + 1) % p != 0) {
      Log::Fail(
        FMT_STRING("Patch size {} does not evenly divide {} (dimension {})"),
        p,
        x.dimension(ii + 1),
        ii);
    }
    nP[ii] = (x.dimension(ii + 1) / p) - 1;
    shift[ii] = int_dist(gen);
  }
  Index const K = x.dimension(0);
  Index const pSz = p * p * p;
  Cx4 lr(x.dimensions());
  lr.setZero();
  auto zTask = [&](Index const lo, Index const hi) {
    for (Index iz = lo; iz < hi; iz++) {
      for (Index iy = 0; iy < nP[1]; iy++) {
        for (Index ix = 0; ix < nP[0]; ix++) {
          Cx4 px = x.slice(
            Sz4{0, ix * p + shift[0], iy * p + shift[1], iz * p + shift[2]}, Sz4{K, p, p, p});
          Eigen::Map<Eigen::MatrixXcf> patch(px.data(), K, pSz);
          auto const svd = patch.transpose().bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
          // Soft-threhold svals
          Eigen::ArrayXf s = svd.singularValues();
          float const sl = s(0) * l;
          s = s * (s.abs() - sl) / s.abs();
          s = (s > sl).select(s, 0.f);
          patch.transpose() = svd.matrixU() * s.matrix().asDiagonal() * svd.matrixV().adjoint();
          lr.slice(
            Sz4{0, ix * p + shift[0], iy * p + shift[1], iz * p + shift[2]}, Sz4{K, p, p, p}) = px;
        }
      }
    }
  };
  auto const now = Log::Now();
  zTask(0, nP[2]);
  // Threads::RangeFor(zTask, nP[2]);
  Log::Print(FMT_STRING("LLR Regularization took {}"), Log::ToNow(now));
  return lr;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// This file is manually converted from PROJ4

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

// 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Geometry Library by Barend Gehrels (Geodan, Amsterdam)

// Original copyright notice:

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

#ifndef BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_GAUSS_HPP
#define BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_GAUSS_HPP

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

namespace boost { namespace geometry { namespace projections {

namespace detail { namespace gauss {

static const int MAX_ITER = 20;

struct GAUSS
{
    double C;
    double K;
    double e;
    double ratexp;
};

static const double DEL_TOL = 1e-14;

inline double srat(double esinp, double exp)
{
    return (pow((1.0 - esinp) / (1.0 + esinp), exp));
}

inline GAUSS gauss_ini(double e, double phi0, double &chi, double &rc)
{
    using std::asin;
    using std::cos;
    using std::sin;
    using std::sqrt;
    using std::tan;

    double sphi = 0;
    double cphi = 0;
    double es = 0;

    GAUSS en;
    es = e * e;
    en.e = e;
    sphi = sin(phi0);
    cphi = cos(phi0);
    cphi *= cphi;

    rc = sqrt(1.0 - es) / (1.0 - es * sphi * sphi);
    en.C = sqrt(1.0 + es * cphi * cphi / (1.0 - es));
    chi = asin(sphi / en.C);
    en.ratexp = 0.5 * en.C * e;
    en.K = tan(0.5 * chi + detail::FORTPI)
           / (pow(tan(0.5 * phi0 + detail::FORTPI), en.C) * srat(en.e * sphi, en.ratexp));

    return en;
}

template <typename T>
inline void gauss(GAUSS const& en, T& lam, T& phi)
{
    phi = 2.0 * atan(en.K * pow(tan(0.5 * phi + FORTPI), en.C)
          * srat(en.e * sin(phi), en.ratexp) ) - geometry::math::half_pi<double>();

    lam *= en.C;
}

template <typename T>
inline void inv_gauss(GAUSS const& en, T& lam, T& phi)
{
    lam /= en.C;
    const double num = pow(tan(0.5 * phi + FORTPI) / en.K, 1.0 / en.C);

    int i = 0;
    for (i = MAX_ITER; i; --i)
    {
        const double elp_phi = 2.0 * atan(num * srat(en.e * sin(phi), - 0.5 * en.e)) - geometry::math::half_pi<double>();

        if (geometry::math::abs(elp_phi - phi) < DEL_TOL)
        {
            break;
        }
        phi = elp_phi;
    }

    /* convergence failed */
    if (!i)
    {
        throw proj_exception(-17);
    }
}

}} // namespace detail::gauss
}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_GAUSS_HPP


#include <simpleuv/meshdatatype.h>
#include <Eigen/Dense>

namespace simpleuv
{

float dotProduct(const Vector3 &first, const Vector3 &second)
{
    Eigen::Vector3d v(first.xyz[0], first.xyz[1], first.xyz[2]);
    Eigen::Vector3d w(second.xyz[0], second.xyz[1], second.xyz[2]);
    return v.dot(w);
}

Vector3 crossProduct(const Vector3 &first, const Vector3 &second)
{
    Eigen::Vector3d v(first.xyz[0], first.xyz[1], first.xyz[2]);
    Eigen::Vector3d w(second.xyz[0], second.xyz[1], second.xyz[2]);
    auto u = v.cross(w);
    Vector3 result;
    result.xyz[0] = u.x();
    result.xyz[1] = u.y();
    result.xyz[2] = u.z();
    return result;
}

}


/*
    This file is part of Mitsuba, a physically based rendering system.

    Copyright (c) 2007-2014 by Wenzel Jakob and others.

    Mitsuba is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

    Mitsuba is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <mitsuba/core/shvector.h>
#include <mitsuba/core/transform.h>
#include <boost/math/special_functions/factorials.hpp>

MTS_NAMESPACE_BEGIN

Float *SHVector::m_normalization = NULL;

SHVector::SHVector(Stream *stream) {
    m_bands = stream->readInt();
    unsigned int size = m_bands*m_bands;
    m_coeffs.resize(size);
    for (size_t i=0; i<size; ++i)
        m_coeffs[i] = stream->readFloat();
}

void SHVector::serialize(Stream *stream) const {
    stream->writeInt(m_bands);
    for (size_t i=0; i<(size_t) m_coeffs.size(); ++i)
        stream->writeFloat(m_coeffs[i]);
}

bool SHVector::isAzimuthallyInvariant() const {
    for (int l=0; l<m_bands; ++l) {
        for (int m=1; m<=l; ++m) {
            if (std::abs(operator()(l, -m)) > Epsilon
             || std::abs(operator()(l, m)) > Epsilon)
                return false;
        }
    }
    return true;
}

Float SHVector::eval(Float theta, Float phi) const {
    Float result = 0;
    Float cosTheta = std::cos(theta);
    Float *sinPhi = (Float *) alloca(sizeof(Float)*m_bands),
          *cosPhi = (Float *) alloca(sizeof(Float)*m_bands);

    for (int m=0; m<m_bands; ++m) {
        sinPhi[m] = std::sin((m+1) * phi);
        cosPhi[m] = std::cos((m+1) * phi);
    }

    for (int l=0; l<m_bands; ++l) {
        for (int m=1; m<=l; ++m) {
            Float L = legendreP(l, m, cosTheta) * normalization(l, m);
            result += operator()(l, -m) * SQRT_TWO * sinPhi[m-1] * L;
            result += operator()(l, m)  * SQRT_TWO * cosPhi[m-1] * L;
        }

        result += operator()(l, 0) * legendreP(l, 0, cosTheta) * normalization(l, 0);
    }
    return result;
}

Float SHVector::findMinimum(int res = 32) const {
    Float hExt = (Float) M_PI / res, hInt = (2 * (Float) M_PI)/(res*2);
    Float minimum = std::numeric_limits<Float>::infinity();

    for (int i=0; i<=res; ++i) {
        Float theta = hExt*i;
        for (int j=0; j<=res*2; ++j) {
            Float phi = hInt*j;
            minimum = std::min(minimum, eval(theta, phi));
        }
    }

    return minimum;
}

void SHVector::addOffset(Float value) {
    operator()(0, 0) += 2 * value * (Float) std::sqrt(M_PI);
}

Float SHVector::eval(const Vector &v) const {
    Float result = 0;
    Float cosTheta = v.z, phi = std::atan2(v.y, v.x);
    if (phi < 0) phi += 2*M_PI;
    Float *sinPhi = (Float *) alloca(sizeof(Float)*m_bands),
          *cosPhi = (Float *) alloca(sizeof(Float)*m_bands);

    for (int m=0; m<m_bands; ++m) {
        sinPhi[m] = std::sin((m+1) * phi);
        cosPhi[m] = std::cos((m+1) * phi);
    }

    for (int l=0; l<m_bands; ++l) {
        for (int m=1; m<=l; ++m) {
            Float L = legendreP(l, m, cosTheta) * normalization(l, m);
            result += operator()(l, -m) * SQRT_TWO * sinPhi[m-1] * L;
            result += operator()(l, m)  * SQRT_TWO * cosPhi[m-1] * L;
        }

        result += operator()(l, 0) * legendreP(l, 0, cosTheta) * normalization(l, 0);
    }
    return result;
}

Float SHVector::evalAzimuthallyInvariant(Float theta, Float phi) const {
    Float result = 0, cosTheta = std::cos(theta);
    for (int l=0; l<m_bands; ++l)
        result += operator()(l, 0) * legendreP(l, 0, cosTheta) * normalization(l, 0);
    return result;
}

Float SHVector::evalAzimuthallyInvariant(const Vector &v) const {
    Float result = 0, cosTheta = v.z;
    for (int l=0; l<m_bands; ++l)
        result += operator()(l, 0) * legendreP(l, 0, cosTheta) * normalization(l, 0);
    return result;
}

void SHVector::normalize() {
    Float correction = 1/(2 * (Float) std::sqrt(M_PI)*operator()(0,0));

    for (size_t i=0; i<(size_t) m_coeffs.size(); ++i)
        m_coeffs[i] *= correction;
}

void SHVector::convolve(const SHVector &kernel) {
    SAssert(kernel.getBands() == m_bands);

    for (int l=0; l<m_bands; ++l) {
        Float alpha = std::sqrt(4 * (Float) M_PI / (2*l + 1));
        for (int m=-l; m<=l; ++m)
            operator()(l, m) *= alpha * kernel(l, 0);
    }
}

Matrix3x3 SHVector::mu2() const {
    const Float sqrt5o3 = std::sqrt((Float) 5/ (Float) 3);
    const Float sqrto3 = std::sqrt((Float) 1/ (Float) 3);
    Matrix3x3 result;
    result.setZero();

    SAssert(m_bands > 0);
    result(0, 0) = result(1, 1) =
        result(2, 2) = sqrt5o3*operator()(0,0);

    if (m_bands >= 3) {
        result(0, 0) += -operator()(2,0)*sqrto3 + operator()(2,2);
        result(0, 1) = operator()(2,-2);
        result(0, 2) = -operator()(2, 1);
        result(1, 0) = operator()(2,-2);
        result(1, 1) += -operator()(2,0)*sqrto3 - operator()(2,2);
        result(1, 2) = -operator()(2,-1);
        result(2, 0) = -operator()(2, 1);
        result(2, 1) = -operator()(2,-1);
        result(2, 2) += 2*sqrto3*operator()(2,0);
    }

    return result * (2*std::sqrt((Float) M_PI / 15));
}

std::string SHVector::toString() const {
    std::ostringstream oss;
    oss << "SHVector[bands=" << m_bands << ", {";
    int pos = 0;
    for (int i=0; i<m_bands; ++i) {
        oss << "{";
        for (int j=0, total=i*2+1; j<total; ++j) {
            oss << m_coeffs[pos++];
            if (j+1 < total)
                oss << ", ";
        }
        oss << "}";
        if (i+1 < m_bands)
            oss << ", ";
    }
    oss << "}]";
    return oss.str();
}

Float SHVector::computeNormalization(int l, int m) {
    SAssert(m>=0);
    return std::sqrt(
            ((2*l+1) * boost::math::factorial<Float>(l-m))
        /    (4 * (Float) M_PI * boost::math::factorial<Float>(l+m)));
}

void SHVector::staticInitialization() {
    m_normalization = new Float[SH_NORMTBL_SIZE*(SH_NORMTBL_SIZE+1)/2];
    for (int l=0; l<SH_NORMTBL_SIZE; ++l)
        for (int m=0; m<=l; ++m)
            m_normalization[l*(l+1)/2 + m] = computeNormalization(l, m);
}

void SHVector::staticShutdown() {
    delete[] m_normalization;
    m_normalization = NULL;
}

struct RotationBlockHelper {
    const SHRotation::Matrix &M1, &Mp;
    SHRotation::Matrix &Mn;
    int prevLevel, level;

    inline RotationBlockHelper(
        const SHRotation::Matrix &M1,
        const SHRotation::Matrix &Mp,
        SHRotation::Matrix &Mn)
        : M1(M1), Mp(Mp), Mn(Mn), prevLevel((int) Mp.rows()/2),
        level((int) Mp.rows()/2+1) { }

    inline Float delta(int i, int j) const {
        return (i == j) ? (Float) 1 : (Float) 0;
    }

    inline Float U(int l, int m, int n) const {
        return P(l, m, n, 0);
    }

    inline Float V(int l, int m, int n) const {
        if (m == 0) {
            return P(l, 1, n, 1) + P(l, -1, n, -1);
        } else if (m > 0) {
            if (m == 1)
                return SQRT_TWO * P(l, 0, n, 1);
            else
                return P(l, m-1, n, 1) - P(l, -m+1, n, -1);
        } else {
            if (m == -1)
                return SQRT_TWO * P(l, 0, n, -1);
            else
                return P(l, -m-1, n, -1) + P(l, m+1, n, 1);
        }
    }

    inline Float W(int l, int m, int n) const {
        if (m > 0) {
            return P(l, m+1, n, 1) + P(l, -m-1, n, -1);
        } else {
            return P(l, m-1, n, 1) - P(l, -m+1, n, -1);
        }
    }

    inline Float u(int l, int m, int n) const {
        int denom = (std::abs(n) == l) ? (2*l*(2*l-1)) : ((l+n)*(l-n));
        return std::sqrt((Float) ((l+m)*(l-m)) / (Float) denom);
    }

    inline Float v(int l, int m, int n) const {
        int denom = (std::abs(n) == l) ? (2*l*(2*l-1)) : ((l+n)*(l-n)), absM = std::abs(m);
        return .5f * (1-2*delta(m, 0)) * std::sqrt(
            (Float) ((1+delta(m, 0)) * (l+absM-1)*(l+absM)) / (Float) denom
        );
    }

    inline Float w(int l, int m, int n) const {
        if (m == 0)
            return 0.0f;
        int absM = std::abs(m);
        int denom = (std::abs(n) < l) ? ((l+n)*(l-n)) : (2*l*(2*l-1));

        return -.5f * std::sqrt((Float) ((l-absM-1)*(l-absM)) / (Float) denom);
    }

    inline Float P(int l, int m, int n, int i) const {
        if (std::abs(n) < l)
            return R(i, 0) * M(m, n);
        else if (n == l)
            return R(i, 1) * M(m, l-1) - R(i, -1) * M(m, -l+1);
        else if (n == -l)
            return R(i, 1) * M(m, -l+1) + R(i, -1) * M(m, l-1);
        else {
            SLog(EError, "Internal error!");
            return 0.0f;
        }
    }

    inline Float R(int m, int n) const {
        return M1(m+1, n+1);
    }

    inline Float M(int m, int n) const {
        return Mp(m+prevLevel, n+prevLevel);
    }

    void compute() {
        for (int m=-level; m<=level; ++m) {
            for (int n=-level; n<=level; ++n) {
                Float uVal = u(level, m, n), vVal = v(level, m, n), wVal = w(level, m, n);
                Mn(m+level, n+level) =
                      (uVal != 0 ? (uVal * U(level, m, n)) : (Float) 0)
                    + (vVal != 0 ? (vVal * V(level, m, n)) : (Float) 0)
                    + (wVal != 0 ? (wVal * W(level, m, n)) : (Float) 0);
            }
        }
    }
};

void SHVector::rotationBlock(
        const SHRotation::Matrix &M1,
        const SHRotation::Matrix &Mp,
        SHRotation::Matrix &Mn) {
    RotationBlockHelper rbh(M1, Mp, Mn);
    rbh.compute();
}

void SHVector::rotation(const Transform &t, SHRotation &rot) {
    rot.blocks[0](0, 0) = 1;
    if (rot.blocks.size() <= 1)
        return;

    const Matrix4x4 &trafo = t.getMatrix();
    rot.blocks[1](0, 0) =  trafo.m[1][1];
    rot.blocks[1](0, 1) = -trafo.m[2][1];
    rot.blocks[1](0, 2) =  trafo.m[0][1];
    rot.blocks[1](1, 0) = -trafo.m[1][2];
    rot.blocks[1](1, 1) =  trafo.m[2][2];
    rot.blocks[1](1, 2) = -trafo.m[0][2];
    rot.blocks[1](2, 0) =  trafo.m[1][0];
    rot.blocks[1](2, 1) = -trafo.m[2][0];
    rot.blocks[1](2, 2) =  trafo.m[0][0];

    if (rot.blocks.size() <= 2)
        return;

    for (size_t i=2; i<rot.blocks.size(); ++i)
        rotationBlock(rot.blocks[1], rot.blocks[i-1], rot.blocks[i]);
}

void SHRotation::operator()(const SHVector &source, SHVector &target) const {
    SAssert(source.getBands() == target.getBands());
    for (int l=0; l<source.getBands(); ++l) {
        const SHRotation::Matrix &M = blocks[l];
        for (int m1=-l; m1<=l; ++m1) {
            Float result = 0;
            for (int m2=-l; m2<=l; ++m2)
                result += M(m1+l, m2+l)*source(l, m2);
            target(l, m1) = result;
        }
    }
}

SHSampler::SHSampler(int bands, int depth) : m_bands(bands), m_depth(depth) {
    m_phiMap = new Float**[depth+1];
    m_legendreMap = new Float**[depth+1];
    m_normalization = new Float[m_bands*(m_bands+1)/2];
    m_dataSize = m_bands*(m_bands+1)/2;
    Assert(depth >= 1);

    for (int i=0; i<=depth; ++i) {
        int res = 1 << i;
        Float zStep  = -2 / (Float) res;
        Float phiStep = 2 * (Float) M_PI / (Float) res;
        m_phiMap[i] = new Float*[res];
        m_legendreMap[i] = new Float*[res];

        for (int j=0; j<res; ++j) {
            m_phiMap[i][j] = phiIntegrals(phiStep*j, phiStep*(j+1));
            m_legendreMap[i][j] = legendreIntegrals(1+zStep*j, 1+zStep*(j+1));
        }
    }

    for (int l=0; l<m_bands; ++l) {
        for (int m=0; m<=l; ++m) {
            Float normFactor = boost::math::tgamma_delta_ratio(
                (Float) (l - m + 1), (Float) (2 * m), boost::math::policies::policy<>());
            normFactor = std::sqrt(normFactor * (2 * l + 1) / (4 * (Float) M_PI));
            if (m != 0)
                normFactor *= SQRT_TWO;
            m_normalization[I(l, m)] = normFactor;
        }
    }
}

std::string SHSampler::toString() const {
    std::ostringstream oss;
    oss << "SHSampler[bands=" << m_bands << ", depth=" << m_depth
        << ", size=" << (m_dataSize*sizeof(double))/1024 << " KiB]";
    return oss.str();
}

Float SHSampler::warp(const SHVector &f, Point2 &sample) const {
    int i = 0, j = 0;
    Float integral = 0, integralRoot = integrate(0, 0, 0, f);

    for (int depth = 1; depth <= m_depth; ++depth) {
        /* Do not sample negative areas */
        Float q00 = std::max(integrate(depth, i, j, f), (Float) 0);
        Float q10 = std::max(integrate(depth, i, j+1, f), (Float) 0);
        Float q01 = std::max(integrate(depth, i+1, j, f), (Float) 0);
        Float q11 = std::max(integrate(depth, i+1, j+1, f), (Float) 0);

        Float z1 = q00 + q10, z2 = q01 + q11, phi1, phi2;
        Float zNorm = (Float) 1 / (z1+z2);
        z1 *= zNorm; z2 *= zNorm;

        if (sample.x < z1) {
            sample.x /= z1;
            phi1 = q00; phi2 = q10;
            i <<= 1;
        } else {
            sample.x = (sample.x - z1) / z2;
            phi1 = q01; phi2 = q11;
            i = (i+1) << 1;
        }

        Float phiNorm = (Float) 1 / (phi1+phi2);
        Float phi1Norm = phi1*phiNorm, phi2Norm = phi2*phiNorm;

        if (sample.y <= phi1Norm) {
            sample.y /= phi1Norm;
            j <<= 1;
            integral = phi1;
        } else {
            sample.y = (sample.y - phi1Norm) / phi2Norm;
            j = (j+1) << 1;
            integral = phi2;
        }
    }

    Float zStep = -2 / (Float) (1 << m_depth);
    Float phiStep = 2 * (Float) M_PI / (Float) (1 << m_depth);
    i >>= 1; j >>= 1;

    Float z = 1 + zStep * i + zStep * sample.x;
    sample.x = std::acos(z);
    sample.y = phiStep * j + phiStep * sample.y;

    /* PDF of sampling the mip-map bin */
    Float pdfBin = integral/integralRoot;

    /* Density within the bin */
    Float density = -1/(zStep*phiStep);

    return density*pdfBin;
}

SHSampler::~SHSampler() {
    for (int i=0; i<=m_depth; ++i) {
        int res = 1 << i;
        for (int j=0; j<res; ++j) {
            delete[] m_phiMap[i][j];
            delete[] m_legendreMap[i][j];
        }
        delete[] m_phiMap[i];
        delete[] m_legendreMap[i];
    }
    delete[] m_phiMap;
    delete[] m_legendreMap;
    delete[] m_normalization;
}

Float SHSampler::integrate(int depth, int zBlock, int phiBlock, const SHVector &f) const {
    Float result = 0;

    for (int l=0; l<m_bands; ++l) {
        for (int m=-l; m<=l; ++m) {
            Float basisIntegral = m_normalization[I(l, std::abs(m))]
                * lookupIntegral(depth, zBlock, phiBlock, l, m);
            result += basisIntegral * f(l, m);
        }
    }
    return result;
}

Float *SHSampler::phiIntegrals(Float a, Float b) {
    Float *sinPhiA = new Float[m_bands+1];
    Float *sinPhiB = new Float[m_bands+1];
    Float *cosPhiA = new Float[m_bands+1];
    Float *cosPhiB = new Float[m_bands+1];
    Float *result = new Float[2*m_bands+1];
    m_dataSize += 2*m_bands+1;

    cosPhiA[0] = 1; sinPhiA[0] = 0;
    cosPhiB[0] = 1; sinPhiB[0] = 0;
    cosPhiA[1] = std::cos(a);
    sinPhiA[1] = std::sin(a);
    cosPhiB[1] = std::cos(b);
    sinPhiB[1] = std::sin(b);

    for (int m=2; m<=m_bands; ++m) {
        sinPhiA[m] = 2*sinPhiA[m-1]*cosPhiA[1] - sinPhiA[m-2];
        sinPhiB[m] = 2*sinPhiB[m-1]*cosPhiB[1] - sinPhiB[m-2];

        cosPhiA[m] = 2*cosPhiA[m-1]*cosPhiA[1] - cosPhiA[m-2];
        cosPhiB[m] = 2*cosPhiB[m-1]*cosPhiB[1] - cosPhiB[m-2];
    }

    for (int m=-m_bands; m<=m_bands; ++m) {
        if (m == 0)
            result[P(m)] = b-a;
        else if (m > 0)
            result[P(m)] = (sinPhiB[m]-sinPhiA[m])/m;
        else
            result[P(m)] = (cosPhiB[-m]-cosPhiA[-m])/m;
    }

    delete[] sinPhiA;
    delete[] sinPhiB;
    delete[] cosPhiA;
    delete[] cosPhiB;
    return result;
}

Float *SHSampler::legendreIntegrals(Float a, Float b) {
    Float *P = new Float[m_bands*(m_bands+1)/2];
    m_dataSize += m_bands*(m_bands+1)/2;

    P[I(0, 0)] = b-a;

    if (m_bands == 1)
        return P;

    Float *Pa = new Float[m_bands*(m_bands+1)/2];
    Float *Pb = new Float[m_bands*(m_bands+1)/2];

    for (int l=0; l<m_bands; ++l) {
        for (int m=0; m<=l; ++m) {
            Pa[I(l,m)] = legendreP(l, m, a);
            Pb[I(l,m)] = legendreP(l, m, b);
        }
    }

    P[I(1,0)] = (b*b - a*a)/2;
    P[I(1,1)] = .5f * (-b*std::sqrt(1-b*b) - std::asin(b) + a*std::sqrt(1-a*a) + std::asin(a));

    for (int l=2; l<m_bands; ++l) {
        for (int m=0; m<=l-2; ++m) {
            Float ga = (2*l-1)*(1-a*a) * Pa[I(l-1,m)];
            Float gb = (2*l-1)*(1-b*b) * Pb[I(l-1,m)];
            P[I(l, m)] = ((l-2)*(l-1+m)*P[I(l-2, m)]-gb+ga)/((l+1)*(l-m));
        }

        P[I(l, l-1)] = (2*l-1)/(Float)(l+1) * ((1-a*a)*Pa[I(l-1, l-1)] - (1-b*b)*Pb[I(l-1, l-1)]);
        P[I(l, l)] = 1/(Float)(l+1) * (l*(2*l-3)*(2*l-1) * P[I(l-2, l-2)] + b*Pb[I(l,l)] - a*Pa[I(l, l)]);
    }

    delete[] Pa;
    delete[] Pb;

    return P;
}

MTS_IMPLEMENT_CLASS(SHSampler, false, Object)
MTS_NAMESPACE_END


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

void find_feature_matches (
    const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches );

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

void bundleAdjustment (
    const vector<Point3f> points_3d,
    const vector<Point2f> points_2d,
    const Mat& K,
    Mat& R, Mat& t
);

int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点
    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    for ( DMatch m:matches )
    {
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        if ( d == 0 )   // bad depth
            continue;
        float dd = d/1000.0;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
    }

    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

    Mat r, t;
    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    Mat R;
    cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵

    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;

    cout<<"calling bundle adjustment"<<endl;

    bundleAdjustment ( pts_3d, pts_2d, K, R, t );
}

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, match );

    //-- 第四步:匹配点对筛选
    double min_dist=10000, max_dist=0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = match[i].distance;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( match[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            matches.push_back ( match[i] );
        }
    }
}

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
}

void bundleAdjustment (
    const vector< Point3f > points_3d,
    const vector< Point2f > points_2d,
    const Mat& K,
    Mat& R, Mat& t )
{
    // 初始化g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    Eigen::Matrix3d R_mat;
    R_mat <<
          R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
               R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
               R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
                            R_mat,
                            Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
                        ) );
    optimizer.addVertex ( pose );

    int index = 1;
    for ( const Point3f p:points_3d )   // landmarks
    {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
        optimizer.addVertex ( point );
    }

    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
        K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // edges
    index = 1;
    for ( const Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId ( index );
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
        edge->setVertex ( 1, pose );
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
        edge->setParameterId ( 0,0 );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        index++;
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose ( true );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
    cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;

    cout<<endl<<"after optimization:"<<endl;
    cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          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 __OPTIMISATION2_ALGO_LM_MANY_CLASSES_SCHUR_COMPLEMENT_HPP__
#define __OPTIMISATION2_ALGO_LM_MANY_CLASSES_SCHUR_COMPLEMENT_HPP__

#include <libv/lma/ttt/traits/wrap.hpp>
#include <libv/lma/global.hpp>
#include <libv/lma/lm/ba/computing.hpp>
#include <libv/lma/lm/ba/make_type.hpp>
#include <libv/lma/lm/ba/create_hessian.hpp>
#include <libv/lma/lm/omp/omp.hpp>
#include <boost/mpl/advance.hpp>
#include <boost/mpl/remove_if.hpp>
#include <boost/mpl/contains.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/type_traits.hpp>
#include <boost/fusion/include/as_map.hpp>
#include <libv/core/tag.hpp>

namespace lma
{

  template<class VTYPE, class Ap, class H, class P, class Y> struct ProdApPH
  {
    Ap& ap;
    const H& h;
    const P& p;
    const Y& y;
    ProdApPH(Ap& ap_, const H& h_, const P& p_, const Y& y_):ap(ap_),h(h_),p(p_),y(y_){}

    template<class T> void operator()(T){}

    template<class Key1,class Key2, class Value, template<class,class> class Pair> void operator()(ttt::wrap<Pair<Pair<Key1,Key2>,Value>>,
      typename boost::disable_if<
          mpl::or_<
                    mpl::not_<br::has_key<H,Pair<Key2,VTYPE>>>,
                    mpl::not_<br::has_key<Y,Pair<Key1,VTYPE>>>
                  >
            >::type * = 0)
    {
      {
        auto& a1 = bf::at_key<Key1>(ap);
        const auto& pr2 = bf::at_key<Key2>(p);
        const auto& y1= bf::at_key<Pair<Key1,VTYPE>>(y);
        const auto& w2 = bf::at_key<Pair<Key2,VTYPE>>(h);
        const auto& v = bf::at_key<Pair<VTYPE,VTYPE>>(h);

        typedef typename boost::remove_reference<decltype(v)>::type V;

        VectorColumn< VTYPE, typename V::MatrixTag > v0;

// 	std::cout << " [1] : " << a1.name() << " = " << w2.name() << " " << pr2.name() << " " << y1.name() << std::endl;
        // v0 = Wt * pr
        prod(v0,w2,pr2);
        // a = - Y * v0 , (Y = W * V)
        prod_minus(a1,y1,v0);

        auto& a2 = bf::at_key<Key2>(ap);
        const auto& pr1 = bf::at_key<Key1>(p);
        const auto& y2= bf::at_key<Pair<Key2,VTYPE>>(y);
        const auto& w1 = bf::at_key<Pair<Key1,VTYPE>>(h);

        typedef typename boost::remove_reference<decltype(v)>::type V;

        VectorColumn< VTYPE, typename V::MatrixTag > v1;

// 	std::cout << " [2] : " << a2.name() << " = " << w1.name() << " " << pr1.name() << " " << y2.name() << std::endl;
        // v1 = Wt * pr
        prod(v1,w1,pr1);
        // a = - Y * v1, (Y = W * V)
        prod_minus(a2,y2,v1);
      }
    }

    //! cas où on est sur la diagonale
    template<class Key1, class Value, template<class,class> class Pair> void operator()(ttt::wrap<Pair<Pair<Key1,Key1>,Value>>,
      typename boost::disable_if<boost::mpl::not_<br::has_key<H,Pair<Key1,VTYPE>>>>::type * = 0)
    {
      const auto& pr = bf::at_key<Key1>(p);
      const auto& u = bf::at_key<Pair<Key1,Key1>>(h);

      auto& a = bf::at_key<Key1>(ap);
      auto& w = bf::at_key<Pair<Key1,VTYPE>>(h);
      auto& y0 = bf::at_key<Pair<Key1,VTYPE>>(y);

      auto& v = bf::at_key<Pair<VTYPE,VTYPE>>(h);

      if (a.size()==0) a.resize(pr.size());
      typedef typename boost::remove_reference<decltype(v)>::type V;
      VectorColumn< VTYPE, typename V::MatrixTag > v0;
      VectorColumn< Key1, typename V::MatrixTag > v1;
      v0.resize(v.size());
      v1.resize(u.size());

//       std::cout << " [0] : " << a.name() << " = " << w.name() << " " << pr.name() << " " << y0.name() << std::endl;
      //v0 = Wt * pr
      prod(v0,w,pr);

      //v1 = Y * v0 , ( Y = W * V )
      prod(v1,y0,v0);

      // a -= u1
//       #pragma omp parallel for if(use_omp())
      for(auto i = w.first() ; i < w.size() ; ++i)
        a(i) -= v1(i);
    }
  };

  template<class VTYPE, class AP, class H, class P, class Y> ProdApPH<VTYPE,AP,H,P,Y> prod_ap_ph(AP& ap, const H& h, const P& p, const Y& y){ return ProdApPH<VTYPE,AP,H,P,Y>(ap,h,p,y); }

  template<class Float, class I, class List, class J, class Result> struct UnrollW2 :
    mpl::push_back<
		    Result,
		    typename MakeTupleTable<
					    typename mpl::at<List,I>::type,
					    typename mpl::at<List,J>::type,
					    Float
					  >::type
		  >::type {};

  template<class Float, class K, class List, class Int, class Result> struct UnrollW1_ :
    For<K::value,mpl::size<List>::value,List,UnrollW2<Float,Int,mpl::_1,mpl::_2,mpl::_3>,Result> {};

  template<class Float, class A> using UnrollW1 = UnrollW1_<Float,A,mpl::_1,mpl::_2,mpl::_3>;

  template<class L, class Int, class Result> struct PushBack_ :
    mpl::push_back<Result,typename mpl::at<L,Int>::type> {};

  typedef PushBack_<mpl::_1,mpl::_2,mpl::_3> PushBack;

  template<class BA, class NumericTag> struct ImplicitSchurContainer
  {
    typedef typename BA::Keys Keys;
    typedef typename BA::MatrixTag MatrixTag;

    // K_ est le nombre de famille à mettre dans Vs
    // K est la position de Vs
    static const size_t K_ = Size<NumericTag>::value;
    static const size_t N = mpl::size<Keys>::value;
    static const size_t K = N - K_;

    static_assert( (N > K) , "Nombre de paramètres dans Schur >= Nombre de paramètre dans le problème");

    typedef typename For<K,N,Keys,PushBack>::type KeyVs;
    typedef typename For<0,K,Keys,PushBack>::type KeyUs;

    typedef typename For<0,K,Keys,UnrollW1<MatrixTag,mpl::int_<K>>>::type TypeWs_;
    typedef typename
                    mpl::remove_if<
                                    TypeWs_,
                                    mpl::not_<
                                              mpl::contains<typename BA::ListeHessien,mpl::_1>
                                            >
                                  >::type TypeWs;
    typedef typename br::as_map<TypeWs>::type TupleWs;

    typedef typename mpl::transform< KeyUs, VectorToPairStruct<mpl::_1,MatrixTag> >::type ListResidu;
    typedef typename mpl::transform< KeyVs, VectorToPairStruct<mpl::_1,MatrixTag> >::type ListResiduVs;
    typedef typename br::as_map<ListResidu>::type TupleResiduUs;
    typedef typename br::as_map<ListResiduVs>::type TupleResiduVs;

    typedef typename mpl::transform<KeyVs,ToTable<Single<mpl::_1>,MatrixTag>>::type TypeVs;
    typedef typename br::as_map<TypeVs>::type TupleVs;

    typedef typename mpl::transform< KeyUs, pair_>::type DiagUs;
    typedef typename mpl::transform< KeyVs, pair_>::type DiagVs;

    ImplicitSchurContainer()
    {
//        std::cout << "Parametres : " << ttt::name<ListeParametre>() << std::endl;
//        std::cout << "Us         : " << ttt::name<Us>() << std::endl;
//        std::cout << "Vs         : " << ttt::name<Vs>() << std::endl;
       //std::cout << "Ws         : " << ttt::name<TypeWs>() << std::endl;
//        std::cout << "ResiduUs   : " << ttt::name<TupleResiduUs>() << std::endl;
//        std::cout << "TypeVs     : " << ttt::name<TypeVs>() << std::endl;
//        std::cout << "TupleVs    : " << ttt::name<TupleVs>() << std::endl;
    }

    TupleWs ys;
    TupleResiduUs bs;
    TupleVs save_vs;
  };
}// eon

#endif


// Author of FLOAM: Wang Han
// Email wh200720041@gmail.com
// Homepage https://wanghan.pro
#ifndef _LIDAR_OPTIMIZATION_ANALYTIC_H_
#define _LIDAR_OPTIMIZATION_ANALYTIC_H_

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>

Eigen::Matrix3d skew(Eigen::Vector3d& mat_in) {
  Eigen::Matrix<double, 3, 3> skew_mat;
  skew_mat.setZero();
  skew_mat(0, 1) = -mat_in(2);
  skew_mat(0, 2) = mat_in(1);
  skew_mat(1, 2) = -mat_in(0);
  skew_mat(1, 0) = mat_in(2);
  skew_mat(2, 0) = -mat_in(1);
  skew_mat(2, 1) = mat_in(0);
  return skew_mat;
}

void getTransformFromSe3(const Eigen::Matrix<double, 6, 1>& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t) {
  Eigen::Vector3d omega(se3.data());
  Eigen::Vector3d upsilon(se3.data() + 3);
  Eigen::Matrix3d Omega = skew(omega);

  double theta = omega.norm();
  double half_theta = 0.5 * theta;

  double imag_factor;
  double real_factor = cos(half_theta);
  if (theta < 1e-10) {
    double theta_sq = theta * theta;
    double theta_po4 = theta_sq * theta_sq;
    imag_factor = 0.5 - 0.0208333 * theta_sq + 0.000260417 * theta_po4;
  } else {
    double sin_half_theta = sin(half_theta);
    imag_factor = sin_half_theta / theta;
  }

  q = Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z());

  Eigen::Matrix3d J;
  if (theta < 1e-10) {
    J = q.matrix();
  } else {
    Eigen::Matrix3d Omega2 = Omega * Omega;
    J = (Eigen::Matrix3d::Identity() + (1 - cos(theta)) / (theta * theta) * Omega + (theta - sin(theta)) / (pow(theta, 3)) * Omega2);
  }

  t = J * upsilon;
}

class PoseSE3Parameterization : public ceres::LocalParameterization {
public:
  PoseSE3Parameterization() {}
  virtual ~PoseSE3Parameterization() {}
  // 从这里看，参数块应该是[q,t]，且q是[x,y,z,w]
  virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const {
    Eigen::Map<const Eigen::Vector3d> trans(x + 4);

    Eigen::Quaterniond delta_q;
    Eigen::Vector3d delta_t;
    getTransformFromSe3(Eigen::Map<const Eigen::Matrix<double, 6, 1>>(delta), delta_q, delta_t);
    Eigen::Map<const Eigen::Quaterniond> quater(x);
    Eigen::Map<Eigen::Quaterniond> quater_plus(x_plus_delta);
    Eigen::Map<Eigen::Vector3d> trans_plus(x_plus_delta + 4);
    // TODO 不需要归一化吗？
    quater_plus = delta_q * quater;
    trans_plus = delta_q * trans + delta_t;

    return true;
  }
  // ref: https://github.com/wh200720041/floam/issues/50
  // 这是全局参数到局部参数的雅可比矩阵。全局是trans,quat，因此是7维，局部则是se3,因此是6维。
  // 答案中提到了J1*J2,其中J1是参数块对[t,q]求导来自于evaluate或者AD，J2则是这里的到李代数的参数化。
  virtual bool ComputeJacobian(const double* x, double* jacobian) const {
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
    (j.topRows(6)).setIdentity();
    (j.bottomRows(1)).setZero();

    return true;
  }
  virtual int GlobalSize() const { return 7; }
  virtual int LocalSize() const { return 6; }
};

#endif  // _LIDAR_OPTIMIZATION_ANALYTIC_H_


/*
 * MIT License
 *
 * Copyright (c) 2020 International Business Machines
 *
 * 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 sample program for education purposes only.
// It implements a very simple homomorphic encryption based
// db search algorithm for demonstration purposes.

// This country lookup example is derived from the BGV database demo
// code originally writte by Jack Crawford for a lunch and learn
// session at IBM Research (Hursley) in 2019.
// The original example code ships with HElib and can be found at
// https://github.com/IBM-HElib/HElib/tree/master/examples/BGV_database_lookup
//
// INFO: The API used to create these examples can be found online
// ML-HElib: https://ibm.github.io/fhe-toolkit-linux
// HElib: https://ibm.github.io/fhe-toolkit-linux/html/helib/index.html
//

#include <iostream>

#include "helayers/hebase/hebase.h"
#include "helayers/hebase/helib/HelibBgvContext.h"
#include <fstream>
#include <helib/ArgMap.h>
#include <NTL/BasicThreadPool.h>

using namespace helayers;
using namespace std;

// Forward declarations. These functions are explained later.
vector<pair<string, string>> read_csv(string filename, int maxLen);
void run(HeContext& he,
         const string& db_filename,
         const std::string& countryName,
         bool debug);
bool isPowerOf2(int v);
vector<int> stringToAscii(const string& val);

int main(int argc, char* argv[])
{
  // Note: The parameters have been chosen to provide a somewhat
  // faster running time with a non-realistic security level.
  // Do Not use these parameters in real applications.

  // Plaintext prime modulus
  unsigned long p = 127;
  // Cyclotomic polynomial - defines phi(m)
  unsigned long m = 128; // this will give 32 slots
  // Hensel lifting (default = 1)
  unsigned long r = 1;
  // Number of bits of the modulus chain
  unsigned long bits = 1000;
  // Number of columns of Key-Switching matrix (default = 2 or 3)
  unsigned long c = 2;
  // Size of NTL thread pool (default =1)
  unsigned long nthreads = 1;
  // input database file name
  string db_filename =
      getExamplesDir() + "/BGV_world_country_db_lookup/countries_dataset.csv";
  // debug output (default no debug output)
  bool debug = false;

  string countryName = "";

  helib::ArgMap amap;
  amap.arg("m", m, "Cyclotomic polynomial ring");
  amap.arg("p", p, "Plaintext prime modulus");
  amap.arg("r", r, "Hensel lifting");
  amap.arg("bits", bits, "# of bits in the modulus chain");
  amap.arg("c", c, "# fo columns of Key-Switching matrix");
  amap.arg("nthreads", nthreads, "Size of NTL thread pool");
  amap.arg(
      "db_filename", db_filename, "Qualified name for the database filename");
  amap.arg("country", countryName, "Country to search for");
  amap.toggle().arg("-debug", debug, "Toggle debug output", "");
  amap.parse(argc, argv);

  // set NTL Thread pool size
  if (nthreads > 1)
    NTL::SetNumThreads(nthreads);

  cout << "\n*********************************************************";
  cout << "\n*           Privacy Preserving Search Example           *";
  cout << "\n*           =================================           *";
  cout << "\n*                                                       *";
  cout << "\n* This is a sample program for education purposes only. *";
  cout << "\n* It implements a very simple homomorphic encryption    *";
  cout << "\n* based db search algorithm for demonstration purposes. *";
  cout << "\n*                                                       *";
  cout << "\n*********************************************************";
  cout << endl;

  cout << "---Initialising HE Environment ... ";
  // Initialize context
  cout << "\nInitializing the Context ... " << endl;

  // To setup helib using the hebase layer, let's first
  // copy all configuration params to an HelibConfig object:
  HelibConfig conf;
  conf.p = p;
  conf.m = m;
  conf.r = r;
  conf.L = bits;
  conf.c = c;

  // Next we'll initialize a BGV scheme in helib.
  // The following two lines perform full intializiation
  // Including key generation.
  // (We added code for timing it).
  HELIB_NTIMER_START(timer_Context);
  HelibBgvContext he;
  he.init(conf);
  HELIB_NTIMER_STOP(timer_Context);

  // Helib-BGV is now ready to start doing some HE work.
  // which we'll do in the follwing function, defined below
  run(he, db_filename, countryName, debug);

  return 0;
}

void run(HeContext& he,
         const string& db_filename,
         const std::string& countryName,
         bool debug)
{

  // The run function receives an abstract HeContext class.
  // Therefore the code below is oblivious to a particular HE scheme
  // implementation.

  // First let's print general information on our library and scheme.
  // This will print their names, and the configuraton details.
  he.printSignature();

  // However we do have some requirements that we can
  // assert exists:
  // We require the plaintext to be over modular arithmetic.
  // We'll rely on that later.
  always_assert(he.getTraits().getIsModularArithmetic());
  // Since we store ascii codes, we need it at least to be able
  // to handle the numbers 0...127
  always_assert(he.getTraits().getArithmeticModulus() >= 127);

  // Next, print the security level
  // Note: This will be negligible to improve performance time.
  cout << "\n***Security Level: " << he.getSecurityLevel()
       << " *** Negligible for this example ***" << endl;

  // Let's also print the number of slots.
  // Each ciphertext will have this many slots.
  cout << "\nNumber of slots: " << he.slotCount() << endl;

  // Now we'll read in the database (in cleartext).
  // This function we'll make sure no string is longer than he.slotCount()
  vector<pair<string, string>> country_db =
      read_csv(db_filename, he.slotCount());

  cout << "\n---Initializing the encrypted key,value pair database ("
       << country_db.size() << " entries)...";
  cout << "\nConverting strings to numeric representation into Ptxt objects ..."
       << endl;

  // We'll now encrypt our country-capital database.
  HELIB_NTIMER_START(timer_CtxtCountryDB);
  // The encoder class handles both encoding and encrypting.
  Encoder enc(he);
  // This is the database: a vector of pairs of CTile-s.
  // A CTile is a ciphertext object.
  vector<pair<CTile, CTile>> encrypted_country_db;
  for (const auto& country_capital_pair : country_db) {
    // Create a country ciphertext, and encrypt inside
    // the ascii vector representation of each country.
    // For example, Norway is represented
    // (78,111,114,119,97,121,  0,0,0, ...)
    CTile country(he);
    enc.encodeEncrypt(country, stringToAscii(country_capital_pair.first));
    // Similarly encrypt the capital name
    CTile capital(he);
    enc.encodeEncrypt(capital, stringToAscii(country_capital_pair.second));
    // Add the pair to the database
    encrypted_country_db.emplace_back(move(country), move(capital));
  }
  HELIB_NTIMER_STOP(timer_CtxtCountryDB);

  if (debug) {
    helib::printNamedTimer(cout << endl, "timer_Context");
    helib::printNamedTimer(cout, "timer_CtxtCountryDB");
  }

  cout << "\nInitialization Completed - Ready for Queries" << endl;
  cout << "--------------------------------------------" << endl;

  /** Create the query **/

  // Read in query from the command line
  string query_string;
  if (countryName == "") {
    cout << "\nPlease enter the name of a Country: ";
    getline(cin, query_string);
  } else
    query_string = countryName;

  cout << "Looking for the Capital of " << query_string << endl;
  cout << "This may take a few minutes ... " << endl;

  HELIB_NTIMER_START(timer_TotalQuery);
  HELIB_NTIMER_START(timer_EncryptQuery);

  // Encrypt the query similar to the way we encrypted
  // the country and capital names
  CTile query(he);
  enc.encodeEncrypt(query, stringToAscii(query_string));

  HELIB_NTIMER_STOP(timer_EncryptQuery);

  /************ Perform the database search ************/

  HELIB_NTIMER_START(timer_QuerySearch);
  vector<CTile> mask;
  mask.reserve(country_db.size());
  NativeFunctionEvaluator eval(he);
  long modulusP = he.getTraits().getArithmeticModulus();

  // For every entry in our database we perform the following
  // calculation:
  for (const auto& encrypted_pair : encrypted_country_db) {
    //  Copy of database key: a country name
    CTile mask_entry = encrypted_pair.first;
    // Calculate the difference
    // In each slot now we'll have 0 when characters match,
    // or non-zero when there's a mismatch
    mask_entry.sub(query);

    // Fermat's little theorem:
    // Since the underlying plaintext are in modular arithmetic,
    // Raising to the power of modulusP convers all non-zero values
    // to 1.
    eval.powerInPlace(mask_entry, modulusP - 1);

    // Negate the ciphertext
    // Now we'll have 0 for match, -1 for mismatch
    mask_entry.negate();

    // Add +1
    // Now we'll have 1 for match, 0 for mismatch
    mask_entry.addScalar(1);

    // We'll now multiply all slots together, since
    // we want a complete match across all slots.

    // If slot count is a power of 2 there's an efficient way
    // to do it:
    // we'll do a rotate-and-multiply algorithm, similar to
    // a rotate-and-sum one.
    if (isPowerOf2(he.slotCount())) {
      for (int rot = 1; rot < he.slotCount(); rot *= 2) {
        CTile tmp(mask_entry);
        tmp.rotate(-rot);
        mask_entry.multiply(tmp);
      }
    } else {
      // Otherwise we'll create all possible rotations, and multiply all of
      // them.
      // Note that for non powers of 2 a rotate-and-multiply algorithm
      // can still be used as well, though it's more complicated and
      // beyond the scope of this example.
      vector<CTile> rotated_masks(he.slotCount(), mask_entry);
      for (int i = 1; i < rotated_masks.size(); i++)
        rotated_masks[i].rotate(-i); // Rotate each of the masks
      eval.totalProduct(mask_entry,
                        rotated_masks); // Multiply each of the masks
    }

    // mask_entry is now either all 1s if query==country,
    // or all 0s otherwise.
    // After we multiply by capital name it will be either
    // the capital name, or all 0s.
    mask_entry.multiply(encrypted_pair.second);
    // We collect all our findings.
    mask.push_back(mask_entry);
  }
  HELIB_NTIMER_STOP(timer_QuerySearch);

  // Aggregate the results into a single ciphertext
  // Note: This code is for educational purposes and thus we try to refrain
  // from using the STL and do not use std::accumulate
  CTile value = mask[0];
  for (int i = 1; i < mask.size(); i++)
    value.add(mask[i]);

  // /************ Decrypt and print result ************/

  HELIB_NTIMER_START(timer_DecryptQueryResult);
  vector<int> res = enc.decryptDecodeInt(value);
  HELIB_NTIMER_STOP(timer_DecryptQueryResult);

  // Convert from ASCII to a string
  string string_result;
  for (long i = 0; i < res.size(); ++i)
    string_result.push_back(static_cast<long>(res[i]));

  HELIB_NTIMER_STOP(timer_TotalQuery);

  // Print DB Query Timers
  if (debug) {
    helib::printNamedTimer(cout << endl, "timer_EncryptQuery");
    helib::printNamedTimer(cout, "timer_QuerySearch");
    helib::printNamedTimer(cout, "timer_DecryptQueryResult");
    cout << endl;
  }

  if (string_result.at(0) == 0x00) {
    string_result = "Country name not in the database.\n*** Please make sure "
                    "to enter the name of an European Country\n*** with the "
                    "first letter in upper case.";
  }

  cout << "\nQuery result: " << string_result << endl;
  helib::printNamedTimer(std::cout, "timer_TotalQuery");
}

// Utility function to read <K,V> CSV data from file
vector<pair<string, string>> read_csv(string filename, int maxLen)
{
  vector<pair<string, string>> dataset;
  ifstream data_file(filename);

  if (!data_file.is_open())
    throw runtime_error(
        "Error: This example failed trying to open the data file: " + filename +
        "\n           Please check this file exists and try again.");

  vector<string> row;
  string line, entry, temp;

  if (data_file.good()) {
    // Read each line of file
    while (getline(data_file, line)) {
      row.clear();
      stringstream ss(line);
      while (getline(ss, entry, ',')) {
        row.push_back(entry);
      }
      if (row[0].size() > maxLen)
        throw runtime_error("Country name " + row[0] + " too long");
      if (row[1].size() > maxLen)
        throw runtime_error("Capital name " + row[1] + " too long");

      // Add key value pairs to dataset
      dataset.push_back(make_pair(row[0], row[1]));
    }
  }

  data_file.close();
  return dataset;
}

// Return a vector of ints with the i'th element containing the ascii
// code of the i'th character
vector<int> stringToAscii(const string& val)
{
  vector<int> res;
  res.reserve(val.size());
  for (size_t i = 0; i < val.size(); ++i) {
    res.push_back(val[i]);
  }
  return res;
}

// Returns true if v is a power of 2
bool isPowerOf2(int v) { return (v & v - 1) == 0; }


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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.

/**
 * @file Solve a double integrator optimal control problem as a nonlinear program.
 */

#include <Eigen/Core>
#include <smooth/feedback/collocation/dyn_error.hpp>
#include <smooth/feedback/compat/ipopt.hpp>
#include <smooth/feedback/ocp_to_nlp.hpp>

#include <chrono>
#include <iostream>

#include "ocp_doubleintegrator.hpp"

#ifdef ENABLE_PLOTTING
#include "common.hpp"
#include <matplot/matplot.h>
#endif

int main()
{
  smooth::feedback::test_ocp_derivatives(ocp_di);

  // target optimality
  double target_err = 1e-6;

  // define mesh
  smooth::feedback::Mesh<5, 10> mesh;

  // declare solution variable
  std::vector<decltype(ocp_di)::Solution> sols;
  std::optional<smooth::feedback::NLPSolution> nlpsol;

  const auto t0 = std::chrono::high_resolution_clock::now();

  for (auto iter = 0u; iter < 10; ++iter) {
    std::cout << "---------- ITERATION " << iter << " ----------" << std::endl;
    std::cout << "mesh: " << mesh.N_ivals() << " intervals, " << mesh.N_colloc()
              << " collocation pts" << std::endl;

    // transcribe optimal control problem to nonlinear programming problem
    const auto nlp = smooth::feedback::ocp_to_nlp<smooth::diff::Type::Analytic>(ocp_di, mesh);

    // solve nonlinear programming problem
    std::cout << "solving..." << std::endl;
    nlpsol = smooth::feedback::solve_nlp_ipopt(
      nlp,
      nlpsol,
      {
        {"print_level", 5},
      },
      {
        {"linear_solver", "mumps"},
        {"hessian_approximation", "exact"},
        // {"derivative_test", "first-order"},
        // {"derivative_test_print_all", "yes"},
        {"print_timing_statistics", "yes"},
      },
      {
        {"tol", 1e-6},
      });

    // convert solution of nlp insto solution of ocp_di
    auto sol = smooth::feedback::nlpsol_to_ocpsol(ocp_di, mesh, nlpsol.value());
    sols.push_back(sol);

    // calculate errors
    mesh.increase_degrees();
    auto errs = smooth::feedback::mesh_dyn_error(ocp_di.f, mesh, sol.t0, sol.tf, sol.x, sol.u);
    mesh.decrease_degrees();

    std::cout << "interval errors " << errs.transpose() << std::endl;

    if (errs.maxCoeff() > target_err) {
      mesh.refine_errors(errs, 0.1 * target_err);
      nlpsol = smooth::feedback::ocpsol_to_nlpsol(ocp_di, mesh, sol);
    } else {
      break;
    }
  }

  const auto dur = std::chrono::high_resolution_clock::now() - t0;

  std::cout << "TOTAL TIME: " << std::chrono::duration_cast<std::chrono::milliseconds>(dur).count()
            << "ms" << std::endl;

#ifdef ENABLE_PLOTTING
  using namespace matplot;

  const auto tt = linspace(0., sols.back().tf, 500);
  const auto tt_nodes =
    r2v(mesh.all_nodes() | std::views::transform([&](double d) { return d * sols.back().tf; }));
  const auto tt_weights = r2v(mesh.all_weights());

  figure();
  hold(on);
  plot(tt_nodes, transform(tt_nodes, [](auto) { return 0; }), "xk")->marker_size(10);
  plot(tt_nodes, tt_weights, "or")->marker_size(5);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&](double t) { return sol.x(t).x(); }), "-r")->line_width(lw);
    plot(tt, transform(tt, [&](double t) { return sol.x(t).y(); }), "-b")->line_width(lw);
  }
  matplot::legend({"nodes", "weights", "pos", "vel"});

  figure();
  hold(on);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&](double t) { return sol.lambda_dyn(t).x(); }), "-r")->line_width(lw);
    plot(tt, transform(tt, [&](double t) { return sol.lambda_dyn(t).y(); }), "-b")->line_width(lw);
  }
  matplot::legend({"lambda_x", "lambda_y"});

  figure();
  hold(on);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&](double t) { return sol.lambda_cr(t).x(); }), "-r")->line_width(lw);
  }
  matplot::legend(std::vector<std::string>{"lambda_{cr}"});

  figure();
  hold(on);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&sol](double t) { return sol.u(t).x(); }), "-r")->line_width(lw);
  }
  matplot::legend(std::vector<std::string>{"input"});

  show();
#endif

  return EXIT_SUCCESS;
}


#include <algorithm>
#include <boost/math/tools/roots.hpp>
#include <cmath>
#include <cstdlib>
#include <ibs>
#include <iostream>
#include <map>
#include <math.h>
#include <string>
#include <vector>

namespace CTESYNCH {
/*
 *****************************************************************************
 *****************************************************************************
 * FUNCTOR TO RETURN VOLTAGE * CHARGE - U0 AND DERIVATIVE AS TUPLE
 * THIS IS USED AS INPUT FOR BOOST NEWTON RAPHSON ROOT SEARCH.
 *****************************************************************************
 * Authors:
 *  - Tom Mertens
 *
 * History:
 *  - 10/08/2021 : updated version from original ste code
 *****************************************************************************
 * Arguments:
 * ----------
 *  - T const &target
 *      Target value for the voltage (RF compensate for U0 so U0 in eV)
 *  - std::vector<double> &voltages
 *      Voltages of RF systems IMPORTANT: first element is for main RF
 *  - std::vector<double> &harmonicNumbers
 *      Harmonic numbers  IMPORTANT: first element is for main RF
 *  - double charge
 *      Particle charge
 *  - T const &phi
 *      phase in rad
 ******************************************************************************
 ******************************************************************************
 */
template <class T> struct synchronousPhaseFunctor {
  synchronousPhaseFunctor(T const &target, std::vector<double> &voltages,
                          std::vector<double> &harmonicNumbers, double charge)
      : U0(target), volts(voltages), hs(harmonicNumbers), ch(charge) {}
  std::tuple<double, double> operator()(T const &phi) {

    // init
    T vrf = ch * volts[0] * sin(phi);
    T dvrf = ch * volts[0] * cos(phi);

    // add the rest taking harmonic numbers into account
    for (int i = 1; i < hs.size(); i++) {
      vrf += ch * volts[i] * sin((hs[i] / hs[0]) * phi);
      dvrf += ch * volts[i] * (hs[i] / hs[0]) * cos((hs[i] / hs[0]) * phi);
    }

    std::tuple<double, double> out = {vrf - U0, dvrf};
    return out;
  }

private:
  T U0;
  std::vector<double> volts;
  std::vector<double> hs;
  double ch;
};

/*
================================================================================
================================================================================
BOOST NEWTON RAPHSON ROOT SEARCH FOR SYNCHRONOUS PHASE.
================================================================================

================================================================================
Arguments:

//phi is in rad
================================================================================
================================================================================
*/
template <class T>
T synchronousPhaseFunctorRoot(T x, std::vector<double> &voltages,
                              std::vector<double> &harmnumbers, double charge,
                              T guess, T min, T max) {
  // return cube root of x using 1st derivative and Newton_Raphson.
  using namespace boost::math::tools;

  const int digits =
      std::numeric_limits<T>::digits; // Maximum possible binary digits accuracy
                                      // for type T.
  int get_digits = static_cast<int>(
      digits * 0.6); // Accuracy doubles with each step, so stop when we have
                     // just over half the digits correct.
  const boost::uintmax_t maxit = 20;
  boost::uintmax_t it = maxit;
  T result = newton_raphson_iterate(
      synchronousPhaseFunctor<T>(x, voltages, harmnumbers, charge), guess, min,
      max, get_digits, it);
  return result;
};

/********************************************************************************
 ********************************************************************************
 * CALCULATE TOTAL RF VOLTAGE FOR GIVEN PHASE (IN RAD)
 *
 ********************************************************************************
 */
double VoltageRf(double phi, std::vector<double> &volts,
                 std::vector<double> &hs) {
  double vrf = volts[0] * sin(phi);

  for (int i = 1; i < hs.size(); i++) {
    vrf += volts[i] * sin((hs[i] / hs[0]) * phi);
  }

  return vrf;
};

double VoltageRfPrime(double phi, double charge, std::vector<double> &volts,
                      std::vector<double> &hs) {
  // init
  double vrf = volts[0] * cos(phi);

  // add other rfs
  for (int i = 1; i < volts.size(); i++) {
    vrf += volts[i] * (hs[i] / hs[0]) * cos((hs[i] / hs[0]) * phi);
  }

  // V -> eV
  vrf *= charge;
  return vrf;
}

double SynchrotronTune(std::map<std::string, double> &twheader,
                       std::map<std::string, double> &longparams,
                       std::vector<double> &volts, std::vector<double> &hs) {

  double charge = twheader["CHARGE"];
  double phis = longparams["phis"];
  double pc = twheader["PC"];

  return sqrt(hs[0] * twheader["eta"] *
              fabs(charge * VoltageRfPrime(phis, charge, volts, hs)) /
              (2 * pi * pc * 1e9));
}
} // namespace CTESYNCH