/* specfunc/mathieu_radfunc.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 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  L. Johnson */

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

int gsl_sf_mathieu_Mc_e(int kind, int order, double qq, double zz,
                      gsl_sf_result *result)
{
  int even_odd, kk, mm, status;
  double maxerr = 1e-14, amax, pi = M_PI, fn, factor;
  double coeff[GSL_SF_MATHIEU_COEFF], fc;
  double j1c, z2c, j1pc, z2pc;
  double u1, u2;
  gsl_sf_result aa;

  /* Check for out of bounds parameters. */
  if (qq <= 0.0)
  {
      GSL_ERROR("q must be greater than zero", GSL_EINVAL);
  }
  if (kind < 1 || kind > 2)
  {
      GSL_ERROR("kind must be 1 or 2", GSL_EINVAL);
  }

  mm = 0;
  amax = 0.0;
  fn = 0.0;
  u1 = sqrt(qq)*exp(-1.0*zz);
  u2 = sqrt(qq)*exp(zz);

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

  /* Compute the characteristic value. */
  status = gsl_sf_mathieu_a_e(order, qq, &aa);
  if (status != GSL_SUCCESS)
  {
      return status;
  }

  /* Compute the series coefficients. */
  status = gsl_sf_mathieu_a_coeff(order, qq, aa.val, coeff);
  if (status != GSL_SUCCESS)
  {
      return status;
  }

  if (even_odd == 0)
  {
      for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
      {
          amax = GSL_MAX(amax, fabs(coeff[kk]));
          if (fabs(coeff[kk])/amax < maxerr)
              break;

          j1c = gsl_sf_bessel_Jn(kk, u1);
          if (kind == 1)
          {
              z2c = gsl_sf_bessel_Jn(kk, u2);
          }
          else /* kind = 2 */
          {
              z2c = gsl_sf_bessel_Yn(kk, u2);
          }

          fc = pow(-1.0, 0.5*order+kk)*coeff[kk];
          fn += fc*j1c*z2c;
      }

      fn *= sqrt(pi/2.0)/coeff[0];
  }
  else
  {
      for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
      {
          amax = GSL_MAX(amax, fabs(coeff[kk]));
          if (fabs(coeff[kk])/amax < maxerr)
              break;

          j1c = gsl_sf_bessel_Jn(kk, u1);
          j1pc = gsl_sf_bessel_Jn(kk+1, u1);
          if (kind == 1)
          {
              z2c = gsl_sf_bessel_Jn(kk, u2);
              z2pc = gsl_sf_bessel_Jn(kk+1, u2);
          }
          else /* kind = 2 */
          {
              z2c = gsl_sf_bessel_Yn(kk, u2);
              z2pc = gsl_sf_bessel_Yn(kk+1, u2);
          }
          fc = pow(-1.0, 0.5*(order-1)+kk)*coeff[kk];
          fn += fc*(j1c*z2pc + j1pc*z2c);
      }

      fn *= sqrt(pi/2.0)/coeff[0];
  }

  result->val = fn;
  result->err = 2.0*GSL_DBL_EPSILON;
  factor = fabs(fn);
  if (factor > 1.0)
      result->err *= factor;

  return GSL_SUCCESS;
}

int gsl_sf_mathieu_Ms_e(int kind, int order, double qq, double zz,
                      gsl_sf_result *result)
{
  int even_odd, kk, mm, status;
  double maxerr = 1e-14, amax, pi = M_PI, fn, factor;
  double coeff[GSL_SF_MATHIEU_COEFF], fc;
  double j1c, z2c, j1mc, z2mc, j1pc, z2pc;
  double u1, u2;
  gsl_sf_result aa;

  /* Check for out of bounds parameters. */
  if (qq <= 0.0)
  {
      GSL_ERROR("q must be greater than zero", GSL_EINVAL);
  }
  if (kind < 1 || kind > 2)
  {
      GSL_ERROR("kind must be 1 or 2", GSL_EINVAL);
  }

  /* Handle the trivial cases where order = 0. */
  if (order == 0)
  {
      result->val = 0.0;
      result->err = 0.0;
      return GSL_SUCCESS;
  }

  mm = 0;
  amax = 0.0;
  fn = 0.0;
  u1 = sqrt(qq)*exp(-1.0*zz);
  u2 = sqrt(qq)*exp(zz);

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

  /* Compute the characteristic value. */
  status = gsl_sf_mathieu_b_e(order, qq, &aa);
  if (status != GSL_SUCCESS)
  {
      return status;
  }

  /* Compute the series coefficients. */
  status = gsl_sf_mathieu_b_coeff(order, qq, aa.val, coeff);
  if (status != GSL_SUCCESS)
  {
      return status;
  }

  if (even_odd == 0)
  {
      for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
      {
          amax = GSL_MAX(amax, fabs(coeff[kk]));
          if (fabs(coeff[kk])/amax < maxerr)
              break;

          j1mc = gsl_sf_bessel_Jn(kk, u1);
          j1pc = gsl_sf_bessel_Jn(kk+2, u1);
          if (kind == 1)
          {
              z2mc = gsl_sf_bessel_Jn(kk, u2);
              z2pc = gsl_sf_bessel_Jn(kk+2, u2);
          }
          else /* kind = 2 */
          {
              z2mc = gsl_sf_bessel_Yn(kk, u2);
              z2pc = gsl_sf_bessel_Yn(kk+2, u2);
          }

          fc = pow(-1.0, 0.5*order+kk+1)*coeff[kk];
          fn += fc*(j1mc*z2pc - j1pc*z2mc);
      }

      fn *= sqrt(pi/2.0)/coeff[0];
  }
  else
  {
      for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
      {
          amax = GSL_MAX(amax, fabs(coeff[kk]));
          if (fabs(coeff[kk])/amax < maxerr)
              break;

          j1c = gsl_sf_bessel_Jn(kk, u1);
          j1pc = gsl_sf_bessel_Jn(kk+1, u1);
          if (kind == 1)
          {
              z2c = gsl_sf_bessel_Jn(kk, u2);
              z2pc = gsl_sf_bessel_Jn(kk+1, u2);
          }
          else /* kind = 2 */
          {
              z2c = gsl_sf_bessel_Yn(kk, u2);
              z2pc = gsl_sf_bessel_Yn(kk+1, u2);
          }

          fc = pow(-1.0, 0.5*(order-1)+kk)*coeff[kk];
          fn += fc*(j1c*z2pc - j1pc*z2c);
      }

      fn *= sqrt(pi/2.0)/coeff[0];
  }

  result->val = fn;
  result->err = 2.0*GSL_DBL_EPSILON;
  factor = fabs(fn);
  if (factor > 1.0)
      result->err *= factor;

  return GSL_SUCCESS;
}

int gsl_sf_mathieu_Mc_array(int kind, int nmin, int nmax, double qq,
                            double zz, gsl_sf_mathieu_workspace *work,
                            double result_array[])
{
  int even_odd, order, ii, kk, mm, status;
  double maxerr = 1e-14, amax, pi = M_PI, fn;
  double coeff[GSL_SF_MATHIEU_COEFF], fc;
  double j1c, z2c, j1pc, z2pc;
  double u1, u2;
  double *aa = work->aa;

  /* Initialize the result array to zeroes. */
  for (ii=0; ii<nmax-nmin+1; ii++)
      result_array[ii] = 0.0;

  /* Check for out of bounds parameters. */
  if (qq <= 0.0)
  {
      GSL_ERROR("q must be greater than zero", GSL_EINVAL);
  }
  if (kind < 1 || kind > 2)
  {
      GSL_ERROR("kind must be 1 or 2", GSL_EINVAL);
  }

  mm = 0;
  amax = 0.0;
  u1 = sqrt(qq)*exp(-1.0*zz);
  u2 = sqrt(qq)*exp(zz);

  /* Compute all eigenvalues up to nmax. */
  gsl_sf_mathieu_a_array(0, nmax, qq, work, aa);

  for (ii=0, order=nmin; order<=nmax; ii++, order++)
  {
      fn = 0.0;
      even_odd = 0;
      if (order % 2 != 0)
          even_odd = 1;

      /* Compute the series coefficients. */
      status = gsl_sf_mathieu_a_coeff(order, qq, aa[order], coeff);
      if (status != GSL_SUCCESS)
      {
          return status;
      }

      if (even_odd == 0)
      {
          for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
          {
              amax = GSL_MAX(amax, fabs(coeff[kk]));
              if (fabs(coeff[kk])/amax < maxerr)
                  break;

              j1c = gsl_sf_bessel_Jn(kk, u1);
              if (kind == 1)
              {
                  z2c = gsl_sf_bessel_Jn(kk, u2);
              }
              else /* kind = 2 */
              {
                  z2c = gsl_sf_bessel_Yn(kk, u2);
              }

              fc = pow(-1.0, 0.5*order+kk)*coeff[kk];
              fn += fc*j1c*z2c;
          }

          fn *= sqrt(pi/2.0)/coeff[0];
      }
      else
      {
          for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
          {
              amax = GSL_MAX(amax, fabs(coeff[kk]));
              if (fabs(coeff[kk])/amax < maxerr)
                  break;

              j1c = gsl_sf_bessel_Jn(kk, u1);
              j1pc = gsl_sf_bessel_Jn(kk+1, u1);
              if (kind == 1)
              {
                  z2c = gsl_sf_bessel_Jn(kk, u2);
                  z2pc = gsl_sf_bessel_Jn(kk+1, u2);
              }
              else /* kind = 2 */
              {
                  z2c = gsl_sf_bessel_Yn(kk, u2);
                  z2pc = gsl_sf_bessel_Yn(kk+1, u2);
              }
              fc = pow(-1.0, 0.5*(order-1)+kk)*coeff[kk];
              fn += fc*(j1c*z2pc + j1pc*z2c);
          }

          fn *= sqrt(pi/2.0)/coeff[0];
      }

      result_array[ii] = fn;
  } /* order loop */

  return GSL_SUCCESS;
}

int gsl_sf_mathieu_Ms_array(int kind, int nmin, int nmax, double qq,
                            double zz, gsl_sf_mathieu_workspace *work,
                            double result_array[])
{
  int even_odd, order, ii, kk, mm, status;
  double maxerr = 1e-14, amax, pi = M_PI, fn;
  double coeff[GSL_SF_MATHIEU_COEFF], fc;
  double j1c, z2c, j1mc, z2mc, j1pc, z2pc;
  double u1, u2;
  double *bb = work->bb;

  /* Initialize the result array to zeroes. */
  for (ii=0; ii<nmax-nmin+1; ii++)
      result_array[ii] = 0.0;

  /* Check for out of bounds parameters. */
  if (qq <= 0.0)
  {
      GSL_ERROR("q must be greater than zero", GSL_EINVAL);
  }
  if (kind < 1 || kind > 2)
  {
      GSL_ERROR("kind must be 1 or 2", GSL_EINVAL);
  }

  mm = 0;
  amax = 0.0;
  u1 = sqrt(qq)*exp(-1.0*zz);
  u2 = sqrt(qq)*exp(zz);

  /* Compute all eigenvalues up to nmax. */
  gsl_sf_mathieu_b_array(0, nmax, qq, work, bb);

  for (ii=0, order=nmin; order<=nmax; ii++, order++)
  {
      fn = 0.0;
      even_odd = 0;
      if (order % 2 != 0)
          even_odd = 1;

      /* Handle the trivial cases where order = 0. */
      if (order == 0)
      {
          result_array[ii] = 0.0;
          continue;
      }

      /* Compute the series coefficients. */
      status = gsl_sf_mathieu_b_coeff(order, qq, bb[order], coeff);
      if (status != GSL_SUCCESS)
      {
          return status;
      }

      if (even_odd == 0)
      {
          for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
          {
              amax = GSL_MAX(amax, fabs(coeff[kk]));
              if (fabs(coeff[kk])/amax < maxerr)
                  break;

              j1mc = gsl_sf_bessel_Jn(kk, u1);
              j1pc = gsl_sf_bessel_Jn(kk+2, u1);
              if (kind == 1)
              {
                  z2mc = gsl_sf_bessel_Jn(kk, u2);
                  z2pc = gsl_sf_bessel_Jn(kk+2, u2);
              }
              else /* kind = 2 */
              {
                  z2mc = gsl_sf_bessel_Yn(kk, u2);
                  z2pc = gsl_sf_bessel_Yn(kk+2, u2);
              }

              fc = pow(-1.0, 0.5*order+kk+1)*coeff[kk];
              fn += fc*(j1mc*z2pc - j1pc*z2mc);
          }

          fn *= sqrt(pi/2.0)/coeff[0];
      }
      else
      {
          for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
          {
              amax = GSL_MAX(amax, fabs(coeff[kk]));
              if (fabs(coeff[kk])/amax < maxerr)
                  break;

              j1c = gsl_sf_bessel_Jn(kk, u1);
              j1pc = gsl_sf_bessel_Jn(kk+1, u1);
              if (kind == 1)
              {
                  z2c = gsl_sf_bessel_Jn(kk, u2);
                  z2pc = gsl_sf_bessel_Jn(kk+1, u2);
              }
              else /* kind = 2 */
              {
                  z2c = gsl_sf_bessel_Yn(kk, u2);
                  z2pc = gsl_sf_bessel_Yn(kk+1, u2);
              }

              fc = pow(-1.0, 0.5*(order-1)+kk)*coeff[kk];
              fn += fc*(j1c*z2pc - j1pc*z2c);
          }

          fn *= sqrt(pi/2.0)/coeff[0];
      }

      result_array[ii] = fn;
  } /* order loop */

  return GSL_SUCCESS;
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_mathieu_Mc(int kind, int order, double qq, double zz)
{
	EVAL_RESULT(gsl_sf_mathieu_Mc_e(kind, order, qq, zz, &result));
}

double gsl_sf_mathieu_Ms(int kind, int order, double qq, double zz)
{
	EVAL_RESULT(gsl_sf_mathieu_Ms_e(kind, order, qq, zz, &result));
}


/* Cholesky Decomposition
 *
 * Copyright (C) 2000  Thomas Walter
 *
 * 3 May 2000: Modified for GSL by Brian Gough
 *
 * This 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, or (at your option) any
 * later version.
 *
 * This source 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.
 */

/*
 * Cholesky decomposition of a symmetrix positive definite matrix.
 * This is useful to solve the matrix arising in
 *    periodic cubic splines
 *    approximating splines
 *
 * This algorthm does:
 *   A = L * L'
 * with
 *   L  := lower left triangle matrix
 *   L' := the transposed form of L.
 *
 */

#include <config.h>

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

int
gsl_linalg_cholesky_decomp (gsl_matrix * A)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (M != N)
    {
      GSL_ERROR("cholesky decomposition requires square matrix", GSL_ENOTSQR);
    }
  else
    {
      size_t i,j,k;
      int status = 0;

      /* Do the first 2 rows explicitly.  It is simple, and faster.  And
       * one can return if the matrix has only 1 or 2 rows.
       */

      double A_00 = gsl_matrix_get (A, 0, 0);

      double L_00 = sqrt(A_00);

      if (A_00 <= 0)
        {
          status = GSL_EDOM ;
        }

      gsl_matrix_set (A, 0, 0, L_00);

      if (M > 1)
        {
          double A_10 = gsl_matrix_get (A, 1, 0);
          double A_11 = gsl_matrix_get (A, 1, 1);

          double L_10 = A_10 / L_00;
          double diag = A_11 - L_10 * L_10;
          double L_11 = sqrt(diag);

          if (diag <= 0)
            {
              status = GSL_EDOM;
            }

          gsl_matrix_set (A, 1, 0, L_10);
          gsl_matrix_set (A, 1, 1, L_11);
        }

      for (k = 2; k < M; k++)
        {
          double A_kk = gsl_matrix_get (A, k, k);

          for (i = 0; i < k; i++)
            {
              double sum = 0;

              double A_ki = gsl_matrix_get (A, k, i);
              double A_ii = gsl_matrix_get (A, i, i);

              gsl_vector_view ci = gsl_matrix_row (A, i);
              gsl_vector_view ck = gsl_matrix_row (A, k);

              if (i > 0) {
                gsl_vector_view di = gsl_vector_subvector(&ci.vector, 0, i);
                gsl_vector_view dk = gsl_vector_subvector(&ck.vector, 0, i);

                gsl_blas_ddot (&di.vector, &dk.vector, &sum);
              }

              A_ki = (A_ki - sum) / A_ii;
              gsl_matrix_set (A, k, i, A_ki);
            }

          {
            gsl_vector_view ck = gsl_matrix_row (A, k);
            gsl_vector_view dk = gsl_vector_subvector (&ck.vector, 0, k);

            double sum = gsl_blas_dnrm2 (&dk.vector);
            double diag = A_kk - sum * sum;

            double L_kk = sqrt(diag);

            if (diag <= 0)
              {
                status = GSL_EDOM;
              }

            gsl_matrix_set (A, k, k, L_kk);
          }
        }

      /* Now copy the transposed lower triangle to the upper triangle,
       * the diagonal is common.
       */

      for (i = 1; i < M; i++)
        {
          for (j = 0; j < i; j++)
            {
              double A_ij = gsl_matrix_get (A, i, j);
              gsl_matrix_set (A, j, i, A_ij);
            }
        }

      if (status == GSL_EDOM)
        {
          GSL_ERROR ("matrix must be positive definite", GSL_EDOM);
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_cholesky_solve (const gsl_matrix * LLT,
                           const gsl_vector * b,
                           gsl_vector * x)
{
  if (LLT->size1 != LLT->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else if (LLT->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (LLT->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* Copy x <- b */

      gsl_vector_memcpy (x, b);

      /* Solve for c using forward-substitution, L c = b */

      gsl_blas_dtrsv (CblasLower, CblasNoTrans, CblasNonUnit, LLT, x);

      /* Perform back-substitution, U x = c */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, LLT, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_cholesky_svx (const gsl_matrix * LLT,
                         gsl_vector * x)
{
  if (LLT->size1 != LLT->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else if (LLT->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* Solve for c using forward-substitution, L c = b */

      gsl_blas_dtrsv (CblasLower, CblasNoTrans, CblasNonUnit, LLT, x);

      /* Perform back-substitution, U x = c */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, LLT, x);

      return GSL_SUCCESS;
    }
}


//This takes univariate X, windows X with W, applies the 1D FFT to each frame,
//takes power, transforms with T mat to B cfs, and then compresses power with log.
//Then the cepstral coeffs (CCs) are obtained by the 1D DCT-II of each frame,
//followed by a lifter (weighting in the cepstral domain).

//This is equivalent to :                                                                                spectrogram -> get_ccs.
//which is equivalent to:                                                                                spectrogram -> dct -> lifter.
//which is equivalent to:                                             stft -> apply_spectrogram_T_mat -> pow_compress -> dct -> lifter.
//which is equivalent to:                     window_univar -> fft_squared -> apply_spectrogram_T_mat -> pow_compress -> dct -> lifter.
//which is equivalent to:             window_univar -> fft_hc -> hc_square -> apply_spectrogram_T_mat -> pow_compress -> dct -> lifter.
//which is equivalent to: frame_univar -> apply_win -> fft_hc -> hc_square -> apply_spectrogram_T_mat -> pow_compress -> dct -> lifter.

#include <stdio.h>
#include <float.h>
#include <math.h>
#include <cblas.h>
#include <fftw3.h>

#ifndef M_PIf
   #define M_PIf 3.141592653589793238462643383279502884f
#endif

#ifndef M_PI
   #define M_PI 3.141592653589793238462643383279502884
#endif

#ifdef __cplusplus
namespace ov {
extern "C" {
#endif

int mfccs_s (float *Y, const int iscolmajor, const int R, const int C, const float *X, const int N, const float *W, const int L, const float *H, const int B, const int dim, const int c0, const float stp, const int mn0, const int nfft, const float preg, const int ndct, const float Q, const int K);
int mfccs_d (double *Y, const int iscolmajor, const int R, const int C, const double *X, const int N, const double *W, const int L, const double *H, const int B, const int dim, const int c0, const double stp, const int mn0, const int nfft, const double preg, const int ndct, const double Q, const int K);

//int mfccs_s (float *Y, sigi Yi, const float *X, const sigi Xi, const float *W, const sigi Wi, const float *H, const sigi H, const int dim, const int c0, const float stp, const float preg, const int ndct, const float Q)
int mfccs_s (float *Y, const int iscolmajor, const int R, const int C, const float *X, const int N, const float *W, const int L, const float *H, const int B, const int dim, const int c0, const float stp, const int mn0, const int nfft, const float preg, const int ndct, const float Q, const int K)
{
    const float z = 0.0f, o = 1.0f;
    const float Q2 = 0.5f*Q, PQ = M_PIf/Q;
    //const int normalize_H = 0;
    const int F = nfft/2 + 1;   //num non-negative FFT freqs
    const int F2 = F-1+nfft%2;  //for square loop below
    const int Lpre = L/2;       //nsamps before center samp
    int ss = c0 - Lpre;         //start samp of current frame
    int r, c, f, b, k;
    float *X1, *Y1, *X1d, *Y1d; //, *freqs, *cfs, *H;
    fftwf_plan fft_plan, dct_plan;
    //struct timespec tic, toc;

    //Checks
    if (N<1) { fprintf(stderr,"error in mfccs_s: N (length X) must be positive\n"); return 1; }
    if (R<1) { fprintf(stderr,"error in mfccs_s: R (nrows Y) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in mfccs_s: C (ncols Y) must be positive\n"); return 1; }
    if (L<1) { fprintf(stderr,"error in mfccs_s: L (winlength) must be positive\n"); return 1; }
    if (c0<0) { fprintf(stderr,"error in mfccs_s: c0 (center samp of 1st frame) must be nonnegative\n"); return 1; }
    if (c0>N-1) { fprintf(stderr,"error in mfccs_s: c0 (center samp of 1st frame) must be < N (length X)\n"); return 1; }
    if (L>=N) { fprintf(stderr,"error in mfccs_s: L (winlength) must be < N (length X)\n"); return 1; }
    if (stp<=0.0f) { fprintf(stderr,"error in mfccs_s: stp (step size) must be positive\n"); return 1; }
    if (nfft<L) { fprintf(stderr,"error in mfccs_s: nfft must be >= L (winlength)\n"); return 1; }
    if (preg!=preg || preg<0.0f) { fprintf(stderr,"error in mfccs_s: preg must be nonnegative\n"); return 1; }
    if (Q<0.0f) { fprintf(stderr,"error in mfccs_s: Q must be positive (or 0 to skip lifter)\n"); return 1; }
    if (K<1) { fprintf(stderr,"error in mfccs_s: K must be positive\n"); return 1; }
    if (K>ndct) { fprintf(stderr,"error in mfccs_s: K must be <= ndct>\n"); return 1; }

    //Initialize fftwf for FFT
    X1 = fftwf_alloc_real((size_t)nfft);
    Y1 = fftwf_alloc_real((size_t)nfft);
    fft_plan = fftwf_plan_r2r_1d(nfft,X1,Y1,FFTW_R2HC,FFTW_ESTIMATE);
    if (!fft_plan) { fprintf(stderr,"error in mfccs_s: problem creating fftw plan for fft\n"); return 1; }
    cblas_scopy(nfft,&z,0,&X1[0],1); //zero-pad

    //Initialize fftwf for DCT
    X1d = fftwf_alloc_real((size_t)ndct);
    Y1d = fftwf_alloc_real((size_t)ndct);
    dct_plan = fftwf_plan_r2r_1d(ndct,X1d,Y1d,FFTW_REDFT10,FFTW_ESTIMATE);
    if (!dct_plan) { fprintf(stderr,"error in mfccs_s: problem creating fftw plan for dct\n"); return 1; }
    cblas_scopy(ndct,&z,0,&X1d[0],1); //zero-pad

    //Get frequencies and H
    //if (get_stft_freqs_s(&freqs[0],F,fs,nfft)) { fprintf(stderr,"error in mfccs_s: problem getting STFT freqs\n"); return 1; }
    //if (get_cfs_T_s(&cfs[0],B,fs,'mel')) { fprintf(stderr,"error in mfccs_s: problem getting Mel cfs\n"); return 1; }
    //if (get_spectrogram_T_mat_s(&H[0],iscolmajor,&freqs[0],F,&cfs[0],B,normalize_H))
    //{ fprintf(stderr,"error in mfccs_s: problem getting STFT freq -> Mel cfs transform matrix\n"); return 1; }

    //clock_gettime(CLOCK_REALTIME,&tic);

    //Initialize lifter
    //float *lift; int n;
    //if (!(lift=(float *)malloc((size_t)K*sizeof(float)))) { fprintf(stderr,"error in get_ccs_s: problem with malloc for lifter. "); perror("malloc"); return 1; }
    //for (k=0; k<K; k++) { lift[k] = fmaf(Q2,sinf(k*PQ),1.0f); }

    if (dim==0)
    {
        if (iscolmajor)
        {
            for (c=0; c<C; c++)
            {
                //Window
                ss = (int)(roundf(c*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_scopy(-ss,&z,0,&X1[0],1);
                    cblas_ssbmv(CblasColMajor,CblasUpper,L+ss,0,1.0f,&W[-ss],1,&X[0],1,0.0f,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_ssbmv(CblasColMajor,CblasUpper,L,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_ssbmv(CblasColMajor,CblasUpper,N-ss,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                    cblas_scopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_scopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_saxpy(L,-cblas_sdot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftwf_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_sgemv(CblasColMajor,CblasNoTrans,B,F,1.0f,&H[0],B,&Y1[0],1,0.0f,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = logf(X1d[b]+preg); }

                //DCT
                fftwf_execute(dct_plan);
                cblas_scopy(K,&Y1d[0],1,&Y[c*K],1);
                //cblas_ssbmv(CblasColMajor,CblasUpper,K,0,1.0f,&lift[0],1,&Y1d[0],1,0.0f,&Y[c*K],1);
                //for (k=0; k<K; k++) { Y[k+c*K] = lift[k]*Y1d[k]; }
            }
            //Lifter
            if (Q>FLT_EPSILON) { for (k=0; k<K; k++) { cblas_sscal(C,fmaf(Q2,sinf(PQ*k),1.0f),&Y[k],K); } }
        }
        else
        {
            for (c=0; c<C; c++)
            {
                //Window
                ss = (int)(roundf(c*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_scopy(-ss,&z,0,&X1[0],1);
                    cblas_ssbmv(CblasRowMajor,CblasUpper,L+ss,0,1.0f,&W[-ss],1,&X[0],1,0.0f,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_ssbmv(CblasRowMajor,CblasUpper,L,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_ssbmv(CblasRowMajor,CblasUpper,N-ss,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                    cblas_scopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_scopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_saxpy(L,-cblas_sdot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftwf_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_sgemv(CblasRowMajor,CblasNoTrans,B,F,1.0f,&H[0],F,&Y1[0],1,0.0f,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = logf(X1d[b]+preg); }

                //DCT
                fftwf_execute(dct_plan);
                cblas_scopy(K,&Y1d[0],1,&Y[c],C);
            }
            //Lifter
            if (Q>FLT_EPSILON) { for (k=0; k<K; k++) { cblas_sscal(C,fmaf(Q2,sinf(PQ*k),1.0f),&Y[k*C],1); } }
        }
    }
    else if (dim==1)
    {
        if (iscolmajor)
        {
            for (r=0; r<R; r++)
            {
                //Window
                ss = (int)(roundf(r*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_scopy(-ss,&z,0,&X1[0],1);
                    cblas_ssbmv(CblasColMajor,CblasUpper,L+ss,0,1.0f,&W[-ss],1,&X[0],1,0.0f,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_ssbmv(CblasColMajor,CblasUpper,L,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_ssbmv(CblasColMajor,CblasUpper,N-ss,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                    cblas_scopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_scopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_saxpy(L,-cblas_sdot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftwf_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_sgemv(CblasColMajor,CblasNoTrans,B,F,1.0f,&H[0],B,&Y1[0],1,0.0f,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = logf(X1d[b]+preg); }

                //DCT
                fftwf_execute(dct_plan);
                cblas_scopy(K,&Y1d[0],1,&Y[r],R);
            }
            //Lifter
            if (Q>FLT_EPSILON) { for (k=0; k<K; k++) { cblas_sscal(R,fmaf(Q2,sinf(PQ*k),1.0f),&Y[k*R],1); } }
        }
        else
        {
            for (r=0; r<R; r++)
            {
                //Window
                ss = (int)(roundf(r*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_scopy(-ss,&z,0,&X1[0],1);
                    cblas_ssbmv(CblasRowMajor,CblasUpper,L+ss,0,1.0f,&W[-ss],1,&X[0],1,0.0f,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_ssbmv(CblasRowMajor,CblasUpper,L,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_ssbmv(CblasRowMajor,CblasUpper,N-ss,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                    cblas_scopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_scopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_saxpy(L,-cblas_sdot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftwf_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_sgemv(CblasRowMajor,CblasNoTrans,B,F,1.0f,&H[0],F,&Y1[0],1,0.0f,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = logf(X1d[b]+preg); }

                //DCT
                fftwf_execute(dct_plan);
                cblas_scopy(K,&Y1d[0],1,&Y[r*K],1);
            }
            //Lifter
            if (Q>FLT_EPSILON) { for (k=0; k<K; k++) { cblas_sscal(R,fmaf(Q2,sinf(PQ*k),1.0f),&Y[k],K); } }
        }
    }
    else
    {
        fprintf(stderr,"error in mfccs_s: dim must be 0 or 1.\n"); return 1;
    }
    //clock_gettime(CLOCK_REALTIME,&toc); fprintf(stderr,"elapsed time = %f ms\n",(toc.tv_sec-tic.tv_sec)*1e3+(toc.tv_nsec-tic.tv_nsec)/1e6);

    //Exit
    fftwf_destroy_plan(fft_plan); fftwf_free(X1); fftwf_free(Y1);
    fftwf_destroy_plan(dct_plan); fftwf_free(X1d); fftwf_free(Y1d);
    return 0;
}

int mfccs_d (double *Y, const int iscolmajor, const int R, const int C, const double *X, const int N, const double *W, const int L, const double *H, const int B, const int dim, const int c0, const double stp, const int mn0, const int nfft, const double preg, const int ndct, const double Q, const int K)
{
    const double z = 0.0, o = 1.0;
    const double Q2 = 0.5*Q, PQ = M_PI/Q;
    const int F = nfft/2 + 1;   //num non-negative FFT freqs
    const int F2 = F-1+nfft%2;  //for square loop below
    const int Lpre = L/2;       //nsamps before center samp
    int ss = c0 - Lpre;         //start samp of current frame
    int r, c, f, b, k;
    double *X1, *Y1, *X1d, *Y1d;
    fftw_plan fft_plan, dct_plan;

    //Checks
    if (N<1) { fprintf(stderr,"error in mfccs_d: N (length X) must be positive\n"); return 1; }
    if (R<1) { fprintf(stderr,"error in mfccs_d: R (nrows Y) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in mfccs_d: C (ncols Y) must be positive\n"); return 1; }
    if (L<1) { fprintf(stderr,"error in mfccs_d: L (winlength) must be positive\n"); return 1; }
    if (c0<0) { fprintf(stderr,"error in mfccs_d: c0 (center samp of 1st frame) must be nonnegative\n"); return 1; }
    if (c0>N-1) { fprintf(stderr,"error in mfccs_d: c0 (center samp of 1st frame) must be < N (length X)\n"); return 1; }
    if (L>=N) { fprintf(stderr,"error in mfccs_d: L (winlength) must be < N (length X)\n"); return 1; }
    if (stp<=0.0) { fprintf(stderr,"error in mfccs_d: stp (step size) must be positive\n"); return 1; }
    if (nfft<L) { fprintf(stderr,"error in mfccs_d: nfft must be >= L (winlength)\n"); return 1; }
    if (preg!=preg || preg<0.0) { fprintf(stderr,"error in mfccs_d: preg must be nonnegative\n"); return 1; }
    if (Q<0.0) { fprintf(stderr,"error in mfccs_d: Q must be positive (or 0 to skip lifter)\n"); return 1; }
    if (K<1) { fprintf(stderr,"error in mfccs_d: K must be positive\n"); return 1; }
    if (K>ndct) { fprintf(stderr,"error in mfccs_d: K must be <= ndct>\n"); return 1; }

    //Initialize fftw for FFT
    X1 = fftw_alloc_real((size_t)nfft);
    Y1 = fftw_alloc_real((size_t)nfft);
    fft_plan = fftw_plan_r2r_1d(nfft,X1,Y1,FFTW_R2HC,FFTW_ESTIMATE);
    if (!fft_plan) { fprintf(stderr,"error in mfccs_d: problem creating fftw plan for fft\n"); return 1; }
    cblas_dcopy(nfft,&z,0,&X1[0],1); //zero-pad

    //Initialize fftw for DCT
    X1d = fftw_alloc_real((size_t)ndct);
    Y1d = fftw_alloc_real((size_t)ndct);
    dct_plan = fftw_plan_r2r_1d(ndct,X1d,Y1d,FFTW_REDFT10,FFTW_ESTIMATE);
    if (!dct_plan) { fprintf(stderr,"error in mfccs_d: problem creating fftw plan for dct\n"); return 1; }
    cblas_dcopy(ndct,&z,0,&X1d[0],1); //zero-pad

    if (dim==0)
    {
        if (iscolmajor)
        {
            for (c=0; c<C; c++)
            {
                //Window
                ss = (int)(round(c*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_dcopy(-ss,&z,0,&X1[0],1);
                    cblas_dsbmv(CblasColMajor,CblasUpper,L+ss,0,1.0,&W[-ss],1,&X[0],1,0.0,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_dsbmv(CblasColMajor,CblasUpper,L,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_dsbmv(CblasColMajor,CblasUpper,N-ss,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                    cblas_dcopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_dcopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_daxpy(L,-cblas_ddot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftw_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_dgemv(CblasColMajor,CblasNoTrans,B,F,1.0,&H[0],B,&Y1[0],1,0.0,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = log(X1d[b]+preg); }

                //DCT
                fftw_execute(dct_plan);
                cblas_dcopy(K,&Y1d[0],1,&Y[c*K],1);
            }
            //Lifter
            if (Q>DBL_EPSILON) { for (k=0; k<K; k++) { cblas_dscal(C,fma(Q2,sin(PQ*k),1.0),&Y[k],K); } }
        }
        else
        {
            for (c=0; c<C; c++)
            {
                //Window
                ss = (int)(round(c*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_dcopy(-ss,&z,0,&X1[0],1);
                    cblas_dsbmv(CblasRowMajor,CblasUpper,L+ss,0,1.0,&W[-ss],1,&X[0],1,0.0,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_dsbmv(CblasRowMajor,CblasUpper,L,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_dsbmv(CblasRowMajor,CblasUpper,N-ss,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                    cblas_dcopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_dcopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_daxpy(L,-cblas_ddot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftw_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_dgemv(CblasRowMajor,CblasNoTrans,B,F,1.0,&H[0],F,&Y1[0],1,0.0,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = log(X1d[b]+preg); }

                //DCT
                fftw_execute(dct_plan);
                cblas_dcopy(K,&Y1d[0],1,&Y[c],C);
            }
            //Lifter
            if (Q>DBL_EPSILON) { for (k=0; k<K; k++) { cblas_dscal(C,fma(Q2,sin(PQ*k),1.0),&Y[k*C],1); } }
        }
    }
    else if (dim==1)
    {
        if (iscolmajor)
        {
            for (r=0; r<R; r++)
            {
                //Window
                ss = (int)(round(r*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_dcopy(-ss,&z,0,&X1[0],1);
                    cblas_dsbmv(CblasColMajor,CblasUpper,L+ss,0,1.0,&W[-ss],1,&X[0],1,0.0,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_dsbmv(CblasColMajor,CblasUpper,L,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_dsbmv(CblasColMajor,CblasUpper,N-ss,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                    cblas_dcopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_dcopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_daxpy(L,-cblas_ddot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftw_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_dgemv(CblasColMajor,CblasNoTrans,B,F,1.0,&H[0],B,&Y1[0],1,0.0,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = log(X1d[b]+preg); }

                //DCT
                fftw_execute(dct_plan);
                cblas_dcopy(K,&Y1d[0],1,&Y[r],R);
            }
            //Lifter
            if (Q>DBL_EPSILON) { for (k=0; k<K; k++) { cblas_dscal(R,fma(Q2,sin(PQ*k),1.0),&Y[k*R],1); } }
        }
        else
        {
            for (r=0; r<R; r++)
            {
                //Window
                ss = (int)(round(r*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_dcopy(-ss,&z,0,&X1[0],1);
                    cblas_dsbmv(CblasRowMajor,CblasUpper,L+ss,0,1.0,&W[-ss],1,&X[0],1,0.0,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_dsbmv(CblasRowMajor,CblasUpper,L,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_dsbmv(CblasRowMajor,CblasUpper,N-ss,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                    cblas_dcopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_dcopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_daxpy(L,-cblas_ddot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftw_execute(fft_plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_dgemv(CblasRowMajor,CblasNoTrans,B,F,1.0,&H[0],F,&Y1[0],1,0.0,&X1d[0],1);

                //Power compress
                for (b=0; b<B; b++) { X1d[b] = log(X1d[b]+preg); }

                //DCT
                fftw_execute(dct_plan);
                cblas_dcopy(K,&Y1d[0],1,&Y[r*K],1);
            }
            //Lifter
            if (Q>DBL_EPSILON) { for (k=0; k<K; k++) { cblas_dscal(R,fma(Q2,sin(PQ*k),1.0),&Y[k],K); } }
        }
    }
    else
    {
        fprintf(stderr,"error in mfccs_d: dim must be 0 or 1.\n"); return 1;
    }

    //Exit
    fftw_destroy_plan(fft_plan); fftw_free(X1); fftw_free(Y1);
    fftw_destroy_plan(dct_plan); fftw_free(X1d); fftw_free(Y1d);
    return 0;
}

#ifdef __cplusplus
}
}
#endif


/* specfunc/bessel_Inu.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.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_bessel.h>

#include "error.h"

#include "bessel.h"
#include "bessel_temme.h"

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int
gsl_sf_bessel_Inu_scaled_e(double nu, double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x < 0.0 || nu < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x*x < 10.0*(nu+1.0)) {
    gsl_sf_result b;
    double ex = exp(-x);
    int stat = gsl_sf_bessel_IJ_taylor_e(nu, x, 1, 100, GSL_DBL_EPSILON, &b);
    result->val  = b.val * ex;
    result->err  = b.err * ex;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat;
  }
  else if(0.5/(nu*nu + x*x) < GSL_ROOT3_DBL_EPSILON) {
    return gsl_sf_bessel_Inu_scaled_asymp_unif_e(nu, x, result);
  }
  else {
    int N = (int)(nu + 0.5);
    double mu = nu - N;      /* -1/2 <= mu <= 1/2 */
    double K_mu, K_mup1, Kp_mu;
    double K_nu, K_nup1, K_num1;
    double I_nu_ratio;
    int stat_Irat;
    int stat_Kmu;
    int n;

    /* obtain K_mu, K_mup1 */
    if(x < 2.0) {
      stat_Kmu = gsl_sf_bessel_K_scaled_temme(mu, x, &K_mu, &K_mup1, &Kp_mu);
    }
    else {
      stat_Kmu = gsl_sf_bessel_K_scaled_steed_temme_CF2(mu, x, &K_mu, &K_mup1, &Kp_mu);
    }

    /* recurse forward to obtain K_num1, K_nu */
    K_nu   = K_mu;
    K_nup1 = K_mup1;

    for(n=0; n<N; n++) {
      K_num1 = K_nu;
      K_nu   = K_nup1;
      K_nup1 = 2.0*(mu+n+1)/x * K_nu + K_num1;
    }

    /* calculate I_{nu+1}/I_nu */
    stat_Irat = gsl_sf_bessel_I_CF1_ser(nu, x, &I_nu_ratio);

    /* solve for I_nu */
    result->val = 1.0/(x * (K_nup1 + I_nu_ratio * K_nu));
    result->err = GSL_DBL_EPSILON * (0.5*N + 2.0) * fabs(result->val);

    return GSL_ERROR_SELECT_2(stat_Kmu, stat_Irat);
  }
}

int
gsl_sf_bessel_Inu_e(double nu, double x, gsl_sf_result * result)
{
  gsl_sf_result b;
  int stat_I = gsl_sf_bessel_Inu_scaled_e(nu, x, &b);
  int stat_e = gsl_sf_exp_mult_err_e(x, fabs(x*GSL_DBL_EPSILON),
                                        b.val, b.err,
                                        result);
  return GSL_ERROR_SELECT_2(stat_e, stat_I);
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_bessel_Inu_scaled(double nu, double x)
{
  EVAL_RESULT(gsl_sf_bessel_Inu_scaled_e(nu, x, &result));
}

double gsl_sf_bessel_Inu(double nu, double x)
{
  EVAL_RESULT(gsl_sf_bessel_Inu_e(nu, x, &result));
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "compearth.h"
#ifdef COMPEARTH_USE_MKL
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wreserved-id-macro"
#pragma clang diagnostic ignored "-Wstrict-prototypes"
#endif
#include <mkl_lapacke.h>
#ifdef __clang__
#pragma clang diagnostic pop
#endif
#else
#include <lapacke.h>
#endif

/*!
 * @brief Convert from moment tnesor to scalar seismic moment.
 *
 * @param[in] nm     Number of moment tensors.
 * @param[in] im0    =1 for Silver and Jordan (1982) formula. \n
 *                   =2 for GCMT formula. \n
 *                   =3 for old `Caltech Way.'
 * @param[in] M      [nm x 6] array of moment tensors packed
 *                   {M11, M22, M33, M12, M13, M23} with leading
 *                   dimension 6.  The Mij element should be of units
 *                   of Newton-meters (although it should not affect
 *                   the function here).
 *
 * @param[out] M0    Scalar moments with units of Newton-meters.  This
 *                   is an array of dimension [nm].
 *
 * @result 0 indicates success.
 *
 * @author Carl Tape and translated to C by Ben Baker
 *
 * @copyright MIT
 *
 */
int compearth_CMT2m0(const int nm, const int im0,
                     const double *__restrict__ M,
                     double *__restrict__ M0)
{
    double Mt9[9], lams[3], work[100], M11, M22, M33, M12, M13, M23;
    const double sqrt2i = 1.0/sqrt(2.0);
    const int lwork = 100;
    int i, ierr, info;
    ierr = 0;
    if (im0 == 2 || im0 == 3)
    {
        for (i=0; i<nm; i++)
        {
            // Fill upper-triangle; column 1
            Mt9[0] = M[6*i];
            Mt9[1] = 0.0;
            Mt9[2] = 0.0;
            // column 2
            Mt9[3] = M[6*i+3];
            Mt9[4] = M[6*i+1];
            Mt9[5] = 0.0;
            // column 3
            Mt9[6] = M[6*i+4];
            Mt9[7] = M[6*i+5];
            Mt9[8] = M[6*i+2];
            // get eigenvalues in ascending order
            info = LAPACKE_dsyev_work(LAPACK_COL_MAJOR, 'N', 'U', 3,
                                      Mt9, 3, lams, work, lwork);
            if (info != 0)
            {
                fprintf(stderr, "%s: Error computing eigenvalues\n", __func__);
                return -1;
            }
            // GCMT double couple formulation
            if (im0 == 2)
            {
                M0[i] = 0.5*(fabs(lams[0]) + fabs(lams[2]));
            }
            else
            {
                M0[i] = fabs(lams[2]);
                if (fabs(lams[0]) > fabs(lams[2])){M0[i] = fabs(lams[0]);}
            }
        }
    }
    // Silver and Jordan
    else if (im0 == 1)
    {
        for (i=0; i<nm; i++)
        {
            M11 = M[6*i];
            M22 = M[6*i+1];
            M33 = M[6*i+2];
            M12 = M[6*i+3];
            M13 = M[6*i+4];
            M23 = M[6*i+5];
            M0[i] = sqrt2i*sqrt((      M11*M11 + M22*M22 + M33*M33
                                + 2.0*(M12*M12 + M13*M13 + M23*M23)));
        }
    }
    else
    {
        fprintf(stderr, "%s: Invalid magnitude type %d\n", __func__, im0);
        for (i=0; i<nm; i++){M0[i] = 0.0;}
        return -1;
    }
    return ierr;
}


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */
/*
 *
 *
 *       Filename:  entropy.c
 *
 *    Description:  Calculate the entropy production for a given state, generator
 *    			and equilibrium state
 *
 *        Version:  1.0
 *        Created:  05/05/2014 20:53:01
 *       Revision:  none
 *        License:  BSD
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:  Università degli Studi di Trieste
 *
 *
 */

#include <gsl/gsl_math.h>
#include <gsl/gsl_sf_log.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>

#include "funcs.h"

/*
 *      FUNCTION
 *         Name:  entropy_production
 *  Description:
 *
 */
double entropy_production ( const gsl_vector* rho, const gsl_vector* rhoeq,
		const gsl_matrix* L  )
{
	/* l1, l2, l3 */
	double l[3] ; unsigned int i, j ;
	for ( i = 1 ; i < 3 ; i++ )
	{
		l[i] = 0 ;
		for ( j = 0 ; j < 3 ; j++ )
			l[i] += gsl_matrix_get(L,i,j)*VECTOR(rho,j) ;
	}

	/* L[rho] */
	double Lr = 0 ;
	for ( i = 1 ; i < 3 ; i++ )
		Lr += l[i]*VECTOR(rho,i) ;

	/* L[rhoeq] */
	double Leq = 0 ;
	for ( i = 1 ; i < 3 ; i++ )
		Leq += l[i]*VECTOR(rhoeq,i) ;

	/* r , req */
	double r, req ;
	r = req = 0 ;

	r = gsl_hypot3(VECTOR(rho,1),VECTOR(rho,2),VECTOR(rho,3)) ;
	req = gsl_hypot3(VECTOR(rhoeq,1),VECTOR(rhoeq,2),
		VECTOR(rhoeq,3)) ;

	/* internal entropy s */
	double s ;
	if ( r < 1 && req < 1 )
		s = -(gsl_sf_log((1+r)/(1-r))*Lr/r -gsl_sf_log((1+req)/(1-req))*Leq/req);
	else if ( r < 1 && req >= 0 )
		s = -gsl_sf_log((1+r)/(1-r))*Lr/r ;
	else
		s = 0 ;

	return s;
}		/* -----  end of function entropy_production  ----- */

/*
 *      FUNCTION
 *         Name:  entropy_of_state
 *  Description:  Calculate the Von Neumann entropy of state 'rho'
 *
 */
double entropy_of_state ( const gsl_vector* rho )
{
	double entr = 0 ;

	/* Finding the eigenvalues */
	gsl_eigen_herm_workspace* rho_ei = gsl_eigen_herm_alloc(2);
	gsl_matrix_complex* dens = gsl_matrix_complex_calloc (2,2);
	gsl_matrix_complex_set (dens, 0, 0, gsl_complex_rect(1+VECTOR(rho, 3),0));
	gsl_matrix_complex_set (dens,0,1,gsl_complex_rect(VECTOR(rho,1),-VECTOR(rho,2)));
	gsl_matrix_complex_set (dens,1,0,gsl_complex_rect(VECTOR(rho,1),VECTOR(rho,2)));
	gsl_matrix_complex_set (dens,1,1,gsl_complex_rect(1-VECTOR(rho,3),0));
	gsl_matrix_complex_scale (dens, gsl_complex_rect(0.5,0));
	gsl_vector* eigenvalues = gsl_vector_calloc(2) ;
	gsl_eigen_herm (dens, eigenvalues, rho_ei) ;

	/* Calculating entropy */
	double norm = gsl_hypot3( VECTOR(rho,1), VECTOR(rho,2),	VECTOR(rho,3) ) ;
	if ( gsl_fcmp(norm, 1, 1e-9) > 0 )
		entr = 0 ;
	else
		entr = - (VECTOR(eigenvalues,0)*gsl_sf_log(VECTOR(eigenvalues,0)) +
			VECTOR(eigenvalues,1)*gsl_sf_log(VECTOR(eigenvalues,1))) ;

	return (entr);
}		/* -----  end of function entropy_of_state  ----- */


//This does CELL (~soma) stage of minimal GRU (gated recurrent unit) model.
//This requires each neuron to have 2 input time series, X and Xf,
//where X is the usual input and Xf the input for the forget-gate.
//Both X and Xf are the output of a linear IN stage (weights and baises).

//For dim=0: F[:,t] = sig(Xf[:,t] + Uf*Y[:,t-1])
//           H[:,t] = F[:,t].*Y[:,t-1]
//           Y[:,t] = H[:,t] + (1-f[:,t]).*tanh(X[:,t] + U*H[:,t])
//
//For dim=1: F[t,:] = sig(Xf[t,:] + Y[t-1,:]*Uf)
//           H[t,:] = F[t,:].*Y[t-1,:]
//           Y[t,:] = H[t,:] + (1-F[t,:]).*tanh(X[t,:] + H[t,:]*U)
//
//where sig is the logistic (sigmoid) nonlinearity = 1/(1+exp(-x)),
//F is the forget gate signal, H is an intermediate vector,
//U and Uf are NxN update matrices, and Y is the output.

//Note that, the neurons of a layer are independent only if U and Uf are diagonal matrices.
//This is only really a CELL (~soma) stage in that case.

//Unlike v2, this version takes only one X input: X = [X; Xf] for dim=0
//                                                X = [X Xf]  for dim=1
//This is essentially to allow treatment as a layer with Linux piping conventions.

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

#ifdef __cplusplus
namespace codee {
extern "C" {
#endif

int gru_min_s (float *Y, const float *X, const float *U, const float *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim);
int gru_min_d (double *Y, const double *X, const double *U, const double *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim);

int gru_min_inplace_s (float *X, const float *U, const float *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim);
int gru_min_inplace_d (double *X, const double *U, const double *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim);

int gru_min_s (float *Y, const float *X, const float *U, const float *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const float o = 1.0f;
    const size_t NT = N*T;
    size_t nT, tN, tN2;

    float *F, *H;
    if (!(F=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in gru_min_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in gru_min_s: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0f / (1.0f+expf(-X[N+n]));
                Y[n] = (1.0f-F[n]) * tanhf(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_scopy((int)N,&X[tN2+N],1,F,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * Y[tN-N+n];
                }
                cblas_scopy((int)N,&X[tN2],1,&Y[tN],1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    Y[tN+n] = H[n] + (1.0f-F[n])*tanhf(Y[tN+n]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                Y[nT] = (1.0f-F[n]) * tanhf(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * Y[t-1+n*T];
                }
                cblas_scopy((int)N,&X[t],(int)T,&Y[t],(int)T);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    Y[t+nT] = H[n] + (1.0f-F[n])*tanhf(Y[t+nT]);
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                Y[nT] = (1.0f-F[n]) * tanhf(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * Y[t-1+n*T];
                }
                cblas_scopy((int)N,&X[t],(int)T,&Y[t],(int)T);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    Y[t+nT] = H[n] + (1.0f-F[n])*tanhf(Y[t+nT]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0f / (1.0f+expf(-X[N+n]));
                Y[n] = (1.0f-F[n]) * tanhf(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_scopy((int)N,&X[tN2+N],1,F,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * Y[tN-N+n];
                }
                cblas_scopy((int)N,&X[tN2],1,&Y[tN],1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    Y[tN+n] = H[n] + (1.0f-F[n])*tanhf(Y[tN+n]);
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in gru_min_s: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int gru_min_d (double *Y, const double *X, const double *U, const double *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const double o = 1.0;
    const size_t NT = N*T;
    size_t nT, tN, tN2;

    double *F, *H;
    if (!(F=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in gru_min_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in gru_min_d: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0 / (1.0+exp(-X[N+n]));
                Y[n] = (1.0-F[n]) * tanh(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_dcopy((int)N,&X[tN2+N],1,F,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * Y[tN-N+n];
                }
                cblas_dcopy((int)N,&X[tN2],1,&Y[tN],1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    Y[tN+n] = H[n] + (1.0-F[n])*tanh(Y[tN+n]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                Y[nT] = (1.0-F[n]) * tanh(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * Y[t-1+n*T];
                }
                cblas_dcopy((int)N,&X[t],(int)T,&Y[t],(int)T);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    Y[t+nT] = H[n] + (1.0-F[n])*tanh(Y[t+nT]);
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                Y[nT] = (1.0-F[n]) * tanh(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * Y[t-1+n*T];
                }
                cblas_dcopy((int)N,&X[t],(int)T,&Y[t],(int)T);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    Y[t+nT] = H[n] + (1.0-F[n])*tanh(Y[t+nT]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0 / (1.0+exp(-X[N+n]));
                Y[n] = (1.0-F[n]) * tanh(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_dcopy((int)N,&X[tN2+N],1,F,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * Y[tN-N+n];
                }
                cblas_dcopy((int)N,&X[tN2],1,&Y[tN],1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    Y[tN+n] = H[n] + (1.0-F[n])*tanh(Y[tN+n]);
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in gru_min_d: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int gru_min_inplace_s (float *X, const float *U, const float *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const float o = 1.0f;
    const size_t NT = N*T, N2 = 2*N;
    size_t nT, tN, tN2;

    float *F, *H;
    if (!(F=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in gru_inplace_min2_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in gru_inplace_min2_s: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            fprintf(stderr,"d0, colmajor \n");
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0f / (1.0f+expf(-X[N+n]));
                X[n] = (1.0f-F[n]) * tanhf(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_scopy((int)N,&X[tN2+N],1,F,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&X[tN2-N2],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n]*X[tN2-N2+n];
                }
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[tN2],1);
                for (size_t n=0u; n<N; ++n)
                {
                    X[tN2+n] = F[n]*X[tN2-N2+n] + (1.0f-F[n])*tanhf(X[tN2+n]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                X[nT] = (1.0f-F[n]) * tanhf(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&X[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n]*X[t-1+n*T];
                }
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    X[t+nT] = F[n]*X[t-1+nT] + (1.0f-F[n])*tanhf(X[t+nT]);
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                X[nT] = (1.0f-F[n]) * tanhf(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&X[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n]*X[t-1+n*T];
                }
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    X[t+nT] = F[n]*X[t-1+nT] + (1.0f-F[n])*tanhf(X[t+nT]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0f / (1.0f+expf(-X[N+n]));
                X[n] = (1.0f-F[n]) * tanhf(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_scopy((int)N,&X[tN2+N],1,F,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&X[tN2-N2],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n]*X[tN2-N2+n];
                }
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[tN2],1);
                for (size_t n=0u; n<N; ++n)
                {
                    X[tN2+n] = F[n]*X[tN2-N2+n] + (1.0f-F[n])*tanhf(X[tN2+n]);
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in gru_inplace_min2_s: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int gru_min_inplace_d (double *X, const double *U, const double *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const double o = 1.0;
    const size_t NT = N*T, N2 = 2*N;
    size_t nT, tN, tN2;

    double *F, *H;
    if (!(F=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in gru_inplace_min2_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in gru_inplace_min2_d: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0 / (1.0+exp(-X[N+n]));
                X[n] = (1.0-F[n]) * tanh(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_dcopy((int)N,&X[tN2+N],1,F,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&X[tN2-N2],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n]*X[tN2-N2+n];
                }
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[tN2],1);
                for (size_t n=0u; n<N; ++n)
                {
                    X[tN2+n] = F[n]*X[tN2-N2+n] + (1.0-F[n])*tanh(X[tN2+n]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                X[nT] = (1.0-F[n]) * tanh(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&X[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n]*X[t-1+n*T];
                }
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    X[t+nT] = F[n]*X[t-1+nT] + (1.0-F[n])*tanh(X[t+nT]);
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                X[nT] = (1.0-F[n]) * tanh(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,F,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&X[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n]*X[t-1+n*T];
                }
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    X[t+nT] = F[n]*X[t-1+nT] + (1.0-F[n])*tanh(X[t+nT]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0 / (1.0+exp(-X[N+n]));
                X[n] = (1.0-F[n]) * tanh(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN2 = 2*tN;
                cblas_dcopy((int)N,&X[tN2+N],1,F,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&X[tN2-N2],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n]*X[tN2-N2+n];
                }
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[tN2],1);
                for (size_t n=0u; n<N; ++n)
                {
                    X[tN2+n] = F[n]*X[tN2-N2+n] + (1.0-F[n])*tanh(X[tN2+n]);
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in gru_inplace_min2_d: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


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

#include <gsl/gsl_math.h>
#include <gsl/gsl_filter.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>

int
main(void)
{
  const size_t N = 1000;                       /* length of time series */
  const size_t K = 61;                         /* window size */
  const double alpha = 3.0;                    /* Gaussian kernel has +/- 3 standard deviations */
  gsl_vector *x = gsl_vector_alloc(N);         /* input vector */
  gsl_vector *y = gsl_vector_alloc(N);         /* filtered output vector */
  gsl_vector *dy = gsl_vector_alloc(N);        /* first derivative filtered vector */
  gsl_vector *d2y = gsl_vector_alloc(N);       /* second derivative filtered vector */
  gsl_rng *r = gsl_rng_alloc(gsl_rng_default);
  gsl_filter_gaussian_workspace *gauss_p = gsl_filter_gaussian_alloc(K);
  size_t i;

  /* generate input signal */
  for (i = 0; i < N; ++i)
    {
      double xi = (i > N / 2) ? 0.5 : 0.0;
      double ei = gsl_ran_gaussian(r, 0.1);

      gsl_vector_set(x, i, xi + ei);
    }

  /* apply filters */
  gsl_filter_gaussian(GSL_FILTER_END_PADVALUE, alpha, 0, x, y, gauss_p);
  gsl_filter_gaussian(GSL_FILTER_END_PADVALUE, alpha, 1, x, dy, gauss_p);
  gsl_filter_gaussian(GSL_FILTER_END_PADVALUE, alpha, 2, x, d2y, gauss_p);

  /* print results */
  for (i = 0; i < N; ++i)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(y, i);
      double dyi = gsl_vector_get(dy, i);
      double d2yi = gsl_vector_get(d2y, i);
      double dxi;

      /* compute finite difference of x vector */
      if (i == 0)
        dxi = gsl_vector_get(x, i + 1) - xi;
      else if (i == N - 1)
        dxi = gsl_vector_get(x, i) - gsl_vector_get(x, i - 1);
      else
        dxi = 0.5 * (gsl_vector_get(x, i + 1) - gsl_vector_get(x, i - 1));

      printf("%.12e %.12e %.12e %.12e %.12e\n",
             xi,
             yi,
             dxi,
             dyi,
             d2yi);
    }

  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_vector_free(dy);
  gsl_vector_free(d2y);
  gsl_rng_free(r);
  gsl_filter_gaussian_free(gauss_p);

  return 0;
}


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

/*
 * This module contains routines for updating the Levenberg-Marquardt
 * damping parameter on each iteration using Nielsen's method:
 *
 * [1] H. B. Nielsen, K. Madsen, Introduction to Optimization and
 *     Data Fitting, Informatics and Mathematical Modeling,
 *     Technical University of Denmark (DTU), 2010.
 *
 * 3 routines are needed to implement the update procedure:
 *
 * 1. init   - initialize parameter prior to iteration
 * 2. accept - update parameter after a step has been accepted
 * 3. reject - update parameter after a step has been rejected
 */

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

#define LM_ONE_THIRD         (0.333333333333333)

static int nielsen_init(const gsl_matrix * JTJ, const gsl_vector * diag,
                        double * mu, long * nu);
static int nielsen_accept(const double rho, double * mu, long * nu);
static int nielsen_reject(double * mu, long * nu);

static int
nielsen_init(const gsl_matrix * JTJ, const gsl_vector * diag,
             double * mu, long * nu)
{
  const double mu0 = 1.0e-3;
  size_t i;
  double max = -1.0;

  *nu = 2;

  if (JTJ == NULL)
    {
      /* not using cholesky solver */
      *mu = mu0;
      return GSL_SUCCESS;
    }

  /* set mu = mu0 * max(diag(J~^T J~)), with J~ = J D^{-1} */

  for (i = 0; i < JTJ->size1; ++i)
    {
      double JTJi = gsl_matrix_get(JTJ, i, i);
      double di = gsl_vector_get(diag, i);
      double val = JTJi / (di * di);
      max = GSL_MAX(max, val);
    }

  *mu = mu0 * max;

  return GSL_SUCCESS;
}

static int
nielsen_accept(const double rho, double * mu, long * nu)
{
  double b;

  /* reset nu */
  *nu = 2;

  b = 2.0 * rho - 1.0;
  b = 1.0 - b*b*b;
  *mu *= GSL_MAX(LM_ONE_THIRD, b);

  return GSL_SUCCESS;
}

static int
nielsen_reject(double * mu, long * nu)
{
  *mu *= (double) *nu;

  /* nu := 2*nu */
  *nu <<= 1;

  return GSL_SUCCESS;
}


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

#include <config.h>
#include <math.h>
#include <float.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>

#include "initialise.c"
#include "set_initial.c"
#include "reset.c"
#include "qpsrt.c"
#include "util.c"
#include "qpsrt2.c"
#include "qelg.c"
#include "positivity.c"

#include "qc25f.c"

int
gsl_integration_qawo (gsl_function * f,
                      const double a,
                      const double epsabs, const double epsrel,
                      const size_t limit,
                      gsl_integration_workspace * workspace,
                      gsl_integration_qawo_table * wf,
                      double *result, double *abserr)
{
  double area, errsum;
  double res_ext, err_ext;
  double result0, abserr0, resabs0, resasc0;
  double tolerance;

  double ertest = 0;
  double error_over_large_intervals = 0;
  double reseps = 0, abseps = 0, correc = 0;
  size_t ktmin = 0;
  int roundoff_type1 = 0, roundoff_type2 = 0, roundoff_type3 = 0;
  int error_type = 0, error_type2 = 0;

  size_t iteration = 0;

  int positive_integrand = 0;
  int extrapolate = 0;
  int extall = 0;
  int disallow_extrapolation = 0;

  struct extrapolation_table table;

  double b = a + wf->L ;
  double abs_omega = fabs (wf->omega) ;

  /* Initialize results */

  initialise (workspace, a, b);

  *result = 0;
  *abserr = 0;

  if (limit > workspace->limit)
    {
      GSL_ERROR ("iteration limit exceeds available workspace", GSL_EINVAL) ;
    }

  /* Test on accuracy */

  if (epsabs <= 0 && (epsrel < 50 * GSL_DBL_EPSILON || epsrel < 0.5e-28))
    {
      GSL_ERROR ("tolerance cannot be achieved with given epsabs and epsrel",
                 GSL_EBADTOL);
    }

  /* Perform the first integration */

  qc25f (f, a, b, wf, 0, &result0, &abserr0, &resabs0, &resasc0);

  set_initial_result (workspace, result0, abserr0);

  tolerance = GSL_MAX_DBL (epsabs, epsrel * fabs (result0));

  if (abserr0 <= 100 * GSL_DBL_EPSILON * resabs0 && abserr0 > tolerance)
    {
      *result = result0;
      *abserr = abserr0;

      GSL_ERROR ("cannot reach tolerance because of roundoff error "
                 "on first attempt", GSL_EROUND);
    }
  else if ((abserr0 <= tolerance && abserr0 != resasc0) || abserr0 == 0.0)
    {
      *result = result0;
      *abserr = abserr0;

      return GSL_SUCCESS;
    }
  else if (limit == 1)
    {
      *result = result0;
      *abserr = abserr0;

      GSL_ERROR ("a maximum of one iteration was insufficient", GSL_EMAXITER);
    }

  /* Initialization */

  initialise_table (&table);

  if (0.5 * abs_omega * fabs(b - a) <= 2)
    {
      append_table (&table, result0);
      extall = 1;
    }

  area = result0;
  errsum = abserr0;

  res_ext = result0;
  err_ext = GSL_DBL_MAX;

  positive_integrand = test_positivity (result0, resabs0);

  iteration = 1;

  do
    {
      size_t current_level;
      double a1, b1, a2, b2;
      double a_i, b_i, r_i, e_i;
      double area1 = 0, area2 = 0, area12 = 0;
      double error1 = 0, error2 = 0, error12 = 0;
      double resasc1, resasc2;
      double resabs1, resabs2;
      double last_e_i;

      /* Bisect the subinterval with the largest error estimate */

      retrieve (workspace, &a_i, &b_i, &r_i, &e_i);

      current_level = workspace->level[workspace->i] + 1;

      if (current_level >= wf->n)
        {
          error_type = -1 ; /* exceeded limit of table */
          break ;
        }

      a1 = a_i;
      b1 = 0.5 * (a_i + b_i);
      a2 = b1;
      b2 = b_i;

      iteration++;

      qc25f (f, a1, b1, wf, current_level, &area1, &error1, &resabs1, &resasc1);
      qc25f (f, a2, b2, wf, current_level, &area2, &error2, &resabs2, &resasc2);

      area12 = area1 + area2;
      error12 = error1 + error2;
      last_e_i = e_i;

      /* Improve previous approximations to the integral and test for
         accuracy.

         We write these expressions in the same way as the original
         QUADPACK code so that the rounding errors are the same, which
         makes testing easier. */

      errsum = errsum + error12 - e_i;
      area = area + area12 - r_i;

      tolerance = GSL_MAX_DBL (epsabs, epsrel * fabs (area));

      if (resasc1 != error1 && resasc2 != error2)
        {
          double delta = r_i - area12;

          if (fabs (delta) <= 1.0e-5 * fabs (area12) && error12 >= 0.99 * e_i)
            {
              if (!extrapolate)
                {
                  roundoff_type1++;
                }
              else
                {
                  roundoff_type2++;
                }
            }
          if (iteration > 10 && error12 > e_i)
            {
              roundoff_type3++;
            }
        }

      /* Test for roundoff and eventually set error flag */

      if (roundoff_type1 + roundoff_type2 >= 10 || roundoff_type3 >= 20)
        {
          error_type = 2;       /* round off error */
        }

      if (roundoff_type2 >= 5)
        {
          error_type2 = 1;
        }

      /* set error flag in the case of bad integrand behaviour at
         a point of the integration range */

      if (subinterval_too_small (a1, a2, b2))
        {
          error_type = 4;
        }

      /* append the newly-created intervals to the list */

      update (workspace, a1, b1, area1, error1, a2, b2, area2, error2);

      if (errsum <= tolerance)
        {
          goto compute_result;
        }

      if (error_type)
        {
          break;
        }

      if (iteration >= limit - 1)
        {
          error_type = 1;
          break;
        }

      /* set up variables on first iteration */

      if (iteration == 2 && extall)
        {
          error_over_large_intervals = errsum;
          ertest = tolerance;
          append_table (&table, area);
          continue;
        }

      if (disallow_extrapolation)
        {
          continue;
        }

      if (extall)
        {
          error_over_large_intervals += -last_e_i;

          if (current_level < workspace->maximum_level)
            {
              error_over_large_intervals += error12;
            }

          if (extrapolate)
            goto label70;
        }

      if (large_interval(workspace))
        {
          continue;
        }

      if (extall)
        {
          extrapolate = 1;
          workspace->nrmax = 1;
        }
      else
        {
          /* test whether the interval to be bisected next is the
             smallest interval. */
          size_t i = workspace->i;
          double width = workspace->blist[i] - workspace->alist[i];

          if (0.25 * fabs(width) * abs_omega > 2)
            continue;

          extall = 1;
          error_over_large_intervals = errsum;
          ertest = tolerance;
          continue;
        }

    label70:
      if (!error_type2 && error_over_large_intervals > ertest)
        {
          if (increase_nrmax (workspace))
            continue;
        }

      /* Perform extrapolation */

      append_table (&table, area);

      if (table.n < 3)
        {
          reset_nrmax(workspace);
          extrapolate = 0;
          error_over_large_intervals = errsum;
          continue;
        }

      qelg (&table, &reseps, &abseps);

      ktmin++;

      if (ktmin > 5 && err_ext < 0.001 * errsum)
        {
          error_type = 5;
        }

      if (abseps < err_ext)
        {
          ktmin = 0;
          err_ext = abseps;
          res_ext = reseps;
          correc = error_over_large_intervals;
          ertest = GSL_MAX_DBL (epsabs, epsrel * fabs (reseps));
          if (err_ext <= ertest)
            break;
        }

      /* Prepare bisection of the smallest interval. */

      if (table.n == 1)
        {
          disallow_extrapolation = 1;
        }

      if (error_type == 5)
        {
          break;
        }

      /* work on interval with largest error */

      reset_nrmax (workspace);
      extrapolate = 0;
      error_over_large_intervals = errsum;

    }
  while (iteration < limit);

  *result = res_ext;
  *abserr = err_ext;

  if (err_ext == GSL_DBL_MAX)
    goto compute_result;

  if (error_type || error_type2)
    {
      if (error_type2)
        {
          err_ext += correc;
        }

      if (error_type == 0)
        error_type = 3;

      if (result != 0 && area != 0)
        {
          if (err_ext / fabs (res_ext) > errsum / fabs (area))
            goto compute_result;
        }
      else if (err_ext > errsum)
        {
          goto compute_result;
        }
      else if (area == 0.0)
        {
          goto return_error;
        }
    }

  /*  Test on divergence. */

  {
    double max_area = GSL_MAX_DBL (fabs (res_ext), fabs (area));

    if (!positive_integrand && max_area < 0.01 * resabs0)
      goto return_error;
  }

  {
    double ratio = res_ext / area;

    if (ratio < 0.01 || ratio > 100 || errsum > fabs (area))
      error_type = 6;
  }

  goto return_error;

compute_result:

  *result = sum_results (workspace);
  *abserr = errsum;

return_error:

  if (error_type > 2)
    error_type--;

  if (error_type == 0)
    {
      return GSL_SUCCESS;
    }
  else if (error_type == 1)
    {
      GSL_ERROR ("number of iterations was insufficient", GSL_EMAXITER);
    }
  else if (error_type == 2)
    {
      GSL_ERROR ("cannot reach tolerance because of roundoff error",
                 GSL_EROUND);
    }
  else if (error_type == 3)
    {
      GSL_ERROR ("bad integrand behavior found in the integration interval",
                 GSL_ESING);
    }
  else if (error_type == 4)
    {
      GSL_ERROR ("roundoff error detected in extrapolation table", GSL_EROUND);
    }
  else if (error_type == 5)
    {
      GSL_ERROR ("integral is divergent, or slowly convergent", GSL_EDIVERGE);
    }
  else if (error_type == -1)
    {
      GSL_ERROR ("exceeded limit of trigonometric table", GSL_ETABLE);
    }
  else
    {
      GSL_ERROR ("could not integrate function", GSL_EFAILED);
    }

}


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <time.h>
#include <cblas.h>
#include <string.h>
#include <mpi.h>

#include "../inc/knnring.h"

knnresult kNN(double *X, double *Y, int n, int m, int d, int k){

	knnresult k_nearest;

	//initalize the result's variable
	k_nearest.nidx  = (int *)malloc(m*k*sizeof(int));
	k_nearest.ndist = (double *)malloc(m*k*sizeof(double));
	k_nearest.m = m;
	k_nearest.k = k;

	  //compute the distance Matrix D
	double *D = compute_D(X,Y,n,m,d,k);

	 //select k-nearest neighbors for every point in QUERY
	k_select(D,k_nearest.ndist, k_nearest.nidx,n,m,k);
	return k_nearest;
}

double *compute_D(double *X, double *Y, int n, int m, int d, int k){

	//compute distances using
	//D = (X*X)ee_t - 2XY_t + ee_t(Y*Y)_t
	double *tempD   = (double *)malloc(n*m*sizeof(double));
	double *e_1     = (double *)malloc(d*m*sizeof(double));
	double *XX      = (double *)malloc(n*d*sizeof(double));

	for(int i=0;i<d*m;i++)
		e_1[i]=1;

	for(int i=0;i<n*d;i++)
		XX[i] = X[i]*X[i];

	cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, n,m,d,1,XX,d,e_1,m,0,tempD,m);

	free(e_1);
	free(XX);

	double *e_2 = (double *)malloc(n*d*sizeof(double));
	double *YY = (double *)malloc(m*d*sizeof(double));

	for(int i=0;i<n*d;i++)
		e_2[i]=1;

	for(int i=0;i<m*d;i++)
		YY[i] = Y[i]*Y[i];

	cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, n,m,d,-2,X,d,Y,d,1,tempD,m);
	cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, n,m,d,1,e_2,d,YY,d,1,tempD,m);

	for(int i=0;i<n*m;i++)
		tempD[i] = sqrt(fabs(tempD[i]));

	free(e_2);
	free(YY);

	return tempD;
}

void k_select(double *D, double *ndist, int *nidx,int n, int m, int k){
	for(int j=0;j<m;j++){
		for(int i=0;i<k;i++){
			ndist[j*k+i] = D[j+i*m];
			nidx[j*k+i]  = i;
		}
		//sort the initial k neighbors
		quick_sort(ndist,nidx,j*k,k*(j+1)-1);
	}

	//search the rest points to find it there are closer neighbors
	for(int j=0;j<m;j++){
		for(int i=k;i<n;i++){
			if(D[j+i*m] < ndist[k*(j+1)-1]){
				ndist[k*(j+1)-1] = D[j+i*m];
				nidx[k*(j+1)-1] = i;
				quick_sort(ndist,nidx,j*k,(j+1)*k-1);
			}
		}
	}
}

void quick_sort(double *k_dist, int *k_neigh, int low, int high){
	if(low<high){

		//actuall sorting an element
		int pi = partition(k_dist,k_neigh,low,high);

		//recursivelly call quick_sort
		quick_sort(k_dist,k_neigh,low,pi-1);
		quick_sort(k_dist,k_neigh,pi+1,high);
	}
}

int partition(double *k_dist, int *k_neigh,int low, int high){

	double temp_dist;
	int temp_point;
	int checker = 0;

	double pivot = k_dist[high];
	//printf("the pivot is %lf \n",pivot);
	int i = (low-1);
	for(int j=low;j<=high-1;j++){
		if(k_dist[j]<=pivot){
			/*swap the two elements*/

			i=i+1;
			temp_dist  = k_dist[i];
			temp_point = k_neigh[i];

			k_dist[i]  = k_dist[j];
			k_neigh[i] = k_neigh[j];

			k_dist[j]  = temp_dist;
			k_neigh[j] = temp_point;
			checker = 1;
		}
	}

	/* swap the pivot */
	i=i+1;
	temp_dist  = k_dist[i];
	temp_point = k_neigh[i];

	k_dist[i]  = k_dist[high];
	k_neigh[i] = k_neigh[high];

	k_dist[high]  = temp_dist;
	k_neigh[high] = temp_point;

	return i;
}

void create_points(double *X,int size){
	double *temp;
	temp = X;
	srand(time(NULL));
	for(int i=0;i<size;i++)
		temp[i] = (double) 1000*rand()/RAND_MAX;
}

void write_to_file(char *str1, int *nidx, double *ndist, int amount, int k){
  FILE *fp;
  //file for indexes
  fp = fopen(str1,"a+");

  int counter = 1;
  for(int i=0;i<amount;i++){
    fprintf(fp,"%d)%d -- %lf\n",counter,nidx[i],ndist[i]);
    counter ++;
	if((i+1)%k == 0){
      fprintf(fp,"k nearest neighbors for next point \n");
      counter = 1;
    }
  }
	fclose(fp);
}

//######################### V1 FUNCTIONS ############################

knnresult distrAllkNN(double *X, int n, int d, int k){

	//results variables
	 knnresult knn_process;
	knnresult knn_temp;

	int p_rank;                            //PROCESS RANK
	int p_size;                            //TOTAL NUMBER OF PROCESSES
	int next,prev;                         //NEXT AND PREVIOUS PROCESSES
	double *receive_buff;                  //BUFFERS FOR COMMUNICATION
	double *Y;                             //QUERY POINTS

	//variables for MPI COMMUNICATION.
	MPI_Status status;
	MPI_Request send_request,receive_request;
	int tag;

	//time variables
	struct timespec ts_start;
	struct timespec ts_end;
	double time;

	//find the number of processes
	MPI_Comm_size(MPI_COMM_WORLD, &p_size);

	//find the id of the process
	MPI_Comm_rank(MPI_COMM_WORLD, &p_rank);

	//define the communication tag
	tag = 1;

	//define the next and the previous process.
	//and create the circular communication
	next = p_rank+1;
	if(next == p_size) next = 0;

	prev = p_rank-1;
	if(prev== -1) prev = p_size-1;

	//initialize Y block
	Y = (double *)malloc(n*d*sizeof(double));

	//in the first iteration every process computes distances between its own points
	memcpy(Y,X,n*d*sizeof(double));

	//initialize receive buffer
	receive_buff = (double *)malloc(n*d*sizeof(double));

	//MASK COMMUNICATION TIME
	//Send and receive point for the next iteration
	//clock_gettime(CLOCK_MONOTONIC,&ts_start);

	MPI_Isend(Y,n*d,MPI_DOUBLE,next,tag,MPI_COMM_WORLD, &send_request);
	MPI_Irecv(receive_buff,n*d, MPI_DOUBLE, prev, tag, MPI_COMM_WORLD, &receive_request);

	//find k-nearest neighbors in this set of points.
	knn_process = kNN(X,Y,n,n,d,k);

	for(int i=0;i<n*k;i++) knn_process.nidx[i] = knn_process.nidx[i]+n*p_rank;

	MPI_Wait(&send_request,&status);
	//make sure receive_buffer is ready
	MPI_Wait(&receive_request,&status);

	//clock_gettime(CLOCK_MONOTONIC,&ts_end);
	//time = 1000000*(double)(ts_end.tv_sec-ts_start.tv_sec)+(double)(ts_end.tv_nsec-ts_start.tv_nsec)/1000;
	//printf("communication time and computation time%lf\n",time);

  for(int i=0;i<p_size-1;i++){
	  memcpy(Y,receive_buff,n*d*sizeof(double));

	  //MASK COMMUNICATION TIME
	  //send the block to the next node
	  clock_gettime(CLOCK_MONOTONIC,&ts_start);
	  MPI_Isend(Y,n*d,MPI_DOUBLE,next,tag,MPI_COMM_WORLD, &send_request);
	  MPI_Irecv(receive_buff,n*d,MPI_DOUBLE,prev,tag,MPI_COMM_WORLD,&receive_request);

	  knn_temp = kNN(Y,X,n,n,d,k);

	  //compare k-nearest points
	  knn_combine(knn_process, knn_temp,n,k,(p_rank-i-1+p_size)%p_size);

	  //make sure Y buffer has been send in order to change it
	  MPI_Wait(&send_request,&status);
	  //make sure receive_buffer is ready
	  MPI_Wait(&receive_request,&status);

	  //clock_gettime(CLOCK_MONOTONIC,&ts_end);
	  //time = 1000000*(double)(ts_end.tv_sec-ts_start.tv_sec)+(double)(ts_end.tv_nsec-ts_start.tv_nsec)/1000;
	  //printf("communication time and knn compute time%lf\n",time);
  }
  free(Y);
  free(knn_temp.nidx);
  free(knn_temp.ndist);

  for(int i=0;i<knn_process.m;i++) quick_sort(knn_process.ndist,knn_process.nidx,i*k,k*(i+1)-1);
  return knn_process;
}

void knn_combine(knnresult knn_total,knnresult knn_temp, int n, int k, int no_block){
	for(int m=0;m<knn_total.m;m++){
		for(int j=0;j<k;j++){
			if(knn_temp.ndist[m*k+j]>knn_total.ndist[k*(m+1)-1])
				break;
			else{
				knn_total.ndist[k*(m+1)-1] = knn_temp.ndist[m*k+j];
				knn_total.nidx[k*(m+1)-1] = knn_temp.nidx[m*k+j] + no_block*n;

				//sort the new element
				quick_sort(knn_total.ndist,knn_total.nidx,m*k,(m+1)*k-1);
			}
		}
	}
}