/*
   Authors:
   G. Jungman
   J. Scott (james.scott@lexifi.com)

 */
/* Implement Niederreiter base 2 generator.
 * See:
 *   Bratley, Fox, Niederreiter, ACM Trans. Model. Comp. Sim. 2, 195 (1992)
 */
#include <config.h>
#include <gsl/gsl_qrng.h>
#include <gsl/gsl_rng.h>
#include <assert.h>

#define NIED2_BIT_COUNT 30
#define NIED2_NBITS (NIED2_BIT_COUNT+1)

/* Z_2 field operations */
#define NIED2_ADD(x,y) (((x)+(y))%2)
#define NIED2_MUL(x,y) (((x)*(y))%2)
#define NIED2_SUB(x,y) NIED2_ADD((x),(y))

void get_primitive_polynomials(int dimension, int *degree_table, int *primitive_polynomials);

static size_t nied2_state_size(unsigned int dimension);
static int nied2_init(void * state, unsigned int dimension);
static int nied2_get(void * state, unsigned int dimension, double * v);

static const gsl_qrng_type nied2_type =
{
  "niederreiter-base-2",
  0,
  nied2_state_size,
  nied2_init,
  nied2_get
};

const gsl_qrng_type * gsl_qrng_niederreiter_2 = &nied2_type;

typedef struct
{
  unsigned int sequence_count;
  int *cj[NIED2_NBITS];
  int *nextq;
} nied2_state_t;

static size_t nied2_state_size(unsigned int dimension)
{
  return sizeof(nied2_state_t) +             /* the struct */
    sizeof(int) * dimension * NIED2_NBITS +  /* cj */
    sizeof(int) * dimension;                 /* nextq */
}

/* Multiply polynomials over Z_2.
 * Notice use of a temporary vector,
 * side-stepping aliasing issues when
 * one of inputs is the same as the output
 * [especially important in the original fortran version, I guess].
 */
static void poly_multiply(
  const int pa[], int pa_degree,
  const int pb[], int pb_degree,
  int pc[], int  * pc_degree, int max_degree
  )
{
  int j, k;
  int *pt;
  int pt_degree;

  pt = (int *) malloc(sizeof(int) * (max_degree+1));
  pt_degree = pa_degree + pb_degree;

  assert(pt_degree <= max_degree);

  for(k=0; k<=pt_degree; k++) {
    int term = 0;
    for(j=0; j<=k; j++) {
      const int conv_term = NIED2_MUL(pa[k-j], pb[j]);
      term = NIED2_ADD(term, conv_term);
    }
    pt[k] = term;
  }

  for(k=0; k<=pt_degree; k++) {
    pc[k] = pt[k];
  }
  for(k=pt_degree+1; k<=max_degree; k++) {
    pc[k] = 0;
  }

  *pc_degree = pt_degree;
  free(pt);
}

/* Calculate the values of the constants V(J,R) as
 * described in BFN section 3.3.
 *
 *   px = appropriate irreducible polynomial for current dimension
 *   pb = polynomial defined in section 2.3 of BFN.
 * pb is modified
 */
static void calculate_v(
  const  int px[],  int px_degree,
   int pb[],  int * pb_degree,
  int v[],  int maxv, int max_degree, gsl_rng *rng
  )
{
  const int nonzero_element = 1;    /* nonzero element of Z_2  */

  /* The polynomial ph is px**(J-1), which is the value of B on arrival.
   * In section 3.3, the values of Hi are defined with a minus sign:
   * don't forget this if you use them later !
   */
  int *ph;
  /* int ph_degree = *pb_degree; */
  int bigm = *pb_degree;      /* m from section 3.3 */
  int m;                      /* m from section 2.3 */
  int r, k, kj;

  ph = (int *) malloc(sizeof(int) * (max_degree+1));

  for(k=0; k<=max_degree; k++) {
    ph[k] = pb[k];
  }

  /* Now multiply B by PX so B becomes PX**J.
   * In section 2.3, the values of Bi are defined with a minus sign :
   * don't forget this if you use them later !
   */
   poly_multiply(px, px_degree, pb, *pb_degree, pb, pb_degree, max_degree);
   m = *pb_degree;

  /* Now choose a value of Kj as defined in section 3.3.
   * We must have 0 <= Kj < E*J = M.
   * The limit condition on Kj does not seem very relevant
   * in this program.
   */
  /* Quoting from BFN: "Our program currently sets each K_q
   * equal to eq. This has the effect of setting all unrestricted
   * values of v to 1."
   * Actually, it sets them to the arbitrary chosen value.
   * Whatever.
   */
   kj = gsl_rng_uniform_int(rng, bigm+1);

  /* Now choose values of V in accordance with
   * the conditions in section 3.3.
   */
  for(r=0; r<kj; r++) {
    v[r] = 0;
  }
  v[kj] = 1;

  if(kj >= bigm) {
    for(r=kj+1; r<m; r++) {
      v[r] =  gsl_rng_uniform_int(rng, 2); /* arbitrary element */
    }
  }
  else {
    /* This block is never reached. */

    int term = NIED2_SUB(0, ph[kj]);

    for(r=kj+1; r<bigm; r++) {
      v[r] =  gsl_rng_uniform_int(rng, 2); /* arbitrary element */

      /* Check the condition of section 3.3,
       * remembering that the H's have the opposite sign.  [????????]
       */
      term = NIED2_SUB(term, NIED2_MUL(ph[r], v[r]));
    }

    /* Now v[bigm] != term. */
    v[bigm] = NIED2_ADD(nonzero_element, term);

    for(r=bigm+1; r<m; r++) {
      v[r] = gsl_rng_uniform_int(rng, 2); /* arbitrary element */
    }
  }

  /* Calculate the remaining V's using the recursion of section 2.3,
   * remembering that the B's have the opposite sign.
   */
  for(r=0; r<=maxv-m; r++) {
    int term = 0;
    for(k=0; k<m; k++) {
      term = NIED2_SUB(term, NIED2_MUL(pb[k], v[r+k]));
    }
    v[r+m] = term;
  }
  free(ph);
}

static int calculate_cj(nied2_state_t * ns,  int dimension)
{
  int ci[NIED2_NBITS][NIED2_NBITS];
  int *v;
  int r;
  int i_dim;
  int max_degree, maxv;
  int *pb, *px;

  gsl_rng *rng = gsl_rng_alloc(gsl_rng_default);

  int *primitive_polynomials;
  int *poly_degree;

  poly_degree = (int *) malloc(sizeof(int) * (1+dimension));
  primitive_polynomials = (int *) malloc(sizeof(int) * (1+dimension));

  if (poly_degree==0 || primitive_polynomials==0)
    GSL_ERROR_NULL ("allocation of degree table failed for niederreiter init", GSL_ENOMEM);

  /* Generate the primitive polynomials.  */
  get_primitive_polynomials(dimension, poly_degree+1, primitive_polynomials+1);

  poly_degree[0]=0;  poly_degree[1]=1;
  primitive_polynomials[0]=1;
  primitive_polynomials[1]=2;

  max_degree=NIED2_NBITS+poly_degree[dimension-1];
  maxv = max_degree;

  v = (int *) malloc(sizeof(int) * (1+maxv));
  pb = (int *) malloc(sizeof(int) * (1+max_degree));
  px = (int *) malloc(sizeof(int) * (1+max_degree));

  if (v==0 || pb==0 || px==0)
    GSL_ERROR_NULL ("allocation of degree table failed for niederreiter init", GSL_ENOMEM);

  for(i_dim=0; i_dim<dimension; i_dim++) {

    const int poly_index = i_dim + 1;
    int j, k;

    /* Niederreiter (page 56, after equation (7), defines two
     * variables Q and U.  We do not need Q explicitly, but we
     * do need U.
     */
    int u = 0;

    /* For each dimension, we need to calculate powers of an
     * appropriate irreducible polynomial, see Niederreiter
     * page 65, just below equation (19).
     * Copy the appropriate irreducible polynomial into PX,
     * and its degree into E.  Set polynomial B = PX ** 0 = 1.
     * M is the degree of B.  Subsequently B will hold higher
     * powers of PX.
     */
    int px_degree;
    int pb_degree;
    px_degree = poly_degree[poly_index];
    pb_degree = 0;

    for(k=0; k<=px_degree; k++) {
      px[k] = (primitive_polynomials[poly_index] >> k) & 1;
      pb[k] = 0;
    }

    for (;k<max_degree+1;k++) {
      px[k] = 0;
      pb[k] = 0;
    }

    pb[0] = 1;

    for(j=0; j<NIED2_NBITS; j++) {
      /* If U = 0, we need to set B to the next power of PX
       * and recalculate V.
       */
      if(u == 0) calculate_v(px, px_degree, pb, &pb_degree, v, maxv, max_degree, rng);

      /* Now C is obtained from V.  Niederreiter
       * obtains A from V (page 65, near the bottom), and then gets
       * C from A (page 56, equation (7)).  However this can be done
       * in one step.  Here CI(J,R) corresponds to
       * Niederreiter's C(I,J,R).
       */
      for(r=0; r<NIED2_NBITS; r++) {
        ci[r][j] = v[r+u];
      }

      /* Advance Niederreiter's state variables. */
      ++u;
      if(u == px_degree) u = 0;
    }

    /* The array CI now holds the values of C(I,J,R) for this value
     * of I.  We pack them into array CJ so that CJ(I,R) holds all
     * the values of C(I,J,R) for J from 1 to NBITS.
     */
    for(r=0; r<NIED2_NBITS; r++) {
      int term = 0;
      for(j=0; j<NIED2_NBITS; j++) {
        term = 2*term + ci[r][j];
      }
      ns->cj[r][i_dim] = term;
    }
  }
  free(primitive_polynomials);
  free(poly_degree);
  free(v);
  free(pb);
  free(px);
  gsl_rng_free(rng);
  return GSL_SUCCESS;
}

static int nied2_init(void * state, unsigned int dimension)
{
  nied2_state_t * n_state = (nied2_state_t *) state;
  unsigned int i_dim, i_bits;
  int ret;

  if(dimension < 1) return GSL_EINVAL;

  for (i_bits=0; i_bits<NIED2_NBITS; i_bits++)
    n_state->cj[i_bits] =
      (int *) ((char *) n_state +
	       sizeof(nied2_state_t) +
	       sizeof(int) * dimension * i_bits);

  n_state->nextq =
    (int *) ((char *) n_state +
	     sizeof(nied2_state_t) +
	     sizeof(int) * dimension * NIED2_NBITS);

  if ((ret = calculate_cj(n_state, dimension)) != GSL_SUCCESS)
    return ret;

  /* skip the '0th' state, which is 0 */
  for(i_dim=0; i_dim<dimension; i_dim++)
    n_state->nextq[i_dim] = n_state->cj[0][i_dim];

  n_state->sequence_count = 1;

  return GSL_SUCCESS;
}

static int nied2_get(void * state, unsigned int dimension, double * v)
{
  static const double recip = 1.0/(double)(1U << NIED2_NBITS); /* 2^(-nbits) */
  nied2_state_t * n_state = (nied2_state_t *) state;
  int r;
  int c;
  unsigned int i_dim;

  /* Load the result from the saved state. */
  for(i_dim=0; i_dim<dimension; i_dim++) {
    v[i_dim] = n_state->nextq[i_dim] * recip;
  }

  /* Find the position of the least-significant zero in sequence_count.
   * This is the bit that changes in the Gray-code representation as
   * the count is advanced.
   */
  r = 0;
  c = n_state->sequence_count;
  while(1) {
    if((c % 2) == 1) {
      ++r;
      c /= 2;
    }
    else break;
  }

  if(r >= NIED2_NBITS) return GSL_EFAILED; /* FIXME: better error code here */

  /* Calculate the next state. */
  for(i_dim=0; i_dim<dimension; i_dim++) {
    n_state->nextq[i_dim] ^= n_state->cj[r][i_dim];
  }

  n_state->sequence_count++;

  return GSL_SUCCESS;
}


/*----------------------------------------------------------------------------
/
/ Filename: fits_wavelet.c
/ Author: Jay Billings
/ Author's email: jayjaybillings@gmail.com
/ Description: This program will perform a Debauchies wavelet transform on
/              an input image and turn off the wavelet coefficients up to
/              a certain percentage of the total size.
/
/              Usage:
/                 ./fits_wavelet <image> <limit>
/
/                 <image> should be an image in FITS format and <limit>
/                 should be a number between 0.0 and 1.0.
/
/              Output:
/                 filtered_<image>
/
/                 The output image is also in the FITS format and is readily
/                 viewable in FV, available from the NASA website, or the
/                 Sky Image Processor, useable as a java applet from Dr.
/                 John Simonetti, Virginia Tech,
/
/                 http://www.phys.vt.edu/~jhs/SIP.
/
/              This code requires the CFITSIO library and the GNU Scientific
/              Library. Both are freely available on the internet. This code
/              compiles fine with gcc-4.1.2 using
/
/                 gcc -o fits_wavelet fits_wavelet.c -lm -lgsl -lgslcblas
/                  -lcfitiso
/
/ Copyright (c) 2008       Jay Jay Billings
/
/ 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
/
/ The license is also available at:
/
/		http://www.gnu.org/copyleft/gpl.html .
/
/ Date: 2008/03/24
/
*///--------------------------------------------------------------------------

#include <string.h>
#include <stdio.h>
#include <fitsio.h>
#include <gsl/gsl_wavelet.h>
#include <gsl/gsl_sort.h>
#include <math.h>

void read_image(double *,char *,int []);
void write_image(double *, char *, int []);

int main(int argc, char *argv[]) {

    int i, j, size[3], a;
    double f, limit, *pixels, *pixels_copy;
    gsl_wavelet *w;
    gsl_wavelet_workspace *work;
    size_t *p;

    // Check to make sure that the inputs are OK.
    if (argc < 3) {
       printf("You need to specify a file! \n\nUsage: ");
       printf("\t fits_wavelet <input_file> <limit>\n\n");
       exit(0);
    }
    f = atof(argv[2]);
    if (f >= 1.0) {
       printf("The limit must be between 0.0 and 1.0!\n\n");
       printf("Try something else! (Like 0.85...)\n");
       exit(0);
    }

    // Allocate some storage arrays. The maximum size is currently
    // set at 1024*1024, or 2^20, initilized to zero.
    if (!(pixels = calloc(1024*1024,sizeof(double)))) {
         printf("Memory allocation error\n");
         exit(0);
    }
    if (!(pixels_copy = calloc(1024*1024,sizeof(double)))) {
         printf("Memory allocation error\n");
         exit(0);
    }
    if (!(p = malloc(1024*1024*sizeof(size_t)))) {
         printf("Memory allocation error\n");
         exit(0);
    }

    // Read the image into the pixels array.
    read_image(pixels,argv[1],size);

    // Allocate the wavelet workspace.
    w = gsl_wavelet_alloc(gsl_wavelet_daubechies,20);
    work = gsl_wavelet_workspace_alloc(1024*1024);

    // Perform the wavelet transfer.
    gsl_wavelet_transform_forward(w,pixels,1,1024,work);

    // Make a copy of the pixels array and sort it.
    for (i = 0; i < size[2]; ++i) {
        pixels_copy[i] = fabs(pixels[i]);

    }
    gsl_sort_index(p,pixels_copy,1,size[0]*size[1]);

    // Loop over the wavelet coefficients and turn them off
    // up to a certain limit.
    limit = (double) size[2] * f;
    for (i = 0; i < (int) limit; ++i) {
        pixels[p[i]] = 0.0;
    }

    // Perform the inverse transform.
    gsl_wavelet_transform_inverse(w,pixels,1,1024,work);

    // Write the image.
    write_image(pixels,argv[1],size);

    // Free all the memory and exit.
    gsl_wavelet_free(w);
    gsl_wavelet_workspace_free(work);
    free(pixels);free(pixels_copy);free(p);
    return 0;
}

// This subroutine was adapted from documentation provided on the FITSIO website.
void read_image(double *pixels_vector, char *filename, int size[3])
{
    fitsfile *fptr;   /* FITS file pointer, defined in fitsio.h */
    int status = 0;   /* CFITSIO status value MUST be initialized to zero! */
    int bitpix, naxis, ii, anynul,a;
    long naxes[2] = {1,1}, fpixel[2] = {1,1};
    double *pixels;
    char format[20], hdformat[20];

    /*if (argc != 2) {
      printf("Usage:  imlist filename[ext][section filter] \n");
      printf("\n");
      printf("List the the pixel values in a FITS image \n");
      printf("\n");
      printf("Example: \n");
      printf("  imlist image.fits                    - list the whole image\n");
      printf("  imlist image.fits[100:110,400:410]   - list a section\n");
      printf("  imlist table.fits[2][bin (x,y) = 32] - list the pixels in\n");
      printf("         an image constructed from a 2D histogram of X and Y\n");
      printf("         columns in a table with a binning factor = 32\n");
      return(0);
    }*/

    if (!fits_open_file(&fptr, filename, READONLY, &status))
    {
        if (!fits_get_img_param(fptr, 2, &bitpix, &naxis, naxes, &status) )
        {
          if (naxis > 2 || naxis == 0)
             printf("Error: only 1D or 2D images are supported\n");
          else
          {
            /* get memory for 1 row */
            if (!(pixels = malloc(naxes[0] * sizeof(double)))) {
                printf("Memory allocation error\n");
                exit(0);
            }

            if (bitpix > 0) {  /* set the default output format string */
               strcpy(hdformat, " %7d");
               strcpy(format,   " %7.0f");
            } else {
               strcpy(hdformat, " %15d");
               strcpy(format,   " %15.5f");
            }

            //printf("\n      ");          /* print column header */
            //for (ii = 1; ii <= naxes[0]; ii++)
            //   printf(hdformat, ii);
            //printf("\n");                /* terminate header line */

            /* loop over all the rows in the image, top to bottom */
            a = 0;
            for (fpixel[1] = naxes[1]; fpixel[1] >= 1; fpixel[1]--)
            {
               if (fits_read_pix(fptr, TDOUBLE, fpixel, naxes[0], NULL,
                    pixels, NULL, &status) )  /* read row of pixels */
                  break;  /* jump out of loop on error */

            //   printf(" %4d \n",fpixel[1]);  /* print row number */
               for (ii = 0; ii < naxes[0]; ii++) {
                  pixels_vector[a] = pixels[ii];
//                  printf(format, pixels_vector[a]);   /* print each value  */
 //                 printf(" %d\n",a);                    /* terminate line */
                  ++a;
               }
            }
            size[0] = naxes[0];
            size[1] = naxes[1];
            size[2] = naxes[0]*naxes[1];
            free(pixels);
          }
        }
        fits_close_file(fptr, &status);
    }

    if (status) {
       fits_report_error(stderr, status); /* print any error message */
       exit(0);
    }
    //return(status);
    return;
}

void write_image(double *pixels_vector, char *f1, int size[3]) {

    fitsfile *infptr, *outfptr;   /* FITS file pointer, defined in fitsio.h */
    int status = 0;   /* CFITSIO status value MUST be initialized to zero! */
    int ii = 1;
    char f2[200];
    long size_l[2], fpixel[2] = {1,1};
    void *pix_ptr = &pixels_vector;

    size_l[0] = (long) size[0];
    size_l[1] = (long) size[1];

    sprintf(f2,"filtered_%s",f1);

    /* Create the output file */
    if (!fits_create_file(&outfptr, f2, &status) ) {
       if (!fits_create_img(outfptr,32,2,size_l,&status)) {
          if (!fits_write_pix(outfptr,TDOUBLE,fpixel,(long) size[2],pixels_vector \
           ,&status)) {
          if (status == END_OF_FILE) status = 0;
          }
          //printf("Write status: %d \n",status);
       }
       fits_close_file(outfptr,  &status);
    }

    return;
}


#include <stdio.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <gsl/gsl_sf.h>

#define HBARC 197.3269804 // MeV•fm
#define MASS_TRITON 2808.92113298 // MeV
#define MASS_DEUTERON 1875.61294257 // MeV
#define MASS_NEUTRON 939.565420 // MeV
#define MASS_ALPHA 3727.37940 // MeV
const double MU_D = MASS_TRITON*MASS_DEUTERON/(MASS_TRITON + MASS_DEUTERON);
const double MU_N = MASS_ALPHA*MASS_NEUTRON/(MASS_ALPHA + MASS_NEUTRON);
const double ALPHA = 1.0/137.035999084;
const double Q = 17.6;
const double J = 1.5;
const double J1 = 0.5;
const double J2 = 1.0;

double relative_momentum(double e, double mu)
{
  return sqrt(2*mu*e)/HBARC;
}

double sommerfeld(double e, double mu)
{
  return ALPHA * sqrt(mu/(2*e));
}

void coulomb_functions(double* F, double* Fp, double* G, double* Gp, int l, double eta,
    double x)
{
  int info;
  double exp_f, exp_g;
  gsl_sf_result f, fp, g, gp;
  info = gsl_sf_coulomb_wave_FG_e(eta, x, l, 0, &f, &fp, &g, &gp, &exp_f, &exp_g);
  // printf("%.4f  %.4f\n", exp_f, exp_g);
  if (info == GSL_EOVRFLW) printf("overflow or underflow encountered\n");
  *F = f.val*exp(exp_f);
  *Fp = fp.val*exp(exp_f);
  *G = g.val*exp(exp_g);
  *Gp = gp.val*exp(exp_g);
}

double penetration_factor(int l, double eta, double x)
{
  double F, Fp, G, Gp;
  coulomb_functions(&F, &Fp, &G, &Gp, l, eta, x);
  return x / (F*F + G*G);
}

double shift_factor(int l, double eta, double x)
{
  double F, Fp, G, Gp;
  coulomb_functions(&F, &Fp, &G, &Gp, l, eta, x);
  return x * (F*Fp + G*Gp) / (F*F + G*G);
}

double Gamma_c(double gamma_c_sq, int l, double eta, double x)
{
  return 2 * gamma_c_sq * penetration_factor(l, eta, x);
}

double Delta_c(double gamma_c_2, double k, double r, double Bc, int l, double eta)
{
	return -gamma_c_2 * (shift_factor(l, eta, k*r)-Bc);
}

double Sdn2(double energy, double e0, double Bd, double Bn, double gamma_d_2,
    double gamma_n_2, double kd, double ad, double kn, double an, double eta,
    double A)
{
	double Gamma_d = Gamma_c(gamma_d_2, 0, eta, kd*ad);
	double Gamma_n = Gamma_c(gamma_n_2, 2, 0, kn*an);
	double Gamma = Gamma_d + Gamma_n;
	double Delta_d = Delta_c(gamma_d_2, kd, ad, Bd, 0, eta);
	double Delta_n = Delta_c(gamma_n_2, kn, an, Bn, 2, 0);
	double Delta = Delta_d + Delta_n;
  // printf("Sdn2: %.2e  %.2e  %.2e\n", Gamma_d, Gamma_n, Delta);
	return Gamma_d*Gamma_n / ((e0+Delta-energy)*(e0+Delta-energy) + Gamma*Gamma/4) +
	       2.0/M_PI*A * penetration_factor(0, eta, kd*ad) * penetration_factor(0, 0.0, kn*an);
}

/*
Evaluates the cross section (barn = 100fm^2) at energy en (MeV) with R-matrix parameters:
		eB (MeV) where Bc = shift_factor(eB),
		gamma_d_2 (MeV) is the reduced deuteron width,
		gamma_n_2 (MeV) is the reduced neutron width,
		ad (fm) is the deuteron channel radius,
		an (fm) is the neutron channel radius,
and ue (MeV) is the electron screening potential.
*/
double cross_section(double en, double e0, double eB, double gamma_d_2,
		double gamma_n_2, double ad, double an, double ue, double A)
{
	if (en < 0.0 || e0 < 0.0 || eB < 0.0 || gamma_d_2 < 0.0 ||
			gamma_n_2 < 0.0 || ad < 0.0 || an < 0.0 || ue < 0.0) {
		return INFINITY;
	}
	double kd = relative_momentum(en, MU_D); // 1/fm
	double kn = relative_momentum(en+Q, MU_N); // 1/fm
	double kdB = relative_momentum(eB, MU_D); // 1/fm
	double knB = relative_momentum(eB+Q, MU_N); // 1/fm
	double eta = sommerfeld(en, MU_D); // dimensionless
	double etaB = sommerfeld(eB, MU_D); // dimensionless
  // printf("kdB = %.2e\n", kdB);
	double Bd = shift_factor(0, etaB, kdB*ad); // dimensionless
	double Bn = shift_factor(2, 0, knB*an); // dimensionless
  // printf("Bd = %.2e  kdB = %.2e  ad = %.2e\n", Bd, kdB, ad);
	return exp(M_PI*eta*ue/en) *
			M_PI/(kd*kd) * (2.0*J+1.0)/((2.0*J1+1.0)*(2.0*J2+1.0)) *
			Sdn2(en, e0, Bd, Bn, gamma_d_2, gamma_n_2, kd, ad, kn, an, eta, A) / 100;
}

/*
Evaluates the S-factor (MeV•barn = MeV•100fm^2) at energy en (MeV) with R-matrix parameters:
		eB (MeV) where Bc = shift_factor(eB),
		gamma_d_2 (MeV) is the reduced deuteron width,
		gamma_n_2 (MeV) is the reduced neutron width,
		ad (fm) is the deuteron channel radius,
		an (fm) is the neutron channel radius,
and ue (MeV) is the electron screening potential.
*/
double S_factor(double en, double e0, double eB, double gamma_d_2,
		double gamma_n_2, double ad, double an, double ue, double A)
{
	if (en < 0.0 || e0 < 0.0 || eB < 0.0 || gamma_d_2 < 0.0 ||
			gamma_n_2 < 0.0 || ad < 0.0 || an < 0.0 || ue < 0.0) {
		return INFINITY;
	}
	double kd = relative_momentum(en, MU_D); // 1/fm
	double kn = relative_momentum(en+Q, MU_N); // 1/fm
	double kdB = relative_momentum(eB, MU_D); // 1/fm
	double knB = relative_momentum(eB+Q, MU_N); // 1/fm
	double eta = sommerfeld(en, MU_D); // dimensionless
	double etaB = sommerfeld(eB, MU_D); // dimensionless
  // printf("kdB = %.2e\n", kdB);
	double Bd = shift_factor(0, etaB, kdB*ad); // dimensionless
	double Bn = shift_factor(2, 0, knB*an); // dimensionless
  // printf("Bd = %.2e  kdB = %.2e  ad = %.2e\n", Bd, kdB, ad);
	return exp(M_PI*eta*ue/en) * en * exp(2*M_PI*eta) *\
			M_PI/(kd*kd) * (2.0*J+1.0)/((2.0*J1+1.0)*(2.0*J2+1.0)) *\
			Sdn2(en, e0, Bd, Bn, gamma_d_2, gamma_n_2, kd, ad, kn, an, eta, A) / 100;
}

double S_factor_unitary_limit(double en, double ue)
{
	if (en < 0.0 || ue < 0.0) {
		return INFINITY;
	}
	double kd = relative_momentum(en, MU_D); // 1/fm
	double eta = sommerfeld(en, MU_D); // dimensionless
	return exp(M_PI*eta*ue/en) * en * exp(2*M_PI*eta) *\
			M_PI/(kd*kd) * (2.0*J+1.0)/((2.0*J1+1.0)*(2.0*J2+1.0)) * 1.0 / 100;
}


/* -*- Mode:C++; c-file-style:"gnu"; indent-tabs-mode:nil; -*- */
/*
 * Copyright (c) 2010 The Boeing Company
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation;
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * Author: Gary Pei <guangyu.pei@boeing.com>
 */

/*
 * This program is used to generate plots found in the paper
 * G. Pei and Tom Henderson, "Validation of ns-3 802.11b PHY model",
 * available online at http://www.nsnam.org/~pei/80211b.pdf
 *
 * It can be compiled as a C program and relies on a library installation of
 * the GNU Scientific Library (gsl).  To compile:
 *   gcc 80211b.c -o 80211b -lm -lgsl -lgslcblas
 *
 * The executable output should be redirected into a text file 80211b.txt
 *   ./80211b > 80211b.txt
 *
 * Then gnuplot can load the associated plot file which references 80211b.txt:
 *   gnuplot 80211b.plt
 */

#include "math.h"
#include "stdlib.h"
#include "stdio.h"
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_sf_bessel.h>
#define min(a,b) ((a) < (b) ? (a) : (b))
#define max(a,b) ((a) > (b) ? (a) : (b))
#define WLAN_SIR_perfect 10.0 // if SIR > 10dB, perfect reception
#define WLAN_SIR_impossible 0.1 // if SIR < -10dB, impossible to receive

/**
 * \ingroup wifi
 * \defgroup wifi-test wifi module tests
 */

/**
 * \ingroup wifi-test
 * \ingroup tests
 *
 * \brief fn_parameter_t structure
 */
typedef struct fn_parameter_t
{
  double beta; ///< beta
  double n; ///< n
} fn_parameters;

double QFunction (double x)
{
  return 0.5 * erfc (x / sqrt (2.0));
}

double f (double x, void * params)
{
  double beta = ((fn_parameters *) params)->beta;
  double n = ((fn_parameters *) params)->n;
  double f = pow ( 2 * gsl_cdf_ugaussian_P (x + beta) - 1, n - 1) * exp (-x * x / 2.0) / sqrt (2.0 * M_PI);
  return f;
}

double p_e2 (double e2)
{
  double sep;
  double error;
  fn_parameters params;
  params.beta = sqrt (2.0 * e2);
  params.n = 8.0;
  gsl_integration_workspace* w = gsl_integration_workspace_alloc (1000);
  gsl_function F;
  F.function = &f;
  F.params = &params;
  gsl_integration_qagiu (&F, -params.beta, 0, 1e-7, 1000, w, &sep, &error);
  gsl_integration_workspace_free (w);
  if (error == 0.0)
    {
      sep = 1.0;
    }
  return 1.0 - sep;
}

double p_e1 (double e1)
{
  return 1.0 - pow ( 1.0 - p_e2 (e1 / 2.0), 2.0);
}

double DbToNoneDb (double x)
{
  return pow (10.0, x / 10.0);
}

double NoneDbToDb (double x)
{
  return 10.0 * log10 (x);
}

double DQPSKFunction (double x)
{
  double pi = acos (-1.0);
  return ( (sqrt (2.0) + 1.0) / sqrt (8.0 * pi * sqrt (2.0))) * (1.0 / sqrt (x)) * exp ( -(2.0 - sqrt (2.0)) * x);
}

double Get80211bDsssDbpskBerIeee (double EcNc)
{
  double ber;
  if (EcNc > WLAN_SIR_perfect)
    {
      ber = 0;
    }
  else if (EcNc < WLAN_SIR_impossible)
    {
      ber =  0.5;
    }
  else
    {
      ber = min (QFunction (sqrt (11.0 * EcNc)),0.5);
    }
  return ber;
}

double Get80211bDsssDbpskBer (double sinr)
{
  double EbN0 = sinr * 22000000.0 / 1000000.0;
  double ber = 0.5 * exp (-EbN0);
  return ber;
}

double Get80211bDsssDqpskBerIeee (double EcNc)
{
  double ber;
  if (EcNc > WLAN_SIR_perfect)
    {
      ber = 0;
    }
  else if (EcNc < WLAN_SIR_impossible)
    {
      ber = 0.5;
    }
  else
    {
      ber = min (QFunction (sqrt (5.5 * EcNc)),0.5);
    }
  return ber;
}

double Get80211bDsssDqpskBer (double sinr)
{
// 2 bits per symbol, 1 MSPS
  double EbN0 = sinr * 22000000.0 / 1000000.0 / 2.0;
  double ber = DQPSKFunction (EbN0);
  return ber;
}

double Get80211bDsssDqpskCCK5_5BerIeee (double EcNc)
{
  double ber;
  if (EcNc > WLAN_SIR_perfect)
    {
      ber = 0.0;
    }
  else if (EcNc < WLAN_SIR_impossible)
    {
      ber = 0.5;
    }
  else
    {
      double pew = 14.0 * QFunction (sqrt (EcNc * 8.0)) + QFunction (sqrt (EcNc * 16.0));
      pew = min (pew, 0.99999);
      ber = 8.0 / 15.0 * pew;
    }
  return ber;
}

double Get80211bDsssDqpskCCK11BerIeee (double EcNc)
{
  double ber;
  if (EcNc > WLAN_SIR_perfect)
    {
      ber = 0.0;
    }
  else if (EcNc < WLAN_SIR_impossible)
    {
      ber = 0.5;
    }
  else
    {
      double pew = 24.0 * QFunction (sqrt (EcNc * 4.0))  + 16.0 * QFunction (sqrt (EcNc * 6.0))  + 174.0 * QFunction (sqrt (EcNc * 8.0)) + 16.0 * QFunction (sqrt (EcNc * 10.0)) + 24.0 * QFunction (sqrt (EcNc * 12.0)) + QFunction (sqrt (EcNc * 16.0));
      pew = min (pew, 0.99999);
      ber = 128.0 / 255.0 * pew;
    }
  return ber;
}

int main (int argc, char * argv[])
{
  double rss, sinr;
  double totalPkt = 200.0;
//double noise = 1.552058;  // (dB) this noise figure value corresponds to
  // -99 dBm noise floor reported in CMU paper
  double noise = 7;  // (dB) this noise figure value corresponds to the
                     // default in YansWifiPhy, and matches CMU testbed results
  double EcNc, EbN01, EbN02, EbN05, EbN011;
  double ieee1,ieee2,ieee5,ieee11;
  double numBits = (1024. + 40. + 14.) * 8.;
  double dbpsk,dqpsk,cck16,cck256,sepcck16,sepcck256;
  noise = DbToNoneDb (noise) * 1.3803e-23 * 290.0 * 22000000;
  for (rss = -102.0; rss <= -80.0; rss += 0.1)
    {
      sinr   = DbToNoneDb (rss) / 1000.0 / noise;
      EcNc   = sinr * 22000000.0 / 11000000.0; // IEEE sir
      EbN01  = sinr * 22000000.0 / 1000000.0;
      // 2 bits per symbol, 1 MSPS
      EbN02  = sinr * 22000000.0 / 1000000.0 / 2.0;
      EbN05  = sinr * 22000000.0 / 1375000.0 / 4.0;
      EbN011 = sinr * 22000000.0 / 1375000.0 / 8.0;
      // 1=rss, 2=EcNc, 3=EbN01, 4=EbN02, 5=EBN05, 6=EbN011
      printf ("%g %g %g %g %g %g ", rss, NoneDbToDb (EcNc),
              NoneDbToDb (EbN01),NoneDbToDb (EbN02),
              NoneDbToDb (EbN05),NoneDbToDb (EbN011));
      ieee1  = Get80211bDsssDbpskBerIeee (EcNc);
      ieee2  = Get80211bDsssDqpskBerIeee (EcNc);
      ieee5  = Get80211bDsssDqpskCCK5_5BerIeee (EcNc);
      ieee11 = Get80211bDsssDqpskCCK11BerIeee (EcNc);
      // 7=ber_ieee1, 8=ber_ieee2, 9=ber_ieee5, 10=ber_ieee11
      printf (" %g %g %g %g ", ieee1, ieee2,ieee5,ieee11);
      ieee1  = totalPkt * pow (1 - ieee1,  numBits);
      ieee2  = totalPkt * pow (1 - ieee2,  numBits);
      ieee5  = totalPkt * pow (1 - ieee5,  numBits);
      ieee11 = totalPkt * pow (1 - ieee11, numBits);
      // 11=pkt_ieee1, 12=pkt_ieee2, 13=pkt_ieee5, 14=pkt_ieee11
      printf (" %g %g %g %g ", ieee1, ieee2,ieee5,ieee11);
      dbpsk  = Get80211bDsssDbpskBer (sinr);
      dqpsk  = Get80211bDsssDqpskBer (sinr);
      cck16  = max (0, 8.0 / 15.0 * p_e2 (4.0 * EbN05 / 2.0));
      cck256 = max (0, 128.0 / 255.0 * p_e1 (8.0 * EbN011 / 2.0));
      // 15=ber_dbpsk, 16=ber_dqpsk, 17=ber_cck16, 18=ber_cck256
      printf (" %g %g %g %g ", dbpsk, dqpsk,cck16,cck256);
      dbpsk     = totalPkt * pow (1 - dbpsk,numBits);
      dqpsk     = totalPkt * pow (1 - dqpsk,numBits);
      sepcck16  = p_e2 (4.0 * EbN05 / 2.0);
      sepcck256 = p_e1 (8.0 * EbN011 / 2.0);
      cck16     = totalPkt * pow (1.0 - sepcck16,numBits / 4.0);
      cck256    = totalPkt * pow (1.0 - sepcck256,numBits / 8.0);
      // 19=pkt_dbpsk, 20=pkt_dqpsk, 21=pkt_cck16, 22=pkt_cck256
      printf (" %g %g %g %g ", dbpsk, dqpsk,cck16,cck256);
      // 23=sinr
      printf (" %g \n",NoneDbToDb (sinr));
    }
  return 0;
}


#include <stdio.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_const_cgsm.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include "global.h"
#include "eos_pres.h"
#include "eos_rho.h"
#include "param.h"

// the system of ODEs to be integrated
int
func (double r, const double y[], double f[], void *params)
{
  struct param *myparams = (struct param *) params;
  double (*pres) (double, void *);	// EOS pressure pointer
  double (*rho) (double, void *);	// EOS density pointer
  pres = &eos_pres;
  rho = &eos_rho;

// set a minimum pressure cutoff. if we don't, the ODE solver will wobble all
// over the surface and crash, or if you make the error tolerance really strict
// it'll integrate forever
  if (y[1] < 1.0e-9 * myparams->pinit)
    {
      if (myparams->single_star == 0)
	{
	  printf ("surface reached!\n");
	  printf ("%12s %12s %12s\n", "R (km)", "M (M_sun)",
		  "rho(0) (g/cm^3)");
	}
      printf ("%12le %12le %12le\n", r / 1.0e+5,
	      y[0] / GSL_CONST_CGSM_SOLAR_MASS, myparams->rho_init);
      return GSL_EBADFUNC;	// this flag tells GSL integrator to quit
    }
  // for a single star we can print M, P and rho as functions of r

  if (myparams->single_star == 0)
    {
      printf ("%12le %12le %12le %12le\n", r / 1.0e+5,
	      y[0] / GSL_CONST_CGSM_SOLAR_MASS, y[1], rho (y[1], myparams));
    }

  // mass conservation equation
  f[0] = 4.0 * M_PI * pow (r, 2.0) * rho (y[1], myparams);

  // TOV equation
  f[1] = -(GSL_CONST_CGSM_GRAVITATIONAL_CONSTANT / pow (r, 2.0))
    * (rho (y[1], myparams)
       + (y[1] / pow (GSL_CONST_CGSM_SPEED_OF_LIGHT, 2.0)))
    * (y[0] + 4 * M_PI * pow (r, 3.0)
       * y[1] / pow (GSL_CONST_CGSM_SPEED_OF_LIGHT, 2.0))
    / (1.0 - (2.0 * GSL_CONST_CGSM_GRAVITATIONAL_CONSTANT * y[0]
	      / (pow (GSL_CONST_CGSM_SPEED_OF_LIGHT, 2.0) * r)));

  // ODEs tend to return NaNs at the same radius where P < 0
  if ((gsl_isnan (f[0]) || gsl_isnan (f[1])))
    {
      /* print values at the radius where ODEs diverge. this is almost
         always at the surface */
      printf ("%12f %12f %12f\n", r / 1.0e+5,
	      y[0] / GSL_CONST_CGSM_SOLAR_MASS, myparams->rho_init);
      return GSL_EBADFUNC;
    }

  return GSL_SUCCESS;
}


//
// gemmtest.c
//
// J. Makino
//    Time-stamp: <10/10/17 13:43:33 makino>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <timerlib.h>
#ifndef NOBLAS
#include <cblas.h>
#endif
#ifdef USEGDR
#include "gdrdgemm.h"
#endif

#define FTYPE double
#include <emmintrin.h>
typedef double v2df __attribute__((vector_size(16)));
typedef union {v2df v; double s[2];}v2u;

#ifndef USEGDR
void  gdrsetboardid(int boardid)
{}
#endif

void matmul2_host(int n,
		  FTYPE a[n][n],
		  FTYPE b[n][n],
		  FTYPE c[n][n])

{
    int i, j, k;
    for(i=0;i<n;i++){
	for(j=0;j<n;j++){
	    c[i][j]=0.0e0;
	    for(k=0;k<n;k++) c[i][j]+= a[i][k]*b[k][j];
	}
    }
}

// simplest version
void matmul_for_small_nk_0(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int kk,
			 int n)
{
    // simplest version
    int i,j,k;
    for(j=0;j<n;j++)
	for(i=0;i<m;i++)
	    for(k=0;k<kk;k++)
		c[i][j] -= a[i][k]*b[k][j];
}

// XMM registers
#define X0	"%xmm0"
#define X1	"%xmm1"
#define X2	"%xmm2"
#define X3	"%xmm3"
#define X4	"%xmm4"
#define X5	"%xmm5"
#define X6	"%xmm6"
#define X7	"%xmm7"
#define X8		"%xmm8"
#define X9		"%xmm9"
#define X10		"%xmm10"
#define X11		"%xmm11"
#define X12		"%xmm12"
#define X13		"%xmm13"
#define X14		"%xmm14"
#define X15		"%xmm15"

#define LOADPD(mem, reg) asm("movapd %0, %"reg::"m"(mem));
#define STORPD(reg, mem) asm("movapd %"reg " , %0"::"m"(mem));
#define MOVNTPD(reg, mem) asm("movntpd %"reg " , %0"::"m"(mem));
#define MOVAPD(src, dst) asm("movapd " src "," dst);
#define MOVQ(src, dst) asm("movq " src "," dst);
#define BCAST0(reg) asm("shufpd $0x00, " reg ","  reg);
#define BCAST1(reg) asm("shufpd $0xff, " reg ","  reg);
#define MULPD(src, dst) asm("mulpd " src "," dst);
#define ADDPD(src, dst) asm("addpd " src ","  dst);
#define SUBPD(src, dst) asm("subpd "  src "," dst);

void daxpy(v2df a[], v2df b[], v2df c[], int n)
{
    int i;
    for(i=0;i<n; i+=8){
	b[i] += a[i]*c[0];
	b[i+1] += a[i+1]*c[0];
	b[i+2] += a[i+2]*c[0];
	b[i+3] += a[i+3]*c[0];
	b[i+4] += a[i+4]*c[0];
	b[i+5] += a[i+5]*c[0];
	b[i+6] += a[i+6]*c[0];
	b[i+7] += a[i+7]*c[0];
    }
}

void matmul_for_nk16_0a(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int n)
{

    int i,j;
    int kk = 16;
    int nh = n/2;
    register int k;
    v2df bcopy2[nh][kk];
    for(j=0;j<nh;j++)
	for(k=0;k<kk;k++)
	    //		bcopy2[j][k] = *((v2df*)(b[k]+j+j));
	    bcopy2[j][k] = *(((v2df*)b)+j*n2+k);
    for(i=0;i<m;i+=2){
	//	BEGIN_TSC;
	v2df *ap = (v2df*) a[i];
	v2df * cp = (v2df*) (&(c[i][0]));
	v2df *app = (v2df*) a[i+1];
	v2df * cpp = (v2df*) (&(c[i+1][0]));

	int k;

	for(j=0;j<nh;j+=2){
	    v2df * bvp0 = bcopy2[j];
	    v2df * bvp1 = bcopy2[j+1];
	    LOADPD(cp[j],X12);
	    LOADPD(cp[j+1],X13);
	    LOADPD(cpp[j],X14);
	    LOADPD(cpp[j+1],X15);

	    for(k=0;k<8;k++){

		LOADPD(ap[k],X0);
		LOADPD(app[k],X2);
		LOADPD(bvp0[k*2],X4);
		LOADPD(bvp1[k*2],X5);
		LOADPD(bvp0[k*2+1],X8);
		LOADPD(bvp1[k*2+1],X9);
		MOVAPD(X4,X6);
		MOVAPD(X5,X7);
		MOVAPD(X8,X10);
		MOVAPD(X9,X11);
		MOVAPD(X0,X1);
		BCAST0(X0);
		BCAST1(X1);
		MOVAPD(X2,X3);
		BCAST0(X2);
		BCAST1(X3);
		MULPD(X0,X4);
		MULPD(X0,X5);
		MULPD(X1,X8);
		MULPD(X1,X9);
		SUBPD(X4,X12);
		SUBPD(X5,X13);
		SUBPD(X8,X12);
		SUBPD(X9,X13);
		MULPD(X2,X6);
		MULPD(X2,X7);
		MULPD(X3,X10);
		MULPD(X3,X11);
		SUBPD(X6,X14);
		SUBPD(X7,X15);
		SUBPD(X10,X14);
		SUBPD(X11,X15);
	    }

	    STORPD(X12,cp[j+0]);
	    STORPD(X13,cp[j+1]);
	    STORPD(X14,cpp[j+0]);
	    STORPD(X15,cpp[j+1]);
	}
    }
}
void matmul_for_nk16_test1(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int n)
{

    int i,j,k;
    int kk = 16;
    int nh = n/2;
    v2df bcopy[nh][kk];
    for(j=0;j<nh;j++)
	for(k=0;k<kk;k++)
		bcopy[j][k] = *((v2df*)(b[k]+j+j));
    for(i=0;i<m;i+=2){

	v2df csub[nh][2];
	for(j=0;j<2;j++){
	    v2df * src = (v2df*) c[i+j];
	    for(k=0;k<nh;k++){
		csub[k][j]=src[k];
	    }
	}

	for(k=0;k<kk;k+=4){
	    v2df *ap = (v2df*) a[i]+k;
	    v2df *app = (v2df*) a[i+1]+k;
	    LOADPD(ap[0],X8);
	    LOADPD(ap[1],X9);
	    LOADPD(app[0],X10);
	    LOADPD(app[1],X11);
	    MOVAPD(X8,X0);
	    BCAST0(X0);
	    MOVAPD(X8,X1);
	    BCAST1(X1);
	    MOVAPD(X9,X2);
	    BCAST0(X2);
	    MOVAPD(X9,X3);
	    BCAST1(X3);
	    MOVAPD(X10,X4);
	    BCAST0(X4);
	    MOVAPD(X10,X5);
	    BCAST1(X5);
	    MOVAPD(X11,X6);
	    BCAST0(X6);
	    MOVAPD(X11,X7);
	    BCAST1(X7);
	    // 2x4 a, size doubled to 4x4
	    for(j=0;j<nh;j++){
		v2df * bvp = bcopy[j]+k;
		v2df * cvp = csub[k];
		LOADPD(bvp[0],X8);
		LOADPD(bvp[1],X9);

		MOVAPD(X8,X14);
		MOVAPD(X9,X15);
		MULPD(X0,X14);
		MULPD(X1,X15);

		LOADPD(bvp[2],X10);
		LOADPD(bvp[3],X11);

		LOADPD(cvp[0],X12);
		LOADPD(cvp[1],X13);

		SUBPD(X14,X12);
		SUBPD(X15,X12);
		MOVAPD(X10,X14);
		MOVAPD(X11,X15);
		MULPD(X2,X14);
		MULPD(X3,X15);
		SUBPD(X14,X12);
		SUBPD(X15,X12);
		MULPD(X4,X8);
		MULPD(X5,X9);
		MULPD(X6,X10);
		MULPD(X7,X11);
		SUBPD(X8,X13);
		SUBPD(X9,X13);
		SUBPD(X10,X13);
		SUBPD(X11,X13);
		STORPD(X12, cvp[0]);
		STORPD(X13, cvp[1]);
	    }
	}
	// copyback cmat
	for(j=0;j<2;j++){
	    v2df * dest = (v2df*) c[i+j];
	    for(k=0;k<nh;k++){
		dest[k] = csub[k][j];
	    }
	}
    }
}

void matmul_for_nk16_0c(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int n)
{

    int i,j;
    int kk = 16;
    int nh = n/2;
    register int k;
    v2df bcopy2[nh][kk];
#ifdef     PREFETCHL
#undef PREFETCHL
#endif
#define PREFETCHL 16

    for(i=0;i<PREFETCHL;i++){
	__builtin_prefetch((double*)a[i],0,0);
	__builtin_prefetch((double*)a[i]+8,0,0);
	__builtin_prefetch(c[i+8],1,0);
	__builtin_prefetch(c[i+8]+8,1,0);
    }
    for(k=0;k<kk;k++)
	for(j=0;j<nh;j++)
		bcopy2[j][k] = *((v2df*)(b[k]+j+j));
    for(i=0;i<m;i++){
	//	BEGIN_TSC;
	v2df *ap = (v2df*) a[i];
	v2df * cp = (v2df*) (&(c[i][0]));

	__builtin_prefetch((double*)a[i+PREFETCHL],0,0);
	__builtin_prefetch((double*)a[i+PREFETCHL]+8,0,0);

	int k;

	for(j=0;j<nh;j+=4){
	    __builtin_prefetch(c[i+PREFETCHL]+j,1,0);
	    v2df * bvp0 = bcopy2[j];
	    v2df * bvp1 = bcopy2[j+1];
	    v2df * bvp2 = bcopy2[j+2];
	    v2df * bvp3 = bcopy2[j+3];
	    LOADPD(cp[j],X12);
	    LOADPD(cp[j+1],X13);
	    LOADPD(cp[j+2],X14);
	    LOADPD(cp[j+3],X15);

	    for(k=0;k<8;k++){

		LOADPD(ap[k],X0);
		LOADPD(bvp0[k*2],X4);
		LOADPD(bvp1[k*2],X5);
		LOADPD(bvp2[k*2],X6);
		LOADPD(bvp3[k*2],X7);
		MOVAPD(X0,X1);
		BCAST0(X0);
		BCAST1(X1);
		LOADPD(bvp0[k*2+1],X8);
		LOADPD(bvp1[k*2+1],X9);
		LOADPD(bvp2[k*2+1],X10);
		LOADPD(bvp3[k*2+1],X11);
		MULPD(X0,X4);
		MULPD(X0,X5);
		MULPD(X0,X6);
		MULPD(X0,X7);
		MULPD(X1,X8);
		MULPD(X1,X9);
		MULPD(X1,X10);
		MULPD(X1,X11);
		SUBPD(X4,X12);
		SUBPD(X5,X13);
		SUBPD(X6,X14);
		SUBPD(X7,X15);
		SUBPD(X8,X12);
		SUBPD(X9,X13);
		SUBPD(X10,X14);
		SUBPD(X11,X15);
	    }

	    STORPD(X12,cp[j+0]);
	    STORPD(X13,cp[j+1]);
	    STORPD(X14,cp[j+2]);
	    STORPD(X15,cp[j+3]);
	}
    }
}
void matmul_for_nk32_0(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int n)
{

    int i,j;
    int kk = 32;
    int nh = n/2;
    register int k;
    v2df bcopy2[nh][kk];
#ifdef     PREFETCHL
#undef PREFETCHL
#endif
#define PREFETCHL 8

    for(i=0;i<PREFETCHL;i++){
	__builtin_prefetch((double*)a[i],0,0);
	__builtin_prefetch((double*)a[i]+8,0,0);
	__builtin_prefetch((double*)a[i]+16,0,0);
	__builtin_prefetch((double*)a[i]+24,0,0);
	__builtin_prefetch(c[i+8],1,0);
	__builtin_prefetch(c[i+8]+8,1,0);
	__builtin_prefetch(c[i+8]+16,1,0);
	__builtin_prefetch(c[i+8]+24,1,0);
    }
    for(k=0;k<kk;k++)
	for(j=0;j<nh;j++)
		bcopy2[j][k] = *((v2df*)(b[k]+j+j));
    for(i=0;i<m;i++){
	//	BEGIN_TSC;
	v2df *ap = (v2df*) a[i];
	v2df * cp = (v2df*) (&(c[i][0]));

	__builtin_prefetch((double*)a[i+PREFETCHL],0,0);
	__builtin_prefetch((double*)a[i+PREFETCHL]+8,0,0);
	__builtin_prefetch((double*)a[i+PREFETCHL]+16,0,0);
	__builtin_prefetch((double*)a[i+PREFETCHL]+24,0,0);

	int k;

	for(j=0;j<nh;j+=4){
	    __builtin_prefetch(c[i+PREFETCHL]+j,1,0);
	    v2df * bvp0 = bcopy2[j];
	    v2df * bvp1 = bcopy2[j+1];
	    v2df * bvp2 = bcopy2[j+2];
	    v2df * bvp3 = bcopy2[j+3];
	    LOADPD(cp[j],X12);
	    LOADPD(cp[j+1],X13);
	    LOADPD(cp[j+2],X14);
	    LOADPD(cp[j+3],X15);

	    for(k=0;k<16;k++){

		LOADPD(ap[k],X0);
		LOADPD(bvp0[k*2],X4);
		LOADPD(bvp1[k*2],X5);
		LOADPD(bvp2[k*2],X6);
		LOADPD(bvp3[k*2],X7);
		MOVAPD(X0,X1);
		BCAST0(X0);
		BCAST1(X1);
		MULPD(X0,X4);
		MULPD(X0,X5);
		MULPD(X0,X6);
		MULPD(X0,X7);
		LOADPD(bvp0[k*2+1],X8);
		LOADPD(bvp1[k*2+1],X9);
		LOADPD(bvp2[k*2+1],X10);
		LOADPD(bvp3[k*2+1],X11);
		MULPD(X1,X8);
		MULPD(X1,X9);
		MULPD(X1,X10);
		MULPD(X1,X11);
		SUBPD(X4,X12);
		SUBPD(X5,X13);
		SUBPD(X6,X14);
		SUBPD(X7,X15);
		SUBPD(X8,X12);
		SUBPD(X9,X13);
		SUBPD(X10,X14);
		SUBPD(X11,X15);
	    }

	    STORPD(X12,cp[j+0]);
	    STORPD(X13,cp[j+1]);
	    STORPD(X14,cp[j+2]);
	    STORPD(X15,cp[j+3]);
	}
    }
}

void matmul_for_nk16_0b(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int n)
{

    int i,j;
    int kk = 16;
    int nh = n/2;
    register int k;
    v2df bcopy2[nh][kk];
#ifdef     PREFETCHL
#undef PREFETCHL
#endif
#define PREFETCHL 16

    for(i=0;i<PREFETCHL;i++){
	__builtin_prefetch((double*)a[i],0,0);
	__builtin_prefetch((double*)a[i]+8,0,0);
	__builtin_prefetch(c[i+8],1,0);
	__builtin_prefetch(c[i+8]+8,1,0);
    }
    for(k=0;k<kk;k++)
	for(j=0;j<nh;j++)
		bcopy2[j][k] = *((v2df*)(b[k]+j+j));
    for(i=0;i<m;i++){
	//	BEGIN_TSC;
	v2df *ap = (v2df*) a[i];
	v2df * cp = (v2df*) (&(c[i][0]));

	__builtin_prefetch((double*)a[i+PREFETCHL],0,0);
	__builtin_prefetch((double*)a[i+PREFETCHL]+8,0,0);

	int k;

	for(j=0;j<nh;j+=4){
	    __builtin_prefetch(c[i+PREFETCHL]+j,1,0);
	    v2df * bvp0 = bcopy2[j];
	    v2df * bvp1 = bcopy2[j+1];
	    v2df * bvp2 = bcopy2[j+2];
	    v2df * bvp3 = bcopy2[j+3];
	    LOADPD(cp[j],X12);
	    LOADPD(cp[j+1],X13);
	    LOADPD(cp[j+2],X14);
	    LOADPD(cp[j+3],X15);

	    LOADPD(ap[0],X0);
	    LOADPD(bvp0[0],X4);
	    LOADPD(bvp1[0],X5);
	    LOADPD(bvp2[0],X6);
	    LOADPD(bvp3[0],X7);
	    LOADPD(bvp0[1],X8);
	    LOADPD(bvp1[1],X9);
	    LOADPD(bvp2[1],X10);
	    LOADPD(bvp3[1],X11);
	    MOVAPD(X0,X1);
	    BCAST0(X0);
	    BCAST1(X1);

	    MULPD(X0,X4);
	    MULPD(X0,X5);
	    MULPD(X0,X6);
	    MULPD(X0,X7);
	    MULPD(X1,X8);
	    MULPD(X1,X9);
	    MULPD(X1,X10);
	    MULPD(X1,X11);
	    SUBPD(X4,X12);
	    SUBPD(X5,X13);
	    SUBPD(X6,X14);
	    SUBPD(X7,X15);

	    SUBPD(X8,X12);
	    SUBPD(X9,X13);
	    SUBPD(X10,X14);
	    SUBPD(X11,X15);

	    LOADPD(ap[1],X0);

	    LOADPD(bvp0[2],X4);
	    LOADPD(bvp1[2],X5);
	    LOADPD(bvp2[2],X6);
	    LOADPD(bvp3[2],X7);
	    LOADPD(bvp0[3],X8);
	    LOADPD(bvp1[3],X9);
	    LOADPD(bvp2[3],X10);
	    LOADPD(bvp3[3],X11);
	    MOVAPD(X0,X1);
	    BCAST0(X0);
	    BCAST1(X1);

	    MULPD(X0,X4);
	    MULPD(X0,X5);
	    MULPD(X0,X6);
	    MULPD(X0,X7);
	    MULPD(X1,X8);
	    MULPD(X1,X9);
	    MULPD(X1,X10);
	    MULPD(X1,X11);
	    SUBPD(X4,X12);
	    SUBPD(X5,X13);
	    SUBPD(X6,X14);
	    SUBPD(X7,X15);

	    SUBPD(X8,X12);
	    SUBPD(X9,X13);
	    SUBPD(X10,X14);
	    SUBPD(X11,X15);

	    LOADPD(ap[2],X0);
	    LOADPD(bvp0[4],X4);
	    LOADPD(bvp1[4],X5);
	    LOADPD(bvp2[4],X6);
	    LOADPD(bvp3[4],X7);
	    LOADPD(bvp0[5],X8);
	    LOADPD(bvp1[5],X9);
	    LOADPD(bvp2[5],X10);
	    LOADPD(bvp3[5],X11);
	    MOVAPD(X0,X1);
	    BCAST0(X0);
	    BCAST1(X1);

	    MULPD(X0,X4);
	    MULPD(X0,X5);
	    MULPD(X0,X6);
	    MULPD(X0,X7);
	    MULPD(X1,X8);
	    MULPD(X1,X9);
	    MULPD(X1,X10);
	    MULPD(X1,X11);
	    SUBPD(X4,X12);
	    SUBPD(X5,X13);
	    SUBPD(X6,X14);
	    SUBPD(X7,X15);

	    SUBPD(X8,X12);
	    SUBPD(X9,X13);
	    SUBPD(X10,X14);
	    SUBPD(X11,X15);

	    LOADPD(ap[3],X0);
	    LOADPD(bvp0[6],X4);
	    LOADPD(bvp1[6],X5);
	    LOADPD(bvp2[6],X6);
	    LOADPD(bvp3[6],X7);
	    LOADPD(bvp0[7],X8);
	    LOADPD(bvp1[7],X9);
	    LOADPD(bvp2[7],X10);
	    LOADPD(bvp3[7],X11);
	    MOVAPD(X0,X1);
	    BCAST0(X0);
	    BCAST1(X1);

	    MULPD(X0,X4);
	    MULPD(X0,X5);
	    MULPD(X0,X6);
	    MULPD(X0,X7);
	    MULPD(X1,X8);
	    MULPD(X1,X9);
	    MULPD(X1,X10);
	    MULPD(X1,X11);
	    SUBPD(X4,X12);
	    SUBPD(X5,X13);
	    SUBPD(X6,X14);
	    SUBPD(X7,X15);

	    SUBPD(X8,X12);
	    SUBPD(X9,X13);
	    SUBPD(X10,X14);
	    SUBPD(X11,X15);

	    LOADPD(ap[4],X0);

	    LOADPD(bvp0[8],X4);
	    LOADPD(bvp1[8],X5);
	    LOADPD(bvp2[8],X6);
	    LOADPD(bvp3[8],X7);
	    LOADPD(bvp0[9],X8);
	    LOADPD(bvp1[9],X9);
	    LOADPD(bvp2[9],X10);
	    LOADPD(bvp3[9],X11);
	    MOVAPD(X0,X1);
	    BCAST0(X0);
	    BCAST1(X1);
	    MULPD(X0,X4);
	    MULPD(X0,X5);
	    MULPD(X0,X6);
	    MULPD(X0,X7);
	    MULPD(X1,X8);
	    MULPD(X1,X9);
	    MULPD(X1,X10);
	    MULPD(X1,X11);
	    SUBPD(X4,X12);
	    SUBPD(X5,X13);
	    SUBPD(X6,X14);
	    SUBPD(X7,X15);

	    SUBPD(X8,X12);
	    SUBPD(X9,X13);
	    SUBPD(X10,X14);
	    SUBPD(X11,X15);

	    LOADPD(ap[5],X0);
	    LOADPD(bvp0[10],X4);
	    LOADPD(bvp1[10],X5);
	    LOADPD(bvp2[10],X6);
	    LOADPD(bvp3[10],X7);
	    LOADPD(bvp0[11],X8);
	    LOADPD(bvp1[11],X9);
	    LOADPD(bvp2[11],X10);
	    LOADPD(bvp3[11],X11);
	    MOVAPD(X0,X1);
	    BCAST0(X0);
	    BCAST1(X1);

	    MULPD(X0,X4);
	    MULPD(X0,X5);
	    MULPD(X0,X6);
	    MULPD(X0,X7);
	    MULPD(X1,X8);
	    MULPD(X1,X9);
	    MULPD(X1,X10);
	    MULPD(X1,X11);
	    SUBPD(X4,X12);
	    SUBPD(X5,X13);
	    SUBPD(X6,X14);
	    SUBPD(X7,X15);

	    SUBPD(X8,X12);
	    SUBPD(X9,X13);
	    SUBPD(X10,X14);
	    SUBPD(X11,X15);

	    LOADPD(ap[6],X0);
	    LOADPD(bvp0[12],X4);
	    LOADPD(bvp1[12],X5);
	    LOADPD(bvp2[12],X6);
	    LOADPD(bvp3[12],X7);
	    LOADPD(bvp0[13],X8);
	    LOADPD(bvp1[13],X9);
	    LOADPD(bvp2[13],X10);
	    LOADPD(bvp3[13],X11);
	    MOVAPD(X0,X1);
	    BCAST0(X0);
	    BCAST1(X1);

	    MULPD(X0,X4);
	    MULPD(X0,X5);
	    MULPD(X0,X6);
	    MULPD(X0,X7);
	    MULPD(X1,X8);
	    MULPD(X1,X9);
	    MULPD(X1,X10);
	    MULPD(X1,X11);
	    SUBPD(X4,X12);
	    SUBPD(X5,X13);
	    SUBPD(X6,X14);
	    SUBPD(X7,X15);

	    SUBPD(X8,X12);
	    SUBPD(X9,X13);
	    SUBPD(X10,X14);
	    SUBPD(X11,X15);

	    LOADPD(ap[7],X0);
	    LOADPD(bvp0[14],X4);
	    LOADPD(bvp1[14],X5);
	    LOADPD(bvp2[14],X6);
	    LOADPD(bvp3[14],X7);
	    LOADPD(bvp0[15],X8);
	    LOADPD(bvp1[15],X9);
	    LOADPD(bvp2[15],X10);
	    LOADPD(bvp3[15],X11);
	    MOVAPD(X0,X1);
	    BCAST0(X0);
	    BCAST1(X1);

	    MULPD(X0,X4);
	    MULPD(X0,X5);
	    MULPD(X0,X6);
	    MULPD(X0,X7);
	    MULPD(X1,X8);
	    MULPD(X1,X9);
	    MULPD(X1,X10);
	    MULPD(X1,X11);
	    SUBPD(X4,X12);
	    SUBPD(X5,X13);
	    SUBPD(X6,X14);
	    SUBPD(X7,X15);

	    SUBPD(X8,X12);
	    SUBPD(X9,X13);
	    SUBPD(X10,X14);
	    SUBPD(X11,X15);

	    STORPD(X12,cp[j+0]);
	    STORPD(X13,cp[j+1]);
	    STORPD(X14,cp[j+2]);
	    STORPD(X15,cp[j+3]);
	}
    }
}

void matmul_for_nk8_0d(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int n)
{

    int i,j;
    int kk = 8;
    int nh = n/2;
    register int k;
    double bcopy[n][kk];
    v2df bcopy2[nh][kk];
    v2df acopy[kk];
    v2df acopy2[kk];
    v2df awork[4];
    v2df awork2[4];
    double *acp = (double*) acopy;
    double *acp2 = (double*) acopy2;
    unsigned long bpcount, apcount, dotcount;
    bpcount= apcount= dotcount=0;
    for(k=0;k<kk;k++)
	for(j=0;j<nh;j++)
		bcopy2[j][k] = *((v2df*)(b[k]+j+j));
    for(i=0;i<m;i++){
	//	BEGIN_TSC;
	double *ap=a[i];
	register v2df tmp, tmp2;
	v2df * cp = (v2df*) (&(c[i][0]));

	v2df * aa = (v2df*)(ap);
	__builtin_prefetch((double*)a[i+8],0,0);

	v2df acopy0=(v2df){a[i][0], a[i][0]};
	v2df acopy1=(v2df){a[i][1], a[i][1]};
	v2df acopy2=(v2df){a[i][2], a[i][2]};
	v2df acopy3=(v2df){a[i][3], a[i][3]};
	v2df acopy4=(v2df){a[i][4], a[i][4]};
	v2df acopy5=(v2df){a[i][5], a[i][5]};
	v2df acopy6=(v2df){a[i][6], a[i][6]};
	v2df acopy7=(v2df){a[i][7], a[i][7]};
	v2df zero=(v2df){0.0, 0.0};

	LOADPD(acopy0,X0);
	LOADPD(acopy1,X1);
	LOADPD(acopy2,X2);
	LOADPD(acopy3,X3);
	LOADPD(acopy4,X4);
	LOADPD(acopy5,X5);
	LOADPD(acopy6,X6);
	LOADPD(acopy7,X7);

	for(j=0;j<nh;j++){
	    __builtin_prefetch(c[i+8]+j,1,0);
	    v2df * bvp = bcopy2[j];
	    LOADPD(cp[j],X14);
	    LOADPD(bvp[0],X8);
	    LOADPD(bvp[1],X9);
	    MULPD(X0,X8);
	    MULPD(X1,X9);
	    LOADPD(bvp[2],X10);
	    LOADPD(bvp[3],X11);
	    ADDPD(X9,X8);
	    MULPD(X2,X10);
	    MULPD(X3,X11);
	    ADDPD(X11,X10);
	    LOADPD(bvp[4],X9);
	    LOADPD(bvp[5],X11);
	    LOADPD(bvp[6],X12);
	    LOADPD(bvp[7],X13);
	    MULPD(X4,X9);
	    MULPD(X5,X11);
	    ADDPD(X10,X8);
	    ADDPD(X11,X9);
	    MULPD(X6,X12);
	    MULPD(X7,X13);
	    ADDPD(X13,X12);
	    ADDPD(X9,X8);
	    ADDPD(X12,X8);
	    SUBPD(X8,X14);
	    STORPD(X14,cp[j]);
	}
    }
}

void matmul_for_nk8_0c(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int n)
{

    int i,j;
    int kk = 8;
    int nh = n/2;
    register int k;
    double bcopy[n][kk];
    v2df bcopy2[nh][kk];
    v2df acopy[kk];
    v2df acopy2[kk];
    v2df awork[4];
    v2df awork2[4];
    double *acp = (double*) acopy;
    double *acp2 = (double*) acopy2;
    unsigned long bpcount, apcount, dotcount;
    bpcount= apcount= dotcount=0;
    for(k=0;k<kk;k++)
	for(j=0;j<nh;j++)
		bcopy2[j][k] = *((v2df*)(b[k]+j+j));
    for(i=0;i<m;i++){
	//	BEGIN_TSC;
	double *ap=a[i];
	register v2df tmp, tmp2;
	v2df * cp = (v2df*) (&(c[i][0]));

	v2df * aa = (v2df*)(ap);
	__builtin_prefetch((double*)a[i+8],0,0);

	register v2df acopy0=(v2df){a[i][0], a[i][0]};
	register v2df acopy1=(v2df){a[i][1], a[i][1]};
	register v2df acopy2=(v2df){a[i][2], a[i][2]};
	register v2df acopy3=(v2df){a[i][3], a[i][3]};
	register v2df acopy4=(v2df){a[i][4], a[i][4]};
	register v2df acopy5=(v2df){a[i][5], a[i][5]};
	register v2df acopy6=(v2df){a[i][6], a[i][6]};
	register v2df acopy7=(v2df){a[i][7], a[i][7]};

	for(j=0;j<nh;j++){
	    tmp =  (v2df){0.0,0.0};
	    v2df ctmp= cp[j];

	    v2df * bp = bcopy2[j];
	    __builtin_prefetch(c[i+4]+j,1,0);
	    v2df *bvp = bp;
	    tmp += acopy0*bvp[0];
	    tmp +=acopy1*bvp[1];
	    tmp +=acopy2*bvp[2];
	    tmp +=acopy3*bvp[3];
	    tmp +=acopy4*bvp[4];
	    tmp +=acopy5*bvp[5];
	    tmp +=acopy6*bvp[6];
	    tmp +=acopy7*bvp[7];
	    cp[j] = ctmp -tmp;
	}
    }
    //    printf("m, kk, n = %d %d %d counts = %g %g  %g\n", m,kk,n,
    //	   (double)bpcount, (double)apcount, (double)dotcount);

}
void matmul_for_nk8_0b(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int n)
{

    int i,j;
    int kk = 8;
    int nh = n/2;
    register int k;
    double bcopy[n][kk];
    v2df bcopy2[nh][kk];
    v2df acopy[kk];
    v2df acopy2[kk];
    v2df awork[4];
    v2df awork2[4];
    double *acp = (double*) acopy;
    double *acp2 = (double*) acopy2;
    unsigned long bpcount, apcount, dotcount;
    bpcount= apcount= dotcount=0;
    for(k=0;k<kk;k++)
	for(j=0;j<nh;j++)
		bcopy2[j][k] = *((v2df*)(b[k]+j+j));
    for(i=0;i<m;i++){
	//	BEGIN_TSC;
	double *ap=a[i];
	register v2df tmp, tmp2;
	v2df * cp = (v2df*) (&(c[i][0]));

	v2df * aa = (v2df*)(ap);
	__builtin_prefetch((double*)a[i+8],0,0);

	acopy[0]=(v2df){a[i][0], a[i][0]};
	acopy[1]=(v2df){a[i][1], a[i][1]};
	acopy[2]=(v2df){a[i][2], a[i][2]};
	acopy[3]=(v2df){a[i][3], a[i][3]};
	acopy[4]=(v2df){a[i][4], a[i][4]};
	acopy[5]=(v2df){a[i][5], a[i][5]};
	acopy[6]=(v2df){a[i][6], a[i][6]};
	acopy[7]=(v2df){a[i][7], a[i][7]};

	for(j=0;j<nh;j++){
	    tmp = tmp2=  (v2df){0.0,0.0};
	    v2df ctmp= cp[j];

	    v2df * bp = bcopy2[j];
	    __builtin_prefetch(c[i+4]+j,1,0);
	    v2df *avp = acopy;
	    v2df *bvp = bp;
	    tmp += avp[0]*bvp[0];
	    tmp +=avp[1]*bvp[1];
	    tmp +=avp[2]*bvp[2];
	    tmp +=avp[3]*bvp[3];
	    tmp += avp[4]*bvp[4];
	    tmp +=avp[5]*bvp[5];
	    tmp +=avp[6]*bvp[6];
	    tmp +=avp[7]*bvp[7];
	    cp[j] = ctmp -tmp;
	}
    }
    //    printf("m, kk, n = %d %d %d counts = %g %g  %g\n", m,kk,n,
    //	   (double)bpcount, (double)apcount, (double)dotcount);

}
void matmul_for_nk8_0a(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int n)
{

    int i,j;
    int kk = 8;
    int nh = n/2;
    register int k;
    double bcopy[n][kk];
    v2df bcopy2[nh][kk];
    v2df acopy[kk];
    v2df acopy2[kk];
    v2df awork[4];
    v2df awork2[4];
    double *acp = (double*) acopy;
    double *acp2 = (double*) acopy2;
    unsigned long bpcount, apcount, dotcount;
    bpcount= apcount= dotcount=0;
    for(k=0;k<kk;k++)
	for(j=0;j<nh;j++)
		bcopy2[j][k] = *((v2df*)(b[k]+j+j));
    for(i=0;i<m;i+=2){
	//	BEGIN_TSC;
	double *ap=a[i];
	double *ap2=a[i+1];
	register v2df tmp, tmp2;
	v2df * cp = (v2df*) (&(c[i][0]));
	v2df * cp2 = (v2df*) (&(c[i+1][0]));

	v2df * aa = (v2df*)(ap);
	__builtin_prefetch((double*)a[i+8],0,0);
	__builtin_prefetch((double*)a[i+9],0,0);

	acopy[0]=(v2df){a[i][0], a[i][0]};
	acopy[1]=(v2df){a[i][1], a[i][1]};
	acopy[2]=(v2df){a[i][2], a[i][2]};
	acopy[3]=(v2df){a[i][3], a[i][3]};
	acopy[4]=(v2df){a[i][4], a[i][4]};
	acopy[5]=(v2df){a[i][5], a[i][5]};
	acopy[6]=(v2df){a[i][6], a[i][6]};
	acopy[7]=(v2df){a[i][7], a[i][7]};

	aa = (v2df*)(ap2);
	acopy2[0]= (v2df)  __builtin_ia32_shufpd(*aa,*aa,0x0);
	acopy2[1]=(v2df) __builtin_ia32_shufpd(*aa,*aa,0xff);
	aa++;
	acopy2[2]= (v2df)  __builtin_ia32_shufpd(*aa,*aa,0x0);
	acopy2[3]=(v2df) __builtin_ia32_shufpd(*aa,*aa,0xff);
	aa++;
	acopy2[4]= (v2df)  __builtin_ia32_shufpd(*aa,*aa,0x0);
	acopy2[5]=(v2df) __builtin_ia32_shufpd(*aa,*aa,0xff);
	aa++;
	acopy2[6]= (v2df)  __builtin_ia32_shufpd(*aa,*aa,0x0);
	acopy2[7]=(v2df) __builtin_ia32_shufpd(*aa,*aa,0xff);
	aa++;
	for(j=0;j<nh;j++){
	    tmp = tmp2=  (v2df){0.0,0.0};
	    v2df ctmp= cp[j];
	    v2df ctmp2 = cp2[j] ;

	    v2df * bp = bcopy2[j];
	    __builtin_prefetch(c[i+4]+j,1,0);
	    __builtin_prefetch(c[i+5]+j,1,0);
	    v2df *avp = acopy;
	    v2df *avp2 = acopy2;
	    v2df *bvp = bp;
	    tmp += avp[0]*bvp[0];
	    tmp2 += avp2[0]*bvp[0];
	    tmp +=avp[1]*bvp[1];
	    tmp2+=avp2[1]*bvp[1];
	    tmp +=avp[2]*bvp[2];
	    tmp2+=avp2[2]*bvp[2];
	    tmp +=avp[3]*bvp[3];
	    tmp2+=avp2[3]*bvp[3];
	    tmp += avp[4]*bvp[4];
	    tmp2 += avp2[4]*bvp[4];
	    tmp +=avp[5]*bvp[5];
	    tmp2+=avp2[5]*bvp[5];
	    tmp +=avp[6]*bvp[6];
	    tmp2+=avp2[6]*bvp[6];
	    tmp +=avp[7]*bvp[7];
	    tmp2+=avp2[7]*bvp[7];
	    cp[j] = ctmp -tmp;
	    cp2[j] = ctmp2 -tmp2;

	}

    }
    //    printf("m, kk, n = %d %d %d counts = %g %g  %g\n", m,kk,n,
    //	   (double)bpcount, (double)apcount, (double)dotcount);

}
void matmul_for_nk8_1(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int n)
{

    int i,j,k;
    const int kk = 8;
    const int kh = kk/2;
    int nh = n/2;
    v2df bcopy[n][kh];
    v2df acopy[kk][kh];
    unsigned long bpcount, apcount, dotcount;
    bpcount= apcount= dotcount=0;
    for(j=0;j<n;j++)
	for(k=0;k<kh;k++)
	    bcopy[j][k] = (v2df){b[k*2][j],b[k*2+1][j]};
    //    printf("copy b end\n");
    for(i=0;i<m;i+=kk){
	for(k=0;k<kk;k++){
	    v2df  *ak = (v2df*)(a[i+k]);
	    v2df * awp =acopy+k;
	    awp[0]=ak[0];
	    awp[1]=ak[1];
	    awp[2]=ak[2];
	    awp[3]=ak[3];
	}
	//	printf("copy a end\n");
	for(k=0;k<kk;k++){
	    v2u tmp, tmp1;
	    v2df * ap = acopy[k];
	    for(j=0;j<n;j+=2){
		tmp.v = ap[0]*bcopy[j][0]
		    + ap[1]*bcopy[j][1]
		    + ap[2]*bcopy[j][2]
		    + ap[3]*bcopy[j][3];
		tmp1.v = ap[0]*bcopy[j+1][0]
		    + ap[1]*bcopy[j+1][1]
		    + ap[2]*bcopy[j+1][2]
		    + ap[3]*bcopy[j+1][3];
		c[k+i][j] -= tmp.s[0]+tmp.s[1];
		c[k+i][j+1] -= tmp1.s[0]+tmp1.s[1];
	    }
	}
	//	printf("calc c end\n");
    }
    //    printf("m, kk, n = %d %d %d counts = %g %g  %g\n", m,kk,n,
    //	   (double)bpcount, (double)apcount, (double)dotcount);
}
void matmul_for_nk8_2(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int n)
{

    int i,j,k;
    const int kk = 8;
    const int kh = kk/2;
    int nh = n/2;
    v2df bcopy[nh][kk];
    v2df acopy[kk][kh];
    v2df ccopy[kk][kh];
    v2df acopy2[kk][kk];
    unsigned long bpcount, apcount, dotcount;
    bpcount= apcount= dotcount=0;
    for(k=0;k<kk;k++)
	for(j=0;j<nh;j++)
		bcopy[j][k] = *((v2df*)(b[k]+j+j));
    //    printf("copy b end\n");
    for(i=0;i<m;i+=kk){
	for(k=0;k<kk;k++){
	    __builtin_prefetch(a+i+k+8,0,0);
	    __builtin_prefetch(c+i+k+8,1,0);
	    v2df  *ak = (v2df*)(a[i+k]);
	    v2df * awp = (v2df*)(acopy+k);
	    v2df  *ck = (v2df*)(c[i+k]);
	    v2df * cwp = (v2df*)(ccopy+k);
	    awp[0]=ak[0];
	    awp[1]=ak[1];
	    awp[2]=ak[2];
	    awp[3]=ak[3];
	    cwp[0]=ck[0];
	    cwp[1]=ck[1];
	    cwp[2]=ck[2];
	    cwp[3]=ck[3];
	}
	for (j=0;j<n;j++){
	    double * ap = (double*)( acopy+j);
	    for (k=0;k<kk;k++){
		acopy2[j][k]=(v2df){ap[k],ap[k]};
	    }
	}
	//	printf("copy a end\n");
	for(k=0;k<kk;k++){
	    v2df * cp = (v2df*) ccopy[k];
	    v2df * ap = acopy2[k];
	    for(j=0;j<nh;j++){
		v2df * bp = bcopy[j];
		cp[j] -= ap[0]*bp[0]
		    + ap[1]*bp[1]
		    + ap[2]*bp[2]
		    + ap[3]*bp[3]
		    + ap[4]*bp[4]
		    + ap[5]*bp[5]
		    + ap[6]*bp[6]
		    + ap[7]*bp[7];
	    }
	}
	for(k=0;k<kk;k++){
	    v2df  *ck = (v2df*)(c[i+k]);
	    v2df * cwp = (v2df*)(ccopy+k);
#if 0
	    ck[0] = cwp[0];
	    ck[1] = cwp[1];
	    ck[2] = cwp[2];
	    ck[3] = cwp[3];
#endif
	    __builtin_ia32_movntpd((double*)(ck),cwp[0]);
	    __builtin_ia32_movntpd((double*)(ck+1),cwp[1]);
	    __builtin_ia32_movntpd((double*)(ck+2),cwp[2]);
	    __builtin_ia32_movntpd((double*)(ck+3),cwp[3]);
	}
	//	printf("calc c end\n");
    }
    //    printf("m, kk, n = %d %d %d counts = %g %g  %g\n", m,kk,n,
    //	   (double)bpcount, (double)apcount, (double)dotcount);
}
void matmul_for_nk8(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int n)
{

    int i,j,k;
    const int kk = 8;
    const int kh = kk/2;
    int nh = n/2;
    v2df bcopy[nh][kk];
    v2df acopy2[kk][kk];
    //    unsigned long bpcount, apcount, dotcount;
    //    bpcount= apcount= dotcount=0;

    for(k=0;k<kk;k++)
	for(j=0;j<nh;j++)
		bcopy[j][k] = *((v2df*)(b[k]+j+j));
    //    END_TSC(bpcount);
    //    printf("copy b end\n");
#pragma omp parallel for private(i,j,k,acopy2)	 schedule(static)
    for(i=0;i<m;i+=kk){
	//	BEGIN_TSC;
	for(k=0;k<kk;k++){
	    __builtin_prefetch(a+i+k+16,0,0);
	    __builtin_prefetch(c+i+k+16,1,0);
	}
	for (j=0;j<n;j++){
	    double * ap = (double*)( a[i+j]);
	    for (k=0;k<kk;k++){
		acopy2[j][k]=(v2df){ap[k],ap[k]};
	    }
	}
	//	END_TSC(apcount);
	//	printf("copy a end\n");
	//	BEGIN_TSC;
	for(k=0;k<kk;k++){
	    v2df * cp = (v2df*) (c[i+k]);
	    v2df * ap = acopy2[k];
	    for(j=0;j<nh;j++){
		v2df * bp = bcopy[j];
		cp[j] -= ap[0]*bp[0] + ap[1]*bp[1]
		    + ap[2]*bp[2]    + ap[3]*bp[3]
		    + ap[4]*bp[4]    + ap[5]*bp[5]
		    + ap[6]*bp[6]    + ap[7]*bp[7];
	    }
	}
	//	printf("calc c end\n");
	//	END_TSC(dotcount);
    }
    //    printf("m, kk, n = %d %d %d counts = %g %g  %g\n", m,kk,n,
    //   (double)bpcount, (double)apcount, (double)dotcount);
}

void matmul_for_nk16(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int n)
{
    int ii;
    int dm = (m+63)/64;
    dm*= 16;
#pragma omp parallel for private(ii)	 schedule(static)
    for(ii=0;ii<4;ii++){
	int ifirst, iend;
	ifirst = ii*dm;
	iend = ifirst+dm;
	if (iend > m) iend = m;
	//	fprintf(stderr, "m, i, ifirst, iend = %d %d %d %d\n", m, ii, ifirst, iend);
	if (ifirst < m){
	    matmul_for_nk16_0a(n1, a[ifirst], n2, b, n3, c[ifirst], iend-ifirst, n);
	}
    }

}

void matmul_for_nk32(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int n)
{
    int ii;
    int dm = (m+127)/128;
    dm*= 32;
#pragma omp parallel for private(ii)	 schedule(static)
    for(ii=0;ii<4;ii++){
	int ifirst, iend;
	ifirst = ii*dm;
	iend = ifirst+dm;
	if (iend > m) iend = m;
	//	fprintf(stderr, "m, i, ifirst, iend = %d %d %d %d\n", m, ii, ifirst, iend);
	if (ifirst < m){
	    matmul_for_nk32_0(n1, a[ifirst], n2, b, n3, c[ifirst], iend-ifirst, n);
	}
    }

}

#ifndef USEGDR
void gdrsetforceswapab(){}
void gdrresetforceswapab(){}
void gdrsetskipsendjmat(){};
void gdrresetskipsendjmat(){}
void gdrsetnboards(){}
void set_matmul_msg_level(int level){}
#endif

#define N 1024
#define K 16
int main()
{
    double a[N][K];
    double c[N][K];
    double b[K][K];
    int i,j;
    for(i=0;i<N;i++)
	for(j=0;j<K;j++){
	    a[i][j] = i*j;
	   c[i][j] = 0;
	}
    for(i=0;i<K;i++)
	for(j=0;j<K;j++)
	    b[i][j] = i*j;

    int k;
    unsigned long int start, end;
    rdtscl(&start);
#define NT 5000
    for(i=0;i<NT;i++){
	matmul_for_nk16_test1(K, a, K, b, K, c, N, K);
    }
    rdtscl(&end);
    printf("cycles = %e; %e ops/clock\n",
	   (double)(end-start), (N*K*K*(NT+0.0))/((double)(end-start)));
}


#include "q_incs.h"
#include <lapacke.h>
#include "eigenvectors.h"

int
eigenvectors(
             uint64_t n,
             double *W,
             double *A,
             double **X
            )
{
  int status = 0;

  char jobz = 'V'; /*want eigenvectors, use 'N' for eigenvalues only*/
  char uplo = 'U'; /*'U' for upper triangle of X, L for lower triangle of X*/
  int N = n;
  int LDA = N; /* dimensions of X = LDA by N*/

  for (uint64_t i = 0; i < n; i++ ) {
    for (uint64_t j = 0; j < n; j++ ) {
      A[i * n + j] = X[i][j];
    }
  }

  status = LAPACKE_dsyev(LAPACK_COL_MAJOR, jobz, uplo, N, A, LDA, W);
  return status;
}


/* -*- c -*- */
/*
 * Copyright (C) 2017 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio 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 GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

/*
 * generate MMSE interpolating differentiator FIR table values
 */

#include <math.h>
#include <assert.h>
#include <gsl/gsl_integration.h>

#define	MU	0.5			/* the MU for which we're computing coeffs */

#define	Ts	(1.0)			/* sampling period 		*/
#define	B	(1.0/(4*Ts))		/* one-sided signal bandwidth 	*/
//#define	B	(1.0/(8./3*Ts))		/* one-sided signal bandwidth 	*/

static unsigned global_n;
static double  *global_h;
double		global_mu = MU;
double		global_B  = B;

gsl_integration_workspace *global_gsl_int_workspace = NULL;

/*
 * This function computes the difference squared between the ideal
 * interpolating differentiator frequency response at frequency OMEGA and the
 * approximation defined by the FIR coefficients in global_h[]
 *
 * See eqn (9-7), "Digital Communication Receivers", Meyr, Moeneclaey
 * and Fechtel, Wiley, 1998. for the jist of what going on here.
 *
 * See eqns (7.99) and (7.100), "Discrete Time Signal Processing",
 * Oppenheim, Alan V., Schafer, Ronald W., Prentice Hall 1989. for the
 * (non-bandlimited) ideal differentiator.
 *
 * Andy Walls derived the expression for the h[n] for the bandlimited
 * differentiator by performing an Inverse DTFT on the bandlimited ideal
 * response. (Integration by parts.)
 */

static double
integrand (double omega, void *params)
{
  double real_ideal;
  double real_approx;
  double real_diff;
  double imag_ideal;
  double imag_approx;
  double imag_diff;

  int	 i, n;
  int	 I1;
  double *h;

  /*
   * Ideal differentiator frequency response,
   * with freq modulation to effect a time shift left (advance),
   * and band limited to (-fc, fc).
   * N.B. fc is global_B, mu is global_mu, Fs is 1/Ts, 2*pi*f is omega.
   * fc and Fs are in Hz, and omega is in radians/second.
   *
   * H(f) = j*2*pi*f*Ts * exp(j*2*pi*f*Ts*mu)  for |f| <= fc <= Fs/2
   */
  real_ideal = -omega * Ts * sin (omega * Ts * global_mu);
  imag_ideal =  omega * Ts * cos (omega * Ts * global_mu);

  n = global_n;
  h = global_h;
  I1 = -(n / 2);

  real_approx = 0;
  imag_approx = 0;

  for (i = 0; i < n; i++){
    real_approx += h[i] * cos (-omega * Ts * (i + I1));
    imag_approx += h[i] * sin (-omega * Ts * (i + I1));
  }

  real_diff = real_ideal - real_approx;
  imag_diff = imag_ideal - imag_approx;

  return real_diff * real_diff + imag_diff * imag_diff;
}

/*
 * Integrate the difference squared over all frequencies of interest.
 */
double
c_fcn (double *x, int n)
{
  gsl_function F;
  double result, error;

  F.function = integrand;
  F.params = NULL;

  assert ((n & 1) == 0);	/* assert n is even */
  global_n = n;
  global_h = x;
  // global_B is fc, the cutoff frequency in Hz.  It defaults to Fs/4.0.
  gsl_integration_qag(&F, -2 * M_PI * global_B, 2 * M_PI * global_B,
                      0.0, 1e-12, 1000, GSL_INTEG_GAUSS61,
                      global_gsl_int_workspace, &result, &error);
  return result;
}

/* this is the interface expected by the calling fortran code */

double
objective (double x[], int *ndim)
{
  return c_fcn (x, *ndim);
}

/*
 * starting guess for optimization
 */
void initpt (double x[], int ndim)
{
  int i;
  double w_c;
  double start;
  double t;
  double arg;
  double denom;

  /* Bandlimited, time shifted, differentiator filter (infinitely long)
   *
   * Andy Walls derived the expression for the h[n] for the bandlimited
   * differentiator by performing an Inverse DTFT on the bandlimited ideal
   * response. (Integration by parts.)
   *
   * Ideal differentiator frequency response,
   * with freq modulation to effect a time shift left (advance),
   * and band limited to (-fc, fc).
   * N.B. fc is global_B, mu is global_mu, Fs is 1/Ts.
   * fc and Fs are in Hz, and omega is in radians/second.
   *
   *   H(f) = j*2*pi*f*Ts * exp(j*2*pi*f*Ts*mu)  for |f| <= fc <= Fs/2
   *   H(f) = 0                                  for |f| >  fc <= Fs/2
   *
   * Substituting normalized radian frequency, w, for 2*pi*f*Ts, we get:
   *
   *   H(w) = j*w * exp(j*w*mu)  for |w| <= w_c = 2*pi*fc*Ts <= pi
   *   H(w) = 0                  for |w| >  w_c = 2*pi*fc*Ts <= pi
   *
   * and the Inverse Discrete Time Fourier Transform is:
   *
   *   h[n] = (1/(2*pi)) * integral(-pi,pi)[ H(w)*exp(j*w*n)*dw ]
   *
   * Performing integration by parts of the above expression, using these hints:
   *   integral [u*dv] = u*v - integral [v*du]
   *   u = j*w
   *   dv = exp(j*w*(n+mu))*dw
   *
   * We eventually get an infinitely long, but decaying impulse response:
   *
   * h[n] = (w_c/pi)/(n+mu) * [cos(pi*(w_c/pi)*(n+mu)) - sinc((w_c/pi)*(n+mu))]
   *
   *      = [ w_c*(n+mu)*cos(w_c*(n+mu)) - sin(w_c*(n+mu)) ] / (pi*(n+mu)^2)
   *
   * w_c is the normalized radian cutoff frequency:
   * 2*pi*fc*Ts, with fc <= Fs/2 and Fs = 1/Ts, so w_c <= pi
   *
   * For the case of w_c = pi and mu = 0, the cos() term alternates
   * between +/- 1 and the sinc() term goes to 0 except at n == 0, so
   * we get the following:
   *
   *    h[n] = (-1)^n / n for n != 0
   *         = 0          for n == 0
   * or
   *
   *    h[n] = ..., 1/5, -1/4, 1/3, -1/2, 1, 0, -1, 1/2, -1/3, 1/4, -1/5, ...
   *
   * Compare with:
   * Eqns (7.99) and (7.100), "Discrete Time Signal Processing",
   * Oppenheim, Alan V., Schafer, Ronald W., Prentice Hall 1989. for the
   * non-bandlimited (w_c = pi) ideal differentiator.
   */

  /* Truncated, bandlimited, time shifted, differentiator filter */
  w_c = 2.0 * M_PI * global_B * Ts;
  start = (double)(-ndim/2) + global_mu;

  for (i = 0; i < ndim; i++){
    t = (double)(i) + start;
    arg = w_c * t;
    denom = M_PI*t*t; /* always >= 0.0 */
    if (denom < 1e-9)
       x[i] = 0.0;
    else
       x[i] = (arg * cos(arg) - sin(arg))/denom;
  }
}


static char help[] = "ODE system solver example using TS.  Models two\n"
"equal-mass balls (particles) moving in the plane with either no\n"
"connection (free) or connected by a spring.  Solved via a Newtonian\n"
"formulation in cartesian coordinates (x,y) and velocities\n"
"(v=dx/dt,w=dy/dt). The system has dimension 8.\n\n";

// DEBUG check BDF2 convergence in free problem using Lagrangian formulation:
//for T in 2 3 4 5 7 8 9 10 11 12; do ./loose -ts_type bdf -ts_rtol 1.0e-$T -ts_atol 1.0e-$T; done

#include <petsc.h>

typedef struct {
    PetscBool    spring;
    PetscReal    g,     // m s-2;  acceleration of gravity
                 m,     // kg;     ball mass
                 l,     // m;      spring or rod length
                 k;     // N m-1;  spring constant
} LooseCtx;

extern PetscErrorCode SetInitial(Vec, LooseCtx*);
extern PetscErrorCode FreeExact(Vec, PetscReal, Vec, LooseCtx*);
extern PetscErrorCode NewtonRHSFcn(TS, PetscReal, Vec, Vec, void*);

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    PetscInt       steps;
    PetscReal      t0 = 0.0, tf = 2.0, dt = 0.1, errnorm;
    Vec            u, u0, uexact;
    TS             ts;
    LooseCtx       user;
    char           fstr[20] = "free";

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

    user.spring = PETSC_FALSE;
    user.g = 9.81;
    user.m = 58.0e-3; // 58 g for a tennis ball
    user.l = 0.5;     // spring length
    user.k = 20.0;    // spring constant (ignored by free)
    ierr = PetscOptionsBegin(PETSC_COMM_WORLD, "lse_", "options for loose",
                             ""); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-g", "acceleration of gravity (m s-2)",
                            "loose.c", user.g, &user.g, NULL); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-k", "spring constant (N m-1)", "loose.c",
                            user.k, &user.k, NULL); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-l", "spring length (m)", "loose.c",
                            user.l, &user.l, NULL); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-m", "mass of each ball (kg)", "loose.c",
                            user.m, &user.m, NULL); CHKERRQ(ierr);
    ierr = PetscOptionsBool("-spring","spring case (not free)",
                            "loose.c", user.spring, &user.spring,
                            NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);

    ierr = VecCreate(PETSC_COMM_WORLD,&u); CHKERRQ(ierr);
    ierr = TSCreate(PETSC_COMM_WORLD,&ts); CHKERRQ(ierr);
    ierr = TSSetApplicationContext(ts,&user); CHKERRQ(ierr);
    ierr = TSSetProblemType(ts,TS_NONLINEAR); CHKERRQ(ierr);

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

    // set up vecs and solver type
    ierr = VecSetSizes(u,PETSC_DECIDE,8); CHKERRQ(ierr);
    ierr = TSSetEquationType(ts, TS_EQ_ODE_EXPLICIT); CHKERRQ(ierr);
    ierr = TSSetRHSFunction(ts,NULL,NewtonRHSFcn,&user); CHKERRQ(ierr);
    ierr = TSSetType(ts,TSRK); CHKERRQ(ierr);

    // set-up of u, ts is complete
    ierr = VecSetFromOptions(u); CHKERRQ(ierr);
    ierr = TSSetFromOptions(ts); CHKERRQ(ierr);

    // set initial values and solve
    ierr = SetInitial(u, &user); CHKERRQ(ierr);
    ierr = TSSolve(ts, u); CHKERRQ(ierr);
    ierr = TSGetTime(ts, &tf); CHKERRQ(ierr);

    // numerical error in free case
    if (!user.spring) {
        ierr = VecDuplicate(u, &uexact); CHKERRQ(ierr);
        // get initial condition for evaluating exact solution
        ierr = VecDuplicate(u, &u0); CHKERRQ(ierr);
        ierr = SetInitial(u0, &user); CHKERRQ(ierr);
        ierr = FreeExact(u0, tf, uexact, &user); CHKERRQ(ierr);
        ierr = VecAXPY(u, -1.0, uexact); CHKERRQ(ierr); // u <- u - uexact
        ierr = VecNorm(u, NORM_INFINITY, &errnorm); CHKERRQ(ierr);
        VecDestroy(&u0);  VecDestroy(&uexact);
        ierr = PetscPrintf(PETSC_COMM_WORLD,
               "numerical error at tf in free problem: |u-uexact|_inf = %.5e\n",
               errnorm); CHKERRQ(ierr);
    }

    // report
    ierr = TSGetStepNumber(ts,&steps); CHKERRQ(ierr);
    if (user.spring)
        strcpy(fstr, "spring");
    ierr = PetscPrintf(PETSC_COMM_WORLD,
               "%s problem solved to tf = %.3f (%d steps)\n",
               fstr, tf, steps); CHKERRQ(ierr);

    VecDestroy(&u);  TSDestroy(&ts);
    return PetscFinalize();
}

/* Variable order used in the following functions:
u[0] = q_1 (= x_1)
u[1] = q_2 (= y_1)
u[2] = q_3 (= x_2)
u[3] = q_4 (= y_2)
u[4] = v_1 (= dx_1/dt)
u[5] = v_2 (= dy_1/dt)
u[6] = v_3 (= dx_2/dt)
u[7] = v_4 (= dy_2/dt)
*/

PetscErrorCode SetInitial(Vec u, LooseCtx *user) {
    /* Set initial conditions. */
    PetscErrorCode   ierr;
    PetscReal        *au;
    ierr = VecGetArray(u,&au); CHKERRQ(ierr);
    au[0] = 0.0;              // q1 = x1
    au[1] = 1.0;              // q2 = y1
    au[2] = 0.0;              // q3 = x2
    au[3] = au[1] + user->l;  // q4 = y2;  satisfies (1)
    au[4] = 10.0;             // v1
    au[5] = 10.0;             // v2
    au[6] = 15.0;             // v3
    au[7] = au[5];            // v4;  satisfies (2)
    ierr = VecRestoreArray(u,&au); CHKERRQ(ierr);
    return 0;
}

PetscErrorCode FreeExact(Vec u0, PetscReal tf, Vec uexact, LooseCtx *user) {
    /* Exact solution based on parabolic motion.  Problem
        m x'' = 0,       x(0) = x0,  x'(0) = v0
        m y'' = - m g,   y(0) = y0,  y'(0) = w0
    has solution
        x(t) = x0 + v0 t,              x'(t) = v0,
        y(t) = y0 + w0 t - (g/2) t^2,  y'(t) = w0 - g t    */
    PetscErrorCode ierr;
    PetscReal *au0, *auex;
    if (user->spring) {
        SETERRQ(PETSC_COMM_SELF,7,"exact solution only implemented for free\n");
    }
    ierr = VecGetArray(u0, &au0); CHKERRQ(ierr);
    ierr = VecGetArray(uexact, &auex); CHKERRQ(ierr);
    auex[0] = au0[0] + au0[4] * tf;
    auex[1] = au0[1] + au0[5] * tf - 0.5 * user->g * tf * tf;
    auex[2] = au0[2] + au0[6] * tf;
    auex[3] = au0[3] + au0[7] * tf - 0.5 * user->g * tf * tf;
    auex[4] = au0[4];
    auex[5] = au0[5] - user->g * tf;
    auex[6] = au0[6];
    auex[7] = au0[7] - user->g * tf;
    ierr = VecRestoreArray(u0, &au0); CHKERRQ(ierr);
    ierr = VecRestoreArray(uexact, &auex); CHKERRQ(ierr);
    return 0;
}

PetscErrorCode NewtonRHSFcn(TS ts, PetscReal t, Vec u, Vec G, void *ctx) {
    PetscErrorCode   ierr;
    LooseCtx         *user = (LooseCtx*)ctx;
    const PetscReal  *au;
    PetscReal        *aG;
    PetscReal        dspring = 0.0, cspring = 0.0, Fx1, Fy1, Fx2, Fy2;

    PetscFunctionBeginUser;
    ierr = VecGetArrayRead(u,&au); CHKERRQ(ierr);
    ierr = VecGetArray(G,&aG); CHKERRQ(ierr);
    Fx1 = 0.0;
    Fy1 = - user->m * user->g;
    Fx2 = 0.0;
    Fy2 = - user->m * user->g;
    if (user->spring) {
        dspring = PetscSqrtReal(  (au[0] - au[2]) * (au[0] - au[2])
                                + (au[1] - au[3]) * (au[1] - au[3]) );
        if (dspring == 0.0) {
            SETERRQ(PETSC_COMM_SELF,1,"exact ball collision (unlikely?)\n");
        }
        cspring = user->k * (1.0 - user->l / dspring);
        Fx1 -= cspring * (au[0] - au[2]);
        Fy1 -= cspring * (au[1] - au[3]);
        Fx2 += cspring * (au[0] - au[2]);
        Fy2 += cspring * (au[1] - au[3]);
    }
    aG[0] = au[4];
    aG[1] = au[5];
    aG[2] = au[6];
    aG[3] = au[7];
    aG[4] = Fx1 / user->m;
    aG[5] = Fy1 / user->m;
    aG[6] = Fx2 / user->m;
    aG[7] = Fy2 / user->m;
    ierr = VecRestoreArrayRead(u,&au); CHKERRQ(ierr);
    ierr = VecRestoreArray(G,&aG); CHKERRQ(ierr);
    PetscFunctionReturn(0);
}


/*solver/fixed_point.c
* Solves a fixed point equation using
* sequence acceleration.
*
* Author: Benjamin Vatter j.
* email : benjaminvatterj@gmail.com
* date: 15 August 2015
*/

#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include "fixed_point.h"

/*
* Finds the fixed point of a function f
* Translated from SciPy's fixed_point function
*/
void fixed_point(int n, ff_function *f, double *x0,
                 double xtol, int maxiter, double *out)
{
	//out = malloc(sizeof(double)*n);
	double *p = malloc(sizeof(double)*n);
	double *p1, *p2;
	double d;
	int pass;
	int i, j;

	for (i=0; i<n; i++)
		out[i] = x0[i];

	for (i=0; i<maxiter; i++)
	{
		p1 = malloc(sizeof(double)*n);
		p2 = malloc(sizeof(double)*n);
		f->function(out, f->params, p1);
		f->function(p1, f->params, p2);

		pass = 1;
		for (j=0; j<n; j++)
		{
			d = p2[j] - 2.0*p1[j] + out[j];
			if (d==0) {
				p[j] = p2[j];
			} else {
				p[j] = out[j] - pow(p1[j] - out[j], 2.0) / d;
			}

			if (out[j] == 0 && fabs(p[j]) > xtol){
				pass = 0;
			}
			if (out[j] != 0 && (p[j] - out[j])/out[j] > xtol){
				pass = 0;
			}
			out[j] = p[j];
		}
		if (pass == 1) {
			free(p1);
			free(p2);
			break;
		}
		if(i+1 >= maxiter){
			free(p1);
			free(p2);
			pass = -1;
			break;
		}
		free(p1);
		free(p2);
	}

	free(p);
	// Free garbage result in case of no convergence
	if (pass == -1) {
		free(out);
	}
}


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

class Randomizer
{
public:
    Randomizer(unsigned long int seed = 0);
    Randomizer(const Randomizer& R);
    ~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);
    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() const { return r; }
    inline unsigned long int Seed() const { return seed; }

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

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

#endif


/*
 * Test the effective difference of a single bit changing on the input.
 * Evaluate each bit position on the first hash against all the bit
 * positions on the other hash for any less than random distribution.
 *
 * This tests for the "Bit Independence Criterion" of this hash.
 */

#include "hash_bbt.h"

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

#include <stdint.h>
#include <stdio.h>
#include <string.h>

#define HASH_PARAMS bbt_table_1
extern struct bbt_hash_patterns HASH_PARAMS;

#define HASH_INPUT_SZ 32
//#define SAMPLE_SZ 1048576
//#define SAMPLE_SZ 100
#define SAMPLE_SZ 65536

#define HASH_BITS_COUNT (8*sizeof(bbt_hash_t))
#define COUNT_MATRIX_SZ (HASH_BITS_COUNT * HASH_BITS_COUNT)
#define BIT_ROTATE(HASH,SHIFT) ((bbt_hash_t)((HASH)<<(SHIFT)) | (bbt_hash_t)((HASH)>>(BBT_HASH_WIDTH - (SHIFT))))

int main(int argc, char **argv) {

	if (argc < 2) {
		printf("Usage: %d\n", argc);
		printf("Usage: %s <bit position> <numeric random seed>\n", argv[0]);
		return 1;
	}

	unsigned argPos = 1;
	unsigned long int bitPos = strtoul(argv[argPos++], NULL, 10);
	if (bitPos > ((8*HASH_INPUT_SZ)-1)) {
		fprintf(stderr, "ERROR: bitPos too large. Beyond end of buffer length.\n");
		return 1;
	}
	unsigned long int seed = strtoul(argv[argPos++], NULL, 10);

	unsigned char hashInput[HASH_INPUT_SZ];
	gsl_rng *rng = gsl_rng_alloc(gsl_rng_mt19937); /* Mersenne Twister */
	gsl_rng_set(rng, seed);

	bbt_hash_ctxt hc;
	bbt_hash_init(&hc, &(HASH_PARAMS));

	unsigned bitIndex = bitPos/8;
	unsigned bitShift = 7 - (bitPos%8);
	bbt_hash_t bitMask = ((bbt_hash_t)1) << bitShift;

	bbt_hash_t bitMatrix[HASH_BITS_COUNT];
	unsigned countMatrix[COUNT_MATRIX_SZ];
	memset(countMatrix, 0, COUNT_MATRIX_SZ * sizeof(unsigned));

	for (unsigned sampleCount = 0; sampleCount < SAMPLE_SZ; sampleCount++) {
		unsigned char *p = hashInput;
		for (unsigned i = 0; i < HASH_INPUT_SZ; i++) {
			*p = gsl_rng_get(rng) & 0xFF;
			p++;
		}

		bbt_hash_calc(&hc, hashInput, HASH_INPUT_SZ);
		bbt_hash_t hashA = bbt_hash_getHash(&hc);
		bbt_hash_reset(&hc);

		hashInput[bitIndex] = hashInput[bitIndex] ^ bitMask;

		bbt_hash_calc(&hc, hashInput, HASH_INPUT_SZ);
		bbt_hash_t hashB = bbt_hash_getHash(&hc);
		bbt_hash_reset(&hc);

		for (unsigned i = 0; i < HASH_BITS_COUNT; i++) {
			bitMatrix[i] = hashB ^ BIT_ROTATE(hashA, i);
		}

		for (unsigned i = 0; i < HASH_BITS_COUNT; i++) {
			bbt_hash_t hashC = bitMatrix[i];
			for (unsigned j = 0; j < HASH_BITS_COUNT; j++) {
				if ((hashC & 1) != 0) {
					countMatrix[j * HASH_BITS_COUNT + i] ++;
				}
				hashC = hashC >> 1;
			}
		}
	}

	printf("cat(\"Bit change at index %u and shift %u.\\n\")\n", bitIndex, bitShift);
	printf("sampleSize <- %u\n", SAMPLE_SZ);
	printf("cat(\"Sample size is %u.\\n\")\n", SAMPLE_SZ);
	printf("cat(\"Half of sample size is %u.\\n\")\n", SAMPLE_SZ/2);
	printf("data <- c(\n");
	for (unsigned i = 0; i < HASH_BITS_COUNT; i++) {
		for (unsigned j = 0; j < HASH_BITS_COUNT; j++) {
			printf("%u, ", countMatrix[j * HASH_BITS_COUNT + i]);
		}
		printf("\n");
	}
	printf("0)\n");
	printf("dataMatrix <- matrix(data[1:%lu], ncol=%lu, byrow=T)\n", COUNT_MATRIX_SZ, HASH_BITS_COUNT);
	printf("cat(\"Summary of the distribution of counts:\\n\")\n");
	printf("print(summary(as.vector(dataMatrix)))\n");
	printf("p <- dataMatrix / sampleSize\n");
	printf("q <- 1-p\n");
	printf("h <- -p*log(p, base=2) -q*log(q, base=2)\n");
	printf("cat(\"Summary of the distribution of H (entropy):\\n\")\n");
	printf("print(summary(as.vector(h)))\n");

	gsl_rng_free(rng);
	return 0;
}


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

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

int
main ()
{
  gsl_rng * r ;

  int i, n = 10;
  double mu = 3.0;

  /* create a generator chosen by the environment variable GSL_RNG_TYPE */

  gsl_rng_env_setup();

  r = gsl_rng_alloc (gsl_rng_default);

  /* print n random variates chosen from the poisson distribution with
     mean parameter mu */

  for (i = 0; i < n; i++)
    {
      unsigned int k = gsl_ran_poisson (r, mu);

      printf(" %u", k);
    }

  printf("\n");
}


/**
 * \file include/shg/numalg.h
 * Numerical algorithms.
 */

#ifndef SHG_NUMALG_H
#define SHG_NUMALG_H

#include <functional>
#include <limits>
#include <stdexcept>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_multimin.h>
#include <shg/fcmp.h>
#include <shg/matrix.h>
#include <shg/vector.h>

namespace SHG {

/**
 * Degree of polynomial in one variable.
 *
 * \returns \f$\max \{i: a_i \neq 0, i = 0, \ldots, m - 1 \}\f$ if it
 * exists, otherwise -1.
 *
 * \throws std::invalid_argument if a.size() == 0 or a.size() > 32768.
 */
template <template <class> class Indexed_container, class T>
int degree_of_polynomial(const Indexed_container<T>& a, T eps);

/**
 * Calculates real roots of polynomial a[n] x^n + ... + a[2] x^2 +
 * a[1] x^1 + a[0]. Calculated roots are put in x[0] < x[1] < ... <
 * x[m - 1], where m = x.size().
 *
 * \throws std::invalid_argument if degree of polynomial is less than
 * 1
 * \throws std::runtime_error if the GSL routine fails
 */
void real_roots(const Vecdouble& a, Vecdouble& x);

template <template <class> class Indexed_container, class T>
int degree_of_polynomial(const Indexed_container<T>& a, T eps) {
     static_assert(std::numeric_limits<int>::max() >= 32767,
                   "According to the standard, INT_MAX must be at "
                   "least 32767.");
     auto k = a.size();
     if (k == 0 || k > 32768u)
          throw std::invalid_argument(
               "invalid polynomial in degree_of_polynomial");
     while (k-- > 0)
          if (fane(a[k], T(0), eps))
               return k;
     return -1;
}

/**
 * Solves the equation \f$a_0 + a_1x + \ldots + a_{n - 1}x^{n - 1} =
 * 0\f$, \f$n \geq 1\f$.
 */
void solve_polynomial(const Vecdouble& a, Veccomplex& x);

/**
 * Solves system of linear equations.
 */
void solve_linear(Matdouble& a, Vecdouble& x);

/** Light wrapper for GSL minimizer. */
class Minimizer_base {
public:
     Minimizer_base(const gsl_min_fminimizer_type* T);
     Minimizer_base(const Minimizer_base&) = delete;
     Minimizer_base& operator=(const Minimizer_base&) = delete;
     virtual ~Minimizer_base();
     gsl_min_fminimizer* get() const;

private:
     gsl_min_fminimizer* s_;
};

/** Wrapper for GSL minimizer. */
class Minimizer : Minimizer_base {
public:
     Minimizer(const gsl_min_fminimizer_type* T);
     Minimizer(const Minimizer&) = delete;
     Minimizer& operator=(const Minimizer&) = delete;

     bool is_set() const { return is_set_; }

     void set(gsl_function* f, double x_minimum, double x_lower,
              double x_upper);

     int iterate(int max_iter, double epsabs, double epsrel);

     int iter() const { return iter_; }

     double x_minimum() const { return x_minimum_; }
     double x_lower() const { return x_lower_; }
     double x_upper() const { return x_upper_; }
     double f_minimum() const { return f_minimum_; }
     double f_lower() const { return f_lower_; }
     double f_upper() const { return f_upper_; }

private:
     gsl_min_fminimizer* s_{get()};
     int iter_{0};
     bool is_set_{false};
     double x_minimum_{};
     double x_lower_{};
     double x_upper_{};
     double f_minimum_{};
     double f_lower_{};
     double f_upper_{};
};

/**
 * Uniform search for minimum of the function of one variable.
 *
 * The method search() searches uniformly the interval \f$[a, b]\f$
 * for minima of the function \f$f\f$. The method is inspired by
 * \cite bronsztejn-siemiendiajew-musiol-muhlig-2004, section
 * 18.2.4.1, point 1, page 946.
 *
 * First, \f$n\f$ is calculated as \f[ n = \min \{ 2, \lceil
 * \frac{2}{\epsilon} (b - a) \rceil \}, \f] and then the interval
 * \f$[a, b]\f$ is divided into \f$n\f$ intervals \f$[x_k, x_{k +
 * 1}]\f$, \f$k = 0, \ldots n - 1\f$, where \f$x_k = a + kh\f$, \f$h =
 * (b - a) / n\f$. The values of \f$f\f$ are calculated in each
 * \f$x_k\f$ and all values \f[ (x_k, x_{k + 1}, x_{k + 2}, f(x_k),
 * f(x_{k + 1}), f(x_{k + 2})) \f] such that \f[ f(x_k) > f(x_{k + 1})
 * < f(x_{k + 2}) \f] are reported in \f$\var{result}\f$. The length
 * of the interval \f$[x_k, x_{k + 2}]\f$ is not greater than
 * \f$\epsilon\f$. The function is evaluated \f$n + 1\f$times.
 *
 * \throws std::invalid_argument if on entry \f$a \geq b\f$ or
 * \f$\epsilon \leq 0\f$.
 */
struct Uniform_search_for_minimum {
     struct Result {
          double x_lower;
          double x_minimum;
          double x_upper;
          double f_lower;
          double f_minimum;
          double f_upper;
     };
     std::vector<Result> result{};
     void search(std::function<double(double)> f, double a, double b,
                 double eps);
};

/** Light wrapper for GSL minimizer for functions of several
    variables. */
class Multimin_fminimizer_base {
public:
     Multimin_fminimizer_base(const gsl_multimin_fminimizer_type* T,
                              size_t n);
     Multimin_fminimizer_base(const Multimin_fminimizer_base&) =
          delete;
     Multimin_fminimizer_base& operator=(
          const Multimin_fminimizer_base&) = delete;
     virtual ~Multimin_fminimizer_base();
     gsl_multimin_fminimizer* get() const;

private:
     gsl_multimin_fminimizer* s_;
};

/** Wrapper for GSL minimizer for functions of several variables. */
class Multimin_fminimizer : public Multimin_fminimizer_base {
public:
     Multimin_fminimizer(const gsl_multimin_fminimizer_type* T,
                         size_t n);
     Multimin_fminimizer(const Multimin_fminimizer&) = delete;
     Multimin_fminimizer& operator=(const Multimin_fminimizer&) =
          delete;
     virtual ~Multimin_fminimizer();

     bool is_set() const { return is_set_; }

     int iter() const { return iter_; }

     void set(gsl_multimin_function* f, const std::vector<double>& x,
              const std::vector<double>& step);
     int iterate(int max_iter, double eps);

     const std::vector<double>& x_minimum() const {
          return x_minimum_;
     }
     double f_minimum() const { return f_minimum_; }

private:
     gsl_multimin_fminimizer* s_{get()};
     int iter_{0};
     bool is_set_{false};
     std::size_t n_;
     gsl_vector* x_;
     gsl_vector* ss_;
     std::vector<double> x_minimum_;
     double f_minimum_;
};

}  // namespace SHG

#endif


/* ode-initval/rk2imp.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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.
 */

/* Runge-Kutta 2, Gaussian implicit */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>

#include "odeiv_util.h"

typedef struct
{
  double *knu;
  double *ytmp;
}
rk2imp_state_t;

static void *
rk2imp_alloc (size_t dim)
{
  rk2imp_state_t *state = (rk2imp_state_t *) malloc (sizeof (rk2imp_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for rk2imp_state",
                      GSL_ENOMEM);
    }

  state->knu = (double *) malloc (dim * sizeof (double));

  if (state->knu == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for knu", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->knu);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  return state;
}

static int
rk2imp_apply (void *vstate,
              size_t dim,
              double t,
              double h,
              double y[],
              double yerr[],
              const double dydt_in[],
              double dydt_out[], const gsl_odeiv_system * sys)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;

  const int iter_steps = 3;
  int status = 0;
  int nu;
  size_t i;

  double *const knu = state->knu;
  double *const ytmp = state->ytmp;

  /* initialization step */
  if (dydt_in != NULL)
    {
      DBL_MEMCPY (knu, dydt_in, dim);
    }
  else
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t, y, knu);
      GSL_STATUS_UPDATE (&status, s);
    }

  /* iterative solution */
  for (nu = 0; nu < iter_steps; nu++)
    {
      for (i = 0; i < dim; i++)
        {
          ytmp[i] = y[i] + 0.5 * h * knu[i];
        }
      {
        int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, knu);
        GSL_STATUS_UPDATE (&status, s);
      }
    }

  /* assignment */
  for (i = 0; i < dim; i++)
    {
      y[i] += h * knu[i];
      yerr[i] = h * h * knu[i];
      if (dydt_out != NULL)
        dydt_out[i] = knu[i];
    }

  return status;
}

static int
rk2imp_reset (void *vstate, size_t dim)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;

  DBL_ZERO_MEMSET (state->knu, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);

  return GSL_SUCCESS;
}

static unsigned int
rk2imp_order (void *vstate)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 2;
}

static void
rk2imp_free (void *vstate)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;
  free (state->knu);
  free (state->ytmp);
  free (state);
}

static const gsl_odeiv_step_type rk2imp_type = { "rk2imp",      /* name */
  1,                            /* can use dydt_in */
  0,                            /* gives exact dydt_out */
  &rk2imp_alloc,
  &rk2imp_apply,
  &rk2imp_reset,
  &rk2imp_order,
  &rk2imp_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_rk2imp = &rk2imp_type;


#pragma once

#ifndef _NOGSL
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>
#include <vector>

using namespace std;

typedef double (* GD_F_TYPE)(const gsl_vector*, void*);
typedef void (* GD_DF_TYPE)(const gsl_vector*, void*, gsl_vector*);
typedef void (* GD_FDF_TYPE)(const gsl_vector*, void*, double*, gsl_vector*);

class GradientDescent {
	int N; // Total number of components
	gsl_vector *x;  // The current state
	gsl_vector *ds; // The step size

	gsl_multimin_fdfminimizer* minidf;  // The GSL minimizer for when derivatives needed
	const gsl_multimin_fdfminimizer_type *Tdf ;
	gsl_multimin_function_fdf myfundf;

public:
	double INIT_STEP_SIZE = 2.0;
	double TOLERANCE = 0.1;
	double PRECISION = 1e-3;
	double GRAD_PRECISION = 1e-3;
	double ITERATIONS = 4000;

	GradientDescent(int N_p): N(N_p) {
		Tdf = gsl_multimin_fdfminimizer_vector_bfgs2;
		minidf = gsl_multimin_fdfminimizer_alloc(Tdf, N);
		myfundf.n = N;
		x = gsl_vector_alloc(N);
	}

	~GradientDescent() {
		gsl_multimin_fdfminimizer_free(minidf);
		gsl_vector_free(x);
	}

	gsl_vector* getResults() {
		return minidf->x;
	}
	void init(GD_F_TYPE f, GD_DF_TYPE df, GD_FDF_TYPE fdf, void* p);
	double optimize(gsl_vector* initX, bool stopAtZero = true); // optimizes until value is 0
};

#endif