text:
/* bspline/bspline.c
 *
 * Copyright (C) 2006 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 2 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.
 */

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>

/*
 * This module contains routines related to calculating B-splines.
 * The algorithms used are described in
 *
 * [1] Carl de Boor, "A Practical Guide to Splines", Springer
 *     Verlag, 1978.
 */

static int bspline_eval_all(const double x, gsl_vector *B, size_t *idx,
                            gsl_bspline_workspace *w);

static inline size_t bspline_find_interval(const double x, int *flag,
                                           gsl_bspline_workspace *w);

/*
gsl_bspline_alloc()
  Allocate space for a bspline workspace. The size of the
workspace is O(5k + nbreak)

Inputs: k      - spline order (cubic = 4)
        nbreak - number of breakpoints

Return: pointer to workspace
*/

gsl_bspline_workspace *
gsl_bspline_alloc(const size_t k, const size_t nbreak)
{
  if (k == 0)
    {
      GSL_ERROR_NULL("k must be at least 1", GSL_EINVAL);
    }
  else if (nbreak < 2)
    {
      GSL_ERROR_NULL("nbreak must be at least 2", GSL_EINVAL);
    }
  else
    {
      gsl_bspline_workspace *w;

      w = (gsl_bspline_workspace *)
          calloc(1, sizeof(gsl_bspline_workspace));
      if (w == 0)
        {
          GSL_ERROR_NULL("failed to allocate space for workspace", GSL_ENOMEM);
        }

      w->k = k;
      w->km1 = k - 1;
      w->nbreak = nbreak;
      w->l = nbreak - 1;
      w->n = w->l + k - 1;

      w->knots = gsl_vector_alloc(w->n + k);
      if (w->knots == 0)
        {
          gsl_bspline_free(w);
          GSL_ERROR_NULL("failed to allocate space for knots vector", GSL_ENOMEM);
        }

      w->deltal = gsl_vector_alloc(k);
      w->deltar = gsl_vector_alloc(k);
      if (!w->deltal || !w->deltar)
        {
          gsl_bspline_free(w);
          GSL_ERROR_NULL("failed to allocate space for delta vectors", GSL_ENOMEM);
        }

      w->B = gsl_vector_alloc(k);
      if (w->B == 0)
        {
          gsl_bspline_free(w);
          GSL_ERROR_NULL("failed to allocate space for temporary spline vector", GSL_ENOMEM);
        }

      return (w);
    }
} /* gsl_bspline_alloc() */

/* Return number of coefficients */
size_t
gsl_bspline_ncoeffs (gsl_bspline_workspace * w)
{
  return w->n;
}

/* Return order */
size_t
gsl_bspline_order (gsl_bspline_workspace * w)
{
  return w->k;
}

/* Return number of breakpoints */
size_t
gsl_bspline_nbreak (gsl_bspline_workspace * w)
{
  return w->nbreak;
}

double
gsl_bspline_breakpoint (size_t i, gsl_bspline_workspace * w)
{
  size_t j = i + w->k - 1;
  return gsl_vector_get(w->knots, j);
}

/*
gsl_bspline_free()
  Free a bspline workspace

Inputs: w - workspace to free

Return: none
*/

void
gsl_bspline_free(gsl_bspline_workspace *w)
{
  if (!w)
    return;

  if (w->knots)
    gsl_vector_free(w->knots);

  if (w->deltal)
    gsl_vector_free(w->deltal);

  if (w->deltar)
    gsl_vector_free(w->deltar);

  if (w->B)
    gsl_vector_free(w->B);

  free(w);
} /* gsl_bspline_free() */

/*
gsl_bspline_knots()
  Compute the knots from the given breakpoints:

knots(1:k) = breakpts(1)
knots(k+1:k+l-1) = breakpts(i), i = 2 .. l
knots(n+1:n+k) = breakpts(l + 1)

where l is the number of polynomial pieces (l = nbreak - 1) and
n = k + l - 1
(using matlab syntax for the arrays)

The repeated knots at the beginning and end of the interval
correspond to the continuity condition there. See pg. 119
of [1].

Inputs: breakpts - breakpoints
        w        - bspline workspace

Return: success or error
*/

int
gsl_bspline_knots(const gsl_vector *breakpts, gsl_bspline_workspace *w)
{
  if (breakpts->size != w->nbreak)
    {
      GSL_ERROR("breakpts vector has wrong size", GSL_EBADLEN);
    }
  else
    {
      size_t i; /* looping */

      for (i = 0; i < w->k; ++i)
        gsl_vector_set(w->knots, i, gsl_vector_get(breakpts, 0));

      for (i = 1; i < w->l; ++i)
        {
          gsl_vector_set(w->knots, w->k - 1 + i,
                         gsl_vector_get(breakpts, i));
        }

      for (i = w->n; i < w->n + w->k; ++i)
        gsl_vector_set(w->knots, i, gsl_vector_get(breakpts, w->l));

      return GSL_SUCCESS;
    }
} /* gsl_bspline_knots() */

/*
gsl_bspline_knots_uniform()
  Construct uniformly spaced knots on the interval [a,b] using
the previously specified number of breakpoints. 'a' is the position
of the first breakpoint and 'b' is the position of the last
breakpoint.

Inputs: a - left side of interval
        b - right side of interval
        w - bspline workspace

Return: success or error

Notes: 1) w->knots is modified to contain the uniformly spaced
          knots

       2) The knots vector is set up as follows (using octave syntax):

          knots(1:k) = a
          knots(k+1:k+l-1) = a + i*delta, i = 1 .. l - 1
          knots(n+1:n+k) = b
*/

int
gsl_bspline_knots_uniform(const double a, const double b,
                          gsl_bspline_workspace *w)
{
  size_t i;     /* looping */
  double delta; /* interval spacing */
  double x;

  delta = (b - a) / (double) w->l;

  for (i = 0; i < w->k; ++i)
    gsl_vector_set(w->knots, i, a);

  x = a + delta;
  for (i = 0; i < w->l - 1; ++i)
    {
      gsl_vector_set(w->knots, w->k + i, x);
      x += delta;
    }

  for (i = w->n; i < w->n + w->k; ++i)
    gsl_vector_set(w->knots, i, b);

  return GSL_SUCCESS;
} /* gsl_bspline_knots_uniform() */

/*
gsl_bspline_eval()
  Evaluate the basis functions B_i(x) for all i. This is
basically a wrapper function for bspline_eval_all()
which formats the output in a nice way.

Inputs: x - point for evaluation
        B - (output) where to store B_i(x) values
            the length of this vector is
            n = nbreak + k - 2 = l + k - 1 = w->n
        w - bspline workspace

Return: success or error

Notes: The w->knots vector must be initialized prior to calling
       this function (see gsl_bspline_knots())
*/

int
gsl_bspline_eval(const double x, gsl_vector *B,
                 gsl_bspline_workspace *w)
{
  if (B->size != w->n)
    {
      GSL_ERROR("B vector length does not match n", GSL_EBADLEN);
    }
  else
    {
      size_t i;     /* looping */
      size_t idx = 0;
      size_t start; /* first non-zero spline */

      /* find all non-zero B_i(x) values */
      bspline_eval_all(x, w->B, &idx, w);

      /* store values in appropriate part of given vector */

      start = idx - w->k + 1;
      for (i = 0; i < start; ++i)
        gsl_vector_set(B, i, 0.0);

      for (i = start; i <= idx; ++i)
        gsl_vector_set(B, i, gsl_vector_get(w->B, i - start));

      for (i = idx + 1; i < w->n; ++i)
        gsl_vector_set(B, i, 0.0);

      return GSL_SUCCESS;
    }
} /* gsl_bspline_eval() */

/****************************************
 *          INTERNAL ROUTINES           *
 ****************************************/

/*
bspline_eval_all()
  Evaluate all non-zero B-splines B_i(x) using algorithm (8)
of chapter X of [1]

The idea is something like this. Given x \in [t_i, t_{i+1}]
with t_i < t_{i+1} and the t_i are knots, the values of the
B-splines not automatically zero fit into a triangular
array as follows:
                         0
            0
0                        B_{i-2,3}
            B_{i-1,2}
B_{i,1}                  B_{i-1,3}       ...
            B_{i,2}
0                        B_{i,3}
            0
                         0

where B_{i,k} is the ith B-spline of order k. The boundary 0s
indicate that those B-splines not in the table vanish at x.

To compute the non-zero B-splines of a given order k, we use
Eqs. (4) and (5) of chapter X of [1]:

(4) B_{i,1}(x) = { 1, t_i <= x < t_{i+1}
                   0, else }

(5) B_{i,k}(x) =     (x - t_i)
                 ----------------- B_{i,k-1}(x)
                 (t_{i+k-1} - t_i)

                      t_{i+k} - x
                 + ----------------- B_{i+1,k-1}(x)
                   t_{i+k} - t_{i+1}

So (4) gives us the first column of the table and we can use
the recurrence relation (5) to get the rest of the columns.

Inputs: x   - point at which to evaluate splines
        B   - (output) where to store B-spline values (length k)
        idx - (output) B-spline function index of last output
              value (B_{idx}(x) is stored in the last slot of 'B')
        w   - bspline workspace

Return: success or error

Notes: 1) the w->knots vector must be initialized before calling
          this function

       2) On output, B contains:

       B = [B_{i-k+1,k}, B_{i-k+2,k}, ..., B_{i-1,k}, B_{i,k}]

          where 'i' is stored in 'idx' on output

       3) based on PPPACK bsplvb
*/

static int
bspline_eval_all(const double x, gsl_vector *B, size_t *idx,
                 gsl_bspline_workspace *w)
{
  if (B->size != w->k)
    {
      GSL_ERROR("B vector not of length k", GSL_EBADLEN);
    }
  else
    {
      size_t i;  /* spline index */
      size_t j;  /* looping */
      size_t ii; /* looping */
      int flag = 0;  /* error flag */
      double saved;
      double term;

      i = bspline_find_interval(x, &flag, w);

      if (flag == -1)
        {
          GSL_ERROR("x outside of knot interval", GSL_EINVAL);
        }
      else if (flag == 1)
        {
          if (x <= gsl_vector_get(w->knots, i) + GSL_DBL_EPSILON)
            {
              --i;
            }
          else
            {
              GSL_ERROR("x outside of knot interval", GSL_EINVAL);
            }
        }

      *idx = i;

      gsl_vector_set(B, 0, 1.0);

      for (j = 0; j < w->k - 1; ++j)
        {
          gsl_vector_set(w->deltar, j,
                         gsl_vector_get(w->knots, i + j + 1) - x);
          gsl_vector_set(w->deltal, j,
                         x - gsl_vector_get(w->knots, i - j));

          saved = 0.0;

          for (ii = 0; ii <= j; ++ii)
            {
              term = gsl_vector_get(B, ii) /
                     (gsl_vector_get(w->deltar, ii) +
                      gsl_vector_get(w->deltal, j - ii));

              gsl_vector_set(B, ii,
                             saved +
                             gsl_vector_get(w->deltar, ii) * term);

              saved = gsl_vector_get(w->deltal, j - ii) * term;
            }

          gsl_vector_set(B, j + 1, saved);
        }

      return GSL_SUCCESS;
    }
} /* bspline_eval_all() */

/*
bspline_find_interval()
  Find knot interval such that

  t_i <= x < t_{i + 1}

where the t_i are knot values.

Inputs: x    - x value
        flag - (output) error flag
        w    - bspline workspace

Return: i (index in w->knots corresponding to left limit of interval)

Notes: The error conditions are reported as follows:

Condition             Return value        Flag
---------             ------------        ----
x < t_0                    0               -1
t_i <= x < t_{i+1}         i                0
t_{n+k-1} <= x           l+k-1             +1
*/

static inline size_t
bspline_find_interval(const double x, int *flag,
                      gsl_bspline_workspace *w)
{
  size_t i;

  if (x < gsl_vector_get(w->knots, 0))
    {
      *flag = -1;
      return 0;
    }

  /* find i such that t_i <= x < t_{i+1} */
  for (i = w->k - 1; i < w->k + w->l - 1; ++i)
    {
      const double ti = gsl_vector_get(w->knots, i);
      const double tip1 = gsl_vector_get(w->knots, i + 1);

      if (tip1 < ti)
        {
          GSL_ERROR("knots vector is not increasing", GSL_EINVAL);
        }

      if (ti <= x && x < tip1)
        break;
    }

  if (i == w->k + w->l - 1)
    *flag = 1;
  else
    *flag = 0;

  return i;
} /* bspline_find_interval() */


text:
#ifndef AMICI_VECTOR_H
#define AMICI_VECTOR_H

#include <vector>
#include <type_traits>

#include <amici/exception.h>

#include <nvector/nvector_serial.h>

#include <gsl/gsl-lite.hpp>

namespace amici {

/** Since const N_Vector is not what we want */
using const_N_Vector =
    std::add_const_t<typename std::remove_pointer_t<N_Vector>> *;

inline const realtype* N_VGetArrayPointerConst(const_N_Vector x) {
    return N_VGetArrayPointer(const_cast<N_Vector>(x));
}

/** AmiVector class provides a generic interface to the NVector_Serial struct */
class AmiVector {
  public:
    /**
     * @brief Default constructor
     */
    AmiVector() = default;

    /** Creates an std::vector<realtype> and attaches the
     * data pointer to a newly created N_Vector_Serial.
     * Using N_VMake_Serial ensures that the N_Vector
     * module does not try to deallocate the data vector
     * when calling N_VDestroy_Serial
     * @brief empty constructor
     * @param length number of elements in vector
     */
    explicit AmiVector(const long int length)
        : vec_(static_cast<decltype(vec_)::size_type>(length), 0.0),
          nvec_(N_VMake_Serial(length, vec_.data())) {}

    /** Moves data from std::vector and constructs an nvec that points to the
     * data
     * @brief constructor from std::vector,
     * @param rvec vector from which the data will be moved
     */
    explicit AmiVector(std::vector<realtype> rvec)
        : vec_(std::move(rvec)),
          nvec_(N_VMake_Serial(static_cast<long int>(vec_.size()), vec_.data())) {}

    /** Copy data from gsl::span and constructs a vector
     * @brief constructor from gsl::span,
     * @param rvec vector from which the data will be copied
     */
    explicit AmiVector(gsl::span<realtype> rvec)
        : AmiVector(std::vector<realtype>(rvec.begin(), rvec.end())) {}

    /**
     * @brief copy constructor
     * @param vold vector from which the data will be copied
     */
    AmiVector(const AmiVector &vold) : vec_(vold.vec_) {
        nvec_ =
            N_VMake_Serial(static_cast<long int>(vold.vec_.size()), vec_.data());
    }

    /**
     * @brief move constructor
     * @param other vector from which the data will be moved
     */
    AmiVector(AmiVector&& other) noexcept : nvec_(nullptr) {
        vec_ = std::move(other.vec_);
        synchroniseNVector();
    }

    /**
     * @brief destructor
     */
    ~AmiVector();

    /**
     * @brief copy assignment operator
     * @param other right hand side
     * @return left hand side
     */
    AmiVector &operator=(AmiVector const &other);

    /**
     * @brief operator *= (element-wise multiplication)
     * @param multiplier multiplier
     * @return result
     */
    AmiVector &operator*=(AmiVector const& multiplier) {
        N_VProd(getNVector(),
               const_cast<N_Vector>(multiplier.getNVector()),
               getNVector());
        return *this;
    }

    /**
     * @brief operator /= (element-wise division)
     * @param divisor divisor
     * @return result
     */
    AmiVector &operator/=(AmiVector const& divisor) {
        N_VDiv(getNVector(),
               const_cast<N_Vector>(divisor.getNVector()),
               getNVector());
        return *this;
    }

    /**
     * @brief Returns an iterator that points to the first element of the
     * vector.
     * @return iterator that points to the first element
     */
    auto begin() { return vec_.begin(); }

    /**
     * @brief Returns an iterator that points to one element after the last
     * element of the vector.
     * @return iterator that points to one element after the last element
     */
    auto end() { return vec_.end(); }

    /**
     * @brief data accessor
     * @return pointer to data array
     */
    realtype *data();

    /**
     * @brief const data accessor
     * @return const pointer to data array
     */
    const realtype *data() const;

    /**
     * @brief N_Vector accessor
     * @return N_Vector
     */
    N_Vector getNVector();

    /**
     * @brief N_Vector accessor
     * @return N_Vector
     */
    const_N_Vector getNVector() const;

    /**
     * @brief Vector accessor
     * @return Vector
     */
    std::vector<realtype> const &getVector() const;

    /**
     * @brief returns the length of the vector
     * @return length
     */
    int getLength() const;

    /**
     * @brief fills vector with zero values
     */
    void zero();

    /**
     * @brief changes the sign of data elements
     */
    void minus();

    /**
     * @brief sets all data elements to a specific value
     * @param val value for data elements
     */
    void set(realtype val);

    /**
     * @brief accessor to data elements of the vector
     * @param pos index of element
     * @return element
     */
    realtype &operator[](int pos);
    /**
     * @brief accessor to data elements of the vector
     * @param pos index of element
     * @return element
     */
    realtype &at(int pos);

    /**
     * @brief accessor to data elements of the vector
     * @param pos index of element
     * @return element
     */
    const realtype &at(int pos) const;

    /**
     * @brief copies data from another AmiVector
     * @param other data source
     */
    void copy(const AmiVector &other);

    /**
     * @brief Take absolute value (in-place)
     */
    void abs() {
        N_VAbs(getNVector(), getNVector());
    };

  private:
    /** main data storage */
    std::vector<realtype> vec_;

    /** N_Vector, will be synchronized such that it points to data in vec */
    N_Vector nvec_ {nullptr};

    /**
     * @brief reconstructs nvec such that data pointer points to vec data array
     */
    void synchroniseNVector();
};

/**
 * @brief AmiVectorArray class.
 *
 * Provides a generic interface to arrays of NVector_Serial structs
 */
class AmiVectorArray {
  public:
    /**
     * @brief Default constructor
     */
    AmiVectorArray() = default;

    /**
     * Creates an std::vector<realype> and attaches the
     * data pointer to a newly created N_VectorArray
     * using CloneVectorArrayEmpty ensures that the N_Vector
     * module does not try to deallocate the data vector
     * when calling N_VDestroyVectorArray_Serial
     * @brief empty constructor
     * @param length_inner length of vectors
     * @param length_outer number of vectors
     */
    AmiVectorArray(long int length_inner, long int length_outer);

    /**
     * @brief copy constructor
     * @param vaold object to copy from
     */
    AmiVectorArray(const AmiVectorArray &vaold);

    ~AmiVectorArray() = default;

    /**
     * @brief copy assignment operator
     * @param other right hand side
     * @return left hand side
     */
    AmiVectorArray &operator=(AmiVectorArray const &other);

    /**
     * @brief accessor to data of AmiVector elements
     * @param pos index of AmiVector
     * @return pointer to data array
     */
    realtype *data(int pos);

    /**
     * @brief const accessor to data of AmiVector elements
     * @param pos index of AmiVector
     * @return const pointer to data array
     */
    const realtype *data(int pos) const;

    /**
     * @brief accessor to elements of AmiVector elements
     * @param ipos inner index in AmiVector
     * @param jpos outer index in AmiVectorArray
     * @return element
     */
    realtype &at(int ipos, int jpos);

    /**
     * @brief const accessor to elements of AmiVector elements
     * @param ipos inner index in AmiVector
     * @param jpos outer index in AmiVectorArray
     * @return element
     */
    const realtype &at(int ipos, int jpos) const;

    /**
     * @brief accessor to NVectorArray
     * @return N_VectorArray
     */
    N_Vector *getNVectorArray();

    /**
     * @brief accessor to NVector element
     * @param pos index of corresponding AmiVector
     * @return N_Vector
     */
    N_Vector getNVector(int pos);

    /**
     * @brief const accessor to NVector element
     * @param pos index of corresponding AmiVector
     * @return N_Vector
     */
    const_N_Vector getNVector(int pos) const;

    /**
     * @brief accessor to AmiVector elements
     * @param pos index of AmiVector
     * @return AmiVector
     */
    AmiVector &operator[](int pos);

    /**
     * @brief const accessor to AmiVector elements
     * @param pos index of AmiVector
     * @return const AmiVector
     */
    const AmiVector &operator[](int pos) const;

    /**
     * @brief length of AmiVectorArray
     * @return length
     */
    int getLength() const;

    /**
     * @brief set every AmiVector in AmiVectorArray to zero
     */
    void zero();

    /**
     * @brief flattens the AmiVectorArray to a vector in row-major format
     * @param vec vector into which the AmiVectorArray will be flattened. Must
     * have length equal to number of elements.
     */
    void flatten_to_vector(std::vector<realtype> &vec) const;

    /**
     * @brief copies data from another AmiVectorArray
     * @param other data source
     */
    void copy(const AmiVectorArray &other);

  private:
    /** main data storage */
    std::vector<AmiVector> vec_array_;

    /**
     * N_Vector array, will be synchronized such that it points to
     * respective elements in the vec_array
     */
    std::vector<N_Vector> nvec_array_;
};

/**
 * @brief Computes z = a*x + b*y
 * @param a coefficient for x
 * @param x a vector
 * @param b coefficient for y
 * @param y another vector with same size as x
 * @param z result vector of same size as x and y
 */
inline void linearSum(realtype a, AmiVector const& x, realtype b,
               AmiVector const& y, AmiVector& z) {
    N_VLinearSum(a, const_cast<N_Vector>(x.getNVector()),
                 b, const_cast<N_Vector>(y.getNVector()),
                 z.getNVector());
}

/**
 * @brief Compute dot product of x and y
 * @param x vector
 * @param y vector
 * @return dot product of x and y
 */
inline realtype dotProd(AmiVector const& x, AmiVector const& y) {
    return N_VDotProd(const_cast<N_Vector>(x.getNVector()),
                      const_cast<N_Vector>(y.getNVector()));
}

} // namespace amici

namespace gsl {
/**
 * @brief Create span from N_Vector
 * @param nv
 * @return
 */
inline span<realtype> make_span(N_Vector nv)
{
    return span<realtype>(N_VGetArrayPointer(nv), N_VGetLength_Serial(nv));
}
} // namespace gsl

#endif /* AMICI_VECTOR_H */

ext: h

max_stars_repo_name: kristianmeyerr/AMICI

max_issues_repo_name: kristianmeyerr/AMICI

max_forks_repo_name: kristianmeyerr/AMICI


text:
#define S_FUNCTION_NAME  solve_chol
#define S_FUNCTION_LEVEL 2

// Example: w = solve_chol(R,b);

/*
 * Need to include simstruc.h for the definition of the SimStruct and
 * its associated macro definitions.
 */

#include <string.h>
#include "simstruc.h"
#include <lapacke.h>

#if !defined(_WIN32) /* not Win32/Matlab */
  #define dpotrs dpotrs_
#endif
extern int_T dpotrs_(char *, int_T *, int_T *, real_T *, int_T *,
                                            real_T *, int_T *, int_T *);
/*=========*
 * Defines *
 *=========*/

//#define SIZE_IDX 50
//#define SIZE_PARAM(S) ssGetSFcnParam(S,SIZE_IDX)

#define NINPUTS  2
#define NOUTPUTS 1

static void mdlInitializeSizes(SimStruct *S)
{
    int_T needsInput   = 1;  /* direct feedthrough     */

	ssSetNumSFcnParams(S, 0);  /* Number of expected parameters */
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
        /*
         * If the number of expected input parameters is not
         * equal to the number of parameters entered in the
         * dialog box, return. The Simulink engine generates an
         * error indicating that there is a parameter mismatch.
         */
        return;
	}

	ssSetNumContStates( S, 0);
    ssSetNumDiscStates( S, 0);

	if (!ssSetNumInputPorts(S, NINPUTS)) return;
	/* R: a dynamically sized 2-D matrix input */
	ssSetInputPortMatrixDimensions(S, 0, 50, 50);
    ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/
    ssSetInputPortDirectFeedThrough(S, 0, 1);
	/* b: a dynamically sized 1-D matrix input */
	ssSetInputPortWidth(S, 0, 50);
    ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/
    ssSetInputPortDirectFeedThrough(S, 0, 1);

	if (!ssSetNumOutputPorts(S, NOUTPUTS)) return;
	/* b: a dynamically sized 1-D matrix output */
    ssSetOutputPortWidth(S, 0, 50);

	/*
     * Set the number of sample times.     */
    ssSetNumSampleTimes(S, 1);

	/* Inherit the sample time. */
	ssSetInputPortSampleTime(S, 0, INHERITED_SAMPLE_TIME);
	ssSetInputPortSampleTime(S, 1, INHERITED_SAMPLE_TIME);
}

/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *      Port based sample times have already been configured, therefore this
 *	method doesn't need to perform any action (you can check the
 *	current port sample times).
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
	ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);
    ssSetModelReferenceSampleTimeDefaultInheritance(S); /* Inherit sample time */
}

/* Function: mdlOutputs =======================================================
 * Abstract:
 *
 * solve_chol - solve a linear system A*X = B using the Cholesky factorization
 * of A (square, symmetric and positive definite) using LAPACK/DPOTRS.
 *
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{
	int_T   *dims1   = ssGetInputPortDimensions(S, 0);
	int_T   *dims2   = ssGetInputPortDimensions(S, 1);
	int_T    n1      = dims1[0];	/* number of rows in inputs A*/
	int_T    m1      = dims1[1]; 	/* number of columns in inputs A*/
	int_T    n2      = dims2[0];	/* number of rows in inputs B*/
	int_T    m2      = dims2[1]; 	/* number of columns in inputs B*/
	int_T    q;
	real_T   *C 	= ssGetOutputPortSignal(S,0);
	real_T   *A  	= ssGetInputPortSignal(S,0);
    real_T   *B  	= ssGetInputPortSignal(S,1);

	UNUSED_ARG(tid); /* not used in single tasking mode */

	if (n1 != m1)
		mexErrMsgTxt("First argument matrix must be square.");

	if (n1 != n2)
		mexErrMsgTxt("Both argument matrices must have the same number of rows.");

	// C = mxCreateDoubleMatrix(n, m, mxREAL);         /* space for output X */

	if (n1 == 0) return;             /* if argument was empty matrix, do no more */
	//memcpy( C, mxGetPr(B), m*n*mxGetElementSize(C) );  /* copy data */
	dpotrs("U", &n1, &m2, mxGetPr(A), &n1, C, &n1, &q);       /* solve system */
	if (q < 0) mexErrMsgTxt("Error: illegal input to solve_chol");

}

ext: c


text:
/* specfunc/mathieu_coeff.c
 *
 * Copyright (C) 2002 Lowell Johnson
 *
 * 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 2 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  L. Johnson */

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_sf_mathieu.h>

/*****************************************************************************
 * backward_recurse
 *
 * Purpose:
 ****************************************************************************/
static void backward_recurse_c(double aa, double qq, double xx, double *ff,
                               double *gx, int even_odd, int ni)
{
  int ii, nn;
  double g1;

  g1 = *gx;
  ff[ni] = xx;

  if (even_odd == 0)
  {
      for (ii=0; ii<ni; ii++)
      {
          nn = GSL_SF_MATHIEU_COEFF - ii - 1;
          ff[ni-ii-1] = -1.0/((4*nn*nn - aa)/qq + ff[ni-ii]);
      }
      if (ni == GSL_SF_MATHIEU_COEFF - 1)
          ff[0] *= 2.0;
  }
  else
  {
      for (ii=0; ii<ni; ii++)
      {
          nn = GSL_SF_MATHIEU_COEFF - ii - 1;
          ff[ni-ii-1] = -1.0/(((2*nn + 1)*(2*nn + 1) - aa)/qq + ff[ni-ii]);
      }
  }

  *gx = ff[0] - g1;
}

static void backward_recurse_s(double aa, double qq, double xx, double *ff,
                               double *gx, int even_odd, int ni)
{
  int ii, nn;
  double g1;

  g1 = *gx;
  ff[ni] = xx;

  if (even_odd == 0)
  {
      for (ii=0; ii<ni; ii++)
      {
          nn = GSL_SF_MATHIEU_COEFF - ii - 1;
          ff[ni-ii-1] = -1.0/((4*(nn + 1)*(nn + 1) - aa)/qq + ff[ni-ii]);
      }
  }
  else
  {
      for (ii=0; ii<ni; ii++)
      {
          nn = GSL_SF_MATHIEU_COEFF - ii - 1;
          ff[ni-ii-1] = -1.0/(((2*nn + 1)*(2*nn + 1) - aa)/qq + ff[ni-ii]);
      }
  }

  *gx = ff[0] - g1;
}

int gsl_sf_mathieu_a_coeff(int order, double qq, double aa, double coeff[])
{
  int ni, nn, ii, even_odd;
  double eps, g1, g2, x1, x2, e1, e2, de, xh, sum, ratio,
         ff[GSL_SF_MATHIEU_COEFF];

  eps = 1e-14;
  coeff[0] = 1.0;

  even_odd = 0;
  if (order % 2 != 0)
      even_odd = 1;

  /* If the coefficient array is not large enough to hold all necessary
     coefficients, error out. */
  if (order > GSL_SF_MATHIEU_COEFF)
      return GSL_FAILURE;

  /* Handle the trivial case where q = 0. */
  if (qq == 0.0)
  {
      for (ii=0; ii<GSL_SF_MATHIEU_COEFF; ii++)
          coeff[ii] = 0.0;

      coeff[order/2] = 1.0;

      return GSL_SUCCESS;
  }

  if (order < 5)
  {
      nn = 0;
      sum = 0.0;
      if (even_odd == 0)
          ratio = aa/qq;
      else
          ratio = (aa - 1 - qq)/qq;
  }
  else
  {
      if (even_odd == 0)
      {
          coeff[1] = aa/qq;
          coeff[2] = (aa - 4)/qq*coeff[1] - 2;
          sum = coeff[0] + coeff[1] + coeff[2];
          for (ii=3; ii<order/2+1; ii++)
          {
              coeff[ii] = (aa - 4*(ii - 1)*(ii - 1))/qq*coeff[ii-1] -
                                                                  coeff[ii-2];
              sum += coeff[ii];
          }
      }
      else
      {
          coeff[1] = (aa - 1)/qq - 1;
          sum = coeff[0] + coeff[1];
          for (ii=2; ii<order/2+1; ii++)
          {
              coeff[ii] = (aa - (2*ii - 1)*(2*ii - 1))/qq*coeff[ii-1] -
                                                                  coeff[ii-2];
              sum += coeff[ii];
          }
      }

      nn = ii - 1;

      ratio = coeff[nn]/coeff[nn-1];
  }

  ni = GSL_SF_MATHIEU_COEFF - nn - 1;

  /* Compute first two points to start root-finding. */
  if (even_odd == 0)
      x1 = -qq/(4.0*GSL_SF_MATHIEU_COEFF*GSL_SF_MATHIEU_COEFF);
  else
      x1 = -qq/((2.0*GSL_SF_MATHIEU_COEFF + 1.0)*(2.0*GSL_SF_MATHIEU_COEFF + 1.0));
  g1 = ratio;
  backward_recurse_c(aa, qq, x1, ff, &g1, even_odd, ni);
  x2 = g1;
  g2 = ratio;
  backward_recurse_c(aa, qq, x2, ff, &g2, even_odd, ni);

  /* Find the root. */
  while (1)
  {
      /* Compute the relative error. */
      e1 = g1 - x1;
      e2 = g2 - x2;
      de = e1 - e2;

      /* If we are close enough to the root, break... */
      if (fabs(de) < eps)
          break;

      /* Otherwise, determine the next guess and try again. */
      xh = (e1*x2 - e2*x1)/de;
      x1 = x2;
      g1 = g2;
      x2 = xh;
      g2 = ratio;
      backward_recurse_c(aa, qq, x2, ff, &g2, even_odd, ni);
  }

  /* Compute the rest of the coefficients. */
  sum += coeff[nn];
  for (ii=nn+1; ii<GSL_SF_MATHIEU_COEFF; ii++)
  {
      coeff[ii] = ff[ii-nn-1]*coeff[ii-1];
      sum += coeff[ii];

      /* If the coefficients are getting really small, set the remainder
         to zero. */
      if (fabs(coeff[ii]) < 1e-20)
      {
          for (; ii<GSL_SF_MATHIEU_COEFF;)
              coeff[ii++] = 0.0;
      }
  }

  /* Normalize the coefficients. */
  for (ii=0; ii<GSL_SF_MATHIEU_COEFF; ii++)
      coeff[ii] /= sum;

  return GSL_SUCCESS;
}

int gsl_sf_mathieu_b_coeff(int order, double qq, double aa, double coeff[])
{
  int ni, nn, ii, even_odd;
  double eps, g1, g2, x1, x2, e1, e2, de, xh, sum, ratio,
         ff[GSL_SF_MATHIEU_COEFF];

  eps = 1e-10;
  coeff[0] = 1.0;

  even_odd = 0;
  if (order % 2 != 0)
      even_odd = 1;

  /* If the coefficient array is not large enough to hold all necessary
     coefficients, error out. */
  if (order > GSL_SF_MATHIEU_COEFF)
      return GSL_FAILURE;

  /* Handle the trivial case where q = 0. */
  if (qq == 0.0)
  {
      for (ii=0; ii<GSL_SF_MATHIEU_COEFF; ii++)
          coeff[ii] = 0.0;

      coeff[(order-1)/2] = 1.0;

      return GSL_SUCCESS;
  }

  if (order < 5)
  {
      nn = 0;
      sum = 0.0;
      if (even_odd == 0)
          ratio = (aa - 4)/qq;
      else
          ratio = (aa - 1 - qq)/qq;
  }
  else
  {
      if (even_odd == 0)
      {
          coeff[1] = (aa - 4)/qq;
          sum = 2*coeff[0] + 4*coeff[1];
          for (ii=2; ii<order/2; ii++)
          {
              coeff[ii] = (aa - 4*ii*ii)/qq*coeff[ii-1] - coeff[ii-2];
              sum += 2*(ii + 1)*coeff[ii];
          }
      }
      else
      {
          coeff[1] = (aa - 1)/qq + 1;
          sum = coeff[0] + 3*coeff[1];
          for (ii=2; ii<order/2+1; ii++)
          {
              coeff[ii] = (aa - (2*ii - 1)*(2*ii - 1))/qq*coeff[ii-1] -
                                                                  coeff[ii-2];
              sum += (2*(ii + 1) - 1)*coeff[ii];
          }
      }

      nn = ii - 1;

      ratio = coeff[nn]/coeff[nn-1];
  }

  ni = GSL_SF_MATHIEU_COEFF - nn - 1;

  /* Compute first two points to start root-finding. */
  if (even_odd == 0)
      x1 = -qq/(4.0*(GSL_SF_MATHIEU_COEFF + 1.0)*(GSL_SF_MATHIEU_COEFF + 1.0));
  else
      x1 = -qq/((2.0*GSL_SF_MATHIEU_COEFF + 1.0)*(2.0*GSL_SF_MATHIEU_COEFF + 1.0));
  g1 = ratio;
  backward_recurse_s(aa, qq, x1, ff, &g1, even_odd, ni);
  x2 = g1;
  g2 = ratio;
  backward_recurse_s(aa, qq, x2, ff, &g2, even_odd, ni);

  /* Find the root. */
  while (1)
  {
      /* Compute the relative error. */
      e1 = g1 - x1;
      e2 = g2 - x2;
      de = e1 - e2;

      /* If we are close enough to the root, break... */
      if (fabs(de) < eps)
          break;

      /* Otherwise, determine the next guess and try again. */
      xh = (e1*x2 - e2*x1)/de;
      x1 = x2;
      g1 = g2;
      x2 = xh;
      g2 = ratio;
      backward_recurse_s(aa, qq, x2, ff, &g2, even_odd, ni);
  }

  /* Compute the rest of the coefficients. */
  sum += 2*(nn + 1)*coeff[nn];
  for (ii=nn+1; ii<GSL_SF_MATHIEU_COEFF; ii++)
  {
      coeff[ii] = ff[ii-nn-1]*coeff[ii-1];
      sum += 2*(ii + 1)*coeff[ii];

      /* If the coefficients are getting really small, set the remainder
         to zero. */
      if (fabs(coeff[ii]) < 1e-20)
      {
          for (; ii<GSL_SF_MATHIEU_COEFF;)
              coeff[ii++] = 0.0;
      }
  }

  /* Normalize the coefficients. */
  for (ii=0; ii<GSL_SF_MATHIEU_COEFF; ii++)
      coeff[ii] /= sum;

  return GSL_SUCCESS;
}

max_stars_repo_name: manggoguy/parsec-modified

max_issues_repo_name: manggoguy/parsec-modified

max_forks_repo_name: manggoguy/parsec-modified


text:
#include <math.h>
#include <gsl/gsl_linalg.h>
#include "advanceCN.h"
#include "applyBC.h"
#include "userFunc.h"
#include "ppmExtrap.h"
#include "getNextIterate.h"

/**************************************************************************/
/* Time step advance routine with Crank-Nicolson time centering           */
/**************************************************************************/

double
advanceCN(const double t, const double dt, const grid *grd,
	  double *col, double *pres, double *eInt,
	  double *mBnd, double *eBnd, double *mSrc, double *eSrc,
	  const bool eos_func, const double gamma_val,
	  const double delta_val,
	  const bool alpha_func, const double alpha_val,
	  const pres_bc_type ibc_pres, const enth_bc_type ibc_enth,
	  const bool ibc_func, const double ibc_pres_val,
	  const double ibc_enth_val,
	  const pres_bc_type obc_pres, const enth_bc_type obc_enth,
	  const bool obc_func, const double obc_pres_val,
	  const double obc_enth_val,
	  const bool massSrc_func, const double massSrc_val,
	  const bool intEnSrc_func, const double intEnSrc_val,
	  const double errTol, const double dtTol,
	  const unsigned long maxIter, const unsigned long interpOrder,
	  const bool noUpdate, const bool verbose,
	  const wksp *w, void *params, unsigned long *itCount
#ifdef TESTING_MODE
	  , double *resid, unsigned long *rtype,
	  double *advanceTime, double *nextIterTime,
	  double *userTime
#endif
	  ) {

  double pIn, qIn, pOut, qOut;
  double err, errCell;
  double dPres, dCol, dEInt, dtMin;
  double ibc_pres_val1, ibc_enth_val1, obc_pres_val1, obc_enth_val1;
  double h_up, h_up1;
  double mBndTemp[2], eBndTemp[2];
  unsigned long maxErrCell, converged, residType;
  long i;
#if AA_M > 0
  long nHist;
  double residMax = 0.0;
#endif
#ifdef TESTING_MODE
  clock_t start_t, end_t, user_start_t, user_end_t;
  clock_t next_iter_start_t, next_iter_end_t;

  start_t = clock();
#endif

  /****************************************************************/
  /* Step 1: initialize workspace arrays from starting conditions */
  /****************************************************************/

  /* Copy column density and pressure */
  for (i=0; i<grd->nr; i++) {
    w->colNew[i] = col[i];
    w->pres_g[i+1] = w->presNew_g[i+1] = pres[i];
  }
  /* Evaluate EOS parameters and get enthalpy */
  if (eos_func == 1) {
#if AA_M > 0
    nHist = 3*grd->nr;
#endif
#ifdef TESTING_MODE
    user_start_t = clock();
#endif
    userEOS(t, dt, grd, col, pres, eInt, params,
	    w->gammaLast, w->deltaLast);
#ifdef TESTING_MODE
    user_end_t = clock();
    *userTime += (double) (user_end_t - user_start_t)
      / CLOCKS_PER_SEC;
#endif
    for (i=0; i<grd->nr; i++) {
      w->eIntTmp[i] = w->eIntNew[i] = eInt[i];
      w->gammaNew[i] = w->gammaLast[i];
      w->deltaNew[i] = w->deltaLast[i];
      w->hint_g[i+1] = (pres[i] + eInt[i])/col[i];
    }
  } else {
#if AA_M > 0
    nHist = 2*grd->nr;
#endif
    for (i=0; i<grd->nr; i++) {
      w->gammaLast[i] = w->gammaNew[i] = gamma_val;
      w->deltaLast[i] = w->deltaNew[i] = delta_val;
      w->hint_g[i+1] = gamma_val/(gamma_val-1.0)*pres[i]/col[i];
    }
  }
  /* Get starting viscosity */
  if (alpha_func == 1) {
#ifdef TESTING_MODE
    user_start_t = clock();
#endif
    userAlpha(t, dt, grd, col, pres, eInt, w->gammaLast, w->deltaLast,
	      params, w->alpha_g+1);
#ifdef TESTING_MODE
    user_end_t = clock();
    *userTime += (double) (user_end_t - user_start_t)
      / CLOCKS_PER_SEC;
#endif
  } else {
    for (i=1; i<=grd->nr; i++) w->alpha_g[i] = alpha_val;
  }

  /* Store starting mass and energy fluxes through boundary */
  mBndTemp[0] = mBnd[0];
  mBndTemp[1] = mBnd[1];
  eBndTemp[0] = eBnd[0];
  eBndTemp[1] = eBnd[1];

  /* Store starting source function values */
  if (massSrc_func == 1) {
    for (i=0; i<grd->nr; i++) {
      w->mSrc[i] = mSrc[i];
      w->eSrc[i] = eSrc[i];
    }
  } else if (intEnSrc_func == 1) {
    for (i=0; i<grd->nr; i++) w->eSrc[i] = eSrc[i];
  }

  /****************************************************************/
  /* Step 2: apply boundary conditions at old time                */
  /****************************************************************/

  /* Set alpha in ghost cells */
  w->alpha_g[0] = w->alpha_g[1];
  w->alpha_g[grd->nr+1] = w->alpha_g[grd->nr];

  /* Get pressure / enthalpy boundary values */
  if (ibc_func == 1) {
#ifdef TESTING_MODE
    user_start_t = clock();
#endif
    userIBC(t, dt, grd, col, pres, eInt, w->gammaLast, w->deltaLast,
	    ibc_pres, ibc_enth, params,
	    &ibc_pres_val1, &ibc_enth_val1);
#ifdef TESTING_MODE
    user_end_t = clock();
    *userTime += (double) (user_end_t - user_start_t) /
      CLOCKS_PER_SEC;
#endif
  } else {
    ibc_pres_val1 = ibc_pres_val;
    ibc_enth_val1 = ibc_enth_val;
  }
  if (obc_func == 1) {
#ifdef TESTING_MODE
    user_start_t = clock();
#endif
    userOBC(t, dt, grd, col, pres, eInt, w->gammaLast, w->deltaLast,
	    obc_pres, obc_enth, params,
	    &obc_pres_val1, &obc_enth_val1);
#ifdef TESTING_MODE
    user_end_t = clock();
    *userTime += (double) (user_end_t - user_start_t) /
      CLOCKS_PER_SEC;
#endif
  } else {
    obc_pres_val1 = obc_pres_val;
    obc_enth_val1 = obc_enth_val;
  }

  /* Set boundary conditions from user-specified parameters */
  applyBC(grd, w->alpha_g,
	  ibc_pres, ibc_enth, ibc_pres_val1, ibc_enth_val1,
	  obc_pres, obc_enth, obc_pres_val1, obc_enth_val1,
	  w->pres_g, w->hint_g, &pIn, &qIn, &pOut, &qOut);

  /****************************************************************/
  /* Step 3: get mass flux, source terms, and RHS at old time     */
  /****************************************************************/

  /* Mass and energy source terms */
  if (massSrc_func == 1) {
#ifdef TESTING_MODE
    user_start_t = clock();
#endif
    userMassSrc(t, dt, grd, col, pres, eInt, w->gammaLast, w->deltaLast,
		params, w->massSrcLast);
#ifdef TESTING_MODE
    user_end_t = clock();
    *userTime += (double) (user_end_t - user_start_t) /
      CLOCKS_PER_SEC;
#endif
    for (i=0; i<grd->nr; i++) {
      w->mSrc[i] += 0.5 * dt * w->massSrcLast[i];
      w->eSrc[i] += 0.5 * dt * w->massSrcLast[i] *
	(grd->psiEff_g[i+1] + w->deltaLast[i]*pres[i]/col[i]);
    }
  } else {
    for (i=0; i<grd->nr; i++) w->massSrcLast[i] = massSrc_val;
  }
  if (intEnSrc_func == 1) {
#ifdef TESTING_MODE
    user_start_t = clock();
#endif
    userIntEnSrc(t, dt, grd, col, pres, eInt, w->gammaLast, w->deltaLast,
		 params, w->intEnSrc);
#ifdef TESTING_MODE
    user_end_t = clock();
    *userTime += (double) (user_end_t - user_start_t) /
      CLOCKS_PER_SEC;
#endif
    for (i=0; i<grd->nr; i++)
      w->eSrc[i] += 0.5 * dt * w->intEnSrc[i];
  } else {
    for (i=0; i<grd->nr; i++) w->intEnSrc[i] = intEnSrc_val;
  }

  /* Reconstruct internal enthalpies at cell edges */
  if (interpOrder == 1) {
    for (i=0; i<=grd->nr+1; i++)
      w->hintL_g[i] = w->hintR_g[i] = w->hint_g[i];
  } else if (interpOrder == 2) {
    if (grd->linear == 1) {
      for (i=1; i<=grd->nr+1; i++)
	w->hintR_g[i-1] = w->hintL_g[i] =
	  ((grd->r_g[i]-grd->r_h[i-1]) * w->hint_g[i-1] +
	   (grd->r_h[i-1]-grd->r_g[i-1]) * w->hint_g[i]) /
	  (grd->r_g[i] - grd->r_g[i-1]);
    } else {
      for (i=1; i<=grd->nr+1; i++)
	w->hintR_g[i-1] = w->hintL_g[i] =
	  (log(grd->r_g[i]/grd->r_h[i-1]) * w->hint_g[i-1] +
	   log(grd->r_h[i-1]/grd->r_g[i-1]) * w->hint_g[i]) /
	  log(grd->r_g[i]/grd->r_g[i-1]);
    }
    /* Limit the slope */
    for (i=1; i<=grd->nr+1; i++) {
      if (w->hintR_g[i-1]/w->hint_g[i-1]-1.0 > SLOPELIMIT) {
	w->hintR_g[i-1] = w->hint_g[i-1]*(1.0+SLOPELIMIT);
      } else if (w->hintR_g[i-1]/w->hint_g[i-1] < 1.0-SLOPELIMIT) {
	w->hintR_g[i-1] = w->hint_g[i-1]*(1.0-SLOPELIMIT);
      }
      if (w->hintL_g[i]/w->hint_g[i]-1.0 > SLOPELIMIT) {
	w->hintL_g[i] = w->hint_g[i]*(1.0+SLOPELIMIT);
      } else if (w->hintL_g[i]/w->hint_g[i] < 1.0-SLOPELIMIT) {
	w->hintL_g[i] = w->hint_g[i]*(1.0-SLOPELIMIT);
      }
    }
  } else if (interpOrder == 3) {
    ppmExtrap(grd->nr+2, grd->dr_g, w->hint_g, w->hintL_g, w->hintR_g,
	      w->ppmwksp_g);
  }

  /* Compute fluxes at cell edges */
  for (i=0; i<=grd->nr; i++) {

    /* Mass flux; initialize the new mass flux to match the old one,
       as is appropriate for the first guess in the iteration below */
    w->fmLast_h[i] = w->fmNew_h[i] = -grd->g_h[i] *
      (SQR(grd->r_g[i+1])*(1-grd->beta_g[i+1]) *
       w->pres_g[i+1]*w->alpha_g[i+1] -
       SQR(grd->r_g[i])*(1-grd->beta_g[i]) *
       w->pres_g[i]*w->alpha_g[i]);

    /* Enthalpy flux; use upwinding */
    if (w->fmLast_h[i] > 0) h_up = w->hintR_g[i] + grd->psiEff_h[i];
    else h_up = w->hintL_g[i+1] + grd->psiEff_h[i];
    w->feLast_h[i] = h_up * w->fmLast_h[i];

    /* Torque flux */
    w->ftLast_h[i] = M_PI * grd->r_h[i] * grd->vphi_h[i] *
      (1-grd->beta_h[i]) *
      (w->pres_g[i+1]*w->alpha_g[i+1] +
       w->pres_g[i]*w->alpha_g[i]);
  }

  /* Fill RHS vector */
  for (i=0; i<grd->nr; i++) {
    gsl_vector_set(w->rhs_g, i+1,
		   pres[i] - 0.5*dt/grd->area[i] *
		   (w->gammaLast[i]-1.0) *
		   (w->feLast_h[i+1] - w->feLast_h[i] +
		    w->ftLast_h[i+1] - w->ftLast_h[i] -
		    (grd->psiEff_g[i+1] +
		     w->deltaLast[i+1]*pres[i]/col[i])
		    *(w->fmLast_h[i+1] - w->fmLast_h[i])) +
		   0.5*dt*(w->gammaLast[i]-1) * w->intEnSrc[i]);
  }

  /* Zero out energy source, since below we expect this to
     be the rate from the previous iteration of the loop */
  for (i=0; i<grd->nr; i++) w->intEnSrc[i] = 0.0;

  /****************************************************************/
  /* Step 4: main iteration loop                                  */
  /****************************************************************/

  /* Status message */
  if (verbose)
    printf("advanceCN: starting iteration, dt = %e\n", dt);

  /* Start loop */
  for (*itCount = 1, converged = 0; 1; (*itCount)++) {

    /**************************************************************/
    /* Step 4a: compute enthalpy, alpha, and BC for current guess */
    /**************************************************************/

    /* Get new EOS parameters and cell-center enthalpies */
    if (eos_func == 1) {
#ifdef TESTING_MODE
      user_start_t = clock();
#endif
      userEOS(t+dt, dt, grd, w->colNew, w->presNew_g+1, w->eIntNew,
	      params, w->gammaNew, w->deltaNew);
#ifdef TESTING_MODE
      user_end_t = clock();
      *userTime += (double) (user_end_t - user_start_t)
	/ CLOCKS_PER_SEC;
#endif
      for (i=1; i<=grd->nr; i++)
	w->hint_g[i] = (w->presNew_g[i] + w->eIntNew[i-1]) /
	  w->colNew[i-1];
    } else {
      for (i=1; i<=grd->nr; i++) {
	w->hint_g[i] = gamma_val/(gamma_val-1.0) *
	  w->presNew_g[i]/w->colNew[i-1];
      }
    }

    /* If alpha is non-constant, get new values */
    if (alpha_func == 1) {
#ifdef TESTING_MODE
      user_start_t = clock();
#endif
      userAlpha(t+dt, dt, grd, w->colNew, w->presNew_g+1,
		w->eIntNew, w->gammaNew,
		w->deltaNew, params, w->alpha_g+1);
#ifdef TESTING_MODE
      user_end_t = clock();
      *userTime += (double) (user_end_t - user_start_t)
	/ CLOCKS_PER_SEC;
#endif
      w->alpha_g[0] = w->alpha_g[1];
      w->alpha_g[grd->nr+1] = w->alpha_g[grd->nr];
    }

    /* Subtract off energy source term from previous iteration; we
       need to do this here because we're about to update gamma */
    for (i=0; i<grd->nr; i++) {
      gsl_vector_set(w->rhs_g, i+1,
		     gsl_vector_get(w->rhs_g, i+1) -
		     0.5*dt*(w->gammaNew[i]-1)*w->intEnSrc[i]);
    }

    /* Get new pressure / enthalpy boundary values */
    if (ibc_func == 1) {
#ifdef TESTING_MODE
      user_start_t = clock();
#endif
      userIBC(t+dt, dt, grd, w->colNew, w->presNew_g+1,
	      w->eIntNew, w->gammaNew, w->deltaNew,
	      ibc_pres, ibc_enth, params,
	      &ibc_pres_val1, &ibc_enth_val1);
#ifdef TESTING_MODE
      user_end_t = clock();
      *userTime += (double) (user_end_t - user_start_t)
	/ CLOCKS_PER_SEC;
#endif
    }
    if (obc_func == 1) {
#ifdef TESTING_MODE
      user_start_t = clock();
#endif
      userOBC(t+dt, dt, grd, w->colNew, w->presNew_g+1,
	      w->eIntNew, w->gammaNew, w->deltaNew,
	      obc_pres, obc_enth, params,
	      &obc_pres_val1, &obc_enth_val1);
#ifdef TESTING_MODE
      user_end_t = clock();
      *userTime += (double) (user_end_t - user_start_t)
	/ CLOCKS_PER_SEC;
#endif
    }

    /* Set boundary conditions from user-specified parameters, and
       also set ghost cell elements in RHS matrix */
    applyBC(grd, w->alpha_g,
	    ibc_pres, ibc_enth, ibc_pres_val1, ibc_enth_val1,
	    obc_pres, obc_enth, obc_pres_val1, obc_enth_val1,
	    w->presNew_g, w->hint_g, &pIn, &qIn, &pOut, &qOut);
    gsl_vector_set(w->rhs_g, 0, pIn);
    gsl_vector_set(w->rhs_g, grd->nr+1, pOut);

    /* Get new enthalpies at cell centers and extrapolated to cell edges */
    if (interpOrder == 1) {
      for (i=0; i<=grd->nr+1; i++)
	w->hintL_g[i] = w->hintR_g[i] = w->hint_g[i];
    } else if (interpOrder == 2) {
      if (grd->linear == 1) {
	for (i=1; i<=grd->nr+1; i++)
	  w->hintR_g[i-1] = w->hintL_g[i] =
	    ((grd->r_g[i]-grd->r_h[i-1]) * w->hint_g[i-1] +
	     (grd->r_h[i-1]-grd->r_g[i-1]) * w->hint_g[i]) /
	    (grd->r_g[i] - grd->r_g[i-1]);
      } else {
	for (i=1; i<=grd->nr+1; i++)
	  w->hintR_g[i-1] = w->hintL_g[i] =
	    (log(grd->r_g[i]/grd->r_h[i-1]) * w->hint_g[i-1] +
	     log(grd->r_h[i-1]/grd->r_g[i-1]) * w->hint_g[i]) /
	    log(grd->r_g[i]/grd->r_g[i-1]);
      }
      /* Limit the slope */
      for (i=1; i<=grd->nr+1; i++) {
	if (w->hintR_g[i-1]/w->hint_g[i-1]-1.0 > SLOPELIMIT) {
	  w->hintR_g[i-1] = w->hint_g[i-1]*(1.0+SLOPELIMIT);
	} else if (w->hintR_g[i-1]/w->hint_g[i-1] < 1.0-SLOPELIMIT) {
	  w->hintR_g[i-1] = w->hint_g[i-1]*(1.0-SLOPELIMIT);
	}
	if (w->hintL_g[i]/w->hint_g[i]-1.0 > SLOPELIMIT) {
	  w->hintL_g[i] = w->hint_g[i]*(1.0+SLOPELIMIT);
	} else if (w->hintL_g[i]/w->hint_g[i] < 1.0-SLOPELIMIT) {
	  w->hintL_g[i] = w->hint_g[i]*(1.0-SLOPELIMIT);
	}
      }
    } else if (interpOrder == 3) {
      ppmExtrap(grd->nr+2, grd->dr_g, w->hint_g, w->hintL_g, w->hintR_g,
		w->ppmwksp_g);
    }

    /**************************************************************/
    /* Step 4b: add energy source term to RHS vector for pressure */
    /* solve                                                      */
    /**************************************************************/

    /* If source term can vary, compute its value for this
       iteration */
    if (intEnSrc_func == 1) {
#ifdef TESTING_MODE
      user_start_t = clock();
#endif
      userIntEnSrc(t+dt, dt, grd, w->colNew, w->presNew_g+1,
		   w->eIntNew, w->gammaNew, w->deltaNew, params,
		   w->intEnSrc);
#ifdef TESTING_MODE
      user_end_t = clock();
      *userTime += (double) (user_end_t - user_start_t)
	/ CLOCKS_PER_SEC;
#endif
    }
    for (i=0; i<grd->nr; i++) {
      gsl_vector_set(w->rhs_g, i+1,
		     gsl_vector_get(w->rhs_g, i+1) +
		     0.5*dt*(w->gammaNew[i]-1)*w->intEnSrc[i]);
    }

    /**************************************************************/
    /* Step 4c: construct the tridiagonal matrix                  */
    /**************************************************************/

    /* Upper diagonal */
    gsl_vector_set(w->ud_g, 0, -qIn);
    for (i=1; i<=grd->nr; i++) {
      if (w->fmNew_h[i] > 0) h_up = w->hintR_g[i] + grd->psiEff_h[i];
      else h_up = w->hintL_g[i+1] + grd->psiEff_h[i];
      gsl_vector_set(w->ud_g, i,
		     0.5*dt*w->alpha_g[i+1]*(w->gammaNew[i-1]-1.0)
		     / grd->area[i-1] *
		     (grd->g_h[i]*SQR(grd->r_g[i+1]) *
		      (1-grd->beta_g[i+1]) *
		      (grd->psiEff_g[i]
		       + w->deltaNew[i-1] *
		       w->presNew_g[i]/w->colNew[i-1]
		       - h_up) +
		      M_PI*grd->r_h[i]*grd->vphi_h[i] *
		      (1-grd->beta_h[i])));
    }

    /* Lower diagonal */
    for (i=0; i<grd->nr; i++) {
      if (w->fmNew_h[i] > 0) h_up = w->hintR_g[i] + grd->psiEff_h[i];
      else h_up = w->hintL_g[i+1] + grd->psiEff_h[i];
      gsl_vector_set(w->ld_g, i,
		     0.5*dt*w->alpha_g[i]*(w->gammaNew[i]-1.0)
		     / grd->area[i] *
		     (grd->g_h[i]*SQR(grd->r_g[i]) *
		      (1-grd->beta_g[i]) *
		      (grd->psiEff_g[i+1]
		       + w->deltaNew[i] *
		       w->presNew_g[i+1]/w->colNew[i]
		       - h_up) -
		      M_PI*grd->r_h[i]*grd->vphi_h[i] *
		      (1-grd->beta_h[i])));
    }
    gsl_vector_set(w->ld_g, grd->nr, -qOut);

    /* Diagonal */
    for (i=1; i<=grd->nr; i++) {
      if (w->fmNew_h[i-1] > 0) h_up = w->hintR_g[i-1] + grd->psiEff_h[i-1];
      else h_up = w->hintL_g[i] + grd->psiEff_h[i-1];
      if (w->fmNew_h[i] > 0) h_up1 = w->hintR_g[i] + grd->psiEff_h[i];
      else h_up1 = w->hintL_g[i+1] + grd->psiEff_h[i];
      gsl_vector_set(w->diag_g, i,
		     1.0 + 0.5*dt*w->alpha_g[i]*(w->gammaNew[i-1]-1.0)
		     / grd->area[i-1] *
		     ((grd->g_h[i]*
		       (h_up1 - grd->psiEff_g[i]
			- w->deltaNew[i-1] *
			w->presNew_g[i]/w->colNew[i-1]) +
		       grd->g_h[i-1]*
		       (h_up - grd->psiEff_g[i]
			- w->deltaNew[i-1] *
			w->presNew_g[i]/w->colNew[i-1])) *
		      SQR(grd->r_g[i]) * (1-grd->beta_g[i]) +
		      M_PI*(grd->r_h[i]*grd->vphi_h[i]*
			    (1-grd->beta_h[i]) -
			    grd->r_h[i-1]*grd->vphi_h[i-1]*
			    (1-grd->beta_h[i-1]))));
    }

    /**************************************************************/
    /* Step 4d: solve the matrix equation to get the new pressure */
    /* guess                                                      */
    /**************************************************************/

    if (gsl_linalg_solve_tridiag(w->diag_g, w->ud_g, w->ld_g,
				 w->rhs_g, w->presTmp_g)) {
      /* Non-zero return code means error has occured, probably due to
	 bad inputs data. Bail out, and let the driver try again */
      if (verbose)
	printf("   ...iteration %ld, detected NaN or Inf in tridiagonal inputs, exiting without convergence!\n", *itCount);
#ifdef TESTING_MODE
      end_t = clock();
      *advanceTime += (double) (end_t - start_t) / CLOCKS_PER_SEC;
#endif
      return(-1.0);
    }

    /**************************************************************/
    /* Step 4e: get the new column density guess                  */
    /**************************************************************/

    /* Get new mass fluxes */
    for (i=0; i<=grd->nr; i++)
      w->fmNew_h[i] = -grd->g_h[i] *
	(SQR(grd->r_g[i+1])*
	 (1-grd->beta_g[i+1]) *
	 gsl_vector_get(w->presTmp_g, i+1)*w->alpha_g[i+1] -
	 SQR(grd->r_g[i])*
	 (1-grd->beta_g[i]) *
	 gsl_vector_get(w->presTmp_g, i)*w->alpha_g[i]);

    /* Get source terms at new time */
    if (massSrc_func == 1) {
#ifdef TESTING_MODE
      user_start_t = clock();
#endif
      userMassSrc(t+dt, dt, grd, w->colNew, w->presNew_g, w->eIntNew,
		  w->gammaNew, w->deltaNew, params, w->massSrcNew);
#ifdef TESTING_MODE
      user_end_t = clock();
      *userTime += (double) (user_end_t - user_start_t)
	/ CLOCKS_PER_SEC;
#endif
    } else {
      for (i=0; i<grd->nr; i++) w->massSrcNew[i] = massSrc_val;
    }

    /* Get new column density guess */
    for (i=0; i<grd->nr; i++) {
      w->colTmp[i] = col[i] - dt/grd->area[i] * 0.5 *
	(w->fmNew_h[i+1] - w->fmNew_h[i] +
	 w->fmLast_h[i+1] - w->fmLast_h[i]) +
	dt * 0.5 * (w->massSrcLast[i] + w->massSrcNew[i]);
    }

    /**************************************************************/
    /* Step 4f: if doing separate internal energy evolution, get  */
    /* new internal energy guess                                  */
    /**************************************************************/

    if (eos_func == 1) {
      for (i=0; i<grd->nr; i++) {
	w->eIntTmp[i] = eInt[i] +
	  0.5 * (1.0/(w->gammaLast[i]-1.0) +
		 1.0/(w->gammaNew[i]-1.0)) *
	  (gsl_vector_get(w->presTmp_g, i+1) - pres[i]) +
	  0.5 *
	  (w->deltaLast[i]*pres[i]/col[i] +
	   w->deltaNew[i]*gsl_vector_get(w->presTmp_g, i+1) /
	   w->colTmp[i]) * (w->colTmp[i]-col[i]);
      }
    }

    /**************************************************************/
    /* Step 4g: check for convergence or loop termination; print  */
    /* status message                                             */
    /**************************************************************/

    /* Compute residuals on pressure */
    err = 0.0;
    maxErrCell = -1;
    residType = 0;
    for (i=0; i<grd->nr; i++) {
      if (!isfinite(gsl_vector_get(w->presTmp_g, i+1))) {
	/* If we've diverged to the point of getting inf's, we won't
	   converge, so bail out */

	if (verbose)
	  printf("   ...iteration %ld, detected NaN or Inf in tridiagonal solution, cell %ld, exiting without convergence!\n", *itCount, i);
#ifdef TESTING_MODE
	end_t = clock();
	*advanceTime += (double) (end_t - start_t) / CLOCKS_PER_SEC;
#endif
	return(-1.0);
      }
      errCell = (gsl_vector_get(w->presTmp_g, i+1) -
		 w->presNew_g[i+1]) /
	gsl_vector_get(w->presTmp_g, i+1);
      if (fabs(errCell) > fabs(err)) {
	maxErrCell = i;
	err = errCell;
      }
    }

    /* Compute residuals on column density */
    for (i=0; i<grd->nr; i++) {
      errCell = (w->colTmp[i] - w->colNew[i]) / w->colTmp[i];
      if (fabs(errCell) > fabs(err)) {
	maxErrCell = i;
	err = errCell;
	residType = 1;
      }
    }

    /* Compute residuals on internal energy */
    for (i=0; i<grd->nr; i++) {
      errCell = (w->eIntTmp[i] - w->eIntNew[i]) / w->eIntTmp[i];
      if (fabs(errCell) > fabs(err)) {
	maxErrCell = i;
	err = errCell;
	residType = 2;
      }
    }

#ifdef TESTING_MODE
    /* In testing mode, store residuals and residual type */
    resid[*itCount-1] += err;
    rtype[*itCount-1] = residType;
#endif

    /* Print status */
    if (fabs(err) < errTol) converged = 1;
    if (verbose) {
      printf("   ...iteration %ld, max residual = %e in cell %ld",
	     *itCount, err, maxErrCell);
      if (residType==0)
	printf(" pressure\n");
      else if (residType==1)
	printf(" density\n");
      else if (residType==2)
	printf(" internal energy\n");
      if (converged == 1) {
	printf("   ...converged!\n");
      } else if (*itCount == maxIter) {
	printf("   ...reached maximum iterations, exiting without convergence! Max residual = %e in cell %ld\n",
	       err, maxErrCell);
      }
    }

    /* If we have converged, shift temporaries into final arrays, then
       exit the loop */
    if (converged) {
      for (i=0; i<grd->nr; i++) {
	w->colNew[i] = w->colTmp[i];
	w->presNew_g[i+1] = gsl_vector_get(w->presTmp_g, i+1);
	if (eos_func == 1)
	  w->eIntNew[i] = w->eIntTmp[i];
      }
      w->presNew_g[0] = gsl_vector_get(w->presTmp_g, 0);
      w->presNew_g[grd->nr+1] = gsl_vector_get(w->presTmp_g, grd->nr+1);
      break;
    }

    /* If we've exceeded iteration limit, bail out */
    if (*itCount == maxIter) {
#ifdef TESTING_MODE
      end_t = clock();
      *advanceTime += (double) (end_t - start_t) / CLOCKS_PER_SEC;
#endif
      return(-1.0);
    }

    /**************************************************************/
    /* Step 4h: if we're here, we haven't converged yet, so use   */
    /* Anderson acceleration to generate new guesses at solution  */
    /**************************************************************/

#ifdef TESTING_MODE
    next_iter_start_t = clock();
#endif
    getNextIterate(grd, eos_func, w
#if AA_M > 0
		   , *itCount, nHist, &residMax
#endif
		   );
#ifdef TESTING_MODE
    next_iter_end_t = clock();
    *nextIterTime += (double) (next_iter_end_t - next_iter_start_t) /
      CLOCKS_PER_SEC;
#endif

  } /* End main iteration loop */

  /****************************************************************/
  /* Step 5: compute rate of change of pressure and column        */
  /* density; use this to estimate new time step                  */
  /****************************************************************/

  dtMin = LARGE;
  for (i=0; i<grd->nr; i++) {
    dCol = fabs(w->colNew[i] - col[i]) + SMALL;
    dPres = fabs(w->presNew_g[i+1] - pres[i]) + SMALL;
    dtMin = fmin( dtMin, dtTol*dt *
		  fmin(col[i]/dCol, pres[i]/dPres) );
    if (eos_func == 1) {
      dEInt = fabs(w->eIntNew[i] - eInt[i]) + SMALL;
      dtMin = fmin( dtMin, dtTol*dt * eInt[i]/dEInt );
    }
  }
  if (verbose > 0)
    printf("advanceCN: estimated dtNew = %e\n", dtMin);

  /****************************************************************/
  /* Step 6: record the total mass and energy transported across  */
  /* the inner and outer boundaries during this time step.        */
  /****************************************************************/

  /* Mass fluxes */
  mBndTemp[0] += 0.5*dt*(w->fmLast_h[0] + w->fmNew_h[0]);
  mBndTemp[1] += 0.5*dt*(w->fmLast_h[grd->nr] + w->fmNew_h[grd->nr]);

  /* Old time energy fluxes */
  eBndTemp[0] += 0.5*dt*(w->feLast_h[0] + w->ftLast_h[0]);
  eBndTemp[1] += 0.5*dt*(w->feLast_h[grd->nr] + w->ftLast_h[grd->nr]);

  /* New time energy fluxes; need to get enthalpy to evaluate f_E */
  if (w->fmNew_h[0] > 0) h_up = w->hintR_g[0] + grd->psiEff_h[0];
  else h_up = w->hintL_g[1] + grd->psiEff_h[0];
  eBndTemp[0] += 0.5*dt*h_up*w->fmNew_h[0];
  if (w->fmNew_h[grd->nr] > 0)
    h_up1 = w->hintR_g[grd->nr] + grd->psiEff_h[grd->nr];
  else h_up1 = w->hintL_g[grd->nr+1] + grd->psiEff_h[grd->nr];
  eBndTemp[1] += 0.5*dt*h_up1*w->fmNew_h[grd->nr];

  /* New time torque fluxes */
  eBndTemp[0] += 0.5*dt*M_PI*grd->r_h[0]*grd->vphi_h[0]*(1.0-grd->beta_h[0]) *
    (w->presNew_g[1]*w->alpha_g[1] + w->presNew_g[0]*w->alpha_g[0]);
  eBndTemp[1] += 0.5*dt*M_PI*grd->r_h[grd->nr]*grd->vphi_h[grd->nr]
    *(1.0-grd->beta_h[grd->nr]) *
    (w->presNew_g[grd->nr+1]*w->alpha_g[grd->nr+1]
     + w->presNew_g[grd->nr]*w->alpha_g[grd->nr]);

  /****************************************************************/
  /* Step 7: if using source functions, record total mass and     */
  /* energy added to every cell by sources                        */
  /****************************************************************/
;
  if (massSrc_func == 1) {
    for (i=0; i<grd->nr; i++) {
      w->mSrc[i] += 0.5 * dt * w->massSrcNew[i];
      w->eSrc[i] += 0.5 * dt * w->massSrcNew[i] *
	(grd->psiEff_g[i+1] +
	 w->deltaNew[i]*w->presNew_g[i+1]/w->colNew[i]);
    }
  }
  if (intEnSrc_func == 1) {
    for (i=0; i<grd->nr; i++)
      w->eSrc[i] += 0.5 * dt * w->intEnSrc[i];
  }

  /****************************************************************/
  /* Step 8: store final values in arrays                         */
  /****************************************************************/

  if (noUpdate==0) {
    for (i=0; i<grd->nr; i++) {
      pres[i] = w->presNew_g[i+1];
      col[i] = w->colNew[i];
    }
    if (eos_func==1)
      for (i=0; i<grd->nr; i++) eInt[i] = w->eIntNew[i];
    mBnd[0] = mBndTemp[0];
    mBnd[1] = mBndTemp[1];
    eBnd[0] = eBndTemp[0];
    eBnd[1] = eBndTemp[1];
    if (massSrc_func == 1) {
      for (i=0; i<grd->nr; i++) {
	mSrc[i] = w->mSrc[i];
	eSrc[i] = w->eSrc[i];
      }
    } else if (intEnSrc_func == 1) {
      for (i=0; i<grd->nr; i++) eSrc[i] = w->eSrc[i];
    }
  }

  /****************************************************************/
  /* Step 9: return new time step                                */
  /****************************************************************/

#ifdef TESTING_MODE
  end_t = clock();
  *advanceTime += (double) (end_t - start_t) / CLOCKS_PER_SEC;
#endif
  return(dtMin);
}

max_stars_repo_name: franciscaconcha/amuse-vader

max_issues_repo_name: franciscaconcha/amuse-vader

max_forks_repo_name: franciscaconcha/amuse-vader


text:
/* interpolation/gsl_interp.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2004 Gerard Jungman
 *
 * 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 2 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.
 */

/* Author:  G. Jungman
 */
#ifndef __GSL_INTERP_H__
#define __GSL_INTERP_H__
#include <stdlib.h>
#include <gsl/gsl_types.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

/* evaluation accelerator */
typedef struct {
  size_t  cache;        /* cache of index   */
  size_t  miss_count;   /* keep statistics  */
  size_t  hit_count;
}
gsl_interp_accel;

/* interpolation object type */
typedef struct {
  const char * name;
  unsigned int min_size;
  void *  (*alloc) (size_t size);
  int     (*init)    (void *, const double xa[], const double ya[], size_t size);
  int     (*eval)    (const void *, const double xa[], const double ya[], size_t size, double x, gsl_interp_accel *, double * y);
  int     (*eval_deriv)  (const void *, const double xa[], const double ya[], size_t size, double x, gsl_interp_accel *, double * y_p);
  int     (*eval_deriv2) (const void *, const double xa[], const double ya[], size_t size, double x, gsl_interp_accel *, double * y_pp);
  int     (*eval_integ)  (const void *, const double xa[], const double ya[], size_t size, gsl_interp_accel *, double a, double b, double * result);
  void    (*free)         (void *);

} gsl_interp_type;

/* general interpolation object */
typedef struct {
  const gsl_interp_type * type;
  double  xmin;
  double  xmax;
  size_t  size;
  void * state;
} gsl_interp;

/* available types */
GSL_VAR const gsl_interp_type * gsl_interp_linear;
GSL_VAR const gsl_interp_type * gsl_interp_polynomial;
GSL_VAR const gsl_interp_type * gsl_interp_cspline;
GSL_VAR const gsl_interp_type * gsl_interp_cspline_periodic;
GSL_VAR const gsl_interp_type * gsl_interp_akima;
GSL_VAR const gsl_interp_type * gsl_interp_akima_periodic;

gsl_interp_accel *
gsl_interp_accel_alloc(void);

size_t
gsl_interp_accel_find(gsl_interp_accel * a, const double x_array[], size_t size, double x);

int
gsl_interp_accel_reset (gsl_interp_accel * a);

void
gsl_interp_accel_free(gsl_interp_accel * a);

gsl_interp *
gsl_interp_alloc(const gsl_interp_type * T, size_t n);

int
gsl_interp_init(gsl_interp * obj, const double xa[], const double ya[], size_t size);

const char * gsl_interp_name(const gsl_interp * interp);
unsigned int gsl_interp_min_size(const gsl_interp * interp);

int
gsl_interp_eval_e(const gsl_interp * obj,
                  const double xa[], const double ya[], double x,
                  gsl_interp_accel * a, double * y);

double
gsl_interp_eval(const gsl_interp * obj,
                const double xa[], const double ya[], double x,
                gsl_interp_accel * a);

int
gsl_interp_eval_deriv_e(const gsl_interp * obj,
                        const double xa[], const double ya[], double x,
                        gsl_interp_accel * a,
                        double * d);

double
gsl_interp_eval_deriv(const gsl_interp * obj,
                      const double xa[], const double ya[], double x,
                      gsl_interp_accel * a);

int
gsl_interp_eval_deriv2_e(const gsl_interp * obj,
                         const double xa[], const double ya[], double x,
                         gsl_interp_accel * a,
                         double * d2);

double
gsl_interp_eval_deriv2(const gsl_interp * obj,
                       const double xa[], const double ya[], double x,
                       gsl_interp_accel * a);

int
gsl_interp_eval_integ_e(const gsl_interp * obj,
                        const double xa[], const double ya[],
                        double a, double b,
                        gsl_interp_accel * acc,
                        double * result);

double
gsl_interp_eval_integ(const gsl_interp * obj,
                      const double xa[], const double ya[],
                      double a, double b,
                      gsl_interp_accel * acc);

void
gsl_interp_free(gsl_interp * interp);

size_t gsl_interp_bsearch(const double x_array[], double x,
                          size_t index_lo, size_t index_hi);

#ifdef HAVE_INLINE
extern inline size_t
gsl_interp_bsearch(const double x_array[], double x,
                   size_t index_lo, size_t index_hi);

extern inline size_t
gsl_interp_bsearch(const double x_array[], double x,
                   size_t index_lo, size_t index_hi)
{
  size_t ilo = index_lo;
  size_t ihi = index_hi;
  while(ihi > ilo + 1) {
    size_t i = (ihi + ilo)/2;
    if(x_array[i] > x)
      ihi = i;
    else
      ilo = i;
  }

  return ilo;
}
#endif

#ifdef HAVE_INLINE
extern inline size_t
gsl_interp_accel_find(gsl_interp_accel * a, const double xa[], size_t len, double x)
{
  size_t x_index = a->cache;

  if(x < xa[x_index]) {
    a->miss_count++;
    a->cache = gsl_interp_bsearch(xa, x, 0, x_index);
  }
  else if(x > xa[x_index + 1]) {
    a->miss_count++;
    a->cache = gsl_interp_bsearch(xa, x, x_index, len-1);
  }
  else {
    a->hit_count++;
  }

  return a->cache;
}
#endif /* HAVE_INLINE */

__END_DECLS

#endif /* __GSL_INTERP_H__ */


text:
#include <math.h>
// Constants not defined in MSVC's math.h
#ifndef M_SQRT1_2
#define M_SQRT1_2 0.70710678118654746172
#endif
#ifndef M_2_SQRTPI
#define M_2_SQRTPI 1.12837916709551255856
#endif
#include <gsl/gsl_spline.h>
#include <galpy_potentials.h>
// ChandrasekharDynamicalFrictionForce: 8 arguments: amp,ms,rhm,gamma^2,
// lnLambda, minr^2, ro, rf
double ChandrasekharDynamicalFrictionForceAmplitude(double R,double z,
						    double phi,double t,
						    double r2,
						    struct potentialArg * potentialArgs,
						    double vR,double vT,
						    double vz){
  double sr,X,Xfactor,d_ind,forceAmplitude;
  double * args= potentialArgs->args;
  //Get args
  double amp= *args;
  double ms= *(args+9);
  double rhm= *(args+10);
  double gamma2= *(args+11);
  double lnLambda= *(args+12);
  double ro= *(args+14);
  double rf= *(args+15);
  double GMvs;
  double r= sqrt( r2 );
  double v2=  vR * vR + vT * vT + vz * vz;
  double v= sqrt( v2 );
  // Constant or variable Lambda
  if ( lnLambda < 0 ) {
    GMvs= ms/v/v;
    if ( GMvs < rhm )
      lnLambda= 0.5 * log ( 1. + r2 / gamma2 / rhm / rhm );
    else
      lnLambda= 0.5 * log ( 1. + r2 / gamma2 / GMvs / GMvs );
  }
  d_ind= (r-ro)/(rf-ro);
  d_ind= d_ind <  0 ? 0. : ( d_ind > 1 ? 1. : d_ind);
  sr= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
  X= M_SQRT1_2 * v / sr;
  Xfactor= erf ( X ) - M_2_SQRTPI * X * exp ( - X * X );
  forceAmplitude= - amp * Xfactor * lnLambda / v2 / v \
    * calcDensity(R,z,phi,t,potentialArgs->nwrapped,
		  potentialArgs->wrappedPotentialArg);
  // Caching
  *(args + 1)= R;
  *(args + 2)= z;
  *(args + 3)= phi;
  *(args + 4)= t;
  *(args + 5)= vR;
  *(args + 6)= vT;
  *(args + 7)= vz;
  *(args + 8)= forceAmplitude;
  return forceAmplitude;
}
double ChandrasekharDynamicalFrictionForceRforce(double R,double z, double phi,
						 double t,
						 struct potentialArg * potentialArgs,
						 double vR,double vT,
						 double vz){
  double forceAmplitude;
  double * args= potentialArgs->args;
  double r2=  R * R + z * z;
  if ( r2 < *(args+13) )  // r < minr, don't bother caching
    return 0.;
  //Get args
  double cached_R= *(args + 1);
  double cached_z= *(args + 2);
  double cached_phi= *(args + 3);
  double cached_t= *(args + 4);
  double cached_vR= *(args + 5);
  double cached_vT= *(args + 6);
  double cached_vz= *(args + 7);
  if ( R != cached_R || phi != cached_phi || z != cached_z || t != cached_t \
       || vR != cached_vR || vT != cached_vT || vz != cached_vz )
    forceAmplitude= ChandrasekharDynamicalFrictionForceAmplitude(R,z,phi,t,r2,
								 potentialArgs,
								 vR,vT,vz);
  else
    forceAmplitude= *(args + 8);
  return forceAmplitude * vR;
}
double ChandrasekharDynamicalFrictionForcezforce(double R,double z, double phi,
						 double t,
						 struct potentialArg * potentialArgs,
						 double vR,double vT,
						 double vz){
  double forceAmplitude;
  double * args= potentialArgs->args;
  double r2=  R * R + z * z;
  if ( r2 < *(args+13) )  // r < minr, don't bother caching
    return 0.;
  //Get args
  double cached_R= *(args + 1);
  double cached_z= *(args + 2);
  double cached_phi= *(args + 3);
  double cached_t= *(args + 4);
  double cached_vR= *(args + 5);
  double cached_vT= *(args + 6);
  double cached_vz= *(args + 7);
  if ( R != cached_R || phi != cached_phi || z != cached_z || t != cached_t \
       || vR != cached_vR || vT != cached_vT || vz != cached_vz )
    //LCOV_EXCL_START
    forceAmplitude= ChandrasekharDynamicalFrictionForceAmplitude(R,z,phi,t,r2,
								 potentialArgs,
								 vR,vT,vz);
    //LCOV_EXCL_STOP
  else
    forceAmplitude= *(args + 8);
  return forceAmplitude * vz;
}
double ChandrasekharDynamicalFrictionForcephiforce(double R,double z,
						   double phi,double t,
						   struct potentialArg * potentialArgs,
						   double vR,double vT,
						   double vz){
  double forceAmplitude;
  double * args= potentialArgs->args;
  double r2=  R * R + z * z;
  if ( r2 < *(args+13) )  // r < minr, don't bother caching
    return 0.;
  //Get args
  double cached_R= *(args + 1);
  double cached_z= *(args + 2);
  double cached_phi= *(args + 3);
  double cached_t= *(args + 4);
  double cached_vR= *(args + 5);
  double cached_vT= *(args + 6);
  double cached_vz= *(args + 7);
  if ( R != cached_R || phi != cached_phi || z != cached_z || t != cached_t \
       || vR != cached_vR || vT != cached_vT || vz != cached_vz )
    //LCOV_EXCL_START
    forceAmplitude= ChandrasekharDynamicalFrictionForceAmplitude(R,z,phi,t,r2,
								 potentialArgs,
								 vR,vT,vz);
    //LCOV_EXCL_STOP
  else
    forceAmplitude= *(args + 8);
  return forceAmplitude * vT * R;
}

max_stars_repo_name: gusbeane/galpy

max_issues_repo_name: gusbeane/galpy

max_forks_repo_name: gusbeane/galpy


text:
// SPDX-License-Identifier: Apache-2.0
/**
 * Copyright (C) 2020 Jijoong Moon <jijoong.moon@samsung.com>
 *
 * @file   blas_interface.h
 * @date   28 Aug 2020
 * @see    https://github.com/nnstreamer/nntrainer
 * @author Jijoong Moon <jijoong.moon@samsung.com>
 * @bug    No known bugs except for NYI items
 * @brief  This is dummy header for blas support
 *
 */

#ifndef __BLAS_INTERFACE_H_
#define __BLAS_INTERFACE_H_
#ifdef __cplusplus

#ifdef USE_BLAS
extern "C" {
#include <cblas.h>
}
#else
enum CBLAS_ORDER { CblasRowMajor = 101, CblasColMajor = 102 };

enum CBLAS_TRANSPOSE {
  CblasNoTrans = 111,
  CblasTrans = 112,
  CblasConjTrans = 113
};
#endif

#ifdef USE_CUBLAS
#include <helper_cuda.h>
#include <helper_functions.h>
#endif

namespace nntrainer {

void sscal(const int N, const float alpha, float *X, const int incX);

float snrm2(const int N, const float *X, const int incX);

void scopy(const unsigned int N, const float *X, const int incX, float *Y,
           const int intY);

float sdot(const unsigned int N, const float *X, const unsigned int incX,
           const float *Y, const unsigned int incY);

void saxpy(const unsigned int N, const float alpha, const float *X,
           const int incX, float *Y, const int incY);

void sgemm(CBLAS_ORDER order, CBLAS_TRANSPOSE TransA, CBLAS_TRANSPOSE TransB,
           const unsigned int M, const unsigned int N, const unsigned int K,
           const float alpha, const float *A, const unsigned int lda,
           const float *B, const unsigned int ldb, const float beta, float *C,
           const unsigned int ldc);

void sgemv(CBLAS_ORDER order, CBLAS_TRANSPOSE TransA, const unsigned int M,
           const unsigned int N, const float alpha, const float *A,
           const unsigned int lda, const float *X, const int incX,
           const float beta, float *Y, const int incY);

unsigned int isamax(const unsigned int N, const float *X, const int incX);

} /* namespace nntrainer */
#endif /* __cplusplus */
#endif /* __BLAS_INTERFACE_H__ */


text:
/****************************************************************
  wigner_gsl.h / wigner_gsl_twice.h

  Defines Wigner coupling and recoupling symbols as wrappers for GSL
  angular momentum functions:

    - wigner_gsl.h -- takes HalfInt angular momentum arguments (RECOMMENDED)
    - wigner_gsl_twice.h -- takes integer "twice value" angular momentum arguments

  Naming convention:
    - Function names *not* ending in '2' accept HalfInt arguments J.
    - Function names ending in '2' accept integer arguments 2*J.

  See, e.g., appendix to de Shalit and Talmi for underlying formulas.

  DOCUMENTATION: See wigner_gsl.h (rather than wigner_gsl_twice.h) for
  more complete function comments.

  Language: C++

  Mark A. Caprio
  University of Notre Dame

  2/16/10 (mac): Created.
  11/13/15 (mac): Add unitary 6J for (12)3-(13)2 recoupling
    and Racah reduction factor.
  2/27/16 (mac): Update includes for restructured header files.
  3/8/16 (mac): Enclose in namespace.
  6/8/16 (mac): Update #define guard directive.
  6/21/16 (mac): Remove Racah reduction factor. Update comments.
  10/18/16 (mac): Update Unitary6J comment. Rename wigner2_gsl.h to
    wigner_gsl_twice.h.
  4/28/18 (mac): Restore missing Hat2 and ParitySign2 to
    wigner_gsl_twice.h.

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

#ifndef WIGNER_GSL_H_
#define WIGNER_GSL_H_

#include <gsl/gsl_sf_coupling.h>

#include "am.h"

namespace am {

  // Wigner3J(ja,jb,jc,ma,mb,mc)
  //   returns Wigner 3-J symbol
  //   wrapper for gsl_sf_coupling_3j

  inline
    double Wigner3J(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& ma, const HalfInt& mb, const HalfInt& mc
      )
  {
    return gsl_sf_coupling_3j(
        TwiceValue(ja), TwiceValue(jb), TwiceValue(jc),
        TwiceValue(ma), TwiceValue(mb), TwiceValue(mc)
      );
  }

  // ClebschGordan(ja,ma,jb,mb,jc,mc)
  //   returns Clebsch-Gordan coefficient
  //   wrapper for gsl_sf_coupling_3j

  inline
    double ClebschGordan(
        const HalfInt& ja, const HalfInt& ma,
        const HalfInt& jb, const HalfInt& mb,
        const HalfInt& jc, const HalfInt& mc
      )
  {
    return Hat(jc)*ParitySign(ja-jb+mc)
      *gsl_sf_coupling_3j(
          TwiceValue(ja), TwiceValue(jb), TwiceValue(jc),
          TwiceValue(ma), TwiceValue(mb), -TwiceValue(mc)
        );
  }

  // Wigner6J(ja,jb,jc,jd,je,jf)
  //   returns Wigner 6-J symbol
  //   wrapper for gsl_sf_coupling_6j

  inline
    double Wigner6J(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& jd, const HalfInt& je, const HalfInt& jf
      )
  {
    return gsl_sf_coupling_6j(
        TwiceValue(ja), TwiceValue(jb), TwiceValue(jc),
        TwiceValue(jd), TwiceValue(je), TwiceValue(jf)
      );
  }

  // Unitary6J(ja,jb,jc,jd,je,jf)
  //   wrapper for gsl_sf_coupling_6j
  //   returns unitary recoupling symbol for (12)3-1(23) recoupling
  //
  //   Arguments follow row order in 6J symbol:
  //     Unitary6J(J1,J2,J12,J3,J,J23)
  //   This is equivalent to U(J1,J2,J,J3,J12,J23), though neither is a
  //   particularly memorable ordering.  Why not (J1,J2,J3,J12,J23,J)?!

  inline
    double Unitary6J(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& jd, const HalfInt& je, const HalfInt& jf
      )
  {
    return ParitySign(ja+jb+jd+je)*Hat(jc)*Hat(jf)
      *Wigner6J(ja,jb,jc,jd,je,jf);
  }

  // Unitary6JZ(ja,jb,jc,jd,je,jf)
  //   wrapper for gsl_sf_coupling_6j
  //   returns unitary recoupling symbol for (12)3-(13)2 recoupling
  //   the name "Z" follows Millener for this purpose
  //
  //   Arguments follow row order in 6J symbol:
  //     Unitary6JZ(J1,J2,J12,J,J3,J13)
  //   This is equivalent to Z(J2,J1,J,J3,J12,J13), though neither is a
  //   particularly memorable ordering.  Why not (J1,J2,J3,J12,J13,J)?!

  inline
    double Unitary6JZ(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& jd, const HalfInt& je, const HalfInt& jf
      )
  {
    return ParitySign(jb+je+jc+jf)*Hat(jc)*Hat(jf)
      *Wigner6J(ja,jb,jc,jd,je,jf);
  }

  // Wigner9J(ja,jb,jc,jd,je,jf,jg,jh,ji)
  //   returns Wigner 9-J symbol
  //   wrapper for gsl_sf_coupling_9j

  inline
    double Wigner9J(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& jd, const HalfInt& je, const HalfInt& jf,
        const HalfInt& jg, const HalfInt& jh, const HalfInt& ji
      )
  {
    return gsl_sf_coupling_9j(
        TwiceValue(ja), TwiceValue(jb), TwiceValue(jc),
        TwiceValue(jd), TwiceValue(je), TwiceValue(jf),
        TwiceValue(jg), TwiceValue(jh), TwiceValue(ji)
      );
  }

  // Unitary9J(ja,jb,jc,jd,je,jf,jg,jh,ji)
  //   returns unitary 9-J symbol
  //   wrapper for gsl_sf_coupling_9j

  inline
    double Unitary9J(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& jd, const HalfInt& je, const HalfInt& jf,
        const HalfInt& jg, const HalfInt& jh, const HalfInt& ji
      )
  {
    return Hat(jc)*Hat(jf)*Hat(jg)*Hat(jh)
      *Wigner9J(
          ja, jb, jc,
          jd, je, jf,
          jg, jh, ji
        );
  }

} // namespace

#endif

max_stars_repo_name: e-eight/am

max_issues_repo_name: e-eight/am

max_forks_repo_name: e-eight/am


text:
/* ode-initval/rk2imp.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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 2 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.
 */

/* Runge-Kutta 2, Gaussian implicit. Also known as the implicit
   midpoint rule. */

/* Author:  G. Jungman */

/* Error estimation by step doubling, see eg. Ascher, U.M., Petzold,
   L.R., Computer methods for ordinary differential and
   differential-algebraic equations, SIAM, Philadelphia, 1998.
   The method is also described in eg. this reference.
*/

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>

#include "odeiv_util.h"

typedef struct
{
  double *Y1;
  double *y0;
  double *ytmp;
  double *y_onestep;
  double *y0_orig;
}
rk2imp_state_t;

static void *
rk2imp_alloc (size_t dim)
{
  rk2imp_state_t *state = (rk2imp_state_t *) malloc (sizeof (rk2imp_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for rk2imp_state",
                      GSL_ENOMEM);
    }

  state->Y1 = (double *) malloc (dim * sizeof (double));

  if (state->Y1 == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for Y1", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->Y1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  state->y0 = (double *) malloc (dim * sizeof (double));

  if (state->y0 == 0)
    {
      free (state->Y1);
      free (state->ytmp);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
    }

  state->y_onestep = (double *) malloc (dim * sizeof (double));

  if (state->y_onestep == 0)
    {
      free (state->Y1);
      free (state->ytmp);
      free (state->y0);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y_onestep", GSL_ENOMEM);
    }

  state->y0_orig = (double *) malloc (dim * sizeof (double));

  if (state->y0_orig == 0)
    {
      free (state->y_onestep);
      free (state->Y1);
      free (state->ytmp);
      free (state->y0);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0_orig", GSL_ENOMEM);
    }

  return state;
}

static int
rk2imp_step (double *y, rk2imp_state_t *state,
	     const double h, const double t,
	     const size_t dim, const gsl_odeiv_system *sys)
{
  /* Makes a Runge-Kutta 2nd order implicit advance with step size h.
     y0 is initial values of variables y.

     The implicit matrix equations to solve are:

     Y1 = y0 + h/2 * f(t + h/2, Y1)

     y = y0 + h * f(t + h/2, Y1)
  */

  const double *y0 = state->y0;
  double *Y1 = state->Y1;
  double *ytmp = state->ytmp;
  int max_iter=3;
  int nu;
  size_t i;

  /* iterative solution of Y1 = y0 + h/2 * f(t + h/2, Y1)
     Y1 should include initial values at call.

     Note: This method does not check for convergence of the
     iterative solution!
  */

  for (nu = 0; nu < max_iter; nu++)
    {
      for (i = 0; i < dim; i++)
        {
          ytmp[i] = y0[i] + 0.5 * h * Y1[i];
        }

      {
	int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, Y1);

	if (s != GSL_SUCCESS)
	  {
	    return s;
	  }
      }
    }

  /* assignment */

  for (i = 0; i < dim; i++)
    {
      y[i] = y0[i] + h * Y1[i];
    }

  return GSL_SUCCESS;
}

static int
rk2imp_apply (void *vstate,
              size_t dim,
              double t,
              double h,
              double y[],
              double yerr[],
              const double dydt_in[],
              double dydt_out[], const gsl_odeiv_system * sys)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;

  size_t i;

  double *Y1 = state->Y1;
  double *y0 = state->y0;
  double *y_onestep = state->y_onestep;
  double *y0_orig = state->y0_orig;

  /* Error estimation is done by step doubling procedure */

  /* initialization step */

  DBL_MEMCPY (y0, y, dim);

  /* Save initial values for possible failures */
  DBL_MEMCPY (y0_orig, y, dim);

  if (dydt_in != NULL)
    {
      DBL_MEMCPY (Y1, dydt_in, dim);
    }

  else
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t, y, Y1);

      if (s != GSL_SUCCESS)
	{
	  return s;
	}
    }

  /* First traverse h with one step (save to y_onestep) */

  DBL_MEMCPY (y_onestep, y, dim);

  {
    int s = rk2imp_step (y_onestep, state, h, t, dim, sys);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

 /* Then with two steps with half step length (save to y) */

  {
    int s = rk2imp_step (y, state, h / 2.0, t, dim, sys);

    if (s != GSL_SUCCESS)
      {
	/* Restore original y vector */
	DBL_MEMCPY (y, y0_orig, dim);

	return s;
      }
  }

  DBL_MEMCPY (y0, y, dim);

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h / 2.0, y, Y1);

    if (s != GSL_SUCCESS)
      {
	/* Restore original y vector */
	DBL_MEMCPY (y, y0_orig, dim);

	return s;
      }
  }

  {
    int s = rk2imp_step (y, state, h / 2.0, t + h / 2.0, dim, sys);

    if (s != GSL_SUCCESS)
      {
	/* Restore original y vector */
	DBL_MEMCPY (y, y0_orig, dim);

	return s;
      }
  }

  /* Derivatives at output */

  if (dydt_out != NULL)
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);

      if (s != GSL_SUCCESS)
	{
	  /* Restore original y vector */
	  DBL_MEMCPY (y, y0_orig, dim);

	  return s;
	}
    }

  /* Error estimation */

  for (i = 0; i < dim; i++)
    {
      yerr[i] = 4.0 * (y[i] - y_onestep[i]) / 3.0;
    }

  return GSL_SUCCESS;
}

static int
rk2imp_reset (void *vstate, size_t dim)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;

  DBL_ZERO_MEMSET (state->Y1, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);
  DBL_ZERO_MEMSET (state->y0, dim);
  DBL_ZERO_MEMSET (state->y_onestep, dim);
  DBL_ZERO_MEMSET (state->y0_orig, dim);

  return GSL_SUCCESS;
}

static unsigned int
rk2imp_order (void *vstate)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 2;
}

static void
rk2imp_free (void *vstate)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;

  free (state->Y1);
  free (state->ytmp);
  free (state->y0);
  free (state->y_onestep);
  free (state->y0_orig);
  free (state);
}

static const gsl_odeiv_step_type rk2imp_type = { "rk2imp",      /* name */
  1,                            /* can use dydt_in */
  1,                            /* gives exact dydt_out */
  &rk2imp_alloc,
  &rk2imp_apply,
  &rk2imp_reset,
  &rk2imp_order,
  &rk2imp_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_rk2imp = &rk2imp_type;


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

#include "Lambda.h"
#include "LambdaCreationFunctions.h"
#include "SelectionComp.h"
#include "PDBVector.h"
#include "SumResult.h"
#include "IntDoubleVectorPair.h"
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <random>
#include <gsl/gsl_vector.h>

/* The class for initializing the topic-probability for a document */
using namespace pdb;
class LDAInitialTopicProbSelection : public SelectionComp<IntDoubleVectorPair, SumResult> {

private:
    Vector<double> prior;
    Handle<Vector<char>> myMem;

public:
    ENABLE_DEEP_COPY

    LDAInitialTopicProbSelection() {}
    LDAInitialTopicProbSelection(Vector<double>& fromPrior) {

        this->prior = fromPrior;

	/* Prepare the random number genrator */
        /* Set up the gsl_rng *src */
        gsl_rng* src = gsl_rng_alloc(gsl_rng_mt19937);
        std::random_device rd;
        std::mt19937 gen(rd());
        gsl_rng_set(src, gen());

        /* Allocate space needed for myRand */
        int spaceNeeded = sizeof(gsl_rng) + src->type->size;
        myMem = makeObject<Vector<char>>(spaceNeeded, spaceNeeded);

        /* Copy src over */
        memcpy(myMem->c_ptr(), src, sizeof(gsl_rng));
        memcpy(myMem->c_ptr() + sizeof(gsl_rng), src->state, src->type->size);

        gsl_rng_free(src);
    }

    Lambda<bool> getSelection(Handle<SumResult> checkMe) override {
        return makeLambda(checkMe, [](Handle<SumResult>& checkMe) { return true; });
    }

    Lambda<Handle<IntDoubleVectorPair>> getProjection(Handle<SumResult> checkMe) override {
        return makeLambda(checkMe, [&](Handle<SumResult>& checkMe) {

            gsl_rng* rng = getRng();
            Handle<IntDoubleVectorPair> result = makeObject<IntDoubleVectorPair>();
            int topicNum = this->prior.size();
            result->setInt(checkMe->getKey());
            Handle<Vector<double>> mySamples = makeObject<Vector<double>>(topicNum, topicNum);

	    /* Sample for the topic-probability */
            gsl_ran_dirichlet(rng, topicNum, this->prior.c_ptr(), mySamples->c_ptr());

            result->setVector(mySamples);

            return result;
        });
    }

    /* Get the GSL RNG from myMem */
    gsl_rng* getRng() {
        gsl_rng* dst = (gsl_rng*)myMem->c_ptr();
        dst->state = (void*)(myMem->c_ptr() + sizeof(gsl_rng));
        dst->type = gsl_rng_mt19937;
        return dst;
    }

    Vector<double> getPrior() {
        return this->prior;
    }
};

#endif

max_stars_repo_name: dimitrijejankov/pdb

max_issues_repo_name: dimitrijejankov/pdb

max_forks_repo_name: dimitrijejankov/pdb