/// \file
/// A helper class defining numerical complex matrices for use with BLAS.
/// Needs a CBLAS compatible BLAS library!

#pragma once
#include <array>
#include <complex>
#include <iomanip>
#include <iostream>
#include <limits>

#ifndef USE_MKL
#include <cblas.h>
#else
#include <mkl.h>
#endif

// **********************************************************************
/// Numerical complex matrix class
/// \brief This class defines a complex matrix with n rows and n columns
///
/// This class implements a complex matrix. The entries are stored in an
/// std::array in row-major order, so that it is easy to implement linear
/// algebra functions using the standard CBLAS interface.
// **********************************************************************
template <std::size_t n>
class nummat {

 private:
  std::array<std::complex<double>, n * n> mat;

 public:
  // **********************************************************************
  // Constructors
  // **********************************************************************

  constexpr nummat() { mat.fill(std::complex<double>(0.0)); }
  /// Construct a matrix from an array
  constexpr nummat(std::array<std::complex<double>, n * n> const &init)
    : mat(init)
  {
  }
  constexpr nummat(std::complex<double> const &c)
  {
    mat.fill(std::complex<double>(0.0));
    for (auto i = 0LU; i < n; ++i) {
      mat[i + n * i] = c;
    }
  }

  ///! Copy constructor
  constexpr nummat(nummat<n> const &orig) = default;

  // **********************************************************************
  // Methods
  // **********************************************************************

  /// Access elements of matrix
  std::complex<double> &operator()(std::size_t const i, std::size_t const j)
  {
    return mat[i * n + j];
  }

  constexpr std::complex<double> const &operator()(std::size_t const i,
                                                   std::size_t const j) const
  {
    return mat[i * n + j];
  }

  std::complex<double> *data() noexcept { return mat.data(); }

  std::complex<double> const *data() const noexcept { return mat.data(); }

  /// Matrix multiplication
  constexpr nummat<n> operator*(nummat<n> const &rhs) const
  {

    constexpr std::complex<double> alpha(1.0);
    constexpr std::complex<double> beta(0.0);

    nummat<n> res;

    cblas_zgemm(CBLAS_ORDER::CblasRowMajor, CBLAS_TRANSPOSE::CblasNoTrans,
                CBLAS_TRANSPOSE::CblasNoTrans, n, n, n,
                reinterpret_cast<const double *>(&alpha),
                reinterpret_cast<const double *>(this->data()), n,
                reinterpret_cast<const double *>(rhs.data()), n,
                reinterpret_cast<const double *>(&beta),
                reinterpret_cast<double *>(res.data()), n);
    return res;
  }

  /// Matrix subtraction
  constexpr nummat<n> operator-(nummat<n> const &rhs) const
  {

    constexpr std::complex<double> alpha(-1.0);

    nummat<n> res(this->mat);

    cblas_zaxpy(n * n, reinterpret_cast<const double *>(&alpha),
                reinterpret_cast<const double *>(rhs.data()), 1,
                reinterpret_cast<double *>(res.data()), 1);
    return res;
  }

  /// Matrix addition
  constexpr nummat<n> operator+(nummat<n> const &rhs) const
  {

    std::complex<double> alpha(1.0);

    nummat<n> res(this->mat);

    cblas_zaxpy(n * n, reinterpret_cast<const double *>(&alpha),
                reinterpret_cast<const double *>(rhs.data()), 1,
                reinterpret_cast<double *>(res.data()), 1);
    return res;
  }

  /// Operator "+=" for matrix addition
  void operator+=(nummat<n> const &rhs)
  {

    constexpr std::complex<double> alpha(1.0);

    cblas_zaxpy(n * n, reinterpret_cast<const double *>(&alpha),
                reinterpret_cast<const double *>(rhs.data()), 1,
                reinterpret_cast<double *>(this->data()), 1);
  }

  /// Operator "-=" for matrix subtraction
  void operator-=(nummat<n> const &rhs)
  {

    constexpr std::complex<double> alpha(-1.0);

    cblas_zaxpy(n * n, reinterpret_cast<const double *>(&alpha),
                reinterpret_cast<const double *>(rhs.data()), 1,
                reinterpret_cast<double *>(this->data()), 1);
  }

  /// Scalar multiplication (from right)
  nummat<n> operator*(std::complex<double> const &rhs) const
  {

    nummat<n> res(this->mat);

    cblas_zscal(n * n, reinterpret_cast<const double *>(&rhs),
                reinterpret_cast<double *>(res.data()), 1);
    return res;
  }

  /// Operator "*=" for scalar multiplication. (Rescaling)
  void operator*=(std::complex<double> const &a)
  {
    cblas_zscal(n * n, reinterpret_cast<const double *>(&a),
                reinterpret_cast<double *>(this->data()), 1);
  }

  /// Scalar division
  constexpr nummat<n> operator/(std::complex<double> const &rhs) const
  {
    return (*this) * (1. / rhs);
  }

  /// Equality check \todo Better float equal?
  constexpr bool operator==(nummat<n> const &right) const
  {
    bool res = true;
    for (auto i = 0LU; i < n; ++i) {
      for (auto j = 0LU; j < n; ++j) {
        res = ((*this)(i, j) == right(i, j));
        if (not res)
          return res;
      }
    }
    return res;
  }

  // Multiplication with Hermitian conjugate
  constexpr nummat<n> mult_conj(nummat<n> const &rhs) const
  {

    constexpr std::complex<double> alpha(1.0);
    constexpr std::complex<double> beta(0.0);

    nummat<n> res;

    cblas_zgemm(CBLAS_ORDER::CblasRowMajor, CBLAS_TRANSPOSE::CblasNoTrans,
                CBLAS_TRANSPOSE::CblasConjTrans, n, n, n,
                reinterpret_cast<const double *>(&alpha),
                reinterpret_cast<const double *>(this->data()), n,
                reinterpret_cast<const double *>(rhs.data()), n,
                reinterpret_cast<const double *>(&beta),
                reinterpret_cast<double *>(res.data()), n);
    return res;
  }

  // Real part
  // constexpr nummat<n> real() const {

  //     nummat<n> res;

  //     for (auto i=0LU; i<n;++i){
  // 	for(auto j=0LU; j<n;++j){
  // 	    res(i,j)= this->mat[i*n+j];
  // 	}
  //     }

  //     return res;
  // }

  /// Hermitian conjugate
  constexpr nummat<n> dagger() const
  {
    nummat<n> res;

    for (auto i = 0LU; i < n; ++i) {
      for (auto j = 0LU; j < n; ++j) {
        res(i, j) = std::conj(this->mat[j * n + i]);
      }
    }

    return res;
  }

  /// Matrix inverse \todo Implement for general matrix
  constexpr nummat<n> inverse() const
  {

    if (*this == nummat<n>(1.)) {
      return nummat<n>(this->mat);
    }
    else {
      throw std::runtime_error("Inverse only implemented for unit matrix!");
    }
  }

  /// Matrix log \todo Implement for general matrix
  constexpr nummat<n> log() const
  {

    nummat<n> res(0.0);

    if (*this == nummat<n>(1.)) {
      return res;
    }
    else {
      throw std::runtime_error("Log only implemented for unit matrix!");
    }
  }

  // **********************************************************************
  // Friends
  // **********************************************************************

  /// Multiplication with scalar from the left
  friend constexpr nummat<n> operator*(std::complex<double> const &lhs,
                                       nummat<n> const &rhs)
  {
    auto res = rhs;
    return res * lhs;
  }

  /// Simple printing of nummat
  friend std::ostream &operator<<(std::ostream &stream, nummat<n> const &nm)
  {

    stream.precision(6);

    for (auto i = 0ul; i < n * n; ++i) {
      stream << std::fixed << "(" << std::setw(13) << std::real(nm.mat[i])
             << "," << std::setw(13) << std::imag(nm.mat[i]) << ")\t";

      if (0 == (i + 1) % n) {
        stream << "\n";
      }
    }
    return stream;
  }
};

// Non-member functions

/// Trace
template <std::size_t n>
constexpr std::complex<double> trace(nummat<n> const &M)
{
  std::complex<double> res(0.0);
  for (auto i = 0LU; i < n; ++i) {
    res += M(i, i);
  }

  return res;
}

/// Dagger (needed as a non-member function for expansions)
template <std::size_t n>
constexpr nummat<n> dagger(nummat<n> const &M)
{
  return M.dagger();
}

/// Inverse (needed as a non-member function for expansions)
///
/// Wraps member function
template <std::size_t n>
constexpr nummat<n> inverse(nummat<n> const &M)
{
  return M.inverse();
}

/// Matrix Log (needed as a non-member function for expansions)
///
/// Wraps member function
template <std::size_t n>
constexpr nummat<n> log(nummat<n> const &M)
{
  return M.log();
}

/// Construct diagonal matrix from array
template <std::size_t n>
constexpr nummat<n> diag(std::array<std::complex<double>, n> const &init)
{
  nummat<n> M;
  for (auto i = 0LU; i < n; ++i) {
    M(i, i) = init[i];
  }

  return M;
}

/// Frobenius norm of a matrix
template <std::size_t n>
double normF(nummat<n> const &M)
{
  return cblas_dznrm2(n * n, reinterpret_cast<const double *>(M.data()), 1);
}

/// Matrix exponential
/// (Via Taylor series. This is probably neither fast nor stable.)
template <std::size_t n>
nummat<n> exp(nummat<n> const &M,
              const double &error = std::numeric_limits<double>::epsilon(),
              const size_t &maxiter = 100LU)
{

  double prec_goal = error;
  // Sanity check
  if (prec_goal < std::numeric_limits<double>::epsilon()) {
    prec_goal = std::numeric_limits<double>::epsilon();
    std::cout
      << "Warning! Precision goal was reset to machine epsilon in matrix exp"
      << prec_goal << std::endl;
  }

  nummat<n> res{ 1.0 }, old{ M }, aux{ 1.0 };

  double eps = 1.0;
  double fac = 1.0;

  size_t iter = 0LU;
  while (eps > error && iter < maxiter) {

    ++iter;
    fac *= static_cast<double>(iter);
    aux = aux * M;

    res += aux / fac;
    // Error estimate. (Quite pessimistic)
    eps = normF(old - res);
    // std::cout << "Error: " << eps << std::endl;
    old = res;
  }

  // std::cout << iter << std::endl;
  return res;
}

/// Inflates compressed Hermitian matrix (CblasUpper format) to a dense matrix
template <size_t NN>
void inflate(nummat<NN> &M)
{
  for (auto i = 1LU; i < NN; ++i) {
    for (auto j = 0LU; j < i; ++j) {
      M(i, j) = std::conj(M(j, i));
    }
  }
}


#ifndef BASIS_PURSUIT_A_H
#define BASIS_PURSUIT_A_H

#include "Matrix.h"
#include "APPROX2.h"
#include "ALM_I_APPROX.h"
#include <string>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <stdio.h>      /* printf */
#include <time.h>
#include <fstream>
#include <algorithm>
#include <iomanip>
#include <ctime>
#include <math.h>

/*This class solves problem of the form f(x)+g(x) under the constraint Mx=c by IPALM_APPROX;
where f(x)=0 and g(x)=frac{lambda2}{2}|x|_2+lambda1 |x|_1.
*/

template<typename L, typename D>
class Basis_pursuit_a: public ALM_I_APPROX<L, D>
{
private:

  D lambda1;

  D lambda2;

  Matrix<L,D> my_M;

  Matrix<L,D> my_A;

  D val_lambda_f;

protected:

public:

  Basis_pursuit_a(const char* Matrix_file,D val_lambda1, D val_lambda2)
  :ALM_I_APPROX<L,D>(), my_M(Matrix_file)
  {
    lambda1=val_lambda1;
    lambda2=val_lambda2;
    val_lambda_f=0;
    this->val_mu_f=0;
    this->val_mu_g=lambda2;
  }

  L get_n(){return my_M.nfeatures;}
  L get_m(){return my_M.nsamples;}

  inline D gradient_of_phi_j(D x1, L i){
    return 0;
  }

  inline D value_of_g_i(D x, L i){
    return lambda2*x*x/2+lambda1*fabs(x);
  }

  inline D value_of_phi_j(D x1, L i){return 0;}

  inline D prox_of_g_i(D x1,D x2,D x3, L i){
    return compute_one_step(1.0/x2, x1, x3)-x3;
  }

  D compute_one_step(D tau, D u, D x){
    D new_x;
    if(x>tau*(lambda1+u))
    new_x=(x-tau*(lambda1+u))/(1+lambda2*tau);
    else if(x<tau*(u-lambda1))
    new_x=(x-tau*(u-lambda1))/(1+lambda2*tau);
    else
    new_x=0;
    return new_x;
  }

  inline D feasible_dual(vector<D> & x, vector<D> & y){

    if(lambda2+this->beta_s>0)
    {
      return 1;
    }
    else
    {
      D scal=1;
      L l=x.size();
      for(L i=0;i<l;i++)
      if(fabs(x[i]+y[i])>lambda1)
      scal=min(lambda1/fabs(x[i]+y[i]),scal);
      return scal;
    }
  }

  inline D rescale(vector<D> & x, vector<D> & y){
    D scal=1;
    L l=x.size();
    for(L i=0;i<l;i++){
      if(fabs(x[i]+y[i])>lambda1){
        scal=min(lambda1/fabs(x[i]+y[i]),scal);
      }
    }
    return scal;
  }

  inline D value_of_phistar_i(D x,L i) {return 0;}

  inline D value_of_g_tilde_star(D scal,vector<D> & x, vector<D> & y){

    if(lambda2+this->tau_s*this->beta_s>0)
    {
      D res=0;
      D ip=0;
      L l=x.size();
      for(L i=0;i<l;i++)
      {
        ip=max(fabs(x[i]+y[i]+this->tau_s*this->beta_s*this->x_s[i])-lambda1,0.0);
        res+=ip*ip;
      }
      D xsnorm=0;
      for(L i=0;i<l;i++)
      xsnorm+=this->tau_s*this->beta_s/2*this->x_s[i]*this->x_s[i];
      return res/2/(lambda2+this->tau_s*this->beta_s)-xsnorm;
    }
    else
    {
      return 0;
    }
  }

  inline D value_of_g_star(D scal,vector<D> & x, vector<D> & y){
    return 0;
  }

  inline void set_matrix_M(){
    this->data_M=my_M;
  }

  inline void set_matrix_A(){
    this->data_A.nsamples=0;
    this->data_A.nfeatures=this->data_M.nfeatures;
    this->data_A.nnz=0;
    this->data_A.ptr.resize(1,0);
    this->data_A.ptr_t.resize(this->data_M.nfeatures+1,0);
  }

  inline D distance_to_subgradient_of_g(){
    D res=0;
    D tmp;
    D xv;
    for(L i=0;i<this->n;i++){
      tmp=-this->gradient_of_f[i]-this->M_tlambda_s[i];
      xv=this->x_s[i];
      if(xv>0) res+=(tmp-lambda2*xv-lambda1)*(tmp-lambda2*xv-lambda1);
      else if(xv<0) res+=(tmp-lambda2*xv+lambda1)*(tmp-lambda2*xv+lambda1);
      else if(tmp-lambda2*xv>lambda1) res+=(tmp-lambda2*xv-lambda1)*(tmp-lambda2*xv-lambda1);
      else if(tmp-lambda2*xv<-lambda1) res+=(tmp-lambda2*xv+lambda1)*(tmp-lambda2*xv+lambda1);
      else res+=0;
    }
    return sqrt(res);
  }

  inline D subgradient_of_Tx(){
    D res=0;
    D tmp;
    D xv;
    for(L i=0;i<this->n;i++){
      xv=this->Tx[i];
      tmp=this->gradient_of_Tx[i]+ lambda2*xv+this->tau_s*this->beta_s*(xv- this->x_s[i]);
      if(xv>0) res+=(tmp+ lambda1)*(tmp+ lambda1);
      else if(xv<0) res+=(tmp-lambda1)*(tmp-lambda1);
      else if(tmp>lambda1) res+=(tmp-lambda1)*(tmp-lambda1);
      else if(tmp<-lambda1) res+=(tmp+lambda1)*(tmp+lambda1);
      else res+=0;
    }
    return sqrt(res);
  }

  void ALM_I_APPROX_solver(D beta_0, D epsilon_0,  D eta, D rho,vector<D> & x0,vector<D> & y0,L val_tau, L max_nb_outer, L p_N_1, L p_N_2,string filename1, string filename2, D time){
    this->ALM_I_APPROX_solve_with_APPROX(beta_0, epsilon_0,  eta, rho,x0,y0, val_tau, max_nb_outer,  p_N_1,  p_N_2,  val_lambda_f, filename1,  filename2, time);
  }

};

#endif


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <limits.h>
#include <sys/time.h>
#include <inttypes.h>

#include <omp.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_heapsort.h>

#include "sph_data_types.h"
#include "sph_linked_list.h"
#include "sph_compute.h"

int compute_density_3d_naive(int N,double h,
                             double* restrict x, double* restrict y,
                             double* restrict z,double* restrict nu,
                             double* restrict Fx){
  #pragma omp parallel for
  for(int64_t ii=0;ii<N;ii+=1){
    Fx[ii] = 0;
    for(int64_t jj=0;jj<N;jj+=1){
      double dist = 0.;

      dist += (x[ii]-x[jj])*(x[ii]-x[jj]);
      dist += (y[ii]-y[jj])*(y[ii]-y[jj]);
      dist += (z[ii]-z[jj])*(z[ii]-z[jj]);

      dist = sqrt(dist);

      Fx[ii] += nu[jj]*w_bspline_3d(dist,h);
    }
  }

  return 0;
}

int compute_density_3d_ref(int N,double h,
                           double* restrict x, double* restrict y,
                           double* restrict z, double* restrict nu,
                           double* restrict Fx){
  const double inv_h = 1./h;
  const double kernel_constant = w_bspline_3d_constant(h);
  #pragma omp parallel for
  for(int64_t ii=0;ii<N;ii+=1){
    double xii = x[ii];
    double yii = y[ii];
    double zii = z[ii];
    double rhoii = 0.0;

    #pragma omp simd reduction(+:rhoii) aligned(x,y,z,nu)
    for(int64_t jj=0;jj<N;jj+=1){
      double q = 0.;

      double xij = xii-x[jj];
      double yij = yii-y[jj];
      double zij = zii-z[jj];

      q += xij*xij;
      q += yij*yij;
      q += zij*zij;

      //q = sqrt(q);//*inv_h;
      q = sqrt(q)*inv_h;

      rhoii += nu[jj]*w_bspline_3d_simd(q);
    }
    Fx[ii] = kernel_constant*rhoii;
  }

  return 0;
}

int compute_density_3d_tiled(int N,double h,
                             double* restrict x, double* restrict y,
                             double* restrict z, double* restrict nu,
                             double* restrict Fx){
  const double inv_h = 1./h;
  const double kernel_constant = w_bspline_3d_constant(h);
  const int64_t STRIP = 1000;
  const int64_t N_tiles = N/STRIP;
  const int64_t N_prime = N - N%STRIP;

  #pragma omp parallel for num_threads(24)
  for(int64_t ii=0;ii<N;ii+=1)
    Fx[ii] = 0.;

  #pragma omp parallel for
  for(int64_t i=0;i<N_prime;i+=STRIP){
    for(int64_t j=0;j<N_prime;j+=STRIP){
      for(int64_t ii=i;ii<i+STRIP;ii+=1){
        double xii = x[ii];
        double yii = y[ii];
        double zii = z[ii];
        double rhoii = 0.0;

        #pragma omp simd reduction(+:rhoii) aligned(x,y,z,nu)
        for(int64_t jj=j;jj<j+STRIP;jj+=1){
          double q = 0.;

          double xij = xii-x[jj];
          double yij = yii-y[jj];
          double zij = zii-z[jj];

          q += xij*xij;
          q += yij*yij;
          q += zij*zij;

          q = sqrt(q)*inv_h;

          rhoii += nu[jj]*w_bspline_3d_simd(q);
        }
        Fx[ii] += kernel_constant*rhoii;
      }
    }
  }

  #pragma omp parallel for
  for(int64_t j=0;j<N_prime;j+=STRIP){
    for(int64_t ii=N_prime;ii<N;ii+=1){
      double xii = x[ii];
      double yii = y[ii];
      double zii = z[ii];
      double rhoii = 0.0;

      #pragma omp simd reduction(+:rhoii) aligned(x,y,z,nu)
      for(int64_t jj=j;jj<j+STRIP;jj+=1){
        double q = 0.;

        double xij = xii-x[jj];
        double yij = yii-y[jj];
        double zij = zii-z[jj];

        q += xij*xij;
        q += yij*yij;
        q += zij*zij;

        q = sqrt(q)*inv_h;

        rhoii += nu[jj]*w_bspline_3d_simd(q);
      }
      Fx[ii] += kernel_constant*rhoii;
    }
  }

  return 0;
}

void swap_int64_t2(int64_t* a, int64_t* b) {
  int64_t t0 = *a;
  int64_t t1 = *(a+1);

  *a     = *b;
  *(a+1) = *(b+1);
  *b     = t0;
  *(b+1) = t1;
}

int64_t partition_int64_t2(int64_t arr[], int64_t low, int64_t high)
{
  int64_t pivot = arr[2*high]; // pivot
  int64_t i     = (low - 1);   // Index of smaller element and indicates the right position of pivot found so far

  for (int64_t j = low; j <= high - 1; j++){
    // If current element is smaller than the pivot
    if (arr[2*j] < pivot){
      i++; // increment index of smaller element
      swap_int64_t2(&arr[2*i], &arr[2*j]);
    }
  }

  swap_int64_t2(&arr[2*(i + 1)], &arr[2*high]);
  return (i + 1);
}

/*
void insertionSort_int64_t2(int64_t *arr, int64_t n){
    // https://www.geeksforgeeks.org/insertion-sort/

    int64_t i=0, key0=0, key1=0, j=0;
    for (i = 1; i < n; i++){
        key0 = arr[2*i+0];
        key1 = arr[2*i+1];
        j = i - 1;

        // Move elements of arr[0..i-1], that are
        // greater than key, to one position ahead
        // of their current position
        while (j >= 0 && arr[2*j] > key0){
            arr[2*(j+1)+0] = arr[2*j+0];
            arr[2*(j+1)+1] = arr[2*j+1];

            j = j-1;
        }

        arr[2*(j+1)+0] = key0;
        arr[2*(j+1)+1] = key1;
    }
}*/

void insertionSort_int64_t2(int64_t *arr, int64_t low, int64_t high)
{
    int64_t i, key0, key1, j;
    for (i = low+1; i < high; i++){
        key0 = arr[2*i+0];
        key1 = arr[2*i+1];

        j = i - 1;

        /* Move elements of arr[0..i-1], that are
        greater than key, to one position ahead
        of their current position */
        while (j >= 0 && arr[2*j] > key0){
            arr[2*(j+1)+0] = arr[2*j+0];
            arr[2*(j+1)+1] = arr[2*j+1];

            j = j-1;
        }
        arr[2*(j+1)+0] = key0;
        arr[2*(j+1)+1] = key1;
    }
}

#define MIN_DEPTH 120

void quickSort_int64_t2(int64_t *arr, int64_t low, int64_t high){
  // https://www.geeksforgeeks.org/quick-sort/

  if (low < high){
    // pi is partitioning index, arr[p] is now
    //   at right place
    int64_t pi = partition_int64_t2(arr, low, high);

    // Separately sort elements before
    // partition and after partition
    //quickSort_int64_t2(arr, low, pi-1);
    //quickSort_int64_t2(arr, pi + 1, high);

    if(pi-low-1 >= MIN_DEPTH){
      #pragma omp task shared(arr)
      quickSort_int64_t2(arr, low, pi-1);
    }
    else{
      //quickSort_int64_t2(arr, low, pi-1);
      //#pragma omp task shared(arr)
      insertionSort_int64_t2(arr,low,pi-1);
    }

    if(high - (pi+1) >= MIN_DEPTH){
      #pragma omp task shared(arr)
      quickSort_int64_t2(arr, pi+1, high);
    }
    else{
      //quickSort_int64_t2(arr, low, pi-1);
      //#pragma omp task shared(arr)
      insertionSort_int64_t2(arr,pi+1,high);
    }
  }
}

int main(){

  int err,dbg=0;
  int64_t N = 1000000;
  double h=0.05;
  linkedListBox *box;
  SPHparticle *lsph;

  err = SPHparticle_SoA_malloc(N,&lsph);
  if(err)
    printf("error in SPHparticle_SoA_malloc\n");

  err = gen_unif_rdn_pos( N,123123123,lsph);
  if(err)
    printf("error in gen_unif_rdn_pos\n");

  box = (linkedListBox*)malloc(1*sizeof(linkedListBox));
  if(box==NULL)
    printf("error alocating the linkedListBox\n");

  box->Xmin = -1.0; box->Ymin = -1.0; box->Zmin = -1.0;
  box->Xmax =  2.0; box->Ymax =  2.0; box->Zmax =  2.0;
  box->Nx = (int)( (box->Xmax-box->Xmin)/(2*h) );
  box->Ny = (int)( (box->Ymax-box->Ymin)/(2*h) );
  box->Nz = (int)( (box->Zmax-box->Zmin)/(2*h) );
  box->N  = (box->Nx)*(box->Ny)*(box->Nz);
  double min_val = fmin((box->Xmax-box->Xmin)/box->Nx,fmin((box->Ymax-box->Ymin)/box->Ny,(box->Zmax-box->Zmin)/box->Nz));
  box->width = (int)( 0.5 + 2*h/min_val );
  box->hbegin = kh_init(0);
  box->hend = kh_init(1);

  double t0,t1,t2,t3,t4,t5,t6,t7;
  t0 = omp_get_wtime();

  if(dbg)
    printf("hello - 3\n");
  err = compute_hash_MC3D(N,lsph,box);

  t1 = omp_get_wtime();

  if(dbg)
    printf("hello - 4\n");

  //qsort(lsph->hash,N,2*sizeof(int64_t),compare_int64_t);
  //quickSort_int64_t2(lsph->hash,0,N-1);

  #pragma omp parallel num_threads(1)
  {
    #pragma omp single
    quickSort_int64_t2(lsph->hash,0,N-1);
  }

  /*
  #pragma omp parallel num_threads(1)
  {
    #pragma omp single
    quicksort_omp(lsph->hash,0,N);
  }*/

  t2 = omp_get_wtime();

  if(dbg)
    printf("hello - 5\n");
  void *swap_arr = malloc(N*sizeof(double));
  err = reorder_lsph_SoA(N,lsph,swap_arr);
  if(err)
    printf("error in reorder_lsph_SoA\n");

  t3 = omp_get_wtime();

  if(dbg)
    printf("hello - 6\n");
  err = setup_interval_hashtables(N,lsph,box);
  if(err)
    printf("error in setup_interval_hashtables\n");

  t4 = omp_get_wtime();

  if(dbg)
    printf("hello - 7\n");

  //err = compute_density_3d(N,h,lsph,box);
  //err = compute_density_3d_load_ballanced(N,h,lsph,box);
  err = compute_density_3d_symmetrical_load_ballance(N,h,lsph,box);
  //err = compute_density_3d_symmetrical_lb_branching(N,h,lsph,box);
  //err = compute_density_3d_symmetrical_lb(N,h,lsph,box);
  if(err)
    printf("error in setup_interval_hashtables\n");

  t5 = omp_get_wtime();

  if(dbg)
    printf("hello - 7\n");

  //err = compute_density_3d(N,h,lsph,box);
  //err = compute_density_3d_innerOmp(N,h,lsph,box);
  //err = compute_density_3d_loopswapped(N,h,lsph,box);
  //
  if(err)
    printf("error in setup_interval_hashtables\n");

  t6 = omp_get_wtime();

  if(dbg)
    printf("hello - 8\n");

  //err = compute_density_3d_tiled(N,h,lsph->x,lsph->y,lsph->z,lsph->nu,lsph->Fx);
  //err = compute_density_3d_ref(N,h,lsph->x,lsph->y,lsph->z,lsph->nu,lsph->Fx);
  //err = compute_density_3d_naive(N,h,lsph->x,lsph->y,lsph->z,lsph->nu,lsph->Fx);
  err = compute_density_3d_load_ballanced(N,h,lsph,box);
  if(err)
    printf("error in compute_density_3d_ref\n");

  t7 = omp_get_wtime();

  printf("compute_hash_MC3D calculation time : %lf : %lf\n",t1-t0,100*(t1-t0)/(t5-t0));
  printf("qsort calculation time : %lf : %lf\n",t2-t1,100*(t2-t1)/(t5-t0));
  printf("reorder_lsph_SoA calculation time : %lf : %lf\n",t3-t2,100*(t3-t2)/(t5-t0));
  printf("setup_interval_hashtables calculation time : %lf : %lf\n",t4-t3,100*(t4-t3)/(t5-t0));
  printf("compute_density_3d base calculation time : %lf : %lf\n",t5-t4,100*(t5-t4)/(t5-t0));
  printf("compute_density_3d load balanced calculation time : %lf : %lf\n",t6-t5,0.);
  printf("Total Linked-List compute_density_3d calculation time : %lf : %lf\n",t5-t0,100*(t5-t0)/(t5-t0));
  printf("Reference tiled compute_density_3d calculation time : %lf\n",t7-t6);

  if(dbg)
    printf("hello - 9\n");

  FILE *fp = fopen("data/sph_density_compute_ref.csv","w");
  if(fp!=NULL){
    for(int64_t i=0;i<N;i+=1)
      fprintf(fp,"%ld %.12lf %.12lf %.6lg\n",i,
                                              lsph->rho[i],
                                              lsph->Fx[i],
                                              fabs(lsph->rho[i]-lsph->Fx[i]));
    fclose(fp);
  }

  if(dbg)
    printf("hello - 10\n");
  SPHparticleSOA_safe_free(N,&lsph);
  safe_free_box(box);
  free(swap_arr);

  return 0;
}


#include <stdio.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

int main()
{
  int MAT_DIM = 4;
  int i,j;
  int myArray[16] = {4, 3, 2, 1, 1,10, 3, 4, 5, 3, 2, -4, 4, 8, 7, 9};

  gsl_matrix *m = gsl_matrix_alloc(MAT_DIM,MAT_DIM);

  for (i=0; i<MAT_DIM; i++)
    for (j=0; j<MAT_DIM; j++)
      gsl_matrix_set (m, i, j, (double) myArray[i*MAT_DIM + j]);

  for (i=0; i<MAT_DIM; i++)
    for (j=0; j<MAT_DIM; j++)
      printf("m(%d,%d) = %g\n", i, j,
              gsl_matrix_get (m, i, j));

  double det;
  int signum;
  gsl_permutation *p = gsl_permutation_alloc(m->size1);

  gsl_linalg_LU_decomp(m, p, &signum);
  det = gsl_linalg_LU_det(m, signum);
  gsl_permutation_free(p);
  gsl_matrix_free(m);

  printf("Determinant: %g\n", det);

  return 0;
}


#ifndef FORCE_H
#define FORCE_H

#include <functional>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <vector>
#include "jspec2/electron_beam.h"
#include "jspec2/constants.h"

using std::vector;
enum class ForceFormula {PARKHOMCHUK, NONMAG_DERBENEV, NONMAG_MESHKOV, NONMAG_NUM1D, NONMAG_NUM3D, MESHKOV, DSM};

class FrictionForceSolver{
protected:
    double time_cooler;
    double mag_field = 0;
    static constexpr double k_f = -4*k_pi*k_c*k_c*k_ke*k_ke*k_e*k_e*k_e/(k_me*1e6);
    static constexpr double k_wp = 4*k_pi*k_c*k_c*k_e*k_ke/(k_me*1e6);
    static constexpr double k_rho_min = k_e*k_ke*k_c*k_c/(k_me*1e6);
    virtual void init(const ElectronBeam& ebeam){};
    virtual void fin(){};
    double max_impact_factor(double v_dlt, int charge_number,double density_e);
public:
    void set_time_cooler(double t){time_cooler = t;}
    void set_mag_field(double x){mag_field = x;}
    double t_cooler() const {return time_cooler;}
    virtual void friction_force(int charge_number, int ion_number,
            const vector<double>& v_tr, const vector<double>& v_l, const vector<double>& density,
            const ElectronBeam& ebeam, vector<double>& force_tr, vector<double>& force_long) = 0;
};

class ForcePark: public FrictionForceSolver {
private:
    static constexpr double k_f = -4*k_c*k_c*k_ke*k_ke*k_e*k_e*k_e/(k_me*1e6);
    double t_eff = 0; //Effective temperature.
    double v_eff = 0; //Effective velocity.
    void rho_lamor_dlt2_eff_e(double v2_eff_e, double mag_field, const vector<double>& v_rms_l, const vector<double>& v_rms_t, ElectronBeam::Temperature tpr,
                          int ion_number, vector<double>& dlt2_eff_e, vector<double>& rho_lamor);
    constexpr double dlt(ElectronBeam::Temperature tpr,  double v2, const vector<double>& dlt2_eff_e, int i);
    constexpr double lc(ElectronBeam::Temperature tpr, double rho_max, double rho_min, const vector<double>& rho_lamor, int i);
public:
    void set_t_eff(double x){t_eff = x; v_eff = sqrt(t_eff*k_c*k_c/(k_me*1e6));}
    void set_v_eff(double v){v_eff = v; t_eff = v_eff*v_eff*k_me*1e6/(k_c*k_c);}
    virtual void friction_force(int charge_number, int ion_number,
            const vector<double>& v_tr, const vector<double>& v_l, const vector<double>& density,
            const ElectronBeam& ebeam, vector<double>& force_tr, vector<double>& force_long) override;
};

class ForceNonMag: public FrictionForceSolver {
protected:
    bool smooth_rho_max = false;
    virtual double f_const(int charge_number) const {return charge_number*charge_number*k_f;}
    static constexpr double rho_min_const(int charge_number) {return charge_number*k_rho_min;}
    static constexpr double rho_max_1(int charge_number, double density_e){return pow(3*charge_number/density_e, 1.0/3);}
    double rho_max_2(double v) const {return v*time_cooler;}
    double rho_max(int charge_number, double v2, double ve2, double ne) const;
    virtual void force(double v, double v_tr, double v_l, double v2, double ve_tr, double ve_l, double ve2,
                       double f_const,double rho_min_const, int charge_number, double ne,
                       double& force_tr, double& force_l) = 0;

public:
    void set_smooth_rho_max(bool b){smooth_rho_max = b;}
    virtual void friction_force(int charge_number, int ion_number,
            const vector<double>& v_tr, const vector<double>& v_l, const vector<double>& density,
            const ElectronBeam& ebeam, vector<double>& force_tr, vector<double>& force_long) override;
};

class ForceNonMagDerbenev: public ForceNonMag {
private:
    constexpr double rho_max_ve_tr(double ve_tr, double ne);
    void force(double v, double v_tr, double v_l, double v2, double ve_tr, double ve_l, double ve2,
                               double f_const, double rho_min_const, int charge_number, double ne,
                               double& force_tr, double& force_l) override;
public:

};

class ForceNonMagMeshkov: public ForceNonMag {
private:
    void force(double v, double v_tr, double v_l, double v2, double ve_tr, double ve_l, double ve2,
                               double f_const, double rho_min_const, int charge_number, double ne,
                               double& force_tr, double& force_l) override;
public:

};

class ForceNonMagNumeric1D: public ForceNonMag {
private:
    double f_const(int charge_number) const override {return 0.28209479177387814*charge_number*charge_number*k_f;} //Coef - 1/sqrt(4*pi)
    static constexpr double k_f = 2*sqrt(2*k_pi)*k_pi*k_c*k_c*k_ke*k_ke*k_e*k_e*k_e/(k_me*1e6);

    size_t limit = 100;
    double espabs = 1e-6;
    double esprel = 1e-6;
    struct P{
        double v_tr;
        double v_l;
        double ve_tr;
        double ve_l;
        int flag;   //0: calculate B_tr; else: calculate B_l;
    };

    #ifdef _OPENMP
    vector<gsl_integration_workspace*> gw;
    vector<P> p;
    #else
    gsl_integration_workspace *gw = nullptr;
    P p;
    #endif // _OPENMP
    double b(double q, void* params);
    void force(double v, double v_tr, double v_l, double v2, double ve_tr, double ve_l, double ve2,
                               double f_const, double rho_min_const, int charge_number, double ne,
                               double& force_tr, double& force_l) override;
public:
    void set_espabs(double x){espabs = x;}
    void set_esprel(double x){esprel = x;}
    ForceNonMagNumeric1D(int n=100);
    ~ForceNonMagNumeric1D();
};

//
class ForceNonMagNumeric3D: public ForceNonMag {
private:
    size_t limit = 100;
    double espabs = 1e-5;
    double esprel = 1e-3;
    struct P{
        double v_tr;
        double v_l;
        double ve_tr;
        double ve_l;
        double vtr;
        double vl;
        double rho_max;
        int charge_number;
        int flag;   //0: calculate B_tr; else: calculate B_l;
    };

    bool use_gsl = false;
    bool use_mean_rho_min = false;

    #ifdef _OPENMP
    vector<gsl_integration_workspace*> giw;
    vector<gsl_integration_workspace*> gmw;
    vector<gsl_integration_workspace*> gow;
    vector<P> p;
    #else
    gsl_integration_workspace *giw = nullptr;
    gsl_integration_workspace *gmw = nullptr;
    gsl_integration_workspace *gow = nullptr;
    P p;
    #endif // _OPENMP

    #ifdef _OPENMP
    static double mean_rho_min;
    static double mean_lc;
    #pragma omp threadprivate(mean_rho_min, mean_lc)
    static bool first_run;
    static vector<vector<double>> exp_vtr;
    static vector<double> hlf_v2tr;
    static vector<double> hlf_v2l;
    static vector<vector<double>> vtr_cos;
    static vector<double> vl;
    static vector<double> vtr;
    static vector<vector<double>> v2tr_sin2;
    #pragma omp threadprivate(exp_vtr, hlf_v2tr, hlf_v2l, vtr_cos, vl, vtr, v2tr_sin2, first_run)
    #else
    double mean_rho_min = 0;
    double mean_lc = 0;
    vector<vector<double>> exp_vtr;
    vector<double> hlf_v2tr;
    vector<double> hlf_v2l;
    vector<vector<double>> vtr_cos;
    vector<double> vl;
    vector<double> vtr;
    vector<vector<double>> v2tr_sin2;
    bool first_run = true;
    #endif // _OPENMP
    bool const_tmpr = true;
    int n_tr = 20;
    int n_l = 10;
    int n_phi = 10;
    double d;
    double f_inv_norm;

    void pre_int(double sgm_vtr, double sgm_vl);
    void calc_exp_vtr(double sgm_vtr, double sgm_vl);

    void init(const ElectronBeam& ebeam) override;
    double inner_integrand(double phi, void* params);
    double middle_integrand(double vl, void* params);
    double outter_integrand(double vtr, void* params);
    double inner_norm_integrand(double vl, void* params);
    double outter_norm_integrand(double vtr, void* params);
    void force_grid(double v, double v_tr, double v_l, double v2, double ve_tr, double ve_l, double ve2,
                               double f_const, double rho_min_const, int charge_number, double ne,
                               double& force_tr, double& force_l);
    void force_gsl(double v, double v_tr, double v_l, double v2, double ve_tr, double ve_l, double ve2,
                               double f_const, double rho_min_const, int charge_number, double ne,
                               double& force_tr, double& force_l);
    void force(double v, double v_tr, double v_l, double v2, double ve_tr, double ve_l, double ve2,
                               double f_const, double rho_min_const, int charge_number, double ne,
                               double& force_tr, double& force_l) override;
public:
    void set_espabs(double x){espabs = x;}
    void set_esprel(double x){esprel = x;}
    void set_gsl(bool b) {use_gsl = b;}
    void set_mean_rho_min(bool b) {use_mean_rho_min = b;}
    void set_grid(int ntr, int nl, int nphi){n_tr = ntr; n_l = nl; n_phi = nphi; first_run = true;}
    ForceNonMagNumeric3D(int n=100);
    ~ForceNonMagNumeric3D();
};

//gsl function wrapper for member functions in class.
template< typename F >
    class gsl_function_pp : public gsl_function {
    public:
    gsl_function_pp(const F& func) : _func(func) {
    function = &gsl_function_pp::invoke;
    params=this;
    }
    private:
    const F& _func;
    static double invoke(double x, void *params) {
    return static_cast<gsl_function_pp*>(params)->_func(x);
    }
};

class ForceMeshkov: public FrictionForceSolver {
protected:
    //A fudge factor to smooth the friction force shape. Using the
        // "Classical" default definition here from the BETACOOL documentation
    double k = 2;
    void force(double ve_tr, double ve_l, double ve2_tr, double ve2_l, double v_tr, double v_l, double v2,
               double rho_min_const, int charge_number,  double density, double f_const, double& force_tr,double& force_l);
public:
    void set_smooth_factor(double x){k = x;}
    virtual void friction_force(int charge_number, int ion_number,
            const vector<double>& v_tr, const vector<double>& v_l, const vector<double>& density,
            const ElectronBeam& ebeam, vector<double>& force_tr, vector<double>& force_long) override;
};

class ForceDSM: public FrictionForceSolver {    //Derbenev-Skrinsky-Meshkov formula for magnetized cooling.
    bool mag_only = false;

     //A fudge factor to smooth the friction force shape. Using the
    // "Classical" default definition here from the BETACOOL documentation
    double k = 2;
    int n_a = 100;
    int n_ve = 100;
    bool const_tpr = true;
    #ifdef _OPENMP
    static bool first_run;
    static vector<double> a;
    static vector<double> cos_a;
    static vector<double> tan_a;
    static vector<double> t2;
    static vector<double> ve;
    static vector<double> exp_ve2;
    #pragma omp threadprivate(a, cos_a, tan_a, t2, ve, exp_ve2, first_run)
    #else
    bool first_run = true;
    vector<double> a;
    vector<double> cos_a;
    vector<double> tan_a;
    vector<double> t2;
    vector<double> ve;
    vector<double> exp_ve2;
    #endif // _OPENMP

    int n_tr = 20;
    int n_l = 10;
    int n_phi = 10;
    #ifdef _OPENMP
    static bool first_run_fa;
    static double f_inv_norm;
    static vector<vector<double>> exp_vtr;
    static vector<double> hlf_v2tr;
    static vector<double> hlf_v2l;
    static vector<vector<double>> vtr_cos;
    static vector<double> vl;
    static vector<double> vtr;
    static vector<vector<double>> v2tr_sin2;
    #pragma omp threadprivate(exp_vtr, hlf_v2tr, hlf_v2l, vtr_cos, vl, vtr, v2tr_sin2, first_run_fa, f_inv_norm)
    #else
    bool first_run_fa = true;
    double f_inv_norm;
    vector<vector<double>> exp_vtr;
    vector<double> hlf_v2tr;
    vector<double> hlf_v2l;
    vector<vector<double>> vtr_cos;
    vector<double> vl;
    vector<double> vtr;
    vector<vector<double>> v2tr_sin2;
    #endif // _OPENMP

protected:
    void init(const ElectronBeam& ebeam) override;
    void pre_int(double sgm_vtr, double sgm_vl);
    void calc_exp_vtr(double sgm_vtr, double sgm_vl);
    void calc_alpha();
    void calc_ve();
    void calc_exp_ve2(double ve2_l, vector<double>& t2);
    void force(double ve_tr, double ve_l, double ve2_tr, double ve2_l, double v_tr, double v_l, double v2,
               double rho_min_const, int charge_number,  double density, double f_const, double& force_tr,double& force_l);
public:
    void set_smooth_factor(double x){k = x;}
    void set_steps(int i) {n_a = i; n_ve = i; first_run = true;}
    void set_grid(int ntr, int nl, int nphi){n_tr = ntr; n_l = nl; n_phi = nphi; first_run_fa = true;}
    void set_mag_only(bool b){mag_only = b;}
    virtual void friction_force(int charge_number, int ion_number,
            const vector<double>& v_tr, const vector<double>& v_l, const vector<double>& density,
            const ElectronBeam& ebeam, vector<double>& force_tr, vector<double>& force_long) override;
};
#endif // FORCE_H


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

#ifndef _syrk_h
#define _syrk_h

/// @file syrk.h Performs matrix-matrix multiplication operations resulting in symmetric matrices.

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

namespace LATL
{
   /// @brief Performs multiplcation of a real matrix with its transpose.
   ///
   /// For real matrix A, real symmetric matrix C, and real scalars alpha and beta
   ///
   ///        C := alpha*A*A'+beta*C  or  C := alpha*A'*A+beta*C
   /// is computed.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param uplo Specifies whether the upper or lower triangular part of the symmetric matrix C
   /// is to be referenced:
   ///
   ///        if uplo = 'U' or 'u' then C is upper triangular,
   ///        if uplo = 'L' or 'l' then C is lower triangular.
   /// @param trans Specifies the operation to be perfomed as follows:
   ///
   ///        if trans = 'N' or 'n' then C := alpha*A*A'+beta*C
   ///        if trans = 'T' or 't' then C := alpha*A'*A+beta*C
   ///        if trans = 'C' or 'c' then C := alpha*A'*A+beta*C
   /// @param n Specifies the order of the symmetric matrix C.  n>=0
   /// @param k Specifies the other dimension of the real matrix A (see below). k>=0
   /// @param alpha Real scalar.
   /// @param A Pointer to real matrix.
   ///
   ///        if trans = 'N' or 'n' then A is n-by-k
   ///        if trans = 'T' or 't' then A is k-by-n
   ///        if trans = 'C' or 'c' then A is k-by-n
   /// @param ldA Column length of the matrix A. If trans = 'N' or 'n' ldA>=n, otherwise ldA>=k.
   /// @param beta Real scalar.
   /// @param C Pointer to real symmetric n-by-n matrix C.
   /// Only the upper or lower triangular part of C is referenced, depending on the value of uplo above.
   /// @param ldC Column length of the matrix C.  ldC>=n
   /// @ingroup BLAS

   template <typename real_t>
   int SYRK(char uplo, char trans, int_t n, int_t k, real_t alpha, real_t *A, int_t ldA, real_t beta, real_t *C, int_t ldC)
   {
      using std::toupper;

      const real_t zero(0.0);
      const real_t one(1.0);
      int_t i,j,l;
      real_t *a,*c,*at;
      real_t t;

      uplo=toupper(uplo);
      trans=toupper(trans);

      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldC<n)
         return -10;
      else if((n==0)||(((alpha==zero)||(k==0))&&(beta==one)))
         return 0;

      if(alpha==zero)
      {
         if(uplo=='U')
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=0;i<=j;i++)
                  c[i]*=beta;
               c+=ldC;
            }

         }
         else
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=j;i<n;i++)
                  c[i]*=beta;
               c+=ldC;
            }
         }
      }
      else if(trans=='N')
      {
         if(uplo=='U')
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=0;i<=j;i++)
                  c[i]*=beta;
               a=A;
               for(l=0;l<k;l++)
               {
                  t=alpha*a[j];
                  for(i=0;i<=j;i++)
                     c[i]+=t*a[i];
                  a+=ldA;
               }
               c+=ldC;
            }
         }
         else
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=j;i<n;i++)
                  c[i]*=beta;
               a=A;
               for(l=0;l<k;l++)
               {
                  t=alpha*a[j];
                  for(i=j;i<n;i++)
                     c[i]+=t*a[i];
                  a+=ldA;
               }
               c+=ldC;
            }
         }
      }
      else
      {
         if(uplo=='U')
         {
            c=C;
            at=A;
            for(j=0;j<n;j++)
            {
               a=A;
               for(i=0;i<=j;i++)
               {
                  t=zero;
                  for(l=0;l<k;l++)
                     t+=a[l]*at[l];
                  c[i]=alpha*t+beta*c[i];
                  a+=ldA;
               }
               at+=ldA;
               c+=ldC;
            }
         }
         else
         {
            at=A;
            c=C;
            for(j=0;j<n;j++)
            {
               a=A+j*ldA;
               for(i=j;i<n;i++)
               {
                  t=zero;
                  for(l=0;l<k;l++)
                     t+=a[l]*at[l];
                  c[i]=alpha*t+beta*c[i];
                  a+=ldA;
               }
               at+=ldA;
               c+=ldC;
            }
         }
      }
      return 0;
   }

   /// @brief Performs multiplcation of a complex matrix with its transpose.
   ///
   /// For complex matrix A, complex symmetric matrix C, and complex scalars alpha and beta
   ///
   ///        C := alpha*A*A.'+beta*C  or  C := alpha*A.'*A+beta*C
   /// is computed.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param uplo Specifies whether the upper or lower triangular part of the symmetric matrix C
   /// is to be referenced:
   ///
   ///        if uplo = 'U' or 'u' then C is upper triangular,
   ///        if uplo = 'L' or 'l' then C is lower triangular.
   /// @param trans Specifies the operation to be perfomed as follows:
   ///
   ///        if trans = 'N' or 'n' then C := alpha*A*A.'+beta*C
   ///        if trans = 'T' or 't' then C := alpha*A.'*A+beta*C
   /// @param n Specifies the order of the complex symmetric matrix C.  n>=0
   /// @param k Specifies the other dimension of the complex matrix A (see below). k>=0
   /// @param alpha Complex scalar.
   /// @param A Pointer to complex matrix.
   ///
   ///        if trans = 'N' or 'n' then A is n-by-k
   ///        if trans = 'T' or 't' then A is k-by-n
   /// @param ldA Column length of the matrix A. If trans = 'N' or 'n' ldA>=n, otherwise ldA>=k.
   /// @param beta Complex scalar.
   /// @param C Pointer to complex symmetric n-by-n matrix C.
   /// Only the upper or lower triangular part of C is referenced, depending on the value of uplo above.
   /// @param ldC Column length of the matrix C.  ldC>=n
   /// @ingroup BLAS

   template <typename real_t>
   int SYRK(char uplo, char trans, int_t n, int_t k, complex<real_t> alpha, complex<real_t> *A, int_t ldA, complex<real_t> beta, complex<real_t> *C, int_t ldC)
   {
      using std::toupper;

      const complex<real_t> zero(0.0,0.0);
      const complex<real_t> one(1.0,0.0);
      int_t i,j,l;
      complex<real_t> *a,*c,*at;
      complex<real_t> t;

      uplo=toupper(uplo);
      trans=toupper(trans);

      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldC<n)
         return -10;
      else if((n==0)||(((alpha==zero)||(k==0))&&(beta==one)))
         return 0;

      if(alpha==zero)
      {
         if(uplo=='U')
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=0;i<=j;i++)
                  c[i]*=beta;
               c+=ldC;
            }

         }
         else
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=j;i<n;i++)
                  c[i]*=beta;
               c+=ldC;
            }
         }
      }
      else if(trans=='N')
      {
         if(uplo=='U')
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=0;i<=j;i++)
                  c[i]*=beta;
               a=A;
               for(l=0;l<k;l++)
               {
                  t=alpha*a[j];
                  for(i=0;i<=j;i++)
                     c[i]+=t*a[i];
                  a+=ldA;
               }
               c+=ldC;
            }
         }
         else
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=j;i<n;i++)
                  c[i]*=beta;
               a=A;
               for(l=0;l<k;l++)
               {
                  t=alpha*a[j];
                  for(i=j;i<n;i++)
                     c[i]+=t*a[i];
                  a+=ldA;
               }
               c+=ldC;
            }
         }
      }
      else
      {
         if(uplo=='U')
         {
            c=C;
            at=A;
            for(j=0;j<n;j++)
            {
               a=A;
               for(i=0;i<=j;i++)
               {
                  t=zero;
                  for(l=0;l<k;l++)
                     t+=a[l]*at[l];
                  c[i]=alpha*t+beta*c[i];
                  a+=ldA;
               }
               at+=ldA;
               c+=ldC;
            }
         }
         else
         {
            at=A;
            c=C;
            for(j=0;j<n;j++)
            {
               a=A+j*ldA;
               for(i=j;i<n;i++)
               {
                  t=zero;
                  for(l=0;l<k;l++)
                     t+=a[l]*at[l];
                  c[i]=alpha*t+beta*c[i];
                  a+=ldA;
               }
               at+=ldA;
               c+=ldC;
            }
         }
      }
      return 0;
   }

#ifdef __latl_cblas
#include <cblas.h>

   template <> int SYRK<float>(char uplo, char trans, int_t n, int_t k, float alpha, float *A, int_t ldA, float beta, float *C, int_t ldC)
   {
      using std::toupper;
      uplo=toupper(uplo);
      trans=toupper(trans);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldC<n)
         return -10;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_ssyrk(CblasColMajor,Uplo,Trans,n,k,alpha,A,ldA,beta,C,ldC);

      return 0;

   }

   template <> int SYRK<double>(char uplo, char trans, int_t n, int_t k, double alpha, double *A, int_t ldA, double beta, double *C, int_t ldC)
   {
      using std::toupper;
      uplo=toupper(uplo);
      trans=toupper(trans);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldC<n)
         return -10;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_dsyrk(CblasColMajor,Uplo,Trans,n,k,alpha,A,ldA,beta,C,ldC);

      return 0;

   }

   template <> int SYRK<float>(char uplo, char trans, int_t n, int_t k, complex<float> alpha, complex<float> *A, int_t ldA, complex<float> beta, complex<float> *C, int_t ldC)
   {
      using std::toupper;
      uplo=toupper(uplo);
      trans=toupper(trans);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldC<n)
         return -10;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_csyrk(CblasColMajor,Uplo,Trans,n,k,&alpha,A,ldA,&beta,C,ldC);

      return 0;

   }

   template <> int SYRK<double>(char uplo, char trans, int_t n, int_t k, complex<double> alpha, complex<double> *A, int_t ldA, complex<double> beta, complex<double> *C, int_t ldC)
   {
      using std::toupper;
      uplo=toupper(uplo);
      trans=toupper(trans);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldC<n)
         return -10;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_zsyrk(CblasColMajor,Uplo,Trans,n,k,&alpha,A,ldA,&beta,C,ldC);

      return 0;

   }

#endif
}

#endif


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

/*
   - Originally written by Tuomo Keskitalo <tuomo.keskitalo@iki.fi>
   - Corrections to nmsimplex_iterate and other functions
     by Ivo Alxneit <ivo.alxneit@psi.ch>
   - Additional help by Brian Gough <bjg@network-theory.co.uk>
   - Optimisations added by Brian Gough <bjg@network-theory.co.uk>
         + use BLAS for frequently-called functions
         + keep track of the center to avoid unnecessary computation
         + compute size as RMS value, allowing linear update on each step
           instead of recomputing from all N+1 vectors.
*/

/* The Simplex method of Nelder and Mead, also known as the polytope
   search alogorithm.  Ref: Nelder, J.A., Mead, R., Computer Journal 7
   (1965) pp. 308-313.

   This implementation uses n+1 corner points in the simplex.
*/

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_matrix_double.h>

typedef struct
{
  gsl_matrix *x1;		/* simplex corner points */
  gsl_vector *y1;		/* function value at corner points */
  gsl_vector *ws1;		/* workspace 1 for algorithm */
  gsl_vector *ws2;		/* workspace 2 for algorithm */
  gsl_vector *center;		/* center of all points */
  gsl_vector *delta;		/* current step */
  gsl_vector *xmc;		/* x - center (workspace) */
  double S2;
  unsigned long count;
}
nmsimplex_state_t;

static int
compute_center (const nmsimplex_state_t * state, gsl_vector * center);
static double
compute_size (nmsimplex_state_t * state, const gsl_vector * center);

static double
try_corner_move (const double coeff,
		 const nmsimplex_state_t * state,
		 size_t corner,
		 gsl_vector * xc, const gsl_multimin_function * f)
{
  /* moves a simplex corner scaled by coeff (negative value represents
     mirroring by the middle point of the "other" corner points)
     and gives new corner in xc and function value at xc as a
     return value
   */

  gsl_matrix *x1 = state->x1;
  const size_t P = x1->size1;
  double newval;

  /* xc = (1-coeff)*(P/(P-1)) * center(all) + ((P*coeff-1)/(P-1))*x_corner */
  {
    double alpha = (1 - coeff) * P / (P - 1.0);
    double beta = (P * coeff - 1.0) / (P - 1.0);
    gsl_vector_const_view row = gsl_matrix_const_row (x1, corner);

    gsl_vector_memcpy (xc, state->center);
    gsl_blas_dscal (alpha, xc);
    gsl_blas_daxpy (beta, &row.vector, xc);
  }

  newval = GSL_MULTIMIN_FN_EVAL (f, xc);

  return newval;
}

static void
update_point (nmsimplex_state_t * state, size_t i,
	      const gsl_vector * x, double val)
{
  gsl_vector_const_view x_orig = gsl_matrix_const_row (state->x1, i);
  const size_t P = state->x1->size1;

  /* Compute delta = x - x_orig */
  gsl_vector_memcpy (state->delta, x);
  gsl_blas_daxpy (-1.0, &x_orig.vector, state->delta);

  /* Compute xmc = x_orig - c */
  gsl_vector_memcpy (state->xmc, &x_orig.vector);
  gsl_blas_daxpy (-1.0, state->center, state->xmc);

  /* Update size: S2' = S2 + (2/P) * (x_orig - c).delta + (P-1)*(delta/P)^2 */
  {
    double d = gsl_blas_dnrm2 (state->delta);
    double xmcd;
    gsl_blas_ddot (state->xmc, state->delta, &xmcd);
    state->S2 += (2.0 / P) * xmcd + ((P - 1.0) / P) * (d * d / P);
  }

  /* Update center:  c' = c + (x - x_orig) / P */

  {
    double alpha = 1.0 / P;
    gsl_blas_daxpy (-alpha, &x_orig.vector, state->center);
    gsl_blas_daxpy (alpha, x, state->center);
  }

  gsl_matrix_set_row (state->x1, i, x);
  gsl_vector_set (state->y1, i, val);
}

static int
contract_by_best (nmsimplex_state_t * state, size_t best,
		  gsl_vector * xc, gsl_multimin_function * f)
{

  /* Function contracts the simplex in respect to best valued
     corner. That is, all corners besides the best corner are moved.
     (This function is rarely called in practice, since it is the last
     choice, hence not optimised - BJG)  */

  /* the xc vector is simply work space here */

  gsl_matrix *x1 = state->x1;
  gsl_vector *y1 = state->y1;

  size_t i, j;
  double newval;

  int status = GSL_SUCCESS;

  for (i = 0; i < x1->size1; i++)
    {
      if (i != best)
	{
	  for (j = 0; j < x1->size2; j++)
	    {
	      newval = 0.5 * (gsl_matrix_get (x1, i, j)
			      + gsl_matrix_get (x1, best, j));
	      gsl_matrix_set (x1, i, j, newval);
	    }

	  /* evaluate function in the new point */

	  gsl_matrix_get_row (xc, x1, i);
	  newval = GSL_MULTIMIN_FN_EVAL (f, xc);
	  gsl_vector_set (y1, i, newval);

	  /* notify caller that we found at least one bad function value.
	     we finish the contraction (and do not abort) to allow the user
	     to handle the situation */

	  if (!gsl_finite (newval))
	    {
	      status = GSL_EBADFUNC;
	    }
	}
    }

  /* We need to update the centre and size as well */
  compute_center (state, state->center);
  compute_size (state, state->center);

  return status;
}

static int
compute_center (const nmsimplex_state_t * state, gsl_vector * center)
{
  /* calculates the center of the simplex and stores in center */

  gsl_matrix *x1 = state->x1;
  const size_t P = x1->size1;
  size_t i;

  gsl_vector_set_zero (center);

  for (i = 0; i < P; i++)
    {
      gsl_vector_const_view row = gsl_matrix_const_row (x1, i);
      gsl_blas_daxpy (1.0, &row.vector, center);
    }

  {
    const double alpha = 1.0 / P;
    gsl_blas_dscal (alpha, center);
  }

  return GSL_SUCCESS;
}

static double
compute_size (nmsimplex_state_t * state, const gsl_vector * center)
{
  /* calculates simplex size as rms sum of length of vectors
     from simplex center to corner points:

     sqrt( sum ( || y - y_middlepoint ||^2 ) / n )
   */

  gsl_vector *s = state->ws1;
  gsl_matrix *x1 = state->x1;
  const size_t P = x1->size1;
  size_t i;

  double ss = 0.0;

  for (i = 0; i < P; i++)
    {
      double t;
      gsl_matrix_get_row (s, x1, i);
      gsl_blas_daxpy (-1.0, center, s);
      t = gsl_blas_dnrm2 (s);
      ss += t * t;
    }

  /* Store squared size in the state */
  state->S2 = (ss / P);

  return sqrt (ss / P);
}

static int
nmsimplex_alloc (void *vstate, size_t n)
{
  nmsimplex_state_t *state = (nmsimplex_state_t *) vstate;

  if (n == 0)
    {
      GSL_ERROR ("invalid number of parameters specified", GSL_EINVAL);
    }

  state->x1 = gsl_matrix_alloc (n + 1, n);

  if (state->x1 == NULL)
    {
      GSL_ERROR ("failed to allocate space for x1", GSL_ENOMEM);
    }

  state->y1 = gsl_vector_alloc (n + 1);

  if (state->y1 == NULL)
    {
      gsl_matrix_free (state->x1);
      GSL_ERROR ("failed to allocate space for y", GSL_ENOMEM);
    }

  state->ws1 = gsl_vector_alloc (n);

  if (state->ws1 == NULL)
    {
      gsl_matrix_free (state->x1);
      gsl_vector_free (state->y1);
      GSL_ERROR ("failed to allocate space for ws1", GSL_ENOMEM);
    }

  state->ws2 = gsl_vector_alloc (n);

  if (state->ws2 == NULL)
    {
      gsl_matrix_free (state->x1);
      gsl_vector_free (state->y1);
      gsl_vector_free (state->ws1);
      GSL_ERROR ("failed to allocate space for ws2", GSL_ENOMEM);
    }

  state->center = gsl_vector_alloc (n);

  if (state->center == NULL)
    {
      gsl_matrix_free (state->x1);
      gsl_vector_free (state->y1);
      gsl_vector_free (state->ws1);
      gsl_vector_free (state->ws2);
      GSL_ERROR ("failed to allocate space for center", GSL_ENOMEM);
    }

  state->delta = gsl_vector_alloc (n);

  if (state->delta == NULL)
    {
      gsl_matrix_free (state->x1);
      gsl_vector_free (state->y1);
      gsl_vector_free (state->ws1);
      gsl_vector_free (state->ws2);
      gsl_vector_free (state->center);
      GSL_ERROR ("failed to allocate space for delta", GSL_ENOMEM);
    }

  state->xmc = gsl_vector_alloc (n);

  if (state->xmc == NULL)
    {
      gsl_matrix_free (state->x1);
      gsl_vector_free (state->y1);
      gsl_vector_free (state->ws1);
      gsl_vector_free (state->ws2);
      gsl_vector_free (state->center);
      gsl_vector_free (state->delta);
      GSL_ERROR ("failed to allocate space for xmc", GSL_ENOMEM);
    }

  state->count = 0;

  return GSL_SUCCESS;
}

static void
nmsimplex_free (void *vstate)
{
  nmsimplex_state_t *state = (nmsimplex_state_t *) vstate;

  gsl_matrix_free (state->x1);
  gsl_vector_free (state->y1);
  gsl_vector_free (state->ws1);
  gsl_vector_free (state->ws2);
  gsl_vector_free (state->center);
  gsl_vector_free (state->delta);
  gsl_vector_free (state->xmc);
}

static int
nmsimplex_set (void *vstate, gsl_multimin_function * f,
	       const gsl_vector * x,
	       double *size, const gsl_vector * step_size)
{
  int status;
  size_t i;
  double val;

  nmsimplex_state_t *state = (nmsimplex_state_t *) vstate;

  gsl_vector *xtemp = state->ws1;

  if (xtemp->size != x->size)
    {
      GSL_ERROR ("incompatible size of x", GSL_EINVAL);
    }

  if (xtemp->size != step_size->size)
    {
      GSL_ERROR ("incompatible size of step_size", GSL_EINVAL);
    }

  /* first point is the original x0 */

  val = GSL_MULTIMIN_FN_EVAL (f, x);

  if (!gsl_finite (val))
    {
      GSL_ERROR ("non-finite function value encountered", GSL_EBADFUNC);
    }

  gsl_matrix_set_row (state->x1, 0, x);
  gsl_vector_set (state->y1, 0, val);

  /* following points are initialized to x0 + step_size */

  for (i = 0; i < x->size; i++)
    {
      status = gsl_vector_memcpy (xtemp, x);

      if (status != 0)
	{
	  GSL_ERROR ("vector memcopy failed", GSL_EFAILED);
	}

      {
	double xi = gsl_vector_get (x, i);
	double si = gsl_vector_get (step_size, i);

	gsl_vector_set (xtemp, i, xi + si);
	val = GSL_MULTIMIN_FN_EVAL (f, xtemp);
      }

      if (!gsl_finite (val))
	{
	  GSL_ERROR ("non-finite function value encountered", GSL_EBADFUNC);
	}

      gsl_matrix_set_row (state->x1, i + 1, xtemp);
      gsl_vector_set (state->y1, i + 1, val);
    }

  compute_center (state, state->center);

  /* Initialize simplex size */
  *size = compute_size (state, state->center);

  state->count++;

  return GSL_SUCCESS;
}

static int
nmsimplex_iterate (void *vstate, gsl_multimin_function * f,
		   gsl_vector * x, double *size, double *fval)
{

  /* Simplex iteration tries to minimize function f value */
  /* Includes corrections from Ivo Alxneit <ivo.alxneit@psi.ch> */

  nmsimplex_state_t *state = (nmsimplex_state_t *) vstate;

  /* xc and xc2 vectors store tried corner point coordinates */

  gsl_vector *xc = state->ws1;
  gsl_vector *xc2 = state->ws2;
  gsl_vector *y1 = state->y1;
  gsl_matrix *x1 = state->x1;

  const size_t n = y1->size;
  size_t i;
  size_t hi, s_hi, lo;
  double dhi, ds_hi, dlo;
  int status;
  double val, val2;

  if (xc->size != x->size)
    {
      GSL_ERROR ("incompatible size of x", GSL_EINVAL);
    }

  /* get index of highest, second highest and lowest point */

  dhi = dlo = gsl_vector_get (y1, 0);
  hi = 0;
  lo = 0;

  ds_hi = gsl_vector_get (y1, 1);
  s_hi = 1;

  for (i = 1; i < n; i++)
    {
      val = (gsl_vector_get (y1, i));
      if (val < dlo)
	{
	  dlo = val;
	  lo = i;
	}
      else if (val > dhi)
	{
	  ds_hi = dhi;
	  s_hi = hi;
	  dhi = val;
	  hi = i;
	}
      else if (val > ds_hi)
	{
	  ds_hi = val;
	  s_hi = i;
	}
    }

  /* try reflecting the highest value point */

  val = try_corner_move (-1.0, state, hi, xc, f);

  if (gsl_finite (val) && val < gsl_vector_get (y1, lo))
    {
      /* reflected point is lowest, try expansion */

      val2 = try_corner_move (-2.0, state, hi, xc2, f);

      if (gsl_finite (val2) && val2 < gsl_vector_get (y1, lo))
	{
	  update_point (state, hi, xc2, val2);
	}
      else
	{
	  update_point (state, hi, xc, val);
	}
    }
  else if (!gsl_finite (val) || val > gsl_vector_get (y1, s_hi))
    {
      /* reflection does not improve things enough, or we got a
         non-finite function value */

      if (gsl_finite (val) && val <= gsl_vector_get (y1, hi))
	{
	  /* if trial point is better than highest point, replace
	     highest point */

	  update_point (state, hi, xc, val);
	}

      /* try one-dimensional contraction */

      val2 = try_corner_move (0.5, state, hi, xc2, f);

      if (gsl_finite (val2) && val2 <= gsl_vector_get (y1, hi))
	{
	  update_point (state, hi, xc2, val2);
	}
      else
	{
	  /* contract the whole simplex about the best point */

	  status = contract_by_best (state, lo, xc, f);

	  if (status != GSL_SUCCESS)
	    {
	      GSL_ERROR ("contraction failed", GSL_EFAILED);
	    }
	}
    }
  else
    {
      /* trial point is better than second highest point.  Replace
         highest point by it */

      update_point (state, hi, xc, val);
    }

  /* return lowest point of simplex as x */

  lo = gsl_vector_min_index (y1);
  gsl_matrix_get_row (x, x1, lo);
  *fval = gsl_vector_get (y1, lo);

  /* Update simplex size */

  {
    double S2 = state->S2;

    if (S2 > 0)
      {
	*size = sqrt (S2);
      }
    else
      {
	/* recompute if accumulated error has made size invalid */
	*size = compute_size (state, state->center);
      }
  }

  return GSL_SUCCESS;
}

static const gsl_multimin_fminimizer_type nmsimplex_type =
{ "nmsimplex2",	/* name */
  sizeof (nmsimplex_state_t),
  &nmsimplex_alloc,
  &nmsimplex_set,
  &nmsimplex_iterate,
  &nmsimplex_free
};

const gsl_multimin_fminimizer_type
  * gsl_multimin_fminimizer_nmsimplex2 = &nmsimplex_type;

static inline double
ran_unif (unsigned long *seed)
{
  unsigned long s = *seed;
  *seed = (s * 69069 + 1) & 0xffffffffUL;
  return (*seed) / 4294967296.0;
}

static int
nmsimplex_set_rand (void *vstate, gsl_multimin_function * f,
		    const gsl_vector * x,
		    double *size, const gsl_vector * step_size)
{
  size_t i, j;
  double val;

  nmsimplex_state_t *state = (nmsimplex_state_t *) vstate;

  gsl_vector *xtemp = state->ws1;

  if (xtemp->size != x->size)
    {
      GSL_ERROR ("incompatible size of x", GSL_EINVAL);
    }

  if (xtemp->size != step_size->size)
    {
      GSL_ERROR ("incompatible size of step_size", GSL_EINVAL);
    }

  /* first point is the original x0 */

  val = GSL_MULTIMIN_FN_EVAL (f, x);

  if (!gsl_finite (val))
    {
      GSL_ERROR ("non-finite function value encountered", GSL_EBADFUNC);
    }

  gsl_matrix_set_row (state->x1, 0, x);
  gsl_vector_set (state->y1, 0, val);

  {
    gsl_matrix_view m =
      gsl_matrix_submatrix (state->x1, 1, 0, x->size, x->size);

    /* generate a random orthornomal basis  */
    unsigned long seed = state->count ^ 0x12345678;

    ran_unif (&seed);		/* warm it up */

    gsl_matrix_set_identity (&m.matrix);

    /* start with random reflections */
    for (i = 0; i < x->size; i++)
      {
        double s = ran_unif (&seed);
        if (s > 0.5) gsl_matrix_set (&m.matrix, i, i, -1.0);
      }

    /* apply random rotations */
    for (i = 0; i < x->size; i++)
      {
	for (j = i + 1; j < x->size; j++)
	  {
	    /* rotate columns i and j by a random angle */
	    double angle = 2.0 * M_PI * ran_unif (&seed);
	    double c = cos (angle), s = sin (angle);
	    gsl_vector_view c_i = gsl_matrix_column (&m.matrix, i);
	    gsl_vector_view c_j = gsl_matrix_column (&m.matrix, j);
	    gsl_blas_drot (&c_i.vector, &c_j.vector, c, s);
	  }
      }

    /* scale the orthonormal basis by the user-supplied step_size in
       each dimension, and use as an offset from the central point x */

    for (i = 0; i < x->size; i++)
      {
	double x_i = gsl_vector_get (x, i);
	double s_i = gsl_vector_get (step_size, i);
	gsl_vector_view c_i = gsl_matrix_column (&m.matrix, i);

	for (j = 0; j < x->size; j++)
	  {
	    double x_ij = gsl_vector_get (&c_i.vector, j);
	    gsl_vector_set (&c_i.vector, j, x_i + s_i * x_ij);
	  }
      }

    /* compute the function values at each offset point */

    for (i = 0; i < x->size; i++)
      {
	gsl_vector_view r_i = gsl_matrix_row (&m.matrix, i);

	val = GSL_MULTIMIN_FN_EVAL (f, &r_i.vector);

	if (!gsl_finite (val))
	  {
	    GSL_ERROR ("non-finite function value encountered", GSL_EBADFUNC);
	  }

	gsl_vector_set (state->y1, i + 1, val);
      }
  }

  compute_center (state, state->center);

  /* Initialize simplex size */
  *size = compute_size (state, state->center);

  state->count++;

  return GSL_SUCCESS;
}

static const gsl_multimin_fminimizer_type nmsimplex2rand_type =
{ "nmsimplex2rand",	/* name */
  sizeof (nmsimplex_state_t),
  &nmsimplex_alloc,
  &nmsimplex_set_rand,
  &nmsimplex_iterate,
  &nmsimplex_free
};

const gsl_multimin_fminimizer_type
  * gsl_multimin_fminimizer_nmsimplex2rand = &nmsimplex2rand_type;


/*
** estimate false discovery rate based on histograms
** input histograms are generated by "vted"
**
** Ref: Lohmann et al (2016) PLoS One, 11(6):e0158185
**
** G.Lohmann, MPI-KYB, Jan 2015
*/
#include <viaio/VImage.h>
#include <viaio/Vlib.h>
#include <viaio/mu.h>
#include <viaio/option.h>

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

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_histogram.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_math.h>

#define SQR(x) ((x)*(x))
#define ABS(x) ((x) > 0 ? (x) : -(x))

#define HMIN 0
#define HMAX 1.001

#define NBINS 10000

size_t CountLines(FILE *fp)
{
  char ch;
  size_t numlines=0;
  while(!feof(fp)) {
    ch = fgetc(fp);
    if(ch == '\n') {
      numlines++;
    }
  }
  rewind(fp);
  return numlines;
}

void ReadTxtHist(char *filename,gsl_histogram *hist)
{
  FILE *fp=fopen(filename,"r");
  if (!fp) VError("err opening %s",filename);

  size_t nbins = CountLines(fp);
  if (nbins != (size_t)NBINS)
    VError(" inconsistent bins: %lu",gsl_histogram_bins (hist));

  size_t i=0;
  double lower=0,hz=0,mx=0;
  for (i=0; i<nbins; i++) {
    if (fscanf(fp,"%lf %lf %lf",&lower,&hz,&mx) != 3) VError(" read error");
    if (lower < HMIN || lower >= HMAX) VError(" value out of histogram range: %f",lower);
    hist->bin[i] = mx;
  }
  fclose(fp);
}

int main(int argc, char *argv[])
{
  static VString realfilename = "";
  static VString nullfilename = "";
  static VString outfilename = "";
  static VFloat  alpha = 0.05;
  static VOptionDescRec  options[] = {
    {"real",VStringRepn,1,(VPointer) &realfilename,VRequiredOpt,NULL,"input real histogram file"},
    {"null",VStringRepn,1,(VPointer) &nullfilename,VRequiredOpt,NULL,"input null histogram file"},
    {"out",VStringRepn,1,(VPointer) &outfilename,VRequiredOpt,NULL,"output txt file"},
    {"alpha",VFloatRepn,1,(VPointer) &alpha,VOptionalOpt,NULL,"alpha level"},
  };
  double lower=0,upper=0,tiny=1.0e-8;
  long i;
  VParseFilterCmd(VNumber(options),options,argc,argv,NULL,NULL);
  gsl_set_error_handler_off();

  /* ini histogram structs */
  size_t nbins = NBINS;
  double hmin = HMIN,hmax = HMAX;
  gsl_histogram *realhist = gsl_histogram_alloc (nbins);
  gsl_histogram_set_ranges_uniform (realhist,hmin,hmax);
  gsl_histogram *nullhist = gsl_histogram_alloc (nbins);
  gsl_histogram_set_ranges_uniform (nullhist,hmin,hmax);

  /* read input histograms */
  ReadTxtHist(realfilename,realhist);
  ReadTxtHist(nullfilename,nullhist);

  /* cumulative distribution functions */
  gsl_histogram_pdf *cdfz = gsl_histogram_pdf_alloc(nbins);
  gsl_histogram_pdf *cdf0 = gsl_histogram_pdf_alloc(nbins);
  gsl_histogram_pdf_init (cdfz,realhist);
  gsl_histogram_pdf_init (cdf0,nullhist);

  FILE *fp = fopen(outfilename,"w");
  if (!fp) VError(" err opening file %s",outfilename);
  fprintf(fp,"#     ted            F0            Fz           FDR \n");
  fprintf(fp,"#------------------------------------------------------\n");

  /* tail-area FDR */
  double cutoff=0;
  int flag = 0;
  for (i=0; i<nbins; i++) {
    gsl_histogram_get_range (realhist,i,&lower,&upper);
    double z = lower + 0.5*(upper-lower);
    double Fz = 1.0-cdfz->sum[i];
    double F0 = 1.0-cdf0->sum[i];
    double Fdr = 0.0;
    if (Fz  > tiny) Fdr = F0/Fz;
    if (Fdr > 1.0) Fdr = 1.0;
    if (Fdr < 0.0) Fdr = 0.0;

    if (Fdr < alpha && flag == 0 && i > 10) {
      flag = 1;
      cutoff = z;
    }
    fprintf(fp," %12.8f  %12.8f  %12.8f  %12.8f\n",z,F0,Fz,Fdr);
  }
  fprintf(fp,"# Fdr < %.3f  if ted > %lf\n",alpha,cutoff);
  fprintf(stderr," Fdr < %.3f  if ted > %lf\n",alpha,cutoff);

  fclose(fp);
  exit(0);
}


//Gets deltas (first order differences) of X.
//Only the deltas are output in Y.

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

//It seems that Kaldi and others use the same method for edge samples,
//which is numpy.edge, so that is used here.
//Comment out those lines to set out-of-range samps to 0.

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

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

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

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

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

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

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

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

    return 0;
}

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

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

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

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

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

    return 0;
}

#ifdef __cplusplus
}
}
#endif


/* cdf/gammainv.c
 *
 * Copyright (C) 2003, 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sf_gamma.h>

#include <stdio.h>

double
gsl_cdf_gamma_Pinv (const double P, const double a, const double b)
{
  double x;

  if (P == 1.0)
    {
      return GSL_POSINF;
    }
  else if (P == 0.0)
    {
      return 0.0;
    }

  /* Consider, small, large and intermediate cases separately.  The
     boundaries at 0.05 and 0.95 have not been optimised, but seem ok
     for an initial approximation.

     BJG: These approximations aren't really valid, the relevant
     criterion is P*gamma(a+1) < 1. Need to rework these routines and
     use a single bisection style solver for all the inverse
     functions.
  */

  if (P < 0.05)
    {
      double x0 = exp ((gsl_sf_lngamma (a) + log (P)) / a);
      x = x0;
    }
  else if (P > 0.95)
    {
      double x0 = -log1p (-P) + gsl_sf_lngamma (a);
      x = x0;
    }
  else
    {
      double xg = gsl_cdf_ugaussian_Pinv (P);
      double x0 = (xg < -0.5*sqrt (a)) ? a : sqrt (a) * xg + a;
      x = x0;
    }

  /* Use Lagrange's interpolation for E(x)/phi(x0) to work backwards
     to an improved value of x (Abramowitz & Stegun, 3.6.6)

     where E(x)=P-integ(phi(u),u,x0,x) and phi(u) is the pdf.
   */

  {
    double lambda, dP, phi;
    unsigned int n = 0;

  start:
    dP = P - gsl_cdf_gamma_P (x, a, 1.0);
    phi = gsl_ran_gamma_pdf (x, a, 1.0);

    if (dP == 0.0 || n++ > 32)
      goto end;

    lambda = dP / GSL_MAX (2 * fabs (dP / x), phi);

    {
      double step0 = lambda;
      double step1 = -((a - 1) / x - 1) * lambda * lambda / 4.0;

      double step = step0;
      if (fabs (step1) < 0.5 * fabs (step0))
        step += step1;

      if (x + step > 0)
        x += step;
      else
        {
          x /= 2.0;
        }

      if (fabs (step0) > 1e-10 * x || fabs(step0 * phi) > 1e-10 * P)
        goto start;
    }

  end:
    if (fabs(dP) > GSL_SQRT_DBL_EPSILON * P)
      {
        GSL_ERROR_VAL("inverse failed to converge", GSL_EFAILED, GSL_NAN);
      }

    return b * x;
  }
}

double
gsl_cdf_gamma_Qinv (const double Q, const double a, const double b)
{
  double x;

  if (Q == 1.0)
    {
      return 0.0;
    }
  else if (Q == 0.0)
    {
      return GSL_POSINF;
    }

  /* Consider, small, large and intermediate cases separately.  The
     boundaries at 0.05 and 0.95 have not been optimised, but seem ok
     for an initial approximation. */

  if (Q < 0.05)
    {
      double x0 = -log (Q) + gsl_sf_lngamma (a);
      x = x0;
    }
  else if (Q > 0.95)
    {
      double x0 = exp ((gsl_sf_lngamma (a) + log1p (-Q)) / a);
      x = x0;
    }
  else
    {
      double xg = gsl_cdf_ugaussian_Qinv (Q);
      double x0 = (xg < -0.5*sqrt (a)) ? a : sqrt (a) * xg + a;
      x = x0;
    }

  /* Use Lagrange's interpolation for E(x)/phi(x0) to work backwards
     to an improved value of x (Abramowitz & Stegun, 3.6.6)

     where E(x)=P-integ(phi(u),u,x0,x) and phi(u) is the pdf.
   */

  {
    double lambda, dQ, phi;
    unsigned int n = 0;

  start:
    dQ = Q - gsl_cdf_gamma_Q (x, a, 1.0);
    phi = gsl_ran_gamma_pdf (x, a, 1.0);

    if (dQ == 0.0 || n++ > 32)
      goto end;

    lambda = -dQ / GSL_MAX (2 * fabs (dQ / x), phi);

    {
      double step0 = lambda;
      double step1 = -((a - 1) / x - 1) * lambda * lambda / 4.0;

      double step = step0;
      if (fabs (step1) < 0.5 * fabs (step0))
        step += step1;

      if (x + step > 0)
        x += step;
      else
        {
          x /= 2.0;
        }

      if (fabs (step0) > 1e-10 * x)
        goto start;
    }

  }

end:
  return b * x;
}