/* specfunc/laguerre.c
 *
 * Copyright (C) 2007 Brian Gough
 * 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 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.
 */

/* 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_laguerre.h>

#include "error.h"

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

/* based on the large 2b-4a asymptotic for 1F1
 * [Abramowitz+Stegun, 13.5.21]
 * L^a_n(x) = (a+1)_n / n! 1F1(-n,a+1,x)
 *
 * The second term (ser_term2) is from Slater,"The Confluent
 * Hypergeometric Function" p.73.  I think there may be an error in
 * the first term of the expression given there, comparing with AS
 * 13.5.21 (cf sin(a\pi+\Theta) vs sin(a\pi) + sin(\Theta)) - but the
 * second term appears correct.
 *
 */
static
int
laguerre_large_n(const int n, const double alpha, const double x,
                 gsl_sf_result * result)
{
  const double a = -n;
  const double b = alpha + 1.0;
  const double eta    = 2.0*b - 4.0*a;
  const double cos2th = x/eta;
  const double sin2th = 1.0 - cos2th;
  const double eps = asin(sqrt(cos2th));  /* theta = pi/2 - eps */
  const double pre_h  = 0.25*M_PI*M_PI*eta*eta*cos2th*sin2th;
  gsl_sf_result lg_b;
  gsl_sf_result lnfact;
  int stat_lg = gsl_sf_lngamma_e(b+n, &lg_b);
  int stat_lf = gsl_sf_lnfact_e(n, &lnfact);
  double pre_term1 = 0.5*(1.0-b)*log(0.25*x*eta);
  double pre_term2 = 0.25*log(pre_h);
  double lnpre_val = lg_b.val - lnfact.val + 0.5*x + pre_term1 - pre_term2;
  double lnpre_err = lg_b.err + lnfact.err + GSL_DBL_EPSILON * (fabs(pre_term1)+fabs(pre_term2));

  double phi1 = 0.25*eta*(2*eps + sin(2.0*eps));
  double ser_term1 = -sin(phi1);

  double A1 = (1.0/12.0)*(5.0/(4.0*sin2th)+(3.0*b*b-6.0*b+2.0)*sin2th - 1.0);
  double ser_term2 = -A1 * cos(phi1)/(0.25*eta*sin(2.0*eps));

  double ser_val = ser_term1 + ser_term2;
  double ser_err = ser_term2*ser_term2 + GSL_DBL_EPSILON * (fabs(ser_term1) + fabs(ser_term2));
  int stat_e = gsl_sf_exp_mult_err_e(lnpre_val, lnpre_err, ser_val, ser_err, result);
  result->err += 2.0 * GSL_SQRT_DBL_EPSILON * fabs(result->val);
  return GSL_ERROR_SELECT_3(stat_e, stat_lf, stat_lg);
}

/* Evaluate polynomial based on confluent hypergeometric representation.
 *
 * L^a_n(x) = (a+1)_n / n! 1F1(-n,a+1,x)
 *
 * assumes n > 0 and a != negative integer greater than -n
 */
static
int
laguerre_n_cp(const int n, const double a, const double x, gsl_sf_result * result)
{
  gsl_sf_result lnfact;
  gsl_sf_result lg1;
  gsl_sf_result lg2;
  double s1, s2;
  int stat_f = gsl_sf_lnfact_e(n, &lnfact);
  int stat_g1 = gsl_sf_lngamma_sgn_e(a+1.0+n, &lg1, &s1);
  int stat_g2 = gsl_sf_lngamma_sgn_e(a+1.0, &lg2, &s2);
  double poly_1F1_val = 1.0;
  double poly_1F1_err = 0.0;
  int stat_e;
  int k;

  double lnpre_val = (lg1.val - lg2.val) - lnfact.val;
  double lnpre_err = lg1.err + lg2.err + lnfact.err + 2.0 * GSL_DBL_EPSILON * fabs(lnpre_val);

  for(k=n-1; k>=0; k--) {
    double t = (-n+k)/(a+1.0+k) * (x/(k+1));
    double r = t + 1.0/poly_1F1_val;
    if(r > 0.9*GSL_DBL_MAX/poly_1F1_val) {
      /* internal error only, don't call the error handler */
      INTERNAL_OVERFLOW_ERROR(result);
    }
    else {
      /* Collect the Horner terms. */
      poly_1F1_val  = 1.0 + t * poly_1F1_val;
      poly_1F1_err += GSL_DBL_EPSILON + fabs(t) * poly_1F1_err;
    }
  }

  stat_e = gsl_sf_exp_mult_err_e(lnpre_val, lnpre_err,
                                    poly_1F1_val, poly_1F1_err,
                                    result);

  return GSL_ERROR_SELECT_4(stat_e, stat_f, stat_g1, stat_g2);
}

/* Evaluate the polynomial based on the confluent hypergeometric
 * function in a safe way, with no restriction on the arguments.
 *
 * assumes x != 0
 */
static
int
laguerre_n_poly_safe(const int n, const double a, const double x, gsl_sf_result * result)
{
  const double b = a + 1.0;
  const double mx = -x;
  const double tc_sgn = (x < 0.0 ? 1.0 : (GSL_IS_ODD(n) ? -1.0 : 1.0));
  gsl_sf_result tc;
  int stat_tc = gsl_sf_taylorcoeff_e(n, fabs(x), &tc);

  if(stat_tc == GSL_SUCCESS) {
    double term = tc.val * tc_sgn;
    double sum_val = term;
    double sum_err = tc.err;
    int k;
    for(k=n-1; k>=0; k--) {
      term *= ((b+k)/(n-k))*(k+1.0)/mx;
      sum_val += term;
      sum_err += 4.0 * GSL_DBL_EPSILON * fabs(term);
    }
    result->val = sum_val;
    result->err = sum_err + 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(stat_tc == GSL_EOVRFLW) {
    result->val = 0.0; /* FIXME: should be Inf */
    result->err = 0.0;
    return stat_tc;
  }
  else {
    result->val = 0.0;
    result->err = 0.0;
    return stat_tc;
  }
}

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

int
gsl_sf_laguerre_1_e(const double a, const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  {
    result->val = 1.0 + a - x;
    result->err = 2.0 * GSL_DBL_EPSILON * (1.0 + fabs(a) + fabs(x));
    return GSL_SUCCESS;
  }
}

int
gsl_sf_laguerre_2_e(const double a, const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(a == -2.0) {
    result->val = 0.5*x*x;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    double c0 = 0.5 * (2.0+a)*(1.0+a);
    double c1 = -(2.0+a);
    double c2 = -0.5/(2.0+a);
    result->val  = c0 + c1*x*(1.0 + c2*x);
    result->err  = 2.0 * GSL_DBL_EPSILON * (fabs(c0) + 2.0 * fabs(c1*x) * (1.0 + 2.0 * fabs(c2*x)));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

int
gsl_sf_laguerre_3_e(const double a, const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(a == -2.0) {
    double x2_6  = x*x/6.0;
    result->val  = x2_6 * (3.0 - x);
    result->err  = x2_6 * (3.0 + fabs(x)) * 2.0 * GSL_DBL_EPSILON;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(a == -3.0) {
    result->val = -x*x/6.0;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    double c0 = (3.0+a)*(2.0+a)*(1.0+a) / 6.0;
    double c1 = -c0 * 3.0 / (1.0+a);
    double c2 = -1.0/(2.0+a);
    double c3 = -1.0/(3.0*(3.0+a));
    result->val  = c0 + c1*x*(1.0 + c2*x*(1.0 + c3*x));
    result->err  = 1.0 + 2.0 * fabs(c3*x);
    result->err  = 1.0 + 2.0 * fabs(c2*x) * result->err;
    result->err  = 2.0 * GSL_DBL_EPSILON * (fabs(c0) + 2.0 * fabs(c1*x) * result->err);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

int
gsl_sf_laguerre_n_e(const int n, const double a, const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(n < 0) {
    DOMAIN_ERROR(result);
  }
  else if(n == 0) {
    result->val = 1.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(n == 1) {
    result->val = 1.0 + a - x;
    result->err = 2.0 * GSL_DBL_EPSILON * (1.0 + fabs(a) + fabs(x));
    return GSL_SUCCESS;
  }
  else if(x == 0.0) {
    double product = a + 1.0;
    int k;
    for(k=2; k<=n; k++) {
      product *= (a + k)/k;
    }
    result->val = product;
    result->err = 2.0 * (n + 1.0) * GSL_DBL_EPSILON * fabs(product) + GSL_DBL_EPSILON;
    return GSL_SUCCESS;
  }
  else if(x < 0.0 && a > -1.0) {
    /* In this case all the terms in the polynomial
     * are of the same sign. Note that this also
     * catches overflows correctly.
     */
    return laguerre_n_cp(n, a, x, result);
  }
  else if(n < 5 || (x > 0.0 && a < -n-1)) {
    /* Either the polynomial will not lose too much accuracy
     * or all the terms are negative. In any case,
     * the error estimate here is good. We try both
     * explicit summation methods, as they have different
     * characteristics. One may underflow/overflow while the
     * other does not.
     */
    if(laguerre_n_cp(n, a, x, result) == GSL_SUCCESS)
      return GSL_SUCCESS;
    else
      return laguerre_n_poly_safe(n, a, x, result);
  }
  else if(n > 1.0e+07 && x > 0.0 && a > -1.0 && x < 2.0*(a+1.0)+4.0*n) {
    return laguerre_large_n(n, a, x, result);
  }
  else if(a >= 0.0 || (x > 0.0 && a < -n-1)) {
    gsl_sf_result lg2;
    int stat_lg2 = gsl_sf_laguerre_2_e(a, x, &lg2);
    double Lkm1 = 1.0 + a - x;
    double Lk   = lg2.val;
    double Lkp1;
    int k;

    for(k=2; k<n; k++) {
      Lkp1 = (-(k+a)*Lkm1 + (2.0*k+a+1.0-x)*Lk)/(k+1.0);
      Lkm1 = Lk;
      Lk   = Lkp1;
    }
    result->val = Lk;
    result->err = (fabs(lg2.err/lg2.val) + GSL_DBL_EPSILON) * n * fabs(Lk);
    return stat_lg2;
  }
  else {
    /* Despair... or magic? */
    return laguerre_n_poly_safe(n, a, x, result);
  }
}

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

#include "eval.h"

double gsl_sf_laguerre_1(double a, double x)
{
  EVAL_RESULT(gsl_sf_laguerre_1_e(a, x, &result));
}

double gsl_sf_laguerre_2(double a, double x)
{
  EVAL_RESULT(gsl_sf_laguerre_2_e(a, x, &result));
}

double gsl_sf_laguerre_3(double a, double x)
{
  EVAL_RESULT(gsl_sf_laguerre_3_e(a, x, &result));
}

double gsl_sf_laguerre_n(int n, double a, double x)
{
  EVAL_RESULT(gsl_sf_laguerre_n_e(n, a, x, &result));
}


/* multfit/lmder.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 <stddef.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <float.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_permutation.h>

typedef struct
  {
    size_t iter;
    double xnorm;
    double fnorm;
    double delta;
    double par;
    gsl_matrix *J;             /* Jacobian matrix */
    gsl_matrix *r;             /* R matrix in J = Q R P^T */
    gsl_vector *tau;
    gsl_vector *diag;          /* scaling matrix D = diag(d1,...,dp) */
    gsl_vector *qtf;           /* Q^T f */
    gsl_vector *newton;
    gsl_vector *gradient;      /* gradient g = J^T f */
    gsl_vector *x_trial;       /* trial step x + dx */
    gsl_vector *f_trial;       /* trial function f(x + dx) */
    gsl_vector *df;
    gsl_vector *sdiag;
    gsl_vector *rptdx;
    const gsl_vector *weights; /* data weights */
    gsl_vector *w;
    gsl_vector *work1;
    gsl_permutation * perm;
  }
lmder_state_t;

static int lmder_alloc (void *vstate, size_t n, size_t p);
static int lmder_set (void *vstate, const gsl_vector * swts, gsl_multifit_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_vector * dx);
static int lmsder_set (void *vstate, const gsl_vector * swts, gsl_multifit_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_vector * dx);
static int set (void *vstate, const gsl_vector * swts, gsl_multifit_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_vector * dx, int scale);
static int lmder_iterate (void *vstate, const gsl_vector * swts, gsl_multifit_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_vector * dx);
static void lmder_free (void *vstate);
static int iterate (void *vstate, const gsl_vector * swts, gsl_multifit_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_vector * dx, int scale);

#include "lmutil.c"
#include "lmpar.c"
#include "lmset.c"
#include "lmiterate.c"

static int
lmder_alloc (void *vstate, size_t n, size_t p)
{
  lmder_state_t *state = (lmder_state_t *) vstate;
  gsl_matrix *r, *J;
  gsl_vector *tau, *diag, *qtf, *newton, *gradient, *x_trial, *f_trial,
   *df, *sdiag, *rptdx, *w, *work1;
  gsl_permutation *perm;

  J = gsl_matrix_alloc (n, p);

  if (J == 0)
    {
      GSL_ERROR ("failed to allocate space for J", GSL_ENOMEM);
    }

  state->J = J;

  r = gsl_matrix_alloc (n, p);

  if (r == 0)
    {
      GSL_ERROR ("failed to allocate space for r", GSL_ENOMEM);
    }

  state->r = r;

  tau = gsl_vector_calloc (GSL_MIN(n, p));

  if (tau == 0)
    {
      gsl_matrix_free (r);

      GSL_ERROR ("failed to allocate space for tau", GSL_ENOMEM);
    }

  state->tau = tau;

  diag = gsl_vector_calloc (p);

  if (diag == 0)
    {
      gsl_matrix_free (r);
      gsl_vector_free (tau);

      GSL_ERROR ("failed to allocate space for diag", GSL_ENOMEM);
    }

  state->diag = diag;

  qtf = gsl_vector_calloc (n);

  if (qtf == 0)
    {
      gsl_matrix_free (r);
      gsl_vector_free (tau);
      gsl_vector_free (diag);

      GSL_ERROR ("failed to allocate space for qtf", GSL_ENOMEM);
    }

  state->qtf = qtf;

  newton = gsl_vector_calloc (p);

  if (newton == 0)
    {
      gsl_matrix_free (r);
      gsl_vector_free (tau);
      gsl_vector_free (diag);
      gsl_vector_free (qtf);

      GSL_ERROR ("failed to allocate space for newton", GSL_ENOMEM);
    }

  state->newton = newton;

  gradient = gsl_vector_calloc (p);

  if (gradient == 0)
    {
      gsl_matrix_free (r);
      gsl_vector_free (tau);
      gsl_vector_free (diag);
      gsl_vector_free (qtf);
      gsl_vector_free (newton);

      GSL_ERROR ("failed to allocate space for gradient", GSL_ENOMEM);
    }

  state->gradient = gradient;

  x_trial = gsl_vector_calloc (p);

  if (x_trial == 0)
    {
      gsl_matrix_free (r);
      gsl_vector_free (tau);
      gsl_vector_free (diag);
      gsl_vector_free (qtf);
      gsl_vector_free (newton);
      gsl_vector_free (gradient);

      GSL_ERROR ("failed to allocate space for x_trial", GSL_ENOMEM);
    }

  state->x_trial = x_trial;

  f_trial = gsl_vector_calloc (n);

  if (f_trial == 0)
    {
      gsl_matrix_free (r);
      gsl_vector_free (tau);
      gsl_vector_free (diag);
      gsl_vector_free (qtf);
      gsl_vector_free (newton);
      gsl_vector_free (gradient);
      gsl_vector_free (x_trial);

      GSL_ERROR ("failed to allocate space for f_trial", GSL_ENOMEM);
    }

  state->f_trial = f_trial;

  df = gsl_vector_calloc (n);

  if (df == 0)
    {
      gsl_matrix_free (r);
      gsl_vector_free (tau);
      gsl_vector_free (diag);
      gsl_vector_free (qtf);
      gsl_vector_free (newton);
      gsl_vector_free (gradient);
      gsl_vector_free (x_trial);
      gsl_vector_free (f_trial);

      GSL_ERROR ("failed to allocate space for df", GSL_ENOMEM);
    }

  state->df = df;

  sdiag = gsl_vector_calloc (p);

  if (sdiag == 0)
    {
      gsl_matrix_free (r);
      gsl_vector_free (tau);
      gsl_vector_free (diag);
      gsl_vector_free (qtf);
      gsl_vector_free (newton);
      gsl_vector_free (gradient);
      gsl_vector_free (x_trial);
      gsl_vector_free (f_trial);
      gsl_vector_free (df);

      GSL_ERROR ("failed to allocate space for sdiag", GSL_ENOMEM);
    }

  state->sdiag = sdiag;

  rptdx = gsl_vector_calloc (n);

  if (rptdx == 0)
    {
      gsl_matrix_free (r);
      gsl_vector_free (tau);
      gsl_vector_free (diag);
      gsl_vector_free (qtf);
      gsl_vector_free (newton);
      gsl_vector_free (gradient);
      gsl_vector_free (x_trial);
      gsl_vector_free (f_trial);
      gsl_vector_free (df);
      gsl_vector_free (sdiag);

      GSL_ERROR ("failed to allocate space for rptdx", GSL_ENOMEM);
    }

  state->rptdx = rptdx;

  w = gsl_vector_calloc (n);

  if (w == 0)
    {
      gsl_matrix_free (r);
      gsl_vector_free (tau);
      gsl_vector_free (diag);
      gsl_vector_free (qtf);
      gsl_vector_free (newton);
      gsl_vector_free (gradient);
      gsl_vector_free (x_trial);
      gsl_vector_free (f_trial);
      gsl_vector_free (df);
      gsl_vector_free (sdiag);
      gsl_vector_free (rptdx);

      GSL_ERROR ("failed to allocate space for w", GSL_ENOMEM);
    }

  state->w = w;

  work1 = gsl_vector_calloc (p);

  if (work1 == 0)
    {
      gsl_matrix_free (r);
      gsl_vector_free (tau);
      gsl_vector_free (diag);
      gsl_vector_free (qtf);
      gsl_vector_free (newton);
      gsl_vector_free (gradient);
      gsl_vector_free (x_trial);
      gsl_vector_free (f_trial);
      gsl_vector_free (df);
      gsl_vector_free (sdiag);
      gsl_vector_free (rptdx);
      gsl_vector_free (w);

      GSL_ERROR ("failed to allocate space for work1", GSL_ENOMEM);
    }

  state->work1 = work1;

  perm = gsl_permutation_calloc (p);

  if (perm == 0)
    {
      gsl_matrix_free (r);
      gsl_vector_free (tau);
      gsl_vector_free (diag);
      gsl_vector_free (qtf);
      gsl_vector_free (newton);
      gsl_vector_free (gradient);
      gsl_vector_free (x_trial);
      gsl_vector_free (f_trial);
      gsl_vector_free (df);
      gsl_vector_free (sdiag);
      gsl_vector_free (rptdx);
      gsl_vector_free (w);
      gsl_vector_free (work1);

      GSL_ERROR ("failed to allocate space for perm", GSL_ENOMEM);
    }

  state->perm = perm;

  return GSL_SUCCESS;
}

static int
lmder_set (void *vstate, const gsl_vector * swts,
           gsl_multifit_function_fdf * fdf, gsl_vector * x,
           gsl_vector * f, gsl_vector * dx)
{
  int status = set (vstate, swts, fdf, x, f, dx, 0);
  return status ;
}

static int
lmsder_set (void *vstate, const gsl_vector * swts,
            gsl_multifit_function_fdf * fdf, gsl_vector * x,
            gsl_vector * f, gsl_vector * dx)
{
  int status = set (vstate, swts, fdf, x, f, dx, 1);
  return status ;
}

static int
lmder_iterate (void *vstate, const gsl_vector * swts,
               gsl_multifit_function_fdf * fdf, gsl_vector * x,
               gsl_vector * f, gsl_vector * dx)
{
  int status = iterate (vstate, swts, fdf, x, f, dx, 0);
  return status;
}

static int
lmsder_iterate (void *vstate, const gsl_vector * swts,
                gsl_multifit_function_fdf * fdf, gsl_vector * x,
                gsl_vector * f, gsl_vector * dx)
{
  int status = iterate (vstate, swts, fdf, x, f, dx, 1);
  return status;
}

static int
lmder_gradient (void *vstate, gsl_vector * g)
{
  lmder_state_t *state = (lmder_state_t *) vstate;
  compute_gradient(state->r, state->qtf, g);
  return GSL_SUCCESS;
}

static int
lmder_jac (void *vstate, gsl_matrix * J)
{
  lmder_state_t *state = (lmder_state_t *) vstate;
  int s = gsl_matrix_memcpy(J, state->J);

  return s;
}

static void
lmder_free (void *vstate)
{
  lmder_state_t *state = (lmder_state_t *) vstate;

  if (state->perm)
    gsl_permutation_free (state->perm);

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

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

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

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

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

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

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

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

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

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

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

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

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

  if (state->J)
    gsl_matrix_free (state->J);
}

static const gsl_multifit_fdfsolver_type lmder_type =
{
  "lmder",                      /* name */
  sizeof (lmder_state_t),
  &lmder_alloc,
  &lmder_set,
  &lmder_iterate,
  &lmder_gradient,
  &lmder_jac,
  &lmder_free
};

static const gsl_multifit_fdfsolver_type lmsder_type =
{
  "lmsder",                     /* name */
  sizeof (lmder_state_t),
  &lmder_alloc,
  &lmsder_set,
  &lmsder_iterate,
  &lmder_gradient,
  &lmder_jac,
  &lmder_free
};

const gsl_multifit_fdfsolver_type *gsl_multifit_fdfsolver_lmder = &lmder_type;
const gsl_multifit_fdfsolver_type *gsl_multifit_fdfsolver_lmsder = &lmsder_type;


/* wavelet/dwt.c
 *
 * Copyright (C) 2004 Ivo Alxneit
 *
 * 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.
 */

/* function dwt_step is based on the public domain function pwt.c
 * available from http://www.numerical-recipes.com
 */

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_wavelet.h>
#include <gsl/gsl_wavelet2d.h>

#define ELEMENT(a,stride,i) ((a)[(stride)*(i)])

static int binary_logn (const size_t n);
static void dwt_step (const gsl_wavelet * w, double *a, size_t stride, size_t n, gsl_wavelet_direction dir, gsl_wavelet_workspace * work);

static int
binary_logn (const size_t n)
{
  size_t ntest;
  size_t logn = 0;
  size_t k = 1;

  while (k < n)
    {
      k *= 2;
      logn++;
    }

  ntest = ((size_t)1 << logn);

  if (n != ntest)
    {
      return -1;                /* n is not a power of 2 */
    }

  return logn;
}

static void
dwt_step (const gsl_wavelet * w, double *a, size_t stride, size_t n,
          gsl_wavelet_direction dir, gsl_wavelet_workspace * work)
{
  double ai, ai1;
  size_t i, ii;
  size_t jf;
  size_t k;
  size_t n1, ni, nh, nmod;

  for (i = 0; i < work->n; i++)
    {
      work->scratch[i] = 0.0;
    }

  nmod = w->nc * n;
  nmod -= w->offset;            /* center support */

  n1 = n - 1;
  nh = n >> 1;

  if (dir == gsl_wavelet_forward)
    {
      for (ii = 0, i = 0; i < n; i += 2, ii++)
        {
          double h = 0, g = 0;

          ni = i + nmod;

          for (k = 0; k < w->nc; k++)
            {
              jf = n1 & (ni + k);
              h += w->h1[k] * ELEMENT (a, stride, jf);
              g += w->g1[k] * ELEMENT (a, stride, jf);
            }

          work->scratch[ii] += h;
          work->scratch[ii + nh] += g;
        }
    }
  else
    {
      for (ii = 0, i = 0; i < n; i += 2, ii++)
        {
          ai = ELEMENT (a, stride, ii);
          ai1 = ELEMENT (a, stride, ii + nh);
          ni = i + nmod;
          for (k = 0; k < w->nc; k++)
            {
              jf = (n1 & (ni + k));
              work->scratch[jf] += (w->h2[k] * ai + w->g2[k] * ai1);
            }
        }
    }

  for (i = 0; i < n; i++)
    {
      ELEMENT (a, stride, i) = work->scratch[i];
    }
}

int
gsl_wavelet_transform (const gsl_wavelet * w,
                       double *data, size_t stride, size_t n,
                       gsl_wavelet_direction dir,
                       gsl_wavelet_workspace * work)
{
  size_t i;

  if (work->n < n)
    {
      GSL_ERROR ("not enough workspace provided", GSL_EINVAL);
    }

  if (binary_logn (n) == -1)
    {
      GSL_ERROR ("n is not a power of 2", GSL_EINVAL);
    }

  if (n < 2)
    {
      return GSL_SUCCESS;
    }

  if (dir == gsl_wavelet_forward)
    {
      for (i = n; i >= 2; i >>= 1)
        {
          dwt_step (w, data, stride, i, dir, work);
        }
    }
  else
    {
      for (i = 2; i <= n; i <<= 1)
        {
          dwt_step (w, data, stride, i, dir, work);
        }
    }

  return GSL_SUCCESS;
}

int
gsl_wavelet_transform_forward (const gsl_wavelet * w,
                               double *data, size_t stride, size_t n,
                               gsl_wavelet_workspace * work)
{
  return gsl_wavelet_transform (w, data, stride, n, gsl_wavelet_forward, work);
}

int
gsl_wavelet_transform_inverse (const gsl_wavelet * w,
                               double *data, size_t stride, size_t n,
                               gsl_wavelet_workspace * work)
{
  return gsl_wavelet_transform (w, data, stride, n, gsl_wavelet_backward, work);
}

/* Leaving this out for now BJG */
#if 0
int
gsl_dwt_vector (const gsl_wavelet * w, gsl_vector *v, gsl_wavelet_direction
                dir, gsl_wavelet_workspace * work)
{
  return gsl_dwt (w, v->data, v->stride, v->size, dir, work);
}
#endif

int
gsl_wavelet2d_transform (const gsl_wavelet * w,
                         double *data, size_t tda, size_t size1,
                         size_t size2, gsl_wavelet_direction dir,
                         gsl_wavelet_workspace * work)
{
  size_t i;

  if (size1 != size2)
    {
      GSL_ERROR ("2d dwt works only with square matrix", GSL_EINVAL);
    }

  if (work->n < size1)
    {
      GSL_ERROR ("not enough workspace provided", GSL_EINVAL);
    }

  if (binary_logn (size1) == -1)
    {
      GSL_ERROR ("n is not a power of 2", GSL_EINVAL);
    }

  if (size1 < 2)
    {
      return GSL_SUCCESS;
    }

  if (dir == gsl_wavelet_forward)
    {
      for (i = 0; i < size1; i++)       /* for every row j */
        {
          gsl_wavelet_transform (w, &ELEMENT(data, tda, i), 1, size1, dir, work);
        }
      for (i = 0; i < size2; i++)       /* for every column j */
        {
          gsl_wavelet_transform (w, &ELEMENT(data, 1, i), tda, size2, dir, work);
        }
    }
  else
    {
      for (i = 0; i < size2; i++)       /* for every column j */
        {
          gsl_wavelet_transform (w, &ELEMENT(data, 1, i), tda, size2, dir, work);
        }
      for (i = 0; i < size1; i++)       /* for every row j */
        {
          gsl_wavelet_transform (w, &ELEMENT(data, tda, i), 1, size1, dir, work);
        }
    }

  return GSL_SUCCESS;
}

int
gsl_wavelet2d_nstransform (const gsl_wavelet * w,
                           double *data, size_t tda, size_t size1,
                           size_t size2, gsl_wavelet_direction dir,
                           gsl_wavelet_workspace * work)
{
  size_t i, j;

  if (size1 != size2)
    {
      GSL_ERROR ("2d dwt works only with square matrix", GSL_EINVAL);
    }

  if (work->n < size1)
    {
      GSL_ERROR ("not enough workspace provided", GSL_EINVAL);
    }

  if (binary_logn (size1) == -1)
    {
      GSL_ERROR ("n is not a power of 2", GSL_EINVAL);
    }

  if (size1 < 2)
    {
      return GSL_SUCCESS;
    }

  if (dir == gsl_wavelet_forward)
    {
      for (i = size1; i >= 2; i >>= 1)
        {
          for (j = 0; j < i; j++)       /* for every row j */
            {
              dwt_step (w, &ELEMENT(data, tda, j), 1, i, dir, work);
            }
          for (j = 0; j < i; j++)       /* for every column j */
            {
              dwt_step (w, &ELEMENT(data, 1, j), tda, i, dir, work);
            }
        }
    }
  else
    {
      for (i = 2; i <= size1; i <<= 1)
        {
          for (j = 0; j < i; j++)       /* for every column j */
            {
              dwt_step (w, &ELEMENT(data, 1, j), tda, i, dir, work);
            }
          for (j = 0; j < i; j++)       /* for every row j */
            {
              dwt_step (w, &ELEMENT(data, tda, j), 1, i, dir, work);
            }
        }
    }

  return GSL_SUCCESS;
}

int
gsl_wavelet2d_transform_forward (const gsl_wavelet * w,
                                 double *data, size_t tda, size_t size1,
                                 size_t size2, gsl_wavelet_workspace * work)
{
  return gsl_wavelet2d_transform (w, data, tda, size1, size2, gsl_wavelet_forward, work);
}

int
gsl_wavelet2d_transform_inverse (const gsl_wavelet * w,
                                  double *data, size_t tda, size_t size1,
                                  size_t size2, gsl_wavelet_workspace * work)
{
  return gsl_wavelet2d_transform (w, data, tda, size1, size2, gsl_wavelet_backward, work);
}

int
gsl_wavelet2d_nstransform_forward (const gsl_wavelet * w,
                                 double *data, size_t tda, size_t size1,
                                 size_t size2, gsl_wavelet_workspace * work)
{
  return gsl_wavelet2d_nstransform (w, data, tda, size1, size2, gsl_wavelet_forward, work);
}

int
gsl_wavelet2d_nstransform_inverse (const gsl_wavelet * w,
                                  double *data, size_t tda, size_t size1,
                                  size_t size2, gsl_wavelet_workspace * work)
{
  return gsl_wavelet2d_nstransform (w, data, tda, size1, size2, gsl_wavelet_backward, work);
}

int
gsl_wavelet2d_transform_matrix (const gsl_wavelet * w,
                                gsl_matrix * a,
                                gsl_wavelet_direction dir,
                                gsl_wavelet_workspace * work)
{
  return gsl_wavelet2d_transform (w, a->data,
                                  a->tda, a->size1, a->size2,
                                  dir, work);
}

int
gsl_wavelet2d_transform_matrix_forward (const gsl_wavelet * w,
                                        gsl_matrix * a,
                                        gsl_wavelet_workspace * work)
{
  return gsl_wavelet2d_transform (w, a->data,
                                  a->tda, a->size1, a->size2,
                                  gsl_wavelet_forward, work);
}

int
gsl_wavelet2d_transform_matrix_inverse (const gsl_wavelet * w,
                                        gsl_matrix * a,
                                        gsl_wavelet_workspace * work)
{
  return gsl_wavelet2d_transform (w, a->data,
                                  a->tda, a->size1, a->size2,
                                  gsl_wavelet_backward, work);
}

int
gsl_wavelet2d_nstransform_matrix (const gsl_wavelet * w,
                                gsl_matrix * a,
                                gsl_wavelet_direction dir,
                                gsl_wavelet_workspace * work)
{
  return gsl_wavelet2d_nstransform (w, a->data,
                                    a->tda, a->size1, a->size2,
                                    dir, work);
}

int
gsl_wavelet2d_nstransform_matrix_forward (const gsl_wavelet * w,
                                        gsl_matrix * a,
                                        gsl_wavelet_workspace * work)
{
  return gsl_wavelet2d_nstransform (w, a->data,
                                    a->tda, a->size1, a->size2,
                                    gsl_wavelet_forward, work);
}

int
gsl_wavelet2d_nstransform_matrix_inverse (const gsl_wavelet * w,
                                        gsl_matrix * a,
                                        gsl_wavelet_workspace * work)
{
  return gsl_wavelet2d_nstransform (w, a->data,
                                    a->tda, a->size1, a->size2,
                                    gsl_wavelet_backward, work);
}


#include <petsc.h>
#include <petscts2.h>

#if PETSC_VERSION_LE(3,3,0)
#define TSRegister(s,f) TSRegister(s,0,0,f)
#endif

typedef struct {
  PetscReal Omega; /* frequency */
  PetscReal Xi;    /* damping   */
} UserParams;

#undef  __FUNCT__
#define __FUNCT__ "Residual1"
PetscErrorCode Residual1(TS ts,PetscReal t,Vec X,Vec A,Vec R,void *ctx)
{
  UserParams *user = (UserParams *)ctx;
  PetscReal Omega = user->Omega;
  PetscScalar *x,*a,*r;
  PetscErrorCode ierr;
  PetscFunctionBegin;

  ierr = VecGetArray(X,&x);CHKERRQ(ierr);
  ierr = VecGetArray(A,&a);CHKERRQ(ierr);
  ierr = VecGetArray(R,&r);CHKERRQ(ierr);

  r[0] = a[0] + (Omega*Omega)*x[0];

  ierr = VecRestoreArray(X,&x);CHKERRQ(ierr);
  ierr = VecRestoreArray(A,&a);CHKERRQ(ierr);
  ierr = VecRestoreArray(R,&r);CHKERRQ(ierr);

  ierr = VecAssemblyBegin(R);CHKERRQ(ierr);
  ierr = VecAssemblyEnd  (R);CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

#undef  __FUNCT__
#define __FUNCT__ "Tangent1"
PetscErrorCode Tangent1(TS ts,PetscReal t,Vec X,Vec A,PetscReal shiftA,Mat J,Mat P,void *ctx)
{
  UserParams *user = (UserParams *)ctx;
  PetscReal Omega = user->Omega;
  PetscReal T = 0;
  PetscErrorCode ierr;
  PetscFunctionBegin;

  T = shiftA + (Omega*Omega);

  ierr = MatSetValue(P,0,0,T,INSERT_VALUES);CHKERRQ(ierr);
  ierr = MatAssemblyBegin(P,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
  ierr = MatAssemblyEnd  (P,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
  if (J != P) {
    ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
  }

  PetscFunctionReturn(0);
}
#if PETSC_VERSION_LT(3,5,0)
PetscErrorCode Tangent1_Legacy(TS ts,PetscReal t,Vec U,Vec V,PetscReal shift,Mat *J,Mat *P,MatStructure *m,void *ctx)
{*m = SAME_NONZERO_PATTERN; return Tangent1(ts,t,U,V,shift,*J,*P,ctx);}
#define Tangent1 Tangent1_Legacy
#endif

#undef  __FUNCT__
#define __FUNCT__ "Residual2"
PetscErrorCode Residual2(TS ts,PetscReal t,Vec X,Vec V,Vec A,Vec R,void *ctx)
{
  UserParams *user = (UserParams *)ctx;
  PetscReal Omega = user->Omega, Xi = user->Xi;
  PetscScalar *x,*v,*a,*r;
  PetscErrorCode ierr;
  PetscFunctionBegin;

  ierr = VecGetArray(X,&x);CHKERRQ(ierr);
  ierr = VecGetArray(V,&v);CHKERRQ(ierr);
  ierr = VecGetArray(A,&a);CHKERRQ(ierr);
  ierr = VecGetArray(R,&r);CHKERRQ(ierr);

  r[0] = a[0] + (2*Xi*Omega)*v[0] + (Omega*Omega)*x[0];

  ierr = VecRestoreArray(X,&x);CHKERRQ(ierr);
  ierr = VecRestoreArray(V,&v);CHKERRQ(ierr);
  ierr = VecRestoreArray(A,&a);CHKERRQ(ierr);
  ierr = VecRestoreArray(R,&r);CHKERRQ(ierr);

  ierr = VecAssemblyBegin(R);CHKERRQ(ierr);
  ierr = VecAssemblyEnd  (R);CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

#undef  __FUNCT__
#define __FUNCT__ "Tangent2"
PetscErrorCode Tangent2(TS ts,PetscReal t,Vec X,Vec V,Vec A,PetscReal shiftV,PetscReal shiftA,Mat J,Mat P,void *ctx)
{
  UserParams *user = (UserParams *)ctx;
  PetscReal Omega = user->Omega, Xi = user->Xi;
  PetscReal T = 0;
  PetscErrorCode ierr;
  PetscFunctionBegin;

  T = shiftA + shiftV * (2*Xi*Omega) + (Omega*Omega);

  ierr = MatSetValue(P,0,0,T,INSERT_VALUES);CHKERRQ(ierr);
  ierr = MatAssemblyBegin(P,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
  ierr = MatAssemblyEnd  (P,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
  if (J != P) {
    ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
  }

  PetscFunctionReturn(0);
}

#undef  __FUNCT__
#define __FUNCT__ "Monitor"
PetscErrorCode Monitor(TS ts,PetscInt i,PetscReal t,Vec U,void *ctx)
{
  const char *filename = (const char *)ctx;
  static FILE *fp = 0;
  Vec X,V;
  const PetscScalar *x,*v;
  TSConvergedReason reason;
  PetscErrorCode ierr;
  PetscFunctionBegin;
  if (i<0) PetscFunctionReturn(0); /*XXX temporary petsc-dev fix */

  if (!fp) {ierr = PetscFOpen(PETSC_COMM_SELF,filename,"w",&fp);CHKERRQ(ierr);}
  ierr = TSGetSolution2(ts,&X,&V);CHKERRQ(ierr);
  ierr = VecGetArrayRead(X,&x);CHKERRQ(ierr);
  ierr = VecGetArrayRead(V,&v);CHKERRQ(ierr);
  ierr = PetscFPrintf(PETSC_COMM_SELF,fp,"%g %g %g\n",(double)t,(double)x[0],(double)v[0]);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(X,&x);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(V,&v);CHKERRQ(ierr);
  ierr = TSGetConvergedReason(ts,&reason); CHKERRQ(ierr);
  if (reason) {ierr = PetscFClose(PETSC_COMM_SELF,fp);CHKERRQ(ierr); fp=0;}

  PetscFunctionReturn(0);
}

EXTERN_C_BEGIN
PetscErrorCode TSCreate_Alpha2(TS);
EXTERN_C_END

#undef  __FUNCT__
#define __FUNCT__ "main"
int main(int argc, char *argv[]) {

  TS             ts;
  Vec            R;
  Mat            J;
  Vec            X,V;
  PetscScalar    *x,*v;
  UserParams     user;
  PetscBool      out;
  char           output[PETSC_MAX_PATH_LEN] = {0};
  PetscErrorCode ierr;

  ierr = PetscInitialize(&argc,&argv,0,0);CHKERRQ(ierr);
  ierr = TSRegister(TSALPHA2,TSCreate_Alpha2);CHKERRQ(ierr);

  user.Omega = 1.0;
  user.Xi    = 0.0;
  ierr = PetscOptionsBegin(PETSC_COMM_SELF,"","Oscillator Options","TS");CHKERRQ(ierr);
  ierr = PetscOptionsReal("-frequency","Frequency",__FILE__,user.Omega,&user.Omega,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsReal("-damping",  "Damping",  __FILE__,user.Xi,   &user.Xi,   NULL);CHKERRQ(ierr);
  ierr = PetscOptionsString("-output","Output",__FILE__,output,output,sizeof(output),&out);CHKERRQ(ierr);
  ierr = PetscOptionsEnd();CHKERRQ(ierr);
  if (out && !output[0]) {ierr = PetscStrcpy(output,"Oscillator.out");CHKERRQ(ierr);}

  ierr = TSCreate(PETSC_COMM_SELF,&ts);CHKERRQ(ierr);
  ierr = TSSetType(ts,TSALPHA2);CHKERRQ(ierr);
  ierr = TSSetDuration(ts,PETSC_MAX_INT,2*M_PI * 5);CHKERRQ(ierr);
  ierr = TSSetTimeStep(ts,0.01);CHKERRQ(ierr);

  ierr = VecCreateSeq(PETSC_COMM_SELF,1,&R);CHKERRQ(ierr);
  ierr = VecSetUp(R);CHKERRQ(ierr);
  ierr = MatCreateSeqDense(PETSC_COMM_SELF,1,1,NULL,&J);CHKERRQ(ierr);
  ierr = MatSetUp(J);CHKERRQ(ierr);
  if (user.Xi <= 0.0) {
    ierr = TSSetIFunction(ts,R,Residual1,&user);CHKERRQ(ierr);
    ierr = TSSetIJacobian(ts,J,J,Tangent1,&user);CHKERRQ(ierr);
  } else {
    ierr = TSSetIFunction2(ts,R,Residual2,&user);CHKERRQ(ierr);
    ierr = TSSetIJacobian2(ts,J,J,Tangent2,&user);CHKERRQ(ierr);
  }
  ierr = VecDestroy(&R);CHKERRQ(ierr);
  ierr = MatDestroy(&J);CHKERRQ(ierr);

  if (output[0]) {
    ierr = TSMonitorSet(ts,Monitor,output,NULL);CHKERRQ(ierr);
  }

  ierr = VecCreateSeq(PETSC_COMM_SELF,1,&X);CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF,1,&V);CHKERRQ(ierr);
  ierr = VecGetArray(X,&x);CHKERRQ(ierr);
  ierr = VecGetArray(V,&v);CHKERRQ(ierr);
  x[0] = 1.0;
  v[0] = 0.0;
  ierr = VecRestoreArray(X,&x);CHKERRQ(ierr);
  ierr = VecRestoreArray(V,&v);CHKERRQ(ierr);

  ierr = TSSetSolution2(ts,X,V);CHKERRQ(ierr);
  ierr = TSSetFromOptions(ts);CHKERRQ(ierr);
  ierr = TSSolve2(ts,X,V);CHKERRQ(ierr);

  ierr = VecDestroy(&X);CHKERRQ(ierr);
  ierr = VecDestroy(&V);CHKERRQ(ierr);
  ierr = TSDestroy(&ts);CHKERRQ(ierr);
  ierr = PetscFinalize();CHKERRQ(ierr);

  return 0;
}


/* linalg/qr_ur.c
 *
 * Copyright (C) 2019, 2020 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.
 */

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

/*
 * this module contains routines for the QR factorization of a matrix
 * using the recursive Level 3 BLAS algorithm of Elmroth and Gustavson with
 * additional modifications courtesy of Julien Langou.
 */

static double qrtr_householder_transform (double *v0, gsl_vector * v);

/*
gsl_linalg_QR_UR_decomp()
  Compute the QR decomposition of the "triangle on top of rectangle" matrix

  [ S ] = Q [ R ]
  [ A ]     [ 0 ]

where S is N-by-N upper triangular and A is M-by-N dense.

Inputs: S   - on input, upper triangular N-by-N matrix
              on output, R factor in upper triangle
        A   - on input, dense M-by-N matrix
              on output, Householder matrix V
        T   - (output) block reflector matrix, N-by-N

Notes:
1) Based on the Elmroth/Gustavson algorithm, taking into account the
sparse structure of the S matrix

2) The Householder matrix V has the special form:

      N
V = [ I  ] N
    [ V~ ] M

The matrix V~ is stored in A on output; the identity is not stored

3) The orthogonal matrix is

Q = I - V T V^T
*/

int
gsl_linalg_QR_UR_decomp (gsl_matrix * S, gsl_matrix * A, gsl_matrix * T)
{
  const size_t M = A->size1;
  const size_t N = S->size1;

  if (N != S->size2)
    {
      GSL_ERROR ("S matrix must be square", GSL_ENOTSQR);
    }
  else if (N != A->size2)
    {
      GSL_ERROR ("S and A have different number of columns", GSL_EBADLEN);
    }
  else if (T->size1 != N || T->size2 != N)
    {
      GSL_ERROR ("T matrix has wrong dimensions", GSL_EBADLEN);
    }
  else if (N == 1)
    {
      /* base case, compute Householder transform for single column matrix */
      double * T00 = gsl_matrix_ptr(T, 0, 0);
      double * S00 = gsl_matrix_ptr(S, 0, 0);
      gsl_vector_view v = gsl_matrix_column(A, 0);
      *T00 = qrtr_householder_transform(S00, &v.vector);
      return GSL_SUCCESS;
    }
  else
    {
      /*
       * partition matrices:
       *
       *       N1  N2              N1  N2
       * N1 [ S11 S12 ] and  N1 [ T11 T12 ]
       * N2 [  0  S22 ]      N2 [  0  T22 ]
       * M  [  A1  A2 ]
       */
      int status;
      const size_t N1 = N / 2;
      const size_t N2 = N - N1;

      gsl_matrix_view S11 = gsl_matrix_submatrix(S, 0, 0, N1, N1);
      gsl_matrix_view S12 = gsl_matrix_submatrix(S, 0, N1, N1, N2);
      gsl_matrix_view S22 = gsl_matrix_submatrix(S, N1, N1, N2, N2);

      gsl_matrix_view A1 = gsl_matrix_submatrix(A, 0, 0, M, N1);
      gsl_matrix_view A2 = gsl_matrix_submatrix(A, 0, N1, M, N2);

      gsl_matrix_view T11 = gsl_matrix_submatrix(T, 0, 0, N1, N1);
      gsl_matrix_view T12 = gsl_matrix_submatrix(T, 0, N1, N1, N2);
      gsl_matrix_view T22 = gsl_matrix_submatrix(T, N1, N1, N2, N2);

      /*
       * Eq. 2: recursively factor
       *
       *       N1           N1
       * N1 [ S11 ] = Q1 [ R11 ] N1
       * N2 [  0  ]      [  0  ] N2
       * M  [  A1 ]      [  0  ] M
       */
      status = gsl_linalg_QR_UR_decomp(&S11.matrix, &A1.matrix, &T11.matrix);
      if (status)
        return status;

      /*
       * Eq. 3:
       *
       *      N2              N2            N2
       * N1 [ R12  ] = Q1^T [ S12 ] = [   S12 - W  ] N1
       * N2 [ S22~ ]        [ S22 ]   [     S22    ] N2
       * M  [  A2~ ]        [  A2 ]   [ A2 - V1~ W ] M
       *
       * where W = T11^T ( S12 + V1~^T A2 ), using T12 as temporary storage, and
       *
       *        N1
       * V1 = [  I  ] N1
       *      [  0  ] N2
       *      [ V1~ ] M
       */
      gsl_matrix_memcpy(&T12.matrix, &S12.matrix);                                                    /* W := S12 */
      gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, &A1.matrix, &A2.matrix, 1.0, &T12.matrix);        /* W := S12 + V1~^T A2 */
      gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, 1.0, &T11.matrix, &T12.matrix); /* W := T11^T W */
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &A1.matrix, &T12.matrix, 1.0, &A2.matrix);     /* A2 := A2 - V1~ W */
      gsl_matrix_sub(&S12.matrix, &T12.matrix);                                                       /* R12 := S12 - W */

      /*
       * Eq. 4: recursively factor
       *
       * [ S22~ ] = Q2~ [ R22 ]
       * [  A2~ ]       [  0  ]
       */
      status = gsl_linalg_QR_UR_decomp(&S22.matrix, &A2.matrix, &T22.matrix);
      if (status)
        return status;

      /*
       * Eq. 13: update T12 := -T11 * V1^T * V2 * T22
       *
       * where:
       *
       *        N1                N2
       * V1 = [  I  ] N1   V2 = [  0  ] N1
       *      [  0  ] N2        [  I  ] N2
       *      [ V1~ ] M         [ V2~ ] M
       *
       * Note: V1^T V2 = V1~^T V2~
       */

      gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, &A1.matrix, &A2.matrix, 0.0, &T12.matrix);           /* T12 := V1~^T * V2~ */
      gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, -1.0, &T11.matrix, &T12.matrix); /* T12 := -T11 * T12 */
      gsl_blas_dtrmm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, &T22.matrix, &T12.matrix); /* T12 := T12 * T22 */

      return GSL_SUCCESS;
    }
}

/*
qrtr_householder_transform()
  This routine is an optimized version of
gsl_linalg_householder_transform(), designed for the QR
decomposition of M-by-N matrices of the form:

B = [ S ]
    [ A ]

where S is N-by-N upper triangular, and A is M-by-N dense.
This routine computes a householder transformation (tau,v) of a
x so that P x = [ I - tau*v*v' ] x annihilates x(1:n-1). x will
be a subcolumn of the matrix B, and so its structure will be:

x = [ x0 ] <- 1 nonzero value for the diagonal element of S
    [ 0  ] <- N - j - 1 zeros, where j is column of matrix in [0,N-1]
    [ x  ] <- M nonzero values for the dense part A

Inputs: v0 - pointer to diagonal element of S
             on input, v0 = x0;
        v  - on input, x vector
             on output, householder vector v
*/

static double
qrtr_householder_transform (double *v0, gsl_vector * v)
{
  /* replace v[0:M-1] with a householder vector (v[0:M-1]) and
     coefficient tau that annihilate v[1:M-1] */

  double alpha, beta, tau ;

  /* compute xnorm = || [ 0 ; v ] ||, ignoring zero part of vector */
  double xnorm = gsl_blas_dnrm2(v);

  if (xnorm == 0)
    {
      return 0.0; /* tau = 0 */
    }

  alpha = *v0;
  beta = - GSL_SIGN(alpha) * hypot(alpha, xnorm) ;
  tau = (beta - alpha) / beta ;

  {
    double s = (alpha - beta);

    if (fabs(s) > GSL_DBL_MIN)
      {
        gsl_blas_dscal (1.0 / s, v);
        *v0 = beta;
      }
    else
      {
        gsl_blas_dscal (GSL_DBL_EPSILON / s, v);
        gsl_blas_dscal (1.0 / GSL_DBL_EPSILON, v);
        *v0 = beta;
      }
  }

  return tau;
}


//Gets deltas (1st order) and delta-deltas (2nd order) differences of X.
//Only the deltas and delta-deltas are output in Y.
//Thus, Y must be pre-allocated to have twice the size of X.

//I implement this just like FIR for speed and shorter code,
//except non-causal and mid-sample of B is 0,
//so I don't explicitly make B (e.g., B[n] just equals sc*n).

//Note that this may treat edge samples differently than other code.
//But this could be changed with a few lines of additional code here,
//without changing the super-efficient FIR implementation.
//That is, since out-of-range samps were assumed to be 0, nothing was
//added to Y for them, so can just add something later.

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

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

int get_delta_deltas_s (float *Y, const float *X, const int iscolmajor, const int R, const int C, const int dim, const int N);
int get_delta_deltas_d (double *Y, const double *X, const int iscolmajor, const int R, const int C, const int dim, const int N);

int get_delta_deltas_s (float *Y, const float *X, const int iscolmajor, const int R, const int C, const int dim, const int N)
{
    const float z = 0.0f;
    int r, c, n;
    float sc = 1.0f;

    //Checks
    if (R<1) { fprintf(stderr,"error in get_delta_deltas_s: R (nrows Y) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in get_delta_deltas_s: C (ncols Y) must be positive\n"); return 1; }
    if (N<1) { fprintf(stderr,"error in get_delta_deltas_s: N (delta winlength) must be positive\n"); return 1; }

    //Get sc (normalizer)
    for (n=2; n<=N; n++) { sc += n*n; }
    sc = 0.5f/sc;

    //Initialize Y
    cblas_scopy(2*R*C,&z,0,&Y[0],1);

    if (dim==0)
    {
        if (iscolmajor)
        {
            for (c=0; c<C; c++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[c*R],0,&Y[c*R],1);            //beg edge samps
                    cblas_saxpy(R-n,-sc*n,&X[c*R],1,&Y[c*R+n],1);        //past samps
                    cblas_saxpy(R-n,sc*n,&X[c*R+n],1,&Y[c*R],1);         //future samps
                    cblas_saxpy(n,sc*n,&X[c*R+R-1],0,&Y[c*R+R-n],1);     //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&Y[c*R],0,&Y[R*C+c*R],1);        //beg edge samps
                    cblas_saxpy(R-n,-sc*n,&Y[c*R],1,&Y[R*C+c*R+n],1);    //past samps
                    cblas_saxpy(R-n,sc*n,&Y[c*R+n],1,&Y[R*C+c*R],1);     //future samps
                    cblas_saxpy(n,sc*n,&Y[c*R+R-1],0,&Y[R*C+c*R+R-n],1); //end edge samps
                }
            }
        }
        else
        {
            for (c=0; c<C; c++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[c],0,&Y[c],2*C);                      //beg edge samps
                    cblas_saxpy(R-n,-sc*n,&X[c],C,&Y[c+n*2*C],2*C);              //past samps
                    cblas_saxpy(R-n,sc*n,&X[c+n*C],C,&Y[c],2*C);                 //future samps
                    cblas_saxpy(n,sc*n,&X[c+C*(R-1)],0,&Y[c+2*C*(R-n)],2*C);     //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&Y[c],0,&Y[C+c],2*C);                    //beg edge samps
                    cblas_saxpy(R-n,-sc*n,&Y[c],2*C,&Y[C+c+n*2*C],2*C);          //past samps
                    cblas_saxpy(R-n,sc*n,&Y[c+n*2*C],2*C,&Y[C+c],2*C);           //future samps
                    cblas_saxpy(n,sc*n,&Y[c+2*C*(R-1)],0,&Y[C+c+2*C*(R-n)],2*C); //end edge samps
                }
            }
        }
    }
    else if (dim==1)
    {
        if (iscolmajor)
        {
            for (r=0; r<R; r++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[r],0,&Y[r],2*R);                      //beg edge samps
                    cblas_saxpy(C-n,-sc*n,&X[r],R,&Y[r+n*2*R],2*R);              //past samps
                    cblas_saxpy(C-n,sc*n,&X[r+n*R],R,&Y[r],2*R);                 //future samps
                    cblas_saxpy(n,sc*n,&X[r+R*(C-1)],0,&Y[r+2*R*(C-n)],2*R);     //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&Y[r],0,&Y[R+r],2*R);                    //beg edge samps
                    cblas_saxpy(C-n,-sc*n,&Y[r],2*R,&Y[R+r+n*2*R],2*R);          //past samps
                    cblas_saxpy(C-n,sc*n,&Y[r+n*2*R],2*R,&Y[R+r],2*R);           //future samps
                    cblas_saxpy(n,sc*n,&Y[r+2*R*(C-1)],0,&Y[R+r+2*R*(C-n)],2*R); //end edge samps
                }
            }
        }
        else
        {
            for (r=0; r<R; r++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[r*C],0,&Y[r*C],1);            //beg edge samps
                    cblas_saxpy(C-n,-sc*n,&X[r*C],1,&Y[r*C+n],1);        //past samps
                    cblas_saxpy(C-n,sc*n,&X[r*C+n],1,&Y[r*C],1);         //future samps
                    cblas_saxpy(n,sc*n,&X[r*C+C-1],0,&Y[r*C+C-n],1);     //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&Y[r*C],0,&Y[R*C+r*C],1);        //beg edge samps
                    cblas_saxpy(C-n,-sc*n,&Y[r*C],1,&Y[R*C+r*C+n],1);    //past samps
                    cblas_saxpy(C-n,sc*n,&Y[r*C+n],1,&Y[R*C+r*C],1);     //future samps
                    cblas_saxpy(n,sc*n,&Y[r*C+C-1],0,&Y[R*C+r*C+C-n],1); //end edge samps
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in get_delta_deltas_s: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int get_delta_deltas_d (double *Y, const double *X, const int iscolmajor, const int R, const int C, const int dim, const int N)
{
    const double z = 0.0;
    int r, c, n;
    double sc = 1.0;

    //Checks
    if (R<1) { fprintf(stderr,"error in get_delta_deltas_d: R (nrows Y) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in get_delta_deltas_d: C (ncols Y) must be positive\n"); return 1; }
    if (N<1) { fprintf(stderr,"error in get_delta_deltas_d: N (delta winlength) must be positive\n"); return 1; }

    //Get sc (normalizer)
    for (n=2; n<=N; n++) { sc += n*n; }
    sc = 0.5/sc;

    //Initialize Y
    cblas_dcopy(2*R*C,&z,0,&Y[0],1);

    if (dim==0)
    {
        if (iscolmajor)
        {
            for (c=0; c<C; c++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[c*R],0,&Y[c*R],1);            //beg edge samps
                    cblas_daxpy(R-n,-sc*n,&X[c*R],1,&Y[c*R+n],1);        //past samps
                    cblas_daxpy(R-n,sc*n,&X[c*R+n],1,&Y[c*R],1);         //future samps
                    cblas_daxpy(n,sc*n,&X[c*R+R-1],0,&Y[c*R+R-n],1);     //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&Y[c*R],0,&Y[R*C+c*R],1);        //beg edge samps
                    cblas_daxpy(R-n,-sc*n,&Y[c*R],1,&Y[R*C+c*R+n],1);    //past samps
                    cblas_daxpy(R-n,sc*n,&Y[c*R+n],1,&Y[R*C+c*R],1);     //future samps
                    cblas_daxpy(n,sc*n,&Y[c*R+R-1],0,&Y[R*C+c*R+R-n],1); //end edge samps
                }
            }
        }
        else
        {
            for (c=0; c<C; c++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[c],0,&Y[c],2*C);                      //beg edge samps
                    cblas_daxpy(R-n,-sc*n,&X[c],C,&Y[c+n*2*C],2*C);              //past samps
                    cblas_daxpy(R-n,sc*n,&X[c+n*C],C,&Y[c],2*C);                 //future samps
                    cblas_daxpy(n,sc*n,&X[c+C*(R-1)],0,&Y[c+2*C*(R-n)],2*C);     //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&Y[c],0,&Y[C+c],2*C);                    //beg edge samps
                    cblas_daxpy(R-n,-sc*n,&Y[c],2*C,&Y[C+c+n*2*C],2*C);          //past samps
                    cblas_daxpy(R-n,sc*n,&Y[c+n*2*C],2*C,&Y[C+c],2*C);           //future samps
                    cblas_daxpy(n,sc*n,&Y[c+2*C*(R-1)],0,&Y[C+c+2*C*(R-n)],2*C); //end edge samps
                }
            }
        }
    }
    else if (dim==1)
    {
        if (iscolmajor)
        {
            for (r=0; r<R; r++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[r],0,&Y[r],2*R);                      //beg edge samps
                    cblas_daxpy(C-n,-sc*n,&X[r],R,&Y[r+n*2*R],2*R);              //past samps
                    cblas_daxpy(C-n,sc*n,&X[r+n*R],R,&Y[r],2*R);                 //future samps
                    cblas_daxpy(n,sc*n,&X[r+R*(C-1)],0,&Y[r+2*R*(C-n)],2*R);     //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&Y[r],0,&Y[R+r],2*R);                    //beg edge samps
                    cblas_daxpy(C-n,-sc*n,&Y[r],2*R,&Y[R+r+n*2*R],2*R);          //past samps
                    cblas_daxpy(C-n,sc*n,&Y[r+n*2*R],2*R,&Y[R+r],2*R);           //future samps
                    cblas_daxpy(n,sc*n,&Y[r+2*R*(C-1)],0,&Y[R+r+2*R*(C-n)],2*R); //end edge samps
                }
            }
        }
        else
        {
            for (r=0; r<R; r++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[r*C],0,&Y[r*C],1);            //beg edge samps
                    cblas_daxpy(C-n,-sc*n,&X[r*C],1,&Y[r*C+n],1);        //past samps
                    cblas_daxpy(C-n,sc*n,&X[r*C+n],1,&Y[r*C],1);         //future samps
                    cblas_daxpy(n,sc*n,&X[r*C+C-1],0,&Y[r*C+C-n],1);     //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&Y[r*C],0,&Y[R*C+r*C],1);        //beg edge samps
                    cblas_daxpy(C-n,-sc*n,&Y[r*C],1,&Y[R*C+r*C+n],1);    //past samps
                    cblas_daxpy(C-n,sc*n,&Y[r*C+n],1,&Y[R*C+r*C],1);     //future samps
                    cblas_daxpy(n,sc*n,&Y[r*C+C-1],0,&Y[R*C+r*C+C-n],1); //end edge samps
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in get_delta_deltas_d: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


/////////////////////////////////////////////////////////////////////
// = NMatrix
//
// A linear algebra library for scientific computation in Ruby.
// NMatrix is part of SciRuby.
//
// NMatrix was originally inspired by and derived from NArray, by
// Masahiro Tanaka: http://narray.rubyforge.org
//
// == Copyright Information
//
// SciRuby is Copyright (c) 2010 - 2014, Ruby Science Foundation
// NMatrix is Copyright (c) 2012 - 2014, John Woods and the Ruby Science Foundation
//
// Please see LICENSE.txt for additional copyright notices.
//
// == Contributing
//
// By contributing source code to SciRuby, you agree to be bound by
// our Contributor Agreement:
//
// * https://github.com/SciRuby/sciruby/wiki/Contributor-Agreement
//
// == getrs.h
//
// getrs function in native C++.
//

/*
 *             Automatically Tuned Linear Algebra Software v3.8.4
 *                    (C) Copyright 1999 R. Clint Whaley
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *   1. Redistributions of source code must retain the above copyright
 *      notice, this list of conditions and the following disclaimer.
 *   2. Redistributions in binary form must reproduce the above copyright
 *      notice, this list of conditions, and the following disclaimer in the
 *      documentation and/or other materials provided with the distribution.
 *   3. The name of the ATLAS group or the names of its contributers may
 *      not be used to endorse or promote products derived from this
 *      software without specific written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE ATLAS GROUP OR ITS CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

#ifndef GETRS_H
#define GETRS_H

extern "C" {
#if defined HAVE_CBLAS_H
  #include <cblas.h>
#elif defined HAVE_ATLAS_CBLAS_H
  #include <atlas/cblas.h>
#endif
}

namespace nm { namespace math {

/*
 * Solves a system of linear equations A*X = B with a general NxN matrix A using the LU factorization computed by GETRF.
 *
 * From ATLAS 3.8.0.
 */
template <typename DType>
int getrs(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE Trans, const int N, const int NRHS, const DType* A,
           const int lda, const int* ipiv, DType* B, const int ldb)
{
  // enum CBLAS_DIAG Lunit, Uunit; // These aren't used. Not sure why they're declared in ATLAS' src.

  if (!N || !NRHS) return 0;

  const DType ONE = 1;

  if (Order == CblasColMajor) {
    if (Trans == CblasNoTrans) {
      nm::math::laswp<DType>(NRHS, B, ldb, 0, N, ipiv, 1);
      nm::math::trsm<DType>(Order, CblasLeft, CblasLower, CblasNoTrans, CblasUnit, N, NRHS, ONE, A, lda, B, ldb);
      nm::math::trsm<DType>(Order, CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, N, NRHS, ONE, A, lda, B, ldb);
    } else {
      nm::math::trsm<DType>(Order, CblasLeft, CblasUpper, Trans, CblasNonUnit, N, NRHS, ONE, A, lda, B, ldb);
      nm::math::trsm<DType>(Order, CblasLeft, CblasLower, Trans, CblasUnit, N, NRHS, ONE, A, lda, B, ldb);
      nm::math::laswp<DType>(NRHS, B, ldb, 0, N, ipiv, -1);
    }
  } else {
    if (Trans == CblasNoTrans) {
      nm::math::trsm<DType>(Order, CblasRight, CblasLower, CblasTrans, CblasNonUnit, NRHS, N, ONE, A, lda, B, ldb);
      nm::math::trsm<DType>(Order, CblasRight, CblasUpper, CblasTrans, CblasUnit, NRHS, N, ONE, A, lda, B, ldb);
      nm::math::laswp<DType>(NRHS, B, ldb, 0, N, ipiv, -1);
    } else {
      nm::math::laswp<DType>(NRHS, B, ldb, 0, N, ipiv, 1);
      nm::math::trsm<DType>(Order, CblasRight, CblasUpper, CblasNoTrans, CblasUnit, NRHS, N, ONE, A, lda, B, ldb);
      nm::math::trsm<DType>(Order, CblasRight, CblasLower, CblasNoTrans, CblasNonUnit, NRHS, N, ONE, A, lda, B, ldb);
    }
  }
  return 0;
}

/*
* Function signature conversion for calling LAPACK's getrs functions as directly as possible.
*
* For documentation: http://www.netlib.org/lapack/double/dgetrs.f
*
* This function should normally go in math.cpp, but we need it to be available to nmatrix.cpp.
*/
template <typename DType>
inline int clapack_getrs(const enum CBLAS_ORDER order, const enum CBLAS_TRANSPOSE trans, const int n, const int nrhs,
                         const void* a, const int lda, const int* ipiv, void* b, const int ldb) {
  return getrs<DType>(order, trans, n, nrhs, reinterpret_cast<const DType*>(a), lda, ipiv, reinterpret_cast<DType*>(b), ldb);
}

} } // end nm::math

#endif // GETRS_H


#include "matrix.h"
#include <cblas.h>
#include <malloc.h>
#include <stdio.h>
#include <stdlib.h>

Matrix *create_mat(char *path) {
  Matrix *m = malloc(sizeof(Matrix));
  m->row_len = 0;
  m->col_len = 0;
  FILE *myfile = fopen(path, "r");
  int ch = 0;
  while (ch != EOF) {
    ch = fgetc(myfile);
    if (m->row_len == 0 && ch == ' ') {
      m->col_len++;
    }
    if (ch == '\n') {
      m->row_len++;
    }
  };

  m->col_len++;

  m->p_mat = calloc(m->row_len, sizeof(float *));
  for (int i = 0; i < m->row_len; i++) {
    m->p_mat[i] = calloc(m->col_len, sizeof(float));
  }

  rewind(myfile);

  for (int i = 0; i < m->row_len; i++) {
    for (int j = 0; j < m->col_len; j++) {
      if (!fscanf(myfile, "%f", &m->p_mat[i][j])) {
        break;
      }
    }
  }

  fclose(myfile);
  return m;
}

void delete_mat(Matrix *mat) {
  for (int i = 0; i < mat->row_len; i++) {
    free(mat->p_mat[i]);
  }
  free(mat->p_mat);
  free(mat);
}

void copy_mat(Matrix *target, Matrix *source) {
  delete_mat(target);
  target->row_len = source->row_len;
  target->col_len = source->col_len;
  target->p_mat = calloc(target->row_len, sizeof(float *));
  for (int i = 0; i < target->row_len; i++) {
    target->p_mat[i] = calloc(target->col_len, sizeof(float));
  }
  for (int i = 0; i < target->row_len; i++) {
    for (int j = 0; j < target->col_len; j++) {
      target->p_mat[i][j] = source->p_mat[i][j];
    }
  }
}

Matrix *multiple_mat(Matrix *mat1, Matrix *mat2) {
  Matrix *result = malloc(sizeof(Matrix));
  result->row_len = mat1->row_len;
  result->col_len = mat2->col_len;
  result->p_mat = calloc(result->row_len, sizeof(float *));
  for (int i = 0; i < result->row_len; ++i)
    result->p_mat[i] = calloc(result->col_len, sizeof(float));

#pragma omp parallel for
  for (int i = 0; i < result->row_len; i++) {
    for (int k = 0; k < mat1->col_len; k++) {
      float tmp = mat1->p_mat[i][k];
      for (int j = 0; j < result->col_len; j++) {
        result->p_mat[i][j] += tmp * mat2->p_mat[k][j];
      }
    }
  }

  return result;
}
void write_mat(char *path, Matrix *mat) {
  FILE *myfile = fopen(path, "w");
  for (int i = 0; i < mat->row_len; i++) {
    for (int j = 0; j < mat->col_len; j++) {
      if (j == mat->col_len - 1) {
        fprintf(myfile, "%f\n", mat->p_mat[i][j]);
      } else {
        fprintf(myfile, "%f ", mat->p_mat[i][j]);
      }
    }
  }
}

void display_mat(Matrix *m1) {
  for (int i = 0; i < m1->row_len; i++) {
    for (int j = 0; j < m1->col_len; j++) {
      if (j == m1->col_len - 1) {
        printf("%f\n", m1->p_mat[i][j]);
      } else {
        printf("%f ", m1->p_mat[i][j]);
      }
    }
  }
}

Matrix *calc_with_OpenBLAS(Matrix *m1, Matrix *m2) {
  Matrix *result = malloc(sizeof(Matrix));
  result->row_len = m1->row_len;
  result->col_len = m2->col_len;
  result->p_mat = calloc(result->row_len, sizeof(float *));
  for (int i = 0; i < result->row_len; ++i)
    result->p_mat[i] = calloc(result->col_len, sizeof(float));
  const int M = m1->row_len;
  const int N = m2->col_len;
  const int K = m1->col_len;
  int lda = K;
  int ldb = N;
  int ldc = N;
  float *A = calloc(M * K, sizeof(float));
  float *B = calloc(K * N, sizeof(float));
  float *C = calloc(M * N, sizeof(float));
  int k = 0;
  for (int i = 0; i < M; i++) {
    for (int j = 0; j < K; j++) {
      A[k] = m1->p_mat[i][j];
      k += 1;
    }
  }
  k = 0;
  for (int i = 0; i < K; i++) {
    for (int j = 0; j < N; j++) {
      B[k] = m2->p_mat[i][j];
      k += 1;
    }
  }
  cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, M, N, K, 1.0f, A, lda,
              B, ldb, 1.0f, C, ldc);
  k = 0;
  for (int i = 0; i < result->row_len; i++) {
    for (int j = 0; j < result->col_len; j++) {
      result->p_mat[i][j] = C[k];
      k++;
    }
  }
  free(A);
  free(B);
  free(C);
  return result;
}