text:
/* tsqr.c
 *
 * Copyright (C) 2015, 2019 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/*
 * This module implements the sequential TSQR algorithm
 * described in
 *
 * [1] Demmel, J., Grigori, L., Hoemmen, M. F., and Langou, J.
 *     "Communication-optimal parallel and sequential QR and LU factorizations",
 *     UCB Technical Report No. UCB/EECS-2008-89, 2008.
 *
 * The algorithm operates on a tall least squares system:
 *
 * [ A_1 ] x = [ b_1 ]
 * [ A_2 ]     [ b_2 ]
 * [ ... ]     [ ... ]
 * [ A_k ]     [ b_k ]
 *
 * as follows:
 *
 * 1. Initialize
 *    a. [Q_1,R_1] = qr(A_1)
 *    b. z_1 = Q_1^T b_1
 * 2. Loop i = 2:k
 *    a. [Q_i,R_i] = qr( [ R_{i-1} ; A_i ] )
 *    b. z_i = Q_i^T [ z_{i-1} ; b_i ]
 * 3. Output:
 *    a. R = R_k
 *    b. Q^T b = z_k
 *
 * Step 2(a) is optimized to take advantage
 * of the sparse structure of the matrix
 */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_multilarge.h>

typedef struct
{
  size_t p;             /* number of columns of LS matrix */
  int nblocks;          /* number of blocks processed */
  double rnorm;         /* || b - A x || residual norm */
  int svd;              /* SVD of R factor has been computed */

  gsl_matrix *T;        /* block reflector matrix, p-by-p */
  gsl_matrix *R;        /* R factor, p-by-p */
  gsl_vector *QTb;      /* [ Q^T b ; b_i ], size p-by-1 */
  gsl_vector *work;     /* workspace, size p */
  gsl_vector *work3;    /* workspace, size 3*p */

  gsl_multifit_linear_workspace * multifit_workspace_p;
} tsqr_state_t;

static void *tsqr_alloc(const size_t p);
static void tsqr_free(void *vstate);
static int tsqr_reset(void *vstate);
static int tsqr_accumulate(gsl_matrix * A, gsl_vector * b,
                           void * vstate);
static int tsqr_solve(const double lambda, gsl_vector * x,
                      double * rnorm, double * snorm,
                      void * vstate);
static int tsqr_rcond(double * rcond, void * vstate);
static int tsqr_lcurve(gsl_vector * reg_param, gsl_vector * rho,
                       gsl_vector * eta, void * vstate);
static const gsl_matrix * tsqr_R(const void * vstate);
static const gsl_vector * tsqr_QTb(const void * vstate);
static int tsqr_svd(tsqr_state_t * state);

/*
tsqr_alloc()
  Allocate workspace for solving large linear least squares
problems using the TSQR approach

Inputs: p - number of columns of LS matrix

Return: pointer to workspace
*/

static void *
tsqr_alloc(const size_t p)
{
  tsqr_state_t *state;

  if (p == 0)
    {
      GSL_ERROR_NULL("p must be a positive integer",
                     GSL_EINVAL);
    }

  state = calloc(1, sizeof(tsqr_state_t));
  if (!state)
    {
      GSL_ERROR_NULL("failed to allocate tsqr state", GSL_ENOMEM);
    }

  state->p = p;
  state->nblocks = 0;
  state->rnorm = 0.0;

  state->R = gsl_matrix_alloc(p, p);
  if (state->R == NULL)
    {
      tsqr_free(state);
      GSL_ERROR_NULL("failed to allocate R matrix", GSL_ENOMEM);
    }

  state->QTb = gsl_vector_alloc(p);
  if (state->QTb == NULL)
    {
      tsqr_free(state);
      GSL_ERROR_NULL("failed to allocate QTb vector", GSL_ENOMEM);
    }

  state->T = gsl_matrix_alloc(p, p);
  if (state->T == NULL)
    {
      tsqr_free(state);
      GSL_ERROR_NULL("failed to allocate T matrix", GSL_ENOMEM);
    }

  state->work = gsl_vector_alloc(p);
  if (state->work == NULL)
    {
      tsqr_free(state);
      GSL_ERROR_NULL("failed to allocate workspace vector", GSL_ENOMEM);
    }

  state->work3 = gsl_vector_alloc(3 * p);
  if (state->work3 == NULL)
    {
      tsqr_free(state);
      GSL_ERROR_NULL("failed to allocate work3 vector", GSL_ENOMEM);
    }

  state->multifit_workspace_p = gsl_multifit_linear_alloc(p, p);
  if (state->multifit_workspace_p == NULL)
    {
      tsqr_free(state);
      GSL_ERROR_NULL("failed to allocate multifit workspace", GSL_ENOMEM);
    }

  return state;
}

static void
tsqr_free(void *vstate)
{
  tsqr_state_t *state = (tsqr_state_t *) vstate;

  if (state->R)
    gsl_matrix_free(state->R);

  if (state->QTb)
    gsl_vector_free(state->QTb);

  if (state->T)
    gsl_matrix_free(state->T);

  if (state->work)
    gsl_vector_free(state->work);

  if (state->work3)
    gsl_vector_free(state->work3);

  if (state->multifit_workspace_p)
    gsl_multifit_linear_free(state->multifit_workspace_p);

  free(state);
}

static int
tsqr_reset(void *vstate)
{
  tsqr_state_t *state = (tsqr_state_t *) vstate;

  gsl_matrix_set_zero(state->R);
  gsl_vector_set_zero(state->QTb);
  state->nblocks = 0;
  state->rnorm = 0.0;
  state->svd = 0;

  return GSL_SUCCESS;
}

/*
tsqr_accumulate()
  Add a new block of rows to the QR system

Inputs: A      - new block of rows, n-by-p
        b      - new rhs vector n-by-1
        vstate - workspace

Return: success/error

Notes:
1) On output, the upper triangular portion of state->R(1:p,1:p)
contains current R matrix

2) state->QTb(1:p) contains current Q^T b vector

3) A and b are destroyed
*/

static int
tsqr_accumulate(gsl_matrix * A, gsl_vector * b, void * vstate)
{
  tsqr_state_t *state = (tsqr_state_t *) vstate;
  const size_t n = A->size1;
  const size_t p = A->size2;

  if (p != state->p)
    {
      GSL_ERROR("columns of A do not match workspace", GSL_EBADLEN);
    }
  else if (n != b->size)
    {
      GSL_ERROR("A and b have different numbers of rows", GSL_EBADLEN);
    }
  else if (state->nblocks == 0 && n < p)
    {
      GSL_ERROR ("n must be >= p", GSL_EBADLEN);
    }
  else if (state->nblocks == 0)
    {
      int status;
      gsl_matrix_view R = gsl_matrix_submatrix(A, 0, 0, p, p);
      gsl_vector_view QTb = gsl_vector_subvector(state->QTb, 0, p);
      gsl_vector_view b1 = gsl_vector_subvector(b, 0, p);

      /* this is the first matrix block A_1, compute its (dense) QR decomposition */

      /* compute QR decomposition of A */
      status = gsl_linalg_QR_decomp_r(A, state->T);
      if (status)
        return status;

      /* store upper triangular R factor in state->R */
      gsl_matrix_tricpy(CblasUpper, CblasNonUnit, state->R, &R.matrix);

      /* compute Q^T b and keep the first p elements */
      gsl_linalg_QR_QTvec_r(A, state->T, b, state->work);
      gsl_vector_memcpy(&QTb.vector, &b1.vector);

      if (n > p)
        {
          gsl_vector_view b2 = gsl_vector_subvector(b, p, n - p);
          state->rnorm = gsl_blas_dnrm2(&b2.vector);
        }
      else
        state->rnorm = 0.0;

      state->nblocks = 1;

      return GSL_SUCCESS;
    }
  else
    {
      int status;

      /* compute QR decomposition of [ R_{i-1} ; A_i ], accounting for
       * sparse structure */
      status = gsl_linalg_QR_UR_decomp(state->R, A, state->T);
      if (status)
        return status;

      /*
       * Compute:
       *
       * Q^T [ QTb_{i-1} ] = [ QTb_{i-1} - w ]
       *     [    b_i    ]   [  b_i - V~ w   ]
       *
       * where:
       *
       * w = T^T (QTb_{i-1} + V~^T b_i)
       *
       *       p
       * V = [ I  ] p
       *     [ V~ ] n
       */
      gsl_vector_memcpy(state->work, state->QTb);
      gsl_blas_dgemv(CblasTrans, 1.0, A, b, 1.0, state->work);                     /* w := w + V~^T b */
      gsl_blas_dtrmv(CblasUpper, CblasTrans, CblasNonUnit, state->T, state->work); /* w := T^T w */
      gsl_vector_sub(state->QTb, state->work);                                     /* QTb := QTb - w */

      /* update residual norm */
      gsl_blas_dgemv(CblasNoTrans, -1.0, A, state->work, 1.0, b);                  /* b := b - V~ w */
      state->rnorm = gsl_hypot(state->rnorm, gsl_blas_dnrm2(b));

      return GSL_SUCCESS;
    }
}

/*
tsqr_solve()
  Solve the least squares system:

chi^2 = || QTb - R x ||^2 + lambda^2 || x ||^2

using the SVD of R

Inputs: lambda - regularization parameter
        x      - (output) solution vector p-by-1
        rnorm  - (output) residual norm ||b - A x||
        snorm  - (output) solution norm ||x||
        vstate - workspace

Return: success/error
*/

static int
tsqr_solve(const double lambda, gsl_vector * x,
           double * rnorm, double * snorm,
           void * vstate)
{
  tsqr_state_t *state = (tsqr_state_t *) vstate;

  if (x->size != state->p)
    {
      GSL_ERROR ("solution vector does not match workspace", GSL_EBADLEN);
    }
  else if (lambda < 0.0)
    {
      GSL_ERROR ("regularization parameter should be non-negative", GSL_EINVAL);
    }
  else
    {
      if (lambda == 0.0)
        {
          /* solve: R x = Q^T b */
          gsl_vector_memcpy(x, state->QTb);
          gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, state->R, x);
          *rnorm = state->rnorm;
          *snorm = gsl_blas_dnrm2(x);
        }
      else
        {
          int status;

          /* compute SVD of R if not already computed */
          if (state->svd == 0)
            {
              status = tsqr_svd(state);
              if (status)
                return status;
            }

          status = gsl_multifit_linear_solve(lambda, state->R, state->QTb, x, rnorm, snorm,
                                             state->multifit_workspace_p);
          if (status)
            return status;

          *rnorm = gsl_hypot(*rnorm, state->rnorm);
        }

      return GSL_SUCCESS;
    }
}

/*
tsqr_lcurve()
  Compute L-curve of least squares system

Inputs: reg_param - (output) vector of regularization parameters
        rho       - (output) vector of residual norms
        eta       - (output) vector of solution norms
        vstate    - workspace

Return: success/error
*/

static int
tsqr_lcurve(gsl_vector * reg_param, gsl_vector * rho,
            gsl_vector * eta, void * vstate)
{
  tsqr_state_t *state = (tsqr_state_t *) vstate;
  int status;
  size_t i;

  /* compute SVD of R if not already computed */
  if (state->svd == 0)
    {
      status = tsqr_svd(state);
      if (status)
        return status;
    }

  status = gsl_multifit_linear_lcurve(state->QTb, reg_param, rho, eta,
                                      state->multifit_workspace_p);

  /* now add contribution to rnorm from Q2 factor */
  for (i = 0; i < rho->size; ++i)
    {
      double *rhoi = gsl_vector_ptr(rho, i);
      *rhoi = gsl_hypot(*rhoi, state->rnorm);
    }

  return status;
}

static const gsl_matrix *
tsqr_R(const void * vstate)
{
  const tsqr_state_t *state = (const tsqr_state_t *) vstate;
  return state->R;
}

static const gsl_vector *
tsqr_QTb(const void * vstate)
{
  const tsqr_state_t *state = (const tsqr_state_t *) vstate;
  return state->QTb;
}

static int
tsqr_rcond(double * rcond, void * vstate)
{
  tsqr_state_t *state = (tsqr_state_t *) vstate;
  return gsl_linalg_tri_rcond(CblasUpper, state->R, rcond, state->work3);
}

/*
tsqr_svd()
  Compute the SVD of the upper triangular
R factor. This allows us to compute the upper/lower
bounds on the regularization parameter and compute
the matrix reciprocal condition number.

Inputs: state - workspace

Return: success/error
*/

static int
tsqr_svd(tsqr_state_t * state)
{
  int status;

  status = gsl_multifit_linear_svd(state->R, state->multifit_workspace_p);
  if (status)
    {
      GSL_ERROR("error computing SVD of R", status);
    }

  state->svd = 1;

  return GSL_SUCCESS;
}

static const gsl_multilarge_linear_type tsqr_type =
{
  "tsqr",
  tsqr_alloc,
  tsqr_reset,
  tsqr_accumulate,
  tsqr_solve,
  tsqr_rcond,
  tsqr_lcurve,
  tsqr_R,
  tsqr_QTb,
  tsqr_free
};

const gsl_multilarge_linear_type * gsl_multilarge_linear_tsqr = &tsqr_type;

max_stars_repo_name: zzpwahaha/Chimera-Control-Trim

max_issues_repo_name: zzpwahaha/Chimera-Control-Trim

max_forks_repo_name: zzpwahaha/Chimera-Control-Trim


text:
#ifndef METHODS_CWA_SMD_OPT_H
#define METHODS_CWA_SMD_OPT_H

#include <gsl/gsl_multimin.h>
#include "quartz_internal/util/gsl_converter.h"

namespace method {
namespace cwa_smd_opt {
namespace details {

inline
gsl_multimin_fdfminimizer_type * minimizer_map(const std::string type) {
  if(type == "steepest_descent") {
    return const_cast<gsl_multimin_fdfminimizer_type *>(gsl_multimin_fdfminimizer_steepest_descent);
  } else if(type == "conjugate_pr") {
    return const_cast<gsl_multimin_fdfminimizer_type *>(gsl_multimin_fdfminimizer_conjugate_pr);
  } else if(type == "conjugate_fr") {
    return const_cast<gsl_multimin_fdfminimizer_type *>(gsl_multimin_fdfminimizer_conjugate_fr);
  } else if(type == "bfgs") {
    return const_cast<gsl_multimin_fdfminimizer_type *>(gsl_multimin_fdfminimizer_vector_bfgs);
  } else if(type == "bfgs2") {
    return const_cast<gsl_multimin_fdfminimizer_type *>(gsl_multimin_fdfminimizer_vector_bfgs2);
  } else {
    throw Error("minimizer " + type + " is not implemented");
  }

}

struct cwa_smd_opt_param {
  arma::mat original_points;
  arma::vec expectations_ref;
  std::vector<math::Polynomial<double>> original_operators;
  arma::vec weights;
  arma::vec scaling;
  long long grade;
};

inline
double penalty_function(
    const arma::mat & points,
    const arma::vec & expectations_ref,
    const std::vector<math::Polynomial<double>> & original_operators,
    const arma::vec & weights,
    const arma::vec & scaling,
    const long long grade) {

  double result = 0;

  for (arma::uword i = 0; i < original_operators.size(); i++) {
    const long long the_grade = original_operators[i].grade();
    if (the_grade < grade && the_grade > 0) {
      const double result_from_cwa =
          cwa_smd::details::expectation(original_operators[i], points, weights,
                                        scaling);

      result += std::pow(result_from_cwa - expectations_ref(i), 2);
    }
  }

  return result;

}

inline
arma::mat penalty_function_derivative(
    const arma::mat & points,
    const arma::vec & expectations_ref,
    const std::vector<math::Polynomial<double>> & original_operators,
    const arma::vec & weights,
    const arma::vec & scaling,
    const long long grade
) {

  arma::mat result(arma::size(points), arma::fill::zeros);

  for (arma::uword i = 0; i < original_operators.size(); i++) {
    const long long the_grade = original_operators[i].grade();
    if (the_grade < grade && the_grade > 0) {
      const double result_from_cwa =
          cwa_smd::details::expectation(original_operators[i], points, weights,
                                        scaling);

      for (arma::uword j = 0; j < points.n_cols; j++) {
        const arma::vec point = arma::diagmat(1.0 / scaling) * points.col(j);
        for (arma::uword k = 0; k < points.n_rows / 2; k++) {
          const math::Polynomial<double> x_derivative =
              original_operators[i].derivative(
                  k) / scaling(k);
          const math::Polynomial<double> p_derivative =
              original_operators[i].derivative(
                  k + points.n_rows / 2) / scaling(k + points.n_rows / 2);

          result(k, j) -=
              2.0 * (result_from_cwa - expectations_ref(i)) * weights(j) *
              x_derivative.at(point) / arma::sum(weights);
          result(k + points.n_rows / 2, j) -=
              2.0 * (result_from_cwa - expectations_ref(i)) * weights(j) *
              p_derivative.at(point) / arma::sum(weights);
        }
      }
    }
  }

  return -result;

}

inline
double penalty_function_gsl_wrapper(const gsl_vector * flattened_points,
                                    void * param) {

  const arma::vec arma_flattened_points = gsl::convert_vec(flattened_points);
  const auto converted_param = *(cwa_smd_opt_param *) param;
  const arma::mat points = arma::reshape(arma_flattened_points, arma::size(
      converted_param.original_points));

  return penalty_function(points,
                          converted_param.expectations_ref,
                          converted_param.original_operators,
                          converted_param.weights,
                          converted_param.scaling,
                          converted_param.grade);
}

inline
void penalty_function_derivative_gsl_wrapper(
    const gsl_vector * flattened_points,
    void * param,
    gsl_vector * g) {
  const arma::vec arma_flattened_points = gsl::convert_vec(flattened_points);
  const auto converted_param = *(cwa_smd_opt_param *) param;
  const arma::mat points = arma::reshape(arma_flattened_points, arma::size(
      converted_param.original_points));

  const arma::vec result =
      arma::vectorise(
          penalty_function_derivative(points,
                                      converted_param.expectations_ref,
                                      converted_param.original_operators,
                                      converted_param.weights,
                                      converted_param.scaling,
                                      converted_param.grade));

  const auto result_pointer = gsl::convert_vec(result);

  gsl_vector_memcpy(g, result_pointer);

  gsl_vector_free(result_pointer);
}

inline
void penalty_function_fdf_gsl_wrapper(
    const gsl_vector * a_derivatives,
    void * param,
    double * f,
    gsl_vector * g) {
  penalty_function_derivative_gsl_wrapper(a_derivatives, param, g);

  *f = penalty_function_gsl_wrapper(a_derivatives, param);
}

inline
std::tuple<arma::mat, double, double, int> cwa_optimize(const cwa_smd_opt_param input,
                                                const double initial_step_size,
                                                const double tolerance,
                                                const double gradient_tolerance,
                                                const size_t total_steps,
                                                const std::string type) {

  /* allocate memory for minimization process */
  const auto minimizer_type = minimizer_map(type);

  const arma::uword n = input.original_points.n_elem;

  const auto penalty_function_value = penalty_function(input.original_points,
                                                       input.expectations_ref,
                                                       input.original_operators,
                                                       input.weights,
                                                       input.scaling,
                                                       input.grade);

  const double gradient_module = arma::norm(
      arma::vectorise(penalty_function_derivative(input.original_points,
                                                  input.expectations_ref,
                                                  input.original_operators,
                                                  input.weights,
                                                  input.scaling,
                                                  input.grade)));

  if (penalty_function_value < tolerance &&
      gradient_module < gradient_tolerance) {
    return {input.original_points, penalty_function_value, gradient_module, 0};
  }

  auto minimizer_environment = gsl_multimin_fdfminimizer_alloc(minimizer_type,
                                                               n);

  /* assigning function to minimizer object */
  gsl_multimin_function_fdf minimizer_object;
  minimizer_object.f = &penalty_function_gsl_wrapper;
  minimizer_object.df = &penalty_function_derivative_gsl_wrapper;
  minimizer_object.fdf = &penalty_function_fdf_gsl_wrapper;
  minimizer_object.n = n;
  minimizer_object.params = (void *) &input;

  /* starting point */
  const arma::vec flattened = arma::vectorise(input.original_points);
  gsl_vector * points = gsl::convert_vec(flattened);

  /* set environment */
  gsl_multimin_fdfminimizer_set(minimizer_environment,
                                &minimizer_object, points,
                                initial_step_size, tolerance);

  size_t iter = 0;
  int status = GSL_CONTINUE;
  do {
    iter++;

    status = gsl_multimin_fdfminimizer_iterate(minimizer_environment);

    if (status) {
      throw Error(gsl_strerror(status));
    }

    status = gsl_multimin_test_gradient(minimizer_environment->gradient,
                                        gradient_tolerance);

    if (status == GSL_SUCCESS) {
      const arma::vec result = gsl::convert_vec(minimizer_environment->x);

      const double f = minimizer_environment->f;
      const double df = arma::norm(gsl::convert_vec(minimizer_environment->gradient));
      gsl_multimin_fdfminimizer_free(minimizer_environment);
      gsl_vector_free(points);

      return {arma::reshape(result, arma::size(input.original_points)), f, df, iter};
    }
  } while (status == GSL_CONTINUE && iter < total_steps);

  throw Error("fail to converge towards the solution");
}

} // namespace details

struct State {
public:
  arma::mat points;
  arma::vec weights;
  arma::vec masses;
  arma::uword grade;
  arma::uvec expectation_table;
  arma::vec expectations;
  arma::uvec positional_indices;
  arma::uvec momentum_indices;
  arma::vec scaling;

  // Establish an easy way to construct your State
  template<typename PhaseSpaceDistribution>
  State(const PhaseSpaceDistribution & initial,
        const arma::uvec & grid,
        const arma::mat & range,
        const arma::vec & scaling,
        const arma::vec & masses,
        const arma::uword grade) :
      points(math::space::points_generate(grid, range)),
      weights(arma::real(at(initial, points))),
      masses(masses),
      grade(grade),
      expectation_table(math::space::grids_to_table(
          grade * arma::ones<arma::uvec>(points.n_rows))),
      scaling(scaling) {
    if (grid.n_rows != range.n_rows) {
      throw Error("Different dimension between the grid and the range");
    }
    if (grid.n_rows != 2 * masses.n_rows) {
      throw Error("Different dimension between the grid and the masses");
    }

    const arma::uword dimension = grid.n_elem;
    const arma::uword length = std::pow(grade, dimension);

    this->expectations = arma::vec(length);
    this->positional_indices = arma::uvec(dimension / 2);
    this->momentum_indices = arma::uvec(dimension / 2);

    const arma::vec ranges = range.col(1) - range.col(0);

    // exponents check in
#pragma omp parallel for
    for (arma::uword i = 0; i < dimension / 2; i++) {
      arma::uvec X = arma::zeros<arma::uvec>(dimension);
      arma::uvec P = arma::zeros<arma::uvec>(dimension);
      X(i) = 1;
      P(i + dimension / 2) = 1;
      this->positional_indices(i) =
          math::space::indices_to_index(X, this->expectation_table);
      this->momentum_indices(i) =
          math::space::indices_to_index(P, this->expectation_table);
    }

    // expectations check in
#pragma omp parallel for
    for (arma::uword i = 0; i < length; i++) {
      const lvec indices =
          arma::conv_to<lvec>::from(
              math::space::index_to_indices(i, this->expectation_table));

      this->expectations(i) =
          cwa_smd::details::expectation(math::polynomial::Term(1.0, indices),
                                        this->points, this->weights,
                                        this->scaling);
    }
  }

  template<typename PhaseSpaceDistribution>
  State(const PhaseSpaceDistribution & initial,
        const arma::uvec & grid,
        const arma::mat & range,
        const arma::uword grade) :
      points(math::space::points_generate(grid, range)),
      weights(arma::real(at(initial, points))),
      masses(arma::ones<arma::vec>(grid.n_rows / 2)),
      grade(grade),
      expectation_table(math::space::grids_to_table(
          grade * arma::ones<arma::uvec>(points.n_rows))) {
    if (grid.n_rows != range.n_rows) {
      throw Error("Different dimension between the grid and the range");
    }
    if (grid.n_rows != 2 * masses.n_rows) {
      throw Error("Different dimension between the grid and the masses");
    }

    const auto dimension = grid.n_elem;
    const auto length = std::pow(grade, dimension);

    this->expectations = arma::vec(length);
    this->positional_indices = arma::uvec(dimension / 2);
    this->momentum_indices = arma::uvec(dimension / 2);

    const arma::vec ranges = range.col(1) - range.col(0);
    this->scaling = ranges;
//    this->scaling = arma::ones(arma::size(ranges));

    // exponents check in
    for (arma::uword i = 0; i < dimension / 2; i++) {
      arma::uvec X = arma::zeros<arma::uvec>(dimension);
      arma::uvec P = arma::zeros<arma::uvec>(dimension);
      X(i) = 1;
      P(i + dimension / 2) = 1;
      this->positional_indices(i) =
          math::space::indices_to_index(X, this->expectation_table);
      this->momentum_indices(i) =
          math::space::indices_to_index(P, this->expectation_table);
    }

    // expectations check in
    for (arma::uword i = 0; i < length; i++) {
      const lvec indices =
          arma::conv_to<lvec>::from(
              math::space::index_to_indices(i, this->expectation_table));

      this->expectations(i) =
          cwa_smd::details::expectation(
              math::polynomial::Term(1.0, indices),
              this->points, this->weights, this->scaling);
    }
  }

  inline
  State(const arma::mat & points,
        const arma::vec & weights,
        const arma::vec & masses,
        const arma::uvec & expectation_table,
        const arma::vec & expectations,
        const arma::uvec & positional_indices,
        const arma::uvec & momentum_indices,
        const arma::vec & scaling,
        const arma::uword grade) :
      points(points),
      weights(weights),
      masses(masses),
      grade(grade),
      expectation_table(expectation_table),
      expectations(expectations),
      positional_indices(positional_indices),
      momentum_indices(momentum_indices),
      scaling(scaling) {}

  inline
  State(const State & state) :
      points(state.points),
      weights(state.weights),
      masses(state.masses),
      grade(state.grade),
      expectation_table(state.expectation_table),
      expectations(state.expectations),
      positional_indices(state.positional_indices),
      momentum_indices(state.momentum_indices),
      scaling(state.scaling) {}

  inline
  arma::uword dim() const {
    return points.n_rows / 2;
  }

  inline
  State normalise() const {
    State state = *this;
    state.weights = state.weights / arma::sum(state.weights);

    return state;
  }

  inline
  arma::vec positional_expectation() const {

    return method::cwa::State(this->points, this->weights, this->masses)
              .positional_expectation();
  }

  inline
  arma::vec momentum_expectation() const {
    return method::cwa::State(this->points, this->weights, this->masses)
        .momentum_expectation();
  }

  State operator+(const State & B) const {
    if (!arma::approx_equal(this->weights, B.weights, "abs_diff", 1e-16) ||
        !arma::approx_equal(this->masses, B.masses, "abs_diff", 1e-16)) {
      throw Error("Different cwa states are being added");
    }

    State state = B;
    state.points += this->points;
    state.expectations += this->expectations;

    return state;
  }

  State operator*(const double B) const {

    State state = *this;
    state.expectations *= B;
    state.points *= B;

    return state;
  }

  template<typename T>
  auto expectation(const math::Polynomial<T> & polynomial) const {
    return cwa_smd::details::at_search(polynomial,
                                       this->points,
                                       this->weights,
                                       this->expectations,
                                       this->expectation_table,
                                       this->scaling,
                                       this->grade) *
           polynomial.at(this->scaling);
  }

  template<typename T>
  arma::vec expectation(const std::vector<math::Polynomial<T>>

                        & polynomials) const {
    arma::vec result(polynomials.size());

#pragma omp parallel for
    for (arma::uword i = 0; i < result.n_elem; i++) {
      result(i) = this->expectation(polynomials[i]);
    }

    return result;
  }

  State & operator=(const State &) = default;
};

struct Operator {

public:
  math::Polynomial<double> potential;
  math::Polynomial<double> H;
  std::vector<math::Polynomial<double>> original_operators;
  std::vector<math::Polynomial<double>> operators;

  Operator(const State & state,
           const math::Polynomial<double> & potential) :
      potential(potential),
      H(hamiltonian(potential, state.masses).scale(state.scaling)),
      operators() {

    std::vector<math::Polynomial<double>>
        op(std::pow(state.grade, state.dim() * 2));
    std::vector<math::Polynomial<double>>
        original_op(std::pow(state.grade, state.dim() * 2));

    op[0] = math::Polynomial<double>(state.dim() * 2, 0.0);
    original_op[0] = math::Polynomial<double>(state.dim() * 2, 1.0);

    for (arma::uword i = 1; i < op.size(); i++) {
      const auto observable =
          math::Polynomial(math::polynomial::Term<double>(1.0,
                                                          math::space::index_to_indices(
                                                              i,
                                                              state.expectation_table)));

      original_op[i] = observable;

      const arma::uword cut_off = std::min(observable.grade(), H.grade()) / 2;
      const auto moyal =
          moyal_bracket(math::Polynomial(observable), H, state.scaling,
                        cut_off);

      op[i] = moyal;
    }

    this->operators = op;
    this->original_operators = original_op;
  }

  inline
  PropagationType propagation_type() const {
    return Classic;
  }

  State operator()(const State & state) const {

    arma::mat p_submatrix = state.points.rows(state.dim(), 2 * state.dim() - 1);
    p_submatrix.each_col() /= state.masses;

    const arma::mat points_change_list =
        arma::join_cols(p_submatrix,
                        cwa::details::force(this->potential,
                                            state.points.rows(0, state.dim() -
                                                                 1)));

    arma::vec expectation_change_list =
        arma::vec(arma::size(state.expectations));

#pragma omp parallel for
    for (arma::uword i = 0; i < expectation_change_list.n_elem; i++) {
      expectation_change_list(i) =
          cwa_smd::details::at_search(this->operators[i],
                                      state.points,
                                      state.weights,
                                      state.expectations,
                                      state.expectation_table,
                                      state.scaling,
                                      state.grade);
    }

    return State(points_change_list,
                 state.weights,
                 state.masses,
                 state.expectation_table,
                 expectation_change_list,
                 state.positional_indices,
                 state.momentum_indices,
                 state.scaling,
                 state.grade);
  }

};

template<typename Potential>
OperatorWrapper<Operator, State, Potential>
cwa_opt(const double initial_step_size,
        const double tolerance,
        const double gradient_tolerance,
        const size_t total_steps,
        const std::string type = "bfgs2",
        const int print_level = 0) {
  return [initial_step_size,
      tolerance,
      gradient_tolerance,
      total_steps,
      type,
      print_level
  ](const Operator & cwa_smd_opt_operator,
    const Potential & potential) -> Propagator<State> {
    return [initial_step_size,
        tolerance,
        gradient_tolerance,
        total_steps,
        &cwa_smd_opt_operator,
        type,
        print_level
    ]
        (const State & state,
         const double dt) -> State {
      const arma::vec & ref_expectations = state.expectations;
      const arma::mat & points = state.points;
      const auto & original_operators = cwa_smd_opt_operator.original_operators;

      details::cwa_smd_opt_param input{points, ref_expectations,
                                       original_operators,
                                       state.weights, state.scaling,
                                       (long long) state.grade};

      const auto opt_result = details::cwa_optimize(input, initial_step_size,
                                tolerance, gradient_tolerance, total_steps, type);

      const arma::mat new_points = std::get<0>(opt_result);
      const double f = std::get<1>(opt_result);
      const double df = std::get<2>(opt_result);
      const int iter = std::get<3>(opt_result);

      if(print_level > 2) {
        fmt::print("f: {0:20.10f}, df: {1:20.10f}, iter: {2}", f, df, iter);
        fmt::print("\n");
      }

      State new_state = state;
      new_state.points = new_points;
      return new_state;
    };
  };
}

} // namespace cwa
}

#endif //METHODS_CWA_SMD_OPT_H

ext: h

max_stars_repo_name: Walter-Feng/Quartz

max_issues_repo_name: Walter-Feng/Quartz

max_forks_repo_name: Walter-Feng/Quartz


text:
#include <math.h>
#include <stdlib.h>
#include <assert.h>
#include <cconfigspace.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>

#define NUM_DISTRIBS 2
#define NUM_SAMPLES 10000

void test_create_multivariate_distribution() {
	const size_t            num_distribs = NUM_DISTRIBS;
	ccs_distribution_t      distrib = NULL;
	ccs_distribution_t      distribs[NUM_DISTRIBS];
	ccs_distribution_t      distribs_ret[NUM_DISTRIBS];
	ccs_result_t            err = CCS_SUCCESS;
	int32_t                 refcount;
	ccs_object_type_t       otype;
	ccs_distribution_type_t dtype;
	ccs_numeric_type_t      data_types[NUM_DISTRIBS];
	ccs_interval_t          intervals[NUM_DISTRIBS];
	size_t                  num_distribs_ret;

	err = ccs_create_uniform_distribution(
		CCS_NUM_FLOAT,
		CCSF(-5.0),
		CCSF(5.0),
		CCS_LINEAR,
		CCSF(0.0),
		distribs);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_INTEGER,
		CCSI(-5),
		CCSI(5),
		CCS_LINEAR,
		CCSI(0),
		distribs + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_multivariate_distribution(
		num_distribs,
		distribs,
		&distrib);
	assert( err == CCS_SUCCESS );

	err = ccs_object_get_type(distrib, &otype);
	assert( err == CCS_SUCCESS );
	assert( otype == CCS_DISTRIBUTION );

	err = ccs_distribution_get_type(distrib, &dtype);
	assert( err == CCS_SUCCESS );
	assert( dtype == CCS_MULTIVARIATE );

	err = ccs_distribution_get_data_types(distrib, data_types);
	assert( err == CCS_SUCCESS );
	assert( data_types[0] == CCS_NUM_FLOAT );
	assert( data_types[1] == CCS_NUM_INTEGER );

        err = ccs_distribution_get_bounds(distrib, intervals);
	assert( err == CCS_SUCCESS );
	assert( intervals[0].type == CCS_NUM_FLOAT );
	assert( intervals[0].lower.f == -5.0 );
	assert( intervals[0].lower_included == CCS_TRUE );
	assert( intervals[0].upper.f == 5.0 );
	assert( intervals[0].upper_included == CCS_FALSE );
	assert( intervals[1].type == CCS_NUM_INTEGER );
	assert( intervals[1].lower.i == -5 );
	assert( intervals[1].lower_included == CCS_TRUE );
	assert( intervals[1].upper.i == 5 );
	assert( intervals[1].upper_included == CCS_FALSE );

	err = ccs_multivariate_distribution_get_num_distributions(distrib, &num_distribs_ret);
	assert( err == CCS_SUCCESS );
	assert( num_distribs_ret == num_distribs );

	err = ccs_multivariate_distribution_get_distributions(distrib, num_distribs, distribs_ret, &num_distribs_ret);
	assert( err == CCS_SUCCESS );
	assert( num_distribs_ret == num_distribs );
	for (size_t i = 0; i < num_distribs; i++)
		assert( distribs_ret[i] == distribs[i] );

	err = ccs_object_get_refcount(distrib, &refcount);
	assert( err == CCS_SUCCESS );
	assert( refcount == 1 );

	for (size_t i = 0; i < num_distribs; i++) {
		err = ccs_release_object(distribs[i]);
		assert( err == CCS_SUCCESS );
	}
	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
}

void test_multivariate_distribution() {
	ccs_distribution_t distrib = NULL, distribs[NUM_DISTRIBS];
	ccs_rng_t          rng = NULL;
	ccs_result_t       err = CCS_SUCCESS;
	const size_t       num_distribs = NUM_DISTRIBS;
	const size_t       num_samples = NUM_SAMPLES;
	ccs_numeric_t      samples[NUM_SAMPLES*NUM_DISTRIBS];

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_FLOAT,
		CCSF(-5.0),
		CCSF(5.0),
		CCS_LINEAR,
		CCSF(0.0),
		distribs);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_INTEGER,
		CCSI(-5),
		CCSI(5),
		CCS_LINEAR,
		CCSI(0),
		distribs + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_multivariate_distribution(
		num_distribs,
		distribs,
		&distrib);
	assert( err == CCS_SUCCESS );

	err = ccs_distribution_samples(distrib, rng, num_samples, samples);
	assert( err == CCS_SUCCESS );

	for (size_t i = 0; i < num_samples; i++) {
		assert( samples[NUM_DISTRIBS*i].f >= -5.0 );
		assert( samples[NUM_DISTRIBS*i].f <   5.0 );
		assert( samples[NUM_DISTRIBS*i + 1].i >= -5 );
		assert( samples[NUM_DISTRIBS*i + 1].i <   5 );
	}
	double mean = gsl_stats_mean((double*)samples, NUM_DISTRIBS, num_samples);
	assert( mean < 0.0 + 0.1 );
	assert( mean > 0.0 - 0.1 );

	for (size_t i = 0; i < num_distribs; i++) {
		err = ccs_release_object(distribs[i]);
		assert( err == CCS_SUCCESS );
	}
	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

void test_multivariate_distribution_strided_samples() {
	ccs_distribution_t distrib = NULL, distribs[NUM_DISTRIBS];
	ccs_rng_t          rng = NULL;
	ccs_result_t       err = CCS_SUCCESS;
	const size_t       num_distribs = NUM_DISTRIBS;
	const size_t       num_samples = NUM_SAMPLES;
	ccs_numeric_t      samples[NUM_SAMPLES*(NUM_DISTRIBS+1)];

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_FLOAT,
		CCSF(-5.0),
		CCSF(5.0),
		CCS_LINEAR,
		CCSF(0.0),
		distribs);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_INTEGER,
		CCSI(-5),
		CCSI(5),
		CCS_LINEAR,
		CCSI(0),
		distribs + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_multivariate_distribution(
		num_distribs,
		distribs,
		&distrib);
	assert( err == CCS_SUCCESS );

	err = ccs_distribution_strided_samples(distrib, rng, num_samples, NUM_DISTRIBS+1, samples);
	assert( err == CCS_SUCCESS );

	for (size_t i = 0; i < num_samples; i++) {
		assert( samples[(NUM_DISTRIBS+1)*i].f >= -5.0 );
		assert( samples[(NUM_DISTRIBS+1)*i].f <   5.0 );
		assert( samples[(NUM_DISTRIBS+1)*i + 1].i >= -5 );
		assert( samples[(NUM_DISTRIBS+1)*i + 1].i <   5 );
	}
	double mean = gsl_stats_mean((double*)samples, NUM_DISTRIBS+1, num_samples);
	assert( mean < 0.0 + 0.1 );
	assert( mean > 0.0 - 0.1 );

	for (size_t i = 0; i < num_distribs; i++) {
		err = ccs_release_object(distribs[i]);
		assert( err == CCS_SUCCESS );
	}
	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

void test_multivariate_distribution_soa_samples() {
	ccs_distribution_t distrib = NULL, distribs[NUM_DISTRIBS];
	ccs_rng_t          rng = NULL;
	ccs_result_t       err = CCS_SUCCESS;
	const size_t       num_distribs = NUM_DISTRIBS;
	const size_t       num_samples = NUM_SAMPLES;
	ccs_numeric_t      samples1[NUM_SAMPLES];
	ccs_numeric_t      samples2[NUM_SAMPLES];
	ccs_numeric_t     *samples[] = { samples1, samples2 };

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_FLOAT,
		CCSF(-5.0),
		CCSF(5.0),
		CCS_LINEAR,
		CCSF(0.0),
		distribs);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_INTEGER,
		CCSI(-5),
		CCSI(5),
		CCS_LINEAR,
		CCSI(0),
		distribs + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_multivariate_distribution(
		num_distribs,
		distribs,
		&distrib);
	assert( err == CCS_SUCCESS );

	err = ccs_distribution_soa_samples(distrib, rng, num_samples, samples);
	assert( err == CCS_SUCCESS );

	for (size_t i = 0; i < num_samples; i++) {
		assert( samples1[i].f >= -5.0 );
		assert( samples1[i].f <   5.0 );
		assert( samples2[i].i >= -5 );
		assert( samples2[i].i <   5 );
	}
	double mean = gsl_stats_mean((double*)samples1, 1, num_samples);
	assert( mean < 0.0 + 0.1 );
	assert( mean > 0.0 - 0.1 );

	for (size_t i = 0; i < num_distribs; i++) {
		err = ccs_release_object(distribs[i]);
		assert( err == CCS_SUCCESS );
	}
	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

void test_distribution_hyperparameters_sample() {
	const size_t         num_distribs = NUM_DISTRIBS;
	ccs_distribution_t   distrib = NULL, distribs[NUM_DISTRIBS];
	ccs_hyperparameter_t params[NUM_DISTRIBS];
	ccs_rng_t            rng = NULL;
	ccs_result_t         err = CCS_SUCCESS;
	const size_t         num_samples = NUM_SAMPLES;
	ccs_datum_t          samples[NUM_SAMPLES*NUM_DISTRIBS];

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_FLOAT,
		CCSF(-4.0),
		CCSF(4.0),
		CCS_LINEAR,
		CCSF(0.0),
		distribs);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_FLOAT,
		CCSF(-3.0),
		CCSF(5.0),
		CCS_LINEAR,
		CCSF(0.0),
		distribs + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_numerical_hyperparameter("param1", CCS_NUM_FLOAT,
	                                          CCSF(-5.0), CCSF(5.0),
	                                          CCSF(0.0), CCSF(0.0),
	                                          NULL, params);
	assert( err == CCS_SUCCESS );

	err = ccs_create_numerical_hyperparameter("param2", CCS_NUM_FLOAT,
	                                          CCSF(-4.0), CCSF(6.0),
	                                          CCSF(0.0), CCSF(1.0),
	                                          NULL, params + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_multivariate_distribution(
		num_distribs,
		distribs,
		&distrib);
	assert( err == CCS_SUCCESS );

	err = ccs_distribution_hyperparameters_samples(distrib, rng, params, num_samples, samples);
	assert( err == CCS_SUCCESS );

	for (size_t i = 0; i < num_samples; i++) {
		assert( samples[NUM_DISTRIBS*i].type == CCS_FLOAT );
		assert( samples[NUM_DISTRIBS*i].value.f >= -5.0 );
		assert( samples[NUM_DISTRIBS*i].value.f <   5.0 );
		assert( samples[NUM_DISTRIBS*i + 1].type == CCS_FLOAT );
		assert( samples[NUM_DISTRIBS*i + 1].value.f >= -4.0 );
		assert( samples[NUM_DISTRIBS*i + 1].value.f <   6.0 );
	}
	double mean = gsl_stats_mean((double*)samples, NUM_DISTRIBS*2, num_samples);
	assert( mean < 0.0 + 0.1 );
	assert( mean > 0.0 - 0.1 );

	mean = gsl_stats_mean((double*)samples + 2, NUM_DISTRIBS*2, num_samples);
	assert( mean < 1.0 + 0.1 );
	assert( mean > 1.0 - 0.1 );

	for (size_t i = 0; i < num_distribs; i++) {
		err = ccs_release_object(distribs[i]);
		assert( err == CCS_SUCCESS );
		err = ccs_release_object(params[i]);
		assert( err == CCS_SUCCESS );
	}
	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

void test_distribution_hyperparameters_sample_oversampling() {
	const size_t         num_distribs = NUM_DISTRIBS;
	ccs_distribution_t   distrib = NULL, distribs[NUM_DISTRIBS];
	ccs_hyperparameter_t params[NUM_DISTRIBS];
	ccs_rng_t            rng = NULL;
	ccs_result_t         err = CCS_SUCCESS;
	const size_t         num_samples = NUM_SAMPLES;
	ccs_datum_t          samples[NUM_SAMPLES*NUM_DISTRIBS];

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );

	err = ccs_create_normal_distribution(
		CCS_NUM_FLOAT,
		0.0,
		4.0,
		CCS_LINEAR,
		CCSF(0.0),
		distribs);
	assert( err == CCS_SUCCESS );

	err = ccs_create_normal_distribution(
		CCS_NUM_FLOAT,
		1.0,
		4.0,
		CCS_LINEAR,
		CCSF(0.0),
		distribs + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_numerical_hyperparameter("param1", CCS_NUM_FLOAT,
	                                          CCSF(-5.0), CCSF(5.0),
	                                          CCSF(0.0), CCSF(0.0),
	                                          NULL, params);
	assert( err == CCS_SUCCESS );

	err = ccs_create_numerical_hyperparameter("param2", CCS_NUM_FLOAT,
	                                          CCSF(-4.0), CCSF(6.0),
	                                          CCSF(0.0), CCSF(1.0),
	                                          NULL, params + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_multivariate_distribution(
		num_distribs,
		distribs,
		&distrib);
	assert( err == CCS_SUCCESS );

	err = ccs_distribution_hyperparameters_samples(distrib, rng, params, num_samples, samples);
	assert( err == CCS_SUCCESS );

	for (size_t i = 0; i < num_samples; i++) {
		assert( samples[NUM_DISTRIBS*i].type == CCS_FLOAT );
		assert( samples[NUM_DISTRIBS*i].value.f >= -5.0 );
		assert( samples[NUM_DISTRIBS*i].value.f <   5.0 );
		assert( samples[NUM_DISTRIBS*i + 1].type == CCS_FLOAT );
		assert( samples[NUM_DISTRIBS*i + 1].value.f >= -4.0 );
		assert( samples[NUM_DISTRIBS*i + 1].value.f <   6.0 );
	}
	double mean = gsl_stats_mean((double*)samples, NUM_DISTRIBS*2, num_samples);
	assert( mean < 0.0 + 0.1 );
	assert( mean > 0.0 - 0.1 );

	mean = gsl_stats_mean((double*)samples + 2, NUM_DISTRIBS*2, num_samples);
	assert( mean < 1.0 + 0.1 );
	assert( mean > 1.0 - 0.1 );

	for (size_t i = 0; i < num_distribs; i++) {
		err = ccs_release_object(distribs[i]);
		assert( err == CCS_SUCCESS );
		err = ccs_release_object(params[i]);
		assert( err == CCS_SUCCESS );
	}
	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

int main() {
	ccs_init();
	test_create_multivariate_distribution();
	test_multivariate_distribution();
	test_multivariate_distribution_strided_samples();
	test_multivariate_distribution_soa_samples();
	test_distribution_hyperparameters_sample();
	test_distribution_hyperparameters_sample_oversampling();
	ccs_fini();
	return 0;
}

ext: c

max_stars_repo_name: deephyper/CCS

max_issues_repo_name: deephyper/CCS

max_forks_repo_name: deephyper/CCS


text:
#ifndef LIBGAD
#define LIBGAD
#ifndef NOPOT
#define POTENTIAL
#endif
#include <stdio.h>
#include <math.h>

#ifndef NOGSL
#ifndef GSL
#define GSL
#endif
#endif

#ifdef GSL
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_linalg.h>
#endif

#define	MIN(a, b)   ((a)<(b)?(a):(b))
#define	MAX(a, b)   ((a)>(b)?(a):(b))
#define ABS(a) ((a) >= 0 ? (a) : -(a))
#define SQR(x) ((x)*(x))
#define OMEGA_M 0.26
#define OMEGA_L 0.74
#define HUB 0.72
#define GRAV 6.6742e-11
#define MSUN 1.989e30
#define KPC 3.085678e19  //meters
#define sec_per_yr 3.155e7
#define GALAGE(a) galage(a, OMEGA_M, OMEGA_L, HUB)
#define TIMEDIFF(a,b) timediff(a, b, OMEGA_M, OMEGA_L, HUB)

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#ifdef LONGIDS
typedef unsigned long long IDtype;
#else
typedef unsigned int IDtype;
#endif

typedef float fltarr[3];
struct header
  {
    int npart[6];
    double massarr[6];
    double time;
    double redsh;
    int flg_sfr;
    int flg_fdbck;
    int nall[6];
    int flag_cool;
    int numfiles;
    double boxsize;
    double omega0;
    double omegal;
    double hubparam;
    int flg_age;
    int flg_mtl;
    int bytesleft[22];
};

typedef struct sphdata
{
  float u;
  float rho;
  float nelec;
  float nh;
  float hsml;
  float sfr;
#ifdef WINDS
  float dtime;
  float metals[4];
  float tmax;
  float n_spawn;
#endif //WINDS
#ifdef METALS
  float temp;
#endif
} sphdata;

#if defined(WINDS) || defined(METALS)
typedef struct stardata
{
#ifdef WINDS
  float metals[4];
  float tmax;
  float n_spawn;
#else
  int let;
  float initialmass;
#endif
} stardata;
#endif //WINDS || METALS

typedef struct gadpart
{
  fltarr pos;
#ifndef NOVEL
  fltarr vel;
#endif //NOVEL
  float mass;
  IDtype id;
  short type;
#ifndef NOGAS
  sphdata *sph;
  float stellarage;
#endif
#ifdef POTENTIAL
  float pot;
#endif

#if defined(WINDS) || defined(METALS)
  stardata *sd;
#endif //WINDS

#ifdef METALS
  float* metals;
#endif
} gadpart;

typedef struct gadpart_dist
{
  gadpart part;
  double dist;
} gadpart_dist;

int cmp_id (const void *first, const void *second);
int cmp_pointer_id(const void *a, const void *b);
int cmp_type (const void *first, const void *second);
int cmp_pos (const void *first, const void *second);
int cmp_x (const void *first, const void *second);
int cmp_y (const void *first, const void *second);
int cmp_z (const void *first, const void *second);
int cmp_dist (const void *first, const void *second);
int cmp_int (const void *first, const void *second);
int cmp_float (const void *first, const void *second);
unsigned int readgadget(char *filename, struct header *h, fltarr **p, fltarr **v, int **n, float **m);
unsigned int readgadget_part(char *filename, struct header *h,struct gadpart **part);
#ifdef LONGIDS
unsigned int writegadget(char *filename, struct header h, fltarr *p, fltarr *v, long *n, float *m);
#else
unsigned int writegadget(char *filename, struct header h, fltarr *p, fltarr *v, int *n, float *m);
#endif
unsigned int writegadget_part(char *filename, struct header h, struct gadpart *part);
unsigned int readgadget_novel(char *filename, struct header *h, fltarr **p, int **n, float **m);
unsigned int readgadget_sph(char *filename, struct header *h, fltarr **p, fltarr **v, int **n, float **m, float **u, float **rho, float **d1, float **d2, float **d3, float **d4 , float **sa);
int convertunits(struct header *head, struct gadpart *part, double convert_mass, double convert_distance);
double distance(fltarr a, fltarr b);
double distance_nopb(fltarr a, fltarr b);
double distbox(fltarr a, fltarr min, fltarr max);
double distbox_nopb(fltarr a, fltarr min, fltarr max);
void cpygadpart(gadpart * to, gadpart * from);
int gadsearch(gadpart_dist *data, double toFind, int start, int end);
double nfwfit(double *par, gadpart_dist *part, int cnt, double rv, double soft, double *rcs);
double densproffit(double *par, gadpart_dist *part, int cnt, double re, double soft, double *rcs, int type);
void calcdist(gadpart_dist *gd, int cnt, float *center);
void simplecenter(gadpart *part, int cnt, double* cm, int use);
void pcenter(gadpart_dist *part, int cnt, double, float*, int);
void findcenter(gadpart *part, int cnt, double maxdist, int use);
void simplecm(gadpart ** part, int cnt, float * cm);
double r200(gadpart_dist* pd, int cnt, double denscontrast, struct header h, int *vcnt, double *mvir);
double xoffset(gadpart_dist* pd, int cnt, float rvir, float* center);
#ifndef NOGAS
double temperature(const gadpart part);
#endif //NOGAS
void dummyfunction();
double galage(double z, double omegam, double omegal, double h);
double timediff(double z1, double z2, double omegam, double omegal, double h);
double a2z(double a);
double z2a(double z);
double angle(fltarr a, fltarr b);
double radvel(fltarr vel, fltarr rad);
struct header cphead(struct header head, gadpart* part, int cnt);

#ifdef GSL
void rotatepart(gadpart *part, int numpart, const gsl_matrix *rotmat);
void xrotate(double angle, gadpart *part, int numpart);
void yrotate(double angle, gadpart *part, int numpart);
void zrotate(double angle, gadpart *part, int numpart);
void rotategalaxy(gadpart *part, int numpart, double rad, int use, double *res, gsl_matrix **rotation);
#endif

#endif //LIBGAD


text:
/*
 * This file implements a Python extension, wrapping the capabilities of the "RADAR5" FORTRAN code.
 */

#include <stdio.h>
#include <assert.h>
#include <stdlib.h>
#include <math.h>
#include <limits.h>
#include <Python.h>
#include "capsulethunk.h"

#define WITH_TCC

#ifdef WITH_TCC
#include "tcc/libtcc.h"
#endif

#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
#include <numpy/arrayobject.h>

#ifdef _MSC_VER
#	define GSL_DLL
#	define WIN32
#endif

#ifdef WITH_GSL
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#else
#include "cspline.c"
#endif

#include "radar5.h"

#ifndef MIN
#define MIN(a,b) (((a)>(b))?(b):(a))
#endif
#ifndef MAX
#define MAX(a,b) (((a)>(b))?(a):(b))
#endif

typedef void (*equ_c_t)(double *F, const double *Y, double X, double (*lag)(int, int, void*), void *ctx, void *params);
typedef double (*lag_c_t)(double X, const double *Y);

typedef struct
{
	double *X,*Y;
	ARGLAG_t ARGLAG;
	PHI_t PHI;
	double *RPAR;
	int *IPAR;
	double *PAST;
	int *IPAST, *NRDS;
	int lag_flag[10];
	int ipos[10];
	double alphas[10];
}  _lag_ctx;

typedef struct params_ params_t;
struct params_
{
	PyObject_HEAD

	int full_output;
	int xpos;
	double *xvalues;
	int xvalues_len;

	double *array;
	unsigned int array_size;
	unsigned int array_pos;
	unsigned int array_incr;

	PyObject *py_fcn;
	equ_c_t c_fcn;

	PyObject *py_y0[10];
	double constant_y0[10];
	PyObject *py_lagfuns[10];
	lag_c_t c_lagfuns[10];
	double constant_lags[10];
	PyThreadState *_thread_save;
	int interactive;
	int fail;
	void *user_params;

	PyObject *py_rpar;
	PyObject *py_lag_callback;

#if WITH_GSL
	gsl_interp_accel *spline_acc;
	gsl_spline **y0_spline;
#else
	cspline_t *y0_spline;
#endif
	double y0_t0;

	_lag_ctx *lag_ctx;

	params_t *next, *prev;
};

static double _lag_c(int il, int ip, _lag_ctx *ctx)
{
	int IL = il + 1, IP = ip + 1;

	/* Store the result of LAGR5 for a little speedup if a delay is used for more than one variable... */
	if(ctx->lag_flag[il] == 0)
	{
		lagr5_(&IL,ctx->X,ctx->Y,ctx->ARGLAG,ctx->PAST,&ctx->alphas[il],&ctx->ipos[il],ctx->RPAR,ctx->IPAR,ctx->PHI,ctx->IPAST,ctx->NRDS);
		ctx->lag_flag[il] = 1;
	}

	/* Effective computation of the delayed value for this variable. */
	return ylagr5_(&IP,&ctx->alphas[il],&ctx->ipos[il],ctx->PHI,ctx->RPAR,ctx->IPAR,ctx->PAST,ctx->IPAST,ctx->NRDS);
}

static PyObject *_lag_py(PyObject *self, PyObject *args)
{
	int il, ip;
	params_t *p = PyCapsule_GetPointer(self, NULL);
	PyArg_ParseTuple(args, "ii", &il, &ip);
	return PyFloat_FromDouble(_lag_c(il, ip, p->lag_ctx));
}

static void FCN(int *N, double *X, double *Y, double *F, ARGLAG_t ARGLAG, PHI_t PHI, double *RPAR, int *IPAR, double *PAST,int *IPAST, int *NRDS)
{
	int i, len;
	PyObject *seq;
	PyObject *ret;
	PyObject *y_array;
	npy_intp dims[1] = {*N};
	params_t *p = (params_t*)IPAR;
	_lag_ctx ctx = {X,Y,ARGLAG,PHI,RPAR,IPAR,PAST,IPAST,NRDS,{0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0}};

	/* If it's a C function then it's easy, just run it. */
	if(p->c_fcn)
	{
		p->c_fcn(F,Y,*X,(void*)_lag_c,&ctx,RPAR);
		return;
	}

	if(p->fail)
		return;

	/* If it's a Python function, then we have to tell Python we're going to maybe modify things. */
	if(!p->interactive)
	{
		PyEval_RestoreThread(p->_thread_save);
		p->_thread_save = NULL;
	}

	p->lag_ctx = &ctx;

	/* Create an array from Y values */
	y_array = PyArray_SimpleNewFromData(1,dims,NPY_DOUBLE,Y);

	/* Call the function, with following signature : func(Y, X, lagfun, params if any) */
	if(p->py_rpar)
		ret = PyObject_CallFunctionObjArgs(p->py_fcn, y_array, PyFloat_FromDouble(*X), p->py_lag_callback, p->py_rpar, NULL);
	else
		ret = PyObject_CallFunctionObjArgs(p->py_fcn, y_array, PyFloat_FromDouble(*X), p->py_lag_callback, NULL);

	if(!ret)
	{
		PyErr_Print();
		Py_DECREF(y_array);
		if(!p->interactive) p->_thread_save = PyEval_SaveThread();
		p->fail = 1;
		return;
	}

	Py_DECREF(y_array);

	seq = PySequence_Fast(ret, "Equation must return a sequence !");
	if(!seq)
	{
		PyObject *a, *b, *c;
		double dy;

		PyErr_Fetch(&a, &b, &c);
		PyErr_Clear();

		dy = PyFloat_AsDouble(ret);

		if(!PyErr_Occurred() && *N == 1)
		{
			if(a) Py_DECREF(a);
			if(b) Py_DECREF(b);
			if(c) Py_DECREF(c);
			F[0] = dy;
			goto cleanup;
		}
		else
		{
			PyErr_Restore(a, b, c);
			if(!p->interactive) p->_thread_save = PyEval_SaveThread();
			p->fail = 4;
			return;
		}
	}

	len = PySequence_Size(ret);

	if(len != *N)
	{
		PySys_WriteStderr("Equation returned a sequence of wrong size (%d, expected %d)...\n",len,*N);
		PyErr_SetString(PyExc_RuntimeError, "Equation function returned bad F(Y) length.");
		p->fail = 5;
		if(!p->interactive) p->_thread_save = PyEval_SaveThread();
		Py_DECREF(ret);
		return;
	}

	/* Give the output values to the integrator */
	for(i = 0; i < *N; i++)
	{
		PyObject *item = PySequence_Fast_GET_ITEM(seq, i);
		F[i] = PyFloat_AsDouble(item);
	}

	Py_DECREF(seq);

cleanup:
	Py_DECREF(ret);

	if(!p->interactive) p->_thread_save = PyEval_SaveThread();
}

static double PHI(int *I, double *X, double *RPAR, int *IPAR)
{
	PyObject *ret;
	int i = *I - 1;
	params_t *p = (params_t*)IPAR;
	PyObject *fun = p->py_y0[i];
	double y0;

	if(fun)
	{
		if(!p->interactive && p->_thread_save) {
			PyEval_RestoreThread(p->_thread_save);
		}

		ret = PyObject_CallFunction(fun, "d", *X);

		if(!ret || PyErr_Occurred())
		{
			PyErr_Print();
			p->fail = 1;
			if(!p->interactive && p->_thread_save) p->_thread_save = PyEval_SaveThread();
			return 0;
		}
		y0 = PyFloat_AsDouble(ret);
		if(PyErr_Occurred())
		{
			PyErr_Print();
			p->fail = 4;
		}
		Py_DECREF(ret);

		if(!p->interactive && p->_thread_save) p->_thread_save = PyEval_SaveThread();
		return y0;
	}
	else if(p->y0_spline)
	{
#ifdef WITH_GSL
		return gsl_spline_eval (p->y0_spline[i], *X + p->y0_t0, p->spline_acc);
#else
		double y = 0;
		cspline_eval(&p->y0_spline[i], *X + p->y0_t0, &y);
		return y;
#endif
//		fprintf(stderr,"PHI(%g->%g)[%d]=%g\n",*X,*X+p->y0_t0,i,y);
	}
	else
	{
		return p->constant_y0[i];
	}
}

static double ARGLAG(int *IL, double *X, double *Y,double *RPAR, int *IPAR, PHI_t PHI, double *PAST, int *IPAST, int *NRDS)
{
	PyObject *ret;
    int ilag = *IL-1;
    params_t *p = (params_t*)IPAR;
    PyObject *pyfun = p->py_lagfuns[ilag];
	lag_c_t cfun = p->c_lagfuns[ilag];
	double r;

    if(!pyfun)
    {
        if(!cfun)
        {
            return *X-p->constant_lags[ilag];
        }
        else
        {
            return cfun(*X,Y);
        }
    }
    else
    {
        if(!p->interactive && p->_thread_save) PyEval_RestoreThread(p->_thread_save);
        ret = PyObject_CallFunction(pyfun, "dd", *X, *Y);
        if(!ret || PyErr_Occurred())
        {
            PyErr_Print();
        }

		r = PyFloat_AsDouble(ret);
        Py_DECREF(ret);
//          fprintf(stderr,"Lag %d: %p -> %f\n", ilag, fun, r);
	if(!p->interactive && p->_thread_save) p->_thread_save = PyEval_SaveThread();
        return r;
    }
}

static void JFCN(int *N, double *X, double *Y, double *DFY, int LDFY, ARGLAG_t ARGLAG, PHI_t PHI, double *RPAR, int *IPAR, double *PAST,int *IPAST, int *NRDS)
{
    fprintf(stderr,"JFCN\n");
}
static void JACLAG(int *N, double *X, double *Y, int *DFYL, ARGLAG_t ARGLAG, PHI_t PHI, int *IVE, int *IVC, int *IVL, double *RPAR, int *IPAR, double *PAST, int *IPAST, int *NRDS)
{
    fprintf(stderr,"JACLAG\n");
}

static void SOLOUT(int *NR, double *XOLD, double *X, double *HSOL, double *Y, double *CONT, int *LRC, int *N, double *RPAR, int *IPAR, int *IRTRN)
{
    int i;
    params_t *p = (params_t*)IPAR;
	double *ptr;
	double *new_array;
	double next_x;

    if(p->interactive)
    {
        if(PyErr_CheckSignals())
        {
            p->fail = 6;
        }
    }

    if(p->fail)
    {
        *IRTRN = -1;
        return;
    }

//   fprintf(stderr,"h=%g, dx=%g\n",*HSOL,*X-*XOLD);
//     fprintf(stderr,"SOLOUT %f %f / %p %d %d %d\n",*X,*Y, p->array,p->array_size,p->array_pos,p->array_incr);

    if(p->xvalues)
    {
	    if(*NR == 1)
	    {
			ptr = p->array + p->array_pos * (*N+1);
			ptr[0] = *X;

			for(i = 0; i < *N; i++)
			{
				ptr[i + 1] = Y[i];
			}

			p->array_pos += 1;
			p->xpos += 1;
	    }

	    next_x = p->xvalues[p->xpos];

	    while(*X >= next_x && p->xpos < p->xvalues_len)
	    {
			if(p->array_size == p->array_pos)
			{
				p->array_size += p->array_incr;
			//         fprintf(stderr,"realloc %d->%d (%i)", p->array_size - p->array_incr, p->array_size , p->array_size*(*N+1)*sizeof(double));
				new_array = PyMem_Realloc(p->array, p->array_size*(*N+1)*sizeof(double));
				if(new_array == NULL)
				{
					p->fail = 2;
					*IRTRN = -1;
					return;
				}
			//         fprintf(stderr," -> %p\n", new_array);
				p->array = new_array;
			}

			ptr = p->array + p->array_pos * (*N+1);
			ptr[0] = next_x;

			for(i = 1; i < *N + 1; i++)
			{
				ptr[i] = contr5_(&i, N, &next_x, CONT, X, HSOL);
			}

			p->array_pos += 1;
			p->xpos += 1;
			next_x = p->xvalues[p->xpos];
	    }
    }

    if(p->full_output)
    {
		if(p->array_size == p->array_pos)
		{
			p->array_size += p->array_incr;
		//         fprintf(stderr,"realloc %d->%d (%i)", p->array_size - p->array_incr, p->array_size , p->array_size*(*N+1)*sizeof(double));
			new_array = PyMem_Realloc(p->array, p->array_size*(*N+1)*sizeof(double));
			if(new_array == NULL)
			{
				p->fail = 2;
				*IRTRN = -1;
				return;
			}
		//         fprintf(stderr," -> %p\n", new_array);
			p->array = new_array;
		}

		ptr = p->array + p->array_pos * (*N+1);
		ptr[0] = *X;

		for(i = 0; i < *N; i++)
		{
			ptr[i+1] = Y[i];
		}

		p->array_pos += 1;
    }
}

void _tcc_error(void *opaque, const char *msg)
{
	PyErr_SetString(PyExc_RuntimeError, msg);
	//PySys_WriteStderr("! %s\n", msg);
}

// radar5(equ,y0,xend,[rpar],grid=grid)

static params_t *current_p = NULL;

static PyMethodDef py_lag_def = {"lag", _lag_py, METH_VARARGS, "Returns delayed values of variables inside right-hand-side function."};

static PyObject *radar5_radar5(PyObject *self, PyObject *args,PyObject *keywds)
{
	PyObject *output_array = NULL;
	int verbose = 0;
	int NGRID = 200;
	double XEND = 100;
	int N = 0;
        int MXST = 4*16384;
	params_t par;
	params_t *p = ∥
	int *IPAR = (void*)p;

	double X = 0;
	double *Y = NULL;

	double H = 1e-6;
	double RTOL = 1e-6;
	double ATOL = RTOL;
	double ITOL = 0;
	int IJAC = 0;
	int MLJAC = N;
	int MUJAC = 0; // not needed
	int NLAGS = 0;
	int NJACL = 0;
	int IMAS = 0;
	int IOUT = verbose ? 1 : 3;
	double WORK[30];
	int IWORK[30];
	double *RPAR = NULL;

	int IDID = 0;

	int *IPAST = NULL;
	int DUMMY;
	int MLMAS = N;
	int MUMAS = 0; // not needed

	int RPAR_len = 0;

	double *GRID = NULL;

	int i;

	int prev_interactive;

	PyObject* rpar_obj=NULL;
	PyObject* lagvars_obj=NULL;
	PyObject* lagfuns_obj=NULL;
	PyObject* equ_obj=NULL;
	PyObject* y0_obj=NULL;
	PyObject* xend_obj=NULL;

	int full_output_flag = 0;

	PyObject *py_p = NULL;

	int len;

#ifdef WITH_TCC
	TCCState *tcc = NULL;
#endif

	/* Parse arguments */
	static char *kwlist[] = {"equ", "y0", "xend", "rpar", "ngrid", "lagvars", "lags", "verbose", "mxst", "full_output", "rtol", "atol", "initial_stepsize", NULL};

	memset(p,0,sizeof(params_t));

	if (!PyArg_ParseTupleAndKeywords(args, keywds, "OOO|OiOOiiifff", kwlist, &equ_obj, &y0_obj, &xend_obj, &rpar_obj, &NGRID, &lagvars_obj, &lagfuns_obj, &verbose, &MXST,&full_output_flag,&RTOL,&ATOL,&H))
		return NULL;

	if(verbose) PySys_WriteStderr("RADAR5 wrapper (verbose = %d)\n", verbose);

	/* SPECIFIED X VALUES OR XEND */
	if(xend_obj)
	{
		PyArrayObject *xvalues = (PyArrayObject*)PyArray_ContiguousFromAny(xend_obj, NPY_DOUBLE, 0, 1);
		if(xvalues == NULL)
		{
			PyErr_SetString(PyExc_ValueError, "Cannot parse x values.");
			goto cleanup;
		}

		p->full_output = full_output_flag;	/* Full output only if specified... */

		if(PyArray_NDIM(xvalues) == 0)	/* if a single value if given for X, force full output */
		{
			p->xvalues = NULL;
			p->full_output = 1;
			XEND = *(double*)PyArray_GETPTR1(xvalues, 0);
			if(verbose) PySys_WriteStderr("Full output up to X=%g (all points)\n", XEND);
		}
		else
		{
			p->xvalues = (double*)PyArray_GETPTR1(xvalues, 0);
			p->xvalues_len = PyArray_DIM(xvalues, 0);
			p->xpos = 0;
			XEND = p->xvalues[p->xvalues_len - 1];
			if(verbose) PySys_WriteStderr("%s output up to X=%g (%d points)\n", p->full_output ? "Full" : "User-specified", XEND, p->xvalues_len);
		}
	}

	/* LOADING INITIAL VALUES : */

	if(y0_obj)
	{
		PyObject *seq = PySequence_Fast(y0_obj, "Initial values must be provided as a sequence.");
		if(!seq)
			return NULL;
		len = PySequence_Size(y0_obj);

		if(len == 2)
		{
			/* It might be a tuple (time, values) */
			PyObject *time = PySequence_Fast_GET_ITEM(seq, 0);
			PyObject *values = PySequence_Fast_GET_ITEM(seq, 1);

			/* Check each of (time, values) is iterables. If it is, we suppose we can use them as array. */
			if(PySequence_Check(time) && PySequence_Check(values))
			{
				int y0_len;

				PyArrayObject *time_array = (PyArrayObject*)PyArray_ContiguousFromAny(time, NPY_DOUBLE, 1, 1);
				PyArrayObject *values_array = (PyArrayObject*)PyArray_ContiguousFromAny(values, NPY_DOUBLE, 1, 2);

				/* Account for format errors */
				if(!time_array || !values_array)
				{
					PyErr_Print();
					return NULL;
				}

				y0_len = PyArray_DIM(time_array, 0);	/* Get initial sequence length */

				if(PyArray_NDIM(values_array) == 1) N = 1;
				else N = PyArray_DIM(values_array, 1);	/* Get system dimension from initial data */

				p->y0_t0 = *(double*)PyArray_GETPTR1(time_array, y0_len - 1);	/* Get the final time of the initial sequence */
				if(verbose) PySys_WriteStderr("Initial values array as (time->%g, [i0:%d])\n", p->y0_t0, y0_len);
#ifdef WITH_GSL
				p->y0_spline = (gsl_spline**)malloc(sizeof(void*) * N);
#else
				p->y0_spline = (cspline_t*)malloc(sizeof(cspline_t) * N);
#endif

				for(i = 0; i < N; i++)
				{
					int j;
#ifdef WITH_GSL
					gsl_spline *spline = gsl_spline_alloc(gsl_interp_cspline, y0_len);
#else
					cspline_t *spline = &p->y0_spline[i];
					cspline_init(spline);
					cspline_alloc_xy(spline, y0_len);
#endif

					/* Copy time data */
					memcpy (spline->x, (double*)PyArray_DATA(time_array), y0_len * sizeof(double));

					/* Copy variable data */
					for(j = 0; j < y0_len; j++)
					{
						spline->y[j] = *(double*)PyArray_GETPTR2(values_array, j, i);
					}

					/* Initialize interpolator */
#ifdef WITH_GSL
					gsl_interp_init (spline->interp, spline->x, spline->y, y0_len);
					p->y0_spline[i] = spline;
#else
					cspline_compute_nat (spline);
#endif
				}

				Py_DECREF(time_array);
				Py_DECREF(values_array);
			}
		}
		else if(len > 0 && PySequence_Check(PySequence_Fast_GET_ITEM(seq, 0)))
		{
			int y0_len;

			/* Looks like a 2D array : time data might be its first row. This is useful to "continue" an integration, because it's the integrator output format. */
			PyArrayObject *full_array = (PyArrayObject*)PyArray_ContiguousFromAny(y0_obj, NPY_DOUBLE, 2, 2);
			if(PyErr_Occurred())
			{
				Py_DECREF(seq);
				goto cleanup;
			}

			y0_len = PyArray_DIM(full_array, 0);
			N = PyArray_DIM(full_array, 1) - 1;

			p->y0_t0 = *(double*)PyArray_GETPTR2(full_array, y0_len - 1, 0);
#ifdef WITH_GSL
			p->y0_spline = (gsl_spline**)malloc(sizeof(void*) * N);
#else
			p->y0_spline = (cspline_t*)malloc(sizeof(cspline_t) * N);
#endif

			if(verbose) PySys_WriteStderr("Initial values array as [time->%g, y0:%d]\n", p->y0_t0, y0_len);

			for(i = 0; i < N; i++)
			{
				int j;
#ifdef WITH_GSL
				gsl_spline *spline = gsl_spline_alloc(gsl_interp_cspline, y0_len);
#else
				cspline_t *spline = &p->y0_spline[i];
				cspline_init(spline);
				cspline_alloc_xy(spline, y0_len);
#endif

				for(j = 0; j < y0_len; j++)
				{
					spline->x[j] = *(double*)PyArray_GETPTR2(full_array, j, 0);
					spline->y[j] = *(double*)PyArray_GETPTR2(full_array, j, i + 1);
				}

#ifdef WITH_GSL
				gsl_interp_init (spline->interp, spline->x, spline->y, y0_len);
				p->y0_spline[i] = spline;
#else
				cspline_compute_nat (spline);
#endif
			}

			Py_DECREF(full_array);
		}

		if(!p->y0_spline)
		{
			/* Each variable is specified on its own. */
			for (i = 0; i < len; i++)
			{
				PyObject *item = PySequence_Fast_GET_ITEM(seq, i);

				/* Check whether it's a Python function or a constant value. */
				if(PyCallable_Check(item))
				{
					p->py_y0[i] = item;
					if(verbose)
					{
						PyObject *repr = PyObject_Repr(item);
#if PY_MAJOR_VERSION >= 3
						PySys_WriteStderr("Initial value: y[%d](t<0) = %s\n", i, PyUnicode_AsUTF8(repr));
#else
						PySys_WriteStderr("Initial value: y[%d](t<0) = %s\n", i, PyString_AsString(repr));
#endif
						Py_DECREF(repr);
					}
				}
				else
				{
					double y0 = PyFloat_AsDouble(item);
					if(PyErr_Occurred())
					{
						Py_DECREF(seq);
						goto cleanup;
					}
					p->constant_y0[i] = y0;
					p->py_y0[i] = NULL;
					if(verbose) PySys_WriteStderr("Initial value: y[%d](t<0) = %g (constant)\n", i, y0);
				}
			}

			N = len;
		}

		Py_DECREF(seq);
	}

	if(N == 0)
	{
		PyErr_SetString(PyExc_ValueError, "Could not guess system dimension from initial values.");
		goto cleanup;
	}

	if(verbose) PySys_WriteStderr("System dimension: N = %d\n", N);

	Y = (double*)malloc(N * sizeof(double));
	IPAST = (int*)malloc(N * sizeof(int));

	/* Set all integrator parameters to default values. */
	for(i = 0; i < 30; i++)
	{
		WORK[i] = 0;
		IWORK[i] = 0;
	}

	/* Load system numerical parameters. */
	if(rpar_obj)
	{
		PyObject *seq = PySequence_Fast(rpar_obj, "Constant parameters must be provided as a sequence.");
		if(!seq) goto cleanup;
		RPAR_len = PySequence_Size(rpar_obj);

		RPAR = malloc(sizeof(double) * RPAR_len);

		for (i = 0; i < RPAR_len; i++) {
			PyObject *item = PySequence_Fast_GET_ITEM(seq, i);
			RPAR[i] = PyFloat_AsDouble(item);

			if(verbose) PySys_WriteStderr("Constant parameter %d : %g\n", i,RPAR[i]);
		}
		Py_DECREF(seq);
	}

	if(lagvars_obj)
	{
		PyObject *seq = PySequence_Fast(lagvars_obj, "Delayed variables must be provided as a sequence.");
		if(!seq) goto cleanup;
		len = PySequence_Size(lagvars_obj);

		for (i = 0; i < len; i++) {
			PyObject *item = PySequence_Fast_GET_ITEM(seq, i);
			int lagparam = PyLong_AsLong(item);

			if(lagparam < 0 || lagparam >= N)
			{
				PyErr_SetString(PyExc_ValueError, "Invalid lag parameter");
				Py_DECREF(seq);
				goto cleanup;
			}

			IPAST[i]=lagparam+1;

			if(verbose) PySys_WriteStderr("Variable %d is delayed (#%d)\n", lagparam, i);
		}

		IWORK[14] = i; // number of dense solution (lags)
		Py_DECREF(seq);
	}
	else
	{
		/* By default, all variables are delayed */
		for (i = 0; i < N; i++)
		{
			IPAST[i]=i+1;
		}
		IWORK[14] = N;
		if(verbose) PySys_WriteStderr("Delayed variables unspecified: by default all %d variables are selected.\n", N);
	}

	if(lagfuns_obj)
	{
		PyObject *seq = PySequence_Fast(lagfuns_obj, "Lags must be provided as a sequence.");
		if(!seq) goto cleanup;
		len = PySequence_Size(lagfuns_obj);

		for (i = 0; i < len; i++) {
			PyObject *item = PySequence_Fast_GET_ITEM(seq, i);

			if(PyCallable_Check(item))
			{
				p->py_lagfuns[i]=item;
				p->c_lagfuns[i]=NULL;

				if(verbose)
				{
					PyObject *repr = PyObject_Repr(item);
#if PY_MAJOR_VERSION >= 3
					PySys_WriteStderr("Lag %d is a Python function: %s\n", i, PyUnicode_AsUTF8(repr));
#else
					PySys_WriteStderr("Lag %d is a Python function: %s\n", i, PyString_AsString(repr));
#endif
					Py_DECREF(repr);
				}
			}
			else if(PyNumber_Check(item))
			{
				double delay = PyFloat_AsDouble(item);
				p->constant_lags[i]=delay;
				p->py_lagfuns[i]=NULL;
				p->c_lagfuns[i]=NULL;
				if(verbose) PySys_WriteStderr("Lag %d is constant: %g\n", i, delay);
			}
			else
			{
				p->c_lagfuns[i]=PyCapsule_GetPointer(item, NULL);
				p->py_lagfuns[i]=NULL;
				if(verbose) PySys_WriteStderr("Lag %d is a raw C function: %p\n", i, p->c_lagfuns[i]);
			}
		}

		Py_DECREF(seq);
	}

	/* Set RHS function, either Python or C. */
	if(PyCallable_Check(equ_obj))
	{
		p->py_fcn = equ_obj;
		p->c_fcn = NULL;

		if(verbose)
		{
			PyObject *repr = PyObject_Repr(equ_obj);
#if PY_MAJOR_VERSION >= 3
			PySys_WriteStderr("Equation is a Python function:  %s\n", PyUnicode_AsUTF8(repr));
#else
			PySys_WriteStderr("Equation is a Python function:  %s\n", PyString_AsString(repr));
#endif
			Py_DECREF(repr);
		}

		/* Create a Python wrapper for the function that will be used to get delayed values in RHS */
		py_p = PyCapsule_New(p, NULL, NULL);
		p->py_lag_callback = PyCFunction_New(&py_lag_def, py_p);

		/* We need a Python version of RPAR */
		if(RPAR_len > 0)
		{
			npy_intp dims[1] = {RPAR_len};
			p->py_rpar = PyArray_SimpleNewFromData(1, dims, NPY_DOUBLE, RPAR);
		}
	}
#if PY_MAJOR_VERSION >= 3
	else if(PyUnicode_Check(equ_obj))
	{
#ifdef WITH_TCC
		PyObject *str = PyUnicode_AsASCIIString(equ_obj);
		if(!str)
			goto cleanup;
#endif
#else
	else if(PyString_Check(equ_obj))
	{
		PyObject *str = equ_obj;
#endif
#ifdef WITH_TCC

		/* We need to provide some math functions in order to do useful stuff. */
		const struct
		{
			const char *name;
			void *ptr;
		} symbols[] = {{"cos", cos},	{"sin", sin},	{"tan", tan},
					   {"acos", acos},	{"asin", asin},	{"atan", atan},
					   {"log", log},	{"pow", pow},	{"log10", log10},
					   {"cosh", cosh},	{"sinh", sinh},	{"tanh", tanh}};
		const int nsymbols = sizeof(symbols)/sizeof(symbols[0]);
		char *source = malloc(PyBytes_Size(str) + nsymbols * 128 + 128);

		/* This one can be useful too. */
		int pos = sprintf(source, "#define M_PI 3.14159265358979323846\n");

		for(i = 0; i < nsymbols; i++)
			pos += sprintf(source + pos, "double %s(double);", symbols[i].name);

		sprintf(source + pos, "\n%s", PyBytes_AsString(str));

		if(verbose) PySys_WriteStderr("Equation is C code, try to compile it...\n");

		/* Create a minimal TCC context */
		tcc = tcc_new();
		tcc_set_options(tcc, "-nostdlib");
		tcc_set_error_func(tcc, NULL, _tcc_error);
		tcc_set_output_type(tcc, TCC_OUTPUT_MEMORY);

		/* Compile the code snippet */
		if(tcc_compile_string(tcc, source) < 0)
		{
			free(source);
			goto cleanup;
		}

		//Py_DECREF(str);
		free(source);

		/* Add math symbols for the linking step. */
		for(i = 0; i < nsymbols; i++)
			tcc_add_symbol(tcc, symbols[i].name, symbols[i].ptr);

		/* Linking */
		if(tcc_relocate(tcc, TCC_RELOCATE_AUTO) < 0)
		{
// 			PyErr_SetString(PyExc_RuntimeError, "Equation could not be compiled (relocation error).");
			goto cleanup;
		}

		p->py_fcn = NULL;
		p->c_fcn = tcc_get_symbol(tcc, "equation");		/* Get pointer to the compiled function */

		if(!p->c_fcn)
		{
			p->c_fcn = tcc_get_symbol(tcc, "y");
			if(!p->c_fcn)
			{
				p->c_fcn = tcc_get_symbol(tcc, "rhs");
				if(!p->c_fcn)
				{
					PyErr_SetString(PyExc_ValueError, "No function named 'equation', 'y' or 'rhs' in provided C code.");
					goto cleanup;
				}
			}
		}

		if(verbose) PySys_WriteStderr("Equation successfully compiled : %p\n", p->c_fcn);
#else
		PyErr_SetString(PyExc_RuntimeError, "Equation provided as string, but we were compiled without TCC.");
		goto cleanup;
#endif
	}
	else
	{
		p->py_fcn = NULL;
		p->c_fcn = PyCapsule_GetPointer(equ_obj, NULL);
		if(verbose) PySys_WriteStderr("Equation is a raw C function: %p\n", p->c_fcn);
	}

	GRID = malloc(sizeof(double) * (NGRID + 1));

	if(p->xvalues)
	{
		p->array_size = p->xvalues_len;
	}
	else
	{
		p->array_size = NGRID;    // taille initiale
	}

	p->array = PyMem_Malloc(p->array_size*(N+1)*sizeof(double));
	if(verbose) PySys_WriteStderr("Allocate initial array (size %gkB) at %p\n", p->array_size*(N+1.)*sizeof(double)/1024, p->array);

	p->array_pos = 0;
	p->array_incr = p->array_size / 4;    // increment taille

	p->user_params = RPAR;

	p->fail = 0;
	p->interactive = 0;

	IWORK[13] = 1;      //  ne pas utiliser JACLAG

	/* NOTE: Black magic... */
	IWORK[12] = NGRID;      //  WORKSPACE FOR GRID

//	MXST = 1024*1024*32; //4*16384*IWORK[14]; //MAX(NGRID*IWORK[14],8192); // pourquoi c'était divisé par /N ?
	IWORK[11] = MXST;       // WORKSPACE FOR PAST

	IWORK[1] = INT_MAX;

	for(i = 0; i < NGRID; i++)
	{
		GRID[i] = XEND/(NGRID+1)*i;
	}

        /* Load initial values */
	prev_interactive = p->interactive;
	p->interactive = 1;
	for(i=1;i<N+1;i++)
	{
		Y[i-1] = PHI(&i, &X, RPAR, IPAR);
		if(verbose) PySys_WriteStderr("Initial value: Y[%d]=%g\n", i-1, Y[i-1]);
	}
	p->interactive = prev_interactive;

	/* MAIN CALL TO THE INTEGRATOR AHEAD */

	if(!p->interactive)
	{
		p->_thread_save = PyEval_SaveThread();	/* Promise Python that we are not going to mess with any of its data. */
							/* This allows other threads/event loops to run during the integration. */
	}

	current_p = p;

	radar5_(&N,FCN,PHI,ARGLAG,&X,Y,&XEND,&H, \
					&RTOL,&ATOL,&ITOL, \
					JFCN,&IJAC,&MLJAC,&MUJAC, \
					JACLAG,&NLAGS,&NJACL, \
					&IMAS,SOLOUT,&IOUT, \
					WORK,IWORK,RPAR,IPAR,&IDID, \
					GRID,IPAST,&DUMMY,&MLMAS,&MUMAS);

	current_p = NULL;

	if(!p->interactive)
	{
		PyEval_RestoreThread(p->_thread_save);
	}

	/* DONE ! */

	if(verbose)
	{
		switch(p->fail)
		{
			case 1: PySys_WriteStderr("Python function produced an error.\n"); break;
			case 3: PySys_WriteStderr("Computation halted by stop().\n"); break;
			case 4:
			case 5: PySys_WriteStderr("Python function provided wrong return type.\n"); break;
			case 6: PySys_WriteStderr("Computation halted by an external signal.\n"); break;
		}
	}

	if(p->fail == 2)
	{
		PyErr_SetString(PyExc_MemoryError, "Cannot grow output array.");
	}

	if(verbose)
	{
		PySys_WriteStderr("Solver output: IDID = %d (", IDID);
		switch(IDID)
		{
			case  1: PySys_WriteStderr("Successful computation"); break;
			case  2: PySys_WriteStderr("Successful computation interrupted by routine SOLOUT"); break;
			case -1: PySys_WriteStderr("Non-consistent input values"); break;
			case -2: PySys_WriteStderr("Too many stepsizes required (>NMAX)"); break;
			case -3: PySys_WriteStderr("Stepsize becomes too small"); break;
			case -4: PySys_WriteStderr("Jacobian matrix repeatedly singular"); break;
			case -5: PySys_WriteStderr("Computation interrupted by routine YLAGR5"); break;
			case -6: PySys_WriteStderr("The equation makes use of advanced arguments"); break;
			default: PySys_WriteStderr("Unexpected return code"); break;
		}
		PySys_WriteStderr(")\nComputed points: %d\n", p->array_pos);
	}

	/* Create numpy array based on integrator output : */
	{
		npy_intp dims[2] = {p->array_pos, N+1};
		output_array = PyArray_SimpleNewFromData(2, dims, NPY_DOUBLE, p->array);
	}

#if NPY_API_VERSION >= 9
	PyArray_ENABLEFLAGS((PyArrayObject*)output_array, NPY_ARRAY_OWNDATA);
#else
	((PyArrayObject*)output_array)->flags |= NPY_OWNDATA;
#endif

	/* CLEANUP : */
cleanup:

	if(RPAR) free(RPAR);
	if(Y) free(Y);
	if(IPAST) free(IPAST);
	if(GRID) free(GRID);

	if(p->y0_spline)
	{
		if(verbose) PySys_WriteStderr("Clearing initial values...\n");
		for(i = 0; i < N; i++)
		{
#ifdef WITH_GSL
			if(p->y0_spline[i])
				gsl_spline_free(p->y0_spline[i]);
#else
			cspline_free(&p->y0_spline[i]);
#endif
		}
		free(p->y0_spline);
	}

#ifdef WITH_GSL
	if(p->spline_acc)
	{
		gsl_interp_accel_free (p->spline_acc);
	}
#endif

	if(p->py_lag_callback)
		Py_DECREF(p->py_lag_callback);

	if(py_p)
		Py_DECREF(py_p);

	if(p->py_rpar)
		Py_DECREF(p->py_rpar);

	if(p->xvalues)
		Py_DECREF(p->xvalues);

#ifdef WITH_TCC
	if(tcc)
		tcc_delete(tcc);
#endif

	return output_array;
}

static PyObject *radar5_stop(PyObject *self, PyObject *args)
{
	if(current_p)
	{
		current_p->fail = 3;
	}
	Py_RETURN_NONE;
}

PyDoc_STRVAR(radar5_doc,
"Call the RADAR5 solver, which numerically solves delayed differential equations (DDE) using an implicit collocation Runge-Kutta method based on Radau nodes.\n"
"\n"
"Beware : some functionalities are not implemented yet :\n"
" - providing custom jacobians ;\n"
" - mass matrix ;\n"
" - lots of options...\n"
"\n"
"For more extensive details, see the documentation [1]_ of the original FORTRAN code, which is the work of Nicola Guglielmi and Ernst Hairer.\n"
"More details can be found on : http://www.unige.ch/~hairer/software.html\n"
"\n"
":param equation : callable, pointer to c function, or string\n"
"    Right-hand side of the equation, computing the time derivatives.\n"
"    It is fundamentally a function that takes three or four arguments:\n"
"        * y[i], an array containing the current values:\n"
"        * t, the current time;\n"
"        * lag(i,j), a function that can be called compute delayed variable i with the lag j.\n"
"        * params, an extra array of user-defined parameters\n\n"
"   This function can be a Python callable, but this can be quite slow when integrating large systems. For better performances, a pointer to a C function can be provided. This function can be loaded by ctypes, or be part of a C extension, etc. It must have the following signature:\n"
"     void equation(double *dy, const double *y, double t, double (*lag)(int,int,void*), void* ctx, void *params)\n"
"   The output derivatives should be written in dy, and the extra 'ctx' argument has to be provided as 'lag' third parameters.\n"
"   For more convenience, a string containing C code can also be provided. It must contain a function called 'equation', 'equ', or 'rhs' with the same signature. It will be internally compiled using the 'tcc' compiler. Standard math function (sin, cos, ...) are made available in the global scope.\n"
"\n"
":param y0 : array, or tuple of arrays\n"
"    Provides initial and past values. It can be either a constant, and in that case all t<0 values are the same. If non-constant past values are needed, they must be provided as a tuple (time, values). Missing points will be computed using a cubic spline interpolation. For a more precise control, a list of Python callable computing the past values for each variable can also be used. Finally, a single array can be used, and its first row will be considered as the time values. This is particularly useful for resuming integration from the output of another call.\n"
"\n"
":param time : array, or float\n"
"    Specifies the points on which the result of the integration has to be stored. Note that this does not correspond to the integrating grid, as the solver uses an adaptative step. If a single number is provided, full output is turned on, and all output steps are stored.\n"
"\n"
":param params : array, optional\n"
"    Extra parameters to be provided to the integrating function.\n"
"\n"
":param lagvars : array, optional\n"
"    List of variables subjected to delay. If it is not specified, all variables are considered delayed variable. You can specifiy an empty list or tuple if no delayed variable are needed.\n"
"\n"
":param lags : array, optional\n"
"    List of delays used in the equations. They can be either constant, or functions of the other variables or current time.\n"
"\n"
":param ngrid : int, optional\n"
"    Changes the size of the internal grid. Might need some tuning if integration fails unexpectedly."
":param mxst : int, optional\n"
"    Changes the size of the internal past storage. Might need some tuning if integration fails unexpectedly with 'MXST' related errors."
":param atol : float, optional\n"
"    Changes the absolute tolerance (default is 1e-6)\n"
":param rtol : float, optional\n"
"    Changes the relative tolerance (default is 1e-6)\n"
":param initial_stepsize : float, optional\n"
"    Initial guess for the stepsize; for stiff equations with initial transient, H=1/(norm of F'); usually 1e-3 or 1e-5 is good. (default is 1e-6)\n"
":param verbose : bool, optional\n"
"    Enable printing of troubleshooting informations."
":param full_output : bool, optional\n"
"    Turns on the output of every integration step. This can make the output array much bigger than the specified times.\n"
"\n"
":return: The result of the integration, the first row of which being the \"time\" variable, and the latter the integrated components.\n"
"\n"
".. References :\n"
"[1] N. Guglielmi and E. Hairer, \"Users' Guide for the code RADAR5 - Version 2.1\", Technical Report, July 2005.\n"
);

static PyMethodDef Radar5Methods[] = {
    {"radar5",  (PyCFunction)radar5_radar5, METH_VARARGS|METH_KEYWORDS, radar5_doc},
    {"stop",  (PyCFunction)radar5_stop, METH_VARARGS, "Stop running computation."},
    {NULL, NULL, 0, NULL}        /* Sentinel */
};

#if PY_MAJOR_VERSION >= 3

static struct PyModuleDef Radar5Def = {
        PyModuleDef_HEAD_INIT,
        "radar5",
        NULL,
        -1,
		Radar5Methods
};

#define INITERROR return NULL

#else
#define INITERROR return
#endif

#if PY_MAJOR_VERSION >= 3
PyMODINIT_FUNC PyInit_radar5(void)
{
    PyObject *module = PyModule_Create(&Radar5Def);
#else
PyMODINIT_FUNC initradar5(void)
{
    PyObject *module = Py_InitModule("radar5", Radar5Methods);
#endif

    if (module == NULL)
        INITERROR;

	import_array();

	// TODO : add dummy integration WITH lag so integration without lag works properly...

#if PY_MAJOR_VERSION >= 3
    return module;
#endif
}


text:
#pragma once

#ifdef HAVE_BLAS

// HOTFIX: cblas.h from the atlas-devel package (version 3.10.1-12.el7) on CentOS 7
// does not declare the functions as `extern "C"`, which breaks name mangling.
// Note that nested `extern "C"` is valid and correct:
// https://stackoverflow.com/questions/48099828/what-happens-if-you-nest-extern-c
extern "C" {
#include <cblas.h>
}

inline int blasIgamax( int n, const float *x, int incx )
{
   return cblas_isamax( n, x, incx );
}

inline int blasIgamax( int n, const double *x, int incx )
{
   return cblas_idamax( n, x, incx );
}

/*inline int blasIgamin( int n, const float *x, int incx )
{
   return cblas_isamin( n, x, incx );
}

inline int blasIgamin( int n, const double *x, int incx )
{
   return cblas_idamin( n, x, incx );
}*/

inline float blasGasum( int n, const float *x, int incx )
{
   return cblas_sasum( n, x, incx );
}

inline double blasGasum( int n, const double *x, int incx )
{
   return cblas_dasum( n, x, incx );
}

inline void blasGaxpy( int n, const float alpha,
                       const float *x, int incx,
                       float *y, int incy )
{
   cblas_saxpy( n, alpha, x, incx, y, incy );
}

inline void blasGaxpy( int n, const double alpha,
                       const double* x, int incx,
                       double *y, int incy )
{
   cblas_daxpy( n, alpha, x, incx, y, incy );
}

inline float blasGdot( int n, const float* x, int incx,
                       const float* y, int incy )
{
   return cblas_sdot( n, x, incx, y, incy );
}

inline double blasGdot( int n, const double* x, int incx,
                        const double* y, int incy )
{
   return cblas_ddot( n, x, incx, y, incy );
}

inline float blasGnrm2( int n, const float* x, int incx )
{
   return cblas_snrm2( n, x, incx );
}

inline double blasGnrm2( int n, const double* x, int incx )
{
   return cblas_dnrm2( n, x, incx );
}

inline void blasGscal( int n, const float alpha,
                       float* x, int incx )
{
   cblas_sscal( n, alpha, x, incx );
}

inline void blasGscal( int n, const double alpha,
                       double* x, int incx )
{
   cblas_dscal( n, alpha, x, incx );
}
#endif

max_stars_repo_name: grinisrit/tnl-dev

max_issues_repo_name: grinisrit/tnl-dev

max_forks_repo_name: grinisrit/tnl-dev


text:
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_math.h>

#include <math.h>
#include <options/options.h>

#include "../csm/csm_all.h"

struct ld_noise_params {
	int seed;

	double sigma_xy;
	double sigma_theta_deg;

	const char* file_input;
	const char* file_output;

	int debug;
};

const char * banner =
	"A simple program for adding slip to odometry \n\n"
		"The 'odometry' field is set to 'true_pose' + noise.\n"
		"If 'true_pose' is not available, then the 'odometry' \n"
		"field is set to 'odometry' + noise.\n\n"
		"Note: this program does *not* simulate the effect of \n"
		"slip or odometry error in a realistic way; each scan \n"
		"in the file is considered separately, the error does \n"
		"not depend on the traveled distance, etc.\n\n";

int main(int argc, const char ** argv) {
	sm_set_program_name(argv[0]);

	struct ld_noise_params p;

	options_banner(banner);

	struct option* ops = options_allocate(10);
	options_double(ops, "sigma_theta_deg", &p.sigma_theta_deg, 0.0,
		"Std deviation of gaussian noise for theta (deg) (disabled if 0)");
	options_double(ops, "sigma_xy", &p.sigma_xy, 0.0,
		"Std deviation of gaussian noise for x,y (disabled if 0)");
	options_int(ops, "seed", &p.seed, 0,
		"Seed for random number generator (if 0, use GSL_RNG_SEED env. variable).");
	options_string(ops, "in", &p.file_input, "stdin", "Input file ");
	options_string(ops, "out", &p.file_output, "stdout", "Output file ");

	options_int(ops, "debug", &p.debug, 0, "Shows debug information");

	if(!options_parse_args(ops, argc, argv)) {
		options_print_help(ops, stderr);
		return -1;
	}

	sm_debug_write(p.debug);

	gsl_rng_env_setup();
	gsl_rng * rng = gsl_rng_alloc (gsl_rng_ranlxs0);
	if(p.seed != 0)
	gsl_rng_set(rng, (unsigned int) p.seed);

	FILE * in = open_file_for_reading(p.file_input);
	if(!in) return -3;

	FILE * out = open_file_for_writing(p.file_output);
	if(!out) return -2;

	LDP ld; int count=0;
	while( (ld = ld_read_smart(in))) {
		count++;
		if(!ld_valid_fields(ld))  {
			sm_error("Invalid laser data (#%d in file)\n", count);
			continue;
		}

		if(!any_nan(ld->true_pose, 3))
			copy_d( (const double*) ld->true_pose, 3, ld->odometry);

		double e[3] = {0,0,0};

		if(p.sigma_xy > 0) {
			e[0]  = gsl_ran_gaussian(rng, p.sigma_xy);
			e[1]  = gsl_ran_gaussian(rng, p.sigma_xy);
		}

		if(p.sigma_theta_deg > 0) {
			e[2] = gsl_ran_gaussian(rng, deg2rad(p.sigma_theta_deg));
		}

		ld->odometry[0] += e[0];
		ld->odometry[1] += e[1];
		ld->odometry[2] += e[2];

		sm_debug("Adding noise %s.\n", friendly_pose(e));

		ld_write_as_json(ld, out);

		ld_free(ld);
	}

	return 0;
}

max_stars_repo_name: ozaslan/basics

max_issues_repo_name: ozaslan/basics

max_forks_repo_name: ozaslan/basics


text:
#include <stdio.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_multimin.h>

void calcmatriceswrapper(int *Omega, int *NumTerms, int *qmax, int *pmax, int *IsTriplet, int *Ordering, int *Method, double *Alpha, double *Beta, double *Gamma, double *Energy);

void testfortran(int *x)
{
	printf("%d\n", *x);
}

double test(const gsl_vector *v, void *params)
{
	double a1, b1, g1, Energy;
	int *p = (int*)params;
	int Omega = p[0], NumTerms = p[1], qmax = p[2], pmax = p[3], IsTriplet = p[4], Ordering = p[5], Method = p[6];

	a1 = gsl_vector_get(v, 0);
	b1 = gsl_vector_get(v, 1);
	g1 = gsl_vector_get(v, 2);

	calcmatriceswrapper(&Omega, &NumTerms, &qmax, &pmax, &IsTriplet, &Ordering, &Method, &a1, &b1, &g1, &Energy);
	printf("Energy: %f\n", Energy);

	return Energy;
}

int optimizewavefn(int MaxIter, int Omega, int NumTerms, int qmax, int pmax, int IsTriplet, int Ordering, int Method, double alpha, double beta, double gamma)
{
	int par[7] = { Omega, NumTerms, qmax, pmax, IsTriplet, Ordering, Method };
	//printf("Test: %f %f %f\n", alpha, beta, gamma);

	const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex;  //gsl_multimin_fminimizer_nmsimplex2;
	gsl_multimin_fminimizer *s = NULL;
	gsl_vector *ss, *x;
	gsl_multimin_function minex_func;

	int iter = 0, status;
	double size;

	x = gsl_vector_alloc(3);
	gsl_vector_set(x, 0, alpha);
	gsl_vector_set(x, 1, beta);
	gsl_vector_set(x, 2, gamma);

	/* Set initial step sizes to 1 */
	//ss = gsl_vector_alloc (2);
	ss = gsl_vector_alloc(3);
	gsl_vector_set_all(ss, 0.05);

	/* Initialize method and iterate */
	//minex_func.n = 2;
	minex_func.n = 3;
	minex_func.f = (void*)test;
	minex_func.params = par;

	//s = gsl_multimin_fminimizer_alloc (T, 2);
	s = gsl_multimin_fminimizer_alloc(T, 3);
	gsl_multimin_fminimizer_set(s, &minex_func, x, ss);

	do
	{
		iter++;
		status = gsl_multimin_fminimizer_iterate(s);

		if (status)
			break;

		size = gsl_multimin_fminimizer_size(s);
		status = gsl_multimin_test_size(size, 1e-5);

		if (status == GSL_SUCCESS) {
			printf ("converged to minimum at\n");
		}

		printf ("%5d %10.5e f() = %12.7f size = %.5f\n",
				iter,
				gsl_vector_get (s->x, 0),
				gsl_vector_get (s->x, 1),
				gsl_vector_get (s->x, 2),
				s->fval, size);
	} while (status == GSL_CONTINUE && iter < MaxIter);

	gsl_vector_free(x);
	gsl_vector_free(ss);
	gsl_multimin_fminimizer_free(s);

	return 0;  //@TODO: How do I make this into a void subroutine?
}

max_stars_repo_name: DentonW/Ps-H-Scattering

max_issues_repo_name: DentonW/Ps-H-Scattering

max_forks_repo_name: DentonW/Ps-H-Scattering


text:
#ifndef ppom_57c54fa0_d309_4aad_9678_16fa29f7af42_h
#define ppom_57c54fa0_d309_4aad_9678_16fa29f7af42_h

#include <rathen\config.h>
#include <gslib\tree.h>
#include <rathen\basis.h>

__rathen_begin__

class ppom
{
public:
    typedef tree<object, _treenode_wrapper<object> > ppom_tree;
    typedef ppom_tree::iterator iterator;
    typedef ppom_tree::const_iterator const_iterator;
    //typedef ;

public:
    ppom() {}

public:
    template<class _cst>
    iterator insert(iterator i) { return _tree.insert_tail<_cst>(i); }
    template<class _cst>
    iterator birth(iterator i) { return _tree.birth_tail<_cst>(i); }

protected:
    ppom_tree       _tree;
};

__rathen_end__

#endif

max_stars_repo_name: lymastee/gslib

max_issues_repo_name: lymastee/gslib

max_forks_repo_name: lymastee/gslib


text:
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "eigenmap.h"
#include <cblas.h>
#include <memory.h>

static void diag(double *d, const double *w, int n_patch);
static void compute_l(double *w, int n_patch);

static void diag_similarity_transform(double *w, int n_patch);

/*
 * laplacian computes the Laplacian matrix based on the weight matrix.
 *
 * w: the weight matrix
 * n_patch: the dimension of dev_w and dev_l
 * Note: the Laplacian matrix is computed in-place and overwrites w.
 */
void laplacian(double *w, int n_patch)
{
    /* ---- corresponding Matlab code ----
     * D = diag(sum(W));
     * L = eye(n_patch) - D^(-1/2)*W*D^(-1/2);
     */

    // W <- D^(-1/2) * W * D^(-1/2)
    diag_similarity_transform(w, n_patch);
    // L <- I - W
    compute_l(w, n_patch);
}

static void diag_similarity_transform(double *w, int n_patch)
{
    double *d = (double *)calloc(n_patch, sizeof(double));
    int i;

    diag(d, w, n_patch);

    // row operations
    for (i = 0; i < n_patch; i++)
        cblas_dscal(n_patch, d[i], &w[i], n_patch);
    // column operations
    for (i = 0; i < n_patch; i++)
        cblas_dscal(n_patch, d[i], &w[i * n_patch], 1);

    free(d);
}

static void diag(double *d, const double *w, int n_patch)
{
    int i, j;
    double sum;
    for (j = 0; j < n_patch; j++){
        sum = 0;
        for (i = 0; i < n_patch; i++)
            sum += w[i + j * n_patch];
        d[j] = 1 / sqrt(sum);
    }
}

static void compute_l(double *w, int n_patch)
{
    int N = n_patch * n_patch;
    int i;

    for (i = 0; i < N; i++)
        w[i] = ((i % (n_patch + 1) == 0) ? 1.0 : 0.0) - w[i];
}