#ifndef UTIL_H
#define UTIL_H

#include <iostream>
#include <stdio.h>
#include <vector>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/Householder>
#include <assert.h>
#include <time.h>
#include <sys/time.h>
#include <random>
#ifdef USE_MKL
#include <mkl_cblas.h>
#include <mkl_lapacke.h>
#else
#include <cblas.h>
#include <lapacke.h>
#endif

typedef Eigen::SparseMatrix<double, 0, int> SpMat;
typedef Eigen::VectorBlock<Eigen::Matrix<double, -1,  1, 0, -1,  1>, -1> Segment;
typedef Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, -1, -1> MatrixBlock;

bool are_connected(Eigen::VectorXi &a, Eigen::VectorXi &b, SpMat &A);
bool should_be_disconnected(int lvl1, int lvl2, int sep1, int sep2);
double elapsed(timeval& start, timeval& end);
void swap2perm(Eigen::VectorXi* swap, Eigen::VectorXi* perm);
bool isperm(Eigen::VectorXi* perm);
SpMat symmetric_graph(SpMat& A);

typedef timeval timer;
timer wctime();

/**
 * C <- alpha A   * B   + beta C
 * C <- alpha A^T * B   + beta C
 * C <- alpha A   * B^T + beta C
 * C <- alpha A^T * B^T + beta C
 * Gemm
 */
void gemm(Eigen::MatrixXd* A, Eigen::MatrixXd* B, Eigen::MatrixXd* C, CBLAS_TRANSPOSE tA, CBLAS_TRANSPOSE tB, double alpha, double beta);

/** Return a new
 * C <- alpha A^(/T) * B^(/T)
 **/
Eigen::MatrixXd* gemm_new(Eigen::MatrixXd* A, Eigen::MatrixXd* B, CBLAS_TRANSPOSE tA, CBLAS_TRANSPOSE tB, double alpha);

/**
 * C <- C - A * A^T
 */
void syrk(Eigen::MatrixXd* A, Eigen::MatrixXd* C);

/**
 * A <- L, L L^T = A
 * Return != 0 if potf failed (not spd)
 */
int potf(Eigen::MatrixXd* A);

/**
 * A <- [L\U] (lower and upper)
 * p <- swap (NOT a permutation)
 * A[p] = L*U
 * L is unit diagonal
 * U is not
 * Return != 0 if getf failed (singular)
 */
int getf(Eigen::MatrixXd* A, Eigen::VectorXi* swap);

/**
 * Compute an estimated 1-norm condition number of A using its LU or Cholesky factorization
 */
double rcond_1_getf(Eigen::MatrixXd* A_LU, double A_1_norm);
double rcond_1_potf(Eigen::MatrixXd* A_LLT, double A_1_norm);

/**
 * B <- B * L^(-1)
 * B <- B * L^(-T)
 * B <- B * U^(-1)
 * B <- B * U^(-T)
 */
void trsm_right(Eigen::MatrixXd* L, Eigen::MatrixXd* B, CBLAS_UPLO uplo, CBLAS_TRANSPOSE trans, CBLAS_DIAG diag);

/**
 * B <- L^(-1) * B
 * B <- L^(-T) * B
 * B <- U^(-1) * B
 * B <- U^(-T) * B
 */
void trsm_left(Eigen::MatrixXd* L, Eigen::MatrixXd* B, CBLAS_UPLO uplo, CBLAS_TRANSPOSE trans, CBLAS_DIAG diag);

/**
 * x <- L^(-1) * x
 * x <- L^(-T) * x
 * x <- U^(-1) * x
 * x <- U^(-T) * x
 */
void trsv(Eigen::MatrixXd* L, Segment* x, CBLAS_UPLO uplo, CBLAS_TRANSPOSE trans, CBLAS_DIAG diag);

/**
 * x <- L^T * x
 */
void trmv_trans(Eigen::MatrixXd* L, Segment* x);

/**
 * A <- L^T * A
 */
void trmm_trans(Eigen::MatrixXd* L, Eigen::MatrixXd* A);

/**
 * x2 <- x2 - A21 * x1
 */
void gemv_notrans(Eigen::MatrixXd* A21, Segment* x1, Segment* x2);

/**
 * x2 <- x2 - A12^T * x1
 */
void gemv_trans(Eigen::MatrixXd* A12, Segment* x1, Segment* x2);

/**
 * AP = QR
 */
void geqp3(Eigen::MatrixXd* A, Eigen::VectorXi* jpvt, Eigen::VectorXd* tau);

/**
 * A = U S VT
 * Compute the full SVD, where if A is mxn, U is mxm, V is nxn, and S is min(M,N)
 * VT is V^T, *not* V.
 */
void gesvd(Eigen::MatrixXd* A, Eigen::MatrixXd* U, Eigen::VectorXd* S, Eigen::MatrixXd* VT);

/**
 * A = U S U^T
 * A <- U
 * Full Symmetric EVD
 */
void syev(Eigen::MatrixXd* A, Eigen::VectorXd* S);

/**
 * x <- Q * x
 */
void ormqr_notrans(Eigen::MatrixXd* v, Eigen::VectorXd* h, Segment* x);

/**
 * x <- Q^T * x
 * A <- Q^T * A
 */
void ormqr_trans(Eigen::MatrixXd* v, Eigen::VectorXd* h, Segment* x);

/**
 * A <- A   * Q
 * A <- A   * Q^T
 * A <- Q   * A
 * A <- Q^T * A
 */
void ormqr(Eigen::MatrixXd* v, Eigen::VectorXd* h, Eigen::MatrixXd* A, char side, char trans);

/**
 * Create the thin Q
 */
void orgqr(Eigen::MatrixXd* v, Eigen::VectorXd* h);

/**
 * Create a square Q
 */
// void orgqr_fat(Eigen::MatrixXd* v, Eigen::VectorXd* h);

/**
 * A = QR
 */
void geqrf(Eigen::MatrixXd* A, Eigen::VectorXd* tau);

int choose_rank(Eigen::VectorXd& s, double tol);

std::size_t hashv(std::vector<size_t> vals);

// Hash function for Eigen matrix and vector.
// The code is from `hash_combine` function of the Boost library. See
// http://www.boost.org/doc/libs/1_55_0/doc/html/hash/reference.html#boost.hash_combine .
template<typename T>
struct matrix_hash : std::unary_function<T, size_t> {
  std::size_t operator()(T const& matrix) const {
    // Note that it is oblivious to the storage order of Eigen matrix (column- or
    // row-major). It will give you the same hash value for two different matrices if they
    // are the transpose of each other in different storage order.
    size_t seed = 0;
    for (size_t i = 0; i < matrix.size(); ++i) {
      auto elem = *(matrix.data() + i);
      seed ^= std::hash<typename T::Scalar>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
    }
    return seed;
  }
};

void block2dense(Eigen::VectorXi &rowval, Eigen::VectorXi &colptr, Eigen::VectorXd &nnzval, int i, int j, int li, int lj, Eigen::MatrixXd *dst, bool transpose);

Eigen::MatrixXd linspace_nd(int n, int dim);

// Returns A[p,p]
SpMat symm_perm(SpMat &A, Eigen::VectorXi &p);

// Random vector with seed
Eigen::VectorXd random(int size, int seed);

Eigen::MatrixXd random(int rows, int cols, int seed);

// Print vector
template<typename T>
std::ostream& operator<<(std::ostream& os, const std::vector<T>& v) {
    for(auto v_ : v) {
        os << v_ << " " ;
    }
    os << std::endl;
    return os;
}

/**
 * Statistics and Logging
 **/

struct Profile {
    public:
        double elim;
        double scale;
        double spars;
        double merge;

        double mergealloc;
        double mergecopy;

        double geqp3;
        double geqrf;
        double potf;
        double trsm;
        double gemm;

        double buildq;
        double scattq;
        double perma;
        double scatta;
        double prese;
        double assmb;
        double phi;

        Profile() :
            elim(0), scale(0), spars(0), merge(0),
            mergealloc(0), mergecopy(0),
            geqp3(0), geqrf(0), potf(0), trsm(0), gemm(0),
            buildq(0), scattq(0), perma(0), scatta(0), prese(0), assmb(0), phi(0)
            {}
};

template<typename T>
struct Stats {
    private:
        T min;
        T max;
        T sum;
        int count;
    public:
        Stats(): min(std::numeric_limits<T>::max()), max(std::numeric_limits<T>::lowest()), sum(0), count(0) {};
        void addData(T value) {
            this->min = (this->min < value ? this->min : value);
            this->max = (this->max > value ? this->max : value);
            this->sum += value;
            this->count += 1;
        }
        T      getMin()   const { return this->count == 0 ? 0 : this->min; }
        T      getMax()   const { return this->count == 0 ? 0 : this->max; }
        double getMean()  const { return ((double)this->sum)/((double)this->count); }
        int    getCount() const { return this->count; }
        T      getSum()   const { return this->sum; }
};

struct Log {
    public:
        int dofs_nd;
        int dofs_left_nd;
        int dofs_left_elim;
        int dofs_left_spars;
        long long int fact_nnz;
        Stats<int> rank_before;
        Stats<int> rank_after;
        Stats<int> nbrs;
        Stats<double> cond_diag;
        Stats<double> norm_diag;

        Log() :
            dofs_nd(0),
            dofs_left_nd(0), dofs_left_elim(0), dofs_left_spars(0),
            fact_nnz(0),
            rank_before(Stats<int>()), rank_after(Stats<int>()), nbrs(Stats<int>()),
            cond_diag(Stats<double>()), norm_diag(Stats<double>())
            {}
};

#endif


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

#include <config.h>

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

#include "recurse.h"

static int complex_tri_invert_L2(CBLAS_UPLO_t Uplo, CBLAS_DIAG_t Diag, gsl_matrix_complex * T);
static int complex_tri_invert_L3(CBLAS_UPLO_t Uplo, CBLAS_DIAG_t Diag, gsl_matrix_complex * T);
static int triangular_singular(const gsl_matrix_complex * T);

/*
gsl_linalg_complex_tri_invert()
  Invert a complex triangular matrix T using Level 3 BLAS

Inputs: Uplo - CblasUpper or CblasLower
        Diag - unit triangular?
        T    - on output the upper (or lower) part of T
               is replaced by its inverse

Return: success/error
*/

int
gsl_linalg_complex_tri_invert(CBLAS_UPLO_t Uplo, CBLAS_DIAG_t Diag, gsl_matrix_complex * T)
{
  const size_t N = T->size1;

  if (N != T->size2)
    {
      GSL_ERROR ("matrix must be square", GSL_ENOTSQR);
    }
  else
    {
      int status;

      status = triangular_singular(T);
      if (status)
        return status;

      return complex_tri_invert_L3(Uplo, Diag, T);
    }
}

/*
complex_tri_invert_L2()
  Invert a complex triangular matrix T

Inputs: Uplo - CblasUpper or CblasLower
        Diag - unit triangular?
        T    - on output the upper (or lower) part of T
               is replaced by its inverse

Return: success/error

Notes:
1) Based on LAPACK routine ZTRTI2 using Level 2 BLAS
*/

static int
complex_tri_invert_L2(CBLAS_UPLO_t Uplo, CBLAS_DIAG_t Diag, gsl_matrix_complex * T)
{
  const size_t N = T->size1;

  if (N != T->size2)
    {
      GSL_ERROR ("matrix must be square", GSL_ENOTSQR);
    }
  else
    {
      size_t i;

      if (Uplo == CblasUpper)
        {
          for (i = 0; i < N; ++i)
            {
              gsl_complex * Tii = gsl_matrix_complex_ptr(T, i, i);
              gsl_complex aii;

              if (Diag == CblasNonUnit)
                {
                  *Tii = gsl_complex_inverse(*Tii);
                  GSL_REAL(aii) = -GSL_REAL(*Tii);
                  GSL_IMAG(aii) = -GSL_IMAG(*Tii);
                }
              else
                aii = GSL_COMPLEX_NEGONE;

              if (i > 0)
                {
                  gsl_matrix_complex_view m = gsl_matrix_complex_submatrix(T, 0, 0, i, i);
                  gsl_vector_complex_view v = gsl_matrix_complex_subcolumn(T, i, 0, i);

                  gsl_blas_ztrmv(CblasUpper, CblasNoTrans, Diag, &m.matrix, &v.vector);
                  gsl_blas_zscal(aii, &v.vector);
                }
            }
        }
      else
        {
          for (i = 0; i < N; ++i)
            {
              size_t j = N - i - 1;
              gsl_complex * Tjj = gsl_matrix_complex_ptr(T, j, j);
              gsl_complex ajj;

              if (Diag == CblasNonUnit)
                {
                  *Tjj = gsl_complex_inverse(*Tjj);
                  GSL_REAL(ajj) = -GSL_REAL(*Tjj);
                  GSL_IMAG(ajj) = -GSL_IMAG(*Tjj);
                }
              else
                ajj = GSL_COMPLEX_NEGONE;

              if (j < N - 1)
                {
                  gsl_matrix_complex_view m = gsl_matrix_complex_submatrix(T, j + 1, j + 1, N - j - 1, N - j - 1);
                  gsl_vector_complex_view v = gsl_matrix_complex_subcolumn(T, j, j + 1, N - j - 1);

                  gsl_blas_ztrmv(CblasLower, CblasNoTrans, Diag, &m.matrix, &v.vector);
                  gsl_blas_zscal(ajj, &v.vector);
                }
            }
        }

      return GSL_SUCCESS;
    }
}

/*
complex_tri_invert_L3()
  Invert a complex triangular matrix T

Inputs: Uplo - CblasUpper or CblasLower
        Diag - unit triangular?
        T    - on output the upper (or lower) part of T
               is replaced by its inverse

Return: success/error

Notes:
1) Based on ReLAPACK using Level 3 BLAS
*/

static int
complex_tri_invert_L3(CBLAS_UPLO_t Uplo, CBLAS_DIAG_t Diag, gsl_matrix_complex * T)
{
  const size_t N = T->size1;

  if (N != T->size2)
    {
      GSL_ERROR ("matrix must be square", GSL_ENOTSQR);
    }
  else if (N <= CROSSOVER_INVTRI)
    {
      /* use Level 2 algorithm */
      return complex_tri_invert_L2(Uplo, Diag, T);
    }
  else
    {
      /*
       * partition matrix:
       *
       * T11 T12
       * T21 T22
       *
       * where T11 is N1-by-N1
       */
      int status;
      const size_t N1 = GSL_LINALG_SPLIT_COMPLEX(N);
      const size_t N2 = N - N1;
      gsl_matrix_complex_view T11 = gsl_matrix_complex_submatrix(T, 0, 0, N1, N1);
      gsl_matrix_complex_view T12 = gsl_matrix_complex_submatrix(T, 0, N1, N1, N2);
      gsl_matrix_complex_view T21 = gsl_matrix_complex_submatrix(T, N1, 0, N2, N1);
      gsl_matrix_complex_view T22 = gsl_matrix_complex_submatrix(T, N1, N1, N2, N2);

      /* recursion on T11 */
      status = complex_tri_invert_L3(Uplo, Diag, &T11.matrix);
      if (status)
        return status;

      if (Uplo == CblasLower)
        {
          /* T21 = - T21 * T11 */
          gsl_blas_ztrmm(CblasRight, Uplo, CblasNoTrans, Diag, GSL_COMPLEX_NEGONE, &T11.matrix, &T21.matrix);

          /* T21 = T22 * T21^{-1} */
          gsl_blas_ztrsm(CblasLeft, Uplo, CblasNoTrans, Diag, GSL_COMPLEX_ONE, &T22.matrix, &T21.matrix);
        }
      else
        {
          /* T12 = - T11 * T12 */
          gsl_blas_ztrmm(CblasLeft, Uplo, CblasNoTrans, Diag, GSL_COMPLEX_NEGONE, &T11.matrix, &T12.matrix);

          /* T12 = T12 * T22^{-1} */
          gsl_blas_ztrsm(CblasRight, Uplo, CblasNoTrans, Diag, GSL_COMPLEX_ONE, &T22.matrix, &T12.matrix);
        }

      /* recursion on T22 */
      status = complex_tri_invert_L3(Uplo, Diag, &T22.matrix);
      if (status)
        return status;

      return GSL_SUCCESS;
    }
}

static int
triangular_singular(const gsl_matrix_complex * T)
{
  size_t i;

  for (i = 0; i < T->size1; ++i)
    {
      gsl_complex z = gsl_matrix_complex_get(T, i, i);
      if (GSL_REAL(z) == 0.0 && GSL_IMAG(z) == 0.0)
        return GSL_ESING;
    }

  return GSL_SUCCESS;
}


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

#include <config.h>

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

#include "recurse.h"

static int triangular_multherm_L2(CBLAS_UPLO_t Uplo, gsl_matrix_complex * T);
static int triangular_multherm_L3(CBLAS_UPLO_t Uplo, gsl_matrix_complex * T);
static int triangular_mult_L2(CBLAS_UPLO_t Uplo, gsl_matrix_complex * LU);
static int triangular_mult_L3(CBLAS_UPLO_t Uplo, gsl_matrix_complex * A);
static void complex_conj_vector(gsl_vector_complex * v);

int
gsl_linalg_complex_tri_LHL(gsl_matrix_complex * L)
{
  return triangular_multherm_L3(CblasLower, L);
}

int
gsl_linalg_complex_tri_UL(gsl_matrix_complex * LU)
{
  return triangular_mult_L3(CblasUpper, LU);
}

/*
triangular_multherm_L2()
  Compute L^H L or U U^H

Inputs: Uplo - CblasUpper or CblasLower
        T    - on output the upper (or lower) part of T
               is replaced by L^H L or U U^H

Return: success/error

Notes:
1) Based on LAPACK routine ZLAUU2 using Level 2 BLAS
*/

static int
triangular_multherm_L2(CBLAS_UPLO_t Uplo, gsl_matrix_complex * T)
{
  const size_t N = T->size1;

  if (N != T->size2)
    {
      GSL_ERROR ("matrix must be square", GSL_ENOTSQR);
    }
  else
    {
      size_t i;

      if (Uplo == CblasUpper)
        {
        }
      else
        {
          for (i = 0; i < N; ++i)
            {
              gsl_complex * Tii = gsl_matrix_complex_ptr(T, i, i);
              gsl_complex z0 = *Tii;

              if (i < N - 1)
                {
                  gsl_vector_complex_view v = gsl_matrix_complex_subcolumn(T, i, i + 1, N - i - 1);
                  double norm = gsl_blas_dznrm2(&v.vector);

                  GSL_REAL(*Tii) = gsl_complex_abs2(*Tii) + norm * norm;

                  if (i > 0)
                    {
                      gsl_vector_complex_view w = gsl_matrix_complex_subrow(T, i, 0, i);
                      gsl_matrix_complex_view m = gsl_matrix_complex_submatrix(T, i + 1, 0, N - i - 1, i);

                      complex_conj_vector(&w.vector);
                      gsl_blas_zgemv(CblasConjTrans, GSL_COMPLEX_ONE, &m.matrix, &v.vector, z0, &w.vector);
                      complex_conj_vector(&w.vector);
                    }
                }
              else
                {
                  gsl_vector_complex_view w = gsl_matrix_complex_row(T, i);
                  gsl_blas_zdscal(GSL_REAL(z0), &w.vector);
                }

              GSL_IMAG(*Tii) = 0.0;
            }
        }

      return GSL_SUCCESS;
    }
}

/*
triangular_multherm_L3()
  Compute L^H L or U U^H

Inputs: Uplo - CblasUpper or CblasLower
        T    - on output the upper (or lower) part of T
               is replaced by L^H L or U U^H

Return: success/error

Notes:
1) Based on ReLAPACK using Level 3 BLAS
*/

static int
triangular_multherm_L3(CBLAS_UPLO_t Uplo, gsl_matrix_complex * T)
{
  const size_t N = T->size1;

  if (N != T->size2)
    {
      GSL_ERROR ("matrix must be square", GSL_ENOTSQR);
    }
  else if (N <= CROSSOVER_TRIMULT)
    {
      /* use Level 2 algorithm */
      return triangular_multherm_L2(Uplo, T);
    }
  else
    {
      /*
       * partition matrix:
       *
       * T11 T12
       * T21 T22
       *
       * where T11 is N1-by-N1
       */
      int status;
      const size_t N1 = GSL_LINALG_SPLIT_COMPLEX(N);
      const size_t N2 = N - N1;
      gsl_matrix_complex_view T11 = gsl_matrix_complex_submatrix(T, 0, 0, N1, N1);
      gsl_matrix_complex_view T12 = gsl_matrix_complex_submatrix(T, 0, N1, N1, N2);
      gsl_matrix_complex_view T21 = gsl_matrix_complex_submatrix(T, N1, 0, N2, N1);
      gsl_matrix_complex_view T22 = gsl_matrix_complex_submatrix(T, N1, N1, N2, N2);

      /* recursion on T11 */
      status = triangular_multherm_L3(Uplo, &T11.matrix);
      if (status)
        return status;

      if (Uplo == CblasLower)
        {
          /* T11 += T21^T T21 */
          gsl_blas_zherk(Uplo, CblasConjTrans, 1.0, &T21.matrix, 1.0, &T11.matrix);

          /* T21 = T22^T * T21 */
          gsl_blas_ztrmm(CblasLeft, Uplo, CblasConjTrans, CblasNonUnit, GSL_COMPLEX_ONE, &T22.matrix, &T21.matrix);
        }
      else
        {
          /* T11 += T12 T12^T */
          gsl_blas_zherk(Uplo, CblasNoTrans, 1.0, &T12.matrix, 1.0, &T11.matrix);

          /* T12 = T12 * T22^T */
          gsl_blas_ztrmm(CblasRight, Uplo, CblasConjTrans, CblasNonUnit, GSL_COMPLEX_ONE, &T22.matrix, &T12.matrix);
        }

      /* recursion on T22 */
      status = triangular_multherm_L3(Uplo, &T22.matrix);
      if (status)
        return status;

      return GSL_SUCCESS;
    }
}

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

/*
triangular_mult_L2()
  Compute U L or L U

Inputs: Uplo - CblasUpper or CblasLower (first triangular factor)
        LU   - on input, matrix in LU form;
               on output U*L or L*U

Return: success/error
*/

static int
triangular_mult_L2(CBLAS_UPLO_t Uplo, gsl_matrix_complex * LU)
{
  const size_t N = LU->size1;

  if (N != LU->size2)
    {
      GSL_ERROR ("matrix must be square", GSL_ENOTSQR);
    }
  else
    {
      size_t i;

      /* quick return */
      if (N == 1)
        return GSL_SUCCESS;

      if (Uplo == CblasUpper)
        {
          /* compute U*L and store in LU */

          for (i = 0; i < N; ++i)
            {
              gsl_complex * Aii = gsl_matrix_complex_ptr(LU, i, i);
              gsl_complex Uii = *Aii;

              if (i < N - 1)
                {
                  gsl_vector_complex_view lb = gsl_matrix_complex_subcolumn(LU, i, i + 1, N - i - 1);
                  gsl_vector_complex_view ur = gsl_matrix_complex_subrow(LU, i, i + 1, N - i - 1);
                  gsl_complex dot;

                  gsl_blas_zdotu(&lb.vector, &ur.vector, &dot);
                  *Aii = gsl_complex_add(*Aii, dot);

                  if (i > 0)
                    {
                      gsl_matrix_complex_view U_TR = gsl_matrix_complex_submatrix(LU, 0, i + 1, i, N - i - 1);
                      gsl_matrix_complex_view L_BL = gsl_matrix_complex_submatrix(LU, i + 1, 0, N - i - 1, i);
                      gsl_vector_complex_view ut = gsl_matrix_complex_subcolumn(LU, i, 0, i);
                      gsl_vector_complex_view ll = gsl_matrix_complex_subrow(LU, i, 0, i);

                      gsl_blas_zgemv(CblasTrans, GSL_COMPLEX_ONE, &L_BL.matrix, &ur.vector, Uii, &ll.vector);
                      gsl_blas_zgemv(CblasNoTrans, GSL_COMPLEX_ONE, &U_TR.matrix, &lb.vector, GSL_COMPLEX_ONE, &ut.vector);
                    }
                }
              else
                {
                  gsl_vector_complex_view v = gsl_matrix_complex_subrow(LU, i, 0, i);
                  gsl_blas_zscal(Uii, &v.vector);
                }
            }
        }
      else
        {
        }

      return GSL_SUCCESS;
    }
}

/*
triangular_mult_L3()
  Compute U L or L U

Inputs: Uplo - CblasUpper or CblasLower (for the first triangular factor)
        A    - on input, matrix in LU format;
               on output, U L or L U

Return: success/error
*/

static int
triangular_mult_L3(CBLAS_UPLO_t Uplo, gsl_matrix_complex * A)
{
  const size_t N = A->size1;

  if (N != A->size2)
    {
      GSL_ERROR ("matrix must be square", GSL_ENOTSQR);
    }
  else if (N <= CROSSOVER_TRIMULT)
    {
      return triangular_mult_L2(Uplo, A);
    }
  else
    {
      /* partition matrix:
       *
       * A11 A12
       * A21 A22
       *
       * where A11 is N1-by-N1
       */
      int status;
      const size_t N1 = GSL_LINALG_SPLIT_COMPLEX(N);
      const size_t N2 = N - N1;
      gsl_matrix_complex_view A11 = gsl_matrix_complex_submatrix(A, 0, 0, N1, N1);
      gsl_matrix_complex_view A12 = gsl_matrix_complex_submatrix(A, 0, N1, N1, N2);
      gsl_matrix_complex_view A21 = gsl_matrix_complex_submatrix(A, N1, 0, N2, N1);
      gsl_matrix_complex_view A22 = gsl_matrix_complex_submatrix(A, N1, N1, N2, N2);

      /* recursion on A11 */
      status = triangular_mult_L3(Uplo, &A11.matrix);
      if (status)
        return status;

      if (Uplo == CblasLower)
        {
        }
      else
        {
          /* form U * L */

          /* A11 += A12 A21 */
          gsl_blas_zgemm(CblasNoTrans, CblasNoTrans, GSL_COMPLEX_ONE, &A12.matrix, &A21.matrix, GSL_COMPLEX_ONE, &A11.matrix);

          /* A12 = A12 * L22 */
          gsl_blas_ztrmm(CblasRight, CblasLower, CblasNoTrans, CblasUnit, GSL_COMPLEX_ONE, &A22.matrix, &A12.matrix);

          /* A21 = U22 * A21 */
          gsl_blas_ztrmm(CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, GSL_COMPLEX_ONE, &A22.matrix, &A21.matrix);
        }

      /* recursion on A22 */
      status = triangular_mult_L3(Uplo, &A22.matrix);
      if (status)
        return status;

      return GSL_SUCCESS;
    }
}

static void
complex_conj_vector(gsl_vector_complex * v)
{
  size_t i;

  for (i = 0; i < v->size; ++i)
    {
      gsl_complex * vi = gsl_vector_complex_ptr(v, i);
      GSL_IMAG(*vi) = -GSL_IMAG(*vi);
    }
}


#pragma once

#include "gdx/algo/algorithm.h"
#include "gdx/exception.h"

#include <gsl/span>
#include <numeric>
#include <type_traits>
#include <unordered_map>

namespace gdx {

template <class T>
struct has_sum_member
{
    template <typename U, U u>
    struct reallyHas;

    template <typename C>
    static constexpr std::true_type test(reallyHas<double (C::*)(), &C::sum>* /*unused*/)
    {
        return {};
    }

    template <typename C>
    static constexpr std::true_type test(reallyHas<double (C::*)() const, &C::sum>* /*unused*/)
    {
        return {};
    }

    template <typename>
    static std::false_type test(...)
    {
        return {};
    }

    static constexpr bool value = decltype(test<T>(0))::value;
};

template <typename T>
inline constexpr bool has_sum_member_v = has_sum_member<T>::value;

template <typename RasterType>
typename std::enable_if_t<has_sum_member_v<RasterType>, double> sum(const RasterType& ras)
{
    return ras.template sum<double>();
}

template <typename RasterType>
typename std::enable_if_t<!has_sum_member_v<RasterType>, double> sum(const RasterType& ras)
{
    double sum      = 0.0;
    const auto size = ras.size();

    for (std::size_t i = 0; i < size; ++i) {
        if (!ras.is_nodata(i)) {
            sum += ras[i];
        }
    }

    return sum;
}

template <typename RasterType>
double ssum(const RasterType& ras)
{
    using T = typename RasterType::value_type;
    return static_cast<double>(std::accumulate(value_begin(ras), value_end(ras), T(0), std::plus<T>()));
}

}


#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>

#include "../src/fastlambertw.h"

#include "testmacros.h"

#include "../src/config.h"
#ifdef HAVE_GSL_GSL_SF_LAMBERT_H
#include <gsl/gsl_sf_lambert.h>
#endif

static inline float
lambertwrange (void)
{
  if (drand48 () < 0.5)
    {
      return -0.36787944117144232f + 1.36787944117144232f * drand48 ();
    }
  else
    {
      return 100.0f * drand48 ();
    }
}

static inline float
lambertwf (float x)
{
  float w = (x < 5) ? 0 : log (x) - log (log (x)) + log (log (x)) / log (x);
  unsigned int n;

  for (n = 0; n < 20; ++n)
    {
      w = (w * w + exp (-w) * x) / (1.0 + w);
    }

  return w;
}

static inline float
lambertwexpxf (float x)
{
  return lambertwf (expf (x));
}

test_scalar (fastlambertw, lambertwf, lambertwrange (), 1e-4f, 100000000)
test_scalar (fasterlambertw, lambertwf, lambertwrange (), 1e-2f, 100000000)
test_scalar (fastlambertwexpx, lambertwexpxf, -3.0f + 6.0f * drand48 (), 1e-3f, 100000000)
test_scalar (fasterlambertwexpx, lambertwexpxf, -3.0f + 6.0f * drand48 (), 1e-2f, 100000000)

#ifdef HAVE_GSL_GSL_SF_LAMBERT_H
test_scalar (gsl_sf_lambert_W0, lambertwf, lambertwrange (), 1e-2f, 1000000)
#endif

test_vector (vfastlambertw, lambertwf, lambertwrange (), 1e-4f, 100000000)
test_vector (vfasterlambertw, lambertwf, lambertwrange (), 1e-2f, 100000000)
test_vector (vfastlambertwexpx, lambertwexpxf, -3.0f + 6.0f * drand48 (), 1e-3f, 100000000)
test_vector (vfasterlambertwexpx, lambertwexpxf, -3.0f + 6.0f * drand48 (), 1e-2f, 100000000)

int
main (int   argc,
      char *argv[])
{
  char buf[4096];

  (void) argc;

  srand48 (69);

//  fprintf (stderr, "fastlambertw (%g) = %g, "
//                   "fastlambertw (%g) = %g, "
//                   "fasterlambertwexpx (%g) = %g (%g)\n",
//           -0.36787944117144232f,
//           fastlambertw (-0.36787944117144232f),
//           -0.36787944117144232f + 0.01f,
//           fastlambertw (-0.36787944117144232f + 0.01f),
//           -5.0f,
//           fasterlambertwexpx (-5.0f),
//           v4sf_index (vfasterlambertwexpx (v4sfl (-5.0f)), 0));

  strncpy (buf, argv[0], sizeof (buf) - 5);
  strncat (buf, ".out", 5);

  fclose (stderr);
  stderr = fopen (buf, "w");

  test_fastlambertw ();
  test_fasterlambertw ();
  test_vfastlambertw ();
  test_vfasterlambertw ();
#ifdef HAVE_GSL_GSL_SF_LAMBERT_H
  test_gsl_sf_lambert_W0 ();
#endif
  test_fastlambertwexpx ();
  test_fasterlambertwexpx ();
  test_vfastlambertwexpx ();
  test_vfasterlambertwexpx ();

  time_fastlambertw ();
  time_fasterlambertw ();
  time_vfastlambertw ();
  time_vfasterlambertw ();
#ifdef HAVE_GSL_GSL_SF_LAMBERT_H
  time_gsl_sf_lambert_W0 ();
#endif
  time_fastlambertwexpx ();
  time_fasterlambertwexpx ();
  time_vfastlambertwexpx ();
  time_vfasterlambertwexpx ();

  return 0;
}


#ifndef __RNG_H__
#define __RNG_H__

#include <gsl/gsl_rng.h>
#include <ctime>
#include<iostream>
#include <cstring>
#include <fstream>

#define ABS(x) ((x) >= 0 ? (x) : -(x))
#define min(a,b) ((a) <= (b) ? (a) : (b))
#define max(a,b) ((a) >= (b) ? (a) : (b))
#define OVERFLO 1e100
#define UNDERFLO 1e-100

namespace weakarg
{

extern gsl_rng * rng;

unsigned long makerng(bool fast=false);
unsigned long seedrng(unsigned long seed=0);
int saverng(std::string fname);
int loadrng(std::string fname);

double RandomReal(double low, double high);
int RandomInteger(int low, int high);
double rnd();
double RGamma(double n,double lambda);
void RDirichlet(const double * a, const int k, double * b);
long RPoisson(double mu);
double RExpon(double av);
double RNormal(double mu,double sd) ;
double fsign( double num, double sign );
double sexpo(void);
 double snorm();
 double genexp(double av);
 long ignpoi(double mean);
 long ignuin(int low, int high);
 double genunf(double low, double high);
 long   Binomial(int n, double p);
 long   Binomial1(int n, double p);
 double BinoProb(int n, double p,int i);
 void LogRDirichlet (const double *a, const int k, double *b,double *c);

} // end namespace weakarg
#endif // __RNG_H__


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

#ifndef _GSL_BOOST_UBLAS_MATRIX_PROD_
#define _GSL_BOOST_UBLAS_MATRIX_PROD_

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/symmetric.hpp>

#include <gsl/gsl_blas.h>

namespace boost { namespace numeric { namespace ublas {

    /*
     * GSL has separate implementations for floats and doubles
     * hence we first have specializations for these two types
     * TSeparate templates for diagonal and symmetric matreces
     * and their combinations. To check  whether a specific
     * routine is called, define GSLDEBUG
     */

    // partial specialization for double precision (dgemm))
    template<class F, class A>
    inline matrix<double,F,A>    // prod( m1, m2 )
    prod(const matrix<double,F,A> &m1, const matrix<double,F,A> &m2)
    {
       #ifdef GSLDEBUG
          std::cout << "\x1b[0;34mGSL [CMD,CM]\x1b[0;39m\n" << std::flush;
       #endif

       gsl_matrix_const_view mA = gsl_matrix_const_view_array (&m1(0,0), m1.size1(), m1.size2());
       gsl_matrix_const_view mB = gsl_matrix_const_view_array (&m2(0,0), m2.size1(), m2.size2());

       boost::numeric::ublas::matrix<double,F,A> AxB( m1.size1(), m2.size2() );
       gsl_matrix_view mC = gsl_matrix_view_array (&AxB(0,0), AxB.size1(), AxB.size2());

       gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
                  1.0, &mA.matrix, &mB.matrix,
                  0.0, &mC.matrix);
       return AxB;
    }

    // partial specialization for single precision (sgemm))
    template<class F, class A>
    inline matrix<float,F,A>    // prod( m1, m2 )
    prod(const matrix<float,F,A> &m1, const matrix<float,F,A> &m2)
    {
       #ifdef GSLDEBUG
          std::cout << "\x1b[0;34mGSL [CMS,CM]\x1b[0;39m\n" << std::flush;
       #endif

       gsl_matrix_float_const_view mA = gsl_matrix_float_const_view_array (&m1(0,0), m1.size1(), m1.size2());
       gsl_matrix_float_const_view mB = gsl_matrix_float_const_view_array (&m2(0,0), m2.size1(), m2.size2());

       boost::numeric::ublas::matrix<float,F,A> AxB( m1.size1(), m2.size2() );
       gsl_matrix_float_view mC = gsl_matrix_float_view_array (&AxB(0,0), AxB.size1(), AxB.size2());

       gsl_blas_sgemm (CblasNoTrans, CblasNoTrans,
                  1.0, &mA.matrix, &mB.matrix,
                  0.0, &mC.matrix);
       return AxB;
    }

    // full specialization
    template<class T, class F, class A>
    inline matrix<T,F,A>    // prod( m1, m2 )
    prod(const matrix<T,F,A> &m1, const matrix<T,F,A> &m2)
    {
       #ifdef GSLDEBUG
          std::cout << "\x1b[0;34mGSL [CM,CM]\x1b[0;39m -> " << std::flush;
       #endif

       return prod(m1,m2);
    }

    /// transpose products
    template<class T, class F, class A>
    inline matrix<T,F,A>    // prod( trans(m1), m2 )
    prod(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m1, const matrix<T,F,A> &m2)
    {
       #ifdef GSLDEBUG
          std::cout << "\x1b[0;32mGSL [CTM,CM]\x1b[0;39m -> " << std::flush;
       #endif
       boost::numeric::ublas::matrix<T,F,A> _m1 = m1;
       return prod(_m1,m2);
    }

    template<class T, class F, class A>
    inline matrix<T,F,A>    // prod( m1, trans(m2) )
    prod(const matrix<T,F,A> &m1, const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m2)
    {
       #ifdef GSLDEBUG
          std::cout << "\x1b[0;32mGSL [CM,CTM]\x1b[0;39m -> " << std::flush;
       #endif

       const boost::numeric::ublas::matrix<T,F,A> _m2 = m2;
       return prod(m1,_m2);
    }

    template<class T, class F, class A>
    inline matrix<T,F,A>    // prod( trans(m1), trans(m2) )
    prod(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m1, const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m2)
    {
       #ifdef GSLDEBUG
          std::cout << "\x1b[0;32mGSL [CTM,CTM]\x1b[0;39m -> " << std::flush;
       #endif

       boost::numeric::ublas::matrix<T,F,A> _m1 = m1;
       boost::numeric::ublas::matrix<T,F,A> _m2 = m2;

       return prod(_m1,_m2);
    }

    /// diagonal matrix
    template<class T, class F, class L, class A>
    inline matrix<T,F,A>    // prod( diagonal m1, m2 )
    prod(const diagonal_matrix<T,L,A> &m1, const matrix<T,F,A> &m2 )
    {
       #ifdef GSLDEBUG
          std::cout << "\x1b[0;33mGSL [CDM, CM]\x1b[0;39m -> " << std::flush;
       #endif
       const matrix<T,F,A> _m1 = m1;
       return prod(_m1,m2);
    }

    template<class T, class F, class L, class A>
    inline matrix<T,F,A>    // prod( m1, diagonal m2 )
    prod(const matrix<T,F,A> &m1, const diagonal_matrix<T,L,A> &m2 )
    {
       #ifdef GSLDEBUG
          std::cout << "\x1b[0;33mGSL [CM, CDM]\x1b[0;39m -> " << std::flush;
       #endif
       const matrix<T,F,A> _m2 = m2;
       return prod(m1,_m2);
    }

    template<class T, class F, class L, class A>
    inline matrix<T,F,A>    // prod( diagonal m1, diagonal m2 )
    prod(const diagonal_matrix<T,L,A> &m1, const diagonal_matrix<T,L,A> &m2 )
    {
       #ifdef GSLDEBUG
          std::cout << "\x1b[0;33mGSL [CDM, CM]\x1b[0;39m -> " << std::flush;
       #endif
       const matrix<T,F,A> _m1 = m1;
       const matrix<T,F,A> _m2 = m2;
       return prod(_m1,_m2);
    }

    template<class T, class F, class L, class A>
    inline matrix<T,F,A>    // prod( diagonal m1, transpose m2 )
    prod(const diagonal_matrix<T,L,A> &m1, const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m2)
    {
       #ifdef GSLDEBUG
          std::cout << "\x1b[0;33mGSL [CDM, CTM]\x1b[0;39m -> " << std::flush;
       #endif

       const matrix<T,F,A> _m1 = m1;
       const matrix<T,F,A> _m2 = m2;

       return prod(_m1,_m2);

    }

   template<class T, class F, class L, class A>
    inline matrix<T,F,A>    // prod( transpose m1, diagonal m2 )
    prod(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m1, const diagonal_matrix<T,L,A> &m2)
    {
       #ifdef GSLDEBUG
          std::cout << "\x1b[0;33mGSL [CTM, CDM]\x1b[0;39m -> " << std::flush;
       #endif

       const matrix<T,F,A> _m1 = m1;
       const matrix<T,F,A> _m2 = m2;

       return prod(_m1,_m2);

    }

    // symmetric matrix
    template<class T, class F, class A, class TRI, class L>
    inline matrix<T,F,A>    // prod( symmetric m1, m2 )
    prod(symmetric_matrix<T, TRI, L, A> &m1, matrix<T,F,A> &m2 )
    {
       #ifdef GSLDEBUG
          std::cout << "\x1b[0;33mGSL [CSM, CM]\x1b[0;39m -> " << std::flush;
       #endif

       assert( m1.size1() == m2.size1() );
       const matrix<T,F,A> _m1 = m1;

       return prod(_m1, m2 );
    }

    template<class T, class F, class A, class TRI, class L>
    inline matrix<T,F,A>    // prod( m1, symmetric m2 )
    prod( matrix<T,F,A> &m1, symmetric_matrix<T, TRI, L, A> &m2 )
    {
       #ifdef GSLDEBUG
          std::cout << "\x1b[0;33mGSL [CSM, CM]\x1b[0;39m -> " << std::flush;
       #endif

       assert( m1.size1() == m2.size1() );
       const matrix<T,F,A> _m2 = m2;

       return prod(m1, _m2 );
    }

    template<class T, class F, class A, class TRI, class L>
    inline matrix<T,F,A>    // prod( symmetric m1, symmetric m2 )
    prod(symmetric_matrix<T, TRI, L, A> &m1, symmetric_matrix<T, TRI, L, A> &m2 )
    {
       #ifdef GSLDEBUG
          std::cout << "\x1b[0;33mGSL [CSM, CM]\x1b[0;39m -> " << std::flush;
       #endif

       assert( m1.size1() == m2.size1() );
       const matrix<T,F,A> _m1 = m1;
       const matrix<T,F,A> _m2 = m2;

       return prod(_m1, _m2 );
    }

}}}

#endif  // _GSL_BOOST_UBLAS_MATRIX_PROD_


/*
 * author: Achim Gaedke
 * created: May 2001
 * file: pygsl/src/sfmodule.c
 * $Id: sfmodule.c,v 1.3 2006/07/13 16:01:00 schnizer Exp $
 */
#include <pygsl/error_helpers.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_errno.h>

static PyObject* gsl_module_error;

int eval_gsl_mode_char(gsl_mode_t* mode, char mode_char) {

  char error_text[]="illegal gsl_mode";
  PyObject* gsl_error_object;
  PyObject* gsl_error_module;
  PyObject* gsl_error_dict;

  switch (mode_char)
    {
    case 's':
    case 'S':
      *mode=GSL_PREC_SINGLE;
      return 0;
    case 'a':
    case 'A':
      *mode=GSL_PREC_APPROX;
      return 0;
    case 'd':
    case 'D':
      *mode=GSL_PREC_DOUBLE;
      return 0;
    }
  /* must set exception */
  gsl_error_module=PyImport_ImportModule("pygsl.errors");
  gsl_error_dict=PyModule_GetDict(gsl_error_module);
  gsl_error_object=PyDict_GetItemString(gsl_error_dict,"gsl_Error");
  PyErr_SetObject(gsl_error_object,
		  PyString_FromString(error_text));

  return -1;
}

static PyObject* gsl_sf_gauss(PyObject *self,
			      PyObject *args
			      )
{
  const double norm=1.0/M_SQRT2/M_SQRTPI;
  double x;
  double sigma=1.0;
  double mu=0;
  double tmp;
  gsl_sf_result result;
  int err_result;
  PyObject* python_result;

  if (!PyArg_ParseTuple(args, "d|dd:gauss", &x, &mu,  &sigma))
    return NULL;

  /*use tmp as temporary variable*/
  tmp=((x-mu)/sigma);
  err_result=gsl_sf_exp_mult_e(tmp*tmp/-2.0,
				  norm/sigma,
				  &result);
  if (err_result)
    return NULL;

  python_result=PyTuple_New(2);
  if (python_result==NULL)
    return NULL;
  PyTuple_SetItem(python_result,0,PyFloat_FromDouble(result.val));
  PyTuple_SetItem(python_result,1,PyFloat_FromDouble(result.err));
  return python_result;
}

/*automatically wrapped functions*/
#include "sf_functions.c"

static PyMethodDef sfMethods[] = {
#include "sf_index.c"
  {"gauss",gsl_sf_gauss, METH_VARARGS},
  {NULL,     NULL}        /* Sentinel */
};

DL_EXPORT(void) initsf(void)
{
  PyObject* gsl_errors_module;
  PyObject* sf_module;
  PyObject* gsl_errors_dict;

  gsl_errors_module=PyImport_ImportModule ("pygsl.errors");
  init_pygsl();
  /* here is an error check needed! */
  gsl_errors_dict=PyModule_GetDict(gsl_errors_module);
  gsl_module_error=PyDict_GetItemString(gsl_errors_dict,"gsl_Error");
  sf_module=Py_InitModule("sf", sfMethods);

  return;
}


/*
ODE: a program to get optime Runge-Kutta and multi-steps methods.

Copyright 2011-2019, Javier Burguete Tolosa.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

	1. Redistributions of source code must retain the above copyright notice,
		this list of conditions and the following disclaimer.

	2. Redistributions in binary form must reproduce the above copyright notice,
		this list of conditions and the following disclaimer in the
		documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY Javier Burguete Tolosa ``AS IS'' AND ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL Javier Burguete Tolosa OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

/**
 * \file rk_3_2.c
 * \brief Source file to optimize Runge-Kutta 3 steps 2nd order methods.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2011-2019.
 */
#define _GNU_SOURCE
#include <string.h>
#include <math.h>
#include <libxml/parser.h>
#include <glib.h>
#include <libintl.h>
#include <gsl/gsl_rng.h>
#include "config.h"
#include "utils.h"
#include "optimize.h"
#include "rk.h"
#include "rk_3_2.h"

#define DEBUG_RK_3_2 0          ///< macro to debug.

/**
 * Function to obtain the coefficients of a 3 steps 2nd order Runge-Kutta
 * method.
 */
int
rk_tb_3_2 (Optimize * optimize) ///< Optimize struct.
{
  long double *tb, *r;
#if DEBUG_RK_3_2
  fprintf (stderr, "rk_tb_3_2: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t3 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  b21 (tb) = r[2];
  b32 (tb) = r[3];
  b31 (tb) = (0.5L - b32 (tb) * t2 (tb)) / t1 (tb);
  if (isnan (b31 (tb)))
    return 0;
  rk_b_3 (tb);
#if DEBUG_RK_3_2
  rk_print_tb (optimize, "rk_tb_3_2", stderr);
  fprintf (stderr, "rk_tb_3_2: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 3 steps 2nd order, 3rd order in
 * equations depending only in time, Runge-Kutta method.
 */
int
rk_tb_3_2t (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb, *r;
#if DEBUG_RK_3_2
  fprintf (stderr, "rk_tb_3_2t: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t3 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  b21 (tb) = r[2];
  b32 (tb) = (1.L / 3.L - 0.5L * t1 (tb)) / (t2 (tb) * (t2 (tb) - t1 (tb)));
  if (isnan (b32 (tb)))
    return 0;
  b31 (tb) = (0.5L - b32 (tb) * t2 (tb)) / t1 (tb);
  if (isnan (b31 (tb)))
    return 0;
  rk_b_3 (tb);
#if DEBUG_RK_3_2
  rk_print_tb (optimize, "rk_tb_3_2t", stderr);
  fprintf (stderr, "rk_tb_3_2t: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 3 steps 1st-2nd order Runge-Kutta
 * pair.
 */
int
rk_tb_3_2p (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_3_2
  fprintf (stderr, "rk_tb_3_2p: start\n");
#endif
  if (!rk_tb_3_2 (optimize))
    return 0;
  tb = optimize->coefficient;
  e31 (tb) = 0.L;
  rk_e_3 (tb);
#if DEBUG_RK_3_2
  rk_print_e (optimize, "rk_tb_3_2p", stderr);
  fprintf (stderr, "rk_tb_3_2p: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 3 steps 1st-2nd order, 1st-3rd order
 * in equations depending only in time, Runge-Kutta method.
 */
int
rk_tb_3_2tp (Optimize * optimize)       ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_3_2
  fprintf (stderr, "rk_tb_3_2tp: start\n");
#endif
  if (!rk_tb_3_2t (optimize))
    return 0;
  tb = optimize->coefficient;
  e31 (tb) = 0.L;
  rk_e_3 (tb);
#if DEBUG_RK_3_2
  rk_print_tb (optimize, "rk_tb_3_2tp", stderr);
  fprintf (stderr, "rk_tb_3_2pt: end\n");
#endif
  return 1;
}

/**
 * Function to calculate the objective function of a 3 steps 2nd order
 * Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_3_2 (RK * rk)   ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_3_2
  fprintf (stderr, "rk_objective_tb_3_2: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b31 (tb) < 0.L)
    o += b31 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L + fmaxl (1.L, fmaxl (t1 (tb), t2 (tb)));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_3_2
  fprintf (stderr, "rk_objective_tb_3_2: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_3_2: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 3 steps 2nd order, 3rd
 * order in equations depending only in time, Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_3_2t (RK * rk)  ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_3_2
  fprintf (stderr, "rk_objective_tb_3_2t: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b31 (tb) < 0.L)
    o += b31 (tb);
  if (b32 (tb) < 0.L)
    o += b32 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L + fmaxl (1.L, fmaxl (t1 (tb), t2 (tb)));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_3_2
  fprintf (stderr, "rk_objective_tb_3_2t: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_3_2t: end\n");
#endif
  return o;
}


#ifndef MSNHINFERENCECFG_H
#define MSNHINFERENCECFG_H
#include <stdint.h>
#include <float.h>
#include <string>
#include <vector>
#include <chrono>
#include "Msnhnet/utils/MsnhException.h"
#include "Msnhnet/config/MsnhnetMacro.h"
#include <math.h>
#include <string.h>

#ifdef USE_OMP
#include <omp.h>
#endif

#ifdef USE_NEON
#include <arm_neon.h>
#endif

#ifdef USE_OPEN_BLAS
#include <cblas.h>
#endif

#ifdef USE_OPENGL
#include <Msnhnet/config/MsnhnetOpenGL.h>
#endif

#ifndef OMP_THREAD
#define OMP_THREAD omp_get_max_threads()
#endif

#define CUDA_THREADS 512

#define MIN_OMP_DATA 10000

#define MSNHNET_VERSION 1200

#define EFFCIENT_ALIGN 16

enum ActivationType
{
    LOGISTIC,
    RELU,
    RELU6,
    RELIE,
    RAMP,
    TANH,
    PRELU,
    PLSE,
    LEAKY,
    ELU,
    LOGGY,
    STAIR,
    HARDTAN,
    LHTAN,
    SOFT_PLUS,
    SELU,
    SWISH,
    HARD_SWISH,
    MISH,
    NORM_CHAN,
    NORM_CHAN_SOFTMAX,
    NORM_CHAN_SOFTMAX_MAXVAL,
    NONE
};

enum LayerType
{
    CONVOLUTIONAL,
    DECONVOLUTIONAL,
    CONNECTED,
    MAXPOOL,
    LOCAL_AVGPOOL,
    GLOBAL_AVGPOOL,
    SOFTMAX,
    CROP,
    ROUTE,
    VARIABLE_OP,
    NORMALIZATION,
    AVGPOOL,
    ACTIVE,
    BATCHNORM,
    NETWORK,
    YOLO,
    YOLO_OUT,
    GAUSSIAN_YOLO,
    UPSAMPLE,
    L2NORM,
    EMPTY,
    VIEW,
    PERMUTE,
    PIXEL_SHUFFLE,
    SLICE,
    REDUCTION,
    CONFIG,
    RES_BLOCK,
    RES_2_BLOCK,
    CONCAT_BLOCK,
    ADD_BLOCK,
    PADDING
};

enum Arithmetic
{
    ARITH_ADD = 0,
    ARITH_SUB,
    ARITH_SUB_INV,
    ARITH_MUL,
    ARITH_DIV,
    ARITH_DIV_INV
};

enum Scientific
{
    SCI_ABS=0,
    SCI_ACOS,
    SCI_ASIN,
    SCI_ATAN,
    SCI_COS,
    SCI_COSH,
    SCI_SIN,
    SCI_SINH,
    SCI_TAN,
    SCI_TANH,
    SCI_EXP,
    SCI_POW,
    SCI_LOG,
    SCI_LOG10,
    SCI_SQRT
};

enum ReductionType
{
    REDUCTION_SUM,
    REDUCTION_MEAN
};

enum WeightsType
{
    NO_WEIGHTS,
    PER_FEATURE,
    PER_CHANNEL
};

enum WeightsNorm
{
    NO_NORM,
    RELU_NORM,
    SOFTMAX_NORM
};

#endif


/* 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 Foundation, Inc., 59 Temple Place, Suite
   330, Boston, MA 02111-1307 USA

   From Robert M. Ziff, "Four-tap shift-register-sequence
   random-number generators," Computers in Physics 12(4), Jul/Aug
   1998, pp 385-392.  A generalized feedback shift-register (GFSR)
   is basically an xor-sum of particular past lagged values.  A
   four-tap register looks like:
      ra[nd] = ra[nd-A] ^ ra[nd-B] ^ ra[nd-C] ^ ra[nd-D]

   Ziff notes that "it is now widely known" that two-tap registers
   have serious flaws, the most obvious one being the three-point
   correlation that comes from the defn of the generator.  Nice
   mathematical properties can be derived for GFSR's, and numerics
   bears out the claim that 4-tap GFSR's with appropriately chosen
   offsets are as random as can be measured, using the author's test.

   This implementation uses the values suggested the the author's
   example on p392, but altered to fit the GSL framework.  The "state"
   is 2^14 longs, or 64Kbytes; 2^14 is the smallest power of two that
   is larger than D, the largest offset.  We really only need a state
   with the last D values, but by going to a power of two, we can do a
   masking operation instead of a modulo, and this is presumably
   faster, though I haven't actually tried it.  The article actually
   suggested a short/fast hack:

   #define RandomInteger (++nd, ra[nd&M]=ra[(nd-A)&M]\
                          ^ra[(nd-B)&M]^ra[(nd-C)&M]^ra[(nd-D)&M])

   so that (as long as you've defined nd,ra[M+1]), then you ca do things
   like: 'if (RandomInteger < p) {...}'.

   Note that n&M varies from 0 to M, *including* M, so that the
   array has to be of size M+1.  Since M+1 is a power of two, n&M
   is a potentially quicker implementation of the equivalent n%(M+1).

   This implementation copyright (C) 1998 James Theiler, based on
   the example mt.c in the GSL, as implemented by Brian Gough.
*/

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>

static inline unsigned long int gfsr4_get (void *vstate);
static double gfsr4_get_double (void *vstate);
static void gfsr4_set (void *state, unsigned long int s);

/* Magic numbers */
#define A 471
#define B 1586
#define C 6988
#define D 9689
#define M 16383 /* = 2^14-1 */
/* #define M 0x0003fff */

typedef struct
  {
    int nd;
    unsigned long ra[M+1];
  }
gfsr4_state_t;

static inline unsigned long
gfsr4_get (void *vstate)
{
  gfsr4_state_t *state = (gfsr4_state_t *) vstate;

  state->nd = ((state->nd)+1)&M;
  return state->ra[(state->nd)] =
      state->ra[((state->nd)+(M+1-A))&M]^
      state->ra[((state->nd)+(M+1-B))&M]^
      state->ra[((state->nd)+(M+1-C))&M]^
      state->ra[((state->nd)+(M+1-D))&M];

}

static double
gfsr4_get_double (void * vstate)
{
  return gfsr4_get (vstate) / 4294967296.0 ;
}

static void
gfsr4_set (void *vstate, unsigned long int s)
{
  gfsr4_state_t *state = (gfsr4_state_t *) vstate;
  int i, j;
  /* Masks for turning on the diagonal bit and turning off the
     leftmost bits */
  unsigned long int msb = 0x80000000UL;
  unsigned long int mask = 0xffffffffUL;

  if (s == 0)
    s = 4357;   /* the default seed is 4357 */

  /* We use the congruence s_{n+1} = (69069*s_n) mod 2^32 to
     initialize the state. This works because ANSI-C unsigned long
     integer arithmetic is automatically modulo 2^32 (or a higher
     power of two), so we can safely ignore overflow. */

#define LCG(n) ((69069 * n) & 0xffffffffUL)

  /* Brian Gough suggests this to avoid low-order bit correlations */
  for (i = 0; i <= M; i++)
    {
      unsigned long t = 0 ;
      unsigned long bit = msb ;
      for (j = 0; j < 32; j++)
        {
          s = LCG(s) ;
          if (s & msb)
            t |= bit ;
          bit >>= 1 ;
        }
      state->ra[i] = t ;
    }

  /* Perform the "orthogonalization" of the matrix */
  /* Based on the orthogonalization used in r250, as suggested initially
   * by Kirkpatrick and Stoll, and pointed out to me by Brian Gough
   */

  /* BJG: note that this orthogonalisation doesn't have any effect
     here because the the initial 6695 elements do not participate in
     the calculation.  For practical purposes this orthogonalisation
     is somewhat irrelevant, because the probability of the original
     sequence being degenerate should be exponentially small. */

  for (i=0; i<32; ++i) {
      int k=7+i*3;
      state->ra[k] &= mask;     /* Turn off bits left of the diagonal */
      state->ra[k] |= msb;      /* Turn on the diagonal bit           */
      mask >>= 1;
      msb >>= 1;
  }

  state->nd = i;
}

static const gsl_rng_type gfsr4_type =
{"gfsr4",                       /* name */
 0xffffffffUL,                  /* RAND_MAX  */
 0,                             /* RAND_MIN  */
 sizeof (gfsr4_state_t),
 &gfsr4_set,
 &gfsr4_get,
 &gfsr4_get_double};

const gsl_rng_type *gsl_rng_gfsr4 = &gfsr4_type;