/* poly/zsolve.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.
 */

/* zsolve.c - finds the complex roots of  = 0 */

#include <config.h>
#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_poly.h>

/* C-style matrix elements */
#define MAT(m,i,j,n) ((m)[(i)*(n) + (j)])

/* Fortran-style matrix elements */
#define FMAT(m,i,j,n) ((m)[((i)-1)*(n) + ((j)-1)])

#include "companion.c"
#include "balance.c"
#include "qr.c"

int
gsl_poly_complex_solve (const double *a, size_t n,
                        gsl_poly_complex_workspace * w,
                        gsl_complex_packed_ptr z)
{
  int status;
  double *m;

  if (n == 0)
    {
      GSL_ERROR ("number of terms must be a positive integer", GSL_EINVAL);
    }

  if (n == 1)
    {
      GSL_ERROR ("cannot solve for only one term", GSL_EINVAL);
    }

  if (a[n - 1] == 0)
    {
      GSL_ERROR ("leading term of polynomial must be non-zero", GSL_EINVAL) ;
    }

  if (w->nc != n - 1)
    {
      GSL_ERROR ("size of workspace does not match polynomial", GSL_EINVAL);
    }

  m = w->matrix;

  set_companion_matrix (a, n - 1, m);

  balance_companion_matrix (m, n - 1);

  status = qr_companion (m, n - 1, z);

  if (status)
    {
      GSL_ERROR("root solving qr method failed to converge", GSL_EFAILED);
    }

  return GSL_SUCCESS;
}


#include <math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_sf_hyperg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>

#include <fastpm/libfastpm.h>

void
fastpm_horizon_init(FastPMHorizon * horizon, FastPMCosmology * cosmology)
{
    gsl_set_error_handler_off(); // Turn off GSL error handler

    horizon->cosmology = cosmology;
    horizon->size = 8192;
    horizon->da = 1.0 / (horizon->size - 1);
    int i;
    for (i = 0; i < horizon->size; i ++) {
        double a = 1.0 * i / (horizon->size - 1);
        horizon->xi_a[i] = HubbleDistance * ComovingDistance(a, horizon->cosmology);
        FastPMGrowthInfo gi;
        fastpm_growth_info_init(&gi, a, horizon->cosmology);
        horizon->growthfactor_a[i] = gi.D1;
    }
}

void
fastpm_horizon_destroy(FastPMHorizon * horizon)
{
}

double
HorizonDistance(double a, FastPMHorizon * horizon)
{
    double x = a * (horizon->size - 1);
    int l = floor(x);
    int r = l + 1;
    if(r >= horizon->size) {
        return horizon->xi_a[horizon->size - 1];
    }
    if(l <= 0) {
        return horizon->xi_a[0];
    }
    return horizon->xi_a[l] * (r - x)
         + horizon->xi_a[r] * (x - l);
}

double
HorizonGrowthFactor(double a, FastPMHorizon * horizon)
{
    double x = a * (horizon->size - 1);
    int l = floor(x);
    int r = l + 1;
    if(r >= horizon->size) {
        return horizon->growthfactor_a[horizon->size - 1];
    }
    if(l <= 0) {
        return horizon->growthfactor_a[0];
    }
    return horizon->growthfactor_a[l] * (r - x)
         + horizon->growthfactor_a[r] * (x - l);
}

void *
fastpm_horizon_solve_start()
{
    const gsl_root_fsolver_type *T = gsl_root_fsolver_brent;
    return gsl_root_fsolver_alloc(T);
}

void
fastpm_horizon_solve_end(void * context)
{
    gsl_root_fsolver_free(context);
}

int
fastpm_horizon_solve(FastPMHorizon * horizon,
    void * context,
    double * solution,
    double a_i, double a_f,
    double (*func)(double a, void * userdata),
    void * userdata)
{

    int status;
    int iter = 0, max_iter;
    double r, x_lo=a_i, x_hi=a_f, eps;

    /* Reorganize to struct later */
    max_iter = 20;
    eps = 1e-5;

    gsl_function F;

    F.function = func;
    F.params = userdata;

    status = gsl_root_fsolver_set(context, &F, x_lo, x_hi);

    if(status == GSL_EINVAL || status == GSL_EDOM) {
        /** Error in value or out of range **/
        return 0;
    }

    do
    {
        iter++;
        //
        // Debug printout #1
        //if(iter == 1) {
        //fastpm_info("ID | [x_lo, x_hi] | r | funct(r) | x_hi - x_lo\n");
        //}
        //

        status = gsl_root_fsolver_iterate(context);
        r = gsl_root_fsolver_root(context);

        x_lo = gsl_root_fsolver_x_lower(context);
        x_hi = gsl_root_fsolver_x_upper(context);

        status = gsl_root_test_interval(x_lo, x_hi, eps, 0.0);
        //
        //Debug printout #2
        //fastpm_info("%5d [%.7f, %.7f] %.7f %.7f %.7f\n", iter, x_lo, x_hi, r, funct(r, &params), x_hi - x_lo);
        //

        if(status == GSL_SUCCESS || iter == max_iter ) {
            *solution = r;
            //
            // Debug printout #3.1
            //fastpm_info("fastpm_lc_intersect() called with parameters %.7f and %.7f, returned status %d.\n\n", a, b, 1);
            //
            return 1;
        }
    }
    while (status == GSL_CONTINUE);
    //
    // Debug printout #3.2
    //fastpm_info("fastpm_lc_intersect() called with parameters %.7f and %.7f, returned status %d.\n\n", a, b, 0);
    //

    return 0;

}


//
//  trmm.h
//  Linear Algebra Template Library
//
//  Created by Rodney James on 1/4/12.
//  Copyright (c) 2012 University of Colorado Denver. All rights reserved.
//

#ifndef _trmm_h
#define _trmm_h

/// @file trmm.h Performs multiplication of a triangular matrix with a rectangular matrix.

#include <cctype>
#include "latl.h"

namespace LATL
{
   /// @brief Performs multiplication of a real triangular matrix with a real rectangular matrix.
   ///
   /// For a real upper or lower triangular matrix A, real rectangular matrix B and real scalar alpha,
   ///
   ///        B := alpha*A*B  or  B := alpha*A'*B  or  B := alpha*B*A  or  B := alpha*B*A'
   /// is computed.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param side Specifies whether the matrix A appears on the left or right side as follows:
   ///
   ///        if side = 'L' or 'l' then B := alpha*A*B or alpha*A'*B
   ///        if side = 'R' or 'r' then C := alpha*B*A or alpha*B*A'
   /// @param uplo Specifies whether A is stored as upper or lower triangular.
   ///
   ///        if uplo = 'U' or 'u' then A is upper triangular
   ///        if uplo = 'L' or 'l' then A is lower triangular
   /// @param trans Specifies wheather the transpose of A is to be used or not:
   ///
   ///        if trans = 'N' or 'n' then B := alpha*A*B or alpha*B*A
   ///        if trans = 'T' or 't' then B := alpha*A'*B or alpha*B*A'
   ///        if trans = 'C' or 'c' then B := alpha*A'*B or alpha*B*A'
   /// @param diag specifies whether or not A is unit triangular as follows:
   ///
   ///        if diag = 'U' or 'u' then A is assumed to be unit triangular
   ///        if diag = 'N' or 'n' then A is not assumed to be unit triangular.
   /// If A is unit triangular, the diagonal elements are assumed to be unity and are not referenced.
   /// @param m Specifies the number of rows of the matrix B.  m>=0
   /// @param n Specifies the number of columns of the matrix B.  n>=0
   /// @param alpha Real scalar.
   /// @param A Pointer to real triangular matrix A.  The order of A is n if side = 'L' or 'l';
   /// the order of A is m is side = 'R' or 'r'.
   /// If uplo = 'U' or 'u, A is upper triangular and the lower triangular part is not referenced.
   /// If uplo = 'L' or 'l A is lower triangular and the upper triangular part is not referenced.
   /// @param ldA Column length of the matrix A.  If side='L' or 'l' then ldA>=n; if side='R' or 'r' then lda>=m.
   /// @param B Pointer to real m-by-n matrix B.
   /// @param ldB Column length of the matrix B.  ldB>=m.
   /// @ingroup BLAS

   template <typename real_t>
   int TRMM(char side, char uplo, char trans, char diag, int_t m, int_t n, real_t alpha, real_t *A, int_t ldA, real_t *B, int_t ldB)
   {
      using std::toupper;

      const real_t zero(0.0);
      int_t i,j,k;
      real_t *a,*b,*bt;
      real_t t;

      side=toupper(side);
      uplo=toupper(uplo);
      trans=toupper(trans);
      diag=toupper(diag);

      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -3;
      else if((diag!='U')&&(diag!='N'))
         return -4;
      else if(m<0)
         return -5;
      else if(n<0)
         return -6;
      else if(ldA<((side=='L')?m:n))
         return -9;
      else if(ldB<m)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      bool nounit=(diag=='N')?1:0;

      if(alpha==zero)
      {
         b=B;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               b[i]=zero;
            b+=ldB;
         }
      }
      else if(side=='L')
      {
         if(trans=='N')
         {
            if(uplo=='U')
            {
               b=B;
               for(j=0;j<n;j++)
               {
                  a=A;
                  for(k=0;k<m;k++)
                  {
                     t=alpha*b[k];
                     for(i=0;i<k;i++)
                        b[i]+=t*a[i];
                     if(nounit)
                        t*=a[k];
                     b[k]=t;
                     a+=ldA;
                  }
                  b+=ldB;
               }
            }
            else
            {
               b=B;
               for(j=0;j<n;j++)
               {
                  a=A+m*ldA;
                  for(k=m-1;k>=0;k--)
                  {
                     a-=ldA;
                     t=alpha*b[k];
                     b[k]=t;
                     if(nounit)
                        b[k]*=a[k];
                     for(i=k+1;i<m;i++)
                        b[i]+=t*a[i];
                  }
                  b+=ldB;
               }
            }
         }
         else
         {
            if(uplo=='U')
            {
               b=B;
               for(j=0;j<n;j++)
               {
                  a=A+m*ldA;
                  for(i=m-1;i>=0;i--)
                  {
                     a-=ldA;
                     t=b[i];
                     if(nounit)
                        t*=a[i];
                     for(k=0;k<i;k++)
                        t+=a[k]*b[k];
                     b[i]=alpha*t;
                  }
                  b+=ldB;
               }
            }
            else
            {
               b=B;
               for(j=0;j<n;j++)
               {
                  a=A;
                  for(i=0;i<m;i++)
                  {
                     t=b[i];
                     if(nounit)
                        t*=a[i];
                     for(k=i+1;k<m;k++)
                        t+=a[k]*b[k];
                     b[i]=alpha*t;
                     a+=ldA;
                  }
                  b+=ldB;
               }
            }
         }
      }
      else
      {
         if(trans=='N')
         {
            if(uplo=='U')
            {
               a=A+n*ldA;
               b=B+n*ldB;
               for(j=n-1;j>=0;j--)
               {
                  a-=ldA;
                  b-=ldB;
                  if(nounit)
                     t=alpha*a[j];
                  else
                     t=alpha;
                  for(i=0;i<m;i++)
                     b[i]*=t;
                  bt=B;
                  for(k=0;k<j;k++)
                  {
                     t=alpha*a[k];
                     for(i=0;i<m;i++)
                        b[i]+=t*bt[i];
                     bt+=ldB;
                  }
               }
            }
            else
            {
               a=A;
               b=B;
               for(j=0;j<n;j++)
               {
                  if(nounit)
                     t=alpha*a[j];
                  else
                     t=alpha;
                  for(i=0;i<m;i++)
                     b[i]*=t;
                  bt=b+ldB;
                  for(k=j+1;k<n;k++)
                  {
                     t=alpha*a[k];
                     for(i=0;i<m;i++)
                        b[i]+=t*bt[i];
                     bt+=ldB;
                  }
                  a+=ldA;
                  b+=ldB;
               }
            }
         }
         else
         {
            if(uplo=='U')
            {
               a=A;
               bt=B;
               for(k=0;k<n;k++)
               {
                  b=B;
                  for(j=0;j<k;j++)
                  {
                     t=alpha*a[j];
                     for(i=0;i<m;i++)
                        b[i]+=t*bt[i];
                     b+=ldB;
                  }
                  if(nounit)
                     t=alpha*a[k];
                  else
                     t=alpha;
                  for(i=0;i<m;i++)
                     b[i]*=t;
                  a+=ldA;
                  bt+=ldB;
               }
            }
            else
            {
               a=A+n*ldA;
               bt=B+n*ldB;
               for(k=n-1;k>=0;k--)
               {
                  a-=ldA;
                  bt-=ldB;
                  b=B+(k+1)*ldB;
                  for(j=k+1;j<n;j++)
                  {
                     t=alpha*a[j];
                     for(i=0;i<m;i++)
                        b[i]+=t*bt[i];
                     b+=ldB;
                  }
                  if(nounit)
                     t=alpha*a[k];
                  else
                     t=alpha;
                  for(i=0;i<m;i++)
                     bt[i]*=t;
               }
            }
         }
      }
      return 0;
   }

   /// @brief Performs multiplication of a complex triangular matrix with a complex rectangular matrix.
   ///
   /// For a complex upper or lower triangular matrix A, complex rectangular matrix B and complex scalar alpha,
   ///
   ///        B:=alpha*A*B or B:=alpha*A'*B or B:=alpha*A.'*B or B:=alpha*B*A or B:=alpha*B*A' or B:=alpha*B*A.'
   /// is computed.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param side Specifies whether the matrix A appears on the left or right side as follows:
   ///
   ///        if side = 'L' or 'l' then B := alpha*A*B or alpha*A'*B or alpha*A.'*B
   ///        if side = 'R' or 'r' then C := alpha*B*A or alpha*B*A' or alpha*B*A.'
   /// @param uplo Specifies whether A is stored as upper or lower triangular.
   ///
   ///        if uplo = 'U' or 'u' then A is upper triangular
   ///        if uplo = 'L' or 'l' then A is lower triangular
   /// @param trans Specifies wheather the transpose or conjugate transpose is applied to A as follows:
   ///
   ///        if trans = 'N' or 'n' then B := alpha*A*B or alpha*B*A
   ///        if trans = 'T' or 't' then B := alpha*A.'*B or alpha*B*A.'
   ///        if trans = 'C' or 'c' then B := alpha*A'*B or alpha*B*A'
   /// @param diag specifies whether or not A is unit triangular as follows:
   ///
   ///        if diag = 'U' or 'u' then A is assumed to be unit triangular
   ///        if diag = 'N' or 'n' then A is not assumed to be unit triangular.
   /// If A is unit triangular, the diagonal elements are assumed to be unity and are not referenced.
   /// @param m Specifies the number of rows of the matrix B.  m>=0
   /// @param n Specifies the number of columns of the matrix B.  n>=0
   /// @param alpha Complex scalar.
   /// @param A Pointer to complex triangular matrix A.  The order of A is n if side = 'L' or 'l';
   /// the order of A is m is side = 'R' or 'r'.
   /// If uplo = 'U' or 'u, A is upper triangular and the lower triangular part is not referenced.
   /// If uplo = 'L' or 'l A is lower triangular and the upper triangular part is not referenced.
   /// @param ldA Column length of the matrix A.  If side='L' or 'l' then ldA>=n; if side='R' or 'r' then lda>=m.
   /// @param B Pointer to complex m-by-n matrix B.
   /// @param ldB Column length of the matrix B.  ldB>=m.
   /// @ingroup BLAS

   template <typename real_t>
   int TRMM(char side, char uplo, char trans, char diag, int_t m, int_t n, complex<real_t> alpha, complex<real_t> *A, int_t ldA, complex<real_t> *B, int_t ldB)
   {
      using std::conj;
      using std::toupper;

      const complex<real_t> zero(0.0,0.0);
      int_t i,j,k;
      complex<real_t> *a,*b,*bt;
      complex<real_t> t;

      side=toupper(side);
      uplo=toupper(uplo);
      trans=toupper(trans);
      diag=toupper(diag);

      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -3;
      else if((diag!='U')&&(diag!='N'))
         return -4;
      else if(m<0)
         return -5;
      else if(n<0)
         return -6;
      else if(ldA<((side=='L')?m:n))
         return -9;
      else if(ldB<m)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      bool nounit=(diag=='N')?1:0;

      if(alpha==zero)
      {
         b=B;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               b[i]=zero;
            b+=ldB;
         }
      }
      else if(side=='L')
      {
         if(trans=='N')
         {
            if(uplo=='U')
            {
               b=B;
               for(j=0;j<n;j++)
               {
                  a=A;
                  for(k=0;k<m;k++)
                  {
                     t=alpha*b[k];
                     for(i=0;i<k;i++)
                        b[i]+=t*a[i];
                     if(nounit)
                        t*=a[k];
                     b[k]=t;
                     a+=ldA;
                  }
                  b+=ldB;
               }
            }
            else
            {
               b=B;
               for(j=0;j<n;j++)
               {
                  a=A+m*ldA;
                  for(k=m-1;k>=0;k--)
                  {
                     a-=ldA;
                     t=alpha*b[k];
                     b[k]=t;
                     if(nounit)
                        b[k]*=a[k];
                     for(i=k+1;i<m;i++)
                        b[i]+=t*a[i];
                  }
                  b+=ldB;
               }
            }
         }
         else if(trans=='T')
         {
            if(uplo=='U')
            {
               b=B;
               for(j=0;j<n;j++)
               {
                  a=A+m*ldA;
                  for(i=m-1;i>=0;i--)
                  {
                     a-=ldA;
                     t=b[i];
                     if(nounit)
                        t*=a[i];
                     for(k=0;k<i;k++)
                        t+=a[k]*b[k];
                     b[i]=alpha*t;
                  }
                  b+=ldB;
               }
            }
            else
            {
               b=B;
               for(j=0;j<n;j++)
               {
                  a=A;
                  for(i=0;i<m;i++)
                  {
                     t=b[i];
                     if(nounit)
                        t*=a[i];
                     for(k=i+1;k<m;k++)
                        t+=a[k]*b[k];
                     b[i]=alpha*t;
                     a+=ldA;
                  }
                  b+=ldB;
               }
            }
         }
         else if(trans=='C')
         {
            if(uplo=='U')
            {
               b=B;
               for(j=0;j<n;j++)
               {
                  a=A+m*ldA;
                  for(i=m-1;i>=0;i--)
                  {
                     a-=ldA;
                     t=b[i];
                     if(nounit)
                        t*=conj(a[i]);
                     for(k=0;k<i;k++)
                        t+=conj(a[k])*b[k];
                     b[i]=alpha*t;
                  }
                  b+=ldB;
               }
            }
            else
            {
               b=B;
               for(j=0;j<n;j++)
               {
                  a=A;
                  for(i=0;i<m;i++)
                  {
                     t=b[i];
                     if(nounit)
                        t*=conj(a[i]);
                     for(k=i+1;k<m;k++)
                        t+=conj(a[k])*b[k];
                     b[i]=alpha*t;
                     a+=ldA;
                  }
                  b+=ldB;
               }
            }
         }
      }
      else
      {
         if(trans=='N')
         {
            if(uplo=='U')
            {
               a=A+n*ldA;
               b=B+n*ldB;
               for(j=n-1;j>=0;j--)
               {
                  a-=ldA;
                  b-=ldB;
                  if(nounit)
                     t=alpha*a[j];
                  else
                     t=alpha;
                  for(i=0;i<m;i++)
                     b[i]*=t;
                  bt=B;
                  for(k=0;k<j;k++)
                  {
                     t=alpha*a[k];
                     for(i=0;i<m;i++)
                        b[i]+=t*bt[i];
                     bt+=ldB;
                  }
               }
            }
            else
            {
               a=A;
               b=B;
               for(j=0;j<n;j++)
               {
                  if(nounit)
                     t=alpha*a[j];
                  else
                     t=alpha;
                  for(i=0;i<m;i++)
                     b[i]*=t;
                  bt=b+ldB;
                  for(k=j+1;k<n;k++)
                  {
                     t=alpha*a[k];
                     for(i=0;i<m;i++)
                        b[i]+=t*bt[i];
                     bt+=ldB;
                  }
                  a+=ldA;
                  b+=ldB;
               }
            }
         }
         else if(trans=='T')
         {
            if(uplo=='U')
            {
               a=A;
               bt=B;
               for(k=0;k<n;k++)
               {
                  b=B;
                  for(j=0;j<k;j++)
                  {
                     t=alpha*a[j];
                     for(i=0;i<m;i++)
                        b[i]+=t*bt[i];
                     b+=ldB;
                  }
                  if(nounit)
                     t=alpha*a[k];
                  else
                     t=alpha;
                  for(i=0;i<m;i++)
                     b[i]*=t;
                  a+=ldA;
                  bt+=ldB;
               }
            }
            else
            {
               a=A+n*ldA;
               bt=B+n*ldB;
               for(k=n-1;k>=0;k--)
               {
                  a-=ldA;
                  bt-=ldB;
                  b=B+(k+1)*ldB;
                  for(j=k+1;j<n;j++)
                  {
                     t=alpha*a[j];
                     for(i=0;i<m;i++)
                        b[i]+=t*bt[i];
                     b+=ldB;
                  }
                  if(nounit)
                     t=alpha*a[k];
                  else
                     t=alpha;
                  for(i=0;i<m;i++)
                     bt[i]*=t;
               }
            }
         }
         else if(trans=='C')
         {
            if(uplo=='U')
            {
               a=A;
               bt=B;
               for(k=0;k<n;k++)
               {
                  b=B;
                  for(j=0;j<k;j++)
                  {
                     t=alpha*conj(a[j]);
                     for(i=0;i<m;i++)
                        b[i]+=t*bt[i];
                     b+=ldB;
                  }
                  if(nounit)
                     t=alpha*conj(a[k]);
                  else
                     t=alpha;
                  for(i=0;i<m;i++)
                     b[i]*=t;
                  a+=ldA;
                  bt+=ldB;
               }
            }
            else
            {
               a=A+n*ldA;
               bt=B+n*ldB;
               for(k=n-1;k>=0;k--)
               {
                  a-=ldA;
                  bt-=ldB;
                  b=B+(k+1)*ldB;
                  for(j=k+1;j<n;j++)
                  {
                     t=alpha*conj(a[j]);
                     for(i=0;i<m;i++)
                        b[i]+=t*bt[i];
                     b+=ldB;
                  }
                  if(nounit)
                     t=alpha*conj(a[k]);
                  else
                     t=alpha;
                  for(i=0;i<m;i++)
                     bt[i]*=t;
               }
            }
         }
      }
      return 0;
   }

#ifdef __latl_cblas
#include <cblas.h>

   template <> int TRMM<float>(char side, char uplo, char trans, char diag, int_t m, int_t n, float alpha, float *A, int_t ldA, float *B, int_t ldB)
   {
      using std::toupper;
      side=toupper(side);
      uplo=toupper(uplo);
      trans=toupper(trans);
      diag=toupper(diag);

      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -3;
      else if((diag!='U')&&(diag!='N'))
         return -4;
      else if(m<0)
         return -5;
      else if(n<0)
         return -6;
      else if(ldA<((side=='L')?m:n))
         return -9;
      else if(ldB<m)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      const CBLAS_SIDE Side=(side=='L')?CblasLeft:CblasRight;
      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_DIAG Diag=(diag=='N')?CblasNonUnit:CblasUnit;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:CblasTrans;

      cblas_strmm(CblasColMajor,Side,Uplo,Trans,Diag,m,n,alpha,A,ldA,B,ldB);

      return 0;
   }

   template <> int TRMM<double>(char side, char uplo, char trans, char diag, int_t m, int_t n, double alpha, double *A, int_t ldA, double *B, int_t ldB)
   {
      using std::toupper;
      side=toupper(side);
      uplo=toupper(uplo);
      trans=toupper(trans);
      diag=toupper(diag);

      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -3;
      else if((diag!='U')&&(diag!='N'))
         return -4;
      else if(m<0)
         return -5;
      else if(n<0)
         return -6;
      else if(ldA<((side=='L')?m:n))
         return -9;
      else if(ldB<m)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      const CBLAS_SIDE Side=(side=='L')?CblasLeft:CblasRight;
      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_DIAG Diag=(diag=='N')?CblasNonUnit:CblasUnit;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:CblasTrans;

      cblas_dtrmm(CblasColMajor,Side,Uplo,Trans,Diag,m,n,alpha,A,ldA,B,ldB);

      return 0;
   }
   template <> int TRMM<float>(char side, char uplo, char trans, char diag, int_t m, int_t n, complex<float> alpha, complex<float> *A, int_t ldA, complex<float> *B, int_t ldB)
   {
      using std::toupper;
      side=toupper(side);
      uplo=toupper(uplo);
      trans=toupper(trans);
      diag=toupper(diag);

      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -3;
      else if((diag!='U')&&(diag!='N'))
         return -4;
      else if(m<0)
         return -5;
      else if(n<0)
         return -6;
      else if(ldA<((side=='L')?m:n))
         return -9;
      else if(ldB<m)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      const CBLAS_SIDE Side=(side=='L')?CblasLeft:CblasRight;
      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_DIAG Diag=(diag=='N')?CblasNonUnit:CblasUnit;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_ctrmm(CblasColMajor,Side,Uplo,Trans,Diag,m,n,&alpha,A,ldA,B,ldB);

      return 0;
   }
   template <> int TRMM<double>(char side, char uplo, char trans, char diag, int_t m, int_t n, complex<double> alpha, complex<double> *A, int_t ldA, complex<double> *B, int_t ldB)
   {
      using std::toupper;
      side=toupper(side);
      uplo=toupper(uplo);
      trans=toupper(trans);
      diag=toupper(diag);

      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -3;
      else if((diag!='U')&&(diag!='N'))
         return -4;
      else if(m<0)
         return -5;
      else if(n<0)
         return -6;
      else if(ldA<((side=='L')?m:n))
         return -9;
      else if(ldB<m)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      const CBLAS_SIDE Side=(side=='L')?CblasLeft:CblasRight;
      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_DIAG Diag=(diag=='N')?CblasNonUnit:CblasUnit;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_ztrmm(CblasColMajor,Side,Uplo,Trans,Diag,m,n,&alpha,A,ldA,B,ldB);

      return 0;
   }
#endif

}
#endif


//==============================================
//---------- by Dmitry R. Gulevich -------------
//--------- drgulevich@corp.ifmo.ru ------------
//--- ITMO University, St Petersburg, Russia ---
//==============================================
// Translation to C++ and augmentation
// S. R. Whiteley, wrcad.com,  Synopsys, Inc
//==============================================
//
// License:  GNU General Public License Version 3, 29 June 2007.

#ifndef MMJCO_H
#define MMJCO_H

#include <stdio.h>
#include <string.h>
#include <math.h>
#include <complex>
#include <cmath>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>

// Uncomment this to use the cminpack library for the least-squares
// fitting, otherwise we will use the cmpfit library provided.
//#define WITH_CMINPACK

#define MM_ECHG 1.602176634e-19     // Electron charge (SI units).
#define MM_BOLTZ 1.3806226e-23      // Boltzmann constant (SI units).

using namespace std;

class mmjco
{
public:
    // Constructor/destructor.
    mmjco(double, double, double, double);
    ~mmjco();

    // Functions return values for arbitrary x (values x<0 obtained by
    // symmetry), not smoothed.

    complex<double> Jpair(double x)
        {
            double absx = maximum(fabs(x), 1e-5);
            return (Repair(absx) + sign(x)*1j*Impair(absx));
        }

    complex<double> Jqp(double x)
        {
            double absx = maximum(fabs(x), 1e-5);
            return (Reqp(absx) + sign(x)*1j*Imqp(absx));
        }

    // As above, but smoothed.

    complex<double> Jpair_smooth(double x)
        {
            return (Jpair(x) + Jpair_correction(x));
        }

    complex<double> Jqp_smooth(double x)
        {
            return (Jqp(x) + Jqp_correction(x));
        }

    // Utilities.

    static double sign(double x)
        {
            if (x > 0.0)
                return (1.0);
            if (x == 0.0)
                return (0.0);
            return (-1.0);
        }

    static double minimum(double x, double y) { return (x < y ? x : y); }
    static double maximum(double x, double y) { return (x > y ? x : y); }

    static void save_data(const char*, const double*, const complex<double>*,
        const complex<double>*, int);

    static const char *version()    { return (mm_version); }

private:
    double Repair(double);
    double Impair(double);
    double Reqp(double);
    double Imqp(double);

    double Repair_integrand_part1(double y, double x)
        {
            return (tanh(mm_b*fabs(y))/
                ( sqrt(mm_dd1-(y-x)*(y-x)) * sqrt(y*y-mm_dd2) ));
        }

    double Repair_integrand_part2(double y, double x)
        {
            return (tanh(mm_b*fabs(y))/
                ( sqrt(y*y-mm_dd1) * sqrt(mm_dd2-(y+x)*(y+x)) ));
        }

    double Impair_integrand(double y, double x)
        {
            return (( tanh(mm_b*(y+x)) - tanh(mm_b*y) )*sign(y)*sign(y+x)/(
                sqrt(y*y-mm_dd1) * sqrt((y+x)*(y+x)-mm_dd2) ));
        }

    double Reqp_integrand_part1(double y, double x)
        {
            return (fabs(y)*tanh(mm_b*y)*(y-x)/( sqrt(y*y-mm_dd1) *
                sqrt(mm_dd2-(y-x)*(y-x)) ));
        }

    double Reqp_integrand_part1b_d1_xd2(double y, double x)
        {
            return (fabs(y)*tanh(mm_b*y)*(y-x)/sqrt((y+mm_d1)*(mm_d2+y-x)));
        }

    double Reqp_integrand_part1b_xd2_xd2(double y, double x)
        {
            return (fabs(y)*tanh(mm_b*y)*(y-x)/sqrt(y*y-mm_dd1));
        }

    double Reqp_integrand_part2_xd1_d2(double y, double x)
        {
            return (fabs(y)*tanh(mm_b*y)*(y+x)/sqrt((mm_d1-y-x)*(mm_d2-y)));
        }

    double Reqp_integrand_part2_xd1_xd1(double y, double x)
        {
            return (fabs(y)*tanh(mm_b*y)*(y+x)/sqrt(y*y-mm_dd2));
        }

    double Imqp_integrand(double y, double x)
        {
            return (( tanh(mm_b*(y+x))-tanh(mm_b*y) )*fabs(y)*fabs(y+x)/
                ( sqrt((y+x)*(y+x)-mm_dd1) * sqrt(y*y-mm_dd2) ));
        }

    // Passed to integration functions.

    static double re_p_i1(double x, void *ptr)
        {
            mmjco *m = (mmjco*)ptr;
            return (m->Repair_integrand_part1(x, m->mm_arg));
        }

    static double re_p_i2(double x, void *ptr)
        {
            mmjco *m = (mmjco*)ptr;
            return (m->Repair_integrand_part2(x, m->mm_arg));
        }

    static double im_p_i(double x, void *ptr)
        {
            mmjco *m = (mmjco*)ptr;
            return (m->Impair_integrand(x, m->mm_arg));
        }

    static double re_q_i1(double x, void *ptr)
        {
            mmjco *m = (mmjco*)ptr;
            return (m->Reqp_integrand_part1(x, m->mm_arg));
        }

    static double re_q_i1b1x2(double x, void *ptr)
        {
            mmjco *m = (mmjco*)ptr;
            return (m->Reqp_integrand_part1b_d1_xd2(x, m->mm_arg));
        }

    static double re_q_i1bx2x2(double x, void *ptr)
        {
            mmjco *m = (mmjco*)ptr;
            return (m->Reqp_integrand_part1b_xd2_xd2(x, m->mm_arg));
        }

    static double re_q_i2x12(double x, void *ptr)
        {
            mmjco *m = (mmjco*)ptr;
            return (m->Reqp_integrand_part2_xd1_d2(x, m->mm_arg));
        }

    static double re_q_i2x1x1(double x, void *ptr)
        {
            mmjco *m = (mmjco*)ptr;
            return (m->Reqp_integrand_part2_xd1_xd1(x, m->mm_arg));
        }

    static double im_q_i(double x, void *ptr)
        {
            mmjco *m = (mmjco*)ptr;
            return (m->Imqp_integrand(x, m->mm_arg));
        }

    // Smoothing.

    complex<double> Jpair_correction(double x)
        {
            if (mm_symj) {
                double absx = maximum(fabs(x),1.e-5);
                return (dRe(absx) + sign(x)*1j*(dIm(absx) + dIm_at_0(absx)));
            }
            else {
                double absx = maximum(fabs(x),1.e-5);
                return (dRe(absx) + dRe_minus(absx) + sign(x)*1j*(dIm(absx) +
                    dIm_minus(absx)));
            }
        }

    complex<double> Jqp_correction(double x)
        {

            if (mm_symj) {
                double absx = maximum(fabs(x),1.e-5);
                return (dRe(absx) + sign(x)*1j*(-dIm(absx) + dIm_at_0(absx)));
            }
            else {
                double absx = maximum(fabs(x),1.e-5);
                return (dRe(absx) - dRe_minus(absx) + sign(x)*1j*(-dIm(absx) +
                    dIm_minus(absx)));
            }
        }

    // Smoothing for Repair, Reqp.
    double dRe(double x)
        {
            double sqpos = (x-1.0)*(x-1.0);
            double sqneg = (x+1.0)*(x+1.0);
            return (-x*(1.0/M_PI)*mm_ip0*0.5*log(
                ((sqpos+mm_dsm*mm_dsm)/sqpos)*(sqneg/(sqneg+mm_dsm*mm_dsm)) ));
        }

    // Smoothing for Repair, -Reqp at x=mm_d2-mm_d1.
    double dRe_minus(double x)
        {
            return (M_PI*x*sqrt(mm_d1*mm_d2)*
                (tanh(mm_b*mm_d2)-tanh(mm_b*mm_d1)) *
                0.5*( (2.0/M_PI)*atan((x-mm_d21)/mm_dsm)
                - sign(x-mm_d21) + (2.0/M_PI)*atan((x+mm_d21)/mm_dsm) -
                sign(x+mm_d21) ) / (4.0*mm_d21));
        }

    // Smoothing for Impair, Imqp at x=mm_d2-mm_d1.
    double dIm_minus(double x)
        {
            double square1 = (x-mm_d21)*(x-mm_d21);
            double square2 = (x+mm_d21)*(x+mm_d21);
            return (-x*sqrt(mm_d1*mm_d2)*(tanh(mm_b*mm_d2)-tanh(mm_b*mm_d1))*
                0.5*log((square1+mm_dsm*mm_dsm)*(square2+mm_dsm*mm_dsm)/
                (square1*square2)) / (4.0*mm_d21));
        }

    // Smoothing for Impair, -Imqp.
    double dIm(double x)
        {
            return (x*0.5*mm_ip0*((2.0/M_PI)*atan((1.0-x)/mm_dsm) -
                sign(1.0-x) + (2.0/M_PI)*atan((1.0+x)/mm_dsm) - sign(1.0+x)));
        }

    // Smoothing for Impair, Imqp at mm_d2-mm_d1=0.
    double dIm_at_0(double x)
        {
            double x2=x*x;
            return (-mm_b*x*mm_expb*0.5*log(
                (x2+mm_dsm*mm_dsm)/x2)/((mm_expb+1.0)*(mm_expb+1.0)));
        }

    static const char *mm_version;

    double mm_d1, mm_d2;
    double mm_d21;
    double mm_dd1, mm_dd2;
    double mm_b;
    double mm_arg;
    int mm_limit;
    int mm_key;
    double mm_epsabs;
    double mm_epsrel;
    gsl_integration_workspace *mm_ws;
    gsl_integration_qaws_table *mm_tbl;

    // Smoothing.
    double mm_expb;
    double mm_ip0;
    double mm_dsm;
    bool mm_symj;;
};

//
// Class to compute optimized TCA fitting parameters.
//

// Largest acceptable fitting table.
#define MAX_NTERMS 20

class mmjco_fit
{
public:
    struct mm_adata
    {
        const double *x;
        double thr;
        const complex<double> *Jpair_data;
        const complex<double> *Jqp_data;
    };

    mmjco_fit()
        {
            mmf_pAB = 0;
            mmf_nterms = 0;
        }

    ~mmjco_fit()
        {
            delete [] mmf_pAB;
        }

    void new_fit_parameters(const double*, const complex<double>*,
        const complex<double>*, int, int, double);
    void save_fit_parameters(const char*);
    void load_fit_parameters(const char*);
    void tca_fit(const double*, int, complex<double>**, complex<double>**);
    double residual(const complex<double>*, const complex<double>*,
        const complex<double>*, const complex<double>*, int, double);

    const complex<double> *params()     const { return (mmf_pAB); }
    int numterms()                      const { return (mmf_nterms); }

private:
    static complex<double> *modelJpair(const complex<double>*, int,
        const double*, int);
    static complex<double> *modelJqp(const complex<double>*, int,
        const double*, int);

    static double rep(double zeta)     { return (-fabs(zeta)); }
    static double invrep(double xi)    { return (-xi); }

    // Relative difference with threshold thr.
    static double Drel(double X, double Xref, double thr)
        {
            double axref = fabs(Xref);
            if (thr > axref)
                axref = thr;
            return (fabs(X-Xref)/axref);
        }

    // Flatten complex array.
    static double *realimag(complex<double> *carray, int sz)
        {
            double *param_list = new double[2*sz];
            for (int i = 0; i < sz; i++) {
                param_list[i*2] = carray[i].real();
                param_list[i*2+1] = carray[i].imag();
            }
            return (param_list);
        }

    // Form complex array.
    static complex<double> *ccombine(const double *param_list, int plsz)
        {
            complex<double> *cpars = new complex<double>[plsz/2];
            for (int i = 0; i < plsz; i++) {
                int n = i/2;
                if (i & 1)
                    cpars[n].imag(param_list[i]);
                else
                    cpars[n].real(param_list[i]);
            }
            return (cpars);
        }

#ifdef WITH_CMINPACK
    static int func(void*, int, int, const double*, double*, int);
#else
    static int func(int, int, double*, double*, double**, void*);
#endif

    complex<double> *mmf_pAB;
    int mmf_nterms;
};

#endif // MMJCO_H


/* Implementation for next reaction method */

#include <math.h>
#include <stdint.h>
#include <time.h>
#include <stdio.h>
#include <stdint.h>
#include <float.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#include "ssa.h"
#include "nrm.h"
#include "priorityq.h"
#include "depgraph.h"

const gsl_rng_type *SSARNGT;
gsl_rng *SSARNG;

void update_reaction(REACTION *r, SYSTEM *s, double t)
{
	double newp = ssa_h(s->R[r->number], s->x, s->n) * s->k[r->number];
	double oldp = s->propensities[r->number];

	if (newp > 0.0) {
		if (oldp > 0.0) {
			pq_update(s->pq, r, (oldp / newp) * (r->tau - t) + t);
		} else {
			r->tau = gsl_ran_gamma(SSARNG, s->steps[r->number], 1 / newp) + t;
			pq_insert(s->pq, r);
		}
	} else if (oldp > 0.0) {
		pq_delete(s->pq, r);
	}

	s->propensities[r->number] = newp;
}

double ssa_nrmstep(SYSTEM *s)
{
	REACTION *r = pq_min(s->pq);
        ssa_doreaction(s->R[r->number], s->P[r->number], s->x, s->n);
	double t = r->tau;

	s->propensities[r->number] = ssa_h(s->R[r->number], s->x, s->n) * s->k[r->number];
	if (s->propensities[r->number] > 0.0)
		pq_update(s->pq, r, gsl_ran_gamma(SSARNG, s->steps[r->number], 1 / s->propensities[r->number]) + t);
	else
		pq_delete(s->pq, r);

	LLIST *head = r->affects;
	while (head != NULL) {
		REACTION *update = (REACTION *) head->data;
		update_reaction(update, s, t);
		head = head->next;
	}

	return t;
}

REACTION **setup_reactions(SYSTEM *s)
{
	REACTION **reactions = malloc(sizeof(REACTION*) * s->m);
	char **adjmat = adjacencymatrix(s->R, s->P, s->n, s->m);

	INDEX i, j;
	for (i = 0; i < s->m; i++) {
		REACTION *reaction = reaction_make(i);
		reactions[i] = reaction;
	}

	for (i = 0; i < s->m; i++) {
		REACTION *r = reactions[i];
		for (j = 0; j < s->m; j++)
			if (i != j && adjmat[i][j] != 0)
				r->affects = llist_push(r->affects, reactions[j]);
	}

        for (i = 0; i < s->m; i++) {
		REACTION *r = reactions[i];
		for (j = 0; j < s->m; j++) {
			if (s->creates[i][j] != 0)
				r->creates = llist_push(r->creates, reactions[j]);
                        if (s->destroys[i][j] != 0)
                                r->destroys = llist_push(r->destroys, reactions[j]);
                }
	}

	for (i = 0; i < s->m; i++)
		free(adjmat[i]);

	free(adjmat);
	return reactions;
}

void ssa_nrm(INDEX **R, INDEX **P, INDEX n, INDEX m, double *k, COUNT *x, COUNT *steps, INDEX **creates, INDEX **destroys, double T)
{
	gsl_rng_env_setup();
	SSARNGT = gsl_rng_default;
	SSARNG = gsl_rng_alloc(SSARNGT);
	gsl_rng_set(SSARNG, time(NULL));

        SYSTEM *s = malloc(sizeof(SYSTEM));
	s->m = m;
        s->n = n;
        s->R = R;
        s->P = P;
        s->creates = creates;
        s->destroys = destroys;

	REACTION **reactions = setup_reactions(s);
	double *propensities = malloc(sizeof(double) * m);
	PQ *pq = pq_make();

        s->pq = pq;
        s->x = x;
        s->steps = steps;
        s->k = k;
        s->propensities = propensities;

	INDEX i;
	for (i = 0; i < m; i++) {
		REACTION *r = reactions[i];
		propensities[i] = ssa_h(R[i], x, n) * k[i];
		if (propensities[i] > 0.0) {
			r->tau = gsl_ran_gamma(SSARNG, steps[i], 1 / propensities[i]);
			pq_insert(pq, r);
		}
	}

	ssa_printstate(0.0, x, n);

	while(!pq_isempty(pq) && pq_min(pq)->tau < T)
		ssa_printstate(ssa_nrmstep(s), x, n);

	pq_free(pq);
	gsl_rng_free(SSARNG);
}


/**
  * Copyright 2019 José Manuel Abuín Mosquera <josemanuel.abuin@usc.es>
  *
  * This file is part of Matrix Market Suite.
  *
  * Matrix Market Suite 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.
  *
  * Matrix Market Suite 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 Matrix Market Suite. If not, see <http://www.gnu.org/licenses/>.
  */

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

#include "ConjugateGradient.h"
#include "ConjugateGradientSolver.h"
#include "ConjugateGradientSolverBasic.h"

/* Include redundant execution header. */
#include "../../../include/ourRMTlib.h"

void usageConjugateGradient(){

	fprintf(stderr, "\n");
	fprintf(stderr, "Usage: MM-Suite ConjugateGradient [options] <input-matrix> <input-vector>\n");
	fprintf(stderr, "Algorithm options:\n\n");
	fprintf(stderr, "       -i INT        Iteration number. Default: number of matrix rows * 2\n");
	fprintf(stderr, "       -b            Uses basic operations instead of optimized BLAS libraries. Default: False\n");
	fprintf(stderr, "\nInput/output options:\n\n");
	fprintf(stderr, "       -o STR        Output file name. Default: stdout\n");
	fprintf(stderr, "       -r            Input format is row per line. Default: False\n");
	fprintf(stderr, "\nPerformance options:\n\n");
	fprintf(stderr, "       -t INT        Number of threads to use in OpenBLAS. Default: 1\n");
	fprintf(stderr, "\n");

}

//int ConjugateGradient(int argc, char *argv[]) {
int main(int argc, char *argv[]) {

	int 			ret_code;
	int 			option;

	unsigned long 		*II;
	unsigned long 		*J;
	double 			*A;

	unsigned long 		M;
	unsigned long 		N;
	unsigned long long 	nz;

	double 			*b;
	unsigned long 		M_Vector;
	unsigned long 		N_Vector;
	unsigned long long 	nz_vector;

	char			*outputFileName = NULL;
	int			iterationNumber = 0;

	char			*inputMatrixFile = NULL;
	char			*inputVectorFile = NULL;

	int			numThreads = 1;

	int			inputFormatRow = 0;
	int			basicOps = 0;
	int			F=0;

	while ((option = getopt(argc, argv,"bro:i:t:f:")) >= 0) {
		switch (option) {
			case 'o' :
				//free(outputFileName);

				outputFileName = (char *) malloc(sizeof(char)*strlen(optarg)+1);
				strcpy(outputFileName,optarg);

				break;

			case 'i' :
				iterationNumber = atoi(optarg);
				break;

			case 'r':
				inputFormatRow = 1;
				break;
			case 'b':
				basicOps = 1;
				break;

			case 't':
				numThreads = atoi(optarg);
				break;

			case 'f':
				F = atoi(optarg);
				break;

			default: break;
		}

	}

	if ((optind + 2 > argc) || (optind + 3 <= argc)) {
		usageConjugateGradient();
		return -1;
	}

	//openblas_set_num_threads(numThreads);

	if(outputFileName == NULL) {
		outputFileName = (char *) malloc(sizeof(char)*7);
		sprintf(outputFileName,"stdout");
	}

	start_timer();

	inputMatrixFile = (char *)malloc(sizeof(char)*strlen(argv[optind])+1);
	inputVectorFile = (char *)malloc(sizeof(char)*strlen(argv[optind+1])+1);

	strcpy(inputMatrixFile,argv[optind]);
	strcpy(inputVectorFile,argv[optind+1]);

	//Read matrix

	//Read matrix
	if(inputFormatRow){

		if(!readDenseCoordinateMatrixRowLine(inputMatrixFile,&II,&J,&A,&M,&N,&nz)){
			fprintf(stderr, "[%s] Can not read Matrix\n",__func__);
			return -1;
		}

		//writeDenseVector("stdout",values,M,N,nz);

	}
	else {
		if(!readDenseCoordinateMatrix(inputMatrixFile,&II,&J,&A,&M,&N,&nz)){
			fprintf(stderr, "[%s] Can not read Matrix\n",__func__);
			return -1;
		}
	}

	//Read vector
	if(!readDenseVector(inputVectorFile, &b,&M_Vector,&N_Vector,&nz_vector)){
		fprintf(stderr, "[%s] Can not read Vector\n",__func__);
		return -1;
	}

    //double *y=(double *) malloc(nz_vector * sizeof(double));
    fprintf(stderr,"[%s] Solving system using conjugate gradient method\n",__func__);
    double t_real = realtime();

	// Vector to store result
	double *x=(double *) calloc(nz_vector,sizeof(double));

	if(basicOps){
		ret_code = ConjugateGradientSolverBasic(II,J,A,M,N,nz,b,M_Vector,N_Vector,nz_vector, x, iterationNumber);
	}
	else{
		# ifdef ENABLE_RMT
			  activateRMT("f-L-lp-lp-2d-l-l-l-1dC-l-l-l-1dC-i-i-ipC", &ConjugateGradientSolver, 14, II, J, A, M, N, M, N, nz , b, N, M_Vector, N_Vector,
																	  nz_vector, x, nz_vector, iterationNumber, F, &ret_code);

		# else
			ret_code = ConjugateGradientSolver(II,J,A,M,N,nz,b,M_Vector,N_Vector,nz_vector, x, iterationNumber, F, &ret_code);
		# endif

	}

	fprintf(stderr, "\n[%s] Time spent in Conjugate Gradient: %.6f sec\n", __func__, realtime() - t_real);

	if(ret_code){

		writeDenseVector(outputFileName, x,M_Vector,N_Vector,nz_vector);

	}
	else{
		fprintf(stderr,"[%s] Error executing ConjugateGradientSolver\n",__func__);
		return -1;

	}

	end_timer();

	return 0;
}


/* ode-initval/test_odeiv.c
 *
 * Copyright (C) 2004 Tuomo Keskitalo
 *
 * 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.
 */

/* Some functions and tests based on test.c by G. Jungman.
*/

#include <config.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_odeiv.h>
#include "odeiv_util.h"

/* Maximum number of ODE equations */
#define MAXEQ 4

/* RHS for f=2. Solution y = 2 * t + t0 */

int
rhs_linear (double t, const double y[], double f[], void *params)
{
  f[0] = 2.0;

  return GSL_SUCCESS;
}

int
jac_linear (double t, const double y[], double *dfdy, double dfdt[],
            void *params)
{
  dfdy[0] = 0.0;
  dfdt[0] = 0.0;

  return GSL_SUCCESS;
}

gsl_odeiv_system rhs_func_lin = {
  rhs_linear,
  jac_linear,
  1,
  0
};

/* RHS for f=y. Equals y=exp(t) with initial value y(0)=1.0 */

int
rhs_exp (double t, const double y[], double f[], void *params)
{
  f[0] = y[0];

  return GSL_SUCCESS;
}

int
jac_exp (double t, const double y[], double *dfdy, double dfdt[],
         void *params)
{
  dfdy[0] = y[0];
  dfdt[0] = 0.0;

  return GSL_SUCCESS;
}

gsl_odeiv_system rhs_func_exp = {
  rhs_exp,
  jac_exp,
  1,
  0
};

/* RHS for f0 = -y1, f1 = y0
   equals y = [cos(t), sin(t)] with initial values [1, 0]
*/

int
rhs_sin (double t, const double y[], double f[], void *params)
{
  f[0] = -y[1];
  f[1] = y[0];

  return GSL_SUCCESS;
}

int
jac_sin (double t, const double y[], double *dfdy, double dfdt[],
         void *params)
{
  dfdy[0] = 0.0;
  dfdy[1] = -1.0;
  dfdy[2] = 1.0;
  dfdy[3] = 0.0;

  dfdt[0] = 0.0;
  dfdt[1] = 0.0;

  return GSL_SUCCESS;
}

gsl_odeiv_system rhs_func_sin = {
  rhs_sin,
  jac_sin,
  2,
  0
};

/*  Sine/cosine with random failures */

int
rhs_xsin (double t, const double y[], double f[], void *params)
{
  static int n = 0;

  n++;

  if (n > 40 && n < 65) {
    f[0] = GSL_NAN;
    f[1] = GSL_NAN;
    return GSL_EFAILED;
  }

  f[0] = -y[1];
  f[1] = y[0];

  return GSL_SUCCESS;
}

int
jac_xsin (double t, const double y[], double *dfdy, double dfdt[],
         void *params)
{
  static int n = 0;

  n++;

  if (n > 50 && n < 55) {
    dfdy[0] = GSL_NAN;
    dfdy[1] = GSL_NAN;
    dfdy[2] = GSL_NAN;
    dfdy[3] = GSL_NAN;

    dfdt[0] = GSL_NAN;
    dfdt[1] = GSL_NAN;
    return GSL_EFAILED;
  }

  dfdy[0] = 0.0;
  dfdy[1] = -1.0;
  dfdy[2] = 1.0;
  dfdy[3] = 0.0;

  dfdt[0] = 0.0;
  dfdt[1] = 0.0;

  return GSL_SUCCESS;
}

gsl_odeiv_system rhs_func_xsin = {
  rhs_xsin,
  jac_xsin,
  2,
  0
};

/* RHS for classic stiff example
   dy0 / dt =  998 * y0 + 1998 * y1    y0(0) = 1.0
   dy1 / dt = -999 * y0 - 1999 * y1    y1(0) = 0.0

   solution is
   y0 = 2 * exp(-t) - exp(-1000 * t)
   y1 = - exp(-t) + exp(-1000 * t)
*/

int
rhs_stiff (double t, const double y[], double f[], void *params)
{
  f[0] = 998.0 * y[0] + 1998.0 * y[1];
  f[1] = -999.0 * y[0] - 1999.0 * y[1];

  return GSL_SUCCESS;
}

int
jac_stiff (double t, const double y[], double *dfdy, double dfdt[],
           void *params)
{
  dfdy[0] = 998.0;
  dfdy[1] = 1998.0;
  dfdy[2] = -999.0;
  dfdy[3] = -1999.0;

  dfdt[0] = 0.0;
  dfdt[1] = 0.0;

  return GSL_SUCCESS;
}

gsl_odeiv_system rhs_func_stiff = {
  rhs_stiff,
  jac_stiff,
  2,
  0
};

/* van Der Pol oscillator:
   f0 = y1                           y0(0) = 1.0
   f1 = -y0 + mu * y1 * (1 - y0^2)   y1(0) = 0.0
*/

int
rhs_vanderpol (double t, const double y[], double f[], void *params)
{
  const double mu = 10.0;

  f[0] = y[1];
  f[1] = -y[0] + mu * y[1] * (1.0 - y[0]*y[0]);

  return GSL_SUCCESS;
}

int
jac_vanderpol (double t, const double y[], double *dfdy, double dfdt[],
	       void *params)
{
  const double mu = 10.0;

  dfdy[0] = 0.0;
  dfdy[1] = 1.0;
  dfdy[2] = -2.0 * mu * y[0] * y[1] - 1.0;
  dfdy[3] = mu * (1.0 - y[0] * y[0]);

  dfdt[0] = 0.0;
  dfdt[1] = 0.0;

  return GSL_SUCCESS;
}

gsl_odeiv_system rhs_func_vanderpol = {
  rhs_vanderpol,
  jac_vanderpol,
  2,
  0
};

/* The Oregonator - chemical Belusov-Zhabotinskii reaction
   y0(0) = 1.0, y1(0) = 2.0, y2(0) = 3.0
*/

int
rhs_oregonator (double t, const double y[], double f[], void *params)
{
  const double c1=77.27;
  const double c2=8.375e-6;
  const double c3=0.161;

  f[0] = c1 * (y[1] + y[0] * (1 - c2 * y[0] - y[1]));
  f[1] = 1/c1 * (y[2] - y[1] * (1 + y[0]));
  f[2] = c3 * (y[0] - y[2]);

  return GSL_SUCCESS;
}

int
jac_oregonator (double t, const double y[], double *dfdy, double dfdt[],
		void *params)
{
  const double c1=77.27;
  const double c2=8.375e-6;
  const double c3=0.161;

  dfdy[0] = c1 * (1 - 2 * c2 * y[0] - y[1]);
  dfdy[1] = c1 * (1 - y[0]);
  dfdy[2] = 0.0;

  dfdy[3] = 1/c1 * (-y[1]);
  dfdy[4] = 1/c1 * (-1 - y[0]);
  dfdy[5] = 1/c1;

  dfdy[6] = c3;
  dfdy[7] = 0.0;
  dfdy[8] = -c3;

  dfdt[0] = 0.0;
  dfdt[1] = 0.0;
  dfdt[2] = 0.0;

  return GSL_SUCCESS;
}

gsl_odeiv_system rhs_func_oregonator = {
  rhs_oregonator,
  jac_oregonator,
  3,
  0
};

/* Volterra-Lotka predator-prey model

   f0 = (a - b * y1) * y0     y0(0) = 3.0
   f1 = (-c + d * y0) * y1    y1(0) = 1.0
 */

int
rhs_vl (double t, const double y[], double f[], void *params)
{
  const double a = 1.0;
  const double b = 1.0;
  const double c = 1.0;
  const double d = 1.0;

  f[0] = (a - b * y[1]) * y[0];
  f[1] = (-c + d * y[0]) * y[1];

  return GSL_SUCCESS;
}

int
jac_vl (double t, const double y[], double *dfdy, double dfdt[],
            void *params)
{
  const double a = 1.0;
  const double b = 1.0;
  const double c = 1.0;
  const double d = 1.0;

  dfdy[0] = a - b * y[1];
  dfdy[1] = -b * y[0];
  dfdy[2] = d * y[1];
  dfdy[3] = -c + d * y[0];

  dfdt[0] = 0.0;
  dfdt[1] = 0.0;

  return GSL_SUCCESS;
}

gsl_odeiv_system rhs_func_vl = {
  rhs_vl,
  jac_vl,
  2,
  0
};

/* Stiff trigonometric example

   f0 = -50 * (y0 - cos(t))    y0(0) = 0.0
 */

int
rhs_stifftrig (double t, const double y[], double f[], void *params)
{
  f[0] = -50 * (y[0] - cos(t));

  return GSL_SUCCESS;
}

int
jac_stifftrig (double t, const double y[], double *dfdy, double dfdt[],
            void *params)
{
  dfdy[0] = -50;

  dfdt[0] = -50 * sin(t);

  return GSL_SUCCESS;
}

gsl_odeiv_system rhs_func_stifftrig = {
  rhs_stifftrig,
  jac_stifftrig,
  1,
  0
};

/* E5 - a stiff badly scaled chemical problem by Enright, Hull &
   Lindberg (1975): Comparing numerical methods for stiff systems of
   ODEs. BIT, vol. 15, pp. 10-48.

   f0 = -a * y0 - b * y0 * y2                            y0(0) = 1.76e-3
   f1 = a * y0 - m * c * y1 * y2                         y1(0) = 0.0
   f2 = a * y0 - b * y0 * y2 - m * c * y1 * y2 + c * y3  y2(0) = 0.0
   f3 = b * y0 * y2 - c * y3                             y3(0) = 0.0
 */

int
rhs_e5 (double t, const double y[], double f[], void *params)
{
  const double a = 7.89e-10;
  const double b = 1.1e7;
  const double c = 1.13e3;
  const double m = 1.0e6;

  f[0] = -a * y[0] - b * y[0] * y[2];
  f[1] = a * y[0] - m * c * y[1] * y[2];
  f[3] = b * y[0] * y[2] - c * y[3];
  f[2] = f[1] - f[3];

  return GSL_SUCCESS;
}

int
jac_e5 (double t, const double y[], double *dfdy, double dfdt[],
            void *params)
{
  const double a = 7.89e-10;
  const double b = 1.1e7;
  const double c = 1.13e3;
  const double m = 1.0e6;

  dfdy[0] = -a - b * y[2];
  dfdy[1] = 0.0;
  dfdy[2] = -b * y[0];
  dfdy[3] = 0.0;

  dfdy[4] = a;
  dfdy[5] = -m * c * y[2];
  dfdy[6] = -m * c * y[1];
  dfdy[7] = 0.0;

  dfdy[8] = a - b * y[2];
  dfdy[9] = -m * c * y[2];
  dfdy[10] = -b * y[0] - m * c * y[1];
  dfdy[11] = c;

  dfdy[12] = b * y[2];
  dfdy[13] = 0.0;
  dfdy[14] = b * y[0];
  dfdy[15] = -c;

  dfdt[0] = 0.0;
  dfdt[1] = 0.0;
  dfdt[2] = 0.0;
  dfdt[3] = 0.0;

  return GSL_SUCCESS;
}

gsl_odeiv_system rhs_func_e5 = {
  rhs_e5,
  jac_e5,
  4,
  0
};

void
test_odeiv_stepper (const gsl_odeiv_step_type *T, const gsl_odeiv_system *sys,
		    const double h, const double t, const char desc[],
		    const double ystart[], const double yfin[],
		    const double relerr)
{
  /* tests stepper T with one fixed length step advance of system sys
     and compares with given values yfin
  */

  double y[MAXEQ] = {0.0};
  double yerr[MAXEQ] = {0.0};
  size_t ne = sys->dimension;
  size_t i;

  gsl_odeiv_step *step = gsl_odeiv_step_alloc (T, ne);

  DBL_MEMCPY (y, ystart, MAXEQ);

  {
    int s = gsl_odeiv_step_apply (step, t, h, y, yerr, 0, 0, sys);
    if (s != GSL_SUCCESS)
      {
	gsl_test(s, "test_odeiv_stepper: %s step_apply returned %d", desc, s);
      }
  }

  for (i = 0; i < ne; i++)
    {
      gsl_test_rel (y[i], yfin[i], relerr,
		    "%s %s step(%d)",
		    gsl_odeiv_step_name (step), desc,i);
    }

  gsl_odeiv_step_free (step);
}

void
test_stepper (const gsl_odeiv_step_type *T)
{
  /* Tests stepper T with a step of selected systems */

  double y[MAXEQ] = {0.0};
  double yfin[MAXEQ] = {0.0};

  /* Step length */
  double h;

  /* Required tolerance */
  double err_target;

  /* linear */
  h = 1e-1;
  err_target = 1e-10;
  y[0] = 0.58;
  yfin[0] = y[0] + 2 * h;
  test_odeiv_stepper (T, &rhs_func_lin, h, 0.0, "linear",
		      y, yfin, err_target);

  /* exponential */
  h = 1e-4;
  err_target = 1e-8;
  y[0] = exp(2.7);
  yfin[0] = exp(2.7 + h);
  test_odeiv_stepper (T, &rhs_func_exp, h, 2.7, "exponential",
		      y, yfin, err_target);
  /* cosine-sine */
  h = 1e-3;
  err_target = 1e-6;
  y[0] = cos(1.2);
  y[1] = sin(1.2);
  yfin[0] = cos(1.2 + h);
  yfin[1] = sin(1.2 + h);
  test_odeiv_stepper (T, &rhs_func_sin, h, 1.2, "cosine-sine",
		      y, yfin, err_target);

  /* classic stiff */
  h = 1e-7;
  err_target = 1e-4;
  y[0] = 1.0;
  y[1] = 0.0;

  {
    const double e1 = exp (-h);
    const double e2 = exp (-1000.0 * h);
    yfin[0] = 2.0 * e1 - e2;
    yfin[1] = -e1 + e2;
  }

  test_odeiv_stepper (T, &rhs_func_stiff, h, 0.0, "classic_stiff",
		      y, yfin, err_target);
}

void
test_evolve_system (const gsl_odeiv_step_type * T,
                    const gsl_odeiv_system * sys,
                    double t0, double t1, double hstart,
                    double y[], double yfin[],
                    double err_target, const char *desc)
{
  /* Tests system sys with stepper T. Step length is controlled by
     error estimation from the stepper.
  */

  int steps = 0;
  size_t i;

  double t = t0;
  double h = hstart;

  /* Tolerance factor in testing errors */
  const double factor = 10;

  gsl_odeiv_step * step = gsl_odeiv_step_alloc (T, sys->dimension);

  gsl_odeiv_control *c =
    gsl_odeiv_control_standard_new (err_target, err_target, 1.0, 0.0);

  gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc (sys->dimension);

  while (t < t1)
    {
      int s = gsl_odeiv_evolve_apply (e, c, step, sys, &t, t1, &h, y);

      if (s != GSL_SUCCESS && sys != &rhs_func_xsin)
	{
	  gsl_test(s, "%s evolve_apply returned %d",
		   gsl_odeiv_step_name (step), s);
	  break;
	}

      if (steps > 100000)
	{
	  gsl_test(GSL_EFAILED,
		   "%s evolve_apply reached maxiter",
		   gsl_odeiv_step_name (step));
	  break;
	}

      steps++;
    }

  /* err_target is target error of one step. Test if stepper has made
     larger error than (tolerance factor times) the number of steps
     times the err_target */

  for (i = 0; i < sys->dimension; i++)
    {
      gsl_test_abs (y[i], yfin[i], factor * e->count * err_target,
		    "%s %s evolve(%d)",
		    gsl_odeiv_step_name (step), desc, i);
    }

  gsl_odeiv_evolve_free (e);
  gsl_odeiv_control_free (c);
  gsl_odeiv_step_free (step);
}

int
sys_driver (const gsl_odeiv_step_type * T,
	    const gsl_odeiv_system * sys,
	    double t0, double t1, double hstart,
	    double y[], double epsabs, double epsrel,
	    const char desc[])
{
  /* This function evolves a system sys with stepper T from t0 to t1.
     Step length is varied via error control with possibly different
     absolute and relative error tolerances.
  */

  int s = 0;
  int steps = 0;

  double t = t0;
  double h = hstart;

  gsl_odeiv_step * step = gsl_odeiv_step_alloc (T, sys->dimension);

  gsl_odeiv_control *c =
    gsl_odeiv_control_standard_new (epsabs, epsrel, 1.0, 0.0);
  gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc (sys->dimension);

  while (t < t1)
    {
      s = gsl_odeiv_evolve_apply (e, c, step, sys, &t, t1, &h, y);

      if (s != GSL_SUCCESS)
	{
	  gsl_test(s, "sys_driver: %s evolve_apply returned %d",
		   gsl_odeiv_step_name (step), s);
	  break;
	}

      if (steps > 1e7)
	{
	  gsl_test(GSL_EMAXITER,
		   "sys_driver: %s evolve_apply reached maxiter at t=%g",
		   gsl_odeiv_step_name (step), t);
	  s = GSL_EMAXITER;
	  break;
	}

      steps++;
    }

  gsl_test(s, "%s %s [%g,%g], %d steps completed",
	   gsl_odeiv_step_name (step), desc, t0, t1, steps);

  gsl_odeiv_evolve_free (e);
  gsl_odeiv_control_free (c);
  gsl_odeiv_step_free (step);

  return s;
}

void
test_compare_vanderpol (void)
{
  /* Compares output of van Der Pol oscillator with several steppers */

  /* system dimension */
  const size_t sd = 2;

  const gsl_odeiv_step_type *steppers[20];
  const gsl_odeiv_step_type **T;

  /* Required error tolerance for each stepper. */
  double err_target[20];

  /* number of ODE solvers */
  const size_t ns = 11;

  /* initial values for each ode-solver */
  double y[11][2];
  double *yp = &y[0][0];

  size_t i, j, k;
  int status = 0;

  /* Parameters for the problem and stepper  */
  const double start = 0.0;
  const double end = 100.0;
  const double epsabs = 1e-8;
  const double epsrel = 1e-8;
  const double initstepsize = 1e-5;

  /* Initialize */

  steppers[0] = gsl_odeiv_step_rk2;
  err_target[0] = 1e-6;
  steppers[1] = gsl_odeiv_step_rk4;
  err_target[1] = 1e-6;
  steppers[2] = gsl_odeiv_step_rkf45;
  err_target[2] = 1e-6;
  steppers[3] = gsl_odeiv_step_rkck;
  err_target[3] = 1e-6;
  steppers[4] = gsl_odeiv_step_rk8pd;
  err_target[4] = 1e-6;
  steppers[5] = gsl_odeiv_step_rk2imp;
  err_target[5] = 1e-5;
  steppers[6] = gsl_odeiv_step_rk2simp;
  err_target[6] = 1e-5;
  steppers[7] = gsl_odeiv_step_rk4imp;
  err_target[7] = 1e-6;
  steppers[8] = gsl_odeiv_step_bsimp;
  err_target[8] = 1e-7;
  steppers[9] = gsl_odeiv_step_gear1;
  err_target[9] = 1e-2;
  steppers[10] = gsl_odeiv_step_gear2;
  err_target[10] = 1e-6;
  steppers[11] = 0;

  T = steppers;

  for (i = 0; i < ns; i++)
    {
      y[i][0] = 1.0;
      y[i][1] = 0.0;
    }

  /* Call each solver for the problem */

  i = 0;
  while (*T != 0)
    {
      {
	int s = sys_driver (*T, &rhs_func_vanderpol,
			    start, end, initstepsize, &yp[i],
			    epsabs, epsrel, "vanderpol");
	if (s != GSL_SUCCESS)
	  {
	    status++;
	  }
      }

      T++;
      i += sd;
    }

  if (status != GSL_SUCCESS)
    {
      return;
    }

  /* Compare results */

  T = steppers;

  for (i = 0; i < ns; i++)
    for (j = i+1; j < ns; j++)
      for (k = 0; k < sd; k++)
	{
	  const double val1 = yp[sd * i + k];
	  const double val2 = yp[sd * j + k];
	  gsl_test_abs (val1, val2,
			( GSL_MAX(err_target[i], err_target[j]) ),
			"%s/%s vanderpol",
			T[i]->name, T[j]->name);
	}

}

void
test_compare_oregonator (void)
{
  /* Compares output of the Oregonator with several steppers */

  /* system dimension */
  const size_t sd = 3;

  const gsl_odeiv_step_type *steppers[20];
  const gsl_odeiv_step_type **T;

  /* Required error tolerance for each stepper. */
  double err_target[20];

  /* number of ODE solvers */
  const size_t ns = 2;

  /* initial values for each ode-solver */
  double y[2][3];
  double *yp = &y[0][0];

  size_t i, j, k;
  int status = 0;

  /* Parameters for the problem and stepper  */
  const double start = 0.0;
  const double end = 360.0;
  const double epsabs = 1e-8;
  const double epsrel = 1e-8;
  const double initstepsize = 1e-5;

  /* Initialize */

  steppers[0] = gsl_odeiv_step_rk2simp;
  err_target[0] = 1e-6;
  steppers[1] = gsl_odeiv_step_bsimp;
  err_target[1] = 1e-6;
  steppers[2] = 0;

  T = steppers;

  for (i = 0; i < ns; i++)
    {
      y[i][0] = 1.0;
      y[i][1] = 2.0;
      y[i][2] = 3.0;
    }

  /* Call each solver for the problem */

  i = 0;
  while (*T != 0)
    {
      {
	int s = sys_driver (*T, &rhs_func_oregonator,
			    start, end, initstepsize, &yp[i],
			    epsabs, epsrel, "oregonator");

	if (s != GSL_SUCCESS)
	  {
	    status++;
	  }
      }

      T++;
      i += sd;
    }

  if (status != GSL_SUCCESS)
    {
      return;
    }

  /* Compare results */

  T = steppers;

  for (i = 0; i < ns; i++)
    for (j = i+1; j < ns; j++)
      for (k = 0; k < sd; k++)
	{
	  const double val1 = yp[sd * i + k];
	  const double val2 = yp[sd * j + k];
	  gsl_test_rel (val1, val2,
			( GSL_MAX(err_target[i], err_target[j]) ),
			"%s/%s oregonator",
			T[i]->name, T[j]->name);
	}

}

void
test_evolve_linear (const gsl_odeiv_step_type * T, double h, double err)
{
  double y[1];
  double yfin[1];

  y[0] = 1.0;
  yfin[0] = 9.0;
  test_evolve_system (T, &rhs_func_lin, 0.0, 4.0, h, y, yfin, err,
		      "linear[0,4]");
}

void
test_evolve_exp (const gsl_odeiv_step_type * T, double h, double err)
{
  double y[1];
  double yfin[1];

  y[0] = 1.0;
  yfin[0] = exp (2.0);
  test_evolve_system (T, &rhs_func_exp, 0.0, 2.0, h, y, yfin, err,
		      "exp[0,2]");
}

void
test_evolve_sin (const gsl_odeiv_step_type * T, double h, double err)
{
  double y[2];
  double yfin[2];

  y[0] = 1.0;
  y[1] = 0.0;
  yfin[0] = cos (2.0);
  yfin[1] = sin (2.0);
  test_evolve_system (T, &rhs_func_sin, 0.0, 2.0, h, y, yfin, err,
		      "sine[0,2]");
}

void
test_evolve_xsin (const gsl_odeiv_step_type * T, double h, double err)
{
  double y[2];
  double yfin[2];

  y[0] = 1.0;
  y[1] = 0.0;
  yfin[0] = cos (2.0);
  yfin[1] = sin (2.0);
  test_evolve_system (T, &rhs_func_xsin, 0.0, 2.0, h, y, yfin, err,
		      "sine[0,2] w/errors");
}

void
test_evolve_stiff1 (const gsl_odeiv_step_type * T, double h, double err)
{
  double y[2];
  double yfin[2];

  y[0] = 1.0;
  y[1] = 0.0;
  {
    double arg = 1.0;
    double e1 = exp (-arg);
    double e2 = exp (-1000.0 * arg);
    yfin[0] = 2.0 * e1 - e2;
    yfin[1] = -e1 + e2;
  }
  test_evolve_system (T, &rhs_func_stiff, 0.0, 1.0, h, y, yfin, err,
                      "stiff[0,1]");
}

void
test_evolve_stiff5 (const gsl_odeiv_step_type * T, double h, double err)
{
  double y[2];
  double yfin[2];

  y[0] = 1.0;
  y[1] = 0.0;
  {
    double arg = 5.0;
    double e1 = exp (-arg);
    double e2 = exp (-1000.0 * arg);
    yfin[0] = 2.0 * e1 - e2;
    yfin[1] = -e1 + e2;
  }
  test_evolve_system (T, &rhs_func_stiff, 0.0, 5.0, h, y, yfin, err,
                      "stiff[0,5]");
}

int
main (void)
{
  int i;

  struct ptype
  {
    const gsl_odeiv_step_type *type;
    double h;
  }
  p[20];

  p[0].type = gsl_odeiv_step_rk2;
  p[0].h = 1.0e-3;
  p[1].type = gsl_odeiv_step_rk4;
  p[1].h = 1.0e-3;
  p[2].type = gsl_odeiv_step_rkf45;
  p[2].h = 1.0e-3;
  p[3].type = gsl_odeiv_step_rkck;
  p[3].h = 1.0e-3;
  p[4].type = gsl_odeiv_step_rk8pd;
  p[4].h = 1.0e-3;
  p[5].type = gsl_odeiv_step_rk2imp;
  p[5].h = 1.0e-3;
  p[6].type = gsl_odeiv_step_rk2simp;
  p[6].h = 1.0e-3;
  p[7].type = gsl_odeiv_step_rk4imp;
  p[7].h = 1.0e-3;
  p[8].type = gsl_odeiv_step_bsimp;
  p[8].h = 1.0e-3;
  p[9].type = gsl_odeiv_step_gear1;
  p[9].h = 1.0e-3;
  p[10].type = gsl_odeiv_step_gear2;
  p[10].h = 1.0e-3;
  p[11].type = 0;

  gsl_ieee_env_setup ();

  for (i = 0; p[i].type != 0; i++)
    {
      test_stepper(p[i].type);
    }

  for (i = 0; p[i].type != 0; i++)
    {
      test_evolve_linear (p[i].type, p[i].h, 1e-10);
      test_evolve_exp (p[i].type, p[i].h, 1e-6);
      test_evolve_sin (p[i].type, p[i].h, 1e-8);
      test_evolve_xsin (p[i].type, p[i].h, 1e-8);
      test_evolve_stiff1 (p[i].type, p[i].h, 1e-7);
      test_evolve_stiff5 (p[i].type, p[i].h, 1e-7);
    }

  test_compare_vanderpol();
  test_compare_oregonator();

  exit (gsl_test_summary ());
}


/* movstat/mvacc.c
 *
 * Moving window mean/variance accumulator - based on a modification
 * to Welford's algorithm, discussed here:
 *
 * https://stackoverflow.com/a/6664212
 *
 * Copyright (C) 2018 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 <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_movstat.h>

typedef double ringbuf_type_t;

#include "ringbuf.c"

typedef struct
{
  size_t n;      /* window size */
  size_t k;      /* number of samples currently in window */
  double mean;   /* current window mean */
  double M2;     /* current window M2 */
  ringbuf *rbuf; /* ring buffer storing current window */
} mvacc_state_t;

static size_t
mvacc_size(const size_t n)
{
  size_t size = 0;

  size += sizeof(mvacc_state_t);
  size += ringbuf_size(n);

  return size;
}

static int
mvacc_init(const size_t n, void * vstate)
{
  mvacc_state_t * state = (mvacc_state_t *) vstate;

  state->n = n;
  state->k = 0;
  state->mean = 0.0;
  state->M2 = 0.0;

  state->rbuf = (ringbuf *) ((unsigned char *) vstate + sizeof(mvacc_state_t));
  ringbuf_init(n, state->rbuf);

  return GSL_SUCCESS;
}

static int
mvacc_insert(const double x, void * vstate)
{
  mvacc_state_t * state = (mvacc_state_t *) vstate;

  if (ringbuf_is_full(state->rbuf))
    {
      /* remove oldest window element and add new one */
      double old = ringbuf_peek_back(state->rbuf);
      double prev_mean = state->mean;

      state->mean += (x - old) / (double) state->n;
      state->M2 += ((old - prev_mean) + (x - state->mean)) * (x - old);
    }
  else
    {
      double delta = x - state->mean;

      /*
       * Welford algorithm:
       *
       * mu_new = mu_old + (x - mu_old) / n
       * M2_new = M2_old + (x - mu_old) * (x - mu_new)
       */

      ++(state->k);
      state->mean += delta / (double) state->k;
      state->M2 += delta * (x - state->mean);
    }

  /* add new element to ring buffer */
  ringbuf_insert(x, state->rbuf);

  return GSL_SUCCESS;
}

static int
mvacc_delete(void * vstate)
{
  mvacc_state_t * state = (mvacc_state_t *) vstate;

  if (!ringbuf_is_empty(state->rbuf))
    {
      if (state->k > 1)
        {
          /*
           * mu_new = mu_old + (mu_old - x_old) / (n - 1)
           * M2_new = M2_old - (mu_old - x_old) * (mu_new - x_old)
           */

          double old = ringbuf_peek_back(state->rbuf);
          double prev_mean = state->mean;
          double delta = prev_mean - old;

          state->mean += delta / (state->k - 1.0);
          state->M2 -= delta * (state->mean - old);
        }
      else if (state->k == 1)
        {
          state->mean = 0.0;
          state->M2 = 0.0;
        }

      ringbuf_pop_back(state->rbuf);
      --(state->k);
    }

  return GSL_SUCCESS;
}

static int
mvacc_mean(void * params, double * result, const void * vstate)
{
  const mvacc_state_t * state = (const mvacc_state_t *) vstate;
  (void) params;
  *result = state->mean;
  return GSL_SUCCESS;
}

static int
mvacc_variance(void * params, double * result, const void * vstate)
{
  const mvacc_state_t * state = (const mvacc_state_t *) vstate;

  (void) params;

  if (state->k < 2)
    *result = 0.0;
  else
    *result = state->M2 / (state->k - 1.0);

  return GSL_SUCCESS;
}

static int
mvacc_sd(void * params, double * result, const void * vstate)
{
  double variance;
  int status = mvacc_variance(params, &variance, vstate);
  *result = sqrt(variance);
  return status;
}

static const gsl_movstat_accum mean_accum_type =
{
  mvacc_size,
  mvacc_init,
  mvacc_insert,
  mvacc_delete,
  mvacc_mean
};

const gsl_movstat_accum *gsl_movstat_accum_mean = &mean_accum_type;

static const gsl_movstat_accum variance_accum_type =
{
  mvacc_size,
  mvacc_init,
  mvacc_insert,
  mvacc_delete,
  mvacc_variance
};

const gsl_movstat_accum *gsl_movstat_accum_variance = &variance_accum_type;

static const gsl_movstat_accum sd_accum_type =
{
  mvacc_size,
  mvacc_init,
  mvacc_insert,
  mvacc_delete,
  mvacc_sd
};

const gsl_movstat_accum *gsl_movstat_accum_sd = &sd_accum_type;


//This computes "the" IDCT (inverse discrete cosine transformation) along dim of matrix X.
//This uses a CBLAS matrix multiplication by the DCT-III matrix.

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

#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif

#ifndef M_SQRT1_2
    #define M_SQRT1_2 0.707106781186547524401
#endif

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

int idct_cblas_s (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc);
int idct_cblas_d (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc);
int idct_cblas_c (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc);
int idct_cblas_z (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc);

int idct_cblas_s (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in idct_cblas_s: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndct<Lx) { fprintf(stderr,"error in idct_cblas_s: ndct must be >= Lx (length of vecs in X)\n"); return 1; }

    if (ndct==0u || N==0u) {}
    else if (ndct==1u)
    {
        for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
    else
    {
        //Scaling
        const float xsc = (sc) ? (float)(2.0*M_SQRT1_2) : 1.0f;
        const float ysc = (sc) ? 2.0f/sqrtf((float)(2u*ndct)) : 2.0f/(float)(2u*ndct);
        const float dcsc = 0.5f * xsc * ysc;

        //Initialize DCT-III matrix
        const size_t LN = Lx * ndct;
        const float P_N = (float)(M_PI/(double)ndct);
        float *DCT, x0;
        DCT = (float *)aligned_alloc(sizeof(float),LN*sizeof(float));
        if (!DCT) { fprintf(stderr,"error in idct_cblas_s: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t n=0u; n<ndct; ++n)
        {
            *DCT++ = xsc * ysc;
            for (size_t l=1u; l<Lx; ++l, ++DCT)
            {
                *DCT = ysc * cosf(P_N*(0.5f+(float)n)*(float)l);
            }
        }
        DCT -= LN;

        if (Lx==N)
        {
            x0 = dcsc * *X;
            cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,1,0.0f,Y,1);
            for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0; }
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=V; v>0u; --v, X+=Lx)
                {
                    x0 = dcsc * *X;
                    cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,1,0.0f,Y,1);
                    for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0; }
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=B*(Lx-1u), Y+=B*(ndct-1u))
                {
                    for (size_t b=B; b>0u; --b, ++X, Y-=K*ndct-1u)
                    {
                        x0 = dcsc * *X;
                        cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,(int)K,0.0f,Y,(int)K);
                        for (size_t n=ndct; n>0u; --n, Y+=K) { *Y -= x0; }
                    }
                }
            }
        }
        free(DCT);
    }

    return 0;
}

int idct_cblas_d (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in idct_cblas_d: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndct<Lx) { fprintf(stderr,"error in idct_cblas_d: ndct must be >= Lx (length of vecs in X)\n"); return 1; }

    if (ndct==0u || N==0u) {}
    else if (ndct==1u)
    {
        for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
     else
    {
        //Scaling
        const double xsc = (sc) ? 2.0*M_SQRT1_2 : 1.0;
        const double ysc = (sc) ? 2.0/sqrt((double)(2u*ndct)) : 2.0/(double)(2u*ndct);
        const double dcsc = 0.5 * xsc * ysc;

        //Initialize DCT-III matrix
        const size_t LN = Lx * ndct;
        const double P_N = M_PI/(double)ndct;
        double *DCT, x0;
        DCT = (double *)aligned_alloc(sizeof(double),LN*sizeof(double));
        if (!DCT) { fprintf(stderr,"error in idct_cblas_d: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t n=0u; n<ndct; ++n)
        {
            *DCT++ = xsc * ysc;
            for (size_t l=1u; l<Lx; ++l, ++DCT)
            {
                *DCT = ysc * cos(P_N*(0.5+(double)n)*(double)l);
            }
        }
        DCT -= LN;

        if (Lx==N)
        {
            x0 = dcsc * *X;
            cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,1,0.0,Y,1);
            for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0; }
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=V; v>0u; --v, X+=Lx)
                {
                    x0 = dcsc * *X;
                    cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,1,0.0,Y,1);
                    for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0; }
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=B*(Lx-1u), Y+=B*(ndct-1u))
                {
                    for (size_t b=B; b>0u; --b, ++X, Y-=K*ndct-1u)
                    {
                        x0 = dcsc * *X;
                        cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,(int)K,0.0,Y,(int)K);
                        for (size_t n=ndct; n>0u; --n, Y+=K) { *Y -= x0; }
                    }
                }
            }
        }
        free(DCT);
    }

    return 0;
}

int idct_cblas_c (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in idct_cblas_c: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndct<Lx) { fprintf(stderr,"error in idct_cblas_c: ndct must be >= Lx (length of vecs in X)\n"); return 1; }

    if (ndct==0u || N==0u) {}
    else if (ndct==1u)
    {
        for (size_t n=2u*N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
    else
    {
        //Scaling
        const float xsc = (sc) ? (float)(2.0*M_SQRT1_2) : 1.0f;
        const float ysc = (sc) ? 2.0f/sqrtf((float)(2u*ndct)) : 2.0f/(float)(2u*ndct);
        const float dcsc = 0.5f * xsc * ysc;

        //Initialize DCT-III matrix
        const size_t LN = Lx * ndct;
        const float P_N = (float)(M_PI/(double)ndct);
        float *DCT, x0r, x0i;
        DCT = (float *)aligned_alloc(sizeof(float),LN*sizeof(float));
        if (!DCT) { fprintf(stderr,"error in idct_cblas_s: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t n=0u; n<ndct; ++n)
        {
            *DCT++ = xsc * ysc;
            for (size_t l=1u; l<Lx; ++l, ++DCT)
            {
                *DCT = ysc * cosf(P_N*(0.5f+(float)n)*(float)l);
            }
        }
        DCT -= LN;

        if (Lx==N)
        {
            x0r = dcsc * *X; x0i = dcsc * *(X+1);
            cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,2,0.0f,Y,2);
            cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X+1,2,0.0f,Y+1,2);
            for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0r; *++Y -= x0i; }
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=0u; v<V; ++v, X+=2u*Lx)
                {
                    x0r = dcsc * *X; x0i = dcsc * *(X+1);
                    cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,2,0.0f,Y,2);
                    cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X+1,2,0.0f,Y+1,2);
                    for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0r; *++Y -= x0i; }
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=2u*B*(Lx-1u), Y+=2u*B*(ndct-1u))
                {
                    for (size_t b=B; b>0u; --b, X+=2u, Y-=2u*K*ndct-2u)
                    {
                        x0r = dcsc * *X; x0i = dcsc * *(X+1);
                        cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,2*(int)K,0.0f,Y,2*(int)K);
                        cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X+1,2*(int)K,0.0f,Y+1,2*(int)K);
                        for (size_t n=ndct; n>0u; --n, Y+=2u*K) { *Y -= x0r; *(Y+1) -= x0i; }
                    }
                }
            }
        }
        free(DCT);
    }

    return 0;
}

int idct_cblas_z (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in idct_cblas_z: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndct<Lx) { fprintf(stderr,"error in idct_cblas_z: ndct must be >= Lx (length of vecs in X)\n"); return 1; }

    if (ndct==0u || N==0u) {}
    else if (ndct==1u)
    {
        for (size_t n=2u*N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
    else
    {
        //Scaling
        const double xsc = (sc) ? 2.0*M_SQRT1_2 : 1.0;
        const double ysc = (sc) ? 2.0/sqrt((double)(2u*ndct)) : 2.0/(double)(2u*ndct);
        const double dcsc = 0.5 * xsc * ysc;

        //Initialize DCT-III matrix
        const size_t LN = Lx * ndct;
        const double P_N = M_PI/(double)ndct;
        double *DCT, x0r, x0i;
        DCT = (double *)aligned_alloc(sizeof(double),LN*sizeof(double));
        if (!DCT) { fprintf(stderr,"error in idct_cblas_z: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t n=0u; n<ndct; ++n)
        {
            *DCT++ = xsc * ysc;
            for (size_t l=1u; l<Lx; ++l, ++DCT)
            {
                *DCT = ysc * cos(P_N*(0.5+(double)n)*(double)l);
            }
        }
        DCT -= LN;

        if (Lx==N)
        {
            x0r = dcsc * *X; x0i = dcsc * *(X+1);
            cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,2,0.0,Y,2);
            cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X+1,2,0.0,Y+1,2);
            for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0r; *++Y -= x0i; }
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=0u; v<V; ++v, X+=2u*Lx)
                {
                    x0r = dcsc * *X; x0i = dcsc * *(X+1);
                    cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,2,0.0,Y,2);
                    cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X+1,2,0.0,Y+1,2);
                    for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0r; *++Y -= x0i; }
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=2u*B*(Lx-1u), Y+=2u*B*(ndct-1u))
                {
                    for (size_t b=B; b>0u; --b, X+=2u, Y-=2u*K*ndct-2u)
                    {
                        x0r = dcsc * *X; x0i = dcsc * *(X+1);
                        cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,2*(int)K,0.0,Y,2*(int)K);
                        cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X+1,2*(int)K,0.0,Y+1,2*(int)K);
                        for (size_t n=ndct; n>0u; --n, Y+=2u*K) { *Y -= x0r; *(Y+1) -= x0i; }
                    }
                }
            }
        }
        free(DCT);
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


/* specfunc/beta_inc.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 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 */
/* Modified for cdfs by Brian Gough, June 2003 */

#include <gsl/gsl_sf_gamma.h>

static double
beta_cont_frac (const double a, const double b, const double x,
                const double epsabs)
{
  const unsigned int max_iter = 512;    /* control iterations      */
  const double cutoff = 2.0 * GSL_DBL_MIN;      /* control the zero cutoff */
  unsigned int iter_count = 0;
  double cf;

  /* standard initialization for continued fraction */
  double num_term = 1.0;
  double den_term = 1.0 - (a + b) * x / (a + 1.0);

  if (fabs (den_term) < cutoff)
    den_term = GSL_NAN;

  den_term = 1.0 / den_term;
  cf = den_term;

  while (iter_count < max_iter)
    {
      const int k = iter_count + 1;
      double coeff = k * (b - k) * x / (((a - 1.0) + 2 * k) * (a + 2 * k));
      double delta_frac;

      /* first step */
      den_term = 1.0 + coeff * den_term;
      num_term = 1.0 + coeff / num_term;

      if (fabs (den_term) < cutoff)
        den_term = GSL_NAN;

      if (fabs (num_term) < cutoff)
        num_term = GSL_NAN;

      den_term = 1.0 / den_term;

      delta_frac = den_term * num_term;
      cf *= delta_frac;

      coeff = -(a + k) * (a + b + k) * x / ((a + 2 * k) * (a + 2 * k + 1.0));

      /* second step */
      den_term = 1.0 + coeff * den_term;
      num_term = 1.0 + coeff / num_term;

      if (fabs (den_term) < cutoff)
        den_term = GSL_NAN;

      if (fabs (num_term) < cutoff)
        num_term = GSL_NAN;

      den_term = 1.0 / den_term;

      delta_frac = den_term * num_term;
      cf *= delta_frac;

      if (fabs (delta_frac - 1.0) < 2.0 * GSL_DBL_EPSILON)
        break;

      if (cf * fabs (delta_frac - 1.0) < epsabs)
        break;

      ++iter_count;
    }

  if (iter_count >= max_iter)
    return GSL_NAN;

  return cf;
}

/* The function beta_inc_AXPY(A,Y,a,b,x) computes A * beta_inc(a,b,x)
   + Y taking account of possible cancellations when using the
   hypergeometric transformation beta_inc(a,b,x)=1-beta_inc(b,a,1-x).

   It also adjusts the accuracy of beta_inc() to fit the overall
   absolute error when A*beta_inc is added to Y. (e.g. if Y >>
   A*beta_inc then the accuracy of beta_inc can be reduced) */

static double
beta_inc_AXPY (const double A, const double Y,
               const double a, const double b, const double x)
{
  if (x == 0.0)
    {
      return A * 0 + Y;
    }
  else if (x == 1.0)
    {
      return A * 1 + Y;
    }
  else if (a > 1e5 && b < 10 && x > a / (a + b))
    {
      /* Handle asymptotic regime, large a, small b, x > peak [AS 26.5.17] */
      double N = a + (b - 1.0) / 2.0;
      return A * gsl_sf_gamma_inc_Q (b, -N * log (x)) + Y;
    }
  else if (b > 1e5 && a < 10 && x < b / (a + b))
    {
      /* Handle asymptotic regime, small a, large b, x < peak [AS 26.5.17] */
      double N = b + (a - 1.0) / 2.0;
      return A * gsl_sf_gamma_inc_P (a, -N * log1p (-x)) + Y;
    }
  else
    {
      double ln_beta = gsl_sf_lnbeta (a, b);
      double ln_pre = -ln_beta + a * log (x) + b * log1p (-x);

      double prefactor = exp (ln_pre);

      if (x < (a + 1.0) / (a + b + 2.0))
        {
          /* Apply continued fraction directly. */
          double epsabs = fabs (Y / (A * prefactor / a)) * GSL_DBL_EPSILON;

          double cf = beta_cont_frac (a, b, x, epsabs);

          return A * (prefactor * cf / a) + Y;
        }
      else
        {
          /* Apply continued fraction after hypergeometric transformation. */
          double epsabs =
            fabs ((A + Y) / (A * prefactor / b)) * GSL_DBL_EPSILON;
          double cf = beta_cont_frac (b, a, 1.0 - x, epsabs);
          double term = prefactor * cf / b;

          if (A == -Y)
            {
              return -A * term;
            }
          else
            {
              return A * (1 - term) + Y;
            }
        }
    }
}

/* Direct series evaluation for testing purposes only */

#if 0
static double
beta_series (const double a, const double b, const double x,
             const double epsabs)
{
  double f = x / (1 - x);
  double c = (b - 1) / (a + 1) * f;
  double s = 1;
  double n = 0;

  s += c;

  do
    {
      n++;
      c *= -f * (2 + n - b) / (2 + n + a);
      s += c;
    }
  while (n < 512 && fabs (c) > GSL_DBL_EPSILON * fabs (s) + epsabs);

  s /= (1 - x);

  return s;
}
#endif