text:
#ifndef DBJ_VARIOUS_SIMPLE_MATMULS_INC
#define DBJ_VARIOUS_SIMPLE_MATMULS_INC

// not using any kind of "matrix framework"

#include "../dbj_matmul_common.h" // DBJ_MATRIX_DATA_TYPE

#include <stdlib.h>
#include <intrin.h> // #define __SSE__ 1
#ifdef HAVE_CBLAS
#include <cblas.h>
#endif

#undef DBJ_API
#define DBJ_API

//////////////////////////////////////////////////////////////////////
#ifndef DBJ_SANITY_MAX_ROW_COUNT
#define DBJ_SANITY_MAX_ROW_COUNT 0xFFFF
#endif

#ifndef DBJ_SANITY_MAX_COL_COUNT
#define DBJ_SANITY_MAX_COL_COUNT 0xFFFF
#endif
//////////////////////////////////////////////////////////////////////

typedef void* (*simple_mat_mul_function)(
	const unsigned, const unsigned, const unsigned,
	DBJ_MATRIX_DATA_TYPE[][*],
	DBJ_MATRIX_DATA_TYPE[][*],
	DBJ_MATRIX_DATA_TYPE[][*] /* the result */
	);

typedef struct
{
	const char* description;
	simple_mat_mul_function function;
} simple_description_function_pair;

#define DF_PAIR(D_, F_) \
	(simple_description_function_pair) { .description = D_, .function = F_ }

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

DBJ_API DBJ_MATRIX_DATA_TYPE* new_simple_matrix(const unsigned, const unsigned);

DBJ_API DBJ_MATRIX_DATA_TYPE* make_simple_matrix(
	const unsigned, const unsigned, DBJ_MATRIX_DATA_TYPE(*)(void));

DBJ_API void* simple_mat_transpose(
	const unsigned, const unsigned, DBJ_MATRIX_DATA_TYPE[][*], DBJ_MATRIX_DATA_TYPE[][*]);

DBJ_API void* simple_mat_mul_null(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
	// used for when algorithms are not implemented yet
	(void)a;
	(void)b;
	(void)m;
	return NULL;
}

DBJ_API void* simple_mat_mul_0_0(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE(*ax)[n_a_cols],
	DBJ_MATRIX_DATA_TYPE(*bx)[n_a_cols],
	DBJ_MATRIX_DATA_TYPE(*mx)[n_a_cols]);

DBJ_API void* simple_mat_mul_0(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/]);

DBJ_API void* simple_mat_mul_1(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/]);

DBJ_API void* simple_mat_mul_3(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/]);

DBJ_API void* simple_mat_mul_4(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/]);

#if __SSE__

DBJ_API void* simple_mat_mul_2(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE a[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE b[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE m[/*n_a_rows*/][* /*n_b_cols*/]);

DBJ_API void* simple_mat_mul_7(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE a[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE b[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE m[/*n_a_rows*/][* /*n_b_cols*/]);

#endif // __SSE__

#ifdef HAVE_CBLAS
DBJ_API void* simple_mat_mul_5(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE a[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE b[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE m[/*n_a_rows*/][* /*n_b_cols*/]);

DBJ_API void* simple_mat_mul_6(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE a[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE b[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE m[/*n_a_rows*/][* /*n_b_cols*/]);
#endif // HAVE_CBLAS

const simple_description_function_pair dbj_simple_matmuls_algo_table[] =
{
	DF_PAIR("0: MAX optimization", simple_mat_mul_0_0),
	DF_PAIR("1: simple naive - no optimization", simple_mat_mul_0),
	DF_PAIR("2: simple transposing the second matrix", simple_mat_mul_1),
	DF_PAIR("3: simple explicitly vectorized sdot() with SSE", simple_mat_mul_2), /* requires __SSE__ */
	DF_PAIR("4: simple explicitly SSE sdot() plus loop tiling", simple_mat_mul_7) , /* requires __SSE__ */
	DF_PAIR("5: simple implicitly vectorized sdot()", simple_mat_mul_3),
	DF_PAIR("6: simple no vectorization hints", simple_mat_mul_4),
#ifdef HAVE_CBLAS
	DF_PAIR("7: simple with sdot() from an external CBLAS library", simple_mat_mul_5),
	DF_PAIR("8: simple with sgemm() from an external CBLAS library", simple_mat_mul_6),
#endif																	// ! HAVE_CBLAS
};

static const unsigned dbj_simple_matmuls_algo_table_size = (sizeof(dbj_simple_matmuls_algo_table) / sizeof(dbj_simple_matmuls_algo_table[0]));

//////////////////////////////////////////////////////////////////////
#ifdef DBJ_VARIOUS_SIMPLE_MATMULS_IMPLEMENTATION
//////////////////////////////////////////////////////////////////////

DBJ_API DBJ_MATRIX_DATA_TYPE* new_simple_matrix(const unsigned n_rows, const unsigned n_cols)
{
	DBJ_MATRIX_DATA_TYPE* retval = calloc(n_rows * n_cols, sizeof(DBJ_MATRIX_DATA_TYPE));

	if (!retval)
	{
		fprintf(stderr, "\n%s(%d) new_simple_matrix() failed?\n", __FILE__, __LINE__);
		perror(" ");
		exit(1);
	}
	return retval;
}

DBJ_API DBJ_MATRIX_DATA_TYPE* make_simple_matrix(
	const unsigned n_rows, const unsigned n_cols, DBJ_MATRIX_DATA_TYPE(*value_provider)(void))
{
	DBJ_MATRIX_DATA_TYPE* rezult = new_simple_matrix(n_rows, n_cols);

	typedef DBJ_MATRIX_DATA_TYPE(*matrix)[n_cols];
	matrix mx = (matrix)rezult;

	for (unsigned i = 0; i < n_rows; ++i)
		for (unsigned j = 0; j < n_cols; ++j)
			mx[i][j] = value_provider(); //
	return rezult;
}

DBJ_API void* simple_mat_transpose(
	const unsigned n_rows, const unsigned n_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_rows][n_cols], DBJ_MATRIX_DATA_TYPE m[static n_cols][n_rows])
{
	for (unsigned i = 0; i < n_rows; ++i)
		for (unsigned j = 0; j < n_cols; ++j)
			m[j][i] = a[i][j];
	return m;
}

DBJ_API DBJ_MATRIX_DATA_TYPE simple_sdot_1(int n, const DBJ_MATRIX_DATA_TYPE x[static n], const DBJ_MATRIX_DATA_TYPE y[static n])
{
	DBJ_MATRIX_DATA_TYPE s = 0.0f;
	for (int i = 0; i < n; ++i)
		s += x[i] * y[i];
	return s;
}

DBJ_API DBJ_MATRIX_DATA_TYPE simple_sdot_8(int n, const DBJ_MATRIX_DATA_TYPE x[static n], const DBJ_MATRIX_DATA_TYPE y[static n])
{
	int i, n8 = n >> 3 << 3;
	DBJ_MATRIX_DATA_TYPE s = 0.0f, t[8] = { 0.0f };
	// t[0] = t[1] = t[2] = t[3] = t[4] = t[5] = t[6] = t[7] = 0.0f;
	for (i = 0; i < n8; i += 8)
	{
		t[0] += x[i + 0] * y[i + 0];
		t[1] += x[i + 1] * y[i + 1];
		t[2] += x[i + 2] * y[i + 2];
		t[3] += x[i + 3] * y[i + 3];
		t[4] += x[i + 4] * y[i + 4];
		t[5] += x[i + 5] * y[i + 5];
		t[6] += x[i + 6] * y[i + 6];
		t[7] += x[i + 7] * y[i + 7];
	}
	for (s = 0.0f; i < n; ++i)
		s += x[i] * y[i];
	s += t[0] + t[1] + t[2] + t[3] + t[4] + t[5] + t[6] + t[7];
	return s;
}

#ifdef __SSE__

DBJ_API DBJ_MATRIX_DATA_TYPE simple_sdot_sse(int n, const DBJ_MATRIX_DATA_TYPE x[static n], const DBJ_MATRIX_DATA_TYPE y[static n])
{
	int i, n8 = n >> 3 << 3;
	__m128 vs1, vs2;
	DBJ_MATRIX_DATA_TYPE s, t[4];
	vs1 = _mm_setzero_ps();
	vs2 = _mm_setzero_ps();
	for (i = 0; i < n8; i += 8)
	{
		__m128 vx1, vx2, vy1, vy2;
		vx1 = _mm_loadu_ps(&x[i]);
		vx2 = _mm_loadu_ps(&x[i + 4]);
		vy1 = _mm_loadu_ps(&y[i]);
		vy2 = _mm_loadu_ps(&y[i + 4]);
		vs1 = _mm_add_ps(vs1, _mm_mul_ps(vx1, vy1));
		vs2 = _mm_add_ps(vs2, _mm_mul_ps(vx2, vy2));
	}
	for (s = 0.0f; i < n; ++i)
		s += x[i] * y[i];
	_mm_storeu_ps(t, vs1);
	s += t[0] + t[1] + t[2] + t[3];
	_mm_storeu_ps(t, vs2);
	s += t[0] + t[1] + t[2] + t[3];
	return s;
}
#endif // __SSE__

/**************************************************

 Various Matrix multiplication algorithms

 NOTE: check and choose for your RT Env
	   there are differences but are *very* dependant on
	   compiler, OS and hardware

 */

DBJ_API void* simple_mat_mul_0_0(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE(*ax)[n_a_cols],
	DBJ_MATRIX_DATA_TYPE(*bx)[n_a_cols],
	DBJ_MATRIX_DATA_TYPE(*mx)[n_a_cols])
{
	// runtime casting takes time
	const unsigned n_b_rows = n_a_cols;
	// DBJ_MATRIX_DATA_TYPE(*ax)[n_a_cols] = a;
	// DBJ_MATRIX_DATA_TYPE(*bx)[n_b_cols] = b;
	// DBJ_MATRIX_DATA_TYPE(*mx)[n_b_rows] = m;

	for (unsigned i = 0; i < n_a_rows; ++i)
	{
		for (unsigned j = 0; j < n_b_cols; ++j)
		{
			DBJ_MATRIX_DATA_TYPE t = 0.0;
			for (unsigned k = 0; k < n_a_cols; ++k)
				t += ax[i][k] * bx[k][j];
			mx[i][j] = t;
		}
	}
	return mx;
}

DBJ_API void* simple_mat_mul_0(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
	for (unsigned i = 0; i < n_a_rows; ++i)
	{
		for (unsigned j = 0; j < n_b_cols; ++j)
		{
			DBJ_MATRIX_DATA_TYPE t = 0.0;
			for (unsigned k = 0; k < n_a_cols; ++k)
				t += a[i][k] * b[k][j];
			m[i][j] = t;
		}
	}
	return m;
}

DBJ_API void* simple_mat_mul_1(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
	const unsigned n_b_rows = n_a_cols;

	DBJ_MATRIX_DATA_TYPE* Temp = new_simple_matrix(n_b_cols, n_b_rows);
	// Temp rows and cols are inverted !
	typedef DBJ_MATRIX_DATA_TYPE(*matrix)[n_b_rows];
	matrix bT = (matrix)Temp;
	(void)simple_mat_transpose(n_b_rows, n_b_cols, b, bT);

	for (unsigned i = 0; i < n_a_rows; ++i)
	{
		const DBJ_MATRIX_DATA_TYPE* ai = a[i];
		DBJ_MATRIX_DATA_TYPE* mi = m[i];
		for (unsigned j = 0; j < n_b_cols; ++j)
		{
			DBJ_MATRIX_DATA_TYPE t = 0.0f, * bTj = bT[j];
			for (unsigned k = 0; k < n_a_cols; ++k)
				t += ai[k] * bTj[k];
			mi[j] = t;
		}
	}

	free(Temp);

	return m;
}

DBJ_API void* simple_mat_mul_2(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
#ifndef __SSE__
#error __SEE__ is required here
#endif

	const unsigned n_b_rows = n_a_cols;

	DBJ_MATRIX_DATA_TYPE* Temp = new_simple_matrix(n_b_cols, n_b_rows);
	// Temp rows and cols are inverted !
	typedef DBJ_MATRIX_DATA_TYPE(*matrix)[n_b_rows];
	matrix bT = (matrix)Temp;
	(void)simple_mat_transpose(n_b_rows, n_b_cols, b, bT);

	for (unsigned i = 0; i < n_a_rows; ++i)
		for (unsigned j = 0; j < n_b_cols; ++j)
			m[i][j] = simple_sdot_sse(n_a_cols, a[i], bT[j]);
	free(Temp);
	return m;
}

DBJ_API void* simple_mat_mul_7(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
#ifndef __SSE__
#error __SEE__ is required here
#endif

	const unsigned x = 16, n_b_rows = n_a_cols;

	DBJ_MATRIX_DATA_TYPE* Temp = new_simple_matrix(n_b_cols, n_b_rows);
	// Temp rows and cols are inverted !
	typedef DBJ_MATRIX_DATA_TYPE(*matrix)[n_b_rows];
	matrix bT = (matrix)Temp;
	(void)simple_mat_transpose(n_b_rows, n_b_cols, b, bT);

	for (unsigned i = 0; i < n_a_rows; i += x)
	{
		for (unsigned j = 0; j < n_b_cols; j += x)
		{
			unsigned je = n_b_cols < j + x ? n_b_cols : j + x;
			unsigned ie = n_a_rows < i + x ? n_a_rows : i + x;
			for (unsigned ii = i; ii < ie; ++ii)
				for (unsigned jj = j; jj < je; ++jj)
					m[ii][jj] += simple_sdot_sse(n_a_cols, a[ii], bT[jj]);
		}
	}
	free(Temp);
	return m;
}

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

DBJ_API void* simple_mat_mul_3(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
	int n_b_rows = n_a_cols;

	DBJ_MATRIX_DATA_TYPE* Temp = new_simple_matrix(n_b_cols, n_b_rows);
	// Temp rows and cols are inverted !
	typedef DBJ_MATRIX_DATA_TYPE(*matrix)[n_b_rows];
	matrix bT = (matrix)Temp;
	(void)simple_mat_transpose(n_b_rows, n_b_cols, b, bT);

	for (unsigned i = 0; i < n_a_rows; ++i)
		for (unsigned j = 0; j < n_b_cols; ++j)
			m[i][j] = simple_sdot_8(n_a_cols, a[i], bT[j]);
	free(Temp);
	return m;
}

DBJ_API void* simple_mat_mul_4(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
	int n_b_rows = n_a_cols;

	DBJ_MATRIX_DATA_TYPE* Temp = new_simple_matrix(n_b_cols, n_b_rows);
	// Temp rows and cols are inverted !
	typedef DBJ_MATRIX_DATA_TYPE(*matrix)[n_b_rows];
	matrix bT = (matrix)Temp;
	(void)simple_mat_transpose(n_b_rows, n_b_cols, b, bT);

	for (unsigned i = 0; i < n_a_rows; ++i)
		for (unsigned j = 0; j < n_b_cols; ++j)
			m[i][j] = simple_sdot_1(n_a_cols, a[i], bT[j]);
	free(Temp);
	return m;
}

#ifdef HAVE_CBLAS

DBJ_API void* simple_mat_mul_5(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
	const unsigned n_b_rows = n_a_cols;

	DBJ_MATRIX_DATA_TYPE* Temp = new_simple_matrix(n_rows, n_cols);
	// Temp rows and cols are inverted !
	typedef DBJ_MATRIX_DATA_TYPE(*matrix)[n_rows];
	matrix bT = (matrix)Temp;
	(void)simple_mat_transpose(n_b_rows, n_b_cols, b, bT);

	for (unsigned i = 0; i < n_a_rows; ++i)
		for (unsigned j = 0; j < n_b_cols; ++j)
			m[i][j] = cblas_sdot(n_a_cols, a[i], 1, bT[j], 1); // clas_sdot() ???
	free(Temp);
	return m;
}

DBJ_API void* simple_mat_mul_6(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
	cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, n_a_rows, n_b_cols, n_a_cols, 1.0f, a[0], n_a_rows, b[0], n_b_rows, 0.0f, m[0], n_a_rows);
	return m;
}
#endif // HAVE_CBLAS

//////////////////////////////////////////////////////////////////////
#endif // DBJ_VARIOUS_SIMPLE_MATMULS_IMPLEMENTATION
//////////////////////////////////////////////////////////////////////

#undef DF_PAIR

#endif //  DBJ_VARIOUS_SIMPLE_MATMULS_INC


text:
#pragma once

#include <type_traits>
#include <utility>

#include <cuda/define_specifiers.hpp>
#include <cuda/runtime_api.hpp>

#include <gsl-lite/gsl-lite.hpp>

namespace thrustshift {

namespace kernel {

template <typename SrcT, typename DstT, class F>
__global__ void transform(gsl_lite::span<const SrcT> src,
                          gsl_lite::span<DstT> dst,
                          F f) {

	const auto gtid = threadIdx.x + blockIdx.x * blockDim.x;
	if (gtid < src.size()) {
		dst[gtid] = f(src[gtid]);
	}
}

} // namespace kernel

namespace async {

template <class SrcRange, class DstRange, class UnaryFunctor>
void transform(cuda::stream_t& stream,
               SrcRange&& src,
               DstRange&& dst,
               UnaryFunctor f) {
	gsl_Expects(src.size() == dst.size());

	if (src.empty()) {
		return;
	}

	using src_value_type =
	    typename std::remove_reference<SrcRange>::type::value_type;
	using dst_value_type =
	    typename std::remove_reference<DstRange>::type::value_type;

	constexpr cuda::grid::block_dimension_t block_dim = 128;
	const cuda::grid::dimension_t grid_dim =
	    (src.size() + block_dim - 1) / block_dim;
	auto c = cuda::make_launch_config(grid_dim, block_dim);
	auto k = kernel::transform<src_value_type, dst_value_type, decltype(f)>;
	cuda::enqueue_launch(k, stream, c, src, dst, f);
}

} // namespace async

namespace array {

namespace detail {

template <typename T,
          std::size_t N,
          template <typename, std::size_t>
          class Arr,
          class F,
          std::size_t... I>
CUDA_FHD auto transform_impl(const Arr<T, N>& arr,
                             F&& f,
                             std::index_sequence<I...>) {
	return Arr<T, N>({f(arr[I])...});
}

} // namespace detail

template <typename T,
          std::size_t N,
          template <typename, std::size_t>
          class Arr,
          class F,
          typename Indices = std::make_index_sequence<N>>
CUDA_FHD auto transform(const Arr<T, N>& arr, F&& f) {
	return detail::transform_impl(arr, std::forward<F>(f), Indices{});
}

} // namespace array

namespace tuple {

namespace detail {

template <typename Tuple, typename F, std::size_t... I>
CUDA_FHD auto transform_impl(Tuple&& t, F&& f, std::index_sequence<I...>) {
	using TupleT = typename std::remove_reference<Tuple>::type;
	using std::get;
	return TupleT(f(get<I>(t))...);
}

} // namespace detail

template <typename... Ts, template <typename...> class TupleT, typename F>
CUDA_FHD auto transform(TupleT<Ts...> const& t, F&& f) {
	using std::tuple_size;
	auto seq = std::make_index_sequence<tuple_size<TupleT<Ts...>>::value>{};
	return detail::transform_impl(t, std::forward<F>(f), seq);
}

} // namespace tuple

} // namespace thrustshift

ext: h

max_stars_repo_name: pauleonix/thrustshift

max_issues_repo_name: pauleonix/thrustshift

max_forks_repo_name: pauleonix/thrustshift


text:
/**
 * \file gps_single.c Implements GPS single point solution using Least Squares
 * \author Duncan Greer
 * $Id: gps_single.c 3579 2010-06-26 10:57:31Z greerd $
 */

#include <stdio.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <errno.h>
#include <unistd.h>
#include <stdlib.h>
#include <signal.h>
#include <string.h>
#include <time.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_linalg.h>
#include "../../novatelstore/src/novatel.h"
#include "gps.h"
#include "../../novatelstore/src/shmdef.h"

#include "matrix.h"

/** Controls whether or not the program should keep running. */
int iShutdown = 0;

#define MODULE_NAME "[GPS_SINGLE]"

#define log(string) 	fprintf(stdout,"%s %s\n", MODULE_NAME, string)
#define err(string)	fprintf(stderr,"%s ERROR: %s\n", MODULE_NAME, string)

#define ELEVATION_CUTOFF_RAD (10.0 * DEG2RAD)

/**
	\function CalculateDOP
	Calculates the Dilution of Precision based on the provided G Matrix.
	\author Duncan Greer and Troy Bruggemann

	Note that the G-Matrix must be provided already transformed to
	the horizontal plane.
*/
void CalculateDOP(gsl_matrix *G, double *DOP);

/**
 *	\fn HandleSignal
 *	\brief Handle caught signals
 *
 * 	This function sets a flag (iShutdown) to shut the program down.
 */
void HandleSignal(int signal)
{
	fprintf(stdout,"Got Signal: %d\n",signal);
	iShutdown = 1;
}

/**
 * 	\fn Usage
 *	\brief Print Usage Information
 */
void Usage(char *arg)
{

	/* options */
	/*
	    b = baudrate
	    p = port
	    l = logging
	    f = log filename
	    t = enable tcp server
	*/
	fprintf(stdout,"Usage: %s <options>\n",arg);
	fprintf(stdout,"    -l Log results to file (OFF)\n");
	fprintf(stdout,"    -f log filename\n");
	fprintf(stdout,"    -L Use specified log file path (/data/gps)\n");

}

int main (int argc, char **argv)
{
	gsl_matrix *G;
	gsl_matrix *G_LTP;
//	gsl_matrix *GT;
//	gsl_matrix *tempNN;
//	gsl_matrix *V;
//	gsl_vector *S;
//	gsl_matrix *U;
	gsl_vector *W;
	gsl_vector *y;
	gsl_vector *delta_y;
	gsl_vector *x;
	gsl_vector *delta_x;
	gsl_vector *x_error;
	gsl_vector *x_true;
	gsl_vector *temp1;

	gsl_multifit_linear_workspace *gsl_workspace;
	gsl_vector *SVPos[MAX_SV];
	double SVPos_calc[4];
	double dSSE;
	gsl_matrix *SolutionCovariance;
	int iReturnValue = 0;

	int iNumberStates = 4;
	int iNumberMeasurements = 9;
	int iNumberValidEphemeris = 0;
	int iNumberValidObservations = 0;
	int iIndex;
	GPSTime gtObservationTime;
	unsigned short usPRN;
	int iIteration;

	int iUseSV[NOVATEL_MAXPRNS];

	double user_x,user_y,user_z,user_t = 0.0;
	double sat_x,sat_y,sat_z,sat_t;
//	double sat_x_calc,sat_y_calc,sat_z_calc,sat_t_calc;
	double dTemp;

	double range;
	double delta_pr_omegaedot;

	double ecef[3];
	double llh[3];
	double TECEF2ENU[3][3];
	gsl_matrix *T_ECEF2NED_44;
	double NEDError[3];
	double ECEFError[3];
	double UserPos[4];
	double ION_Alpha[4];
	double ION_Beta[4];
	double IonoDelay[NOVATEL_MAXCHANNELS];
	double TropoDelay[NOVATEL_MAXCHANNELS];
	double SV_Azimuth[NOVATEL_MAXCHANNELS];
	double SV_Elevation[NOVATEL_MAXCHANNELS];

	double dTransmissionTime;

	GPSEPHEM  GPSEphemData[NOVATEL_MAXPRNS];
	RANGE GPSRangeData;

	double GDOP,PDOP;
	double DOPS[5];
	double limit;

	int iUseSHM = 1;
	int iSHM_FID;

	int iRow,iCol;

	shm_struct *shm = NULL;

	unsigned long ulSignalType = 0;

	char copt;

	/* log files */
	FILE *fpLogFile = NULL;    /* pointer to log file stream */
	char azcLogFileName[256];  /* log file name */
	char azcLogFilePath[255];
	char azcLogFileNameWithPath[1024];

	struct tm *timestruct;
	time_t tCurrentTime = 0;
	time_t tLogFileStart = 0;

	/* options flags */
	struct
	{
		int iLogResults;
		int iCustomLogFile;
	} options;

	// default options
	options.iLogResults = 0;
	options.iCustomLogFile = 0;

	/* setup signal handlers */
	signal(SIGHUP, HandleSignal);
	signal(SIGTERM, HandleSignal);
	signal(SIGKILL, HandleSignal);
	signal(SIGALRM, HandleSignal);
	signal(SIGINT, HandleSignal);

	/* get the current time to use as a timestamp in the log file name */
	tCurrentTime = time(NULL);
	timestruct = gmtime(&tCurrentTime);

	/* setting default log file path */
	sprintf(azcLogFilePath,"%s","/data/gps");

	/* read input parameters */
	while((copt = getopt(argc,argv,"lf:L:")) && copt != -1)
	{
	  switch(copt)
	  {
	   case 'l':
		/* log results to file */
		options.iLogResults = 1;
		log("Logging enabled");
		strftime(azcLogFileName,sizeof(azcLogFileName),"gps_single_%g%m%d%H%M%S.csv",timestruct);
		break;

	  case 'f':
		/* set the log file name */
		sprintf(azcLogFileName,"%s",optarg);
		options.iCustomLogFile = 1; /* since we're using a user specified log file name, we dont want to recycle it every hour */
		break;

	  case 'L':
	  	/* setting log file path */
	  	sprintf(azcLogFilePath,"%s",optarg);

	  	fprintf(stdout,"%s Setting log file path to %s",MODULE_NAME,azcLogFilePath);
	  	break;
	  case '?':
		Usage(argv[0]);
		return 0;
		break;
	  }
	}

	for(iIndex=0;iIndex<NOVATEL_MAXPRNS;iIndex++)
	{
		iUseSV[iIndex] = 0;
	}

	fprintf(stdout,"Allocating Matrices\n");
	x = gsl_vector_alloc(iNumberStates);
	delta_x = gsl_vector_alloc(iNumberStates);
	x_true = gsl_vector_alloc(iNumberStates);
	x_error = gsl_vector_alloc(iNumberStates);
	SolutionCovariance = gsl_matrix_alloc(iNumberStates,iNumberStates);

	temp1 = gsl_vector_alloc(3);

	for(iIndex=0;iIndex<MAX_SV;iIndex++)
	{
		SVPos[iIndex] = gsl_vector_alloc(4);
	}

	T_ECEF2NED_44 = gsl_matrix_alloc(4,4);

       	/* setup shared memory */
	if(iUseSHM == 1)
	{
	  iSHM_FID = shm_open("/novatel0",O_RDWR,0777);
	  if(iSHM_FID == -1)
	  {
		perror("shm_open: ");
		iUseSHM = 0;
 	  } else
	  {
	     	/* memory map the shm object */
	     	shm = mmap(0,sizeof(*shm),PROT_READ,MAP_SHARED,iSHM_FID,0);
	     	if(shm == MAP_FAILED)
	     	{
		   perror("shm mmap: ");
		   iUseSHM = 0;
	     	} else
		{
		  fprintf(stdout,"%s SHM Address Is: 0x%08x\n",MODULE_NAME,(unsigned int)shm);
		}
	  }
	}

	/* setup time */
	gtObservationTime.usGPSWeek = 1416;
	gtObservationTime.fGPSSecondOfWeek = 135900.0;

	/* setup measurements */
/*	GPSRangeData.usPRN[0] = 10;
	GPSRangeData.usPRN[1] = 12;
	GPSRangeData.usPRN[2] = 30;
	GPSRangeData.usPRN[3] = 29;
	GPSRangeData.usPRN[4] = 7;
	GPSRangeData.usPRN[5] = 26;
	GPSRangeData.usPRN[6] = 22;
	GPSRangeData.usPRN[7] = 24;
	GPSRangeData.dPseudorange[0] = 23677691.85548;
	GPSRangeData.dPseudorange[1] = 24609189.10747;
	GPSRangeData.dPseudorange[2] = 22288602.64148;
	GPSRangeData.dPseudorange[3] = 22641003.92448;
	GPSRangeData.dPseudorange[4] = 20042814.45649;
	GPSRangeData.dPseudorange[5] = 23024522.23148;
	GPSRangeData.dPseudorange[6] = 24837761.16146;
	GPSRangeData.dPseudorange[7] = 21114887.48349;
*/
	/* setup sat positions */

	/* estimated position - starting point */
	gsl_vector_set(x,0,-5046773.0);
	gsl_vector_set(x,1, 2568446.0);
	gsl_vector_set(x,2,-2925289.0);
	gsl_vector_set(x,3, 0.0);

	llh[0] = -0.47;
	llh[1] = 2.53;
	llh[2] = 0.0;

	//gsl_vector_fprintf(stdout,x,"%10.3f");

	/* true position */
	gsl_vector_set(x_true,0,-5053034.84 );
	gsl_vector_set(x_true,1, 2562951.08);
	gsl_vector_set(x_true,2,-2919245.94);
	gsl_vector_set(x_true,3,0.0);

/*
	// PRN 10
	gsl_vector_set(SVPos[10],0,-11106779.576);
	gsl_vector_set(SVPos[10],1,-10162086.179);
	gsl_vector_set(SVPos[10],2,-21984106.076);
	gsl_vector_set(SVPos[10],3, 98.034009e-6*GPS_SPEEDOFLIGHT);

	// PRN 12
	gsl_vector_set(SVPos[12],0,-20114520.008);
	gsl_vector_set(SVPos[12],1,  6748657.642);
	gsl_vector_set(SVPos[12],2, 16058860.089);
	gsl_vector_set(SVPos[12],3,-47.814630e-6*GPS_SPEEDOFLIGHT);

	// PRN 30
	gsl_vector_set(SVPos[30],0, -19111202.845);
	gsl_vector_set(SVPos[30],1, 17011909.480);
	gsl_vector_set(SVPos[30],2, 6599201.305);
	gsl_vector_set(SVPos[30],3, 26.513082e-6*GPS_SPEEDOFLIGHT);

	// PRN 29
	gsl_vector_set(SVPos[29],0, -22931680.210);
	gsl_vector_set(SVPos[29],1, -10007127.790);
	gsl_vector_set(SVPos[29],2,  -9208106.350);
	gsl_vector_set(SVPos[29],3, 358.103982e-6*GPS_SPEEDOFLIGHT);

	// PRN 7
	gsl_vector_set(SVPos[7],0, -18000552.275);
	gsl_vector_set(SVPos[7],1,  10196913.772);
	gsl_vector_set(SVPos[7],2, -16363130.228);
	gsl_vector_set(SVPos[7],3, 402.849928e-6*GPS_SPEEDOFLIGHT);

	// PRN 26
	gsl_vector_set(SVPos[26],0, -24783876.733);
	gsl_vector_set(SVPos[26],1,  -8910991.974);
	gsl_vector_set(SVPos[26],2,  -5742918.813);
	gsl_vector_set(SVPos[26],3, -62.236139e-6*GPS_SPEEDOFLIGHT);

	// PRN 22
	gsl_vector_set(SVPos[22],0, -8616964.914);
	gsl_vector_set(SVPos[22],1, 22105308.026);
	gsl_vector_set(SVPos[22],2, 12074730.766);
	gsl_vector_set(SVPos[22],3, 169.425839e-6*GPS_SPEEDOFLIGHT);

	// PRN 24
	gsl_vector_set(SVPos[24],0, -15110809.416);
	gsl_vector_set(SVPos[24],1,   2514877.198);
	gsl_vector_set(SVPos[24],2, -21514889.442);
	gsl_vector_set(SVPos[24],3, 80.656626e-6*GPS_SPEEDOFLIGHT);

	*/

	/* open log file */
	if(options.iLogResults == 1)
	{
		sprintf(azcLogFileNameWithPath,"%s/%s",azcLogFilePath,azcLogFileName);

		if((fpLogFile = fopen(azcLogFileNameWithPath,"a")) == NULL)
		{
			fprintf(stderr,"Error opening %s for writing: %s\n",azcLogFileNameWithPath,strerror(errno));

			return -1;
		} else
		{
			tLogFileStart = tCurrentTime;
			fprintf(stdout,"Using log file: %s\n",azcLogFileNameWithPath);
		}

	}

  while(iShutdown == 0)
  {

	/* if log file has been opend for > 1 hour, start a new one */
	if(options.iLogResults == 1 && options.iCustomLogFile == 0)
	{
		tCurrentTime = time(NULL);
		if(difftime(tCurrentTime,tLogFileStart) > 3600.0)
		{
			/* start a new log file */
			fclose(fpLogFile);

			timestruct = gmtime(&tCurrentTime);
			strftime(azcLogFileName,sizeof(azcLogFileName),"gps_single_%g%m%d%H%M%S.csv",timestruct);

			sprintf(azcLogFileNameWithPath,"%s/%s",azcLogFilePath,azcLogFileName);

			if((fpLogFile = fopen(azcLogFileNameWithPath,"a")) == NULL)
			{
				fprintf(stderr,"Error opening %s for writing: %s\n",azcLogFileNameWithPath,strerror(errno));
				options.iLogResults = 0;
			} else
			{
				tLogFileStart = tCurrentTime;
				fprintf(stdout,"%s Started New Logfile: %s\n",MODULE_NAME,azcLogFileNameWithPath);

			}
		}

	}

	/* TODO:  replace this sleep with a semaphore to wait for new data */
  	sleep(1);

	fprintf(stdout,"====== Single-Point Solution ======\n");
	/* retrive the current data from shm */
	if(iUseSHM == 1)
	{
	  memcpy(&GPSEphemData,&shm->CurrentGPSEPHEM,sizeof(GPSEPHEM)*NOVATEL_MAXPRNS);

	  // get the current observation time to compare with ephemeris
	  memcpy(&gtObservationTime,&shm->CurrentRANGE.gtTimeStamp,sizeof(GPSTime));

	  /* ensure we have ephemeris for all measurements, otherwise exclude */
	  iNumberValidEphemeris = 0;
	  for(iIndex=0;iIndex<NOVATEL_MAXPRNS;iIndex++)
	  {
	  	if((GPSEphemData[iIndex].ulPRN != 0) &&
		   (GPSEphemData[iIndex].ulHealth == 0) &&
		   (GPSEphemData[iIndex].dTOE < (gtObservationTime.fGPSSecondOfWeek+7500.0)) &&
		   (GPSEphemData[iIndex].dTOE > (gtObservationTime.fGPSSecondOfWeek-7500.0))
	  	)
	  	{
	  	  iUseSV[iIndex] = 1;
	  	  iNumberValidEphemeris++;
//	  	  fprintf(stdout,"SV%d ",iIndex);
	  	} else
	  	{
	  	  iUseSV[iIndex] = 0;
	  	}
	  }

	  /* loop through measurements and see
	     if we have valid ephem for each */

	  memcpy(&GPSRangeData.gtTimeStamp,&shm->CurrentRANGE.gtTimeStamp,sizeof(GPSTime));
	  iNumberValidObservations = 0;

	  for(iIndex=0;iIndex<shm->CurrentRANGE.lNumberObservations;iIndex++)
	  {
	  	if(iUseSV[shm->CurrentRANGE.usPRN[iIndex]] == 1)
	  	{
	  		ulSignalType = shm->CurrentRANGE.ulTrackingStatus[iIndex] & 0x3E00000;

	  		//  check that we're only getting hte L1 signal
	  		if (ulSignalType == 0)
	  		{
	  			// use current range data for now
	  			memcpy(&GPSRangeData.usPRN[iNumberValidObservations],
				        &shm->CurrentRANGE.usPRN[iIndex],sizeof(unsigned short));
				memcpy(&GPSRangeData.dPseudorange[iNumberValidObservations],
					&shm->CurrentRANGE.dPseudorange[iIndex], sizeof(double));

					iNumberValidObservations++;

	  		} else
	  		{

				// not an L1 obs
				//fprintf(stdout,"Value of Signal Type for PRN%02d was 0x%0x\n",
				//	shm->CurrentRANGE.usPRN[iIndex],
				//	ulSignalType);
	  		}
	  	} else
	  	{
	  	  fprintf(stdout,"NE: %d; ",shm->CurrentRANGE.usPRN[iIndex]);
	  	}
	  }

	  fprintf(stdout,"Obs: %d; \n",iNumberValidObservations);
	  GPSRangeData.lNumberObservations = iNumberValidObservations;

	  iNumberMeasurements = GPSRangeData.lNumberObservations;

	  if(iNumberMeasurements < 4)
	  {
		fprintf(stdout,"Less than 4 measurements\n");
		continue;
	  }

	/*
		  if(iNumberMeasurements > 9)
		  {
			 iNumberMeasurements = 9;
		  }

		  if(iNumberMeasurements < 9)
		  {
			err("Less than 9 measurements!");
		  }
	*/
		G = gsl_matrix_alloc(iNumberMeasurements,iNumberStates);
		G_LTP = gsl_matrix_alloc(iNumberMeasurements,iNumberStates);
		y = gsl_vector_alloc(iNumberMeasurements);
		delta_y = gsl_vector_alloc(iNumberMeasurements);
		W = gsl_vector_alloc(iNumberMeasurements);
		gsl_workspace = gsl_multifit_linear_alloc (iNumberMeasurements,iNumberStates);

		/* calculate G-matrix */
		gsl_matrix_set_zero (G);
		//gsl_matrix_fprintf(stdout,G,"%5.3f");

		/* calculate W-matrix */
		gsl_vector_set_all(W,1.0);

	} else
	{
		log("No Shared Memory");

	}

		for(iIteration=0;iIteration<20;iIteration++)
		{
		//	fprintf(stdout,"\n===== ITERATION %d =====\n",iIteration+1);

			user_x = gsl_vector_get(x,0);
			user_y = gsl_vector_get(x,1);
			user_z = gsl_vector_get(x,2);
			user_t = gsl_vector_get(x,3);

			UserPos[0] = user_x;
			UserPos[1] = user_y;
			UserPos[2] = user_z;
			UserPos[3] = user_t;

			ION_Alpha[0] = shm->CurrentIONUTC.a0;
			ION_Alpha[1] = shm->CurrentIONUTC.a1;
			ION_Alpha[2] = shm->CurrentIONUTC.a2;
			ION_Alpha[3] = shm->CurrentIONUTC.a3;

			ION_Beta[0] = shm->CurrentIONUTC.b0;
			ION_Beta[1] = shm->CurrentIONUTC.b1;
			ION_Beta[2] = shm->CurrentIONUTC.b2;
			ION_Beta[3] = shm->CurrentIONUTC.b3;

	//		fprintf(stdout,"User: [%+10.3f\t%+10.3f\t%+10.3f\t%+10.3f]\n",user_x,user_y,user_z,user_t);

			for (iIndex=0;iIndex<iNumberMeasurements;iIndex++)
			{

				dTransmissionTime = (double)gtObservationTime.fGPSSecondOfWeek - GPSRangeData.dPseudorange[iIndex]/GPS_SPEEDOFLIGHT;

				GPSOrbitPropagator(dTransmissionTime,
						GPSRangeData.usPRN[iIndex],
						&GPSEphemData[GPSRangeData.usPRN[iIndex]],
						7500.0,
						SVPos_calc);

				usPRN = GPSRangeData.usPRN[iIndex];

				sat_x = SVPos_calc[0];
				sat_y = SVPos_calc[1];
				sat_z = SVPos_calc[2];
				sat_t = SVPos_calc[3]*GPS_SPEEDOFLIGHT;

				// find the range
				//gsl_vector_memcpy(temp1,SVPos[usPRN]);
				//gsl_vector_sub(temp1,x);
				gsl_vector_set(temp1,0,sat_x - user_x);
				gsl_vector_set(temp1,1,sat_y - user_y);
				gsl_vector_set(temp1,2,sat_z - user_z);

				range = gsl_blas_dnrm2(temp1);

				gsl_matrix_set(G,iIndex,0,-(sat_x - user_x)/range);
				gsl_matrix_set(G,iIndex,1,-(sat_y - user_y)/range);
				gsl_matrix_set(G,iIndex,2,-(sat_z - user_z)/range);
				gsl_matrix_set(G,iIndex,3,1.0);

				gsl_vector_set(y,iIndex,GPSRangeData.dPseudorange[iIndex]);

				// calculate earth rotation correction
				delta_pr_omegaedot = -(GPS_OMEGAEDOT/GPS_SPEEDOFLIGHT) * (sat_x*user_y - sat_y*user_x);

				// calculate iono correction
				ICD200_IonoModel(gtObservationTime,
						UserPos,
						SVPos_calc,
						ION_Alpha,
						ION_Beta,
						&IonoDelay[iIndex],
						&SV_Azimuth[iIndex],
						&SV_Elevation[iIndex]);

				// examine sv elevations - if less than cutoff, set corresponding row of 'G' to zeros
				if (SV_Elevation[iIndex] < ELEVATION_CUTOFF_RAD)
				{
					gsl_matrix_set(G,iIndex,0,0.0);
					gsl_matrix_set(G,iIndex,1,0.0);
					gsl_matrix_set(G,iIndex,2,0.0);
					gsl_matrix_set(G,iIndex,3,0.0);
					fprintf(stdout,"Ignoring PRN%02d due low elevation (%3.1fdeg)\n",
						GPSRangeData.usPRN[iIndex],SV_Elevation[iIndex]*RAD2DEG);

					TropoDelay[iIndex] = 0.0;
					gsl_vector_set(delta_y,iIndex,0.0);
				} else
				{

					TropoDelay[iIndex] = TropoDelayModel(SV_Elevation[iIndex],llh[2]);

					gsl_vector_set(delta_y,iIndex,GPSRangeData.dPseudorange[iIndex]-range-(user_t-sat_t)+delta_pr_omegaedot-IonoDelay[iIndex]-TropoDelay[iIndex]);
				}
	//			fprintf(stdout,"Sat %d:\t[%+10.3f\t%+10.3f\t%+10.3f\t%+10.3f] range=%f, delta_pr=%f\n",usPRN,sat_x,sat_y,sat_z,sat_t,range,delta_pr_omegaedot);
	//			fprintf(stdout,"Sat %d:\t[%+10.3f\t%+10.3f\t%+10.3f\t%+10.3f]\n",usPRN,sat_x,sat_y,sat_z,sat_t);

			}

	//		fprintf(stdout,"delta_y: \n");
	//		gsl_vector_fprintf(stdout,delta_y,"%10.3f");

//			fprintf(stdout,"G-Matrix: \n");
//			for (iIndex=0;iIndex<iNumberMeasurements;iIndex++)
//			{
//				fprintf(stdout,"[%+10.3f\t%+10.3f\t%+10.3f\t%+10.3f]\n",
//					gsl_matrix_get(G,iIndex,0),
//					gsl_matrix_get(G,iIndex,1),
//					gsl_matrix_get(G,iIndex,2),
//					gsl_matrix_get(G,iIndex,3));
//
//			}

			/* weight measurements according to elevation */
//			for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
//			{
//			  gsl_vector_set(W,iIndex,SV_Elevation[iIndex]);
//			}

			//gsl_vector_fprintf(stdout,W,"%f");

			iReturnValue = gsl_multifit_wlinear(G,
							W,
							delta_y,
							delta_x,
							SolutionCovariance,
							&dSSE,
							gsl_workspace);

			/*iReturnValue = gsl_multifit_linear(G,
							delta_y,
							delta_x,
							SolutionCovariance,
							&dSSE,
							gsl_workspace);
			*/
			gsl_vector_add(x,delta_x);

		//	fprintf(stdout,"Least-squares fit returned value: %d; RSS=%f\n",iReturnValue,sqrt(dSSE));
		//	fprintf(stdout,"delta_x: \n");
		//	gsl_vector_fprintf(stdout,delta_x,"%20.12f");

			gsl_vector_memcpy(x_error,x);
			gsl_vector_sub(x_error,x_true);

			/* test if we should break out */
			limit = gsl_blas_dnrm2(delta_x);
			if (limit < 1e-8)
			{
				fprintf(stdout,"%f:Least-Squares solved in %d iterations; RSS=%f\n",
					gtObservationTime.fGPSSecondOfWeek,iIteration+1,sqrt(dSSE));

				/* convert to LLH */
				ecef[0] = gsl_vector_get(x,0);
				ecef[1] = gsl_vector_get(x,1);
				ecef[2] = gsl_vector_get(x,2);

				WGS84_ECEF2LLH(ecef, llh);
				T_ECEF2ENU(llh[0], llh[1], (double **)TECEF2ENU);

	//			fprintf(stdout,"[%f %f %f]\n",TECEF2ENU[0][0],TECEF2ENU[0][1],TECEF2ENU[0][2]);
	//			fprintf(stdout,"[%f %f %f]\n",TECEF2ENU[1][0],TECEF2ENU[1][1],TECEF2ENU[1][2]);
	//			fprintf(stdout,"[%f %f %f]\n",TECEF2ENU[2][0],TECEF2ENU[2][1],TECEF2ENU[2][2]);

				ECEFError[0] = gsl_vector_get(x_error,0);
				ECEFError[1] = gsl_vector_get(x_error,1);
				ECEFError[2] = gsl_vector_get(x_error,2);

				MatMul331(TECEF2ENU, ECEFError, NEDError);

				/* must swap ENU to NED */
				dTemp = NEDError[0];
				NEDError[0] = NEDError[1];
				NEDError[1] = dTemp;
				NEDError[2] = -NEDError[2];

				//fprintf(stdout,"X error: \n");
				//gsl_vector_fprintf(stdout,x_error,"%10.5f m");

				//fprintf(stdout,"X: \n");
				//gsl_vector_fprintf(stdout,x,"%10.5f m");

				/* calculate DOPS */
				for(iRow=0;iRow<3;iRow++)
				{
				  for(iCol=0;iCol<3;iCol++)
				  {
					// note that TECEF2ENU must be transposed
					gsl_matrix_set(T_ECEF2NED_44,
						iRow,iCol,TECEF2ENU[iCol][iRow]);
				  }
				  gsl_matrix_set(T_ECEF2NED_44,iRow,3,0.0);
				}
				for(iCol=0;iCol<3;iCol++)
				{
				  gsl_matrix_set(T_ECEF2NED_44,3,iCol,0.0);
				}
				gsl_matrix_set(T_ECEF2NED_44,3,3,1.0);

				gsl_matrix_set_zero(G_LTP);
				gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,
					1.0, G, T_ECEF2NED_44, 1.0, G_LTP);

				CalculateDOP(G_LTP,DOPS);
				GDOP = DOPS[0];
				PDOP = DOPS[1];

				/* write results to shared memory */
//				memcpy(&shm->dDOPS[0],&DOPS[0],sizeof(double));
//				memcpy(shm->dLLH_LSQ,llh,sizeof(double)*3);
//				memcpy(shm->dECEF_LSQ,ecef,sizeof(double)*3);
//				shm->dRxClock_LSQ = gsl_vector_get(x,3);

				fprintf(stdout,"Lat: %f, Long: %f, Height: %f\n",
					llh[0]*RAD2DEG,llh[1]*RAD2DEG,llh[2]);
				fprintf(stdout,"GDOP=%f, PDOP=%f, HDOP=%f, VDOP=%f, TDOP=%f\n",
					GDOP,PDOP,DOPS[2],DOPS[3],DOPS[4]);
				fprintf(stdout,"NED Error: %fm %fm %fm\n",NEDError[0],
					NEDError[1],
					NEDError[2]);

				fprintf(stdout,"SV: ");
				for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
				{
				  fprintf(stdout,"%d ",GPSRangeData.usPRN[iIndex]);
				}
				fprintf(stdout,"\n");

				fprintf(stdout,"Iono: ");
				for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
				{
				  fprintf(stdout,"%3.1fm ",IonoDelay[iIndex]);
				}
				fprintf(stdout,"\n");

				fprintf(stdout,"Tropo: ");
				for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
				{
				  fprintf(stdout,"%3.1fm ",TropoDelay[iIndex]);
				}
				fprintf(stdout,"\n");

				fprintf(stdout,"Elevation: ");
				for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
				{
				  fprintf(stdout,"%3.1f ",SV_Elevation[iIndex]*RAD2DEG);
				}
				fprintf(stdout,"\n");
				fprintf(stdout,"Azimuth: ");
				for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
				{
				  fprintf(stdout,"%3.1f ",SV_Azimuth[iIndex]*RAD2DEG);
				}
				fprintf(stdout,"\n");

				// check if we should log to disk
				if(options.iLogResults == 1 && fpLogFile != NULL)
				{

					fprintf(fpLogFile, "%d,%f,%d,%lf,%d,%lf,%lf,%lf,%.10lf,%.10lf,%lf,%f,%f,%f,%f,%f\n",
						gtObservationTime.usGPSWeek,
						gtObservationTime.fGPSSecondOfWeek,
						iIteration+1,
						sqrt(dSSE),
						iNumberMeasurements,
						ecef[0],
						ecef[1],
						ecef[2],
						llh[0],
						llh[1],
						llh[2],
						DOPS[0],
						DOPS[1],
						DOPS[2],
						DOPS[3],
						DOPS[4]
					);
					fflush(fpLogFile);
				}

				break;

			}

		}

		if (iIteration == 20)
		{
			fprintf(stderr,"Iterated 20 times with no solution (limit=%f)", limit);
		}

		gsl_matrix_free(G);
		gsl_matrix_free(G_LTP);
		gsl_vector_free(W);
		gsl_vector_free(y);
		gsl_vector_free(delta_y);
		gsl_multifit_linear_free(gsl_workspace);
	}

	fclose(fpLogFile);

	fprintf(stdout,"Freeing Matrices\n");

	gsl_vector_free(x);
	gsl_vector_free(delta_x);
	gsl_vector_free(x_true);
	gsl_vector_free(x_error);

	gsl_vector_free(temp1);
	gsl_matrix_free(SolutionCovariance);

	gsl_matrix_free(T_ECEF2NED_44);

	for(iIndex=0;iIndex<MAX_SV;iIndex++)
	{
		gsl_vector_free(SVPos[iIndex]);
	}

	return 0;
}

void CalculateDOP(gsl_matrix *G, double *DOP)
{
	gsl_matrix *GT;
	gsl_matrix *tempNN;
	gsl_matrix *V;
	gsl_vector *S;
	gsl_vector *Sinv;
	gsl_matrix *SinvMat;
	gsl_matrix *A;  // = inv(GT*G);
	gsl_vector *SVDWork;
	gsl_matrix *U;

	int iReturnValue,iIndex;
	int iNumberMeasurements,iNumberStates;

	double var_x,var_y,var_z,var_dt;

	iNumberMeasurements = G->size1;
	iNumberStates = G->size2;

	GT = gsl_matrix_alloc(iNumberStates,iNumberMeasurements);
	tempNN = gsl_matrix_alloc(iNumberStates,iNumberStates);
	V = gsl_matrix_alloc(iNumberStates,iNumberStates);
	S = gsl_vector_alloc(iNumberStates);
	Sinv = gsl_vector_alloc(iNumberStates);
	SinvMat = gsl_matrix_alloc(iNumberStates,iNumberStates);
	A = gsl_matrix_alloc(iNumberStates,iNumberStates);
	SVDWork = gsl_vector_alloc(iNumberStates);
	U = gsl_matrix_alloc(iNumberStates,iNumberStates);

	// calculate DOP
	gsl_matrix_transpose_memcpy(GT,G);
	gsl_matrix_set_zero(tempNN);
	gsl_blas_dgemm (CblasNoTrans,CblasNoTrans, 1.0, GT, G, 1.0, tempNN);

	// take the SVD of GT*T (stored in TempNN) to find inverse - tempNN now contains 'U'
	iReturnValue =  gsl_linalg_SV_decomp (tempNN,V,S,SVDWork);

	// get element wise inverse of S
	gsl_vector_set_all(Sinv,1.0);
	gsl_vector_div(Sinv, S);

	gsl_matrix_set_zero (SinvMat);
	for(iIndex=0;iIndex<iNumberStates;iIndex++)
	{
		gsl_matrix_set(SinvMat,iIndex,iIndex,gsl_vector_get(Sinv,iIndex));
	}

	// inv(S)*UT
	gsl_matrix_memcpy(U,tempNN);
	gsl_matrix_set_zero (tempNN);
	gsl_blas_dgemm(CblasNoTrans,CblasTrans, 1.0, SinvMat, U, 1.0, tempNN);

	// A = V * inv(S) * UT
	gsl_matrix_set_zero(A);
	gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, V, tempNN, 1.0, A);

	var_x = gsl_matrix_get(A,0,0);
	var_y = gsl_matrix_get(A,1,1);
	var_z = gsl_matrix_get(A,2,2);
	var_dt = gsl_matrix_get(A,3,3);

	DOP[0] = sqrt(var_x+var_y+var_z+var_dt);
	DOP[1] = sqrt(var_x+var_y+var_z);
	DOP[2] = sqrt(var_x+var_y);
	DOP[3] = sqrt(var_z);
	DOP[4] = sqrt(var_dt);

	gsl_matrix_free(GT);
	gsl_matrix_free(tempNN);

	gsl_matrix_free(A);
	gsl_matrix_free(V);
	gsl_vector_free(S);
	gsl_vector_free(Sinv);
	gsl_matrix_free(U);
	gsl_vector_free(SVDWork);
	gsl_matrix_free(SinvMat);

}

ext: c

max_stars_repo_name: duncang/gpsins

max_issues_repo_name: duncang/gpsins

max_forks_repo_name: duncang/gpsins


text:
// Copyright (c) 2005  Stanford University (USA).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org); you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public License as
// published by the Free Software Foundation; either version 3 of the License,
// or (at your option) any later version.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// $URL$
// $Id$
//
//
// Author(s)     : Daniel Russel <drussel@alumni.princeton.edu>

#ifndef CGAL_POLYNOMIAL_INTERNAL_GSL_NUMERIC_SOLVER_H
#define CGAL_POLYNOMIAL_INTERNAL_GSL_NUMERIC_SOLVER_H
#include <CGAL/Polynomial/basic.h>
#include <CGAL/Polynomial/internal/numeric_solvers_support.h>
#include <vector>

#ifdef CGAL_POLYNOMIAL_USE_GSL

#include <gsl/gsl_poly.h>
#include <gsl/gsl_errno.h>

namespace CGAL { namespace POLYNOMIAL { namespace internal {

/*template <bool CLEAN>
inline double gsl_max_error()
{
    if (CLEAN) return .0000005;
    else return 0;
    }*/

template <bool CLEAN>
inline void gsl_compute_roots(const double *begin, const double *end,
double lb, double ub,
std::vector<double> &roots_)
{

  //double max_error=gsl_max_error<CLEAN>();

    gsl_poly_complex_workspace *workspace;
    int degree= end-begin-1;
    workspace= gsl_poly_complex_workspace_alloc(degree+1);

//! I can't use auto_ptr because it is an array
    double *roots= new double[2*degree];

/*for ( int i=0; i< 2*fn.degree(); ++i){
  roots[i]=0;
  }*/

    int ret = gsl_poly_complex_solve(begin, degree+1, workspace,
        roots);
    roots_.reserve(ret);

    if (ret!=GSL_SUCCESS) {
        std::cerr << "GSL solver did not converge on ";
        std::copy(begin, end, std::ostream_iterator<double>(std::cerr, " "));
        std::cerr << std::endl;
    }

    double last= -std::numeric_limits<double>::infinity();
    for ( int i=degree-1; i>=0; --i) {
        double r= roots[2*i];
        double c= roots[2*i+1];
        if (root_is_good(r, c, lb, ub)) {
            roots_.push_back(r);
        } else if (CLEAN && root_is_good(r,c, last, ub)) {
	  last= r;
	}
    }

    std::sort(roots_.begin(), roots_.end(), std::greater<double>() );
    delete roots;

    gsl_poly_complex_workspace_free(workspace);

    if (CLEAN) filter_solver_roots(begin,end, lb, ub, last, roots_);
    return;
}

template <bool CLEAN>
inline void gsl_compute_cubic_roots(const double *begin, const double *end,
double lb, double ub,
std::vector<double> &roots_)
{
    CGAL_Polynomial_precondition(begin[3] != 0);
    //double max_error=gsl_max_error<CLEAN>();

    double r[3];
    int num_roots= gsl_poly_solve_cubic(begin[2]/begin[3],
        begin[1]/begin[3],
        begin[0]/begin[3], &r[0],&r[1],&r[2]);
    roots_.reserve(num_roots);
    double last= -std::numeric_limits<double>::infinity();
// I want reverse sorted roots
    for (int i=num_roots-1; i>=0; --i) {
        if (r[i]>  lb && r[i] < ub) roots_.push_back(r[i]);
	else if (CLEAN && r[i] <lb && r[i] > last){
	  last= r[i];
	}
    }
    if (CLEAN) filter_solver_roots(begin, end, lb, ub, last, roots_);
}

inline void gsl_polynomial_compute_roots(const double *begin, const double *end,
double lb, double ub,
std::vector<double> &roots)
{
    int degree= end-begin-1;
    switch( degree) {
        case -1:
        case 0:
            break;
        case 1:
            compute_linear_roots(begin,end, lb, ub, roots);
            break;
        case 2:
            compute_quadratic_roots(begin, end, lb, ub, roots);
            break;
        case 3:
            gsl_compute_cubic_roots<false>(begin, end, lb, ub, roots);
            break;
        default:
            gsl_compute_roots<false>(begin, end, lb, ub, roots);
    }
}

inline void gsl_polynomial_compute_cleaned_roots(const double *begin, const double *end,
double lb, double ub,
std::vector<double> &roots)
{
    int degree= end-begin-1;
    switch( degree) {
        case -1:
        case 0:
            break;
        case 1:
            compute_linear_cleaned_roots(begin,end, lb, ub, roots);
            break;
        case 2:
            compute_quadratic_cleaned_roots(begin, end, lb, ub, roots);
            break;
        case 3:
            gsl_compute_cubic_roots<true>(begin, end, lb, ub, roots);
            break;
        default:
            gsl_compute_roots<true>(begin, end, lb, ub, roots);
    }
}

inline double gsl_evaluate_polynomial(const double *b, const double *e, double t)
{
    if (b==e) return 0;
/*double *d= new double[coefs.size()];
for (unsigned int i=0; i< coefs.size(); ++i){
  d[i]= coefs[i];
  }*/
    double v= gsl_poly_eval(b, e-b, t);
    return v;
}

/*

inline void gsl_polynomial_compute_roots(const double *begin, const double *end,
                  double lb, double ub,
                  std::vector<double> &roots){
 CGAL_assertion(0&& no_gsl_support_built);
}

inline void gsl_polynomial_compute_cleaned_roots(const double *begin, const double *end,
                  double lb, double ub,
                  std::vector<double> &roots){
CGAL_assertion(0&& no_gsl_support_built);
}

inline double gsl_evaluate_polynomial(const double *b, const double *e, double t) {
CGAL_assertion(0&& no_gsl_support_built);
return std::numeric_limits<double>::nan();
}
*/

/*// GSL
void gsl_polynomial_compute_roots(const double *begin, const double *end,
                  double lb, double ub, std::vector<double> &roots);

void gsl_polynomial_compute_cleaned_roots(const double *begin, const double *end,
double lb, double ub, std::vector<double> &roots);*/

struct GSL_numeric_solver
{
    void operator()(const double *begin, const double *end,
        double lb, double ub,
        std::vector<double> &roots) const
    {
        gsl_polynomial_compute_roots(begin, end, lb, ub, roots);
    }
};

struct GSL_cleaned_numeric_solver
{
    void operator()(const double *begin, const double *end,
        double lb, double ub,
        std::vector<double> &roots) const
    {
        gsl_polynomial_compute_cleaned_roots(begin, end, lb, ub, roots);
    }
};

} } } //namespace CGAL::POLYNOMIAL::internal

#endif // CGAL_POLYNOMIAL_USE_GSL

#endif

max_stars_repo_name: vipuserr/vipuserr-Geological-hazard


text:
/*
 This file forms part of hpGEM. This package has been developed over a number of
 years by various people at the University of Twente and a full list of
 contributors can be found at http://hpgem.org/about-the-code/team

 This code is distributed using BSD 3-Clause License. A copy of which can found
 below.

 Copyright (c) 2020, University of Twente
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:

 1. Redistributions of source code must retain the above copyright notice, this
 list of conditions and the following disclaimer.

 2. Redistributions in binary form must reproduce the above copyright notice,
 this list of conditions and the following disclaimer in the documentation
 and/or other materials provided with the distribution.

 3. Neither the name of the copyright holder nor the names of its contributors
 may be used to endorse or promote products derived from this software without
 specific prior written permission.

 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
#ifndef HPGEM_MATRIXBLOCKS_H
#define HPGEM_MATRIXBLOCKS_H

#include <vector>
#include <petsc.h>

#include "LinearAlgebra/MiddleSizeMatrix.h"

namespace DGMax {

using namespace hpgem;

/// A block from the global matrix, or a pair of blocks that are placed
/// symmetrically.
class MatrixBlocks {
   public:
    MatrixBlocks(std::vector<PetscInt> rowIndices,
                 std::vector<PetscInt> columnIndices,
                 LinearAlgebra::MiddleSizeMatrix block)
        : rowIndices_(std::move(rowIndices)),
          columnIndices_(std::move(columnIndices)),
          block1_(std::move(block)),
          pair_(false){};

    MatrixBlocks(std::vector<PetscInt> rowIndices,
                 std::vector<PetscInt> columnIndices,
                 LinearAlgebra::MiddleSizeMatrix block1,
                 LinearAlgebra::MiddleSizeMatrix block2)
        : rowIndices_(std::move(rowIndices)),
          columnIndices_(std::move(columnIndices)),
          block1_(std::move(block1)),
          block2_(std::move(block2)),
          pair_(true){};

    /// Number of entries in the matrix block. If this is pair, then this gives
    /// the size of a single block.
    std::size_t getBlockSize() const {
        return rowIndices_.size() * columnIndices_.size();
    }

    /// Whether this is a pair of blocks or a single block.
    bool isPair() const { return pair_; }

    /// \brief Load blocks into temporary storage for insertion.
    ///
    /// This stores the raw entries of the block in the storage space for use
    /// with insertBlocks(). The first getBlockSize() entries of the storage are
    /// used for storing the first matrix block. If this object is a pair then
    /// the subsequent getBlockSize() entries will be used for the second block.
    ///
    /// \param storage The storage to use, will be resized if necessary.
    void loadBlocks(std::vector<PetscScalar>& storage) const {
        // Make sure we have enough space
        std::size_t storageSize = getBlockSize();
        std::size_t storageMultiplier = pair_ ? 2 : 1;
        if (storage.size() < storageMultiplier * storageSize) {
            storage.resize(storageMultiplier * storageSize);
        }
        // Copy data
        for (std::size_t i = 0; i < storageSize; ++i) {
            storage[i] = block1_[i];
        }
        if (pair_) {
            for (std::size_t i = 0; i < storageSize; ++i) {
                storage[i + storageSize] = block2_[i];
            }
        }
    }

    /// \brief Insert the block or blocks in a PetscMatrix.
    void insertBlocks(std::vector<PetscScalar>& storage, Mat mat) const {
        insertBlock(storage, false, mat);
        if (pair_) {
            insertBlock(storage, true, mat);
        }
    }

   private:
    /// The global indices for the rows
    std::vector<PetscInt> rowIndices_;
    /// The global indices for the columns
    std::vector<PetscInt> columnIndices_;

    /// The primary block
    LinearAlgebra::MiddleSizeMatrix block1_;
    /// The symmetrically placed block
    LinearAlgebra::MiddleSizeMatrix block2_;

    /// Whether this is a single block or a symmetrically placed pair.
    bool pair_;

    /// Helper function inserting a single block.
    void insertBlock(std::vector<PetscScalar>& storage, bool hermitianPart,
                     Mat mat) const;

    /// Validate the correctnees
    void validate() const;
};

}  // namespace DGMax

#endif  // HPGEM_MATRIXBLOCKS_H

max_stars_repo_name: hpgem/hpgem

max_issues_repo_name: hpgem/hpgem

max_forks_repo_name: hpgem/hpgem


text:
#ifndef __Rand_H__
#define __Rand_H__

#define RAND Rand::Instance()

#include <gsl/gsl_rng.h>                                    // GSL random number generator
#include <gsl/gsl_randist.h>

/*
 * Rand Singleton - interface to GSL rng (random number generator)
 *
 * Singletons and global variables are sometimes frowned-upon, but doing it this
 * way means the objects don't need to be passed around to all and sundry.
 * I think convenience and clarity sometimes trump dogma.
 */

class Rand {

private:

    Rand() { m_Rng = NULL; };
    Rand(Rand const&) = delete;
    Rand& operator = (Rand const&) = delete;

    static Rand*      m_Instance;

    gsl_rng*          m_Rng;                                                                           // GSL random number generator

    unsigned long int m_Seed;

public:

    static Rand*  Instance();

    void          Initialise();
    void          Free();

    unsigned long int CurrentSeed()                    { return m_Seed; }
    unsigned long int DefaultSeed()                    { return gsl_rng_default_seed; }
    unsigned long int Seed(const unsigned long p_Seed) { gsl_rng_set(m_Rng, p_Seed); m_Seed = p_Seed; return p_Seed; }

    double        Random();
    double        Random(const double p_Lower, const double p_Upper);
    int           RandomInt(const int p_Lower, const int p_Upper);
    int           RandomInt(const int p_Upper) { return p_Upper < 0 ? 0 : RandomInt(0, p_Upper); }
    double        RandomGaussian(const double p_Sigma);
};

#endif // __Rand_H__


text:
/* -----------------------------------------------------------------------------
  JAM_AXI_VEL_LOSINT

    Calculates integrand for line-of-sight integral required for first moment
    calculation.

    INPUTS
      zp     : line-of-sight coordinate z' (integration variable)
      params : function parameters passed as a structure

    NOTES
      * Based on janis1_jeans_mge_los_integrand IDL code by Michele Cappellari.

  Laura L Watkins [lauralwatkins@gmail.com]

  This code is released under a BSD 2-clause license.
  If you use this code for your research, please cite:
  Watkins et al. 2013, MNRAS, 436, 2598
  "Discrete dynamical models of omega Centauri"
  http://adsabs.harvard.edu/abs/2013MNRAS.436.2598W
----------------------------------------------------------------------------- */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include "jam.h"
#include "../mge/mge.h"

double jam_axi_vel_losint(double zp, void *params) {

    struct params_losint *lp;
    struct params_mgeint mp;
    double xp, yp, si, ci, r, z, r2, z2, nu, intg, result, error, nu_i;
    double sign_kappa, sum;
    int i;

    // get parameters
    lp = params;
    xp = lp->xp;
    yp = lp->yp;

    // if the integrationFlag is already set, do not proceed
    if (*lp->integrationFlag!=0) {
        return 0.;
    }

    // intrinsic R and z
    si = sin(lp->incl);
    ci = cos(lp->incl);
    r = sqrt(pow(zp*si - yp*ci, 2) + pow(xp, 2));                   // eqn 25
    z = sqrt(pow(zp*ci + yp*si, 2));

    // do some prep for the integrand to avoid repeat calculations
    r2 = r * r;
    z2 = z * z;

    // parameters for integrand function
    mp.r2 = r2;
    mp.z2 = z2;
    mp.pot = lp->pot;
    mp.s2p = lp->s2p;
    mp.e2p = lp->e2p;

    // perform integration
    gsl_integration_workspace *w = gsl_integration_workspace_alloc(1000);
    gsl_set_error_handler_off();
    gsl_function F;
    F.function = &jam_axi_vel_mgeint;

    sum = 0.;
    for (i=0; i<lp->lum->ntotal; i++) {
        if (lp->kappa[i]==0.) sign_kappa = 0.;
        else sign_kappa = lp->kappa[i]/fabs(lp->kappa[i]);
        nu_i = lp->lum->area[i] * exp(-0.5/lp->s2l[i]*(r2+z2/lp->q2l[i]));

        mp.bani = lp->bani[i];
        mp.s2l = lp->s2l[i];
        mp.q2l = lp->q2l[i];
        mp.s2q2l = lp->s2q2l[i];
        F.params = ∓
        *lp->integrationFlag += gsl_integration_qag(&F, 0., 1., 0., 1e-5,
            1000, 6, w, &result, &error);
        sum += sign_kappa * pow(lp->kappa[i], 2) * nu_i * fabs(result);
    }

    gsl_integration_workspace_free(w);

    // check if the integration failed
    if (*lp->integrationFlag!=0) {
        return 0.;
    }

    // mge volume density
    nu = mge_dens(lp->lum, r, z);

    // keep track of kappa signs - see note 8 p77 of Cappellari 2008
    intg = nu*sum/fabs(nu*sum)*sqrt(fabs(nu*sum));

    intg *= pow(zp, lp->zpow);

    return intg;

}

max_stars_repo_name: slzoutendijk/cjam

max_issues_repo_name: slzoutendijk/cjam

max_forks_repo_name: slzoutendijk/cjam


text:
	#ifndef LVV_MULTIMIN_H
	#define LVV_MULTIMIN_H

#include <stdlib.h>
#include <iostream>
using std::cerr;
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_multimin.h>
#include <lvv/lvv.h>
#include <lvv/math.h>

namespace lvv {

class f_minimizer {  public:

	f_minimizer (const int N, double F(const gsl_vector*, void *), double *xa,  double *ssa, void *var, bool _trace=false)
	:	T (gsl_multimin_fminimizer_nmsimplex), trace(_trace), found(false), fmin(999999)
	{
		minex_func.n = N;
		minex_func.f = F;
		minex_func.params = var;

		mk_gsl_vector x (xa,N);// this is auto on stack(temp), will be copied in minimizer, //  so we are fine when it will go out of scope
		mk_gsl_vector ss(ssa, N);
		//gv_x  = gsl_vector_alloc(N);	for (int i=0; i<N; i++)  gsl_vector_set(gv_x,  i, xa [i]);
		//gv_ss = gsl_vector_alloc(N);	for (int i=0; i<N; i++)  gsl_vector_set(gv_ss, i, ssa[i]);

		minimizer = gsl_multimin_fminimizer_alloc(T, N);
		  gsl_multimin_fminimizer_set(minimizer, &minex_func, &x  , &ss);
		//gsl_multimin_fminimizer_set(minimizer, &minex_func, gv_x, gv_ss);
		xmin = minimizer->x->data;
		if (trace)   cout << "#  minimizer: "  <<  gsl_multimin_fminimizer_name (minimizer)  <<  endl;
	};

	~f_minimizer () { gsl_multimin_fminimizer_free(minimizer);  /* gsl_vector_free(gv_x);   gsl_vector_free(gv_ss) ;*/ };

	void   find_min (double characteristic_size, int max_iter) {
		int	test_status=GSL_CONTINUE;			// test_status:  GSL_SUCCESS==0; GSL_CONTINUE==-2;

		if (trace)  FMT("# Itr  %10t Y   %20t Step  %30t X[0..]\n");

		for  ( iter=0;  iter<max_iter && (test_status==GSL_CONTINUE);   ++iter )   {

			int  iterate_status = gsl_multimin_fminimizer_iterate(minimizer);

			if (iterate_status) {
				cerr << "error: FMinimizer: in gsl_multimin_fminimizer_iterate()\n";
				break;
			}

			double size = gsl_multimin_fminimizer_size(minimizer);
			test_status = gsl_multimin_test_size(size, characteristic_size);

			if (test_status == GSL_SUCCESS)  {
				found=true;
				fmin=minimizer->fval;
				if (trace )    cout  << "converged to minimum at\n";
			}

			if (trace) {
				FMT("%5d %10.5d %10.5d") %iter   %(minimizer->fval)  %size;
				for (unsigned i=0; i < minimizer->x->size; ++i) FMT("%10.5d") %gsl_vector_get(minimizer->x, i);
				cout << endl;
			}

		}
	}

	const gsl_multimin_fminimizer_type *T;
	gsl_multimin_fminimizer            *minimizer;
	gsl_multimin_function               minex_func;
	bool                                trace;
	bool                                found;
	double                             *xmin;
	double                              fmin;
	gsl_vector                         *gv_x;
	gsl_vector                         *gv_ss;
	int					iter;
 };
 } // namespace

    #endif // LVV_MULTIMIN_H

max_stars_repo_name: lvv/lvvlib

max_issues_repo_name: lvv/lvvlib

max_forks_repo_name: lvv/lvvlib


text:
/*
* This file contains functions used to call sequential MKL/BLAS/LAPACK routines
*
* Authors : Sebastien Cayrols
*         : Olivier Tissot
* Email   : sebastien.cayrols@[(gmail.com) | (inria.fr)]
*         : olivier.tissot@inria.fr
*/

/******************************************************************************/
/*                                  INCLUDE                                   */
/******************************************************************************/
/* STD */
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
/* MKL/LAPACK */
#ifdef LAPACKEACTIVATE
  #include <lapacke.h>
  #include <cblas.h>
#endif
/* MPI */
#include <mpi.h>
/* CPaLAMeM */
//#include "cpalamem_macro.h"
#undef TIMERACTIVATE
#include "cplm_utils.h"
#include "cplm_timing.h"
#include "cplm_matcsr.h"
#include "cplm_kernels.h"
#include "cplm_QS.h"
//#include "cpalamem_instrumentation.h"
/******************************************************************************/

/******************************************************************************/
/*                                    CODE                                    */
/******************************************************************************/

int CPLM_MatDenseKernelCholesky(CPLM_Mat_Dense_t* C_io)
{
CPLM_PUSH
CPLM_BEGIN_TIME
  int ierr          = 0;
  int matrix_layout = 0;
  int order         = 0;
  int lda           = 0;
  char uplo         = 0;

  CPLM_ASSERT(C_io       !=  NULL);
  CPLM_ASSERT(C_io->val  !=  NULL);

  matrix_layout = (C_io->info.stor_type == ROW_MAJOR) ? LAPACK_ROW_MAJOR
    : LAPACK_COL_MAJOR;
  uplo          = 'U';
  order         = C_io->info.n;
  lda           = C_io->info.lda;

  ierr = LAPACKE_dpotrf(  matrix_layout,
                          uplo,
                          order,
                          C_io->val,
                          lda);CPLM_CHKERR(ierr);
  if (ierr > 0)
  {
    CPLM_Abort("The matrix is not SPD!");
  }

CPLM_END_TIME
CPLM_POP
  return ierr;
}

//TODO Remove malloc and explain why H
int CPLM_MatDenseKernelQR( CPLM_Mat_Dense_t*  A_io,
                      CPLM_Mat_Dense_t*  H_io,
                      int           index_i,
                      int           index_j) //to be verified
{
CPLM_PUSH
CPLM_BEGIN_TIME
  int ierr          = 0;
  int matrix_layout = -1;
  double *tau = NULL;

  tau = malloc(A_io->info.N*sizeof(double));
  CPLM_ASSERT(tau       !=  NULL);

  matrix_layout = (A_io->info.stor_type == ROW_MAJOR) ? LAPACK_ROW_MAJOR
    : LAPACK_COL_MAJOR;

  ierr = LAPACKE_dgeqrf(  matrix_layout,
                          A_io->info.M,
                          A_io->info.N,
                          A_io->val,
                          A_io->info.lda,
                          tau);CPLM_CHKERR(ierr);

  if(H_io->val != NULL)//[Q] why test on val and not on H alone?
  {
    ierr = CPLM_MatDenseTriangularFillBlock( A_io,
                                        H_io,
                                        index_i,
                                        index_j,
                                        A_io->info.N);CPLM_CHKERR(ierr);
  }

  ierr = LAPACKE_dorgqr(  matrix_layout,
                          A_io->info.M,
                          A_io->info.N,
                          A_io->info.N,
                          A_io->val,
                          A_io->info.lda,
                          tau);CPLM_CHKERR(ierr);
  free(tau);
CPLM_END_TIME
CPLM_POP
  return ierr;
}

int CPLM_MatDenseKernelLowerTriangularLeftSolve(CPLM_Mat_Dense_t* L, CPLM_Mat_Dense_t* B)
{
CPLM_PUSH
CPLM_BEGIN_TIME
  int ierr          = 0;
  int matrix_layout = 0;
  int m             = 0;
  int nrhs          = 0;
  char uplo  = 'L'; // L is lower triangular
  char trans = 'N'; // No transpose, no conjugacy
  char diag  = 'N'; // No unitary

  CPLM_ASSERT(L      !=  NULL);
  CPLM_ASSERT(L->val !=  NULL);
  CPLM_ASSERT(B      !=  NULL);
  CPLM_ASSERT(B->val !=  NULL);

  matrix_layout = (L->info.stor_type == ROW_MAJOR) ? LAPACK_ROW_MAJOR
    : LAPACK_COL_MAJOR;
  m             = B->info.m;
  nrhs          = B->info.n;

  ierr = LAPACKE_dtrtrs(matrix_layout,
                        uplo,
                        trans,
                        diag,
                        m,
                        nrhs,
                        L->val,
                        L->info.lda,
                        B->val,
                        B->info.lda);CPLM_CHKERR(ierr);
CPLM_END_TIME
CPLM_POP
  return ierr;
}

int CPLM_MatDenseKernelUpperTriangularLeftSolve(CPLM_Mat_Dense_t* R, CPLM_Mat_Dense_t* B)
{
CPLM_PUSH
CPLM_BEGIN_TIME
  int ierr          = 0;
  int matrix_layout = -1;
  int m             = 0;
  int nrhs          = 0;
  char uplo  = 'U'; // R is upper triangular
  char trans = 'N'; // No transpose, no conjugacy
  char diag  = 'N'; // No unitary

  CPLM_ASSERT(R      !=  NULL);
  CPLM_ASSERT(R->val !=  NULL);
  CPLM_ASSERT(B      !=  NULL);
  CPLM_ASSERT(B->val !=  NULL);

  matrix_layout = (R->info.stor_type == ROW_MAJOR) ? LAPACK_ROW_MAJOR
    : LAPACK_COL_MAJOR;
  m             = B->info.m;
  nrhs          = B->info.n;

  ierr = LAPACKE_dtrtrs(matrix_layout,
                        uplo,
                        trans,
                        diag,
                        m,
                        nrhs,
                        R->val,
                        R->info.lda,
                        B->val,
                        B->info.lda);CPLM_CHKERR(ierr);
CPLM_END_TIME
CPLM_POP
  return ierr;
}

//#ifdef MKLACTIVATE

//B is triangular
int CPLM_MatDenseDtrmm(CPLM_Mat_Dense_t *A_in ,
                  CPLM_Mat_Dense_t *B_io ,
                  char        side  ,
                  char        uplo  ,
                  char        trans ,
                  char        diag)
{
CPLM_PUSH
  int ierr          = 0;
  CBLAS_LAYOUT matrix_layout = -1;
  CBLAS_DIAG  blasDiag = CblasNonUnit;
  CBLAS_SIDE blasSide = CblasLeft;
  CBLAS_UPLO blasUPLO = CblasUpper;
  CBLAS_TRANSPOSE blasTrans = CblasTrans;

  CPLM_ASSERT(A_in->val != NULL);
  CPLM_ASSERT(B_io->val != NULL);

  blasDiag = (diag == 'U') ? CblasUnit : CblasNonUnit;
  blasSide = (side == 'R') ? CblasRight : CblasLeft;
  blasUPLO = (uplo == 'L') ? CblasLower : CblasUpper;
  blasTrans = (trans == 'T') ? CblasTrans : CblasNoTrans;

  matrix_layout = (A_in->info.stor_type == ROW_MAJOR) ? CblasRowMajor
    : CblasColMajor;

  cblas_dtrmm(matrix_layout,
      blasSide,
      blasUPLO,
      blasTrans,
      blasDiag,
      A_in->info.m,
      A_in->info.n,
      1.0,
      A_in->val,
      A_in->info.lda,
      B_io->val,
      B_io->info.lda);
CPLM_POP
  return ierr;
}
//#else
//  #ifdef LAPACKEACTIVATE
//  int CPLM_MatDenseDtrmm(CPLM_Mat_Dense_t *A_in ,
//                    CPLM_Mat_Dense_t *B_io ,
//                    char        side  ,
//                    char        uplo  ,
//                    char        trans ,
//                    char        diag)
//  {
//  CPLM_PUSH
//    int ierr = 0;
//
//    CPLM_ASSERT(A_in->val != NULL);
//    CPLM_ASSERT(B_io->val != NULL);
//
//    if(A_in->info.stor_type == ROW_MAJOR)
//    {
//      CPLM_Abort("Do not call trmm with row major");
//    }
//
//    dtrmm(
//        side,
//        uplo,
//        trans,
//        diag,
//        A_in->info.m,
//        A_in->info.n,
//        1.0,
//        A_in->val,
//        A_in->info.lda,
//        B_io->val,
//        B_io->info.lda);
//  CPLM_POP
//    return ierr;
//  }
//  #endif
//#endif

// C=alpha A^{transa} * B^{transb} + beta C
int CPLM_MatDenseKernelMatMult(CPLM_Mat_Dense_t*      A,
                          char              ptransa,
                          CPLM_Mat_Dense_t*      B,
                          char              ptransb,
                          CPLM_Mat_Dense_t*      C,
                          double            alpha,
                          double            beta)
{
CPLM_PUSH
CPLM_BEGIN_TIME
  int ierr = 0;
  CBLAS_TRANSPOSE transa;
  CBLAS_TRANSPOSE transb;
  // Allocate memory if needed
  if (C->val == NULL)
  {
    int Crow, Ccol, CrowGlob, CcolGlob;
    CrowGlob = (ptransa == 'N') ? A->info.M : A->info.N;
    Crow     = (ptransa == 'N') ? A->info.m : A->info.n;
    CcolGlob = (ptransb == 'N') ? B->info.N : B->info.M;
    Ccol     = (ptransb == 'N') ? B->info.n : B->info.m;
    ierr = CPLM_MatDenseSetInfo(C,
                           CrowGlob,
                           CcolGlob,
                           Crow,
                           Ccol,
                           A->info.stor_type); // We use A storage type but this
                                               // is arbitrary
    CPLM_CHKERR(ierr);
    ierr = CPLM_MatDenseMalloc(C);
    CPLM_CHKERR(ierr);
  }

  transa = (ptransa == 'N') ?  CblasNoTrans : CblasTrans;
  transb = (ptransb == 'N') ?  CblasNoTrans : CblasTrans;

  // BLAS parameters
  CBLAS_LAYOUT layout = (A->info.stor_type == ROW_MAJOR) ? CblasRowMajor
    : CblasColMajor;
  int nbColOpA = (transa == CblasNoTrans) ? A->info.n : A->info.m;

  cblas_dgemm (layout,
               transa,
               transb,
               C->info.m,
               C->info.n,
               nbColOpA,
               alpha,
               A->val,
               A->info.lda,
               B->val,
               B->info.lda,
               beta,
               C->val,
               C->info.lda);CPLM_CHKERR(ierr);

CPLM_END_TIME
CPLM_POP
  return ierr;

}

int CPLM_MatDenseKernelUpperTriangularRightSolve(CPLM_Mat_Dense_t* R, CPLM_Mat_Dense_t* B)
{
CPLM_PUSH
CPLM_BEGIN_TIME
  int ierr = 0;
  CBLAS_LAYOUT matrix_layout = -1;
#ifdef MKLACTIVATE
  MKL_INT m    = 0;
  MKL_INT nrhs = 0;
#elif defined(LAPACKEACTIVATE)
  int m    = 0;
  int nrhs = 0;
#endif
  double alpha = 1e0;

  CPLM_ASSERT(R      !=  NULL);
  CPLM_ASSERT(R->val !=  NULL);
  CPLM_ASSERT(B      !=  NULL);
  CPLM_ASSERT(B->val !=  NULL);

  matrix_layout = (R->info.stor_type == ROW_MAJOR) ? CblasRowMajor
    : CblasColMajor;
  m             = B->info.m;
  nrhs          = B->info.n;

  cblas_dtrsm(matrix_layout,
              CblasRight,
              CblasUpper,
              CblasNoTrans,
              CblasNonUnit,
              m,
              nrhs,
              alpha,
              R->val,
              R->info.lda,
              B->val,
              B->info.lda);
CPLM_END_TIME
CPLM_POP
  return ierr;
}

// y = beta*y + alpha*A*x
int CPLM_MatDenseKernelMatVec(CPLM_Mat_Dense_t* A_in,
                              double*           x_in,
                              double*           y_io,
                              double            alpha,
                              double            beta)
{
CPLM_PUSH
CPLM_BEGIN_TIME
  int ierr = 0;
  CBLAS_LAYOUT layout = -1;

  CPLM_ASSERT(A_in       !=  NULL);
  CPLM_ASSERT(x_in       !=  NULL);
  CPLM_ASSERT(y_io       !=  NULL);
  CPLM_ASSERT(A_in->val  !=  NULL);

  /* if (*y_io == NULL) */
  /* { */
  /*   y_io = malloc(A_in->info.m*sizeof(double)); */
  /* } */

  // BLAS parameters
  layout = (A_in->info.stor_type == ROW_MAJOR) ? CblasRowMajor : CblasColMajor;

  cblas_dgemv(layout,
              CblasNoTrans,
              A_in->info.m,
              A_in->info.n,
              alpha,
              A_in->val,
              A_in->info.lda,
              x_in,
              1,             // increment for the elements of x
              beta,
              y_io,
              1);            // increment of the elements of y
CPLM_END_TIME
CPLM_POP
  return ierr;
}

int CPLM_MatDenseKernelSumColumns(CPLM_Mat_Dense_t* A_in, double* sumCol)
{
CPLM_PUSH
CPLM_BEGIN_TIME
  int ierr = 0;
  CPLM_ASSERT(A_in->val != NULL);
  CPLM_ASSERT(sumCol != NULL);

  double* ones = (double*) malloc(A_in->info.n*sizeof(double));
  for (int i = 0; i < A_in->info.n; ++i)
    ones[i] = 1.E0;

  ierr = CPLM_MatDenseKernelMatVec(A_in, ones, sumCol, 1.0, 0.0);CPLM_CHKERR(ierr);

  if (ones != NULL) free(ones);
CPLM_END_TIME
CPLM_POP
  return ierr;
}

#ifdef MKLACTIVATE
// C = A + B using mkl
int CPLM_MatDenseKernelMatAdd(CPLM_Mat_Dense_t* A_in,
                         CPLM_Mat_Dense_t* B_in,
                         CPLM_Mat_Dense_t* C_out,
                         double       alpha,
                         double       beta)
{
CPLM_PUSH
CPLM_BEGIN_TIME

  int ierr = 0;
  char ordering = 0;
  char transa   = 0;
  char transb   = 0;

  if(!CPLM_MatDenseIsSameLocalInfo(A_in,B_in))
  {
    CPLM_MatDensePrintfInfo("A info",A_in);
    CPLM_MatDensePrintfInfo("B info",B_in);
    CPLM_Abort("A and B do not have the same structure");
  }

  // Allocate memory if needed
  if(C_out->val == NULL)
  {
    ierr = CPLM_MatDenseInit(C_out, A_in->info);CPLM_CHKERR(ierr);
    ierr = CPLM_MatDenseMalloc(C_out);CPLM_CHKERR(ierr);
  }

  // BLAS parameters
  ordering = (A_in->info.stor_type == ROW_MAJOR) ? 'R' : 'C';
  transa = 'N';
  transb = 'N';

  mkl_domatadd (ordering,
                transa,
                transb,
                A_in->info.m,
                A_in->info.n,
                alpha,
                A_in->val,
                A_in->info.lda,
                beta,
                B_in->val,
                B_in->info.lda,
                C_out->val,
                C_out->info.lda);
CPLM_END_TIME
CPLM_POP
  return ierr;
}

int CPLM_MatCSRKernelMatDenseMult(CPLM_Mat_CSR_t    *A_in,
                             CPLM_Mat_Dense_t  *B_in,
                             CPLM_Mat_Dense_t  *C_io,
                             double       alpha,
                             double       beta)
{
CPLM_PUSH
CPLM_BEGIN_TIME
CPLM_OPEN_TIMER
  int ierr = 0;
  char matdescra[6];
  char trans  = 'N';

  CPLM_ASSERT(A_in       !=  NULL);
  CPLM_ASSERT(B_in       !=  NULL);
  CPLM_ASSERT(C_io       !=  NULL);
  CPLM_ASSERT(A_in->val  != NULL);
  CPLM_ASSERT(B_in->val  != NULL);

  matdescra[0]  = 'G';
  matdescra[1]  = 'L';//ignored if G
  matdescra[2]  = 'N';//ignored if G
  matdescra[3]  = 'C';

  if(C_io->val == NULL)
  {
    ierr = CPLM_MatDenseSetInfo(C_io,
                          A_in->info.m,
                          B_in->info.n,
                          A_in->info.m,
                          B_in->info.n,
                          B_in->info.stor_type);CPLM_CHKERR(ierr);
    ierr = CPLM_MatDenseMalloc(C_io);CPLM_CHKERR(ierr);
  }

#ifdef DEBUG
  CPLM_MatCSRPrintf2D("M1",A_in);
  CPLM_MatDensePrintf2D("* M2",B_in);
#endif
CPLM_TIC(step1, "ConvertTo1BasedIndexing")
  // If COL_MAJOR we need to use 1-base indexes (don't ask why...)
  if (B_in->info.stor_type == COL_MAJOR)
  {
    matdescra[3]  = 'F';
    CPLM_MatCSRConvertTo1BasedIndexing(A_in);
  }
CPLM_TAC(step1)
CPLM_TIC(step2, "Call SpBLAS")
  mkl_dcsrmm(&trans,
             &A_in->info.m,
             &B_in->info.n,
             &A_in->info.n,
             &alpha,
             matdescra,
             A_in->val,
             A_in->colInd,
             A_in->rowPtr,
             &(A_in->rowPtr[1]),
             B_in->val,
             &(B_in->info.lda),
             &beta,
             C_io->val,
             &C_io->info.lda);
CPLM_TAC(step2)
  // Back to 0-base indexes :)
CPLM_TIC(step3, "ConvertTo0BasedIndexing")
  if (B_in->info.stor_type == COL_MAJOR)
  {
    CPLM_MatCSRConvertTo0BasedIndexing(A_in);
  }
CPLM_TAC(step3)

#ifdef DEBUG
  CPLM_MatDensePrintf2D("= ",C_io);
#endif

CPLM_CLOSE_TIMER
CPLM_END_TIME
CPLM_POP
  return ierr;
}

int CPLM_MatCSRKernelGenMatDenseMult(double      *val_in,
                                int         *colInd_in,
                                int         *rowPtrB_in,
                                int         *rowPtrE_in,
                                int          nrowA_in,
                                int          ncolA_in,
                                CPLM_Mat_Dense_t *B_in,
                                CPLM_Mat_Dense_t *C_io,
                                double       alpha,
                                double       beta)
{
CPLM_PUSH
CPLM_BEGIN_TIME
CPLM_OPEN_TIMER
  int ierr = 0;
  char matdescra[6];

  matdescra[0]  = 'G';
  matdescra[1]  = 'L';//ignored if G
  matdescra[2]  = 'N';//ignored if G
  char trans  = 'N';

  CPLM_ASSERT(C_io->val != NULL);

  if (B_in->info.stor_type == COL_MAJOR)
    matdescra[3] = 'F';
  else
    matdescra[3]  = 'C';

CPLM_TIC(step1, "Call SpBLAS")
  mkl_dcsrmm(&trans,
             &nrowA_in,
             &B_in->info.n,
             &ncolA_in,
             &alpha,
             matdescra,
             val_in,
             colInd_in,
             rowPtrB_in,
             rowPtrE_in,
             B_in->val,
             &B_in->info.lda,
             &beta,
             C_io->val,
             &C_io->info.lda);
CPLM_TAC(step1)

CPLM_CLOSE_TIMER
CPLM_END_TIME
CPLM_POP
  return ierr;
}

void CPLM_MatCSRPARDISOSetParameters(MKL_INT* iparam_io)
{
CPLM_PUSH
  CPLM_ASSERT(iparam_io != NULL);

  memset(iparam_io,0,64*sizeof(MKL_INT));

  iparam_io[0]  = 1; // Non standard solver
  iparam_io[1]  = 2; // Metis permutation to reduce fill-in
  iparam_io[4]  = 0; // Return Metis permutation
  iparam_io[9] = 13; // Pivot perturbation
  iparam_io[24] = 0; // Sequential forward and backward solve
  iparam_io[26] = 0; // Check A
  iparam_io[34] = 1; // C-style array indexing (starts from 0)
  //  iparam_io[17] = -1;
  //  iparam_io[18] = -1;
CPLM_POP
}

int CPLM_MatCSRPARDISOFree(CPLM_Mat_CSR_t*  A_in,
                      _MKL_DSS_HANDLE_t       pardisoHandle_io,
                      MKL_INT*    iparam_in,
                      MKL_INT     mtype_in)
{
CPLM_PUSH
  MKL_INT maxfct  = 1;
  MKL_INT mnum    = 1;
  MKL_INT phase   = -1;
  MKL_INT n       = A_in->info.m;
  MKL_INT nrhs    = 0;
  MKL_INT msglvl  = 0;
  MKL_INT error   = 0;
  MKL_INT iDummy  = -1;
  double dDummy = -1e0;

  pardiso(pardisoHandle_io,
          &maxfct,
          &mnum,
          &mtype_in,
          &phase,
          &n,
          &dDummy,
          A_in->rowPtr,
          A_in->colInd,
          &iDummy,
          &nrhs,
          iparam_in,
          &msglvl,
          &dDummy, // b: not needed here
          &dDummy, // x: not needed here
          &error);

CPLM_POP
  return error;
}

int CPLM_MatCSRPARDISOFactorization( CPLM_Mat_CSR_t*  A_in,
                                _MKL_DSS_HANDLE_t       pardisoHandle_out,
                                MKL_INT*    iparam_in,
                                MKL_INT     mtype_in,
                                MKL_INT*    perm_out)
{
CPLM_PUSH
CPLM_BEGIN_TIME
  MKL_INT maxfct  = 1;
  MKL_INT mnum    = 1;
  MKL_INT phase   = 12;
  MKL_INT n       = A_in->info.m;
  MKL_INT nrhs    = 0;
  MKL_INT msglvl  = 0;
  MKL_INT error   = 0;
  double dDummy = -1e0;

  CPLM_ASSERT(A_in               !=  NULL);
  CPLM_ASSERT(A_in->val          !=  NULL);
  CPLM_ASSERT(pardisoHandle_out  !=  NULL);
  CPLM_ASSERT(iparam_in          !=  NULL);
  //  CPLM_ASSERT(perm_out           !=  NULL);

  pardiso(pardisoHandle_out,
          &maxfct,
          &mnum,
          &mtype_in,
          &phase,
          &n,
          A_in->val,
          A_in->rowPtr,
          A_in->colInd,
          perm_out, // the permutation returned by PARDISO
          &nrhs,
          iparam_in,
          &msglvl,
          &dDummy,  // b: not needed here
          &dDummy,  // x: not needed here
          &error);

CPLM_END_TIME
CPLM_POP
  return error;
}

int CPLM_MatCSRPARDISOGeneralSolve(CPLM_Mat_CSR_t*    A_in,
                              CPLM_Mat_Dense_t*  B_in,
                              CPLM_Mat_Dense_t*  X_out,
                              MKL_INT       phase,
                              _MKL_DSS_HANDLE_t         pardisoHandle_in,
                              MKL_INT*      iparam_in,
                              MKL_INT       mtype_in,
                              MKL_INT*      perm_in)
{
CPLM_PUSH
CPLM_BEGIN_TIME

  int ierr = 0;
  MKL_INT maxfct  = 1;
  MKL_INT mnum    = 1;
  MKL_INT n       = 0;
  MKL_INT nrhs    = 0;
  MKL_INT msglvl  = 0;
  MKL_INT error   = 0;

  CPLM_ASSERT(A_in               !=  NULL);
  CPLM_ASSERT(B_in               !=  NULL);
  CPLM_ASSERT(X_out              !=  NULL);
  CPLM_ASSERT(A_in->val          !=  NULL);
  CPLM_ASSERT(B_in->val          !=  NULL);
  CPLM_ASSERT(pardisoHandle_in   !=  NULL);
  CPLM_ASSERT(iparam_in          !=  NULL);
  //  CPLM_ASSERT(perm_in            !=  NULL);

  if (X_out->val == NULL)
  {
    X_out->info = B_in->info;
    ierr = CPLM_MatDenseMalloc(X_out);CPLM_CHKERR(ierr);
  }
  else if (!CPLM_MatDenseIsSameLocalInfo(X_out,B_in))
  {
    X_out->info = B_in->info;
    ierr = CPLM_MatDenseRealloc(X_out);CPLM_CHKERR(ierr);
  }

  n       = (MKL_INT) A_in->info.m;
  nrhs    = (MKL_INT) B_in->info.n;

  pardiso(pardisoHandle_in,
          &maxfct,
          &mnum,
          &mtype_in,
          &phase,
          &n,
          A_in->val,
          A_in->rowPtr,
          A_in->colInd,
          perm_in,
          &nrhs,
          iparam_in,
          &msglvl,
          B_in->val,
          X_out->val,
          &error);

CPLM_END_TIME
CPLM_POP
  return error || ierr;
}

int CPLM_MatCSRPARDISOSolve( CPLM_Mat_CSR_t*    A_in,
                        CPLM_Mat_Dense_t*  B_in,
                        CPLM_Mat_Dense_t*  X_out,
                        _MKL_DSS_HANDLE_t         pardisoHandle_in,
                        MKL_INT*      iparam_in,
                        MKL_INT       mtype_in,
                        MKL_INT*      perm_in)
{
CPLM_PUSH
  MKL_INT phase = 33;
  int ierr = CPLM_MatCSRPARDISOGeneralSolve( A_in,
                                        B_in,
                                        X_out,
                                        phase,
                                        pardisoHandle_in,
                                        iparam_in,
                                        mtype_in,
                                        perm_in);
CPLM_POP
  return ierr;
}

int CPLM_MatCSRPARDISOSolveForward(CPLM_Mat_CSR_t*    A_in,
                              CPLM_Mat_Dense_t*  B_in,
                              CPLM_Mat_Dense_t*  X_out,
                              _MKL_DSS_HANDLE_t         pardisoHandle_in,
                              MKL_INT*      iparam_in,
                              MKL_INT       mtype_in,
                              MKL_INT*      perm_in)
{
CPLM_PUSH
  MKL_INT phase = 331;
  int ierr = CPLM_MatCSRPARDISOGeneralSolve( A_in,
                                        B_in,
                                        X_out,
                                        phase,
                                        pardisoHandle_in,
                                        iparam_in,
                                        mtype_in,
                                        perm_in);
  if (ierr != 0)
  {
    CPLM_Abort("PARDISO Solve forward");
  }
CPLM_POP
  return ierr;
}

int CPLM_MatCSRPARDISOSolveBackward( CPLM_Mat_CSR_t*    A_in,
                                CPLM_Mat_Dense_t*  B_in,
                                CPLM_Mat_Dense_t*  X_out,
                                _MKL_DSS_HANDLE_t         pardisoHandle_in,
                                MKL_INT*      iparam_in,
                                MKL_INT       mtype_in,
                                MKL_INT*      perm_in)
{
CPLM_PUSH
  MKL_INT phase = 333;
  int ierr = CPLM_MatCSRPARDISOGeneralSolve( A_in,
                                        B_in,
                                        X_out,
                                        phase,
                                        pardisoHandle_in,
                                        iparam_in,
                                        mtype_in,
                                        perm_in);
  if (ierr != 0)
  {
    CPLM_Abort("PARDISO Solve backward");
  }
CPLM_POP
  return ierr;
}
#endif

int CPLM_MatDenseNorm(CPLM_Mat_Dense_t *A_in, const char type, double *norm)
{
  CPLM_PUSH
  int ierr = 0;
  int major  = (A_in->info.stor_type == COL_MAJOR) ? LAPACK_COL_MAJOR
    : LAPACK_ROW_MAJOR;

  *norm = LAPACKE_dlange(major,
      type,
      A_in->info.m,
      A_in->info.n,
      A_in->val,
      A_in->info.lda);

  CPLM_POP
  return ierr;
}

int CPLM_MatDenseMatDotProd(CPLM_Mat_Dense_t* A, CPLM_Mat_Dense_t* B, CPLM_Mat_Dense_t* C, MPI_Comm comm)
{
CPLM_PUSH
CPLM_BEGIN_TIME
  int ierr = 0;
  // Do local dot product
  ierr = CPLM_MatDenseKernelMatDotProd(A, B, C);

  // Sum local dot products in place (no mem alloc needed)
  ierr = MPI_Allreduce(MPI_IN_PLACE, C->val, C->info.nval, MPI_DOUBLE, MPI_SUM, comm);CPLM_checkMPIERR(ierr,"MatDenseMatDotProd::MPI_Allreduce");
CPLM_END_TIME
CPLM_POP
  return ierr;
}

/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/*                                  HANDLER                                   */
/******************************************************************************/
/*
void CPLM_MatDenseSVDHandler(int ierr)
{

  if (ierr > 0)
  {
    CPLM_eprintf("the eigensolver did not converge!");
  }
  else if (ierr < 0)
  {
    CPLM_eprintf("parameter %d has an illegal value!", -ierr + 1);
  }

}
*/

max_stars_repo_name: W-Wuxian/preAlps

max_issues_repo_name: W-Wuxian/preAlps

max_forks_repo_name: W-Wuxian/preAlps