#include <boost\math\special_functions.hpp>

#include "common.h"

#include "cusolverOperations.h"

namespace matCUDA
{
	template< typename TElement>
	cusolverStatus_t cusolverOperations<TElement>::ls( Array<TElement> *A, Array<TElement> *x, Array<TElement> *C )
	{
		cusolverDnHandle_t handle;
		CUSOLVER_CALL( cusolverDnCreate(&handle) );

		TElement *d_A, *Workspace, *d_C;
		int INFOh = 2;

		CUDA_CALL( cudaMalloc(&d_A, A->getDim(0) * A->getDim(1) * sizeof(TElement)) );
		CUDA_CALL( cudaMalloc(&d_C, C->getDim(0) * C->getDim(1) * sizeof(TElement)) );

		CUDA_CALL( cudaMemcpy(d_A, A->data(), A->getDim(0) * A->getDim(1) * sizeof(TElement), cudaMemcpyHostToDevice) );
		CUDA_CALL( cudaMemcpy(d_C, C->data(), C->getDim(0) * C->getDim(1) * sizeof(TElement), cudaMemcpyHostToDevice) );

		int Lwork = 0;
		CUSOLVER_CALL( cusolverDnTgetrf_bufferSize(&handle, A->getDim(0), A->getDim(1), d_A, A->getDim(0), &Lwork) );

		CUDA_CALL( cudaMalloc( &Workspace, Lwork * sizeof(TElement) ) );

		int *devIpiv, *devInfo;
		size_t size_pivot = std::min(C->getDim(0),C->getDim(1));

		CUDA_CALL( cudaMalloc( &devIpiv, size_pivot * sizeof(int) ) );
		CUDA_CALL( cudaMalloc( &devInfo, sizeof(int) ) );

		/////***** performance test *****/////
		//CUDA_CALL( cudaDeviceSynchronize() );
		//tic();
		//for( int i = 0; i < 10; i++ ) {

		CUSOLVER_CALL( cusolverDnTgetrf( &handle, A->getDim(0), A->getDim(1), d_A, A->getDim(0), Workspace, devIpiv, devInfo ) );
		CUDA_CALL( cudaDeviceSynchronize() );

		// copy from GPU
		CUDA_CALL( cudaMemcpy( &INFOh, devInfo, sizeof( int ), cudaMemcpyDeviceToHost ) );

		if( INFOh > 0 )
		{
			printf("Factorization Failed: Matrix is singular\n");
			return CUSOLVER_STATUS_EXECUTION_FAILED;
		}

		CUSOLVER_CALL( cusolverDnTgetrs( &handle, CUBLAS_OP_N, std::min(A->getDim(0),A->getDim(1)), C->getDim(1), d_A, A->getDim(0), devIpiv, d_C, C->getDim(0), devInfo ) );

		//}
		//CUDA_CALL( cudaDeviceSynchronize() );
		//toc();
		////***** end of performance test *****/////

		CUDA_CALL( cudaDeviceSynchronize() );

		// copy from GPU
		INFOh = 2;
		CUDA_CALL( cudaMemcpy( &INFOh, devInfo, sizeof( int ), cudaMemcpyDeviceToHost ) );

		if( INFOh > 0 )
		{
			printf("Inversion Failed: Matrix is singular\n");
			return CUSOLVER_STATUS_EXECUTION_FAILED;
		}

		CUDA_CALL( cudaMemcpy( C->data(), d_C, C->getNElements()*sizeof( TElement ), cudaMemcpyDeviceToHost ) );
		for( int i = 0; i< x->GetDescriptor().GetDim( 0 ); i++ ) {
			for( int j = 0; j < x->GetDescriptor().GetDim( 1 ); j++ )
				(*x)( i, j ) = (*C)( i, j );
		}

		// free memory
		CUDA_CALL( cudaFree( d_A ) );
		CUDA_CALL( cudaFree( d_C ) );
		CUDA_CALL( cudaFree( Workspace ) );
		CUDA_CALL( cudaFree( devIpiv ) );
		CUDA_CALL( cudaFree( devInfo ) );

		// Destroy the handle
		CUSOLVER_CALL( cusolverDnDestroy(handle) );

		return CUSOLVER_STATUS_SUCCESS;
	}

	template cusolverStatus_t cusolverOperations<int>::ls( Array<int> *A, Array<int> *x, Array<int> *C );
	template cusolverStatus_t cusolverOperations<float>::ls( Array<float> *A, Array<float> *x, Array<float> *C );
	template cusolverStatus_t cusolverOperations<double>::ls( Array<double> *A, Array<double> *x, Array<double> *C );
	template cusolverStatus_t cusolverOperations<ComplexFloat>::ls( Array<ComplexFloat> *A, Array<ComplexFloat> *x, Array<ComplexFloat> *C );
	template cusolverStatus_t cusolverOperations<ComplexDouble>::ls( Array<ComplexDouble> *A, Array<ComplexDouble> *x, Array<ComplexDouble> *C );

	template< typename TElement>
	cusolverStatus_t cusolverOperations<TElement>::invert( Array<TElement> *result, Array<TElement> *data )
	{
		cusolverDnHandle_t handle;
		CUSOLVER_CALL( cusolverDnCreate(&handle) );

		size_t M = data->getDim(0);
		size_t N = data->getDim(1);
		size_t minMN = std::min(M,N);

		TElement *d_A, *Workspace, *d_B;
		int INFOh = 2;

		CUDA_CALL( cudaMalloc(&d_A, M * N * sizeof(TElement)) );
		CUDA_CALL( cudaMalloc(&d_B, M * N * sizeof(TElement)) );

		CUDA_CALL( cudaMemcpy(d_A, data->data(), M * N * sizeof(TElement), cudaMemcpyHostToDevice) );
		cuda_eye<TElement>( d_B, minMN );

		int Lwork = 0;
		CUSOLVER_CALL( cusolverDnTgetrf_bufferSize(&handle, M, N, d_A, M, &Lwork) );

		CUDA_CALL( cudaMalloc( &Workspace, Lwork * sizeof(TElement) ) );

		int *devIpiv, *devInfo;
		size_t size_pivot = std::min(data->getDim(0),data->getDim(1));

		CUDA_CALL( cudaMalloc( &devIpiv, size_pivot * sizeof(int) ) );
		CUDA_CALL( cudaMalloc( &devInfo, sizeof(int) ) );

		/////***** performance test *****/////
		//CUDA_CALL( cudaDeviceSynchronize() );
		//tic();
		//for( int i = 0; i < 10; i++ ) {

		CUSOLVER_CALL( cusolverDnTgetrf( &handle, M, N, d_A, M, Workspace, devIpiv, devInfo ) );
		CUDA_CALL( cudaDeviceSynchronize() );

		// copy from GPU
		CUDA_CALL( cudaMemcpy( &INFOh, devInfo, sizeof( int ), cudaMemcpyDeviceToHost ) );

		if( INFOh > 0 )
		{
			printf("Factorization Failed: Matrix is singular\n");
			return CUSOLVER_STATUS_EXECUTION_FAILED;
		}

		CUSOLVER_CALL( cusolverDnTgetrs( &handle, CUBLAS_OP_N, data->getDim(0), data->getDim(1), d_A, data->getDim(0), devIpiv, d_B, data->getDim(0), devInfo ) );

		//}
		//CUDA_CALL( cudaDeviceSynchronize() );
		//toc();
		////***** end of performance test *****/////

		CUDA_CALL( cudaDeviceSynchronize() );

		// copy from GPU
		INFOh = 2;
		CUDA_CALL( cudaMemcpy( &INFOh, devInfo, sizeof( int ), cudaMemcpyDeviceToHost ) );

		if( INFOh > 0 )
		{
			printf("Inversion Failed: Matrix is singular\n");
			return CUSOLVER_STATUS_EXECUTION_FAILED;
		}

		CUDA_CALL( cudaMemcpy( result->data(), d_B, result->getNElements()*sizeof( TElement ), cudaMemcpyDeviceToHost ) );

		// free memory
		CUDA_CALL( cudaFree( d_A ) );
		CUDA_CALL( cudaFree( d_B ) );
		CUDA_CALL( cudaFree( Workspace ) );
		CUDA_CALL( cudaFree( devIpiv ) );
		CUDA_CALL( cudaFree( devInfo ) );

		// Destroy the handle
		CUSOLVER_CALL( cusolverDnDestroy(handle) );

		return CUSOLVER_STATUS_SUCCESS;
	}

	template cusolverStatus_t cusolverOperations<int>::invert( Array<int> *result, Array<int> *data );
	template cusolverStatus_t cusolverOperations<float>::invert( Array<float> *result, Array<float> *data );
	template cusolverStatus_t cusolverOperations<double>::invert( Array<double> *result, Array<double> *data );
	template cusolverStatus_t cusolverOperations<ComplexFloat>::invert( Array<ComplexFloat> *result, Array<ComplexFloat> *data );
	template cusolverStatus_t cusolverOperations<ComplexDouble>::invert( Array<ComplexDouble> *result, Array<ComplexDouble> *data );

	template< typename TElement>
	cusolverStatus_t cusolverOperations<TElement>::invert_zerocopy( Array<TElement> *result, Array<TElement> *data )
	{
		cusolverDnHandle_t handle;
		CUSOLVER_CALL( cusolverDnCreate(&handle) );

		size_t M = data->getDim(0);
		size_t N = data->getDim(1);
		size_t minMN = std::min(M,N);

		TElement *d_A, *Workspace, *d_B;

		CUDA_CALL( cudaMalloc(&d_A, M * N * sizeof(TElement)) );

		// pass host pointer to device
		CUDA_CALL( cudaHostGetDevicePointer( &d_B, result->data(), 0 ) );

		CUDA_CALL( cudaMemcpy(d_A, data->data(), M * N * sizeof(TElement), cudaMemcpyHostToDevice) );
		cuda_eye<TElement>( d_B, minMN );

		int Lwork = 0;
		CUSOLVER_CALL( cusolverDnTgetrf_bufferSize(&handle, M, N, d_A, M, &Lwork) );

		CUDA_CALL( cudaMalloc( &Workspace, Lwork * sizeof(TElement) ) );

		int *devIpiv, *devInfo;
		size_t size_pivot = std::min(data->getDim(0),data->getDim(1));

		CUDA_CALL( cudaMalloc( &devIpiv, size_pivot * sizeof(int) ) );
		CUDA_CALL( cudaMalloc( &devInfo, sizeof(int) ) );

		CUSOLVER_CALL( cusolverDnTgetrf( &handle, M, N, d_A, M, Workspace, devIpiv, devInfo ) );
		CUDA_CALL( cudaDeviceSynchronize() );

		// copy from GPU
		int INFOh = 2;
		CUDA_CALL( cudaMemcpy( &INFOh, devInfo, sizeof( int ), cudaMemcpyDeviceToHost ) );

		if( INFOh > 0 )
		{
			printf("Factorization Failed: Matrix is singular\n");
			return CUSOLVER_STATUS_EXECUTION_FAILED;
		}

		CUSOLVER_CALL( cusolverDnTgetrs( &handle, CUBLAS_OP_N, data->getDim(0), data->getDim(1), d_A, data->getDim(0), devIpiv, d_B, data->getDim(0), devInfo ) );
		CUDA_CALL( cudaDeviceSynchronize() );

		// copy from GPU
		INFOh = 2;
		CUDA_CALL( cudaMemcpy( &INFOh, devInfo, sizeof( int ), cudaMemcpyDeviceToHost ) );

		if( INFOh > 0 )
		{
			printf("Inversion Failed: Matrix is singular\n");
			return CUSOLVER_STATUS_EXECUTION_FAILED;
		}

		// free memory
		CUDA_CALL( cudaFree( d_A ) );
		CUDA_CALL( cudaFree( Workspace ) );
		CUDA_CALL( cudaFree( devIpiv ) );
		CUDA_CALL( cudaFree( devInfo ) );

		// Destroy the handle
		CUSOLVER_CALL( cusolverDnDestroy(handle) );

		return CUSOLVER_STATUS_SUCCESS;
	}

	template cusolverStatus_t cusolverOperations<int>::invert_zerocopy( Array<int> *result, Array<int> *data );
	template cusolverStatus_t cusolverOperations<float>::invert_zerocopy( Array<float> *result, Array<float> *data );
	template cusolverStatus_t cusolverOperations<double>::invert_zerocopy( Array<double> *result, Array<double> *data );
	template cusolverStatus_t cusolverOperations<ComplexFloat>::invert_zerocopy( Array<ComplexFloat> *result, Array<ComplexFloat> *data );
	template cusolverStatus_t cusolverOperations<ComplexDouble>::invert_zerocopy( Array<ComplexDouble> *result, Array<ComplexDouble> *data );

	template< typename TElement>
	cusolverStatus_t cusolverOperations<TElement>::lu( Array<TElement> *A, Array<TElement> *lu, Array<TElement> *Pivot )
	{
		cusolverDnHandle_t handle;
		CUSOLVER_CALL( cusolverDnCreate(&handle) );

		size_t M = A->getDim(0);
		size_t N = A->getDim(1);
		size_t minMN = std::min(M,N);

		TElement *d_A, *Workspace;

		CUDA_CALL( cudaMalloc(&d_A, M * N * sizeof(TElement)) );
		CUDA_CALL( cudaMemcpy(d_A, A->data(), M * N * sizeof(TElement), cudaMemcpyHostToDevice) );

		int Lwork = 0;
		CUSOLVER_CALL( cusolverDnTgetrf_bufferSize(&handle, M, N, d_A, M, &Lwork) );

		CUDA_CALL( cudaMalloc( &Workspace, Lwork * sizeof(TElement) ) );

		int *devIpiv, *devInfo;
		size_t size_pivot = std::min(A->getDim(0),A->getDim(1));

		CUDA_CALL( cudaMalloc( &devIpiv, size_pivot * sizeof(int) ) );
		CUDA_CALL( cudaMalloc( &devInfo, sizeof(int) ) );

		CUSOLVER_CALL( cusolverDnTgetrf( &handle, M, N, d_A, M, Workspace, devIpiv, devInfo ) );
		CUDA_CALL( cudaDeviceSynchronize() );

		// copy from GPU
		int INFOh = 2;
		CUDA_CALL( cudaMemcpy( &INFOh, devInfo, sizeof( int ), cudaMemcpyDeviceToHost ) );

		if( INFOh > 0 )
		{
			printf("Factorization Failed: Matrix is singular\n");
			return CUSOLVER_STATUS_EXECUTION_FAILED;
		}

		Array<int> pivotVector( size_pivot );
		CUDA_CALL( cudaMemcpy( lu->data(), d_A, lu->getNElements()*sizeof( TElement ), cudaMemcpyDeviceToHost ) );
		CUDA_CALL( cudaMemcpy( pivotVector.data(), devIpiv, size_pivot*sizeof( int ), cudaMemcpyDeviceToHost ) );

		from_permutation_vector_to_permutation_matrix( Pivot, &pivotVector );

		// free memory
		CUDA_CALL( cudaFree( d_A ) );
		CUDA_CALL( cudaFree( Workspace ) );
		CUDA_CALL( cudaFree( devIpiv ) );
		CUDA_CALL( cudaFree( devInfo ) );

		// Destroy the handle
		CUSOLVER_CALL( cusolverDnDestroy(handle) );

		return CUSOLVER_STATUS_SUCCESS;
	}

	template cusolverStatus_t cusolverOperations<int>::lu( Array<int> *A, Array<int> *lu, Array<int> *Pivot );
	template cusolverStatus_t cusolverOperations<float>::lu( Array<float> *A, Array<float> *lu, Array<float> *Pivot );
	template cusolverStatus_t cusolverOperations<double>::lu( Array<double> *A, Array<double> *lu, Array<double> *Pivot );
	template cusolverStatus_t cusolverOperations<ComplexFloat>::lu( Array<ComplexFloat> *A, Array<ComplexFloat> *lu, Array<ComplexFloat> *Pivot );
	template cusolverStatus_t cusolverOperations<ComplexDouble>::lu( Array<ComplexDouble> *A, Array<ComplexDouble> *lu, Array<ComplexDouble> *Pivot );

	template< typename TElement>
	cusolverStatus_t cusolverOperations<TElement>::lu( Array<TElement> *A, Array<TElement> *lu )
	{
		cusolverDnHandle_t handle;
		CUSOLVER_CALL( cusolverDnCreate(&handle) );

		size_t M = A->getDim(0);
		size_t N = A->getDim(1);
		size_t minMN = std::min(M,N);

		TElement *d_A, *Workspace;

		CUDA_CALL( cudaMalloc(&d_A, M * N * sizeof(TElement)) );
		CUDA_CALL( cudaMemcpy(d_A, A->data(), M * N * sizeof(TElement), cudaMemcpyHostToDevice) );

		int Lwork = 0;
		CUSOLVER_CALL( cusolverDnTgetrf_bufferSize(&handle, M, N, d_A, M, &Lwork) );

		CUDA_CALL( cudaMalloc( &Workspace, Lwork * sizeof(TElement) ) );

		int *devIpiv, *devInfo;
		size_t size_pivot = std::min(A->getDim(0),A->getDim(1));

		CUDA_CALL( cudaMalloc( &devIpiv, size_pivot * sizeof(int) ) );
		CUDA_CALL( cudaMalloc( &devInfo, sizeof(int) ) );

		/////***** performance test *****/////
		//CUDA_CALL( cudaDeviceSynchronize() );
		//tic();
		//for( int i = 0; i < 10; i++ ) {

		CUSOLVER_CALL( cusolverDnTgetrf( &handle, M, N, d_A, M, Workspace, devIpiv, devInfo ) );

		//}
		//CUDA_CALL( cudaDeviceSynchronize() );
		//toc();
		////***** end of performance test *****/////

		CUDA_CALL( cudaDeviceSynchronize() );

		// copy from GPU
		int INFOh = 2;
		CUDA_CALL( cudaMemcpy( &INFOh, devInfo, sizeof( int ), cudaMemcpyDeviceToHost ) );

		if( INFOh > 0 )
		{
			printf("Factorization Failed: Matrix is singular\n");
			return CUSOLVER_STATUS_EXECUTION_FAILED;
		}

		CUDA_CALL( cudaMemcpy( lu->data(), d_A, lu->getNElements()*sizeof( TElement ), cudaMemcpyDeviceToHost ) );

		// free memory
		CUDA_CALL( cudaFree( d_A ) );
		CUDA_CALL( cudaFree( Workspace ) );
		CUDA_CALL( cudaFree( devIpiv ) );
		CUDA_CALL( cudaFree( devInfo ) );

		// Destroy the handle
		CUSOLVER_CALL( cusolverDnDestroy(handle) );

		return CUSOLVER_STATUS_SUCCESS;
	}

	template cusolverStatus_t cusolverOperations<int>::lu( Array<int> *A, Array<int> *lu );
	template cusolverStatus_t cusolverOperations<float>::lu( Array<float> *A, Array<float> *lu );
	template cusolverStatus_t cusolverOperations<double>::lu( Array<double> *A, Array<double> *lu );
	template cusolverStatus_t cusolverOperations<ComplexFloat>::lu( Array<ComplexFloat> *A, Array<ComplexFloat> *lu );
	template cusolverStatus_t cusolverOperations<ComplexDouble>::lu( Array<ComplexDouble> *A, Array<ComplexDouble> *lu );

	template <typename TElement>
	cusolverStatus_t cusolverOperations<TElement>::qr( Array<TElement> *A, Array<TElement> *Q, Array<TElement> *R )
	{
		cusolverDnHandle_t handle;
		CUSOLVER_CALL( cusolverDnCreate(&handle) );

		int M = A->getDim(0);
		int N = A->getDim(1);
		int minMN = std::min(M,N);

		TElement *d_A, *h_A, *TAU, *Workspace, *d_Q;
		h_A = A->data();
		CUDA_CALL( cudaMalloc(&d_A, M * N * sizeof(TElement)) );
		CUDA_CALL( cudaMemcpy(d_A, h_A, M * N * sizeof(TElement), cudaMemcpyHostToDevice) );

		int Lwork = 0;
		CUSOLVER_CALL( cusolverDnTgeqrf_bufferSize(&handle, M, N, d_A, M, &Lwork) );
		CUDA_CALL( cudaMalloc(&TAU, minMN * sizeof(TElement)) );
		CUDA_CALL(cudaMalloc(&Workspace, Lwork * sizeof(TElement)));

		int *devInfo;
		CUDA_CALL( cudaMalloc(&devInfo, sizeof(int)) );
		CUDA_CALL( cudaMemset( (void*)devInfo, 0, 1 ) );

		CUSOLVER_CALL( cusolverDnTgeqrf(&handle, M, N, d_A, M, TAU, Workspace, Lwork, devInfo) );

		int devInfo_h = 0;
		CUDA_CALL( cudaMemcpy(&devInfo_h, devInfo, sizeof(int), cudaMemcpyDeviceToHost) );
		if (devInfo_h != 0)
			return CUSOLVER_STATUS_INTERNAL_ERROR;

		// CALL CUDA FUNCTION
		CUDA_CALL( cudaMemcpy( R->data(), d_A, R->getNElements()*sizeof( TElement ), cudaMemcpyDeviceToHost ) );
		for(int j = 0; j < M; j++)
			for(int i = j + 1; i < N; i++)
				(*R)(i,j) = 0;

		// --- Initializing the output Q matrix (Of course, this step could be done by a kernel function directly on the device)
		//*Q = eye<TElement> ( std::min(Q->getDim(0),Q->getDim(1)) );
		CUDA_CALL( cudaMalloc(&d_Q, M*M*sizeof(TElement)) );
		cuda_eye<TElement>( d_Q, std::min(Q->getDim(0),Q->getDim(1)) );

		// --- CUDA qr execution
		CUSOLVER_CALL( cusolverDnTormqr(&handle, CUBLAS_SIDE_LEFT, CUBLAS_OP_N, M, N, std::min(M, N), d_A, M, TAU, d_Q, M, Workspace, Lwork, devInfo) );

		// --- At this point, d_Q contains the elements of Q. Showing this.
		CUDA_CALL( cudaMemcpy(Q->data(), d_Q, M*M*sizeof(TElement), cudaMemcpyDeviceToHost) );
		CUDA_CALL( cudaDeviceSynchronize() );

		CUSOLVER_CALL( cusolverDnDestroy(handle) );

		return CUSOLVER_STATUS_SUCCESS;
	}

	template cusolverStatus_t cusolverOperations<int>::qr( Array<int> *A, Array<int> *Q, Array<int> *R );
	template cusolverStatus_t cusolverOperations<float>::qr( Array<float> *A, Array<float> *Q, Array<float> *R );
	template cusolverStatus_t cusolverOperations<double>::qr( Array<double> *A, Array<double> *Q, Array<double> *R );
	template cusolverStatus_t cusolverOperations<ComplexFloat>::qr( Array<ComplexFloat> *A, Array<ComplexFloat> *Q, Array<ComplexFloat> *R );
	template cusolverStatus_t cusolverOperations<ComplexDouble>::qr( Array<ComplexDouble> *A, Array<ComplexDouble> *Q, Array<ComplexDouble> *R );

	template<> cusolverStatus_t cusolverOperations<ComplexFloat>::dpss( Array<ComplexFloat> *eigenvector, index_t N, double NW, index_t degree )
	{
		return CUSOLVER_STATUS_NOT_INITIALIZED;
	}

	template<> cusolverStatus_t cusolverOperations<ComplexDouble>::dpss( Array<ComplexDouble> *eigenvector, index_t N, double NW, index_t degree )
	{
		return CUSOLVER_STATUS_NOT_INITIALIZED;
	}

	template <typename TElement>
	cusolverStatus_t cusolverOperations<TElement>::dpss( Array<TElement> *eigenvector, index_t N, double NW, index_t degree )
	{
		// define matrix T (NxN)
		TElement** T = new TElement*[ N ];
		for(int i = 0; i < N; ++i)
			T[ i ] = new TElement[ N ];

		// fill in T as function of ( N, W )
		// T is a tridiagonal matrix, i. e., it has diagonal, subdiagonal and superdiagonal
		// the others elements are 0
		for (int i = 0; i < N; i++) {
			for (int j = 0; j < N; j++) {
				if( j == i - 1 ) // subdiagonal
					T[ i ][ j ] = ( (TElement)N - i )*i/2;
				else if( j == i ) // diagonal
					T[ i ][ j ] = pow( (TElement)(N-1)/2 - i, 2 )*boost::math::cos_pi( 2*NW/(TElement)N/boost::math::constants::pi<TElement>() );
				else if( j == i + 1 ) // superdiagonal
					T[ i ][ j ] = ( i + 1 )*( (TElement)N - 1 - i )/2*( j == i + 1 );
				else // others elements
					T[ i ][ j ] = 0;
			}
		}

		// declarations needed
		cusolverStatus_t statCusolver = CUSOLVER_STATUS_SUCCESS;
		cusolverSpHandle_t handleCusolver = NULL;
		cusparseHandle_t handleCusparse = NULL;
		cusparseMatDescr_t descrA = NULL;
		int *h_cooRowIndex = NULL, *h_cooColIndex = NULL;
		TElement *h_cooVal = NULL;
		int *d_cooRowIndex = NULL, *d_cooColIndex = NULL, *d_csrRowPtr = NULL;
		TElement *d_cooVal = NULL;
		int nnz;
		TElement *h_eigenvector0 = NULL, *d_eigenvector0 = NULL, *d_eigenvector = NULL;
		int maxite = 1e6; // number of maximum iteration
		TElement tol = 1; // tolerance
		TElement mu, *d_mu;
		TElement max_lambda;

		// define interval of eigenvalues of T
		// interval is [-max_lambda,max_lambda]
		max_lambda = ( N - 1 )*( N + 2 ) + N*( N + 1 )/8 + 0.25;

		// amount of nonzero elements of T
		nnz = 3*N - 2;

		// allocate host memory
		h_cooRowIndex = new int[ nnz*sizeof( int ) ];
		h_cooColIndex = new int[ nnz*sizeof( int ) ];
		h_cooVal = new TElement[ nnz*sizeof( TElement ) ];
		h_eigenvector0 = new TElement[ N*sizeof( TElement ) ];

		// fill in vectors that describe T as a sparse matrix
		int counter = 0;
		for (int i = 0; i < N; i++ ) {
			for( int j = 0; j < N; j++ ) {
				if( T[ i ][ j ] != 0 ) {
					h_cooRowIndex[counter] = i;
					h_cooColIndex[counter] = j;
					h_cooVal[counter++] = T[ i ][ j ];
				}
			}
		}

		// fill in initial eigenvector guess
		for( int i = 0; i < N; i++ )
			h_eigenvector0[ i ] =  1/( abs( i - N/2 ) + 1 );

		// allocate device memory
		CUDA_CALL( cudaMalloc((void**)&d_cooRowIndex,nnz*sizeof( int )) );
		CUDA_CALL( cudaMalloc((void**)&d_cooColIndex,nnz*sizeof( int )) );
		CUDA_CALL( cudaMalloc((void**)&d_cooVal, nnz*sizeof( TElement )) );
		CUDA_CALL( cudaMalloc((void**)&d_csrRowPtr, (N+1)*sizeof( int )) );
		CUDA_CALL( cudaMalloc((void**)&d_eigenvector0, N*sizeof( TElement )) );
		CUDA_CALL( cudaMalloc((void**)&d_eigenvector, N*sizeof( TElement )) );
		CUDA_CALL( cudaMalloc( &d_mu, sizeof( TElement ) ) );
		CUDA_CALL( cudaMemset( d_mu, -max_lambda, sizeof( TElement ) ) );

		// copy data to device
		CUDA_CALL( cudaMemcpy( d_cooRowIndex, h_cooRowIndex, (size_t)(nnz*sizeof( int )), cudaMemcpyHostToDevice ) );
		CUDA_CALL( cudaMemcpy( d_cooColIndex, h_cooColIndex, (size_t)(nnz*sizeof( int )), cudaMemcpyHostToDevice ) );
		CUDA_CALL( cudaMemcpy( d_cooVal, h_cooVal, (size_t)(nnz*sizeof( TElement )), cudaMemcpyHostToDevice ) );
		CUDA_CALL( cudaMemcpy( d_eigenvector0, h_eigenvector0, (size_t)(N*sizeof( TElement )), cudaMemcpyHostToDevice ) );
		CUDA_CALL( cudaMemcpy( &mu, d_mu, sizeof( TElement ), cudaMemcpyDeviceToHost ) );

		// initialize cusparse and cusolver
		CUSOLVER_CALL( cusolverSpCreate( &handleCusolver ) );
		CUSPARSE_CALL( cusparseCreate( &handleCusparse ) );

		// create and define cusparse matrix descriptor
		CUSPARSE_CALL( cusparseCreateMatDescr(&descrA) );
		CUSPARSE_CALL( cusparseSetMatType(descrA, CUSPARSE_MATRIX_TYPE_GENERAL ) );
		CUSPARSE_CALL( cusparseSetMatIndexBase(descrA, CUSPARSE_INDEX_BASE_ZERO ) );

		// transform from coordinates (COO) values to compressed row pointers (CSR) values
		CUSPARSE_CALL( cusparseXcoo2csr( handleCusparse, d_cooRowIndex, nnz, N, d_csrRowPtr, CUSPARSE_INDEX_BASE_ZERO ) );

		// call cusolverSp<type>csreigvsi
		CUSOLVER_CALL( cusolverSpTcsreigvsi( &handleCusolver, N, nnz, &descrA, d_cooVal, d_csrRowPtr, d_cooColIndex, max_lambda, d_eigenvector0, maxite, tol, d_mu, d_eigenvector ) );

		cudaDeviceSynchronize();
		CUDA_CALL( cudaGetLastError() );

		// copy from device to host
		CUDA_CALL( cudaMemcpy( &mu, d_mu, (size_t)sizeof( TElement ), cudaMemcpyDeviceToHost ) );
		CUDA_CALL( cudaMemcpy( eigenvector->data(), d_eigenvector, (size_t)(N*sizeof( TElement )), cudaMemcpyDeviceToHost ) );

		// destroy and free stuff
		CUSPARSE_CALL( cusparseDestroyMatDescr( descrA ) );
		CUSPARSE_CALL( cusparseDestroy( handleCusparse ) );
		CUSOLVER_CALL( cusolverSpDestroy( handleCusolver ) );
		CUDA_CALL( cudaFree( d_cooRowIndex ) );
		CUDA_CALL( cudaFree( d_cooColIndex ) );
		CUDA_CALL( cudaFree( d_cooVal ) );
		CUDA_CALL( cudaFree( d_csrRowPtr ) );
		CUDA_CALL( cudaFree( d_eigenvector0 ) );
		CUDA_CALL( cudaFree( d_eigenvector ) );
		CUDA_CALL( cudaFree( d_mu ) );
		delete[] h_eigenvector0;
		delete[] h_cooRowIndex;
		delete[] h_cooColIndex;
		delete[] h_cooVal;

		return CUSOLVER_STATUS_SUCCESS;
	}

	template cusolverStatus_t cusolverOperations<float>::dpss( Array<float> *eigenvector, index_t N, double NW, index_t degree );
	template cusolverStatus_t cusolverOperations<double>::dpss( Array<double> *eigenvector, index_t N, double NW, index_t degree );

	template <typename TElement>
	cusolverStatus_t cusolverOperations<TElement>::qr_zerocopy( Array<TElement> *A, Array<TElement> *Q, Array<TElement> *R )
	{
		cusolverDnHandle_t handle;
		CUSOLVER_CALL( cusolverDnCreate(&handle) );

		int M = A->getDim(0);
		int N = A->getDim(1);
		int minMN = std::min(M,N);

		TElement *d_A, *h_A, *TAU, *Workspace, *d_Q, *d_R;
		h_A = A->data();
		CUDA_CALL( cudaMalloc(&d_A, M * N * sizeof(TElement)) );
		CUDA_CALL( cudaMemcpy(d_A, h_A, M * N * sizeof(TElement), cudaMemcpyHostToDevice) );

		int Lwork = 0;
		CUSOLVER_CALL( cusolverDnTgeqrf_bufferSize(&handle, M, N, d_A, M, &Lwork) );
		CUDA_CALL( cudaMalloc(&TAU, minMN * sizeof(TElement)) );
		CUDA_CALL(cudaMalloc(&Workspace, Lwork * sizeof(TElement)));

		int *devInfo;
		CUDA_CALL( cudaMalloc(&devInfo, sizeof(int)) );
		CUDA_CALL( cudaMemset( (void*)devInfo, 0, 1 ) );

		CUSOLVER_CALL( cusolverDnTgeqrf(&handle, M, N, d_A, M, TAU, Workspace, Lwork, devInfo) );

		int devInfo_h = 0;
		CUDA_CALL( cudaMemcpy(&devInfo_h, devInfo, sizeof(int), cudaMemcpyDeviceToHost) );
		if (devInfo_h != 0)
			return CUSOLVER_STATUS_INTERNAL_ERROR;

		// CALL CUDA FUNCTION
		CUDA_CALL( cudaMemcpy( R->data(), d_A, R->getNElements()*sizeof( TElement ), cudaMemcpyDeviceToHost ) );

		// pass host pointer to device
		CUDA_CALL( cudaHostGetDevicePointer( &d_R, R->data(), 0 ) );
		CUDA_CALL( cudaHostGetDevicePointer( &d_Q, Q->data(), 0 ) );

		zeros_under_diag<TElement>( d_R, std::min(R->getDim(0),R->getDim(1)) );
		//for(int j = 0; j < M; j++)
		//	for(int i = j + 1; i < N; i++)
		//		(*R)(i,j) = 0;

		// --- Initializing the output Q matrix (Of course, this step could be done by a kernel function directly on the device)
		//*Q = eye<TElement> ( std::min(Q->getDim(0),Q->getDim(1)) );
		cuda_eye<TElement>( d_Q, std::min(Q->getDim(0),Q->getDim(1)) );
		CUDA_CALL( cudaDeviceSynchronize() );

		// --- CUDA qr_zerocopy execution
		CUSOLVER_CALL( cusolverDnTormqr(&handle, CUBLAS_SIDE_LEFT, CUBLAS_OP_N, M, N, std::min(M, N), d_A, M, TAU, d_Q, M, Workspace, Lwork, devInfo) );
		CUDA_CALL( cudaDeviceSynchronize() );

		CUSOLVER_CALL( cusolverDnDestroy(handle) );

		return CUSOLVER_STATUS_SUCCESS;
	}

	template cusolverStatus_t cusolverOperations<int>::qr_zerocopy( Array<int> *A, Array<int> *Q, Array<int> *R );
	template cusolverStatus_t cusolverOperations<float>::qr_zerocopy( Array<float> *A, Array<float> *Q, Array<float> *R );
	template cusolverStatus_t cusolverOperations<double>::qr_zerocopy( Array<double> *A, Array<double> *Q, Array<double> *R );
	template cusolverStatus_t cusolverOperations<ComplexFloat>::qr_zerocopy( Array<ComplexFloat> *A, Array<ComplexFloat> *Q, Array<ComplexFloat> *R );
	template cusolverStatus_t cusolverOperations<ComplexDouble>::qr_zerocopy( Array<ComplexDouble> *A, Array<ComplexDouble> *Q, Array<ComplexDouble> *R );// transform permutation vector into permutation matrix
	// TODO (or redo) - too slow!!!

	template <typename TElement>
	void cusolverOperations<TElement>::from_permutation_vector_to_permutation_matrix( Array<TElement> *pivotMatrix, Array<int> *pivotVector )
	{
		//pivotVector->print();
		//*pivotMatrix = eye<TElement>(pivotVector->getDim(0));
		//index_t idx1, idx2;
		//for( int i = 0; i < pivotVector->GetDescriptor().GetDim(0); i++ ) {
		//	if( i + 1 == (*pivotVector)(i) )
		//		continue;
		//	else
		//	{
		//		idx1 = i;
		//		idx2 = (*pivotVector)(i)-1;
		//		(*pivotMatrix)( idx1, idx1 ) = 0;
		//		(*pivotMatrix)( idx2, idx2 ) = 0;
		//		(*pivotMatrix)( idx1, idx2 ) = 1;
		//		(*pivotMatrix)( idx2, idx1 ) = 1;
		//	}
		//	pivotMatrix->print();
		//}
		//pivotMatrix->print();
		//
		//*pivotMatrix = eye<TElement>(pivotVector->getDim(0));

		//pivotVector->print();
		//eye<double>(pivotVector->getDim(0)).print();
		Array<TElement> pivotAux = eye<TElement>(pivotVector->GetDescriptor().GetDim(0));
		index_t idx1, idx2;
		for( int i = 0; i < pivotVector->GetDescriptor().GetDim(0); i++ ) {
			idx1 = i;
			idx2 = (*pivotVector)(i)-1;
			pivotAux( idx1, idx1 ) = 0;
			pivotAux( idx2, idx2 ) = 0;
			pivotAux( idx1, idx2 ) = 1;
			pivotAux( idx2, idx1 ) = 1;

			(*pivotMatrix) = pivotAux*(*pivotMatrix);
			pivotAux = eye<TElement>(pivotVector->GetDescriptor().GetDim(0));
			//pivotMatrix->print();
		}
		//pivotMatrix->print();
	}

	cusolverStatus_t cusolverOperations<float>::cusolverSpTcsreigvsi( cusolverSpHandle_t *handle, int m, int nnz, cusparseMatDescr_t *descrA, const float *csrValA, const int *csrRowPtrA, const int *csrColIndA, float mu0, const float *x0, int maxite, float tol, float *mu, float *x )
	{
		return cusolverSpScsreigvsi( *handle, m ,nnz, *descrA, csrValA, csrRowPtrA, csrColIndA, mu0, x0, maxite, tol, mu, x );;
	}

	cusolverStatus_t cusolverOperations<double>::cusolverSpTcsreigvsi( cusolverSpHandle_t *handle, int m, int nnz, cusparseMatDescr_t *descrA, const double *csrValA, const int *csrRowPtrA, const int *csrColIndA, double mu0, const double *x0, int maxite, double tol, double *mu, double *x )
	{
		return cusolverSpDcsreigvsi( *handle, m ,nnz, *descrA, csrValA, csrRowPtrA, csrColIndA, mu0, x0, maxite, tol, mu, x );
	}

	cusolverStatus_t cusolverOperations<ComplexFloat>::cusolverSpTcsreigvsi( cusolverSpHandle_t *handle, int m, int nnz, cusparseMatDescr_t *descrA, const ComplexFloat *csrValA, const int *csrRowPtrA, const int *csrColIndA, ComplexFloat mu0, const ComplexFloat *x0, int maxite, ComplexFloat tol, ComplexFloat *mu, ComplexFloat *x )
	{
		cuFloatComplex mu02 = make_cuFloatComplex( mu0.real(), mu0.imag() );
		return cusolverSpCcsreigvsi( *handle, m ,nnz, *descrA, (const cuFloatComplex*)csrValA, csrRowPtrA, csrColIndA, mu02, (const cuFloatComplex*)x0, maxite, tol.real(), (cuFloatComplex*)mu, (cuFloatComplex*)x );
	}

	cusolverStatus_t cusolverOperations<ComplexDouble>::cusolverSpTcsreigvsi( cusolverSpHandle_t *handle, int m, int nnz, cusparseMatDescr_t *descrA, const ComplexDouble *csrValA, const int *csrRowPtrA, const int *csrColIndA, ComplexDouble mu0, const ComplexDouble *x0, int maxite, ComplexDouble tol, ComplexDouble *mu, ComplexDouble *x )
	{
		cuDoubleComplex mu02 = make_cuDoubleComplex( mu0.real(), mu0.imag() );
		return cusolverSpZcsreigvsi( *handle, m ,nnz, *descrA, (const cuDoubleComplex*)csrValA, csrRowPtrA, csrColIndA, mu02, (const cuDoubleComplex*)x0, maxite, tol.real(), (cuDoubleComplex*)mu, (cuDoubleComplex*)x );
	}

	cusolverStatus_t cusolverOperations<int>::cusolverDnTgeqrf_bufferSize( cusolverDnHandle_t *handle, int m, int n, int *A, int lda, int *Lwork )
	{
		return CUSOLVER_STATUS_SUCCESS;
	}

	cusolverStatus_t cusolverOperations<float>::cusolverDnTgeqrf_bufferSize( cusolverDnHandle_t *handle, int m, int n, float *A, int lda, int *Lwork )
	{
		return cusolverDnSgeqrf_bufferSize( *handle, m, n, A, lda, Lwork );
	}

	cusolverStatus_t cusolverOperations<double>::cusolverDnTgeqrf_bufferSize( cusolverDnHandle_t *handle, int m, int n, double *A, int lda, int *Lwork )
	{
		return cusolverDnDgeqrf_bufferSize( *handle, m, n, A, lda, Lwork );
	}

	cusolverStatus_t cusolverOperations<ComplexFloat>::cusolverDnTgeqrf_bufferSize( cusolverDnHandle_t *handle, int m, int n, ComplexFloat *A, int lda, int *Lwork )
	{
		return cusolverDnCgeqrf_bufferSize( *handle, m, n, (cuFloatComplex*)A, lda, Lwork );
	}

	cusolverStatus_t cusolverOperations<ComplexDouble>::cusolverDnTgeqrf_bufferSize( cusolverDnHandle_t *handle, int m, int n, ComplexDouble *A, int lda, int *Lwork )
	{
		return cusolverDnZgeqrf_bufferSize( *handle, m, n, (cuDoubleComplex*)A, lda, Lwork );
	}

	cusolverStatus_t cusolverOperations<int>::cusolverDnTgeqrf( cusolverDnHandle_t *handle, int m, int n, int *A, int lda, int *TAU, int *Workspace, int Lwork, int *devInfo )
	{
		return CUSOLVER_STATUS_SUCCESS;
	}

	cusolverStatus_t cusolverOperations<float>::cusolverDnTgeqrf( cusolverDnHandle_t *handle, int m, int n, float *A, int lda, float *TAU, float *Workspace, int Lwork, int *devInfo )
	{
		return cusolverDnSgeqrf( *handle, m, n, A, lda, TAU, Workspace, Lwork, devInfo );
	}

	cusolverStatus_t cusolverOperations<double>::cusolverDnTgeqrf( cusolverDnHandle_t *handle, int m, int n, double *A, int lda, double *TAU, double *Workspace, int Lwork, int *devInfo )
	{
		return cusolverDnDgeqrf( *handle, m, n, A, lda, TAU, Workspace, Lwork, devInfo );
	}

	cusolverStatus_t cusolverOperations<ComplexFloat>::cusolverDnTgeqrf( cusolverDnHandle_t *handle, int m, int n, ComplexFloat *A, int lda, ComplexFloat *TAU, ComplexFloat *Workspace, int Lwork, int *devInfo )
	{
		return cusolverDnCgeqrf( *handle, m, n, (cuFloatComplex*)A, lda, (cuFloatComplex*)TAU, (cuFloatComplex*)Workspace, Lwork, devInfo );
	}

	cusolverStatus_t cusolverOperations<ComplexDouble>::cusolverDnTgeqrf( cusolverDnHandle_t *handle, int m, int n, ComplexDouble *A, int lda, ComplexDouble *TAU, ComplexDouble *Workspace, int Lwork, int *devInfo )
	{
		return cusolverDnZgeqrf( *handle, m, n, (cuDoubleComplex*)A, lda, (cuDoubleComplex*)TAU, (cuDoubleComplex*)Workspace, Lwork, devInfo );
	}

	cusolverStatus_t cusolverOperations<int>::cusolverDnTormqr( cusolverDnHandle_t *handle, cublasSideMode_t side, cublasOperation_t trans, int m, int n, int k, const int *A, int lda, const int *tau, int *C, int ldc, int *work, int lwork, int *devInfo )
	{
		return CUSOLVER_STATUS_SUCCESS;
	}

	cusolverStatus_t cusolverOperations<float>::cusolverDnTormqr( cusolverDnHandle_t *handle, cublasSideMode_t side, cublasOperation_t trans, int m, int n, int k, const float *A, int lda, const float *tau, float *C, int ldc, float *work, int lwork, int *devInfo )
	{
		return cusolverDnSormqr( *handle, side, trans, m, n, k, A, lda, tau, C, ldc, work, lwork, devInfo );
	}

	cusolverStatus_t cusolverOperations<double>::cusolverDnTormqr( cusolverDnHandle_t *handle, cublasSideMode_t side, cublasOperation_t trans, int m, int n, int k, const double *A, int lda, const double *tau, double *C, int ldc, double *work, int lwork, int *devInfo )
	{
		return cusolverDnDormqr( *handle, side, trans, m, n, k, A, lda, tau, C, ldc, work, lwork, devInfo );
	}

	cusolverStatus_t cusolverOperations<ComplexFloat>::cusolverDnTormqr( cusolverDnHandle_t *handle, cublasSideMode_t side, cublasOperation_t trans, int m, int n, int k, const ComplexFloat *A, int lda, const ComplexFloat *tau, ComplexFloat *C, int ldc, ComplexFloat *work, int lwork, int *devInfo )
	{
		return cusolverDnCunmqr( *handle, side, trans, m, n, k, (const cuFloatComplex*)A, lda, (const cuFloatComplex*)tau, (cuFloatComplex*)C, ldc, (cuFloatComplex*)work, lwork, devInfo );
	}

	cusolverStatus_t cusolverOperations<ComplexDouble>::cusolverDnTormqr( cusolverDnHandle_t *handle, cublasSideMode_t side, cublasOperation_t trans, int m, int n, int k, const ComplexDouble *A, int lda, const ComplexDouble *tau, ComplexDouble *C, int ldc, ComplexDouble *work, int lwork, int *devInfo )
	{
		return cusolverDnZunmqr( *handle, side, trans, m, n, k, (const cuDoubleComplex*)A, lda, (const cuDoubleComplex*)tau, (cuDoubleComplex*)C, ldc, (cuDoubleComplex*)work, lwork, devInfo );
	}

	cusolverStatus_t cusolverOperations<int>::cusolverDnTgetrf_bufferSize( cusolverDnHandle_t *handle, int m, int n, int *A, int lda, int *Lwork )
	{
		return CUSOLVER_STATUS_SUCCESS;
	}

	cusolverStatus_t cusolverOperations<float>::cusolverDnTgetrf_bufferSize( cusolverDnHandle_t *handle, int m, int n, float *A, int lda, int *Lwork )
	{
		return cusolverDnSgetrf_bufferSize( *handle, m, n, A, lda, Lwork );
	}

	cusolverStatus_t cusolverOperations<double>::cusolverDnTgetrf_bufferSize( cusolverDnHandle_t *handle, int m, int n, double *A, int lda, int *Lwork )
	{
		return cusolverDnDgetrf_bufferSize( *handle, m, n, A, lda, Lwork );
	}

	cusolverStatus_t cusolverOperations<ComplexFloat>::cusolverDnTgetrf_bufferSize( cusolverDnHandle_t *handle, int m, int n, ComplexFloat *A, int lda, int *Lwork )
	{
		return cusolverDnCgetrf_bufferSize( *handle, m, n, (cuFloatComplex*)A, lda, Lwork );
	}

	cusolverStatus_t cusolverOperations<ComplexDouble>::cusolverDnTgetrf_bufferSize( cusolverDnHandle_t *handle, int m, int n, ComplexDouble *A, int lda, int *Lwork )
	{
		return cusolverDnZgetrf_bufferSize( *handle, m, n, (cuDoubleComplex*)A, lda, Lwork );
	}

	cusolverStatus_t cusolverOperations<int>::cusolverDnTgetrf( cusolverDnHandle_t *handle, int m, int n, int *A, int lda, int *Workspace, int *devIpiv, int *devInfo )
	{
		return CUSOLVER_STATUS_SUCCESS;
	}

	cusolverStatus_t cusolverOperations<float>::cusolverDnTgetrf( cusolverDnHandle_t *handle, int m, int n, float *A, int lda, float *Workspace, int *devIpiv, int *devInfo )
	{
		return cusolverDnSgetrf( *handle, m, n, A, lda, Workspace, devIpiv, devInfo );
	}

	cusolverStatus_t cusolverOperations<double>::cusolverDnTgetrf( cusolverDnHandle_t *handle, int m, int n, double *A, int lda, double *Workspace, int *devIpiv, int *devInfo )
	{
		return cusolverDnDgetrf( *handle, m, n, A, lda, Workspace, devIpiv, devInfo );
	}

	cusolverStatus_t cusolverOperations<ComplexFloat>::cusolverDnTgetrf( cusolverDnHandle_t *handle, int m, int n, ComplexFloat *A, int lda, ComplexFloat *Workspace, int *devIpiv, int *devInfo )
	{
		return cusolverDnCgetrf( *handle, m, n, (cuFloatComplex*)A, lda, (cuFloatComplex*)Workspace, devIpiv, devInfo );
	}

	cusolverStatus_t cusolverOperations<ComplexDouble>::cusolverDnTgetrf( cusolverDnHandle_t *handle, int m, int n, ComplexDouble *A, int lda, ComplexDouble *Workspace, int *devIpiv, int *devInfo )
	{
		return cusolverDnZgetrf( *handle, m, n, (cuDoubleComplex*)A, lda, (cuDoubleComplex*)Workspace, devIpiv, devInfo );
	}

	cusolverStatus_t cusolverOperations<int>::cusolverDnTgetrs( cusolverDnHandle_t *handle, cublasOperation_t trans, int n, int nrhs, const int *A, int lda, const int *devIpiv, int *B, int ldb, int *devInfo )
	{
		return CUSOLVER_STATUS_SUCCESS;
	}

	cusolverStatus_t cusolverOperations<float>::cusolverDnTgetrs( cusolverDnHandle_t *handle, cublasOperation_t trans, int n, int nrhs, const float *A, int lda, const int *devIpiv, float *B, int ldb, int *devInfo )
	{
		return cusolverDnSgetrs( *handle, trans, n, nrhs, A, lda, devIpiv, B, ldb, devInfo );
	}

	cusolverStatus_t cusolverOperations<double>::cusolverDnTgetrs( cusolverDnHandle_t *handle, cublasOperation_t trans, int n, int nrhs, const double *A, int lda, const int *devIpiv, double *B, int ldb, int *devInfo )
	{
		return cusolverDnDgetrs( *handle, trans, n, nrhs, A, lda, devIpiv, B, ldb, devInfo );
	}

	cusolverStatus_t cusolverOperations<ComplexFloat>::cusolverDnTgetrs( cusolverDnHandle_t *handle, cublasOperation_t trans, int n, int nrhs, const ComplexFloat *A, int lda, const int *devIpiv, ComplexFloat *B, int ldb, int *devInfo )
	{
		return cusolverDnCgetrs( *handle, trans, n, nrhs, (const cuFloatComplex*)A, lda, devIpiv, (cuFloatComplex*)B, ldb, devInfo );
	}

	cusolverStatus_t cusolverOperations<ComplexDouble>::cusolverDnTgetrs( cusolverDnHandle_t *handle, cublasOperation_t trans, int n, int nrhs, const ComplexDouble *A, int lda, const int *devIpiv, ComplexDouble *B, int ldb, int *devInfo )
	{
		return cusolverDnZgetrs( *handle, trans, n, nrhs, (const cuDoubleComplex*)A, lda, devIpiv, (cuDoubleComplex*)B, ldb, devInfo );
	}
}


// dlib dnn practice

#include <dlib/dnn.h>
#include <iostream>
#include <dlib/data_io.h>

using namespace dlib; // conv layer

// define a resnet block layer
template<int num_filter,
    template<typename> class BN,
    int stride,
    typename SUBNET>
    using block = BN < con<num_filter, 3, 3, 1, 1,
    relu<BN<con<num_filter, 3, 3, stride, stride,
    SUBNET>>>>>;

// define a resnet residual block
template<
    template<int, template<typename> class, int, typename> class block,
    int num_filter,
    template<typename> class BN,
    typename SUBNET>
    using residual = add_prev1<block<num_filter, BN, 1, tag1<SUBNET>>>;

// define downsampling (stride2) residual block
template<
    template<int, template<typename> class, int, typename> class block,
    int num_filter,
    template<typename> class BN,
    typename SUBNET>
    using residual_down = add_prev2 < avg_pool<2, 2, 2, 2,
    skip1<tag2<block<num_filter, BN, 2, tag1<SUBNET>>>>>>;

// now define 4 different residual blocks
template <typename SUBNET> using res = relu<residual<block, 8, bn_con, SUBNET>>;
template <typename SUBNET> using res_a = relu < residual<block, 8, affine, SUBNET>>;
template <typename  SUBNET> using res_down = relu<residual_down<block, 8, bn_con, SUBNET>>;
template <typename SUBNET> using resa_down = relu<residual_down<block, 8, affine, SUBNET>>;

// building the net type
const unsigned long num_classes = 10;
using net_type = loss_multiclass_log < fc<num_classes,
    avg_pool_everything<res<res<res<res_down<
    repeat<9, res,
    res_down<res<
    input<matrix<unsigned char>>>>>>>>>>>>;

int main(int argc, char**argv)
{
    // load data
    std::vector<matrix<unsigned char>> training_images;
    std::vector<unsigned long> training_labels;
    std::vector<matrix<unsigned char>> testing_images;
    std::vector<unsigned long> testing_labels;
    load_mnist_dataset("D:/Develop/DL/projects/resources/datasets/mnist", training_images, training_labels, testing_images, testing_labels);

    // use smaller ram
    set_dnn_prefer_smallest_algorithms();

    // create a net
    net_type net;

    // print layer info
    std::cout << "net has " << net.num_layers << " layers\n";
    //std::cout << net << "\n";

    // get output for layer 3 ==>  layer<3>(net).get_output()

    // now set trainer
    dnn_trainer<net_type, adam> trainer(net, adam(0.0005, 0.9, 0.999));
    trainer.be_verbose();
    trainer.set_iterations_without_progress_threshold(2000);
    trainer.set_learning_rate_shrink_factor(0.1);
    trainer.set_learning_rate(0.001);
    trainer.set_synchronization_file("D:/Develop/DL/projects/digital-image-processing/temp/mnist_res_sync",
        std::chrono::seconds(100));

    // set mini batch
    std::vector<matrix<unsigned char>> mini_batch_samples;
    std::vector<unsigned long> mini_batch_labels;
    dlib::rand rnd(time(0));

    while (trainer.get_learning_rate() >= 1e-6)
    {
        mini_batch_samples.clear();
        mini_batch_labels.clear();
        // make 128 mini batch
        while (mini_batch_samples.size() < 128)
        {
            auto idx = rnd.get_random_32bit_number() % training_images.size();
            mini_batch_samples.push_back(training_images[idx]);
            mini_batch_labels.push_back(training_labels[idx]);
        }
        // train mini batch
        trainer.train_one_step(mini_batch_samples, mini_batch_labels);
        // can also use test_one_step to show test accuracy
    }

    // train_one_step is multithreaded implementation. So need to use
    // trainer.get_net to perform synchronization
    trainer.get_net();

    // save net
    net.clean();
    serialize("D:/Develop/DL/projects/digital-image-processing/temp/mnist_res_network.dat") << net;

    // test net: batchnorm will be replaced by affine layer
    using test_net_type = loss_multiclass_log < fc < num_classes,
        avg_pool_everything<res_a<res_a<res_a<resa_down<
        repeat<9, res_a,
        resa_down<res_a<
        input<matrix<unsigned char>>>>>>>>>>>>;

    // can assign trained net to test net
    test_net_type tnet = net;
    // or deserialize from saved file

    // run training data
    std::vector<unsigned long> predicted_labels = tnet(training_images);
    int num_right = 0;
    int num_wrong = 0;
    for (size_t i = 0; i < training_images.size(); i++)
    {
        if (predicted_labels[i] == training_labels[i])
        {
            num_right++;
        }
        else
        {
            num_wrong++;
        }
    }
    std::cout << "training num right= " << num_right << "\n";
    std::cout << "training num wrong= " << num_wrong << "\n";
    std::cout << "training accuracy= " << num_right / double(num_right + num_wrong) << "\n";

    // run test data
    predicted_labels = tnet(testing_images);
    num_right = 0;
    num_wrong = 0;
    for (size_t i = 0; i < testing_images.size(); ++i)
    {
        if (predicted_labels[i] == testing_labels[i])
        {
            num_right++;
        }
        else
        {
            num_wrong++;
        }
    }
    std::cout << "testing num right= " << num_right << "\n";
    std::cout << "testing num wrong= " << num_wrong << "\n";
    std::cout << "testing accuracy= " << num_right / double(num_right + num_wrong) << "\n";

    std::system("pause");
    return 0;
}


// This is to check whether I can sidestep having to provide a Jacobian in terms of the overparameterized version

#include <ceres/ceres.h>
#include <ceres/gradient_checker.h>
#include <Eigen/Core>
#include <unsupported/Eigen/MatrixFunctions>

#include "wave/optimization/ceres/local_params/null_SE3_parameterization.hpp"
#include "wave/wave_test.hpp"
#include "wave/utils/math.hpp"
#include "wave/geometry_og/transformation.hpp"

namespace {

class TestPointToPoint : public ceres::SizedCostFunction<3, 12> {
 private:
    const double *const P1;
    const double *const P2;

 public:
    virtual ~TestPointToPoint(){};
    TestPointToPoint(const double *const p1, const double *const p2) : P1(p1), P2(p2) {}

    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
        // modelled as P1 = T*P2
        // r = T*P2 - P1;
        // parameters in order:
        // R11 R21 R31 R12 R22 R32 R13 R23 R33 X Y Z
        double Tp[3] = {parameters[0][0] * this->P2[0] + parameters[0][3] * this->P2[1] + parameters[0][6] * this->P2[2] + parameters[0][9],
                        parameters[0][1] * this->P2[0] + parameters[0][4] * this->P2[1] + parameters[0][7] * this->P2[2] + parameters[0][10],
                        parameters[0][2] * this->P2[0] + parameters[0][5] * this->P2[1] + parameters[0][8] * this->P2[2] + parameters[0][11]};

        residuals[0] = Tp[0] - this->P1[0];
        residuals[1] = Tp[1] - this->P1[1];
        residuals[2] = Tp[2] - this->P1[2];

        if(jacobians) {
            Eigen::Matrix<double, 3, 6> local_jacobian;
            local_jacobian.setZero();
            local_jacobian.block<3,3>(0,3).setIdentity();
            local_jacobian(0,1) = Tp[2];
            local_jacobian(0,2) = -Tp[1];
            local_jacobian(1,0) = -Tp[2];
            local_jacobian(1,2) = Tp[0];
            local_jacobian(2,0) = Tp[1];
            local_jacobian(2,1) = -Tp[0];

            Eigen::Matrix<double, 6, 12> bogus_inflation;
            bogus_inflation.setZero();
            bogus_inflation.block<6,6>(0,0).setIdentity();

            Eigen::Matrix<double, 3, 12> bogus_jacobian = local_jacobian * bogus_inflation;

            Eigen::Map<Eigen::Matrix<double, 3, 12, Eigen::RowMajor>>(jacobians[0], 3, 12) = bogus_jacobian;
        }
        return true;
    }
};

}

namespace wave{

TEST(local_jacobians, fake_lift_jacobian) {
    Vec6 transformation_twist_parameters;
    transformation_twist_parameters << 0.068924613882066, 0.213225926957886, 0.288748939228676, 0.965590777183138,
            1.960945901104432, 3.037052911306709;
    Transformation<Eigen::Matrix<double, 3, 4>> exact_transform;
    exact_transform.setFromExpMap(transformation_twist_parameters);

    const int size = 3;
    Vec3 points[3];
    Vec3 points_transformed[3];
    points[0] = Vec3(0, 2, 0);
    points[1] = Vec3(40, 0, 0);
    points[2] = Vec3(40, 10, 0);

    double transform[12] = {0};
    transform[0] = 1;
    transform[4] = 1;
    transform[8] = 1;

    ceres::Problem problem;
    for (int i = 0; i < size; i++) {
        points_transformed[i] = exact_transform.transform(points[i]);
        ceres::CostFunction *cost_function = new TestPointToPoint(points_transformed[i].data(), points[i].data());
        problem.AddResidualBlock(cost_function, NULL, transform);
    }
    ceres::LocalParameterization *se3 = new NullSE3Parameterization();
    problem.SetParameterization(transform, se3);

    ceres::Solver::Options options;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.FullReport();

    Transformation<Eigen::Matrix<double, 3, 4>> solved_transform;
    Mat4 sol_matrix;

    sol_matrix << transform[0], transform[3], transform[6], transform[9],  //
            transform[1], transform[4], transform[7], transform[10],             //
            transform[2], transform[5], transform[8], transform[11],             //
            0, 0, 0, 1;                                                          //

    Mat4 errmat = sol_matrix - exact_transform.getMatrix();
    ASSERT_LE(errmat.norm(), 1e-9);
}

/**
 * This test is to verify that this hack breaks the gradient checker in Ceres
 * FYI, hack breaks the gradient checker in Ceres
 */
TEST(local_jacobians, gradient_checker) {
    Vec6 transformation_twist_parameters;
    transformation_twist_parameters << 0.068924613882066, 0.213225926957886, 0.288748939228676, 0.965590777183138,
            1.960945901104432, 3.037052911306709;
    Transformation<Eigen::Matrix<double, 3, 4>> exact_transform;
    exact_transform.setFromExpMap(transformation_twist_parameters);

    Vec3 point;
    Vec3 points_transformed;
    point = Vec3(5, 2, 0);

    points_transformed = exact_transform.transform(point);
    ceres::CostFunction *cost_function = new TestPointToPoint(points_transformed.data(), point.data());

    ceres::LocalParameterization *se3 = new NullSE3Parameterization();
    std::vector<const ceres::LocalParameterization*> local_param_vec;
    local_param_vec.emplace_back(se3);

    const double **parameters;
    parameters = new const double *[1];
    parameters[0] = exact_transform.storage.data();

    ceres::NumericDiffOptions ndiff_options;
    ceres::GradientChecker g_check(cost_function, &local_param_vec, ndiff_options);
    ceres::GradientChecker::ProbeResults g_results;
    EXPECT_FALSE(g_check.Probe(parameters, 1e-6, &g_results));
    LOG_INFO("%s", g_results.error_log.c_str());
}

}


// Copyright (c) 2016 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "amount.h"
#include "test/test_bitcoin.h"

#include <boost/test/unit_test.hpp>

#include <array>

BOOST_FIXTURE_TEST_SUITE(amount_tests, BasicTestingSetup)

static void CheckAmounts(int64_t aval, int64_t bval) {
    Amount a(aval), b(bval);

    // Equality
    BOOST_CHECK_EQUAL(a == b, aval == bval);
    BOOST_CHECK_EQUAL(b == a, aval == bval);

    BOOST_CHECK_EQUAL(a != b, aval != bval);
    BOOST_CHECK_EQUAL(b != a, aval != bval);

    // Comparison
    BOOST_CHECK_EQUAL(a < b, aval < bval);
    BOOST_CHECK_EQUAL(b < a, bval < aval);

    BOOST_CHECK_EQUAL(a > b, aval > bval);
    BOOST_CHECK_EQUAL(b > a, bval > aval);

    BOOST_CHECK_EQUAL(a <= b, aval <= bval);
    BOOST_CHECK_EQUAL(b <= a, bval <= aval);

    BOOST_CHECK_EQUAL(a >= b, aval >= bval);
    BOOST_CHECK_EQUAL(b >= a, bval >= aval);

    // Unary minus
    BOOST_CHECK_EQUAL(-a, Amount(-aval));
    BOOST_CHECK_EQUAL(-b, Amount(-bval));

    // Addition and subtraction.
    BOOST_CHECK_EQUAL(a + b, b + a);
    BOOST_CHECK_EQUAL(a + b, Amount(aval + bval));

    BOOST_CHECK_EQUAL(a - b, -(b - a));
    BOOST_CHECK_EQUAL(a - b, Amount(aval - bval));

    // Multiplication
    BOOST_CHECK_EQUAL(aval * b, bval * a);
    BOOST_CHECK_EQUAL(aval * b, Amount(aval * bval));

    // Division
    if (b != Amount(0)) {
        BOOST_CHECK_EQUAL(a / b, aval / bval);
        BOOST_CHECK_EQUAL(a / bval, Amount(a / b));
    }

    if (a != Amount(0)) {
        BOOST_CHECK_EQUAL(b / a, bval / aval);
        BOOST_CHECK_EQUAL(b / aval, Amount(b / a));
    }

    // Modulus
    if (b != Amount(0)) {
        BOOST_CHECK_EQUAL(a % b, aval % bval);
        BOOST_CHECK_EQUAL(a % bval, Amount(a % b));
    }

    if (a != Amount(0)) {
        BOOST_CHECK_EQUAL(b % a, bval % aval);
        BOOST_CHECK_EQUAL(b % aval, Amount(b % a));
    }

    // OpAssign
    Amount v(0);
    v += a;
    BOOST_CHECK_EQUAL(v, a);
    v += b;
    BOOST_CHECK_EQUAL(v, a + b);
    v += b;
    BOOST_CHECK_EQUAL(v, a + 2 * b);
    v -= 2 * a;
    BOOST_CHECK_EQUAL(v, 2 * b - a);
}

BOOST_AUTO_TEST_CASE(AmountTests) {
    std::array<int64_t, 8> values = {{-23, -1, 0, 1, 2, 3, 42, 99999999}};

    for (int64_t i : values) {
        for (int64_t j : values) {
            CheckAmounts(i, j);
        }
    }

    BOOST_CHECK_EQUAL(COIN + COIN, 2 * COIN);
    BOOST_CHECK_EQUAL(2 * COIN + COIN, 3 * COIN);
    BOOST_CHECK_EQUAL(-1 * COIN + COIN, Amount(0));

    BOOST_CHECK_EQUAL(COIN - COIN, Amount(0));
    BOOST_CHECK_EQUAL(COIN - 2 * COIN, -1 * COIN);
}

BOOST_AUTO_TEST_CASE(GetFeeTest) {
    CFeeRate feeRate;

    feeRate = CFeeRate(Amount(0));
    // Must always return 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), Amount(0));
    BOOST_CHECK_EQUAL(feeRate.GetFee(1e5), Amount(0));

    feeRate = CFeeRate(Amount(1000));
    // Must always just return the arg
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), Amount(0));
    BOOST_CHECK_EQUAL(feeRate.GetFee(1), Amount(1));
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), Amount(121));
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), Amount(999));
    BOOST_CHECK_EQUAL(feeRate.GetFee(1000), Amount(1000));
    BOOST_CHECK_EQUAL(feeRate.GetFee(9000), Amount(9000));

    feeRate = CFeeRate(Amount(-1000));
    // Must always just return -1 * arg
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), Amount(0));
    BOOST_CHECK_EQUAL(feeRate.GetFee(1), Amount(-1));
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), Amount(-121));
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), Amount(-999));
    BOOST_CHECK_EQUAL(feeRate.GetFee(1000), Amount(-1000));
    BOOST_CHECK_EQUAL(feeRate.GetFee(9000), Amount(-9000));

    feeRate = CFeeRate(Amount(123));
    // Truncates the result, if not integer
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), Amount(0));
    // Special case: returns 1 instead of 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(8), Amount(1));
    BOOST_CHECK_EQUAL(feeRate.GetFee(9), Amount(1));
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), Amount(14));
    BOOST_CHECK_EQUAL(feeRate.GetFee(122), Amount(15));
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), Amount(122));
    BOOST_CHECK_EQUAL(feeRate.GetFee(1000), Amount(123));
    BOOST_CHECK_EQUAL(feeRate.GetFee(9000), Amount(1107));

    feeRate = CFeeRate(Amount(-123));
    // Truncates the result, if not integer
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), Amount(0));
    // Special case: returns -1 instead of 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(8), Amount(-1));
    BOOST_CHECK_EQUAL(feeRate.GetFee(9), Amount(-1));

    // Check full constructor
    // default value
    BOOST_CHECK(CFeeRate(Amount(-1), 1000) == CFeeRate(Amount(-1)));
    BOOST_CHECK(CFeeRate(Amount(0), 1000) == CFeeRate(Amount(0)));
    BOOST_CHECK(CFeeRate(Amount(1), 1000) == CFeeRate(Amount(1)));
    // lost precision (can only resolve satoshis per kB)
    BOOST_CHECK(CFeeRate(Amount(1), 1001) == CFeeRate(Amount(0)));
    BOOST_CHECK(CFeeRate(Amount(2), 1001) == CFeeRate(Amount(1)));
    // some more integer checks
    BOOST_CHECK(CFeeRate(Amount(26), 789) == CFeeRate(Amount(32)));
    BOOST_CHECK(CFeeRate(Amount(27), 789) == CFeeRate(Amount(34)));
    // Maximum size in bytes, should not crash
    CFeeRate(MAX_MONEY, std::numeric_limits<size_t>::max() >> 1).GetFeePerK();
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * @Description: LIO key frame
 * @Author: Ren Qian
 * @Date: 2020-02-28 19:13:26
 */
#ifndef LIDAR_LOCALIZATION_SENSOR_DATA_KEY_FRAME_HPP_
#define LIDAR_LOCALIZATION_SENSOR_DATA_KEY_FRAME_HPP_

#include <Eigen/Eigen>
#include <Eigen/Core>
#include <Eigen/Dense>

#include <sophus/so3.hpp>

#include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_prvag.hpp"

namespace lidar_localization {

struct KeyFrame {
public:
    static const int INDEX_P = 0;
    static const int INDEX_R = 3;
    static const int INDEX_V = 6;
    static const int INDEX_A = 9;
    static const int INDEX_G = 12;

    double time = 0.0;

    // key frame ID:
    unsigned int index = 0;

    // a. position & orientation:
    Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
    // b. velocity:
    struct {
      Eigen::Vector3f v = Eigen::Vector3f::Zero();
      Eigen::Vector3f w = Eigen::Vector3f::Zero();
    } vel;
    // c. bias:
    struct {
      // c.1. accelerometer:
      Eigen::Vector3f accel = Eigen::Vector3f::Zero();
      // c.2. gyroscope:
      Eigen::Vector3f gyro = Eigen::Vector3f::Zero();
    } bias;

    KeyFrame() {}

    explicit KeyFrame(const int vertex_id, const g2o::PRVAG &prvag) {
      // set time:
      time = prvag.time;
      // set seq. ID:
      index = vertex_id;
      // set state:
      pose.block<3, 1>(0, 3) = prvag.pos.cast<float>();
      pose.block<3, 3>(0, 0) = prvag.ori.matrix().cast<float>();
      vel.v = prvag.vel.cast<float>();
      bias.accel = prvag.b_a.cast<float>();
      bias.gyro = prvag.b_g.cast<float>();
    }

    explicit KeyFrame(const int param_index, const double &T, const double *prvag) {
      // set time:
      time = T;
      // set seq. ID:
      index = param_index;
      // set state:
      Eigen::Map<const Eigen::Vector3d>     pos(prvag + INDEX_P);
      Eigen::Map<const Eigen::Vector3d> log_ori(prvag + INDEX_R);
      Eigen::Map<const Eigen::Vector3d>       v(prvag + INDEX_V);
      Eigen::Map<const Eigen::Vector3d>     b_a(prvag + INDEX_A);
      Eigen::Map<const Eigen::Vector3d>     b_g(prvag + INDEX_G);

      pose.block<3, 1>(0, 3) = pos.cast<float>();
      pose.block<3, 3>(0, 0) = Sophus::SO3d::exp(log_ori).matrix().cast<float>();

      vel.v = v.cast<float>();

      bias.accel = b_a.cast<float>();
      bias.gyro = b_g.cast<float>();
    }

    Eigen::Quaternionf GetQuaternion() const;
    Eigen::Vector3f GetTranslation() const;
};

}

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_EXPONENT_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_EXPONENT_HPP_INCLUDED
#include <boost/simd/ieee/functions/exponent.hpp>
#include <boost/dispatch/meta/adapted_traits.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/functions/scalar/is_invalid.hpp>
#include <boost/simd/include/functions/scalar/shri.hpp>
#include <boost/simd/include/functions/scalar/exponentbits.hpp>
#include <boost/simd/include/functions/scalar/is_nez.hpp>
#include <boost/simd/include/functions/scalar/is_eqz.hpp>
#include <boost/simd/include/functions/scalar/if_else_zero.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/simd/sdk/math.hpp>

namespace boost { namespace simd { namespace ext
{
#ifdef BOOST_SIMD_HAS_ILOGB
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::exponent_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      if (is_invalid(a0) || is_eqz(a0)) return Zero<result_type>();
      return ::ilogb(a0);
    }
  };
#endif

#ifdef BOOST_SIMD_HAS_ILOGBF
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::exponent_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      if (is_invalid(a0) || is_eqz(a0)) return Zero<result_type>();
      return ::ilogbf(a0);
    }
  };
#endif

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::exponent_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      if (is_invalid(a0) || is_eqz(a0)) return Zero<result_type>();
      const int nmb = int(Nbmantissabits<A0>());
      const result_type x = shri(exponentbits(a0), nmb);
      return x-if_else_zero(is_nez(a0), Maxexponent<A0>());
    }
  };
} } }

#endif


#include <functional>
#include <iostream>
#include <memory>

#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN
#define DOCTEST_CONFIG_SUPER_FAST_ASSERTS
#include "doctest/doctest.h"

#include <tvm/Variable.h>
#include <tvm/VariableVector.h>
#include <tvm/constraint/BasicLinearConstraint.h>
#include <tvm/hint/internal/Substitutions.h>
#include <tvm/hint/Substitution.h>
#include <tvm/scheme/internal/Assignment.h>
#include <tvm/scheme/internal/AssignmentTarget.h>

#include <Eigen/Core>
#include <Eigen/QR>

//FIXME see src/Assignment.cpp
static const double large = tvm::constant::big_number;

using namespace tvm;
using namespace tvm::constraint;
using namespace tvm::hint;
using namespace tvm::hint::internal;
using namespace tvm::scheme::internal;
using namespace tvm::requirements;
using namespace Eigen;

using BLCPtr = std::shared_ptr<BasicLinearConstraint>;

struct Constraints
{
  VectorXd p0;
  VectorXd pl;
  VectorXd pu;

  BLCPtr Ax_eq_0;
  BLCPtr Ax_geq_0;
  BLCPtr Ax_leq_0;
  BLCPtr Ax_eq_b;
  BLCPtr Ax_geq_b;
  BLCPtr Ax_leq_b;
  BLCPtr Ax_eq_minus_b;
  BLCPtr Ax_geq_minus_b;
  BLCPtr Ax_leq_minus_b;
  BLCPtr l_leq_Ax_leq_u;
  BLCPtr minus_l_leq_Ax_leq_minus_u;
};

struct Memory
{
  Memory(int m, int n) : A(m, n), b(m), l(m), u(m)
  {
    reset();
  }

  void reset()
  {
    A.setZero();
    b.setZero();
    l.setZero();
    u.setZero();
  }

  void randomize()
  {
    A.setRandom();
    b.setRandom();
    l.setRandom();
    u.setRandom();
  }

  MatrixXd A;
  VectorXd b;
  VectorXd l;
  VectorXd u;
};

//Check if the constraint is satisfied for the current value of the variable
bool check(BLCPtr c, const VectorXd& x)
{
  const double eps = 1e-12;
  const_cast<VariableVector&>(c->variables()).value(x);
  c->updateValue();
  auto v = c->value();
  if (c->type() == Type::DOUBLE_SIDED)
  {
    if (c->rhs() == RHS::AS_GIVEN)
      return (c->l().array()-eps <= v.array()).all() && (v.array() <= c->u().array()+eps).all();
    else
      return (-c->l().array()-eps <= v.array()).all() && (v.array() <= -c->u().array()+eps).all();
  }
  else
  {
    std::function<bool(const VectorXd&, const VectorXd&)> comp;
    const VectorXd&(BasicLinearConstraint::*rhs)() const;
    switch (c->type())
    {
    case Type::EQUAL: comp = [](const VectorXd& u, const VectorXd& v) {return u.isApprox(v); }; rhs =& BasicLinearConstraint::e;  break;
    case Type::GREATER_THAN: comp = [eps](const VectorXd& u, const VectorXd& v) {return (u.array() + eps >= v.array()).all(); }; rhs = &BasicLinearConstraint::l; break;
    case Type::LOWER_THAN: comp = [eps](const VectorXd& u, const VectorXd& v) {return (u.array() - eps <= v.array()).all(); }; rhs = &BasicLinearConstraint::u; break;
    default: break;
    }
    switch (c->rhs())
    {
    case RHS::AS_GIVEN: return comp(v, (c.get()->*rhs)()); break;
    case RHS::OPPOSITE: return comp(v, -(c.get()->*rhs)()); break;
    case RHS::ZERO: return comp(v, VectorXd::Zero(c->size())); break;
    default:
      return false;
    }
  }
}

bool check(const Memory& mem, Type ct, RHS cr, const VectorXd& x, bool bound=false)
{
  const double eps = 1e-12;
  VectorXd v;
  if (bound)
    v = x;
  else
    v = mem.A*x;

  if (ct == Type::DOUBLE_SIDED)
  {
    if (cr == RHS::AS_GIVEN)
      return (mem.l.array() - eps <= v.array()).all() && (v.array() <= mem.u.array() + eps).all();
    else
      return (-mem.l.array() - eps <= v.array()).all() && (v.array() <= -mem.u.array() + eps).all();
  }
  else
  {
    std::function<bool(const VectorXd&, const VectorXd&)> comp;
    switch (ct)
    {
    case Type::EQUAL: comp = [](const VectorXd& u, const VectorXd& v) {return u.isApprox(v); };  break;
    case Type::GREATER_THAN: comp = [eps](const VectorXd& u, const VectorXd& v) {return (u.array() + eps >= v.array()).all(); }; break;
    case Type::LOWER_THAN: comp = [eps](const VectorXd& u, const VectorXd& v) {return (u.array() - eps <= v.array()).all(); }; break;
    default: break;
    }
    switch (cr)
    {
    case RHS::AS_GIVEN: return comp(v, mem.b); break;
    case RHS::OPPOSITE: return comp(v, -mem.b); break;
    case RHS::ZERO: return comp(v, VectorXd::Zero(mem.b.rows())); break;
    default:
      return false;
    }
  }
}

Constraints buildConstraints(int m, int n)
{
  Constraints cstr;
  VariablePtr x = Space(n).createVariable("x");

  //generate matrix
  MatrixXd A = MatrixXd::Random(m, n);
  VectorXd l = -VectorXd::Random(m).cwiseAbs();
  VectorXd u = VectorXd::Random(m).cwiseAbs();

  //Point p0 such that Ap0 = 0
  cstr.p0 = A.householderQr().solve(VectorXd::Zero(m));
  //Point pl such that Apl = l
  cstr.pl = A.householderQr().solve(l);
  //Point pu such that Apu = u
  cstr.pu = A.householderQr().solve(u);

  cstr.Ax_eq_0 = std::make_shared<BasicLinearConstraint>(A, x, Type::EQUAL);
  cstr.Ax_geq_0 = std::make_shared<BasicLinearConstraint>(A, x, Type::GREATER_THAN);
  cstr.Ax_leq_0 = std::make_shared<BasicLinearConstraint>(A, x, Type::LOWER_THAN);

  cstr.Ax_eq_b = std::make_shared<BasicLinearConstraint>(A, x, l, Type::EQUAL);
  cstr.Ax_geq_b = std::make_shared<BasicLinearConstraint>(A, x, l, Type::GREATER_THAN);
  cstr.Ax_leq_b = std::make_shared<BasicLinearConstraint>(A, x, u, Type::LOWER_THAN);

  cstr.Ax_eq_minus_b = std::make_shared<BasicLinearConstraint>(A, x, -l, Type::EQUAL, RHS::OPPOSITE);
  cstr.Ax_geq_minus_b = std::make_shared<BasicLinearConstraint>(A, x, -l, Type::GREATER_THAN, RHS::OPPOSITE);
  cstr.Ax_leq_minus_b = std::make_shared<BasicLinearConstraint>(A, x, -u, Type::LOWER_THAN, RHS::OPPOSITE);

  cstr.l_leq_Ax_leq_u = std::make_shared<BasicLinearConstraint>(A, x, l, u);
  cstr.minus_l_leq_Ax_leq_minus_u = std::make_shared<BasicLinearConstraint>(A, x, -l, -u, RHS::OPPOSITE);
  return cstr;
}

//build constraints x op 0, x op +/-2 such that 0 is feasible, and -2 <= x <= 2
//if minus = true, we take -x instead of x
Constraints buildSimpleConstraints(double s = 1, VariablePtr y = nullptr)
{
  assert(s != 0);
  Constraints cstr;
  VariablePtr x;
  if (y)
    x = y;
  else
    x = Space(1).createVariable("x");

  tvm::internal::MatrixProperties p;
  if (s == -1)
    p = { tvm::internal::MatrixProperties::MINUS_IDENTITY };
  else if (s == 1)
    p = { tvm::internal::MatrixProperties::IDENTITY };
  else
    p = { tvm::internal::MatrixProperties::MULTIPLE_OF_IDENTITY,
          tvm::internal::MatrixProperties::Invertibility(s != 0) };

  //generate matrix
  MatrixXd A = s*MatrixXd::Identity(1, 1);
  VectorXd l = VectorXd::Constant(1, -2);
  VectorXd u = VectorXd::Constant(1, 2);

  cstr.Ax_eq_0 = std::make_shared<BasicLinearConstraint>(A, x, Type::EQUAL);
  cstr.Ax_eq_0->A(A, p);
  cstr.Ax_geq_0 = std::make_shared<BasicLinearConstraint>(A, x, Type::GREATER_THAN);
  cstr.Ax_geq_0->A(A, p);
  cstr.Ax_leq_0 = std::make_shared<BasicLinearConstraint>(A, x, Type::LOWER_THAN);
  cstr.Ax_leq_0->A(A, p);

  cstr.Ax_eq_b = std::make_shared<BasicLinearConstraint>(A, x, l, Type::EQUAL);
  cstr.Ax_eq_b->A(A, p);
  cstr.Ax_geq_b = std::make_shared<BasicLinearConstraint>(A, x, l, Type::GREATER_THAN);
  cstr.Ax_geq_b->A(A, p);
  cstr.Ax_leq_b = std::make_shared<BasicLinearConstraint>(A, x, u, Type::LOWER_THAN);
  cstr.Ax_leq_b->A(A, p);

  cstr.Ax_eq_minus_b = std::make_shared<BasicLinearConstraint>(A, x, -l, Type::EQUAL, RHS::OPPOSITE);
  cstr.Ax_eq_minus_b->A(A, p);
  cstr.Ax_geq_minus_b = std::make_shared<BasicLinearConstraint>(A, x, -l, Type::GREATER_THAN, RHS::OPPOSITE);
  cstr.Ax_geq_minus_b->A(A, p);
  cstr.Ax_leq_minus_b = std::make_shared<BasicLinearConstraint>(A, x, -u, Type::LOWER_THAN, RHS::OPPOSITE);
  cstr.Ax_leq_minus_b->A(A, p);

  cstr.l_leq_Ax_leq_u = std::make_shared<BasicLinearConstraint>(A, x, l, u);
  cstr.l_leq_Ax_leq_u->A(A, p);
  cstr.minus_l_leq_Ax_leq_minus_u = std::make_shared<BasicLinearConstraint>(A, x, -l, -u, RHS::OPPOSITE);
  cstr.minus_l_leq_Ax_leq_minus_u->A(A, p);
  return cstr;
}

//create a set of constraints -3x+4y op +/-2 and a constraint for substitution
// -x + 2y - z = 0/1/-1 (the latter value depending on the rhs param).
std::pair<Constraints, BLCPtr> buildSimpleSubstitution(RHS rhs)
{
  Constraints cstr;
  VariablePtr x = Space(1).createVariable("x");
  VariablePtr y = Space(1).createVariable("y");
  VariablePtr z = Space(1).createVariable("z");

  //generate matrix
  MatrixXd I = MatrixXd::Identity(1, 1);
  MatrixXd Ax = -3 * I;
  MatrixXd Ay = 4 * I;
  VectorXd l = VectorXd::Constant(1, -2);
  VectorXd u = VectorXd::Constant(1, 2);

  std::vector<Ref<const MatrixXd>> A = { Ax, Ay };
  std::vector<VariablePtr> v = { x, y };

  cstr.Ax_eq_0 = std::make_shared<BasicLinearConstraint>( A, v, Type::EQUAL);
  cstr.Ax_geq_0 = std::make_shared<BasicLinearConstraint>(A, v, Type::GREATER_THAN);
  cstr.Ax_leq_0 = std::make_shared<BasicLinearConstraint>(A, v, Type::LOWER_THAN);

  cstr.Ax_eq_b = std::make_shared<BasicLinearConstraint>( A, v, l, Type::EQUAL);
  cstr.Ax_geq_b = std::make_shared<BasicLinearConstraint>(A, v, l, Type::GREATER_THAN);
  cstr.Ax_leq_b = std::make_shared<BasicLinearConstraint>(A, v, u, Type::LOWER_THAN);

  cstr.Ax_eq_minus_b = std::make_shared<BasicLinearConstraint>( A, v, -l, Type::EQUAL, RHS::OPPOSITE);
  cstr.Ax_geq_minus_b = std::make_shared<BasicLinearConstraint>(A, v, -l, Type::GREATER_THAN, RHS::OPPOSITE);
  cstr.Ax_leq_minus_b = std::make_shared<BasicLinearConstraint>(A, v, -u, Type::LOWER_THAN, RHS::OPPOSITE);

  cstr.l_leq_Ax_leq_u = std::make_shared<BasicLinearConstraint>(A, v, l, u);
  cstr.minus_l_leq_Ax_leq_minus_u = std::make_shared<BasicLinearConstraint>(A, v, -l, -u, RHS::OPPOSITE);

  MatrixXd Cx = -I;
  MatrixXd Cy = 2 * I;
  MatrixXd Cz = -I;
  std::vector<Ref<const MatrixXd>> C = { Cx, Cy, Cz };
  std::vector<VariablePtr> w = { x, y, z };
  VectorXd d = VectorXd::Constant(1, 1);
  if (rhs == RHS::ZERO)
  {
    return std::make_pair(cstr, std::make_shared<BasicLinearConstraint>(C, w, Type::EQUAL));
  }
  else
  {
    return std::make_pair(cstr, std::make_shared<BasicLinearConstraint>(C, w, d, Type::EQUAL, rhs));
  }
}

//check that each point -3, -2, -1, 0, 1, 2 and 3 is either verifying at the same time
//the two descriptions of the constraint or violating both of them.
void checkSimple(BLCPtr cstr, const Memory& mem, Type t, RHS r, bool bound = false)
{
  VectorXd pm3 = VectorXd::Constant(1, -3);
  VectorXd pm2 = VectorXd::Constant(1, -2);
  VectorXd pm1 = VectorXd::Constant(1, -1);
  VectorXd p0 = VectorXd::Constant(1, 0);
  VectorXd p1 = VectorXd::Constant(1, 1);
  VectorXd p2 = VectorXd::Constant(1, 2);
  VectorXd p3 = VectorXd::Constant(1, 3);

  FAST_CHECK_EQ(check(cstr, pm3), check(mem, t, r, pm3, bound));
  FAST_CHECK_EQ(check(cstr, pm2), check(mem, t, r, pm2, bound));
  FAST_CHECK_EQ(check(cstr, pm1), check(mem, t, r, pm1, bound));
  FAST_CHECK_EQ(check(cstr, p0), check(mem, t, r, p0, bound));
  FAST_CHECK_EQ(check(cstr, p1), check(mem, t, r, p1, bound));
  FAST_CHECK_EQ(check(cstr, p2), check(mem, t, r, p2, bound));
  FAST_CHECK_EQ(check(cstr, p3), check(mem, t, r, p3, bound));
}

//check for the intersection of 2 bound constraints
void checkSimple(BLCPtr cstr1, BLCPtr cstr2, const Memory& mem)
{
  Type t = Type::DOUBLE_SIDED;
  RHS r = RHS::AS_GIVEN;

  VectorXd pm3 = VectorXd::Constant(1, -3);
  VectorXd pm2 = VectorXd::Constant(1, -2);
  VectorXd pm1 = VectorXd::Constant(1, -1);
  VectorXd p0 = VectorXd::Constant(1, 0);
  VectorXd p1 = VectorXd::Constant(1, 1);
  VectorXd p2 = VectorXd::Constant(1, 2);
  VectorXd p3 = VectorXd::Constant(1, 3);

  FAST_CHECK_EQ(check(cstr1, pm3) && check(cstr2, pm3), check(mem, t, r, pm3, true));
  FAST_CHECK_EQ(check(cstr1, pm2) && check(cstr2, pm2), check(mem, t, r, pm2, true));
  FAST_CHECK_EQ(check(cstr1, pm1) && check(cstr2, pm1), check(mem, t, r, pm1, true));
  FAST_CHECK_EQ(check(cstr1, p0) && check(cstr2, p0), check(mem, t, r, p0, true));
  FAST_CHECK_EQ(check(cstr1, p1) && check(cstr2, p1), check(mem, t, r, p1, true));
  FAST_CHECK_EQ(check(cstr1, p2) && check(cstr2, p2), check(mem, t, r, p2, true));
  FAST_CHECK_EQ(check(cstr1, p3) && check(cstr2, p3), check(mem, t, r, p3, true));
}

// Check that each point (x,z) with z=-x, for x = -5,-3,1,0,1,3,5 verifies or violates the
// substituted constraint in mem at the same time as the corresponding (x, y) point verifies
// or violates the constraint.
void checkSubstitution(BLCPtr cstr, RHS subRhs, const Memory& mem, Type t, RHS r)
{
  double y;
  switch (subRhs)
  {
  case RHS::ZERO: y = 0; break;
  case RHS::AS_GIVEN: y = 0.5; break;
  case RHS::OPPOSITE: y = -0.5; break;
  }
  VectorXd xym5(2); xym5 << -2, y;
  VectorXd xym3(2); xym3 << -4./3, y;
  VectorXd xym1(2); xym1 << -2./3, y;
  VectorXd xy0(2); xy0 << 0, y;
  VectorXd xy1(2); xy1 << 2./3, y;
  VectorXd xy3(2); xy3 << 4./3, y;
  VectorXd xy5(2); xy5 << 2, y;
  VectorXd xzm5(2); xzm5 << -2, 2;
  VectorXd xzm3(2); xzm3 << -4./3, 4./3;
  VectorXd xzm1(2); xzm1 << -2./3, 2./3;
  VectorXd xz0(2); xz0 << 0, 0;
  VectorXd xz1(2); xz1 << 2./3, -2./3;
  VectorXd xz3(2); xz3 << 4./3, -4./3;
  VectorXd xz5(2); xz5 << 2, -2;

  //std::cout << check(cstr, xym5) <<", " << check(mem, t, r, xzm5, false) << std::endl;
  //std::cout << check(cstr, xym3) <<", " << check(mem, t, r, xzm3, false) << std::endl;
  //std::cout << check(cstr, xym1) <<", " << check(mem, t, r, xzm1, false) << std::endl;
  //std::cout << check(cstr, xy0)  <<", " << check(mem, t, r, xz0, false)  << std::endl;
  //std::cout << check(cstr, xy1)  <<", " << check(mem, t, r, xz1, false)  << std::endl;
  //std::cout << check(cstr, xy3)  <<", " << check(mem, t, r, xz3, false)  << std::endl;
  //std::cout << check(cstr, xy5)  <<", " << check(mem, t, r, xz5, false)  << std::endl;

  FAST_CHECK_EQ(check(cstr, xym5), check(mem, t, r, xzm5, false));
  FAST_CHECK_EQ(check(cstr, xym3), check(mem, t, r, xzm3, false));
  FAST_CHECK_EQ(check(cstr, xym1), check(mem, t, r, xzm1, false));
  FAST_CHECK_EQ(check(cstr, xy0), check(mem, t, r, xz0, false));
  FAST_CHECK_EQ(check(cstr, xy1), check(mem, t, r, xz1, false));
  FAST_CHECK_EQ(check(cstr, xy3), check(mem, t, r, xz3, false));
  FAST_CHECK_EQ(check(cstr, xy5), check(mem, t, r, xz5, false));
}

void checkAssignment(BLCPtr c, const AssignmentTarget& at, Memory& mem, Type t, RHS r, bool throws)
{
  auto req = std::make_shared<SolvingRequirements>();
  VariableVector vars(c->variables());
  mem.randomize();
  if (throws)
  {
    CHECK_THROWS(Assignment a(c, req, at, vars));
  }
  else
  {
    Assignment a(c, req, at, vars);
    a.run();
    checkSimple(c, mem, t, r);
  }
}

void checkSubstitutionAssignment(BLCPtr c, Substitutions& s, const AssignmentTarget& at, Memory& mem, Type t, RHS r, bool throws)
{
  auto req = std::make_shared<SolvingRequirements>();
  VariableVector vars;
  vars.add(s.otherVariables());
  mem.randomize();
  if (throws)
  {
    CHECK_THROWS(Assignment a(c, req, at, vars, &s));
  }
  else
  {
    s.updateSubstitutions();
    Assignment a(c, req, at, vars, &s);
    a.run();
    checkSubstitution(c, s.substitutions()[0].constraints()[0]->rhs(), mem, t, r);
  }
}

void checkBoundAssignment(BLCPtr c, const AssignmentTarget& at, Memory& mem)
{
  Assignment a(c, at, c->variables()[0], true);
  a.run();
  checkSimple(c, mem, Type::DOUBLE_SIDED, RHS::AS_GIVEN, true);
}

void checkBoundAssignment(BLCPtr c1, BLCPtr c2, const AssignmentTarget& at, Memory& mem)
{
  assert(c1->variables()[0] == c2->variables()[0]);
  Assignment a1(c1, at, c1->variables()[0], true);
  a1.run();
  Assignment a2(c2, at, c2->variables()[0], false);
  a2.run();
  checkSimple(c1, c2, mem);
}

/** Check the assignement of a constraint \p c to a target
  * \p throws is a vector of 11 bool indicating if the assignement construction is
  * expected to throw. The order of the targets conventions are:
  * Cx=0, Cx=d, Cx=-d, Cx>=0, Cx>=d, Cx>=-d, Cx<=0, Cx<=d, Cx<=-d, l<=Cx<=u, -l<=Cx<=-u
  */
void checkSimple(BLCPtr c, std::vector<bool> throws)
{
  int s = c->type() == Type::DOUBLE_SIDED ? 2 : 1;
  auto sMem = std::make_shared<Memory>(s, 1);
  auto sRange = std::make_shared<Range>(0, s);
  auto dMem = std::make_shared<Memory>(1, 1);
  auto dRange = std::make_shared<Range>(0, 1);

  // target Cx=0
  {
    auto t = Type::EQUAL;
    auto r = RHS::ZERO;
    AssignmentTarget at(sRange, sMem->A, t);
    checkAssignment(c, at, *sMem, t, r, throws[0]);
  }
  // target Cx=d
  {
    auto t = Type::EQUAL;
    auto r = RHS::AS_GIVEN;
    AssignmentTarget at(sRange, sMem->A, sMem->b, t, r);
    checkAssignment(c, at, *sMem, t, r, throws[1]);
  }
  // target Cx=-d
  {
    auto t = Type::EQUAL;
    auto r = RHS::OPPOSITE;
    sMem->randomize();
    AssignmentTarget at(sRange, sMem->A, sMem->b, t, r);
    checkAssignment(c, at, *sMem, t, r, throws[2]);
  }
  // target Cx>=0
  {
    auto t = Type::GREATER_THAN;
    auto r = RHS::ZERO;
    AssignmentTarget at(sRange, sMem->A, t);
    checkAssignment(c, at, *sMem, t, r, throws[3]);
  }
  // target Cx>=d
  {
    auto t = Type::GREATER_THAN;
    auto r = RHS::AS_GIVEN;
    AssignmentTarget at(sRange, sMem->A, sMem->b, t, r);
    checkAssignment(c, at, *sMem, t, r, throws[4]);
  }
  // target Cx>=-d
  {
    auto t = Type::GREATER_THAN;
    auto r = RHS::OPPOSITE;
    sMem->randomize();
    AssignmentTarget at(sRange, sMem->A, sMem->b, t, r);
    checkAssignment(c, at, *sMem, t, r, throws[5]);
  }
  // target Cx<=0
  {
    auto t = Type::LOWER_THAN;
    auto r = RHS::ZERO;
    AssignmentTarget at(sRange, sMem->A, t);
    checkAssignment(c, at, *sMem, t, r, throws[6]);
  }
  // target Cx<=d
  {
    auto t = Type::LOWER_THAN;
    auto r = RHS::AS_GIVEN;
    AssignmentTarget at(sRange, sMem->A, sMem->b, t, r);
    checkAssignment(c, at, *sMem, t, r, throws[7]);
  }
  // target Cx<=-d
  {
    auto t = Type::LOWER_THAN;
    auto r = RHS::OPPOSITE;
    sMem->randomize();
    AssignmentTarget at(sRange, sMem->A, sMem->b, t, r);
    checkAssignment(c, at, *sMem, t, r, throws[8]);
  }
  // target l<=Cx<=u
  {
    auto t = Type::DOUBLE_SIDED;
    auto r = RHS::AS_GIVEN;
    AssignmentTarget at(dRange, dMem->A, dMem->l, dMem->u, r);
    checkAssignment(c, at, *dMem, t, r, throws[9]);
  }
  // target -l<=Cx<=-u
  {
    auto t = Type::DOUBLE_SIDED;
    auto r = RHS::AS_GIVEN;
    AssignmentTarget at(dRange, dMem->A, dMem->l, dMem->u, r);
    checkAssignment(c, at, *dMem, t, r, throws[10]);
  }
}

//same as above but introducing a substitution
void checkSimple(BLCPtr c, const Substitution& sub, std::vector<bool> throws)
{
  int s = c->type() == Type::DOUBLE_SIDED ? 2 : 1;
  auto sMem = std::make_shared<Memory>(s, 2);
  auto sRange = std::make_shared<Range>(0, s);
  auto dMem = std::make_shared<Memory>(1, 2);
  auto dRange = std::make_shared<Range>(0, 1);

  Substitutions subs;
  subs.add(sub);
  subs.finalize();

  // target Cx=0
  {
    auto t = Type::EQUAL;
    auto r = RHS::ZERO;
    AssignmentTarget at(sRange, sMem->A, t);
    checkSubstitutionAssignment(c, subs, at, *sMem, t, r, throws[0]);
  }
  // target Cx=d
  {
    auto t = Type::EQUAL;
    auto r = RHS::AS_GIVEN;
    AssignmentTarget at(sRange, sMem->A, sMem->b, t, r);
    checkSubstitutionAssignment(c, subs, at, *sMem, t, r, throws[1]);
  }
  // target Cx=-d
  {
    auto t = Type::EQUAL;
    auto r = RHS::OPPOSITE;
    sMem->randomize();
    AssignmentTarget at(sRange, sMem->A, sMem->b, t, r);
    checkSubstitutionAssignment(c, subs, at, *sMem, t, r, throws[2]);
  }
  // target Cx>=0
  {
    auto t = Type::GREATER_THAN;
    auto r = RHS::ZERO;
    AssignmentTarget at(sRange, sMem->A, t);
    checkSubstitutionAssignment(c, subs, at, *sMem, t, r, throws[3]);
  }
  // target Cx>=d
  {
    auto t = Type::GREATER_THAN;
    auto r = RHS::AS_GIVEN;
    AssignmentTarget at(sRange, sMem->A, sMem->b, t, r);
    checkSubstitutionAssignment(c, subs, at, *sMem, t, r, throws[4]);
  }
  // target Cx>=-d
  {
    auto t = Type::GREATER_THAN;
    auto r = RHS::OPPOSITE;
    sMem->randomize();
    AssignmentTarget at(sRange, sMem->A, sMem->b, t, r);
    checkSubstitutionAssignment(c, subs, at, *sMem, t, r, throws[5]);
  }
  // target Cx<=0
  {
    auto t = Type::LOWER_THAN;
    auto r = RHS::ZERO;
    AssignmentTarget at(sRange, sMem->A, t);
    checkSubstitutionAssignment(c, subs, at, *sMem, t, r, throws[6]);
  }
  // target Cx<=d
  {
    auto t = Type::LOWER_THAN;
    auto r = RHS::AS_GIVEN;
    AssignmentTarget at(sRange, sMem->A, sMem->b, t, r);
    checkSubstitutionAssignment(c, subs, at, *sMem, t, r, throws[7]);
  }
  // target Cx<=-d
  {
    auto t = Type::LOWER_THAN;
    auto r = RHS::OPPOSITE;
    sMem->randomize();
    AssignmentTarget at(sRange, sMem->A, sMem->b, t, r);
    checkSubstitutionAssignment(c, subs, at, *sMem, t, r, throws[8]);
  }
  // target l<=Cx<=u
  {
    auto t = Type::DOUBLE_SIDED;
    auto r = RHS::AS_GIVEN;
    AssignmentTarget at(dRange, dMem->A, dMem->l, dMem->u, r);
    checkSubstitutionAssignment(c, subs, at, *dMem, t, r, throws[9]);
  }
  // target -l<=Cx<=-u
  {
    auto t = Type::DOUBLE_SIDED;
    auto r = RHS::AS_GIVEN;
    AssignmentTarget at(dRange, dMem->A, dMem->l, dMem->u, r);
    checkSubstitutionAssignment(c, subs, at, *dMem, t, r, throws[10]);
  }
}

/** Check the assignement of a bound \p c to a target */
void checkSimpleBound(BLCPtr c)
{
  auto dMem = std::make_shared<Memory>(1, 1);
  auto dRange = std::make_shared<Range>(0, 1);

  // target l<=x<=u
  AssignmentTarget at(dRange, dMem->l, dMem->u);
  checkBoundAssignment(c, at, *dMem);
}

void checkSimpleBound(BLCPtr c1, BLCPtr c2)
{
  auto dMem = std::make_shared<Memory>(1, 1);
  auto dRange = std::make_shared<Range>(0, 1);

  // target l<=x<=u
  AssignmentTarget at(dRange, dMem->l, dMem->u);
  checkBoundAssignment(c1, c2, at, *dMem);
}

// test for correct signs on the matrices and vector
TEST_CASE("Test simple assignment")
{
  Constraints cstr = buildSimpleConstraints();
  using T = std::vector<bool>;
  const bool t = true;
  const bool f = false;
  // constraint Ax = 0
  {
    //     Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v = { f,      f,      f,      t,      t,      t,      t,      t,      t,      f,      f };
    checkSimple(cstr.Ax_eq_0, v);
  }
  // constraint Ax >= 0
  {
    //     Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v = { t,      t,      t,      f,      f,      f,      f,      f,      f,      f,      f };
    checkSimple(cstr.Ax_geq_0, v);
  }
  // constraint Ax <= 0
  {
    //     Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v = { t,      t,      t,      f,      f,      f,      f,      f,      f,      f,      f };
    checkSimple(cstr.Ax_leq_0, v);
  }
  // constraint Ax = d
  {
    //     Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v = { t,      f,      f,      t,      t,      t,      t,      t,      t,      f,      f };
    checkSimple(cstr.Ax_eq_b, v);
  }
  // constraint Ax >= d
  {
    //     Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    checkSimple(cstr.Ax_geq_b, v);
  }
  // constraint Ax <= d
  {
    //     Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    checkSimple(cstr.Ax_leq_b, v);
  }
  // constraint Ax = -d
  {
    //     Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v = { t,      f,      f,      t,      t,      t,      t,      t,      t,      f,      f };
    checkSimple(cstr.Ax_eq_minus_b, v);
  }
  // constraint Ax >= -d
  {
    //     Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    checkSimple(cstr.Ax_geq_minus_b, v);
  }
  // constraint Ax <= -d
  {
    //     Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    checkSimple(cstr.Ax_leq_minus_b, v);
  }
  // constraint l <= Ax <= u
  {
    //     Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    checkSimple(cstr.l_leq_Ax_leq_u, v);
  }
  // constraint -l <= Ax <= -u
  {
    //     Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    checkSimple(cstr.l_leq_Ax_leq_u, v);
  }

  //now the bounds
  checkSimpleBound(cstr.Ax_eq_0);
  checkSimpleBound(cstr.Ax_geq_0);
  checkSimpleBound(cstr.Ax_leq_0);
  checkSimpleBound(cstr.Ax_eq_b);
  checkSimpleBound(cstr.Ax_geq_b);
  checkSimpleBound(cstr.Ax_leq_b);
  checkSimpleBound(cstr.Ax_eq_minus_b);
  checkSimpleBound(cstr.Ax_geq_minus_b);
  checkSimpleBound(cstr.Ax_leq_minus_b);
  checkSimpleBound(cstr.l_leq_Ax_leq_u);
  checkSimpleBound(cstr.minus_l_leq_Ax_leq_minus_u);

  //with -Identity
  auto cstr2 = buildSimpleConstraints(-1, cstr.Ax_eq_0->variables()[0]);
  checkSimpleBound(cstr2.Ax_eq_0);
  checkSimpleBound(cstr2.Ax_geq_0);
  checkSimpleBound(cstr2.Ax_leq_0);
  checkSimpleBound(cstr2.Ax_eq_b);
  checkSimpleBound(cstr2.Ax_geq_b);
  checkSimpleBound(cstr2.Ax_leq_b);
  checkSimpleBound(cstr2.Ax_eq_minus_b);
  checkSimpleBound(cstr2.Ax_geq_minus_b);
  checkSimpleBound(cstr2.Ax_leq_minus_b);
  checkSimpleBound(cstr2.l_leq_Ax_leq_u);
  checkSimpleBound(cstr2.minus_l_leq_Ax_leq_minus_u);

  // with diagonal
  auto cstr3 = buildSimpleConstraints(2, cstr.Ax_eq_0->variables()[0]);
  checkSimpleBound(cstr3.Ax_eq_0);
  checkSimpleBound(cstr3.Ax_geq_0);
  checkSimpleBound(cstr3.Ax_leq_0);
  checkSimpleBound(cstr3.Ax_eq_b);
  checkSimpleBound(cstr3.Ax_geq_b);
  checkSimpleBound(cstr3.Ax_leq_b);
  checkSimpleBound(cstr3.Ax_eq_minus_b);
  checkSimpleBound(cstr3.Ax_geq_minus_b);
  checkSimpleBound(cstr3.Ax_leq_minus_b);
  checkSimpleBound(cstr3.l_leq_Ax_leq_u);
  checkSimpleBound(cstr3.minus_l_leq_Ax_leq_minus_u);

  // with diagonal (negative)
  auto cstr4 = buildSimpleConstraints(-2, cstr.Ax_eq_0->variables()[0]);
  checkSimpleBound(cstr4.Ax_eq_0);
  checkSimpleBound(cstr4.Ax_geq_0);
  checkSimpleBound(cstr4.Ax_leq_0);
  checkSimpleBound(cstr4.Ax_eq_b);
  checkSimpleBound(cstr4.Ax_geq_b);
  checkSimpleBound(cstr4.Ax_leq_b);
  checkSimpleBound(cstr4.Ax_eq_minus_b);
  checkSimpleBound(cstr4.Ax_geq_minus_b);
  checkSimpleBound(cstr4.Ax_leq_minus_b);
  checkSimpleBound(cstr4.l_leq_Ax_leq_u);
  checkSimpleBound(cstr4.minus_l_leq_Ax_leq_minus_u);

  std::vector<BLCPtr> c1 = {cstr.Ax_eq_0, cstr.Ax_geq_0, cstr.Ax_leq_0, cstr.Ax_eq_b,
                            cstr.Ax_geq_b, cstr.Ax_leq_b, cstr.Ax_eq_minus_b, cstr.Ax_geq_minus_b,
                            cstr.Ax_leq_minus_b, cstr.l_leq_Ax_leq_u, cstr.minus_l_leq_Ax_leq_minus_u };
  std::vector<BLCPtr> c2 = {cstr2.Ax_eq_0, cstr2.Ax_geq_0, cstr2.Ax_leq_0, cstr2.Ax_eq_b,
                            cstr2.Ax_geq_b, cstr2.Ax_leq_b, cstr2.Ax_eq_minus_b, cstr2.Ax_geq_minus_b,
                            cstr2.Ax_leq_minus_b, cstr2.l_leq_Ax_leq_u, cstr2.minus_l_leq_Ax_leq_minus_u };

  //check the intersection of bounds constraints
  for (size_t i = 0; i < 11; ++i)
  {
    for (size_t j = 0; j < 11; ++j)
    {
      checkSimpleBound(c1[i], c1[j]);
      checkSimpleBound(c1[i], c2[j]);
    }
  }
}

TEST_CASE("Test assignements with substitution")
{
  auto p0 = buildSimpleSubstitution(RHS::ZERO);
  auto p1 = buildSimpleSubstitution(RHS::AS_GIVEN);
  auto p2 = buildSimpleSubstitution(RHS::OPPOSITE);
  Substitution s0(p0.second, p0.second->variables()[1]); //substitution of y
  Substitution s1(p1.second, p1.second->variables()[1]); //substitution of y
  Substitution s2(p2.second, p2.second->variables()[1]); //substitution of y

  using T = std::vector<bool>;
  const bool t = true;
  const bool f = false;
  // constraint Ax = 0
  {
    //      Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v0 = { f,      f,      f,      t,      t,      t,      t,      t,      t,      f,      f };
    T v1 = { t,      f,      f,      t,      t,      t,      t,      t,      t,      f,      f };
    T v2 = { t,      f,      f,      t,      t,      t,      t,      t,      t,      f,      f };

    checkSimple(p0.first.Ax_eq_0, s0, v0);
    checkSimple(p1.first.Ax_eq_0, s1, v1);
    checkSimple(p2.first.Ax_eq_0, s2, v2);
  }

  // constraint Ax >= 0
  {
    //      Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v0 = { t,      t,      t,      f,      f,      f,      f,      f,      f,      f,      f };
    T v1 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    T v2 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };

    checkSimple(p0.first.Ax_geq_0, s0, v0);
    checkSimple(p1.first.Ax_geq_0, s1, v1);
    checkSimple(p2.first.Ax_geq_0, s2, v2);
  }

  // constraint Ax <= 0
  {
    //      Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v0 = { t,      t,      t,      f,      f,      f,      f,      f,      f,      f,      f };
    T v1 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    T v2 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };

    checkSimple(p0.first.Ax_leq_0, s0, v0);
    checkSimple(p1.first.Ax_leq_0, s1, v1);
    checkSimple(p2.first.Ax_leq_0, s2, v2);
  }

  // constraint Ax = 0
  {
    //      Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v0 = { t,      f,      f,      t,      t,      t,      t,      t,      t,      f,      f };
    T v1 = { t,      f,      f,      t,      t,      t,      t,      t,      t,      f,      f };
    T v2 = { t,      f,      f,      t,      t,      t,      t,      t,      t,      f,      f };

    checkSimple(p0.first.Ax_eq_b, s0, v0);
    checkSimple(p1.first.Ax_eq_b, s1, v1);
    checkSimple(p2.first.Ax_eq_b, s2, v2);
  }

  // constraint Ax >= b
  {
    //      Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v0 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    T v1 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    T v2 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };

    checkSimple(p0.first.Ax_geq_b, s0, v0);
    checkSimple(p1.first.Ax_geq_b, s1, v1);
    checkSimple(p2.first.Ax_geq_b, s2, v2);
  }

  // constraint Ax <= b
  {
    //      Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v0 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    T v1 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    T v2 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };

    checkSimple(p0.first.Ax_leq_b, s0, v0);
    checkSimple(p1.first.Ax_leq_b, s1, v1);
    checkSimple(p2.first.Ax_leq_b, s2, v2);
  }

  // constraint Ax >= b
  {
    //      Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v0 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    T v1 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    T v2 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };

    checkSimple(p0.first.l_leq_Ax_leq_u, s0, v0);
    checkSimple(p1.first.l_leq_Ax_leq_u, s1, v1);
    checkSimple(p2.first.l_leq_Ax_leq_u, s2, v2);
  }

  // constraint Ax <= b
  {
    //      Cx=0,  Cx=d,  Cx=-d,  Cx>=0,  Cx>=d, Cx>=-d,  Cx<=0,  Cx<=d, Cx<=-d,l<=Cx<=u,-l<=Cx<=-u
    T v0 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    T v1 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };
    T v2 = { t,      t,      t,      t,      f,      f,      t,      f,      f,      f,      f };

    checkSimple(p0.first.minus_l_leq_Ax_leq_minus_u, s0, v0);
    checkSimple(p1.first.minus_l_leq_Ax_leq_minus_u, s1, v1);
    checkSimple(p2.first.minus_l_leq_Ax_leq_minus_u, s2, v2);
  }
}

TEST_CASE("Test assigments")
{
  Constraints cstr = buildConstraints(3, 7);

  {
    auto mem = std::make_shared<Memory>(6, 7);
    //assignment to a target with convention l <= Ax <= u, from convention Ax >= -b
    auto range = std::make_shared<Range>(2, 3);
    AssignmentTarget at(range, mem->A, mem->l, mem->u , RHS::AS_GIVEN);
    auto req = std::make_shared<SolvingRequirements>(Weight(2.));
    VariableVector vv(cstr.Ax_eq_0->variables());
    Assignment a(cstr.Ax_geq_minus_b, req, at, vv);
    a.run();

    {
      const auto & cstr_A = cstr.Ax_geq_minus_b->jacobian(*cstr.Ax_geq_minus_b->variables()[0]);
      const auto & cstr_l = cstr.Ax_geq_minus_b->l();
      FAST_CHECK_EQ(mem->A.block(range->start, 0, 3, 7), sqrt(2)*cstr_A);
      FAST_CHECK_EQ(mem->l.block(range->start, 0, 3, 1), -sqrt(2)*cstr_l);
      FAST_CHECK_EQ(mem->u.block(range->start, 0, 3, 1), sqrt(2)*VectorXd(3).setConstant(large));
    }

    FAST_CHECK_EQ(check(cstr.Ax_geq_minus_b, cstr.p0), check(*mem.get(), at.constraintType(), at.constraintRhs(), cstr.p0));
    FAST_CHECK_EQ(check(cstr.Ax_geq_minus_b, cstr.pl), check(*mem.get(), at.constraintType(), at.constraintRhs(), cstr.pl));
    FAST_CHECK_EQ(check(cstr.Ax_geq_minus_b, cstr.pu), check(*mem.get(), at.constraintType(), at.constraintRhs(), cstr.pu));

    //now we change the range of the target and refresh the assignment
    range->start = 0;
    a.onUpdatedTarget();
    mem->A.setZero();
    mem->l.setZero();
    mem->u.setZero();
    a.run();

    {
      const auto & cstr_A = cstr.Ax_geq_minus_b->jacobian(*cstr.Ax_geq_minus_b->variables()[0]);
      const auto & cstr_l = cstr.Ax_geq_minus_b->l();
      FAST_CHECK_EQ(mem->A.block(range->start, 0, 3, 7), sqrt(2)*cstr_A);
      FAST_CHECK_EQ(mem->l.block(range->start, 0, 3, 1), -sqrt(2)*cstr_l);
      FAST_CHECK_EQ(mem->u.block(range->start, 0, 3, 1), sqrt(2)*VectorXd(3).setConstant(large));
    }
  }

  {
    auto mem = std::make_shared<Memory>(6, 7);
    //assignment to a target with convention Ax <= b, from convention l <= Ax <= u
    auto range = std::make_shared<Range>(0, 6); //we need double range
    AssignmentTarget at(range, mem->A, mem->b, Type::LOWER_THAN, RHS::AS_GIVEN);
    Vector3d aW = {1., 2., 3.};
    auto req = std::make_shared<SolvingRequirements>(AnisotropicWeight{ aW });
    VariableVector vv(cstr.Ax_eq_0->variables());
    Assignment a(cstr.l_leq_Ax_leq_u, req, at, vv);
    a.run();

    {
      const auto & cstr_A = cstr.l_leq_Ax_leq_u->jacobian(*cstr.l_leq_Ax_leq_u->variables()[0]);
      const auto & cstr_l = cstr.l_leq_Ax_leq_u->l();
      const auto & cstr_u = cstr.l_leq_Ax_leq_u->u();
      for(size_t i = 0; i < 3; ++i)
      {
        FAST_CHECK_EQ(mem->A.row(i), sqrt(aW(i))*cstr_A.row(i));
        FAST_CHECK_EQ(mem->A.row(i + 3), -sqrt(aW(i))*cstr_A.row(i));
        FAST_CHECK_EQ(mem->b(i), sqrt(aW(i))*cstr_u(i));
        FAST_CHECK_EQ(mem->b(i + 3), -sqrt(aW(i))*cstr_l(i));
      }
    }

    FAST_CHECK_EQ(check(cstr.l_leq_Ax_leq_u, cstr.p0), check(*mem.get(), at.constraintType(), at.constraintRhs(), cstr.p0));
    FAST_CHECK_EQ(check(cstr.l_leq_Ax_leq_u, cstr.pl), check(*mem.get(), at.constraintType(), at.constraintRhs(), cstr.pl));
    FAST_CHECK_EQ(check(cstr.l_leq_Ax_leq_u, cstr.pu), check(*mem.get(), at.constraintType(), at.constraintRhs(), cstr.pu));
  }
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. 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_

//
// Compare arithmetic results using fixed_int to GMP results.
//

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#include <boost/multiprecision/integer.hpp>
#include "test.hpp"

#ifdef BOOST_MSVC
#pragma warning(disable:4146)
#endif

template <class I, class H>
void test()
{
   using namespace boost::multiprecision;

   I i(0);

   BOOST_CHECK_THROW(lsb(i), std::range_error);
   BOOST_CHECK(bit_test(bit_set(i, 0), 0));
   BOOST_CHECK_EQUAL(bit_set(i, 0), 1);
   BOOST_CHECK_EQUAL(bit_unset(i, 0), 0);
   BOOST_CHECK_EQUAL(bit_flip(bit_set(i, 0), 0), 0);

   unsigned max_index = (std::numeric_limits<I>::digits) - 1;
   BOOST_CHECK(bit_test(bit_set(i, max_index), max_index));
   BOOST_CHECK_EQUAL(bit_unset(i, max_index), 0);
   BOOST_CHECK_EQUAL(bit_flip(bit_set(i, max_index), max_index), 0);

   if(std::numeric_limits<I>::is_signed)
   {
      i = static_cast<I>(-1);
      BOOST_CHECK_THROW(lsb(i), std::range_error);
   }

   H mx = (std::numeric_limits<H>::max)();

   BOOST_CHECK_EQUAL(multiply(i, mx, mx), static_cast<I>(mx) * static_cast<I>(mx));
   BOOST_CHECK_EQUAL(add(i, mx, mx), static_cast<I>(mx) + static_cast<I>(mx));
   if(std::numeric_limits<I>::is_signed)
   {
      BOOST_CHECK_EQUAL(subtract(i, mx, static_cast<H>(-mx)), static_cast<I>(mx) - static_cast<I>(-mx));
      BOOST_CHECK_EQUAL(add(i, static_cast<H>(-mx), static_cast<H>(-mx)), static_cast<I>(-mx) + static_cast<I>(-mx));
   }

   i = (std::numeric_limits<I>::max)();
   I j = 12345;
   I r, q;
   divide_qr(i, j, q, r);
   BOOST_CHECK_EQUAL(q, i / j);
   BOOST_CHECK_EQUAL(r, i % j);
   BOOST_CHECK_EQUAL(integer_modulus(i, j), i % j);
   I p = 456;
   BOOST_CHECK_EQUAL(powm(i, p, j), pow(cpp_int(i), static_cast<unsigned>(p)) % j);
}

int main()
{
   using namespace boost::multiprecision;

   test<boost::int32_t, boost::int16_t>();
   test<boost::int64_t, boost::int32_t>();
   test<boost::uint32_t, boost::uint16_t>();
   test<boost::uint64_t, boost::uint32_t>();

   return boost::report_errors();
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/any_of.hpp>
#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/mod.hpp>
#include <boost/hana/not.hpp>
#include <boost/hana/not_equal.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <type_traits>
namespace hana = boost::hana;
using namespace hana::literals;

BOOST_HANA_CONSTEXPR_LAMBDA auto is_odd = [](auto x) {
    return x % 2_c != 0_c;
};

int main() {
    BOOST_HANA_CONSTEXPR_CHECK(hana::any_of(hana::make_tuple(1, 2), is_odd));
    BOOST_HANA_CONSTANT_CHECK(!hana::any_of(hana::make_tuple(2_c, 4_c), is_odd));

    BOOST_HANA_CONSTANT_CHECK(hana::any_of(
        hana::make_tuple(hana::type_c<void>, hana::type_c<char&>), hana::trait<std::is_void>
    ));
    BOOST_HANA_CONSTANT_CHECK(!hana::any_of(
        hana::make_tuple(hana::type_c<void>, hana::type_c<char&>), hana::trait<std::is_integral>
    ));
}


#ifndef PA_MATH_VECTOR_FIELD_HPP
#define PA_MATH_VECTOR_FIELD_HPP

#include <memory>

#include <boost/multi_array.hpp>

#include <pa/math/tensor_field.hpp>
#include <pa/math/types.hpp>
#include <pa/export.hpp>

namespace pa
{
struct PA_EXPORT vector_field
{
  bool                          contains   (const vector4& position) const;
  vector3                       interpolate(const vector4& position) const;
  std::unique_ptr<tensor_field> gradient   ();

  boost::multi_array<vector3, 3> data    {};
  vector3                        offset  {};
  vector3                        size    {};
  vector3                        spacing {};
};
}

#endif