/**
 *
 * @generated s Tue Jan  7 11:45:26 2014
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <cblas.h>
#include <lapacke.h>
#include <plasma.h>
#include <core_blas.h>
#include "auxiliary.h"

/*-------------------------------------------------------------------
 * Check the orthogonality of Q
 */

int s_check_orthogonality(int M, int N, int LDQ, float *Q)
{
    float alpha, beta;
    float normQ;
    int info_ortho;
    int i;
    int minMN = min(M, N);
    float eps;
    float *work = (float *)malloc(minMN*sizeof(float));

    eps = LAPACKE_slamch_work('e');
    alpha = 1.0;
    beta  = -1.0;

    /* Build the idendity matrix USE DLASET?*/
    float *Id = (float *) malloc(minMN*minMN*sizeof(float));
    memset((void*)Id, 0, minMN*minMN*sizeof(float));
    for (i = 0; i < minMN; i++)
        Id[i*minMN+i] = (float)1.0;

    /* Perform Id - Q'Q */
    if (M >= N)
        cblas_ssyrk(CblasColMajor, CblasUpper, CblasTrans, N, M, alpha, Q, LDQ, beta, Id, N);
    else
        cblas_ssyrk(CblasColMajor, CblasUpper, CblasNoTrans, M, N, alpha, Q, LDQ, beta, Id, M);

    normQ = LAPACKE_slansy_work(LAPACK_COL_MAJOR, 'i', 'u', minMN, Id, minMN, work);

    printf("============\n");
    printf("Checking the orthogonality of Q \n");
    printf("||Id-Q'*Q||_oo / (N*eps) = %e \n",normQ/(minMN*eps));

    if ( isnan(normQ / (minMN * eps)) || (normQ / (minMN * eps) > 10.0) ) {
        printf("-- Orthogonality is suspicious ! \n");
        info_ortho=1;
    }
    else {
        printf("-- Orthogonality is CORRECT ! \n");
        info_ortho=0;
    }

    free(work); free(Id);

    return info_ortho;
}

/*------------------------------------------------------------
 *  Check the factorization QR
 */

int s_check_QRfactorization(int M, int N, float *A1, float *A2, int LDA, float *Q)
{
    float Anorm, Rnorm;
    float alpha, beta;
    int info_factorization;
    int i,j;
    float eps;

    eps = LAPACKE_slamch_work('e');

    float *Ql       = (float *)malloc(M*N*sizeof(float));
    float *Residual = (float *)malloc(M*N*sizeof(float));
    float *work              = (float *)malloc(max(M,N)*sizeof(float));

    alpha=1.0;
    beta=0.0;

    if (M >= N) {
        /* Extract the R */
        float *R = (float *)malloc(N*N*sizeof(float));
        memset((void*)R, 0, N*N*sizeof(float));
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'u', M, N, A2, LDA, R, N);

        /* Perform Ql=Q*R */
        memset((void*)Ql, 0, M*N*sizeof(float));
        cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, N, (alpha), Q, LDA, R, N, (beta), Ql, M);
        free(R);
    }
    else {
        /* Extract the L */
        float *L = (float *)malloc(M*M*sizeof(float));
        memset((void*)L, 0, M*M*sizeof(float));
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'l', M, N, A2, LDA, L, M);

    /* Perform Ql=LQ */
        memset((void*)Ql, 0, M*N*sizeof(float));
        cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, M, (alpha), L, M, Q, LDA, (beta), Ql, M);
        free(L);
    }

    /* Compute the Residual */
    for (i = 0; i < M; i++)
        for (j = 0 ; j < N; j++)
            Residual[j*M+i] = A1[j*LDA+i]-Ql[j*M+i];

    Rnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, N, Residual, M, work);
    Anorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, N, A2, LDA, work);

    if (M >= N) {
        printf("============\n");
        printf("Checking the QR Factorization \n");
        printf("-- ||A-QR||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));
    }
    else {
        printf("============\n");
        printf("Checking the LQ Factorization \n");
        printf("-- ||A-LQ||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));
    }

    if (isnan(Rnorm / (Anorm * N *eps)) || (Rnorm / (Anorm * N * eps) > 10.0) ) {
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else {
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(work); free(Ql); free(Residual);

    return info_factorization;
}

/*------------------------------------------------------------------------
 *  Check the factorization of the matrix A2
 */

int s_check_LLTfactorization(int N, float *A1, float *A2, int LDA, int uplo)
{
    float Anorm, Rnorm;
    float alpha;
    int info_factorization;
    int i,j;
    float eps;

    eps = LAPACKE_slamch_work('e');

    float *Residual = (float *)malloc(N*N*sizeof(float));
    float *L1       = (float *)malloc(N*N*sizeof(float));
    float *L2       = (float *)malloc(N*N*sizeof(float));
    float *work              = (float *)malloc(N*sizeof(float));

    memset((void*)L1, 0, N*N*sizeof(float));
    memset((void*)L2, 0, N*N*sizeof(float));

    alpha= 1.0;

    LAPACKE_slacpy_work(LAPACK_COL_MAJOR,' ', N, N, A1, LDA, Residual, N);

    /* Dealing with L'L or U'U  */
    if (uplo == PlasmaUpper){
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L1, N);
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L2, N);
        cblas_strmm(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, N, N, (alpha), L1, N, L2, N);
    }
    else{
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L1, N);
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L2, N);
        cblas_strmm(CblasColMajor, CblasRight, CblasLower, CblasTrans, CblasNonUnit, N, N, (alpha), L1, N, L2, N);
    }

    /* Compute the Residual || A -L'L|| */
    for (i = 0; i < N; i++)
        for (j = 0; j < N; j++)
           Residual[j*N+i] = L2[j*N+i] - Residual[j*N+i];

    Rnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', N, N, Residual, N, work);
    Anorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', N, N, A1, LDA, work);

    printf("============\n");
    printf("Checking the Cholesky Factorization \n");
    printf("-- ||L'L-A||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));

    if ( isnan(Rnorm/(Anorm*N*eps)) || (Rnorm/(Anorm*N*eps) > 10.0) ){
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else{
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(Residual); free(L1); free(L2); free(work);

    return info_factorization;
}

/*--------------------------------------------------------------
 * Check the gemm
 */
float s_check_gemm(PLASMA_enum transA, PLASMA_enum transB, int M, int N, int K,
                   float alpha, float *A, int LDA,
                   float *B, int LDB,
                   float beta, float *Cplasma,
                   float *Cref, int LDC,
                   float *Cinitnorm, float *Cplasmanorm, float *Clapacknorm )
{
    float beta_const = -1.0;
    float Rnorm;
    float *work = (float *)malloc(max(K,max(M, N))* sizeof(float));

    *Cinitnorm   = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, N, Cref,    LDC, work);
    *Cplasmanorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, N, Cplasma, LDC, work);

    cblas_sgemm(CblasColMajor, (CBLAS_TRANSPOSE)transA, (CBLAS_TRANSPOSE)transB, M, N, K,
                (alpha), A, LDA, B, LDB, (beta), Cref, LDC);

    *Clapacknorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, N, Cref, LDC, work);

    cblas_saxpy(LDC * N, (beta_const), Cplasma, 1, Cref, 1);

    Rnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, N, Cref, LDC, work);

    free(work);

    return Rnorm;
}

/*--------------------------------------------------------------
 * Check the trsm
 */
float s_check_trsm(PLASMA_enum side, PLASMA_enum uplo, PLASMA_enum trans, PLASMA_enum diag,
                   int M, int NRHS, float alpha,
                   float *A, int LDA,
                   float *Bplasma, float *Bref, int LDB,
                   float *Binitnorm, float *Bplasmanorm, float *Blapacknorm )
{
    float beta_const = -1.0;
    float Rnorm;
    float *work = (float *)malloc(max(M, NRHS)* sizeof(float));
    /*float eps = LAPACKE_slamch_work('e');*/

    *Binitnorm   = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, NRHS, Bref,    LDB, work);
    *Bplasmanorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'm', M, NRHS, Bplasma, LDB, work);

    cblas_strsm(CblasColMajor, (CBLAS_SIDE)side, (CBLAS_UPLO)uplo,
                (CBLAS_TRANSPOSE)trans, (CBLAS_DIAG)diag, M, NRHS,
                (alpha), A, LDA, Bref, LDB);

    *Blapacknorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'm', M, NRHS, Bref, LDB, work);

    cblas_saxpy(LDB * NRHS, (beta_const), Bplasma, 1, Bref, 1);

    Rnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'm', M, NRHS, Bref, LDB, work);
    Rnorm = Rnorm / *Blapacknorm;
    /* max(M,NRHS) * eps);*/

    free(work);

    return Rnorm;
}

/*--------------------------------------------------------------
 * Check the solution
 */

float s_check_solution(int M, int N, int NRHS, float *A, int LDA,
                      float *B,  float *X, int LDB,
                      float *anorm, float *bnorm, float *xnorm )
{
/*     int info_solution; */
    float Rnorm = -1.00;
    float zone  =  1.0;
    float mzone = -1.0;
    float *work = (float *)malloc(max(M, N)* sizeof(float));

    *anorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, N,    A, LDA, work);
    *xnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, NRHS, X, LDB, work);
    *bnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', N, NRHS, B, LDB, work);

    cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, NRHS, N, (zone), A, LDA, X, LDB, (mzone), B, LDB);

    Rnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', N, NRHS, B, LDB, work);

    free(work);

    return Rnorm;
}


/* integration/qng.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 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 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

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

#include "err.c"
#include "qng.h"

int
gsl_integration_qng (const gsl_function *f,
                     double a, double b,
                     double epsabs, double epsrel,
                     double * result, double * abserr, size_t * neval)
{
  double fv1[5], fv2[5], fv3[5], fv4[5];
  double savfun[21];  /* array of function values which have been computed */
  double res10, res21, res43, res87;    /* 10, 21, 43 and 87 point results */
  double result_kronrod, err ;
  double resabs; /* approximation to the integral of abs(f) */
  double resasc; /* approximation to the integral of abs(f-i/(b-a)) */

  const double half_length =  0.5 * (b - a);
  const double abs_half_length = fabs (half_length);
  const double center = 0.5 * (b + a);
  const double f_center = GSL_FN_EVAL(f, center);

  int k ;

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

  /* Compute the integral using the 10- and 21-point formula. */

  res10 = 0;
  res21 = w21b[5] * f_center;
  resabs = w21b[5] * fabs (f_center);

  for (k = 0; k < 5; k++)
    {
      const double abscissa = half_length * x1[k];
      const double fval1 = GSL_FN_EVAL(f, center + abscissa);
      const double fval2 = GSL_FN_EVAL(f, center - abscissa);
      const double fval = fval1 + fval2;
      res10 += w10[k] * fval;
      res21 += w21a[k] * fval;
      resabs += w21a[k] * (fabs (fval1) + fabs (fval2));
      savfun[k] = fval;
      fv1[k] = fval1;
      fv2[k] = fval2;
    }

  for (k = 0; k < 5; k++)
    {
      const double abscissa = half_length * x2[k];
      const double fval1 = GSL_FN_EVAL(f, center + abscissa);
      const double fval2 = GSL_FN_EVAL(f, center - abscissa);
      const double fval = fval1 + fval2;
      res21 += w21b[k] * fval;
      resabs += w21b[k] * (fabs (fval1) + fabs (fval2));
      savfun[k + 5] = fval;
      fv3[k] = fval1;
      fv4[k] = fval2;
    }

  resabs *= abs_half_length ;

  {
    const double mean = 0.5 * res21;

    resasc = w21b[5] * fabs (f_center - mean);

    for (k = 0; k < 5; k++)
      {
        resasc +=
          (w21a[k] * (fabs (fv1[k] - mean) + fabs (fv2[k] - mean))
          + w21b[k] * (fabs (fv3[k] - mean) + fabs (fv4[k] - mean)));
      }
    resasc *= abs_half_length ;
  }

  result_kronrod = res21 * half_length;

  err = rescale_error ((res21 - res10) * half_length, resabs, resasc) ;

  /*   test for convergence. */

  if (err < epsabs || err < epsrel * fabs (result_kronrod))
    {
      * result = result_kronrod ;
      * abserr = err ;
      * neval = 21;
      return GSL_SUCCESS;
    }

  /* compute the integral using the 43-point formula. */

  res43 = w43b[11] * f_center;

  for (k = 0; k < 10; k++)
    {
      res43 += savfun[k] * w43a[k];
    }

  for (k = 0; k < 11; k++)
    {
      const double abscissa = half_length * x3[k];
      const double fval = (GSL_FN_EVAL(f, center + abscissa)
                           + GSL_FN_EVAL(f, center - abscissa));
      res43 += fval * w43b[k];
      savfun[k + 10] = fval;
    }

  /*  test for convergence */

  result_kronrod = res43 * half_length;
  err = rescale_error ((res43 - res21) * half_length, resabs, resasc);

  if (err < epsabs || err < epsrel * fabs (result_kronrod))
    {
      * result = result_kronrod ;
      * abserr = err ;
      * neval = 43;
      return GSL_SUCCESS;
    }

  /* compute the integral using the 87-point formula. */

  res87 = w87b[22] * f_center;

  for (k = 0; k < 21; k++)
    {
      res87 += savfun[k] * w87a[k];
    }

  for (k = 0; k < 22; k++)
    {
      const double abscissa = half_length * x4[k];
      res87 += w87b[k] * (GSL_FN_EVAL(f, center + abscissa)
                          + GSL_FN_EVAL(f, center - abscissa));
    }

  /*  test for convergence */

  result_kronrod = res87 * half_length ;

  err = rescale_error ((res87 - res43) * half_length, resabs, resasc);

  if (err < epsabs || err < epsrel * fabs (result_kronrod))
    {
      * result = result_kronrod ;
      * abserr = err ;
      * neval = 87;
      return GSL_SUCCESS;
    }

  /* failed to converge */

  * result = result_kronrod ;
  * abserr = err ;
  * neval = 87;

  GSL_ERROR("failed to reach tolerance with highest-order rule", GSL_ETOL) ;
}


#ifndef METHODS_HELLER_H
#define METHODS_HELLER_H

#include <gsl/gsl_multimin.h>

namespace method {
// use your method name to create a subspace for your
// implementation of details
namespace heller_cwa {
namespace details {

struct HellerParam {

  // exp(a[0] p^2 + a[1] p + a[2] p q + a[3] q + a[4] q^2 + a[5])
  // alpha = 0.5 / a[0]
  // delta = a[1]
  // epsilon = a[2]

  arma::cx_vec a;
  arma::mat points;
  math::Polynomial<cx_double> V_eff_0;
  double mass;
};

inline
cx_double heller_gaussian(const arma::cx_vec & a,
                          const double q,
                          const double p) {
  return std::exp(a(0) * std::pow(p, 2)
                  + a(1) * p
                  + a(2) * p * q
                  + a(3) * q
                  + a(4) * std::pow(q, 2)
                  + a(5));
}

inline
arma::cx_vec heller_gaussian(const arma::cx_vec & a,
                             const arma::mat & points) {
  if (points.n_rows != 2) {
    throw Error("heller_gaussian only supports one-dimensional phase space");
  }

  arma::cx_vec result(points.n_cols);
#pragma omp parallel for
  for (arma::uword i = 0; i < result.n_elem; i++) {
    result(i) = heller_gaussian(a, points(0, i), points(1, i));
  }

  return result;
}

inline
math::Gaussian<cx_double> heller_gaussian(const arma::cx_vec & a) {

  const arma::cx_mat binomial{{{-a(4) * 2.0, -a(2)}, {-a(2), -a(0) * 2.0}}};
  const arma::cx_mat covariance = arma::inv(binomial);
  const arma::cx_vec center = covariance * arma::cx_vec{a(3),a(1)};

  return math::Gaussian<cx_double>(covariance, center,
      std::exp(a(5)) * std::exp(0.5 * arma::dot(center, arma::cx_vec{a(3),a(1)})));

}

template<typename Potential>
arma::mat force(const Potential & potential,
                const arma::mat & positions) {
  arma::mat result = arma::mat(arma::size(positions));

#pragma omp parallel for
  for (arma::uword i = 0; i < positions.n_cols; i++) {
    const arma::vec position = positions.col(i);
    for (arma::uword j = 0; j < positions.n_rows; j++) {
      result(j, i) = -potential.derivative(j).at(position);
    }
  }

  return result;
}

template<typename Function>
inline
auto effective_potential(const Function & potential,
                         const cx_double alpha,
                         const cx_double delta,
                         const cx_double epsilon,
                         const arma::uword l,
                         const arma::uword cut_off) {

  if (potential.dim() > 1) {
    throw Error("Currently effective potential is only valid "
                "for one dimensional potential");
  }

  const auto functor = [alpha](const Function & function) -> Function {
    return function.derivative(0).derivative(0) / 8.0 / alpha;
  };

  const auto a = exp(functor, derivative(potential, arma::uvec{l}), cut_off) *
                 std::pow(-1, l + 1);
  const auto b = exp(functor, derivative(potential, arma::uvec{l}), cut_off);

  const std::vector<math::Polynomial<cx_double>>
      a_inner =
      {math::Polynomial<cx_double>(
          arma::cx_vec{1.0 + epsilon / 2.0,
                       cx_double{0.0, 1.0} / 2.0 / alpha,
                       0.5 * delta},
          lmat{{1, 0, 0},
               {0, 1, 0}})};

  const std::vector<math::Polynomial<cx_double>>
      b_inner =
      {math::Polynomial<cx_double>(
          arma::cx_vec{1.0 - epsilon / 2.0,
                       -cx_double{0.0, 1.0} / 2.0 / alpha,
                       -0.5 * delta},
          lmat{{1, 0, 0},
               {0, 1, 0}})};

  const auto combined = (a(a_inner) + b(b_inner)) * 0.5;

  return combined;
}

inline
math::Polynomial<cx_double> E_function(const arma::cx_vec & a,
                                       const arma::cx_vec & a_derivatives,
                                       const math::Polynomial<cx_double> & V_eff_0,
                                       const double mass) {

  if (a.n_elem != 6 || a_derivatives.n_elem != 6) {
    throw Error(
        "The number of elements for a's or their derivatives is invalid for E function");
  }

  const math::Polynomial<cx_double> without_derivatives(
      arma::cx_vec{-a(2) / mass, -2.0 * a(4) / mass, -a(3) / mass},
      lmat{{{0, 1, 0}, {2, 1, 1}}}
  );

  const math::Polynomial<cx_double> derivatives(
      arma::cx_vec{-a_derivatives(0),
                   -a_derivatives(1),
                   -a_derivatives(2),
                   -a_derivatives(3),
                   -a_derivatives(4),
                   -a_derivatives(5)},
      lmat{{{0, 0, 1, 1, 2, 0}, {2, 1, 1, 0, 0, 0}}}
  );

  return without_derivatives + derivatives - V_eff_0 * cx_double{0.0, 2.0};

}

inline
double I_function(
    const arma::cx_vec & a,
    const arma::cx_vec & a_derivatives,
    const arma::mat & points,
    const math::Polynomial<cx_double> & V_eff_0,
    const double mass) {

  return arma::sum(arma::pow(arma::abs(
      at(E_function(a, a_derivatives, V_eff_0, mass), points)
      % heller_gaussian(a, points)), 2));

}

inline
arma::vec I_function_derivative(
    const arma::cx_vec & a,
    const arma::cx_vec & a_derivatives,
    const arma::mat & points,
    const math::Polynomial<cx_double> & V_eff_0,
    const double mass
) {

  arma::vec result(12);

  const auto E_func = E_function(a, a_derivatives, V_eff_0, mass);

  const auto polynomial_term_list = lmat{{{0, 0, 1, 1, 2, 0}, {2, 1, 1, 0, 0, 0}}};

#pragma omp parallel for
  for (arma::uword i = 0; i < 6; i++) {

    const lvec E_derivative_term = polynomial_term_list.col(i);
    const auto E_derivative =
        math::polynomial::Term<double>(1.0, E_derivative_term);

    result(i) = -arma::sum(2.0 *
                           arma::real(at(E_func.conj() * E_derivative, points))
                           %
                           arma::pow(arma::abs(heller_gaussian(a, points)), 2));

    result(i + 6) = arma::sum(2.0 *
                              arma::imag(
                                  at(E_func.conj() * E_derivative, points))
                              %
                              arma::pow(arma::abs(heller_gaussian(a, points)),
                                        2));
  }

  return result;
}

inline
double I_function_gsl_wrapper(
    const gsl_vector * a_derivatives,
    void * param
) {
  const HellerParam heller_param = *(HellerParam *) param;
  const arma::cx_vec a = heller_param.a;
  const arma::vec a_derivatives_all = gsl::convert_vec(a_derivatives);
  const arma::cx_vec a_derivatives_arma =
      arma::cx_vec{a_derivatives_all.rows(0, 5),
                   a_derivatives_all.rows(6, 11)};

  return I_function(a,
                    a_derivatives_arma,
                    heller_param.points,
                    heller_param.V_eff_0,
                    heller_param.mass);

}

inline
void I_function_derivative_gsl_wrapper(
    const gsl_vector * a_derivatives,
    void * param,
    gsl_vector * g
) {
  const HellerParam heller_param = *(HellerParam *) param;
  const arma::cx_vec a = heller_param.a;
  const arma::vec a_derivatives_all = gsl::convert_vec(a_derivatives);

  const arma::cx_vec a_derivatives_arma =
      arma::cx_vec{a_derivatives_all.rows(0, 5),
                   a_derivatives_all.rows(6, 11)};

  const auto result_pointer =
      gsl::convert_vec(I_function_derivative(a,
                                             a_derivatives_arma,
                                             heller_param.points,
                                             heller_param.V_eff_0,
                                             heller_param.mass));

  gsl_vector_memcpy(g, result_pointer);

  gsl_vector_free(result_pointer);
}

inline
void I_function_fdf_gsl_wrapper(
    const gsl_vector * a_derivatives,
    void * param,
    double * f,
    gsl_vector * g
) {
  I_function_derivative_gsl_wrapper(a_derivatives, param, g);

  *f = I_function_gsl_wrapper(a_derivatives, param);
}

inline
arma::cx_vec a_derivative(HellerParam input,
                          const double initial_step_size,
                          const double tolerance,
                          const double gradient_tolerance,
                          const size_t total_steps) {

  /* allocate memory for minimization process */
  const auto minimizer_type = gsl_multimin_fdfminimizer_vector_bfgs2;
  auto minimizer_environment = gsl_multimin_fdfminimizer_alloc(minimizer_type,
                                                               12);

  /* assigning function to minimizer object */
  gsl_multimin_function_fdf minimizer_object;
  minimizer_object.f = &I_function_gsl_wrapper;
  minimizer_object.df = &I_function_derivative_gsl_wrapper;
  minimizer_object.fdf = &I_function_fdf_gsl_wrapper;
  minimizer_object.n = 12;
  minimizer_object.params = (void *) &input;

  /* starting point */
  const auto a_derivatives = gsl_vector_calloc(12);

  /* set environment */
  gsl_multimin_fdfminimizer_set(minimizer_environment,
                                &minimizer_object, a_derivatives,
                                initial_step_size, tolerance);

  size_t iter = 0;
  int status = GSL_CONTINUE;
  do {
    iter++;
    status = gsl_multimin_fdfminimizer_iterate(minimizer_environment);

    if (status) {
      throw Error(gsl_strerror(status));
    }

    status = gsl_multimin_test_gradient(minimizer_environment->gradient,
                                        gradient_tolerance);

    if (status == GSL_SUCCESS) {
      const arma::vec result = gsl::convert_vec(minimizer_environment->x);

      gsl_multimin_fdfminimizer_free(minimizer_environment);
      gsl_vector_free(a_derivatives);

      return arma::cx_vec{result.rows(arma::span(0, 5)),
                          result.rows(arma::span(6, 11))};
    }
  } while (status == GSL_CONTINUE && iter < total_steps);

  return arma::ones<arma::cx_vec>(6) * 2.8375;
}

} // namespace details

struct State {
public:
  arma::mat points;
  arma::vec weights;
  arma::vec masses;

  // Establish an easy way to construct your State
  template<typename PhaseSpaceDistribution>
  State(const PhaseSpaceDistribution & initial,
        const arma::uvec & grid,
        const arma::mat & range,
        const arma::vec & masses) :
      points(math::space::points_generate(grid, range)),
      weights(arma::real(at(initial, points))),
      masses(masses) {
    if (grid.n_rows != range.n_rows) {
      throw Error("Different dimension between the grid and the range");
    }
    if (grid.n_rows != 2 * masses.n_rows) {
      throw Error("Different dimension between the grid and the masses");
    }
  }

  template<typename PhaseSpaceDistribution>
  State(const PhaseSpaceDistribution & initial,
        const arma::uvec & grid,
        const arma::mat & range) :
      points(math::space::points_generate(grid, range)),
      weights(arma::real(at(initial, points))),
      masses(arma::ones<arma::vec>(grid.n_elem / 2)) {
    if (grid.n_elem % 2 != 0) {
      throw Error("Odd number of dimension - it is not likely a phase space");
    }
    if (grid.n_rows != range.n_rows) {
      throw Error("Different dimension between the grid and the range");
    }
    if (grid.n_rows != 2 * masses.n_rows) {
      throw Error("Different dimension between the grid and the masses");
    }
  }

  inline
  State(const arma::mat & points,
        const arma::vec & weights,
        const arma::vec & masses) :
      points(points),
      weights(weights),
      masses(masses) {
    if (points.n_cols != weights.n_elem) {
      throw Error("Different number of points and corresponding weights");
    }

    if (points.n_rows != 2 * masses.n_rows) {
      throw Error("Different dimension between the points and the masses");
    }
  }

  inline
  arma::uword dim() const {
    return points.n_rows / 2;
  }

  inline
  State normalise() const {
    return State(this->points, this->weights / arma::sum(this->weights),
                 this->masses);
  }

  template<typename Function>
  arma::vec expectation(const std::vector<Function> & function) const {
    arma::vec result(function.size());

#pragma omp parallel for
    for (arma::uword i = 0; i < result.n_elem; i++) {

      if (function[i].dim() != this->dim() * 2) {
        throw Error(
            "The dimension of the function is invalid for the calculation of expectation");
      }
      result(i) = arma::dot(at(function[i], this->points), weights) /
                  arma::sum(weights);
    }

    return result;
  }

  template<typename Function>
  double expectation(const Function & function) const {
    const arma::vec result = at(function, this->points);

    return arma::dot(result, weights) / arma::sum(weights);
  }

  inline
  arma::vec positional_expectation() const {
    arma::uword dim = this->dim();

    return this->points.rows(0, dim - 1) * this->weights /
           arma::sum(this->weights);
  }

  inline
  arma::vec momentum_expectation() const {
    arma::uword dim = this->dim();

    return this->points.rows(dim, 2 * dim - 1) * this->weights /
           arma::sum(this->weights);
  }

  State operator+(const State & B) const {
    if (!arma::approx_equal(this->weights, B.weights, "abs_diff", 1e-16) ||
        !arma::approx_equal(this->masses, B.masses, "abs_diff", 1e-16)) {
      throw Error("Different cwa states are being added");
    }

    return State(this->points + B.points, this->weights, this->masses);
  }

  State operator*(const double B) const {
    return State(this->points * B, this->weights, this->masses);
  }
};

template<typename Potential>
struct Operator {

private:
  PropagationType type = Classic;

public:
  Potential potential;

  Operator(const State & state,
           const Potential & potential) :
      potential(potential) {}

  inline
  PropagationType propagation_type() const {
    return Classic;
  }

  State operator()(const State & state) const {

    arma::mat p_submatrix = state.points.rows(state.dim(), 2 * state.dim() - 1);
    p_submatrix.each_col() /= state.masses;

    const arma::mat change_list =
        arma::join_cols(p_submatrix,
                        details::force(potential,
                                       state.points.rows(0, state.dim() - 1)));

    return State(change_list, state.weights, state.masses);
  }

};

} // namespace heller
}

#endif //METHODS_HELLER_H


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <time.h>
#include <gsl/gsl_math.h>

#include "cosmocalc.h"

void test_nonlinear_corrfunc(void)
{
  double lnrmin = log(1e-5);
  double lnrmax = log(3e2);
  int N = 10000;
  double r,dlnr = (lnrmax - lnrmin)/N;
  int i;
  FILE *fp;
  double a;

  a = 1.0;
  fp = fopen("xinltest.txt","w");
  for(i=0;i<N;++i)
    {
      r = exp(dlnr*i + lnrmin);
      fprintf(fp,"%.20e\t%.20e\t%.20e\n",r,nonlinear_corrfunc(r,a),linear_corrfunc(r,a));
    }
  fclose(fp);
}

void test_nonlinear_powspec(void)
{
  double lnkmin = log(1e-4);
  double lnkmax = log(1e2);
  double dlnk,k;
  int N = 10000,i;
  FILE *fp;
  double a = 1.0;

  dlnk = (lnkmax - lnkmin)/N;

  a = 1.0;
  fp = fopen("pknltest.txt","w");
  for(i=0;i<N;++i)
    {
      k = exp(dlnk*i + lnkmin);
      fprintf(fp,"%.20e\t%.20e\t%.20e\n",k,nonlinear_powspec(k,a),linear_powspec(k,a));
    }
  fclose(fp);

  a = 1.0/(1.0 + 0.5);
  fp = fopen("pknltest1.txt","w");
  for(i=0;i<N;++i)
    {
      k = exp(dlnk*i + lnkmin);
      fprintf(fp,"%.20e\t%.20e\t%.20e\n",k,nonlinear_powspec(k,a),linear_powspec(k,a));
    }
  fclose(fp);

  a = 1.0/(1.0 + 1.0);
  fp = fopen("pknltest2.txt","w");
  for(i=0;i<N;++i)
    {
      k = exp(dlnk*i + lnkmin);
      fprintf(fp,"%.20e\t%.20e\t%.20e\n",k,nonlinear_powspec(k,a),linear_powspec(k,a));
    }
  fclose(fp);

  a = 1.0/(1.0 + 2.00);
  fp = fopen("pknltest3.txt","w");
  for(i=0;i<N;++i)
    {
      k = exp(dlnk*i + lnkmin);
      fprintf(fp,"%.20e\t%.20e\t%.20e\n",k,nonlinear_powspec(k,a),linear_powspec(k,a));
    }
  fclose(fp);

  a = 1.0/(1.0 + 3.00);
  fp = fopen("pknltest4.txt","w");
  for(i=0;i<N;++i)
    {
      k = exp(dlnk*i + lnkmin);
      fprintf(fp,"%.20e\t%.20e\t%.20e\n",k,nonlinear_powspec(k,a),linear_powspec(k,a));
    }
  fclose(fp);
}

void test_linxi_fftlog(void)
{
  double rmin;
  long i,N = 2048;
  double dlnr;
  FILE *fp;
  double r;
  double a = 1.0;

  double *pk,*xi;
  double k0,L,q,r0,k;
  double kmin = 1e-7;
  double kmax = 1e7;
  double dlnk = log(kmax/kmin)/N;

  L = log(kmax/kmin);
  k0 = exp(log(kmin) + L/2.0);
  q = 0.0;

  pk = (double*)malloc(sizeof(double)*N);
  assert(pk != NULL);
  xi = (double*)malloc(sizeof(double)*N);
  assert(xi != NULL);

  for(i=0;i<N;++i)
    {
      k = exp(i*dlnk + log(kmin));
      pk[i] = linear_powspec_exact(k,a);
    }

  r0 = 1.0;
  compute_discrete_spherical_fft(pk,N,k0,L,q,xi,&r0);

  if(0)
    {
      for(i=0;i<N;++i)
	xi[i] = xi[i]/pow(2.0*M_PI,3.0);
      compute_discrete_spherical_fft(xi,N,r0,L,q,pk,&k0);

      kmin = exp(log(k0)-L/2.0);
      //fprintf(stderr,"k0 = %e, kmin = %e, L = %f, N = %ld\n",k0,kmin,L,N);
      fp = fopen("linxitest.txt","w");
      for(i=0;i<N;++i)
	{
	  k = exp(i*dlnk + log(kmin));
	  //fprintf(stderr,"k = %e, pk in,out = %e|%e\n",k,linear_powspec(k,a),pk[i]);
	  fprintf(fp,"%.20e \t %.20e \t %.20e \n",k,pk[i],linear_powspec_exact(k,a));
	}
      fclose(fp);

      return;
    }

  rmin = exp(log(r0)-L/2.0);
  dlnr = dlnk;
  fp = fopen("linxitest.txt","w");
  for(i=0;i<N;++i)
    {
      r = exp(i*dlnr + log(rmin));
      xi[i] = xi[i]/pow(2.0*M_PI,3.0);
      //fprintf(stderr,"%.20e \t %.20e \t %.20e \n",r,xi[i],linear_corrfunc_exact(r,a));
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",r,xi[i],linear_corrfunc_exact(r,a));
    }
  fclose(fp);
}

void test_linxi(void)
{
  double rmin=1e-9;
  double rmax=1e5;
  long i,Ntest = 1000;
  double dlnr = log(rmax/rmin)/Ntest;
  FILE *fp;
  double r;
  double a = 1.0;

  fp = fopen("linxitest.txt","w");
  for(i=0;i<Ntest;++i)
    {
      r = exp(i*dlnr + log(rmin));
      //fprintf(stderr,"%.20e \t %.20e \t %.20e \n",r,linear_corrfunc_exact(r,a),linear_corrfunc_exact(r,a));
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",r,linear_corrfunc(r,a),linear_corrfunc_exact(r,a));
    }
  fclose(fp);
}

void test_biasfunc(void)
{
  FILE *fp;
  double m,lgm;
  double mmin = 6.0;
  double mmax = 16.0;
  double dm = 0.01;
  double a = 1.0;

  fp = fopen("btest.txt","w");
  lgm = mmax;
  while(lgm >= mmin)
    {
      m = pow(10.0,lgm);
      fprintf(fp,"%.20e \t %.20e \n",1.686/sigmaMtophat(m,a),bias_function(m,a));
      lgm -= dm;
    }
  fclose(fp);

}

void test_massfunc(void)
{
  FILE *fp;
  double m,lgm;
  double mmin = 10.0;
  double mmax = 16.0;
  double dm = 0.01;
  double a = 1.0;

  fp = fopen("mftest.txt","w");
  lgm = mmax;
  while(lgm >= mmin)
    {
      m = pow(10.0,lgm);
      fprintf(fp,"%.20e \t %.20e \n",m,mass_function(m,a)*m*m/RHO_CRIT/cosmoData.OmegaM);
      lgm -= dm;
    }
  fclose(fp);
}

void test_peakheight(void)
{
  FILE *fp;
  double r;
  double rmin = 1e-40;
  double rmax = 1e3;
  double dr = 0.99;
  double a = 1.0;
  double numin = 0.15;
  double numax = 6.0;
  double dnu = 0.99;
  double nu;

  double kmin=1e-10;
  double kmax=1e10;
  long i,Ntest = 1000;
  double dlnk = log(kmax/kmin)/Ntest;
  double k,p = 1e-6;

  fp = fopen("phtest_integ.txt","w");
  for(i=0;i<Ntest;++i)
    {
      k = (i*dlnk + log(kmin));
      fprintf(fp,"%.20e \t %.20e \n",exp(k),tophatradnorm_linear_powspec_exact_nonorm_lnk_integ_funct_I0(k,&p));
    }
  fclose(fp);

  fp = fopen("phtest.txt","w");
  r = rmax;
  while(r >= rmin)
    {
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",r,sigmaRtophat(r,a),sigmaRtophat_exact(r,a));
      r *= dr;
    }
  fclose(fp);

  fp = fopen("phtestrev.txt","w");
  nu = numax;
  while(nu >= numin)
    {
      fprintf(fp,"%.20e \t %.20e \n",inverse_nuRtophat(nu,a),DELTAC/nu);
      nu *= dnu;
    }
  fclose(fp);
}

void test_gf(void)
{
  FILE *fp;
  double a;
  double amin = 0.1;
  double amax = 1.0;
  double da = 0.0001;

  fp = fopen("gftest.txt","w");
  a = amax;
  while(a >= amin)
    {
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",a,growth_function(a),growth_function_exact(a));
      a -= da;
    }
  fclose(fp);
}

void test_linpk(void)
{
  double kmin=1e-8;
  double kmax=1e40;
  long i,Ntest = 10000;
  double dlnk = log(kmax/kmin)/Ntest;
  FILE *fp;
  double k;
  double a = 1.0;

  fp = fopen("pktest.txt","w");
  for(i=0;i<Ntest;++i)
    {
      k = exp(i*dlnk + log(kmin));
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",k,linear_powspec(k,a),linear_powspec_exact(k,a));
    }
  fclose(fp);

}

void test_transfunct(void)
{
  double kmin=1e-8;
  double kmax=1e30;
  long i,Ntest = 10000;
  double dlnk = log(kmax/kmin)/Ntest;
  FILE *fp;
  double k;

  fp = fopen("tftest.txt","w");
  for(i=0;i<Ntest;++i)
    {
      k = exp(i*dlnk + log(kmin));
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",k,transfer_function(k),transfunct_eh98(k));
    }
  fclose(fp);

  cosmoData.OmegaM = 0.2;
  cosmoData.h = 0.5;
  cosmoData.OmegaB = cosmoData.OmegaM*0.5;
  cosmoData.cosmoNum = 2;

  fp = fopen("tftest_highb.txt","w");
  for(i=0;i<Ntest;++i)
    {
      k = exp(i*dlnk + log(kmin));
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",k,transfer_function(k),transfunct_eh98(k));
    }
  fclose(fp);
}

void test_distances(void)
{
  FILE *fp;

  double amin = 0.1;
  double amax = 1.0;
  double da = 0.0001;
  double dmin = 0.0;
  double dmax = 3000.0;
  double dd = 1.0;
  double a,d;

  fp = fopen("disttest_a2d.txt","w");
  a = amax;
  while(a >= amin)
    {
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",a,comvdist(a),comvdist_exact(a));
      a -= da;
    }
  fclose(fp);

  fp = fopen("disttest_d2a.txt","w");
  d = dmin;
  while(d <= dmax)
    {
      a = acomvdist(d);
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",d,a,comvdist_exact(a));
      d += dd;
    }
  fclose(fp);
}


/////////////////
//example25.5.c
/////////////////
#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>
#include <math.h>

typedef struct{
  double g;//gravity
  double M;//mass of the car
  double m;//mass of the load
  double r;//length of the rope
  double C;//damping coefficient

  double F;//input force proportional to torque
  double T;//tension to the rope

  double x,x0;//x position of the load
  double y,y0;//y position of the load
  double X;//X position of the car
  double a;//angle (theta)

  double dx,dx0;
  double dy,dy0;
  double dX,dX0;
  double dr;
  double da,da0;

  double ddx;
  double ddy;
  double ddX;
  double ddr;
  double dda;
  double dXmax;
  double ddXmax;
  double Fmax;//Force

  double h;
  int nh;
  double _h;
  int _dim;
  double *_y;
  double *_y_err;
  double *_dydt_in;
  double *_dydt_out;
  const gsl_odeiv_step_type *_T;
  gsl_odeiv_step *_s;
  gsl_odeiv_system _sys;
  double _t;
} CRANE;

#define dim_crane 4
int cranefunc (double t, const double y[], double f[], void *params)
{
  CRANE *c = (CRANE *)params;
  //y[0]=crane.p, y[1]=crane.da, y[2]=crane.X, y[3]=crane.dX
  f[0] = y[1];
  f[1] = (-(2*c->dr+c->C)*y[1] -c->ddX*cos(y[0]) -c->g*sin(y[0]))/c->r;
  //  f[1] = (-2*c->dr -c->ddX*cos(y[0]) -c->g*sin(y[0]))/c->r;
  f[2] = y[3];
  //  f[3] = c->ddX;
  f[3] = (c->F+c->T*sin(y[0]))/c->M;
  return GSL_SUCCESS;
}
////////////////
#define square(x) ((x)*(x))
CRANE crane;
int initialize()
{

  crane.Fmax=20.0;//check//  crane.Fmax=30;//check
  //  crane.dXmax=1.0;
  crane.dXmax=1.0;
  crane.ddXmax=0.2;
  crane.g=9.8;
  crane.M=100;
  crane.m=20;//10;//10;//20
  crane.r=5;//5;
  crane.C=1;//5;
  crane.dr=crane.ddr=0;
  crane.x0=0;
  crane.y0=crane.r;
  crane.dX0=crane.dx0=crane.dy0=crane.da0=0;
#ifdef CRANESUB
  //  crane.Fmax=_crane_Fmax;//check//  crane.Fmax=30;//check
  crane.m=_crane_m;
  crane.C=_crane_C;
  crane.r=_crane_r;
  crane.h=AP_tS;
  crane.dXmax=_crane_dXmax;
  if(_AP_umax>0) AP_u_max=crane.Fmax=_AP_umax;
  else AP_u_max=crane.Fmax;
  AP_u_min=-AP_u_max;
  //  starttime=0;
  //  totaltime=100;//  totaltime=40;
  //  totaltime=50;//  totaltime=40;
  //  int kmax=totaltime/AP_tS;
  //  _rr=(double*)malloc(kmax*sizeof(double));
  rr=AP_r=_AP_r;//10
  //  int k;for(k=0;k<kmax;k++) _rr[k]=AP_r;//10;
  rr_kyoyou=_rr_kyoyou;
  //  p=(double *)malloc(buffsize*sizeof(double));
  C_MODE=11;
  //  iteration=_iteration;
  //  fprintf(stdout,"?????????iteration=%d\n",_iteration);
#else
  crane.h=0.01;
#endif
  crane.nh=10;
  crane.F=crane.a=crane.da=crane.dda=0;
  crane.T=crane.m*crane.g;
  crane._h=crane.h/crane.nh;//0.001
  crane._dim=4;
  if(crane._y==NULL){
    crane._y=(double*)malloc(crane._dim*sizeof(double));
    crane._y_err=(double*)malloc(crane._dim*sizeof(double));
    crane._dydt_in=(double*)malloc(crane._dim*sizeof(double));
    crane._dydt_out=(double*)malloc(crane._dim*sizeof(double));
  }
  crane._T= gsl_odeiv_step_rk4;
  crane._s= gsl_odeiv_step_alloc (crane._T, crane._dim);
  crane._sys= (gsl_odeiv_system){cranefunc, NULL, crane._dim, &crane};
  crane._t=crane._y[0]=crane._y[1]=crane._y[2]=crane._y[3]=0;
  int i;for(i=0;i<crane._dim;i++) crane._dydt_in[i]=0;
  GSL_ODEIV_FN_EVAL(&(crane._sys), crane._t, crane._y, crane._dydt_in);
  return(0);
}
#ifndef CRANESUB
char *fn="crane2io.dat";
FILE *fp;
#endif//#ifndef CRANESUB

double plant(double uu)
{
  //  crane.F=uu;
  if(uu>crane.Fmax) uu=crane.Fmax;
  else if(uu<-crane.Fmax) uu=-crane.Fmax;
  int n;
  for(n=0;n<crane.nh;n++){
    crane.F=uu;//
    if(crane.dX>=crane.dXmax && crane.F>0) crane.F=0;
    else if(crane.dX<=-crane.dXmax && crane.F<0) crane.F=0;
    int status = gsl_odeiv_step_apply (crane._s, crane._t, crane._h,
				       crane._y, crane._y_err,
				       crane._dydt_in,
				       crane._dydt_out,
				       &crane._sys);
    if (status != GSL_SUCCESS)   break;

    int i;for(i=0;i<dim_crane;i++) crane._dydt_in[i]=crane._dydt_out[i];
    crane._t+=crane._h;

    crane.a  =crane._y[0];
    crane.da =crane._y[1];
    crane.X  =crane._y[2];
    crane.dX =crane._y[3];

    crane.x  =crane.X+crane.r * sin(crane.a);
    crane.y  =crane.r*cos(crane.a);
    crane.dx=(crane.x-crane.x0)/crane._h;
    crane.dy=(crane.y-crane.y0)/crane._h;
    crane.ddx=(crane.dx-crane.dx0)/crane._h;
    crane.ddy=(crane.dy-crane.dy0)/crane._h;
    crane.T=crane.m*sqrt(square(crane.ddx)+square(crane.ddy-crane.g));
    crane.ddX=(crane.dX-crane.dX0)/crane._h;
    //    crane.F=crane.M*crane.ddX-crane.T*sin(crane.a);
    crane.dda=(crane.da-crane.da0)/crane._h;
    /*    crane.dda=(-2*crane.dr*crane.da-crane.g*sin(crane.a)-ddX[n]*cos(crane.a))/crane.r; */
    crane.x0=crane.x;
    crane.y0=crane.y;
    crane.dx0=crane.dx;
    crane.dy0=crane.dy;
    crane.da0=crane.da;
    crane.dX0=crane.dX;
  }
#ifndef CRANESUB
  fprintf(fp,"%.7e %.7e %.7e", crane._t,crane._y[0],crane._y[1]);//crane.a,crane.da
  fprintf(fp," %.7e %.7e %.7e" ,crane.X,crane.dX,crane.ddX);
  fprintf(fp," %.7e %.7e %.7e" ,crane.x,crane.dx,crane.ddx);
  fprintf(fp," %.7e %.7e %.7e" ,crane.y,crane.dy,crane.ddy);
  fprintf(fp," %.7e %.7e" ,crane.T,crane.F);
  fprintf(fp,"\n");
#endif
  return(crane.x);
  //  return(crane.X);
}

#ifndef CRANESUB
int main (void)
{
  //  CRANE crane;
  crane.g=9.8;
  crane.M=100;
  crane.m=20;
  crane.r=5;
  crane.dr=crane.ddr=0;

  ////////////////////////////////////////////////////
  /// method 2                                     ///
  /// input F                                     ///
  /// output p,x,y,X,...                          ///
  ////////////////////////////////////////////////////
  double h;//,hh=0.1;
  //result for different h
  /*gnuplot
   set style data lines;n=1;
   n=n+1;plot "crane1io.dat" using 1:n, "crane2io.dat" using 1:n;print "n=",n
   #strange at n=9(ddx),12(ddy),13(T)
  */
  ////////////////////////////////////////////////////
  ///   read F                                    ///
  ////////////////////////////////////////////////////
  char *fn1="crane1io.dat";
  FILE *fp1=fopen(fn1,"r");
  int n4;
#define buffsize 512
  char buff[buffsize];
  int n0;
  fgets(buff,buffsize,fp1);
  sscanf(buff,"#%lf%d%d%lf%lf%lf%lf",&h,&n0,&n4,&crane.M,&crane.m,&crane.r,&crane.C);
  crane.h=h;
  double t4=n4*h;
  double *F  =(double*)malloc(sizeof(double)*(n4+1));
  //  int M=n4+1;double *F  =(double*)malloc(sizeof(double)*M);
  //  double *ddX=(double*)malloc(sizeof(double)*M);

  int n;
  double t;
  //  double dx0=0,dy0=0,dX0=0,da0=0;
  //  double x0=0,y0=crane.r;

  fgets(buff,buffsize,fp1);
  for(n=0;n<n4;n++){//
    double _x1,_x2,_x3,_x4,_x5,_x6,_x7,_x8,_x9,_x10,_x11,_x12,_x13,_x14;
    fgets(buff,buffsize,fp1);
    sscanf(buff,"%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf",
	   &_x1,&_x2,&_x3,&_x4,&_x5,&_x6,&_x7,&_x8,&_x9,&_x10,&_x11,&_x12,&_x13,&_x14);
    F[n]=_x14;
  }
  ////////////////////////////////////////////////////
  ///   Solve by the Runge Kutta Method of GSL     ///
  ////////////////////////////////////////////////////
  initialize();
  fp=fopen(fn,"w");
  for(t=0;t<t4;t+=crane.h){
    n=t/crane.h;
    plant(F[n]);
  }
  fclose(fp);
  fprintf(stdout,"Results are stored in '%s'.\n",fn);

  gsl_odeiv_step_free (crane._s);
  return 0;
}
#endif


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_fit.h>

#include "cosmocalc.h"

static double linear_corrfunc_integ_funct(double k, void *p);

static double linear_corrfunc_integ_funct(double k, void *p)
{
  double r = ((double*)p)[0];
  double a = ((double*)p)[1];
  return linear_powspec(k,a)*k/r;
}

double linear_corrfunc_exact(double r, double a)
{
  double I0;
  double abserr,p[2];
  gsl_integration_workspace *workspace;
  gsl_function F;
  gsl_integration_qawo_table *wf;
  gsl_integration_workspace *cycle_workspace;

#define WORKSPACE_NUM 70
#define ABSERR 1e-3
  workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);
  cycle_workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);
  wf = gsl_integration_qawo_table_alloc(r,1.0,GSL_INTEG_SINE,(size_t) WORKSPACE_NUM);

  F.function = &linear_corrfunc_integ_funct;
  p[0] = r;
  p[1] = a;
  F.params = p;
  gsl_integration_qawf(&F,1e-8,ABSERR,(size_t) WORKSPACE_NUM,workspace,cycle_workspace,wf,&I0,&abserr);

  gsl_integration_qawo_table_free(wf);
  gsl_integration_workspace_free(cycle_workspace);
  gsl_integration_workspace_free(workspace);
#undef ABSERR
#undef WORKSPACE_NUM

  return I0/2.0/M_PI/M_PI;
}

double linear_corrfunc(double r, double a)
{
  static int initFlag = 1;
  static int currCosmoNum;
  static gsl_spline *cosmocalc_linear_corrfunc_spline = NULL;
  static gsl_interp_accel *cosmocalc_linear_corrfunc_acc = NULL;

  double linear_corrfunc_table[COSMOCALC_LINEAR_CORRFUNC_TABLE_LENGTH];
  double r_table[COSMOCALC_LINEAR_CORRFUNC_TABLE_LENGTH];
  long i;
  double gf;

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum)
    {
      initFlag = 0;
      currCosmoNum = cosmoData.cosmoNum;

      for(i=0;i<COSMOCALC_LINEAR_CORRFUNC_TABLE_LENGTH;++i)
	{
	  r_table[i] = log(CF_R_MAX/CF_R_MIN)/(COSMOCALC_LINEAR_CORRFUNC_TABLE_LENGTH-1.0)*((double) i) + log(CF_R_MIN);
	  linear_corrfunc_table[i] = linear_corrfunc_exact(exp(r_table[i]),1.0);
	}

      //init the spline and accelerators
      if(cosmocalc_linear_corrfunc_spline != NULL)
	gsl_spline_free(cosmocalc_linear_corrfunc_spline);
      cosmocalc_linear_corrfunc_spline = gsl_spline_alloc(GSL_SPLINE_TYPE,(size_t) (COSMOCALC_LINEAR_CORRFUNC_TABLE_LENGTH));
      gsl_spline_init(cosmocalc_linear_corrfunc_spline,r_table,linear_corrfunc_table,(size_t) (COSMOCALC_LINEAR_CORRFUNC_TABLE_LENGTH));
      if(cosmocalc_linear_corrfunc_acc != NULL)
	gsl_interp_accel_reset(cosmocalc_linear_corrfunc_acc);
      else
	cosmocalc_linear_corrfunc_acc = gsl_interp_accel_alloc();
    }

  gf = growth_function(a);

  if(r < CF_R_MIN)
    return 0.0;
  else if(r < CF_R_MAX)
    return gsl_spline_eval(cosmocalc_linear_corrfunc_spline,log(r),cosmocalc_linear_corrfunc_acc)*gf*gf;
  else
    return 0.0;
}


/**
 * @file spectralestimator.h
 * @brief Speech recognition front end.
 * @author John McDonough, Matthias Woelfel, Kenichi Kumatani
 */

#ifndef SPECTRALESTIMATOR_H
#define SPECTRALESTIMATOR_H

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_vector_complex.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>
#include <math.h>
#include <algorithm>

#include "matrix/gslmatrix.h"
#include "stream/stream.h"
#include "common/mlist.h"

#include <stdio.h>
#include <iostream>

/**
   @brief calculate the auto-correlation vector, r, with the time-discrete signal, x[n].
          Each component, r[k], represents the auto-correlation function of the input sample for lag, k.
          Accordingly, r[k] can be expressed as \cE{ x[n] x[n-k] }.
   @param const gsl_vector *samples[in] time-discrete signal which corresponds to x[n].
   @param gsl_vector *autoCorrelationVector[out] auto-correlation vector which corresponds to r.
   @note  the size of "autoCorrelationVector" should be "order+1" in order to calculate the LP model of the order, "order".
 */
void calcAutoCorrelationVector( const gsl_vector_float *samples, gsl_vector *autoCorrelationVector );

/**
   @brief calculate the linear prediction coefficients (LPC) with the Levinson-Durbin Algorithm.
   @param gsl_vector_float *autoCorrelationVector[in]
   @param gsl_vector_float *lpcs[out] lpcs[0,...,p-1] where p is the order of the model.
   @return prediction error
   @notice The estimated signal is defined as
           \hat{x}[n] = sum_{k=1}^p lpcs[k-1] * x[n-k].
           That is different from the definition in MATLAB lpc()
	   where it is \hat{x}(n) = 1.0 - sum_{k=1}^p lpcs(k+1) * x(n-k).
 */
float calcLPCswithLevinsonDurbin( gsl_vector_float *autoCorrelationVector, gsl_vector_float *lpcs );

// ----- definition for class `LPCSpectrumEstimator' -----
//

/**
   @class calculate LPCs with the Levinson-Durbin algorithm, and estimate the LPC envelope.
   @note  You can feed samples to this object with calcLPCs( const gsl_vector_float* samples ),
	  and the LPCs are then computed.
 */
class LPCSpectrumEstimator : public VectorFloatFeatureStream {
public:
  LPCSpectrumEstimator( const VectorFloatFeatureStreamPtr& samp, unsigned order, unsigned fftLen, const String& nm = "LPCSpectrumEstimator");
  ~LPCSpectrumEstimator();

  const gsl_vector_float* next(int frame_no = -5);
  void reset();

  /**
     @brief get the auto-correlation vector, r[0,...,order-1],
            where r[k] represents the auto-correlation function of the input sample for lag, k+1.
     @note you have to call calcLPCs() before you call this method.
   */
  const gsl_vector_float* getAutoCorrelationVector(){
    return (const gsl_vector_float*)_r;
  }
  /**
     @brief get the prediction error of the LP model.
     @note you have to call calcLPCs() before you call this method.
   */
  float getPredictionError(){
    return _e;
  }
  /**
     @brief get the LPCs.
     @note you have to call calcLPCs() before you call this method.
   */
  const gsl_vector_float* getLPCs(){
    return _lpcs; //_vector;
  }

  gsl_vector_complex* getFTLPCs(){
    return _ftlpcs;
  }

  const gsl_vector_float* calcLPCs( const gsl_vector_float* samples );
  void calcLPEnvelope();

private:
  void complexPackedArrayUnpack(const double* halfcomplex_coefficient, gsl_vector_complex* complex_coefficient) const;
private:
  static const double             _epsilon;
  VectorFloatFeatureStreamPtr        _samp;
  unsigned                          _order; /* the LP model of the order */
  unsigned                         _fftLen;
  unsigned                        _fftLen2;
  float                                 _e; /* prediction error */
  gsl_vector_float*                     _r; /* _order x 1 auto-correlation vector _r[1,...,p]; See [1] */
  gsl_vector*             _autoCorrelation; /* (_order+1) x 1 auto-correlation vector, where the first element corresponds to the power */
  gsl_vector_float*                  _lpcs; /* LPCs */
  double*                          _wslpcs; /* workspace for the LPCs */
  gsl_vector_complex*              _ftlpcs;
};

typedef Inherit<LPCSpectrumEstimator, VectorFloatFeatureStreamPtr> LPCSpectrumEstimatorPtr;

// ----- definition for class `CepstralSpectrumEstimator' -----
//
class CepstralSpectrumEstimator : public VectorFloatFeatureStream {
public:
  CepstralSpectrumEstimator(const VectorComplexFeatureStreamPtr& source, unsigned order = 14, unsigned fftLen = 256,
			    double logPadding = 1.0, const String& nm = "CepstralSpectrumEstimator");
  ~CepstralSpectrumEstimator();

  const gsl_vector_float* next(int frame_no = -5);
  void reset() { _source->reset();  VectorFloatFeatureStream::reset(); }

private:
  VectorComplexFeatureStreamPtr			_source;
  const unsigned				_order;
  const unsigned				_fftLen;
  const double					_logPadding;

  double*					_samples;
  gsl_vector_complex*				_spectrum;
  gsl_vector_complex*				_cepstrum;
};

typedef Inherit<CepstralSpectrumEstimator, VectorFloatFeatureStreamPtr> CepstralSpectrumEstimatorPtr;

// ----- definition for class `SEMNB' -----
//

/**
   @class
 */

class SEMNB : public Countable {
public:
  SEMNB( unsigned order, unsigned fftLen, const String& nm = "SEMNB");
  ~SEMNB();

  const gsl_vector*	calcDerivativeOfDeviation( LPCSpectrumEstimatorPtr &lpcSEPtr );
  virtual void		reset();
  const gsl_vector*	getLPEnvelope() const { return _lpEnvelope; }

private:
  void			_calcEigen();
  void			_calcLambda_bar();

  double		_dLambdak_dPm(unsigned k, unsigned m);
  void			_dLambdabar_dPm(unsigned m);
  void			_da_dPm(unsigned m);
  float			_dSigma_dP(const gsl_vector_float* lpcs, double epsilonp, unsigned m);

  double		_dSp_dan(const gsl_vector_float* lpcs, unsigned m, unsigned n);
  double		_dSp_dPm(const gsl_vector_float* lpcs, unsigned m);
  double		_depsilonp_dPm(const gsl_vector_float* lpcs, unsigned m);
  void			_calc_dr_dPm(unsigned m);
  double		_dPhat_dP(const gsl_vector_float* lpcs, double epsilonp, unsigned m);

  const unsigned				_order;
  const unsigned				_fftLen;
  const unsigned				_fftLen2;

  gsl_vector*					_derivative;
  gsl_matrix*					_R;
  gsl_matrix*					_Rcopy;
  gsl_eigen_symmv_workspace*			_workSpace;
  gsl_vector*					_evalues;
  gsl_matrix*					_evectors;
  gsl_vector*	                                _lpEnvelope;
  gsl_vector_complex*                           _ftlpcs;

  gsl_vector*					_r;
  gsl_vector*					_dr_dPm;
  gsl_vector*					_dak_dPm;
  gsl_matrix*					_Lambda_bar;
  gsl_matrix*					_dLambdamatrix;

  gsl_matrix*					_C;
  gsl_matrix*					_D;
};

typedef refcountable_ptr<SEMNB> SEMNBPtr;

#endif


/* randist/gumbel.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 James Theiler, 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 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/* The Type I Gumbel distribution has the form,

   p(x) dx = a b exp(-(b exp(-ax) + ax)) dx

   and the Type II Gumbel distribution has the form,

   p(x) dx = b a x^-(a+1) exp(-b x^-a)) dx

 */

double
gsl_ran_gumbel1 (const gsl_rng * r, const double a, const double b)
{
  double x = gsl_rng_uniform_pos (r);

  double z = (log(b) - log(-log(x))) / a;

  return z;
}

double
gsl_ran_gumbel1_pdf (const double x, const double a, const double b)
{
  double p = a * b *  exp (-(b * exp(-a * x) + a * x));
  return p;
}

double
gsl_ran_gumbel2 (const gsl_rng * r, const double a, const double b)
{
  double x = gsl_rng_uniform_pos (r);

  double z = pow(-b / log(x), 1/a);

  return z;
}

double
gsl_ran_gumbel2_pdf (const double x, const double a, const double b)
{
  if (x <= 0)
    {
      return 0 ;
    }
  else
    {
      double p = b * a *  pow(x,-(a+1)) * exp (-b * pow(x, -a));
      return p;
    }
}


//
// beta.h
//
// code using beta distribution
//
// copyright 2020 Peter Andrews
//

#ifndef PAA_BETA_H_
#define PAA_BETA_H_

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#include <cmath>
#include <mutex>
#include <utility>

#include "error.h"
#include "utility.h"

namespace paa {

class GSLRNG {
 public:
  explicit GSLRNG(const uint64_t seed) : gen{gsl_rng_alloc(gsl_rng_mt19937)} {
    if (!gen) throw Error("Problem creating gsl generator");
    gsl_rng_set(gen, seed);
  }
  ~GSLRNG() { gsl_rng_free(gen); }
  operator gsl_rng * () const { return gen; }
  GSLRNG(const GSLRNG &) = delete;
  GSLRNG & operator=(const GSLRNG &) = delete;
  void lock() { mutex.lock(); }
  void unlock() { mutex.unlock(); }

 private:
  gsl_rng * gen;
  std::mutex mutex{};
};

template<class Val>
class BetaT {
 public:
  using Params = std::pair<Val, Val>;
  BetaT(const Val alpha__, const Val beta__) :
      alpha_{alpha__}, beta_{beta__} {
        if (alpha_ <= 0) throw Error("Nonpositive alpha in Beta distribution");
        if (beta_ <= 0) throw Error("Nonpositive beta in Beta distribution");
      }
  explicit BetaT(const Params params) :
      BetaT{params.first, params.second} {}
  BetaT(const Val mean_, const Val stdev_, const bool) :
      alpha_{get_alpha(mean_, stdev_)},
      beta_{get_beta(mean_, stdev_)} {
        if (stdev_ >= max_stdev(mean_))
          throw Error("Maximum stdev for basic Beta distribution is")
              << max_stdev(mean_);
      }
  Val alpha() const { return alpha_; }
  Val beta() const { return beta_; }
  Val low() const { return 0.0; }
  Val high() const { return 1.0; }
  Val range() const { return 1.0; }
  Val middle() const { return 0.5; }
  static Val get_alpha(const Val mean_, const Val stdev_) {
    return mean_ * (mean_ * (1 - mean_) / sqr(stdev_) - 1);
  }
  static Val get_beta(const Val mean_, const Val stdev_) {
    return (1 - mean_) * (mean_ * (1 - mean_) / sqr(stdev_) - 1);
  }
  static Val max_stdev(const Val mean_) {
    return sqrt(mean_ * (1 - mean_));
  }
  Val mean() const { return alpha_ / (alpha_ + beta_); }
  Val stdev() const {
    return sqrt((alpha_ * beta_) /
                (sqr(alpha_ + beta_) * (alpha_ + beta_ + 1)));
  }
  Val operator()(const Val val) const {
    return gsl_ran_beta_pdf(val, alpha_, beta_);
  }
  Val operator()(gsl_rng * gen) const {
    return gsl_ran_beta(gen, alpha_, beta_);
  }

 private:
  Val alpha_;
  Val beta_;
};
using Beta = BetaT<double>;
using fBeta = BetaT<float>;

template<class Val>
class Beta4T {
 public:
  Beta4T(const Val alpha__, const Val beta__,
        const Val low__, const Val high__) :
      beta_dist{alpha__, beta__}, low_{low__}, high_{high__} {
        if (mean() < low())
          throw Error("Mean less than low in Beta distribution");
        if (mean() > high())
          throw Error("Mean greater than high in Beta distribution");
      }
  Beta4T(const Val mean_, const Val stdev_,
        const Val low__, const Val high__,
        const bool) :
      beta_dist{get_alpha_beta(mean_, stdev_, low__, high__)},
      low_{low__}, high_{high__} {}

  Val alpha() const { return beta_dist.alpha(); }
  Val beta() const { return beta_dist.beta(); }
  Val low() const { return low_; }
  Val high() const { return high_; }
  Val range() const { return (high_ - low_); }
  Val middle() const { return (high_ - low_) / 2; }
  Val mean() const { return low_ + scale * beta_dist.mean(); }
  Val stdev() const { return scale * beta_dist.stdev(); }
  static Val max_stdev(const Val mean_, const Val low__, const Val high__) {
    const Val sf{high__ - low__};
    const Val m{(mean_ - low__) / sf};
    return sf * BetaT<Val>::max_stdev(m);
  }
  Val operator()(const Val val) const {
    return static_cast<Val>(beta_dist((val - low_) / scale)) / scale;
  }
  Val operator()(gsl_rng * gen) const {
    return low_ + scale * static_cast<Val>(beta_dist(gen));
  }

 private:
  static typename BetaT<Val>::Params get_alpha_beta(
      const Val mean_, const Val stdev_,
      const Val low__, const Val high__) {
    const Val sf{high__ - low__};
    const Val m{(mean_ - low__) / sf};
    const Val sd{stdev_ / sf};
    return typename BetaT<Val>::Params{
      BetaT<Val>::get_alpha(m, sd), BetaT<Val>::get_beta(m, sd)};
  }

  BetaT<Val> beta_dist;
  Val low_;
  Val high_;
  Val scale{high_ - low_};
};
using Beta4 = Beta4T<double>;
using fBeta4 = Beta4T<float>;

}  // namespace paa

#endif  // PAA_BETA_H_


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_linalg.h>

typedef struct Vortex
{
    double x, y;
    double vorticity;

    int active;
} Vortex;

/* possibly should be replaced with one function? */
double Qfield_x(double x, double y)
{
    double r = x*x+y*y;
    return -y/(2*M_PI*fmax(r,1/1024/1024));
//    return -y/(2*M_PI*fmax(r,1/1024));
}

double Qfield_y(double x, double y)
{
    double r = x*x+y*y;
    return  x/(2*M_PI*fmax(r,1/1024/1024));
}

double Pfield_x(double x, double y)
{
    double r = hypot(x,y);
    if(r == 0)
        return 1.;
    return x/r*exp(-r);
}

double Pfield_y(double x, double y)
{
    double r = hypot(x,y);
    if(r == 0)
        return 1.;
    return y/r*exp(-r);
}

double Mfield(double x, double y)
{
    double r2= x*x+y*y;
    double r = sqrt(r2);
    if(x==0 && y==0)
        return 0.;
    return (r+1.)*exp(-r)/r2;
}

int VortexInBody(double x, double y)
{
    if(x*x+y*y<1)
        return 1;
    else
        return 0;
}

int main(int argc, char **argv)
{
    int NumberOfPanels = 12, MaxVortexes = 16500, Niterations = 200,      i,j,k,m,n, iterator;
    double nu = 1/13, tau = 0.005/*, eps = 0.002*/, rho = 1.;
    double vx_inf = 1., vy_inf = 0.;

    double  PanelNodes [NumberOfPanels][2],
            PanelMids  [NumberOfPanels][2],
            PanelNorms [NumberOfPanels][2],
            PanelTaus  [NumberOfPanels][2],
            PanelLength[NumberOfPanels];

    double DeployPoints[NumberOfPanels][2];

    Vortex InFlow[MaxVortexes], NextInFlow[MaxVortexes];

    int ActiveVortexesInFLow=0;

    if(1)/* cylinder profile forming & deploys */
    {

        for(i=0;i<NumberOfPanels;i++)
        {
            PanelNodes[i][0] = cos(2.*M_PI/NumberOfPanels*i);
            PanelNodes[i][1] = sin(2.*M_PI/NumberOfPanels*i);

            DeployPoints[i][0] = 1.05*PanelNodes[i][0];
            DeployPoints[i][1] = 1.05*PanelNodes[i][1];

            PanelNorms[i][0] = cos(2.*M_PI/NumberOfPanels*(i+0.5));
            PanelNorms[i][1] = sin(2.*M_PI/NumberOfPanels*(i+0.5));

        }

        for(i=0;i<NumberOfPanels-1;i++)
        {
            PanelMids[i][0] = (PanelNodes[i][0]+PanelNodes[i+1][0])/2;
            PanelMids[i][1] = (PanelNodes[i][1]+PanelNodes[i+1][1])/2;

            PanelTaus[i][0] = (PanelNodes[i+1][0]-PanelNodes[i][0]);
            PanelTaus[i][1] = (PanelNodes[i+1][1]-PanelNodes[i][1]);
            PanelLength[i]  = sqrt(PanelTaus[i][0]*PanelTaus[i][0]+
                                   PanelTaus[i][1]*PanelTaus[i][1]);
            PanelTaus[i][0] /= PanelLength[i];
            PanelTaus[i][1] /= PanelLength[i];
//            PanelNorms[i][0] = -PanelTaus[i][1];
//            PanelNorms[i][1] = PanelTaus[i][0];
        }
        PanelMids[NumberOfPanels-1][0] = (PanelNodes[NumberOfPanels-1][0]+PanelNodes[0][0])/2;
        PanelMids[NumberOfPanels-1][1] = (PanelNodes[NumberOfPanels-1][1]+PanelNodes[0][1])/2;
        PanelTaus[NumberOfPanels-1][0] = (PanelNodes[0][0]-PanelNodes[NumberOfPanels-1][0]);
        PanelTaus[NumberOfPanels-1][1] = (PanelNodes[0][1]-PanelNodes[NumberOfPanels-1][1]);
        PanelLength[NumberOfPanels-1]  = hypot(PanelTaus[NumberOfPanels-1][0],PanelTaus[NumberOfPanels-1][1]);
        PanelTaus[NumberOfPanels-1][0] /= PanelLength[NumberOfPanels-1];
        PanelTaus[NumberOfPanels-1][1] /= PanelLength[NumberOfPanels-1];
//        PanelNorms[Np-1][0] = -PanelTaus[Np-1][1];
//        PanelNorms[Np-1][1] = PanelTaus[Np-1][0];
    }

    // defining and completing Vortex Birth Matrix
    gsl_matrix *OriginalVortexGenerationMatrix = gsl_matrix_alloc(NumberOfPanels+1,NumberOfPanels+1);
    gsl_matrix *VortexGenerationMatrix = gsl_matrix_alloc(NumberOfPanels+1,NumberOfPanels+1);

    for(i=0; i<NumberOfPanels; i++)
    {
        for(j=0; j<NumberOfPanels; j++)
        {
            gsl_matrix_set(VortexGenerationMatrix,i,j,
            PanelNorms[i][0]*Qfield_x(PanelMids[i][0]-DeployPoints[j][0],
                                      PanelMids[i][1]-DeployPoints[j][1])
           +PanelNorms[i][1]*Qfield_y(PanelMids[i][0]-DeployPoints[j][0],
                                      PanelMids[i][1]-DeployPoints[j][1]));
        }
    }
    for(i=0;i<NumberOfPanels;i++)
    {
        gsl_matrix_set(VortexGenerationMatrix,NumberOfPanels,i,1);
        gsl_matrix_set(VortexGenerationMatrix,i,NumberOfPanels,1);
    }
    gsl_matrix_set(VortexGenerationMatrix,NumberOfPanels,NumberOfPanels,0);
    gsl_matrix_memcpy(OriginalVortexGenerationMatrix,VortexGenerationMatrix);

    // recieve initial aproximation for vortexes on surface
    gsl_vector *VelocityOnInfProjections = gsl_vector_alloc(NumberOfPanels+1); //Velocity On Infinity
    gsl_vector *VelocityProjections = gsl_vector_alloc(NumberOfPanels+1); //Velocity from active vortexes
//    gsl_vector *velocities_on_surface = gsl_vector_alloc(Np+1); //full velocity on surface = VOI + VFV
    gsl_vector *NewVorticities = gsl_vector_alloc(NumberOfPanels+1);

    for(i=0;i<NumberOfPanels;i++)
    {
        gsl_vector_set(VelocityOnInfProjections,i,-(PanelNorms[i][0]*vx_inf+PanelNorms[i][1]*vy_inf));
    }
    gsl_vector_set(VelocityOnInfProjections,NumberOfPanels,0.);

    gsl_permutation * p = gsl_permutation_alloc (NumberOfPanels+1);
    gsl_linalg_LU_decomp(VortexGenerationMatrix,p,&k);
    gsl_linalg_LU_solve (VortexGenerationMatrix,p,VelocityOnInfProjections,NewVorticities);

    FILE *forces;
    forces = fopen("forces.dat","w");

    for(iterator=0; iterator<Niterations; iterator++)
    {
        double f_x = 0., f_y = 0./*, M_z = 0.*/;

        //add generated vortexes to flow
        for(i=0;i<NumberOfPanels;i++)
        {
            InFlow[ActiveVortexesInFLow+i].active = 1;
            InFlow[ActiveVortexesInFLow+i].vorticity = gsl_vector_get(NewVorticities,i);
            InFlow[ActiveVortexesInFLow+i].x = DeployPoints[i][0];
            InFlow[ActiveVortexesInFLow+i].y = DeployPoints[i][1];
        }
        ActiveVortexesInFLow += NumberOfPanels;

        // time iteration
        printf("active vortexes %d\n",ActiveVortexesInFLow);

        for(i=0;i<ActiveVortexesInFLow;i++)
        {
            while(InFlow[i].active!=1)i++;
            double eps = sqrt(4/3)*PanelLength[0]/1000;
            double  I0 = 2*M_PI*eps*eps,
                    I1 = 0.,
                    I2_x = 0., I2_y = 0.,
                    I3_x = 0., I3_y = 0.;

            double u_x = vx_inf;
            double u_y = vy_inf;

            //Epsilon update
            if(iterator>5 && 0)
            {
                double sq1=1., sq2=1., sq3=1.;
                for(j=0;j<ActiveVortexesInFLow;j++)
                {
                    double temp = (InFlow[i].x-InFlow[j].x)*(InFlow[i].x-InFlow[j].x)
                                 +(InFlow[i].y-InFlow[j].y)*(InFlow[i].y-InFlow[j].y);
                    if(i!=j && temp < sq1
                            && InFlow[j].active==1
                            && InFlow[i].vorticity*InFlow[j].vorticity>0
                       )
                    {
                        sq1 = temp;
                        sq2 = sq1;
                        sq3 = sq2;
                    }
                }
                eps = sqrt( (sq1+sq2+sq3)/3 );
            }

            //Vortex-caused part of velocity
            for(j=0;j<ActiveVortexesInFLow;j++)
            {
                double rx = InFlow[i].x-InFlow[j].x;
                double ry = InFlow[i].y-InFlow[j].y;
                if( InFlow[j].active==1 &&  i!=j /*&& fabs(rx) > 1/1024.*0.25 && fabs(ry) > 1/1024.*0.25*/ )
                {
                    u_x += InFlow[j].vorticity*Qfield_x(rx,ry);
                    u_y += InFlow[j].vorticity*Qfield_y(rx,ry);
                }
            }

            //Viscous component of velocity
            for(j=0;j<ActiveVortexesInFLow;j++)
            {
                if( InFlow[j].active==1 )
                {
                    I2_x += InFlow[j].vorticity*Pfield_x((InFlow[i].x-InFlow[j].x)/eps,(InFlow[i].y-InFlow[j].y)/eps)/eps;
                    I2_y += InFlow[j].vorticity*Pfield_y((InFlow[i].x-InFlow[j].x)/eps,(InFlow[i].y-InFlow[j].y)/eps)/eps;
                    I1  += InFlow[j].vorticity*exp(-hypot((InFlow[i].x-InFlow[j].x),(InFlow[i].y-InFlow[j].y))/eps);
                }
            }
            for(j=0;j<NumberOfPanels;j++)
            {
                double Rr = hypot((InFlow[i].x-PanelMids[j][0]),(InFlow[i].y-PanelMids[j][1]));
                I3_x -= PanelNorms[j][0]*exp(-Rr/eps)*PanelLength[j];
                I3_y -= PanelNorms[j][1]*exp(-Rr/eps)*PanelLength[j];
                I0   -= ((InFlow[i].x-PanelMids[j][0])*PanelNorms[j][0]
                        +(InFlow[i].y-PanelMids[j][1])*PanelNorms[j][1])
                        *PanelLength[j]
                        *Mfield((InFlow[i].x-PanelMids[j][0])/eps,
                        (InFlow[i].y-PanelMids[j][1])/eps);

            }

            //        printf("%f %f\n",u_x,u_y);
            u_x += nu*(-I2_x/I1+I3_x/I0);
            u_y += nu*(-I2_y/I1+I3_y/I0);
            //        printf("%f %f\n",u_x,u_y);

            if(tau*u_x < 1000 && tau*u_y < 1000 )
            {
                NextInFlow[i].x = InFlow[i].x + tau*u_x;
                NextInFlow[i].y = InFlow[i].y + tau*u_y;

//                int minRmid = 0;
//контроль протекания
                double Rmid = sqrt((PanelMids[0][0]-NextInFlow[i].x)*(PanelMids[0][0]-NextInFlow[i].x)+
                                   (PanelMids[0][1]-NextInFlow[i].y)*(PanelMids[0][1]-NextInFlow[i].y));
                double projectionDistOnMidVector = (PanelMids[0][0]*NextInFlow[i].x+PanelMids[0][1]*NextInFlow[i].y)/
                                                   (PanelMids[0][0]*PanelMids[0][0]+PanelMids[0][1]*PanelMids[0][1]);

                for(m=1;m<NumberOfPanels;m++)
                {
                    double temp = sqrt((PanelMids[m][0]-NextInFlow[i].x)*(PanelMids[m][0]-NextInFlow[i].x)+
                                       (PanelMids[m][1]-NextInFlow[i].y)*(PanelMids[m][1]-NextInFlow[i].y));
                    if(fmin(Rmid,temp)<Rmid)
                    {
                        Rmid = temp;
//                        minRmid = m;
                        projectionDistOnMidVector = (PanelMids[m][0]*NextInFlow[i].x+PanelMids[m][1]*NextInFlow[i].y)/
                                                    (PanelMids[m][0]*PanelMids[m][0]+PanelMids[m][1]*PanelMids[m][1]);
                    }
                }

//                if(!VortexInBody(NextInFlow[i].x,NextInFlow[i].y))
                if(projectionDistOnMidVector > 1.)
                {
                    NextInFlow[i].active = InFlow[i].active;
                    NextInFlow[i].vorticity = InFlow[i].vorticity;
                }
                else
                {
                    NextInFlow[i].active = 3;
                    NextInFlow[i].vorticity = InFlow[i].vorticity;
                }
            }
            else
            {
                NextInFlow[i].x = InFlow[i].x;
                NextInFlow[i].y = InFlow[i].y;
                NextInFlow[i].active = 2;
                NextInFlow[i].vorticity = InFlow[i].vorticity;
            }
//            if(NextInFlow[i].vorticity<0.01)
        }

        for(m = 0; m < NumberOfPanels; m++)
        {
            f_x += -rho*(gsl_vector_get(NewVorticities,m)/tau* PanelMids[m][1]);
            f_y +=  rho*(gsl_vector_get(NewVorticities,m)/tau* PanelMids[m][0]);
        }

        fprintf(forces,"%f %f %f\n",iterator*tau, f_x, f_y);

        //updating coords
        for(i=0;i<ActiveVortexesInFLow;i++)
        {
            InFlow[i] = NextInFlow[i];
        }

        //definition of speed from vortexes after iteration
        double VelocityProjInControlPoint[NumberOfPanels];
        for(j=0; j<NumberOfPanels; j++)
            VelocityProjInControlPoint[j] = PanelNorms[j][0]*vx_inf + PanelNorms[j][1]*vy_inf;

        for(i=0; i<ActiveVortexesInFLow; i++)
        {
            if(InFlow[i].active==1)
            {
                for(j=0; j<NumberOfPanels; j++)
                {
                    VelocityProjInControlPoint[j] +=
                            (PanelNorms[j][0]*Qfield_x((PanelMids[j][0]-InFlow[i].x),
                                                       (PanelMids[j][1]-InFlow[i].y))
                            +PanelNorms[j][1]*Qfield_y((PanelMids[j][0]-InFlow[i].x),
                                                       (PanelMids[j][1]-InFlow[i].y)))
                            *InFlow[i].vorticity;
                }
            }
        }

        for(j=0; j<NumberOfPanels; j++)
            gsl_vector_set(VelocityProjections,j,-VelocityProjInControlPoint[j]);
        gsl_vector_set(VelocityProjections,NumberOfPanels,0);

        gsl_matrix_memcpy(VortexGenerationMatrix, OriginalVortexGenerationMatrix);

        gsl_linalg_LU_decomp(VortexGenerationMatrix,p,&k);
        gsl_linalg_LU_solve (VortexGenerationMatrix,p,VelocityProjections,NewVorticities);

        if(iterator%10==0)
        {
            /*
            FILE *VortexesOut, *BodyOut, *averagedOut;
            VortexesOut = fopen("MVE_flow.dat","w");
            BodyOut =     fopen("MVE_body.dat","w");
            averagedOut = fopen("MVE_aver.dat","w");

            for(i=0;i<ActiveVortexesInFLow;i++)
            {
                if(InFlow[i].active==1)
                    fprintf(VortexesOut,"%f %f %f\n",InFlow[i].x,InFlow[i].y,InFlow[i].vorticity);
            }

            for(i=0; i<Np; i++)
                fprintf(BodyOut,"%f %f\n", DeployPoints[i][0], DeployPoints[i][1]);

            double averagedVorticity[101][101];

            for(i=0;i<ActiveVortexesInFLow;i++)
            {
                if(fabs(InFlow[i].x)<=10 && fabs(InFlow[i].y)<=10 && InFlow[i].active == 1)
                {
                    averagedVorticity[(int)(50+(InFlow[i].x)/0.2)][(int)(50+(InFlow[i].y)/0.2)] += InFlow[i].vorticity*0.04;
                }
            }

            for(i=0;i<101;i++)
            {
                for(j=0;j<101;j++)
                    fprintf(averagedOut,"%f %f %f\n", -10+i*0.2, -10+j*0.02, averagedVorticity[i][j]);

            }

            fclose(averagedOut);
            fclose(VortexesOut);
            fclose(BodyOut);
            int temp;
            system("screen -S gnuplot -X stuff \"replot^M\" ");
            scanf("%d",&temp);
            if(ActiveVortexesInFLow<16000)
                Niterations +=temp;
            else
                return 0;
//            system("screen -S gnuplot -X stuff \"plot \'MVE_flow.dat\' using 1:2, \'MVE_body.dat\' w line^M\" ");
*/
            char FileName[32];
            sprintf(FileName,"VelocityFiled%6.6d.dat",iterator);
            FILE *VelocityField;
            VelocityField = fopen(FileName,"w");
            fprintf(VelocityField,"TITLE = \"Flow Model\"\n");
            fprintf(VelocityField,"VARIABLES = \"x\", \"y\", \"vx\", \"vy\"\n");
            fprintf(VelocityField,"ZONE T=\"Frame 0\", I=%d, J=%d\n", 200, 100);
            for(i=0;i<200;i++)
            {
                for(j=0;j<100;j++)
                {
                    double vx=vx_inf, vy=vy_inf;
                    for(n=0;n<ActiveVortexesInFLow;n++)
                    {
                        if(InFlow[n].active==1)
                        {
                            vx += InFlow[n].vorticity*Qfield_x((0.04*i-2)-InFlow[n].x,
                                                               (0.04*j-2)-InFlow[n].y);
                            vy += InFlow[n].vorticity*Qfield_y((0.04*i-2)-InFlow[n].x,
                                                               (0.04*j-2)-InFlow[n].y);
                        }
                    }
                    fprintf(VelocityField,"%f %f %f %f\n",0.04*i-2, 0.04*j-2, vx, vy);
                }
            }
        }
    }
/*
    //the most interesting part: output of results
    FILE *VortexesOut, *BodyOut, *averagedOut;
    VortexesOut = fopen("MVE_flow.dat","w");
    BodyOut =     fopen("MVE_body.dat","w");
    averagedOut = fopen("MVE_aver.dat","w");

    for(i=0;i<ActiveVortexesInFLow;i++)
    {
        if(InFlow[i].active==1)
            fprintf(VortexesOut,"%f %f %f\n",InFlow[i].x,InFlow[i].y,InFlow[i].vorticity);
    }

    for(i=0; i<Np; i++)
        fprintf(BodyOut,"%f %f\n", DeployPoints[i][0], DeployPoints[i][1]);

    double averagedVorticity[101][101];

    for(i=0;i<ActiveVortexesInFLow;i++)
    {
        if(fabs(InFlow[i].x)<=10 && fabs(InFlow[i].y)<=10 && InFlow[i].active == 1)
        {
            averagedVorticity[(int)(50+(InFlow[i].x)/0.2)][(int)(50+(InFlow[i].y)/0.2)] += InFlow[i].vorticity*0.04;
        }
    }

    for(i=0;i<101;i++)
    {
        for(j=0;j<101;j++)
            fprintf(averagedOut,"%f %f %f\n", -10+i*0.2, -10+j*0.02, averagedVorticity[i][j]);

    }

    fclose(averagedOut);
    fclose(VortexesOut);
    fclose(BodyOut);
    */
    return 0;
}


/*  @file mvn.c
 *  @title multivariate normal random variables
 *  @author Carl Boettiger, <cboettig@gmail.com>
 *
 *  Based on the R function rmvnorm, from the mvtnorm package
 *  by Friedrich Leisch and Fabian Scheipl, implemented
 *  using the GSL libraries
 */

#include <gsl/gsl_math.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_eigen.h>

int mvn(gsl_rng * rng, const gsl_vector * mean, gsl_matrix * covar, gsl_vector * ANS)
{
	int i;
	size_t n = mean->size;

	/* Calculate eigenvalues and eigenvectors of covar matrix */
	gsl_vector *eval = gsl_vector_alloc (n);
	gsl_matrix *evec = gsl_matrix_alloc (n, n);
	gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (n);
	gsl_eigen_symmv (covar, eval, evec, w);
	gsl_eigen_symmv_free (w);
//	gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_DESC);

	/* Setup for: evec * matrix(diag(eval)) * transpose(evec)  */
	gsl_matrix *eval_mx = gsl_matrix_calloc (n, n);
	gsl_matrix * x_M = gsl_matrix_alloc (n,n);
	gsl_matrix * x_M_x = gsl_matrix_alloc (n,n);

	gsl_vector_view diagonal = gsl_matrix_diagonal(eval_mx);
	gsl_vector_memcpy(&diagonal.vector, eval);
	for(i=0;i<n;i++)
	{
		gsl_vector_set( &diagonal.vector,
						i,
						sqrt( gsl_vector_get(&diagonal.vector, i) )
					  );
	}

	/* evec * matrix(diag(eval)) * transpose(evec)  */
//	gsl_blas_dsymm (CblasLeft, CblasUpper,
//					1.0, evec, eval_mx, 0.0, x_M);

	gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
					1.0, evec, eval_mx, 0.0, x_M);
	gsl_blas_dgemm (CblasNoTrans, CblasTrans,
					1.0, x_M, evec, 0.0, x_M_x);

	gsl_matrix_free(x_M);
	gsl_matrix_free(eval_mx);
	gsl_matrix_free(evec);
	gsl_vector_free(eval);

	gsl_vector * rnorms = gsl_vector_alloc(n);
	for(i=0;i<n;i++)
	{
		gsl_vector_set
			( rnorms, i,
			  gsl_ran_gaussian_ziggurat(rng, 1)
			);
	}

	gsl_blas_dgemv( CblasTrans, 1.0, x_M_x, rnorms, 0, ANS);
	gsl_vector_add(ANS, mean);
	gsl_matrix_free(x_M_x);
  gsl_vector_free(rnorms);

	return 0;
	/* answer provided through pass by reference */
}

/*
int main()
{
	const int n_tips = 2;
	gsl_rng * rng = gsl_rng_alloc(gsl_rng_default);
	gsl_vector * simdata = gsl_vector_alloc(n_tips);
	gsl_matrix * I = gsl_matrix_alloc(n_tips, n_tips);
	gsl_matrix_set_identity(I);
	gsl_vector * A = gsl_vector_calloc(n_tips);
	gsl_vector_set_all(A, 5.);
	mvn(rng, A, I, simdata);
	gsl_vector_fprintf(stdout, A, "%g");
	return 0;
}
*/

/*
#define N 3
int main(void)
{

	double mean[] = {1,2,5};
	double sigma[] = {1, 0, 0,
					  0, 1, .9,
					  0, .9, 1};
	double out[N] = {0,0,0};

	gsl_vector_view mean_view = gsl_vector_view_array(mean, N);
	gsl_matrix_view sigma_view = gsl_matrix_view_array(sigma, N,N);
	gsl_vector_view out_view = gsl_vector_view_array(out, N);

	gsl_rng * rng = gsl_rng_alloc(gsl_rng_default);

	int i;
	for(i=0;i<10000;i++) {
		mvn(rng, &mean_view.vector, &sigma_view.matrix, &out_view.vector);
		printf("%g %g %g\n", out[0], out[1], out[2]);
	}
	return 0;
}
*/


/**
 *
 * @file core_zpltmg_circul.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.8.0
 * @author Mathieu Faverge
 * @date 2010-11-15
 * @precisions normal z -> c d s
 *
 **/
#include <lapacke.h>
#include <math.h>
#include "common.h"

/***************************************************************************//**
 *
 * @ingroup dplasma_cores_complex64
 *
 *  CORE_zpltmg_circul is a kernel used in circulant matrix generation
 *
 *  See http://www.mathworks.fr/fr/help/matlab/ref/gallery.html#f84-999880
 *
 *  Circulant matrix
 *
 *  A circulant matrix has the property that each row is obtained from the
 *  previous one by cyclically permuting the entries one step forward. It is a
 *  special Toeplitz matrix in which the diagonals "wrap around."
 *
 *  The eigensystem of C (n-by-n) is known explicitly: If t is an nth root of
 *  unity, then the inner product of v and w = [1 t t2 ... t(n – 1)] is an
 *  eigenvalue of C and w(n:-1:1) is an eigenvector, where v is the first column of
 *  C.
 *
 *
 *******************************************************************************
 *
 * @param[in] M
 *         The number of rows of the tile A to initialize. M >= 2.
 *
 * @param[in] N
 *         The number of columns of the tile A to initialize. N >= 0.
 *
 * @param[out] A
 *         On entry, the M-by-N tile to be initialized.
 *
 * @param[in] LDA
 *         The leading dimension of the tile A. LDA >= max(1,M).
 *
 * @param[in] gM
 *         The global number of rows of the full matrix, A is belonging to. gM >= (m0+gM).
 *
 * @param[in] m0
 *         The index of the first row of tile A in the full matrix. m0 >= 0.
 *
 * @param[in] n0
 *         The index of the first column of tile A in the full matrix. n0 >= 0.
 *
 * @param[in] V
 *          Workspace of size gM, that contains the first clumn of the full matrix
 *
 *******************************************************************************
 *
 * @return
 *         \retval PLASMA_SUCCESS successful exit
 *         \retval <0 if INFO = -k, the k-th argument had an illegal value
 *
 ******************************************************************************/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_zpltmg_circul = PCORE_zpltmg_circul
#define CORE_zpltmg_circul PCORE_zpltmg_circul
#endif
int CORE_zpltmg_circul( int M, int N, PLASMA_Complex64_t *A, int LDA,
                        int gM, int m0, int n0,
                        const PLASMA_Complex64_t *V )
{
    int i, j, ii, jj;

    /* Check input arguments */
    if (M < 0) {
        coreblas_error(1, "Illegal value of M");
        return -1;
    }
    if (N < 0) {
        coreblas_error(2, "Illegal value of N");
        return -2;
    }
    if ((LDA < max(1,M)) && (M > 0)) {
        coreblas_error(4, "Illegal value of LDA");
        return -4;
    }
    if (m0 < 0) {
        coreblas_error(6, "Illegal value of m0");
        return -6;
    }
    if (n0 < 0) {
        coreblas_error(7, "Illegal value of n0");
        return -7;
    }
    if (gM < m0+M) {
        coreblas_error(5, "Illegal value of gM");
        return -5;
    }

    for (j=0, jj=n0; j<N; j++, jj++) {
        for (i=0, ii=m0; i<M; i++, ii++) {
            A[LDA*j + i] = V[ (jj-ii+gM)%gM ];
        }
    }

    return PLASMA_SUCCESS;
}


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

/*
   Random Discrete Events

   Given K discrete events with different probabilities P[k]
   produce a value k consistent with its probability.

   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
*/

/*
 * Based on: Alastair J Walker, An efficient method for generating
 * discrete random variables with general distributions, ACM Trans
 * Math Soft 3, 253-256 (1977).  See also: D. E. Knuth, The Art of
 * Computer Programming, Volume 2 (Seminumerical algorithms), 3rd
 * edition, Addison-Wesley (1997), p120.

 * Walker's algorithm does some preprocessing, and provides two
 * arrays: floating point F[k] and integer A[k].  A value k is chosen
 * from 0..K-1 with equal likelihood, and then a uniform random number
 * u is compared to F[k].  If it is less than F[k], then k is
 * returned.  Otherwise, A[k] is returned.

 * Walker's original paper describes an O(K^2) algorithm for setting
 * up the F and A arrays.  I found this disturbing since I wanted to
 * use very large values of K.  I'm sure I'm not the first to realize
 * this, but in fact the preprocessing can be done in O(K) steps.

 * A figure of merit for the preprocessing is the average value for
 * the F[k]'s (that is, SUM_k F[k]/K); this corresponds to the
 * probability that k is returned, instead of A[k], thereby saving a
 * redirection.  Walker's O(K^2) preprocessing will generally improve
 * that figure of merit, compared to my cheaper O(K) method; from some
 * experiments with a perl script, I get values of around 0.6 for my
 * method and just under 0.75 for Walker's.  Knuth has pointed out
 * that finding _the_ optimum lookup tables, which maximize the
 * average F[k], is a combinatorially difficult problem.  But any
 * valid preprocessing will still provide O(1) time for the call to
 * gsl_ran_discrete().  I find that if I artificially set F[k]=1 --
 * ie, better than optimum! -- I get a speedup of maybe 20%, so that's
 * the maximum I could expect from the most expensive preprocessing.
 * Folding in the difference of 0.6 vs 0.75, I'd estimate that the
 * speedup would be less than 10%.

 * I've not implemented it here, but one compromise is to sort the
 * probabilities once, and then work from the two ends inward.  This
 * requires O(K log K), still lots cheaper than O(K^2), and from my
 * experiments with the perl script, the figure of merit is within
 * about 0.01 for K up to 1000, and no sign of diverging (in fact,
 * they seemed to be converging, but it's hard to say with just a
 * handful of runs).

 * The O(K) algorithm goes through all the p_k's and decides if they
 * are "smalls" or "bigs" according to whether they are less than or
 * greater than the mean value 1/K.  The indices to the smalls and the
 * bigs are put in separate stacks, and then we work through the
 * stacks together.  For each small, we pair it up with the next big
 * in the stack (Walker always wanted to pair up the smallest small
 * with the biggest big).  The small "borrows" from the big just
 * enough to bring the small up to mean.  This reduces the size of the
 * big, so the (smaller) big is compared again to the mean, and if it
 * is smaller, it gets "popped" from the big stack and "pushed" to the
 * small stack.  Otherwise, it stays put.  Since every time we pop a
 * small, we are able to deal with it right then and there, and we
 * never have to pop more than K smalls, then the algorithm is O(K).

 * This implementation sets up two separate stacks, and allocates K
 * elements between them.  Since neither stack ever grows, we do an
 * extra O(K) pass through the data to determine how many smalls and
 * bigs there are to begin with and allocate appropriately.  In all
 * there are 2*K*sizeof(double) transient bytes of memory that are
 * used than returned, and K*(sizeof(int)+sizeof(double)) bytes used
 * in the lookup table.

 * Walker spoke of using two random numbers (an integer 0..K-1, and a
 * floating point u in [0,1]), but Knuth points out that one can just
 * use the integer and fractional parts of K*u where u is in [0,1].
 * In fact, Knuth further notes that taking F'[k]=(k+F[k])/K, one can
 * directly compare u to F'[k] without having to explicitly set
 * u=K*u-int(K*u).

 * Usage:

 * Starting with an array of probabilities P, initialize and do
 * preprocessing with a call to:

 *    gsl_rng *r;
 *    gsl_ran_discrete_t *f;
 *    f = gsl_ran_discrete_preproc(K,P);

 * Then, whenever a random index 0..K-1 is desired, use

 *    k = gsl_ran_discrete(r,f);

 * Note that several different randevent struct's can be
 * simultaneously active.

 * Aside: A very clever alternative approach is described in
 * Abramowitz and Stegun, p 950, citing: Marsaglia, Random variables
 * and computers, Proc Third Prague Conference in Probability Theory,
 * 1962.  A more accesible reference is: G. Marsaglia, Generating
 * discrete random numbers in a computer, Comm ACM 6, 37-38 (1963).
 * If anybody is interested, I (jt) have also coded up this version as
 * part of another software package.  However, I've done some
 * comparisons, and the Walker method is both faster and more stingy
 * with memory.  So, in the end I decided not to include it with the
 * GSL package.

 * Written 26 Jan 1999, James Theiler, jt@lanl.gov
 * Adapted to GSL, 30 Jan 1999, jt

 */

#include <config.h>
#include <stdio.h>              /* used for NULL, also fprintf(stderr,...) */
#include <stdlib.h>             /* used for malloc's */
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#define DEBUG 0
#define KNUTH_CONVENTION 1      /* Saves a few steps of arithmetic
                                 * in the call to gsl_ran_discrete()
                                 */

/*** Begin Stack (this code is used just in this file) ***/

/* Stack code converted to use unsigned indices (i.e. s->i == 0 now
   means an empty stack, instead of -1), for consistency and to give a
   bigger allowable range. BJG */

typedef struct {
    size_t N;                      /* max number of elts on stack */
    size_t *v;                     /* array of values on the stack */
    size_t i;                      /* index of top of stack */
} gsl_stack_t;

static gsl_stack_t *
new_stack(size_t N) {
    gsl_stack_t *s;
    s = (gsl_stack_t *)malloc(sizeof(gsl_stack_t));
    s->N = N;
    s->i = 0;                  /* indicates stack is empty */
    s->v = (size_t *)malloc(sizeof(size_t)*N);
    return s;
}

static int
push_stack(gsl_stack_t *s, size_t v)
{
    if ((s->i) >= (s->N)) {
      return -1; /* stack overflow (shouldn't happen) */
    }
    (s->v)[s->i] = v;
    s->i += 1;
    return 0;
}

static size_t pop_stack(gsl_stack_t *s)
{
    if ((s->i) == 0) {
      GSL_ERROR ("internal error - stack exhausted", GSL_ESANITY);
    }
    s->i -= 1;
    return ((s->v)[s->i]);
}

static inline size_t size_stack(const gsl_stack_t *s)
{
    return s->i;
}

static void free_stack(gsl_stack_t *s)
{
    free((char *)(s->v));
    free((char *)s);
}

/*** End Stack ***/

/*** Begin Walker's Algorithm ***/

gsl_ran_discrete_t *
gsl_ran_discrete_preproc(size_t Kevents, const double *ProbArray)
{
    size_t k,b,s;
    gsl_ran_discrete_t *g;
    size_t nBigs, nSmalls;
    gsl_stack_t *Bigs;
    gsl_stack_t *Smalls;
    double *E;
    double pTotal = 0.0, mean, d;

    if (Kevents < 1) {
      /* Could probably treat Kevents=1 as a special case */

      GSL_ERROR_VAL ("number of events must be a positive integer",
                        GSL_EINVAL, 0);
    }

    /* Make sure elements of ProbArray[] are positive.
     * Won't enforce that sum is unity; instead will just normalize
     */

    for (k=0; k<Kevents; ++k) {
        if (ProbArray[k] < 0) {
          GSL_ERROR_VAL ("probabilities must be non-negative",
                            GSL_EINVAL, 0) ;
        }
        pTotal += ProbArray[k];
    }

    /* Begin setting up the main "object" (just a struct, no steroids) */
    g = (gsl_ran_discrete_t *)malloc(sizeof(gsl_ran_discrete_t));
    g->K = Kevents;
    g->F = (double *)malloc(sizeof(double)*Kevents);
    g->A = (size_t *)malloc(sizeof(size_t)*Kevents);

    E = (double *)malloc(sizeof(double)*Kevents);

    if (E==NULL) {
      GSL_ERROR_VAL ("Cannot allocate memory for randevent", GSL_ENOMEM, 0);
    }

    for (k=0; k<Kevents; ++k) {
        E[k] = ProbArray[k]/pTotal;
    }

    /* Now create the Bigs and the Smalls */
    mean = 1.0/Kevents;
    nSmalls=nBigs=0;
    {
      /* Temporarily use which[k] = g->A[k] to indicate small or large */
      size_t * const which = g->A;

      for (k=0; k<Kevents; ++k) {
        if (E[k] < mean) {
          ++nSmalls; which[k] = 0;
        } else {
          ++nBigs; which[k] = 1;
        }
      }

      Bigs   = new_stack(nBigs);
      Smalls = new_stack(nSmalls);
      for (k=0; k<Kevents; ++k) {
        gsl_stack_t * Dest = which[k] ? Bigs : Smalls;
        int status = push_stack(Dest,k);
        if (status)
          GSL_ERROR_VAL ("failed to build stacks", GSL_EFAILED, 0);
      }
    }

    /* Now work through the smalls */
    while (size_stack(Smalls) > 0) {
        s = pop_stack(Smalls);
        if (size_stack(Bigs) == 0) {
            (g->A)[s]=s;
            (g->F)[s]=1.0;
            continue;
        }
        b = pop_stack(Bigs);
        (g->A)[s]=b;
        (g->F)[s]=Kevents*E[s];
#if DEBUG
        fprintf(stderr,"s=%2d, A=%2d, F=%.4f\n",s,(g->A)[s],(g->F)[s]);
#endif
        d = mean - E[s];
        E[s] += d;              /* now E[s] == mean */
        E[b] -= d;
        if (E[b] < mean) {
            push_stack(Smalls,b); /* no longer big, join ranks of the small */
        }
        else if (E[b] > mean) {
            push_stack(Bigs,b); /* still big, put it back where you found it */
        }
        else {
            /* E[b]==mean implies it is finished too */
            (g->A)[b]=b;
            (g->F)[b]=1.0;
        }
    }
    while (size_stack(Bigs) > 0) {
        b = pop_stack(Bigs);
        (g->A)[b]=b;
        (g->F)[b]=1.0;
    }
    /* Stacks have been emptied, and A and F have been filled */

    if ( size_stack(Smalls) != 0) {
      GSL_ERROR_VAL ("Smalls stack has not been emptied",
                     GSL_ESANITY, 0 );
    }

#if 0
    /* if 1, then artificially set all F[k]'s to unity.  This will
     * give wrong answers, but you'll get them faster.  But, not
     * that much faster (I get maybe 20%); that's an upper bound
     * on what the optimal preprocessing would give.
     */
    for (k=0; k<Kevents; ++k) {
        (g->F)[k] = 1.0;
    }
#endif

#if KNUTH_CONVENTION
    /* For convenience, set F'[k]=(k+F[k])/K */
    /* This saves some arithmetic in gsl_ran_discrete(); I find that
     * it doesn't actually make much difference.
     */
    for (k=0; k<Kevents; ++k) {
        (g->F)[k] += k;
        (g->F)[k] /= Kevents;
    }
#endif

    free_stack(Bigs);
    free_stack(Smalls);
    free((char *)E);

    return g;
}

size_t
gsl_ran_discrete(const gsl_rng *r, const gsl_ran_discrete_t *g)
{
    size_t c=0;
    double u,f;
    u = gsl_rng_uniform(r);
#if KNUTH_CONVENTION
    c = (u*(g->K));
#else
    u *= g->K;
    c = u;
    u -= c;
#endif
    f = (g->F)[c];
    /* fprintf(stderr,"c,f,u: %d %.4f %f\n",c,f,u); */
    if (f == 1.0) return c;

    if (u < f) {
        return c;
    }
    else {
        return (g->A)[c];
    }
}

void gsl_ran_discrete_free(gsl_ran_discrete_t *g)
{
    RETURN_IF_NULL (g);
    free((char *)(g->A));
    free((char *)(g->F));
    free((char *)g);
}

double
gsl_ran_discrete_pdf(size_t k, const gsl_ran_discrete_t *g)
{
    size_t i,K;
    double f,p=0;
    K= g->K;
    if (k>K) return 0;
    for (i=0; i<K; ++i) {
        f = (g->F)[i];
#if KNUTH_CONVENTION
        f = K*f-i;
#endif
        if (i==k) {
            p += f;
        } else if (k == (g->A)[i]) {
            p += 1.0 - f;
        }
    }
    return p/K;
}


#include <stdio.h>
#include <stdlib.h>
#include "cs.h"
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include "adkCSparse.h"
#include "adkGSL.h"

//gslMatrixToCSsparse -- returns new cs_sparse object. compressed
struct cs_di_sparse *gslMatrixToCSsparse(gsl_matrix *gm, int numNonZero){
	int i, j;
	struct cs_di_sparse *triplet, *compmat;
	double tmp;

	//now allocate new cs_sparse obj
	triplet = cs_spalloc(gm->size1, gm->size2, numNonZero, 1, 1);
	for(i=0;i<gm->size1;i++){
		for(j=0;j<gm->size2;j++){
			tmp = gsl_matrix_get(gm,i,j);
			if(tmp != 0.0) cs_entry(triplet,i,j,tmp);
		}
	}
	compmat = cs_compress(triplet);
	cs_spfree(triplet);
	return(compmat);
}

//gslMatrixToCSsparse -- returns new cs_sparse object. compressed
struct cs_di_sparse *arraysToCSsparse(int rank, int numNonZero, int *dim1, int *dim2,double *x){
	int i;
	struct cs_di_sparse *triplet, *compmat;

	//now allocate new cs_sparse obj
	triplet = cs_spalloc(rank, rank, numNonZero, 1, 1);
	for(i=0;i<numNonZero;i++) cs_entry(triplet,dim1[i],dim2[i],x[i]);
	compmat = cs_compress(triplet);
	cs_spfree(triplet);
	return(compmat);
}

//cs_max -- returns the maximum value of a cs_sparse object
double cs_max(struct cs_di_sparse *mat){
	int j,p;
	double max=0.0;
	double val;

	if (mat->nz < 0){
		for (j = 0 ; j < mat->n ; j++){
			for(p = mat->p[j] ; p < mat->p[j+1] ; p++){
				val = mat->x[p];
				if(val >max){
					max=val;
				}
			}
		}
	}
	else{ //below is untested
		for(j=0;j<mat->nzmax;j++){
			if(mat->x[j] > max){
				max = mat->x[j];
			}
		}
	}
	return(max);
}

//cs_scale -- scales each value by some constant
void cs_scale(struct cs_di_sparse *mat, double c){
	int i;
	for(i=0;i<mat->nzmax;i++){
		mat->x[i] *= c;
	}
}

//cs_scale -- scales each value by some constant
void cs_print_adk(struct cs_di_sparse *mat){
	int i,j,col,row,p;
	double val;
	gsl_matrix *gm;
	gm = gsl_matrix_alloc(mat->m,mat->n);
	gsl_matrix_set_zero(gm);

	if (mat->nz < 0){
		for (j = 0 ; j < mat->n ; j++){
			col=j;
			for(p = mat->p[j] ; p < mat->p[j+1] ; p++){
				row = mat->i [p];
				val = mat->x[p];
				gsl_matrix_set(gm,row,col,val);
			}
		}
	}
	else{

		for(i=0;i<mat->nz;i++){
			gsl_matrix_set(gm,mat->p[i],mat->i[i],mat->x[i]);
		}
	}
	gsl_matrix_prettyPrint(gm);
	gsl_matrix_free(gm);

}