text:
#include <math.h>
#include <float.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>

#include <cblas.h>

#include "funcsfa.h"

#define min(x, y) ((x)>(y)?(y):(x))
#define max(x, y) ((x)>(y)?(x):(y))

#define DEBUG 0
#define debug_print(fmt, ...) \
        do { if (DEBUG) fprintf(stderr, "%s:%4d:%25s(): " fmt, __FILE__, \
                                __LINE__, __func__, __VA_ARGS__); } while (0)

double eps_ev = 1e-20;

// **************************************
// Lapack
// **************************************

// Lapack Fortran DGESV interface
void
dgesv_(int *n, int *nrhs, double *a, int *lda, int *ipiv, double *b, int *ldb,
       int *info);
// DGESV wrapper, for solving linear equations.
static int
dgesv(int n, int nrhs, double *a, int lda, int *ipiv, double *b, int ldb)
{
  int info = -1;
  dgesv_(&n, &nrhs, a, &lda, ipiv, b, &ldb, &info);
  return info;
}

// Lapack Fortran DGESVD interface
void
dgesvd_(char *jobu, char *jobvt, int *m, int *n, double *a, int *lda,
        double *s, double *u, int *ldu, double *vt, int *ldvt, double *work,
        int *lwork, int *info);
// DGESVD wrapper, for Singular Value Decomposition.
static int
dgesvd(char jobu, char jobvt, int m, int n, double *a, int lda, double *s,
       double *u, int ldu, double *vt, int ldvt)
{
  int info = -1;
  int lwork = -1;
  double optimal_lwork;
  double *work;
  // Query optimal work size
  dgesvd_(&jobu, &jobvt, &m, &n, a, &lda,
      s, u, &ldu, vt, &ldvt, &optimal_lwork,
      &lwork, &info);
  if (info != 0) {
    return info;
  }
  lwork = (int) optimal_lwork;

  work = malloc(sizeof(double)*lwork);

  dgesvd_(&jobu, &jobvt, &m, &n, a, &lda,
      s, u, &ldu, vt, &ldvt, work,
      &lwork, &info);

  free(work);
  return info;
}

// **************************************
// Objects
// **************************************

funcsfa_Factorization*
funcsfa_Factorization_alloc(size_t n_features, size_t n_factors,
                            size_t n_samples, size_t n_datatypes,
                            size_t *n_features_split, double *data)
{
  size_t i;
  funcsfa_Factorization *m;
  size_t sum_n_features_split = 0;

  debug_print("%s\n", "Allocating SFA object");

  m = malloc(sizeof(funcsfa_Factorization));
  m->n_features = n_features;
  m->n_factors = n_factors;
  m->n_samples = n_samples;
  m->n_datatypes = n_datatypes;
  m->n_features_split = malloc(sizeof(size_t)*n_datatypes);
  for(i=0; i<n_datatypes; i++)
  {
    m->n_features_split[i] = n_features_split[i];
    sum_n_features_split += n_features_split[i];
  }
  if (sum_n_features_split != n_features)
  {
    debug_print("Total number of features (%zd) does not match sum (%zd)\n",
        n_features, sum_n_features_split);
    free(m->n_features_split);
    free(m);
    return NULL;
  }
  m->data = data;
  m->coefficients = malloc(sizeof(double)*n_features*n_factors);
  m->factors = malloc(sizeof(double)*n_factors*n_samples);
  m->factor_cov = malloc(sizeof(double)*n_factors*n_factors);
  m->residual_var = malloc(sizeof(double)*n_features);

  return(m);
}

void
funcsfa_Factorization_free(funcsfa_Factorization *m)
{
  debug_print("%s\n", "Freeing SFA object");

  if (m != NULL)
  {
    free(m->n_features_split);
    free(m->coefficients);
    free(m->factors);
    free(m->factor_cov);
    free(m->residual_var);
  }
  free(m);
}

funcsfa_Monitor*
funcsfa_Monitor_alloc(int max_iter)
{
  funcsfa_Monitor *mon;
  int i;

  debug_print("%s\n", "Allocating Monitor object");

  mon = malloc(sizeof(funcsfa_Monitor));
  if (mon == NULL) {
    return NULL;
  }
  mon->n_iter = 0;
  mon->reconstruction_error = malloc(sizeof(double)*(max_iter+1));
  mon->max_diff_factors = malloc(sizeof(double)*(max_iter+1));
  mon->max_diff_coefficients = malloc(sizeof(double)*(max_iter+1));
  if ((mon->reconstruction_error == NULL) ||
      (mon->max_diff_factors == NULL) ||
      (mon->max_diff_coefficients == NULL))
  {
    funcsfa_Monitor_free(mon);
    return NULL;
  }
  for (i = 0; i < max_iter+1; i++)
  {
    mon->reconstruction_error[i] = NAN;
    mon->max_diff_factors[i] = NAN;
    mon->max_diff_coefficients[i] = NAN;
  }

  return mon;
}

void
funcsfa_Monitor_free(funcsfa_Monitor *mon)
{
  debug_print("%s\n", "Freeing Monitor object");

  if (mon != NULL)
  {
    free(mon->reconstruction_error);
    free(mon->max_diff_factors);
    free(mon->max_diff_coefficients);
  }
  free(mon);
}

/** Bunch of memory for temporary matrices.
 * @private
 */
typedef struct
{
  /** Integer vector n_factors long */
  int *factors_int;
  /** Double vector n_features long */
  double *features_double;
  /** Double matrix n_features x n_factors 1 */
  double *features_factors_double_a;
  /** Double matrix n_features x n_factors 2 */
  double *features_factors_double_b;
  /** Double matrix n_factors x n_factors 1 */
  double *factors_factors_double_a;
  /** Double matrix n_factors x n_factors 2 */
  double *factors_factors_double_b;

  /** Precomputed value of diag(data * t(data)) */
  double *diag_data_square;
  /** Precomputed value of sum(diag(data * t(data))) per data type */
  double *dt_var;
  /** Factors of previous iteration */
  double *prev_factors;
  /** Coefficients of previous iteration */
  double *prev_coefficients;
} Workspace;

static Workspace*
ws_alloc(funcsfa_Factorization *m)
{
  Workspace *ws;

  debug_print("%s\n", "Allocating Workspace");

  ws = malloc(sizeof(Workspace));
  ws->factors_int = malloc(sizeof(int)*m->n_factors);
  ws->features_double = malloc(sizeof(double)*m->n_features);

  ws->features_factors_double_a = malloc(sizeof(double)*m->n_features*m->n_factors);
  ws->features_factors_double_b = malloc(sizeof(double)*m->n_features*m->n_factors);
  ws->factors_factors_double_a = malloc(sizeof(double)*m->n_factors*m->n_factors);
  ws->factors_factors_double_b = malloc(sizeof(double)*m->n_factors*m->n_factors);

  ws->diag_data_square = malloc(sizeof(double)*m->n_features);
  ws->dt_var = malloc(sizeof(double)*m->n_factors);
  ws->prev_factors = malloc(sizeof(double)*m->n_factors*m->n_samples);
  ws->prev_coefficients = malloc(sizeof(double)*m->n_features*m->n_factors);

  return ws;
}

static void
ws_init(Workspace *ws, funcsfa_Factorization *m)
{
  double *d_d;
  size_t dt_i, feature_start, n_features, i;

  debug_print("%s\n", "Initializing Workspace");

  d_d = malloc(sizeof(double)*m->n_features*m->n_features);

  cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans,
      m->n_features, m->n_features, m->n_samples, 1,
      m->data, m->n_samples,
      m->data, m->n_samples,
      0, d_d, m->n_features);

  for (i = 0; i < m->n_features; i++)
  {
    ws->diag_data_square[i] = d_d[i*m->n_features + i];
  }
  feature_start = 0;
  for(dt_i = 0; dt_i < m->n_datatypes; dt_i++)
  {
    ws->dt_var[dt_i] = 0;
    n_features = m->n_features_split[dt_i];
    for(i = feature_start; i < (feature_start + n_features); i++)
    {
      ws->dt_var[dt_i] += ws->diag_data_square[i];
    }
    ws->dt_var[dt_i] /= n_features;
    feature_start = feature_start + n_features;
  }

  free(d_d);
}

static void
ws_free(Workspace *ws)
{
  debug_print("%s\n", "Freeing Workspace");

  free(ws->factors_int);
  free(ws->features_double);
  free(ws->features_factors_double_a);
  free(ws->features_factors_double_b);
  free(ws->factors_factors_double_a);
  free(ws->factors_factors_double_b);

  free(ws->diag_data_square);
  free(ws->dt_var);
  free(ws->prev_factors);
  free(ws->prev_coefficients);

  free(ws);
}

// **************************************
// Utility Functions
// **************************************

static double
max_diff(double *a, double *b, size_t length)
{
  size_t i;
  double md, d;

  md = 0;
  for (i = 0; i < length; i++)
  {
    d = fabs(a[i] - b[i]);
    if (d > md)
    {
      md = d;
    }
  }
  return md;
}

static bool
finite_array(double *a, size_t length)
{
  for (size_t i=0; i<length; i++)
  {
    if (!isfinite(a[i]))
    {
      return false;
    }
  }
  return true;
}

static bool
positive_array(double *a, size_t length)
{
  for (size_t i=0; i<length; i++)
  {
    if (!(a[i] > 0.0))
    {
      return false;
    }
  }
  return true;
}

#define check_finite(a, length) \
    if (DEBUG && !finite_array(a, length)) {\
      debug_print("%s\n", "Error, non-finite values in matrix");}

#define check_positive(a, length) \
    if (DEBUG && !positive_array(a, length)) {\
      debug_print("%s\n", "Error, non-positive values in matrix");}

// **************************************
// Algorithm
// **************************************

static int
expectation_factors(funcsfa_Factorization *m, Workspace *ws)
{
  size_t i, j;
  double *r_c, *c_r_c, *c_r_c_i, *o, *c_o;
  double factor_mean, factor_var, factor_sd, deviance;
  int *ipiv;
  int info;

  debug_print("%s\n", "Computing Expectation and Covariance of Factors");
  check_finite(m->coefficients, m->n_features*m->n_factors);
  check_finite(m->residual_var, m->n_features);
  check_positive(m->residual_var, m->n_features);

  /* Compute $\Psi^{-1} B$
   * r_c = diag(1/residual_var) * coefficients */
  r_c = ws->features_factors_double_a;
  for (i=0; i < m->n_features; i++)
  {
    for (j=0; j < m->n_factors; j++)
    {
      r_c[j * m->n_features + i] =
          m->coefficients[j * m->n_features + i] / m->residual_var[i];
    }
  }

  /* Compute $B^T \Psi^{-1} B + I$
   * c_r_c = t(coefficients) * r_c + I */
  c_r_c = ws->factors_factors_double_a;
  cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans,
      m->n_factors, m->n_factors, m->n_features, 1,
      m->coefficients, m->n_features,
      r_c, m->n_features,
      0, c_r_c, m->n_factors);
  for (i = 0; i < m->n_factors; i++)
  {
    c_r_c[i*m->n_factors + i] += 1;
  }

  /* Compute $O = \Psi^{-1}B(B^T \Psi^{-1} B + I)^{-1}$
   * o = r_c*solve(c_r_c) */
  c_r_c_i = ws->factors_factors_double_b;
  ipiv = ws->factors_int;
  for (i = 0; i < m->n_factors; i++)
  {
    for (j = 0; j < m->n_factors; j++)
    {
      c_r_c_i[j * m->n_factors + i] = (i == j);
    }
  }

  info = dgesv(m->n_factors, m->n_factors,
      c_r_c, m->n_factors, ipiv,
      c_r_c_i, m->n_factors);
  if (info != 0) {
    debug_print("Lapack error: %d\n", info);
    return -1;
  }
  c_r_c = NULL;
  o = ws->features_factors_double_b;
  cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
     m->n_features, m->n_factors, m->n_factors, 1,
     r_c, m->n_features,
     c_r_c_i, m->n_factors,
     0, o, m->n_features);

  /* Compute $E[Z|X] = (\Psi^{-1}B(B^T\Psi^{-1}B+I)^{-1})^TX$
   * factors = t(o) * t(X)
   */
  cblas_dgemm(CblasColMajor, CblasTrans, CblasTrans,
      m->n_factors, m->n_samples, m->n_features, 1,
      o, m->n_features,
      m->data, m->n_samples,
      0, m->factors, m->n_factors);

  /* Compute $B^T\Psi^{-1}B(B^T\Psi^{-1}B+I)^{-1}$
   * c_o = t(coefficients) * o
   */
  c_o = ws->factors_factors_double_a;
  cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans,
      m->n_factors, m->n_factors, m->n_features, 1,
      m->coefficients, m->n_features,
      o, m->n_features,
      0, c_o, m->n_factors);

  /*
   * Compute $E[ZZ^T|X] = I - (B^T\Psi^{-1}B(B^T\Psi^{-1}B+I)^{-1})^T + E[Z|X]E[Z|X]^T$
   * factor_cov = factors * t(factors) + I - c_o
   */
  cblas_dgemm(CblasColMajor, CblasNoTrans, CblasTrans,
      m->n_factors, m->n_factors, m->n_samples, 1,
      m->factors, m->n_factors,
      m->factors, m->n_factors,
      0, m->factor_cov, m->n_factors);

  for (i = 0; i < m->n_factors; i++)
  {
    for (j = 0; j < m->n_factors; j++)
    {
      m->factor_cov[j * m->n_factors + i] += (i==j) - c_o[j * m->n_factors + i];
    }
  }

  debug_print("Factor value: %g\n", m->factors[0]);

  // Rescale factors
  for (i = 0; i < m->n_factors; i++)
  {
    factor_mean = 0;
    for (j = 0; j < m->n_samples; j++)
    {
      factor_mean += m->factors[i + j*m->n_factors];
    }
    factor_mean /= m->n_samples;
    debug_print("  Factor mean %zd: %g\n", i, factor_mean);
    factor_var = 0;
    for (j = 0; j < m->n_samples; j++)
    {
      deviance = m->factors[i + j*m->n_factors] - factor_mean;
      factor_var += deviance*deviance;
    }
    factor_var /= m->n_samples;
    debug_print("  Factor variation %zd: %g\n", i, factor_var);
    for (j = 0; j < m->n_factors; j++)
    {
      m->factor_cov[i + j*m->n_factors] /= factor_var;
    }
    factor_sd = sqrt(factor_var);
    for (j = 0; j < m->n_samples; j++)
    {
      m->factors[i + j * m->n_factors] /= factor_sd;
    }
  }

  return 0;
}

static int
maximize(funcsfa_Factorization *m, double *lambdas, Workspace *ws)
{
  const size_t n_lambda = 2;
  size_t dt_i, f_i, lambda_i, k;
  double *ZXt, *BZZt;
  double unpenalized_coeff, l1, l2, dt_var;
  size_t n_features, feature_start;

  debug_print("%s\n", "Estimating Coefficients");

  check_finite(m->factor_cov, m->n_factors*m->n_factors);
  check_finite(m->factors, m->n_factors*m->n_samples);

  ZXt = ws->features_factors_double_a;
  BZZt = ws->features_double;
  // Compute E[Z|X]X^T
  cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
      m->n_factors, m->n_features, m->n_samples, 1,
      m->factors, m->n_factors,
      m->data, m->n_samples,
      0, ZXt, m->n_factors);

  // Coordinate Descent over factors
  for(k = 0; k < m->n_factors; k++)
  {
    debug_print("\tCoordinate descent for factor: %zd\n", k);
    // Compute $BE[ZZ^T|X]_(\cdot k)$
    cblas_dgemv(CblasColMajor, CblasNoTrans,
      m->n_features, m->n_factors, 1.0,
      m->coefficients, m->n_features,
      m->factor_cov+(k*m->n_factors), 1,
      0.0, BZZt, 1);

    lambda_i = 0;
    feature_start = 0;
    for(dt_i = 0; dt_i < m->n_datatypes; dt_i++)
    {
      n_features = m->n_features_split[dt_i];
      l1 = lambdas[lambda_i];
      l2 = lambdas[lambda_i+1];
      for(f_i = feature_start; f_i < (feature_start + n_features); f_i++)
      {
        // Compute: $$
        //  \hat B_{(ij)} \leftarrow \frac
        //    {\operatorname{S}(
        //      \hat B_{(ij)} + E[Z|X]_{(j \cdot)} X_{(i \cdot)}^T -
        //      \hat B_{(i\cdot)} \operatorname{E}[ZZ^T|X]_{(\cdot j)},
        //      l_1)}
        //    {1+l_2}
        // $$
        unpenalized_coeff = m->coefficients[f_i + (k * m->n_features)] +
                            ((ZXt[k + (f_i * m->n_factors)] - BZZt[f_i])
                             / m->n_samples);
        m->coefficients[f_i + (k * m->n_features)] =
          copysign(fmax(0.0, fabs(unpenalized_coeff) - l1),
                   unpenalized_coeff) /
          (1 + l2);
      }
      feature_start = feature_start + n_features;
      lambda_i = lambda_i + n_lambda;
    }
  }

  debug_print("%s\n", "Estimating Residual Variance");

  // Residual variance per data type
  feature_start = 0;
  for(dt_i = 0; dt_i < m->n_datatypes; dt_i++)
  {
    dt_var = 0;
    n_features = m->n_features_split[dt_i];
    for(f_i = feature_start; f_i < (feature_start + n_features); f_i++)
    {
      for(k = 0; k < m->n_factors; k++)
      {
        dt_var += (ZXt[k + (f_i * m->n_factors)] *
                   m->coefficients[f_i + (k * m->n_features)]);
      }
    }
    dt_var = (ws->dt_var[dt_i] - (dt_var / n_features)) / m->n_samples;
    for(f_i = feature_start; f_i < (feature_start + n_features); f_i++)
    {
      if (dt_var > eps_ev) {
        m->residual_var[f_i] = dt_var;
      } else {
        m->residual_var[f_i] = eps_ev;
      }
    }
    debug_print("\tResidual Variance %zd: %g\n", dt_i, dt_var);
    feature_start = feature_start + n_features;
  }
  return 0;
}

static int
init_svd(funcsfa_Factorization *m, Workspace *ws)
{
  int info;
  size_t i, j;
  double *x, *s, *vt, *u;
  double scale, f, explained_var, data_var;
  size_t total_n_factors;

  debug_print("%s\n", "Initializing with SVD");

  total_n_factors = min(m->n_features, m->n_samples);

  x = malloc(sizeof(double) * m->n_features * m->n_samples);
  s = malloc(sizeof(double) * total_n_factors);
  vt = malloc(sizeof(double) * total_n_factors * m->n_features);
  u = malloc(sizeof(double) * total_n_factors * m->n_samples);
  if (x == NULL || s == NULL || vt == NULL)
  {
    free(x);
    free(vt);
    free(s);
    free(u);
    return -1;
  }

  for (i=0; i < m->n_features * m->n_samples; i++)
  {
    x[i] = m->data[i];
  }

  /* vt = svd(X).V.t */
  debug_print("%s\n", "\tStarting Lapack SVD");
  info = dgesvd('S', 'S',
      m->n_samples, m->n_features,
      x, m->n_samples,
      s, u, m->n_samples,
      vt, total_n_factors);
  if (info != 0) {
    debug_print("Lapack error: %d\n", info);
    free(x);
    free(vt);
    free(u);
    free(s);
    return -1;
  }
  debug_print("%s\n", "\tDone Lapack SVD");

  scale = 0;
  for (i=0; i < m->n_factors; i++)
  {
    for (j=0; j < m->n_samples; j++)
    {
      f = u[j + i*m->n_samples] * s[i];
      m->factors[i + j*m->n_factors] = f;
      scale += f*f;
    }
  }
  scale /= m->n_factors * m->n_samples;
  scale = sqrt(scale);
  for (i=0; i < m->n_factors; i++)
  {
    for (j=0; j < m->n_features; j++)
    {
      m->coefficients[j + i*m->n_features] = vt[i + j*total_n_factors] * scale;
    }
  }
  for (i=0; i < m->n_factors; i++)
  {
    for (j=0; j < m->n_samples; j++)
    {
      m->factors[i + j*m->n_factors] /= scale;
    }
  }

  explained_var = 0;
  for (i=0; i < m->n_factors; i++)
  {
    explained_var += s[i]*s[i];
  }
  explained_var /= m->n_samples * m->n_features;
  data_var = 0;
  for (i=0; i < m->n_features; i++)
  {
    data_var += ws->diag_data_square[i];
  }
  data_var /= m->n_samples * m->n_features;
  debug_print("\tData Var: %g\n", data_var);
  explained_var = data_var - explained_var;
  explained_var = fmax(0, explained_var) + eps_ev;
  for (i = 0; i < m->n_features; i++) {
    m->residual_var[i] = explained_var;
  }
  debug_print("\tResidual Variance: %g\n", explained_var);

  free(x);
  free(vt);
  free(u);
  free(s);
  return 0;
}

double
calc_reconstruction_error(funcsfa_Factorization *m, Workspace *ws)
{
  int i, j;
  double error, e;
  double *prediction;

  error = 0.0;
  for (i = 0; i < m->n_samples; i++)
  {
    prediction = ws->features_double;
    cblas_dgemv(CblasColMajor, CblasNoTrans,
        m->n_features, m->n_factors,
        1.0, m->coefficients, m->n_features,
        m->factors + (i*m->n_factors), 1,
        0.0, prediction, 1);
    for (j = 0; j < m->n_features; j++)
    {
      e = m->data[i + (j * m->n_samples)] - prediction[j];
      error += e*e;
    }
  }
  return error;
}

int
funcsfa(funcsfa_Factorization *m, double eps, int max_iter,
        char regularizations[], double *lambdas, funcsfa_Monitor *mon)
{
  int iter = 0;
  int err = 0;
  size_t i;
  double diff = DBL_MAX;
  double diff_f, diff_c;
  double rec_error;
  int const LEN_PREV_REC_ERROR = 10;
  double prev_rec_error[LEN_PREV_REC_ERROR];
  int prev_rec_error_i=0;

  Workspace *ws;

  for (i=0; i<m->n_datatypes; i++)
  {
    if (regularizations[i] != 'e')
    {
      debug_print("%s\n", "Only elastic net regularization is supported");
      return -4;
    }
  }

  ws = ws_alloc(m);
  ws_init(ws, m);

  debug_print("%s\n", "Starting Factorization");

  err = init_svd(m, ws);
  if (err != 0)
  {
    return(1);
  }

  rec_error = calc_reconstruction_error(m, ws);
  if (mon != NULL)
  {
    mon->reconstruction_error[0] = rec_error;
    mon->max_diff_factors[0] = 0;
    mon->max_diff_coefficients[0] = 0;
  }
  for (i=0; i<LEN_PREV_REC_ERROR; i++) {
    prev_rec_error[i] = 0.0;
  }

  /* EM Loop */
  while (iter < max_iter && diff > eps) {
    debug_print("Iteration %d\n", iter);
    memcpy(ws->prev_factors, m->factors,
        sizeof(double)*m->n_factors*m->n_samples);
    memcpy(ws->prev_coefficients, m->coefficients,
        sizeof(double)*m->n_features*m->n_factors);
    err = expectation_factors(m, ws);
    if (err != 0)
    {
      debug_print("Error in computing expectation of factors: %d\n", err);
      ws_free(ws);
      return(2);
    }
    err = maximize(m, lambdas, ws);
    if (err != 0)
    {
      debug_print("Error in maximization: %d\n", err);
      ws_free(ws);
      return(3);
    }

    diff_f = max_diff(m->factors,
        ws->prev_factors, m->n_factors*m->n_samples);
    diff_c = max_diff(m->coefficients, ws->prev_coefficients,
        m->n_features*m->n_factors);
    debug_print("Diff (factor/coefficients): %g; %g\n", diff_f, diff_c);
    prev_rec_error[prev_rec_error_i] = rec_error;
    prev_rec_error_i += 1;
    if (prev_rec_error_i > LEN_PREV_REC_ERROR) {
      prev_rec_error_i = 0;
    }
    rec_error = calc_reconstruction_error(m, ws);
    debug_print("Reconstruction Error: %g\n", rec_error);
    diff = 0.0;
    for (i=0; i<LEN_PREV_REC_ERROR; i++) {
      diff += fabs(prev_rec_error[i] - rec_error);
    }
    diff /= LEN_PREV_REC_ERROR;
    debug_print("Diff (Reconstruction Error): %g\n", diff);

    if (mon != NULL)
    {
      mon->reconstruction_error[iter+1] = rec_error;
      mon->max_diff_factors[iter+1] = diff_f;
      mon->max_diff_coefficients[iter+1] = diff_c;
      mon->n_iter = iter;
    }
    iter = iter + 1;
  }
  ws_free(ws);

  return 0;
}

max_stars_repo_name: NKI-CCB/funcsfa

max_issues_repo_name: NKI-CCB/funcsfa

max_forks_repo_name: NKI-CCB/funcsfa


text:
// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2013.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// --------------------------------------------------------------------------
// $Maintainer: Stephan Aiche$
// $Authors: Stephan Aiche, Marc Sturm $
// --------------------------------------------------------------------------

#ifndef OPENMS_TRANSFORMATIONS_FEATUREFINDER_TRACEFITTER_H
#define OPENMS_TRANSFORMATIONS_FEATUREFINDER_TRACEFITTER_H

#include <OpenMS/TRANSFORMATIONS/FEATUREFINDER/FeatureFinderAlgorithmPickedHelperStructs.h>

#include <OpenMS/DATASTRUCTURES/DefaultParamHandler.h>
#include <OpenMS/DATASTRUCTURES/ListUtils.h>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_blas.h>

namespace OpenMS
{

  /**
   * @brief Abstract fitter for RT profile fitting
   *
   * This class provides the basic interface and some functionality to fit multiple mass traces to
   * a given RT shape model using the Levenberg-Marquardt algorithm.
   *
   * @todo docu needs update
   *
   */
  template <class PeakType>
  class TraceFitter :
    public DefaultParamHandler
  {

public:
    /// default constructor.
    TraceFitter() :
      DefaultParamHandler("TraceFitter")
    {
      defaults_.setValue("max_iteration", 500, "Maximum number of iterations used by the Levenberg-Marquardt algorithm.", ListUtils::create<String>("advanced"));
      defaults_.setValue("epsilon_abs", 0.0001, "Absolute error used by the Levenberg-Marquardt algorithm.", ListUtils::create<String>("advanced"));
      defaults_.setValue("epsilon_rel", 0.0001, "Relative error used by the Levenberg-Marquardt algorithm.", ListUtils::create<String>("advanced"));
      defaults_.setValue("weighted", "false", "Weight mass traces according to their theoretical intensities.", ListUtils::create<String>("advanced"));
      defaults_.setValidStrings("weighted", ListUtils::create<String>("true,false"));
      defaultsToParam_();
    }

    /// copy constructor
    TraceFitter(const TraceFitter& source) :
      DefaultParamHandler(source),
      epsilon_abs_(source.epsilon_abs_),
      epsilon_rel_(source.epsilon_rel_),
      max_iterations_(source.max_iterations_),
      weighted_(source.weighted_)
    {
      updateMembers_();
    }

    /// assignment operator
    virtual TraceFitter& operator=(const TraceFitter& source)
    {
      DefaultParamHandler::operator=(source);
      max_iterations_ = source.max_iterations_;
      epsilon_abs_  = source.epsilon_abs_;
      epsilon_rel_ = source.epsilon_rel_;
      weighted_ = source.weighted_;
      updateMembers_();

      return *this;
    }

    /// destructor
    virtual ~TraceFitter()
    {
    }

    /**
     * Main method of the TraceFitter which triggers the actual fitting.
     */
    virtual void fit(FeatureFinderAlgorithmPickedHelperStructs::MassTraces<PeakType>& traces) = 0;

    /**
     * Returns the lower bound of the fitted RT model
     */
    virtual DoubleReal getLowerRTBound() const = 0;

    /**
     * Returns the upper bound of the fitted RT model
     */
    virtual DoubleReal getUpperRTBound() const = 0;

    /**
     * Returns the height of the fitted model
     */
    virtual DoubleReal getHeight() const = 0;

    /**
     * Returns the center position of the fitted model
     */
    virtual DoubleReal getCenter() const = 0;

    /**
     * Returns the mass trace width at half max (FWHM)
     */
    virtual DoubleReal getFWHM() const = 0;

    /**
     * Evaluate the fitted model at a time point
     */
    virtual DoubleReal getValue(DoubleReal rt) const = 0;

    /**
     * Returns the theoretical value of the fitted model at position k in the passed mass trace
     *
     * @param trace the mass trace for which the value should be computed
     * @param k  use the position of the k-th peak to compute the value
     */
    DoubleReal computeTheoretical(const FeatureFinderAlgorithmPickedHelperStructs::MassTrace<PeakType>& trace, Size k)
    {
      double rt = trace.peaks[k].first;

      return trace.theoretical_int * getValue(rt);
    }

    /**
     * Checks if the fitted model fills out at least 'min_rt_span' of the RT span
     *
     * @param rt_bounds RT boundaries of the fitted model
     * @param min_rt_span Minimum RT span in relation to extended area that has to remain after model fitting
     */
    virtual bool checkMinimalRTSpan(const std::pair<DoubleReal, DoubleReal>& rt_bounds, const DoubleReal min_rt_span) = 0;

    /**
     * Checks if the fitted model is not to big
     *
     * @param max_rt_span Maximum RT span in relation to extended area that the model is allowed to have
     */
    virtual bool checkMaximalRTSpan(const DoubleReal max_rt_span) = 0;

    /**
     * Returns the peak area of the fitted model
     */
    virtual DoubleReal getArea() = 0;

    /**
     * Returns a textual representation of the fitted model function, that can be plotted using Gnuplot
     *
     * @param trace The mass trace that should be plotted
     * @param function_name The name of the function (e.g. f(x) -> function_name = f)
     * @param baseline The intensity of the baseline
     * @param rt_shift A shift value, that allows to plot all RT profiles side by side, even if they would overlap in reality.
     *                 This should be 0 for the first mass trace and increase by a fixed value for each mass trace.
     */
    virtual String getGnuplotFormula(const FeatureFinderAlgorithmPickedHelperStructs::MassTrace<PeakType>& trace, const char function_name, const DoubleReal baseline, const DoubleReal rt_shift) = 0;

protected:

    /// Structure for passing data to GSL functions
    struct ModelData
    {
      FeatureFinderAlgorithmPickedHelperStructs::MassTraces<PeakType>* traces_ptr;
      bool weighted;
    };

    /**
     * Prints the state of the current iteration (e.g., values of the parameters)
     *
     * @param iter Number of current iteration.
     * @param s The solver that also contains all the parameters.
     */
    virtual void printState_(SignedSize iter, gsl_multifit_fdfsolver* s) = 0;

    virtual void updateMembers_()
    {
      max_iterations_ = this->param_.getValue("max_iteration");
      epsilon_abs_ = this->param_.getValue("epsilon_abs");
      epsilon_rel_ = this->param_.getValue("epsilon_rel");
      weighted_ = this->param_.getValue("weighted") == "true";
    }

    /**
     * Updates all member variables to the fitted values stored in the solver.
     *
     * @param s The solver containing the fitted parameter values.
     */
    virtual void getOptimizedParameters_(gsl_multifit_fdfsolver* s) = 0;

    /**
     * Optimize the given parameters using the Levenberg-Marquardt algorithm.
     */
    void optimize_(FeatureFinderAlgorithmPickedHelperStructs::MassTraces<PeakType> & traces, const Size num_params, double x_init[],
                   Int (* residual)(const gsl_vector* x, void* params, gsl_vector* f),
                   Int (* jacobian)(const gsl_vector* x, void* params, gsl_matrix* J),
                   Int (* evaluate)(const gsl_vector* x, void* params, gsl_vector* f, gsl_matrix * J))
    {
      const gsl_multifit_fdfsolver_type* T;
      gsl_multifit_fdfsolver* s;

      const size_t data_count = traces.getPeakCount();

      // gsl always expects N>=p or default gsl error handler invoked,
      // cause Jacobian be rectangular M x N with M>=N
      if (data_count < num_params) throw Exception::UnableToFit(__FILE__, __LINE__, __PRETTY_FUNCTION__, "UnableToFit-FinalSet", "Skipping feature, gsl always expects N>=p");

      gsl_multifit_function_fdf func;
      gsl_vector_view x = gsl_vector_view_array(x_init, num_params);
      gsl_rng_env_setup();
      func.f = (residual);
      func.df = (jacobian);
      func.fdf = (evaluate);
      func.n = data_count;
      func.p = num_params;
      ModelData params = {&traces, weighted_};
      func.params = &params;
      T = gsl_multifit_fdfsolver_lmsder;
      s = gsl_multifit_fdfsolver_alloc(T, data_count, num_params);
      gsl_multifit_fdfsolver_set(s, &func, &x.vector);
      SignedSize iter = 0;
      Int gsl_status_;
      do
      {
        iter++;
        gsl_status_ = gsl_multifit_fdfsolver_iterate(s);
        printState_(iter, s);
        if (gsl_status_) break;
        gsl_status_ = gsl_multifit_test_delta(s->dx, s->x, epsilon_abs_, epsilon_rel_);
      }
      while (gsl_status_ == GSL_CONTINUE && iter < max_iterations_);

      // get the parameters out of the fdfsolver
      getOptimizedParameters_(s);

      gsl_multifit_fdfsolver_free(s);
    }

    /** Test for the convergence of the sequence by comparing the last iteration step dx with the absolute error epsabs and relative error epsrel to the current position x */
    /// Absolute error
    DoubleReal epsilon_abs_;
    /// Relative error
    DoubleReal epsilon_rel_;
    /// Maximum number of iterations
    SignedSize max_iterations_;
    /// Whether to weight mass traces by theoretical intensity during the optimization
    bool weighted_;

  };

}

#endif // #ifndef OPENMS_TRANSFORMATIONS_FEATUREFINDER_FEATUREFINDERALGORITHMPICKED_RTFITTING_H

ext: h

max_stars_repo_name: kreinert/OpenMS

max_issues_repo_name: kreinert/OpenMS

max_forks_repo_name: kreinert/OpenMS


text:
// Copyright (c) 2021. Pfizer Inc. All rights reserved.
#define PY_SSIZE_T_CLEAN
#include "Python.h"
#include "numpy/arrayobject.h"

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_movstat.h>
#include <gsl/gsl_vector.h>

PyObject * moving_median(PyObject *NPY_UNUSED(self), PyObject *args){
    PyObject *x_;
    long wlen;

    if (!PyArg_ParseTuple(args, "Ol:moving_median", &x_, &wlen)) return NULL;

    PyArrayObject *data = (PyArrayObject *)PyArray_FromAny(
        x_, PyArray_DescrFromType(NPY_DOUBLE), 1, 0,
        NPY_ARRAY_ENSUREARRAY | NPY_ARRAY_CARRAY_RO, NULL
    );
    if (!data) return NULL;

    // get the number of dimensions, and the shape
    int ndim = PyArray_NDIM(data);
    npy_intp *ddims = PyArray_DIMS(data);

    // data pointers
    double *dptr = (double *)PyArray_DATA(data);

    gsl_movstat_workspace *w = gsl_movstat_alloc2(0, (size_t)wlen - 1);

    gsl_vector x;
    x.size = ddims[ndim-1];
    x.stride = 1;
    // x.data = dptr;  // set this later
    x.block = NULL;
    x.owner = 0;

    // RETURN
    PyArrayObject *rmed = (PyArrayObject *)PyArray_EMPTY(ndim, ddims, NPY_DOUBLE, 0);
    if (!rmed){
        Py_XDECREF(data);
        Py_XDECREF(rmed);
        gsl_movstat_free(w);
        return NULL;
    }
    double *rptr = (double *)PyArray_DATA(rmed);

    gsl_vector xmean;
    xmean.size = ddims[ndim-1];
    xmean.stride = 1;
    // xmean.data = rptr;  // set this later
    xmean.block = NULL;
    xmean.owner = 0;

    // for iterating over the data
    long stride = ddims[ndim - 1];
    int nrepeats = PyArray_SIZE(data) / stride;

    for (int i = 0; i < nrepeats; ++i){
        x.data = dptr;
        xmean.data = rptr;

        gsl_movstat_median(GSL_MOVSTAT_END_PADZERO, &x, &xmean, w);

        dptr += stride;
        rptr += stride;
    }
    gsl_movstat_free(w);
    Py_XDECREF(data);

    return (PyObject *)rmed;
}

static struct PyMethodDef methods[] = {
    {"moving_median",   moving_median,   1, NULL},  // last is the docstring
    {NULL, NULL, 0, NULL}          /* sentinel */
};

static struct PyModuleDef moduledef = {
        PyModuleDef_HEAD_INIT,
        "moving_median",
        NULL,
        -1,
        methods,
        NULL,
        NULL,
        NULL,
        NULL
};

/* Initialization function for the module */
PyMODINIT_FUNC PyInit_moving_median(void)
{
    PyObject *m;
    m = PyModule_Create(&moduledef);
    if (m == NULL) {
        return NULL;
    }

    /* Import the array object */
    import_array();

    /* XXXX Add constants here */

    return m;
}

ext: c

max_stars_repo_name: PfizerRD/scikit-digital-health

max_issues_repo_name: PfizerRD/scikit-digital-health

max_forks_repo_name: PfizerRD/scikit-digital-health


text:
#ifndef LIC_TYPE_DEFINITION
#define LIC_TYPE_DEFINITION

#include <gsl/gsl>

#include "Metadata.h"

namespace lic
{

class MethodDefinition;

class TypeDefinition : Metadata
{
public:
    TypeDefinition(Assembly& assembly, MetadataTable table, size_t rid, gsl::byte* row);
    ~TypeDefinition();

    const char* Name() const;
    const char* Namespace() const;
    const gsl::span<MethodDefinition> Methods();

private:
    size_t FirstMethodRid();
};

}

#endif // !LIC_TYPE_DEFINITION

max_stars_repo_name: roberthusak/lic

max_issues_repo_name: roberthusak/lic

max_forks_repo_name: roberthusak/lic


text:
/**
 * \file ModellerFilter.h
 */

#ifndef ATK_MODELLING_MODELLERFILTER_H
#define ATK_MODELLING_MODELLERFILTER_H

#include <ATK/Modelling/config.h>
#include <ATK/Modelling/Types.h>

#include <ATK/Core/TypedBaseFilter.h>

#include <gsl/gsl>

#include <Eigen/Eigen>

#include <tuple>
#include <unordered_set>
#include <vector>

namespace ATK
{
    /// The common class for ModellerFilter
  template<typename DataType_>
  class ATK_MODELLING_EXPORT ModellerFilter: public TypedBaseFilter<DataType_>
  {
  public:
    using Parent = TypedBaseFilter<DataType_>;
    using DataType = DataType_;

  public:
    /**
     * The main ModellerFilter constructor
     * @param nb_dynamic_pins is the number of dymanic pins (that have a voltage that may vary with time)
     * @param nb_input_pins is the number of input pins (that will have varying voltage with time)
     */
    ModellerFilter(gsl::index nb_dynamic_pins, gsl::index nb_input_pins);

    virtual Eigen::Matrix<DataType, Eigen::Dynamic, 1> get_static_state() const = 0;

    /// Returns the number of dynamic pins
    virtual gsl::index get_nb_dynamic_pins() const = 0;

    /// Returns the number of static pins
    virtual gsl::index get_nb_static_pins() const = 0;

    /// Returns the number of input pins
    virtual gsl::index get_nb_input_pins() const = 0;

    /// Returns the number of components
    virtual gsl::index get_nb_components() const = 0;

    /// Returns the name of a dynamic pin, usefull to set output
    virtual std::string get_dynamic_pin_name(gsl::index identifier) const = 0;

    /// Returns the name of a input pin, usefull to set input
    virtual std::string
        get_input_pin_name(gsl::index identifier) const = 0;

    /// Returns the name of a static pin, usefull to set static
    virtual std::string get_static_pin_name(gsl::index identifier) const = 0;

    gsl::index find_dynamic_pin(const std::string& name);
    gsl::index find_input_pin(const std::string& name);
    gsl::index find_static_pin(const std::string& name);

    /// Get number of parameters
    virtual gsl::index get_number_parameters() const = 0;

    /// Get the name of a parameter
    virtual std::string get_parameter_name(gsl::index identifier) const = 0;

    /// Get the value of a parameter
    virtual DataType_ get_parameter(gsl::index identifier) const = 0;

    /// Set the value of a parameter
    virtual void set_parameter(gsl::index identifier, DataType_ value) = 0;
  };
}

#endif

max_stars_repo_name: AudioTK/ATK-Modelling-free

max_issues_repo_name: AudioTK/ATK-Modelling-free

max_forks_repo_name: AudioTK/ATK-Modelling-free


text:
#include <gsl/gsl_errno.h>
#include <math.h>
#include "acceleration.h"

int func(double t, const double y[], double f[], void *params) {
    /*
        y[0], y[1], y[2] are x, y, z
        y[3], y[4], y[5] are v_x, v_y, v_z
    */

    (void)(t); /* avoid unused parameter warning */
    struct HernquistParams pars = *(struct HernquistParams *)params;

    double r;
    double dPhi_dr;

    r = sqrt(y[0]*y[0] + y[1]*y[1] + y[2]*y[2]);
    dPhi_dr = pars.G * pars.m / pow(r + pars.c, 2);

    // Derivative of position is just velocity
    f[0] = y[3];
    f[1] = y[4];
    f[2] = y[5];

    // Derivative of velocity is acceleration from potential
    f[3] = -dPhi_dr * y[0] / r;
    f[4] = -dPhi_dr * y[1] / r;
    f[5] = -dPhi_dr * y[2] / r;

    return GSL_SUCCESS;
}

max_stars_repo_name: HaifengWangNAOC/Somethingnew

max_issues_repo_name: achapkowski/cython-tutorial

max_forks_repo_name: achapkowski/cython-tutorial