/*
 *  PIXON
 *  A Pixon-based method for reconstructing velocity-delay map in reverberation mapping.
 *
 *  Yan-Rong Li, liyanrong@mail.ihep.ac.cn
 *
 */

/*!
 *  \file mathfun.c
 *  \brief mathematic functions.
 */

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

#include "mathfun.h"

#define EPS (DBL_MIN)

/*!
 * This function calculates matrix multiply C(nxn) = A(nxn) * B(nxn).
 */
void multiply_mat(double * a, double *b, double *c, int n)
{
  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, n, n, n, 1.0f
                             , a, n, b, n, 0.0f, c, n);
}

/*!
 * This function calculates matrix multiply C(nxn) = A^T(nxn) * B(nxn).
 */
void multiply_mat_transposeA(double * a, double *b, double *c, int n)
{
  cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, n, n, n, 1.0f
                             , a, n, b, n, 0.0f, c, n);
}

/*!
 * This function calculates matrix multiply C(nxn) = A(nxn) * B^T(nxn).
 */
void multiply_mat_transposeB(double * a, double *b, double *c, int n)
{
  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, n, n, n, 1.0f
                             , a, n, b, n, 0.0f, c, n);
}

/*!
 * This function calculates matrix multiply Y(n) = A^T(nxn) * X(n).
 */
void multiply_matvec_transposeA(double *a, double *x, int n, double *y)
{
  cblas_dgemv(CblasRowMajor, CblasTrans, n, n, 1.0f, a, n, x, 1, 0.0f, y, 1);
}

/*!
 * This function calculates matrix multiply Y(n) = A(nxn) * X(n).
 */
void multiply_matvec(double *a, double *x, int n, double *y)
{
  cblas_dgemv(CblasRowMajor, CblasNoTrans, n, n, 1.0f, a, n, x, 1, 0.0f, y, 1);
}

/*!
 * This function calculates  Y(m) = A(m, n) * X(n).
 */
void multiply_matvec_MN(double * a, int m, int n, double *x, double *y)
{
  cblas_dgemv(CblasRowMajor, CblasNoTrans, m, n, 1.0f, a, n, x, 1, 0.0f, y, 1);
}

/* C(m*n) = A(m*k) * B(k*n) */
void multiply_mat_MN(double * a, double *b, double *c, int m, int n, int k)
{
  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, m, n, k, 1.0f
                             , a, k, b, n, 0.0f, c, n);
}

/* C(m*n) = A^T(m*k) * B(k*n) */
void multiply_mat_MN_transposeA(double * a, double *b, double *c, int m, int n, int k)
{
  cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, m, n, k, 1.0f
                             , a, m, b, n, 0.0f, c, n);
}
/* C(m*n) = A(m*k) * B^T(k*n) */
void multiply_mat_MN_transposeB(double * a, double *b, double *c, int m, int n, int k)
{
  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, m, n, k, 1.0f
                             , a, k, b, k, 0.0f, c, n);
}

/* A(mxm)^-1 * B(mxn), store the output in B
 * note that A will be changed on exit. */
int multiply_mat_MN_inverseA(double * a, double *b, int m, int n)
{
  int * ipiv, info;
  ipiv=malloc(m*sizeof(int));

  info=LAPACKE_dgetrf(LAPACK_ROW_MAJOR, m, m, a, m, ipiv);
  if(info!=0)
  {
    printf("multiply_mat_MN_inverseA 1.\n this usually caused by improper nc.\n increase the low limit of nc");
    exit(0);
    return info;
  }
  info = LAPACKE_dgetrs(LAPACK_ROW_MAJOR, 'N', m, n, a, m, ipiv, b, n);
  if(info!=0)
  {
    printf("multiply_mat_MN_inverseA 2\n this usually caused by improper nc.\n increase the low limit of nc");
    exit(0);
    return info;
  }

  free(ipiv);
  return info;
}

/*!
 * This functions calculate A^-1(nxn).
 * A is a generic matrix.
 */
void inverse_mat(double * a, int n, int *info)
{
  int * ipiv;
  ipiv=malloc(n*sizeof(int));

//  dgetrf_(&n, &n, a, &n, ipiv, info);
//  dgetri_(&n, a, &n, ipiv, work, &lwork, info);

  LAPACKE_dgetrf(LAPACK_ROW_MAJOR, n, n, a, n, ipiv);
  LAPACKE_dgetri(LAPACK_ROW_MAJOR, n, a, n, ipiv);

  free(ipiv);
  return;
}

/*!
 * This functions calculate A^-1(nxn).
 * A is a symmetric matrix.
 */
void inverse_symat(double * a, int n, int *info)
{
  int * ipiv, i, j;
  ipiv=malloc(n*sizeof(int));

  LAPACKE_dsytrf(LAPACK_ROW_MAJOR, 'U', n, a, n, ipiv);
  LAPACKE_dsytri(LAPACK_ROW_MAJOR, 'U', n, a, n, ipiv);

  /* fill up the lower triangle */
  for(i=0; i<n; i++)
    for(j=0; j<i; j++)
      a[i*n+j] = a[j*n+i];

  free(ipiv);
  return;
}

/*!
 * This functions calculate A^-1(nxn).
 * A is a postive-definite symmetrix matrix.
 */
void inverse_pomat(double * a, int n, int *info)
{
  int i, j;

  LAPACKE_dpotrf(LAPACK_ROW_MAJOR, 'U', n, a, n);
  LAPACKE_dpotri(LAPACK_ROW_MAJOR, 'U', n, a, n);

  /* fill up the lower triangle */
  for(i=0; i<n; i++)
    for(j=0; j<i; j++)
      a[i*n+j] = a[j*n+i];
  return;
}

/*!
 * This function calculate eigenvectors and eigenvalues of matrix.
 */
void eigen_sym_mat(double *a, int n, double *val, int *info)
{
    char jobz='V', uplo='U';

/* store the eigenvectors  in a by rows.
 * store the eigenvalues in val in ascending order.
 */
//    dsyev_(&jobz, &uplo, &n, a, &n, val, work, &lwork, info);

/* store the eigenvectors  in a by columns.
 * store the eigenvalues in val in ascending order.
 */
    LAPACKE_dsyev(LAPACK_ROW_MAJOR, jobz, uplo, n, a, n, val);
    return;
}

/*!
 * This function calculate A(nxn) = X^T(1xn)*X(1xn)
 */
void multiply_vec2mat(double * x, double * a, int n)
{
//  cblas_dsyr(CblasRowMajor, CblasUpper, n, 1.0f, x, 1, a, n);
  int i, j;
  for(i=0; i<n; i++)
    for(j=0; j<=i; j++)
    {
      a[i*n+j] = a[j*n+i] = x[i]*x[j];
    }
}

/*!
 * This function calculates determinant of matrix A. \n
 * There are two versions in the internet, the main difference lies at dealing with the sign of det. \n
 * The version II is verifed to be \b INCORRECT. \n
 * Note that LAPACK is written in Fortran, the indix diffes by 1 with that in C.
 * LAPACK version 3.5.0
 * ********************************
 * Version I:
 * \code{.sh}
 *  det = 1.0;
 *  for(i=0; i<n; i++)
 *  {
 *   det *= a[i*n+i];
 *   if (ipiv[i] != i+1)
 *   {
 *     det = -det;
 *   }
 *  }
 * \endcode
 * ********************************
 * Version II:
 * \code{.sh}
 *  det = 1.0;
 *  for(i=0; i<n; i++)
 *  {
 *   det *= a[i*n+i];
 *   if (ipiv[i] != i+1)
 *   {
 *     ipiv[ipiv[i]-1] = ipiv[i];
 *     det = -det;
 *   }
 *  }
 * \endcode
 * ********************************
 */
double det_mat(double *a, int n, int *info)
{
  int * ipiv;
  int i;
  double det;
  ipiv=malloc(n*sizeof(int));

  /* LU decomposition */
//  dgetrf_(&n, &n, a, &n, ipiv, info);
  *info=LAPACKE_dgetrf(LAPACK_ROW_MAJOR, n, n, a, n, ipiv);
  if(*info!=0)
  {
    printf("# Error, Wrong in det_mat!\n");
    exit(-1);
  }

  det = 1.0;
  for(i=0; i<n; i++)
  {
    printf("%d\n", ipiv[i]);
    det *= a[i*n+i];
    if (ipiv[i] != i+1) // note that LAPACK is written in C, the indix diffes by 1 with C.
    {
      det = -det;
    }
  }
  //det=fabs(det);
  free(ipiv);
  return det;
}

/*!
 * This function calculates logarithm determinant of matrix.
 */
double lndet_mat(double *a, int n, int *info)
{
  int * ipiv;
  int i;
  double lndet;
  ipiv=malloc(n*sizeof(int));

  /* LU factorization */
//  dgetrf_(&n, &n, a, &n, ipiv, info);
  *info=LAPACKE_dgetrf(LAPACK_ROW_MAJOR, n, n, a, n, ipiv);
  if(*info!=0)
  {
    printf("Wrong!\n");
    exit(-1);
  }

  lndet = 0.0;
  for(i=0; i<n; i++)
  {
    lndet += log(a[i*n+i]);
  }
  free(ipiv);
  return lndet;
}

/*!
 * This function performs Cholesky decomposition of matrix into upper triangle matrixs
 */
void Chol_decomp_U(double *a, int n, int *info)
{
  int i,j;
  char uplo = 'U';   // decomposite as A = U^T*U, i.e., upper triangle matrix.
//  dpotrf_(&uplo, &n, a, &n, info);
  *info=LAPACKE_dpotrf(LAPACK_ROW_MAJOR, uplo, n, a, n);
  if(*info<0)
  {
    fprintf(stderr, "The %d-th argument had an illegal value!\n", *info);
//    exit(-1);
    return;
  }
  else if (*info>0)
  {
    fprintf(stderr, "The leading minor of order %d is not positive definite, and the factorization could not be completed.\n", *info);
//    exit(-1);
    return;
  }

  // only the upper triangle is referenced by dpotrf output,
  // so the strictly lower triangle must be set to zero
  for(i=0;i<n;i++)
    for(j=0;j<i;j++)
      a[i*n+j] = 0.0;
  return;
}

/*!
 * This function performs Cholesky decomposition of matrix into lower triangle matrixs
 */
void Chol_decomp_L(double *a, int n, int *info)
{
  int i,j;
  char uplo = 'L';   // decomposite as A = L*L^T, i.e., lower triangle matrix.
//  dpotrf_(&uplo, &n, a, &n, info);
  *info=LAPACKE_dpotrf(LAPACK_ROW_MAJOR, uplo, n, a, n);
  if(*info<0)
  {
    fprintf(stderr, "The %d-th argument had an illegal value!\n", *info);
//    exit(-1);
    return;
  }
  else if (*info>0)
  {
    fprintf(stderr, "The leading minor of order %d is not positive definite, and the factorization could not be completed.\n", *info);
//    exit(-1);
    return;
  }

  // only the lower triangle is referenced by dpotrf output,
  // so the strictly upper triangle must be set to zero
  for(i=0;i<n;i++)
    for(j=i+1;j<n;j++)
      a[i*n+j] = 0.0;
  return;
}

/*
 * semiseparable matrix
 */
void compute_semiseparable_drw(double *t, int n, double a1, double c1, double *sigma, double syserr, double *W, double *D, double *phi)
{
  int i;
  double S, A;
  phi[0] = 0.0;
  for(i=1; i<n; i++)
  {
    phi[i] = exp(-c1 * (t[i] - t[i-1]));
  }

  S = 0.0;
  A = sigma[0]*sigma[0] + syserr*syserr + a1;
  D[0] = A;
  W[0] = 1.0/D[0];
  for(i=1; i<n; i++)
  {
    S = phi[i]*phi[i] * (S + D[i-1]*W[i-1]*W[i-1]);
    A = sigma[i]*sigma[i] + syserr*syserr + a1;
    D[i] = A - a1 * a1 * S;
    W[i] = 1.0/D[i] * (1.0 - a1*S);
  }
}
/*
 * z = C^-1 x y
 *
 * y is a vector
 */
void multiply_matvec_semiseparable_drw(double *y, double  *W, double *D, double *phi, int n, double a1, double *z)
{
  int i;
  double f, g;

  // forward substitution
  f = 0.0;
  z[0] = y[0];
  for(i=1; i<n;i++)
  {
    f = phi[i] * (f + W[i-1] * z[i-1]);
    z[i] = y[i] - a1*f;
  }

  //backward substitution
  g = 0.0;
  z[n-1] = z[n-1]/D[n-1];
  for(i=n-2; i>=0; i--)
  {
    g = phi[i+1] *(g + a1*z[i+1]);
    z[i] = z[i]/D[i] - W[i]*g;
  }
}
/*
 * Z = C^-1 x Y
 *
 * Y is an (nxm) matrix.
 * Note that Y is row-major
 */
void multiply_mat_semiseparable_drw(double *Y, double  *W, double *D, double *phi, int n, int m, double a1, double *Z)
{
  int i, j;
  double f, g;

  // forward substitution
  for(j=0; j<m; j++)
  {
    f = 0.0;
    Z[0*m+j] = Y[0*m+j];
    for(i=1; i<n;i++)
    {
      f = phi[i] * (f + W[i-1] * Z[(i-1)*m + j]);
      Z[i*m+j] = Y[i*m+j] - a1*f;
    }
  }

  //backward substitution
  for(j=0; j<m; j++)
  {
    g = 0.0;
    Z[(n-1)*m+j] = Z[(n-1)*m+j]/D[n-1];
    for(i=n-2; i>=0; i--)
    {
      g = phi[i+1] *(g + a1*Z[(i+1)*m+j]);
      Z[i*m+j] = Z[i*m+j]/D[i] - W[i]*g;
    }
  }
}

/*
 * Z = C^-1 x Y^T
 *
 * Y is an (mxn) matrix.
 * Note that Y is row-major
 */
void multiply_mat_transposeB_semiseparable_drw(double *Y, double  *W, double *D, double *phi, int n, int m, double a1, double *Z)
{
  int i, j;
  double f, g;

  // forward substitution
  for(j=0; j<m; j++)
  {
    f = 0.0;
    Z[0*m+j] = Y[0+j*n];
    for(i=1; i<n;i++)
    {
      f = phi[i] * (f + W[i-1] * Z[(i-1)*m + j]);
      Z[i*m+j] = Y[i+j*n] - a1*f;
    }
  }

  //backward substitution
  for(j=0; j<m; j++)
  {
    g = 0.0;
    Z[(n-1)*m+j] = Z[(n-1)*m+j]/D[n-1];
    for(i=n-2; i>=0; i--)
    {
      g = phi[i+1] *(g + a1*Z[(i+1)*m+j]);
      Z[i*m+j] = Z[i*m+j]/D[i] - W[i]*g;
    }
  }
}

/**
 *  calculate A^-1.
 *
 *  A = LxDxL^T, L = I + tril(UxW^T), D is a diagonal matrix.
 *
 *  M = LxD^1/2,  A = MxM^T,  A^-1 = (M^T)^-1xM^-1.
 */
void inverse_semiseparable(double *t, int n, double a1, double c1, double *sigma,
                           double syserr, double *W, double *D, double *phi,
                           double *A, double *lndet)
{
  int i, j;

  compute_semiseparable_drw(t, n, a1, c1, sigma, syserr, W, D, phi);

  *lndet = 0.0;
  for(i=0; i<n; i++)
  {
    A[i*n + i] = 1.0 * sqrt(D[i]);
    for(j=0; j<i; j++)
    {
      A[i*n + j] = a1 * (exp(-c1*(t[i]-t[j]))*W[j]) * sqrt(D[j]);
    }

    *lndet += log(D[i]);
  }
  LAPACKE_dtrtri(LAPACK_ROW_MAJOR, 'L', 'N', n, A, n);

  LAPACKE_dlauum(LAPACK_ROW_MAJOR, 'L', n, A, n);

  /* fill up upper triangle */
  for(i=0; i<n; i++)
    for(j=i+1; j<n; j++)
      A[i*n+j] = A[j*n+i];
  return;
}

/**
 *  caclulate inverse of a DRW semiseparable matrix, which
 *  is a tridiagonal matrix.
 */
void inverse_semiseparable_uv(double *t, int n, double a1, double c1, double *A)
{
  int i, j;
  double b1, b2, dt;

  dt = t[1] - t[0];
  b1 = 1.0/( a1 * (exp(-c1*dt) - exp(c1*dt)) );
  A[0] = - b1 * exp(c1*dt);
  A[1] = A[1*n+0] = b1;
  for(i=2; i<n; i++)
    A[i] = 0.0;

  for(i=1; i<n-1; i++)
  {
    for(j=0; j<i-1; j++)
      A[i*n+j] = A[j*n+i] = 0.0;

    for(j=i+1; j<n; j++)
      A[i*n+j] = A[j*n+i] = 0.0;

    dt = t[i+1] - t[i];
    b2 = 1.0/( a1 * (exp(-c1*dt) - exp(c1*dt)) );
    dt = t[i+1] - t[i-1];
    A[i*n+i] = -b1 * b2 *  a1 * (exp(-c1*dt) - exp(c1*dt));

    A[i*n+(i+1)] = A[(i+1)*n + i] = b2;

    b1 = b2;
  }

  i = n-1;
  dt = t[i] - t[i-1];
  A[i*n+i] = - b1 * exp(c1 * dt);

  for(j=0; j<n-2; j++)
    A[i*n+j] = 0.0;
  return;
}

/**
 *  calculate Q = [S^-1+N^-1]^-1, where S is a DRW symmetric semiseparable matrix
 *  and N is a diagonal matrix.
 *
 *  S^-1 + N^-1 is a tridiagonal symmetric matrix.
 *
 *  [S^-1 + N^-1]^-1 is a semiseparable symmetric matrix.
 *
 */
void compute_inverse_semiseparable_plus_diag(double *t, int n, double a1, double c1,
                double *sigma, double syserr, double *u, double *v,
                double *W, double *D, double *phi, double *work)
{
  int i;
  double *a, *b, f1, f2;

  a = work;
  b = work + n;

  phi[0] = 0.0;
  for(i=1; i<n; i++)
  {
    phi[i] = exp(-c1 * (t[i] - t[i-1]));
  }

  /* first S^-1 + N^-1 */
  f1 = (phi[1]*phi[1] - 1.0) + EPS;
  b[0] = phi[1]/( a1 * f1);
  a[0] = - 1.0/( a1 * f1) + 1.0/(sigma[0]*sigma[0] + syserr*syserr);

  for(i=1; i<n-1; i++)
  {
    f2 = (phi[i+1]*phi[i+1] -1.0) + EPS;
    b[i] = phi[i+1]/( a1 *  f2);
    a[i] = -1.0/a1 * (1.0 + 1.0/f1 + 1.0/f2)  + 1.0/(sigma[i]*sigma[i] + syserr*syserr);

    f1 = f2;
  }
  i = n-1;
  a[i] = - 1.0 / (a1 * f1) + 1.0/(sigma[i]*sigma[i] + syserr*syserr);;

  /* now inverse [S^-1+N^-1]^-1*/
  v[0] = 1.0 ;
  v[1] = -a[0]/b[0] * phi[1];
  for(i=2; i<n; i++)
  {
    v[i] = -(a[i-1]*v[i-1] * phi[i]  + b[i-2]*v[i-2] * phi[i]*phi[i-1] )/b[i-1];
  }

  u[n-1] = 1.0/(b[n-2]*v[n-2] * phi[n-1] + a[n-1]*v[n-1]);
  for(i=n-2; i>0; i--)
  {
    u[i] = (1.0 - b[i]*v[i]*u[i+1] * phi[i+1] )/(a[i]*v[i]+b[i-1]*v[i-1]*phi[i]);
  }
  u[0] = (1.0-b[0]*v[0]*u[1] * phi[1] )/(a[0]*v[0]);

  /* calculate W, D */
  D[0] = u[0]*v[0];
  W[0] = 1.0/D[0];
  for(i=1; i<n; i++)
  {
    /*S = phi[i]*phi[i]*(S + D[i-1]*W[i-1]*W[i-1]);
    A = u[i]*v[i];
    D[i] = A - u[i]*u[i] * S;
    W[i] = 1.0/D[i] * (v[i] - u[i]*S);*/

    D[i] = -u[i]/u[i-1]/b[i-1] * phi[i];
    W[i] = 1.0/u[i];
    //printf("%e %e %e %e %e %e\n", u[i], v[i], D[i], W[i], a[i], b[i]);
  }
  return;
}

/*
 * z = C^1/2 x y
 *
 * C is a semiseparable matrix with (u,v) representation, y is a vector
 *
 * C = LxDxL^T,   L=I + tril(UxW^T)
 * C^1/2 = LxD^1/2
 */
void multiply_matvec_semiseparable_uv(double *y, double *u, double  *W, double *D, double *phi,
                                      int n, double *z)
{
  int i;
  double f;

  f = 0.0;
  z[0] = sqrt(D[0]) * y[0];
  for(i=1; i<n; i++)
  {
    f = phi[i] * (f + W[i-1] * sqrt(D[i-1]) * y[i-1]);
    z[i] = sqrt(D[i]) * y[i] + u[i] * f;
  }
  return;
}

/*!
 * This function display matrix on the screen.
 */
void display_mat(double *a, int m, int n)
{
    int i, j;
    for(i=0;i<m;i++)
    {
        for(j=0;j<n;j++)
        {
            printf("%e\t", a[i*n+j]);
        }
        printf("\n");
    }
}

/*!
 * This function allocates memory for matrix.
 */
double ** matrix_malloc(int n1, int n2)
{
  double ** mat;
  int i;

  if(!(mat = malloc(n1*sizeof(double*))))
  {
    fprintf(stderr, "Unable to allocate the matrix!\n");
    exit(-1);
  }

  for(i=0; i<n1; i++)
  {
    if(!(mat[i] = malloc(n2*sizeof(double))))
    {
      fprintf(stderr, "Unable to allocate the matrix!\n");
      exit(-1);
    }
  }
  return mat;
}

/*!
 * This function allocates memory for array.
 */
double * array_malloc(int n)
{
  double *array;

  if(!(array = malloc(n*sizeof(double))))
  {
    fprintf(stderr, "Unable to allocate the matrix!\n");
    exit(-1);
  }

  return array;
}

/*!
 * This function is to test the functions defined in mathfun.c.
 */
void test_mathfun()
{
  double *A, det;
  int n = 3, info;
  A = malloc(n*n*sizeof(double));

  A[0*n+0] = 1.0;
  A[0*n+1] = 10.3;
  A[0*n+2] = 6.3;

  A[1*n+0] = -8.3;
  A[1*n+1] = -8.0;
  A[1*n+2] = 5.3;

  A[2*n+0] = 0.3;
  A[2*n+1] = -9.3;
  A[2*n+2] = -3.0;

  det = det_mat(A, n, &info);
  printf("%f\n", det);
}

/* used for qsort sorting, ascending order */
int compare(const void* a, const void* b)
{
	if (*(double *)a > *(double *)b)
		return 1;
	else if (*(double *)a < *(double *)b)
		return -1;
	return 0;
}


// Wrapper of the GSL interpolation functions that lets you define a
// 1D function by points and then query points on that function.
//
// Author: Mark Desnoyer (mdesnoyer@gmail.com)
// Date: Oct 2012

#ifndef __INTERPOLATION_H__
#define __INTERPOLATION_H__
#include <vector>
#include <gsl/gsl_spline.h>
#include <assert.h>
#include <algorithm>
#include <limits>
#include <exception>

template <typename T>
class SortOrder {
public:
  SortOrder(const std::vector<T>& sortArray) : sortArray_(sortArray) {}

  bool operator()(int lhs, int rhs) const {
    return sortArray_[lhs] < sortArray_[rhs];
  }

private:
  const std::vector<T>& sortArray_;
};

// Classes that inherit this abstract class must intialize spline_ in
// their constructors.
class Interpolator {
public:
  class out_of_bounds : public std::exception {};

  template <typename InputIterator>
  Interpolator(InputIterator xFirst, InputIterator xLast,
               InputIterator yFirst, InputIterator yLast)
    : x_(), y_(), acc_(gsl_interp_accel_alloc()) {
    std::vector<double> x(xFirst, xLast);
    std::vector<double> y(yFirst, yLast);
    std::vector<int> idx;
    for (unsigned int i = 0u; i < x.size(); ++i) {
      idx.push_back(i);
    }

    assert(x.size() == y.size());

    // Get the sorting indicies based on the x matrix
    std::sort(idx.begin(), idx.end(), SortOrder<double>(x));

    // Populate the data in ascending x order skipping equal values
    double curX = std::numeric_limits<double>::quiet_NaN();
    for (unsigned int i = 0u; i < idx.size(); ++i) {
      if (curX != x[idx[i]]) {
        x_.push_back(x[idx[i]]);
        y_.push_back(y[idx[i]]);
      }
      curX = x[idx[i]];
    }

  }

  virtual ~Interpolator();

  // Evaluate the interpolation at xi
  double operator()(double xi) const {
    if (xi < x_[0] || xi > x_.back()) {
      throw out_of_bounds();
    }
    return gsl_spline_eval(spline_, xi, acc_);
  }

  // Evaluate the derivative at xi
  double EvalDeriv(double xi) const {
    if (xi < x_[0] || xi > x_.back()) {
      throw out_of_bounds();
    }
    return gsl_spline_eval_deriv(spline_, xi, acc_);
  }

  double minY() const { return y_.front(); }
  double maxY() const { return y_.back(); }

protected:
  std::vector<double> x_;
  std::vector<double> y_;

  gsl_interp_accel* acc_;
  gsl_spline* spline_;

};

class LinearInterpolator : public Interpolator {
public:
  LinearInterpolator(const std::vector<double>& x, const std::vector<double>& y)
    : Interpolator(x.begin(), x.end(), y.begin(), y.end()) {
    InitSpline();
  }

  template <typename InputIterator>
  LinearInterpolator(InputIterator xFirst, InputIterator xLast,
                     InputIterator yFirst, InputIterator yLast)
    : Interpolator(xFirst, xLast, yFirst, yLast) {
    InitSpline();
  }

  virtual ~LinearInterpolator();

private:
  void InitSpline();

};

class SplineInterpolator : public Interpolator {
public:
  SplineInterpolator(const std::vector<double>& x, const std::vector<double>& y)
    : Interpolator(x.begin(), x.end(), y.begin(), y.end()) {
    InitSpline();
  }

  template <typename InputIterator>
  SplineInterpolator(InputIterator xFirst, InputIterator xLast,
                     InputIterator yFirst, InputIterator yLast)
    : Interpolator(xFirst, xLast, yFirst, yLast) {
    InitSpline();
  }

  virtual ~SplineInterpolator();

private:
  void InitSpline();

};

#endif // __INTERPOLATION_H__


#include <math.h>
#include <stdlib.h>
#include <assert.h>
#include <cconfigspace.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>

#define NUM_AREAS 4
#define NUM_SAMPLES 10000

void test_create_roulette_distribution() {
	ccs_distribution_t      distrib = NULL;
	ccs_result_t            err = CCS_SUCCESS;
	int32_t                 refcount;
	ccs_object_type_t       otype;
	ccs_distribution_type_t dtype;
	ccs_numeric_type_t      data_type;
	ccs_interval_t          interval;
	const size_t            num_areas = NUM_AREAS;
	ccs_float_t             areas[NUM_AREAS];
	size_t                  num_areas_ret;
	ccs_float_t             areas_ret[NUM_AREAS];
	const ccs_float_t       epsilon = 1e-15;

	for(size_t i = 0; i < num_areas; i++) {
		areas[i] = (double)(i+1);
	}

	err = ccs_create_roulette_distribution(
		num_areas,
		areas,
		&distrib);
	assert( err == CCS_SUCCESS );

	err = ccs_object_get_type(distrib, &otype);
	assert( err == CCS_SUCCESS );
	assert( otype == CCS_DISTRIBUTION );

	err = ccs_distribution_get_type(distrib, &dtype);
	assert( err == CCS_SUCCESS );
	assert( dtype == CCS_ROULETTE );

	err = ccs_distribution_get_data_types(distrib, &data_type);
	assert( err == CCS_SUCCESS );
	assert( data_type == CCS_NUM_INTEGER );

        err = ccs_distribution_get_bounds(distrib, &interval);
	assert( err == CCS_SUCCESS );
	assert( interval.type == CCS_NUM_INTEGER );
	assert( interval.lower.i == 0 );
	assert( interval.lower_included == CCS_TRUE );
	assert( interval.upper.i == 4 );
	assert( interval.upper_included == CCS_FALSE );

	err = ccs_roulette_distribution_get_num_areas(distrib, &num_areas_ret);
	assert( err == CCS_SUCCESS );
	assert( num_areas_ret == num_areas );

	err = ccs_roulette_distribution_get_areas(distrib, num_areas, areas_ret, &num_areas_ret);
	assert( err == CCS_SUCCESS );
	assert( num_areas_ret == num_areas );

	ccs_float_t inv_sum = 2.0 / (num_areas * (num_areas + 1));
	for (size_t i = 0; i < num_areas; i++) {
		assert( areas_ret[i] <= areas[i] * inv_sum + epsilon &&
		        areas_ret[i] >= areas[i] * inv_sum - epsilon );
	}

	err = ccs_object_get_refcount(distrib, &refcount);
	assert( err == CCS_SUCCESS );
	assert( refcount == 1 );

	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
}

void test_create_roulette_distribution_errors() {
	ccs_distribution_t      distrib = NULL;
	ccs_result_t            err = CCS_SUCCESS;
	const size_t            num_areas = NUM_AREAS;
	ccs_float_t             areas[NUM_AREAS];

	for(size_t i = 0; i < num_areas; i++) {
		areas[i] = (double)(i+1);
	}

	err = ccs_create_roulette_distribution(
		0,
		areas,
		&distrib);
	assert( err == -CCS_INVALID_VALUE );

	err = ccs_create_roulette_distribution(
		SIZE_MAX,
		areas,
		&distrib);
	assert( err == -CCS_INVALID_VALUE );

	err = ccs_create_roulette_distribution(
		num_areas,
		NULL,
		&distrib);
	assert( err == -CCS_INVALID_VALUE );

	err = ccs_create_roulette_distribution(
		num_areas,
		areas,
		NULL);
	assert( err == -CCS_INVALID_VALUE );

	areas[1] = -2;
	err = ccs_create_roulette_distribution(
		num_areas,
		areas,
		&distrib);
	assert( err == -CCS_INVALID_VALUE );

}

void test_roulette_distribution() {
	ccs_distribution_t distrib = NULL;
	ccs_rng_t          rng = NULL;
	ccs_result_t       err = CCS_SUCCESS;
	const size_t       num_samples = NUM_SAMPLES;
	ccs_numeric_t      samples[NUM_SAMPLES];
	const size_t       num_areas = NUM_AREAS;
	ccs_float_t        areas[NUM_AREAS];
	int                counts[NUM_AREAS];

	for(size_t i = 0; i < num_areas; i++) {
		areas[i] = (double)(i+1);
	        counts[i] = 0;
	}

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );
	err = ccs_create_roulette_distribution(
		num_areas,
		areas,
		&distrib);
	assert( err == CCS_SUCCESS );

	err = ccs_distribution_samples(distrib, rng, num_samples, samples);
	assert( err == CCS_SUCCESS );

	ccs_float_t sum = 0.0;
	ccs_float_t inv_sum = 0.0;
	for(size_t i = 0; i < num_areas; i++) {
		sum += areas[i];
	}
	inv_sum = 1.0 / sum;
	for(size_t i = 0; i < num_samples; i++) {
		assert( samples[i].i >=0 && samples[i].i < 4 );
		counts[samples[i].i]++;
	}
	for(size_t i = 0; i < num_areas; i++) {
		ccs_float_t target = num_samples * areas[i] * inv_sum;
		assert( counts[i] >= target * 0.95 && counts[i] <= target * 1.05 );
	}

	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

void test_roulette_distribution_zero() {
	ccs_distribution_t distrib = NULL;
	ccs_rng_t          rng = NULL;
	ccs_result_t       err = CCS_SUCCESS;
	const size_t       num_samples = NUM_SAMPLES;
	ccs_numeric_t      samples[NUM_SAMPLES];
	const size_t       num_areas = NUM_AREAS;
	ccs_float_t        areas[NUM_AREAS];
	int                counts[NUM_AREAS];

	for(size_t i = 0; i < num_areas; i++) {
		areas[i] = (double)(i+1);
	        counts[i] = 0;
	}
	areas[1] = 0.0;

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );
	err = ccs_create_roulette_distribution(
		num_areas,
		areas,
		&distrib);
	assert( err == CCS_SUCCESS );

	err = ccs_distribution_samples(distrib, rng, num_samples, samples);
	assert( err == CCS_SUCCESS );

	ccs_float_t sum = 0.0;
	ccs_float_t inv_sum = 0.0;
	for(size_t i = 0; i < num_areas; i++) {
		sum += areas[i];
	}
	inv_sum = 1.0 / sum;
	for(size_t i = 0; i < num_samples; i++) {
		assert( samples[i].i >=0 && samples[i].i < 4 );
		counts[samples[i].i]++;
	}
	for(size_t i = 0; i < num_areas; i++) {
		ccs_float_t target = num_samples * areas[i] * inv_sum;
		assert( counts[i] >= target * 0.95 && counts[i] <= target * 1.05 );
	}

	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

void test_roulette_distribution_strided_samples() {
	ccs_distribution_t distrib1 = NULL;
	ccs_distribution_t distrib2 = NULL;
	ccs_rng_t          rng = NULL;
	ccs_result_t       err = CCS_SUCCESS;
	const size_t       num_samples = NUM_SAMPLES;
	ccs_numeric_t      samples[NUM_SAMPLES*2];
	const size_t       num_areas = NUM_AREAS;
	ccs_float_t        areas1[NUM_AREAS];
	ccs_float_t        areas2[NUM_AREAS];
	int                counts1[NUM_AREAS];
	int                counts2[NUM_AREAS];

	for(size_t i = 0; i < num_areas; i++) {
		areas1[i] = (double)(i+1);
	        counts1[i] = 0;
		areas2[i] = (double)(i+2);
	        counts2[i] = 0;
	}

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );
	err = ccs_create_roulette_distribution(
		num_areas,
		areas1,
		&distrib1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_roulette_distribution(
		num_areas,
		areas2,
		&distrib2);
	assert( err == CCS_SUCCESS );

	err = ccs_distribution_strided_samples(distrib1, rng, num_samples, 2, samples);
	assert( err == CCS_SUCCESS );

	err = ccs_distribution_strided_samples(distrib2, rng, num_samples, 2, &(samples[0]) + 1);
	assert( err == CCS_SUCCESS );

	ccs_float_t sum = 0.0;
	ccs_float_t inv_sum = 0.0;
	for(size_t i = 0; i < num_areas; i++) {
		sum += areas1[i];
	}
	inv_sum = 1.0 / sum;
	for(size_t i = 0; i < num_samples; i++) {
		assert( samples[2*i].i >=0 && samples[2*i].i < 4 );
		counts1[samples[2*i].i]++;
	}
	for(size_t i = 0; i < num_areas; i++) {
		ccs_float_t target = num_samples * areas1[i] * inv_sum;
		assert( counts1[i] >= target * 0.95 && counts1[i] <= target * 1.05 );
	}

	sum = 0.0;
	inv_sum = 0.0;
	for(size_t i = 0; i < num_areas; i++) {
		sum += areas2[i];
	}
	inv_sum = 1.0 / sum;
	for(size_t i = 0; i < num_samples; i++) {
		assert( samples[2*i+1].i >=0 && samples[2*i+1].i < 4 );
		counts2[samples[2*i+1].i]++;
	}
	for(size_t i = 0; i < num_areas; i++) {
		ccs_float_t target = num_samples * areas2[i] * inv_sum;
		assert( counts2[i] >= target * 0.95 && counts2[i] <= target * 1.05 );
	}

	err = ccs_release_object(distrib1);
	assert( err == CCS_SUCCESS );
	err = ccs_release_object(distrib2);
	assert( err == CCS_SUCCESS );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

void test_roulette_distribution_soa_samples() {
	ccs_distribution_t distrib = NULL;
	ccs_rng_t          rng = NULL;
	ccs_result_t       err = CCS_SUCCESS;
	const size_t       num_samples = NUM_SAMPLES;
	ccs_numeric_t      samples[NUM_SAMPLES];
	const size_t       num_areas = NUM_AREAS;
	ccs_float_t        areas[NUM_AREAS];
	int                counts[NUM_AREAS];
	ccs_numeric_t     *p_samples;

	for(size_t i = 0; i < num_areas; i++) {
		areas[i] = (double)(i+1);
	        counts[i] = 0;
	}

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );
	err = ccs_create_roulette_distribution(
		num_areas,
		areas,
		&distrib);
	assert( err == CCS_SUCCESS );

	p_samples = &(samples[0]);
	err = ccs_distribution_soa_samples(distrib, rng, num_samples, &p_samples);
	assert( err == CCS_SUCCESS );

	ccs_float_t sum = 0.0;
	ccs_float_t inv_sum = 0.0;
	for(size_t i = 0; i < num_areas; i++) {
		sum += areas[i];
	}
	inv_sum = 1.0 / sum;
	for(size_t i = 0; i < num_samples; i++) {
		assert( samples[i].i >=0 && samples[i].i < 4 );
		counts[samples[i].i]++;
	}
	for(size_t i = 0; i < num_areas; i++) {
		ccs_float_t target = num_samples * areas[i] * inv_sum;
		assert( counts[i] >= target * 0.95 && counts[i] <= target * 1.05 );
	}

	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

int main() {
	ccs_init();
	test_create_roulette_distribution();
	test_create_roulette_distribution_errors();
	test_roulette_distribution();
	test_roulette_distribution_zero();
	test_roulette_distribution_strided_samples();
	test_roulette_distribution_soa_samples();
	ccs_fini();
	return 0;
}


/* specfunc/gsl_sf_coulomb.h
 *
 * 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.
 */

/* Author:  G. Jungman */

#ifndef __GSL_SF_COULOMB_H__
#define __GSL_SF_COULOMB_H__

#include <gsl/gsl_mode.h>
#include <gsl/gsl_sf_result.h>

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

__BEGIN_DECLS

/* Normalized hydrogenic bound states, radial dependence. */

/* R_1 := 2Z sqrt(Z) exp(-Z r)
 */
int gsl_sf_hydrogenicR_1_e(const double Z, const double r, gsl_sf_result * result);
double gsl_sf_hydrogenicR_1(const double Z, const double r);

/* R_n := norm exp(-Z r/n) (2Z/n)^l Laguerre[n-l-1, 2l+1, 2Z/n r]
 *
 * normalization such that psi(n,l,r) = R_n Y_{lm}
 */
int gsl_sf_hydrogenicR_e(const int n, const int l, const double Z, const double r, gsl_sf_result * result);
double gsl_sf_hydrogenicR(const int n, const int l, const double Z, const double r);

/* Coulomb wave functions F_{lam_F}(eta,x), G_{lam_G}(eta,x)
 * and their derivatives; lam_G := lam_F - k_lam_G
 *
 * lam_F, lam_G > -0.5
 * x > 0.0
 *
 * Conventions of Abramowitz+Stegun.
 *
 * Because there can be a large dynamic range of values,
 * overflows are handled gracefully. If an overflow occurs,
 * GSL_EOVRFLW is signalled and exponent(s) are returned
 * through exp_F, exp_G. These are such that
 *
 *   F_L(eta,x)  =  fc[k_L] * exp(exp_F)
 *   G_L(eta,x)  =  gc[k_L] * exp(exp_G)
 *   F_L'(eta,x) = fcp[k_L] * exp(exp_F)
 *   G_L'(eta,x) = gcp[k_L] * exp(exp_G)
 */
int
gsl_sf_coulomb_wave_FG_e(const double eta, const double x,
                            const double lam_F,
			    const int  k_lam_G,
                            gsl_sf_result * F, gsl_sf_result * Fp,
			    gsl_sf_result * G, gsl_sf_result * Gp,
			    double * exp_F, double * exp_G);

/* F_L(eta,x) as array */
int gsl_sf_coulomb_wave_F_array(
  double lam_min, int kmax,
  double eta, double x,
  double * fc_array,
  double * F_exponent
  );

/* F_L(eta,x), G_L(eta,x) as arrays */
int gsl_sf_coulomb_wave_FG_array(double lam_min, int kmax,
                                double eta, double x,
                                double * fc_array, double * gc_array,
				double * F_exponent,
				double * G_exponent
                                );

/* F_L(eta,x), G_L(eta,x), F'_L(eta,x), G'_L(eta,x) as arrays */
int gsl_sf_coulomb_wave_FGp_array(double lam_min, int kmax,
                                double eta, double x,
                                double * fc_array, double * fcp_array,
				double * gc_array, double * gcp_array,
				double * F_exponent,
				double * G_exponent
                                );

/* Coulomb wave function divided by the argument,
 * F(eta, x)/x. This is the function which reduces to
 * spherical Bessel functions in the limit eta->0.
 */
int gsl_sf_coulomb_wave_sphF_array(double lam_min, int kmax,
                                        double eta, double x,
	              	                double * fc_array,
			                double * F_exponent
	      	      	                );

/* Coulomb wave function normalization constant.
 * [Abramowitz+Stegun 14.1.8, 14.1.9]
 */
int gsl_sf_coulomb_CL_e(double L, double eta, gsl_sf_result * result);
int gsl_sf_coulomb_CL_array(double Lmin, int kmax, double eta, double * cl);

__END_DECLS

#endif /* __GSL_SF_COULOMB_H__ */


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "eigenmap.h"
#include <cblas.h>
#include <memory.h>

static void diag(double *d, const double *w, int n_patch);
static void compute_l(double *w, int n_patch);

static void diag_similarity_transform(double *w, int n_patch);

/*
 * laplacian computes the Laplacian matrix based on the weight matrix.
 *
 * w: the weight matrix
 * n_patch: the dimension of dev_w and dev_l
 * Note: the Laplacian matrix is computed in-place and overwrites w.
 */
void laplacian(double *w, int n_patch)
{
    /* ---- corresponding Matlab code ----
     * D = diag(sum(W));
     * L = eye(n_patch) - D^(-1/2)*W*D^(-1/2);
     */

    // W <- D^(-1/2) * W * D^(-1/2)
    diag_similarity_transform(w, n_patch);
    // L <- I - W
    compute_l(w, n_patch);
}

static void diag_similarity_transform(double *w, int n_patch)
{
    double *d = (double *)calloc(n_patch, sizeof(double));
    int i;

    diag(d, w, n_patch);

    // row operations
    for (i = 0; i < n_patch; i++)
        cblas_dscal(n_patch, d[i], &w[i], n_patch);
    // column operations
    for (i = 0; i < n_patch; i++)
        cblas_dscal(n_patch, d[i], &w[i * n_patch], 1);

    free(d);
}

static void diag(double *d, const double *w, int n_patch)
{
    int i, j;
    double sum;
    for (j = 0; j < n_patch; j++){
        sum = 0;
        for (i = 0; i < n_patch; i++)
            sum += w[i + j * n_patch];
        d[j] = 1 / sqrt(sum);
    }
}

static void compute_l(double *w, int n_patch)
{
    int N = n_patch * n_patch;
    int i;

    for (i = 0; i < N; i++)
        w[i] = ((i % (n_patch + 1) == 0) ? 1.0 : 0.0) - w[i];
}


#include <stdio.h>
#include <stdlib.h>
#include <cblas.h>
#include <sys/time.h>
#include "../ulmblas.h"
#define M 1024
#define K 128
#define N 1024

void
ULMBLAS(dgemm)(const enum Trans  transA,
               const enum Trans  transB,
               const int         m,
               const int         n,
               const int         k,
               const double      alpha,
               const double      *A,
               const int         ldA,
               const double      *B,
               const int         ldB,
               const double      beta,
               double            *C,
               const int         ldC);
int main(){
  // init
  double (*a)[K] = (double (*)[K])malloc(sizeof(double)*M*K);
  double (*b)[N] = (double (*)[N])malloc(sizeof(double)*N*K);
  double (*c)[N] = (double (*)[N])malloc(sizeof(double)*M*N);
  struct timeval stop, start;
  for(int i=0;i<M;i++){
    for(int j=0;j<K;j++){
      a[i][j] = 0.33*(float)i+0.29*(float)j;
    }
  }
  for(int i=0;i<K;i++){
    for(int j=0;j<N;j++){
      b[i][j] = 0.67*(float)i+0.98*(float)j;
    }
  }
  //C=A*B^T
  // openblas
  for(int i=0;i<M;i++){
    for(int j=0;j<N;j++){
      c[i][j] = 0;
    }
  }
  gettimeofday(&start, NULL);
  cblas_dgemm(CblasRowMajor,
              CblasNoTrans,
              CblasNoTrans,
              M,N,K,1,(double*)a,K,(double*)b,N,0,(double*)c,N);
  gettimeofday(&stop, NULL);
  printf("li took %lu get %lf\n", (stop.tv_sec - start.tv_sec) * 1000000 + stop.tv_usec - start.tv_usec, c[1][1]);
  // me
  /*
  for(int i=0;i<M;i++){
    for(int j=0;j<N;j++){
      c[i][j] = 0;
    }
  }
  gettimeofday(&start, NULL);
  for(int i=0;i<M;i++)
    for(int j=0;j<N;j++){
      double *cp = &c[i][j];
      double *ap = a[i];
      double *bp = b[j];
      for(int p=0;p<K;p++){
        *cp += *(ap++) * *(bp++);
      }
    }
  gettimeofday(&stop, NULL);
  printf("me took %lu get %lf\n", (stop.tv_sec - start.tv_sec) * 1000000 + stop.tv_usec - start.tv_usec, c[1][1]);
  */
  // ulm
  for(int i=0;i<M;i++){
    for(int j=0;j<N;j++){
      c[i][j] = 0;
    }
  }
  gettimeofday(&start, NULL);
  ULMBLAS(dgemm)(Trans,
         Trans,
         M,N,K,1,(double*)a,K,(double*)b,N,0,(double*)c,N);
  gettimeofday(&stop, NULL);
  printf("ul took %lu get %lf\n", (stop.tv_sec - start.tv_sec) * 1000000 + stop.tv_usec - start.tv_usec, c[1][1]);
  return 0;
}


#include <gsl/gsl_spline.h>
#include "eos_pres.h"
#include "global.h"

// "forward" EOS. input: rho, T. output: P
double
eos_pres (double rho, void *params)
{
  struct param *eos_pres_params = (struct param *) params;
  return (gsl_spline_eval (spline, rho, acc));
  // return (pow(rho, 1.0/params.Gamma)); // polytrope
}


/* coupled_oscillators.c Kuramoto model of coupled phase oscillators */
/*
 * Dynamique Populations Cellulaires
 * Université Lyon 1, 2014
 *
 * Fichiers:
 *  coupled_oscillators.c
 *  makefile
 *
 * Ce code utilise la librairie gsl (GNU Scientific Library)
 *
 * Pour compiler sur mac ou linux:
 *
 * ouvrir un terminal
 * >> make
 * >> ./coupled_oscillators
 *
 * Deux fichiers sont générés en sortie:
 *  osc.txt : series temporelles
 *  order.txt: paramètre d'ordre
 *
 * Le fichier osc.txt contient est structuré en colonnes::
 * 0   w1  w2 ...      wNOSC
 * t1  1   2   3   ...  NOSC
 *
 * La premiere ligne est composee des frequences des oscillateurs. Les
 * oscillateurs sont places en ordre croissant de frequence.
 * t est le temps, r le module du paramètre d'ordre, psi l'argument du paramètre
 * d'ordre, 1...NOSC les phases des oscillateur.
 * Les phases des oscillateurs sont dans R (pas dans [0, 2*pi]).
 *
 * Pour visualiser les résultats, on peut utiliser gnuplot ou Matlab
 *
 * GNUPLOT:
 * plot 'osc.txt' u 1:2:(sin($3)) nonuniform matrix with image
 *
 * Pour tracer le paramètre d'ordre
 *
 * pour r:
 * plot 'order.txt' using 1:2 w lines
 *
 * pour psi:
 * plot 'order.txt' using 1:3 w lines
 *
 * Pour Matlab, utiliser le script coupled_oscillators.m
 *
 */

/* ================================================================= */
/*                              Libraries                            */
/* ================================================================= */
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_odeiv2.h>

struct par {
    uint32_t NOSC;
    double K;
    double *w;
};

int ode_rhs(double t, const double y[], double f[], void *params);
int compare(const void *, const void *);

int main ( int argc, char *argv[] )
{
    uint32_t NOSC=50; /* nbr oscillators */
    double K=.5;  /* coupling stength */
    double sigma=0.5; /* std w distribution */
    double *y;     /* phase of oscillators */
    struct par mu; /* parameters */
    FILE *file_order,*file_out;
    uint32_t i;
    double t,tfinal;
    double h,hmin;
    double w_mean;
    double r,psi, rx, ry;

	/* get command line input parameters */
	if (argc == 4)
    {
        NOSC = strtoul(argv[1],NULL,10);
    	K = strtod(argv[2],NULL);
        tfinal = strtod(argv[3],NULL);
    }
	else
    {
    	fprintf(stderr, "Error: call to coupled_oscillators must have the following three mandatory arguments: unsigned long NOSC, double K, and double tfinal\n");
    	exit(EXIT_FAILURE);
    }

	/* init random number generator */
	const gsl_rng_type * rngT;
	gsl_rng * rand_nbr_gen;
	gsl_rng_env_setup();
	rngT = gsl_rng_default;
	rand_nbr_gen = gsl_rng_alloc(rngT);

    /* gsl ode */
    const gsl_odeiv2_step_type * odeT;
    gsl_odeiv2_step * s;
    gsl_odeiv2_control * c;
    gsl_odeiv2_evolve * e;
    gsl_odeiv2_system sys;
    int status;

    mu.NOSC = NOSC;
    mu.K = K;
    mu.w = malloc(NOSC*sizeof(double));
    y = malloc(NOSC*sizeof(double));
    w_mean = 0.0;
    for (i = 0; i < NOSC; i++)
    {
        y[i] = 2*M_PI*gsl_rng_uniform(rand_nbr_gen)-M_PI;
        mu.w[i] = gsl_ran_gaussian(rand_nbr_gen,sigma*sigma);
        w_mean += mu.w[i];
    }
    w_mean /= NOSC;

    /* sort the frequencies from slowest to fastest */
    qsort(mu.w, NOSC, sizeof(double), compare);

    /* remove the mean to mu.w */
    file_out = fopen("osc.txt","w");
    fprintf(file_out,"%.5e ",0.0);
    for ( i = 0; i < NOSC; i++)
    {
        mu.w[i] -= w_mean;
        fprintf(file_out,"%.5e ",mu.w[i]);
    }
    fprintf(file_out,"\n");
    /* DO NOT TOUCH mu.w BELOW */

    /* initialise the ode solver */
    /* the solver used is a Runge-Kutta-Fehlberg (4, 5) method */
    odeT = gsl_odeiv2_step_rkf45;
    s = gsl_odeiv2_step_alloc(odeT,NOSC);
    c = gsl_odeiv2_control_y_new(1e-6,0.0);
    e = gsl_odeiv2_evolve_alloc(NOSC);

    h = 1e-2;
    hmin = 1e-5;
    t = 0.0;

    file_order = fopen("order.txt","w");
    while (t < tfinal )
    {

        /* apply one step of the ode solver */
        sys = (gsl_odeiv2_system) {ode_rhs, NULL, NOSC, &mu};
        status = gsl_odeiv2_evolve_apply(e,c,s,&sys,&t,tfinal,&h,y);
        if (status != GSL_SUCCESS)
            break;

        rx = 0;
        ry = 0;
        for ( i = 0; i < NOSC; i++)
        {
            rx += cos(y[i]);
            ry += sin(y[i]);

        }
        rx /= NOSC;
        ry /= NOSC;
        r = hypot(rx,ry);
        psi = atan2(ry,rx);

        if (h < hmin) h = hmin;
        printf(".\n");

        fprintf(file_order,"%.5e %.5e %.5e\n",t,r,psi);
        fprintf(file_out,"%.5e ",t);

        for (i = 0; i < NOSC; i++)
        {
            fprintf (file_out,"%.5e ",y[i]);
        }
        fprintf(file_out,"\n");

    }

    gsl_odeiv2_evolve_free(e);
    gsl_odeiv2_control_free(c);
    gsl_odeiv2_step_free(s);

    fclose(file_out);
    fclose(file_order);

    free(y);
    free(mu.w);

	return 0;
}

int ode_rhs(double t, const double y[], double f[], void *params)
{

    struct par mu = *(struct par*)params;
    uint32_t NOSC = mu.NOSC;
    double K  = mu.K;
    double *w = mu.w;
    uint32_t i,j;
    double *c;

    /* printf("call to ode_rhs, NOSC=%d\n",NOSC); */
    c = malloc(NOSC*sizeof(double));
    /* compute coupling */
    for ( i = 0; i < NOSC; i++)
    {
        c[i] = 0.0;
        for ( j = 0; j < NOSC; j++)
        {
            c[i] += sin(y[j]-y[i]);
        }

        f[i] = w[i] + K/NOSC*c[i];
        /* printf("f[%d]=%f\n",i,f[i]); */

    }

    free(c);

    return GSL_SUCCESS;

}

int compare(const void *a, const void *b)
{
    const double *da = (const double *) a;
    const double *db = (const double *) b;

    return (*da > *db) - (*da < *db);
}