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

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_permutation.h>

static int
test_LU_band_decomp_eps(const size_t p, const size_t q, const gsl_matrix * A, const double eps, const char * desc)
{
  int s = 0;
  const size_t M = A->size1;
  const size_t N = A->size2;
  const size_t minMN = GSL_MIN(M, N);
  size_t i, j;

  gsl_matrix * AB = gsl_matrix_alloc(N, 2*p + q + 1);
  gsl_vector_uint * piv = gsl_vector_uint_alloc(minMN);
  gsl_matrix * L = gsl_matrix_alloc(M, minMN);
  gsl_matrix * U = gsl_matrix_alloc(minMN, N);
  gsl_matrix * B = gsl_matrix_alloc(M, N);

  /* convert A to packed banded format */
  gen2band_matrix(p, q, A, AB);

  s += gsl_linalg_LU_band_decomp(M, p, q, AB, piv);
  s += gsl_linalg_LU_band_unpack(M, p, q, AB, piv, L, U);

  /* compute B = L U */
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, L, U, 0.0, B);

  for (i = 0; i < M; ++i)
    {
      for (j = 0; j < N; ++j)
        {
          double aij = gsl_matrix_get(A, i, j);
          double bij = gsl_matrix_get(B, i, j);

          gsl_test_rel(bij, aij, eps, "%s (M=%lu,N=%lu)(p=%lu,q=%lu)[%lu,%lu]: %22.18g   %22.18g\n",
                       desc, M, N, p, q, i, j, aij, bij);
        }
    }

  gsl_matrix_free(AB);
  gsl_vector_uint_free(piv);
  gsl_matrix_free(L);
  gsl_matrix_free(U);
  gsl_matrix_free(B);

  return s;
}

static int
test_LU_band_decomp(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 20;
  gsl_matrix * A = gsl_matrix_alloc(N_max, N_max);
  size_t M, N, p, q;

  for (M = 1; M <= N_max; ++M)
    {
      for (N = 1; N <= N_max; ++N)
        {
          gsl_matrix_view m = gsl_matrix_submatrix(A, 0, 0, M, N);

          for (p = 0; p < GSL_MIN(M, 10); ++p)
            {
              for (q = 0; q < GSL_MIN(N, 10); ++q)
                {
                  create_band_matrix(p, q, &m.matrix, r);
                  s += test_LU_band_decomp_eps(p, q, &m.matrix, 1.0e5 * GSL_MAX(M,N) * GSL_DBL_EPSILON, "LU_band_decomp random");
                }
            }
        }
    }

  gsl_matrix_free(A);

  return s;
}

static int
test_LU_band_solve_eps(const size_t p, const size_t q, const gsl_matrix * A,
                       const gsl_vector * rhs, const gsl_vector * sol,
                       const double eps, const char * desc)
{
  int s = 0;
  const size_t N = A->size2;
  size_t i;

  gsl_matrix * AB = gsl_matrix_alloc(N, 2*p + q + 1);
  gsl_vector_uint * piv = gsl_vector_uint_alloc(N);
  gsl_matrix * L = gsl_matrix_alloc(N, N);
  gsl_matrix * U = gsl_matrix_alloc(N, N);
  gsl_vector * x = gsl_vector_alloc(N);

  /* convert A to packed banded format */
  gen2band_matrix(p, q, A, AB);

  s += gsl_linalg_LU_band_decomp(N, p, q, AB, piv);
  s += gsl_linalg_LU_band_solve(p, q, AB, piv, rhs, x);

  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(sol, i);

      gsl_test_rel(xi, yi, eps, "%s: %3lu[%lu]: %22.18g    %22.18g\n",
                   desc, N, i, xi, yi);
    }

  gsl_matrix_free(AB);
  gsl_vector_uint_free(piv);
  gsl_matrix_free(L);
  gsl_matrix_free(U);
  gsl_vector_free(x);

  return s;
}

static int
test_LU_band_solve(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 20;
  gsl_matrix * A = gsl_matrix_alloc(N_max, N_max);
  gsl_vector * b = gsl_vector_alloc(N_max);
  gsl_vector * x = gsl_vector_alloc(N_max);
  size_t N, p, q;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix_view m = gsl_matrix_submatrix(A, 0, 0, N, N);
      gsl_vector_view sol = gsl_vector_subvector(x, 0, N);
      gsl_vector_view rhs = gsl_vector_subvector(b, 0, N);

      for (p = 0; p < GSL_MIN(N, 10); ++p)
        {
          for (q = 0; q < GSL_MIN(N, 10); ++q)
            {
              create_band_matrix(p, q, &m.matrix, r);
              create_random_vector(&sol.vector, r);
              gsl_blas_dgemv(CblasNoTrans, 1.0, &m.matrix, &sol.vector, 0.0, &rhs.vector);

              s += test_LU_band_solve_eps(p, q, &m.matrix, &rhs.vector, &sol.vector,
                                          1.0e8 * N * GSL_DBL_EPSILON, "LU_band_solve random");
            }
        }
    }

  gsl_matrix_free(A);
  gsl_vector_free(b);
  gsl_vector_free(x);

  return s;
}


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

#ifndef _her2k_h
#define _her2k_h

/// @file her2k.h Performs multiplication of two complex matrices resulting in a Hermitian matrix.

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

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

   template <typename real_t>
   int HER2K(char uplo, char trans, int_t n, int_t k, complex<real_t> alpha, complex<real_t> *A, int_t ldA, complex<real_t> *B, int_t ldB, real_t beta, complex<real_t> *C, int_t ldC)
   {
      using std::toupper;
      uplo=toupper(uplo);
      trans=toupper(trans);
      using std::real;
      using std::conj;
      const complex<real_t> zero(0.0,0.0);
      const complex<real_t> one(1.0,0.0);
      int_t i,j,l;
      complex<real_t> *a,*b,*c,*at,*bt;
      complex<real_t> s,t;

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

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

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

#ifdef __latl_cblas
#include <cblas.h>

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

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

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

      return 0;
   }

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

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

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

      return 0;
   }
#endif

}
#endif


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

#include "stdio.h"
#include <math.h>
#include "snpFile.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 -- stationary solution for snp frequency density*/
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;
  size_t size= 1000;

  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;
    //adaptive integration-- slow and steady
//	gsl_integration_qags(&f,0.0,1.0,1e-5,1e-5, 100000,w, &result,&error);
	//non-adaptive version is rougher and faster
	gsl_integration_qng(&f, 0.0, 1.0, 1e-2,1e-2,&result,&error,&size);
//    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;
  }
}

/* integral of f over allele freq-- using adaptive integration */
double my_FAdapt(double beta, void * p){
  struct my_F_params * params = (struct my_F_params *) p;
  struct my_f_params fParams;
  gsl_function f;
  double result,error;
  size_t size= 100;

  if (beta == 0){
    return 1.0 / params->i;
  }
  else{
    fParams.beta = beta;
    fParams.i = params->i;
    fParams.n = params->n;
    f.function = &(my_f);
    f.params = &fParams;
    //adaptive integration-- slow and steady
	gsl_integration_qags(&f,0.0,1.0,1e-3,1e-3,size,params->w, &result,&error);
    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;
    tempParams.w = params->w;
    F.function =  &my_F;
    F.params = &tempParams;
    tot += GSL_FN_EVAL(&F, beta);
  }

  /* reset parameters */
  F.function =  &my_FAdapt;
  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;
	tempParams.w = params->w;
	F.function =  &my_FAdapt;
	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);
}

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

  //initialize bools vector
  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);
  }
}
/* 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;
	gsl_integration_workspace * w = gsl_integration_workspace_alloc (1000);
	probs = gsl_vector_alloc(maxSampleSize + 1);

	tempParams.w = w;
/* 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_FAdapt;
				F.params = &tempParams;
				tot += GSL_FN_EVAL(&F, beta);
			}
			gsl_vector_set(probs, j, tot);
		}
	}
	gsl_integration_workspace_free(w);
	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);
  FParams.w = gsl_integration_workspace_alloc(1000);
  //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);
  gsl_integration_workspace_free(FParams.w);
  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;
	gsl_integration_workspace * w = gsl_integration_workspace_alloc (100000);
	int i, j;

	FParams.w = w;
//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);
}

gsl_vector *snpProbVectorNotLogPre(double beta,  int sampleSize, gsl_vector *probs){
  double denom;
  struct my_snpProb_params FParams;
  gsl_function f;
  int i;

   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;
		FParams.w = params->w;
		f.function = &snpProb;
		f.params = &FParams;
		//	printf("here lik:%f beta:%f i: %d n: %d\n",lik,gsl_vector_get(betas, i),FParams.i,FParams.n);

		lik += log(GSL_FN_EVAL(&f, gsl_vector_get(betas, i)));
		}
	return -lik;
}

/*likelihood from Bustamante et al. 2001 */
double bustaLik(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;
}

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

/* Folded SFS version of above; weighted likelihood function for SFS given the data; here contains the data and the weights and uses the snpProbLookup routine */
double weightedLikLookFolded(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);
	lik += gsl_matrix_get(probs,params->data[i].n, params->data[i].n - params->data[i].i)  * gsl_vector_get(params->weights,i);
  }
  gsl_matrix_free(probs);
  return -lik;
}

/* PRF log Likelihood  */
double prfLik(double beta, void * p){
	 struct prfLik_params * params = (struct prfLik_params *) p;
	double lik;
	double term1, term2,theta;
  	int i;
	struct my_F_params tempParams;
	gsl_function F;
	double fTerm = 0.;

	theta = gsl_vector_get(params->betas,1);
	term1 = term2 = 0;
	for(i = 1; i < params->maxSampleSize;i++){
		tempParams.i = i;
		tempParams.n = params->maxSampleSize;
		F.function = &my_F;
		F.params = &tempParams;
		fTerm = GSL_FN_EVAL(&F,beta);
		term1 += gsl_matrix_get(params->sfsCounts,params->maxSampleSize,i) * log( theta* fTerm) ;
		term2 += fTerm;
	}
	return((term1 - (theta * term2)) * -1);
}

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;
}

double weightedLikLookCIFolded(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 = &weightedLikLookFolded;
  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  wlikWrapLookFolded(double beta, void * p){
	gsl_function f;
	struct my_lik_params * params = (struct my_lik_params *) p;

	f.function = &weightedLikLookFolded;
	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);
}

/* wrapper for prfLik for use in mnbrak */
double  prfLikWrap(double beta, void * p){
	gsl_function f;
	struct prfLik_params * params = (struct prfLik_params *) p;

	f.function = &prfLik;
	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 = 1000;
	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 = 20000;
	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;
	}

	if (fb == fc){
		return HUGE_VAL;
	}
	// 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.01, 0.01);
	}
	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;
	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
	s = params->min;
	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.01, 0.01);
	}
	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;
}

//Folded Version
double weighted_ml_est_lookup_folded(double * lik_beta_hat, void * p){
	int status;
	int iter = 0;
	int max_iter = 1000;
	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 = 20000;
	L.function = &weightedLikLook;
	L.params = params;
	mnbrak2(&ax, &bx, &cx, &fa, &fb, &fc, &wlikWrapLookFolded, bx, params);
	//swap bounds if needed
	if (cx < ax){
	  dummy = cx;
	  cx = ax;
	  ax = dummy;
	}

	if (fb == fc){
		return HUGE_VAL;
	}
	// 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.01, 0.01);
	}
	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_lookupFolded(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 = &weightedLikLookCIFolded;
	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_lookupFolded(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 = &weightedLikLookCIFolded;
	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;
}

//ML estimation using PRF likelihood model
double ml_prf(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 prfLik_params * params = (struct prfLik_params *) p;

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

	mnbrak2(&ax, &bx, &cx, &fa, &fb, &fc, &prfLikWrap, bx, params);
	//swap bounds if needed
	if (cx < ax){
	  dummy = cx;
	  cx = ax;
	  ax = dummy;
	}

	if (fb == fc){
		return HUGE_VAL;
	}
	// do mimization

	//initialize lik function
	L.function = &prfLik;
	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;
}

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

}

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

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

  //fill matrix of size maxSampleSize by maxSampleSize
  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);
  }
}

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

int summarizeSFSFillFromTo(gsl_matrix *counts, struct snp data[], int snpNumber, int from, int to, int startPos){
  int i;

  //fill matrix of size maxSampleSize by maxSampleSize
  gsl_matrix_set_zero(counts);
  assert(counts != NULL);
  gsl_matrix_set_zero(counts);

  //go through data, fill up matrix
	i = startPos;
	while(data[i].pos < to){
		if(data[i].pos >= from)
    		gsl_matrix_set(counts, data[i].n, data[i].i, gsl_matrix_get(counts, data[i].n, data[i].i) + 1);
		i++;
  	}
	return(i);
}

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

}

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

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

  //fill 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);
  }
}

/* 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;
	int sampleSize, ascSize, derived;
	double num = 0.0;

	sampleSize = params->n;
	ascSize = params->ascSize;
	derived = params->derivedFlag;
	if(derived){
	    num =  xchoosey( sampleFreq, ascSize);
	  }
	  else{
	    num =  xchoosey((sampleSize - sampleFreq), ascSize);
	  }
	  return(num /xchoosey(sampleSize, ascSize));
}

/*same as above for case where ascSamp is not subset of sample */
double outgroupAscertain2(double sampleFreq, void * p){
	struct my_F_params * params = (struct my_F_params *) p;
	int sampleSize, ascSize,ascFreq, derived, N, j;
	double num = 0.0, denom = 0.0;

	sampleSize = params->n;
	ascSize = params->ascSize;
	ascFreq = params->ascFreq;
	derived = params->derivedFlag;

	N = sampleSize + ascSize;
	j = sampleFreq + ascFreq;
  	//was ascertainment for derived?
  	if(derived == 1){
		num =  xchoosey(j,sampleFreq) * xchoosey((N-j), sampleSize - sampleFreq);
  	}
  	else{
		num =  xchoosey((N-j), (sampleSize - sampleFreq)) * xchoosey(j,  sampleFreq);
  	}
//	printf("num: %f\n", num );
//	printf("N: %d j: %d sampleSize: %d ascSize: %d derivedFlag: %d\n",N,j, sampleSize,ascSize,derived);
	denom = xchoosey(N, sampleSize);
	if (denom == 0 || num == 0){
		return(0);
	}
//	printf("oaVal: %f\n",num)/denom;
  	return(num / denom);
}

/* 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. Also has inelegance
of calculating derived or anscertral matrix  */
gsl_matrix *snpOutgroupAscMatrix(int maxSampleSize, gsl_vector *sampleSizeVector, int ascSize, int derived, \
int runMode){
	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;
				FParams.ascFreq = derived;
				FParams.derivedFlag = derived;
				if(runMode == 0){
					f.function = &outgroupAscertain;
				}
				else{
					f.function = &outgroupAscertain2;
				}
				f.params = &FParams;
				gsl_matrix_set(probs, i, j, GSL_FN_EVAL(&f, j));
			}
		}
	}
	return(probs);
}

gsl_matrix *snpOutgroupAscMatrixPre(int maxSampleSize, gsl_vector *sampleSizeVector, int ascSize, int derived, \
int runMode, gsl_matrix *probs){
	struct my_F_params FParams;
	gsl_function f;
	int i, j;

	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;
				FParams.ascFreq = derived;
				FParams.derivedFlag = derived;
				if(runMode == 0){
					f.function = &outgroupAscertain;
				}
				else{
					f.function = &outgroupAscertain2;
				}
				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, *ascProbsDerived, *ascProbsAncestral;
	gsl_vector *ascDenomsDerived, *ascDenomsAncestral;

	//make prob matrix
	probs = snpProbMatrixNotLogFull(beta, params->maxSampleSize, params->sampleSizeVector);
	//make ascProb matrices, one for derived one for ancestral
	ascProbsAncestral = snpOutgroupAscMatrix(params->maxSampleSize, params->sampleSizeVector, \
		params->ascSize, 0, params->runMode);

	ascProbsDerived = snpOutgroupAscMatrix(params->maxSampleSize, params->sampleSizeVector, \
		params->ascSize, 1, params->runMode);

	//make vectors of denoms- derived, ancestral
	ascDenomsDerived = gsl_vector_alloc(params->maxSampleSize + 1);
	ascDenomsAncestral = gsl_vector_alloc(params->maxSampleSize + 1);
	gsl_vector_set_zero(ascDenomsDerived);
	gsl_vector_set_zero(ascDenomsAncestral);
	for(i = params->ascSize; i <= params->maxSampleSize; i++){
		gsl_vector_set(ascDenomsDerived,i,probAscertainmentGivenModelLookup(beta, i, probs, ascProbsDerived));
		gsl_vector_set(ascDenomsAncestral,i,probAscertainmentGivenModelLookup(beta, i, probs, ascProbsAncestral));
	}

//now go and tally likelihood
	lik = 0;
	num = 0;
	for(i = 0; i < params->snpNumber; i++){
		if(params->data[i].derivedFlag == 1){
			num = (gsl_matrix_get(probs,params->data[i].n,params->data[i].i) * gsl_matrix_get(ascProbsDerived,params->data[i].n,params->data[i].i)) \
				/ gsl_vector_get(ascDenomsDerived, params->data[i].n);
			}
		else{
			num = (gsl_matrix_get(probs,params->data[i].n,params->data[i].i) * gsl_matrix_get(ascProbsAncestral,params->data[i].n,params->data[i].i)) \
				/ gsl_vector_get(ascDenomsAncestral, params->data[i].n);
		}
		lik += log(num);
	}

	gsl_matrix_free(probs);
	gsl_matrix_free(ascProbsDerived);
	gsl_matrix_free(ascProbsAncestral);
	gsl_vector_free(ascDenomsDerived);
	gsl_vector_free(ascDenomsAncestral);
	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 *ascProbsDerived, *ascProbsAncestral;
	gsl_vector *snpProbs;

	lik = 0;
//make ascProb matrix
	ascProbsAncestral = snpOutgroupAscMatrix(params->maxSampleSize, params->sampleSizeVector, \
		params->ascSize, 0, params->runMode);

	ascProbsDerived = snpOutgroupAscMatrix(params->maxSampleSize, params->sampleSizeVector, \
		params->ascSize, 1, params->runMode);
//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
		if(params->data[i].derivedFlag){
			pAsc = gsl_matrix_get(ascProbsDerived, params->data[i].n, params->data[i].i);
			//get pAscModel
			pAscModel = probAscertainmentGivenModelHemiLookup(gsl_vector_get(betas, i),params->data[i].n, \
					snpProbs, ascProbsDerived);
		}
		else{
			pAsc = gsl_matrix_get(ascProbsAncestral, params->data[i].n, params->data[i].i);
			//get pAscModel
			pAscModel = probAscertainmentGivenModelHemiLookup(gsl_vector_get(betas, i),params->data[i].n, \
					snpProbs, ascProbsAncestral);
		}
		tmp = pSNP * pAsc / pAscModel;
		lik += log(tmp);
		gsl_vector_free(snpProbs);
	}
	gsl_matrix_free(ascProbsAncestral);
	gsl_matrix_free(ascProbsDerived);
	return -lik;
}

double sfsLikBetaVectorOutgroupAscPre(gsl_vector *betas, gsl_matrix *ascProbsAncestral,gsl_matrix *ascProbsDerived, gsl_vector *snpProbs, void * p){
	struct my_lik_params * params = (struct my_lik_params *) p;
	double pSNP, pAsc, pAscModel,lik, tmp;
	int i;

	lik = 0.0;
//make ascProb matrix
	ascProbsAncestral = snpOutgroupAscMatrixPre(params->maxSampleSize, params->sampleSizeVector, \
		params->ascSize, 0, params->runMode,ascProbsAncestral);

	ascProbsDerived = snpOutgroupAscMatrixPre(params->maxSampleSize, params->sampleSizeVector, \
		params->ascSize, 1, params->runMode,ascProbsDerived);
//go through snps
	for(i = 0; i < params->snpNumber; i++){
	//create vector of snpProbs, find pSNP_i
		snpProbs = snpProbVectorNotLogPre(gsl_vector_get(betas, i), params->data[i].n,snpProbs);
		pSNP = gsl_vector_get(snpProbs, params->data[i].i);

	//lookup pAsc_i
		if(params->data[i].derivedFlag){
			pAsc = gsl_matrix_get(ascProbsDerived, params->data[i].n, params->data[i].i);
			//get pAscModel
			pAscModel = probAscertainmentGivenModelHemiLookup(gsl_vector_get(betas, i),params->data[i].n, \
					snpProbs, ascProbsDerived);
		}
		else{
			pAsc = gsl_matrix_get(ascProbsAncestral, params->data[i].n, params->data[i].i);
			//get pAscModel
			pAscModel = probAscertainmentGivenModelHemiLookup(gsl_vector_get(betas, i),params->data[i].n, \
					snpProbs, ascProbsAncestral);
		}
		tmp = pSNP * pAsc / pAscModel;
		lik += log(tmp);
//		printf("%f\n",lik);
	}
	return(-lik);
}
//classical summary stats

//tajimas theta_pi
double theta_pi(struct snp *data, int sites){
	int i;
	double n,p, sum;

	sum = 0.0;
	for(i=0;i<sites;i++){
		n = (double) data[i].n;
		p = (double) data[i].i / n;
		sum += (n / (n-1.0)) * (2.0 * p * (1.0 - p));
	}

	return(sum);
}

//this simply counts size with 0 < i < sampleSize
int segSites(struct snp *data, int sites){
	int i, count = 0;
	//go thru sites
	for(i=0;i<sites;i++){
		//check if monomorphic ##note I'm doing zero and n
		if  (data[i].i != 0 && data[i].i != data[i].n)
			count++;
	}
	return(count);
}
/*
double theta_w(int segSites, int n){
	return((double)segSites / a1f(n));
}

//harmonic sum for Waterson's theta
double a1f(int n){
	int i;
	double sum = 0;

	for(i=1;i<n;i++){
		sum += 1.0 / (double) i;
	}
	return(sum);
}
/*
//tajd
double tajd(int nsam, int segsites, double sumk){
  double  a1, a2, b1, b2, c1, c2, e1, e2;

  if( segsites == 0 ) return( 0.0) ;

  a1 = a1f(nsam);
  a2 = a2f(nsam);
  b1 = b1f(nsam);
  b2 = b2f(nsam);
  c1 = c1f(a1, b1);
  c2 = c2f(nsam, a1, a2, b2);
  e1 = e1f(a1, c1);
  e2 = e2f(a1, a2, c2);

  return( (sumk - (segsites/a1))/sqrt((e1*segsites) + ((e2*segsites)*(segsites
								      -1))) ) ;
}

double a2f(int nsam) {
  double a2;
  int i;
  a2 = 0.0;
  for (i=1; i<=nsam-1; i++) a2 += 1.0/(i*i);
  return (a2);
}

double b1f(int nsam){
  double b1;
  b1 = (nsam + 1.0)/(3.0*(nsam-1.0));
  return (b1);
}

double b2f(int nsam){
  double b2;
  b2 = (2*(nsam*nsam + nsam + 3.0))/(9*nsam*(nsam - 1));
  return (b2);
}

double e1f(double a1f, double c1){
  double e1;
  e1 = c1/a1f;
  return (e1);
}

double e2f(double a1f, double a2, double c2){
  double e2;
  e2 = c2/((a1f*a1f)+a2);
  return (e2);
}

double c1f(double a1f, double b1){
  double c1;
  c1 = b1 - (1/a1f);
  return (c1);
}

double c2f(int nsam, double a1f, double a2, double b2){
  double c2;
  c2 = b2 - ((nsam+2)/(a1f*nsam)) + (a2/(a1f * a1f));
  return (c2);
}

*/

/////////
/////
////  MPI stuff

//MPI version of above for parallel processing
double sfsLikBetaVectorMPI(gsl_vector *betas, void * p, int myrank, int mprocs, MPI_Comm comm){
	struct my_lik_params * params = (struct my_lik_params *) p;
	struct my_F_params FParams;
	gsl_function f;
	double lik, final;
	int snpNumber = params->snpNumber;
	int i, start, end, window;

	//initialize; where am i?
	lik = 0.;
	window = ceil((float)snpNumber / mprocs);
	start = myrank * window;
	end = start + window;
	FParams.w = params->w;
	f.function = &snpProb;
	for(i = start; i < end && i < snpNumber; i++){
		FParams.i = params->data[i].i;
		FParams.n = params->data[i].n;

		f.params = &FParams;
	//	printf("here lik:%f beta:%f i: %d n: %d\n",lik,gsl_vector_get(betas, i),FParams.i,FParams.n);

		lik += log(GSL_FN_EVAL(&f, gsl_vector_get(betas, i)));
		}
	MPI_Reduce(&lik,&final,1, MPI_DOUBLE,MPI_SUM,0,comm);
	return -final;
}

double sfsLikBetaVectorOutgroupAscPreMPI(gsl_vector *betas, gsl_matrix *ascProbsAncestral,gsl_matrix *ascProbsDerived, \
	gsl_vector *snpProbs, void * p,int myrank, int mprocs, MPI_Comm comm){

	struct my_lik_params * params = (struct my_lik_params *) p;
	double pSNP, pAsc, pAscModel,lik, tmp, final;
	int i,start, end, window,snpNumber;

	//initialization; window for proc to process
	lik = 0;
	snpNumber = params->snpNumber;
	window = ceil((float)snpNumber / mprocs);
	start = myrank * window;
	end = start + window;

	//currently every process calculates the matrices below-- could be faster
	//make ascProb matrix
	ascProbsAncestral = snpOutgroupAscMatrixPre(params->maxSampleSize, params->sampleSizeVector, \
		params->ascSize, 0, params->runMode,ascProbsAncestral);

	ascProbsDerived = snpOutgroupAscMatrixPre(params->maxSampleSize, params->sampleSizeVector, \
		params->ascSize, 1, params->runMode,ascProbsDerived);

	//go through snps
	for(i = start; i < end && i < snpNumber; i++){
	//create vector of snpProbs, find pSNP_i
		snpProbs = snpProbVectorNotLogPre(gsl_vector_get(betas, i), params->data[i].n,snpProbs);
		pSNP = gsl_vector_get(snpProbs, params->data[i].i);

	//lookup pAsc_i
		if(params->data[i].derivedFlag){
			pAsc = gsl_matrix_get(ascProbsDerived, params->data[i].n, params->data[i].i);
			//get pAscModel
			pAscModel = probAscertainmentGivenModelHemiLookup(gsl_vector_get(betas, i),params->data[i].n, \
					snpProbs, ascProbsDerived);
		}
		else{
			pAsc = gsl_matrix_get(ascProbsAncestral, params->data[i].n, params->data[i].i);
			//get pAscModel
			pAscModel = probAscertainmentGivenModelHemiLookup(gsl_vector_get(betas, i),params->data[i].n, \
					snpProbs, ascProbsAncestral);
		}
		tmp = pSNP * pAsc / pAscModel;
		lik += log(tmp);
	}
	MPI_Reduce(&lik,&final,1, MPI_DOUBLE,MPI_SUM,0,comm);
	return -final;
}


#include <stdio.h>
#include <gsl/gsl_multifit.h>

int
main (int argc, char **argv)
{
  int i, n;
  double xi, yi, ei, chisq;
  gsl_matrix *X, *cov;
  gsl_vector *y, *w, *c;

  if (argc != 2)
    {
      fprintf (stderr,"usage: fit n < data\n");
      exit (-1);
    }

  n = atoi (argv[1]);

  X = gsl_matrix_alloc (n, 3);
  y = gsl_vector_alloc (n);
  w = gsl_vector_alloc (n);

  c = gsl_vector_alloc (3);
  cov = gsl_matrix_alloc (3, 3);

  for (i = 0; i < n; i++)
    {
      int count = fscanf (stdin, "%lg %lg %lg",
                          &xi, &yi, &ei);

      if (count != 3)
        {
          fprintf (stderr, "error reading file\n");
          exit (-1);
        }

      printf ("%g %g +/- %g\n", xi, yi, ei);

      gsl_matrix_set (X, i, 0, 1.0);
      gsl_matrix_set (X, i, 1, xi);
      gsl_matrix_set (X, i, 2, xi*xi);

      gsl_vector_set (y, i, yi);
      gsl_vector_set (w, i, 1.0/(ei*ei));
    }

  {
    gsl_multifit_linear_workspace * work
      = gsl_multifit_linear_alloc (n, 3);
    gsl_multifit_wlinear (X, w, y, c, cov,
                          &chisq, work);
    gsl_multifit_linear_free (work);
  }

#define C(i) (gsl_vector_get(c,(i)))
#define COV(i,j) (gsl_matrix_get(cov,(i),(j)))

  {
    printf ("# best fit: Y = %g + %g X + %g X^2\n",
            C(0), C(1), C(2));

    printf ("# covariance matrix:\n");
    printf ("[ %+.5e, %+.5e, %+.5e  \n",
               COV(0,0), COV(0,1), COV(0,2));
    printf ("  %+.5e, %+.5e, %+.5e  \n",
               COV(1,0), COV(1,1), COV(1,2));
    printf ("  %+.5e, %+.5e, %+.5e ]\n",
               COV(2,0), COV(2,1), COV(2,2));
    printf ("# chisq = %g\n", chisq);
  }

  gsl_matrix_free (X);
  gsl_vector_free (y);
  gsl_vector_free (w);
  gsl_vector_free (c);
  gsl_matrix_free (cov);

  return 0;
}


/* randist/geometric.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 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.
 */

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

/* Geometric distribution (bernoulli trial with probability p)

   prob(k) =  p (1 - p)^(k-1) for n = 1, 2, 3, ...

   It gives the distribution of "waiting times" for an event that
   occurs with probability p. */

unsigned int
gsl_ran_geometric (const gsl_rng * r, const double p)
{
  double u = gsl_rng_uniform_pos (r);

  unsigned int k;

  if (p == 1)
    {
      k = 1;
    }
  else
    {
      k = log (u) / log (1 - p) + 1;
    }

  return k;
}

double
gsl_ran_geometric_pdf (const unsigned int k, const double p)
{
  if (k == 0)
    {
      return 0 ;
    }
  else if (k == 1)
    {
      return p ;
    }
  else
    {
      double P = p * pow (1 - p, k - 1.0);
      return P;
    }
}


static char help[] =
"ODE system solver example using TS.  Solves a 2D system\n"
"    dy/dt = G(t,y)\n"
"with y(t0) = y0 to compute y(tf).  Sets problem type to NONLINEAR and\n"
"TS type to Runge-Kutta.  No Jacobian is supplied; compare odejac.c.\n"
"Exact solution is known.\n\n";

#include <petsc.h>

extern PetscErrorCode ExactSolution(PetscReal, Vec);
extern PetscErrorCode FormRHSFunction(TS, PetscReal, Vec, Vec, void*);

//STARTMAIN
int main(int argc,char **argv) {
  PetscErrorCode ierr;
  PetscInt   steps;
  PetscReal  t0 = 0.0, tf = 20.0, dt = 0.1, err;
  Vec        y, yexact;
  TS         ts;

  ierr = PetscInitialize(&argc,&argv,NULL,help); if (ierr) return ierr;

  ierr = VecCreate(PETSC_COMM_WORLD,&y); CHKERRQ(ierr);
  ierr = VecSetSizes(y,PETSC_DECIDE,2); CHKERRQ(ierr);
  ierr = VecSetFromOptions(y); CHKERRQ(ierr);
  ierr = VecDuplicate(y,&yexact); CHKERRQ(ierr);

  ierr = TSCreate(PETSC_COMM_WORLD,&ts); CHKERRQ(ierr);
  ierr = TSSetProblemType(ts,TS_NONLINEAR); CHKERRQ(ierr);
  ierr = TSSetRHSFunction(ts,NULL,FormRHSFunction,NULL); CHKERRQ(ierr);
  ierr = TSSetType(ts,TSRK); CHKERRQ(ierr);

  // set time axis
  ierr = TSSetTime(ts,t0); CHKERRQ(ierr);
  ierr = TSSetMaxTime(ts,tf); CHKERRQ(ierr);
  ierr = TSSetTimeStep(ts,dt); CHKERRQ(ierr);
  ierr = TSSetExactFinalTime(ts,TS_EXACTFINALTIME_MATCHSTEP); CHKERRQ(ierr);
  ierr = TSSetFromOptions(ts); CHKERRQ(ierr);

  // set initial values and solve
  ierr = TSGetTime(ts,&t0); CHKERRQ(ierr);
  ierr = ExactSolution(t0,y); CHKERRQ(ierr);
  ierr = TSSolve(ts,y); CHKERRQ(ierr);

  // compute error and report
  ierr = TSGetStepNumber(ts,&steps); CHKERRQ(ierr);
  ierr = TSGetTime(ts,&tf); CHKERRQ(ierr);
  ierr = ExactSolution(tf,yexact); CHKERRQ(ierr);
  ierr = VecAXPY(y,-1.0,yexact); CHKERRQ(ierr);    // y <- y - yexact
  ierr = VecNorm(y,NORM_INFINITY,&err); CHKERRQ(ierr);
  ierr = PetscPrintf(PETSC_COMM_WORLD,
              "error at tf = %.3f with %d steps:  |y-y_exact|_inf = %g\n",
              tf,steps,err); CHKERRQ(ierr);

  VecDestroy(&y);  VecDestroy(&yexact);  TSDestroy(&ts);
  return PetscFinalize();
}
//ENDMAIN

//STARTCALLBACKS
PetscErrorCode ExactSolution(PetscReal t, Vec y) {
    PetscReal *ay;
    VecGetArray(y,&ay);
    ay[0] = t - PetscSinReal(t);
    ay[1] = 1.0 - PetscCosReal(t);
    VecRestoreArray(y,&ay);
    return 0;
}

PetscErrorCode FormRHSFunction(TS ts, PetscReal t, Vec y, Vec g,
                               void *ptr) {
    const PetscReal *ay;
    PetscReal       *ag;
    VecGetArrayRead(y,&ay);
    VecGetArray(g,&ag);
    ag[0] = ay[1];            // = g_1(t,y)
    ag[1] = - ay[0] + t;      // = g_2(t,y)
    VecRestoreArrayRead(y,&ay);
    VecRestoreArray(g,&ag);
    return 0;
}
//ENDCALLBACKS


/*
copied from http://www.gnu.org/software/gsl/manual/html_node/Basis-Splines.html
*/

#include <assert.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_sf_erf.h>

void geterf_(double *xanderfx) {
  xanderfx[1] = gsl_sf_erf(xanderfx[0]);
}

//spline locations held fixed in Mpc^-1; CMB basically fixed P(k) in these units
void dopksmoothbspline_(double *kvals, double *lnpklinear, double *lnpksmooth, int *npts)  {

	double kmaxsuppress = 0.01*0.7;
	size_t n, ncoeffs, nbreak;
	gsl_bspline_workspace *bw;
	gsl_vector *B;
	gsl_vector *c, *w, *x, *y;
	gsl_matrix *X, *cov;
	gsl_multifit_linear_workspace *mw;
	double deltak,lastk;
	int i,j,countkeep;

	nbreak = 9;
	gsl_vector *mybreaks = gsl_vector_alloc(nbreak);
	gsl_vector_set(mybreaks,0,(0.001*0.7));
	gsl_vector_set(mybreaks,1,(0.025*0.7));
	gsl_vector_set(mybreaks,2,(0.075*0.7));
	gsl_vector_set(mybreaks,3,(0.125*0.7));
	gsl_vector_set(mybreaks,4,(0.175*0.7));
	gsl_vector_set(mybreaks,5,(0.225*0.7));
	gsl_vector_set(mybreaks,6,(0.275*0.7));
	gsl_vector_set(mybreaks,7,(0.325*0.7));
	gsl_vector_set(mybreaks,8,(0.375*0.7));

	countkeep = 0;
	for(i=0;i<(*npts);i++)  {
		if((kvals[i]) >= gsl_vector_get(mybreaks,0) && (kvals[i]) <= gsl_vector_get(mybreaks,nbreak-1)) {
			countkeep += 1;
			}
		}
	n = countkeep;
	ncoeffs = nbreak + 2;

	/* allocate a cubic bspline workspace (k = 4) */
	bw = gsl_bspline_alloc(4, nbreak);
	B = gsl_vector_alloc(ncoeffs);
	x = gsl_vector_alloc(n);
	y = gsl_vector_alloc(n);
	X = gsl_matrix_alloc(n, ncoeffs);
	c = gsl_vector_alloc(ncoeffs);
	w = gsl_vector_alloc(n);
	cov = gsl_matrix_alloc(ncoeffs, ncoeffs);
	mw = gsl_multifit_linear_alloc(n, ncoeffs);
	i=0;
	for(j=0;j<(*npts);j++)  {
		if((kvals[j]) >= gsl_vector_get(mybreaks,0) && (kvals[j]) <= gsl_vector_get(mybreaks,nbreak-1)) {
			gsl_vector_set(x,i,(kvals[j]));
			gsl_vector_set(y,i,exp(lnpklinear[j])*pow(kvals[j],1.5));
			if(j>0)  {
				deltak = kvals[j] - kvals[j-1];
				}
			else {
				deltak = kvals[0];
				if(kvals[1] - kvals[0] < deltak)  {
					deltak = kvals[1]-kvals[0];
					}
				}
			gsl_vector_set(w,i,deltak);
			i+=1;
			}
		}
	gsl_bspline_knots(mybreaks,bw);
	for(i=0;i<n;i++)  {
		double xi = gsl_vector_get(x,i);
		gsl_bspline_eval(xi,B,bw);
		for(j=0;j<ncoeffs;j++)  {
			double Bj = gsl_vector_get(B,j);
			gsl_matrix_set(X,i,j,Bj);
			}
		}
	//do fit
	double yi,yierr,chisq;
	gsl_multifit_wlinear(X,w,y,c,cov,&chisq,mw);
	i = 0;
	for(j=0;j<(*npts);j++)  {
		if((kvals[j]) >= gsl_vector_get(mybreaks,0) && (kvals[j]) <= gsl_vector_get(mybreaks,nbreak-1)) {
			gsl_bspline_eval(gsl_vector_get(x,i),B,bw);
			gsl_multifit_linear_est(B,c,cov,&yi,&yierr);
			lnpksmooth[j] = log(yi*pow(kvals[j],-1.5));
			i += 1;
			}
		else {
			lnpksmooth[j] = lnpklinear[j];
			}
		//spline is wacky at small k -- suppress difference at k < 0.01
		if(kvals[j] < kmaxsuppress)  {
			lnpksmooth[j] = lnpklinear[j];
			}
		}
	assert(i==n);
	gsl_bspline_free(bw);
	gsl_vector_free(B);
	gsl_vector_free(x);
	gsl_vector_free(y);
	gsl_vector_free(mybreaks);
	gsl_matrix_free(X);
	gsl_vector_free(c);
	gsl_vector_free(w);
	gsl_matrix_free(cov);
	gsl_multifit_linear_free(mw);
	}
/*

FILE *open_file_read(char *filename) {
	FILE *ifp;

	if(!(ifp=fopen(filename,"r")))  {
		fprintf(stderr,"Can't open %s\n",filename);
		return NULL;
		}
	else {
		return ifp;
		}
	}

int get_file_length(FILE *ifp, int *headercount, int *linecount) {
	int i;
	char line[256];

	*headercount = 0;
	*linecount = 0;

    while(fgets(line,256,ifp))  {
        if(!feof(ifp))  {
            (*linecount)++;
            }
		else {
			break;
			}
		if(strstr(line,"#"))  {
			(*headercount)++;
			}
        }
	(void) rewind(ifp);
	}

int main()  {
	double *lnpksmooth, *lnpklinear, *kvals;
	FILE *ifp;
	int headercount, linecount,i,j;
	double kval,pnw,plin;
	ifp = open_file_read("/Users/breid/cosmomcmnu/test1.cut");
	get_file_length(ifp,&headercount,&linecount);
	lnpksmooth = (double *) malloc(sizeof(double)*linecount);
	lnpklinear = (double *) malloc(sizeof(double)*linecount);
	kvals = (double *) malloc(sizeof(double)*linecount);
	for(i=0;i<linecount;i++)  {
		fscanf(ifp,"%lf %lf %lf\n",&kval,&pnw,&plin);
		lnpklinear[i] = plin;
		kvals[i] = kval*0.701;
		//kvals[i] = kval;
		}
	dopksmoothbspline_(kvals,lnpklinear,lnpksmooth,&linecount);
	for(i=0;i<linecount;i++)  {
		printf("%f %f %f\n",kvals[i],lnpklinear[i],lnpksmooth[i]);
		}
	return 0;
	}
*/


#include <stdio.h>
#include <string.h>
#include <time.h>
#include <MaxSLiCInterface.h>
#include "Maxfiles.h"
#include "dgemm.h"

#define TILE_SIZE DGEMM_tileSize
#define TILE_SIZE_2D (TILE_SIZE * TILE_SIZE)
#define FREQUENCY DGEMM_frequency

static size_t sizeInTiles(size_t n) {
	return (n + TILE_SIZE - 1) / TILE_SIZE;
}

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

void dgemm_model(
		const char* trans_a, const char* trans_b, size_t m, size_t n, size_t k, double alpha,
		const double* A, size_t lda, const double* B, size_t ldb, double beta, double* C, size_t ldc)
{
	// TODO support transform

#ifdef USE_BLAS
	cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, m, n, k, alpha, A, lda, B, ldb, beta, C, ldc);
#else
	for (size_t mm = 0; mm < m; ++mm) {
		for (size_t nn = 0; nn < n; ++nn) {
			C[mm*ldc+nn] *= beta;
			for (size_t kk = 0; kk < k; ++kk) {
				C[mm*ldc+nn] += alpha * A[mm*lda+kk] * B[kk*ldb+nn];
			}
		}
	}
#endif
}

static struct timespec diff(struct timespec start, struct timespec finish) {
	struct timespec diff;
	if ((finish.tv_nsec < start.tv_nsec)) {
		diff.tv_sec  = finish.tv_sec  - start.tv_sec  - 1;
		diff.tv_nsec = finish.tv_nsec - start.tv_nsec + 1000000000;
	} else {
		diff.tv_sec  = finish.tv_sec  - start.tv_sec;
		diff.tv_nsec = finish.tv_nsec - start.tv_nsec;
	}

	return diff;
}

static int check_result(int m, int n, const double* expected, const double* actual) {
	for (int i = 0; i < m; ++i) {
		for (int j = 0; j < n; ++j) {
			int idx = i*n + j;
			if (expected[idx] != actual[idx]) {
				return 1;
			}
		}
	}
	return 0;
}

int main(int argc, char** argv) {
	unsigned seed = time(NULL);
	printf("Random seed: %u\n", seed);
	srand(seed);

	size_t m;
	size_t n;
	size_t k;

	if (argc == 1) {
		m = (random() % (5 * TILE_SIZE)) + TILE_SIZE;
		n = (random() % (5 * TILE_SIZE)) + TILE_SIZE;
		k = (random() % (5 * TILE_SIZE)) + TILE_SIZE;
	} else if (argc == 2) {
		m = n = k = atoi(argv[1]);
	} else {
		printf("Usage: %s [size]\n", argv[0]);
		exit(1);
	}

	double* A   = calloc(m * k, sizeof(double));
	double* B   = calloc(k * n, sizeof(double));
	double* Csw = calloc(m * n, sizeof(double));
	double* Chw = calloc(m * n, sizeof(double));

	printf("Matrix dimensions: m = %lu, n = %lu, k = %lu\n", m, n, k);

	dgemm_init(m, n, k);

	size_t mTiles = sizeInTiles(m);
	size_t nTiles = sizeInTiles(n);
	size_t kTiles = sizeInTiles(k);

	printf("DFE tile size: %d\n", TILE_SIZE);
	printf("DFE compute dimensions: m = %zu, n = %zu, k = %zu\n",
			mTiles * TILE_SIZE, nTiles * TILE_SIZE, kTiles * TILE_SIZE);

	double cpuPoints = ((double) m) * n * k;
	double dfePoints = ((double) mTiles) * nTiles * kTiles * TILE_SIZE_2D;
	printf("DFE compute efficiency: %f\n", cpuPoints / (dfePoints * TILE_SIZE));
	printf("DFE frequency: %d MHz\n", FREQUENCY);
	// add one tile to account for stream offset TODO measure pipeline depth properly?
	printf("DFE predicted compute time: %f s\n", (dfePoints + TILE_SIZE_2D) / (((double) FREQUENCY) * 1e6));

	for (size_t i = 0; i < m*k; ++i) {
		A[i] = random() % 100;
	}

	for (size_t i = 0; i < k*n; ++i) {
		B[i] = random() % 100;
	}

	for (size_t i = 0; i < m*n; ++i) {
		Csw[i] = random() % 100;
		Chw[i] = Csw[i];
	}

	// TODO random
	double alpha = 1;
	double beta  = 0;

	struct timespec start;
	struct timespec finish;

	for (int xx = 0; xx < 5; ++xx) {
		printf("Repeat %d\n", xx);
		printf("Running HW... "); fflush(stdout);
		clock_gettime(CLOCK_REALTIME, &start);
		dgemm("n", "n", m, n, k, alpha, A, k, B, n, beta, Chw, n);
		clock_gettime(CLOCK_REALTIME, &finish);

		struct timespec hw_time  = diff(start, finish);

		printf("took: %ld.%09ld s\n", hw_time.tv_sec, hw_time.tv_nsec);
	}

	printf("Running SW... "); fflush(stdout);
	clock_gettime(CLOCK_REALTIME, &start);
	dgemm_model("n", "n", m, n, k, alpha, A, k, B, n, beta, Csw, n);
	clock_gettime(CLOCK_REALTIME, &finish);

	struct timespec sw_time = diff(start, finish);

	printf("took: %ld.%09ld s\n", sw_time.tv_sec, sw_time.tv_nsec);

	printf("Comparing results... ");
	int failed = check_result(m, n, Csw, Chw);
	printf("%s\n", failed ? "WRONG" : "CORRECT");
	printf("Done.\n");
	return failed;
}


/* bspline/test.c
 *
 * Copyright (C) 2006 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 <gsl/gsl_test.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_ieee_utils.h>

void
test_bspline (gsl_bspline_workspace * bw)
{
  gsl_vector *B;
  size_t i, j;
  size_t n = 100;
  size_t ncoeffs = gsl_bspline_ncoeffs (bw);
  size_t order = gsl_bspline_order (bw);
  size_t nbreak = gsl_bspline_nbreak (bw);
  double a = gsl_bspline_breakpoint (0, bw);
  double b = gsl_bspline_breakpoint (nbreak - 1, bw);

  B = gsl_vector_alloc (ncoeffs);

  for (i = 0; i < n; i++)
    {
      double xi = a + (b - a) * (i / (n - 1.0));
      double sum = 0;
      gsl_bspline_eval (xi, B, bw);

      for (j = 0; j < ncoeffs; j++)
        {
          double Bj = gsl_vector_get (B, j);
          int s = (Bj < 0 || Bj > 1);
          gsl_test (s,
                    "basis-spline coefficient %u is in range [0,1] for x=%g",
                    j, xi);
          sum += Bj;
        }

      gsl_test_rel (sum, 1.0, order * GSL_DBL_EPSILON,
                    "basis-spline order %u is normalized for x=%g", order,
                    xi);
    }

  gsl_vector_free (B);
}

int
main (int argc, char **argv)
{
  int status = 0;
  size_t order, breakpoints, i;

  gsl_ieee_env_setup ();

  argc = 0;                     /* prevent warnings about unused parameters */
  argv = 0;

  for (order = 1; order < 10; order++)
    {
      for (breakpoints = 2; breakpoints < 100; breakpoints++)
        {
          double a = -1.23 * order, b = 45.6 * order;
          gsl_bspline_workspace *bw = gsl_bspline_alloc (order, breakpoints);
          gsl_bspline_knots_uniform (a, b, bw);
          test_bspline (bw);
          gsl_bspline_free (bw);
        }
    }

  for (order = 1; order < 10; order++)
    {
      for (breakpoints = 2; breakpoints < 100; breakpoints++)
        {
          double a = -1.23 * order, b = 45.6 * order;
          gsl_bspline_workspace *bw = gsl_bspline_alloc (order, breakpoints);
          gsl_vector *k = gsl_vector_alloc (breakpoints);
          for (i = 0; i < breakpoints; i++)
            {
              double f, x;
              f = sqrt (i / (breakpoints - 1.0));
              x = (1 - f) * a + f * b;
              gsl_vector_set (k, i, x);
            };
          gsl_bspline_knots (k, bw);
          test_bspline (bw);
          gsl_vector_free (k);
          gsl_bspline_free (bw);
        }
    }

  exit (gsl_test_summary ());
}


#include "example.h"
#include <stdio.h>
#include <gsl/gsl_sf_bessel.h>

int
print_gsl_sf_bessel_J0 (double x)
{
  double y = gsl_sf_bessel_J0 (x);
  printf ("J0(%g) = %.18e\n", x, y);
  return 0;
}