/*
CUDA speed test without the memory overhead

gcc-mp-4.8 -O3 cudadgemmtest.c common.c -o cudadgemmtest -I../../../../netlib/CBLAS -I/usr/local/cuda/include/ -L/usr/local/cuda/lib -lcublas
export DYLD_LIBRARY_PATH=/usr/local/cuda/lib
./cudadgemmtest  > ../../../results/mac_os_x-x86_64-dgemm-cuda_nooh.csv

*/

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <cblas.h>
#include "common.h"
#include <cublas.h>

// does AB == C ? If not, complain on stderr
void test(int m, double* a, double *b, double *c) {
	int i, j, k, exact = 0, wrong = 0;
	double diff;
	double* d = calloc(m * m, sizeof(double));
	for (i = 0 ; i < m ; i++) {
		for (j = 0 ; j < m ; j++) {
			for (k = 0 ; k < m ; k++) {
				d[i + j * m] += a[i + k * m] * b[j * m + k];
			}
		}
	}
	for (i = 0 ; i < m ; i++) {
		for (j = 0 ; j < m ; j++) {
			diff = c[i * m + j] - d[i * m + j];
			if (diff != 0.0) {
				exact++;
			}
			if (abs(diff) > 0.000001) {
				wrong++;
			}
		}
	}
	free(d);
	if (wrong > 0) {
		fprintf(stderr, "not exact = %d, wrong = %d\n", exact, wrong);
	}
}

void checkStatus(char* message, cublasStatus status) {
    if (status != CUBLAS_STATUS_SUCCESS) {
    	fprintf (stderr, "!!!! %s fail %d\n", message, status);
    	exit(EXIT_FAILURE);
    }
}

long benchmark(int size) {
    int m = sqrt(size);
	long requestStart, requestEnd;

    double* a = random_array(m * m);
    double* b = random_array(m * m);
    double* c = calloc(m * m, sizeof(double));

	double *cuA, *cuB, *cuC;
	cublasStatus status;

	status = cublasAlloc(m * m, sizeof(double),(void**)&cuA);
	checkStatus("A", status);
	status = cublasAlloc(m * m, sizeof(double),(void**)&cuB);
	checkStatus("B", status);
	status = cublasAlloc(m * m, sizeof(double),(void**)&cuC);
	checkStatus("C", status);

	status = cublasSetMatrix(m, m, sizeof(double), a, m, cuA, m);
	checkStatus("setA", status);

	status = cublasSetMatrix(m, m, sizeof(double), b, m, cuB, m);
	checkStatus("setB", status);

	requestStart = currentTimeNanos();

	cublasDgemm('N', 'N', m, m, m, 1, cuA, m, cuB, m, 0, cuC, m);

	requestEnd = currentTimeNanos();

	status = cublasGetMatrix(m, m, sizeof(double), cuC, m, c, m);
	checkStatus("setB", status);

	status = cublasFree(cuA);
	checkStatus("freeA", status);
	status = cublasFree(cuB);
	checkStatus("freeB", status);
	status = cublasFree(cuC);
	checkStatus("freeC", status);

#ifdef __TEST__
	test(m, a, b, c);
#endif

    free(a);
    free(b);
    free(c);

    return (requestEnd - requestStart);
  }

main()
{
	cublasStatus status;

	srand(time(NULL));

	status = cublasInit();
	checkStatus("init",  status);

    double factor = 6.0 / 100.0;
    int i, j;
    for (i = 0 ; i < 10 ; i++) {
        for (j = 1 ; j <= 100 ; j++) {
            int size = (int) pow(10.0, factor * j);
            if (size < 10) continue;
            long took = benchmark(size);
            printf("\"%d\",\"%lu\"\n", size, took);
			fflush(stdout);
        }
    }
}


/* interpolation/gsl_spline2d.h
 *
 * Copyright 2012 David Zaslavsky
 *
 * 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.
 */

#ifndef __GSL_SPLINE2D_H__
#define __GSL_SPLINE2D_H__

#if !defined( GSL_FUN )
#  if !defined( GSL_DLL )
#    define GSL_FUN extern
#  elif defined( BUILD_GSL_DLL )
#    define GSL_FUN extern __declspec(dllexport)
#  else
#    define GSL_FUN extern __declspec(dllimport)
#  endif
#endif

#include <gsl/gsl_interp.h>
#include <gsl/gsl_interp2d.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

/*
 * A 2D interpolation object which stores the arrays defining the function.
 * In all other respects, this is just like a gsl_interp2d object.
 */
typedef struct
{
  gsl_interp2d interp_object; /* low-level interpolation object */
  double * xarr;              /* x data array */
  double * yarr;              /* y data array */
  double * zarr;              /* z data array */
} gsl_spline2d;

GSL_FUN gsl_spline2d * gsl_spline2d_alloc(const gsl_interp2d_type * T, size_t xsize, size_t ysize);

GSL_FUN int gsl_spline2d_init(gsl_spline2d * interp, const double xa[],
                      const double ya[], const double za[],
                      size_t xsize, size_t ysize);

GSL_FUN void gsl_spline2d_free(gsl_spline2d * interp);

GSL_FUN double gsl_spline2d_eval(const gsl_spline2d * interp, const double x,
                         const double y, gsl_interp_accel* xa, gsl_interp_accel* ya);

GSL_FUN int gsl_spline2d_eval_e(const gsl_spline2d * interp, const double x,
                        const double y, gsl_interp_accel* xa, gsl_interp_accel* ya,
                        double * z);

GSL_FUN double gsl_spline2d_eval_deriv_x(const gsl_spline2d * interp, const double x,
                                 const double y, gsl_interp_accel* xa, gsl_interp_accel* ya);

GSL_FUN int gsl_spline2d_eval_deriv_x_e(const gsl_spline2d * interp, const double x,
                                const double y, gsl_interp_accel* xa,
                                gsl_interp_accel* ya, double * z);

GSL_FUN double gsl_spline2d_eval_deriv_y(const gsl_spline2d * interp, const double x,
                                 const double y, gsl_interp_accel* xa,
                                 gsl_interp_accel* ya);

GSL_FUN int gsl_spline2d_eval_deriv_y_e(const gsl_spline2d * interp, const double x,
                                const double y, gsl_interp_accel* xa,
                                gsl_interp_accel* ya, double * z);

GSL_FUN double gsl_spline2d_eval_deriv_xx(const gsl_spline2d * interp, const double x,
                                  const double y, gsl_interp_accel* xa, gsl_interp_accel* ya);

GSL_FUN int gsl_spline2d_eval_deriv_xx_e(const gsl_spline2d * interp, const double x,
                                 const double y, gsl_interp_accel* xa,
                                 gsl_interp_accel* ya, double * z);

GSL_FUN double gsl_spline2d_eval_deriv_yy(const gsl_spline2d * interp, const double x,
                                  const double y, gsl_interp_accel* xa, gsl_interp_accel* ya);

GSL_FUN int gsl_spline2d_eval_deriv_yy_e(const gsl_spline2d * interp, const double x,
                                 const double y, gsl_interp_accel* xa,
                                 gsl_interp_accel* ya, double * z);

GSL_FUN double gsl_spline2d_eval_deriv_xy(const gsl_spline2d * interp, const double x,
                                  const double y, gsl_interp_accel* xa, gsl_interp_accel* ya);

GSL_FUN int gsl_spline2d_eval_deriv_xy_e(const gsl_spline2d * interp, const double x,
                                 const double y, gsl_interp_accel* xa,
                                 gsl_interp_accel* ya, double * z);

GSL_FUN size_t gsl_spline2d_min_size(const gsl_spline2d * interp);

GSL_FUN const char * gsl_spline2d_name(const gsl_spline2d * interp);

GSL_FUN int gsl_spline2d_set(const gsl_spline2d * interp, double zarr[],
                     const size_t i, const size_t j, const double z);
GSL_FUN double gsl_spline2d_get(const gsl_spline2d * interp, const double zarr[],
                        const size_t i, const size_t j);

__END_DECLS

#endif /* __GSL_SPLINE2D_H__ */


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

/*
 * This module handles the solution of the linear least squares
 * system:
 *
 * [     J      ] dx = - [ f ]
 * [ sqrt(mu)*D ]        [ 0 ]
 *
 * using an SVD approach. The system above is transformed to "standard form"
 * via:
 *
 * J~ = J D^{-1}
 * dx~ = D dx
 *
 * so that
 *
 * [     J~     ] dx~ = - [ f ]
 * [ sqrt(mu)*I ]         [ 0 ]
 *
 * can be solved with a standard SVD method, and then dx is recovered
 * from dx~ via: dx = D^{-1} dx~
 */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_multifit_nlinear.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>

typedef struct
{
  size_t n;                  /* number of residuals */
  size_t p;                  /* number of parameters */
  gsl_matrix *U;             /* U factor of J, n-by-p */
  gsl_matrix *V;             /* V factor of J, p-by-p */
  gsl_vector *S;             /* singular values, size p */
  gsl_vector *workp;         /* workspace, length p */
  double mu;                 /* LM parameter */
} svd_state_t;

static int svd_init(const void * vtrust_state, void * vstate);
static int svd_presolve(const double mu, const void * vtrust_state, void * vstate);
static int svd_solve(const gsl_vector * f, gsl_vector *x,
                     const void * vtrust_state, void *vstate);
static int svd_rcond(double * rcond, void * vstate);

static void *
svd_alloc (const size_t n, const size_t p)
{
  svd_state_t *state;

  (void)n;

  state = calloc(1, sizeof(svd_state_t));
  if (state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate svd state", GSL_ENOMEM);
    }

  state->U = gsl_matrix_alloc(n, p);
  if (state->U == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for U", GSL_ENOMEM);
    }

  state->V = gsl_matrix_alloc(p, p);
  if (state->V == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for V", GSL_ENOMEM);
    }

  state->S = gsl_vector_alloc(p);
  if (state->S == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for S",
                      GSL_ENOMEM);
    }

  state->workp = gsl_vector_alloc(p);
  if (state->workp == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workp",
                      GSL_ENOMEM);
    }

  state->mu = 0.0;
  state->n = n;
  state->p = p;

  return state;
}

static void
svd_free(void *vstate)
{
  svd_state_t *state = (svd_state_t *) vstate;

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

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

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

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

  free(state);
}

/* compute svd of J */
static int
svd_init(const void * vtrust_state, void * vstate)
{
  int status;
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  svd_state_t *state = (svd_state_t *) vstate;
  size_t i;

  gsl_matrix_set_zero(state->U);

  /* compute U = J D^{-1} */
  for (i = 0; i < state->p; ++i)
    {
      gsl_vector_const_view Ji = gsl_matrix_const_column(trust_state->J, i);
      gsl_vector_view ui = gsl_matrix_column(state->U, i);
      double di = gsl_vector_get(trust_state->diag, i);

      gsl_blas_daxpy(1.0 / di, &Ji.vector, &ui.vector);
    }

  status = gsl_linalg_SV_decomp(state->U, state->V, state->S, state->workp);

  return status;
}

static int
svd_presolve(const double mu, const void * vtrust_state, void * vstate)
{
  svd_state_t *state = (svd_state_t *) vstate;

  state->mu = mu;

  (void)vtrust_state;

  return GSL_SUCCESS;
}

static int
svd_solve(const gsl_vector * f, gsl_vector *x,
          const void * vtrust_state, void *vstate)
{
  int status = GSL_SUCCESS;
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  svd_state_t *state = (svd_state_t *) vstate;
  const size_t p = state->p;
  const double tol = GSL_DBL_EPSILON;
  const double s0 = gsl_vector_get(state->S, 0);
  size_t j;

  /* compute workp = - U^T f */
  gsl_blas_dgemv(CblasTrans, -1.0, state->U, f, 0.0, state->workp);

  /*
   * compute:
   *
   * workp = sum_i s_i / (s_i^2 + mu) (-u_i^T f)
   */

  if (state->mu == 0.0)
    {
      /*
       * compute Gauss-Newton direction by solving
       * J x = -f
       */

      for (j = 0; j < p; ++j)
        {
          double sj = gsl_vector_get(state->S, j);
          double *ptr = gsl_vector_ptr(state->workp, j);
          double alpha;

          if (sj <= tol * s0)
            alpha = 0.0;
          else
            alpha = 1.0 / sj;

          *ptr *= alpha;
        }
    }
  else
    {
      /*
       * solve:
       *
       * [  J D^{-1}  ] (D x) = -[ f ]
       * [ sqrt(mu) I ]          [ 0 ]
       *
       * using SVD factorization of J D^{-1}
       */

      for (j = 0; j < p; ++j)
        {
          double sj = gsl_vector_get(state->S, j);
          double *ptr = gsl_vector_ptr(state->workp, j);

          *ptr *= sj / (sj*sj + state->mu);
        }
    }

  /* compute: x = V * workp */
  gsl_blas_dgemv(CblasNoTrans, 1.0, state->V, state->workp, 0.0, x);

  /* compute D^{-1} x */
  gsl_vector_div(x, trust_state->diag);

  return status;
}

static int
svd_rcond(double * rcond, void * vstate)
{
  int status = GSL_SUCCESS;
  svd_state_t *state = (svd_state_t *) vstate;
  double smax = gsl_vector_get(state->S, 0);
  double smin = gsl_vector_get(state->S, state->p - 1);

  *rcond = smin / smax;

  return status;
}

static const gsl_multifit_nlinear_solver svd_type =
{
  "svd",
  svd_alloc,
  svd_init,
  svd_presolve,
  svd_solve,
  svd_rcond,
  svd_free
};

const gsl_multifit_nlinear_solver *gsl_multifit_nlinear_solver_svd = &svd_type;


#pragma once

#include <cblas.h>

#include <aml/defs.h>
#include <aml/handle.h>

namespace aml {
namespace impl {
namespace cpu {

inline CBLAS_TRANSPOSE convert_op(char op) {
  if (op == 'n') {
    return CblasNoTrans;
  } else {
    return CblasTrans;
  }
}

/** BLAS LEVEL 1 **************************************************************/

inline float nrm2(aml::Handle h,
                  Index n,
                  const float *x) {
  AML_ASSERT_INT(n);
  auto tic = h.tic("cpu_snrm2_" + std::to_string(n));

  float res = cblas_snrm2(n, x, 1);

  tic.stop();
  return res;
}

inline double nrm2(aml::Handle h,
                   Index n,
                   const double *x) {
  AML_ASSERT_INT(n);
  auto tic = h.tic("cpu_dnrm2_" + std::to_string(n));

  double res = cblas_dnrm2(n, x, 1);

  tic.stop();
  return res;
}

/** BLAS LEVEL 2 **************************************************************/

inline void gemv(aml::Handle h,
                 char op,
                 Index m,
                 Index n,
                 float alpha,
                 const float *a,
                 Index lda,
                 const float *x,
                 float beta,
                 float *y) {
  AML_ASSERT_INT(m, n, lda);
  auto tic = h.tic("cpu_sgemv_" + std::to_string(m) + "_" + std::to_string(n));

  cblas_sgemv(CblasColMajor,
      convert_op(op), m, n, alpha, a, lda, x, 1, beta, y, 1);

  tic.stop();
}

inline void gemv(aml::Handle h,
                 char op,
                 Index m,
                 Index n,
                 double alpha,
                 const double *a,
                 Index lda,
                 const double *x,
                 double beta,
                 double *y) {
  AML_ASSERT_INT(m, n, lda);
  auto tic = h.tic("cpu_dgemv_" + std::to_string(m) + "_" + std::to_string(n));

  cblas_dgemv(CblasColMajor,
      convert_op(op), m, n, alpha, a, lda, x, 1, beta, y, 1);

  tic.stop();
}

inline void trsv(aml::Handle h,
                 char op,
                 Index m,
                 const float *a,
                 Index lda,
                 float *x) {
  AML_ASSERT_INT(m, lda);
  auto tic = h.tic("cpu_strsv_" + std::to_string(m));

  cblas_strsv(CblasColMajor, CblasLower,
      convert_op(op), CblasNonUnit, m, a, lda, x, 1);

  tic.stop();
}

inline void trsv(aml::Handle h,
                 char op,
                 Index m,
                 const double *a,
                 Index lda,
                 double *x) {
  AML_ASSERT_INT(m, lda);

  auto tic = h.tic("cpu_dtrsv_" + std::to_string(m));

  cblas_dtrsv(CblasColMajor, CblasLower,
      convert_op(op), CblasNonUnit, m, a, lda, x, 1);

  tic.stop();
}

/** BLAS LEVEL 3 **************************************************************/

inline void gemm(aml::Handle h,
                 char op_a,
                 char op_b,
                 Index m,
                 Index n,
                 Index k,
                 float alpha,
                 const float *a,
                 Index lda,
                 const float *b,
                 Index ldb,
                 float beta,
                 float *c,
                 Index ldc) {
  AML_ASSERT_INT(m, n, k, lda, ldb, ldc);
  auto tic = h.tic("cpu_sgemm_"
      + std::to_string(m) + "_" + std::to_string(n) + "_" + std::to_string(k));

  cblas_sgemm(CblasColMajor, convert_op(op_a), convert_op(op_b),
      m, n, k, alpha, a, lda, b, ldb, beta, c, ldc);

  tic.stop();
}

inline void gemm(aml::Handle h,
                 char op_a,
                 char op_b,
                 Index m,
                 Index n,
                 Index k,
                 double alpha,
                 const double *a,
                 Index lda,
                 const double *b,
                 Index ldb,
                 double beta,
                 double *c,
                 Index ldc) {
  AML_ASSERT_INT(m, n, k, lda, ldb, ldc);
  auto tic = h.tic("cpu_dgemm_"
      + std::to_string(m) + "_" + std::to_string(n) + "_" + std::to_string(k));

  cblas_dgemm(CblasColMajor, convert_op(op_a), convert_op(op_b),
      m, n, k, alpha, a, lda, b, ldb, beta, c, ldc);

  tic.stop();
}

inline void syrk(aml::Handle h,
                 char op,
                 Index n,
                 Index k,
                 float alpha,
                 const float *a,
                 Index lda,
                 float beta,
                 float *c,
                 Index ldc) {
  AML_ASSERT_INT(n, k, lda, ldc);
  auto tic = h.tic("cpu_ssyrk_" + std::to_string(n) + "_" + std::to_string(k));

  cblas_ssyrk(CblasColMajor, CblasLower,
      convert_op(op), n, k, alpha, a, lda, beta, c, ldc);

  tic.stop();
}

inline void syrk(aml::Handle h,
                 char op,
                 Index n,
                 Index k,
                 double alpha,
                 const double *a,
                 Index lda,
                 double beta,
                 double *c,
                 Index ldc) {
  AML_ASSERT_INT(n, k, lda, ldc);
  auto tic = h.tic("cpu_dsyrk_" + std::to_string(n) + "_" + std::to_string(k));

  cblas_dsyrk(CblasColMajor, CblasLower,
      convert_op(op), n, k, alpha, a, lda, beta, c, ldc);

  tic.stop();
}

inline void trsm(aml::Handle h,
                 char op,
                 Index m,
                 Index n,
                 float alpha,
                 const float *a,
                 Index lda,
                 float *b,
                 Index ldb) {
  AML_ASSERT_INT(m, n, lda, ldb);
  auto tic = h.tic("cpu_strsm_" + std::to_string(m) + "_" + std::to_string(n));

  cblas_strsm(
      CblasColMajor, CblasRight, CblasLower, convert_op(op), CblasNonUnit,
      m, n, alpha, a, lda, b, ldb);

  tic.stop();
}

inline void trsm(aml::Handle h,
                 char op,
                 Index m,
                 Index n,
                 double alpha,
                 const double *a,
                 Index lda,
                 double *b,
                 Index ldb) {
  AML_ASSERT_INT(m, n, lda, ldb);
  auto tic = h.tic("cpu_dtrsm_" + std::to_string(m) + "_" + std::to_string(n));

  cblas_dtrsm(
      CblasColMajor, CblasRight, CblasLower, convert_op(op), CblasNonUnit,
      m, n, alpha, a, lda, b, ldb);

  tic.stop();
}

}  // namespace cpu
}  // namespace impl
}  // namespace aml


#ifndef LA_WRAPPER
#define LA_WRAPPER

// #include <gsl/gsl_cblas.h>
#include <cblas.h>
#include <complex>
#include <lapack_header.h>

namespace blas_wrapper
{

//! @param n : Anzahl Elemente.
//! @param x : Quellvektor x.
//! @param incx : Speicher Abstand zwischen Elemente in Vector x.
//! @param y : Zielvektor (y= alpha*x+y).
//! @param incy: Speicher Abstand zwischen Elemente in Vector y.

static void
copy(int n, const float *x, int incx, float *y, int incy)
{
    cblas_scopy(n, x, incx, y, incy);
}

static void
copy(int n, const double *x, int incx, double *y, int incy)
{
    cblas_dcopy(n, x, incx, y, incy);
}

static void
copy(int n, const std::complex<double> *x, int incx, std::complex<double> *y, int incy)
{
    cblas_zcopy(n, reinterpret_cast<const double*>(x), incx, reinterpret_cast<double*>(y), incy);
}

static void
copy(int n, const std::complex<float> *x, int incx, std::complex<float> *y, int incy)
{
    cblas_ccopy(n, reinterpret_cast<const float*>(x), incx, reinterpret_cast<float*>(y), incy);
}

static void
copy(int n, const int *x, int incx, int *y, int incy)
{
    for(int i=0; i<n; i++)
    {
        y[i*incy]=x[i*incx];
    }
}

//! @param n : Anzahl Elemente.
//! @param alpha: Skalar.
//! @param x : Quellvektor x (auch output).
//! @param incx : Speicher Abstand zwischen Elemente in Vector x.

static void
scal (int n, float alpha, float *x, int incx)
{
    cblas_sscal (n, alpha, x, incx);
}

static void
scal (int n, double alpha, double *x, int incx)
{
    cblas_dscal (n, alpha, x, incx);
}

static void
scal (int n, std::complex<float> alpha, std::complex<float> *x, int incx)
{
   cblas_cscal(n, reinterpret_cast<const float*>(&alpha), reinterpret_cast<float*>(x), incx);
}

static void
scal (int n, std::complex<double>  alpha, std::complex<double> *x, int incx)
{
   cblas_zscal(n, reinterpret_cast<const double*>(&alpha), reinterpret_cast<double*>(x), incx);
}

//! @param trans : gibt an ob A transponiert ist oder nicht. Sei trans = 'N' oder 'n' so ist op(A)= A, sei trans = 'T', 't','C' oder 'c' so ist op(A)= trans(A)
//! @param m : Anzahl Zeilen in Matrix A.
//! @param n : Anzahl Spalten in Matrix A.
//! @param alpha: Skalar fuer A.
//! @param A : Matrix A
//! @param lda : leading dimension von A.
//! @param x : Vektor mit der laenge von mindestens (1+(n-1)*abs(incx)) falls trans = 'N' oder 'n', sonst mindestens der laenge (1+(m-1)*abs(incx)).
//! @param incx : Speicher Abstand zwischen Elemente in Vector x.
//! @param beta : Skalar fuer Vektor y.
//! @param y : Vektor mit der laenge von mindestens (1+(n-1)*abs(incy)) falls trans = 'N' oder 'n', sonst mindestens der laenge (1+(m-1)*abs(incy)).
//! @param incy: Speicher Abstand zwischen Elemente in Vector y.

static void gemv (char trans, int m, int n, float alpha,
                  const float * const A, int lda,
                  const float * const x,  int incx, float beta,
                  float *y, int incy)
{
    CBLAS_TRANSPOSE tr = ( ( (trans == 't') || (trans == 'T') ) ? CblasTrans : CblasNoTrans );
    cblas_sgemv (CblasColMajor, tr, m, n, alpha, A, lda, x, incx, beta, y, incy);
}

static void gemv (char trans, int m, int n, double alpha,
                  const double * const A, int lda,
                  const double * const x,  int incx, double beta,
                  double *y, int incy)
{
    CBLAS_TRANSPOSE tr = ( ( (trans == 't') || (trans == 'T') ) ? CblasTrans : CblasNoTrans );
    cblas_dgemv (CblasColMajor, tr, m, n, alpha, A, lda, x, incx, beta, y, incy);
}

static void gemv (char trans, int m, int n, std::complex<float> & alpha,
                  const std::complex<float> * const A, int lda,
                  const std::complex<float> * const x,  int incx, std::complex<float> & beta,
                  std::complex<float> *y, int incy)
{
    CBLAS_TRANSPOSE tr = ( ( (trans == 't') || (trans == 'T') ) ? CblasTrans : CblasNoTrans );
    cblas_cgemv (CblasColMajor, tr, m, n, &alpha, A, lda, x, incx, &beta, y, incy);
}

static void gemv (char trans, int m, int n, std::complex<double> & alpha,
                  const std::complex<double> * const A, int lda,
                  const std::complex<double> * const x,  int incx, std::complex<double>  & beta,
                  std::complex<double> *y, int incy)
{
    CBLAS_TRANSPOSE tr = ( ( (trans == 't') || (trans == 'T') ) ? CblasTrans : CblasNoTrans );
    cblas_zgemv (CblasColMajor, tr, m, n, &alpha, A, lda, x, incx, &beta, y, incy);
}

//! @param transa : gibt an ob A transponiert ist oder nicht. Sei transa = 'N' oder 'n' so ist op(A)= A, sei transa = 'T' oder 't' so ist op(A)= trans(A), sei transa = 'C' oder 'c' so ist op(A)=adjoint(A)
//! @param transb : gibt an ob B transponiert ist oder nicht. Sei transb = 'N' oder 'n' so ist op(B)= A, sei transb = 'T' oder 't' so ist op(B)= trans(B), sei transb = 'C' oder 'c' so ist op(B)=adjoint(B)
//! @param m : Anzahl Zeilen in Matrix A und Matrix C.
//! @param n : Anzahl Spalten in Matrix B und Matrix C.
//! @param k : Anzahl Spalten in Matrix A und Zeilen in Matrix B.
//! @param alpha: Skalar fuer op(A)*op(B).
//! @param A : Matrix A
//! @param lda : leading dimension von A.
//! @param B : Matrix B.
//! @param ldb : leading dimension von B.
//! @param beta : Skalar fuer Matrix C.
//! @param C : Matrix C.
//! @param ldc : leading dimension von C.
static void gemm(char transa, char transb, int m, int n, int k, float alpha,
                 const float * const A, int lda, const float * const B, int ldb,
                 float beta, float * C, int ldc)
{
    CBLAS_TRANSPOSE tr_a = ( ( (transa == 't') || (transa == 'T') ) ? CblasTrans : ( (transa == 'c') || (transa == 'C') ) ? CblasTrans : CblasNoTrans );
    CBLAS_TRANSPOSE tr_b = ( ( (transb == 't') || (transb == 'T') ) ? CblasTrans : ( (transb == 'c') || (transb == 'C') ) ? CblasTrans : CblasNoTrans );

    cblas_sgemm(CblasColMajor,
                tr_a, tr_b,
                m, n, k,
                alpha,
                A, lda,
                B, ldb,
                beta,
                C, ldc);
}

static void gemm(char transa, char transb, int m, int n, int k, double alpha,
                 const double * const A, int lda, const double * const B, int ldb,
                 double beta, double * C, int ldc)
{
    CBLAS_TRANSPOSE tr_a = ( ( (transa == 't') || (transa == 'T') ) ? CblasTrans : ( (transa == 'c') || (transa == 'C') ) ? CblasTrans : CblasNoTrans );
    CBLAS_TRANSPOSE tr_b = ( ( (transb == 't') || (transb == 'T') ) ? CblasTrans : ( (transb == 'c') || (transb == 'C') ) ? CblasTrans : CblasNoTrans );

    cblas_dgemm(CblasColMajor, tr_a, tr_b, m, n, k, alpha,
                A, lda, B, ldb,
                beta, C, ldc);
}

static void gemm(char transa, char transb, int m, int n, int k, const std::complex<float> alpha,
                 const std::complex<float> * const A, int lda, const std::complex<float> * const B, int ldb,
                 const std::complex<float> beta, std::complex<float> * C, int ldc)
{
    CBLAS_TRANSPOSE tr_a = ( ( (transa == 't') || (transa == 'T') ) ? CblasTrans : ( (transa == 'c') || (transa == 'C') ) ? CblasConjTrans : CblasNoTrans );
    CBLAS_TRANSPOSE tr_b = ( ( (transb == 't') || (transb == 'T') ) ? CblasTrans : ( (transb == 'c') || (transb == 'C') ) ? CblasConjTrans : CblasNoTrans );

    cblas_cgemm(CblasColMajor,
                tr_a, tr_b,
                m, n, k,
                reinterpret_cast<const float*>(&alpha),
                reinterpret_cast<const float*>(A), lda,
                reinterpret_cast<const float*>(B), ldb,
                reinterpret_cast<const float*>(&beta),
                reinterpret_cast<float*>(C), ldc);
}

static void gemm(char transa, char transb, int m, int n, int k, const std::complex<double> alpha,
                 const std::complex<double> * const A, int lda, const std::complex<double> * const B, int ldb,
                 const std::complex<double> beta, std::complex<double> * C, int ldc)
{
    CBLAS_TRANSPOSE tr_a = ( ( (transa == 't') || (transa == 'T') ) ? CblasTrans : ( (transa == 'c') || (transa == 'C') ) ? CblasConjTrans : CblasNoTrans );
    CBLAS_TRANSPOSE tr_b = ( ( (transb == 't') || (transb == 'T') ) ? CblasTrans : ( (transb == 'c') || (transb == 'C') ) ? CblasConjTrans : CblasNoTrans );

    cblas_zgemm(CblasColMajor,
                tr_a, tr_b,
                m, n, k,
                reinterpret_cast<const double*>(&alpha),
                reinterpret_cast<const double*>(A), lda,
                reinterpret_cast<const double*>(B), ldb,
                reinterpret_cast<const double*>(&beta),
                reinterpret_cast<double*>(C), ldc);
}

} // END NAMESPACE blas_wrapper

namespace lapack_wrapper
{

// @sect5{Lapack function wrappers}
// @sect6{Wrapper: SVD lapack-wrapper}
// Wraps the general matrix svd lapack function for the four types s, d, c and z.
// Lapack doc: http://www.netlib.org/lapack/explore-html/d8/d49/sgesvd_8f.html
inline static void gesvd(char * JOBU, char * JOBVT, const LAPACK_INT * M, const LAPACK_INT * N, float * A, const LAPACK_INT * LDA, float * S, float * U, const LAPACK_INT * LDU, float * VT, const LAPACK_INT * LDVT, float * WORK, const LAPACK_INT * LWORK, float * /*RWORK*/, LAPACK_INT * INFO)
{
    sgesvd_(JOBU, JOBVT, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, INFO);
}

inline static void gesvd(char * JOBU, char * JOBVT, const LAPACK_INT * M, const LAPACK_INT * N, double * A, const LAPACK_INT * LDA, double * S, double * U, const LAPACK_INT * LDU, double * VT, const LAPACK_INT * LDVT, double * WORK, const LAPACK_INT * LWORK, double * /*RWORK*/, LAPACK_INT * INFO)
{
    dgesvd_(JOBU, JOBVT, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, INFO);
}

inline static void gesvd(char * JOBU, char * JOBVT, const LAPACK_INT * M, const LAPACK_INT * N, std::complex<float> * A, const LAPACK_INT * LDA, float * S, std::complex<float> * U, const LAPACK_INT  * LDU, std::complex<float> * VT, const LAPACK_INT * LDVT, std::complex<float> * WORK, const LAPACK_INT * LWORK, float * RWORK, LAPACK_INT * INFO)
{
    cgesvd_(JOBU, JOBVT, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, RWORK, INFO);
}

inline static void gesvd(char * JOBU, char * JOBVT, const LAPACK_INT * M, const LAPACK_INT * N, std::complex<double> * A, const LAPACK_INT * LDA, double * S, std::complex<double> * U, const LAPACK_INT * LDU, std::complex<double> * VT, const LAPACK_INT * LDVT, std::complex<double> * WORK, const LAPACK_INT * LWORK, double * RWORK, LAPACK_INT * INFO)
{
    zgesvd_(JOBU, JOBVT, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, RWORK, INFO);
}

// @sect6{Wrapper: SVD divide and conquer lapack-wrapper}
// Wraps the general matrix sdd lapack function for the four types s, d, c and z.
// Lapack doc: http://www.netlib.org/lapack/explore-html/d1/d7e/group__double_g_esing_ga76f797b6a9e278ad7b21aae2b4a55d76.html#ga76f797b6a9e278ad7b21aae2b4a55d76
inline static void gesdd(char * JOBZ, const LAPACK_INT * M, const LAPACK_INT * N, float * A, const LAPACK_INT * LDA, float * S, float * U, const LAPACK_INT * LDU, float * VT, const LAPACK_INT * LDVT, float * WORK, const LAPACK_INT * LWORK, float * /*RWORK*/, LAPACK_INT * IWORK, LAPACK_INT * INFO)
{
    return sgesdd_(JOBZ, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, IWORK, INFO);
}
inline static void gesdd(char * JOBZ, const LAPACK_INT * M, const LAPACK_INT * N, double * A, const LAPACK_INT * LDA, double * S, double * U, const LAPACK_INT * LDU, double * VT, const LAPACK_INT * LDVT, double * WORK, const LAPACK_INT * LWORK, double * /*RWORK*/, LAPACK_INT * IWORK, LAPACK_INT * INFO)
{
    return dgesdd_(JOBZ, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, IWORK, INFO);
}
inline static void gesdd(char * JOBZ, const LAPACK_INT * M, const LAPACK_INT * N, std::complex<float> * A, const LAPACK_INT * LDA, float * S, std::complex<float> * U, const LAPACK_INT * LDU, std::complex<float> * VT, const LAPACK_INT * LDVT, std::complex<float> * WORK, const LAPACK_INT * LWORK, float * RWORK, LAPACK_INT * IWORK, LAPACK_INT * INFO)
{
    return cgesdd_(JOBZ, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, RWORK, IWORK, INFO);
}
inline static void gesdd(char * JOBZ, const LAPACK_INT * M, const LAPACK_INT * N, std::complex<double> * A, const LAPACK_INT * LDA, double * S, std::complex<double> * U, const LAPACK_INT * LDU, std::complex<double> * VT, const LAPACK_INT * LDVT, std::complex<double> * WORK, const LAPACK_INT * LWORK, double * RWORK, LAPACK_INT * IWORK, LAPACK_INT * INFO)
{
    return zgesdd_(JOBZ, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, RWORK, IWORK, INFO);
}

// @sect6{Wrapper: LUD lapack-wrapper}
// Wraps the general matrix LU decomposition lapack function for the four types s, d, c and z.
// Lapack doc: http://www.netlib.org/lapack/explore-html/de/de2/sgetrf_8f.html
inline static void getrf(const LAPACK_INT * M, const LAPACK_INT * N, float * A, const LAPACK_INT * LDA, LAPACK_INT * IPIV, LAPACK_INT * INFO)
{
    sgetrf_(M, N, A, LDA, IPIV, INFO);
}
inline static void getrf(const LAPACK_INT * M, const LAPACK_INT * N, double * A, const LAPACK_INT * LDA, LAPACK_INT * IPIV, LAPACK_INT * INFO)
{
    dgetrf_(M, N, A, LDA, IPIV, INFO);
}
inline static void getrf(const LAPACK_INT * M, const LAPACK_INT * N, std::complex<float> * A, const LAPACK_INT * LDA, LAPACK_INT * IPIV, LAPACK_INT * INFO)
{
    cgetrf_(M, N, A, LDA, IPIV, INFO);
}
inline static void getrf(const LAPACK_INT * M, const LAPACK_INT * N, std::complex<double> * A, const LAPACK_INT * LDA, LAPACK_INT * IPIV, LAPACK_INT * INFO)
{
    zgetrf_(M, N, A, LDA, IPIV, INFO);
}

// @sect6{Wrapper: LU-Inverse lapack-wrapper}
// Wraps the general matrix inversion lapack function for the two types s, d.
 // Lapack doc: http://www.netlib.org/lapack/explore-html/de/de2/sgetri_8f.html
inline static void getri (const LAPACK_INT * N, float * A, const LAPACK_INT * LDA, const LAPACK_INT * IPIV, float * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    sgetri_(N, A, LDA, IPIV, WORK, LWORK, INFO);
}
inline static void getri (const LAPACK_INT * N, double * A, const LAPACK_INT * LDA, const LAPACK_INT * IPIV, double * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    dgetri_(N, A, LDA, IPIV, WORK, LWORK, INFO);
}

// @sect6{Wrapper: QRF lapack-wrapper}
// Wraps the general matrix QR factorization lapack function for the four types s, d, c and z.
// Lapack doc: http://www.netlib.org/lapack/explore-html/df/d97/sgeqrf_8f.html
inline static void geqrf(const LAPACK_INT * M, const LAPACK_INT * N, float * A, const LAPACK_INT * LDA, float * TAU, float * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    sgeqrf_(M, N, A, LDA, TAU, WORK, LWORK, INFO);
}
inline static void geqrf(const LAPACK_INT * M, const LAPACK_INT * N, double * A, const LAPACK_INT * LDA, double * TAU, double * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    dgeqrf_(M, N, A, LDA, TAU, WORK, LWORK, INFO);
}
inline static void geqrf(const LAPACK_INT * M, const LAPACK_INT * N, std::complex<float> * A, const LAPACK_INT * LDA, std::complex<float> * TAU, std::complex<float> * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    cgeqrf_(M, N, A, LDA, TAU, WORK, LWORK, INFO);
}
inline static void geqrf(const LAPACK_INT * M, const LAPACK_INT * N, std::complex<double> * A, const LAPACK_INT * LDA, std::complex<double> * TAU, std::complex<double> * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    zgeqrf_(M, N, A, LDA, TAU, WORK, LWORK, INFO);
}

// @sect6{Wrapper: MQR lapack-wrapper}
// Wraps the lapack function of the product of elementary reflectors from the QR factorization for the four types s, d, c and z.
// Lapack doc: http://www.netlib.org/lapack/explore-html/d0/d98/sormqr_8f.html
inline static void xxmqr(char * SIDE, char * TRANS, const LAPACK_INT * M, const LAPACK_INT * N, const LAPACK_INT * K, float * A, const LAPACK_INT * LDA, float * TAU, float * C, const LAPACK_INT * LDC, float * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    sormqr_(SIDE, TRANS, M, N, K, A, LDA, TAU, C, LDC, WORK, LWORK, INFO);
}
inline static void xxmqr(char * SIDE, char * TRANS, const LAPACK_INT * M, const LAPACK_INT * N, const LAPACK_INT * K, double * A, const LAPACK_INT * LDA, double * TAU, double * C, const LAPACK_INT * LDC, double * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    dormqr_(SIDE, TRANS, M, N, K, A, LDA, TAU, C, LDC, WORK, LWORK, INFO);
}
inline static void xxmqr(char * SIDE, char * TRANS, const LAPACK_INT * M, const LAPACK_INT * N, const LAPACK_INT * K, std::complex<float> * A, const LAPACK_INT * LDA, std::complex<float> * TAU, std::complex<float> * C, const LAPACK_INT * LDC, std::complex<float> * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    cunmqr_(SIDE, TRANS, M, N, K, A, LDA, TAU, C, LDC, WORK, LWORK, INFO);
}
inline static void xxmqr(char * SIDE, char * TRANS, const LAPACK_INT * M, const LAPACK_INT * N, const LAPACK_INT * K, std::complex<double> * A, const LAPACK_INT * LDA, std::complex<double> * TAU, std::complex<double> * C, const LAPACK_INT * LDC, std::complex<double> * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    zunmqr_(SIDE, TRANS, M, N, K, A, LDA, TAU, C, LDC, WORK, LWORK, INFO);
}

// @sect6{Wrapper: EV lapack-wrapper}
// Wraps the lapack function for computing the eigenvalues and, optionally, the left and/or right eigenvectors for the four types s, d, c and z.
// Lapack doc: http://www.netlib.org/lapack/explore-html/d1/d74/group__eigen_s_y.html and http://www.netlib.org/lapack/explore-html/df/db2/cheev_8f.html
inline static void xxev(char * JOBZ, char * UPLO, const LAPACK_INT * N, float * A, const LAPACK_INT * LDA, float * W, float * WORK, const LAPACK_INT * LWORK, float * /*RWORK*/, LAPACK_INT * INFO)
{
    return ssyev_(JOBZ, UPLO, N, A, LDA, W, WORK, LWORK, INFO);
}
inline static void xxev(char * JOBZ, char * UPLO, const LAPACK_INT * N, double * A, const LAPACK_INT * LDA, double * W, double * WORK, const LAPACK_INT * LWORK, double * /*RWORK*/, LAPACK_INT * INFO)
{
    return dsyev_(JOBZ, UPLO, N, A, LDA, W, WORK, LWORK, INFO);
}
inline static void xxev(char * JOBZ, char * UPLO, const LAPACK_INT * N, std::complex<float> * A, const LAPACK_INT * LDA, float * W, std::complex<float> * WORK, const LAPACK_INT * LWORK, float * RWORK, LAPACK_INT * INFO)
{
    return cheev_(JOBZ, UPLO, N, A, LDA, W, WORK, LWORK, RWORK, INFO);
}
inline static void xxev(char * JOBZ, char * UPLO, const LAPACK_INT * N, std::complex<double> * A, const LAPACK_INT * LDA, double * W, std::complex<double> * WORK, const LAPACK_INT * LWORK, double * RWORK, LAPACK_INT * INFO)
{
    return zheev_(JOBZ, UPLO, N, A, LDA, W, WORK, LWORK, RWORK, INFO);
}

// @sect6{Wrapper: EV lapack-wrapper}
// Wraps the lapack function for computing the eigenvalues and, optionally, the left and/or right eigenvectors for the two real types s and d.
// Lapack doc: http://www.netlib.org/lapack/explore-html/d1/d74/group__eigen_s_y.html and http://www.netlib.org/lapack/explore-html/df/db2/cheev_8f.html
inline static void xtev(char * JOBZ, const LAPACK_INT * N, float * D, float * E, float * Z, const LAPACK_INT * LDZ, float * WORK, LAPACK_INT * INFO)
{
    return sstev_(JOBZ, N, D, E, Z, LDZ, WORK, INFO);
}
inline static void xtev(char * JOBZ, const LAPACK_INT * N, double * D, double * E, double * Z, const LAPACK_INT * LDZ, double * WORK, LAPACK_INT * INFO)
{
    dstev_(JOBZ, N, D, E, Z, LDZ, WORK, INFO);
}

// @sect6{Wrapper: Matrix norm lapack-wrapper}
// Wraps the lapack function for computing the eigenvalues and, optionally, the left and/or right eigenvectors for the two real types s and d.
// Lapack doc: http://www.netlib.org/lapack/explore-html/d1/d74/group__eigen_s_y.html and http://www.netlib.org/lapack/explore-html/df/db2/cheev_8f.html
inline static float lange(char * NORM, const LAPACK_INT * M, const LAPACK_INT * N, const float * A, const LAPACK_INT * LDA, float * WORK)
{
    return slange_( NORM, M, N, A, LDA, WORK );
}
inline static double lange(char * NORM, const LAPACK_INT * M, const LAPACK_INT * N, const double * A, const LAPACK_INT * LDA, double * WORK)
{
    return dlange_( NORM, M, N, A, LDA, WORK );
}
inline static float lange(char * NORM, const LAPACK_INT * M, const LAPACK_INT * N, const std::complex<float> * A, const LAPACK_INT * LDA, float * WORK)
{
    return clange_( NORM, M, N, A, LDA, WORK );
}
inline static double lange(char * NORM, const LAPACK_INT * M, const LAPACK_INT * N, const std::complex<double> * A, const LAPACK_INT * LDA, double * WORK)
{
    return zlange_( NORM, M, N, A, LDA, WORK );
}

} // END NAMESPACE LAPACK_WRAPPER

#endif // LA_WRAPPER


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

#include <time.h>
#include <sys/time.h>

// http://stackoverflow.com/questions/5167269
#ifdef __MACH__
#include <mach/clock.h>
#include <mach/mach.h>
#endif

// http://stackoverflow.com/questions/7034930
double drand() {
  return (rand()+1.0)/(RAND_MAX+1.0);
}

double random_normal()  {
  return sqrt(-2*log(drand())) * cos(2*M_PI*drand());
}

double* random_array(int size) {
	double *a = malloc(sizeof(double) * size);
	int i;

	for (i = 0 ; i < size; i++) {
		a[i] = random_normal();
	}

	return a;
}

long currentTimeNanos() {
struct timespec ts;

#ifdef __MACH__
clock_serv_t cclock;
mach_timespec_t mts;
host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
clock_get_time(cclock, &mts);
mach_port_deallocate(mach_task_self(), cclock);
ts.tv_sec = mts.tv_sec;
ts.tv_nsec = mts.tv_nsec;

#else
clock_gettime(CLOCK_REALTIME, &ts);
#endif

return (ts.tv_sec * 1000000000) + ts.tv_nsec;
}


// Author: Samuel D. Relton
#include <stdio.h>
#include <cblas.h>
#include "sgemm_batched.h"

float A1[] = {
3, 1, 3,
1, 5, 9,
2, 6, 5
};

float B1[] = {
3, 1, 3,
1, 5, 9,
2, 6, 5
};

float C1[] = {
0, 0, 0,
0, 0, 0,
0, 0, 0
};

float A2[] = {
3, 1, 3,
1, 5, 9,
2, 6, 5
};

float B2[] = {
3, 1, 3,
1, 5, 9,
2, 6, 5
};

float C2[] = {
0, 0, 0,
0, 0, 0,
0, 0, 0
};

float ALPHA[] = {1.0, 1.0};
float BETA[] = {0.0, 0.0};

int LDA[] = {3, 3};
int LDB[] = {3, 3};
int LDC[] = {3, 3};

bblas_trans_t  TRANSA[] = {CblasNoTrans, CblasNoTrans};
bblas_trans_t  TRANSB[] = {CblasNoTrans, CblasNoTrans};

int M[] = {3, 3};
int N[] = {3, 3};
int K[] = {3, 3};

int BATCHCOUNT = 2;
bblas_batch_type_t BATCH_TYPE = BBLASFixed;

int INFO[] = {0, 0};

float *arrayA[] = {A1, A2};
float *arrayB[] = {B1, B2};
float *arrayC[] = {C1, C2};

int main()
{
	int i, j, k;

	sgemm_batched(TRANSA, TRANSB, M, N, K, ALPHA, arrayA,
		      LDA, arrayB, LDB, BETA, arrayC, LDC, BATCHCOUNT,
		      BATCH_TYPE, INFO);

	for (k = 0; k < BATCHCOUNT; k++) {
		putchar('\n');
		for (i=0; i<3; ++i) {
			for (j=0; j<3; ++j) {
				printf("%5.1f", arrayC[k][i*3+j]);

			}
			putchar('\n');
		}
	}

    return 0;
}


#ifdef GSLODE1

////////////////////////////// ODE V1 //////////////////////////////////////////

#include <gsl/gsl_odeiv.h>

typedef struct {int n; int (*f)(double,int, const double*, int, double *); int (*j)(double,int, const double*, int, int, double*);} Tode;

int odefunc (double t, const double y[], double f[], void *params) {
    Tode * P = (Tode*) params;
    (P->f)(t,P->n,y,P->n,f);
    return GSL_SUCCESS;
}

int odejac (double t, const double y[], double *dfdy, double dfdt[], void *params) {
     Tode * P = ((Tode*) params);
     (P->j)(t,P->n,y,P->n,P->n,dfdy);
     int j;
     for (j=0; j< P->n; j++)
        dfdt[j] = 0.0;
     return GSL_SUCCESS;
}

int ode(int method, double h, double eps_abs, double eps_rel,
        int f(double, int, const double*, int, double*),
        int jac(double, int, const double*, int, int, double*),
        KRVEC(xi), KRVEC(ts), RMAT(sol)) {

    const gsl_odeiv_step_type * T;

    switch(method) {
        case 0 : {T = gsl_odeiv_step_rk2; break; }
        case 1 : {T = gsl_odeiv_step_rk4; break; }
        case 2 : {T = gsl_odeiv_step_rkf45; break; }
        case 3 : {T = gsl_odeiv_step_rkck; break; }
        case 4 : {T = gsl_odeiv_step_rk8pd; break; }
        case 5 : {T = gsl_odeiv_step_rk2imp; break; }
        case 6 : {T = gsl_odeiv_step_rk4imp; break; }
        case 7 : {T = gsl_odeiv_step_bsimp; break; }
        case 8 : { printf("Sorry: ODE rk1imp not available in this GSL version\n"); exit(0); }
        case 9 : { printf("Sorry: ODE msadams not available in this GSL version\n"); exit(0); }
        case 10: { printf("Sorry: ODE msbdf not available in this GSL version\n"); exit(0); }
        default: ERROR(BAD_CODE);
    }

    gsl_odeiv_step * s = gsl_odeiv_step_alloc (T, xin);
    gsl_odeiv_control * c = gsl_odeiv_control_y_new (eps_abs, eps_rel);
    gsl_odeiv_evolve * e = gsl_odeiv_evolve_alloc (xin);

    Tode P;
    P.f = f;
    P.j = jac;
    P.n = xin;

    gsl_odeiv_system sys = {odefunc, odejac, xin, &P};

    double t = tsp[0];

    double* y = (double*)calloc(xin,sizeof(double));
    int i,j;
    for(i=0; i< xin; i++) {
        y[i] = xip[i];
        solp[i] = xip[i];
    }

       for (i = 1; i < tsn ; i++)
         {
           double ti = tsp[i];
           while (t < ti)
             {
               gsl_odeiv_evolve_apply (e, c, s,
                                       &sys,
                                       &t, ti, &h,
                                       y);
               // if (h < hmin) h = hmin;
             }
           for(j=0; j<xin; j++) {
               solp[i*xin + j] = y[j];
           }
         }

    free(y);
    gsl_odeiv_evolve_free (e);
    gsl_odeiv_control_free (c);
    gsl_odeiv_step_free (s);
    return 0;
}

#else

///////////////////// ODE V2 ///////////////////////////////////////////////////

#include <gsl/gsl_odeiv2.h>

typedef struct {int n; int (*f)(double,int, const double*, int, double *); int (*j)(double,int, const double*, int, int, double*);} Tode;

int odefunc (double t, const double y[], double f[], void *params) {
    Tode * P = (Tode*) params;
    (P->f)(t,P->n,y,P->n,f);
    return GSL_SUCCESS;
}

int odejac (double t, const double y[], double *dfdy, double dfdt[], void *params) {
     Tode * P = ((Tode*) params);
     (P->j)(t,P->n,y,P->n,P->n,dfdy);
     int j;
     for (j=0; j< P->n; j++)
        dfdt[j] = 0.0;
     return GSL_SUCCESS;
}

int ode(int method, double h, double eps_abs, double eps_rel,
        int f(double, int, const double*, int, double*),
        int jac(double, int, const double*, int, int, double*),
        KRVEC(xi), KRVEC(ts), RMAT(sol)) {

    const gsl_odeiv2_step_type * T;

    switch(method) {
        case 0 : {T = gsl_odeiv2_step_rk2; break; }
        case 1 : {T = gsl_odeiv2_step_rk4; break; }
        case 2 : {T = gsl_odeiv2_step_rkf45; break; }
        case 3 : {T = gsl_odeiv2_step_rkck; break; }
        case 4 : {T = gsl_odeiv2_step_rk8pd; break; }
        case 5 : {T = gsl_odeiv2_step_rk2imp; break; }
        case 6 : {T = gsl_odeiv2_step_rk4imp; break; }
        case 7 : {T = gsl_odeiv2_step_bsimp; break; }
        case 8 : {T = gsl_odeiv2_step_rk1imp; break; }
        case 9 : {T = gsl_odeiv2_step_msadams; break; }
        case 10: {T = gsl_odeiv2_step_msbdf; break; }
        default: ERROR(BAD_CODE);
    }

    Tode P;
    P.f = f;
    P.j = jac;
    P.n = xin;

    gsl_odeiv2_system sys = {odefunc, odejac, xin, &P};

    gsl_odeiv2_driver * d =
         gsl_odeiv2_driver_alloc_y_new (&sys, T, h, eps_abs, eps_rel);

    double t = tsp[0];

    double* y = (double*)calloc(xin,sizeof(double));
    int i,j;
    int status;
    for(i=0; i< xin; i++) {
        y[i] = xip[i];
        solp[i] = xip[i];
    }

       for (i = 1; i < tsn ; i++)
         {
           double ti = tsp[i];

           status = gsl_odeiv2_driver_apply (d, &t, ti, y);

           if (status != GSL_SUCCESS) {
         	  printf ("error in ode, return value=%d\n", status);
         	  break;
        	}

//           printf ("%.5e %.5e %.5e\n", t, y[0], y[1]);

           for(j=0; j<xin; j++) {
               solp[i*xin + j] = y[j];
           }
         }

    free(y);
    gsl_odeiv2_driver_free (d);

    return status;
}

#endif


/* population genetics tools- mainly for working with site frequency spectrum
/
/
/ Andrew Kern
*/

#include "stdio.h"
#include "math.h"
#include "popGenTools.h"
#include "numerical.h"
#include "assert.h"
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_sf.h>

/* function for sfs generation */
double my_f(double q, void * p){
  struct my_f_params * params = (struct my_f_params *) p;
  double beta = params->beta;
  int i = params->i;
  int n = params->n;

  return ((1.0 - exp(-2.0 * beta * (1.0 - q))) / (1.0 - exp(-2.0 * beta))) * (2.0 / (q * (1.0 - q))) * gsl_ran_binomial_pdf(i, q, n);
}

/* integral of f over allele freq */
double my_F(double beta, void * p){
  struct my_F_params * params = (struct my_F_params *) p;
  struct my_f_params fParams;
  gsl_function f;
  gsl_integration_workspace * w = gsl_integration_workspace_alloc (100000);
  double result,error;

  if (beta == 0){
    gsl_integration_workspace_free(w);
    return 1.0 / params->i;
  }
  else{
    fParams.beta = beta;
    fParams.i = params->i;
    fParams.n = params->n;
    f.function = &(my_f);
    f.params = &fParams;
    gsl_integration_qags(&f,0.0,1.0,1e-5,1e-5, 100000,w, &result,&error);
    gsl_integration_workspace_free(w);
    return result;
  }
}

/* integral of f over allele freq using numerical recipes integration */
double my_F2(double beta, void * p){
  struct my_F_params * params = (struct my_F_params *) p;
  struct my_f_params fParams;
  double result;

  if (beta == 0){
    return 1.0 / params->i;
  }
  else{
    fParams.beta = beta;
    fParams.i = params->i;
    fParams.n = params->n;
    result = d_qromb2(&my_f, 0.0, 1.0, &fParams);
    return result;
  }
}

/* this returns the prob of SNP freq i given n and beta */
double snpProb(double beta, void * p){
  struct my_F_params * params = (struct my_F_params *) p;
  struct my_F_params tempParams;
  long double tot;
  int i;
  gsl_function F;

  /* add up total prob space */
  tot = 0.0;
  tempParams.n = params->n;

  for(i = 1; i < params->n ; i++){
    tempParams.i = i;
    F.function =  &my_F;
    F.params = &tempParams;
    tot += GSL_FN_EVAL(&F, beta);
  }

  /* reset parameters */
  F.function =  &my_F;
  F.params = params;

  return   GSL_FN_EVAL(&F, beta) / tot ;
}

/* trying to speedup the snpProb routine with a lookup of the denominators (in snpProb_params) */
double snpProbDenomLookup(double beta, void * p){
  struct my_snpProb_params * params = (struct my_snpProb_params *) p;
  struct my_F_params tempParams;
  gsl_function F;

 /* set parameters */
  tempParams.n = params->n;
  tempParams.i = params->i;
  F.function =  &my_F;
  F.params = &tempParams;

  return   GSL_FN_EVAL(&F, beta) / params->denom ;
}

/* sampleSize vector - returns a vector of 1s or 0s depending on sample sizes in the data */
gsl_vector *sampleSizeVector(struct snp data[], int snpNumber, int maxSampleSize){
  gsl_vector *bools;
  int i;

  //alloc and initialize bools vector
  bools = gsl_vector_alloc(maxSampleSize + 1);
  gsl_vector_set_zero(bools);

  //go through data, set each sampleSize value to 1
  for(i = 0; i < snpNumber; i++){
    gsl_vector_set(bools,data[i].n, 1);
  }
  return(bools);
}

/* returns a vector of snpProbDenominators from i = 2 to n, 1 indexed */
gsl_vector *makeSnpProbDenomVector(double beta, int maxSampleSize, gsl_vector *sampleSizeVector){
  gsl_vector *probs;
  int i, j, test;
  double tot;
  struct my_F_params tempParams;
  gsl_function F;

  probs = gsl_vector_alloc(maxSampleSize + 1);

  /* go from n = 2 to maxSampleSize, calc total prob space for each, put in vector probs */
  for(j = 2; j < maxSampleSize + 1; j++){
    //check if sampleSize is included
    test = gsl_vector_get(sampleSizeVector,j);
      if (test){
      tot = 0.0;
      tempParams.n = j;
      for(i = 1; i < j ; i++){
	tempParams.i = i;
	F.function =  &my_F;
	F.params = &tempParams;
	tot += GSL_FN_EVAL(&F, beta);
      }
      gsl_vector_set(probs, j, tot);
    }
  }
   return probs;
}

/* snpProbDenom - returns denom for snpProb */
double snpProbDenom(double beta, int sampleSize){
  int i;
  double tot;
  struct my_F_params tempParams;
  gsl_function F;

  tot = 0.0;
  tempParams.n = sampleSize;
  for(i = 1; i < sampleSize ; i++){
	tempParams.i = i;
	F.function =  &my_F;
	F.params = &tempParams;
	tot += GSL_FN_EVAL(&F, beta);
      }
  return(tot);
}

/* snpProbMatrix- returns a matrix of log snpProbs. matrix is maxSampleSize+1 by maxSampleSize. rows represent
   variable sampleSizes (2 to max), columns represent freqs (1 to max -1) */
gsl_matrix *snpProbMatrix(double beta,  int maxSampleSize, gsl_vector *sampleSizeVector, gsl_matrix *sfsBools){
  gsl_vector *denoms;
  gsl_matrix *probs;
  struct my_snpProb_params FParams;
  gsl_function f;
  int i, j;

  //alloc probs
  probs = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize);
  gsl_matrix_set_zero(probs);
  //get denoms
  denoms = makeSnpProbDenomVector(beta,maxSampleSize, sampleSizeVector);

  //go through sampleSizes
  for(i = 2; i <= maxSampleSize; i++){
    //have sample size i?
    if (gsl_vector_get(sampleSizeVector, i)){
      //go through freqs
      for(j = 1; j < maxSampleSize; j++){
	//have freq j?
	if(gsl_matrix_get(sfsBools, i, j)){
	  //calc prob
	  FParams.i = j;
	  FParams.n = i;
	  FParams.denom = gsl_vector_get(denoms,i);
	  f.function = &snpProbDenomLookup;
	  f.params = &FParams;
	  gsl_matrix_set(probs, i, j, log(GSL_FN_EVAL(&f, beta)));
	}
      }
    }
  }
  gsl_vector_free(denoms);
  return(probs);
}
/* snpProbMatrix- returns a matrix of  snpProbs. matrix is maxSampleSize+1 by maxSampleSize. rows represent
   variable sampleSizes (2 to max), columns represent freqs (1 to max -1) */
gsl_matrix *snpProbMatrixNotLog(double beta,  int maxSampleSize, gsl_vector *sampleSizeVector, gsl_matrix *sfsBools){
  gsl_vector *denoms;
  gsl_matrix *probs;
  struct my_snpProb_params FParams;
  gsl_function f;
  int i, j;

  //alloc probs
  probs = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize);
  gsl_matrix_set_zero(probs);
  //get denoms
  denoms = makeSnpProbDenomVector(beta,maxSampleSize, sampleSizeVector);

  //go through sampleSizes
  for(i = 2; i <= maxSampleSize; i++){
    //have sample size i?
    if (gsl_vector_get(sampleSizeVector, i)){
      //go through freqs
      for(j = 1; j < maxSampleSize; j++){
	//have freq j?
	if(gsl_matrix_get(sfsBools, i, j)){
	  //calc prob
	  FParams.i = j;
	  FParams.n = i;
	  FParams.denom = gsl_vector_get(denoms,i);
	  f.function = &snpProbDenomLookup;
	  f.params = &FParams;
	  gsl_matrix_set(probs, i, j, GSL_FN_EVAL(&f, beta));
	}
      }
    }
  }
  gsl_vector_free(denoms);
  return(probs);
}
/* snpProbMatrixNotLogFull- returns a matrix of  snpProbs. matrix is maxSampleSize+1 by maxSampleSize. rows represent
   variable sampleSizes (2 to max), columns represent freqs (1 to max -1). All freqs/samplesizes included */
gsl_matrix *snpProbMatrixNotLogFull(double beta,  int maxSampleSize, gsl_vector *sampleSizeVector){
  gsl_vector *denoms;
  gsl_matrix *probs;
  struct my_snpProb_params FParams;
  gsl_function f;
  int i, j;

  //alloc probs
  probs = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize);
  gsl_matrix_set_zero(probs);

  //get denoms

  denoms = makeSnpProbDenomVector(beta,maxSampleSize, sampleSizeVector);

  //go through sampleSizes
  for(i = 2; i <= maxSampleSize; i++){
    //check for sampleSize
    if (gsl_vector_get(sampleSizeVector,i)){
      //go through freqs
      for(j = 1; j < maxSampleSize; j++){
	//calc prob
	FParams.i = j;
	FParams.n = i;
	FParams.denom = gsl_vector_get(denoms,i);
	f.function = &snpProbDenomLookup;
	f.params = &FParams;
	gsl_matrix_set(probs, i, j, GSL_FN_EVAL(&f, beta));
      }
    }
  }
  gsl_vector_free(denoms);
  return(probs);
}

/* snpProbVectorNotLog- returns a vector of log snpProbs at specified sampleSize columns represent freqs (1 to max -1) */
gsl_vector *snpProbVectorNotLog(double beta,  int sampleSize){
  double denom;
  gsl_vector *probs;
  struct my_snpProb_params FParams;
  gsl_function f;
  int i;

  //alloc probs
  probs = gsl_vector_alloc(sampleSize);
  gsl_vector_set_zero(probs);
  //get denoms
  denom = snpProbDenom(beta, sampleSize);

  //go through freqs
  for(i = 1; i < sampleSize; i++){
    FParams.i = i;
    FParams.n = sampleSize;
    FParams.denom = denom;
    f.function = &snpProbDenomLookup;
    f.params = &FParams;
    gsl_vector_set(probs, i, GSL_FN_EVAL(&f, beta));
  }
  return(probs);
}

/* likelihood function for SFS given the data; p here contains the data and the weights */
double my_lik(double beta, void * p){
	struct my_lik_params * params = (struct my_lik_params *) p;
	struct my_F_params FParams;
	gsl_function f;
	double lik;
	int i;

	lik = 0;
	for(i = 0; i < params->snpNumber; i++){
		FParams.i = params->data[i].i;
		FParams.n = params->data[i].n;
		f.function = &snpProb;
		f.params = &FParams;
		lik += log(GSL_FN_EVAL(&f, beta));
		}
	return -lik;
}

/* likelihood function for SFS where each SNP has independent selection coeff */
double sfsLikBetaVector(gsl_vector *betas, void * p){
	struct my_lik_params * params = (struct my_lik_params *) p;
	struct my_F_params FParams;
	gsl_function f;
	double lik;
	int i;

	lik = 0;
	for(i = 0; i < params->snpNumber; i++){
		FParams.i = params->data[i].i;
		FParams.n = params->data[i].n;
		f.function = &snpProb;
		f.params = &FParams;
		lik += log(GSL_FN_EVAL(&f, gsl_vector_get(betas, i)));
		}
	return -lik;
}

/* weighted likelihood function for SFS given the data; here contains the data and the weights */
double weightedLik(double beta, void * p){
  struct my_lik_params * params = (struct my_lik_params *) p;
  struct my_F_params FParams;
  gsl_function f;
  double lik;
  int i;

  lik = 0;
  for(i = 0; i < params->snpNumber; i++){
    FParams.i = params->data[i].i;
    FParams.n = params->data[i].n;
    f.function = &snpProb;
    f.params = &FParams;
    lik += gsl_vector_get(params->weights, i) * log(GSL_FN_EVAL(&f, beta));
  }
  return -lik;
}

/* weighted likelihood function for SFS given the data; here contains the data and the weights and uses the snpProbLookup routine */
double weightedLikLook(double beta, void * p){
  struct my_lik_params * params = (struct my_lik_params *) p;
  double lik;
  int i;
  gsl_matrix *probs;

  //make prob matrix (note this is in logs)
  probs = snpProbMatrix(beta, params->maxSampleSize, params->sampleSizeVector, params->sfsBools);
  //now go and tally likelihood
  lik = 0;
  for(i = 0; i < params->snpNumber; i++){
    lik += gsl_matrix_get(probs,params->data[i].n,params->data[i].i)  * gsl_vector_get(params->weights,i);
  }
  gsl_matrix_free(probs);
  return -lik;
}

double weightedLikLookCI(double beta, void * p){
  struct my_likCI_params *params = (struct my_likCI_params *) p;
  struct my_lik_params likParams;
  gsl_function l;
  double result;

  likParams.data = params->data;
  likParams.snpNumber = params->snpNumber;
  likParams.sampleSizeVector = params->sampleSizeVector;
  likParams.maxSampleSize = params->maxSampleSize;
  likParams.weights = params->weights;
  likParams.sfsBools = params->sfsBools;
  l.function = &weightedLikLook;
  l.params = &likParams;
  result = GSL_FN_EVAL(&l, beta) - (params->lMax) - (params->logUnits) ;
  //  printf("%f\t%f\t%f\n",params->lMax,GSL_FN_EVAL(&l, beta),result);
  return result;
}

/* wrapper for my_lik for use in mnbrak */
double likWrap(double beta){
	gsl_function f;

	f.function = &my_lik;
	f.params = NULL;
	return GSL_FN_EVAL(&f, beta);
}

/* wrapper for my_lik for use in mnbrak */
double  wlikWrap(double beta, void * p){
	gsl_function f;
	struct my_lik_params * params = (struct my_lik_params *) p;

	f.function = &weightedLik;
	f.params = params;
	return GSL_FN_EVAL(&f, beta);
}

/* wrapper for my_lik for use in mnbrak */
double  wlikWrapLook(double beta, void * p){
	gsl_function f;
	struct my_lik_params * params = (struct my_lik_params *) p;

	f.function = &weightedLikLook;
	f.params = params;
	return GSL_FN_EVAL(&f, beta);
}

double  wlikCIWrapLook(double beta, void * p){
	gsl_function f;
	struct my_likCI_params * params = (struct my_likCI_params *) p;

	f.function = &weightedLikLookCI;
	f.params = params;
	return GSL_FN_EVAL(&f, beta);
}

/*error checking function for likelihood function */
void printProfile(double min, double max, void * p){
  gsl_function f;
  struct my_lik_params *params =  (struct my_lik_params *) p;
  int i;

  f.function = &weightedLikLook;
  f.params = params;
  for(i = min; i < max; i++){
    printf("%d\t%f\n",i, GSL_FN_EVAL(&f, i));
  }

}

/* get ML estimate of beta using likelihood function; BRENT method for gsl */
double ml_est(double * lik_beta_hat){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_min_fminimizer_type *T;
	gsl_min_fminimizer *s;
	double m, a, b, ax, bx, cx, fa, fb, fc;
	gsl_function L;

	/* get minimum bracket */
	ax = -10.0;
	bx = 0.0;
	cx = 10.0;

	mnbrak(&ax, &bx, &cx, &fa, &fb, &fc, &my_lik);

	/* check if there is a minimum, if not return inf */
	if (fa == fb || fc == fb || ax > cx){
		*lik_beta_hat = fa;
		return 666.0;
		}
	else{

		/* do mimization */

		L.function = &my_lik;
		//should have param line here but no params....

		T = gsl_min_fminimizer_brent;
		s = gsl_min_fminimizer_alloc(T);
		gsl_min_fminimizer_set(s, &L, bx, ax, cx);

		do{
			iter++;
			status = gsl_min_fminimizer_iterate(s);
			 m = gsl_min_fminimizer_x_minimum (s);
			  a = gsl_min_fminimizer_x_lower (s);
			  b = gsl_min_fminimizer_x_upper (s);

			  status = gsl_min_test_interval (a, b, 0.001, 0.0);

		}
	  while (status == GSL_CONTINUE && iter < max_iter);
	  gsl_min_fminimizer_free(s);

	  return m;
	}
}

/* get weighted ML estimate of beta using likelihood function; BRENT method for gsl */
double weighted_ml_est(double * lik_beta_hat, void * p){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_min_fminimizer_type *T;
	gsl_min_fminimizer *s;
	double m, a, b, ax, bx, cx, fa, fb, fc, min;
	gsl_function L;
	struct my_lik_params * params = (struct my_lik_params *) p;

	// get minimum bracket
	ax = -10.0;
	bx = 1.0;
	cx = 10;
	L.function = &weightedLik;
	L.params = params;
	mnbrak2(&ax, &bx, &cx, &fa, &fb, &fc, &wlikWrap, bx, params);

	//check if there is a minimum, if not return 666
	if (fa == fb || fc == fb || ax > cx){
	  min = fa;
	  if(fb < min){
	    min = fb;
	  }
	  if(fc < min){
	    min = fc;
	  }
	  *lik_beta_hat = min;

	  return 666.0;
	}
	else{
	  // do mimization

	  //initialize lik function
	  L.function = &weightedLik;
	  L.params = params;

	  //min routine
	  T = gsl_min_fminimizer_brent;
	  s = gsl_min_fminimizer_alloc(T);
	  gsl_min_fminimizer_set(s, &L, bx, ax, cx);
	  do{
	    iter++;
	    status = gsl_min_fminimizer_iterate(s);
	    m = gsl_min_fminimizer_x_minimum (s);
	    a = gsl_min_fminimizer_x_lower (s);
	    b = gsl_min_fminimizer_x_upper (s);
	    status = gsl_min_test_interval (a, b, 0.001, 0.0);
	  }
	  while (status == GSL_CONTINUE && iter < max_iter);
	  *lik_beta_hat = gsl_min_fminimizer_f_minimum(s);
	  gsl_min_fminimizer_free(s);
	  return m;
	}
}

double weighted_ml_est_lookup(double * lik_beta_hat, void * p){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_min_fminimizer_type *T;
	gsl_min_fminimizer *s;
	double m, a, b, ax, bx, cx, fa, fb, fc, dummy;
	gsl_function L;
	struct my_lik_params * params = (struct my_lik_params *) p;

	// get minimum bracket
	ax = -20.0;
	bx = -1.0;
	cx = 20;
	L.function = &weightedLikLook;
	L.params = params;
	mnbrak2(&ax, &bx, &cx, &fa, &fb, &fc, &wlikWrapLook, bx, params);
	//swap bounds if needed
	if (cx < ax){
	  dummy = cx;
	  cx = ax;
	  ax = dummy;
	}

	// do mimization

	//initialize lik function
	L.function = &weightedLikLook;
	L.params = params;

	//min routine
	T = gsl_min_fminimizer_brent;
	s = gsl_min_fminimizer_alloc(T);
	gsl_min_fminimizer_set(s, &L, bx, ax, cx);
	do{
	  iter++;
	  status = gsl_min_fminimizer_iterate(s);
	  m = gsl_min_fminimizer_x_minimum (s);
	  a = gsl_min_fminimizer_x_lower (s);
	  b = gsl_min_fminimizer_x_upper (s);
	  status = gsl_min_test_interval (a, b, 0.001, 0.0);
	}
	while (status == GSL_CONTINUE && iter < max_iter);
	*lik_beta_hat = gsl_min_fminimizer_f_minimum(s);
	gsl_min_fminimizer_free(s);
	return m;
}

/* same as above but outputs 666 on minimization error */
double weighted_ml_est_lookup_errReport(double * lik_beta_hat, void * p){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_min_fminimizer_type *T;
	gsl_min_fminimizer *s;
	double m, a, b, ax, bx, cx, fa, fb, fc, dummy;
	gsl_function L;
	struct my_lik_params * params = (struct my_lik_params *) p;

	// get minimum bracket
	ax = -20.0;
	bx = -1.0;
	cx = 20;
	L.function = &weightedLikLook;
	L.params = params;
	mnbrak2(&ax, &bx, &cx, &fa, &fb, &fc, &wlikWrapLook, bx, params);
	//swap bounds if needed
	if (cx < ax){
	  dummy = cx;
	  cx = ax;
	  ax = dummy;
	}
	//check for error
	if (fb >= fc || fb >= fa){
	  return(666.0);
	}

	// do mimization

	//initialize lik function
	L.function = &weightedLikLook;
	L.params = params;

	//min routine
	T = gsl_min_fminimizer_brent;
	s = gsl_min_fminimizer_alloc(T);
	gsl_set_error_handler_off ();
	status = gsl_min_fminimizer_set(s, &L, bx, ax, cx);
	if (status == GSL_EINVAL){
	  return(666);
	}
	do{
	  iter++;
	  status = gsl_min_fminimizer_iterate(s);
	  m = gsl_min_fminimizer_x_minimum (s);
	  a = gsl_min_fminimizer_x_lower (s);
	  b = gsl_min_fminimizer_x_upper (s);
	  status = gsl_min_test_interval (a, b, 0.001, 0.0);
	}
	while (status == GSL_CONTINUE && iter < max_iter);
	*lik_beta_hat = gsl_min_fminimizer_f_minimum(s);
	gsl_min_fminimizer_free(s);
	return m;
}

/* get weighted ML lower CI beta using weightedLikLookCI method; BRENT method for gsl */
double weighted_ml_CILower_lookup(double * lik_beta_hat, void * p){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_root_fsolver_type *T;
	gsl_root_fsolver *s;
	double m, a, b, ax, cx;
	gsl_function L;
	struct my_likCI_params * params = (struct my_likCI_params *) p;

	// get minimum bracket
	ax = -50.0;
	cx = params->beta_hat;

	// do root finding
	//initialize lik function
	L.function = &weightedLikLookCI;
	L.params = params;

	//min routine
	T = gsl_root_fsolver_brent;
	s = gsl_root_fsolver_alloc(T);
	gsl_root_fsolver_set(s, &L, ax, cx);
	do{
	  iter++;
	  status = gsl_root_fsolver_iterate(s);
	  m = gsl_root_fsolver_root(s);
	  a = gsl_root_fsolver_x_lower(s);
	  b = gsl_root_fsolver_x_upper(s);
	  status = gsl_min_test_interval (a, b, 0.001, 0.0);
	}
	while (status == GSL_CONTINUE && iter < max_iter);
	gsl_root_fsolver_free(s);
	return m;
}

/* get weighted ML upper CI beta using weightedLikLookCI method; BRENT method for gsl */
double weighted_ml_CIUpper_lookup(double * lik_beta_hat, void * p){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_root_fsolver_type *T;
	gsl_root_fsolver *s;
	double m, a, b, ax, cx;
	gsl_function L;
	struct my_likCI_params * params = (struct my_likCI_params *) p;

	// get minimum bracket
	cx = 100.0;
	ax = params->beta_hat;

	// do root finding
	//initialize lik function
	L.function = &weightedLikLookCI;
	L.params = params;

	//min routine
	T = gsl_root_fsolver_brent;
	s = gsl_root_fsolver_alloc(T);
	gsl_root_fsolver_set(s, &L, ax, cx);
	do{
	  iter++;
	  status = gsl_root_fsolver_iterate(s);
	  m = gsl_root_fsolver_root(s);
	  a = gsl_root_fsolver_x_lower(s);
	  b = gsl_root_fsolver_x_upper(s);
	  status = gsl_min_test_interval (a, b, 0.001, 0.0);
	}
	while (status == GSL_CONTINUE && iter < max_iter);
	gsl_root_fsolver_free(s);
	return m;
}

/* summarizeSFS- returns a matrix of counts which summarize the SFS conditional
   upon sampleSize. rows = sampleSize, column = freqs */

gsl_matrix *summarizeSFS(int maxSampleSize, struct snp data[], int snpNumber){
  gsl_matrix *counts;
  int i;

  //alloc matrix of size maxSampleSize by maxSampleSize
  counts = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize + 1);
  gsl_matrix_set_zero(counts);
  assert(counts != NULL);

  gsl_matrix_set_zero(counts);

  //go through data, fill up matrix
  for(i = 0; i < snpNumber; i++){
    gsl_matrix_set(counts, data[i].n, data[i].i, gsl_matrix_get(counts, data[i].n, data[i].i) + 1);
  }
  return(counts);

}

/* summarizeSFS- returns a matrix of booleans which summarize the SFS conditional
   upon sampleSize. rows = sampleSize, column = freqs */

gsl_matrix *summarizeSFSBool(int maxSampleSize, struct snp data[], int snpNumber){
  gsl_matrix *counts;
  int i;

  //alloc matrix of size maxSampleSize by maxSampleSize
  counts = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize + 1);
  gsl_matrix_set_zero(counts);
  assert(counts != NULL);

  gsl_matrix_set_zero(counts);

  //go through data, fill up matrix
  for(i = 0; i < snpNumber; i++){
    gsl_matrix_set(counts, data[i].n, data[i].i, 1);
  }
  return(counts);

}

/* simulateSiteFreq- returns an int representing the frequency of
   of a site (unfolded), i, in sampleSize n, conditional on sel.
   coeff. beta , also needs a pointer to a gsl random number generator */

int simulateSiteFreq(int sampleSize, double alpha, void *r){
  int i;
  double sum, probs[sampleSize - 1], rand;
  gsl_function p;
  struct my_F_params simParams;
  gsl_rng * rn = (gsl_rng *) r;

  simParams.n = sampleSize;
  /* put probs in vector */
  for(i = 0; i < sampleSize - 1; i++){
    simParams.i = i + 1;
    p.function = &snpProb;
    p.params = &simParams;
    probs[i] = GSL_FN_EVAL(&p, alpha);
  }

  /* choose frequency */
  rand = gsl_rng_uniform(rn);
  sum = 0;
  for(i = 0; i < sampleSize - 1; i++){
    sum += probs[i];
    if (rand <= sum){
      return(i+1);
    }
  }
  return(666);
}

/* ascertainment stuff -- following Nielsen et al. 2004  */

/* simpleAscertain--  Nielsen et al. 2004 eqn 2 *, returns the prob of ascertainment
given a sample freq */
double simpleAscertain(double sampleFreq, void * p){
  struct my_F_params * params = (struct my_F_params *) p;
  double sampleSize, ascSize,tmpSampleFreq;
  double num1 = 0.0;
  double num2 = 0.0;

  sampleSize = params->n;
  ascSize = params->ascSize;
  tmpSampleFreq = (int) sampleFreq;

  //test for weirdness

  if (sampleFreq < ascSize){
    num1 = 0;
  }
  else{
    num1 = gsl_sf_choose(sampleFreq, ascSize);
  }

  if ((sampleSize -sampleFreq) < ascSize){
    num2 = 0;
  }
  else{
    num2 =  gsl_sf_choose(sampleSize - sampleFreq, ascSize);
  }

  return(1.0 - ((num1 + num2) / gsl_sf_choose(sampleSize, ascSize)));
}

/* outgroupAscertain-- equivalent to Nielsen et al. 2004 eqn 2,
 returns the prob of ascertainment given a sample freq, but
is for use when ascertainment was based on single outgroup comparisons
(e.g. divergence between reference genomes). specifically this
calculates the prob of sampling only ancestral alleles.
IMPORTANT- sampleFreq here is derived allele freq */
double outgroupAscertain(double sampleFreq, void * p){
  struct my_F_params * params = (struct my_F_params *) p;
  double sampleSize, ascSize;
  double num = 0.0;

  sampleSize = params->n;
  ascSize = params->ascSize;

  //test for weirdness
  if ((sampleSize - sampleFreq) < ascSize){
    num = 0;
  }
  else{
    num =  gsl_sf_choose((sampleSize - sampleFreq), ascSize);
  }

  return(num / gsl_sf_choose(sampleSize, ascSize));
}

/* probAscertainmentGivenModel-- Nielsen et al. 2005 eqn 8
   returns the prob of ascertainment given the model */
double probAscertainmentGivenModel(double beta, void *p){
  struct my_F_params * params = (struct my_F_params *) p;
  struct my_F_params tempParams;

  long double tot, tmpres, tmpres2;
  int i=1;
  gsl_function F, G;

  /* add up total prob space */
  tot = 0.0;
  tempParams.n = params->n;
  tempParams.ascSize = params->ascSize;

  for(i = 1; i < params->n ; i++){
    tempParams.i = i;

    F.function =  &(snpProb);
    F.params = &tempParams;
    tmpres = 0.0;
    tmpres = GSL_FN_EVAL(&F, beta);
    G.function = &simpleAscertain;
    G.params = &tempParams;
    tmpres2 = GSL_FN_EVAL(&G, i) ;
    tot += tmpres * tmpres2;

  }

  return(tot) ;
}

/* probAscertainmentGivenModel-- Nielsen et al. 2005 eqn 8
   returns the prob of ascertainment given the model */
double probOutgroupAscertainmentGivenModel(double beta, void *p){
  struct my_F_params * params = (struct my_F_params *) p;
  struct my_F_params tempParams;

  long double tot, tmpres, tmpres2;
  int i=1;
  gsl_function F, G;

  /* add up total prob space */
  tot = 0.0;
  tempParams.n = params->n;
  tempParams.ascSize = params->ascSize;

  for(i = 1; i < params->n ; i++){
    tempParams.i = i;

    F.function =  &(snpProb);
    F.params = &tempParams;
    tmpres = 0.0;
    tmpres = GSL_FN_EVAL(&F, beta);
    G.function = &outgroupAscertain;
    G.params = &tempParams;
    tmpres2 = GSL_FN_EVAL(&G, i) ;
    tot += tmpres * tmpres2;

  }

  return(tot) ;
}

/* probAscertainmentGivenModelLookup-- Nielsen et al. 2005 eqn 8
   returns the prob of ascertainment given the model, lookup version
   requires snpProb matrix and ascProb matrix */
double probAscertainmentGivenModelLookup(double beta, int sampSize, gsl_matrix *snpProbs, gsl_matrix *ascProbs){

  long double tot;
  int i=1;

  /* add up total prob space */
  tot = 0.0;

  for(i = 1; i < sampSize ; i++){
    tot += gsl_matrix_get(snpProbs, sampSize, i) * gsl_matrix_get(ascProbs, sampSize, i);
  }
  return(tot) ;
}

/* probAscertainmentGivenModelHemiLookup-- same
   as above but expects vector not matrix of snpProbs */
double probAscertainmentGivenModelHemiLookup(double beta, int sampSize, gsl_vector *snpProbs, gsl_matrix *ascProbs){

  long double tot;
  int i=1;

  /* add up total prob space */
  tot = 0.0;

  for(i = 1; i < sampSize ; i++){
    tot += gsl_vector_get(snpProbs, i) * gsl_matrix_get(ascProbs, sampSize, i);
  }
  return(tot) ;
}

/* ascSize vector - returns a vector of 1s or 0s depending on ascertainment sample sizes in the data */
gsl_vector *ascSizeVector(struct snp data[], int snpNumber, int maxSampleSize){
  gsl_vector *bools;
  int i;

  //alloc and initialize bools vector
  bools = gsl_vector_alloc(maxSampleSize + 1);
  gsl_vector_set_zero(bools);

  //go through data, set each sampleSize value to 1
  for(i = 0; i < snpNumber; i++){
    gsl_vector_set(bools,data[i].ascSize, 1);
  }
  return(bools);
}

/* snpAscMatrix- returns a matrix of simpleAscertainment probs. matrix is maxSampleSize+1 by maxSampleSize. rows represent
   variable sampleSizes (2 to max), columns represent freqs (1 to max -1). Currently this only supports a single ascertainment size  */
gsl_matrix *snpAscMatrix(int maxSampleSize, gsl_vector *sampleSizeVector, gsl_matrix *sfsBools, \
		 int ascSize){
  gsl_matrix *probs;
  struct my_F_params FParams;
  gsl_function f;
  int i, j;

  //alloc probs
  probs = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize);
  gsl_matrix_set_zero(probs);
  //go through sampleSizes
  for(i = 2; i <= maxSampleSize; i++){
    //have sample size i?
    if (gsl_vector_get(sampleSizeVector, i)){
      //go through freqs
      for(j = 1; j < maxSampleSize; j++){
	//have freq j?
	if(gsl_matrix_get(sfsBools, i, j)){
	  //calc prob
	  FParams.i = j;
	  FParams.n = i;
	  FParams.ascSize = ascSize;
	  f.function = &simpleAscertain;
	  f.params = &FParams;
	  gsl_matrix_set(probs, i, j, GSL_FN_EVAL(&f, j));
	}
      }
    }
  }
  return(probs);
}

/* snpOutgroupAscMatrix- returns a matrix of outgroupAscertainment probs. matrix is maxSampleSize+1 by maxSampleSize. rows represent
   variable sampleSizes (2 to max), columns represent freqs (1 to max -1). Currently this only supports a single ascertainment size  */
gsl_matrix *snpOutgroupAscMatrix(int maxSampleSize, gsl_vector *sampleSizeVector, int ascSize){
  gsl_matrix *probs;
  struct my_F_params FParams;
  gsl_function f;
  int i, j;

  //alloc probs
  probs = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize);
  gsl_matrix_set_zero(probs);
  //go through sampleSizes
  for(i = 2; i <= maxSampleSize; i++){
    //have sample size i?
    if (gsl_vector_get(sampleSizeVector, i)){
      //go through freqs
      for(j = 1; j < i; j++){
	//calc prob
	FParams.i = j;
	FParams.n = i;
	FParams.ascSize = ascSize;
	f.function = &outgroupAscertain;
	f.params = &FParams;
	gsl_matrix_set(probs, i, j, GSL_FN_EVAL(&f, j));
      }
    }
  }
  return(probs);
}

/*estimateAscSFS-- ML estimates of SFS, given ascertainment. returns
 a matrix of probs. matrix is maxSampleSize+1 by maxSampleSize. rows represent
variable sampleSizes (2 to max), columns represent freqs (1 to max -1). Currently this only supports a single ascertainment size */

gsl_matrix *estimateAscSFS(int maxSampleSize, gsl_vector *sampleSizeVector, gsl_matrix *sfsBools, \
			   int ascSize, gsl_matrix *sfsSummary){
  gsl_matrix *probs, *ascProbs;
    int i, j;
  double ssTmp, p_hat;
  double xij;

  //alloc probs
  probs = gsl_matrix_alloc(maxSampleSize + 1, maxSampleSize);
  gsl_matrix_set_zero(probs);
  //calculate ascProbs
  ascProbs = snpAscMatrix(maxSampleSize, sampleSizeVector, sfsBools, \
			  ascSize);

  //go through sampleSizes
  for(i = 2; i <= maxSampleSize; i++){
    //have sample size i?
    if (gsl_vector_get(sampleSizeVector, i)){
      //go through freqs to tally up denom at sampleSize
      ssTmp = 0.0;
      for(j = 1; j < maxSampleSize; j++){
	xij = gsl_matrix_get(sfsSummary, i, j);
	if (xij > 0){
	  ssTmp += xij / gsl_matrix_get(ascProbs, i, j);
	}
      }
      //go back through freqs to assign p_hats at sampleSize
      for(j = 1; j < maxSampleSize; j++){
	xij = gsl_matrix_get(sfsSummary, i, j);
	p_hat = (double) xij / gsl_matrix_get(ascProbs, i, j);
	gsl_matrix_set(probs, i, j, p_hat / ssTmp);
      }
    }
  }
  return(probs);
}

/* weighted likelihood function for SFS given the data and ascertainment; here contains the data and the weights and uses the snpProbLookup routine */
double weightedLikLookAsc(double beta, void * p){
  struct my_lik_params * params = (struct my_lik_params *) p;
  double lik, num;
  int i;
  gsl_matrix *probs, *ascProbs;
  gsl_vector *ascDenoms;

  //make prob matrix (note this is in logs)
  probs = snpProbMatrix(beta, params->maxSampleSize, params->sampleSizeVector, params->sfsBools);
  //make ascProb matrix (NOT in logs!)
  ascProbs = snpAscMatrix(params->maxSampleSize, params->sampleSizeVector, params->sfsBools, params->ascSize);

  //make vector of denoms
  ascDenoms = gsl_vector_alloc(params->maxSampleSize + 1);
  gsl_vector_set_zero(ascDenoms);
  for(i = params->ascSize; i <= params->maxSampleSize; i++){
     gsl_vector_set(ascDenoms,i,probAscertainmentGivenModelLookup(beta, i, probs, ascProbs));
  }

  //now go and tally likelihood
  lik = 0;
  num = 0;
  for(i = 0; i < params->snpNumber; i++){
    lik += gsl_matrix_get(probs,params->data[i].n,params->data[i].i)	\
      + log(gsl_matrix_get(ascProbs,params->data[i].n,params->data[i].i)) \
      - log(gsl_vector_get(ascDenoms, params->data[i].n));
     }
  gsl_matrix_free(probs);
  gsl_matrix_free(ascProbs);
  gsl_vector_free(ascDenoms);

  return -lik;
}

/* weighted likelihood function for SFS given the data and outgroup ascertainment; here contains the data and the weights and uses the snpProbLookup routine */
double weightedLikLookOutgroupAsc(double beta, void * p){
  struct my_lik_params * params = (struct my_lik_params *) p;
  double lik, num;
  int i;
  gsl_matrix *probs, *ascProbs;
  gsl_vector *ascDenoms;

  //make prob matrix
  probs = snpProbMatrixNotLogFull(beta, params->maxSampleSize, params->sampleSizeVector);
  //make ascProb matrix
  ascProbs = snpOutgroupAscMatrix(params->maxSampleSize, params->sampleSizeVector, params->ascSize);

  //make vector of denoms
  ascDenoms = gsl_vector_alloc(params->maxSampleSize + 1);
  gsl_vector_set_zero(ascDenoms);
  for(i = params->ascSize; i <= params->maxSampleSize; i++){
     gsl_vector_set(ascDenoms,i,probAscertainmentGivenModelLookup(beta, i, probs, ascProbs));
  }

  //now go and tally likelihood
  lik = 0;
  num = 0;
  for(i = 0; i < params->snpNumber; i++){
    num = (gsl_matrix_get(probs,params->data[i].n,params->data[i].i) * gsl_matrix_get(ascProbs,params->data[i].n,params->data[i].i)) \
      / gsl_vector_get(ascDenoms, params->data[i].n);
    lik += log(num);
    //printf("first: %f\t second: %f\t third: %f\n",gsl_matrix_get(probs,params->data[i].n,params->data[i].i),gsl_matrix_get(ascProbs,params->data[i].n,params->data[i].i),gsl_vector_get(ascDenoms, params->data[i].n));
    }

  gsl_matrix_free(probs);
  gsl_matrix_free(ascProbs);
  gsl_vector_free(ascDenoms);

  return -lik;
}

/* wrapper for my_lik for use in mnbrak */
double  wlikWrapLookAsc(double beta, void * p){
	gsl_function f;
	struct my_lik_params * params = (struct my_lik_params *) p;

	f.function = &weightedLikLookAsc;
	f.params = params;
	return GSL_FN_EVAL(&f, beta);
}

/* wrapper for my_lik for use in mnbrak */
double  wlikWrapLookOutgroupAsc(double beta, void * p){
	gsl_function f;
	struct my_lik_params * params = (struct my_lik_params *) p;

	f.function = &weightedLikLookOutgroupAsc;
	f.params = params;
	return GSL_FN_EVAL(&f, beta);
}

/* get weighted ML estimate of beta using likelihood function (lookup) conditional on ascertainment
; BRENT method for gsl */
double weighted_ml_est_lookup_asc(double * lik_beta_hat, void * p){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_min_fminimizer_type *T;
	gsl_min_fminimizer *s;
	double m, a, b, ax, bx, cx, fa, fb, fc, dummy;
	gsl_function L;
	struct my_lik_params * params = (struct my_lik_params *) p;

	// get minimum bracket
	ax = -20.0;
	bx = -1.0;
	cx = 20;
	L.function = &weightedLikLookAsc;
	L.params = params;
	mnbrak2(&ax, &bx, &cx, &fa, &fb, &fc, &wlikWrapLookAsc, bx, params);
	//swap bounds if needed
	if (cx < ax){
	  dummy = cx;
	  cx = ax;
	  ax = dummy;
	}
	// do mimization

	//initialize lik function
	L.function = &weightedLikLookAsc;
	L.params = params;

	//min routine
	T = gsl_min_fminimizer_brent;
	s = gsl_min_fminimizer_alloc(T);
	gsl_min_fminimizer_set(s, &L, bx, ax, cx);
	do{
	  iter++;
	  status = gsl_min_fminimizer_iterate(s);
	  m = gsl_min_fminimizer_x_minimum (s);
	  a = gsl_min_fminimizer_x_lower (s);
	  b = gsl_min_fminimizer_x_upper (s);
	  status = gsl_min_test_interval (a, b, 0.001, 0.0);
	}
	while (status == GSL_CONTINUE && iter < max_iter);
	*lik_beta_hat = gsl_min_fminimizer_f_minimum(s);
	gsl_min_fminimizer_free(s);
	return m;
}

/* get weighted ML estimate of beta using likelihood function (lookup) conditional on outgroup ascertainment
; BRENT method for gsl */
double weighted_ml_est_lookup_outgroup_asc(double * lik_beta_hat, void * p){
	int status;
	int iter = 0;
	int max_iter = 100;
	const gsl_min_fminimizer_type *T;
	gsl_min_fminimizer *s;
	double m, a, b, ax, bx, cx, fa, fb, fc, dummy;
	gsl_function L;
	struct my_lik_params * params = (struct my_lik_params *) p;

	// get minimum bracket
	ax = -20.0;
	bx = -1.0;
	cx = 20;
	L.function = &weightedLikLookOutgroupAsc;
	L.params = params;
	mnbrak2(&ax, &bx, &cx, &fa, &fb, &fc, &wlikWrapLookOutgroupAsc, bx, params);
	//swap bounds if needed
	if (cx < ax){
	  dummy = cx;
	  cx = ax;
	  ax = dummy;
	}
	// do mimization

	//initialize lik function
	L.function = &weightedLikLookOutgroupAsc;
	L.params = params;

	//min routine
	T = gsl_min_fminimizer_brent;
	s = gsl_min_fminimizer_alloc(T);
	gsl_min_fminimizer_set(s, &L, bx, ax, cx);
	do{
	  iter++;
	  status = gsl_min_fminimizer_iterate(s);
	  m = gsl_min_fminimizer_x_minimum (s);
	  a = gsl_min_fminimizer_x_lower (s);
	  b = gsl_min_fminimizer_x_upper (s);
	  status = gsl_min_test_interval (a, b, 0.001, 0.0);
	}
	while (status == GSL_CONTINUE && iter < max_iter);
	*lik_beta_hat = gsl_min_fminimizer_f_minimum(s);
	gsl_min_fminimizer_free(s);
	return m;
}

/* likelihood function for SFS where each SNP has
 independent selection coeff and outgroup ascertainment */
double sfsLikBetaVectorOutgroupAsc(gsl_vector *betas, void * p){
  struct my_lik_params * params = (struct my_lik_params *) p;
  double pSNP, pAsc, pAscModel,lik, tmp;
  int i;
  gsl_matrix *ascProbs;
  gsl_vector *snpProbs;

  lik = 0;
  //make ascProb matrix
  ascProbs = snpOutgroupAscMatrix(params->maxSampleSize, params->sampleSizeVector, \
				  params->ascSize);
  //go through snps
  for(i = 0; i < params->snpNumber; i++){
    //create vector of snpProbs, find pSNP_i
    snpProbs = snpProbVectorNotLog(gsl_vector_get(betas, i), params->data[i].n);
    pSNP = gsl_vector_get(snpProbs, params->data[i].i);

    //lookup pAsc_i
    pAsc = gsl_matrix_get(ascProbs, params->data[i].n, params->data[i].i);

    //get pAscModel
    pAscModel = probAscertainmentGivenModelHemiLookup(gsl_vector_get(betas, i),params->data[i].n, \
						      snpProbs, ascProbs);
    tmp = pSNP * pAsc / pAscModel;
    lik += log(tmp);
    gsl_vector_free(snpProbs);
  }
  gsl_matrix_free(ascProbs);
  return -lik;
}


#ifndef __MATH_LOGSPACE_H__
#define __MATH_LOGSPACE_H__
#include <cmath>
#include <vector>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_sf_gamma.h>
#include "logspace_base.h"
#include "specialfunc.h"

// Given two log vectors, log a_i and log b_i, compute
// log sum (a_i * b_i).
double log_dot_product(const gsl_vector* log_a, const gsl_vector* log_b);

// Given a log vector, log a_i, compute log sum a_i.  Returns the sum.
double log_normalize(gsl_vector* x);

// Compute the log sum over all elements in the vector
double log_sum(const gsl_vector* x);

// Given a log matrix, log a_i, compute log sum a_i.  Returns the sum.
double log_normalize_matrix(gsl_matrix* x);

double log_dirichlet_likelihood(const double sum,
                                const double prior_sum,
                                const std::vector<int>& counts,
                                bool debug = false);

double log_dirichlet_likelihood(const double sum,
                                const double prior_scale,
                                const gsl_vector* prior,
                                const std::vector<int>& counts);

#endif  // __MATH_LOGSPACE_H__


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

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

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

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

#ifdef USE_NNPACK
#include <nnpack.h>
#endif

#ifndef OMP_THREAD
#define OMP_THREAD omp_get_max_threads()
#endif

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

enum LayerType
{
    CONVOLUTIONAL,
    DECONVOLUTIONAL,
    CONNECTED,
    MAXPOOL,
    LOCAL_AVGPOOL,
    GLOBAL_AVGPOOL,
    SOFTMAX,
    CROP,
    ROUTE,
    NORMALIZATION,
    AVGPOOL,
    ACTIVE,
    BATCHNORM,
    NETWORK,
    YOLOV3,
    YOLOV3_OUT,
    GAUSSIAN_YOLO,
    UPSAMPLE,
    L2NORM,
    EMPTY,
    BLANK,
    CONFIG,
    RES_BLOCK,
    RES_2_BLOCK,
    CONCAT_BLOCK,
    ADD_BLOCK,
    PADDING
};

enum WeightsType
{
    NO_WEIGHTS,
    PER_FEATURE,
    PER_CHANNEL
};

enum WeightsNorm
{
    NO_NORM,
    RELU_NORM,
    SOFTMAX_NORM
};

#endif