text:
/* rng/rand.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 James Theiler, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

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

/* This is the old BSD rand() generator. The sequence is

   x_{n+1} = (a x_n + c) mod m

   with a = 1103515245, c = 12345 and m = 2^31 = 2147483648. The seed
   specifies the initial value, x_1.

   The theoretical value of x_{10001} is 1910041713.

   The period of this generator is 2^31.

   The rand() generator is not very good -- the low bits of successive
   numbers are correlated. */

static inline unsigned long int rand_get (void *vstate);
static double rand_get_double (void *vstate);
static void rand_set (void *state, unsigned long int s);

typedef struct
  {
    unsigned long int x;
  }
rand_state_t;

static inline unsigned long int
rand_get (void *vstate)
{
  rand_state_t *state = (rand_state_t *) vstate;

  /* The following line relies on unsigned 32-bit arithmetic */

  state->x = (1103515245 * state->x + 12345) & 0x7fffffffUL;

  return state->x;
}

static double
rand_get_double (void *vstate)
{
  return rand_get (vstate) / 2147483648.0 ;
}

static void
rand_set (void *vstate, unsigned long int s)
{
  rand_state_t *state = (rand_state_t *) vstate;

  state->x = s;

  return;
}

static const gsl_rng_type rand_type =
{"rand",			/* name */
 0x7fffffffUL,			/* RAND_MAX */
 0,				/* RAND_MIN */
 sizeof (rand_state_t),
 &rand_set,
 &rand_get,
 &rand_get_double};

const gsl_rng_type *gsl_rng_rand = &rand_type;

ext: c


text:
/* numerical stuff */

#include <math.h>
#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include "numerical.h"
#include <gsl/gsl_sf.h>
//#include "nrutil.h"

/* Numerical Recipes Stuff */

#define GOLD 1.618034
#define GLIMIT 100.0
#define TINY  1.0e-20
#define MAX(a,b) ((a) > (b)  ? (a) : (b))
#define MIN(a,b) ((a) < (b)  ? (a) : (b))
#define SIGN(a,b)  ((b)  > 0.0 ? fabs(a) : -fabs(a))
#define SHFT(a,b,c,d)  (a)=(b);(b)=(c);(c)=(d);
#define FUNC(x) ((*func)(x))
#define FUNC2(x,p) ((*func)(x,p))
#define MIN_P_VALUE 1.0e-200
#define EPS 1.0e-5
#define JMAX 50

/* this is from numerical recipes; brackets a minimum with ax, bx, cx */
void mnbrak(double *ax,double *bx,double *cx,double *fa,double *fb,double *fc,double (*func)()){
	double ulim,u,r,q,fu,dum;

	*fa=(*func)(*ax);
	*fb=(*func)(*bx);
	if(*fb > *fa){
		SHFT(dum,*ax,*bx,dum)
		SHFT(dum,*fb,*fa,dum)
	}
	*cx = *bx + GOLD * (*bx-*ax);
	*fc=(*func)(*cx);
	while(*fb > *fc){
		r=(*bx-*ax)*(*fb-*fc);
		q=(*bx-*cx)*(*fb-*fa);
		u = *bx-((*bx-*cx)*q-(*bx-*ax)*r) /
		    (2.*SIGN(MAX(fabs(q-r),TINY),q-r));
		ulim = *bx + GLIMIT * (*cx-*bx);
		if((*bx-u) * (u-*cx) > 0.0){
			fu=(*func)(u);
			if(fu < *fc){
				*ax = *bx;
				*bx = u;
				*fa = *fb;
				*fb = fu;
				return;
   	 			}
			else if(fu > *fb){
				*cx=u;
				*fc=fu;
				return;
				}
			u = *cx + GOLD * (*cx-*bx);
			fu = (*func)(u);
			}
		else if((*cx-u)*(u-ulim) > 0.0){
			fu=(*func)(u);
			if(fu < *fc){
				SHFT(*bx,*cx,u,*cx + GOLD * (*cx - *bx));
				SHFT(*fb,*fc,fu,(*func)(u));
			}
		}
		else if((u-ulim)*(ulim-*cx) >= 0.0){
			u=ulim;
			fu=(*func)(u);
		}
		else{
			u = *cx+GOLD*(*cx-*bx);
			fu=(*func)(u);
			}
		SHFT(*ax,*bx,*cx,u)
		SHFT(*fa,*fb,*fc,fu)
		}
	return;
}

/* this is from numerical recipes; brackets a minimum with ax, bx, cx */
void mnbrak2(double *ax,double *bx,double *cx,double *fa,double *fb,double *fc,double (*func)(double, void * ), double beta, void * p){
	double ulim,u,r,q,fu,dum;

	*fa=(*func)(*ax, p);
	*fb=(*func)(*bx, p);
	if(*fb > *fa){
		SHFT(dum,*ax,*bx,dum)
		SHFT(dum,*fb,*fa,dum)
	}
	*cx = *bx + GOLD * (*bx-*ax);
	*fc=(*func)(*cx, p);
	while(*fb > *fc){
		r=(*bx-*ax)*(*fb-*fc);
		q=(*bx-*cx)*(*fb-*fa);
		u = *bx-((*bx-*cx)*q-(*bx-*ax)*r) /
		    (2.*SIGN(MAX(fabs(q-r),TINY),q-r));
		ulim = *bx + GLIMIT * (*cx-*bx);
		if((*bx-u) * (u-*cx) > 0.0){
			fu=(*func)(u, p);
			if(fu < *fc){
				*ax = *bx;
				*bx = u;
				*fa = *fb;
				*fb = fu;
				return;
   	 			}
			else if(fu > *fb){
				*cx=u;
				*fc=fu;
				return;
				}
			u = *cx + GOLD * (*cx-*bx);
			fu = (*func)(u, p);
			}
		else if((*cx-u)*(u-ulim) > 0.0){
			fu=(*func)(u, p);
			if(fu < *fc){
				SHFT(*bx,*cx,u,*cx + GOLD * (*cx - *bx));
				SHFT(*fb,*fc,fu,(*func)(u, p));
			}
		}
		else if((u-ulim)*(ulim-*cx) >= 0.0){
			u=ulim;
			fu=(*func)(u, p);
		}
		else{
			u = *cx+GOLD*(*cx-*bx);
			fu=(*func)(u, p);
			}
		SHFT(*ax,*bx,*cx,u)
		SHFT(*fa,*fb,*fc,fu)
		}
	return;
}

/* Numerical Integration Routines */

#define FUNC(x) ((*func)(x))
double d_trapzd(double (*func)(double), double a, double b, int n)
{
	double x,tnm,sum,del;
	static double s;
	int it,j;

	if (n == 1) {
		return (s=0.5*(b-a)*(FUNC(a)+FUNC(b)));
	} else {
		for (it=1,j=1;j<n-1;j++) it <<= 1;
		tnm=it;
		del=(b-a)/tnm;
		x=a+0.5*del;
		for (sum=0.0,j=1;j<=it;j++,x+=del) sum += FUNC(x);
		s=0.5*(s+(b-a)*sum/tnm);
		return s;
	}
}

/* specific version for GSL function */
double d_trapzd2(double (*func)(double, void *), double a, double b, int n, void * p)
{
	double x,tnm,sum,del;
	static double s;
	int it,j;

	if (n == 1) {
	  return (s=0.5*(b-a)*(FUNC2(a,p)+FUNC2(b,p)));
	} else {
		for (it=1,j=1;j<n-1;j++) it <<= 1;
		tnm=it;
		del=(b-a)/tnm;
		x=a+0.5*del;
		for (sum=0.0,j=1;j<=it;j++,x+=del) sum += FUNC2(x,p);
		s=0.5*(s+(b-a)*sum/tnm);
		return s;
	}
}

double d_qtrap(double (*func)(double), double a, double b)
{
	double trapzd(double (*func)(double), double a, double b, int n);
	void nrerror(char error_text[]);
	double s,olds;
	int j;

	olds = -1.0e30;
	for (j=1;j<=JMAX;j++) {
		s=d_trapzd(func,a,b,j);
		if (fabs(s-olds) < EPS*fabs(olds)) return s;
		olds=s;
	}
	nrerror("Too many steps in routine qtrap");
	return 0.0;
}

/* specific version for GSL function */
double d_qtrap2(double (*func)(double, void *), double a, double b, void * p){
  double s,olds;
  int j;

  olds = -1.0e30;
  for (j=1;j<=JMAX;j++) {
    s=d_trapzd2(func,a,b,j,p);
    if (fabs(s-olds) < EPS*fabs(olds)) return s;
    olds=s;
  }
  nrerror("Too many steps in routine qtrap");
  return 0.0;
}

double d_qsimp(double (*func)(double), double a, double b)
{
	double trapzd(double (*func)(double), double a, double b, int n);
	void nrerror(char error_text[]);
	int j;
	double s,st,ost,os;

	ost = os = -1.0e30;
	for (j=1;j<=JMAX;j++) {
		st=d_trapzd(func,a,b,j);
		s=(4.0*st-ost)/3.0;
		if (fabs(s-os) < EPS*fabs(os)) return s;
		os=s;
		ost=st;
	}
	nrerror("Too many steps in routine qsimp");
	return 0.0;
}

#define JMAXP (JMAX+1)
#define K 5
double d_qromb(double (*func)(double), double a, double b)
{
	void polint(double xa[], double ya[], int n, double x, double *y, double *dy);
	double trapzd(double (*func)(double), double a, double b, int n);
	void nrerror(char error_text[]);
	double ss,dss;
	double s[JMAXP+1],h[JMAXP+1];
	int j;

	h[1]=1.0;
	for (j=1;j<=JMAX;j++) {
		s[j]=d_trapzd(func,a,b,j);
		if (j >= K) {
			d_polint(&h[j-K],&s[j-K],K,0.0,&ss,&dss);
			if (fabs(dss) < EPS*fabs(ss)) return ss;
		}
		s[j+1]=s[j];
		h[j+1]=0.25*h[j];
	}
	nrerror("Too many steps in routine qromb");
	return 0.0;
}

double d_qromb2(double (*func)(double, void *), double a, double b, void * p){

  double ss,dss;
  double s[JMAXP+1],h[JMAXP+1];
  int j;

  h[1]=1.0;
  for (j=1;j<=JMAX;j++) {
    s[j]=midpnt2(func,a,b,j,p);
    if (j >= K) {
      d_polint(&h[j-K],&s[j-K],K,0.0,&ss,&dss);
      if (fabs(dss) < EPS*fabs(ss)) return ss;
    }
    s[j+1]=s[j];
    h[j+1]=0.25*h[j];
  }
  nrerror("Too many steps in routine qromb");
  return 0.0;
}
#undef JMAXP
#undef K

#define NRANSI
#include "nrutil.h"
void d_polint(double xa[], double ya[], int n, double x, double *y, double *dy)
{
	int i,m,ns=1;
	double den,dif,dift,ho,hp,w;
	double *c,*d;

	dif=fabs(x-xa[1]);
	c=dvector(1,n);
	d=dvector(1,n);
	for (i=1;i<=n;i++) {
		if ( (dift=fabs(x-xa[i])) < dif) {
			ns=i;
			dif=dift;
		}
		c[i]=ya[i];
		d[i]=ya[i];
	}
	*y=ya[ns--];
	for (m=1;m<n;m++) {
		for (i=1;i<=n-m;i++) {
			ho=xa[i]-x;
			hp=xa[i+m]-x;
			w=c[i+1]-d[i];
			if ( (den=ho-hp) == 0.0) nrerror("Error in routine polint");
			den=w/den;
			d[i]=hp*den;
			c[i]=ho*den;
		}
		*y += (*dy=(2*ns < (n-m) ? c[ns+1] : d[ns--]));
	}
	free_dvector(d,1,n);
	free_dvector(c,1,n);
}
#undef NRANSI

/* from chp. 4 of numerical recipes */
double midpnt(double (*func)(double), double a, double b, int n){
  double x,tnm,sum,del,ddel;
  static double s;
  int it,j;

  if (n == 1) {
    return (s=(b-a)*FUNC(0.5*(a+b)));
  } else {
    for(it=1,j=1;j<n-1;j++) it *= 3;
    tnm=it;
    del=(b-a)/(3.0*tnm);
    ddel=del+del;
    x=a+0.5*del;
    sum=0.0;
    for (j=1;j<=it;j++) {
      sum += FUNC(x);
      x += ddel;
      sum += FUNC(x);
      x += del;
    }
    s=(s+(b-a)*sum/tnm)/3.0;
    return s;
  }
}

/* from chp. 4 of numerical recipes, specific for GSL functions */
double midpnt2(double (*func)(double, void *), double a, double b, int n, void * p){
  double x,tnm,sum,del,ddel;
  static double s;
  int it,j;

  if (n == 1) {
    return (s=(b-a)*FUNC2(0.5*(a+b),p));
  } else {
    for(it=1,j=1;j<n-1;j++) it *= 3;
    tnm=it;
    del=(b-a)/(3.0*tnm);
    ddel=del+del;
    x=a+0.5*del;
    sum=0.0;
    for (j=1;j<=it;j++) {
      sum += FUNC2(x,p);
      x += ddel;
      sum += FUNC2(x,p);
      x += del;
    }
    s=(s+(b-a)*sum/tnm)/3.0;
    return s;
  }
}

/* factorials */

double xchoosey(int n, int k){
  if(n<k) return(0);
  if(n == k || k == 0) return(1);
  return(gsl_sf_choose(n,k));
}

double multinom(int x, int n, ...) {
  int i, y;
  va_list ap;
  double ret_val=0.0;

  va_start(ap, n);
  for (i=0; i<n; i++) {
    y = va_arg(ap, int);
    if (x < y) return 0.0;
    ret_val *= xchoosey(x, y);
    x -= y;
    if (x < 0) return 0.0;
  }
  va_end(ap);
  return exp(ret_val);
}

//from Chuck Langley
/* +++++++++++  chl   19JAN2010  ++++++++++++++++++++++++++++++++++++++++++  */

double FsXctTst(int m11, int m12, int m21, int m22) {
	double      onetail, twotail;
	double		ep[(m11+m12+m21+m22 + 1)];
	double		cov[(m11+m12+m21+m22 + 1)];
	int			nt11,nt12,nt21,nt22 ;
	int			i ;
	if( ((m11+m21)<1) || ((m12+m22)<1) )   return(-99.0);
	nt11 = 0 ;
	do
	{
		nt12 = m11 + m12 - nt11 ;
		nt21 = m11 + m21 - nt11 ;
		nt22 = m11 + m12 + m21 + m22 - nt11 - nt12 - nt21 ;

		ep[nt11] = 0.0 ;
		cov[nt11] = 0.0 ;

		if ((nt12 < 0) || (nt21 < 0) || (nt22 < 0))
			nt11++ ;
		else
		{
			ep[nt11] = ExactProb(nt11,nt12,nt21,nt22) ;
			cov[nt11] = nt11*nt22 - nt12*nt21 ;

			/*	fprintf(stderr,"\nep[%2d] = %e\tcov[%2d] = %e", nt11,ep[nt11],nt11,cov[nt11]) ;	*/
			nt11++ ;
		}
	}	while (nt11 <= (MIN( (m11 + m12),(m11 + m21))) ) ;

	if(cov[m11] <= 0.0){
		onetail = 0.0 ;

		for(i=0;i<=m11;i++)	onetail += ep[i] ;

		twotail = onetail ;
		i = nt11 - 1 ;
		if(ep[i] <= (ep[m11] + 0.0001))
		{
			do
			{
				twotail += ep[i] ;
				i-- ;
			}	while ((ep[i] <= (ep[m11] + 0.0001)) && (i > m11)) ;
		}
	}
	else
	{
		onetail = 0.0 ;

		for(i=m11;i<nt11;i++)	onetail += ep[i] ;

		twotail = onetail ;
		i = 0 ;
		if(ep[0] <= (ep[m11] + 0.0001))
		{
			do
			{
				twotail += ep[i] ;
				i++ ;
			}	while ((ep[i] <= (ep[m11] + 0.0001)) && (i < m11)) ;
		}
	}
	if(cov[m11] == 0.0)	onetail = 99 ;

	/*	fprintf(stderr,"\n onetail = %e \t twotail = %e ", *onetail, *twotail) ;		*/
	if(twotail < MIN_P_VALUE)   printf("\nFsXctTst(%d,%d,%d,%d) = %lf", m11, m12, m21, m22, twotail);
	return(twotail) ;
}

double	ExactProb(int nn11, int nn12, int nn21, int nn22){
	double	xp = 0.0;

	xp  = logfact(nn11+nn12) ;
	xp += logfact(nn11+nn21) ;
	xp += logfact(nn12+nn22) ;
	xp += logfact(nn21+nn22) ;
	xp -= logfact(nn11+nn12+nn21+nn22) ;
	xp -= logfact(nn11) ;
	xp -= logfact(nn12) ;
	xp -= logfact(nn21) ;
	xp -= logfact(nn22) ;

	return(exp(xp)) ;
}

double	logfact(int num){
	double	sumlog = 0.0 ;

	if((num == 0) || (num == 1))	return(0.0) ;
	else
	{
		do
		{
			sumlog += log(((double)num)) ;
			num-- ;
		} while (num > 1) ;

		return(sumlog) ;
	}
    return(-99.99) ;
}

/* #define ITMAX 200 */
/* #define NRANSI */

/* void powellMin(double p[], double **xi, int n, double ftol, int *iter, double *fret, */
/* 	double (*func)(double [])) */
/* { */
/* 	void linmin(double p[], double xi[], int n, double *fret, */
/* 		float (*func)(double [])); */
/* 	int i,ibig,j; */
/* 	float del,fp,fptt,t,*pt,*ptt,*xit; */

/* 	pt=vector(1,n); */
/* 	ptt=vector(1,n); */
/* 	xit=vector(1,n); */
/* 	*fret=(*func)(p); */
/* 	for (j=1;j<=n;j++) pt[j]=p[j]; */
/* 	for (*iter=1;;++(*iter)) { */
/* 		fp=(*fret); */
/* 		ibig=0; */
/* 		del=0.0; */
/* 		for (i=1;i<=n;i++) { */
/* 			for (j=1;j<=n;j++) xit[j]=xi[j][i]; */
/* 			fptt=(*fret); */
/* 			linmin(p,xit,n,fret,func); */
/* 			if (fabs(fptt-(*fret)) > del) { */
/* 				del=fabs(fptt-(*fret)); */
/* 				ibig=i; */
/* 			} */
/* 		} */
/* 		if (2.0*fabs(fp-(*fret)) <= ftol*(fabs(fp)+fabs(*fret))) { */
/* 			free_vector(xit,1,n); */
/* 			free_vector(ptt,1,n); */
/* 			free_vector(pt,1,n); */
/* 			return; */
/* 		} */
/* 		if (*iter == ITMAX) nrerror("powell exceeding maximum iterations."); */
/* 		for (j=1;j<=n;j++) { */
/* 			ptt[j]=2.0*p[j]-pt[j]; */
/* 			xit[j]=p[j]-pt[j]; */
/* 			pt[j]=p[j]; */
/* 		} */
/* 		fptt=(*func)(ptt); */
/* 		if (fptt < fp) { */
/* 			t=2.0*(fp-2.0*(*fret)+fptt)*SQR(fp-(*fret)-del)-del*SQR(fp-fptt); */
/* 			if (t < 0.0) { */
/* 				linmin(p,xit,n,fret,func); */
/* 				for (j=1;j<=n;j++) { */
/* 					xi[j][ibig]=xi[j][n]; */
/* 					xi[j][n]=xit[j]; */
/* 				} */
/* 			} */
/* 		} */
/* 	} */
/* } */
/* #undef ITMAX */
/* #undef NRANSI */

/* #define NRANSI */
/* #define TOL 2.0e-4 */

/* int ncom; */
/* double *pcom,*xicom,(*nrfunc)(double []); */

/* void linmin(double p[], double xi[], int n, double *fret, double (*func)(double [])) */
/* { */
/* 	float brent(double ax, double bx, double cx, */
/* 		double (*f)(double), double tol, double *xmin); */
/* 	double f1dim(double x); */
/* 	void mnbrak(double *ax, double *bx, double *cx, double *fa, double *fb, */
/* 		double *fc, double (*func)(double)); */
/* 	int j; */
/* 	double xx,xmin,fx,fb,fa,bx,ax; */

/* 	ncom=n; */
/* 	pcom=vector(1,n); */
/* 	xicom=vector(1,n); */
/* 	nrfunc=func; */
/* 	for (j=1;j<=n;j++) { */
/* 		pcom[j]=p[j]; */
/* 		xicom[j]=xi[j]; */
/* 	} */
/* 	ax=0.0; */
/* 	xx=1.0; */
/* 	mnbrak(&ax,&xx,&bx,&fa,&fx,&fb,f1dim); */
/* 	*fret=brent(ax,xx,bx,f1dim,TOL,&xmin); */
/* 	for (j=1;j<=n;j++) { */
/* 		xi[j] *= xmin; */
/* 		p[j] += xi[j]; */
/* 	} */
/* 	free_vector(xicom,1,n); */
/* 	free_vector(pcom,1,n); */
/* } */
/* #undef TOL */
/* #undef NRANSI */

/* #define NRANSI */
/* #define ITMAX 100 */
/* #define CGOLD 0.3819660 */
/* #define ZEPS 1.0e-10 */
/* #define SHFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d); */

/* double brent(double ax, double bx, double cx, double (*f)(double), double tol, */
/* 	double *xmin) */
/* { */
/* 	int iter; */
/* 	double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm; */
/* 	double e=0.0; */

/* 	a=(ax < cx ? ax : cx); */
/* 	b=(ax > cx ? ax : cx); */
/* 	x=w=v=bx; */
/* 	fw=fv=fx=(*f)(x); */
/* 	for (iter=1;iter<=ITMAX;iter++) { */
/* 		xm=0.5*(a+b); */
/* 		tol2=2.0*(tol1=tol*fabs(x)+ZEPS); */
/* 		if (fabs(x-xm) <= (tol2-0.5*(b-a))) { */
/* 			*xmin=x; */
/* 			return fx; */
/* 		} */
/* 		if (fabs(e) > tol1) { */
/* 			r=(x-w)*(fx-fv); */
/* 			q=(x-v)*(fx-fw); */
/* 			p=(x-v)*q-(x-w)*r; */
/* 			q=2.0*(q-r); */
/* 			if (q > 0.0) p = -p; */
/* 			q=fabs(q); */
/* 			etemp=e; */
/* 			e=d; */
/* 			if (fabs(p) >= fabs(0.5*q*etemp) || p <= q*(a-x) || p >= q*(b-x)) */
/* 				d=CGOLD*(e=(x >= xm ? a-x : b-x)); */
/* 			else { */
/* 				d=p/q; */
/* 				u=x+d; */
/* 				if (u-a < tol2 || b-u < tol2) */
/* 					d=SIGN(tol1,xm-x); */
/* 			} */
/* 		} else { */
/* 			d=CGOLD*(e=(x >= xm ? a-x : b-x)); */
/* 		} */
/* 		u=(fabs(d) >= tol1 ? x+d : x+SIGN(tol1,d)); */
/* 		fu=(*f)(u); */
/* 		if (fu <= fx) { */
/* 			if (u >= x) a=x; else b=x; */
/* 			SHFT(v,w,x,u) */
/* 			SHFT(fv,fw,fx,fu) */
/* 		} else { */
/* 			if (u < x) a=u; else b=u; */
/* 			if (fu <= fw || w == x) { */
/* 				v=w; */
/* 				w=u; */
/* 				fv=fw; */
/* 				fw=fu; */
/* 			} else if (fu <= fv || v == x || v == w) { */
/* 				v=u; */
/* 				fv=fu; */
/* 			} */
/* 		} */
/* 	} */
/* 	nrerror("Too many iterations in brent"); */
/* 	*xmin=x; */
/* 	return fx; */
/* } */

/* #undef ITMAX */
/* #undef CGOLD */
/* #undef ZEPS */
/* #undef SHFT */
/* #undef NRANSI */

/* #define NRANSI */
/* extern int ncom; */
/* extern double *pcom,*xicom,(*nrfunc)(double []); */

/* double f1dim(double x) */
/* { */
/* 	int j; */
/* 	double f,*xt; */

/* 	xt=vector(1,ncom); */
/* 	for (j=1;j<=ncom;j++) xt[j]=pcom[j]+x*xicom[j]; */
/* 	f=(*nrfunc)(xt); */
/* 	free_vector(xt,1,ncom); */
/* 	return f; */
/* } */
/* #undef NRANSI */

max_stars_repo_name: andrewkern/pgLib

max_issues_repo_name: andrewkern/pgLib

max_forks_repo_name: andrewkern/pgLib


text:
// randomizer.h
// (C) 2013-2020 Nicholas G Davies

#ifndef RANDOMIZER_H
#define RANDOMIZER_H

#include <vector>
#include <limits>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <cmath>
#include <stdexcept>
#include <string>
#include <iostream>

class Randomizer
{
public:
    Randomizer(unsigned long int seed = 0);
    ~Randomizer();

    void Reset();

    double Uniform(double min = 0.0, double max = 1.0);
    double RoundedUniform(double min = 0.0, double max = 1.0, double shoulder = 0.01);
    double Normal(double mean = 0.0, double sd = 1.0);
    double Normal(double mean, double sd, double clamp);
    double Cauchy(double x0 = 0.0, double gamma = 1.0);
    double LogNormal(double zeta = 0.0, double sd = 1.0);
    double Exponential(double rate = 1.0);
    double Gamma(double shape, double scale);
    double Beta(double alpha, double beta);
    unsigned int Discrete(unsigned int size);
    int Discrete(int min, int max);
    // int Discrete(std::vector<unsigned int>& cumulative_weights);
    // int Discrete(std::vector<double>& cumulative_weights);
    void Multinomial(unsigned int N, std::vector<double>& p, std::vector<unsigned int>& n);
    bool Bernoulli(double p);
    unsigned int Binomial(unsigned int n, double p);
    unsigned int BetaBinomial(unsigned int n, double p, double a_plus_b);
    int Poisson(double mean);
    int Geometric(double p);
    int Round(double x);

    template <typename T>
    void Shuffle(std::vector<T>& vec);

    inline gsl_rng* GSL_RNG() { return r; }

private:
    unsigned long int seed;
    gsl_rng* r;
};

Randomizer::Randomizer(unsigned long int s)
 : seed(s), r(gsl_rng_alloc(gsl_rng_mt19937))
{
    Reset();
}

Randomizer::~Randomizer()
{
    gsl_rng_free(r);
}

void Randomizer::Reset()
{
    gsl_rng_set(r, seed);
}

double Randomizer::Uniform(double min, double max)
{
    return min + gsl_rng_uniform(r) * (max - min);
}

double Randomizer::RoundedUniform(double min, double max, double shoulder)
{
    if (min >= max)
        return min;
    double z = Uniform();
    double sd = shoulder * (max - min) / ((1 - shoulder) * 2.50662827463);
    if (z < shoulder / 2)
        return min - abs(Normal(0, sd));
    else if (z < shoulder)
        return max + abs(Normal(0, sd));
    else
        return Uniform(min, max);
}

double Randomizer::Normal(double mean, double sd)
{
    return mean + gsl_ran_gaussian_ziggurat(r, sd);
}

double Randomizer::Normal(double mean, double sd, double clamp)
{
    double n;
    do n = Normal(mean, sd); while (std::fabs(n - mean) > clamp);
    return n;
}

double Randomizer::LogNormal(double zeta, double sd)
{
    return gsl_ran_lognormal(r, zeta, sd);
}

double Randomizer::Cauchy(double x0, double gamma)
{
    return x0 + gsl_ran_cauchy(r, gamma);
}

double Randomizer::Exponential(double rate)
{
    return gsl_ran_exponential(r, 1. / rate);
}

double Randomizer::Gamma(double shape, double scale)
{
    return gsl_ran_gamma(r, shape, scale);
}

double Randomizer::Beta(double alpha, double beta)
{
    return gsl_ran_beta(r, alpha, beta);
}

unsigned int Randomizer::Discrete(unsigned int size)
{
    // TODO improve...
    if (size > gsl_rng_max(r))
        throw std::runtime_error("Generator cannot produce integers larger than " + std::to_string(gsl_rng_max(r)));
    return gsl_rng_get(r) % size;
}

int Randomizer::Discrete(int min, int max)
{
    return min + gsl_rng_uniform_int(r, max - min + 1);
}
// ///
// int Randomizer::Discrete(std::vector<unsigned int>& cumulative_weights)
// {

//     if (cumulative_weights.back() == 0)
//         return Discrete(cumulative_weights.size());
//     return std::distance(cumulative_weights.begin(),
//                 std::lower_bound(cumulative_weights.begin(),
//                     cumulative_weights.end(),
//                     Discrete(cumulative_weights.back())));
// }
// ///
// int Randomizer::Discrete(std::vector<double>& cumulative_weights)
// {
//     if (cumulative_weights.back() == 0)
//         return Discrete(cumulative_weights.size());
//     return std::distance(cumulative_weights.begin(),
//                 std::lower_bound(cumulative_weights.begin(),
//                     cumulative_weights.end(),
//                     Uniform(0.0, cumulative_weights.back())));
// }

void Randomizer::Multinomial(unsigned int N, std::vector<double>& p, std::vector<unsigned int>& n)
{
    gsl_ran_multinomial(r, p.size(), N, &p[0], &n[0]);
}

bool Randomizer::Bernoulli(double p)
{
    if (p <= 0) return false;
    if (p >= 1) return true;
    return gsl_rng_uniform(r) < p;
}

unsigned int Randomizer::Binomial(unsigned int n, double p)
{
    if (p <= 0) return 0;
    if (p >= 1) return n;
    return gsl_ran_binomial(r, p, n);
}

unsigned int Randomizer::BetaBinomial(unsigned int n, double p, double a_plus_b)
{
    if (a_plus_b > 0)
        p = gsl_ran_beta(r, a_plus_b * p, a_plus_b * (1 - p));
    return gsl_ran_binomial(r, p, n);
}

int Randomizer::Poisson(double mean)
{
    if (mean <= 0) return 0;
    return gsl_ran_poisson(r, mean);
}

int Randomizer::Geometric(double p)
{
    if (p <= 0) return 0;
    return gsl_ran_geometric(r, p);
}

int Randomizer::Round(double x)
{
    int sign = x < 0 ? -1 : 1;
    double intpart, fracpart;
    fracpart = std::modf(std::fabs(x), &intpart);
    return sign * (intpart + Bernoulli(fracpart));
}

template <typename T>
void Randomizer::Shuffle(std::vector<T>& vec)
{
    gsl_ran_shuffle(r, &vec[0], vec.size(), sizeof(vec[0]));
}

#endif

ext: h

max_stars_repo_name: nicholasdavies/covid-tiers


text:
/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */
/*
 *
 *
 *       Filename:  current_tdel.c
 *
 *    Description:  Stationary current as a function of T/D
 *
 *        Version:  1.0
 *        Created:  10/05/2014 19:52:35
 *       Revision:  none
 *        License:  BSD
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:  Università degli Studi di Trieste
 *
 *
 */

#include <gsl/gsl_ieee_utils.h>
#include <stdlib.h>
#include <stdio.h>
#include "funcs.h"
#include "initial.h"

/*
 *      FUNCTION
 *         Name:  current_red_T
 *  Description:  Given the physical parameters and the ratio x = T/Delta,
 *  		  determine the generators matrix in Redfield case
 *  		  and the stationary current.
 *
 */
double current_red_T ( double x, void* params )
{
	struct f_params* pars = (struct f_params*) params ;

	/* Calculate beta and set into pars */
	double Temp = x*D ;
	pars->beta = 1.0/Temp ;

	/* Calculate the stationary current */
	gsl_vector* stat_state = gsl_vector_calloc (4) ;
	gsl_matrix* red_m = gsl_matrix_calloc ( 4, 4 ) ;
	red_mat ( red_m, pars ) ;

	stationary ( red_m, stat_state ) ;

	double curr = -VECTOR(stat_state,3)*OMEGA/pars->omega_1 ;

	gsl_matrix_free (red_m) ;

	return curr ;
}		/* -----  end of function current_red_T  ----- */

/*
 *      FUNCTION
 *         Name:  current_cp_T
 *  Description:
 *
 */
double current_cp_T ( double x, void* params )
{
	struct f_params* pars = (struct f_params*) params ;

	/* Calculate beta and set into pars */
	double Temp = x*D ;
	pars->beta = 1.0/Temp ;

	/* Calculate the stationary current */
	gsl_vector* stat_state = gsl_vector_calloc (4) ;
	gsl_matrix* cp_m = gsl_matrix_calloc ( 4, 4 ) ;
	cp_mat ( cp_m, pars ) ;

	stationary ( cp_m, stat_state ) ;

	double curr = -VECTOR(stat_state,3)*OMEGA/pars->omega_1 ;

	gsl_matrix_free (cp_m) ;

	return curr ;
}		/* -----  end of function current_cp_T  ----- */

/*
 *      FUNCTION
 *         Name:  write_red_curr_T
 *  Description:
 *
 */
int write_red_curr_T ( void* params )
{
	FILE* f = fopen ( "RED-STAT-CURR-T.dat" , "w" ) ;

	double x = 0.00 ;

	int i ;
	for ( i = 0 ; i < 1000 ; i++ )
	{
		x += 0.001 ;
		fprintf ( f, "%.3f %.9f\n", x, current_red_T (x, params) ) ;
	}

	for ( i = 0 ; i < 900 ; i++ )
	{
		x += 0.01 ;
		fprintf ( f, "%.2f %.9f\n", x, current_red_T (x, params) ) ;
	}

	for ( i = 0 ; i < 900 ; i++ )
	{
		x += 0.1 ;
		fprintf ( f, "%.1f %.9f\n", x, current_red_T (x, params) ) ;
	}

	fclose (f) ;

	return 0;
}		/* -----  end of function write_red_curr_T  ----- */

/*
 *      FUNCTION
 *         Name:  write_cp_curr_T
 *  Description:
 *
 */
int write_cp_curr_T ( void* params )
{
	FILE* f = fopen ( "CP-STAT-CURR-T.dat" , "w" ) ;

	double x = 0.00 ;

	int i ;
	for ( i = 0 ; i < 1000 ; i++ )
	{
		x += 0.001 ;
		fprintf ( f, "%.3f %.9f\n", x, current_cp_T (x, params) ) ;
	}

	for ( i = 0 ; i < 900 ; i++ )
	{
		x += 0.01 ;
		fprintf ( f, "%.2f %.9f\n", x, current_cp_T (x, params) ) ;
	}

	for ( i = 0 ; i < 900 ; i++ )
	{
		x += 0.1 ;
		fprintf ( f, "%.1f %.9f\n", x, current_cp_T (x, params) ) ;
	}

	fclose (f) ;

	return 0;
}		/* -----  end of function write_cp_curr_T  ----- */

/*
 *      FUNCTION
 *         Name:  main
 *  Description:
 *
 */
int main ( int argc, char *argv[] )
{
	double omega_1 = gsl_hypot(OMEGA,D) ;   /* omega' */

	struct f_params params;
	params.omega_c = omega_c ;
	params.beta = 1 ;                       /* it will be changed after... */
	params.Omega = OMEGA ;
	params.omega_1 = omega_1 ;
	params.alpha = alpha ;

	int status1 = write_red_curr_T ( &params ) ;
	int status2 = write_cp_curr_T ( &params ) ;

	return status1+status2 ;
}				/* ----------  end of function main  ---------- */


text:
#include <cblas.h>
#ifdef LAPACKE_FOLDER
#include <lapacke/lapacke.h>
#else
#include <lapacke.h>
#endif

#include "math.h"
#include "minimal_opencv.h"
#include "stdbool.h"
#include "stdio.h"
#include "string.h"

#include <limits.h>
#include <stdarg.h>

#include "linmath.h"

#ifdef _WIN32
#define SURVIVE_LOCAL_ONLY
#include <malloc.h>
#define alloca _alloca
#else
#define SURVIVE_LOCAL_ONLY __attribute__((visibility("hidden")))
#endif

SURVIVE_LOCAL_ONLY int cvRound(float f) { return roundf(f); }
#define CV_Error(code, msg) assert(0 && msg); // cv::error( code, msg, CV_Func, __FILE__, __LINE__ )

const int DECOMP_SVD = 1;
const int DECOMP_LU = 2;

void print_mat(const CvMat *M);

static size_t mat_size_bytes(const CvMat *mat) { return (size_t)CV_ELEM_SIZE(mat->type) * mat->cols * mat->rows; }

SURVIVE_LOCAL_ONLY void cvCopy(const CvMat *srcarr, CvMat *dstarr, const CvMat *mask) {
	assert(mask == 0 && "This isn't implemented yet");
	assert(srcarr->rows == dstarr->rows);
	assert(srcarr->cols == dstarr->cols);
	assert(dstarr->type == srcarr->type);
	memcpy(CV_RAW_PTR(dstarr), CV_RAW_PTR(srcarr), mat_size_bytes(srcarr));
}

#ifdef USE_FLOAT
#define cblas_gemm cblas_sgemm
#define cblas_symm cblas_ssymm
#define LAPACKE_getrs LAPACKE_sgetrs
#define LAPACKE_getrf LAPACKE_sgetrf
#define LAPACKE_getri LAPACKE_sgetri
#define LAPACKE_gelss LAPACKE_sgelss
#define LAPACKE_gesvd LAPACKE_sgesvd
#else
#define cblas_gemm cblas_dgemm
#define cblas_symm cblas_dsymm
#define LAPACKE_getrs LAPACKE_dgetrs
#define LAPACKE_getrf LAPACKE_dgetrf
#define LAPACKE_getri LAPACKE_dgetri
#define LAPACKE_gelss LAPACKE_dgelss
#define LAPACKE_gesvd LAPACKE_dgesvd
#endif

// dst = alpha * src1 * src2 + beta * src3 or dst = alpha * src2 * src1 + beta * src3 where src1 is symm
SURVIVE_LOCAL_ONLY void cvSYMM(const CvMat *src1, const CvMat *src2, double alpha, const CvMat *src3, double beta,
							   CvMat *dst, bool src1First) {

	int rows1 = src1->rows;
	int cols1 = src1->cols;

	int rows2 = src2->rows;
	int cols2 = src2->cols;

	if (src3) {
		int rows3 = src3->rows;
		int cols3 = src3->cols;
		assert(rows3 == dst->rows);
		assert(cols3 == dst->cols);
	}

	// assert(src3 == 0 || beta != 0);
	assert(cols1 == rows2);
	assert(rows1 == dst->rows);
	assert(cols2 == dst->cols);

	lapack_int lda = src1->cols;
	lapack_int ldb = src2->cols;

	if (src3)
		cvCopy(src3, dst, 0);
	else
		beta = 0;

	assert(CV_RAW_PTR(dst) != CV_RAW_PTR(src1));
	assert(CV_RAW_PTR(dst) != CV_RAW_PTR(src2));
	/*
		void cblas_dsymm(OPENBLAS_CONST enum CBLAS_ORDER Order,
						 OPENBLAS_CONST enum CBLAS_SIDE Side,
						 OPENBLAS_CONST enum CBLAS_UPLO Uplo,
						 OPENBLAS_CONST blasint M,
						 OPENBLAS_CONST blasint N,
						 OPENBLAS_CONST double alpha,
						 OPENBLAS_CONST double *A,
						 OPENBLAS_CONST blasint lda,
						 OPENBLAS_CONST double *B,
						 OPENBLAS_CONST blasint ldb,
						 OPENBLAS_CONST double beta,
						 double *C,
						 OPENBLAS_CONST blasint ldc);
	*/
	cblas_symm(CblasRowMajor, src1First ? CblasLeft : CblasRight, CblasUpper, dst->rows, dst->cols, alpha,
			   CV_RAW_PTR(src1), lda, CV_RAW_PTR(src2), ldb, beta, CV_RAW_PTR(dst), dst->cols);
}

// Special case dst = alpha * src2 * src1 * src2' + beta * src3
void mulBABt(const CvMat *src1, const CvMat *src2, double alpha, const CvMat *src3, double beta, CvMat *dst) {
	size_t dims = src2->rows;
	assert(src2->cols == src2->rows);
	CREATE_STACK_MAT(tmp, dims, dims);

	// This has been profiled; and weirdly enough the SYMM version is slower for a 19x19 matrix. Guessing access order
	// or some other cache thing matters more than the additional 2x multiplications.
//#define USE_SYM
#ifdef USE_SYM
	cvSYMM(src1, src2, 1, 0, 0, &tmp, false);
	cvGEMM(&tmp, src2, alpha, src3, beta, dst, CV_GEMM_B_T);
#else
	cvGEMM(src1, src2, 1, 0, 0, &tmp, CV_GEMM_B_T);
	cvGEMM(src2, &tmp, alpha, src3, beta, dst, 0);
#endif
}
// dst = alpha * src1 * src2 + beta * src3
SURVIVE_LOCAL_ONLY void cvGEMM(const CvMat *src1, const CvMat *src2, double alpha, const CvMat *src3, double beta,
							   CvMat *dst, int tABC) {

	int rows1 = (tABC & CV_GEMM_A_T) ? src1->cols : src1->rows;
	int cols1 = (tABC & CV_GEMM_A_T) ? src1->rows : src1->cols;

	int rows2 = (tABC & CV_GEMM_B_T) ? src2->cols : src2->rows;
	int cols2 = (tABC & CV_GEMM_B_T) ? src2->rows : src2->cols;

	if (src3) {
		int rows3 = (tABC & CV_GEMM_C_T) ? src3->cols : src3->rows;
		int cols3 = (tABC & CV_GEMM_C_T) ? src3->rows : src3->cols;
		assert(rows3 == dst->rows);
		assert(cols3 == dst->cols);
	}

	// assert(src3 == 0 || beta != 0);
	assert(cols1 == rows2);
    assert(rows1 == dst->rows);
    assert(cols2 == dst->cols);

	lapack_int lda = src1->cols;
	lapack_int ldb = src2->cols;

	if (src3)
		cvCopy(src3, dst, 0);
	else
		beta = 0;

	assert(CV_RAW_PTR(dst) != CV_RAW_PTR(src1));
	assert(CV_RAW_PTR(dst) != CV_RAW_PTR(src2));

	cblas_gemm(CblasRowMajor, (tABC & CV_GEMM_A_T) ? CblasTrans : CblasNoTrans,
			   (tABC & CV_GEMM_B_T) ? CblasTrans : CblasNoTrans, dst->rows, dst->cols, cols1, alpha, CV_RAW_PTR(src1),
			   lda, CV_RAW_PTR(src2), ldb, beta, CV_RAW_PTR(dst), dst->cols);
}

// dst = scale * src ^ t * src     iff order == 1
// dst = scale *     src * src ^ t iff order == 0
SURVIVE_LOCAL_ONLY void cvMulTransposed(const CvMat *src, CvMat *dst, int order, const CvMat *delta, double scale) {
	lapack_int rows = src->rows;
	lapack_int cols = src->cols;

	lapack_int drows = order == 0 ? dst->rows : dst->cols;
	assert(drows == dst->cols);
	assert(order == 1 ? (dst->cols == src->cols) : (dst->cols == src->rows));
	assert(delta == 0 && "This isn't implemented yet");
	double beta = 0;

	bool isAT = order == 1;
	bool isBT = !isAT;

	lapack_int dstCols = dst->cols;

	cblas_gemm(CblasRowMajor, isAT ? CblasTrans : CblasNoTrans, isBT ? CblasTrans : CblasNoTrans, dst->rows, dst->cols,
			   order == 1 ? src->rows : src->cols, scale, CV_RAW_PTR(src), src->cols, CV_RAW_PTR(src), src->cols, beta,
			   CV_RAW_PTR(dst), dstCols);
}

SURVIVE_LOCAL_ONLY void *cvAlloc(size_t size) { return malloc(size); }

static void icvCheckHuge(CvMat *arr) {
	if ((int64_t)arr->step * arr->rows > INT_MAX)
		arr->type &= ~CV_MAT_CONT_FLAG;
}

static CvMat *cvInitMatHeader(CvMat *arr, int rows, int cols, int type) {
	type = CV_MAT_TYPE(type);

	assert(!(rows < 0 || cols < 0));

	int min_step = CV_ELEM_SIZE(type);
	assert(!(min_step <= 0));
	min_step *= cols;

	arr->step = min_step;
	arr->type = CV_MAT_MAGIC_VAL | type | CV_MAT_CONT_FLAG;
	arr->rows = rows;
	arr->cols = cols;
	arr->data.ptr = 0;
	arr->refcount = 0;
	arr->hdr_refcount = 1;

	icvCheckHuge(arr);
	return arr;
}

SURVIVE_LOCAL_ONLY CvMat *cvCreateMatHeader(int rows, int cols, int type) {
	return cvInitMatHeader((CvMat *)cvAlloc(sizeof(CvMat)), rows, cols, type);
}

/* the alignment of all the allocated buffers */
#define CV_MALLOC_ALIGN 16

/* IEEE754 constants and macros */
#define CV_TOGGLE_FLT(x) ((x) ^ ((int)(x) < 0 ? 0x7fffffff : 0))
#define CV_TOGGLE_DBL(x) ((x) ^ ((int64)(x) < 0 ? CV_BIG_INT(0x7fffffffffffffff) : 0))

#define CV_DbgAssert assert

static inline void *cvAlignPtr(const void *ptr, int align) {
	CV_DbgAssert((align & (align - 1)) == 0);
	return (void *)(((size_t)ptr + align - 1) & ~(size_t)(align - 1));
}

SURVIVE_LOCAL_ONLY void cvCreateData(CvMat *arr) {
	if (CV_IS_MAT_HDR_Z(arr)) {
		size_t step, total_size;
		CvMat *mat = (CvMat *)arr;
		step = mat->step;

		if (mat->rows == 0 || mat->cols == 0)
			return;

		if (mat->data.ptr != 0)
			CV_Error(CV_StsError, "Data is already allocated");

		if (step == 0)
			step = CV_ELEM_SIZE(mat->type) * mat->cols;

		int64_t _total_size = (int64_t)step * mat->rows + sizeof(int) + CV_MALLOC_ALIGN;
		total_size = (size_t)_total_size;
		if (_total_size != (int64_t)total_size)
			CV_Error(CV_StsNoMem, "Too big buffer is allocated");
		mat->refcount = (int *)cvAlloc((size_t)total_size);
		mat->data.ptr = (uchar *)cvAlignPtr(mat->refcount + 1, CV_MALLOC_ALIGN);
		*mat->refcount = 1;
	} else if (CV_IS_MATND_HDR(arr)) {
		assert("There is no support for ND types");
	} else
		CV_Error(CV_StsBadArg, "unrecognized or unsupported array type");
}

#define CV_CREATE_MAT_HEADER_ALLOCA(stack_mat, rows, cols, type)                                                       \
	CvMat *stack_mat = cvInitMatHeader(alloca(sizeof(CvMat)), rows, cols, type);

#define CV_CREATE_MAT_ALLOCA(stack_mat, height, width, type)                                                           \
	CV_CREATE_MAT_HEADER_ALLOCA(stack_mat, height, width, type);                                                       \
	stack_mat->data.ptr = alloca(mat_size_bytes(stack_mat));

SURVIVE_LOCAL_ONLY CvMat *cvCreateMat(int height, int width, int type) {
	CvMat *arr = cvCreateMatHeader(height, width, type);
	cvCreateData(arr);

	return arr;
}

#define CREATE_CV_STACK_MAT(name, rows, cols, type)		\
  FLT *_##name = alloca(rows * cols * sizeof(FLT));		\
  CvMat name = cvMat(rows, cols, SURVIVE_CV_F, _##name);

SURVIVE_LOCAL_ONLY double cvInvert(const CvMat *srcarr, CvMat *dstarr, int method) {
	lapack_int inf;
	lapack_int rows = srcarr->rows;
	lapack_int cols = srcarr->cols;
	lapack_int lda = srcarr->cols;

	cvCopy(srcarr, dstarr, 0);
	FLT *a = CV_RAW_PTR(dstarr);

#ifdef DEBUG_PRINT
	printf("a: \n");
	print_mat(srcarr);
#endif
	if (method == DECOMP_LU) {
		lapack_int *ipiv = alloca(sizeof(lapack_int) * MIN(srcarr->rows, srcarr->cols));
		inf = LAPACKE_getrf(LAPACK_ROW_MAJOR, rows, cols, a, lda, ipiv);
		assert(inf == 0);

		inf = LAPACKE_getri(LAPACK_ROW_MAJOR, rows, a, lda, ipiv);
		assert(inf >= 0);
		if (inf > 0) {
			printf("Warning: Singular matrix: \n");
			// print_mat(srcarr);
		}

		//free(ipiv);

	} else if (method == DECOMP_SVD) {
		// TODO: There is no way this needs this many allocations,
		// but in my defense I was very tired when I wrote this code
	  CREATE_CV_STACK_MAT(w, 1, MIN(dstarr->rows, dstarr->cols), dstarr->type);
	  CREATE_CV_STACK_MAT(u, dstarr->cols, dstarr->cols, dstarr->type);
	  CREATE_CV_STACK_MAT(v, dstarr->rows, dstarr->rows, dstarr->type);
	  CREATE_CV_STACK_MAT(um, w.cols, w.cols, w.type);

		cvSVD(dstarr, &w, &u, &v, 0);

		cvSetZero(&um);
		for (int i = 0; i < w.cols; i++) {
			cvmSet(&um, i, i, 1. / (_w)[i]);
		}

		CvMat *tmp = cvCreateMat(dstarr->cols, dstarr->rows, dstarr->type);
		cvGEMM(&v, &um, 1, 0, 0, tmp, CV_GEMM_A_T);
		cvGEMM(tmp, &u, 1, 0, 0, dstarr, CV_GEMM_B_T);

		cvReleaseMat(&tmp);
	} else {
		assert(0 && "Bad argument");
		return -1;
	}

	return 0;
}

#define CV_CLONE_MAT_ALLOCA(stack_mat, mat)                                                                            \
	CV_CREATE_MAT_ALLOCA(stack_mat, mat->rows, mat->cols, mat->type)                                                   \
	cvCopy(mat, stack_mat, 0);

SURVIVE_LOCAL_ONLY CvMat *cvCloneMat(const CvMat *mat) {
	CvMat *rtn = cvCreateMat(mat->rows, mat->cols, mat->type);
	cvCopy(mat, rtn, 0);
	return rtn;
}

SURVIVE_LOCAL_ONLY int cvSolve(const CvMat *Aarr, const CvMat *xarr, CvMat *Barr, int method) {
	lapack_int inf;
	lapack_int arows = Aarr->rows;
	lapack_int acols = Aarr->cols;
	lapack_int xcols = xarr->cols;
	lapack_int xrows = xarr->rows;
	lapack_int lda = acols; // Aarr->step / sizeof(double);
	lapack_int type = CV_MAT_TYPE(Aarr->type);

	if (method == DECOMP_LU) {
		assert(Aarr->cols == Barr->rows);
		assert(xarr->rows == Aarr->rows);
		assert(Barr->cols == xarr->cols);
		assert(type == CV_MAT_TYPE(Barr->type) && (type == CV_32F || type == CV_64F));

		cvCopy(xarr, Barr, 0);
		FLT *a_ws = alloca(mat_size_bytes(Aarr));
		memcpy(a_ws, CV_RAW_PTR(Aarr), mat_size_bytes(Aarr));

		lapack_int brows = Barr->rows;
		lapack_int bcols = Barr->cols;
		lapack_int ldb = bcols; // Barr->step / sizeof(double);

		lapack_int *ipiv = alloca(sizeof(lapack_int) * MIN(Aarr->rows, Aarr->cols));

		inf = LAPACKE_getrf(LAPACK_ROW_MAJOR, arows, acols, (a_ws), lda, ipiv);
		assert(inf >= 0);
		if (inf > 0) {
			printf("Warning: Singular matrix: \n");
			// print_mat(a_ws);
		}

#ifdef DEBUG_PRINT
		printf("Solve A * x = B:\n");
		// print_mat(a_ws);
		print_mat(Barr);
#endif

		inf = LAPACKE_getrs(LAPACK_ROW_MAJOR, CblasNoTrans, arows, bcols, (a_ws), lda, ipiv, CV_RAW_PTR(Barr), ldb);
		assert(inf == 0);

		//free(ipiv);
		// cvReleaseMat(&a_ws);
	} else if (method == DECOMP_SVD) {

#ifdef DEBUG_PRINT
		printf("Solve |b - A * x|:\n");
		print_mat(Aarr);
		print_mat(xarr);
#endif
		bool xLargerThanB = xarr->rows > acols;
		CvMat *xCpy = 0;
		if (xLargerThanB) {
			CV_CLONE_MAT_ALLOCA(xCpyStack, xarr);
			xCpy = xCpyStack;
		} else {
			xCpy = Barr;
			memcpy(CV_RAW_PTR(Barr), CV_RAW_PTR(xarr), mat_size_bytes(xarr));
		}

		// CvMat *aCpy = cvCloneMat(Aarr);
		FLT *aCpy = alloca(mat_size_bytes(Aarr));
		memcpy(aCpy, CV_RAW_PTR(Aarr), mat_size_bytes(Aarr));

		FLT *S = alloca(sizeof(FLT) * MIN(arows, acols));
		// FLT *S = malloc(sizeof(FLT) * MIN(arows, acols));
		FLT rcond = -1;
		lapack_int *rank = alloca(sizeof(lapack_int) * MIN(arows, acols));
		// lapack_int *rank = malloc(sizeof(lapack_int) * MIN(arows, acols));
		lapack_int inf = LAPACKE_gelss(LAPACK_ROW_MAJOR, arows, acols, xcols, (aCpy), acols, CV_RAW_PTR(xCpy), xcols, S,
									   rcond, rank);
		assert(Barr->rows == acols);
		assert(Barr->cols == xCpy->cols);

		if (xLargerThanB) {
			xCpy->rows = acols;
			cvCopy(xCpy, Barr, 0);
			// cvReleaseMat(&xCpy);
		}

		// cvReleaseMat(&aCpy);
#ifdef DEBUG_PRINT
		print_mat(Barr);
#endif
		assert(inf == 0);
	}
	return 0;
}

SURVIVE_LOCAL_ONLY void cvTranspose(const CvMat *M, CvMat *dst) {
	bool inPlace = M == dst || CV_RAW_PTR(M) == CV_RAW_PTR(dst);
	FLT *src = CV_RAW_PTR(M);

	if (inPlace) {
		src = alloca(mat_size_bytes(M));
		memcpy(src, CV_RAW_PTR(M), mat_size_bytes(M));
	} else {
	  assert(M->rows == dst->cols);
	  assert(M->cols == dst->rows);
	}

	for (unsigned i = 0; i < M->rows; i++) {
		for (unsigned j = 0; j < M->cols; j++) {
			CV_RAW_PTR(dst)[j * M->rows + i] = src[i * M->cols + j];
		}
	}
}

SURVIVE_LOCAL_ONLY void cvSVD(CvMat *aarr, CvMat *warr, CvMat *uarr, CvMat *varr, int flags) {
	char jobu = 'A';
	char jobvt = 'A';

	lapack_int inf;

	if ((flags & CV_SVD_MODIFY_A) == 0) {
		aarr = cvCloneMat(aarr);
	}

	if (uarr == 0)
		jobu = 'N';
	if (varr == 0)
		jobvt = 'N';

	FLT *pw, *pu, *pv;
	lapack_int arows = aarr->rows, acols = aarr->cols;

	pw = warr ? CV_RAW_PTR(warr) : (FLT *)alloca(sizeof(FLT) * arows * acols);
	pu = uarr ? CV_RAW_PTR(uarr) : (FLT *)alloca(sizeof(FLT) * arows * arows);
	pv = varr ? CV_RAW_PTR(varr) : (FLT *)alloca(sizeof(FLT) * acols * acols);

	lapack_int ulda = uarr ? uarr->cols : acols;
	lapack_int plda = varr ? varr->cols : acols;

	FLT *superb = alloca(sizeof(FLT) * MIN(arows, acols));
	inf = LAPACKE_gesvd(LAPACK_ROW_MAJOR, jobu, jobvt, arows, acols, CV_RAW_PTR(aarr), acols, pw, pu, ulda, pv, plda,
						superb);

	// free(superb);

	switch (inf) {
	case -6:
		assert(false && "matrix has NaNs");
		break;
	case 0:
		break;
	default:
		assert(inf == 0);
	}

	if (uarr && (flags & CV_SVD_U_T)) {
		cvTranspose(uarr, uarr);
	}

	if (varr && (flags & CV_SVD_V_T) == 0) {
		cvTranspose(varr, varr);
	}

	if ((flags & CV_SVD_MODIFY_A) == 0) {
		cvReleaseMat(&aarr);
	}
}

SURVIVE_LOCAL_ONLY void cvSetZero(CvMat *arr) {
	for (int i = 0; i < arr->rows; i++)
		for (int j = 0; j < arr->cols; j++)
			CV_RAW_PTR(arr)[i * arr->cols + j] = 0;
}
SURVIVE_LOCAL_ONLY void cvSetIdentity(CvMat *arr) {
	for (int i = 0; i < arr->rows; i++)
		for (int j = 0; j < arr->cols; j++)
			CV_RAW_PTR(arr)[i * arr->cols + j] = i == j;
}

SURVIVE_LOCAL_ONLY void cvReleaseMat(CvMat **mat) {
	assert(*(*mat)->refcount == 1);
	free((*mat)->refcount);
	free(*mat);
	*mat = 0;
}

SURVIVE_LOCAL_ONLY double cvDet(const CvMat *M) {
	assert(M->rows == M->cols);
	assert(M->rows <= 3 && "cvDet unimplemented for matrices >3");

	FLT *m = CV_RAW_PTR(M);

	switch (M->rows) {
	case 1:
		return m[0];
	case 2: {
		return m[0] * m[3] - m[1] * m[2];
	}
	case 3: {
		FLT m00 = m[0], m01 = m[1], m02 = m[2], m10 = m[3], m11 = m[4], m12 = m[5], m20 = m[6], m21 = m[7], m22 = m[8];

		return m00 * (m11 * m22 - m12 * m21) - m01 * (m10 * m22 - m12 * m20) + m02 * (m10 * m21 - m11 * m20);
	}
	default:
		abort();
	}
}

max_stars_repo_name: tiiuae/libsurvive

max_issues_repo_name: tiiuae/libsurvive

max_forks_repo_name: tiiuae/libsurvive


text:
/*
 * Licensed to the OpenAirInterface (OAI) Software Alliance under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The OpenAirInterface Software Alliance licenses this file to You under
 * the OAI Public License, Version 1.0  (the "License"); you may not use this file
 * except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.openairinterface.org/?page_id=698
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *-------------------------------------------------------------------------------
 * For more information about the OpenAirInterface (OAI) Software Alliance:
 *      contact@openairinterface.org
 */

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

#include "PHY/TOOLS/defs.h"
#include "defs.h"

// NEW code with lookup table for sin/cos based on delay profile (TO BE TESTED)

double **cos_lut=NULL,**sin_lut=NULL;

//#if 1

int init_freq_channel(channel_desc_t *desc,uint16_t nb_rb,int16_t n_samples)
{

  double delta_f,freq;  // 90 kHz spacing
  double delay;
  int16_t f;
  uint8_t l;

  if ((n_samples&1)==0) {
    fprintf(stderr, "freq_channel_init: n_samples has to be odd\n");
    return(-1);
  }

  cos_lut = (double **)malloc(n_samples*sizeof(double*));
  sin_lut = (double **)malloc(n_samples*sizeof(double*));

  delta_f = nb_rb*180000/(n_samples-1);

  for (f=-(n_samples>>1); f<=(n_samples>>1); f++) {
    freq=delta_f*(double)f*1e-6;// due to the fact that delays is in mus

    cos_lut[f+(n_samples>>1)] = (double *)malloc((int)desc->nb_taps*sizeof(double));
    sin_lut[f+(n_samples>>1)] = (double *)malloc((int)desc->nb_taps*sizeof(double));

    for (l=0; l<(int)desc->nb_taps; l++) {
      if (desc->nb_taps==1)
        delay = desc->delays[l];
      else
        delay = desc->delays[l]+NB_SAMPLES_CHANNEL_OFFSET/desc->sampling_rate;

      cos_lut[f+(n_samples>>1)][l] = cos(2*M_PI*freq*delay);
      sin_lut[f+(n_samples>>1)][l] = sin(2*M_PI*freq*delay);
      //printf("values cos:%d, sin:%d\n", cos_lut[f][l], sin_lut[f][l]);

    }
  }

  return(0);
}

int freq_channel(channel_desc_t *desc,uint16_t nb_rb,int16_t n_samples)
{

  int16_t f,f2,d;
  uint8_t aarx,aatx,l;
  double *clut,*slut;
  static int freq_channel_init=0;
  static int n_samples_max=0;

  // do some error checking
  // n_samples has to be a odd number because we assume the spectrum is symmetric around the DC and includes the DC
  if ((n_samples&1)==0) {
    fprintf(stderr, "freq_channel: n_samples has to be odd\n");
    return(-1);
  }

  // printf("no of taps:%d,",(int)desc->nb_taps);

  if (freq_channel_init == 0) {
    // we are initializing the lut for the largets possible n_samples=12*nb_rb+1
    // if called with n_samples<12*nb_rb+1, we decimate the lut
    n_samples_max=12*nb_rb+1;
    if (init_freq_channel(desc,nb_rb,n_samples_max)==0)
      freq_channel_init=1;
    else
      return(-1);
  }

  d=(n_samples_max-1)/(n_samples-1);

  //printf("no_samples=%d, n_samples_max=%d, d=%d\n",n_samples,n_samples_max,d);

  start_meas(&desc->interp_freq);

  for (f=-n_samples_max/2,f2=-n_samples/2; f<n_samples_max/2; f+=d,f2++) {
    clut = cos_lut[n_samples_max/2+f];
    slut = sin_lut[n_samples_max/2+f];

    for (aarx=0; aarx<desc->nb_rx; aarx++) {
      for (aatx=0; aatx<desc->nb_tx; aatx++) {
        desc->chF[aarx+(aatx*desc->nb_rx)][n_samples/2+f2].x=0.0;
        desc->chF[aarx+(aatx*desc->nb_rx)][n_samples/2+f2].y=0.0;

        for (l=0; l<(int)desc->nb_taps; l++) {

          desc->chF[aarx+(aatx*desc->nb_rx)][n_samples/2+f2].x+=(desc->a[l][aarx+(aatx*desc->nb_rx)].x*clut[l]+
              desc->a[l][aarx+(aatx*desc->nb_rx)].y*slut[l]);
          desc->chF[aarx+(aatx*desc->nb_rx)][n_samples/2+f2].y+=(-desc->a[l][aarx+(aatx*desc->nb_rx)].x*slut[l]+
              desc->a[l][aarx+(aatx*desc->nb_rx)].y*clut[l]);
        }
      }
    }
  }

  stop_meas(&desc->interp_freq);

  return(0);
}

double compute_pbch_sinr(channel_desc_t *desc,
                         channel_desc_t *desc_i1,
                         channel_desc_t *desc_i2,
                         double snr_dB,double snr_i1_dB,
                         double snr_i2_dB,
                         uint16_t nb_rb)
{

  double avg_sinr,snr=pow(10.0,.1*snr_dB),snr_i1=pow(10.0,.1*snr_i1_dB),snr_i2=pow(10.0,.1*snr_i2_dB);
  uint16_t f;
  uint8_t aarx,aatx;
  double S;
  struct complex S_i1;
  struct complex S_i2;

  avg_sinr=0.0;

  //  printf("nb_rb %d\n",nb_rb);
  for (f=(nb_rb-6); f<(nb_rb+6); f++) {
    S = 0.0;
    S_i1.x =0.0;
    S_i1.y =0.0;
    S_i2.x =0.0;
    S_i2.y =0.0;

    for (aarx=0; aarx<desc->nb_rx; aarx++) {
      for (aatx=0; aatx<desc->nb_tx; aatx++) {
        S    += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc->chF[aarx+(aatx*desc->nb_rx)][f].x +
                 desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc->chF[aarx+(aatx*desc->nb_rx)][f].y);
        //  printf("%d %d chF[%d] => (%f,%f)\n",aarx,aatx,f,desc->chF[aarx+(aatx*desc->nb_rx)][f].x,desc->chF[aarx+(aatx*desc->nb_rx)][f].y);

        if (desc_i1) {
          S_i1.x += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].x +
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].y);
          S_i1.y += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].y -
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].x);
        }

        if (desc_i2) {
          S_i2.x += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].x +
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].y);
          S_i2.y += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].y -
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].x);
        }
      }
    }

    //    printf("snr %f f %d : S %f, S_i1 %f, S_i2 %f\n",snr,f-nb_rb,S,snr_i1*sqrt(S_i1.x*S_i1.x + S_i1.y*S_i1.y),snr_i2*sqrt(S_i2.x*S_i2.x + S_i2.y*S_i2.y));
    avg_sinr += (snr*S/(desc->nb_tx+snr_i1*sqrt(S_i1.x*S_i1.x + S_i1.y*S_i1.y)+snr_i2*sqrt(S_i2.x*S_i2.x + S_i2.y*S_i2.y)));
  }

  //  printf("avg_sinr %f (%f,%f,%f)\n",avg_sinr/12.0,snr,snr_i1,snr_i2);
  return(10*log10(avg_sinr/12.0));
}

double compute_sinr(channel_desc_t *desc,
                    channel_desc_t *desc_i1,
                    channel_desc_t *desc_i2,
                    double snr_dB,double snr_i1_dB,
                    double snr_i2_dB,
                    uint16_t nb_rb)
{

  double avg_sinr,snr=pow(10.0,.1*snr_dB),snr_i1=pow(10.0,.1*snr_i1_dB),snr_i2=pow(10.0,.1*snr_i2_dB);
  uint16_t f;
  uint8_t aarx,aatx;
  double S;
  struct complex S_i1;
  struct complex S_i2;

  DevAssert( nb_rb > 0 );

  avg_sinr=0.0;

  //  printf("nb_rb %d\n",nb_rb);
  for (f=0; f<2*nb_rb; f++) {
    S = 0.0;
    S_i1.x =0.0;
    S_i1.y =0.0;
    S_i2.x =0.0;
    S_i2.y =0.0;

    for (aarx=0; aarx<desc->nb_rx; aarx++) {
      for (aatx=0; aatx<desc->nb_tx; aatx++) {
        S    += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc->chF[aarx+(aatx*desc->nb_rx)][f].x +
                 desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc->chF[aarx+(aatx*desc->nb_rx)][f].y);

        if (desc_i1) {
          S_i1.x += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].x +
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].y);
          S_i1.y += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].y -
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].x);
        }

        if (desc_i2) {
          S_i2.x += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].x +
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].y);
          S_i2.y += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].y -
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].x);
        }
      }
    }

    //        printf("f %d : S %f, S_i1 %f, S_i2 %f\n",f-nb_rb,snr*S,snr_i1*sqrt(S_i1.x*S_i1.x + S_i1.y*S_i1.y),snr_i2*sqrt(S_i2.x*S_i2.x + S_i2.y*S_i2.y));
    avg_sinr += (snr*S/(desc->nb_tx+snr_i1*sqrt(S_i1.x*S_i1.x + S_i1.y*S_i1.y)+snr_i2*sqrt(S_i2.x*S_i2.x + S_i2.y*S_i2.y)));
  }

  //  printf("avg_sinr %f (%f,%f,%f)\n",avg_sinr/12.0,snr,snr_i1,snr_i2);
  return(10*log10(avg_sinr/(nb_rb*2)));
}

int pbch_polynomial_degree=6;
double pbch_awgn_polynomial[7]= {-7.2926e-05, -2.8749e-03, -4.5064e-02, -3.5301e-01, -1.4655e+00, -3.6282e+00, -6.6907e+00};

void load_pbch_desc(FILE *pbch_file_fd)
{

  int i, ret;
  char dummy[25];

  ret = fscanf(pbch_file_fd,"%d",&pbch_polynomial_degree);

  if (ret < 0) {
    printf("fscanf failed: %s\n", strerror(errno));
    exit(EXIT_FAILURE);
  }

  if (pbch_polynomial_degree>6) {
    printf("Illegal degree for pbch interpolation polynomial %d\n",pbch_polynomial_degree);
    exit(-1);
  }

  printf("PBCH polynomial : ");

  for (i=0; i<=pbch_polynomial_degree; i++) {
    ret = fscanf(pbch_file_fd,"%s",dummy);

    if (ret < 0) {
      printf("fscanf failed: %s\n", strerror(errno));
      exit(EXIT_FAILURE);
    }

    pbch_awgn_polynomial[i] = strtod(dummy,NULL);
    printf("%f ",pbch_awgn_polynomial[i]);
  }

  printf("\n");
}

double pbch_bler(double sinr)
{

  int i;
  double log10_bler=pbch_awgn_polynomial[pbch_polynomial_degree];
  double sinrpow=sinr;
  double bler=0.0;

  //  printf("log10bler %f\n",log10_bler);
  if (sinr<-10.0)
    bler= 1.0;
  else if (sinr>=0.0)
    bler=0.0;
  else  {
    for (i=1; i<=pbch_polynomial_degree; i++) {
      //    printf("sinrpow %f\n",sinrpow);
      log10_bler += (pbch_awgn_polynomial[pbch_polynomial_degree-i]*sinrpow);
      sinrpow *= sinr;
      //    printf("log10bler %f\n",log10_bler);
    }

    bler = pow(10.0,log10_bler);
  }

  //printf ("sinr %f bler %f\n",sinr,bler);
  return(bler);

}


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

#ifndef __NUPM_DAX_DATA_H__
#define __NUPM_DAX_DATA_H__

#include <libpmem.h>
#include <city.h> /* CityHash */
#include <common/pointer_cast.h>
#include <common/string_view.h>
#include <common/types.h>
#include <common/utils.h>
#include <boost/icl/split_interval_map.hpp>
#include <gsl/span>
#include <algorithm>
#include <list>
#include <stdexcept>

namespace
{
	std::uint64_t make_uuid(const common::string_view name_)
	{
		if ( 255 < name_.size() )
		{
			throw std::invalid_argument("invalid file name (too long)");
		}
		auto region_id = ::CityHash64(name_.begin(), name_.size());
		if (region_id == 0) throw std::invalid_argument("invalid region_id (and extreme bad luck!)");
		return region_id;
	}
}

namespace nupm
{
static std::uint32_t constexpr DM_REGION_MAGIC = 0xC0070000;
static unsigned constexpr DM_REGION_NAME_MAX_LEN = 1024;
static std::uint32_t constexpr DM_REGION_VERSION = 3;
static unsigned constexpr dm_region_log_grain_size = DM_REGION_LOG_GRAIN_SIZE; // log2 granularity (CMake default is 25, i.e. 32 MiB)

class DM_undo_log {
  static constexpr unsigned MAX_LOG_COUNT = 4;
  static constexpr unsigned MAX_LOG_SIZE  = 272;
  struct log_entry_t {
    byte   log[MAX_LOG_SIZE];
    void * ptr;
    size_t length; /* zero indicates log freed */
  };

  void log(void *ptr, size_t length)
  {
    assert(length > 0);
    assert(ptr);

    if (length > MAX_LOG_SIZE)
      throw std::invalid_argument("log length exceeds max. space");

    for (unsigned i = 0; i < MAX_LOG_COUNT; i++) {
      if (_log[i].length == 0) {
        _log[i].length = length;
        _log[i].ptr    = ptr;
        pmem_memcpy_nodrain(_log[i].log, ptr, length);
        // TODO
        //memcpy(_log[i].log, ptr, length);
        //mem_flush(&_log[i], sizeof(log_entry_t));
        return;
      }
    }
    throw API_exception("undo log full");
  }

 public:
  template <typename T>
    void log(T *ptr)
    {
      static_assert(sizeof *ptr <= MAX_LOG_SIZE, "object too big to log");
      log(ptr, sizeof *ptr);
    }

  void clear_log()
  {
    for (unsigned i = 0; i < MAX_LOG_COUNT; i++) _log[i].length = 0;
  }

  void check_and_undo()
  {
    for (unsigned i = 0; i < MAX_LOG_COUNT; i++) {
      if (_log[i].length > 0) {
        PLOG("undo log being applied (ptr=%p, len=%lu).", _log[i].ptr,
             _log[i].length);
        // TODO
        pmem_memcpy_persist(_log[i].ptr, _log[i].log, _log[i].length);
        //memcpy(_log[i].ptr, _log[i].log, _log[i].length);
        //        mem_flush_nodrain(_log[i].ptr, _log[i].length);
        _log[i].length = 0;
      }
    }
  }

 private:
  log_entry_t _log[MAX_LOG_COUNT];
} __attribute__((packed));

class DM_region {
public:
  /* "grain" is the unit of suballocation within a region. It is a fixed power
   * of 2 common to all regions in the devdax namespace.
   */
  using grain_offset_t = uint32_t;
  grain_offset_t offset_grain;
  grain_offset_t length_grain;
  uint64_t region_id;
  /* File name. Saved and returned.
   * Internal logic uses region_id (file name hash) to reduce code changes.
   */
  char file_name[256];

 public:
  /* re-zeroing constructor */
  DM_region() : offset_grain(0), length_grain(0), region_id(0) { assert(check_aligned(this, 8)); }

  void initialize(size_t space_size, std::size_t grain_size)
  {
    offset_grain = 0;
    length_grain = boost::numeric_cast<uint32_t>(space_size / grain_size);
    region_id = 0; /* zero indicates free */
  }

  friend class DM_region_header;
} __attribute__((packed));

/* Note: "region" has at least two meanings:
 *  1. The space starting with, and described by, DM_region_header. ndctl calls this a namespace.
 e  2. The space described by DM_region
 */
class DM_region_header {
 private:
  static constexpr uint16_t DEFAULT_MAX_REGIONS = 1024;
  using string_view = common::string_view;

  uint32_t    _magic;         // 4
  uint32_t    _version;       // 8
  uint64_t    _device_size;   // 16
  uint32_t    _region_count;  // 20
  uint16_t    _log_grain_size; // 22
  uint16_t    _resvd;         // 24
  uint8_t     _padding[40];   // 64
  DM_undo_log _undo_log;

  /*
   * Following this data, there is
   * 1. region_table_base, immediately following (this + 1)
   * 2. arena_base, at ptr_cast<byte>(this) + grain_size. Code assumes
   *    (but does not verify) that DM_region_header plus all DM_region
   *    descriptors fit within a single grain.
   *
   * grain 0:
   *    "region_header"
   *    "region_table"
   * grains 1 and following:
   *    "arena"
   */

 public:
  auto grain_size() const { return std::size_t(1) << _log_grain_size; }

  /* Rebuilding constructor */
  DM_region_header(size_t device_size)
    : _magic(DM_REGION_MAGIC)
    , _version(DM_REGION_VERSION)
    , _device_size(device_size)
    , _region_count( (::pmem_flush(this, sizeof(DM_region_header)), DEFAULT_MAX_REGIONS) )
    , _log_grain_size(dm_region_log_grain_size)
    , _resvd()
    , _undo_log()
  {
    (void)_resvd; // unused
    (void)_padding; // unused
    DM_region *region_p = region_table_base();
    /* initialize first region with all capacity with size of space, and size of grain */
    region_p->initialize(device_size - grain_size(), grain_size());
    _undo_log.clear_log();
    region_p++;

    for (uint16_t r = 1; r < _region_count; r++) {
      new (region_p) DM_region();
      _undo_log.clear_log();
      region_p++;
    }
    major_flush();
  }

  void check_undo_logs()
  {
    _undo_log.check_and_undo();
  }

  void debug_dump()
  {
    PINF("DM_region_header:");
    PINF(
        " magic [0x%8x]\n version [%u]\n device_size [%lu]\n region_count [%u]",
        _magic, _version, _device_size, _region_count);
    PINF(" base [%p]", common::p_fmt(this));

    const auto regions = region_table_span();
    for (auto const & reg : regions ) {
      if (reg.region_id > 0) {
        PINF(" - USED: %lu (%lx-%lx)", reg.region_id,
             grain_to_bytes(reg.offset_grain),
             grain_to_bytes(reg.offset_grain + reg.length_grain) - 1);
        assert(reg.length_grain > 0);
      }
      else if (reg.length_grain > 0) {
        PINF(" - FREE: %lu (%lx-%lx)", reg.region_id,
             grain_to_bytes(reg.offset_grain),
             grain_to_bytes(reg.offset_grain + reg.length_grain) - 1);
      }
    }
  }

  void *get_region(string_view name_, size_t *out_size)
  {
    auto region_id = make_uuid(name_);

    const auto regions = region_table_span();
    for (auto const & reg : regions ) {
      if (reg.region_id == region_id) {
#if 0
        PLOG("%s: found matching region (%lx)", __func__, region_id);
#endif
        if (out_size) *out_size = grain_to_bytes(reg.length_grain);
        return arena_base() + grain_to_bytes(reg.offset_grain);
      }
    }
    return nullptr; /* not found */
  }

  void erase_region(string_view name_)
  {
    auto region_id = make_uuid(name_);

    const auto regions = region_table_span();
    for (auto & reg : regions ) {
      if (reg.region_id == region_id) {
        reg.region_id = 0; /* power-fail atomic */
        pmem_flush(&reg.region_id, sizeof(reg.region_id));
        return;
      }
    }
    throw std::runtime_error("region not found");
  }

  void *allocate_region(string_view name_, DM_region::grain_offset_t size_in_grain)
  {
    auto region_id = make_uuid(name_);

    const auto regions = region_table_span();
    for (auto & reg : regions ) {
      if (reg.region_id == region_id)
        throw std::bad_alloc();
    }

    bool found = false;
    for (DM_region & reg : regions)
    {
      /* If we have found a sufficiently large free region */
      if (reg.region_id == 0 && reg.length_grain >= size_in_grain) {
        if (reg.length_grain == size_in_grain) {
          /* exact match */
          void *rp = arena_base() + grain_to_bytes(reg.offset_grain);
          /* write file name to region */
          pmem_memcpy_persist(reg.file_name, name_.begin(), name_.size());
          pmem_memcpy_persist(&reg.file_name[name_.size()], "\0", 1);
          // claim region
          tx_atomic_write(&reg, region_id);
          return rp;
        }
        else {
          /* cut out */
          const uint32_t new_offset = reg.offset_grain;

          const auto changed_length = reg.length_grain - size_in_grain;
          const auto changed_offset = reg.offset_grain + size_in_grain;

	  /* reg_n is the new region for unallocated space */
          auto reg_n = std::find_if(regions.begin(), regions.end(), [] (const DM_region &r) { return r.region_id == 0 && r.length_grain == 0; });
          if ( reg_n != regions.end() )
          {
            void *rp = arena_base() + grain_to_bytes(new_offset);
            /* write file name to region */
            pmem_memcpy_persist(reg.file_name, name_.begin(), name_.size());
            pmem_memcpy_persist(&reg.file_name[name_.size()], "\0", 1);
            // claim region
            tx_atomic_write(&*reg_n, boost::numeric_cast<uint16_t>(changed_offset), boost::numeric_cast<uint16_t>(changed_length), &reg,
                            new_offset, size_in_grain, region_id);
            return rp;
          }
        }
      }
    }
    if (!found)
      throw General_exception("no more regions (size in grain=%u)", size_in_grain);

    throw General_exception("no spare slots");
  }

  size_t get_max_available() const
  {
    auto regions = region_table_span();
    auto max_grain_element =
      std::max_element(
        regions.begin()
	, regions.end()
        , [] (const DM_region &a, const DM_region &b) -> bool { return a.length_grain < b.length_grain; }
      );
    return grain_to_bytes( max_grain_element == regions.end() ? 0 : max_grain_element->length_grain);
  }

  inline size_t grain_to_bytes(unsigned grain) const { return size_t(grain) << _log_grain_size; }

  inline void major_flush()
  {
    pmem_flush(this, sizeof(DM_region_header) + (sizeof(DM_region) * _region_count));
  }

  bool check_magic() const
  {
    return (_magic == DM_REGION_MAGIC) && (_version == DM_REGION_VERSION);
  }

  std::list<std::string> names_list()
  {
    std::list<std::string> nl;
    auto regions = region_table_span();
    for ( const auto &r : regions )
    {
      if ( 0 != r.region_id )
      {
        nl.push_back(std::string(r.file_name));
      }
    }
    return nl;
  }

 private:
  void tx_atomic_write(DM_region *dst, uint64_t region_id)
  {
#pragma GCC diagnostic push
#if 9 <= __GNUC__
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
#endif
    _undo_log.log(&dst->region_id);
#pragma GCC diagnostic pop
    dst->region_id = region_id;
    pmem_flush(&dst->region_id, sizeof(region_id));
    _undo_log.clear_log();
  }

  void tx_atomic_write(DM_region *dst0, // offset0, size0, offset1, size1 all expressed in grains
                       uint32_t   offset0,
                       uint32_t   size0,
                       DM_region *dst1,
                       uint32_t   offset1,
                       uint32_t   size1,
                       uint64_t   region_id1)
  {
    _undo_log.log(dst0);
    _undo_log.log(dst1);

    dst0->offset_grain = offset0;
    dst0->length_grain = size0;
    pmem_flush(dst0, sizeof(DM_region));

    dst1->region_id = region_id1;
    dst1->offset_grain = offset1;
    dst1->length_grain = size1;
    pmem_flush(dst1, sizeof(DM_region));

    _undo_log.clear_log();
  }

  inline unsigned char *arena_base()
  {
    return common::pointer_cast<unsigned char>(this) + grain_size();
  }

  /* region descriptors immediately follow the DM_region_header. */
  inline DM_region *region_table_base() { return common::pointer_cast<DM_region>(this + 1); }
  inline const DM_region *region_table_base() const { return common::pointer_cast<const DM_region>(this + 1); }
  gsl::span<DM_region> region_table_span() { return gsl::span<DM_region>(region_table_base(), _region_count); }
  gsl::span<const DM_region> region_table_span() const { return gsl::span<const DM_region>(region_table_base(), _region_count); }

  inline DM_region *region(size_t idx)
  {
    if (idx >= _region_count) return nullptr;
    DM_region *p = static_cast<DM_region *>(region_table_base());
    return &p[idx];
  }

  void reset_header(size_t device_size)
  {
    _magic       = DM_REGION_MAGIC;
    _version     = DM_REGION_VERSION;
    _device_size = device_size;
    pmem_flush(this, sizeof(DM_region_header));
  }
} __attribute__((packed));

}  // namespace nupm

#endif  //__NUPM_DAX_DATA_H__

max_stars_repo_name: omriarad/mcas

max_issues_repo_name: omriarad/mcas

max_forks_repo_name: omriarad/mcas


text:
/*
 * Evan Lezar
 * An implementation of a dense eigensolver that uses the ARPACK Fortran backend.
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <omp.h>
#include <cblas.h>
// #include <acml.h>

// Include the interfaces to the Fortran routines.
#include "interface.h"

#define ARPACK_CALL 0
#define SHIFT 1
#define SGETRF_S 2
#define SGEMV 9

//!
//! A data structure used to pass the eigenproblem configuration to Fortran and
//! back to C again.
struct data_struct {
    float* A;           //! A pointer to the matrix representing the eigenproblem.
    double sgemv_time;  //! The total time required to calculate the matrix-vector products.
    int sgemv_calls;    //! The number of times the matrix-vector product was called.
    int LDA;            //! The leading dimension of the matrix.
};
typedef struct data_struct data_struct;

//!
//! A utility function to print the structure representing the eigenproblem.
void print_data ( const char* desc, data_struct DATA )
{
    printf ( "%s: A = %p LDA = %d\n", desc, DATA.A, DATA.LDA );
}

//!
//! Calculate the matrix-vector product  y <- Ax for the eigensystem defined by DATA. This
//! routine is called from the Fortran backend whenever ARPACK requires a matrix-vector
//! product to be calculated.
//! \param[in] N The dimension of the eigensystem.
//! \param[in,out] DATA The structure representing the eigenproblem.
//! \param[in] x The vector that must be multiplied by A.
//! \param[out] y The vector that must store the result.
void sgemv_wrapper ( int N, data_struct* DATA, float* x, float* y )
{
    checkpoint t0 = tic();
    // read the relevant data from the struct
    float* A = DATA->A;
    int LDA = DATA->LDA;
    // calculate y <-- Ax
    cblas_sgemv ( CblasColMajor, CblasNoTrans, N, N, 1.0, A, LDA, x, 1, 0.0, y, 1);
    DATA->sgemv_time += toc ( t0 );
    DATA->sgemv_calls += 1;
}

//!
//! Initialise the struct representing the eigensystem.
void init_data ( data_struct* DATA, float* A, int LDMAT )
{
    DATA->A = A;
    DATA->LDA = LDMAT;
    DATA->sgemv_calls = 0;
    DATA->sgemv_time = 0.0;
}

//!
//! Solve the actual eigensystem. This takes the eigensystem defined in the structure DATA and allocates
//! the required temporary workspaces before calling the Fortran backend.
void calculate_eigen_values ( int N, void* DATA, int NEV, float* eigenvalues, float* eigenvectors, char* which )
{
    int use_N_ev = 2*NEV;
    if ( use_N_ev > ( N/2 - 1 ) )
        use_N_ev = N/2 - 1;
    // select the number of Arnoldi vectors to generate
    int NCV = 2*use_N_ev + 1;
    if ( NCV > N )
        NCV = N;

    // allocate temporary storage for the vectors
    float* temp_ev = (float*)malloc ( NCV*2*sizeof(float) );
    float* temp_vectors = (float*) malloc (N*NCV*sizeof(float));
    float* temp_residuals = (float*)malloc ( (NCV )*sizeof(float));

    // solve the eigenvalue problem using ARPACK
    arpack_ssev(N, (void*)DATA, use_N_ev, NCV, temp_ev, temp_vectors, temp_residuals, which );

    // Copy the resultant eigenvalues to the previously allocated space.
    memcpy(eigenvalues, temp_ev, NEV*2*sizeof(float));
    memcpy(eigenvectors, temp_vectors, NEV*N*sizeof(float));

    // free the temporary storage
    free ( temp_ev );
    free ( temp_vectors );
    free ( temp_residuals );
}

//!
//! Solve the standard eigensystem Ax = lx.eigen
//! \param[in] N The number of columns in the matrix A
//! \param[in] A The matrix representing the eigensystem to be solved.
//! \param[in] NEV The number of eigenvalues to calculate.
//! \param[out] eigenvalues A vector of the NEV eigenvalues. Note that the eigenvectors are complex.
//! \param[out] eigenvectors An NxNEV matrix with the eigenvectors as columns.
//! \param[out] timing_data_10 A 10-vector representing the timing data for various phases of the process.
//! \param[out] int_data_10 A 10-vector containing some run information.
//! \return A non-zero error code if an error occured.
int dense_seev ( int N, float* A, int LDMAT, int NEV, float* eigenvalues, float* eigenvectors, double* timing_data_10, int* int_data_10 )
{
    checkpoint t0;
    int result = 0;

    // Initialise the data structure that is passed to the ARPACK routines.
    data_struct DATA;
    init_data( &DATA, A, LDMAT );

    t0 = tic();
    // Call a C wrapper function that allows for the calculation of the NEV largest eigenvalues.
    calculate_eigen_values ( N, &DATA, NEV, eigenvalues, eigenvectors, "LM" );
    timing_data_10[ARPACK_CALL] = toc( t0 );

    timing_data_10[SGEMV] = DATA.sgemv_time;
    int_data_10[SGEMV] = DATA.sgemv_calls;
    return result;
}

max_stars_repo_name: elezar/gpu-arpack

max_issues_repo_name: elezar/gpu-arpack

max_forks_repo_name: elezar/gpu-arpack


text:
#pragma once
#include "GradUtil.h"
#include "DistanceGrad.h"

#ifndef _NOGSL
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#else
#include "CustomSolver.h"
#endif

#include <limits>
#include <math.h>
#include <vector>
#include "BasicError.h"

using namespace std;

class IntervalGrad {
	double low;
	double high;
	gsl_vector* lgrad;
	gsl_vector* hgrad;

	//static constexpr double DELTA = 0.7;

public:
	bool singleton;
	IntervalGrad(double _low, double _high, gsl_vector* _lgrad, gsl_vector* _hgrad): low(_low), high(_high), lgrad(_lgrad), hgrad(_hgrad) {
		singleton = false;
	}
	~IntervalGrad(void) {
		gsl_vector_free(lgrad);
		gsl_vector_free(hgrad);
	}
	double getLow() const { return low; }
	double getHigh() const { return high; }
	gsl_vector* getLGrad() const { return lgrad; }
	gsl_vector* getHGrad() const { return hgrad; }
	void update(double _low, double _high) {
		low = _low;
		high = _high;
	}
	static void ig_plus(IntervalGrad* m, IntervalGrad* f, IntervalGrad* o); // o = m + f
	static void ig_times(IntervalGrad* m, IntervalGrad* f, IntervalGrad* o); // o = m * f
	static void ig_div(IntervalGrad* m, IntervalGrad* f, IntervalGrad* o); // o = m / f
	static void ig_neg(IntervalGrad* m, IntervalGrad* o); // o = -m
	static void ig_union(const vector<IntervalGrad*>& m, IntervalGrad* o); // o = m_1 V m_2 V ...
	static void ig_equal(IntervalGrad* m, IntervalGrad* f, DistanceGrad* o); // o = m == f
	static void ig_lt(IntervalGrad* m, IntervalGrad* f, DistanceGrad* o); // o = m < f
	static void ig_square(IntervalGrad* m, IntervalGrad* o); // o = m * m
	static void ig_arctan(IntervalGrad* m, IntervalGrad* o); // o = arctan(m)
	static void ig_sin(IntervalGrad* m, IntervalGrad* o); // o = sin(m)
	static void ig_cos(IntervalGrad* m, IntervalGrad* o); // o = cos(m)
	static void ig_tan(IntervalGrad* m, IntervalGrad* o); // o = tan(m)
	static void ig_sqrt(IntervalGrad* m, IntervalGrad* o); // o = sqrt(m)
	static void ig_exp(IntervalGrad* m, IntervalGrad* o);
	static void ig_copy(IntervalGrad* i1, IntervalGrad* i2); // copy i1 into i2
	static void ig_cast_int_float(IntervalGrad* m, IntervalGrad* o);

	static void ig_conditionalUnion(IntervalGrad* m, IntervalGrad* f, DistanceGrad* d, IntervalGrad* o);
	static void ig_intersect(const vector<IntervalGrad*>& m, IntervalGrad* o);

	string print() {
		stringstream str;
		str << "Range: " << low << " " << high;
		return str.str();
	}

	string printFull() {
		stringstream str;
		str << "Range: " << low << " " << high << endl;
		str << "LGrads: ";
		for (int i = 0; i < lgrad->size; i++) {
		 str << gsl_vector_get(lgrad, i) << ", ";
		}
		str << endl;
		str << "HGrads: " ;
		for (int i = 0; i < hgrad->size; i++) {
		 str << gsl_vector_get(hgrad, i) << ", ";
		}
		return str.str();
	}

private:
	void bound() {
		double oldLow = low;
		low = GradUtil::bound(low);
		if (oldLow != low) {
			GradUtil::default_grad(lgrad);
		}
		double oldHigh = high;
		high = GradUtil::bound(high);
		if (oldHigh != high) {
			GradUtil::default_grad(hgrad);
		}

		for (int i = 0; i < lgrad->size; i++) {
			gsl_vector_set(lgrad, i, GradUtil::bound(gsl_vector_get(lgrad, i)));
			gsl_vector_set(hgrad, i, GradUtil::bound(gsl_vector_get(hgrad, i)));
		}
	}
};

max_stars_repo_name: natebragg/sketch-backend

max_issues_repo_name: natebragg/sketch-backend

max_forks_repo_name: natebragg/sketch-backend


text:
#ifndef UTIL_H
#define UTIL_H

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

#include "spaND.h"

namespace spaND {

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

typedef timeval timer;
timer wctime();

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

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

/**
 * C <- alpha A * A^T + beta C (or A^T * A)
 */
void syrk(Eigen::MatrixXd* A, Eigen::MatrixXd* C, CBLAS_TRANSPOSE tA, double alpha, double beta);

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

int ldlt(Eigen::MatrixXd* A, Eigen::MatrixXd* L, Eigen::VectorXd* d, Eigen::VectorXi* p, double* rcond);

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

/**
 * A = P L U Q
 * A <- [L\U] (lower and upper)
 * L is unit diagonal
 * U is not
 */
void fpgetf(Eigen::MatrixXd* A, Eigen::VectorXi* p, Eigen::VectorXi* q);

/**
 * A = L U
 * breaks down into L & U, and split the diagonal between the two
 * A = L * (D + U') = L * D (I + D^-1 U') = { L * |D|^1/2 } { |D|^1/2 sign(D) (I + D^-1 U') }
 */
void split_LU(Eigen::MatrixXd* A, Eigen::MatrixXd* L, Eigen::MatrixXd* U);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

// Set Eigen::MatrixXd to zero using memset
void setZero(Eigen::MatrixXd* A);

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

// Range
template<typename T>
struct ItRange
{
    public:
        ItRange(const T& begin, const T& end) : begin_(begin), end_(end) {}
        const T& begin() const {
            return this->begin_;
        }
        const T& end() {
            return this->end_;
        }
    private:
        T begin_;
        T end_;
};

/**
 * Statistics and Logging
 **/

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

        double elim_pivot;
        double elim_panel;
        double elim_schur;

        double scale_pivot;
        double scale_panel;

        double spars_assmb;
        double spars_spars;
        double spars_scatt;

        double merge_alloc;
        double merge_copy;

        double potf;
        double ldlt;
        double trsm;
        double gemm;
        double geqp3;
        double geqrf;
        double syev;
        double gesvd;
        double getf;

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

        Profile() :
            elim(0), scale(0), spars(0), merge(0),
            elim_pivot(0), elim_panel(0), elim_schur(0),
            scale_pivot(0), scale_panel(0),
            spars_assmb(0), spars_spars(0), spars_scatt(0),
            merge_alloc(0), merge_copy(0),
            potf(0), ldlt(0), trsm(0), gemm(0), geqp3(0), geqrf(0), syev(0), gesvd(0), getf(0),
            buildq(0), scattq(0), perma(0), scatta(0), assmb(0), phi(0)
            {}
};

struct PivotFlops {
    long rows;
    double time;
};

struct PanelFlops {
    long rows;
    long cols;
    double time;
};

struct GemmFlops {
    long rows;
    long cols;
    long inner;
    double time;
};

struct RRQRFlops {
    long rows;
    long cols;
    double time;
};

struct ProfileFlops {
    public:
        std::vector<PivotFlops> pivot;
        std::vector<PanelFlops> panel;
        std::vector<GemmFlops>  gemm;
        std::vector<RRQRFlops>  rrqr;
        ProfileFlops() {};
};

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

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

        Stats<double> cond_diag_elim;
        Stats<double> norm_diag_elim;
        Stats<double> cond_U_elim;
        Stats<double> cond_L_elim;

        Stats<double> cond_diag_scal;
        Stats<double> norm_diag_scal;
        Stats<double> cond_U_scal;
        Stats<double> cond_L_scal;

        double Asym_before_scaling;
        double Asym_after_scaling;
        Stats<double> Anorm_before_lower;
        Stats<double> Anorm_before_upper;
        Stats<double> Anorm_after_lower;
        Stats<double> Anorm_after_upper;

        Log() :
            dofs_nd(0),
            dofs_left_nd(0), dofs_left_elim(0), dofs_left_spars(0),
            fact_nnz(0), ignored(0),
            Asym_before_scaling(0), Asym_after_scaling(0)
            {}
};

}

#endif


text:
/*
 *  SpecialFunctions.h
 *  PSCAcoustic
 *
 *  Special functions and more.
 *
 *
 * Copyright 2014 Pierre-David Letourneau
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal in the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
*/

#ifndef SPECIALFUNCTIONS_H
#define SPECIALFUNCTIONS_H

#include "General.h"

// GSL library
//#include "../gsl-1.16/include/gsl/gsl_sf_bessel.h" //spherical bessel functions
#include <gsl/gsl_sf_bessel.h> //spherical bessel functions
#include "./gsl-1.16/include/gsl/gsl_sf_legendre.h" //associated Legendre polynomials (for spherical harmonics)
#include "./gsl-1.16/include/gsl/gsl_complex_math.h"
#include "./gsl-1.16/include/gsl/gsl_sf_gamma.h"
#include "./gsl-1.16/include/gsl/gsl_sf_hyperg.h"
#include "./gsl-1.16/include/gsl/gsl_sf_coupling.h"
#include "./gsl-1.16/include/gsl/gsl_integration.h"
#include "./gsl-1.16/include/gsl/gsl_complex.h"
#include "./gsl-1.16/include/gsl/gsl_sf_coupling.h"
#include "./gsl-1.16/include/gsl/gsl_math.h"
#include "./gsl-1.16/include/gsl/gsl_ieee_utils.h"
#include "./gsl-1.16/include/gsl/gsl_errno.h"
#include "./gsl-1.16/include/gsl/gsl_integration.h"

// Complex Bessel functions
//#include "../AmosBessel/libAmosBessel.h"

//GSL wrappers (Cannot handle complex  arguments)

//Hankel function of the 1st kind (gsl)
inline complex  gsl_sf_hankel_1( int l, double x){
  return gsl_sf_bessel_jl(l,x) + gsl_sf_bessel_yl(l,x) * CI;
}

inline double gsl_sf_legendre( int l, int m, double x){
  double C = 1;
  if( m < 0 )
    C *= pow(-1, std::abs(m));

  return (C * gsl_sf_legendre_sphPlm ( l, std::abs(m), x) );
}

// Y_lm (spherical harmonics)
//\sqrt{(2l+1)/(4\pi)} \sqrt{(l-m)!/(l+m)!} P_l^m(x) exp(I*m*phi) )
inline complex  gsl_sf_harmonic( int l, int m, double theta, double phi){
  if( l >= std::abs(m) ){
    return gsl_sf_legendre(l, m, cos(theta)) * exp(m*phi*CI);
  } else {
    return 0.;
  }
}

inline complex  gsl_sf_clebsch(int l1, int l2, int l3, int m1, int m2, int m3){
  return pow(-1., (double) l1-l2+m3) * sqrt(2.*l3+1.) * gsl_sf_coupling_3j(2*l1, 2*l2, 2*l3, 2*m1, 2*m2, -2*m3);
}

inline double gsl_sf_gaunt(int n, int m, int nu, int mu, int q){
  return pow(-1., (double) (m+mu) ) * sqrt((2*n+1)*(2*nu+1)*(2*q+1)/(4*PI)) *
    gsl_sf_coupling_3j(2.*n,2.*nu,2.*q,0,0,0) * gsl_sf_coupling_3j(2.*n,2.*nu,2.*q,2.*m,2.*mu,-2.*(m+mu));
}

// TODO: FIX AMOS
//Amos wrappers for Bessel functions (these can handle complex  arguments)
inline complex  Amos_sf_bessel_jl(int l, complex  z){
  	complex  value;
	//AmosBessel('j', z, (double) l, 1, 0, &value);
	value = gsl_sf_bessel_jl(l,std::real(z));
	return value;
}

inline complex  Amos_sf_bessel_yl(int l, complex  z){
  	complex  value;
	//AmosBessel('y', z, (double) l, 1, 0, &value);
	value = gsl_sf_bessel_yl(l,std::real(z));
	return value;
}

inline complex  Amos_sf_hankel_1(int l, complex  z){
  //complex  valueR;
	//AmosBessel('j', z, (double) l, 1, 0, &valueR);
  	//complex  valueI;
	//AmosBessel('y', z, (double) l, 1, 0, &valueI);
	//return (valueR + valueI*CI);

	complex value = gsl_sf_hankel_1(l, std::real(z));
	return value;
}

// Derivatives

inline double gsl_sf_bessel_jl_prime(int l, double z){
  return l/z * gsl_sf_bessel_jl(l,z) - gsl_sf_bessel_jl(l+1,z);
}

inline complex  gsl_sf_hankel_l_prime(int l, double z){
  return l/z * gsl_sf_hankel_1( l, z) - gsl_sf_hankel_1(l+1, z);
}

inline complex  Amos_sf_bessel_jl_prime(double l, complex  z){
  return l/z * Amos_sf_bessel_jl(l,z) - Amos_sf_bessel_jl(l+1,z);
}

inline complex  Amos_sf_hankel_l_prime(double l, complex  z){
  return l/z * Amos_sf_hankel_1( l, z) - Amos_sf_hankel_1(l+1, z);
}

// Chebyshev polynomial evaluation
inline double Cheb(int n, double x){
  return cos(n*acos(x));
}

// Gauss-Legendre quadrature
inline void getGaussLegendreQuad(int N, std::vector<double>& x, std::vector<double>& w)
{
  x.resize(N);
  w.resize(N);

  gsl_integration_glfixed_table* GLTable = gsl_integration_glfixed_table_alloc(N);

  for( int n = 0; n < (N+1)/2; ++n ) {
    x[n + N/2] = GLTable->x[n];
    w[n + N/2] = GLTable->w[n];
  }
  for( int n = 0; n < N/2; ++n ) {
    x[n] = -x[N-1-n];
    w[n] =  w[N-1-n];
  }

  gsl_integration_glfixed_table_free( GLTable );
}

// Gauss-Chebyshev quadrature
inline void getGaussChebyshevQuad(int N, std::vector<double>& x, std::vector<double>& w)
{
  x.resize(N);
  w.resize(N);

  for( int n = 0; n < N; n++ ) {
    x[n] = cos( (2.*n+1.)/(2.*N)*PI  );
    w[n] = 2./N;
  }
}

//---------General Functions--------//

#define ISODD(x) ((x) & 1)
#define ISNAN(x) ((x) != (x))

//TODO : still needed?
inline int choose( int n, int r ){

  if( (n < r) || (r < 0) || (n < 0) ){
    return 0;
  } else {
    return gsl_sf_gamma(n+1) / (gsl_sf_gamma(r+1) * gsl_sf_gamma(n-r+1));
  }

}

// Implementation os Sparse Matrix class
// TODO:  this takes a lot of memory?
class SpMatrix
{
  map<std::pair<int,int>, complex > M;

 public:

  // Empty constructor
  SpMatrix(){}

  ~SpMatrix(){}

  void add(int i, int j, complex  val){
    assert( i >= 0 );
    assert( j >= 0 );

    std::pair<int,int> coord = std::make_pair(i,j);
    if( M.count(coord) ){
      M.insert(std::make_pair(coord, val));
    } else {
      M[coord] = val;
    }

  }

  complex  operator () (int i, int j){
    std::pair<int,int> coord = std::make_pair(i,j);
    complex  val = 0.;

    ( M.count(coord) ) ? val = M.find(coord)->second : val = 0.;

    return val;
  }

  inline int size() { return M.size(); }

};

#endif