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

#include <config.h>
#include <stdlib.h>
#include <math.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_spmatrix.h>
#include <gsl/gsl_spblas.h>

/*
create_random_sparse()
  Create a random sparse matrix with approximately
M*N*density non-zero entries

Inputs: M       - number of rows
        N       - number of columns
        density - sparse density \in [0,1]
                  0 = no non-zero entries
                  1 = all m*n entries are filled
        r       - random number generator

Return: pointer to sparse matrix in triplet format (must be freed by caller)

Notes:
1) non-zero matrix entries are uniformly distributed in [0,1]
*/

static gsl_spmatrix *
create_random_sparse(const size_t M, const size_t N, const double density,
                     const gsl_rng *r)
{
  size_t nnzwanted = (size_t) floor(M * N * GSL_MIN(density, 1.0));
  gsl_spmatrix *m = gsl_spmatrix_alloc_nzmax(M, N,
                                             nnzwanted,
                                             GSL_SPMATRIX_TRIPLET);

  while (gsl_spmatrix_nnz(m) < nnzwanted)
    {
      /* generate a random row and column */
      size_t i = gsl_rng_uniform(r) * M;
      size_t j = gsl_rng_uniform(r) * N;

      /* generate random m_{ij} and add it */
      double x = gsl_rng_uniform(r);
      gsl_spmatrix_set(m, i, j, x);
    }

  return m;
} /* create_random_sparse() */

static void
create_random_vector(gsl_vector *v, const gsl_rng *r)
{
  size_t i;

  for (i = 0; i < v->size; ++i)
    {
      double x = gsl_rng_uniform(r);
      gsl_vector_set(v, i, x);
    }
} /* create_random_vector() */

static int
test_vectors(gsl_vector *observed, gsl_vector *expected, const double tol,
             const char *str)
{
  int s = 0;
  size_t N = observed->size;
  size_t i;

  for (i = 0; i < N; ++i)
    {
      double x_obs = gsl_vector_get(observed, i);
      double x_exp = gsl_vector_get(expected, i);

      gsl_test_rel(x_obs, x_exp, tol, "N=%zu i=%zu %s", N, i, str);
    }

  return s;
} /* test_vectors() */

static void
test_dgemv(const size_t N, const size_t M, const double alpha,
           const double beta, const CBLAS_TRANSPOSE_t TransA,
           const gsl_rng *r)
{
  gsl_spmatrix *A = create_random_sparse(M, N, 0.2, r);
  gsl_spmatrix *B, *C;
  gsl_matrix *A_dense = gsl_matrix_alloc(M, N);
  gsl_vector *x, *y, *y_gsl, *y_sp;
  size_t lenX, lenY;

  if (TransA == CblasNoTrans)
    {
      lenX = N;
      lenY = M;
    }
  else
    {
      lenX = M;
      lenY = N;
    }

  x = gsl_vector_alloc(lenX);
  y = gsl_vector_alloc(lenY);
  y_gsl = gsl_vector_alloc(lenY);
  y_sp = gsl_vector_alloc(lenY);

  /* create random dense vectors */
  create_random_vector(x, r);
  create_random_vector(y, r);

  /* copy A into A_dense */
  gsl_spmatrix_sp2d(A_dense, A);

  gsl_vector_memcpy(y_gsl, y);
  gsl_vector_memcpy(y_sp, y);

  /* compute y = alpha*op(A)*x + beta*y0 with gsl */
  gsl_blas_dgemv(TransA, alpha, A_dense, x, beta, y_gsl);

  /* compute y = alpha*op(A)*x + beta*y0 with spblas/triplet */
  gsl_spblas_dgemv(TransA, alpha, A, x, beta, y_sp);

  /* test y_sp = y_gsl */
  test_vectors(y_sp, y_gsl, 1.0e-10, "test_dgemv: triplet format");

  /* compute y = alpha*op(A)*x + beta*y0 with spblas/CCS */
  B = gsl_spmatrix_ccs(A);
  gsl_vector_memcpy(y_sp, y);
  gsl_spblas_dgemv(TransA, alpha, B, x, beta, y_sp);

  /* test y_sp = y_gsl */
  test_vectors(y_sp, y_gsl, 1.0e-10, "test_dgemv: CCS format");

  /* compute y = alpha*op(A)*x + beta*y0 with spblas/CRS */
  C = gsl_spmatrix_crs(A);
  gsl_vector_memcpy(y_sp, y);
  gsl_spblas_dgemv(TransA, alpha, C, x, beta, y_sp);

  /* test y_sp = y_gsl */
  test_vectors(y_sp, y_gsl, 1.0e-10, "test_dgemv: CRS format");

  gsl_spmatrix_free(A);
  gsl_spmatrix_free(B);
  gsl_spmatrix_free(C);
  gsl_matrix_free(A_dense);
  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_vector_free(y_gsl);
  gsl_vector_free(y_sp);
} /* test_dgemv() */

static void
test_dgemm(const double alpha, const size_t M, const size_t N,
           const gsl_rng *r)
{
  const size_t max = GSL_MAX(M, N);
  size_t i, j, k;
  gsl_matrix *A_dense = gsl_matrix_alloc(M, max);
  gsl_matrix *B_dense = gsl_matrix_alloc(max, N);
  gsl_matrix *C_dense = gsl_matrix_alloc(M, N);
  gsl_spmatrix *C = gsl_spmatrix_alloc_nzmax(M, N, 1, GSL_SPMATRIX_CCS);

  for (k = 1; k <= max; ++k)
    {
      gsl_matrix_view Ad = gsl_matrix_submatrix(A_dense, 0, 0, M, k);
      gsl_matrix_view Bd = gsl_matrix_submatrix(B_dense, 0, 0, k, N);
      gsl_spmatrix *TA = create_random_sparse(M, k, 0.2, r);
      gsl_spmatrix *TB = create_random_sparse(k, N, 0.2, r);
      gsl_spmatrix *A = gsl_spmatrix_ccs(TA);
      gsl_spmatrix *B = gsl_spmatrix_ccs(TB);

      gsl_spmatrix_set_zero(C);
      gsl_spblas_dgemm(alpha, A, B, C);

      /* make dense matrices and use standard dgemm to multiply them */
      gsl_spmatrix_sp2d(&Ad.matrix, TA);
      gsl_spmatrix_sp2d(&Bd.matrix, TB);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, alpha, &Ad.matrix,
                     &Bd.matrix, 0.0, C_dense);

      /* compare C and C_dense */
      for (i = 0; i < M; ++i)
        {
          for (j = 0; j < N; ++j)
            {
              double Cij = gsl_spmatrix_get(C, i, j);
              double Dij = gsl_matrix_get(C_dense, i, j);

              gsl_test_rel(Cij, Dij, 1.0e-12, "test_dgemm: _dgemm");
            }
        }

      gsl_spmatrix_free(TA);
      gsl_spmatrix_free(TB);
      gsl_spmatrix_free(A);
      gsl_spmatrix_free(B);
    }

  gsl_spmatrix_free(C);
  gsl_matrix_free(A_dense);
  gsl_matrix_free(B_dense);
  gsl_matrix_free(C_dense);
} /* test_dgemm() */

int
main()
{
  const size_t N_max = 40;
  size_t m, n;
  gsl_rng *r = gsl_rng_alloc(gsl_rng_default);

  for (m = 1; m <= N_max; ++m)
    {
      for (n = 1; n <= N_max; ++n)
        {
          test_dgemv(m, n, 1.0, 0.0, CblasNoTrans, r);
          test_dgemv(m, n, 1.0, 0.0, CblasTrans, r);

          test_dgemv(m, n, 2.4, -0.5, CblasNoTrans, r);
          test_dgemv(m, n, 2.4, -0.5, CblasTrans, r);

          test_dgemv(m, n, 0.1, 10.0, CblasNoTrans, r);
          test_dgemv(m, n, 0.1, 10.0, CblasTrans, r);
        }
    }

  test_dgemm(1.0, 10, 10, r);
  test_dgemm(2.3, 20, 15, r);
  test_dgemm(1.8, 12, 30, r);
  test_dgemm(0.4, 45, 35, r);

  gsl_rng_free(r);

  exit (gsl_test_summary());
} /* main() */


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

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_legendre.h>

/*
 * The routines in this module compute associated Legendre functions
 * (ALFs) up to order and degree 2700, using the method described
 * in
 *
 * [1] S. A. Holmes and W. E. Featherstone, A unified approach
 *     to the Clenshaw summation and the recursive computation of very
 *     high degree and order normalised associated Legendre functions,
 *     Journal of Geodesy, 76, pg. 279-299, 2002.
 *
 * Further information on ALFs can be found in
 *
 * [2] Abramowitz and Stegun, Handbook of Mathematical Functions,
 *     Chapter 8, 1972.
 */

static void legendre_sqrts(const size_t lmax, double *array);

#define LEGENDRE
#include "legendre_source.c"
#undef LEGENDRE

#define LEGENDRE_DERIV
#include "legendre_source.c"
#undef LEGENDRE_DERIV

#define LEGENDRE_DERIV_ALT
#include "legendre_source.c"
#undef LEGENDRE_DERIV_ALT

#define LEGENDRE_DERIV2
#include "legendre_source.c"
#undef LEGENDRE_DERIV2

#define LEGENDRE_DERIV2_ALT
#include "legendre_source.c"
#undef LEGENDRE_DERIV2_ALT

/* number of P_{lm} functions for a given lmax */
size_t
gsl_sf_legendre_nlm(const size_t lmax)
{
  return ((lmax + 1) * (lmax + 2) / 2);
}

/*
gsl_sf_legendre_array_n()
  This routine returns the minimum result_array[] size needed
for a given lmax
*/

size_t
gsl_sf_legendre_array_n(const size_t lmax)
{
  size_t nlm = gsl_sf_legendre_nlm(lmax);
  size_t nsqrt = 2 * lmax + 2; /* extra room to precompute sqrt factors */

  return (nlm + nsqrt);
} /* gsl_sf_legendre_array_n() */

/*
gsl_sf_legendre_array_index()
This routine computes the index into a result_array[] corresponding
to a given (l,m)
*/

size_t
gsl_sf_legendre_array_index(const size_t l, const size_t m)
{
  return (l * (l + 1) / 2 + m);
} /* gsl_sf_legendre_array_index() */

/*********************************************************
 *                 INTERNAL ROUTINES                     *
 *********************************************************/

/*
legendre_sqrts()
  Precompute square root factors needed for Legendre recurrence.
On output, array[i] = sqrt(i)
*/

static void
legendre_sqrts(const size_t lmax, double *array)
{
  size_t l;
  for (l = 0; l <= 2 * lmax + 1; ++l)
    array[l] = sqrt((double) l);
}


/**
 * (c) Author:    Woongkyu Jee, woong.jee.16@ucl.ac.uk, wldndrb1@gmail.com
 * Created:   02.06.2019 ~
 *
 * University College London, Department of Chemistry
 **/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>

#include"sp_cluster_type.h"

#define SP_SUPPORT_TRUE 1
#define SP_SUPPORT_FALSE -1

#define DEBUG_SUPPORT

/* Get the Lowest energy state */
int sp_cluster_support_get_lowest_state( gsl_vector* v )
{
    int Return = 0;
    double min;

    min = gsl_vector_get(v,0);
    for(int i=0;i<4;i++)
    {
        if( gsl_vector_get(v,i) < min  )
        {
            min = gsl_vector_get(v,i);
            Return = i;
        }
    }
    return Return;
    // Returns the index of element in gsl_vector* eval
}

void sp_cluster_support_sign_gs_eigenvector( void* sp_sys_void )
{
    sp_cluster_system* sp_sys = (sp_cluster_system*)sp_sys_void;
    int low_idx;

    for(int i=0;i<sp_sys->number_of_sp_ion;i++)
    {
        low_idx = sp_cluster_support_get_lowest_state(sp_sys->sp_ion[i].eigen_value);
        if( gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,0,low_idx) < 0. )          // eigen_vector : matrix(4,4) // eigen_vector_gs : vector(4)
        {
            gsl_matrix_set(sp_sys->sp_ion[i].eigen_vector,0,low_idx,-1.*gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,0,low_idx));
            gsl_matrix_set(sp_sys->sp_ion[i].eigen_vector,1,low_idx,-1.*gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,1,low_idx));
            gsl_matrix_set(sp_sys->sp_ion[i].eigen_vector,2,low_idx,-1.*gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,2,low_idx));
            gsl_matrix_set(sp_sys->sp_ion[i].eigen_vector,3,low_idx,-1.*gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,3,low_idx));
        }
    }
    return;
}

void sp_cluster_support_load_gs_eigenvector( void* sp_sys_void )
{
    sp_cluster_system* sp_sys = (sp_cluster_system*)sp_sys_void;
    int low_idx;
    for(int i=0;i<sp_sys->number_of_sp_ion;i++)
    {   low_idx = sp_cluster_support_get_lowest_state(sp_sys->sp_ion[i].eigen_value);

        gsl_vector_set(sp_sys->sp_ion[i].eigen_vector_gs,0,gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,0,low_idx));
        gsl_vector_set(sp_sys->sp_ion[i].eigen_vector_gs,1,gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,1,low_idx));
        gsl_vector_set(sp_sys->sp_ion[i].eigen_vector_gs,2,gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,2,low_idx));
        gsl_vector_set(sp_sys->sp_ion[i].eigen_vector_gs,3,gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,3,low_idx));
    }
    return;
}

/* Matrix Viewer: Print out a N x N matrix on console */
void sp_cluster_support_matrix_view( const gsl_matrix* m )
{
    if( m != NULL )
    {   for(int i=0;i<m->size1;i++)
        {   for(int j=0;j<m->size2;j++)
            {   //printf("%s%12.4lf",gsl_matrix_get(m,i,j)>0.?"+":"",gsl_matrix_get(m,i,j));
                printf("%12.4e",gsl_matrix_get(m,i,j));
            }
        puts("");
        }
    }
    else
        puts("sp_cluster_support_matrix_view input 'm' (gsl_matrix*) is a null pointer ... in SP_Support.c or SP_Support.h");

    return;
}

/* Vector Viewer: Print out a vector on console */
void sp_cluster_support_vector_view( const gsl_vector* v )
{
    if( v != NULL )
    {   for(int i=0;i<v->size;i++)
        {       //printf("%s%12.4f",gsl_vector_get(v,i)>0.?"+":"",gsl_vector_get(v,i));
                printf("%12.4e",gsl_vector_get(v,i));
        }
        puts("");
    }
    else
        puts("sp_cluster_support_vector_view input 'v' (gsl_vector*) is a null pointer ... in SP_Support.c or SP_Support.h");

    return;
}

/* Matrix Viewer: Print out a N x N matrix on file */
void sp_cluster_support_matrix_view_f( FILE* fp, const gsl_matrix* m )
{
    if( m != NULL )
    {   for(int i=0;i<m->size1;i++)
        {   for(int j=0;j<m->size2;j++)
                fprintf(fp,"%s%.18lf\t",gsl_matrix_get(m,i,j)>0.?"+":"",gsl_matrix_get(m,i,j));
        fprintf(fp,"\n");
        }
    }
    else
        fputs("sp_cluster_support_matrix_view input 'm' (gsl_matrix*) is a null pointer ... in SP_Support.c or SP_Support.h",fp);

    fflush(fp);

    return;
}

/* Vector Viewer: Print out a vector on file */
void sp_cluster_support_vector_view_f( FILE* fp, const gsl_vector* v )
{
    if( v != NULL )
    {   for(int i=0;i<v->size;i++)
            fprintf(fp,"%s%.18f\t",gsl_vector_get(v,i)>0.?"+":"",gsl_vector_get(v,i));
        fprintf(fp,"\n");
    }
    else
        fputs("sp_cluster_support_vector_view input 'v' (gsl_vector*) is a null pointer ... in SP_Support.c or SP_Support.h",fp);

    fflush(fp);

    return;
}

/* Transformation Matrix calculator */
/*
 * this function takes a vector, and the vector is transformed along transformed z-axis
 * the final return is the transformation matrix (rank 2 tensor), and the this data type is
 * gsl_matrix*
 */

gsl_matrix* sp_cluster_support_get_transformation_matrix( const gsl_vector* v )
{
    // gsl_vector_get(v,0) == 'x'
    // gsl_vector_get(v,1) == 'y'
    // gsl_vector_get(v,2) == 'z'
    gsl_matrix* pReturn = NULL;

    const double rxy = sqrt(pow(gsl_vector_get(v,0),2.)+pow(gsl_vector_get(v,1),2.));
    const double R   = sqrt(pow(gsl_vector_get(v,0),2.)+pow(gsl_vector_get(v,1),2.)+pow(gsl_vector_get(v,2),2.));
    double n1, n2, tmp; // dummy variables for workspace
    pReturn = gsl_matrix_calloc(4,4);   // Normally a rotation matrix is 3x3
                                        // For the 1st column or row has an element of '1' at T_11.

    if( pReturn != NULL )
    {
        gsl_matrix_set(pReturn,0,0,1.);

        if( gsl_vector_get(v,0) == 0. && gsl_vector_get(v,1) == 0. && gsl_vector_get(v,2) > 0. ) // if vector 'v' is on z-axis
        {   gsl_matrix_set(pReturn,1,1,1.); gsl_matrix_set(pReturn,2,2,1.); gsl_matrix_set(pReturn,3,3,1.);
            // set the matrix as I
        }
        else if( gsl_vector_get(v,0) == 0. && gsl_vector_get(v,1) == 0. && gsl_vector_get(v,2) < 0. )   // if vector 'v' is on negative z-axis
        {   gsl_matrix_set(pReturn,1,1,1.); gsl_matrix_set(pReturn,2,2,1.); gsl_matrix_set(pReturn,3,3,-1.);
            // set the matrix has xy-plane reflection
        }
        else
        {   gsl_matrix_set(pReturn,3,1,gsl_vector_get(v,0)/R);
            gsl_matrix_set(pReturn,3,2,gsl_vector_get(v,1)/R);
            gsl_matrix_set(pReturn,3,3,gsl_vector_get(v,2)/R);  // set k' in the transformed (local) symmetry

            gsl_matrix_set(pReturn,2,1,gsl_vector_get(v,2)*gsl_vector_get(v,0)/rxy);
            gsl_matrix_set(pReturn,2,2,gsl_vector_get(v,2)*gsl_vector_get(v,1)/rxy);
            gsl_matrix_set(pReturn,2,3,-R*sqrt(1.-gsl_vector_get(v,2)*gsl_vector_get(v,2)/R/R));

            n1 = 1./sqrt(pow(gsl_matrix_get(pReturn,2,1),2.)
               + pow(gsl_matrix_get(pReturn,2,2),2.)
               + pow(gsl_matrix_get(pReturn,2,3),2.));

            for(int i=0;i<3;i++)
            {   tmp = gsl_matrix_get(pReturn,2,i+1);
                tmp = tmp*n1;
                gsl_matrix_set(pReturn,2,i+1,tmp);  // set j' in the transformed (local) symmetry
            }

            gsl_matrix_set(pReturn,1,1,n1/R*(pow(gsl_vector_get(v,2),2.)*gsl_vector_get(v,1)/rxy
                        + R*gsl_vector_get(v,1)*sqrt(1.-pow(gsl_vector_get(v,2),2.)/R/R)));
            gsl_matrix_set(pReturn,1,2,n1/R*(-pow(gsl_vector_get(v,2),2.)*gsl_vector_get(v,0)/rxy
                        - R*gsl_vector_get(v,0)*sqrt(1.-pow(gsl_vector_get(v,2),2.)/R/R)));
            gsl_matrix_set(pReturn,1,3,0.);

            n2 = 1./sqrt(pow(gsl_matrix_get(pReturn,1,1),2.)
                    + pow(gsl_matrix_get(pReturn,1,2),2.)
                    + pow(gsl_matrix_get(pReturn,1,3),2.));

            for(int i=0;i<3;i++)
            {   tmp = gsl_matrix_get(pReturn,1,i+1);
                tmp = tmp/n2;
                gsl_matrix_set(pReturn,1,i+1,tmp);  // set i' in the transformed (local) symmetry
            }
        }
    }
    else
        puts("sp_cluster_get_trans_mat 'pReturn' alloc error ... in SP_support.c or SP_support.h");

    return pReturn;
}   /* SEE THE DETAILS IN THE 'N1' RING BINDER */

double sp_cluster_support_kronecker_delta( int a, int b )
{
    double Return = 0.;

    if( a == b )
        Return = 1.;

    return Return;
}

// norm of vector
double sp_cluster_support_get_norm( double v1, double v2, double v3 )
{   return pow(v1*v1+v2*v2+v3*v3,0.5);
}

// SPLINER

double** sp_cluster_support_get_spline( const double** data, const int knot_stride )
{
    double** pReturn = (double**)malloc((knot_stride-1)*sizeof(double*));
    for(int i=0;i<knot_stride-1;i++)
        pReturn[i] = (double*)calloc(4,sizeof(double));

    // Work Space
    const int n = knot_stride;
    double f[n];  double t[n];
    double a[n];  double b[n];  double c[n];  double d[n];
    double z[n];  double L[n];  double h[n];
    double alpha[n];  double mu[n];

    // Measuring slopes
    const double rslope = 0.;
    const double lslope = (data[3][1]-data[0][1])/(data[3][0]-data[0][0]);

    // Memset
    memset(f,0,n*sizeof(double));   memset(t,0,n*sizeof(double));
    memset(a,0,n*sizeof(double));   memset(b,0,n*sizeof(double));   memset(c,0,n*sizeof(double));   memset(d,0,n*sizeof(double));
    memset(z,0,n*sizeof(double));   memset(L,0,n*sizeof(double));   memset(h,0,n*sizeof(double));
    memset(alpha,0,n*sizeof(double));   memset(mu,0,n*sizeof(double));

    // Make Spline

    for(int i=0;i<n;i++)
    {   f[i] = data[i][1];
        t[i] = data[i][0];      }

    h[0] = t[1] - t[0];
    alpha[0] = 3.*(f[1]-f[0])/h[0]-3.*lslope;
    L[0] = 2.*h[0];
    mu[0] = 0.5;
    z[0] = alpha[0]/L[0];
    b[0] = lslope;
    // End Left Init

    for(int k=1;k<n-1;k++)
    {
        h[k] = t[k+1] - t[k];

        alpha[k] = (3./(h[k]*h[k-1]))*(f[k+1]*h[k-1]-f[k]*(h[k]+h[k-1])+f[k-1]*h[k]);

        L[k] = 2.*(h[k]+h[k-1])-h[k-1]*mu[k-1];

        mu[k] = h[k]/L[k];

        z[k] = (alpha[k]-h[k-1]*z[k-1])/L[k];
    }

    // Right End init
    alpha[n-1] = 3.*rslope - 3.*(f[n-1]-f[n-2])/h[n-2];
    L[n-1] = h[n-2]*(2.-mu[n-2]);
    z[n-1] = (alpha[n-1]-h[n-2]*z[n-2])/L[n-1];
    c[n-1] = z[n-1];

    for(int j=n-2;j>=0;j--)
    {
        c[j] = z[j]-mu[j]*c[j+1];

        b[j] = (f[j+1]-f[j])/h[j] - h[j]*(c[j+1]+2.*c[j])/3.;

        d[j] = (c[j+1]-c[j])/(3.*h[j]);

        a[j] = f[j];
    }
    // R-E END

    // SAVE DATA
    for(int i=0;i<knot_stride-1;i++)
    {   pReturn[i][0] = d[i];
        pReturn[i][1] = c[i] - 3.*t[i]*d[i];
        pReturn[i][2] = b[i] - 2.*c[i]*t[i] + 3.*t[i]*t[i]*d[i];
        pReturn[i][3] = a[i] - b[i]*t[i] + c[i]*t[i]*t[i] - d[i]*t[i]*t[i]*t[i];
    }

    return pReturn;
}

//int sp_cluster_support_get_atom_number(

void sp_cluster_support_print_xyz( void* sp_sys_void, const double cur_energy, const int rank, const int numtasks )
{
	int offset;
	// cast the type into "sp_cluster_system*"
	sp_cluster_system* sp_sys = (sp_cluster_system*)sp_sys_void;

	int atom_number=0;
	for(int i=0;i<sp_sys->number_of_classic_ion;i++)
	{	if( sp_sys->classic_ion[i].if_shell == SP_SUPPORT_FALSE )
			atom_number++;
		// CNT ONLY WHEN ITS CORE
	}

	if( cur_energy != 0. )
	{
		//printf("\t%d\n",sp_sys->number_of_classic_ion+sp_sys->number_of_sp_ion);
		printf("\t%d\n",atom_number+sp_sys->number_of_sp_ion);
		printf(" SCF DONE %.6lf\n",cur_energy);
	}

	// CORE POSITION ONLY ... "xyz" Format Compatible
	for(int n=0;n<sp_sys->number_of_sp_ion+sp_sys->number_of_classic_ion;n++)
	{
    	    offset = n-sp_sys->number_of_classic_ion;
	    if( n < sp_sys->number_of_classic_ion )
	    {
		if( sp_sys->classic_ion[n].if_shell == SP_SUPPORT_FALSE ) // if it is core
		{	fprintf(stdout,"%3s%12.6lf%12.6lf%12.6lf\n", sp_sys->classic_ion[n].atom_name,
			gsl_vector_get(sp_sys->classic_ion[n].core_position,0), gsl_vector_get(sp_sys->classic_ion[n].core_position,1), gsl_vector_get(sp_sys->classic_ion[n].core_position,2));
		}
	    }
	    else // this is for printing sp-ions
	    {
		    offset = n - sp_sys->number_of_classic_ion;
		    fprintf(stdout,"%3s%12.6lf%12.6lf%12.6lf\n", sp_sys->sp_ion[offset].atom_name,
			    gsl_vector_get(sp_sys->sp_ion[offset].core_position,0), gsl_vector_get(sp_sys->sp_ion[offset].core_position,1), gsl_vector_get(sp_sys->sp_ion[offset].core_position,2));
	    }
	}

	printf("\n");
	printf(" CONFIGURATION_XYZ_SC_INFO\n");
	printf(" %d\t%d\n",sp_sys->number_of_classic_ion,sp_sys->number_of_sp_ion);

	// SHOW SHELL CORE POSITION BOTH
	for(int n=0;n<sp_sys->number_of_sp_ion+sp_sys->number_of_classic_ion;n++)
	{
    	    offset = n-sp_sys->number_of_classic_ion;
	    if( n < sp_sys->number_of_classic_ion )
	    {
		if( sp_sys->classic_ion[n].if_shell == SP_SUPPORT_FALSE )
		{
			fprintf(stdout,"%3s%3s%12.6lf%12.6lf%12.6lf\n", sp_sys->classic_ion[n].atom_name,"c",
			gsl_vector_get(sp_sys->classic_ion[n].core_position,0), gsl_vector_get(sp_sys->classic_ion[n].core_position,1), gsl_vector_get(sp_sys->classic_ion[n].core_position,2));
		}
		else if( sp_sys->classic_ion[n].if_shell == SP_SUPPORT_TRUE )
		{
			fprintf(stdout,"%3s%3s%12.6lf%12.6lf%12.6lf\n", sp_sys->classic_ion[n].atom_name,"s",
			gsl_vector_get(sp_sys->classic_ion[n].core_position,0), gsl_vector_get(sp_sys->classic_ion[n].core_position,1), gsl_vector_get(sp_sys->classic_ion[n].core_position,2));
		}
	    }
	    else // this is for printing sp-ions
	    {
		    offset = n - sp_sys->number_of_classic_ion;
		    fprintf(stdout,"%3s%15.6lf%12.6lf%12.6lf\n", sp_sys->sp_ion[offset].atom_name,
			    gsl_vector_get(sp_sys->sp_ion[offset].core_position,0), gsl_vector_get(sp_sys->sp_ion[offset].core_position,1), gsl_vector_get(sp_sys->sp_ion[offset].core_position,2));
	    }
	}

	return;
}

/// DIIS_SUPPORT TOOLS

/*
int if_diis;
int diis_max_depth;
int diis_cur_depth;

gsl_vector** diis_error_vector;                         // SAVE AS CIRCULAR QUEUE
gsl_vector*  diis_coefficient_vector;
gsl_permutation* diis_ws_p;
gsl_matrix*  diis_error_matrix;
gsl_matrix*  diis_error_matrix_inv;
*/

void sp_cluster_support_diis_error_vector_queue_init( void* sp_sys_void )
{
    sp_cluster_system* sp_sys = (sp_cluster_system*)sp_sys_void;
    sp_sys->diis_error_vector_queue_front = 0;
    sp_sys->diis_error_vector_queue_rear  = 0;
    return;
}

int sp_cluster_support_diis_error_vector_queue_isfull( void* sp_sys_void )
{   sp_cluster_system* sp_sys = (sp_cluster_system*)sp_sys_void;
    if( (sp_sys->diis_error_vector_queue_rear+1)%sp_sys->diis_max_depth == sp_sys->diis_error_vector_queue_front )
        return SP_SUPPORT_TRUE;
    else
        return SP_SUPPORT_FALSE;
}

int sp_cluster_support_diis_error_vector_queue_isempty( void* sp_sys_void )
{   sp_cluster_system* sp_sys = (sp_cluster_system*)sp_sys_void;
    if( sp_sys->diis_error_vector_queue_front == sp_sys->diis_error_vector_queue_rear )
        return SP_SUPPORT_TRUE;
    else
        return SP_SUPPORT_FALSE;
}

void sp_cluster_support_diis_error_vector_enqueue( void* sp_sys_void )
{
    sp_cluster_system* sp_sys = (sp_cluster_system*)sp_sys_void;
    double delta_evec[4];
    int low_idx;

    double delta_evec_sum = 0.;

    //if( !((sp_sys->diis_error_vector_queue_rear+1)%sp_sys->diis_max_depth == sp_sys->diis_error_vector_queue_front) )   // check if queue is full
    if( sp_cluster_support_diis_error_vector_queue_isfull( sp_sys ) == SP_SUPPORT_FALSE )
    {
        sp_sys->diis_error_vector_queue_rear = (sp_sys->diis_error_vector_queue_rear+1)%sp_sys->diis_max_depth;         // rear index ++
        sp_sys->diis_cur_depth++;

        for(int i=0;i<sp_sys->number_of_sp_ion;i++)
        {
            low_idx = sp_cluster_support_get_lowest_state(sp_sys->sp_ion[i].eigen_value);

            // current eigenvector - (loaded) previous eigenvector_gs
            delta_evec[0] = gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,0,low_idx) - gsl_vector_get(sp_sys->sp_ion[i].eigen_vector_gs,0);
            delta_evec[1] = gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,1,low_idx) - gsl_vector_get(sp_sys->sp_ion[i].eigen_vector_gs,1);
            delta_evec[2] = gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,2,low_idx) - gsl_vector_get(sp_sys->sp_ion[i].eigen_vector_gs,2);
            delta_evec[3] = gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,3,low_idx) - gsl_vector_get(sp_sys->sp_ion[i].eigen_vector_gs,3);

            delta_evec_sum += delta_evec[0]*delta_evec[0] + delta_evec[1]*delta_evec[1] + delta_evec[2]*delta_evec[2] + delta_evec[3]*delta_evec[3];

            gsl_vector_set(sp_sys->diis_error_vector[ sp_sys->diis_error_vector_queue_rear ],i*4+0,delta_evec[0]);
            gsl_vector_set(sp_sys->diis_error_vector[ sp_sys->diis_error_vector_queue_rear ],i*4+1,delta_evec[0]);
            gsl_vector_set(sp_sys->diis_error_vector[ sp_sys->diis_error_vector_queue_rear ],i*4+2,delta_evec[0]);
            gsl_vector_set(sp_sys->diis_error_vector[ sp_sys->diis_error_vector_queue_rear ],i*4+3,delta_evec[0]);

            // loading .. previous eigen_vector_gs (note that the function 'sp_cluster_support_load_gs_eigenvector()' must be called before enqueue)
            gsl_vector_set(sp_sys->diis_prev_eigen_vector[ sp_sys->diis_error_vector_queue_rear ],i*4+0, gsl_vector_get(sp_sys->sp_ion[i].eigen_vector_gs,0));
            gsl_vector_set(sp_sys->diis_prev_eigen_vector[ sp_sys->diis_error_vector_queue_rear ],i*4+1, gsl_vector_get(sp_sys->sp_ion[i].eigen_vector_gs,1));
            gsl_vector_set(sp_sys->diis_prev_eigen_vector[ sp_sys->diis_error_vector_queue_rear ],i*4+2, gsl_vector_get(sp_sys->sp_ion[i].eigen_vector_gs,2));
            gsl_vector_set(sp_sys->diis_prev_eigen_vector[ sp_sys->diis_error_vector_queue_rear ],i*4+3, gsl_vector_get(sp_sys->sp_ion[i].eigen_vector_gs,3));

            #ifdef DEBUG_SUPPORT
            printf("\ndelta evec\n");
            printf("%12.6lf%12.6lf%12.6lf%12.6lf\n",delta_evec[0],delta_evec[1],delta_evec[2],delta_evec[3]);
            printf("%12.6lf%12.6lf%12.6lf%12.6lf\n",
            gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,0,low_idx),
            gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,1,low_idx),
            gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,2,low_idx),
            gsl_matrix_get(sp_sys->sp_ion[i].eigen_vector,3,low_idx));
            #endif
/*
            printf("%12.6lf%12.6lf%12.6lf%12.6lf\n",gsl_vector_get(sp_sys->diis_prev_eigen_vector[ sp_sys->diis_error_vector_queue_rear ], i*4 + 0 ),
            gsl_vector_get(sp_sys->diis_prev_eigen_vector[ sp_sys->diis_error_vector_queue_rear ], i*4 + 1 ),
            gsl_vector_get(sp_sys->diis_prev_eigen_vector[ sp_sys->diis_error_vector_queue_rear ], i*4 + 2 ),
            gsl_vector_get(sp_sys->diis_prev_eigen_vector[ sp_sys->diis_error_vector_queue_rear ], i*4 + 3 ));
*/
        /* Note that the both,

            Error vector ( sp_sys->diis_error_vector )
            Prev  evecs  ( sp_sys->diis_prev_eigen_vector )

            Follows the form of (gsl_vector)v[i][j]

            v[i]    ... 'i' -> index in the queue // queue is circular type, max dept is 'sp_sys->diis_max_depth'
            v[i][j] ... 'j' -> DATA, s_sp1, px_sp1, py_sp1, pz_sp1 // s_sp2, px_sp2, py_sp2, pz_sp2 // ....
                                i.e., length of number_of_sp_ions * 4 (each lone pair ground-state MO saved with stride of 4)
        */
        }
        printf("delta_evec_sum: %12.8lf\n",sqrt(delta_evec_sum)/sp_sys->number_of_sp_ion);

    }
    return;     // if queue is full do nothing
}

void sp_cluster_support_diis_error_vector_dequeue( void* sp_sys_void )
{   sp_cluster_system* sp_sys = (sp_cluster_system*)sp_sys_void;
    if( sp_cluster_support_diis_error_vector_queue_isempty( sp_sys ) == SP_SUPPORT_FALSE )
    {   sp_sys->diis_error_vector_queue_front = (sp_sys->diis_error_vector_queue_front+1)%sp_sys->diis_max_depth;
        sp_sys->diis_cur_depth--;
    }
    #ifdef DEBUG_SUPPORT
    const int front = sp_sys->diis_error_vector_queue_front;
    const int rear  = sp_sys->diis_error_vector_queue_rear;
    printf("%d\t%d\n",front,rear);
    #endif

    return;     // if queue is empty do nothing
}

double sp_cluster_support_diis_get_error_vector_gnorm( void* sp_sys_void )      // returns latest .. i.e., the one at rear
{   sp_cluster_system* sp_sys = (sp_cluster_system*)sp_sys_void;
    double ret;
    if( sp_cluster_support_diis_error_vector_queue_isempty( sp_sys ) == SP_SUPPORT_FALSE )
    {   ret = gsl_blas_dnrm2( sp_sys->diis_error_vector[ sp_sys->diis_error_vector_queue_rear ] );
        return ret/sp_sys->number_of_sp_ion;
    }
    return 0.;
}

void sp_cluster_support_diis_solve_least_square_problem( void* sp_sys_void )
{   sp_cluster_system* sp_sys = (sp_cluster_system*)sp_sys_void;

    // int count = front > rear ? (MAX - front + rear) : (rear - front);
    const int queue_capacity = sp_sys->diis_max_depth;
    const int front = sp_sys->diis_error_vector_queue_front;
    const int rear  = sp_sys->diis_error_vector_queue_rear;
    const int cur_size = front > rear ? (queue_capacity - front + rear) : (rear - front);       // number of elements in the queue (error_vector)

/*
gsl_vector*  diis_coefficient_vector;
gsl_permutation* diis_ws_p;
gsl_matrix*  diis_error_matrix;
gsl_matrix*  diis_error_matrix_inv;
*/
    size_t len = cur_size + 1;

printf("\n#### IN LSP SOLVER / len : %zu\n",len);
printf("ws size : ErrorMat / %zu %zu\n",sp_sys->diis_error_matrix->size1,sp_sys->diis_error_matrix->size2);
printf("cursize/max_depth/cur_depth : %4d%4d%4d\n", cur_size, sp_sys->diis_max_depth - 1, sp_sys->diis_cur_depth );
    //if( !(cur_size == (sp_sys->diis_max_depth - 1)) )
    //if( !(cur_size == (sp_sys->diis_max_depth)) )
    //if( !(len == (sp_sys->diis_max_depth)) )
    if( !(len == sp_sys->diis_error_matrix->size1) )
    {
printf("Resizing!!\n");
        // if current size is not same with actual max_depth in use ... i.e., max_depth - 1, since circular queue in use
        // have to resize the workspaces ... diis_ws_p (gsl_permutation*) / diis_error_matrix (gsl_matrix*) / diis_error_matrix_inv (gsl_matrix*)

        gsl_vector_free(sp_sys->diis_coefficient_vector);
        gsl_vector_free(sp_sys->diis_least_square_condition);
        gsl_permutation_free(sp_sys->diis_ws_p);
        gsl_matrix_free(sp_sys->diis_error_matrix);
        gsl_matrix_free(sp_sys->diis_error_matrix_inv);

        sp_sys->diis_coefficient_vector = gsl_vector_calloc(len);
        sp_sys->diis_least_square_condition = gsl_vector_calloc(len);
        sp_sys->diis_ws_p = gsl_permutation_calloc(len);
        sp_sys->diis_error_matrix = gsl_matrix_calloc(len,len);
        sp_sys->diis_error_matrix_inv = gsl_matrix_calloc(len,len);      // len = cur_size (error_vector length) + 1 // 1 is to represent least square sense of coeff
    }

    int ii=0; int jj=0;     // tags to get actual matrix indices !
    double elem_tmp;
    for(int i=(front+1)%queue_capacity; i!=(rear+1)%queue_capacity ; i=(i+1)%queue_capacity)    // for loop circulate over queue elements
    {
        for(int j=(front+1)%queue_capacity; j!=(rear+1)%queue_capacity ; j=(j+1)%queue_capacity)
        {
            #ifdef DEBUG_SUPPORT
            printf("i,j / tag_i,tag_j : %d\t%d / %d\t%d\n",i,j,ii,jj);
            //printf("size1 / size2 : %d\t%d\n", sp_sys->diis_error_matrix->size1, sp_sys->diis_error_matrix->size2);
            #endif

            gsl_blas_ddot(sp_sys->diis_error_vector[j],sp_sys->diis_error_vector[i],&elem_tmp);
            gsl_matrix_set(sp_sys->diis_error_matrix,ii,jj,elem_tmp);
            jj++;
        }
        jj=0;
        ii++;
    }
    printf("ERROR - 0 ?\n");
    ///// SET RESTS
     for(int i=0;i<len;i++)
    {   gsl_matrix_set(sp_sys->diis_error_matrix,i,len-1,-1.);
        gsl_matrix_set(sp_sys->diis_error_matrix,len-1,i,-1.);        }
    printf("ERROR?\n");
    gsl_matrix_set(sp_sys->diis_error_matrix,len-1,len-1,0.);
    printf("ERROR?\n");

    ///// SET least Square Condition vector
    gsl_vector_set(sp_sys->diis_least_square_condition,len-1,-1.);

    //// calculate inverse
    int signum; // variable for LU_decomp

    #ifdef DEBUG_SUPPORT
    printf("ErrorMatrix\n");
    sp_cluster_support_matrix_view( sp_sys->diis_error_matrix );
    #endif

    /*  Say,
        Error matrix                    E
        Coefficient Vector              C
        RHS Least Square Condition      L

        Need to find 'C' vector

        Solve : EC = L, i.e., C = E_Inverse  L
    */

    gsl_linalg_LU_decomp(sp_sys->diis_error_matrix,sp_sys->diis_ws_p,&signum);
    gsl_linalg_LU_invert(sp_sys->diis_error_matrix,sp_sys->diis_ws_p,sp_sys->diis_error_matrix_inv);    // invert is saved in 'sp_sys->diis_error_matrix_inv'
    // Inverse Found

    // int gsl_blas_dgemv(CBLAS_TRANSPOSE_t TransA, double alpha, const gsl_matrix *A, const gsl_vector *x, double beta, gsl_vector *y)
    gsl_blas_dgemv(CblasNoTrans,1.,sp_sys->diis_error_matrix_inv,sp_sys->diis_least_square_condition,0.,sp_sys->diis_coefficient_vector);
    // Solve LeaseSquare Problem Answer saved in 'sp_sys->diis_coefficient_vector'

    #ifdef DEBUG_SUPPORT
    //sp_cluster_support_vector_view( sp_sys->diis_least_square_condition );
    printf("coefficient vector, ignore the last dummy lambda value\n");
    sp_cluster_support_vector_view( sp_sys->diis_coefficient_vector );
    //sp_cluster_support_matrix_view( sp_sys->diis_error_matrix );
    printf("InverseMatrix\n");
    sp_cluster_support_matrix_view( sp_sys->diis_error_matrix_inv );
    printf("cursize/max_depth/cur_depth : %d\t%d\t%d\n", cur_size, sp_sys->diis_max_depth - 1, sp_sys->diis_cur_depth );

    printf(" ---- FINALISE LEAST SQUARE SOLVER \n\n");
    #endif
    return;
}

void sp_cluster_support_diis_least_square_result_update( void* sp_sys_void )
{   sp_cluster_system* sp_sys = (sp_cluster_system*)sp_sys_void;
    int low_idx;
    int jj = 0;
    double cs, cx, cy, cz, norm_factor;

    const int queue_capacity = sp_sys->diis_max_depth;
    const int front = sp_sys->diis_error_vector_queue_front;
    const int rear  = sp_sys->diis_error_vector_queue_rear;

    for(int i=0;i<sp_sys->number_of_sp_ion;i++)
    {
        low_idx = sp_cluster_support_get_lowest_state(sp_sys->sp_ion[i].eigen_value);

        // results are in 'sp_sys->diis_coefficient_vector' ... data structure sp_sys->diis_coefficient_vector->size or 'stride'?
        // size can also be obtained by 'sp_sys->diis_cur_depth'

        jj = 0;
        cs = 0.; cx = 0.; cy = 0.; cz = 0.;

        for(int j=(front+1)%queue_capacity; j!=(rear+1)%queue_capacity ; j=(j+1)%queue_capacity)
        {
            cs += gsl_vector_get( sp_sys->diis_coefficient_vector,jj) * gsl_vector_get(sp_sys->diis_prev_eigen_vector[j],i*4+0);
            cx += gsl_vector_get( sp_sys->diis_coefficient_vector,jj) * gsl_vector_get(sp_sys->diis_prev_eigen_vector[j],i*4+1);
            cy += gsl_vector_get( sp_sys->diis_coefficient_vector,jj) * gsl_vector_get(sp_sys->diis_prev_eigen_vector[j],i*4+2);
            cz += gsl_vector_get( sp_sys->diis_coefficient_vector,jj) * gsl_vector_get(sp_sys->diis_prev_eigen_vector[j],i*4+3);
            jj++;

            printf("index j / tag jj: %d\t%d\n",j,jj);
        }

#ifdef DEBUG_SUPPORT
printf("%dth\t%12.4lf%12.4lf%12.4lf%12.4lf%20.4lf\n",i+1,cs,cx,cy,cz,sqrt(cs*cs+cx*cx+cy*cy+cz*cz));
#endif
        norm_factor = sqrt(cs*cs+cx*cx+cy*cy+cz*cz);
        norm_factor = 1.;

        gsl_matrix_set(sp_sys->sp_ion[i].eigen_vector,0,low_idx,cs/norm_factor);
        gsl_matrix_set(sp_sys->sp_ion[i].eigen_vector,1,low_idx,cx/norm_factor);
        gsl_matrix_set(sp_sys->sp_ion[i].eigen_vector,2,low_idx,cy/norm_factor);
        gsl_matrix_set(sp_sys->sp_ion[i].eigen_vector,3,low_idx,cz/norm_factor);
        // set eigenvector ... updated!!!

        // finish up the rest ...
    }

    return;
}


#pragma once
#include <gsl/gsl_fit.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_movstat.h>
#include <gsl/gsl_vector.h>
#include <cstdlib>
#include "DataFrame.h"
#include "Ops.h"

namespace adsl {

	// Evaluate the result of a fit at a given point based on the DataFrame description
	// double <- DataFrame
	/*
	auto evalFit = [](double t) {
		auto retFunc = [t](DataFrame& df) {
			if (df.getDesc() == "gsl_fit_linear") {
				double m = df + select({ "slope" }) + getFirst + single();
				double b = df + select({ "intercept" }) + getFirst + single();
				return (m * t) + b;
			}
			else {
				return (double)NAN;
			}
		};
		return retFunc;
	};
	*/

	// Perform a linear fit using gsl_fit_linear
	// df must have only 2 columns
	// DataFrame <- DataFrame
	auto fitLinear = [](DataFrame& df) {
		if (df.getCols() == 2) {
			if (df.getData()[0].type != DBL || df.getData()[1].type != DBL) {
				std::cout << "[fitLinear] <<ERROR>> At least one of the columns is not of type DBL " << std::endl;
				exit(1);
				DataFrame empty_df;
				return empty_df;
			}
			// Setup
			DataFrame ret;
			auto xData = df.getData()[0].toVec_dbl();
			auto yData = df.getData()[1].toVec_dbl();
			size_t length = xData.size();
			double* x;
			double* y;
			x = (double*)malloc(length * sizeof(double));
			y = (double*)malloc(length * sizeof(double));
			std::copy(xData.begin(), xData.end(), x);
			std::copy(yData.begin(), yData.end(), y);

			// Fitting & r-squared
			double c0, c1, cov00, cov01, cov11, sumsq;
			gsl_fit_linear(x, 1, y, 1, length, &c0, &c1, &cov00, &cov01, &cov11, &sumsq);
			vd tmpSlope = {c1};
			DataList slope(&tmpSlope, DataType::DBL, "slope");
			vd tmpIntercept = {c0};
			DataList intercept(&tmpIntercept, DataType::DBL, "intercept");
			vd tmpRsqrd = { gsl_stats_correlation(x, 1, y, 1, length) };
			DataList rsqrd(&tmpRsqrd, DataType::DBL, "r-squared");

			// Clean up
			free(x);
			free(y);

			// Return
			ret.addCol(slope);
			ret.addCol(intercept);
			ret.addCol(rsqrd);
			ret.setDesc("gsl_fit_linear");
			return ret;
		}
		else {
			std::cout << "[fitLinear] <<ERROR>> DataFrame doesn't have two columns; has " << df.getCols() << std::endl;
			exit(1);
			DataFrame empty_df;
			return empty_df;
		}
	};

	// TODO: add more fit types

	// Calculate the moving average of a DataList
	// DataList <- DataList <- double
	/*
	auto SMA = [](double period) {
		auto retFunc = [period](DataList& dl) {
			DataFrame ret;
			double* input;
			double* output;
			// TODO
		};
		return retFunc;
	};
	*/

 }


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

#ifndef __GSL_STATISTICS_CHAR_H__
#define __GSL_STATISTICS_CHAR_H__

#include <stddef.h>
#include <gsl/gsl_types.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

GSL_EXPORT double gsl_stats_char_mean (const char data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_char_variance (const char data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_char_sd (const char data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_char_variance_with_fixed_mean (const char data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_char_sd_with_fixed_mean (const char data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_char_absdev (const char data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_char_skew (const char data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_char_kurtosis (const char data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_char_lag1_autocorrelation (const char data[], const size_t stride, const size_t n);

GSL_EXPORT double gsl_stats_char_covariance (const char data1[], const size_t stride1,const char data2[], const size_t stride2, const size_t n);

GSL_EXPORT double gsl_stats_char_variance_m (const char data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_char_sd_m (const char data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_char_absdev_m (const char data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_char_skew_m_sd (const char data[], const size_t stride, const size_t n, const double mean, const double sd);
GSL_EXPORT double gsl_stats_char_kurtosis_m_sd (const char data[], const size_t stride, const size_t n, const double mean, const double sd);
GSL_EXPORT double gsl_stats_char_lag1_autocorrelation_m (const char data[], const size_t stride, const size_t n, const double mean);

GSL_EXPORT double gsl_stats_char_covariance_m (const char data1[], const size_t stride1,const char data2[], const size_t stride2, const size_t n, const double mean1, const double mean2);

GSL_EXPORT double gsl_stats_char_pvariance (const char data1[], const size_t stride1, const size_t n1, const char data2[], const size_t stride2, const size_t n2);
GSL_EXPORT double gsl_stats_char_ttest (const char data1[], const size_t stride1, const size_t n1, const char data2[], const size_t stride2, const size_t n2);

GSL_EXPORT char gsl_stats_char_max (const char data[], const size_t stride, const size_t n);
GSL_EXPORT char gsl_stats_char_min (const char data[], const size_t stride, const size_t n);
GSL_EXPORT void gsl_stats_char_minmax (char * min, char * max, const char data[], const size_t stride, const size_t n);

GSL_EXPORT size_t gsl_stats_char_max_index (const char data[], const size_t stride, const size_t n);
GSL_EXPORT size_t gsl_stats_char_min_index (const char data[], const size_t stride, const size_t n);
GSL_EXPORT void gsl_stats_char_minmax_index (size_t * min_index, size_t * max_index, const char data[], const size_t stride, const size_t n);

GSL_EXPORT double gsl_stats_char_median_from_sorted_data (const char sorted_data[], const size_t stride, const size_t n) ;
GSL_EXPORT double gsl_stats_char_quantile_from_sorted_data (const char sorted_data[], const size_t stride, const size_t n, const double f) ;

__END_DECLS

#endif /* __GSL_STATISTICS_CHAR_H__ */


/* Copyright (c) 2017 Bradley Worley <geekysuavo@gmail.com>
 * Released under the MIT License
 */

/* ensure once-only inclusion. */
#ifndef __MATTE_BLAS_H__
#define __MATTE_BLAS_H__

/* include the vector and matrix headers. */
#include <matte/vector.h>
#include <matte/matrix.h>

/* include the complex vector and matrix headers. */
#include <matte/complex-vector.h>
#include <matte/complex-matrix.h>

/* include the atlas blas header. */
#include <cblas.h>

/* function declarations, level 1, double (blas.c): */

int matte_daxpy (double alpha, Vector x, Vector y);

int matte_dscal (double alpha, Vector x);

int matte_dcopy (Vector x, Vector y);

int matte_dswap (Vector x, Vector y);

int matte_ddot (Vector x, Vector y, double *out);

int matte_dnrm2 (Vector x, double *out);

/* function declarations, level 1, complex double (blas.c): */

int matte_zaxpy (complex double alpha, ComplexVector x, ComplexVector y);

int matte_zscal (complex double alpha, ComplexVector x);

int matte_zcopy (ComplexVector x, ComplexVector y);

int matte_zswap (ComplexVector x, ComplexVector y);

int matte_zdotu (ComplexVector x, ComplexVector y, complex double *out);

int matte_zdotc (ComplexVector x, ComplexVector y, complex double *out);

int matte_znrm2 (ComplexVector x, double *out);

/* function declarations, level 2, double (blas.c): */

int matte_dgemv (MatteTranspose trans, double alpha, Matrix A, Vector x,
                 double beta, Vector y);

int matte_dsymv (MatteTriangle uplo, double alpha, Matrix A, Vector x,
                 double beta, Vector y);

int matte_dtrmv (MatteTriangle uplo, MatteTranspose trans,
                 MatteDiagonal diag, Matrix A, Vector x);

int matte_dtrsv (MatteTriangle uplo, MatteTranspose trans,
                 MatteDiagonal diag, Matrix A, Vector x);

int matte_dger (double alpha, Vector x, Vector y, Matrix A);

int matte_dsyr (MatteTriangle uplo, double alpha, Vector x, Matrix A);

int matte_dsyr2 (MatteTriangle uplo, double alpha, Vector x, Vector y,
                 Matrix A);

/* function declarations, level 2, complex double (blas.c): */

int matte_zgemv (MatteTranspose trans, complex double alpha, ComplexMatrix A,
                 ComplexVector x, complex double beta, ComplexVector y);

int matte_zhemv (MatteTriangle uplo, complex double alpha, ComplexMatrix A,
                 ComplexVector x, complex double beta, ComplexVector y);

int matte_ztrmv (MatteTriangle uplo, MatteTranspose trans,
                 MatteDiagonal diag, ComplexMatrix A,
                 ComplexVector x);

int matte_ztrsv (MatteTriangle uplo, MatteTranspose trans,
                 MatteDiagonal diag, ComplexMatrix A,
                 ComplexVector x);

int matte_zher (MatteTriangle uplo, double alpha,
                ComplexVector x, ComplexMatrix A);

int matte_zher2 (MatteTriangle uplo, complex double alpha,
                 ComplexVector x, ComplexVector y,
                 ComplexMatrix A);

/* function declarations, level 3, double (blas.c): */

int matte_dgemm (MatteTranspose transA, MatteTranspose transB,
                 double alpha, Matrix A, Matrix B,
                 double beta, Matrix C);

/* FIXME */

/* function declarations, level 3, complex double (blas.c): */

int matte_zgemm (MatteTranspose transA, MatteTranspose transB,
                 complex double alpha, ComplexMatrix A, ComplexMatrix B,
                 complex double beta, ComplexMatrix C);

/* FIXME */

#endif /* !__MATTE_BLAS_H__ */


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

#include <config.h>
#include <stddef.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <float.h>

#include <time.h>

#include <gsl/gsl_complex.h>
#include <gsl/gsl_fft_complex.h>

#include <gsl/gsl_errno.h>

#include "complex_internal.h"
#include "bitreverse.h"
#include "urand.c"

void my_error_handler (const char *reason, const char *file,
                       int line, int err);

int
main (int argc, char *argv[])
{
  double *data, *fft_data;
  gsl_fft_complex_wavetable * cw;
  unsigned int i, logn;
  int result;
  int status;
  clock_t start, end;
  int resolution = CLOCKS_PER_SEC;
  unsigned int n = 1;

  gsl_set_error_handler (&my_error_handler);

  if (argc == 2)
    {
      n = strtol (argv[1], NULL, 0);
    }
  else
    {
      printf ("test n\n");
      exit (EXIT_FAILURE);
    }

  cw = gsl_fft_complex_wavetable_alloc (n);
  cwork = gsl_fft_complex_wavetable_workspace_alloc (n);
  status = gsl_fft_complex_init (n, cw, cwork);
  status = gsl_fft_complex_generate (n, cw, cwork);

  data = (double *) malloc (n * 2 * sizeof (double));
  fft_data = (double *) malloc (n * 2 * sizeof (double));

  for (i = 0; i < n; i++)
    {
      REAL(data,1,i) = urand ();
      IMAG(data,1,i) = urand ();
    }

  /* compute the fft */

  memcpy (fft_data, data, n * 2 * sizeof (double));

  start = clock ();
  i = 0;
  do
    {
      status = gsl_fft_complex_forward (fft_data, 1, n, cw, cwork);
      i++;
      end = clock ();
    }
  while (end < start + resolution && status == 0);

  if (status == 0)
    {
      printf ("n = %d gsl_fft_complex_forward %f seconds\n", n, (end - start) / ((double) i) / ((double) CLOCKS_PER_SEC));
    }
  else
    {
      printf ("MR fft failed\n");
    }

  /* compute the fft with radix2 */
  memcpy (fft_data, data, n * 2 * sizeof (double));

  result = fft_binary_logn(n) ;

  if (result == -1) {
    exit(EXIT_SUCCESS) ;
  } else {
    logn = result ;
  }

  start = clock ();
  i = 0;
  do
    {
      status = gsl_fft_complex_radix2_forward (fft_data, 1, n);
      i++;
      end = clock ();
    }
  while (end < start + resolution && status == 0);

  if (status == 0)
    {
      printf ("n = %d gsl_fft_complex_radix2_forward %f seconds\n", n, (end - start) / ((double) i) / ((double) CLOCKS_PER_SEC));
    }
  else
    {
      printf ("fft_radix2: not a power of 2\n");
    }

  start = clock ();
  i = 0;
  do
    {
      status = fft_complex_bitreverse_order (fft_data, 1, n, logn);
      i++;
      end = clock ();
    }
  while (end < start + resolution && status == 0);

  printf ("n = %d gsl_fft_complex_bitreverse_order %f seconds\n", n, (end - start) / ((double) i) / ((double) CLOCKS_PER_SEC));

  return 0;
}

void
my_error_handler (const char *reason, const char *file, int line, int err)
{
  printf ("error: %s in %s at %d (gsl_errno=%d)\n", reason, file, line, err);
}


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

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>

#include <gsl/gsl_linalg.h>

#include "apply_givens.c"

/* Factorise a general M x N matrix A into
 *
 *   A P = Q R
 *
 * where Q is orthogonal (M x M) and R is upper triangular (M x N).
 * When A is rank deficient, r = rank(A) < n, then the permutation is
 * used to ensure that the lower n - r rows of R are zero and the first
 * r columns of Q form an orthonormal basis for A.
 *
 * Q is stored as a packed set of Householder transformations in the
 * strict lower triangular part of the input matrix.
 *
 * R is stored in the diagonal and upper triangle of the input matrix.
 *
 * P: column j of P is column k of the identity matrix, where k =
 * permutation->data[j]
 *
 * The full matrix for Q can be obtained as the product
 *
 *       Q = Q_k .. Q_2 Q_1
 *
 * where k = MIN(M,N) and
 *
 *       Q_i = (I - tau_i * v_i * v_i')
 *
 * and where v_i is a Householder vector
 *
 *       v_i = [1, m(i+1,i), m(i+2,i), ... , m(M,i)]
 *
 * This storage scheme is the same as in LAPACK.  See LAPACK's
 * dgeqpf.f for details.
 *
 */

int
gsl_linalg_QRPT_decomp (gsl_matrix * A, gsl_vector * tau, gsl_permutation * p, int *signum, gsl_vector * norm)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (tau->size != GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (p->size != N)
    {
      GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
    }
  else if (norm->size != N)
    {
      GSL_ERROR ("norm size must be N", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      *signum = 1;

      gsl_permutation_init (p); /* set to identity */

      /* Compute column norms and store in workspace */

      for (i = 0; i < N; i++)
        {
          gsl_vector_view c = gsl_matrix_column (A, i);
          double x = gsl_blas_dnrm2 (&c.vector);
          gsl_vector_set (norm, i, x);
        }

      for (i = 0; i < GSL_MIN (M, N); i++)
        {
          /* Bring the column of largest norm into the pivot position */

          double max_norm = gsl_vector_get(norm, i);
          size_t j, kmax = i;

          for (j = i + 1; j < N; j++)
            {
              double x = gsl_vector_get (norm, j);

              if (x > max_norm)
                {
                  max_norm = x;
                  kmax = j;
                }
            }

          if (kmax != i)
            {
              gsl_matrix_swap_columns (A, i, kmax);
              gsl_permutation_swap (p, i, kmax);
              gsl_vector_swap_elements(norm,i,kmax);

              (*signum) = -(*signum);
            }

          /* Compute the Householder transformation to reduce the j-th
             column of the matrix to a multiple of the j-th unit vector */

          {
            gsl_vector_view c_full = gsl_matrix_column (A, i);
            gsl_vector_view c = gsl_vector_subvector (&c_full.vector,
                                                      i, M - i);
            double tau_i = gsl_linalg_householder_transform (&c.vector);

            gsl_vector_set (tau, i, tau_i);

            /* Apply the transformation to the remaining columns */

            if (i + 1 < N)
              {
                gsl_matrix_view m = gsl_matrix_submatrix (A, i, i + 1, M - i, N - (i+1));

                gsl_linalg_householder_hm (tau_i, &c.vector, &m.matrix);
              }
          }

          /* Update the norms of the remaining columns too */

          if (i + 1 < M)
            {
              for (j = i + 1; j < N; j++)
                {
                  double x = gsl_vector_get (norm, j);

                  if (x > 0.0)
                    {
                      double y = 0;
                      double temp= gsl_matrix_get (A, i, j) / x;

                      if (fabs (temp) >= 1)
                        y = 0.0;
                      else
                        y = x * sqrt (1 - temp * temp);

                      /* recompute norm to prevent loss of accuracy */

                      if (fabs (y / x) < sqrt (20.0) * GSL_SQRT_DBL_EPSILON)
                        {
                          gsl_vector_view c_full = gsl_matrix_column (A, j);
                          gsl_vector_view c =
                            gsl_vector_subvector(&c_full.vector,
                                                 i+1, M - (i+1));
                          y = gsl_blas_dnrm2 (&c.vector);
                        }

                      gsl_vector_set (norm, j, y);
                    }
                }
            }
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QRPT_decomp2 (const gsl_matrix * A, gsl_matrix * q, gsl_matrix * r, gsl_vector * tau, gsl_permutation * p, int *signum, gsl_vector * norm)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (q->size1 != M || q->size2 !=M)
    {
      GSL_ERROR ("q must be M x M", GSL_EBADLEN);
    }
  else if (r->size1 != M || r->size2 !=N)
    {
      GSL_ERROR ("r must be M x N", GSL_EBADLEN);
    }
  else if (tau->size != GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (p->size != N)
    {
      GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
    }
  else if (norm->size != N)
    {
      GSL_ERROR ("norm size must be N", GSL_EBADLEN);
    }

  gsl_matrix_memcpy (r, A);

  gsl_linalg_QRPT_decomp (r, tau, p, signum, norm);

  /* FIXME:  aliased arguments depends on behavior of unpack routine! */

  gsl_linalg_QR_unpack (r, tau, q, r);

  return GSL_SUCCESS;
}

/* Solves the system A x = b using the Q R P^T factorisation,

   R z = Q^T b

   x = P z;

   to obtain x. Based on SLATEC code. */

int
gsl_linalg_QRPT_solve (const gsl_matrix * QR,
                       const gsl_vector * tau,
                       const gsl_permutation * p,
                       const gsl_vector * b,
                       gsl_vector * x)
{
  if (QR->size1 != QR->size2)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (QR->size1 != p->size)
    {
      GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
    }
  else if (QR->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (QR->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      gsl_vector_memcpy (x, b);

      gsl_linalg_QRPT_svx (QR, tau, p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QRPT_svx (const gsl_matrix * QR,
                     const gsl_vector * tau,
                     const gsl_permutation * p,
                     gsl_vector * x)
{
  if (QR->size1 != QR->size2)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (QR->size1 != p->size)
    {
      GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
    }
  else if (QR->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* compute sol = Q^T b */

      gsl_linalg_QR_QTvec (QR, tau, x);

      /* Solve R x = sol, storing x inplace in sol */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

/* Find the least squares solution to the overdetermined system
 *
 *   A x = b
 *
 * for M >= N using the QRPT factorization A P = Q R. Assumes
 * A has full column rank.
 */

int
gsl_linalg_QRPT_lssolve (const gsl_matrix * QR, const gsl_vector * tau, const gsl_permutation * p,
                         const gsl_vector * b, gsl_vector * x, gsl_vector * residual)
{
  const size_t N = QR->size2;
  int status = gsl_linalg_QRPT_lssolve2(QR, tau, p, b, N, x, residual);
  return status;
}

/* Find the least squares solution to the overdetermined system
 *
 *   A x = b
 *
 * for M >= N using the QRPT factorization A P = Q R, where A
 * is assumed rank deficient with a given rank.
 */

int
gsl_linalg_QRPT_lssolve2 (const gsl_matrix * QR, const gsl_vector * tau, const gsl_permutation * p,
                          const gsl_vector * b, const size_t rank, gsl_vector * x, gsl_vector * residual)
{
  const size_t M = QR->size1;
  const size_t N = QR->size2;

  if (M < N)
    {
      GSL_ERROR ("QR matrix must have M>=N", GSL_EBADLEN);
    }
  else if (M != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (rank == 0 || rank > N)
    {
      GSL_ERROR ("rank must have 0 < rank <= N", GSL_EBADLEN);
    }
  else if (N != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else if (M != residual->size)
    {
      GSL_ERROR ("matrix size must match residual size", GSL_EBADLEN);
    }
  else
    {
      gsl_matrix_const_view R11 = gsl_matrix_const_submatrix (QR, 0, 0, rank, rank);
      gsl_vector_view QTb1 = gsl_vector_subvector(residual, 0, rank);
      gsl_vector_view x1 = gsl_vector_subvector(x, 0, rank);
      size_t i;

      /* compute work = Q^T b */
      gsl_vector_memcpy(residual, b);
      gsl_linalg_QR_QTvec (QR, tau, residual);

      /* solve R_{11} x(1:r) = [Q^T b](1:r) */
      gsl_vector_memcpy(&(x1.vector), &(QTb1.vector));
      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, &(R11.matrix), &(x1.vector));

      /* x(r+1:N) = 0 */
      for (i = rank; i < N; ++i)
        gsl_vector_set(x, i, 0.0);

      /* compute x = P y */
      gsl_permute_vector_inverse (p, x);

      /* compute residual = b - A x = Q (Q^T b - R x) */
      gsl_vector_set_zero(&(QTb1.vector));
      gsl_linalg_QR_Qvec(QR, tau, residual);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QRPT_QRsolve (const gsl_matrix * Q, const gsl_matrix * R,
                         const gsl_permutation * p,
                         const gsl_vector * b,
                         gsl_vector * x)
{
  if (Q->size1 != Q->size2 || R->size1 != R->size2)
    {
      return GSL_ENOTSQR;
    }
  else if (Q->size1 != p->size || Q->size1 != R->size1
           || Q->size1 != b->size)
    {
      return GSL_EBADLEN;
    }
  else
    {
      /* compute b' = Q^T b */

      gsl_blas_dgemv (CblasTrans, 1.0, Q, b, 0.0, x);

      /* Solve R x = b', storing x inplace */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, R, x);

      /* Apply permutation to solution in place */

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QRPT_Rsolve (const gsl_matrix * QR,
                        const gsl_permutation * p,
                        const gsl_vector * b,
                        gsl_vector * x)
{
  if (QR->size1 != QR->size2)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (QR->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (QR->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match x size", GSL_EBADLEN);
    }
  else if (p->size != x->size)
    {
      GSL_ERROR ("permutation size must match x size", GSL_EBADLEN);
    }
  else
    {
      /* Copy x <- b */

      gsl_vector_memcpy (x, b);

      /* Solve R x = b, storing x inplace */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QRPT_Rsvx (const gsl_matrix * QR,
                      const gsl_permutation * p,
                      gsl_vector * x)
{
  if (QR->size1 != QR->size2)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (QR->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match x size", GSL_EBADLEN);
    }
  else if (p->size != x->size)
    {
      GSL_ERROR ("permutation size must match x size", GSL_EBADLEN);
    }
  else
    {
      /* Solve R x = b, storing x inplace */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

/* Update a Q R P^T factorisation for A P= Q R ,  A' = A + u v^T,

   Q' R' P^-1 = QR P^-1 + u v^T
   = Q (R + Q^T u v^T P ) P^-1
   = Q (R + w v^T P) P^-1

   where w = Q^T u.

   Algorithm from Golub and Van Loan, "Matrix Computations", Section
   12.5 (Updating Matrix Factorizations, Rank-One Changes)  */

int
gsl_linalg_QRPT_update (gsl_matrix * Q, gsl_matrix * R,
                        const gsl_permutation * p,
                        gsl_vector * w, const gsl_vector * v)
{
  const size_t M = R->size1;
  const size_t N = R->size2;

  if (Q->size1 != M || Q->size2 != M)
    {
      GSL_ERROR ("Q matrix must be M x M if R is M x N", GSL_ENOTSQR);
    }
  else if (w->size != M)
    {
      GSL_ERROR ("w must be length M if R is M x N", GSL_EBADLEN);
    }
  else if (v->size != N)
    {
      GSL_ERROR ("v must be length N if R is M x N", GSL_EBADLEN);
    }
  else
    {
      size_t j, k;
      double w0;

      /* Apply Given's rotations to reduce w to (|w|, 0, 0, ... , 0)

         J_1^T .... J_(n-1)^T w = +/- |w| e_1

         simultaneously applied to R,  H = J_1^T ... J^T_(n-1) R
         so that H is upper Hessenberg.  (12.5.2) */

      for (k = M - 1; k > 0; k--)
        {
          double c, s;
          double wk = gsl_vector_get (w, k);
          double wkm1 = gsl_vector_get (w, k - 1);

          gsl_linalg_givens (wkm1, wk, &c, &s);
          gsl_linalg_givens_gv (w, k - 1, k, c, s);
          apply_givens_qr (M, N, Q, R, k - 1, k, c, s);
        }

      w0 = gsl_vector_get (w, 0);

      /* Add in w v^T  (Equation 12.5.3) */

      for (j = 0; j < N; j++)
        {
          double r0j = gsl_matrix_get (R, 0, j);
          size_t p_j = gsl_permutation_get (p, j);
          double vj = gsl_vector_get (v, p_j);
          gsl_matrix_set (R, 0, j, r0j + w0 * vj);
        }

      /* Apply Givens transformations R' = G_(n-1)^T ... G_1^T H
         Equation 12.5.4 */

     for (k = 1; k < GSL_MIN(M,N+1); k++)
        {
          double c, s;
          double diag = gsl_matrix_get (R, k - 1, k - 1);
          double offdiag = gsl_matrix_get (R, k, k - 1);

          gsl_linalg_givens (diag, offdiag, &c, &s);
          apply_givens_qr (M, N, Q, R, k - 1, k, c, s);

          gsl_matrix_set (R, k, k - 1, 0.0);    /* exact zero of G^T */
        }

      return GSL_SUCCESS;
    }
}

/*
gsl_linalg_QRPT_rank()
  Estimate rank of triangular matrix from QRPT decomposition

Inputs: QR  - QRPT decomposed matrix
        tol - tolerance for rank determination; if < 0,
              a default value is used:
              20 * (M + N) * eps(max(|diag(R)|))

Return: rank estimate
*/

size_t
gsl_linalg_QRPT_rank (const gsl_matrix * QR, const double tol)
{
  const size_t M = QR->size1;
  const size_t N = QR->size2;
  gsl_vector_const_view diag = gsl_matrix_const_diagonal(QR);
  double eps;
  size_t i;
  size_t r = 0;

  if (tol < 0.0)
    {
      double min, max, absmax;
      int ee;

      gsl_vector_minmax(&diag.vector, &min, &max);
      absmax = GSL_MAX(fabs(min), fabs(max));
      ee = (int) (log(absmax) / M_LN2); /* can't use log2 since its not ANSI */

      eps = 20.0 * (M + N) * pow(2.0, (double) ee) * GSL_DBL_EPSILON;
    }
  else
    eps = tol;

  /* count number of diagonal elements with |di| > eps */
  for (i = 0; i < GSL_MIN(M, N); ++i)
    {
      double di = gsl_vector_get(&diag.vector, i);
      if (fabs(di) > eps)
        ++r;
    }

  return r;
}

int
gsl_linalg_QRPT_rcond(const gsl_matrix * QR, double * rcond, gsl_vector * work)
{
  const size_t M = QR->size1;
  const size_t N = QR->size2;

  if (M < N)
    {
      GSL_ERROR ("M must be >= N", GSL_EBADLEN);
    }
  else if (work->size != 3 * N)
    {
      GSL_ERROR ("work vector must have length 3*N", GSL_EBADLEN);
    }
  else
    {
      gsl_matrix_const_view R = gsl_matrix_const_submatrix (QR, 0, 0, N, N);
      int status;

      status = gsl_linalg_tri_upper_rcond(&R.matrix, rcond, work);

      return status;
    }
}


/* Copyright 2019, Alistair Boyle, 3-clause BSD License */
#include "config.h"
#include <stdlib.h> /* malloc, free */
#include <assert.h> /* assert */
#include <stdio.h> /* printf */
#include <string.h> /* memset */
#include <float.h> /* DBL_MIN */
#include <lapacke.h>

#include "matrix.h"
#include "fwd.h"
#include "inv.h"

#ifdef UNIT_TESTING
extern void * _mock_test_malloc(const size_t size, const char * file, const int line);
extern void _test_free(void * const ptr, const char * file, const int line);
#define malloc(size) _mock_test_malloc(size, __FILE__, __LINE__)
#define free(ptr) _test_free(ptr, __FILE__, __LINE__)
#endif

#define SUCCESS 1
#define FAILURE 0

// #define DEBUG_SVD_MATRICES 1

#ifdef DEBUG_SVD_MATRICES
static void set_matrix(matrix * X, char symbol[], char name[], char units[], int m, int n, double * x)
{
    memset(X, 0, sizeof(matrix));
    X->m = m;
    X->n = n;
    X->type = DENSE;
    X->symmetric = false;
    X->scale = 1.0;
    X->symbol = &(symbol[0]);
    X->name = &(name[0]);
    X->units = &(units[0]);
    X->x.dense = x;
}
#endif

/*
static int svd(matrix * J, matrix * U, matrix * sv, matrix * Vt)
{
    assert(J != NULL);
    const int m = J->m;
    const int n = J->n;
    return m*n;
}
*/

/* max/min: watch out for double evaluation of arguments e.g. a++ == a+=2 */
#define min(a,b) (((a) < (b)) ? (a) : (b) )
#define max(a,b) (((a) > (b)) ? (a) : (b) )
/* dense matrix indexing */
#define colmaj(i,j,ld) ((i) + (j)*(ld)) /* column major indexing */

/* solve Δx=(JᵀJ+λ²I)⁻¹Jb for b=b₂-b₁
 * where J=UΣVᵀ (by the SVD)
 * and
 * Δx=VΣ⁺Uᵀb=VϕΣ⁻¹Uᵀb with Σ⁺=(Σ²+λ²I)⁻¹Σ or ϕₖ=σₖ²/(σₖ²+λ²)
 * for filter factor ϕ
 */
#define phi(s,hp) ((s*s)/((s*s)+(hp*hp)))
static int inv_solve_svd(model * const mdl, double const * b, double * x)
{
    int ret = FAILURE;
    assert(mdl != NULL);
    assert(b != NULL);
    assert(x != NULL);
    const int m = mdl->n_stimmeas;
    const int n = mdl->n_params[0];
    /* init J */
    // printf("J is %d x %d = %d --> %d B\n", m, n, m * n, m * n * (int) sizeof(double));
    double * Ux = NULL, * sx = NULL, * Vtx = NULL, * superb = NULL;
    double * Jx = malloc(sizeof(double) * m * n);
    if(Jx == NULL) {
        printf("error: %s: out of memory\n", "jacobian");
        return FAILURE;
    }
    if(!calc_jacobian(mdl, &Jx[0])) {
        printf("error: inv_solve/calc_jacobian failed\n");
        free(Jx);
        return FAILURE;
    }
#ifdef DEBUG_SVD_MATRICES
    matrix J;
    set_matrix(&J, "J", "Jacobian", "Vm/S", m, n, &Jx[0]);
    printf_matrix(&J);
#endif
    /* calculate SVD of J; J is destroyed in the process */
    /* SVD config */
    const int layout = LAPACK_COL_MAJOR;
    /* A= m cols of U --> u; S= min(m,n) cols U --> u; O=min(m,n) cols U --> a (overwrite); 'N'=no U */
    const char jobu = 'A';
    const char jobvt = 'A';
    const int ldJ = (layout == LAPACK_ROW_MAJOR) ? n : m;
    const int ldU = ((jobu == 'S') && (layout == LAPACK_ROW_MAJOR)) ? min(m, n) : m;
    const int ldVt = ((jobu == 'S') && (layout == LAPACK_COL_MAJOR)) ? min(m, n) : n;
    /* SVD outputs */
    const int Usz = ((jobu == 'S') && (layout == LAPACK_COL_MAJOR)) ? ldU * min(m, n) : ldU * m;
    const int ssz = min(m, n);
    const int Vtsz = ((jobu == 'S') && (layout == LAPACK_COL_MAJOR)) ? ldVt * min(m, n) : ldVt * n;
    const int sbsz = min(m, n) - 1;
    Ux = malloc(sizeof(double) * Usz);
    sx = malloc(sizeof(double) * ssz);
    Vtx = malloc(sizeof(double) * Vtsz);
    superb = malloc(sizeof(double) * sbsz);
    assert(Ux != NULL);
    assert(sx != NULL);
    assert(Vtx != NULL);
    assert(superb != NULL);
    int info = LAPACKE_dgesvd(layout, jobu, jobvt,
                              m, n, Jx, ldJ,
                              sx,
                              Ux, ldU,
                              Vtx, ldVt,
                              superb);
    //printf("LAPACKE_dgesvd info = %d\n", info);
    assert(info == 0);
    free(Jx);
    free(superb);
#if DEBUG_SVD_MATRICES
    matrix U, s, Vt;
    set_matrix(&U, "U", "left singular vectors", NULL, m, Usz / ldU, &Ux[0]);
    set_matrix(&s, "Σ", "singular values", NULL, ssz, 1, &sx[0]);
    set_matrix(&Vt, "Vᵀ", "right singular vectors", NULL, n, Vtsz / ldVt, &Vtx[0]);
    printf_matrix(&U);
    printf_matrix(&s);
    printf_matrix(&Vt);
#endif
    /* calculate Utb = Uᵀb */
    double Utb[ssz];
    memset(Utb, 0, sizeof(double)*ssz);
    for(int i = 0; i < m; i++) { /* i: b row = Uᵀ col = U row */
        for(int j = 0; j < ssz; j++) { /* j: Uᵀb row = Uᵀ row = U col */
            Utb[j] += Ux[colmaj(i, j, ldU)] * b[i];
        }
    }
#if DEBUG_SVD_MATRICES
    matrix Utbm;
    set_matrix(&Utbm, "Uᵀb", NULL, NULL, ssz, 1, &Utb[0]);
    printf_matrix(&Utbm);
#endif
    /* calculate Utb = ϕΣ⁻¹(Uᵀb) */
    const double hp = mdl->hp;
    for(int j = 0; j < ssz; j++) {
        Utb[j] = (phi(sx[j], hp) * Utb[j]) / (sx[j] + DBL_EPSILON); /* filtered Uᵀb */
    }
#ifdef DEBUG_SVD_MATRICES
    for(int j = 0; j < ssz; j++) {
        printf("ϕ[%d]=%8g Utb[%d]=%8g\n", j, phi(sx[j], hp), j, Utb[j]);
    }
#endif
    /* complete the solution as Δx=V(ϕΣ⁻¹Uᵀb) */
    memset(x, 0, sizeof(double) * n);
    for(int i = 0; i < ssz; i++) { /* i: Uᵀb row = V col = Vᵀ row */
        const double Utbi = Utb[i];
        for(int j = 0; j < n; j++) { /* j: x row = V row = Vᵀ col */
            x[j] += Vtx[colmaj(i, j, ldVt)] * Utbi;
        }
    }
#ifdef DEBUG_SVD_MATRICES
    matrix xm;
    set_matrix(&xm, "x", NULL, NULL, n, 1, &x[0]);
    printf_matrix(&xm);
#endif
    free(Ux);
    free(sx);
    free(Vtx);
    ret = SUCCESS;
    return ret;
}

static int inv_solve_diff(model * const mdl, double const * b1, double const * b2, double * x)
{
    assert(mdl != NULL);
    assert(b1 != NULL);
    assert(b2 != NULL);
    const int m = mdl->n_stimmeas;
    double b[m];
    for(int i = 0; i < m; i++) {
        b[i] = b2[i] - b1[i];
    }
#ifdef DEBUG_SVD_MATRICES
    printf("b = ");
    for(int i = 0; i < m; i++) {
        printf(" %g", b[i]);
    }
    printf("\n");
#endif
    return inv_solve_svd(mdl, &b[0], x);
}

int inv_solve(model * const mdl, double * x)
{
    assert(mdl != NULL);
    assert(x != NULL);
    const int m = mdl->n_stimmeas;
    const int n = mdl->n_params[0];
    for(int frame_idx = 1; frame_idx < mdl->n_data[1]; frame_idx++) {
        int ret = inv_solve_diff(mdl, &(mdl->data[0]), &(mdl->data[frame_idx * m]), &x[(frame_idx - 1) * n]);
        if(!ret) {
            return FAILURE;
        }
        if(mdl->n_params[1] > 1) {
            break;
        }
    }
    // printf_model(mdl);
    return SUCCESS;
}


#ifndef GBPINTERPOLATE_AWAKE
#define GBPINTERPOLATE_AWAKE
#include <gbpLib.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_integration.h>

typedef struct interp_struct interp_info;
struct interp_struct {
    gsl_interp *           interp;
    gsl_interp_accel *     accel;
    size_t                 n;
    const gsl_interp_type *T;
    double *               x;
    double *               y;
};

void   free_interpolate(void **interp, void *params);
void   init_interpolate(double *x, double *y, size_t n, const gsl_interp_type *T, interp_info **interp);
void   ADaPS_store_interp(ADaPS **list, void *data, const char *name, ...);
double interpolate(interp_info *interp, double x);
double interpolate_derivative(interp_info *interp, double x);
double interpolate_integral(interp_info *interp, double x_lo, double x_hi);
double interpolate_maximum_function(double x, void *params);
void   interpolate_maximum(interp_info *interp,
                           double       x_lo_in,
                           double       x_guess_in,
                           double       x_hi_in,
                           double       threshold,
                           double *     x_maxima,
                           double *     y_maxima);
double interpolate_minimum_function(double x, void *params);
void   interpolate_minimum(interp_info *interp,
                           double       x_lo_in,
                           double       x_guess_in,
                           double       x_hi_in,
                           double       threshold,
                           double *     x_minima,
                           double *     y_minima);

#endif


/*****************************************************************\
           __
          / /
		 / /                     __  __
		/ /______    _______    / / / / ________   __       __
	   / ______  \  /_____  \  / / / / / _____  | / /      / /
	  / /      | / _______| / / / / / / /____/ / / /      / /
	 / /      / / / _____  / / / / / / _______/ / /      / /
	/ /      / / / /____/ / / / / / / |______  / |______/ /
   /_/      /_/ |________/ / / / /  \_______/  \_______  /
                          /_/ /_/                     / /
			                                         / /
		       High Level Game Framework            /_/

  ---------------------------------------------------------------

  Copyright (c) 2007-2011 - Rodrigo Braz Monteiro.
  This file is subject to the terms of halley_license.txt.

\*****************************************************************/

#pragma once

#include <cmath>
#include <iostream>
#include "angle.h"
#include <halley/utils/utils.h>
#include <gsl/gsl_assert>

namespace Halley {
	//////////////////////////////
	// Vector4D class declaration
	template <typename T=float>
	class Vector4D {
	private:
		T mod(T a, T b) const { return a % b; }

	public:
		T x, y, z, w;

		// Constructors
		Vector4D ()
			: x(0)
			, y(0)
			, z(0)
			, w(0)
		{}

		Vector4D (T x, T y, T z, T w)
			: x(x)
			, y(y)
			, z(z)
			, w(w)
		{}

		template <typename V>
		explicit Vector4D (Vector4D<V> vec)
			: x(T(vec.x))
			, y(T(vec.y))
			, z(T(vec.z))
			, w(T(vec.w))
		{}

		// Getter
		inline T& operator[](size_t n)
		{
			Expects(n <= 3);
			return reinterpret_cast<T*>(this)[n];
		}

		inline T operator[](size_t n) const
		{
			Expects(n <= 3);
			return reinterpret_cast<const T*>(this)[n];
		}

		// Assignment and comparison
		inline Vector4D& operator = (Vector4D param) { x = param.x; y = param.y; z = param.z; w = param.w; return *this; }
		inline Vector4D& operator = (T param) { x = param; y = param; z = param; w = param; return *this; }
		inline bool operator == (Vector4D param) const { return x == param.x && y == param.y && z == param.z && w == param.w; }
		inline bool operator != (Vector4D param) const { return x != param.x || y != param.y || z != param.z || w != param.w; }

		// Basic algebra
		inline Vector4D operator + (Vector4D param) const { return Vector4D(x + param.x, y + param.y, z + param.z, w + param.w); }
		inline Vector4D operator - (Vector4D param) const { return Vector4D(x - param.x, y - param.y, z - param.z, w - param.w); }
		inline Vector4D operator * (Vector4D param) const { return Vector4D(x * param.x, y * param.y, z * param.z, w * param.w); }
		inline Vector4D operator / (Vector4D param) const { return Vector4D(x / param.x, y / param.y, z / param.z, w / param.w); }
		inline Vector4D operator % (Vector4D param) const { return Vector4D(mod(x, param.x), mod(y, param.y), mod(z, param.z), mod(w, param.w)); }

		inline Vector4D modulo(Vector4D param) const { return Vector4D(Halley::modulo<T>(x, param.x), Halley::modulo<T>(y, param.y), Halley::modulo<T>(z, param.z), Halley::modulo<T>(w, param.w)); }
		inline Vector4D floorDiv(Vector4D param) const { return Vector4D(Halley::floorDiv(x, param.x), Halley::floorDiv(y, param.y), Halley::floorDiv(z, param.z), Halley::floorDiv(w, param.w)); }

		inline Vector4D operator - () const { return Vector4D(-x, -y, -z, -w); }

		template <typename V>
		inline Vector4D operator * (V param) const { return Vector4D(T(x * param), T(y * param), T(z * param), T(w * param)); }

		template <typename V>
		inline Vector4D operator / (V param) const { return Vector4D(T(x / param), T(y / param), T(z * param), T(w * param)); }

		// In-place operations
		inline Vector4D& operator += (Vector4D param) { x += param.x; y += param.y; z += param.z; w += param.w; return *this; }
		inline Vector4D& operator -= (Vector4D param) { x -= param.x; y -= param.y; z -= param.z; w -= param.w; return *this; }
		inline Vector4D& operator *= (const T param) { x *= param; y *= param; z *= param; w *= param; return *this; }
		inline Vector4D& operator /= (const T param) { x /= param; y /= param; z /= param; w /= param; return *this; }

		// Get the normalized vector (unit vector)
		inline Vector4D unit () const
		{
			float len = length();
			if (len != 0) {
				return (*this) / len;
			} else {
				return Vector4D();
			}
		}

		inline Vector4D normalized() const
		{
			return unit();
		}

		inline void normalize()
		{
			*this = unit();
		}

		// Dot product
		inline T dot (Vector4D param) const { return (x * param.x) + (y * param.y) + (z * param.z) + (w * param.w); }

		// Length
		inline T length () const { return sqrt(squaredLength()); }
		inline T len () const { return length(); }

		// Squared length, often useful and much faster
		inline T squaredLength () const { return x*x + y*y + z*z + w*w; }

		// Floor
		inline Vector4D floor() const { return Vector4D(floor(x), floor(y), floor(z), floor(w)); }
		inline Vector4D ceil() const { return Vector4D(ceil(x), ceil(y), ceil(z), ceil(w)); }
	};

	////////////////////
	// Global operators
	template <typename T, typename V>
	inline Vector4D<T> operator * (V f, const Vector4D<T> &v)
	{
		return Vector4D<T>(T(v.x * f), T(v.y * f), T(v.z * f), T(v.w * f));
	}

	template <typename T >
	std::ostream& operator<< (std::ostream& ostream, const Vector4D<T>& v)
	{
		ostream << "(" << v.x << "," << v.y << "," << v.z << "," << v.w << ")" ; return ostream;
	}

	////////////
	// Typedefs
	using Vector4d = Vector4D<double>;
	using Vector4f = Vector4D<float>;
	using Vector4i = Vector4D<int>;
	using Vector4s = Vector4D<short>;
	using Vector4c = Vector4D<char>;
}


#include <math.h>
#include <gsl/gsl_nan.h>

#include "csm_all.h"

#include <egsl/egsl_macros.h>

void find_neighbours(LDP ld, int i, int max_num, int*indexes, size_t*num_found);
void filter_orientation(double theta0, double rho0, size_t n,
 	const double*thetas, const double*rhos, double *alpha, double*cov0_alpha );

/** Requires the "cluster" field to be set */
void ld_compute_orientation(LDP ld, int size_neighbourhood, double sigma) {
	int i;
	for(i=0;i<ld->nrays;i++){
		if(!ld_valid_ray(ld,i) || (ld->cluster[i] == -1)) {
			ld->alpha[i] = GSL_NAN;
			ld->cov_alpha[i] = GSL_NAN;
			ld->alpha_valid[i] = 0;
			continue;
		}

		int neighbours[size_neighbourhood*2];
		size_t num_neighbours;
		find_neighbours(ld, i, size_neighbourhood, neighbours, &num_neighbours);

		if(0==num_neighbours) {
			ld->alpha[i] = GSL_NAN;
			ld->cov_alpha[i] = GSL_NAN;
			ld->alpha_valid[i] = 0;
			continue;
		}

/*		printf("orientation for i=%d:\n",i); */
		double thetas[num_neighbours];
		double readings[num_neighbours];
		size_t a=0;
		for(a=0;a<num_neighbours;a++) {
			thetas[a] = ld->theta[neighbours[a]];
			readings[a] = ld->readings[neighbours[a]];
			/* printf(" j = %d theta = %f rho = %f\n", neighbours[a], thetas[a],readings[a]); */
		}

		double alpha=42, cov0_alpha=32;
		filter_orientation(ld->theta[i],ld->readings[i],num_neighbours,
			thetas,readings,&alpha,&cov0_alpha);

		if(gsl_isnan(alpha)) {
			ld->alpha[i] = GSL_NAN;
			ld->cov_alpha[i] = GSL_NAN;
			ld->alpha_valid[i] = 0;
		} else {
			ld->alpha[i] = alpha;
			ld->cov_alpha[i] = cov0_alpha * square(sigma);
			ld->alpha_valid[i] = 1;
		}
		/* printf("---------- i = %d alpha = %f sigma=%f cov_alpha = %f\n", i, alpha, ld->cov_alpha[i]);*/
	}
}

/** A very cool algorithm for finding the orientation */

void filter_orientation(double theta0, double rho0, size_t n,
 	const double*thetas, const double*rhos, double *alpha, double*cov0_alpha ) {

	egsl_push();
	/* Y = L x + R epsilon */
	val Y = zeros(n,1);
	val L = ones(n,1);
	val R = zeros(n,n+1);

 	size_t i; for(i=0;i<n;i++) {
		*egsl_atmp(Y, i, 0)   = (rhos[i]-rho0)/(thetas[i]-theta0);
		*egsl_atmp(R, i, 0)   =             -1/(thetas[i]-theta0);
		*egsl_atmp(R, i, i+1) =             +1/(thetas[i]-theta0);
	}

	val eRinv = inv(m(R, tr(R)));
	val vcov_f1 = inv(m3(tr(L),eRinv, L));
	val vf1 =   m4(vcov_f1, tr(L), eRinv, Y);

	double cov_f1 = *egsl_atmp(vcov_f1,0,0);
	double f1 = *egsl_atmp(vf1,0,0);

	*alpha = theta0 - atan(f1/rho0);

	if(cos(*alpha)*cos(theta0)+sin(*alpha)*sin(theta0)>0)
		*alpha = *alpha + M_PI;

	double dalpha_df1  = rho0 / (square(rho0) + square(f1));
	double dalpha_drho = -f1 /  (square(rho0) + square(f1));

	*cov0_alpha	= square(dalpha_df1) * cov_f1 + square(dalpha_drho);

	if(gsl_isnan(*alpha)) {
		egsl_print("Y",Y);
		egsl_print("L",L);
		egsl_print("R",R);
		egsl_print("eRinv",eRinv);
		egsl_print("vcov_f1",vcov_f1);

		printf("   f1 = %f cov =%f \n", f1,cov_f1);
		printf("   f1/rho = %f \n", f1/rho0);
		printf("   atan = %f \n", atan(f1/rho0));
		printf("   theta0= %f \n", theta0);
	}

	egsl_pop();
/*
//	printf("dalpha_df1 = %f dalpha_drho = %f\n",dalpha_df1,dalpha_drho);
//	printf("f1 = %f covf1 = %f alpha = %f cov_alpha = %f\n ",f1,cov_f1,*alpha,*cov0_alpha);
//	printf("sotto = %f\n ",(square(rho0) + square(f1)));

//	printf("   alpha = %f sigma= %f°\n", *alpha, rad2deg(0.01*sqrt(*cov0_alpha)));

	printf("l= ");
	gsl_matrix_fprintf(stdout, l, "%f");
	printf("\ny= ");
	gsl_matrix_fprintf(stdout, y, "%f");
	printf("\nr= ");
	gsl_matrix_fprintf(stdout, r, "%f");
	printf("\ninv(r*r)= ");
	gsl_matrix_fprintf(stdout, Rinv, "%f");
	printf("\nf1 = %lf ",f1);
	printf("\ncov_f1 = %lf ",cov_f1);
*/
}

/* indexes: an array of size "max_num*2" */
void find_neighbours(LDP ld, int i, int max_num, int*indexes, size_t*num_found) {
	*num_found = 0;

	int up = i;
	while ((up+1 <= i+max_num) && (up+1<ld->nrays) && ld_valid_ray(ld,up+1)
			&& (ld->cluster[up+1] == ld->cluster[i])) {
		up+=1;
		indexes[(*num_found)++] = up;
	}
	int down = i;
	while ((down >= i-max_num) && (down-1>=0) && ld_valid_ray(ld,down-1) &&
			(ld->cluster[down-1] == ld->cluster[i])) {
		down-=1;
		indexes[(*num_found)++] = down;
	}
}


/*
   NAME:
      bovy_randvec
   PURPOSE:
      returns a (uniform) random vector with a maximum length
   CALLING SEQUENCE:
      bovy_randvec(gsl_vector * eps, int d, double length)
   INPUT:
      d      - dimension of the vector
      length - maximum length of the random vector
   OUTPUT:
      eps    - random vector
   REVISION HISTORY:
      2008-09-21
*/
#include <math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_rng.h>
#include <proj_gauss_mixtures.h>

void bovy_randvec(gsl_vector * eps, int d, double length){
  length /= sqrt((double)d);
  int dd;
  for (dd = 0; dd != d; ++dd)
    gsl_vector_set(eps,dd,(2.*gsl_rng_uniform(randgen)-1.)*length);

  return;
}


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

int
main (void)
{
  const gsl_rng_type * T;
  gsl_rng * r;

  int i, n = 10;

  gsl_rng_env_setup();

  T = gsl_rng_default;
  r = gsl_rng_alloc (T);

  for (i = 0; i < n; i++)
    {
      double u = gsl_rng_uniform (r);
      printf ("%.5f\n", u);
    }

  gsl_rng_free (r);

  return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_histogram.h>

#include "allvars.h"
#include "proto.h"

void get_cov_matrix(double *theta, int nstep, int ntheta)
{
  int i, j;
  double cov;

  for(i=0; i<ntheta; i++)
  {
    cov = gsl_stats_covariance(&theta[i*nstep], 1, &theta[i*nstep], 1, nstep);
    cov = cov>1.0e-6 ? cov : 1.0e-6;
    cov_matrix[i*ntheta + i] = cov;
    for(j=0; j<i; j++)
    {
      cov = gsl_stats_covariance(&theta[i*nstep], 1, &theta[j*nstep], 1, nstep);
      cov = cov>0.0001?cov:0.0;
      cov_matrix[i*ntheta + j] = cov_matrix[j*ntheta + i] = cov;
    }
  }
}
void get_cov_matrix_diag(double *theta, int nstep, int ntheta)
{
  int i, j;
  double cov;

  for(i=0; i<ntheta; i++)
  {
    cov = gsl_stats_covariance(&theta[i*nstep], 1, &theta[i*nstep], 1, nstep);
    cov = cov>1.0e-6 ? cov : 1.0e-6;
    cov_matrix[i*ntheta + i] = cov;
    for(j=0; j<i; j++)
    {
      cov_matrix[i*ntheta + j] = cov_matrix[j*ntheta + i] = 0.0;
    }
  }
}

void mcmc_stats(char * fname)
{
  FILE *fp;
  double **theta, tmp, err_max1, err_max2, theta_up, theta_low;
  int i, nstep, istep;

  theta = malloc(ntheta*sizeof(double));
  for(i=0; i<ntheta; i++)
  {
    theta[i] = malloc((nmcmc)*sizeof(double));
  }

  fp = fopen(fname, "r");
  if(fp==NULL)
  {
    strcpy(str_error_exit, fname);
    error_exit(2);
  }

  istep = 0;
  nstep = 0;
  while(!feof(fp) && nstep < nmcmc)
  {
    fscanf(fp, "%d", &istep);
    for(i=0; i<ntheta; i++)
    {
      fscanf(fp, "%lf", &theta[i][nstep]);
    }
    fscanf(fp,"%lf\n", &tmp);
    nstep++;
  }
  printf("nstep: %d\n", nstep);
  if(nstep <= nbuilt)
  {
    strcpy(str_error_exit, "mcmc.txt");
    error_exit(10);
  }

  printf("Sts: ID    par   err1   err2\n");
  for(i=0; i<ntheta; i++)
  {
    gsl_sort(&theta[i][nbuilt-1], 1, nstep-nbuilt);
    //printf("ss:%f %f %f\n", theta[i*n_mcmc + 40000], theta[i*n_mcmc+ 40100], theta[i*n_mcmc+ 40200]);
    theta_best[i] = gsl_stats_mean(&theta[i][nbuilt-1], 1, nstep-nbuilt);
    //theta_best[i] = gsl_stats_quantile_from_sorted_data(&theta[i][nbuilt-1], 1, nstep-nbuilt, 0.500);
    theta_low = gsl_stats_quantile_from_sorted_data(&theta[i][nbuilt-1], 1, nstep-nbuilt, 0.1585);
    theta_up = gsl_stats_quantile_from_sorted_data(&theta[i][nbuilt-1], 1, nstep-nbuilt, 1.0-0.1585);

    err_max1 = (theta_best[i] - theta_range[i][0])/2.35;
    err_max2 = (theta_range[i][1] - theta_best[i])/2.35;

    theta_best_var[i*2] = max(min( theta_best[i] - theta_low, err_max1 ), 0.0);
    theta_best_var[i*2+1] = max( min( theta_up - theta_best[i], err_max2 ), 0.0);

    printf("Sts: %d %f %f %f\n", i, theta_best[i], theta_best_var[i*2], theta_best_var[i*2+1]);

  }

  fclose(fp);
  return;
}


#pragma once

#include <assert.h>
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

#include "defines.h"
#include "matrix_vector_ops.h"

#define DEFAULT_DIFF_ERROR_TOLERANCE 5e-5
#define DEFAULT_DIFF_STEPSIZE 1e-2

#define MAX_DIFF_CORRECTION_ATTEMPTS 100

#define RUNGE_KUTTA_ORDER 5

typedef int (*DynamicsFunction) (gsl_vector *dy,
                                 double t,
                                 const gsl_vector *y,
                                 const gsl_vector *u);

// Runge-Kutte-Dormand-Prince embedded method modified for controlled systems
//TODO implement adaptive step size
int rungeKutteStep(DynamicsFunction dyn,
                   double t,
                   gsl_vector *y_next,
                   gsl_vector *rk_e_next,
                   const VehicleState *vehicle,
                   const Controller *controller,
                   double h);

int rungeKutteAdaptiveStep(DynamicsFunction dyn,
                           double t,
                           gsl_vector *y_next,
                           gsl_vector *rk_e_next,
                           const VehicleState *vehicle,
                           const Controller *controller,
                           double *stepSize,
                           double tolerance);

//TODO implement implicit RK method as well for stiff equations


static char help[] = "Solves the Liouville-Bratu reaction-diffusion problem in 1D.  Option prefix lb_.  Solves\n"
"    - u'' - lambda e^u = 0\n"
"on [0,1] subject to homogeneous Dirichlet boundary conditions.  Optionally uses manufactured solution to problem with f(x) on right-hand-side.\n\n";

#include <petsc.h>

typedef struct {
    PetscBool  manufactured;
    PetscReal  lambda;
} AppCtx;

extern PetscErrorCode Exact(DM, DMDALocalInfo*, Vec, AppCtx*);
extern PetscErrorCode FormFunctionLocal(DMDALocalInfo*, PetscReal*,
                                 PetscReal*, AppCtx*);
extern PetscErrorCode FormJacobianLocal(DMDALocalInfo*, PetscReal*,
                                 Mat, Mat, AppCtx*);

int main(int argc,char **args) {
  PetscErrorCode ierr;
  DM                  da;
  SNES                snes;
  AppCtx              user;
  Vec                 u, uexact;
  PetscReal           unorm, errnorm;
  DMDALocalInfo       info;

  PetscInitialize(&argc,&args,(char*)0,help);
  user.lambda = 1.0;
  user.manufactured = PETSC_FALSE;
  ierr = PetscOptionsBegin(PETSC_COMM_WORLD,
                           "lb_","options for bratu1D",""); CHKERRQ(ierr);
  ierr = PetscOptionsReal("-lambda","coefficient of nonlinear zeroth-order term",
                          "bratu1D.c",user.lambda,&(user.lambda),NULL); CHKERRQ(ierr);
  ierr = PetscOptionsBool("-manu","if set, use a manufactured solution",
                          "bratu1D.c",user.manufactured,&(user.manufactured),NULL); CHKERRQ(ierr);
  ierr = PetscOptionsEnd(); CHKERRQ(ierr);

  ierr = DMDACreate1d(PETSC_COMM_WORLD,DM_BOUNDARY_NONE,9,1,1,NULL,&da); CHKERRQ(ierr);
  ierr = DMSetFromOptions(da); CHKERRQ(ierr);
  ierr = DMSetUp(da); CHKERRQ(ierr);
  ierr = DMDASetUniformCoordinates(da,0.0,1.0,-1.0,-1.0,-1.0,-1.0); CHKERRQ(ierr);
  ierr = DMSetApplicationContext(da,&user); CHKERRQ(ierr);

  ierr = SNESCreate(PETSC_COMM_WORLD,&snes); CHKERRQ(ierr);
  ierr = SNESSetDM(snes,da); CHKERRQ(ierr);
  ierr = DMDASNESSetFunctionLocal(da,INSERT_VALUES,
             (DMDASNESFunction)FormFunctionLocal,&user); CHKERRQ(ierr);
  ierr = DMDASNESSetJacobianLocal(da,
             (DMDASNESJacobian)FormJacobianLocal,&user); CHKERRQ(ierr);
  ierr = SNESSetFromOptions(snes); CHKERRQ(ierr);

  ierr = DMCreateGlobalVector(da,&u); CHKERRQ(ierr);
  ierr = VecSet(u,0.0); CHKERRQ(ierr);
  ierr = SNESSolve(snes,NULL,u); CHKERRQ(ierr);

  ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
  if (user.manufactured) {
      ierr = VecDuplicate(u,&uexact); CHKERRQ(ierr);
      ierr = Exact(da,&info,uexact,&user); CHKERRQ(ierr);
      ierr = VecNorm(u,NORM_INFINITY,&unorm); CHKERRQ(ierr);
      ierr = VecAXPY(u,-1.0,uexact); CHKERRQ(ierr);    // u <- u + (-1.0) uxact
      ierr = VecNorm(u,NORM_INFINITY,&errnorm); CHKERRQ(ierr);
      ierr = PetscPrintf(PETSC_COMM_WORLD,
              "on %d point grid:  |u-u_exact|_inf/|u|_inf = %g\n",
              info.mx,errnorm/unorm); CHKERRQ(ierr);
      VecDestroy(&uexact);
  } else {
      ierr = PetscPrintf(PETSC_COMM_WORLD,"done on %d point grid\n",info.mx); CHKERRQ(ierr);
  }

  VecDestroy(&u);  SNESDestroy(&snes);  DMDestroy(&da);
  return PetscFinalize();
}

PetscErrorCode Exact(DM da, DMDALocalInfo *info, Vec uex, AppCtx *user) {
    PetscErrorCode ierr;
    PetscInt   i;
    PetscReal  h = 1.0 / (info->mx-1), x, *auex;
    ierr = DMDAVecGetArray(da,uex,&auex); CHKERRQ(ierr);
    for (i=info->xs; i<info->xs+info->xm; i++) {
        x = i * h;
        auex[i] = sin(PETSC_PI * x);
    }
    ierr = DMDAVecRestoreArray(da,uex,&auex); CHKERRQ(ierr);
    return 0;
}

PetscErrorCode FormFunctionLocal(DMDALocalInfo *info, PetscReal *u,
                                 PetscReal *f, AppCtx *user) {
    PetscInt   i;
    PetscReal  h = 1.0 / (info->mx-1), x, R, compf, uex;
    for (i=info->xs; i<info->xs+info->xm; i++) {
        if ((i == 0) || (i == info->mx-1)) {
            f[i] = u[i];
        } else {  // interior location
            R = user->lambda * PetscExpReal(u[i]);
            f[i] = - u[i+1] + 2.0 * u[i] - u[i-1] - h*h * R;
            if (user->manufactured) {
                x = i * h;
                uex = sin(PETSC_PI * x);
                compf = PETSC_PI * PETSC_PI * uex - user->lambda * PetscExpReal(uex);
                f[i] -= h * h * compf;
            }
        }
    }
    return 0;
}

PetscErrorCode FormJacobianLocal(DMDALocalInfo *info, PetscReal *u,
                                 Mat J, Mat P, AppCtx *user) {
    PetscErrorCode ierr;
    PetscInt   i, col[3];
    PetscReal  h = 1.0 / (info->mx-1), dRdu, v[3];
    for (i=info->xs; i<info->xs+info->xm; i++) {
        if ((i == 0) | (i == info->mx-1)) {
            v[0] = 1.0;
            ierr = MatSetValues(P,1,&i,1,&i,v,INSERT_VALUES); CHKERRQ(ierr);
        } else {
            dRdu = user->lambda * PetscExpReal(u[i]);
            col[0] = i-1;  v[0] = - 1.0;
            col[1] = i;    v[1] = 2.0 - h*h * dRdu;
            col[2] = i+1;  v[2] = - 1.0;
            ierr = MatSetValues(P,1,&i,3,col,v,INSERT_VALUES); CHKERRQ(ierr);
        }
    }
    ierr = MatAssemblyBegin(P,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    ierr = MatAssemblyEnd(P,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    if (J != P) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <cblas.h>
#include <lapacke.h>
#include <string.h>

void eigs(double *F, double *Es, double *l, int n_eigs, int n_patch)
{
    double *eigenvalue = (double *)malloc(n_patch*sizeof(double));
    lapack_int n = n_patch;
    lapack_int info;

    printf("n_patch = %d\n", n);
    info = LAPACKE_dsyevd(LAPACK_COL_MAJOR, 'V', 'L', n, l, n, eigenvalue);
    printf("info = %d\n", info);
    memcpy(F, l, n_eigs*n_patch*sizeof(double));
    memcpy(Es, eigenvalue, n_eigs*sizeof(double));
    free(eigenvalue);
}


#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>

int normalizeVector( gsl_vector *vector )
{
	double min = gsl_vector_min( vector );
	double norm = gsl_vector_max( vector ) - min;
	gsl_vector_add_constant( vector, -min );
	gsl_vector_scale( vector, 1/norm );
	return 0;
}