#include <gsl/gsl_spline.h>
#include <galpy_potentials.h>
// interpSphericalPotential: 6 arguments: amp (not used here), rmin, rmax,
// M(<rmax), Phi0, Phimax
double interpSphericalPotentialrevaluate(double r,double t,
					 struct potentialArg * potentialArgs){
  double * args= potentialArgs->args;
  //Get args
  double rmin= *(args+1);
  double rmax= *(args+2);
  double Mmax= *(args+3);
  double Phi0= *(args+4);
  double Phimax= *(args+5);
  if ( r >= rmax ) {
    return -Mmax/r+Phimax;
  }
  else {
    return r < rmin ? 0. : \
      -gsl_spline_eval_integ(*potentialArgs->spline1d,
			     rmin,r,*potentialArgs->acc1d) + Phi0;
  }
}
double interpSphericalPotentialrforce(double r,double t,
				      struct potentialArg * potentialArgs){
  double * args= potentialArgs->args;
  //Get args
  double rmin= *(args+1);
  double rmax= *(args+2);
  double Mmax= *(args+3);
  if ( r >= rmax ) {
    return -Mmax/r/r;
  }
  else {
    return r < rmin ? 0. : gsl_spline_eval(*potentialArgs->spline1d,
					   r,*potentialArgs->acc1d);
  }
}
double interpSphericalPotentialr2deriv(double r,double t,
				       struct potentialArg * potentialArgs){
  double * args= potentialArgs->args;
  //Get args
  double rmin= *(args+1);
  double rmax= *(args+2);
  double Mmax= *(args+3);
  if ( r >= rmax ) {
    return -2. * Mmax / r / r / r;
  }
  else {
    return r < rmin ? 0. : -gsl_spline_eval_deriv(*potentialArgs->spline1d,
						  r,*potentialArgs->acc1d);
  }
}
double interpSphericalPotentialrdens(double r,double t,
				     struct potentialArg * potentialArgs){
  double * args= potentialArgs->args;
  //Get args
  double rmin= *(args+1);
  double rmax= *(args+2);
  if ( r >= rmax ) {
    return 0.;
  }
  else {
    return r < rmin ? 0. : M_1_PI / 4. \
      * ( interpSphericalPotentialr2deriv(r,t,potentialArgs)
	  - 2. * interpSphericalPotentialrforce(r,t,potentialArgs)/r);
  }
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gbpLib.h>
#include <gbpMath.h>
#include <gbpCosmo_core.h>
#include <gbpCosmo_NFW_etc.h>
#include <gsl/gsl_sf_expint.h>

void init_Vmax_to_Mvir_NFW(cosmo_info **cosmo, int mode, double z) {
    interp_info *interp;
    if(!ADaPS_exist((*cosmo), "lVmax_to_lMvir_%.5f_interp", z)) {
        SID_log("Initializing Vmax->M_vir interpolation...", SID_LOG_OPEN);
        int     n_k;
        double *lk_P;
        double *lM;
        double *lVmax;
        int     n_M   = 201;
        double  lM_lo = 0.;
        double  lM_hi = 20.;
        double  dlM   = (lM_hi - lM_lo) / (double)(n_M - 1);
        lM            = (double *)SID_malloc(sizeof(double) * n_M);
        lVmax         = (double *)SID_malloc(sizeof(double) * n_M);
        int i_M;
        for(i_M = 0; i_M < n_M; i_M++) {
            if(i_M == 0)
                lM[i_M] = lM_lo;
            else if(i_M == (n_M - 1))
                lM[i_M] = lM_hi;
            else
                lM[i_M] = lM[i_M - 1] + dlM;
        }
        for(i_M = 0; i_M < n_M; i_M++) {
            lM[i_M] += take_log10(M_SOL);
            lVmax[i_M] = take_log10(V_max_NFW(take_alog10(lM[i_M]), z, mode, cosmo));
        }
        init_interpolate(lVmax, lM, (size_t)n_M, gsl_interp_cspline, &interp);
        ADaPS_store_interp(cosmo, (void *)(interp), "lVmax_to_lMvir_%.5f_interp", z);
        SID_free(SID_FARG lM);
        SID_free(SID_FARG lVmax);
        SID_log("Done.", SID_LOG_CLOSE);
    }
}
double Vmax_to_Mvir_NFW(double V_max, double z, int mode, cosmo_info **cosmo) {
    double c_vir;
    double R_vir;
    double V2_vir;
    double g_c;
    double r_val = 0.;
    if(V_max > 0.) {
        interp_info *interp;
        init_Vmax_to_Mvir_NFW(cosmo, mode, z);
        interp = (interp_info *)ADaPS_fetch(*cosmo, "lVmax_to_lMvir_%.5f_interp", z);
        r_val  = take_alog10(interpolate(interp, take_log10(V_max)));
    }
    return (r_val);
}


#include <stdio.h>
#include <math.h>
#include <sys/types.h>
#include <time.h>
#include <complex.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_sf_gamma.h>

#define FALSE 0
#define TRUE 1

#ifndef M_PI
#define M_PI            3.141592653589793238462643383280        /* pi */
#endif

#define LN2PI 1.83787706640934548356065947

extern char parameterfilename[50];
extern char parameterlogfilename[50];

#define MBIG 1000000000
#define MSEED 161803398
#define MZ 0
#define FAC (1.0/MBIG)
#define MIN(A,B) ((A) < (B) ? (A) : (B))
#define MAX(A,B) ((A) > (B) ? (A) : (B))
enum {NOT_AVAILABLE=0, ABOVE_DETECTION=1, BELOW_DETECTION=2, MEASURED=3};

typedef struct {
  int n;
  int m;
  int **a;
} Matrix;

void ReadParam( char*, void*, char* ) ;
void ExperimentNumber( char*, char* );
void Error( char* );
void FileError( char*, char* );
double ***doublematrix3( int, int, int );
double **doublematrix( int, int );
int **integermatrix( int, int );
uint **uintegermatrix( int, int );
uint *uintegervector( int );
char **charmatrix( int, int );
double *doublevector( int );
int *integervector( int );
char *charvector( int );

void Int_HeapSort(int*, int);
void HeapSort( double*, int );
void circmeanvar( double*, int, double*, double* );
void meanvar( double*, int, double*, double* );
void meanvarl( long double*, int, long double*, long double* );
void wmeanvar( long double *x, int n, long double *m, long double *s2 );
double mean( double*, int, int );
int choldc_block(double**, long long);

uint   binomialRNG(uint, double, const gsl_rng*);
void   multinomialRNG(uint, const double*, size_t, unsigned int*,
		      const gsl_rng*);
int    poissonRNG(double, const gsl_rng*);
int    uniform_intRNG(int, int, const gsl_rng*);
double normalRNG(double, double, const gsl_rng*);
double betaRNG(double, double, const gsl_rng*);
double chisqRNG(double, const gsl_rng*);
double scaleinvchisqRNG(double, double, const gsl_rng*);
double invgammaRNG(double, double, const gsl_rng*);
double gammaRNG(double, double, const gsl_rng*);
double uniformRNG(double, double, const gsl_rng*);
double paretoRNG(double, double, const gsl_rng*);
void   dirichlet_multinomialRNG(uint, const double*, size_t, uint*,
				const gsl_rng*);
void   dirichletRNG(const double*, size_t, double*, const gsl_rng*);
uint   beta_binomialRNG(uint, double, double, const gsl_rng*);
uint   negative_binomialRNG(double, double, const gsl_rng*);
uint   beta_negative_binomialRNG(uint, double, double, const gsl_rng*);
/* Matrix *wishartRNG(int, int, Matrix*, Matrix*, const gsl_rng*); */
/* void   MVNRNG(int, Matrix*, Matrix*, Matrix*, const gsl_rng*); */

double poissonCMF(const double, const double, int);
double gamma_scalePDF(const double, const double, const double);
double betaPDF(const double, const double, const double);
double exponentialPDF(const double, const double);
double poissonPMF(const double, const double);
double geometricPDF(const double, const double);
double trnormalPDF(const double, const double, const double);
double normalCDF(const double, const double, const double, int);
double normalPDF(const double, const double, const double);
double binomialPMF(const double, const double, const double);
double multinomialPMF(const unsigned int, const double*, const double*);
double uniformPDF(const double, const double, const double);
double logisticPDF(const double, const double, const double);
double betabinomialPMF(const double, const double, const double, const double);
/* double MVNPDF(Matrix*, Matrix*, Matrix*, const double, int); */

double identity(double);
double logit(double);
double invlogit(double);

Matrix *MatrixInit(Matrix*, int, int, int*);
Matrix *MatrixDestroy(Matrix*);
/* Matrix *Matrix_Init(Matrix*, char*, int, int); */
/* Matrix *Matrix_Fill(Matrix*, int, int, ...); */
/* Matrix *Matrix_Destroy(Matrix*); */
/* Matrix *Matrix_Copy(Matrix*, Matrix*); */
/* Matrix *Matrix_Sub_Copy(Matrix*, Matrix*, int); */
/* Matrix *Matrix_Scalar_Multiply(Matrix*, double, Matrix*); */
/* Matrix *Matrix_Multiply(Matrix*, Matrix*, Matrix*); */
/* Matrix *Matrix_Multiply(Matrix*, Matrix*, Matrix*); */
/* Matrix *Matrix_Cholesky(Matrix*, Matrix*); */
/* Matrix *Matrix_Invert(Matrix*, Matrix*); */
/* Matrix *Matrix_Add(Matrix*, Matrix*, Matrix*); */
/* Matrix *Matrix_Add_Diagonal(Matrix*, Matrix*); */
/* Matrix *Matrix_Subtract(Matrix*, Matrix*, Matrix*); */
/* Matrix *Matrix_Transpose(Matrix*, Matrix*); */
/* double Matrix_Log_Determinant(Matrix*, int); */
/* void   Matrix_Print(Matrix*); */

double rtnewt(void(*)(double, double*, double*, double*),
	      double, double, double, double*);
double rtbis( double (*)(double, double*), double, double, double, int*,
	      double* );

size_t strlen(const char*);
char *strncpy(char*, const char*, size_t);
char *strcpy(char*, const char*);
char *strcat(char*, const char*);
char *strchr ( const char*, int );
int  strcmp (const char*, const char*);
void *calloc(size_t, size_t);
void *malloc(size_t);
void free(void*);
void exit(int);

double von_mises_cdf ( double x, double a, double b );
double von_mises_cdf_inv ( double cdf, double a, double b );


/*
 # This file is part of the Astrometry.net suite.
 # Licensed under a 3-clause BSD style license - see LICENSE
 */

#include <gsl/gsl_matrix_double.h>
#include <gsl/gsl_vector_double.h>

#include "compat.h"
#include "sip-utils.h"
#include "gslutils.h"
#include "mathutil.h"
#include "sip.h"

double wcs_pixel_center_for_size(double size) {
    return 0.5 + 0.5 * size;
}

int sip_compute_inverse_polynomials(sip_t* sip, int NX, int NY,
                                    double xlo, double xhi,
                                    double ylo, double yhi) {
    int inv_sip_order;
    int M, N;
    int i, j, p, q, gu, gv;
    double maxu, maxv, minu, minv;
    double u, v, U, V;
    gsl_matrix *mA;
    gsl_vector *b1, *b2, *x1, *x2;
    tan_t* tan;

    assert(sip->a_order == sip->b_order);
    assert(sip->ap_order == sip->bp_order);
    tan = &(sip->wcstan);

    logverb("sip_compute-inverse_polynomials: A %i, AP %i\n",
            sip->a_order, sip->ap_order);

    /*
     basic idea: lay down a grid in image, for each gridpoint, push
     through the polynomial to get yourself into warped image
     coordinate (but not yet lifted onto the sky).  Then, using the
     set of warped gridpoints as inputs, fit back to their original
     grid locations as targets.
     */
    inv_sip_order = sip->ap_order;

    // Number of grid points to use:
    if (NX == 0)
        NX = 10 * (inv_sip_order + 1);
    if (NY == 0)
        NY = 10 * (inv_sip_order + 1);
    if (xhi == 0)
        xhi = tan->imagew;
    if (yhi == 0)
        yhi = tan->imageh;

    logverb("NX,NY %i,%i, x range [%f, %f], y range [%f, %f]\n",
            NX,NY, xlo, xhi, ylo, yhi);

    // Number of coefficients to solve for:
    // We only compute the upper triangle polynomial terms
    N = (inv_sip_order + 1) * (inv_sip_order + 2) / 2;

    // Number of samples to fit.
    M = NX * NY;

    mA = gsl_matrix_alloc(M, N);
    b1 = gsl_vector_alloc(M);
    b2 = gsl_vector_alloc(M);
    assert(mA);
    assert(b1);
    assert(b2);

    /*
     *  Rearranging formula (4), (5), and (6) from the SIP paper gives the
     *  following equations:
     *
     *    +----------------------- Linear pixel coordinates in PIXELS
     *    |                        before SIP correction
     *    |                   +--- Intermediate world coordinates in DEGREES
     *    |                   |
     *    v                   v
     *                   -1
     *    U = [CD11 CD12]   * x
     *    V   [CD21 CD22]     y
     *
     *    +---------------- PIXEL distortion delta from telescope to
     *    |                 linear coordinates
     *    |    +----------- Linear PIXEL coordinates before SIP correction
     *    |    |       +--- Polynomial U,V terms in powers of PIXELS
     *    v    v       v
     *
     *    -f(u1,v1) =  p11 p12 p13 p14 p15 ... * ap1
     *    -f(u2,v2) =  p21 p22 p23 p24 p25 ...   ap2
     *    ...
     *
     *    -g(u1,v1) =  p11 p12 p13 p14 p15 ... * bp1
     *    -g(u2,v2) =  p21 p22 p23 p24 p25 ...   bp2
     *    ...
     *
     *  which recovers the A and B's.
     */

    minu = xlo - tan->crpix[0];
    maxu = xhi - tan->crpix[0];
    minv = ylo - tan->crpix[1];
    maxv = yhi - tan->crpix[1];

    // Sample grid locations.
    i = 0;
    for (gu=0; gu<NX; gu++) {
        for (gv=0; gv<NY; gv++) {
            double fuv, guv;
            // Calculate grid position in original image pixels
            u = (gu * (maxu - minu) / (NX-1)) + minu;
            v = (gv * (maxv - minv) / (NY-1)) + minv;
            // compute U=u+f(u,v) and V=v+g(u,v)
            sip_calc_distortion(sip, u, v, &U, &V);
            fuv = U - u;
            guv = V - v;
            // Polynomial terms...
            j = 0;
            for (p = 0; p <= inv_sip_order; p++)
                for (q = 0; q <= inv_sip_order; q++) {
                    if (p + q > inv_sip_order)
                        continue;
                    assert(j < N);
                    gsl_matrix_set(mA, i, j,
                                   pow(U, (double)p) * pow(V, (double)q));
                    j++;
                }
            assert(j == N);
            gsl_vector_set(b1, i, -fuv);
            gsl_vector_set(b2, i, -guv);
            i++;
        }
    }
    assert(i == M);

    // Solve the linear equation.
    if (gslutils_solve_leastsquares_v(mA, 2, b1, &x1, NULL, b2, &x2, NULL)) {
        ANERROR("Failed to solve SIP inverse matrix equation!");
        return -1;
    }

    // Extract the coefficients
    j = 0;
    for (p = 0; p <= inv_sip_order; p++)
        for (q = 0; q <= inv_sip_order; q++) {
            if ((p + q > inv_sip_order))
                continue;
            assert(j < N);
            sip->ap[p][q] = gsl_vector_get(x1, j);
            sip->bp[p][q] = gsl_vector_get(x2, j);
            j++;
        }
    assert(j == N);

    // Check that we found values that actually invert the polynomial.
    // The error should be particularly small at the grid points.
    if (true) {
        // rms error accumulators:
        double sumdu = 0;
        double sumdv = 0;
        int Z;
        for (gu = 0; gu < NX; gu++) {
            for (gv = 0; gv < NY; gv++) {
                double newu, newv;
                // Calculate grid position in original image pixels
                u = (gu * (maxu - minu) / (NX-1)) + minu;
                v = (gv * (maxv - minv) / (NY-1)) + minv;
                sip_calc_distortion(sip, u, v, &U, &V);
                sip_calc_inv_distortion(sip, U, V, &newu, &newv);
                sumdu += square(u - newu);
                sumdv += square(v - newv);
            }
        }
        sumdu /= (NX*NY);
        sumdv /= (NX*NY);
        debug("RMS error of inverting a distortion (at the grid points, in pixels):\n");
        debug("  du: %g\n", sqrt(sumdu));
        debug("  dv: %g\n", sqrt(sumdu));
        debug("  dist: %g\n", sqrt(sumdu + sumdv));

        sumdu = 0;
        sumdv = 0;
        Z = 1000;
        for (i=0; i<Z; i++) {
            double newu, newv;
            u = uniform_sample(minu, maxu);
            v = uniform_sample(minv, maxv);
            sip_calc_distortion(sip, u, v, &U, &V);
            sip_calc_inv_distortion(sip, U, V, &newu, &newv);
            sumdu += square(u - newu);
            sumdv += square(v - newv);
        }
        sumdu /= Z;
        sumdv /= Z;
        debug("RMS error of inverting a distortion (at random points, in pixels):\n");
        debug("  du: %g\n", sqrt(sumdu));
        debug("  dv: %g\n", sqrt(sumdu));
        debug("  dist: %g\n", sqrt(sumdu + sumdv));
    }

    gsl_matrix_free(mA);
    gsl_vector_free(b1);
    gsl_vector_free(b2);
    gsl_vector_free(x1);
    gsl_vector_free(x2);

    return 0;
}

bool tan_pixel_is_inside_image(const tan_t* wcs, double x, double y) {
    return (x >= 1 && x <= wcs->imagew && y >= 1 && y <= wcs->imageh);
}

bool sip_pixel_is_inside_image(const sip_t* wcs, double x, double y) {
    return tan_pixel_is_inside_image(&(wcs->wcstan), x, y);
}

int* sip_filter_stars_in_field(const sip_t* sip, const tan_t* tan,
                               const double* xyz, const double* radec,
                               int N, double** p_xy, int* inds, int* p_Ngood) {
    int i, Ngood;
    int W, H;
    double* xy = NULL;
    bool allocd = false;

    assert(sip || tan);
    assert(xyz || radec);
    assert(p_Ngood);

    Ngood = 0;
    if (!inds) {
        inds = malloc(N * sizeof(int));
        allocd = true;
    }

    if (p_xy)
        xy = malloc(N * 2 * sizeof(double));

    if (sip) {
        W = (int)sip->wcstan.imagew;
        H = (int)sip->wcstan.imageh;
    } else {
        W = (int)tan->imagew;
        H = (int)tan->imageh;
    }

    for (i=0; i<N; i++) {
        double x, y;
        if (xyz) {
            if (sip) {
                if (!sip_xyzarr2pixelxy(sip, xyz + i*3, &x, &y))
                    continue;
            } else {
                if (!tan_xyzarr2pixelxy(tan, xyz + i*3, &x, &y))
                    continue;
            }
        } else {
            if (sip) {
                if (!sip_radec2pixelxy(sip, radec[i*2], radec[i*2+1], &x, &y))
                    continue;
            } else {
                if (!tan_radec2pixelxy(tan, radec[i*2], radec[i*2+1], &x, &y))
                    continue;
            }
        }
        // FIXME -- check half- and one-pixel FITS issues.
        if ((x < 0) || (y < 0) || (x >= W) || (y >= H))
            continue;

        inds[Ngood] = i;
        if (xy) {
            xy[Ngood * 2 + 0] = x;
            xy[Ngood * 2 + 1] = y;
        }
        Ngood++;
    }

    if (allocd)
        inds = realloc(inds, Ngood * sizeof(int));

    if (xy)
        xy = realloc(xy, Ngood * 2 * sizeof(double));
    if (p_xy)
        *p_xy = xy;

    *p_Ngood = Ngood;

    return inds;
}


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

 }


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


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


#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 <stdlib.h>
#include <string.h>

#include <math.h>
#include <complex.h>
#include <float.h>

#ifdef _MACOSX
#include <Accelerate/Accelerate.h>
#else
#include <cblas.h>
#endif

#include <gsl/gsl_sf_legendre.h>

#include "util.h"
#include "spbessel.h"

/* Use the modified Gram-Schmidt process to compute (in place) the portion of
 * the n-dimensional vector v orthogonal to each of the nv vectors s. The
 * projection * of the vector onto each of the basis vectors is stored in the
 * length-nv array c. */
int cmgs (complex double *v, complex double *c, complex double *s, int n, int nv) {
	int i, j, k;
	complex double *sv, cv;

	for (i = 0, sv = s; i < nv; ++i, sv += n) {
		c[i] = 0;
		k = 0;

		do {
			cv = pardot (sv, v, n);
			c[i] += cv;
#pragma omp parallel for default(shared) private(j)
			for (j = 0; j < n; ++j) v[j] -= cv * sv[j];
		} while (cabs(cv / c[i]) > IMGS_TOL && ++k < IMGS_ITS);

	}

	return n;
}

complex double pardot (complex double *x, complex double *y, int n) {
	complex double dp;

	/* Compute the local portion. */
	cblas_zdotc_sub (n, x, 1, y, 1, &dp);

	return dp;
}

/* The MSE between two vectors. */
double rmserror (complex double *v, complex double *r, int n) {
	double err = 0, errd = 0, e;
	int i;

	for (i = 0; i < n; ++i) {
		e = cabs (v[i] - r[i]);
		err += e * e;
		e = cabs (r[i]);
		errd += e * e;
	}

	return sqrt (err / errd);
}

/* Open a file or die. */
FILE *critopen (char *fname, char *mode) {
	FILE *fptr;

	fptr = fopen (fname, mode);

	if (!fptr) {
		fprintf (stderr, "ERROR: Could not open input file %s.\n", fname);
		exit (EXIT_FAILURE);

		return NULL;
	}

	return fptr;
}

/* Compute the number of harmonics required using the excess bandwidth
 * formula. */
int exband (complex double kr, double ndig) {
	double akr;
	int l;

	akr = creal (kr);
	ndig *= ndig;

	l = ceil (akr + 1.8 * cbrt(ndig * akr));

	return l;
}

/* Computes the Legendre polynomials up to order n for the argument x. The
 * values are stored in the array v. */
int legpoly (int n, double x, double *v) {
	int i;

	/* Don't bother computing anything for order less than zero. */
	if (n < 0) return -1;

	/* Make sure the argument is within [-1,1] as expected. */
	if (fabs(x) > 1.0 + DBL_EPSILON) return -2;

	/* The first function value. */
	v[0] = 1.0;

	/* If the order happens to be zero, there is no going further. */
	if (n < 1) return 0;

	/* The second function value. */
	v[1] = x;

	/* The recursion formula for Legendre polynomials. */
	for (i = 1; i < n - 1; ++i)
		v[i + 1] = ((2 * i + 1) * x * v[i] - i * v[i - 1]) / (i + 1);

	return 0;
}

/* Computes the wave number from the relative sound speed and
 * unitless (dB) attenuation coefficient. */
complex double wavenum (double cr, double alpha) {
	double kr, ki;

	ki = log(10) * alpha / 20;
	kr = 2 * M_PI / cr;

	return kr + I * ki;
}

/* Computes the inverse wave number from the relative sound speed and
 * unitless (dB) attenuation coefficient. */
complex double invwavenum (double cr, double alpha) {
	double kr, ki, mag, csq;

	csq = cr * cr;

	kr = 2 * M_PI * cr;
	ki = log(10) * alpha / 20;
	mag = 4 * M_PI * M_PI + ki * ki * csq;

	return (kr / mag) - I * (ki * csq / mag);
}

/* Copy the spherical harmonic representation from one location to another. */
int copysh (int deg, complex double *out, int ldo, complex double *in, int ldi) {
	int i, j, offo, offi;

	/* Copy the spherical harmonic coefficients into the right place. */
	for (i = 0; i < deg; ++i) {
		offo = i * ldo;
		offi = i * ldi;
		/* Copy the zero-order coefficients. */
		out[offo] = in[offi];

		/* Copy the other coefficients. */
		for (j = 1; j <= i; ++j) {
			out[offo + j] = in[offi + j];
			out[offo + ldo - j] = in[offi + ldi - j];
		}
	}

	return deg;
}

/* Multiply in a radial component to the spherical harmonic. */
int shradial (int deg, complex double *a, int lda, complex double k, double r) {
	complex double kr, *hlkr;
	int i, j, off;

	hlkr = malloc (deg * sizeof(complex double));

	/* Compute the Hankel functions for all degrees. */
	kr = k * r;
	spbesh (hlkr, kr, deg);

	for (i = 0; i < deg; ++i) {
		off = i * lda;

		/* The zero-order coefficients. */
		a[off] *= hlkr[i];

		/* The other coefficients. */
		for (j = 1; j <= i; ++j) {
			a[off + j] *= hlkr[i];
			a[off + lda - j] *= hlkr[i];
		}
	}

	free (hlkr);

	return deg;
}

/* Compute the harmonic coefficients for an incident plane wave. */
int shincident (int deg, complex double *a, int lda,
		complex double mag, double theta, double phi) {
	double cth, *lgvals;
	int l, m, dm1, npm, off;
	long lgi, lgn;
	complex double scale[4] = { 1, -I, -1, I }, cx, fx;

	/* The coefficient has a 4 pi factor that must be included. */
	mag *= 4 * M_PI;

	dm1 = deg - 1;
	cth = cos(theta);

	lgn = gsl_sf_legendre_array_n (dm1);
	lgvals = malloc (lgn * sizeof(double));

	/* Compute the Legendre polynomials of zero order for all degrees. */
	gsl_sf_legendre_array_e (GSL_SF_LEGENDRE_SPHARM, dm1, cth, 1., lgvals);

	/* Compute the zero-order coefficients for all degrees. */
	for (l = 0; l < deg; ++l) {
		lgi = gsl_sf_legendre_array_index(l, 0);
		a[l * lda] += scale[l % 4] * mag * lgvals[lgi];
	}

	for (m = 1; m < deg; ++m) {
		npm = lda - m;

		/* Compute the phi variation. */
		cx = cexp (I * m * phi);

#pragma omp critical(incaug)
		for (l = m; l < deg; ++l) {
			off = l * lda;
			lgi = gsl_sf_legendre_array_index(l, m);
			fx = scale[l % 4] * mag * lgvals[lgi];
			/* The positive-order coefficient. */
			a[off + m] += fx * conj (cx);
			/* The negative-order coefficient. */
			a[off + npm] += fx * cx;
		}
	}

	free (lgvals);

	return deg;
}

static int lgval (double *p, double *dp, double t, int m) {
	double p0 = 1.0, p1 = t;
	int k;

	/* Set values explicitly for low orders. */
	if (m < 1) {
		*p = 1.0;
		*dp = 0.0;
		return m;
	} else if (m < 2) {
		*p = t;
		*dp = 1.0;
		return m;
	}

	/* Otherwise compute the values explicitly. */
	for (k = 1; k < m; ++k) {
		*p = ((2.0 * k + 1.0) * t * p1 - k * p0) / (1.0 + k);
		p0 = p1; p1 = *p;
	}

	*dp = m * (p0 - t * p1) / (1.0 - t * t);

	return m;
}

int gauleg (int m, double *nodes, double *weights) {
	int i, j, nroots = (m + 1) / 2;
	double t, p, dp, dt;
	const int maxit = 100;
	const double tol = 1.0e-14;

#pragma omp parallel for default(shared) private(i,j,t,p,dp,dt)
	for (i = 0; i < nroots; ++i) {
		t = cos (M_PI * (i + 0.75) / (m + 0.5));

		for (j = 0; j < maxit; ++j) {
			/* Compute the value of the Legendre polynomial. */
			lgval (&p, &dp, t, m);

			/* Perform a Newton-Raphson update. */
			dt = -p / dp;
			t += dt;

			/* Break if convergence has been achieved. */
			if (fabs(dt) < tol) break;
		}

		/* Update the nodes and weights. */
		nodes[i] = t;
		nodes[m - i - 1] = -t;

		weights[i] = 2.0 / (1.0 - t * t) / (dp * dp);
		weights[m - i - 1] = weights[i];
	}

	return 0;
}


/*
 * Copyright 2016 Maikel Nadolski
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <iostream>
#include <gsl_assert.h>

#ifndef HMM_ALGORITHM_BACKWARD_H_
#define HMM_ALGORITHM_BACKWARD_H_

namespace maikel { namespace hmm {

template <class I, class J, class T>
    class backward_range_fn {
      public:
        using model       = hidden_markov_model<T>;
        using row_vector  = typename model::row_vector;
        using matrix      = typename model::matrix;
        using symbol_type = typename std::iterator_traits<I>::value_type;

        backward_range_fn() = delete;

        backward_range_fn(I seq_it, I seq_end, J scaling_it, model const& hmm)
        :  hmm_{&hmm}, seq_it_{seq_it}, seq_end_{seq_end}, scaling_it_{scaling_it},
          beta_(hmm.states()), next_beta_(hmm.states())
        {
          if (seq_it != seq_end)
            initial_coefficients(*scaling_it);
        }

        class iterator
        : public boost::iterator_facade<
              iterator, row_vector, std::input_iterator_tag, row_vector const&
         > {
          public:
            iterator() = default;
          private:
            friend class backward_range_fn;
            friend class boost::iterator_core_access;

            iterator(backward_range_fn& parent)
            : parent_{parent ? &parent : nullptr} {}

            backward_range_fn* parent_ = nullptr;

            row_vector const& dereference() const
            {
              Expects(parent_ && *parent_);
              return parent_->beta_;
            }

            void increment()
            {
              Expects(parent_ && *parent_);
              if (!parent_->next())
                parent_ = nullptr;
            }

            bool equal(iterator other) const noexcept
            {
              return parent_ == other.parent_;
            }
        };

        operator bool() const noexcept
        {
          return seq_it_ != seq_end_;
        }

        iterator begin() noexcept
        {
          return {*this};
        }

        iterator end() noexcept
        {
          return {};
        }

      private:
        model const* hmm_; // not owning
        I seq_it_, seq_end_;
        J scaling_it_;
        row_vector beta_;
        row_vector next_beta_;

        void initial_coefficients(T scaling) noexcept
        {
          Expects(beta_.size() == hmm_->states());
          beta_.fill(scaling);
        }

        void recursion_advance(symbol_type s, T scaling) noexcept
        {
          using size_type = typename model::size_type;
          matrix const& A = hmm_->transition_matrix();
          matrix const& B = hmm_->symbol_probabilities();
          next_beta_.swap(beta_);

          // check pre conditions
          size_type states = A.rows();
          Expects(A.cols() == states);
          Expects(B.rows() == states);
          Expects(next_beta_.size() == states);
          Expects(beta_.size() == states);
          size_type ob = gsl::narrow<size_type>(s);
          Expects(0 <= ob && ob < B.cols());

          // recursion formula
          for (size_type i = 0; i < states; ++i) {
            beta_(i) = 0.0;
            for (size_type j = 0; j < states; ++j)
              beta_(i) += A(i,j)*B(j,ob)*next_beta_(j);
            beta_(i) *= scaling;
          }
        }

        bool next()
        {
          Expects(*this);
          symbol_type s = *seq_it_++;
          ++scaling_it_;
          if (seq_it_ == seq_end_)
            return false;
          recursion_advance(s, *scaling_it_);
          return true;
        }
    };

  template <class I, class J, class T>
    backward_range_fn<I, J, T>
    backward(I begin, I end, J scaling, hidden_markov_model<T> const& hmm)
    {
      return {begin, end, scaling, hmm};
    }

} // namespace hmm
} // namespace maikel

#endif /* HMM_ALGORITHM_BACKWARD_H_ */


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

#pragma once

#include <runtime/local/context/DaphneContext.h>
#include <runtime/local/datastructures/DataObjectFactory.h>
#include <runtime/local/datastructures/DenseMatrix.h>

#include <cblas.h>
#include <lapacke.h>

#include <cassert>
#include <cstddef>

// ****************************************************************************
// Struct for partial template specialization
// ****************************************************************************

template<class DTRes, class DTLhs, class DTRhs>
struct Solve {
    static void apply(DTRes *& res, const DTLhs * lhs, const DTRhs * rhs, DCTX(ctx)) = delete;
};

// ****************************************************************************
// Convenience function
// ****************************************************************************

template<class DTRes, class DTLhs, class DTRhs>
void solve(DTRes *& res, const DTLhs * lhs, const DTRhs * rhs, DCTX(ctx)) {
    Solve<DTRes, DTLhs, DTRhs>::apply(res, lhs, rhs, ctx);
}

// ****************************************************************************
// (Partial) template specializations for different data/value types
// ****************************************************************************

// ----------------------------------------------------------------------------
// DenseMatrix <- DenseMatrix, DenseMatrix
// ----------------------------------------------------------------------------

template<>
struct Solve<DenseMatrix<float>, DenseMatrix<float>, DenseMatrix<float>> {
    static void apply(DenseMatrix<float> *& res, const DenseMatrix<float> * lhs, const DenseMatrix<float> * rhs, DCTX(ctx)) {
        const auto nr1 = static_cast<int>(lhs->getNumRows());
        const auto nc1 = static_cast<int>(lhs->getNumCols());
        const auto nc2 = static_cast<int>(rhs->getNumCols());
        assert((nr1 == static_cast<int>(rhs->getNumRows())) && "#rows of lhs and #rows of rhs must be the same");
        assert((nr1 == nc1) && "#rows and #cols of lhs must be the same");
        assert((static_cast<int>(lhs->getRowSkip()) == nc1) && "#cols of lhs must match row skip");
        assert((nc2==1) && "#cols of rhs must be 1");

        if(res == nullptr)
            res = DataObjectFactory::create<DenseMatrix<float>>(nr1, nc2, false);

        // solve system of equations via LU decomposition
        int ipiv[nr1];       // permutation indexes
        float work[nr1*nc1]; // LU factorization of gesv
        memcpy(work, lhs->getValues(), nr1*nc1*sizeof(float));         //for in-place A
        memcpy(res->getValues(), rhs->getValues(), nr1*sizeof(float)); //for in-place b-out
        [[maybe_unused]] int info = LAPACKE_sgesv(LAPACK_ROW_MAJOR, nr1, 1, work, nc1, ipiv, res->getValues(), 1);
        assert((info<=0) && "A factor Ui is exactly singular, so the solution could not be computed");
    }
};

template<>
struct Solve<DenseMatrix<double>, DenseMatrix<double>, DenseMatrix<double>> {
    static void apply(DenseMatrix<double> *& res, const DenseMatrix<double> * lhs, const DenseMatrix<double> * rhs, DCTX(ctx)) {
        const auto nr1 = static_cast<int>(lhs->getNumRows());
        const auto nc1 = static_cast<int>(lhs->getNumCols());
        const auto nc2 = static_cast<int>(rhs->getNumCols());
        assert((nr1 == static_cast<int>(rhs->getNumRows())) && "#rows of lhs and #rows of rhs must be the same");
        assert((nr1 == nc1) && "#rows and #cols of lhs must be the same");
        assert((static_cast<int>(lhs->getRowSkip()) == nc1) && "#cols of lhs must match row skip");
        assert((nc2==1) && "#cols of rhs must be 1");

        if(res == nullptr)
            res = DataObjectFactory::create<DenseMatrix<double>>(nr1, nc2, false);

        // solve system of equations via LU decomposition
        int ipiv[nr1];       // permutation indexes
        double work[nr1*nc1]; // LU factorization of gesv
        memcpy(work, lhs->getValues(), nr1*nc1*sizeof(double));         //for in-place A
        memcpy(res->getValues(), rhs->getValues(), nr1*sizeof(double)); //for in-place b-out
        [[maybe_unused]] int info = LAPACKE_dgesv(LAPACK_ROW_MAJOR, nr1, 1, work, nc1, ipiv, res->getValues(), 1);
        assert((info<=0) && "A factor Ui is exactly singular, so the solution could not be computed");
    }
};