/*
MIT License

Copyright (c) 2016 Vernam Group

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 "../include/fntru.h"
#include <math.h>
#include <NTL/mat_ZZ.h>
#include <NTL/RR.h>

////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////
// Compute sigma it is larger than sqrt(q)
long sigma(long n, long logq, long e){
	long s;

	RR n_ 		= to_RR(n);
	RR q_		= to_RR("2");	q_ = power(q_, logq);
	RR e_ 		= power(to_RR("2"), -e);//to_RR(e);
	RR s_;

	s_ = 2*n_*sqrt(log(8*n_*q_)/log(2.0));
	s_ = log(s_)/log(2.0);
	s_ = s_ + to_RR((0.5-2/e_)*logq);
	return s = to_long(s_);
}
// Create fntru
fntru::fntru(int cyclotM_, int bitsizeMod_, int radixSize_,  ZZ messagePrime_){

	n =  new ssntru(cyclotM_, 1, bitsizeMod_, 1, messagePrime_, 1);
	int sig = sigma(*n->Pointer_N(), bitsizeMod_, 80);	//Compute sigma and set

	n->SetSigma(sig);	// Set Sigma
	n->Func_SetModulus();
	n->Func_ModulusFind(1, bitsizeMod_);
	n->Func_ComputeKeys();

	radixSize	= radixSize_;	// Set radix size
	matrixDim 	= (bitsizeMod_/radixSize) + ((bitsizeMod_%radixSize == 0)? 0 : 1); // Compute matrix size = total bitsize / wordsize
/////////////////////////////////////////////////
	//Compute powers of radix
	PowTwo 		= new ZZ[matrixDim];
	ZZ c = to_ZZ("1");
	c = c << radixSize;
	PowTwo[0] = 1;
	for(int i=1; i<matrixDim; i++)
		PowTwo[i] = PowTwo[i-1]*c;
/////////////////////////////////////////////////
	//Set arrays in the matrix
	mulMat.row = new vec_ZZX[matrixDim];
	for(int i=0; i<matrixDim; i++)
		mulMat.row[i].SetLength(matrixDim);
}

//Initialize matrix
void fntru::InitializeCipher(fntru_cipher &m){
	m.row.SetLength(matrixDim);
	m.len = matrixDim;
}
//Compute the bit decomposition oof the given polynomial
void fntru::BitDecomp(vec_ZZX &BD, ZZX &c, int N, int bitlen){
	ZZX r;
	ZZ t;
	int b[radixSize], t_;
	for(int i=0; i<bitlen; i++){
		for(int j=0; j<N; j++){
			t_ = 0;
			for(int k=0; k<radixSize; k++)
				b[k] = bit(coeff(c, j), radixSize*i+k);

			for(int k=0; k<radixSize; k++)
				t_  = t_ + (b[k] << k);

			t = to_ZZ(t_);
			SetCoeff(r, j, t);
		}
		BD[i] = r;
	}
}
///////////////////////////////////////////////
// Create the polynomial by using the bit decomposition vector
void fntru::BitDecompInv(ZZX &out, vec_ZZX &BD, int bitlen){
	ZZX t;
	t = 0;
	for(int i=0; i<bitlen; i++){
		t = t + BD[i]*PowTwo[i];
	}
	out = t;
}

//////////////// FLATTEN ////////////////////////
//Take the ciphertext and apply flattening
void fntru::Flatten(fntru_cipher &c){
	Flatten(c, *n->Pointer_N(), matrixDim);
}

void fntru::Flatten(fntru_cipher &c, int &degree, int &l){
		Flatten(c.row, degree, l);
}
// Apply flattening for the vector
// First apply inverse bit decomposition and later apply bit decomposition
void fntru::Flatten(vec_ZZX &BD, int &degree, int &l){
	ZZX t;

	BitDecompInv(t, BD, l);
	for(int i=0; i<degree; i++)
		SetCoeff(t, i, coeff(t, i) %(*n->Pointer_Q(0)));

	BitDecomp(BD, t, degree, l);
}
// Apply flattening to the matrix (row by row)
void fntru::Flatten(mat_ZZX &c, int &degree, int &l){
	for(int i=0; i<l; i++){
		Flatten(c.row[i], degree, l);
	}
}
//////////////// FLATTEN ////////////////////////

void fntru::Encrypt(fntru_cipher &c, ZZX m){
	ZZX zero;
	ZZX E[matrixDim];
	mat_ZZX tempCipher;

	// Set Temp matrix
	tempCipher.row = new vec_ZZX[matrixDim];
	for(int i=0; i<matrixDim; i++)
		tempCipher.row[i].SetLength(matrixDim);

	// Encryption vector
	zero = 0;
	for(int i=0; i<matrixDim; i++)
		E[i] = n->Prim_Encrypt(zero, 0);

	// Binary Decomp of encryption vector into the temp matrix
	for(int i=0;i<matrixDim; i++)
		BitDecomp(tempCipher.row[i], E[i], *n->Pointer_N(), matrixDim);

	// add message
	for(int i=0; i<matrixDim; i++)
		tempCipher.row[i][i] = tempCipher.row[i][i]+m;

	// flatten
	Flatten(tempCipher, *n->Pointer_N(), matrixDim);

	// convert matrix into the encryption vector
	for(int i=0;i<matrixDim; i++)
		BitDecompInv(c.row[i], tempCipher.row[i], matrixDim);

	// free memory
	for(int i=0; i<matrixDim; i++)
		tempCipher.row[i].kill();
}

void fntru::Decrypt(ZZX &m, fntru_cipher &c){
	m = n->Prim_Decrypt(c.row[0], 0);
}

// if sel == 1: do l*l matrix multiplication by matrix*vector
// if sel == 0: do 1*l matrix multiplication by vector*vector
// When we are computing multiplication, we optimize it by performing
// vector*matrix multiplication. This way we do not need the flattening, since
// we do not do matrix*matrix multiplication.
//
// The function has 2 options. When we do vector=matrix*vector, it results in
// a ciphertext vector. All the elements (ntru ciphers) inside fntru_cipher is computed.
// In case of vector=vector*vector, we only compute the first row (first ntru cipher).
// This way we reduce the computation by a factor of matrixDim (number of ciphertexts inside fntru_cipher)
// If the second option is used it destroys the structure of fntru_cipher. Only first element
// protects its ciphertext form. Therefore, the user should use that as the output while evaluating all the circuit.
void fntru::Mult(fntru_cipher &c, fntru_cipher &a, fntru_cipher &b, int sel){
	if(sel == 1){
		//Bit decompose all the rows of the operand to turn it into a matrix
		for(int i=0; i<matrixDim; i++)
			BitDecomp( mulMat.row[i], a.row[i], *n->Pointer_N(), matrixDim);
		// compute matrix multiplication
		MatrixVectorMul(c.row, mulMat, b.row, matrixDim, *n);

		PolyReduce(c, matrixDim, *n);
		CoeffReduce(c, matrixDim, *n);
		// clear temp matrix
		for(int i=0; i<matrixDim; i++)
			for(int j=0; j<matrixDim; j++)
				mulMat.row[i][j]=0;
	}
	else {
		// Bit decompose only first ciphertext
		BitDecomp( mulMat.row[0], a.row[0], *n->Pointer_N(), matrixDim);
		VectorDotProd(c.row[0], mulMat.row[0], b.row, matrixDim, *n);

		// !!!TODO: Add a function for row
		n->Arith_PolyReduce(c.row[0], c.row[0]);
		for(int j=0; j< (*n->Pointer_N()); j++)
			SetCoeff(c.row[0], j, coeff(c.row[0], j) % (*n->Pointer_Q(0)));
		// Clear Matrix
		for(int j=0; j<matrixDim; j++)
			mulMat.row[0][j]=0;
	}
}

//////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////
//Compute matrix multiplication
void MatrixMul(mat_ZZX &c, mat_ZZX &a, mat_ZZX &b, int &vectorLength, ssntru &n){
	ZZX t;
	for(int i=0; i<vectorLength; i++){
		for(int j=0; j<vectorLength; j++){
			t = 0;
			for(int k=0; k<vectorLength; k++)
				t = t + a.row[i][k]*b.row[k][j];

			n.Arith_PolyReduce(t,t);
			c.row[i][j] = t;
		}
	}
}

// Compute the dot product of two vectors. In other words, we compute multiplication
// between the first row of the matrix and the input ciphertext vector.
// If thread is defined it uses NTL thread routine to compute the multiplications.
void VectorDotProd(ZZX &c, vec_ZZX &a, vec_ZZX &b, int &vectorLength, ssntru &n){
#ifdef Thread
	ZZX tt[vectorLength];
	PartitionInfo pinfo(vectorLength);
	long cnt = pinfo.NumIntervals();
	NTL_EXEC_INDEX(cnt, index)
			long first, last;
			pinfo.interval(first, last, index);
			for (int k = first; k<last; k++) {
				mul(tt[k], a[k], b[k]);
			}
	NTL_EXEC_INDEX_END

	c = 0;
	for (int k = 0; k<vectorLength; k++)
		c = c + tt[k];
#endif

#ifndef Thread
	c = 0;
	ZZX tt;
	for (int k = 0; k<vectorLength; k++){
		mul(tt, a[k], b[k]);
//		mul(tt, a[k], b[k]);
		c=c+tt;
	}
#endif
}

//Matrix vector multiplication. TODO: Check 3 options for which one is more optimized for speed and area.
void MatrixVectorMul(vec_ZZX &c, mat_ZZX &a, vec_ZZX &b, int &vectorLength, ssntru &n){

#ifdef Thread
#ifdef MULT1
	ZZX acc, tmp;

	ZZX t, t2;
	ZZX tt[vectorLength];

	for (int i = 0; i<vectorLength; i++) {
		//clear(acc);
		acc = 0;

		PartitionInfo pinfo(vectorLength);
		long cnt = pinfo.NumIntervals();

		NTL_EXEC_INDEX(cnt, index)
			long first, last;
			pinfo.interval(first, last, index);
			for (int k = first; k<last; k++) {
				mul(tt[k], a.row[i][k], b[k]);
			}
		NTL_EXEC_INDEX_END

		acc = 0;
		for(int j=0; j<vectorLength; j++)
			acc = acc + tt[j];
		c[i] = acc;
	}
#endif
#ifdef MULT2
	ZZX acc, tmp;

	ZZX t, t2;
	ZZX tt[vectorLength];

	myTimerReal tr;
	double multtime = 0, addtime = 0;
	for (int i = 0; i<vectorLength; i++) {
		//clear(acc);
		acc = 0;
		start(tr);
		PartitionInfo pinfo(vectorLength);
		long cnt = pinfo.NumIntervals();

		NTL_EXEC_INDEX(cnt, index)
			long first, last;
			pinfo.interval(first, last, index);
			for (int k = first; k<last; k++) {
				mul(tt[k], a.row[i][k], b[k]);
			}
		NTL_EXEC_INDEX_END
		stop(tr);
		multtime += getseconds(tr);
		acc = 0;
		start(tr);
		for(int j=0; j<vectorLength; j++)
			acc = acc + tt[j];
		c[i] = acc;
		stop(tr);
		addtime += getseconds(tr);
	}

#endif
#ifdef MULT3
	ZZX tmp;

	ZZX t, t2;
	ZZX acc[vectorLength], tt[vectorLength];

	for(int j=0; j<vectorLength; j++){
		acc[j] = 0;
		c[j] = 0;
	}

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

		PartitionInfo pinfo(vectorLength);
		long cnt = pinfo.NumIntervals();

		NTL_EXEC_INDEX(cnt, index)
			long first, last;
			pinfo.interval(first, last, index);
			for (int k = first; k<last; k++) {
				mul(tt[k], a.row[k][i], b[i]);
			}
		NTL_EXEC_INDEX_END

		for(int j=0; j<vectorLength; j++)
			c[j] = c[j] + tt[j];

//		for(int j=0; j<vectorLength; j++)
//			c[j] = c[j] + acc[j];
	}
#endif
#endif

#ifndef Thread
	ZZX acc, tmp;
	ZZX t, t2;
	ZZX tt[vectorLength];

	for (int i = 0; i<vectorLength; i++) {
		acc = 0;
		for (int k = 0; k<vectorLength; k++)
			mul(tt[k], a.row[i][k], b[k]);

		acc = 0;
		for(int j=0; j<vectorLength; j++)
			acc = acc + tt[j];
		c[i] = acc;
	}

#endif
}
// Coefficient modulus reduction for all ciphertexts in fntru ciphertext
void CoeffReduce(fntru_cipher &c, int &vectorLength, ssntru &n){
	for(int i=0; i<vectorLength; i++)
		for(int j=0; j< (*n.Pointer_N()); j++)
			SetCoeff(c.row[i], j, coeff(c.row[i], j) % (*n.Pointer_Q(0)));
}
// Polynomial modulus reduction for all ciphertexts in fntru ciphertext
void PolyReduce(fntru_cipher &c, int &vectorLength, ssntru &n){
	for(int i=0; i<vectorLength; i++)
		n.Arith_PolyReduce(c.row[i], c.row[i]);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////

void fntru::AND(fntru_cipher &c, fntru_cipher &a, fntru_cipher &b, int sel){
	Mult(c, a, b, sel);
}

void fntru::XOR(fntru_cipher &c, fntru_cipher &a, fntru_cipher &b, int sel){

	if(sel == 1){
		ZZX t[a.len];
		for(int i=0; i<a.len; i++)
			t[i] = a.row[i] + b.row[i];

		Mult(c, a, b, 1);
		for(int i=0; i<a.len; i++)
			c.row[i] = t[i] - 2*c.row[i];

		PolyReduce(c, c.len, *n);
		CoeffReduce(c, c.len, *n);
	}
	else if(sel == 0){
		ZZX t;
		int vecSize = 1;
		t = a.row[0] + b.row[0];
		Mult(c, a, b, 0);
		c.row[0] = t - 2*c.row[0];
		PolyReduce(c, vecSize, *n);
		CoeffReduce(c, vecSize, *n);
	}
}

void fntru::OR(fntru_cipher &c, fntru_cipher &a, fntru_cipher &b, int sel){

	if(sel == 1){
		ZZX t[a.len];
		for(int i=0; i<a.len; i++)
			t[i] = a.row[i] + b.row[i];

		Mult(c, a, b, 1);
		for(int i=0; i<a.len; i++)
			c.row[i] = t[i] - c.row[i];

		PolyReduce(c, c.len, *n);
		CoeffReduce(c, c.len, *n);
	}
	else if(sel == 0){
		ZZX t;
		int vecSize = 1;
		t = a.row[0] + b.row[0];
		Mult(c, a, b, 0);
		c.row[0] = t - c.row[0];
		PolyReduce(c, vecSize, *n);
		CoeffReduce(c, vecSize, *n);
	}
}

void fntru::NOT(fntru_cipher &c, fntru_cipher &a, int sel){

	if(sel == 1){
		ZZX two;
		two = to_ZZ("1");
		for(int i=0; i<a.len; i++){
			c.row[i] = two - a.row[i];
			two = two << 1;
		}
		CoeffReduce(c, c.len, *n);
	}
	else if(sel == 0){
		int vecSize = 1;
		c.row[0] = to_ZZ("1") - a.row[0];
		CoeffReduce(c, vecSize, *n);
	}
}

void fntru::NAND(fntru_cipher &c, fntru_cipher &a, fntru_cipher &b, int sel){

	Mult(c, a, b, sel);
	if(sel == 1){
		ZZX two;
		two = to_ZZ("1");
		for(int i=0; i<a.len; i++){
			c.row[i] = two - c.row[i];
			two = two << 1;
		}
		CoeffReduce(c, c.len, *n);
	}
	else if(sel == 0){
		int vecSize = 1;
		c.row[0] = to_ZZ("1") - c.row[0];
		CoeffReduce(c, vecSize, *n);
	}
}


#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

#include <gtest/gtest.h>
#include <array>

TEST(Eigen, TensorSlice) {
	Eigen::Tensor<double, 3> m(3, 10, 10);          //Initialize
	m.setRandom();                               //Set random values
	std::array<long, 3> offset = { 0,0,0 };         //Starting point
	std::array<long, 3> extent = { 1,10,10 };       //Finish point
	std::array<long, 2> shape2 = { 10,10 };         //Shape of desired rank-2 tensor (matrix)
	std::cout << m.slice(offset, extent).reshape(shape2) << std::endl;  //Extract slice and reshape it into a 10x10 matrix.
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// With contributions from Cornelius Steinhardt

#include <cstdlib>
#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/operation/cuppen.hpp>

using namespace std;

const double 			tol= 1.0e-5;

template <typename Matrix, typename Value, typename Vector>
void test_vector(const Matrix& A, const Value& alpha, const Vector& v, int i)
{
    Vector diff(A*v-alpha*v), v1(A*v), v2(alpha*v); // , diff(v1-v2);
    if (size(v1) < 17)
	cout << "A*v is     " << v1 << "\nalpha*v is " << v2 << '\n';
    if (two_norm(diff) > tol) cout << "two_norm(difference) of the " << i << "-th eigenvector is " << two_norm(diff) << '\n'; // throw "wrong eigenvector";
}

template <typename Matrix, typename Value, typename Vector>
void test(const Matrix& B, Matrix& BQ, Value scaling, Vector& lambda_b)
{
    if (num_rows(B) <= 20)
	std::cout << "B=\n" << B << "\n";

    mtl::dense_vector<double> eig_b= eigenvalue_symmetric(B,22);
    sort(eig_b);
    eig_b*= scaling;
    std::cout<<"eigenvalues with QR ="<< eig_b <<"\n";

    cuppen(B, BQ, lambda_b);
    lambda_b*= scaling;
    // std::cout<<"B  =\n"<< B <<"\n";
    if (num_rows(B) <= 20)
	std::cout<<"Q  =\n"<< BQ <<"\n";
    std::cout<<"eigenvalues with Cuppen ="<< lambda_b <<"\n";

    eig_b-= lambda_b;
    std::cout<<"two_norm(diff)  ="<< two_norm(eig_b) <<"\n";
    MTL_THROW_IF(two_norm(eig_b) > tol, mtl::runtime_error("Cuppen computes wrong eigenvalues"));

    Matrix Bs(scaling * B);
    for (unsigned i= 0; i < num_rows(B); i++)
	test_vector(Bs, lambda_b[i], mtl::dense_vector<double>(BQ[mtl::iall][i]), i);
}

int main(int argc, char** argv)
{
    using namespace mtl;
    int size= 16;
    if (argc > 1) size= atoi(argv[1]);

    dense_vector<double>        eig, lam(2), lambda(4),  lambda_b(size), eig_b(size);
#if 1
    dense2D<double> Mini(2, 2), QM(2, 2);
    Mini= 1, 2, 2, -11;
    cuppen(Mini, QM, lam);

    std::cout << "eigenvalues of Mini  =" << lam <<"\n";
    std::cout << "eigenvectors of Mini are\n" << QM;
#endif

    double array[][4]= {{1,  2,   0,  0},
                        {2, -9,  -2,  0},
                        {0, -2,   1,  3},
                        {0,  0,   3, 10}};
    dense2D<double> A(array), Q(4,4);
    std::cout << "A=\n" << A << "\n";

    eig= eigenvalue_symmetric(A,22);
    sort(eig);
    std::cout<<"eigenvalues  ="<< eig <<"\n";

    cuppen(A, Q, lambda);
    std::cout<<"A  =\n"<< A <<"\n";
    std::cout<<"Q  =\n"<< Q <<"\n";
    std::cout<<"eigenvalues  ="<< lambda <<"\n";

    eig-= lambda;
    std::cout<<"two_norm(diff)  ="<< two_norm(eig) <<"\n";
    MTL_THROW_IF(two_norm(eig) > tol, mtl::runtime_error("Cuppen computes wrong eigenvalues"));

    for (unsigned i= 0; i < num_rows(A); i++)
	test_vector(A, lambda[i], dense_vector<double>(Q[iall][i]), i);

    dense2D<double> B(size,size), BQ(size,size);
    B= 0; BQ= 0;

    const double scale= 64.0 / double(size);
    for(int i= 1; i < size ; i++){
      B[i][i]= scale*i+6;
      B[i][i-1]= 1;
      B[i-1][i]= 1;
    }
    B[0][0]= 6;

    test(B, BQ, 1.0, lambda_b);

    const double maxv= B[size-1][size-1];
    B/= maxv;
    test(B, BQ, maxv, lambda_b);

#if 0
    // Poisson equation cannot be solved, double eigenvalues are now correctly handled by the secular equation
    // but Q_tilde in cuppen contains nans (0/0)
    int lsize= 4;
    if (argc > 1) lsize= atoi(argv[1]);

    dense2D<double> C(lsize, lsize), CQ(lsize, lsize);
    C= 0; CQ= 0;

    C[0][0]= 2;
    for(int i= 1; i < lsize; i++) {
	C[i][i]= 2;
	C[i][i-1]= -1;
	C[i-1][i]= -1;
    }
    cout << "The matrix of the 1D-Poisson equations I\n" << C << '\n';

    dense_vector<double> lambda_c(lsize);
    cuppen(C, CQ, lambda_c);

    if (lsize <= 100)
	cout << "The eigenvalues of the 1D-Poisson equations are " << lambda_c << '\n';
    if (lsize <= 20)
	cout << "The eigenvectors of the 1D-Poisson equations are\n" << CQ << '\n';

    for (unsigned i= 0; i < num_rows(C); i++);
	//test_vector(C, lambda_c[i], dense_vector<double>(CQ[iall][i]));
#endif

    return 0;
}


#include <shift/math/utility.hpp>
#include <shift/core/boost_disable_warnings.hpp>
#include <boost/test/unit_test.hpp>
#include <shift/core/boost_restore_warnings.hpp>

using namespace shift::math;

BOOST_AUTO_TEST_CASE(utility_almost_equal)
{
  const auto value1 = 0.2;
  const auto value2 = 1.0 / std::sqrt(5.0) / std::sqrt(5.0);

  BOOST_CHECK_NE(value1, value2);
  BOOST_CHECK(almost_equal(value1, value2));
}

BOOST_AUTO_TEST_CASE(utility_is_power_of_two)
{
  static_assert(!is_power_of_two(0x00000000u), "Error in is_power_of_two");
  static_assert(is_power_of_two(0x00000001u), "Error in is_power_of_two");
  static_assert(is_power_of_two(0x00000002u), "Error in is_power_of_two");
  static_assert(!is_power_of_two(0x00000003u), "Error in is_power_of_two");
  static_assert(is_power_of_two(0x80000000u), "Error in is_power_of_two");
}

BOOST_AUTO_TEST_CASE(utility_next_power_of_two)
{
  static_assert(next_power_of_two(0x00000100u) == 0x00000200u,
                "Error in next_power_of_two.");
  static_assert(next_power_of_two(0x00001001u) == 0x00002000u,
                "Error in next_power_of_two.");
  static_assert(next_power_of_two(0x00110111u) == 0x00200000u,
                "Error in next_power_of_two.");
  static_assert(next_power_of_two(0x01ffffffu) == 0x02000000u,
                "Error in next_power_of_two.");
  static_assert(next_power_of_two(0x40000000u) == 0x80000000u,
                "Error in next_power_of_two.");
  static_assert(next_power_of_two(0x00000000u) == 0x00000001u,
                "Error in next_power_of_two.");
  static_assert(next_power_of_two(0x80000000u) == 0x00000000u,
                "Error in next_power_of_two.");
}

template <typename T>
void test_NaNs()
{
  if constexpr (std::numeric_limits<T>::has_quiet_NaN)
    BOOST_CHECK(is_nan(quiet_nan<T>()));
  else
    BOOST_CHECK(!is_nan(quiet_nan<T>()));

  if constexpr (std::numeric_limits<T>::has_signaling_NaN)
    BOOST_CHECK(is_nan(signaling_nan<T>()));
  else
    BOOST_CHECK(!is_nan(signaling_nan<T>()));

  if constexpr (std::numeric_limits<T>::has_quiet_NaN &&
                std::numeric_limits<T>::has_signaling_NaN)
  {
    // Comparison of any value with a NaN value returns false.
    BOOST_CHECK_NE(quiet_nan<T>(), T{0});
    BOOST_CHECK_NE(signaling_nan<T>(), T{0});

    // Comparison of two identical NaN values should also return false.
    /// ToDo: Visual C++ fails here. Find the reason why.
    BOOST_CHECK_NE(quiet_nan<T>(), quiet_nan<T>());
    BOOST_CHECK_NE(signaling_nan<T>(), signaling_nan<T>());

    // Comparison of different NaN values should return false as well.
    BOOST_CHECK_NE(quiet_nan<T>(), signaling_nan<T>());

    // Signalling and quiet NaN values should be different in binary
    // representation.
    auto qnan = quiet_nan<T>();
    auto snan = signaling_nan<T>();
    BOOST_CHECK(std::memcmp(&qnan, &snan, sizeof(T)) != 0);
  }
}

BOOST_AUTO_TEST_CASE(utility_float_NaNs)
{
  test_NaNs<float>();
  test_NaNs<double>();
  test_NaNs<long double>();
}

BOOST_AUTO_TEST_CASE(utility_integer_NaNs)
{
  test_NaNs<char>();
  test_NaNs<short>();
  test_NaNs<int>();
}

BOOST_AUTO_TEST_CASE(utility_literals)
{
  using namespace shift::math::literals;

  BOOST_CHECK_EQUAL(static_cast<float>(180_deg), pi<float>);
  BOOST_CHECK_EQUAL(180_deg, pi<double>);
  BOOST_CHECK_EQUAL(180_fdeg, pi<float>);

  BOOST_CHECK_EQUAL(static_cast<float>(180.0_deg), pi<float>);
  BOOST_CHECK_EQUAL(180.0_deg, pi<double>);
  BOOST_CHECK_EQUAL(180.0_fdeg, pi<float>);
}


#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Core>

#include <sophus/se3.h>
#include <sophus/so3.h>

#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

using namespace std;
using Sophus::SE3;
using Sophus::SO3;

/************************************************
 * 本程序演示如何用 gtsam 进行位姿图优化
 * sphere.g2o 是人工生成的一个 Pose graph，我们来优化它。
 * 与 g2o 相似，在 gtsam 中添加的是因子，相当于误差
 * **********************************************/

int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"Usage: pose_graph_gtsam sphere.g2o"<<endl;
        return 1;
    }
    ifstream fin ( argv[1] );
    if ( !fin )
    {
        cout<<"file "<<argv[1]<<" does not exist."<<endl;
        return 1;
    }

    // TO-DO：使用gtsam的因子图优化方法
    gtsam::NonlinearFactorGraph::shared_ptr graph ( new gtsam::NonlinearFactorGraph );  // gtsam的因子图
    gtsam::Values::shared_ptr initial ( new gtsam::Values ); // 初始值
    // 从g2o文件中读取节点和边的信息
    int cntVertex=0, cntEdge = 0;
    cout<<"reading from g2o file"<<endl;

    while ( !fin.eof() )
    {
        string tag;
        fin>>tag;
        if ( tag == "VERTEX_SE3:QUAT" )
        {
            // 顶点
            gtsam::Key id;
            fin>>id;
            double data[7];
            for ( int i=0; i<7; i++ ) fin>>data[i];
            // 转换至gtsam的Pose3
            gtsam::Rot3 R = gtsam::Rot3::Quaternion ( data[6], data[3], data[4], data[5] );
            gtsam::Point3 t ( data[0], data[1], data[2] );
            initial->insert ( id, gtsam::Pose3 ( R,t ) );       // 添加初始值
            cntVertex++;
        }
        else if ( tag == "EDGE_SE3:QUAT" )
        {
            // 边，对应到因子图中的因子
            gtsam::Matrix m = gtsam::I_6x6;     // 信息矩阵
            gtsam::Key id1, id2;
            fin>>id1>>id2;
            double data[7];
            for ( int i=0; i<7; i++ ) fin>>data[i];
            gtsam::Rot3 R = gtsam::Rot3::Quaternion ( data[6], data[3], data[4], data[5] );
            gtsam::Point3 t ( data[0], data[1], data[2] );
            for ( int i=0; i<6; i++ )
                for ( int j=i; j<6; j++ )
                {
                    double mij;
                    fin>>mij;
                    m ( i,j ) = mij;
                    m ( j,i ) = mij;
                }

            // g2o的信息矩阵定义方式与gtsam不同，这里对它进行修改
            gtsam::Matrix mgtsam = gtsam::I_6x6;
            mgtsam.block<3,3> ( 0,0 ) = m.block<3,3> ( 3,3 ); // cov rotation
            mgtsam.block<3,3> ( 3,3 ) = m.block<3,3> ( 0,0 ); // cov translation
            mgtsam.block<3,3> ( 0,3 ) = m.block<3,3> ( 0,3 ); // off diagonal
            mgtsam.block<3,3> ( 3,0 ) = m.block<3,3> ( 3,0 ); // off diagonal

            // 单独书写噪声模型
            gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information ( mgtsam );        // 高斯噪声模型
            gtsam::NonlinearFactor::shared_ptr factor (
                new gtsam::BetweenFactor<gtsam::Pose3> ( id1, id2, gtsam::Pose3 ( R,t ), model ) // 添加一个因子
            );
            graph->push_back ( factor );
            cntEdge++;
        }
        if ( !fin.good() )
            break;
    }

    cout<<"read total "<<cntVertex<<" vertices, "<<cntEdge<<" edges."<<endl;
    // 固定第一个顶点，在gtsam中相当于添加一个先验因子
    gtsam::NonlinearFactorGraph graphWithPrior = *graph;
    gtsam::noiseModel::Diagonal::shared_ptr priorModel =
        gtsam::noiseModel::Diagonal::Variances (
            ( gtsam::Vector ( 6 ) <<1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6 ).finished()
        );
    gtsam::Key firstKey = 0;
    for ( const gtsam::Values::ConstKeyValuePair& key_value: *initial )
    {
        cout<<"Adding prior to g2o file "<<endl;
        // prior先验因子
        graphWithPrior.add ( gtsam::PriorFactor<gtsam::Pose3> (
            key_value.key, key_value.value.cast<gtsam::Pose3>(), priorModel )
        );
        break;
    }

    // 开始因子图优化，配置优化选项
    cout<<"optimizing the factor graph"<<endl;
    // 我们使用 LM 优化
    gtsam::LevenbergMarquardtParams params_lm;
    params_lm.setVerbosity("ERROR");
    params_lm.setMaxIterations(20); // 事实上5次就收敛了
    params_lm.setLinearSolverType("MULTIFRONTAL_QR");
    gtsam::LevenbergMarquardtOptimizer optimizer_LM( graphWithPrior, *initial, params_lm );

    // 你可以尝试下 GN
    // gtsam::GaussNewtonParams params_gn;
    // params_gn.setVerbosity("ERROR");
    // params_gn.setMaxIterations(20);
    // params_gn.setLinearSolverType("MULTIFRONTAL_QR");
    // gtsam::GaussNewtonOptimizer optimizer ( graphWithPrior, *initial, params_gn );

    gtsam::Values result = optimizer_LM.optimize();
    cout<<"Optimization complete"<<endl;
    cout<<"initial error: "<<graph->error ( *initial ) <<endl;
    cout<<"final error: "<<graph->error ( result ) <<endl;

    cout<<"done. write to g2o ... "<<endl;
    // 写入 g2o 文件，同样伪装成 g2o 中的顶点和边，以便用 g2o_viewer 查看。
    // 顶点
    ofstream fout ( "result_gtsam.g2o" );
    for ( const gtsam::Values::ConstKeyValuePair& key_value: result )
    {
        gtsam::Pose3 pose = key_value.value.cast<gtsam::Pose3>();
        gtsam::Point3 p = pose.translation();
        gtsam::Quaternion q = pose.rotation().toQuaternion();
        fout<<"VERTEX_SE3:QUAT "<<key_value.key<<" "
            <<p.x() <<" "<<p.y() <<" "<<p.z() <<" "
            <<q.x()<<" "<<q.y()<<" "<<q.z()<<" "<<q.w()<<" "<<endl;
    }
    // 边
    for ( gtsam::NonlinearFactor::shared_ptr factor: *graph )
    {
        gtsam::BetweenFactor<gtsam::Pose3>::shared_ptr f = dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose3>>( factor );
        if ( f )
        {
            gtsam::SharedNoiseModel model = f->noiseModel();
            gtsam::noiseModel::Gaussian::shared_ptr gaussianModel = dynamic_pointer_cast<gtsam::noiseModel::Gaussian>( model );
            if ( gaussianModel )
            {
                // write the edge information
                gtsam::Matrix info = gaussianModel->R().transpose() * gaussianModel->R();
                gtsam::Pose3 pose = f->measured();
                gtsam::Point3 p = pose.translation();
                gtsam::Quaternion q = pose.rotation().toQuaternion();
                fout<<"EDGE_SE3:QUAT "<<f->key1()<<" "<<f->key2()<<" "
                    <<p.x() <<" "<<p.y() <<" "<<p.z() <<" "
                    <<q.x()<<" "<<q.y()<<" "<<q.z()<<" "<<q.w()<<" ";
                gtsam::Matrix infoG2o = gtsam::I_6x6;
                infoG2o.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
                infoG2o.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
                infoG2o.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal
                infoG2o.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
                for ( int i=0; i<6; i++ )
                    for ( int j=i; j<6; j++ )
                    {
                        fout<<infoG2o(i,j)<<" ";
                    }
                fout<<endl;
            }
        }
    }
    fout.close();
    cout<<"done."<<endl;
}


//
// Created by haohanwang on 1/24/16.
//

#include "LinearRegression.hpp"

#include <Eigen/Dense>
#include <stdexcept>
#include <iostream>
#include <unordered_map>

#ifdef BAZEL
#include "Models/ModelOptions.hpp"
#else
#include "ModelOptions.hpp"
#endif

using namespace Eigen;
using namespace std;

LinearRegression::LinearRegression() {
    L1_reg = default_L1_reg;
    L2_reg = default_L2_reg;
    betaAll = MatrixXf::Ones(1,1);
    logisticFlag = false;
};

LinearRegression::LinearRegression(const unordered_map<string, string>& options) {
    try {
        L1_reg = stof(options.at("lambda"));
    } catch (std::out_of_range& oor) {
        L1_reg = default_L1_reg;
    }
    try {
        L2_reg = stof(options.at("L2_lambda"));
    } catch (std::out_of_range& oor) {
        L2_reg = default_L2_reg;
    }
    betaAll = MatrixXf::Ones(1,1);
    logisticFlag = false;
};

void LinearRegression::assertReadyToRun() {
    // X and y matrices must be initialized with the same number of rows for a LR run to make sense.
    const bool ready = ((X.rows() > 0) && (X.rows() == y.rows())
                     && (X.cols() > 0) && (y.cols() > 0));
    const string err_str = "X and Y matrices of size (" + to_string(X.rows()) + "," + to_string(X.cols()) + "), and ("
            + to_string(y.rows()) + "," + to_string(y.cols()) + ") are not compatible.";
    checkLogisticRegression();
    if (!ready) {
        //throw runtime_error(err_str);
    }
}

void LinearRegression::setL1_reg(float l1) { L1_reg = l1; };

void LinearRegression::setL2_reg(float l2) { L2_reg = l2; };

float LinearRegression::cost() {
    if (logisticFlag){
        return 0.5 * (y - (X * beta).unaryExpr(&sigmoid)).squaredNorm()/X.rows() + L1_reg * beta.lpNorm<1>() + L2_reg * beta.squaredNorm();
    }
    else{
        return 0.5 * (y - X * beta).squaredNorm()/X.rows() + L1_reg * beta.lpNorm<1>() + L2_reg * beta.squaredNorm();
    }
};

MatrixXf LinearRegression::derivative() {
    return ((-1.0 * X.transpose() * (y - X * beta)).array() + L1_reg * (beta.array() / beta.cwiseAbs().array()).sum() +
           L2_reg * beta.sum()).matrix();
};

MatrixXf LinearRegression::proximal_derivative() {
    if (logisticFlag){
        return -1.0 * X.transpose() * (y - (X * beta).unaryExpr(&sigmoid));
    }
    else{
        return -1.0 * X.transpose() * (y - X * beta);
    }
};

MatrixXf LinearRegression::proximal_operator(VectorXf in, float lr) {
    if (L1_reg == 0 && L2_reg == 0){
        return in;
    }
    if (L1_reg != 0 && L2_reg == 0){
        VectorXf sign = ((in.array()>0).matrix()).cast<float>();//sign
        sign += -1.0*((in.array()<0).matrix()).cast<float>();
        in = ((in.array().abs()-lr*L1_reg).max(0)).matrix();//proximal
        return (in.array()*sign.array()).matrix();//proximal multipled back with sign
    }
    else if (L2_reg != 0){
        return in/(1+2*lr*L2_reg);
    }
    else{
        VectorXf sign = ((in.array()>0).matrix()).cast<float>();
        sign += -1.0*((in.array()<0).matrix()).cast<float>();
        in = ((in.array().abs()-lr*L1_reg).max(0)).matrix();
        in = in.array()*sign.array()/(1+2*lr*L2_reg);
        return in.matrix();
    }
}

void LinearRegression::updateBetaAll(MatrixXf b) {
    if (betaAll.rows() == 1){
        betaAll = b;
    }
    else{
        betaAll.conservativeResize(betaAll.rows(),betaAll.cols()+1);
        betaAll.col(betaAll.cols()-1) = b;
    }
}

MatrixXf LinearRegression::getBetaAll() {
    return betaAll;
}

float LinearRegression::getL1_reg() {
    return L1_reg;
}

float LinearRegression::getL2_reg() {
    return L2_reg;
}


// Standard includes
#include <fstream>
#include <iostream>
#include <sstream>
#include <algorithm>
#include <string>
#include <vector>

// For date parsing
#include <boost/date_time/local_time/local_time.hpp>

// For converting WGS84 <-> LTP coordinates
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>

// Main entry point
int main(int argc, char* argv[])
{
	// We are in science here :)
	std::cout.precision(16);

	// Base station coordinates (Friday)
	double org_lat = 51.710979902;
	double org_lon = -0.210839049;
	double org_alt = 141.1027;

	// Setup a converter
	GeographicLib::Geocentric wgs84_ecef(
		GeographicLib::Constants::WGS84_a(),
		GeographicLib::Constants::WGS84_f()
	);
	GeographicLib::LocalCartesian   wgs84_enu(
		org_lat,
		org_lon,
		org_alt,
		wgs84_ecef
	);

	// Argument check
	if (argc < 2)
	{
		std::cout << "Usage: " << argv[0] << " <file> " << std::endl;
		return 1;
	}

	std::ifstream 	namefile(argv[1]);
	std::string 	input;
	int counter = 0;
	int line    = 0;

	// Eat up the heading
	for (int i = 0; i < 15; i++)
		namefile >> input;

  	boost::posix_time::ptime epoch(boost::gregorian::date(1970,1,1));

	// Now process the data
	std::string d, t;
	double x, y, z;
	double lat, lon, alt, en, ee, eu, ede, edn, edu, age, ratio;
	int q, ns;
	while(namefile >> d >> t >> lat >> lon >> alt >> q >> ns >> en >> ee >> eu >> edn >> ede >> edu >> age >> ratio)
	{
		// Extract a meaningful value in seconds
	    std::stringstream ss;
	    boost::local_time::local_date_time ldt(boost::local_time::not_a_date_time);
	    boost::local_time::local_time_input_facet* input_facet = new boost::local_time::local_time_input_facet();
	    input_facet->format("%Y/%m/%d %H:%M:%s");
	    ss.imbue(std::locale(ss.getloc(), input_facet));
		ss << d << " " << t;
 		ss >> ldt;

 		boost::posix_time::ptime a = ldt.utc_time();
    	boost::posix_time::ptime epoch(boost::gregorian::date(1970,1,1));
    	double ms = (a - epoch).total_milliseconds();
    	ms /= 1e3;

 		// Convert value to LTP
		wgs84_enu.Forward(
			lat,
			lon,
			alt,
			x,
			y,
			z
		);

		// Print seconds, WGS84 and LTP
		std::cout << ms << "," << lat << "," << lon << "," << alt << "," << x << "," << y  << "," << z << std::endl;
	}

	// Success
	return 0;
}


#pragma once

#include "modprop/compo/Interfaces.h"

#include <Eigen/Cholesky>
#include <iostream>

namespace percepto
{

// TODO Support other types of decompositions
template <typename Data>
class LogDeterminantCost
: public Source<double>
{
public:

	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	typedef Data InputType;
	typedef Source<Data> InputSourceType;
	typedef Sink<Data> SinkType;
	typedef Source<double> OutputSourceType;

	LogDeterminantCost()
	: _input( this ), _scale( 1.0 ), _initialized( false ) {}

	LogDeterminantCost( const LogDeterminantCost& other )
	: _input( this ), _scale( other._scale ), _initialized( other._initialized ),
	_dody( other.dody ) {}

	void SetSource( InputSourceType* r ) { r->RegisterConsumer( &_input ); }
	void SetScale( double s ) { _scale = s; }

	virtual unsigned int OutputDim() const { return 1; }

	virtual void Foreprop()
	{
		InputType input = _input.GetInput();
		if( input.rows() != input.cols() )
		{
			throw std::invalid_argument( "LogDeterminantCost: Input must be square." );
		}

		_initialized = false;

		_solver = _solver.compute( input );
		VectorType d( _solver.vectorD() );
		if( !( d.array() > 0.0 ).all() )
		{
			std::cout << "in: " << input << std::endl;
			std::cout << "D: " << d.transpose() << std::endl;
			throw std::invalid_argument( "LogDeterminantCost: Input determinant must be positive." );
		}
		double logdet = 0;
		for( unsigned int i = 0; i < _solver.vectorD().size(); ++i )
		{
			logdet += std::log( _solver.vectorD()(i) );;
		}
		OutputSourceType::SetOutput( logdet );
		OutputSourceType::Foreprop();
	}

	virtual void BackpropImplementation( const MatrixType& nextDodx )
	{
		if( !_initialized )
		{
			InputType input = _input.GetInput();
			InputType inputInv = _solver.solve( MatrixType::Identity( input.rows(), input.cols() ) );
			std::cout << "input: " << input << std::endl;
			std::cout << "inputInv: " << inputInv << std::endl;
			_dody = Eigen::Map<MatrixType>( inputInv.data(), 1, inputInv.size() );
			_initialized = true;
		}

		if( nextDodx.size() == 0 )
		{
			_input.Backprop( _dody );
		}
		else
		{
			std::cout << "thisDodx: " << nextDodx * _dody << std::endl;
			_input.Backprop( nextDodx * _dody );
		}
	}

private:

	typedef Eigen::LDLT<Data> SolverType;

	SinkType _input;
	double _scale;
	bool _initialized;
	MatrixType _dody;
	SolverType _solver;
};

}


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-ieee
    This function object returns ulp distance between its arguments.

    It is often difficult to answer to the following question: "are
    these two floating computations results similar enough?". The
    ulpdist is a way to answer which is tuned for relative errors
    estimations and peculiarly adapted to cope with the limited bits
    accuracy of floating point representations.

    @par Header <boost/simd/function/ulpdist.hpp>

    @par Notes

    - If the common type is integral  @c ulpdist is the same as @c dist

    - If the common type is floating point the ulpdist is is computed,
    by the above described method.

    The method is the following:

     - If one and only one of the parameters is @ref Nan the result is @ref Nan,
     if both are Nans the result is @ref Zero

     - Else, properly normalize the two numbers by the same factor in a way
     that the largest of the two numbers exponents will be brought to
     zero

     - Return the absolute difference of these normalized numbers
      divided by the rounding error Eps

    The rounding error is the ulp (unit in the last place) value, i.e. the
    floating number, the exponent of which is 0 and the mantissa is all zeros
    but a 1 in the last digit (it is not hard coded that way however).
    This means \f$2^{-23}\f$ for float and \f$2^{-52}\f$ for double.

    \arg For instance if two floating numbers (of same type) have an ulpdist of
    @ref Zero that means that their floating representation are identical or they are
    both Nans.

    \arg Generally equality up to 0.5 ulp is the best that one can wish beyond
    strict equality.

    \arg Typically if a double is compared to the float representation of
    its floating conversion (they are exceptions as for fully representable
    reals) the ulpdist will be around \f$2^{26.5}\f$ (~\f$10^8\f$)

    \arg  @c ulpdist(1.0,1+Eps\<double\>())==0.5
    \arg  @c ulpdist(1.0,1+Eps\<double\>()/2)==0.0
    \arg  @c ulpdist(1.0,1-Eps\<double\>()/2)==0.25
    \arg  @c ulpdist(1.0,1-Eps\<double\>())==0.5
    \arg  @c ulpdist(double(Pi\<float\>()),Pi\<double\>())==9.84293e+07

    @see ulp, Eps, eps

    @par Example:

      @snippet ulpdist.cpp ulpdist

    @par Possible output:

      @snippet ulpdist.txt ulpdist

  **/
  IEEEValue ulpdist(IEEEValue const& x, IEEEValue const& y);
} }
#endif

#include <boost/simd/function/scalar/ulpdist.hpp>
#include <boost/simd/function/simd/ulpdist.hpp>

#endif


#include <Eigen/Dense>
#include <iostream>
#include "general.h"

using EigenMatrix = Eigen::MatrixXd;
using EigenVector = Eigen::VectorXd;

EigenMatrix oneD()
{
    EigenMatrix m(1,4);
    m(0,0) = 3;
    m(0,1) = 2.5;
    m(0,2) = -1;
    m(0,3) = 1.5;
    return m;
}

EigenMatrix lapack_oneD()
{
    auto eigenM = oneD();
    int n = eigenM.cols();
    std::cout << "Cols: " << n << std::endl;
    int nn = n ;
    int rc = nn;
    int nnp = nn;
    int one = 1;
    double *w = nullptr;
    double *aux = nullptr;
    double *aux2 = nullptr;
    w = (double*)malloc(sizeof(double)*nn);
    aux = (double*)malloc(sizeof(double)*nn);
    aux2 = (double*)malloc(sizeof(double)*nn);
    for(int i=0;i<nn;i++)
      w[i] = eigenM(0, i+1) - eigenM(0, i); /* Dy */

    for(int i=0;i<nn-1;i++){
        aux[i] = 2;
        aux2[i] = -1;
    }
    aux[nn-1] = 2;

    dpttrf_(&nnp, aux, aux2, &rc);
    std::cout << "LDL factorization:" << std::endl;
    std::cout << "aux (Diagonal):" << std::endl;
    for(int i=0;i<nn;i++) {
      std::cout << aux[i] << ", ";
    }
    std::cout << std::endl;
    std::cout << "aux2 (SubDiagonal):" << std::endl;
    for(int i=0;i<nn;i++) {
      std::cout << aux2[i] << ", ";
    }
    std::cout << std::endl;
    dpttrs_(&nnp, &one, aux, aux2, w, &nnp, &rc);

    std::cout << "Solution w:" << std::endl;
    for(int i=0;i<nn;i++)
      std::cout << w[i] << ", ";
    std::cout << std::endl;

    EigenVector m(4);
    for(int i=0;i<nn;i++)
      m(i) = w[i];
    std::cout << m << std::endl;

    if(w) free(w);
    if(aux) free(aux);
    if(aux2) free(aux2);

    return m;
}

/* https://eigen.tuxfamily.org/dox/group__TutorialMapClass.html */
EigenMatrix eigen_lapack_equivalent_oneD()
{
    // Solve X in A*X = B, where A is triangular definite
    EigenMatrix eigenM(4,4);
    {
        EigenVector diag(eigenM.cols());
        diag.setConstant(2);
        EigenVector subUpperDiag(eigenM.cols());
        subUpperDiag.setConstant(-1);
        eigenM.diagonal() = diag;
        eigenM.diagonal(1) = subUpperDiag;
        eigenM.diagonal(-1) = subUpperDiag;
    }
    std::cout << "Matrix eigenM:\n" << eigenM << std::endl;
    EigenVector b(eigenM.cols());
    EigenMatrix oneM = oneD();
    for (int i = 0; i < oneM.cols(); ++i) {
        b[i] = oneM(0, i+1) - oneM(0, i); /* Dy */
    }

    // EigenVector v = eigenM.llt().solve(b);
    // EigenVector v = eigenM.ldlt().solve(b);
    Eigen::LDLT<EigenMatrix> ldltOfM(eigenM);
    EigenVector v = ldltOfM.solve(b);
    std::cout << v << std::endl;
    // EigenVector v = eigenM.colPivHouseholderQr().solve(b);
    EigenMatrix ldltMatrix = ldltOfM.matrixLDLT();
    std::cout << ldltMatrix << std::endl;
    EigenMatrix lMatrix = ldltOfM.matrixL();
    std::cout << lMatrix << std::endl;
    std::cout << "Diagonal of LDLT:" << std::endl;
    std::cout << ldltMatrix.diagonal() << std::endl;
    std::cout << "SubDiagonal of LDLT:" << std::endl;
    std::cout << ldltMatrix.diagonal(-1) << std::endl;

    return v;
}

/// The idea is that input and output are raw pointers to couple with existing code
EigenMatrix eigen_interface_raw_pointers_oneD()
{
    // Create the typemaps
    using EigenMatrixMap = Eigen::Map<EigenMatrix>;
    using EigenMatrixReadOnlyMap = Eigen::Map<const EigenMatrix>;
    using EigenVectorMap = Eigen::Map<EigenVector>;
    using EigenVectorReadOnlyMap = Eigen::Map<const EigenVector>;
    // Populate values using Eigen (just because easier)
    EigenMatrix eigenM(4,4);
    {
        EigenVector diag(eigenM.cols());
        diag.setConstant(2);
        EigenVector subUpperDiag(eigenM.cols());
        subUpperDiag.setConstant(-1);
        eigenM.diagonal() = diag;
        eigenM.diagonal(1) = subUpperDiag;
        eigenM.diagonal(-1) = subUpperDiag;
    }
    std::cout << "Matrix eigenM:\n" << eigenM << std::endl;
    EigenVector b(eigenM.cols());
    EigenMatrix oneM = oneD();
    for (int i = 0; i < oneM.cols(); ++i) {
        b[i] = oneM(0, i+1) - oneM(0, i); /* Dy */
    }
    // Set input raw pointers
    int nn = eigenM.cols();
    double *w = (double*)malloc(sizeof(double)*nn);
    for(int i=0;i<nn;i++) w[i] = b[i];
    // w is b as input (Ax=b), and solution x as output in lapack code.
    // http://www.netlib.org/lapack/lapack-3.1.1/html/dpttrs.f.html
    // Create a VectorMap of w to interact with Eigen
    EigenVectorMap wMap(w, b.size());
    EigenVector v = eigenM.ldlt().solve(wMap);
    std::cout << "v:\n" << v << std::endl;
    std::cout << "b:\n" << b << std::endl;
    std::cout << "wMap:\n" << wMap << std::endl;
    wMap = eigenM.ldlt().solve(wMap);
    std::cout << "InPlace wMap:\n" << wMap << std::endl;
    v = wMap;
    if(w) free(w);
    return v;
}

int main()
{
    bool test_passed = true;
    std::cout << "-- oneD Eigen --" << std::endl;
    auto mOneD = oneD();
    std::cout << mOneD << std::endl;
    std::cout << "-- lapack (from PN_TV1_Weighted) --" << std::endl;
    auto mLapack = lapack_oneD();
    std::cout << "-- eigen equivalent cholesky --" << std::endl;
    auto mEigen = eigen_lapack_equivalent_oneD();
    if(mLapack != mEigen){
        std::cerr << "Result vectors between mLapack and mEigen are not equal"  << std::endl;
        test_passed = false;
    }
    std::cout << "-- eigen interface with raw pointers --" << std::endl;
    auto mEigenRaw = eigen_interface_raw_pointers_oneD();
    if(mLapack != mEigenRaw){
        std::cerr << "Result vectors between mLapack and mEigenRaw are not equal"  << std::endl;
        test_passed = false;
    }

    if(!test_passed) return EXIT_FAILURE;
}


#include "gtest/gtest.h"
#include <Eigen/Core>
#include "util/common.h"
#include "util/resource_usage.h"
#include <omp.h>
using namespace PS;
TEST(EIGEN3, perf) {

  int n = 3000;
  Eigen::MatrixXd a = Eigen::MatrixXd::Random(n,n);
  Eigen::MatrixXd b = Eigen::MatrixXd::Random(n,1);

  auto tv = tic();
  Eigen::MatrixXd c = a * b;
  LL << toc(tv);

  tv = tic();
  Eigen::MatrixXd d = a * b;
  LL << toc(tv);
}
// #include "util/eigen3.h"

// TEST(EIGEN3, LOADXY) {
//   using namespace PS;
//   DSMat X_;
//   DVec Y_;
//   auto t = tic();
//   LoadXY("../data/rcv1.train", 0, 5000, &Y_, &X_);
//   LL << toc(t);

//   DSMat data_;
//   DVec label_;

//   LoadXY("../data/smalldata", 0, 10, &label_, &data_);

//   LL << data_;

//   CHECK_EQ(data_.coeffRef(4, 2), -29);
//   CHECK_EQ(data_.coeffRef(6, 4), -26);
//   CHECK_EQ(data_.coeffRef(1, 0), 46);
//   CHECK_EQ(data_.coeffRef(8, 1), 33);
//   CHECK_EQ(data_.coeffRef(1, 4), -13);
//   CHECK_EQ(data_.coeffRef(0, 0), 0);
//   CHECK_EQ(data_.coeffRef(9, 4), -48);

//   // TODO test the correctness
// }


/* test_binomial_distribution.cpp
 *
 * Copyright Steven Watanabe 2010
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id: test_binomial_distribution.cpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 */

#include <boost/random/binomial_distribution.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::binomial_distribution<>
#define BOOST_RANDOM_ARG1 t
#define BOOST_RANDOM_ARG2 p
#define BOOST_RANDOM_ARG1_DEFAULT 1
#define BOOST_RANDOM_ARG2_DEFAULT 0.5
#define BOOST_RANDOM_ARG1_VALUE 10
#define BOOST_RANDOM_ARG2_VALUE 0.25

#define BOOST_RANDOM_DIST0_MIN 0
#define BOOST_RANDOM_DIST0_MAX 1
#define BOOST_RANDOM_DIST1_MIN 0
#define BOOST_RANDOM_DIST1_MAX 10
#define BOOST_RANDOM_DIST2_MIN 0
#define BOOST_RANDOM_DIST2_MAX 10

#define BOOST_RANDOM_TEST1_PARAMS
#define BOOST_RANDOM_TEST1_MIN 0
#define BOOST_RANDOM_TEST1_MAX 1

#define BOOST_RANDOM_TEST2_PARAMS (10, 0.25)
#define BOOST_RANDOM_TEST2_MIN 0
#define BOOST_RANDOM_TEST2_MAX 10

#include "test_distribution.ipp"


#ifndef _UNIVERSAL_APPROXIMATOR_HPP
#define _UNIVERSAL_APPROXIMATOR_HPP

#include <armadillo>
#include <vector>
#include <math.h>

using namespace arma;
using namespace std;

class universal_approximator {
    double sigma;
    vector<long double> w;
    vector<long double> G;
public:
    universal_approximator(const vector<double> &x, const vector<double> &y,
                           double lambda);

    double gaussian(double xi, double xj);

    double get_output(double x);

    vector<double> test(const vector<double> &x);
};

#endif


#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
#include <iostream>
#include <random>
#include "gtest/gtest.h"
#include "interpolate.h"
#include "VMD.h"

VMD_Frame recreate_bezier_parameter(VMD_Frame head, VMD_Frame tail)
{
  vector<VMD_Frame> fv = {head, tail};
  fv = fill_bone_frame(fv, true);
  optimize_bezier_parameter(tail, fv, head.number, tail.number);
  return tail;
}

TEST(InterpolationTest, BezierCurveFitting) {
  Eigen::Quaternionf q1(Eigen::AngleAxisf(3.14f/4, Eigen::Vector3f::UnitY()));
  VMD_Frame head = VMD_Frame("center", 10, Eigen::Vector3f(1, 2, 3), q1);
  Eigen::Quaternionf q2(Eigen::AngleAxisf(3.14f/4, Eigen::Vector3f::UnitX()));
  VMD_Frame tail = VMD_Frame("center", 20, Eigen::Vector3f(2, -2, 2), q2);
  VMD_Frame f;

  tail.set_interpolation_x(10, 100, 110, 30);
  f = recreate_bezier_parameter(head, tail);
  for (int i = 0; i < 64; i += 4) {
    EXPECT_NEAR(f.interpolation[i], tail.interpolation[i], 5) << "i = " << i;
  }

  tail.set_interpolation_r(20, 5, 90, 127);
  f = recreate_bezier_parameter(head, tail);
  for (int i = 0; i < 64; i += 4) {
    EXPECT_NEAR(f.interpolation[i], tail.interpolation[i], 5) << "i = " << i;
  }

}


// Copyright 2019 Google LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     https://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <trajectory_math/float_comparison.h>
#include <trajectory_math/trajectory_algorithms.h>

#include <Eigen/Geometry>

namespace bookbot {
double TriangleArea(Eigen::Vector2d start_point, Eigen::Vector2d corner_point,
                    Eigen::Vector2d end_point) {
  const Eigen::Vector3d corner_to_start{start_point[0] - corner_point[0],
                                        start_point[1] - corner_point[1], 0};
  const Eigen::Vector3d corner_to_end{end_point[0] - corner_point[0],
                                      end_point[1] - corner_point[1], 0};
  return 0.5 *
         std::abs(Eigen::Vector3d(corner_to_start.cross(corner_to_end))[2]);
}

double ApproximateCurvatureMagnitude(Eigen::Vector2d start_point,
                                     Eigen::Vector2d corner_point,
                                     Eigen::Vector2d end_point) {
  const double area = TriangleArea(start_point, corner_point, end_point);
  const double start_to_corner =
      Eigen::Vector2d(start_point - corner_point).squaredNorm();
  const double start_to_end =
      Eigen::Vector2d(start_point - end_point).squaredNorm();
  const double corner_to_end =
      Eigen::Vector2d(corner_point - end_point).squaredNorm();
  const double normalization = start_to_corner * start_to_end * corner_to_end;
  if (ApproxZero(normalization)) {
    return 0.;
  }
  return 4 * area / std::sqrt(normalization);
}

}  // namespace bookbot


#include "LSCM.h"
#include <assert.h>
#include <math.h>
#include <float.h>
#include <Eigen/Sparse>
#include <iostream>
#include <sstream>
#include <Eigen/IterativeLinearSolvers>

using namespace MeshLib;

typedef Eigen::SparseMatrix<double> SpMat;
typedef Eigen::VectorXd VectorXd;

LSCM::LSCM(Mesh * mesh) {
	m_mesh = mesh;
}

LSCM::~LSCM(){}

void LSCM::set_coefficients() {
	for (MeshEdgeIterator eiter(m_mesh); !eiter.end(); ++eiter) {
		Edge * e = *eiter;
		e_l(e) = m_mesh->edge_length(e);
	}

	for (MeshFaceIterator fiter(m_mesh); !fiter.end(); ++fiter) {
		Point  p[3];
		Face *f = *fiter;

		double l[3];
		HalfEdge * he = f->halfedge();
		for (int j = 0; j < 3; j++) {
			Edge * e = he->edge();
			l[j] = e_l(e);
			he = he->he_next();
		}

		double a = acos((l[0]*l[0] + l[2]*l[2] - l[1]*l[1]) / (2*l[0]*l[2]));

		p[0] = Point(0, 0, 0);
		p[1] = Point(l[0], 0, 0);
		p[2] = Point(l[2]*cos(a), l[2]*sin(a), 0);

		Point n = (p[1]-p[0]) ^ (p[2]-p[0]);
		double area = n.norm() / 2.0;
		n /= area;

		he = f->halfedge();
		for (int j = 0; j < 3; j++) {
			Point s = (n ^ (p[(j + 1) % 3] - p[j])) / sqrt(area);
			c_s(he) = s;
			he = he->he_next();
		}
	}
}

void LSCM::project() {
	set_coefficients();

	std::vector<Vertex*> vertices;
    std::vector<Face*> faces;

	for (MeshVertexIterator viter(m_mesh); !viter.end(); ++viter){
		Vertex * v = *viter;
		if (v->string().substr(0,3) != "fix") {
			vertices.push_back(v);
		}
		else {
			m_fix_vertices.push_back(v);
		}
	}
	assert(m_fix_vertices.size()>=2);

	for (int k = 0; k < (int)vertices.size(); k++ ){
		v_idx(vertices[k]) = k;
	}
	for (int k = 0; k < (int)m_fix_vertices.size(); k++) {
		v_idx(m_fix_vertices[k]) = k;
		Vertex *v = m_fix_vertices[k];
		std::string tmp;
		double uv0, uv1;
		std::stringstream(v->string()) >> tmp >> uv0 >> uv1;
		v_uv(v) = Point2(uv0, uv1);
	}

	int fn = m_mesh->numFaces();
	int	vfn = m_fix_vertices.size();
	int	vn = m_mesh->numVertices() - vfn;

	typedef Eigen::Triplet<double> T;
	std::vector<T> tripletList1;
	std::vector<T> tripletList2;
	tripletList1.reserve(fn);
	tripletList2.reserve(fn);
	VectorXd b(vfn * 2);

	int fid = 0;
	for (MeshFaceIterator fiter(m_mesh); !fiter.end(); ++fiter, ++fid) {
		Face *f = *fiter;
		HalfEdge *he = f->halfedge();

		for (int j = 0; j < 3; j++) {
			Point s = c_s(he);

			Vertex * v = he->he_next()->target();
			int vid = v_idx(v);

			if (v->string().substr(0,3) != "fix") {
				tripletList1.push_back(T(fid,vid,s[0]));
				tripletList1.push_back(T(fn + fid, vn + vid, s[0]));
				tripletList1.push_back(T(fid, vn + vid, -s[1]));
				tripletList1.push_back(T(fn + fid, vid, s[1]));
			}
			else {
				Point2 uv = v_uv(v);

				tripletList2.push_back(T(fid, vid, s[0]));
				tripletList2.push_back(T(fn + fid, vfn + vid, s[0]));
				tripletList2.push_back(T(fid, vfn + vid, -s[1]));
				tripletList2.push_back(T(fn + fid, vid, s[1]));

				b[vid] = uv[0];
				b[vfn + vid] = uv[1];
			}
			he = he->he_next();
		}
	}
	SpMat A(2*fn, 2*vn);
	SpMat B(2*fn, 2*vfn);

	A.setFromTriplets(tripletList1.begin(), tripletList1.end());
	B.setFromTriplets(tripletList2.begin(), tripletList2.end());

	VectorXd r, x;
	r = B * b;
	r = r * -1;

    // Solve the linear system Ax = r
	Eigen::LeastSquaresConjugateGradient<Eigen::SparseMatrix<double>> lscg;
	lscg.compute(A);
	x = lscg.solve(r);

	for (int i = 0; i < vn; i++) {
		Vertex * v = vertices[i];
		v_uv(v) = Point2(x[i], x[i + vn]);
	}
	for (MeshVertexIterator viter(m_mesh); !viter.end(); viter++) {
		Vertex * v = *viter;
		Point2 p = v_uv(v);
		Point p3(p[0], p[1], 0);
		v->point() = p3;
	}
}


#include "LonLatDistance.h"

#include <boost/math/constants/constants.hpp>
#include <macgyver/Exception.h>

namespace SmartMet
{
namespace Plugin
{
namespace TimeSeries
{
/// @brief Earth's quatratic mean radius for WGS-84
static const double EARTH_RADIUS_IN_METERS = 6372797.560856;

double deg_to_rad(const double& degrees)
{
  try
  {
    return degrees * (boost::math::constants::pi<double>() / 180.0);
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

double rad_to_deg(const double& radians)
{
  try
  {
    return radians * (180.0 / boost::math::constants::pi<double>());
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

/**
 * Returns the (initial) bearing from this point to the supplied point, in degrees
 *   see http://williams.best.vwh.net/avform.htm#Crs
 *
 * @param   {LatLon} point: Latitude/longitude of destination point
 * @returns {Number} Initial bearing in degrees from North
 */
double initial_bearing(const std::pair<double, double>& from, const std::pair<double, double>& to)
{
  try
  {
    double lat1 = deg_to_rad(from.second);
    double lat2 = deg_to_rad(to.second);
    double dLon = deg_to_rad(to.first - from.first);

    double y = sin(dLon) * cos(lat2);
    double x = (cos(lat1) * sin(lat2)) - (sin(lat1) * cos(lat2) * cos(dLon));
    double brng = atan2(y, x);

    return fmod((rad_to_deg(brng) + 360.0), 360.0);
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

/**
 * Returns final bearing arriving at supplied destination point from this point; the final bearing
 * will differ from the initial bearing by varying degrees according to distance and latitude
 *
 * @param   {LatLon} point: Latitude/longitude of destination point
 * @returns {Number} Final bearing in degrees from North
 */
double final_bearing(const std::pair<double, double>& from, const std::pair<double, double>& to)
{
  try
  {
    // get initial bearing from supplied point back to this point...
    double lat1 = deg_to_rad(to.second);
    double lat2 = deg_to_rad(from.second);
    double dLon = deg_to_rad(from.first - to.first);

    double y = sin(dLon) * cos(lat2);
    double x = (cos(lat1) * sin(lat2)) - (sin(lat1) * cos(lat2) * cos(dLon));
    double brng = atan2(y, x);
    // ... & reverse it by adding 180°
    return fmod((rad_to_deg(brng) + 180.0), 360.0);
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

/**
 * Returns the midpoint between this point and the supplied point.
 *   see http://mathforum.org/library/drmath/view/51822.html for derivation
 *
 * @param   {LatLon} point: Latitude/longitude of destination point
 * @returns {LatLon} Midpoint between this point and the supplied point
 */
std::pair<double, double> midpoint(const std::pair<double, double>& from,
                                   const std::pair<double, double>& to)
{
  try
  {
    double lat1 = deg_to_rad(from.second);
    double lon1 = deg_to_rad(from.first);
    double lat2 = deg_to_rad(to.second);
    double dLon = deg_to_rad(to.first - from.first);

    double Bx = cos(lat2) * cos(dLon);
    double By = cos(lat2) * sin(dLon);

    double lat3 = atan2(sin(lat1) + sin(lat2), sqrt((cos(lat1) + Bx) * (cos(lat1) + Bx) + By * By));
    double lon3 = lon1 + atan2(By, cos(lat1) + Bx);
    double pi(boost::math::constants::pi<double>());
    lon3 = fmod(lon3 + (3 * pi), 2 * pi) - pi;  // normalise to -180..+180º

    return std::pair<double, double>(rad_to_deg(lon3), rad_to_deg(lat3));
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

/**
 * Returns the destination point from this point having travelled the given distance (in km) on the
 * given initial bearing (bearing may vary before destination is reached)
 *
 *   see http://williams.best.vwh.net/avform.htm#LL
 *
 * @param   {Number} brng: Initial bearing in degrees
 * @param   {Number} dist: Distance in km
 * @returns {LatLon} Destination point
 */
std::pair<double, double> destination_point(const std::pair<double, double>& from,
                                            const std::pair<double, double>& to,
                                            const double& distance)
{
  try
  {
    double brng(initial_bearing(from, to));

    double dist = distance /
                  (EARTH_RADIUS_IN_METERS / 1000.0);  // convert dist to angular distance in radians
    brng = deg_to_rad(brng);
    double lat1 = deg_to_rad(from.second);
    double lon1 = deg_to_rad(from.first);

    double lat2 = asin(sin(lat1) * cos(dist) + cos(lat1) * sin(dist) * cos(brng));
    double lon2 =
        lon1 + atan2(sin(brng) * sin(dist) * cos(lat1), cos(dist) - sin(lat1) * sin(lat2));
    double pi(boost::math::constants::pi<double>());
    lon2 = fmod(lon2 + 3 * pi, 2 * pi) - pi;  // normalise to -180..+180º

    return {rad_to_deg(lon2), rad_to_deg(lat2)};
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

/** @brief Computes the arc, in radian, between two WGS-84 positions.
 *
 * The result is equal to Distance(from,to)/EARTH_RADIUS_IN_METERS
 *    = 2*asin(sqrt(h(d/EARTH_RADIUS_IN_METERS )))
 *
 * where:

 *
d is the distance in meters between 'from' and 'to' positions.

 *
h is the haversine function: h(x)=sin²(x/2)

 *

 *
 * The haversine formula gives:
 *    h(d/R) = h(from.lat-to.lat)+h(from.lon-to.lon)+cos(from.lat)*cos(to.lat)
 *
 * @sa http://en.wikipedia.org/wiki/Law_of_haversines
 */
double arc_in_radians(const std::pair<double, double>& from, const std::pair<double, double>& to)
{
  try
  {
    double latitudeArc = deg_to_rad(from.second - to.second);
    double longitudeArc = deg_to_rad(from.first - to.first);
    double latitudeH = sin(latitudeArc * 0.5);
    latitudeH *= latitudeH;
    double lontitudeH = sin(longitudeArc * 0.5);
    lontitudeH *= lontitudeH;
    double tmp = cos(deg_to_rad(from.second)) * cos(deg_to_rad(to.second));
    return 2.0 * asin(sqrt(latitudeH + tmp * lontitudeH));
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

/** @brief Computes the distance, in meters, between two WGS-84 positions.
 *
 * The result is equal to EARTH_RADIUS_IN_METERS*ArcInRadians(from,to)
 *
 * @sa ArcInRadians
 */
double distance_in_meters(const std::pair<double, double>& from,
                          const std::pair<double, double>& to)
{
  try
  {
    return EARTH_RADIUS_IN_METERS * arc_in_radians(from, to);
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

double distance_in_kilometers(const std::pair<double, double>& from,
                              const std::pair<double, double>& to)
{
  try
  {
    return (distance_in_meters(from, to) / 1000.0);
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

}  // namespace TimeSeries
}  // namespace Plugin
}  // namespace SmartMet


/**
 * @file radical_main.cpp
 * @author Nishant Mehta
 *
 * Test for RADICAL.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/radical/radical.hpp>
#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

BOOST_AUTO_TEST_SUITE(RadicalTest);

using namespace mlpack;
using namespace mlpack::radical;
using namespace std;
using namespace arma;

BOOST_AUTO_TEST_CASE(Radical_Test_Radical3D)
{
  mat matX;
  data::Load("data_3d_mixed.txt", matX);

  Radical rad(0.175, 5, 100, matX.n_rows - 1);

  mat matY;
  mat matW;
  rad.DoRadical(matX, matY, matW);

  mat matYT = trans(matY);
  double valEst = 0;

  for (uword i = 0; i < matYT.n_cols; i++)
  {
    vec y = vec(matYT.col(i));
    valEst += rad.Vasicek(y);
  }

  mat matS;
  data::Load("data_3d_ind.txt", matS);
  rad.DoRadical(matS, matY, matW);

  matYT = trans(matY);
  double valBest = 0;

  for (uword i = 0; i < matYT.n_cols; i++)
  {
    vec y = vec(matYT.col(i));
    valBest += rad.Vasicek(y);
  }

  BOOST_REQUIRE_CLOSE(valBest, valEst, 0.25);
}

BOOST_AUTO_TEST_SUITE_END();


//
// Created by Alex Beccaro on 18/12/17.
//

#include "problem29.hpp"
#include <boost/unordered_set.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <generics.hpp>

using boost::unordered_set;
using boost::multiprecision::uint1024_t;
using generics::int_pow;

namespace problems {
    uint32_t problem29::solve(uint32_t max_base, uint32_t max_exp) {
        unordered_set<uint1024_t> powers;

        for (uint32_t a = 2; a <= max_base; a++)
            for (uint32_t b = 2; b <= max_exp; b++)
                powers.insert(int_pow<uint1024_t>(a, b));

        return (uint32_t) powers.size();
    }
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2014 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <gudhi/Simplex_tree.h>
#include <gudhi/Points_3D_off_io.h>

#include <boost/variant.hpp>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/Alpha_shape_3.h>
#include <CGAL/Alpha_shape_vertex_base_3.h>
#include <CGAL/Alpha_shape_cell_base_3.h>
#include <CGAL/iterator.h>

#include <fstream>
#include <cmath>
#include <string>
#include <tuple>  // for tuple<>
#include <map>
#include <utility>  // for pair<>
#include <list>
#include <vector>

// Alpha_shape_3 templates type definitions
typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef CGAL::Alpha_shape_vertex_base_3<Kernel> Vb;
typedef CGAL::Alpha_shape_cell_base_3<Kernel> Fb;
typedef CGAL::Triangulation_data_structure_3<Vb, Fb> Tds;
typedef CGAL::Delaunay_triangulation_3<Kernel, Tds> Triangulation_3;
typedef CGAL::Alpha_shape_3<Triangulation_3> Alpha_shape_3;

// From file type definition
typedef Kernel::Point_3 Point;

// filtration with alpha values needed type definition
typedef Alpha_shape_3::FT Alpha_value_type;
typedef CGAL::Object Object;
typedef CGAL::Dispatch_output_iterator<
CGAL::cpp11::tuple<Object, Alpha_value_type>,
CGAL::cpp11::tuple<std::back_insert_iterator< std::vector<Object> >,
    std::back_insert_iterator< std::vector<Alpha_value_type> > > > Dispatch;
typedef Alpha_shape_3::Cell_handle Cell_handle;
typedef Alpha_shape_3::Facet Facet;
typedef Alpha_shape_3::Edge Edge;
typedef std::list<Alpha_shape_3::Vertex_handle> Vertex_list;

// gudhi type definition
typedef Gudhi::Simplex_tree<> Simplex_tree;
typedef Simplex_tree::Vertex_handle Simplex_tree_vertex;
typedef std::map<Alpha_shape_3::Vertex_handle, Simplex_tree_vertex > Alpha_shape_simplex_tree_map;
typedef std::pair<Alpha_shape_3::Vertex_handle, Simplex_tree_vertex> Alpha_shape_simplex_tree_pair;
typedef std::vector< Simplex_tree_vertex > Simplex_tree_vector_vertex;

Vertex_list from(const Cell_handle& ch) {
  Vertex_list the_list;
  for (auto i = 0; i < 4; i++) {
#ifdef DEBUG_TRACES
    std::cout << "from cell[" << i << "]=" << ch->vertex(i)->point() << std::endl;
#endif  // DEBUG_TRACES
    the_list.push_back(ch->vertex(i));
  }
  return the_list;
}

Vertex_list from(const Facet& fct) {
  Vertex_list the_list;
  for (auto i = 0; i < 4; i++) {
    if (fct.second != i) {
#ifdef DEBUG_TRACES
      std::cout << "from facet=[" << i << "]" << fct.first->vertex(i)->point() << std::endl;
#endif  // DEBUG_TRACES
      the_list.push_back(fct.first->vertex(i));
    }
  }
  return the_list;
}

Vertex_list from(const Edge& edg) {
  Vertex_list the_list;
  for (auto i = 0; i < 4; i++) {
    if ((edg.second == i) || (edg.third == i)) {
#ifdef DEBUG_TRACES
      std::cout << "from edge[" << i << "]=" << edg.first->vertex(i)->point() << std::endl;
#endif  // DEBUG_TRACES
      the_list.push_back(edg.first->vertex(i));
    }
  }
  return the_list;
}

Vertex_list from(const Alpha_shape_3::Vertex_handle& vh) {
  Vertex_list the_list;
#ifdef DEBUG_TRACES
  std::cout << "from vertex=" << vh->point() << std::endl;
#endif  // DEBUG_TRACES
  the_list.push_back(vh);
  return the_list;
}

int main(int argc, char * const argv[]) {
  // program args management
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0]
        << " path_to_off_file \n";
    return 0;
  }

  // Read points from file
  std::string offInputFile(argv[1]);
  // Read the OFF file (input file name given as parameter) and triangulate points
  Gudhi::Points_3D_off_reader<Point> off_reader(offInputFile);
  // Check the read operation was correct
  if (!off_reader.is_valid()) {
    std::cerr << "Unable to read file " << argv[1] << std::endl;
    return 0;
  }
  // Retrieve the triangulation
  std::vector<Point> lp = off_reader.get_point_cloud();

  // alpha shape construction from points. CGAL has a strange behavior in REGULARIZED mode.
  Alpha_shape_3 as(lp.begin(), lp.end(), 0, Alpha_shape_3::GENERAL);
#ifdef DEBUG_TRACES
  std::cout << "Alpha shape computed in GENERAL mode" << std::endl;
#endif  // DEBUG_TRACES

  // filtration with alpha values from alpha shape
  std::vector<Object> the_objects;
  std::vector<Alpha_value_type> the_alpha_values;

  Dispatch disp = CGAL::dispatch_output<Object, Alpha_value_type>(std::back_inserter(the_objects),
      std::back_inserter(the_alpha_values));

  as.filtration_with_alpha_values(disp);
#ifdef DEBUG_TRACES
  std::cout << "filtration_with_alpha_values returns : " << the_objects.size() << " objects" << std::endl;
#endif  // DEBUG_TRACES

  Alpha_shape_3::size_type count_vertices = 0;
  Alpha_shape_3::size_type count_edges = 0;
  Alpha_shape_3::size_type count_facets = 0;
  Alpha_shape_3::size_type count_cells = 0;

  // Loop on objects vector
  Vertex_list vertex_list;
  Simplex_tree simplex_tree;
  Alpha_shape_simplex_tree_map map_cgal_simplex_tree;
  std::vector<Alpha_value_type>::iterator the_alpha_value_iterator = the_alpha_values.begin();
  for (auto object_iterator : the_objects) {
    // Retrieve Alpha shape vertex list from object
    if (const Cell_handle * cell = CGAL::object_cast<Cell_handle>(&object_iterator)) {
      vertex_list = from(*cell);
      count_cells++;
    } else if (const Facet * facet = CGAL::object_cast<Facet>(&object_iterator)) {
      vertex_list = from(*facet);
      count_facets++;
    } else if (const Edge * edge = CGAL::object_cast<Edge>(&object_iterator)) {
      vertex_list = from(*edge);
      count_edges++;
    } else if (const Alpha_shape_3::Vertex_handle * vertex =
              CGAL::object_cast<Alpha_shape_3::Vertex_handle>(&object_iterator)) {
      count_vertices++;
      vertex_list = from(*vertex);
    }
    // Construction of the vector of simplex_tree vertex from list of alpha_shapes vertex
    Simplex_tree_vector_vertex the_simplex_tree;
    for (auto the_alpha_shape_vertex : vertex_list) {
      Alpha_shape_simplex_tree_map::iterator the_map_iterator = map_cgal_simplex_tree.find(the_alpha_shape_vertex);
      if (the_map_iterator == map_cgal_simplex_tree.end()) {
        // alpha shape not found
        Simplex_tree_vertex vertex = map_cgal_simplex_tree.size();
#ifdef DEBUG_TRACES
        std::cout << "vertex [" << the_alpha_shape_vertex->point() << "] not found - insert_simplex " << vertex << "\n";
#endif  // DEBUG_TRACES
        the_simplex_tree.push_back(vertex);
        map_cgal_simplex_tree.insert(Alpha_shape_simplex_tree_pair(the_alpha_shape_vertex, vertex));
      } else {
        // alpha shape found
        Simplex_tree_vertex vertex = the_map_iterator->second;
#ifdef DEBUG_TRACES
        std::cout << "vertex [" << the_alpha_shape_vertex->point() << "] found in " << vertex << std::endl;
#endif  // DEBUG_TRACES
        the_simplex_tree.push_back(vertex);
      }
    }
    // Construction of the simplex_tree
#ifdef DEBUG_TRACES
    std::cout << "filtration = " << *the_alpha_value_iterator << std::endl;
#endif  // DEBUG_TRACES
    simplex_tree.insert_simplex(the_simplex_tree, std::sqrt(*the_alpha_value_iterator));
    if (the_alpha_value_iterator != the_alpha_values.end())
      ++the_alpha_value_iterator;
    else
      std::cerr << "This shall not happen" << std::endl;
  }
#ifdef DEBUG_TRACES
  std::cout << "vertices \t\t" << count_vertices << std::endl;
  std::cout << "edges \t\t" << count_edges << std::endl;
  std::cout << "facets \t\t" << count_facets << std::endl;
  std::cout << "cells \t\t" << count_cells << std::endl;

  std::cout << "Information of the Simplex Tree:\n";
  std::cout << "  Number of vertices = " << simplex_tree.num_vertices() << " ";
  std::cout << "  Number of simplices = " << simplex_tree.num_simplices() << std::endl << std::endl;
#endif  // DEBUG_TRACES

#ifdef DEBUG_TRACES
  std::cout << "Iterator on vertices: \n";
  for (auto vertex : simplex_tree.complex_vertex_range()) {
    std::cout << vertex << " ";
  }
#endif  // DEBUG_TRACES

  std::cout << simplex_tree << std::endl;

#ifdef DEBUG_TRACES
  std::cout << std::endl << std::endl << "Iterator on simplices:\n";
  for (auto simplex : simplex_tree.complex_simplex_range()) {
    std::cout << "   ";
    for (auto vertex : simplex_tree.simplex_vertex_range(simplex)) {
      std::cout << vertex << " ";
    }
    std::cout << std::endl;
  }
#endif  // DEBUG_TRACES
#ifdef DEBUG_TRACES
  std::cout << std::endl << std::endl << "Iterator on Simplices in the filtration, with [filtration value]:\n";
  for (auto f_simplex : simplex_tree.filtration_simplex_range()) {
    std::cout << "   " << "[" << simplex_tree.filtration(f_simplex) << "] ";
    for (auto vertex : simplex_tree.simplex_vertex_range(f_simplex)) {
      std::cout << vertex << " ";
    }
    std::cout << std::endl;
  }
#endif  // DEBUG_TRACES
#ifdef DEBUG_TRACES
  std::cout << std::endl << std::endl << "Iterator on Simplices in the filtration, and their boundary simplices:\n";
  for (auto f_simplex : simplex_tree.filtration_simplex_range()) {
    std::cout << "   " << "[" << simplex_tree.filtration(f_simplex) << "] ";
    for (auto vertex : simplex_tree.simplex_vertex_range(f_simplex)) {
      std::cout << vertex << " ";
    }
    std::cout << std::endl;

    for (auto b_simplex : simplex_tree.boundary_simplex_range(f_simplex)) {
      std::cout << "      " << "[" << simplex_tree.filtration(b_simplex) << "] ";
      for (auto vertex : simplex_tree.simplex_vertex_range(b_simplex)) {
        std::cout << vertex << " ";
      }
      std::cout << std::endl;
    }
  }
#endif  // DEBUG_TRACES

  return 0;
}


#include <MNIST.h>
#include <Matrix.h>
#include <Function.h>
#include <Random.h>
#include <Eigen/Dense>
#include "TwoLayerNet.h"

static constexpr std::size_t ITERS_NUM  = 10000;
static constexpr std::size_t BATCH_SIZE = 100;
static constexpr double LEARNING_RATE = 0.1;

int main() {
    MNIST mnistTrain;
    if (mnistTrain.load("./../dataset/train-images-idx3-ubyte", "./../dataset/train-labels-idx1-ubyte") != 0) {
        std::fprintf(stderr, "Failed to load MNIST training set.\n");
        return -1;
    }

    MNIST mnistTest;
    if (mnistTest.load("./../dataset/t10k-images-idx3-ubyte", "./../dataset/t10k-labels-idx1-ubyte") != 0) {
        std::fprintf(stderr, "Failed to load MNIST test set.\n");
        return -1;
    }

    const MatrixXd& XTrain = mnistTrain.getImages();
    const VectorXi& tTrain = mnistTrain.getLabels();
    const MatrixXd& XTest = mnistTest.getImages();
    const VectorXi& tTest = mnistTest.getLabels();

    const std::size_t iterPerEpoch = std::max(XTrain.rows() / BATCH_SIZE, 1ul);

    TwoLayerNet network(784, 50, 10);
    for (std::size_t i = 0; i < ITERS_NUM; ++i) {
        const std::vector<std::size_t> batchIndex = choice(XTrain.rows(), BATCH_SIZE);

        const MatrixXd XBatch = createMatrixXdBatch(XTrain, batchIndex);
        const VectorXi tBatch = createVectorXiBatch(tTrain, batchIndex);

        network.gradient(XBatch, tBatch);
        network.update(LEARNING_RATE);

        if (i % iterPerEpoch == 0) {
            const double trainAcc = network.accuracy(XTrain, tTrain);
            const double testAcc = network.accuracy(XTest, tTest);
            std::printf("train acc, test acc | %lf, %lf\n", trainAcc, testAcc);
        }
    }

    return 0;
}


#pragma once

#include <vector>

#include <Eigen/Geometry>

namespace common_robotics_utilities
{
namespace conversions
{
Eigen::Quaterniond QuaternionFromRPY(const double R,
                                     const double P,
                                     const double Y);

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Quaterniond QuaternionFromUrdfRPY(const double R,
                                         const double P,
                                         const double Y);

// Returns XYZ Euler angles
Eigen::Vector3d EulerAnglesFromRotationMatrix(
    const Eigen::Matrix3d& rot_matrix);

// Returns XYZ Euler angles
Eigen::Vector3d EulerAnglesFromQuaternion(const Eigen::Quaterniond& quat);

// Returns XYZ Euler angles
Eigen::Vector3d EulerAnglesFromIsometry3d(const Eigen::Isometry3d& trans);

Eigen::Isometry3d TransformFromXYZRPY(const double x,
                                      const double y,
                                      const double z,
                                      const double roll,
                                      const double pitch,
                                      const double yaw);

Eigen::Isometry3d TransformFromRPY(const Eigen::Vector3d& translation,
                                   const Eigen::Vector3d& rotation);

Eigen::Isometry3d TransformFromRPY(const Eigen::VectorXd& components);

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Isometry3d TransformFromUrdfXYZRPY(const double x,
                                          const double y,
                                          const double z,
                                          const double roll,
                                          const double pitch,
                                          const double yaw);

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Isometry3d TransformFromUrdfRPY(const Eigen::Vector3d& translation,
                                       const Eigen::Vector3d& rotation);

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Isometry3d TransformFromUrdfRPY(const Eigen::VectorXd& components);

Eigen::VectorXd TransformToRPY(const Eigen::Isometry3d& transform);

Eigen::Vector3d StdVectorDoubleToEigenVector3d(
    const std::vector<double>& vector);

Eigen::VectorXd StdVectorDoubleToEigenVectorXd(
    const std::vector<double>& vector);

std::vector<double> EigenVector3dToStdVectorDouble(
    const Eigen::Vector3d& point);

std::vector<double> EigenVectorXdToStdVectorDouble(
    const Eigen::VectorXd& eigen_vector);

// Takes <x, y, z, w> as is the ROS custom!
Eigen::Quaterniond StdVectorDoubleToEigenQuaterniond(
    const std::vector<double>& vector);

// Returns <x, y, z, w> as is the ROS custom!
std::vector<double> EigenQuaterniondToStdVectorDouble(
    const Eigen::Quaterniond& quat);
}  // namespace conversions
}  // namespace common_robotics_utilities


#include <list>                // std::list
#include <boost/format.hpp>    // only needed for printing
#include <functional>          // std::ref
#include <numeric>             // std::accumulate
#include "histogram.h"
#include "vars.h"

std::list<double> init_bins(int cell_no) {
    //initalise histogram bins db
    std::vector<int> b(cell_no + 1);
    std::iota(b.begin(), b.end(), 0);

    double cell_L = cell_length;
    std::vector<double> db(b.begin(), b.end());
    std::transform(db.begin(), db.end(), db.begin(), [&cell_L](auto& c) {return c * cell_L;});

    std::list<double> bins(db.begin(), db.end());

    return bins;
}

std::vector<double> rho_dist(std::vector<double> x) {

    std::list<double> bins_list = init_bins(std::round(L / cell_length));
    //auto bins = axis::regular<double, use_default, axis::option::none_t>{ axis::step(cell_length), 0, L };

    auto bins = axis::variable<double, use_default, axis::option::none_t>(bins_list);

    auto h_rho = make_histogram(bins); //initialise histogram of length L with bins of width cell_length

    std::for_each(x.begin(), x.end(), std::ref(h_rho)); //populate histogram with x_evolved

    std::vector<double> vec_h_rho(h_rho.axis().size());

    for (unsigned i = 0; i < vec_h_rho.size(); i++) {
        vec_h_rho[i] = h_rho.at(i); //convert hist to vector because C++ can't deal with autos :(
    }
    return vec_h_rho;
}

/*
* Create averaged cell velocity distribution using density histogram
*/

std::vector<double> cell_velocity_dist(std::vector<double> v, std::vector<double> x) {

    std::vector<double> vel_hist;
    unsigned cum_bincount = 0;

    // calculate all the mean cell velocities bar the last cell
    for (unsigned i = 0; i != x.size(); i++) {
        unsigned n_i = x.at(i);

        if (n_i == 0) {
            vel_hist.push_back(0);
        }

        else {
            double splice_sum = std::accumulate(v.begin() + cum_bincount, v.begin() + (cum_bincount + n_i), 0.0);
            double cell_vbar = splice_sum / n_i;
            vel_hist.push_back(cell_vbar);
            cum_bincount += n_i;
        }

    }
    return vel_hist;

}
/*
* Create histograms {h_rho,h_j} for a sample run
*/
std::vector<std::vector<double>> rho_j_dist(std::vector<double> x, std::vector<double> v, std::list<double> bins_list) {

    //std::list<double> bins_list = init_bins(std::round(L / cell_length));
    //auto bins = axis::regular<double, use_default, axis::option::none_t>{ axis::step(cell_length), 0, L };
    auto bins = axis::variable<>(bins_list);

    auto h_rho = make_histogram(bins); //initialise histogram of length L with bins of width cell_length

    std::for_each(x.begin(), x.end(), std::ref(h_rho)); //populate histogram with x_evolved

    std::vector<double> vec_h_rho(h_rho.axis().size());

    for (unsigned i = 0; i != vec_h_rho.size(); i++) {
        vec_h_rho[i] = h_rho.at(i); //convert hist to vector because C++ can't deal with autos    }
    }

    std::vector<double> h_v = cell_velocity_dist(v, vec_h_rho);

    std::vector<double> h_j(h_v.size());

    for (unsigned i = 0; i < h_v.size(); i++) {
        h_j[i] = h_v[i] *h_rho.at(i);
    }

    return { vec_h_rho,h_v };
}

/*
* compute averaged_rho_dist with a matrix of rod_positions (dim. no_samples * no_rods) as input
*/
auto averaged_rho_dist(std::vector<std::vector<double>> x_matrix) {

    auto bins = axis::regular<double, use_default, axis::option::none_t>{ axis::step(cell_length), 0, L };

    auto h_rho = make_histogram(bins); //initialise histogram of length L with bins of width cell_length

    for (unsigned i = 0; i < x_matrix.size(); i++) {
        for (unsigned i = 0; i < x_matrix[i].size(); i++) {
            std::for_each(x_matrix[i].begin(), x_matrix[i].end(), std::ref(h_rho));
        }
    }

    return h_rho / x_matrix.size();
}


#include <CGAL/Simple_cartesian.h>

#include <CGAL/Surface_mesh.h>
#include <CGAL/boost/graph/graph_traits_Surface_mesh.h>

#include <CGAL/subdivision_method_3.h>
#include <CGAL/Timer.h>

#include <boost/lexical_cast.hpp>

#include <iostream>
#include <fstream>

typedef CGAL::Simple_cartesian<double>          Kernel;
typedef CGAL::Surface_mesh<Kernel::Point_3>     PolygonMesh;

using namespace std;
using namespace CGAL;
namespace params = CGAL::parameters;

int main(int argc, char **argv) {
  if (argc > 4) {
    cerr << "Usage: Sqrt3_subdivision [d] [filename_in] [filename_out] \n";
    cerr << "         d -- the depth of the subdivision (default: 1) \n";
    cerr << "         filename_in -- the input mesh (.off) (default: data/quint_tris.off) \n";
    cerr << "         filename_out -- the output mesh (.off) (default: result.off)" << endl;
    return 1;
  }

  int d = (argc > 1) ? boost::lexical_cast<int>(argv[1]) : 2;
  const std::string in_file = (argc > 2) ? argv[2] : CGAL::data_file_path("meshes/quint_tris.off");
  const char* out_file = (argc > 3) ? argv[3] : "result.off";

  PolygonMesh pmesh;
  std::ifstream in(in_file);
  if(in.fail()) {
    std::cerr << "Could not open input file " << in_file << std::endl;
    return 1;
  }
  in >> pmesh;

  Timer t;
  t.start();
  Subdivision_method_3::Sqrt3_subdivision(pmesh, params::number_of_iterations(d));
  std::cerr << "Done (" << t.time() << " s)" << std::endl;

  std::ofstream out(out_file);
  out << pmesh;

  return 0;
}