#include <iostream>
#include <ctime>
#include <sys/time.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <genecis/math/matrix.h>

using namespace std ;
using namespace genecis::math ;

int main() {

//	matrix<int> mt(3,3) ;
//	mt(0,0) = 1 ; mt(0,1) = 4 ; mt(0,2) = 7 ;
//	mt(1,0) = 0 ; mt(1,1) = 1 ; mt(1,2) = 0 ;
//	mt(2,0) = 0 ; mt(2,1) = 0 ; mt(2,2) = 1 ;
//	cout << "mt: " << mt << endl ;

//	srand(time(NULL)) ;
	matrix<double> mymatx(5,5) ;
	for(unsigned i=0; i<mymatx.rows(); ++i) {
		for(unsigned j=0; j<mymatx.cols(); ++j) {
			mymatx(i,j) = rand() % 15 ;
			mymatx(i,j) = (16+i*8 - j*2) % 7 ;
		}
	}
//	cout << "mymatx: " << mymatx << endl ;
//	mymatx.resize(2,2) ;
//	cout << "resized:" << mymatx << endl ;
//	for(unsigned i=0; i<mymatx.rows(); ++i) {
//		mymatx(i,i) = 1 ;
//	}
//	matrix<double> omatx(5,1) ;
//	for(unsigned i=0; i<omatx.rows(); ++i) {
//		for(unsigned j=0; j<omatx.cols(); ++j) {
//			omatx(i,j) = i+j ;
//		}
//	}

//	bool result = typeid(double) == typeid(int) ;
//	cout << "double == int: " ;
//	cout << ((result) ? "true" : "false") << endl ;

//	mymatx.transpose() ;
//	cout << "transpose: " << mymatx << endl ;
//	cout << "omatx: " << omatx << endl ;
//	omatx.transpose() ;
//	cout << "transpose: " << omatx << endl ;
//	omatx = mymatx ;
//	cout << "reassign: " << omatx << endl ;

//	matrix<double> mtx ;
//	mtx.transpose() ;
//	cout << "mtx: " << mtx << endl ;
//	mtx *= omatx ;
//	cout << "after *= mtx: " << mtx << endl ;
//	omatx *= mymatx ;
//	cout << "new omatx: " << omatx << endl ;
//	mtx = &omatx ;
	matrix<double> mtx(2,2) ;
	for(unsigned i=0; i<mtx.rows(); ++i) {
		for(unsigned j=0; j<mtx.cols(); ++j) {
			mtx(i,j) = i*900+j*100 ;
		}
	}
//	cout << "mtx: " << mtx << endl ;
//	(*mtx)(0,0) = 2 ;
//	(*mtx)^2 ;
//	cout << "mtx^2: " << *mtx << endl ;
//
	matrix<double> dmtx(2,2) ;
	for(unsigned i=0; i<dmtx.rows(); ++i) {
		for(unsigned j=0; j<dmtx.cols(); ++j) {
			dmtx(i,j) = i+j*0.0234+1 ;
		}
	}
	cout << "dmtx: " << dmtx << endl ;
//	dmtx.swap_row(0,1) ;
//	cout << "dmtx after swap_row: " << dmtx << endl ;
//	dmtx.swap_col(0,1) ;
//	cout << "dmtx after swap_col: " << dmtx << endl ;
//
//	omatx = omatx + omatx ;
//	cout << "+omatx: " << omatx << endl ;
//	dmtx.inverse() ;
	dmtx.lu_decomp(mtx,mymatx) ;
	cout << "dmtx decomp:\nupper" << mtx << endl ;
	cout << "lower" << mymatx << endl ;
	matrix<double> lu = matrix_product(mymatx,mtx) ;
	cout << "LU" << lu << endl ;
	matrix<double> t = mymatx * mtx ;
	cout << "t: " << t << endl ;

//	matrix<double> omatx2 ;
//	omatx.transpose(omatx2) ;
//	cout << "omatx2: " << omatx2 << endl ;
//	cout << "omatx: " << omatx << endl ;

//	srand(time(0)) ;
//	unsigned N = 10 ;
//	matrix<double> m(N,N) ;
//	matrix<double> m2(N,N) ;
//	boost::numeric::ublas::matrix<double> m3(N,N) ;
//	for(unsigned i=0; i<N; ++i) {
//		for(unsigned j=0; j<N; ++j) {
//			m(i,j) = rand() % 10 + 1 ;
//			m2(i,j) = rand() % 10 + 1 ;
//			m3(i,j) = rand() % 10 + 1 ;
//		}
//	}

//	cout << "m: " << m << endl ;
//	cout << "m2: " << m2 << endl ;
//	matrix<double> m3 = m * m2 ;
//	cout << "m3: " << m3 << endl ;
//	if (true) {
//	matrix<double> mn = m-m2 ;
//	cout << "m-m2: " << mn << endl ;
//	delete mn ;
//	}
//	cout << "m: " << m << endl ;
//	cout << "m2: " << m2 << endl ;
//	matrix<double> temp = 2.0 * m2 ;
//	cout << "m2*2.0: " << temp << endl ;
//	cout << "Starting matrix multiply" << endl ;
//	struct timeval time ;
//    struct timezone zone ;
//    double start ;
//    double complete ;
//    double avg = 0 ;
//    for(unsigned i=0; i<N; ++i) {
//	    gettimeofday( &time, &zone ) ;
//    	start = time.tv_sec + time.tv_usec * 1e-6 ;
//		temp = m2 * m2 ;
//		prod(m3, m3) ;
//		m2 *= 4.0 ;
//		gettimeofday( &time, &zone ) ;
//    	complete = time.tv_sec + time.tv_usec * 1e-6 ;
//    	avg += (complete-start) ;
//    	cout << "matrix.h Complete...time: " << (complete-start) << endl ;
//    }
//    cout << "Average execute time: " << avg/N << endl ;
//    cout << "temp: " << temp << endl ;
//    gettimeofday( &time, &zone ) ;
//    start = time.tv_sec + time.tv_usec * 1e-6 ;
//	gettimeofday( &time, &zone ) ;
//    complete = time.tv_sec + time.tv_usec * 1e-6 ;
//    cout << "Ublas Complete...time: " << (complete-start) << endl ;

}


#include <fstream>
#include <mlpack/methods/kmeans/kmeans.hpp>
#include <armadillo>

//g++ k_means.cpp -o kmeans_test -O3 -std=c++11 -larmadillo -lmlpack -lboost_serialization && ./kmeans_test

int main() {

    int k = 2;
    int dim = 2;
    int samples = 50;
    int max_iter = 10;

    arma::mat data(dim, samples, arma::fill::zeros);

    // create data
    int i = 0;
    for(; i < samples / 2; ++i)
    {
        data.col(i) = arma::vec({1, 1}) + 0.25*arma::randn<arma::vec>(dim);
    }
    for(; i < samples; ++i)
    {
        data.col(i) = arma::vec({2, 3}) + 0.25*arma::randn<arma::vec>(dim);
    }

    //cluster the data
    arma::Row<size_t> clusters;
    arma::mat centroids;

    mlpack::kmeans::KMeans<> mlpack_kmeans(max_iter);

    mlpack_kmeans.Cluster(data, k, clusters, centroids);

    centroids.print("Centroids:");

    return 0;
}


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

// Include Files
#include "savgol.h"

#include <Eigen/Dense>
#include <Eigen/LU>
#include <Eigen/Core>
#include <Eigen/QR>
#include <queue>

using namespace Eigen;
using namespace std;

/*Compute the polynomial basis vectors s_0, s_1, s_2 ... s_n using the vandermonde matrix.*/
MatrixXi vander(const int F)
{
  auto v = VectorXi::LinSpaced(F,(-(F-1)/2),((F-1)/2)).transpose().eval();

  MatrixXi A(F, F+1);     //We basically compute an F X F+1 matrix;

  for(auto i = 0; i < F; ++ i)
  {
    for(auto j=1; j < F+1; ++j)
    {
      A(i,j) = pow(v(i), (j-1) );
    }
  }

  A = A.block(0, 1, F, F );   //and retrieve the right F X F matrix block, excluding the first column block to find the vandermonde matrix.

  return A;
}

/*brief Compute the S-Golay Matrix of differentiators
*
*/
MatrixXf sgdiff(int k, int F, double Fd)
{
  //We set the weighting matrix to an identity matrix if no weighting matrix is supplied
  auto W = MatrixXf::Identity(Fd, Fd);

  //Compute Projection Matrix B
  auto s = vander(F);

  //Retrieve the rank deficient matrix from the projection matrix
  auto S = s.block(0, 0, s.rows(), (k+1) ) ;

  //Compute sqrt(W)*S
  auto Sd = S.cast<float> ();    //cast S to float
  auto inter = W * Sd;              //W is assumed to be identity. Change this if you have reasons to.

  //Compute the QR Decomposition
  HouseholderQR<MatrixXf> qr(inter);
  qr.compute(inter);

  FullPivLU<MatrixXf>lu_decomp(inter);      //retrieve rank of matrix

  auto Rank = lu_decomp.rank() ;

  //For rank deficient matrices. The S matrix block will always be rank deficient.
  // MatrixXf Q = qr.householderQ();  //unsused
  MatrixXf R = qr.matrixQR().topLeftCorner(Rank, Rank).template triangularView<Upper>();

  //Compute Matrix of Differentiators
  auto Rinv = R.inverse();
  MatrixXf RinvT = Rinv.transpose();

  MatrixXf G = Sd * Rinv * RinvT;           /*G = S(S'S)^(-1)   -- eqn 8.3.90 (matrix of differentiation filters)*/

  MatrixXf SdT = Sd.transpose().eval();

  MatrixXf B = G * SdT * W;   //SG-Smoothing filters of length F and polynomial order k

  return B;
}

template<typename T>
RowVectorXf savgolfilt(std::queue<T> const & x, int k, int F)
{
  auto DIM = Matrix4f::Zero();        //initialize DIM as a matrix of zeros if it is not supplied
  auto siz = x.size();       //Reshape depth values by working along the first non-singleton dimension

  //Find leading singleton dimensions
  auto Fd = static_cast<double>(F);        //sets the frame size for the savgol differentiation coefficients. This must be odd

  auto B = sgdiff(k, F, Fd);       //retrieve matrix B

  /*Transient On*/
  auto id_size = (F+1)/2 - 1;
  auto Bbutt = B.bottomLeftCorner((F-1)/2, B.cols());

  auto n = Bbutt.rows();
  //flip Bbutt from top all the way down
  MatrixXf Bbuttflipped(n, Bbutt.cols());

    for(auto j = n - 1; j >= 0;)
    {
      for(auto i = 0; i < n ; ++i)
      {
        Bbuttflipped.row(i) = Bbutt.row(j);
        j--;
      }
    }

  //flip x_on up and down as above
  VectorXf x_onflipped(x.size(), 1);  //pre-allocate
  x_onflipped.transpose().eval();

  auto m = x.size();                          //retrieve total # coefficients

    for(auto j = m -1; j >=0;)
    {
      for(auto i = 0; i < m; ++i)
      {
        x_onflipped.row(i) = x.row(j);
        j--;
      }
    }

  VectorXf y_on = Bbuttflipped * x_onflipped;  //Now compute the transient on

 /*Compute the steady state output*/
  size_t idzeroth = floor(B.cols()/2);
  auto Bzeroth = B.col(idzeroth);
  auto Bzerothf = Bzeroth.cast<float>();

  auto y_ss = Bzerothf.transpose().eval() * x;     //This is the steady-state smoothed value

  /*Compute the transient off for non-sequential data*/
  auto Boff = B.topLeftCorner((F-1)/2, B.cols());

  auto p = Boff.rows();                        //flip Boff along the horizontal axis

  MatrixXf Boff_flipped(p, Boff.cols());

  for(auto j = p - 1; j >= 0;)
  {
    for(auto i = 0; i < p ; ++i)
    {
      Boff_flipped.row(i) = Boff.row(j);
      j--;
    }
  }

/*x_off will be the last (F-1) x-values. Note, if you are smoothing in real time, you need to find
  a way to let your compiler pick the last F-length samples from your data in order to compute your x_off.
  You could have the program wait for x_milliseconds before you pick
  the transient off, for example*/
  auto x_off = VectorXf::LinSpaced(F, x(0), x(F-1)).transpose();
  VectorXf x_offflipped(x_off.rows(), x_off.cols());      //pre-allocate
  //flip x_off along the horizontal axis
    auto q = x_off.size();                          //retrieve total # coefficients

    for(auto j = q -1; j >=0;)
    {
      for(auto i = 0; i < q; ++i)
      {
        x_offflipped.row(i) = x.row(j);
        j--;
      }
    }
  auto y_off = Boff_flipped * x_offflipped;   //This is the transient off

  /*Make Y into the shape of X and return the smoothed values!*/
  RowVectorXf y(F);
  y << y_off.transpose().eval(), y_ss, y_on.transpose().eval();

  return y;
}


#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <Eigen/Dense>

#include "DataMnist.h"

using namespace std;
using namespace Eigen;

DataMnist::DataMnist(std::string data_path)
{
    //Initialize DataMnist loader according to directory passed as argument
	_path = data_path;
    //Initialize default file names
	initFileNames();
    //Initialize sizes of arrays according to MNIST specifications
	initSizes();
}

DataMnist::~DataMnist(void)
{
}

int DataMnist::initSizes()
{
	_xtest.resize(test_size);
	_xtrain.resize(train_size);
	_ytest.resize(test_size);
	_ytrain.resize(train_size);
	for (int i = 0; i<test_size; ++i)
	{
		_xtest[i].resize(flat_image_size);
	}
	for (int i = 0; i<train_size; ++i)
	{
		_xtrain[i].resize(flat_image_size);
	}
	return 0;
}

int DataMnist::initFileNames()
{
	_images_file_names.push_back("train-images.idx3-ubyte");
	_images_file_names.push_back("t10k-images.idx3-ubyte");
	_labels_file_names.push_back("train-labels.idx1-ubyte");
	_labels_file_names.push_back("t10k-labels.idx1-ubyte");
	return 0;
}

//This method is a bit long, but we did not had a choice :
//We have to unpack the file in one shot
//as we cannot have access to a precise point in the file.

//beg = 0 ou 1
//if beg = 0, loads xtrain and xtest
//if beg = 1, loads only xtest
//Once it is run on an element of the class Data,
//This element has _xtrain and _xtest as attributes (if beg=0)
//Else it only has _xtest
int DataMnist::loadImages(int beg)
{
	//Test to verify if the users has given a right value
	//If not, default to 1
	if (beg != 0 && beg != 1)
	{
		beg = 1;
	}

	//If beg == 1, this loop only has one iteration
	//As we only load one file (the test one)
	for (int k = beg; k < 2; ++k)
	{
		//The ios::binary parameters is very important here
		//(without it the data we get is all zeros)

		ifstream file ( (_path + _images_file_names[k]).c_str(), ios::binary);

		//test to be sure that the file is open
		if (!file.is_open())
		{
			cerr << "Could not open file" << endl;
			return 1;
		}

		//We initialize variables to unload the header variables of the MNIST database
		//magic_number is useless, but we need to unload it to keep going in the data (cf next commentary)
		int magic_number = 0;
		int number_of_images = 0;
		int number_of_rows = 0;
		int number_of_cols = 0;

		//The method fstream::read() takes two parameters :
		//		- a pointer to an array of char where the characters are stored
		//      (thus we cast &magic_number as a char* for instance)
		//		- the number of character it should extract
		//The method fstream::read() works serially :
		//Once we call it on a file, the next time we will call it on that same file
		//it will continue unloading the data at the point it stopped the previous time
		//Thus we unload the first elements of the database even though they won't be useful
		file.read((char*)&magic_number,sizeof(magic_number));
		file.read((char*)&number_of_images,sizeof(number_of_images));
		file.read((char*)&number_of_rows,sizeof(number_of_rows));
		file.read((char*)&number_of_cols,sizeof(number_of_cols));

		//Storing the data into class attributes for train images
		if (k == 0 )
		{
			for (int i = 0; i < train_size; ++i)
			{
				int count = 0;
				for(int r = 0; r < image_size; ++r)
				{
					for(int c = 0; c < image_size; ++c)
					{
						//In the structure of the MNIST database,
						//pixels are unsigned bytes which can be interpreted as numbers
						//describing the nuance of black of the pixel.
						//As the file.read method needs a pointer to a character
						//to store the data, we create an unsigned char (value between 0 and 255)
						//instead of a char (value between -127 and 128)
						//It is more practical since we want our pixel coloration to take positive value
						unsigned char pix = 0;
						file.read((char*)&pix,sizeof(pix));
						_xtrain[i](count)= (double) pix / 255.;
						++ count;

					}
				}
			}
		}
		//Storing the data for test images
		else
		{
			for (int i = 0; i < test_size; ++i)
			{
				int count = 0;
				for(int r = 0; r < image_size; ++r)
				{
					for(int c = 0; c < image_size; ++c)
					{
						//In the structure of the MNIST database,
						//pixels are unsigned bytes which can be interpreted as numbers
						//describing the nuance of black of the pixel.
						//As the file.read method needs a pointer to a character
						//to store the data, we create an unsigned char (value between 0 and 255)
						//instead of a char (value between -127 and 128)
						//it does not matter since they both correspond to a byte memorywise
						unsigned char pix = 0;
						file.read((char*)&pix,sizeof(pix));
						_xtest[i](count)= (double) pix / 255.;
						++ count;

					}
				}
			}
		}
		cout << "Images loaded successfully" << endl;
	}
	return 0;
}

//Same principle as Data::loadImages with labels
int DataMnist :: loadLabels(int beg)
{
	//Test to verify if the users has given a right value
	//If not, default to 1
	if (beg !=0 && beg !=1)
	{
		beg = 1;
	}

	//If beg == 1, this loop only has one iteration
	//As we only load one file
	for (int k = beg; k < 2; ++k)
	{
		ifstream file ( (_path + _labels_file_names[k]).c_str(), ios::binary);

	//test to be sure that the file is open
		if (!file.is_open())
		{
			cerr << "Could not open file" << endl;
			return 1;
		}

		//We initialize variables to unload the header variables of the MNIST database
		//magic_number is useless, but we need to unload it to keep going in the data (cf next commentary)
		int magic_number = 0;
		int number_of_items = 0;

		file.read((char*)&magic_number,sizeof(magic_number));
		file.read((char*)&number_of_items,sizeof(number_of_items));

		//Storing the data
		if (k==0)
		{
			for (int i = 0; i < train_size ; ++i)
			{
				unsigned char temporary = 0;
				file.read((char*)&temporary,sizeof(temporary));
				_ytrain[i]=temporary;
			}
		}
		else
		{
			for (int i = 0; i < test_size ; ++i)
			{
				unsigned char temporary = 0;
				file.read((char*)&temporary,sizeof(temporary));
				_ytest[i]=temporary;
			}
		}
		cout << "Labels loaded successfully" << endl;

	}
	return 0;
}

//Accessors
int DataMnist::trainLabel (int i)
{
	return _ytrain[i];
}

int DataMnist::testLabel (int i)
{
	return _ytest[i];
}

Eigen::VectorXd DataMnist::trainImage (int i)
{
	return _xtrain[i];
}

Eigen::VectorXd DataMnist::testImage (int i)
{
	return _xtest[i];
}

vector<VectorXd> DataMnist::GetTrainImages()
{
    return _xtrain;
}

vector<int> DataMnist::GetTrainLabels()
{
    return _ytrain;
}


#define BOOST_TEST_MODULE matrix

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/matrix/all.h++>
#include <mla/matrix/convert.h++>

typedef boost::mpl::list<
	mla::matrix::DenseRowMajor<float>,
	mla::matrix::DenseRowMajor<double>,
	mla::matrix::SparseDOK<float>,
	mla::matrix::SparseDOK<double>,
	mla::matrix::SparseCOO<float>,
	mla::matrix::SparseCOO<double>,
	mla::matrix::SparseCRS<float>,
	mla::matrix::SparseCRS<double>,
	mla::matrix::SparseCCS<float>,
	mla::matrix::SparseCCS<double>
> matrix_type_list;

BOOST_AUTO_TEST_SUITE(test_matrix)

BOOST_AUTO_TEST_CASE_TEMPLATE( matrix_convert, MatrixTypeTo, matrix_type_list )
{
	size_t matrix_size = 6;

	mla::matrix::DenseRowMajor<float> from(matrix_size, matrix_size);

	from(0,0) = (typename MatrixTypeTo::scalar_type)1;
	from(0,3) = (typename MatrixTypeTo::scalar_type)3;
	from(3,0) = (typename MatrixTypeTo::scalar_type)3;
	from(2,3) = (typename MatrixTypeTo::scalar_type)5;

	MatrixTypeTo to(matrix_size, matrix_size);

	mla::matrix::convert(from, to);

	BOOST_CHECK_EQUAL(from.rows(), to.rows());
	BOOST_CHECK_EQUAL(from.columns(), to.columns());

	for(size_t i = 0; i < matrix_size; i++)
	{
		for(size_t j = 0; i < matrix_size; i++)
		{
			BOOST_CHECK_CLOSE( from.getValue(i, j), to.getValue(i,j), 0.001f );
		}
	}
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * gauss_packet.cpp
 *
 * Schroedinger equation with potential barrier and periodic boundary conditions
 * Initial Gauss packet moving to the right
 *
 * pipe output into gnuplot to see animation
 *
 * Implementation of Hamilton operator via MTL library
 *
 * Copyright 2011-2013 Mario Mulansky
 * Copyright 2011-2012 Karsten Ahnert
 *
 * 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)
 */

#include <iostream>
#include <complex>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/mtl4/mtl4.hpp>

#include <boost/numeric/mtl/mtl.hpp>

using namespace std;
using namespace boost::numeric::odeint;

typedef mtl::dense_vector< complex< double > > state_type;

struct hamiltonian {

    typedef mtl::compressed2D< complex< double > > matrix_type;
    matrix_type m_H;

    hamiltonian( const int N ) : m_H( N , N )
    {
        // constructor with zero potential
        m_H = 0.0;
        initialize_kinetic_term();
    }

    //template< mtl::compressed2D< double > >
    hamiltonian( mtl::compressed2D< double > &V ) : m_H( num_rows( V ) , num_cols( V ) )
    {
        // use potential V in hamiltonian
        m_H = complex<double>( 0.0 , -1.0 ) * V;
        initialize_kinetic_term();
    }

    void initialize_kinetic_term( )
    {
        const int N = num_rows( m_H );
        mtl::matrix::inserter< matrix_type , mtl::update_plus< complex<double> > > ins( m_H );
        const double z = 1.0;
        // fill diagonal and upper and lower diagonal
        for( int i = 0 ; i<N ; ++i )
        {
            ins[ i ][ (i+1) % N ] << complex< double >( 0.0 , -z );
            ins[ i ][ i ] << complex< double >( 0.0 , z );
            ins[ (i+1) % N ][ i ] << complex< double >( 0.0 , -z );
        }
    }

    void operator()( const state_type &psi , state_type &dpsidt , const double t )
    {
        dpsidt = m_H * psi;
    }

};

struct write_for_gnuplot
{
    size_t m_every , m_count;

    write_for_gnuplot( size_t every = 10 )
    : m_every( every ) , m_count( 0 ) { }

    void operator()( const state_type &x , double t )
    {
        if( ( m_count % m_every ) == 0 )
        {
            //clog << t << endl;
            cout << "p [0:" << mtl::size(x) << "][0:0.02] '-'" << endl;
            for( size_t i=0 ; i<mtl::size(x) ; ++i )
            {
                cout << i << "\t" << norm(x[i]) << "\n";
            }
            cout << "e" << endl;
        }

        ++m_count;
    }
};

static const int N = 1024;
static const int N0 = 256;
static const double sigma0 = 20;
static const double k0 = -1.0;

int main( int argc , char** argv )
{
    state_type x( N , 0.0 );

    // initialize gauss packet with nonzero velocity
    for( int i=0 ; i<N ; ++i )
    {
        x[i] = exp( -(i-N0)*(i-N0) / ( 4.0*sigma0*sigma0 ) ) * exp( complex< double >( 0.0 , k0*i ) );
        //x[i] += 2.0*exp( -(i+N0-N)*(i+N0-N) / ( 4.0*sigma0*sigma0 ) ) * exp( complex< double >( 0.0 , -k0*i ) );
    }
    x /= mtl::two_norm( x );

    typedef runge_kutta4< state_type > stepper;

    // create potential barrier
    mtl::compressed2D< double > V( N , N );
    V = 0.0;
    {
        mtl::matrix::inserter< mtl::compressed2D< double > > ins( V );
        for( int i=0 ; i<N ; ++i )
        {
            //ins[i][i] << 1E-4*(i-N/2)*(i-N/2);

            if( i < N/2 )
                ins[ i ][ i ] << 0.0 ;
            else
                ins[ i ][ i ] << 1.0 ;

        }
    }

    // perform integration, output can be piped to gnuplot
    integrate_const( stepper() , hamiltonian( V ) , x , 0.0 , 1000.0 , 0.1 , write_for_gnuplot( 10 ) );

    clog << "Norm: " << mtl::two_norm( x ) << endl;

    return 0;
}


#include <stan/math/fwd/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/digamma.hpp>
#include <math/fwd/scal/fun/nan_util.hpp>

TEST(AgradFwdLogFallingFactorial, Fvar) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::log_falling_factorial;

  fvar<double> a(4.0, 1.0);
  fvar<double> x = log_falling_factorial(a, 2);
  EXPECT_FLOAT_EQ(std::log(12.0), x.val_);
  EXPECT_FLOAT_EQ((boost::math::digamma(5) - boost::math::digamma(3)), x.d_);

  // finite diff
  double eps = 1e-6;
  EXPECT_FLOAT_EQ((stan::math::log_falling_factorial(4.0 + eps, 2.0)
                   - stan::math::log_falling_factorial(4.0 - eps, 2.0))
                      / (2 * eps),
                  x.d_);

  fvar<double> c(-3.0, 2.0);

  EXPECT_THROW(log_falling_factorial(c, 2), std::domain_error);
  EXPECT_THROW(log_falling_factorial(c, c), std::domain_error);

  x = log_falling_factorial(a, a);
  EXPECT_FLOAT_EQ(std::log(24.0), x.val_);
  EXPECT_FLOAT_EQ(boost::math::digamma(5), x.d_);

  x = log_falling_factorial(5, a);
  EXPECT_FLOAT_EQ(std::log(120.0), x.val_);
  EXPECT_FLOAT_EQ(digamma(2.0), x.d_);

  // finite diff
  EXPECT_FLOAT_EQ((stan::math::log_falling_factorial(5.0, 4.0 + eps)
                   - stan::math::log_falling_factorial(5.0, 4.0 - eps))
                      / (2 * eps),
                  x.d_);
}

TEST(AgradFwdLogFallingFactorial, FvarFvarDouble) {
  using stan::math::fvar;
  using stan::math::log_falling_factorial;

  fvar<fvar<double> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;

  fvar<fvar<double> > y;
  y.val_.val_ = 3.0;
  y.d_.val_ = 1.0;

  fvar<fvar<double> > a = log_falling_factorial(x, y);

  EXPECT_FLOAT_EQ(3.1780539, a.val_.val_);
  EXPECT_FLOAT_EQ(1.0833334, a.val_.d_);
  EXPECT_FLOAT_EQ(0.42278433, a.d_.val_);
  EXPECT_FLOAT_EQ(0.64493406, a.d_.d_);
}

struct log_falling_factorial_fun {
  template <typename T0, typename T1>
  inline typename boost::math::tools::promote_args<T0, T1>::type operator()(
      const T0 arg1, const T1 arg2) const {
    return log_falling_factorial(arg1, arg2);
  }
};

TEST(AgradFwdLogFallingFactorial, nan_0) {
  log_falling_factorial_fun log_falling_factorial_;
  test_nan_fwd(log_falling_factorial_, 3.0, 5.0, false);
}


/*
 * poisson test.
 *
 * Takaaki MINOMO.
 */

#include <iostream>
#include <cmath>
#include <array>
#include <Eigen/Sparse>

constexpr int N = 10;
/*
namespace rittai3d{
	namespace utility{
		// [ minimum, maximum ) の範囲でラップアラウンド
		template <typename T>
		constexpr T wrap_around(T value, T minimum, T maximum){
			const T n = (value - minimum) % (maximum - minimum);
			return n >= 0 ? (n + minimum) : (n + maximum);
		}
	}
}

namespace sksat {
	template<std::size_t Num, typename T = double>
	class array_wrapper{
	private:
		std::array<T, Num> arr;
	public:
		constexpr array_wrapper() : arr() {}
		~array_wrapper() = default;

		T& operator[](int i){
			i = rittai3d::utility::wrap_around(i, 0, static_cast<int>(Num));
			return arr[i];
		}
	};
}

using extendedArray = sksat::array_wrapper<N + 1, sksat::array_wrapper<N + 1>>;
*/
int main(){

    Eigen::SparseMatrix<double> A(N, N);
    Eigen::VectorXd b(N), x(N);

    for(int i=0; i<N; ++i){
        b(i) = 0.0;
    }

    b(0)   = 0.0;
    b(N-1) = - 1.0;

    for(int i=0; i<N; ++i){
        for(int j=0; j<N; ++j){
            if(i == j){
                A.insert(i,j) = - 2.0;
            }else if(i == (j+1) || i == (j-1)){
                A.insert(i,j) = 1.0;
            }else{
                A.insert(i,j) = 0.0;
            }
        }
    }

    std::cout << A << std::endl;
    std::cout << std::endl;
    std::cout << b << std::endl;
    std::cout << std::endl;
    Eigen::ConjugateGradient<Eigen::SparseMatrix<double> > cg;
    cg.compute(A);
    x = cg.solve(b);

    std::cout << x << std::endl;

}


#pragma once
// kalman.hpp
//
// Copyright (C) 2017-2018 Stillwater Supercomputing, Inc.
//
// This file is part of the HPR-BLAS project, which is released under an MIT Open Source license.
#include <boost/numeric/mtl/mtl.hpp>

template<typename Ty>
class KalmanFilter {
public:

	/**
	* Create a Kalman filter with the specified matrices.
	*   A - System dynamics matrix
	*   C - Output matrix
	*   Q - Process noise covariance
	*   R - Measurement noise covariance
	*   P - Estimate error covariance
	*/
	KalmanFilter(
		double dt,
		const mtl::dense2D<Ty>& A,
		const mtl::dense2D<Ty>& C,
		const mtl::dense2D<Ty>& Q,
		const mtl::dense2D<Ty>& R,
		const mtl::dense2D<Ty>& P
	) : A(A), C(C), Q(Q), R(R), P0(P),
		m(num_rows(C)), n(num_rows(A)), dt(dt), initialized(false),
		I(n, n), x_hat(n), t(0), t0(0)
	{
		I = mtl::mat::identity2D(m,n); // I.setIdentity();
	}
	KalmanFilter() {}

	/**
	* Initialize the filter with initial states as zero.
	*/
	void init() {
		x_hat.setToZero();
		P = P0;
		t0 = 0;
		t = t0;
	}

	/**
	* Initialize the filter with a guess for initial states.
	*/
	void init(double _t0, const mtl::dense_vector<Ty>& _x0) {
		x_hat = _x0;
		P = P0;
		t0 = _t0;
		t = t0;
	}

	/**
	* Update the estimated state based on measured values. The
	* time step is assumed to remain constant.
	*/
	void update(const mtl::dense_vector<Ty>& y) {
		mtl::dense_vector<Ty> x_hat_new(n);
		x_hat_new = A * x_hat;
//		P = A * P * trans(A) + Q;
//		K = P * trans(C) * (C * P * trans(C) + R).inverse();
//		x_hat_new += K * (y - C * x_hat_new);
//		P = (I - K * C) * P;
		x_hat = x_hat_new;
	}

	/**
	* Update the estimated state based on measured values,
	* using the given time step and dynamics matrix.
	*/
	void update(const mtl::dense_vector<Ty>& _y, double _dt, const mtl::dense2D<Ty> _A) {
		A = _A;
		dt = _dt;
		update(_y);
	}

	/**
	* Return the current state and time.
	*/
	mtl::dense_vector<Ty> state() { return x_hat; };
	double time() { return t; };

private:
	bool initialized;

	// Matrices for computation
	mtl::dense2D<Ty> A, C, Q, R, P, K, P0;

	// System dimensions
	size_t m, n;

	// Initial and current time
	double t0, t;

	// Discrete time step
	double dt;

	// n-size identity
	mtl::dense2D<Ty> I;

	// Estimated states
	mtl::dense_vector<Ty> x_hat;
};


//
//  cal_Bmat.hpp
//  hybrid_fem_bie
//
//  Created by Max on 3/21/18.
//
//

#ifndef cal_Bmat_hpp
#define cal_Bmat_hpp

#include <stdio.h>
#include <Eigen/Eigen>

using namespace Eigen;

void cal_Bmat(MatrixXd coord ,double E, double nu, std::vector<Eigen::MatrixXd> &B_mat, double &detJ);

#endif /* cal_Bmat_hpp */


#pragma once

#include <Eigen/Geometry>

#include <autodiff/autodiff.h>
#include <interval/interval.hpp>
#include <utils/eigen_ext.hpp>

namespace ipc::rigid {

double sinc(const double& x);

Interval sinc(const Interval& x);

/// Compute the L2 norm (∑|xᵢ|²)
template <typename T> T norm(const VectorMax3<T>& x)
{
    // Do an explicit abs to avoid possible problems with intervals
    VectorMax3<T> absx(x.size());
    for (int i = 0; i < x.size(); i++) {
        absx(i) = abs(x(i));
    }
    return sqrt(absx.dot(absx));
}

/// Compute sinc(||x||) for x ∈ Rⁿ
template <typename T> T sinc_normx(const VectorMax3<T>& x)
{
    static_assert(
        !std::is_base_of<DiffScalarBase, T>::value,
        "This version does not work with autodiff!");
    return sinc(norm(x));
}

/// Compute ∇sinc(||x||) for x ∈ Rⁿ
VectorMax3d sinc_normx_grad(const VectorMax3d& x);

/// Compute ∇²sinc(||x||) for x ∈ Rⁿ
MatrixMax3d sinc_normx_hess(const VectorMax3d& x);

template <typename Scalar, typename Gradient>
DScalar1<Scalar, Gradient>
sinc_normx(const VectorMax3<DScalar1<Scalar, Gradient>>& x)
{
    const int m = DiffScalarBase::getVariableCount(), n = x.size();

    // Extract the vector of values
    VectorMax3<Scalar> x_vals(n);
    for (int i = 0; i < n; i++) {
        x_vals(i) = x(i).getValue();
    }

    // Compute the value and gradient (without chain-rule)
    Scalar value = sinc_normx(x_vals);
    VectorMax3d grad = sinc_normx_grad(x_vals);

    // Apply chain-rule
    Gradient full_grad = Gradient::Zero(m);
    for (int j = 0; j < n; j++) {
        full_grad += grad(j) * x(j).getGradient();
    }

    return DScalar1<Scalar, Gradient>(value, full_grad);
}

template <typename Scalar, typename Gradient, typename Hessian>
DScalar2<Scalar, Gradient, Hessian>
sinc_normx(const VectorMax3<DScalar2<Scalar, Gradient, Hessian>>& x)
{
    const int m = DiffScalarBase::getVariableCount(), n = x.size();

    // Extract the vector of values
    VectorMax3<Scalar> x_vals(n);
    for (int i = 0; i < n; i++) {
        x_vals(i) = x(i).getValue();
    }

    // Compute the value, gradient, and hessian (without chain-rule)
    Scalar value = sinc_normx(x_vals);
    VectorMax3d grad = sinc_normx_grad(x_vals);
    MatrixMax3d hess = sinc_normx_hess(x_vals);

    // Apply chain-rule
    Gradient full_grad = Gradient::Zero(m);
    for (int k = 0; k < n; k++) {
        full_grad += grad(k) * x(k).getGradient();
    }

    Hessian full_hess = Hessian::Zero(m, m);
    for (int k = 0; k < n; k++) {
        for (int l = 0; l < n; l++) {
            full_hess += hess(k, l) * x(l).getGradient()
                * x(k).getGradient().transpose();
        }
        full_hess += grad(k) * x(k).getHessian();
    }

    return DScalar2<Scalar, Gradient, Hessian>(value, full_grad, full_hess);
}

} // namespace ipc::rigid


#ifndef TVMTL_MANIFOLD_SON_HPP
#define TVMTL_MANIFOLD_SON_HPP

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

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/SVD>
#include <unsupported/Eigen/MatrixFunctions>
#include <unsupported/Eigen/KroneckerProduct>

#include "enumerators.hpp"
#include "matrix_utils.hpp"

namespace tvmtl {

// Specialization SO(N)
template <int N>
struct Manifold< SO, N> {

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

	static const bool non_isometric_embedding;

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

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

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

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

	typedef Eigen::Matrix<scalar_type, N*N, N*N>				deriv2_type;
	typedef deriv2_type&							deriv2_ref_type;
	typedef	Eigen::Matrix<scalar_type, N * (N - 1) / 2, N * (N - 1) / 2>	restricted_deriv2_type;

	// Helper Types
	typedef Eigen::PermutationMatrix<N*N, N*N, int> perm_type;

	inline static perm_type ConstructPermutationMatrix();

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

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

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

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

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

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

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

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

};

/*-----IMPLEMENTATION SO----------*/

// Static constants, Outside definition to avoid linker error

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

template <int N>
const int Manifold < SO, N>::manifold_dim = N * (N - 1) / 2;

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

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

// PermutationMatrix
template <int N>
typename Manifold < SO, N>::perm_type Manifold<SO, N>::ConstructPermutationMatrix(){
    perm_type P;
    P.setIdentity();
    for(int i=0; i<N; i++)
	for(int j=0; j<i; j++)
	    P.applyTranspositionOnTheRight(j*N + i, i*N + j);
    return P;
}

template <int N>
const typename Manifold < SO, N>::perm_type Manifold<SO, N>::permutation_matrix = ConstructPermutationMatrix();

// Squared SO distance function
template <int N>
inline typename Manifold < SO, N>::dist_type Manifold < SO, N>::dist_squared( cref_type x, cref_type y ){
    #ifdef TV_SON_DEBUG
	std::cout << "\nDist2 function with x=\n" << x << "\nand y=\n" << y << std::endl;
    #endif
    return (x.transpose() * y).log().squaredNorm();
}

// Derivative of Squared SO distance w.r.t. first argument
template <int N>
inline void Manifold < SO, N>::deriv1x_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    result = -2.0 * x * (x.transpose() * y).log();
}
// Derivative of Squared SO distance w.r.t. second argument
template <int N>
inline void Manifold < SO, N>::deriv1y_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    result =  -2.0 * y * (y.transpose() * x).log();
}

// Second Derivative of Squared SO distance w.r.t first argument
template <int N>
inline void Manifold < SO, N>::deriv2xx_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    value_type XtY = x.transpose()*y;

    deriv2_type logXtY_kron_I ,I_kron_x, Yt_kron_I, dlog;
    logXtY_kron_I = Eigen::kroneckerProduct(XtY.log().eval(),value_type::Identity());
    I_kron_x = Eigen::kroneckerProduct(value_type::Identity(),x);
    Yt_kron_I = Eigen::kroneckerProduct(y.transpose(),value_type::Identity());
    KroneckerDLog(XtY, dlog);

    result = -2.0 * (logXtY_kron_I.transpose() + I_kron_x * dlog * Yt_kron_I * permutation_matrix );
}
// Second Derivative of Squared SO distance w.r.t first and second argument
template <int N>
inline void Manifold < SO, N>::deriv2xy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    value_type XtY = x.transpose()*y;

    deriv2_type I_kron_x, dlog;
    I_kron_x = Eigen::kroneckerProduct(value_type::Identity(),x);
    KroneckerDLog(XtY, dlog);

    result = -2.0 * I_kron_x * dlog * I_kron_x.transpose();
}
// Second Derivative of Squared SO distance w.r.t second argument
template <int N>
inline void Manifold < SO, N>::deriv2yy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    deriv2xx_dist_squared(y, x, result);
}

// Exponential and Logarithm Map
template <int N>
template <typename DerivedX, typename DerivedY>
inline void Manifold <SO, N>::exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedX>& result){
    result = x * (x.transpose() * y).exp();
}

template <int N>
inline void Manifold <SO, N>::log(cref_type x, cref_type y, ref_type result){
    result = x * (x.transpose() * y).log();
}

// Tangent Plane restriction
template <int N>
inline void Manifold <SO, N>::tangent_plane_base(cref_type x, tm_base_ref_type result){
    int d = value_type::RowsAtCompileTime;
    int k = 0;

    value_type T;

    for(int i=0; i<d-1; i++)
	for(int j=i+1; j<d; j++){
	    T.setZero();
	    scalar_type sqrt = 1.0/std::sqrt(2);
	    T.col(i) = -sqrt * x.col(j);
	    T.col(j) =  sqrt * x.col(i);

	    result.col(k) = Eigen::Map<Eigen::VectorXd>(T.data(), T.size());
	    k++;
	}
}

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

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

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

}

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

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

}

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

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

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

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

    log(x, y1, temp1);

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

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

    #ifdef TV_SON_DEBUG
	std::cout << "\n\nProjector with initial x=\n" << x << std::endl;
    #endif

    Eigen::JacobiSVD<value_type> svd(x, Eigen::ComputeThinU | Eigen::ComputeThinV);
    x = svd.matrixU() * svd.matrixV().transpose();
    //if(x.determinant() < 0)
    //	x.row(1)*=-1.0;

    #ifdef TV_SON_DEBUG
	std::cout << "\nProjector with final x=\n" << x << std::endl;
    #endif
}

} // end namespace tvmtl

#endif


/*****************************************************************************
*
* Copyright (C) 2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>
*
* 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)
*
*****************************************************************************/

#include <iomanip>
#include <iostream>
#include <cmath>
#include <boost/math/constants/constants.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>

int main() {
  using std::abs; using std::sqrt;
  typedef double real_t;

  auto f = [](real_t x) { return sqrt(1 - x * x); };
  real_t r = boost::math::constants::pi<real_t>() / 2;

  boost::math::quadrature::tanh_sinh<real_t> integrator;
  real_t termination = sqrt(std::numeric_limits<real_t>::epsilon());
  real_t error, L1;
  size_t levels;
  real_t q = integrator.integrate(f, -1, 1, termination, &error, &L1, &levels);

  std::cout << std::scientific << std::setprecision(std::numeric_limits<real_t>::digits10)
            << "result: " << q << std::endl
            << "estimated error: " << error << std::endl
            << "real error: " << abs(q - r) << std::endl
            << "L1 * error: " << L1 * error << std::endl
            << "levels: " << levels << std::endl;
}


#include <boost/program_options.hpp>
#include <iostream>
#include <fstream>
#include <vector>
#include <cassert>
#include <cmath>
#include <stdlib.h>
#include <stdint.h>
#include <random>
#include <algorithm>    // std::min_element, std::max_element

#include "structures.h"
#include "clear_ciffers.h"

// Questions 2 and 3
// Display the linear approximation matrix.
void linearApproximationMatrix(int nbElt=16) {
    std::vector<std::pair<int,int>> goodKeys ;
    std::cout << "\\[\n\\bordermatrix{\n& " ;
    for(int b = 0 ; b < nbElt-1 ; b++) {
        std::cout << b << " & " ;
    }
    std::cout << nbElt-1 << "\\cr\n" ;
    for(int a = 0 ; a < nbElt ; a++) {
        std::cout << a << " & " ;
        for(int b = 0 ; b < nbElt ; b++) {
            int nbKey = 0 ;
            for(int c = 0 ; c < nbElt ; c++) {
                Block blockA(a) ;
                Block blockB(b) ;
                Block key(c) ;
                blockA.product(key) ;
                key.substitution() ;
                blockB.product(key) ;
                if(blockA.bitsXor() == blockB.bitsXor()) {
                    nbKey ++ ;
                }
            }
            std::cout << nbKey ;
            double proba = ((double)nbKey)/16 ;
            if(std::abs(proba - 0.5) > 0.3) {
                // std::cout << nbKey ;
                goodKeys.push_back(std::pair<int,int>(a, b)) ;
            }
            if(b < nbElt-1)
                std::cout << " & " ;
        }
        std::cout << "\\cr\n" ;
    }
    std::cout << "}\n\\]\n";
    std::cout << "Couples (a, b) with the highest probability:\n\\[" ;
    if(goodKeys.size() > 0)
        std::cout << "(" << goodKeys[0].first << ", " << goodKeys[0].second << ")" ;
    for(unsigned i = 1 ; i < goodKeys.size() ; i++) {
        std::cout << ", (" << goodKeys[i].first << ", " << goodKeys[i].second << ")" ;
    }
    std::cout << "\\]\n" ;
}

// Question 4
// Check experimentaly that Prob(a.m = P(b).c) is in {1/2-1/8, 1/2+1/8}
void experimentalCheck(uint32_t a=1, uint32_t b=5) {
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<unsigned int> dis(0, UINT32_MAX);
    Block A(a<<28);
    Block B(b<<28);
    B.permutation() ;
    // int nbMess = 1000000 ;
    int nbMess = 100000 ;
    for(int i = 0 ; i < 10 ; i++) {
        Block K0(dis(gen)) ;
        Block K1(dis(gen)) ;
        int nbEqual = 0 ;
        for(int j = 0 ; j < nbMess ; j++) {
            Block m(dis(gen)) ;
            Block x(m) ;
            x.addition(K0) ;
            x.turn(K1) ;
            m.product(A) ;
            x.product(B) ;
            if(m.bitsXor() == x.bitsXor())
                nbEqual ++ ;
        }
        std::cout << "& $" << (double)nbEqual/(double)nbMess << "$ ";//std::endl ;
    }
    std::cout << "\\\\" << std::endl ;
}

// Questions 7-8
// Guess the block of given index of the key K2.
int guessKeyBox(int blockIndex, uint32_t a=4, uint32_t b=8) {
    std::vector<int> keyCount(16, 0) ;
    Block A(a<<(blockIndex*4)) ;
    Block B(b<<(blockIndex*4)) ;
    B.permutation(2) ;
    // Compute the distribution of each key over all the couples plaintext/ciphertext
    for(unsigned int i = 0 ; i < Plaintext.size() ; i++) {
        Block M(Plaintext[i]) ;
        Block C(Ciphertext[i]) ;
        C.permutation(-2) ;
        for(uint32_t k = 0 ; k < 16 ; k++) {
            Block K(k<<(blockIndex*4)) ;
            K.addition(C) ;
            K.substitution(REVERSE_DEFAULT_SUBST) ;
            Block Mcopy(M) ;
            Mcopy.product(A) ;
            K.product(B) ;
            if(Mcopy.bitsXor() == K.bitsXor())
                keyCount[k] ++ ;
        }
    }
    // The key with the distribution the farthest from 1/2 is probably the key that we are looking for.
    int kmin = std::min_element(keyCount.begin(), keyCount.end()) - keyCount.begin();
    int kmax = std::max_element(keyCount.begin(), keyCount.end()) - keyCount.begin();
    if(std::abs((double)Plaintext.size()/2 - keyCount[kmin]) > std::abs((double)Plaintext.size()/2 - keyCount[kmax]))
        return kmin ;
    else
        return kmax ;
}

// Questions 7-8
// Guess the key K2 block by block.
Block guessK2() {
    Block K(0) ;
    for(int i = 0 ; i < 8 ; i++) {
        K.setBox(i, guessKeyBox(i)) ;
    }
    K.permutation() ;
    return K ;
}

// Check if the given keys are the keys which crypted the table Plaintext.
bool checkSolution(Block K0, Block K1, Block K2) {
    unsigned int i ;
    for(i = 0 ; i < Plaintext.size() ; i++) {
        Block m(Plaintext[i]) ;
        Block c(Ciphertext[i]) ;
        m.encrypt(K0, K1, K2) ;
        if(m.getBits() != c.getBits())
            break ;
    }
    return i == Plaintext.size() ;
}

// Not in the subject
// Retrieve the keys with a brute force attack.
// Too long, not fully tested.
void bruteForce() { // too long
    uint32_t k = 0 ;
    Block K0, K1, K2 ;
    while(1) {
        if(k%10000 == 0)
            std::cout << k << "/" << UINT32_MAX << std::endl ;
        Block K(k) ;
        K.generateSubKeys(&K0, &K1, &K2) ;
        if(checkSolution(K0, K1, K2))
            break ;
        k++ ;
    }
    std::cout << "Found keys by brute force:" << std::endl ;
    std::cout << "K  = " << k << std::endl ;
    std::cout << "K0 = " << K0.getBits() << std::endl ;
    std::cout << "K1 = " << K1.getBits() << std::endl ;
    std::cout << "K2 = " << K2.getBits() << std::endl ;
}

// Question 9
// Find the keys with the method of the actives boxes.
Block guessKey() {
    int bit ;
    std::vector<int> K2Subst = DEFAULT_K2_SUBST ;
    Block K2 = guessK2() ;
    Block K ;
    std::vector<bool> knownBit(32, false) ;
    // Build the key K with the key K2
    for(int i = 0 ; i < 32 ; i++) {
        K.setBox(K2Subst[i], K2.getBox(i, 1), 1) ;
        knownBit[K2Subst[i]] = true ;
    }
    // Bits of K that we do not know
    std::vector<int> unknownPositions ;
    for(int i = 0 ; i < 32 ; i++) {
        if(!knownBit[i])
            unknownPositions.push_back(i) ;
    }
    std::vector<int> bitsToGuess(unknownPositions.size(), 0) ;
    Block K0, K1, K2bis ;
    // Brute force attack on the unknown bits
    K.generateSubKeys(&K0, &K1, &K2bis) ;
    assert(K2bis.getBits() == K2.getBits()) ;
    while(!checkSolution(K0, K1, K2)) {
        int i = bitsToGuess.size() - 1 ;
        while(bitsToGuess[i] == 1) {
            bitsToGuess[i] = 0 ;
            K.setBox(unknownPositions[i], 0, 1) ;
            i-- ;
        }
        bitsToGuess[i] = 1 ;
        K.setBox(unknownPositions[i], 1, 1) ;
        K.generateSubKeys(&K0, &K1, &K2bis) ;
        assert(K2bis.getBits() == K2.getBits()) ;
    }
    return K ;
}

namespace po = boost::program_options;

int main(int argc, char *argv[]) {
    po::options_description description("Usage");

    description.add_options()
        ("help,h", "produce help message")
        ("linearApproximationMatrix", "display the linear approximation matrix")
        ("experimentalCheck", "display the experimental results")
        ("bruteForce", "retrieve the keys by a brute force attack (very long)")
        ("activeBox", "retrieve the keys by active boxes attack") ;
    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(description).run(), vm);
    po::notify(vm);

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

    if(vm.count("linearApproximationMatrix")) {
        linearApproximationMatrix() ;
    }

    if(vm.count("experimentalCheck")) {
        std::vector<std::pair<int, int>> couples = {std::pair<int, int>(1, 5),\
            std::pair<int, int>(3, 15), std::pair<int, int>(4, 8), std::pair<int, int>(7, 7),\
            std::pair<int, int>(9, 4), std::pair<int, int>(10, 11), std::pair<int, int>(13, 12)} ;
        for(unsigned i = 0 ; i < couples.size() ; i++) {
            std::cout << "$(" << couples[i].first << ", " << couples[i].second << ")$ " ;
            experimentalCheck(couples[i].first, couples[i].second) ;
        }
    }

    if(vm.count("bruteForce")) {
        bruteForce() ;
    }

    if(vm.count("activeBox")) {
        Block K = guessKey() ;
        Block K0, K1, K2 ;
        K.generateSubKeys(&K0, &K1, &K2) ;
        std::cout << "K  = " << K.getBits() << std::endl ;
        std::cout << "K0 = " << K0.getBits() << std::endl ;
        std::cout << "K1 = " << K1.getBits() << std::endl ;
        std::cout << "K2 = " << K2.getBits() << std::endl ;
    }

    return 0 ;
}


/*
 * MultivariateNormalDistribution.hpp
 *
 *  Created on: Oct 6, 2012
 *      Author: bing
 */

#ifndef MULTIVARIATENORMALDISTRIBUTION_HPP_
#define MULTIVARIATENORMALDISTRIBUTION_HPP_

#include <boost/random.hpp>
#include <Eigen/Cholesky>

template<class T, size_t N>
class MultivariateNormalDistribution {
public:
	MultivariateNormalDistribution(unsigned int seed = time(0));

	MultivariateNormalDistribution(const Eigen::Matrix<T, N, N>& covarianceMatrix, unsigned int seed = time(0));

	virtual ~MultivariateNormalDistribution();

	void setCovarianceMatrix(const Eigen::Matrix<T, N, N>& covarianceMatrix);

	Eigen::Matrix<T, N, 1> draw();

	Eigen::Matrix<T, N, 1> draw(const Eigen::Matrix<T, N, N>& covarianceMatrix);

private:
	Eigen::Matrix<T, N, 1> draw(const Eigen::LLT<Eigen::Matrix<T, N, N> >& llt);

private:
	Eigen::Matrix<T, N, N> covarianceMatrix_;
	Eigen::LLT<Eigen::Matrix<T, N, N> > llt_;
	boost::mt19937 rng_;
};

template<class T, size_t N>
MultivariateNormalDistribution<T, N>::MultivariateNormalDistribution(unsigned int seed) :
		covarianceMatrix_(Eigen::Matrix<T, N, N>::Identity()), llt_(Eigen::Matrix<T, N, N>::Identity()), rng_(seed) {

}

template<class T, size_t N>
MultivariateNormalDistribution<T, N>::MultivariateNormalDistribution(const Eigen::Matrix<T, N, N>& covarianceMatrix, unsigned int seed) :
		covarianceMatrix_(covarianceMatrix), llt_(covarianceMatrix), rng_(seed) {

}

template<class T, size_t N>
MultivariateNormalDistribution<T, N>::~MultivariateNormalDistribution() {

}

template<class T, size_t N>
void MultivariateNormalDistribution<T, N>::setCovarianceMatrix(const Eigen::Matrix<T, N, N>& covarianceMatrix) {
	covarianceMatrix_ = covarianceMatrix;
	llt_ = Eigen::LLT<Eigen::Matrix<T, N, N> >(covarianceMatrix);
}

template<class T, size_t N>
Eigen::Matrix<T, N, 1> MultivariateNormalDistribution<T, N>::draw() {
	return draw(llt_);
}

template<class T, size_t N>
Eigen::Matrix<T, N, 1> MultivariateNormalDistribution<T, N>::draw(const Eigen::Matrix<T, N, N>& covarianceMatrix) {
	return draw(Eigen::LLT<Eigen::Matrix<T, N, N> >(covarianceMatrix));
}

template<class T, size_t N>
Eigen::Matrix<T, N, 1> MultivariateNormalDistribution<T, N>::draw(const Eigen::LLT<Eigen::Matrix<T, N, N> >& llt) {
	// Draw N independent random numbers from standard normal distribution.
	boost::normal_distribution<double> normalDistribution(0.0, 1.0);
	boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > variateGenerator(rng_, normalDistribution);
	Eigen::Matrix<T, N, 1> r(N);
	for (unsigned int i = 0; i < N; i++) {
		r(i) = variateGenerator();
	}

	// Transform numbers to multivariate normal distribution.
	Eigen::Matrix<T, N, 1> z = llt.matrixL() * r;

	return z;
}

#endif /* MULTIVARIATENORMALDISTRIBUTION_HPP_ */


/*

	stest01.cpp

	koliのうち統計関係部分のテスト

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

	TODO:

	ksvggraph.cppのテストを書く。

	-std:c++17でコンパイルが通るかやってみる。
	gccでも。

	kstat.cppのテストを書く。

	kdatasetに戻る。

	Nelder-Meadを実装。

	↓以下は前から書いてあるもの。
	度数分布表をつくる。kstatを見て。
	　別に、連続変数用の機能をつける。
	　　start/end, width, bin を指定する方式。
	　　自動で、スタージェスの公式を使う方式。
	　　階級の端点の表を与える方式。

*/

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

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

#include <boost/algorithm/string.hpp>

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

using namespace std;

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

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

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

int main( int, char *[]);

SvgGraph calculatePi( double);

SvgGraph createScatterAndCircle(
	const std::vector <double> &xvec,
	const std::vector <double> &yvec
);

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

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

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

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

int main( int, char *[])
{

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

	{
		Dataset ds;
		bool b;

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

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

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

		cout << "Specifying missing cases and excluding very big values...";
		ds.specifyValid(
			"v403",
			[]( double v)->bool{ return ( v < 99999.0 && v < 2500.0);}
		);
		cout << "Done." << endl;

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

	}

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

	// 度数分布表

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

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

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

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

	ft.printPadding( cout);

	// ヒストグラムをつくる。

	{
//		SvgHistogramMaker histm( leftvec, rightvec, counts);
		SvgHistogramMaker histm( dvecclean);
		histm.setGraphTitle( "Frequency - restricted to v less than 2500");
		histm.setXAxisTitle( "Household Income");
		histm.setYAxisTitle( "#Cases");
		SvgGraph svgg = histm.createGraph();
		svgg.writeFile( "stest01out01.svg");
	}

	// animation version
	{
//		SvgHistogramMaker histm( leftvec, rightvec, counts, true); // アニメバージョン
		SvgHistogramMaker histm( dvecclean, true);
		histm.setGraphTitle( "Frequency - restricted to v less than 2500");
		histm.setXAxisTitle( "Household Income");
		histm.setYAxisTitle( "#Cases");
		SvgGraph svgg = histm.createGraph();
		svgg.writeFile( "stest01out02.svg");
	}

	{
		SvgGraph svgg = calculatePi( 4000);
		svgg.writeFile( "stest01out03.svg");
	}

	return 0;

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

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

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

}

SvgGraph calculatePi( double n)
{

	std::vector <double> xvec;
	std::vector <double> yvec;
	xvec.reserve( n);
	yvec.reserve( n);

	for ( int i = 0; i < n; i++) {
    	xvec.push_back( randomUniform( -1.0, 1.0));
    	yvec.push_back( randomUniform( -1.0, 1.0));
	}

	int ninside = 0;
	for ( int i = 0; i < n; i++){
		if ( xvec[ i] * xvec[ i] + yvec[ i] * yvec[ i] < 1.0){
			ninside++;
		}
	}
	std::cout << "N of Draws: " << n << std::endl;
	std::cout << "Estimated PI: " << ( ( double)ninside / n ) * 4.0 << std::endl << std::endl;

	SvgGraph svgg = createScatterAndCircle( xvec, yvec);

	return svgg;

}

SvgGraph createScatterAndCircle(
	const std::vector <double> &xvec,
	const std::vector <double> &yvec
)
{

	std::string graph_title = "Random Numbers and Circle";
	std::string xaxis_title = "x";
	std::string yaxis_title = "y";

	SvgScatterMaker maker( xvec, yvec);
	maker.setGraphTitle( graph_title);
	maker.setXAxisTitle( xaxis_title);
	maker.setYAxisTitle( yaxis_title);

	auto svgg = maker.createGraph();

	GraphEllipse el( 0, 0, 1, 1);
	el.setFill( "none");
	el.setStroke( "red");
	el.setStrokewidth( 3);
	svgg.addElement( el);

	return svgg;

}

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


#include <Eigen/Dense>

#include <iostream>
#include <iomanip>

#include <cmath>

//! \brief Solve the autonomous IVP y' = f(y), y(0) = y0 using Rosenbrock method
//! Use semi-implicit Rosenbrock method using Jacobian evaluation. Equidistant steps of size T/N.
//! \tparam Func function type for r.h.s. f
//! \tparam DFunc function type for Jacobian df
//! \tparam StateType type of solution space y and initial data y0
//! \param[in] f r.h.s. func f
//! \param[in] df Jacobian df of f
//! \param[in] y0 initial data y(0)
//! \param[in] N number of equidistant steps
//! \param[in] T final time
//! \return vector of y_k for each step k from 0 to N
template <class Func, class DFunc, class StateType>
std::vector<StateType> solveRosenbrock(const Func & f, const DFunc & df,
                                       const StateType & y0, unsigned int N, double T) {
    const double h = T/N;
    const double a = 1. / (std::sqrt(2) + 2.);

    // TODO: implement rosenbrock method
}

int main() {
    // Final time
    const double T = 10;
    // All mesh sizes
    const std::vector<int> N = {16, 32, 64, 128, 256, 512, 1024, 2048, 4096};
    // Reference mesh size
    const int N_ref = 16384;
    // Initial data
    Eigen::Vector2d y0;
    y0 << 1., 1.;

    // Function and his Jacobian
    // TODO: implement r.h.s. and Jacobian

    // TODO: compute reference solutions, solution, error and output approximate order of convergence
    }
}

Nummerical Methods for CSE/PS14/templates_ps14/rosenbrock_template.cpp


#include <boost/python.hpp>
#include <boost/math/special_functions/ellint_1.hpp>
#include <boost/math/special_functions/ellint_2.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <cmath>

#define MU_0 M_PI*4e-7

/* Functional code because making it into a class that is interfaceable with python is a massive
   sweffort.  It will instead be used and wrapped into a python class for ease of use.
 */

double ellipk(double _arg){
  return boost::math::ellint_1(_arg);
}

double ellipe(double _arg){
  return boost::math::ellint_2(_arg);
}

std::tuple<double, double> field_from_loop(double current, double radius, double R, double Z){
  double rho = sqrt(R*R + Z*Z);
  double alpha = sqrt(radius*radius + rho*rho - 2*radius*R);
  double beta = sqrt(radius*radius + rho*rho + 2*radius*R);
  double k = sqrt(1 - (alpha*alpha)/(beta*beta));

  double C = (MU_0*current)/M_PI;
  double a = 1.0/(2*(alpha*alpha)*beta);
  double b = Z/(2*(alpha*alpha)*beta*R);

  double Bz = C*a*((radius*radius - rho*rho)*ellipe(k) + (alpha*alpha)*ellipk(k));
  double Br = C*b*((radius*radius + rho*rho)*ellipe(k) - (alpha*alpha)*ellipk(k));

  if (std::isnan(Bz) == true || std::isinf(Bz) == true){
    Bz = 0;
  }
  if (std::isnan(Br) == true || std::isinf(Br) == true){
    Br = 0;
  }

  return std::make_tuple(Br, Bz);
}

std::tuple<double, double> make_current_layer(int nTurns, double separation, double startPos, double current, double radius, double R, double Z){

  double offset = startPos - separation;
  double newZ = 0;
  std::tuple<double, double> newBrBz (0, 0);
  double totalBr = 0;
  double totalBz = 0;

  for (int i = 0; i < nTurns; i++){
    offset += separation;
    newZ = Z - offset;
    newBrBz = field_from_loop(current, radius, R, newZ);
    totalBr += std::get<0>(newBrBz);
    totalBz += std::get<1>(newBrBz);
  }
  return std::make_tuple(totalBr, totalBz);

}

std::tuple<double, double> make_coil_from_layers(int nLayers, int nTurns, double layerSep, double loopSep, double startPos, double current, double minR, double R, double Z){
  double totalBr = 0;
  double totalBz = 0;

  std::tuple<double, double> newBrBz (0, 0);

  double radius = minR - layerSep;

  for (int i = 0; i < nLayers; i++){
    radius += layerSep;
    newBrBz = make_current_layer(nTurns, loopSep, startPos, current, radius, R, Z);
    totalBr += std::get<0>(newBrBz);
    totalBz += std::get<1>(newBrBz);
  }
  return std::make_tuple(totalBr, totalBz);
}

std::tuple<double, double> make_coil(double currentDensity, double centrePos, double length, double rInner, double rOuter, int nLayers, int nTurns, double R, double Z){
  double z1 = centrePos - 0.5*length;
  double z2 = centrePos + 0.5*length;
  double height = rOuter - rInner;

  double loopSep = length/double(nTurns);
  double layerSep = height/double(nLayers);

  double firstLoopZ = z1 + 0.5*loopSep;
  double firstLoopR = rInner + 0.5*layerSep;

  int totalLoops = nLayers*nTurns;
  double totalCurrent = length*height*currentDensity*1.0e6; //convert from A/mm^2 to A/m^2
  double currentPerLoop = totalCurrent/double(totalLoops);

  std::tuple<double, double> newBrBz = make_coil_from_layers(nLayers, nTurns, layerSep, loopSep, firstLoopZ, currentPerLoop, firstLoopR, R, Z);

  return newBrBz;
}

double current_to_density(double current, double rInner, double rOuter, double length, int nLayers, int nTurns){
  return (current*double(nLayers)*double(nTurns))/(length*(rOuter - rInner)*1e6); //in A/mm^2
}

std::tuple<double, double, double, double, double, double> convert_to_cartesian(double r, double phi, double z, double Br, double Bphi, double Bz){
  using namespace boost::numeric::ublas;
  double x = r*cos(phi);
  double y = r*sin(phi);

  //form vector of (Br, Bphi)
  vector<double> B(2);
  B(0) = Br; B(1) = Bphi;

  //form matrix that converts B_r/phi to B_x/y
  matrix<double> xy_rphi(2, 2);
  xy_rphi(0,0) = cos(phi); xy_rphi(0,1) = -1.0*sin(phi);
  xy_rphi(1,0) = sin(phi); xy_rphi(1,1) = cos(phi);

  vector<double> Bxy = prod(xy_rphi, B);

  return std::make_tuple(x, y, z, Bxy(0), Bxy(1), Bz);
}

std::tuple<double, double, double, double, double, double> convert_to_polar(double x, double y, double z, double Bx, double By, double Bz){
  using namespace boost::numeric::ublas;
  double r = sqrt(x*x + y*y);
  double phi = atan2(y, x);

  //form vector of (Bx, By)
  vector<double> B(2);
  B(0) = Bx; B(1) = By;

  //form matrix that converts B_x/y to B_r/phi
  matrix<double> rphi_xy(2, 2);
  rphi_xy(0, 0) = cos(phi); rphi_xy(0, 1) = sin(phi);
  rphi_xy(1, 0) = -1.0*sin(phi) ; rphi_xy(1, 1) = cos(phi);

  vector<double> Brphi = prod(rphi_xy, B);

  return std::make_tuple(r, phi, z, Brphi(0), Brphi(1), Bz);
}

std::tuple<double, double, double, double, double, double> rotate(double x, double y, double z, double Bx, double By, double Bz, double thetaX, double thetaY, double px, double py, bool transpose){
  using namespace boost::numeric::ublas;

  //set up vectors for x,y,z and Bxyz to manipulate on
  vector<double> X(3);
  X(0) = x; X(1) = y; X(2) = z;

  vector<double> B(3);
  B(0) = Bx; B(1) = By; B(2) = Bz;

  //set up offset vector
  vector<double> offset(3);
  offset(0) = px; offset(1) = py; offset(2) = 0;

  //set up rotation matrix
  matrix<double> R(3, 3);
  R(0, 0) = cos(thetaY); R(0, 1) = 0; R(0, 2) = sin(thetaY);
  R(1, 0) = sin(thetaX)*sin(thetaY); R(1, 1) = cos(thetaX); R(1, 2) = -1.0*cos(thetaY)*sin(thetaX);
  R(2, 0) = -1.0*sin(thetaY)*cos(thetaX); R(2, 1) = sin(thetaX); R(2, 2) = cos(thetaY)*cos(thetaX);

  if (transpose == false){
    vector<double> newX = prod(R, X) + offset;
    vector<double> newB = prod(R, B);
    return std::make_tuple(newX(0), newX(1), newX(2), newB(0), newB(1), newB(2));

  } else{
    vector<double> newX = prod(trans(R), X) - offset;
    vector<double> newB = prod(trans(R), B);
    return std::make_tuple(newX(0), newX(1), newX(2), newB(0), newB(1), newB(2));
  }
}

boost::python::tuple get_field_at_point(double current, double centrePos, double length, double rInner, double rOuter, int nLayers, int nTurns, double thetaX, double thetaY, double px, double py, double R, double PHI, double Z){
  double currentDensity = current_to_density(current, rInner, rOuter, length, nLayers, nTurns);

  //convert coords to cartesian
  auto tmp = convert_to_cartesian(R, PHI, Z, 0, 0, 0);

  //rotate with R
  tmp = rotate(std::get<0>(tmp), std::get<1>(tmp), std::get<2>(tmp), 0, 0, 0, thetaX, thetaY, px, py, false);

  //convert back to polar
  tmp = convert_to_polar(std::get<0>(tmp), std::get<1>(tmp), std::get<2>(tmp), std::get<3>(tmp), std::get<4>(tmp), std::get<5>(tmp));

  //now calculate the field
  std::tuple<double, double> newBrBz = make_coil(currentDensity, centrePos, length, rInner, rOuter, nLayers, nTurns, std::get<0>(tmp), std::get<2>(tmp));

  //convert to cartesian again INCLUDING B FIELD!!
  auto tmp_2 = convert_to_cartesian(std::get<0>(tmp), std::get<1>(tmp), std::get<2>(tmp), std::get<0>(newBrBz), 0, std::get<1>(newBrBz));

  //Rotate with R_t
  tmp_2 = rotate(std::get<0>(tmp_2), std::get<1>(tmp_2), std::get<2>(tmp_2), std::get<3>(tmp_2), std::get<4>(tmp_2), std::get<5>(tmp_2), thetaX, thetaY, px, py, true);

  //convert back to polars
  tmp_2 = convert_to_polar(std::get<0>(tmp_2), std::get<1>(tmp_2), std::get<2>(tmp_2), std::get<3>(tmp_2), std::get<4>(tmp_2), std::get<5>(tmp_2));

  //Should check whether the coords are the same as R, PHI, Z

  return  boost::python::make_tuple(std::get<3>(tmp_2), std::get<4>(tmp_2), std::get<5>(tmp_2));
}

//This func is like above but takes (x,y,z) as args and returns Bx, By, Bz for g4blgrid field
boost::python::tuple get_field_at_point_xyz(double current, double centrePos, double length, double rInner, double rOuter, int nLayers, int nTurns, double thetaX, double thetaY, double px, double py, double X, double Y, double Z){
  double currentDensity = current_to_density(current, rInner, rOuter, length, nLayers, nTurns);

  auto tmp = rotate(X, Y, Z, 0, 0, 0, thetaX, thetaY, px, py, false);

  tmp = convert_to_polar(std::get<0>(tmp), std::get<1>(tmp), std::get<2>(tmp), std::get<3>(tmp), std::get<4>(tmp), std::get<5>(tmp));

  std::tuple<double, double> newBrBz =  make_coil(currentDensity, centrePos, length, rInner, rOuter, nLayers, nTurns, std::get<0>(tmp), std::get<2>(tmp));

  auto tmp_2 = convert_to_cartesian(std::get<0>(tmp), std::get<1>(tmp), std::get<2>(tmp), std::get<0>(newBrBz), 0, std::get<1>(newBrBz));

  tmp_2 = rotate(std::get<0>(tmp_2), std::get<1>(tmp_2), std::get<2>(tmp_2), std::get<3>(tmp_2), std::get<4>(tmp_2), std::get<5>(tmp_2), thetaX, thetaY, px, py, true);

  return boost::python::make_tuple(std::get<3>(tmp_2), std::get<4>(tmp_2), std::get<5>(tmp_2));

}

BOOST_PYTHON_MODULE(makefield_cpp){
  using namespace boost::python;
  def ("get_field_at_point", get_field_at_point);
  def ("get_field_at_point_xyz", get_field_at_point_xyz);
}


#pragma once
#include <boost/multiprecision/cpp_int.hpp>
#include <tuple>
#include "utils.hpp"

namespace mp = boost::multiprecision;

namespace rsa {

    template<unsigned int N>
    class keys {

    public:

        using number = typename num_utils<N>::number;

        keys() {
            std::tie(_n, _e, _d, _phi) = generate_keys();
        }

        keys(const number& n , const number& e, const number& d, const number& phi) :
            _n(n), _e(e), _d(d), _phi(phi) {

        }

        static auto generate_keys() {
            const auto p = static_cast<number>(num_utils<N / 2>::generate_random_prime());
            const auto q = static_cast<number>(num_utils<N / 2>::generate_random_prime());
            const auto n = p * q;
            const auto phi = (p - 1) * (q - 1);
            const auto rand = num_utils<N>::get_int_random(2, phi - 1);
            auto e = rand();
            while (mp::gcd(phi, e) != 1)
                e = rand();
            const auto d = num_utils<N>::bezout_identity(e, phi).first % phi;
            return std::make_tuple(n, e, static_cast<number>(d > 0 ? d : phi + d), phi);
        }

        const auto& get_n() const {
            return _n;
        }

        const auto& get_e() const {
            return _e;
        }
        const auto& get_d() const {
            return _d;
        }
        const auto& get_phi() const {
            return _phi;
        }

    private:

        number _n, _e, _d, _phi;

    };

}// namespace rsa


#include <Eigen/Eigen>
#include <cfloat>
#include <iostream>
#include "GQ_Characterize.hpp"

GQ_Characterize::GQ_Characterize(double A, double B, double C,
                                 double D, double E, double F,
                                 double G, double H, double J,
                                 double K):
    A_(A),B_(B),C_(C),D_(D),E_(E),F_(F),G_(G),H_(H),J_(J),K_(K) {
    //determine canonical form of GQ and determine transformation
    make_canonical();
}

// The GQ Characterization method used here is based on work from
// Skip Thompson at Radford university
// https://www.radford.edu/~thompson/RP/quadrics.pdf
void GQ_Characterize::make_canonical()
{
  // create coefficient matrix
  Eigen::Matrix3f Aa;
  Aa << A_, D_/2, F_/2,
        D_/2, B_, E_/2,
        F_/2, E_/2, C_;

  // create hessian matrix
  Eigen::Matrix4f Ac;
  Ac << A_, D_/2, F_/2, G_/2,
        D_/2, B_, E_/2, H_/2,
        F_/2, E_/2, C_, J_/2,
        G_/2,  H_/2, J_/2, K_;

  // characterization values
  int rnkAa, rnkAc, delta, S, D;
  Eigen::FullPivLU<Eigen::Matrix3f> lu_decomp_Aa(Aa);
  Eigen::FullPivLU<Eigen::Matrix4f> lu_decomp_Ac(Ac);
  rnkAa = lu_decomp_Aa.rank();
  rnkAc = lu_decomp_Ac.rank();

  double determinant = Ac.determinant();
  if (fabs(determinant) < gq_tol)
    delta = 0;
  else
    delta = (determinant < 0) ? -1 : 1;

  Eigen::Vector3f eigenvals;
  Eigen::Matrix3f eigenvects;
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> AaEigs;
  // completes Eiegen value computation, stored in AaEigs
  AaEigs.compute(Aa, Eigen::ComputeEigenvectors);
  eigenvals = AaEigs.eigenvalues();
  eigenvects = AaEigs.eigenvectors();
  Eigen::Vector3f signs;

  // determine signs of the eigenvalues
  for(unsigned int i = 0; i < 3; i++) {
    if (eigenvals[i] > -1 * gq_tol)
      signs[i] = 1;  //Sign counted as + if above -tolerance
    else
      signs[i] = -1; //Sign counted as - if less than -tolerance
  }

  // set S parameter based on agreement of eigenvalue signs
  S = (fabs(signs.sum()) == 3) ? 1 : -1;

  // may need to adjust delta for speical cases using the new scaling factor, K_
  // so we'll calculate that now
  Eigen::Vector3f b;
  b << -G_/2, -H_/2, -J_/2;

  // Use a Moore-Penrose pseudoinverse to ensure minimal norm least squares solution
  // this particular inverse ensures we get the "minimal" translation of the GQ
  // surface. A standard inverse may get the correct new basis set (rotation)
  // for the GQ, but with arbitrary scaling and in turn an arbitrarily scaled
  // translation vector
  Eigen::Matrix3f Aai = Aa.completeOrthogonalDecomposition().pseudoInverse();

  // Compute the translation from the inverse
  Eigen::Vector3f c = Aai * b;
  double dx = c[0], dy = c[1], dz = c[2];

  // Update the constant using the resulting translation
  K_ += (G_/2) * dx + (H_/2) * dy + (J_/2) * dz;

  // For the special case of the elliptic cylinder, delta will be needed.
  // Delta is set based on whether the sign of the equation constant, K, and the
  // signs of the eigenvalues are the same.
  if (rnkAa == 2 && rnkAc == 3 && S == 1) {
    delta = (K_ * signs[0]) ? -1 : 1;
  }

  D = (K_*signs[0]) ? -1:1;

  // characterize this GQ equation using the parameter values calculated above
  type = find_type(rnkAa, rnkAc, delta, S, D);
  // set the translation
  translation = Vector3d(dx,dy,dz);
  // set the rotaion matrix. LINE BELOW MAY BE UNNECESSARY but is saved in case
  // it is needed
  std::copy(eigenvects.data(),eigenvects.data()+9,rotation_mat);

  // update equation coefficients
  for(unsigned int i = 0; i < 3; i ++ ) if (fabs(eigenvals[i]) < gq_tol) eigenvals[i] = 0;
  A_ = eigenvals[0]; B_ = eigenvals[1]; C_ = eigenvals[2];
  D_ = 0; E_ = 0; F_ = 0;
  G_ = 0; H_ = 0; J_ = 0;
  // K is set above

  // simplify the GQ if possible
  reduce_type();
}

// this method reduces a complex GQ to a geometrically equivalent
// and more CAD-friendly form if appropriate
void GQ_Characterize::reduce_type() {

  if( ONE_SHEET_HYPERBOLOID == type ) {
    // if the K value is near-zero, reduce to Elliptic Cone
    if ( fabs(K_) < equivalence_tol ) {
      K_ = 0;
      type = ELLIPTIC_CONE;
      return;
    }
  }

  if ( TWO_SHEET_HYPERBOLOID == type ) {
    // if the K value is near-zero, reduce to Elliptic Cone
    if ( fabs(K_) < equivalence_tol ) {
      K_ = 0;
      type = ELLIPTIC_CONE;
      return;
    }
  }

  if ( ELLIPSOID == type ) {
    //if any of the 2nd order terms are near-zero, reduce to Elliptic Cylinder
    if ( fabs(A_) < equivalence_tol ) {
      A_ = 0;
      type = ELLIPTIC_CYL;
      return;
    }
    else if ( fabs(B_) < equivalence_tol ) {
      B_ = 0;
      type = ELLIPTIC_CYL;
      return;
    }
    else if ( fabs(C_) < equivalence_tol ) {
      C_ = 0;
      type = ELLIPTIC_CYL;
      return;
    }
  }

};

GQ_TYPE GQ_Characterize::find_type(int rt, int rf, int del, int s, int d) {

  GQ_TYPE t;
  if( 3 == rt && 4 == rf && -1 == del && 1 == s){
    t = ELLIPSOID;
  }
  else if( 3 == rt && 4 == rf && 1 == del && -1 == s){
    t = ONE_SHEET_HYPERBOLOID;
  }
  else if( 3 == rt && 4 == rf && -1 == del && -1 == s){
    t = TWO_SHEET_HYPERBOLOID;
  }
  else if( 3 == rt && 3 == rf && 0 == del && -1 == s){
    t = ELLIPTIC_CONE;
  }
  else if( 2 == rt && 4 == rf && -1 == del && 1 == s){
    t = ELLIPTIC_PARABOLOID;
  }
  else if( 2 == rt && 4 == rf && 1 == del && -1 == s){
    t = HYPERBOLIC_PARABOLOID;
  }
  else if( 2 == rt && 3 == rf && -1 == del && 1 == s){
    t = ELLIPTIC_CYL;
  }
  else if( 2 == rt && 3 == rf && 0 == del && -1 == s){
    t = HYPERBOLIC_CYL;
  }
  else if( 1 == rt && 3 == rf && 0 == del && 1 == s){
    t = PARABOLIC_CYL;
  }
  else{
    t = UNKNOWN;
  }

  //special case, replace delta with D
  if(2 == rt && 3 == rf && 1 == s && d != 0) {
    t = find_type(rt, rf, d, s, 0);
  }

  return t;
}


// Petter Strandmark 2012–2013.
#include <array>
#include <limits>

#include <Eigen/Dense>

#include <spii/solver.h>

namespace spii {

namespace
{
	// constexpr when supported.
	const double nan = std::numeric_limits<double>::quiet_NaN();
}

double
polynomial_interpolation(std::array<double, 2> x,
                         std::array<double, 2> f,
                         std::array<double, 2> df,
                         double xmin = nan, double xmax = nan)
{
	int minpos = 0;
	if (x[1] < x[0]) {
		minpos = 1;
	}
	int maxpos = 1 - minpos;

	if (xmin != xmin) {
		xmin = x[minpos];
	}
	if (xmax != xmax) {
		xmax = x[maxpos];
	}

	auto d1 = df[minpos] + df[maxpos] - 3*(f[minpos] - f[maxpos]) / (x[minpos] - x[maxpos]);
	auto inside_sqrt = d1*d1 - df[0]*df[1];
	if (inside_sqrt >= 0) {
		auto d2 = std::sqrt(inside_sqrt);
		auto t = x[maxpos] - (x[maxpos] - x[minpos])*((df[maxpos] + d2 - d1)/(df[maxpos] - df[minpos] + 2*d2));
		return std::min(std::max(t, xmin), xmax);
	}
	else {
		return (xmin + xmax) / 2;
	}
}

double perform_Wolfe_linesearch(const Solver& solver,
                                const Function& function,
                                const Eigen::VectorXd& x,
                                const double fval,
                                const Eigen::VectorXd& g,
                                const Eigen::VectorXd& p,
                                Eigen::VectorXd* scratch,
                                const double start_alpha)
{
	Eigen::VectorXd g_prev = g;
	auto f = function.evaluate(x);
	auto f_prev = f;
	double gtp = g.dot(p);
	auto gtp_prev = gtp;

	auto n = x.size();
	Eigen::VectorXd g_new(n);  // TODO: Somehow use scratch space.

	double alpha = start_alpha;
	double alpha_prev = 0;

	*scratch = x + alpha * p;
	double f_new = function.evaluate(*scratch, &g_new);
	double gtp_new  = g_new.dot(p);

	//
	auto c1 = solver.line_search_c;
	auto c2 = solver.line_search_c2;
	//

	std::array<double, 2> bracket;
	std::array<double, 2> bracket_fval;
	std::array<double, 2> bracket_gTpval;
	bool done = false;

	//
	// Bracketing phase.
	//
	int iterations = 0;
	const int max_iterations = 30;
	while (iterations <= max_iterations) {

		if (f_new > f + c1 * alpha * gtp || (iterations > 1 && f_new >= f_prev)) {
			// Double braces for GCC 4.7 compatibility. Remove later.
			bracket        = {{alpha_prev, alpha}};
			bracket_fval   = {{f_prev, f_new}};
			bracket_gTpval = {{ g_prev.dot(p), g_new.dot(p)}};
			break;
		}
		else if (std::abs(gtp_new) <= -c2 * gtp) {
			// We are done.
			return alpha;
		}
		else if (gtp_new >= 0) {
			// Double braces for GCC 4.7 compatibility. Remove later.
			bracket        = {{alpha_prev, alpha}};
			bracket_fval   = {{f_prev, f_new}};
			bracket_gTpval = {{g_prev.dot(p), g_new.dot(p)}};
			break;
		}

		double temp = alpha_prev;
		alpha_prev = alpha;
		double minStep = alpha + 0.01*(alpha - temp);
		double maxStep = 10 * alpha;

		if (solver.wolfe_interpolation_strategy == Solver::BISECTION) {
			alpha = maxStep;
		}
		else {
			// Double braces for GCC 4.7 compatibility. Remove later.
			alpha = polynomial_interpolation({{temp, alpha}},
			                                 {{f_prev, f_new}},
			                                 {{gtp_prev, gtp_new}},
			                                 minStep, maxStep);
		}

		f_prev = f_new;
		g_prev = g_new;
		gtp_prev = gtp_new;

		*scratch = x + alpha * p;
		f_new = function.evaluate(*scratch, &g_new);
		gtp_new  = g_new.dot(p);

		iterations++;
	}

	//
	// Zoom phase.
	//
	bool insufficient_progress = false;
	while (!done && iterations <= max_iterations) {

		// Compute new trial value.
		if (solver.wolfe_interpolation_strategy == Solver::BISECTION) {
			alpha = (bracket[0] + bracket[1]) / 2.0;
		}
		else {
			alpha = polynomial_interpolation(bracket,
			                                 bracket_fval,
			                                 bracket_gTpval);
		}

		auto max_bracket = std::max({bracket[0], bracket[1]});
		auto min_bracket = std::min({bracket[0], bracket[1]});

		if (std::min({max_bracket-alpha, alpha-min_bracket}) / (max_bracket - min_bracket) < 0.1) {
			if (insufficient_progress || alpha >= max_bracket || alpha <= min_bracket) {
				if (std::abs(alpha - max_bracket) < std::abs(alpha - min_bracket)) {
					alpha = max_bracket - 0.1*(max_bracket - min_bracket);
				}
				else {
					alpha = min_bracket + 0.1*(max_bracket - min_bracket);
				}
				insufficient_progress = false;
			}
			else {
				insufficient_progress = true;
			}
		}

		*scratch = x + alpha * p;
		f_new = function.evaluate(*scratch, &g_new);
		gtp_new  = g_new.dot(p);

		int lo_pos, hi_pos;
		double f_low;
		if (bracket_fval[0] < bracket_fval[1]) {
			f_low = bracket_fval[0];
			lo_pos = 0;
			hi_pos = 1;
		}
		else {
			f_low = bracket_fval[1];
			lo_pos = 1;
			hi_pos = 0;
		}

		bool armijo = f_new < f + c1 * alpha * gtp;

		if (!armijo || f_new >= f_low) {
			// Armijo condition not satisfied or not lower than lowest
			// point
			bracket[hi_pos]        = alpha;
			bracket_fval[hi_pos]   = f_new;
			bracket_gTpval[hi_pos] = g_new.dot(p);
		}
		else {
			if (std::abs(gtp_new) <= - c2*gtp) {
				// Wolfe conditions satisfied
				done = true;
			}
			else if (gtp_new * (bracket[hi_pos] - bracket[lo_pos]) >= 0) {
				// Old HI becomes new LO
				bracket[hi_pos]        = bracket[lo_pos];
				bracket_fval[hi_pos]   = bracket_fval[lo_pos];
				bracket_gTpval[hi_pos] = bracket_gTpval[lo_pos];
			}

			// New point becomes new LO
			bracket[lo_pos]        = alpha;
			bracket_fval[lo_pos]   = f_new;
			bracket_gTpval[lo_pos] = g_new.dot(p);
		}

		iterations++;
	}

	if (!done) {
		if (solver.log_function) {
			solver.log_function("Wolfe line search maximum iterations exceeded.");
		}
		return 0.0;
	}

	if (bracket_fval[0] < bracket_fval[1]) {
		alpha = bracket[0];
	}
	else {
		alpha = bracket[1];
	}

	return alpha;
}

double perform_Armijo_linesearch(const Solver& solver,
                                 const Function& function,
                                 const Eigen::VectorXd& x,
                                 const double fval,
                                 const Eigen::VectorXd& g,
                                 const Eigen::VectorXd& p,
                                 Eigen::VectorXd* scratch,
                                 const double start_alpha)
{
	//
	// Perform back-tracking line search.
	//

	// Starting value for alpha during line search. Newton and
	// quasi-Newton methods should choose 1.0.
	double alpha = start_alpha;
	double rho = solver.line_search_rho;
	double c = solver.line_search_c;
	double gTp = g.dot(p);
	if (gTp != gTp) {
		if (solver.log_function) {
			solver.log_function("Backtracking encountered NaN, returning zero step.");
		}
		return 0.0;
	}

	int backtracking_attempts = 0;
	while (true) {
		*scratch = x + alpha * p;
		double lhs = function.evaluate(*scratch);
		double rhs = fval + c * alpha * gTp;
		if (lhs <= rhs) {
			break;
		}
		alpha *= rho;

		backtracking_attempts++;
		if (backtracking_attempts > 1000) {
			if (solver.log_function) {
				solver.log_function("Backtracking failed, returning zero step.");
			}
			return 0.0;
		}
	}

	return alpha;
}

double Solver::perform_linesearch(const Function& function,
                                  const Eigen::VectorXd& x,
                                  const double fval,
                                  const Eigen::VectorXd& g,
                                  const Eigen::VectorXd& p,
                                  Eigen::VectorXd* scratch,
                                  const double start_alpha) const
{
	if (this->line_search_type == ARMIJO) {
		return perform_Armijo_linesearch(*this, function, x, fval, g, p, scratch, start_alpha);
	}
	else {
		return perform_Wolfe_linesearch(*this, function, x, fval, g, p, scratch, start_alpha);
	}
}

}  // namespace spii


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace std;
namespace ublas = boost::numeric::ublas;

int main() {
    ublas::vector<double> v1(3), v2(3);
    for (unsigned i = 0; i < 3; ++i)
        v1 (i) = i, v2 (i) = i*10;

    cout << v1 << endl
        << v2 << endl
        << outer_prod(v1, v2) << endl;
}


#include "Mesh3D.hpp"

#include <Eigen/Geometry>

float noob::mesh_3d::get_volume()
{
	if (!volume_calculated)
	{
		// Proudly gleaned from U of R website!
		// http://mathcentral.uregina.ca/QQ/database/QQ.09.09/h/ozen1.html
		// The volume of the tetrahedron with vertices (0 ,0 ,0), (a1 ,a2 ,a3), (b1, b2, b3) and (c1, c2, c3) is [a1b2c3 + a2b3c1 + a3b1c2 - a1b3c2 - a2b1c3 - a3b2c1] / 6.
		double accum = 0.0;
		for (uint32_t i = 0; i < indices.size(); i += 3)
		{
			const noob::vec3f first = vertices[i].position;
			const noob::vec3f second = vertices[i+1].position;
			const noob::vec3f third = vertices[i+2].position;

			accum += ((first[0] * second[1] * third[2]) + (first[1] * second[2] * third[0]) + (first[2] * second[0] * third.v[1]) - (first[0] * second.v[2] * third.v[1]) - (first.v[1] * second[0] * third.v[2]) - (first[2] * second[1] * third.v[0])) / 6.0;

		}

		volume_calculated = true;
		volume = static_cast<float>(accum);
	}

	return volume;
}

void noob::mesh_3d::calculate_dims() noexcept(true)
{
	if (vertices.size() > 0)
	{
		bbox.min = bbox.max = vertices[0].position;
		for (noob::mesh_3d::vert v : vertices)
		{
			bbox.min[0] = std::min(bbox.min[0], v.position[0]);
			bbox.min[1] = std::min(bbox.min[1], v.position[1]);
			bbox.min[2] = std::min(bbox.min[2], v.position[2]);
			bbox.max[0] = std::max(bbox.max[0], v.position[0]);
			bbox.max[1] = std::max(bbox.max[1], v.position[1]);
			bbox.max[2] = std::max(bbox.max[2], v.position[2]);
		}
	}
}

/*
   void noob::mesh_3d::to_origin()
   {
   noob::vec3f dims = bbox.get_dims();
   for (size_t i = 0; i < vertices.size(); ++i)
   {
   vertices[i] = (vertices[i] + dims);
   }
   }

   std::string noob::mesh_3d::save() const
   {
   fmt::MemoryWriter w;
   w << "OFF" << "\n" << vertices.size() << " " << indices.size() / 3 << " " << 0 <<  "\n";
   for (auto v : vertices)
   {
   w << v.v[0] << " " << v.v[1] << " " << v.v[2] <<  "\n";
   }
   for (size_t i = 0; i < indices.size(); i = i + 3)
   {
   w << 3 << " " << indices[i] << " " << indices[i+1] << " " << indices[i+2] << "\n";
   }

   return w.str();
   }

   void noob::mesh_3d::save(const std::string& filename) const
   {

   }
   */


#include <iostream>
#include <cstdio>
#include <armadillo>
#include <random>

#include "histograms.hh"

using namespace std;
using namespace arma;

/**
 * Save histogram data to file.
 *
 * Will save file with four data points for each histogram bin:
 *  bin_start     Low edge of bin
 *  bin_end       High edge of bin
 *  count         Absolute count of values in bin
 *  relcount      Relative count (i.e., divided by total number of values)
 *
 * Arguments:
 *  v           - Vector containing all values
 *  dm          - Bin width
 *  filename    - Name of file to save to
**/
void save_histogram(const vector<double> &v, double dm, const char *filename) {
  // create armadillo vector from std::vector
  vec m(v);

  double mmin = 0, mmax = m.max();
  size_t nbins = ceil((mmax - mmin) / dm);
  size_t total = m.n_elem;

  vec bin_edges(nbins + 1);
  for(size_t i = 0; i <= nbins; i++)
    bin_edges[i] = mmin + dm * i;

  uvec bins = histc(m, bin_edges);

  // save histogram bins to file
  FILE *fp = fopen(filename, "w");
  fprintf(fp, "m_start\tm_end\tcount\trelcount\n");
  for(size_t i = 0; i < nbins; i++) {
    int count = bins(i);
    fprintf(fp, "%.3E\t%.3E\t%d\t%.3E\n", bin_edges(i), bin_edges(i + 1), count, (double)count / total);
  }
  fprintf(fp, "%.3E\t%.3E\t%d\t%.3E\n", bin_edges(nbins), bin_edges(nbins), 0, 0.0);
}


// https://www.codechef.com/problems/FCTRL2/
#include<iostream>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;

using namespace std;
int main()
{
    int t,i,a;
    cin>>t;
    for(i=0;i<t;i++){
        cin>>a;
        cpp_int fact;
        fact=1;
        for(int j=1;j<=a;j++){
            fact*=j;
        }
        cout<<fact<<endl;

    }
    return 0;
}


#pragma once

#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <unsupported/Eigen/MatrixFunctions>

using std::vector;
using Eigen::MatrixXf;

class MultiwayMatcher{

public:
	MultiwayMatcher();
	~MultiwayMatcher();
	void initialize(Eigen::MatrixXf A, vector<unsigned> numSmp);
	void set_verbose(bool verbose);

	void estimate_universe_size();
	void CLEAR();
	void get_X(Eigen::MatrixXf& X);
	void get_Y(Eigen::MatrixXf& Y);
	void get_assignments(vector<int>& assignments);
	void get_fused_counts(vector<unsigned>& fused_counts);
	void save_data();

private:
	void construct_D();
	void construct_L();
	void construct_Lnrm();

	vector<unsigned> numSmp_;
	vector<unsigned> cumSum_;

	Eigen::SparseMatrix<float> A_sp;
	Eigen::SparseMatrix<float> D_sp;
	Eigen::SparseMatrix<float> L_sp;
	Eigen::SparseMatrix<float> Lnrm_sp;

	// MatrixXf A_;
	// MatrixXf D_;
	// MatrixXf L_;
	MatrixXf Lnrm_; // Normalized Laplacian
	MatrixXf sl_; // vector of singular values of Lnrm_ (decreasing order)
	MatrixXf Vl_; // right singular vectors of Lnrm_
	MatrixXf N_; // embedding matrix (CLEAR)
	MatrixXf C_; // cluster centers (CLEAR)
	MatrixXf Y_; // lifting permutation (CLEAR)
	MatrixXf X_; // optimized pairwise permutations (CLEAR)

	unsigned m_; // estimated size of the universe
	vector<int> assignments_; // assignment of each local observation to the universe
	vector<unsigned> fused_counts_; // observation count of each global object

	// threshold for estimating universe size
	double thresh_ = 0.50;

	bool verbose_ = true;

};


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

#include <votca/tools/cubicspline.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <votca/tools/linalg.h>
#include <iostream>
#include <cmath>

namespace votca { namespace tools {

using namespace std;

void CubicSpline::Interpolate(ub::vector<double> &x, ub::vector<double> &y)
{
    if(x.size() != y.size())
        throw std::invalid_argument("error in CubicSpline::Interpolate : sizes of vectors x and y do not match");

    if(x.size()<3)
        throw std::invalid_argument("error in CubicSpline::Interpolate : vectors x and y have to contain at least 3 points");

    const int N = x.size();

    // adjust the grid
    _r.resize(N);
    _f.resize(N);
    _f2.resize(N);

    // create vector proxies to individually access f and f''

    // copy the grid points into f
    _r = x;
    _f = y;
    _f2 = ub::zero_vector<double>(N);

    // not calculate the f''
    ub::matrix<double> A(N, N);
    A = ub::zero_matrix<double>(N,N);

    for(int i=0; i<N - 2; ++i) {
            _f2(i+1) = -( A_prime_l(i)*_f(i)
            + (B_prime_l(i) - A_prime_r(i)) * _f(i+1)
            -B_prime_r(i) * _f(i+2));

            A(i+1, i) = C_prime_l(i);
            A(i+1, i+1) = D_prime_l(i) - C_prime_r(i);
            A(i+1, i+2) = -D_prime_r(i);
    }

    switch(_boundaries) {
        case splineNormal:
            A(0, 0) = 1;
            A(N - 1, N-1) = 1;
            break;
        case splinePeriodic:
            A(0,0) = 1; A(0,N-1) = -1;
            A(N-1,0) = 1; A(N-1,N-1) = -1;
            break;
        case splineDerivativeZero:
	    throw std::runtime_error("erro in CubicSpline::Interpolate: case splineDerivativeZero not implemented yet");
	    break;
    }

    votca::tools::linalg_qrsolve(_f2, A, _f2);
}

void CubicSpline::Fit(ub::vector<double> &x, ub::vector<double> &y)
{
    if(x.size() != y.size())
        throw std::invalid_argument("error in CubicSpline::Fit : sizes of vectors x and y do not match");

    const int N = x.size();
    const int ngrid = _r.size();

    // construct the equation
    // A*u = b
    // where u = { {f[i]}, {f''[i]} }
    // and b[i] = y[i] for 0<=i<N
    // and b[i]=0 for i>=N (for smoothing condition)
    // A[i,j] contains the data fitting + the spline smoothing conditions

    ub::matrix<double> A(N, 2*ngrid);
    ub::vector<double> b(N);
    ub::matrix<double> B_constr(ngrid, 2*ngrid);  // Matrix with smoothing conditions

    A = ub::zero_matrix<double>(N, 2*ngrid);
    b  = ub::zero_vector<double>(N);
    B_constr = ub::zero_matrix<double>(ngrid, 2*ngrid);

    // Construct smoothing matrix
    AddBCToFitMatrix(B_constr, 0);

    // construct the matrix to fit the points and the vector b
    AddToFitMatrix(A, x, 0);
    b = -y; // why is it -y?

    // now do a constrained qr solve
    ub::vector<double> sol(2*ngrid);
    votca::tools::linalg_constrained_qrsolve(sol, A, b, B_constr);

    // check vector "sol" for nan's
    for(int i=0; i<2*ngrid; i++) {
        if( (isinf(sol(i))) || (isnan(sol(i))) ) {
            throw std::runtime_error("error in CubicSpline::Fit : value nan occurred due to wrong fitgrid boundaries");
        }
    }

    _f = ub::vector_range<ub::vector<double> >(sol, ub::range (0, ngrid));
    _f2 = ub::vector_range<ub::vector<double> >(sol, ub::range (ngrid, 2*ngrid));
}

}}


// Boost.Geometry
// QuickBook Example

// Copyright (c) 2021, Oracle and/or its affiliates
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

//[azimuth_strategy
//` Shows how to calculate azimuth in geographic coordinate system

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>

int main()
{
    namespace bg = boost::geometry;
    typedef bg::model::point<double, 2, bg::cs::geographic<bg::degree> > point_type;

    point_type p1(0, 0);
    point_type p2(1, 1);

    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);
    bg::strategies::azimuth::geographic<> strategy(spheroid);

    auto azimuth = boost::geometry::azimuth(p1, p2, strategy);

    std::cout << "azimuth: " << azimuth << std::endl;

    return 0;
}

//]

//[azimuth_strategy_output
/*`
Output:
[pre
azimuth: 0.788674
]
*/
//]