#include <petsc.h>

static PetscErrorCode SetInitialCondition(PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx){

    u[0] = x[0];
    return 0;
}

static PetscErrorCode PrintFVGradients(DM dm){
    PetscFunctionBeginUser;
    PetscErrorCode ierr;

    // Convert the dm to a plex
    DM plex;
    DMConvert(dm, DMPLEX, &plex);

    // Get the start/end
    PetscInt fStart;
    PetscInt fEnd;
    ierr = DMPlexGetHeightStratum(plex, 1, &fStart, &fEnd);CHKERRQ(ierr);

    // Extract the cell geometry, and the dm that holds the information
    Vec faceGeometry;
    DM dmFace;
    ierr = DMPlexGetGeometryFVM(plex, &faceGeometry, NULL, NULL);CHKERRQ(ierr);

    ierr = VecGetDM(faceGeometry, &dmFace);CHKERRQ(ierr);

    const PetscScalar *facegeom;
    ierr = VecGetArrayRead(faceGeometry, &facegeom);CHKERRQ(ierr);

    // Get the dim
    PetscInt dim;
    ierr = DMGetDimension(dm, &dim);CHKERRQ(ierr);

    // March over each cell volume
    for (PetscInt f = fStart; f < fEnd; ++f) {
        PetscFVFaceGeom       *fg;

        ierr = DMPlexPointLocalRead(dmFace, f, facegeom, &fg);CHKERRQ(ierr);

        printf("Face Grad %d: ", f);
        for(PetscInt d =0; d< dim; d++){
            printf("%f/%f, ", fg->grad[0][d], fg->grad[1][d]);
        }
        printf("\n");
    }

    PetscFunctionReturn(0);
}

int main(int argc, char **argv)
{
    DM             dm;
    PetscErrorCode ierr;

    ierr = PetscInitialize(&argc, &argv, NULL, "HELP");if (ierr) return(ierr);
    ierr = DMPlexCreateBoxMesh(PETSC_COMM_WORLD, 2, PETSC_FALSE, NULL, NULL, NULL, NULL, PETSC_TRUE, &dm);CHKERRQ(ierr);
    ierr = DMViewFromOptions(dm, NULL, "-dm_view");CHKERRQ(ierr);
    ierr = DMSetFromOptions(dm);CHKERRQ(ierr);

    {
        DM gdm;

        ierr = DMPlexConstructGhostCells(dm, NULL, NULL, &gdm);CHKERRQ(ierr);
        ierr = DMDestroy(&dm);CHKERRQ(ierr);
        dm   = gdm;
    }
    ierr = DMViewFromOptions(dm, NULL, "-dm_view_ghost");CHKERRQ(ierr);

    PetscFV fvm;
    PetscDS ds;

    ierr = PetscFVCreate(PETSC_COMM_WORLD, &fvm);CHKERRQ(ierr);
    ierr = PetscFVSetFromOptions(fvm);CHKERRQ(ierr);
    ierr = PetscFVSetNumComponents(fvm, 1);CHKERRQ(ierr);
    ierr = PetscFVSetSpatialDimension(fvm, 2);CHKERRQ(ierr);
    ierr = PetscObjectSetName((PetscObject) fvm, "outputField");CHKERRQ(ierr);

    /* FV is now structured with one field having all physics as components */
    ierr = DMAddField(dm, NULL, (PetscObject) fvm);CHKERRQ(ierr);
    ierr = PetscFVDestroy(&fvm);CHKERRQ(ierr);
    ierr = DMCreateDS(dm);CHKERRQ(ierr);
    ierr = DMGetDS(dm, &ds);CHKERRQ(ierr);

    Vec X;
    PetscErrorCode     (*func[1]) (PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx) = {SetInitialCondition};

    ierr = DMCreateGlobalVector(dm, &X);CHKERRQ(ierr);
    ierr = PetscObjectSetName((PetscObject) X, "solution");CHKERRQ(ierr);
    ierr = DMProjectFunction(dm, 0.0, func, NULL, INSERT_ALL_VALUES, X);CHKERRQ(ierr);
    ierr = VecViewFromOptions(X, NULL, "-vec_view");CHKERRQ(ierr);

    PrintFVGradients(dm);

    ierr = VecDestroy(&X);CHKERRQ(ierr);
    ierr = DMDestroy(&dm);CHKERRQ(ierr);
    return PetscFinalize();
}


#include <stdio.h>
#include "sweeny_bfs.h"
#include "../src/queue_2.h"
#include <ctype.h>
#include <strings.h>
#include <unistd.h>
#include <math.h>
#include <gsl/gsl_rng.h>

// Define Macro SEQUENTIAL to use sequential BFS; Default is interleaved BFS

#define MIN(a,b) a<=b ? a: b
#define MAX(a,b) a>=b? a:b

static char setup=0; // 0 not setup; 1 ibfs; 2 sbfs
static char verbose=0;
static double rcWeights[4]; // array with precalculated mc weights
static double p_min_del,p_max_del, p_min_ins,p_max_ins;
static __s8 dN,dB;
static char equilibration=1;
static __u32 DX;
static __u32 N;
static __u32 seed;
static double q;
static double coupling;
static double beta;
static double v;
static double K;
static __u32 steps;
static __s8 *bonds;
static gsl_rng * r;
static __u32 edge[2];
static __s32 adj1[4];
static __s32 adj2[4];
static __u32 offset_1;
static __u32 offset_2;
static __u32 cs2;
static struct queue_2_node *todo_pool;
static __u32 *visited;
static __u32 cs1;
static __u32 activeEdges=0;
static __u32 cutoff;
static __u32 *num_bonds , *num_cluster, *size_giant;
static __u64 *sec_cs_moment, *four_cs_moment;

/******************************************************************************
 *****************************************************************************/
static char init(void)
{
    K=coupling*beta;
    v = exp(K) - 1.0;
    N = DX*DX;
    // Deletion accept. ratios
    rcWeights[0] = pow(v,-1); //db==-1,dN==0
    rcWeights[1] = rcWeights[0]*q; //db==-1,dN=1
    // Insertion accept. ratios
    rcWeights[2] = v; //db==1,dN==0
    rcWeights[3] = v*pow(q,-1); //db==1,dN==-1

    p_min_del = MIN(rcWeights[0],rcWeights[1]);
    p_max_del = MAX(rcWeights[0],rcWeights[1]);
    p_min_ins = MIN(rcWeights[2],rcWeights[3]);
    p_max_ins = MAX(rcWeights[2],rcWeights[3]);

    r = gsl_rng_alloc (gsl_rng_mt19937);
    if(!r)
        return 0;
    gsl_rng_set(r,seed);
    __u32 i;
    bonds = malloc(sizeof(*bonds)*2*N);
    if(!bonds) {
        return 0;
    }
    for(i=0;i<2*N;i++) bonds[i] = -1;
    visited = calloc(N,sizeof(*visited));
    if(!visited) {
        return 0;
    }
    todo_pool = malloc(N*sizeof(struct queue_2_node));
    if(!todo_pool) {
        return 0;
    }
    return 1;

}
/******************************************************************************
 *****************************************************************************/
static void destroy(void)
{
    if(!setup)return;
    free(todo_pool);
    free(bonds);
    gsl_rng_free(r);
    free(visited);
}
/******************************************************************************
 *****************************************************************************/
static inline __u32 ltcXnext(__u32 idx)
{
    if((idx+1) % DX)
        return idx+1;
    else
        return idx +1- DX ;
}
/******************************************************************************
 *****************************************************************************/
static inline __u32 ltcXprev(__u32 idx)
{
    if((idx%DX))
        return idx-1;
    else
        return idx + DX  -1;
}
/******************************************************************************
 *****************************************************************************/
 static inline __u32 ltcYnext(__u32 idx)
{

    if(idx <= N -1 && idx >= N - DX)
        return idx +DX- N ;
    else
        return idx+DX;
}
/******************************************************************************
 *****************************************************************************/
static inline __u32 ltcYprev(__u32 idx)
{
    if(idx <= DX - 1)
        return idx + N - DX;
    else
        return idx - DX;

}
/******************************************************************************
 *****************************************************************************/
static void Adjacent(__u32 bidx)
{

  if(bidx%2) {
    bidx = (bidx -1)/2;
    edge[0] = bidx;
    edge[1] = ltcXnext(bidx);
  }
  else {
    bidx = bidx/2;
    edge[0] = bidx;
    edge[1] = ltcYprev(bidx);
  }

}
/******************************************************************************
 *****************************************************************************/
static void neighbours(__u32 idx,__u8 a) {
  if(a == 1)
  {
    if(bonds[2*idx] == 1)  adj1[0] = ltcYprev(idx);
    else  adj1[0] = -1;
    if(bonds[(2*idx)+1] == 1)  adj1[1] = ltcXnext(idx);
    else adj1[1] = -1;
    if(bonds[2*ltcYnext(idx)] == 1)	adj1[2] = ltcYnext(idx);
    else adj1[2] = -1;
    if(bonds[(2*ltcXprev(idx))+1] ==1)adj1[3] = ltcXprev(idx);
    else adj1[3] = -1;
  }
  else {
    if(bonds[2*idx] == 1)adj2[0] = ltcYprev(idx);
    else adj2[0] = -1;
    if(bonds[(2*idx)+1] == 1)adj2[1] = ltcXnext(idx);
    else adj2[1] = -1;
    if(bonds[2*ltcYnext(idx)] == 1)	adj2[2] = ltcYnext(idx);
    else adj2[2] = -1;
    if(bonds[(2*ltcXprev(idx))+1] == 1)adj2[3] = ltcXprev(idx);
    else adj2[3] = -1;
  }

}
/******************************************************************************
 *****************************************************************************/
static __u8 breadthFirstSearch_s(__u32 start, __u32 goal)
{
  static struct queue_2 todo1;
  __u32 i=0,activeP1=0;
  init_queue_2(&todo1); //todo_pool);
  cs1=0;
  enqueue_2(&todo1,start);
  cs1++; // Increase cluster size of bfs 1
  visited[start] = offset_1; // mark starting point as visited
    while(!queue_2_empty_p(&todo1)) {
      dequeue_2(&todo1,&activeP1);
      neighbours(activeP1,1);
      for(i=0;i<4;i++) {
        if(adj1[i] != -1) {
            if(visited[adj1[i]] == offset_1) continue;
            if((__u32)adj1[i] == goal) {
                    while(!queue_2_empty_p(&todo1)){ dequeue_2(&todo1,&activeP1);}
                    return 1;
                }
            enqueue_2(&todo1,adj1[i]);
            visited[adj1[i]] = offset_1;
            cs1++;
        }
      }
  }
  return 0;
}
/******************************************************************************
 *****************************************************************************/
static __u8 breadthFirstSearch(__u32 start, __u32 goal)
{
  static struct queue_2 todo1,todo2;
  __u32 i=0,activeP1=0,activeP2=0;
  init_queue_2(&todo1);
  init_queue_2(&todo2);
  cs1=0;
  cs2=0;
  enqueue_2(&todo1,start); // Put starting point onto queue_2 1
  cs1++; // Increase cluster size of bfs 1
  visited[start] = offset_1; // mark starting point as visited
  enqueue_2(&todo2,goal); // Put goal point onto queue_2 2
  visited[goal] = offset_2; // mark goal point as visited
  cs2++; // increase cluster size of bfs 2
    while(!queue_2_empty_p(&todo1) && !queue_2_empty_p(&todo2)) {
      dequeue_2(&todo2,&activeP2); // get next vertex of bfs 2
      neighbours(activeP2,2);	 // get all adjacent vertices of current vertex
      for(i=0;i<4;i++) {

        if(adj2[i] != -1) { // if accessable (edge exists)
	  if(visited[adj2[i]] == offset_2) continue; // already visited
	  if((__u32)adj2[i] == start || visited[adj2[i]] == offset_1) { // reconnected

            while(!queue_2_empty_p(&todo1)){ dequeue_2(&todo1,&activeP1);}
            while(!queue_2_empty_p(&todo2)){ dequeue_2(&todo2,&activeP2);}
	    return 1; // 1 indicates success
	  }
	  enqueue_2(&todo2,adj2[i]);
	  visited[adj2[i]] = offset_2; // mark as visited
	  cs2++; // increase cluster size
	  }
      }
      dequeue_2(&todo1,&activeP1);
      neighbours(activeP1,1);
      for(i=0;i<4;i++) {
	if(adj1[i] != -1) {
	  if(visited[adj1[i]] == offset_1) continue;
	  if((__u32)adj1[i] == goal || visited[adj1[i]] == offset_2) {
            while(!queue_2_empty_p(&todo1)){ dequeue_2(&todo1,&activeP1);}
            while(!queue_2_empty_p(&todo2)){ dequeue_2(&todo2,&activeP2);}
			return 1;
	  }
	  enqueue_2(&todo1,adj1[i]);
	  visited[adj1[i]] = offset_1;
	  cs1++;
	}
      }
  }
  while(!queue_2_empty_p(&todo1)){dequeue_2(&todo1,&activeP1);}
  while(!queue_2_empty_p(&todo2)){dequeue_2(&todo2,&activeP2);}
  return 0;
}
/******************************************************************************
 *****************************************************************************/
static inline __u8 connected(__u32 start, __u32 goal) {
    offset_1+=2;
    offset_2+=2;
    return breadthFirstSearch(start,goal);
}

/******************************************************************************
 *****************************************************************************/
__u8 static inline connected_s(__u32 start, __u32 goal) {
    ++offset_1;
    return breadthFirstSearch_s(start,goal);
}
/******************************************************************************
 *****************************************************************************/
static inline void   sweep_ibfs(void)
{
    static __u32 bidx;
    static double rnd_num;
    __u32 i=0;

    for(;i<2*N ;++i) {
        bidx = gsl_rng_uniform_int(r,2*N);
        rnd_num  = gsl_rng_uniform(r);
        Adjacent(bidx);
        if(bonds[bidx] == 1) { //edge is active hence delete it
            dB = -1;
            if(rnd_num < p_min_del) { // accept bond deletion
                bonds[bidx]*=-1;
                activeEdges--;
            }
            else {
                if(rnd_num < p_max_del) {
                    bonds[bidx]*=-1;
                    dN = (connected(edge[0],edge[1])? 0 : 1);
                    if(rnd_num >= rcWeights[dB == -1? dN : 2-dN]) bonds[bidx]*=-1; // undo bond deletion
                    else activeEdges--; // accept bond deletion
                }

            }

        }
        else {
            // Insert cedge. If adjacent vertices are already connected, the new
            // edge will be a non-tree edge, i.e. dN = 0
            dB = 1;
            if(rnd_num < p_min_ins) { // accept bond insertion
                bonds[bidx] *=-1;
                activeEdges++;
            }
            else {
                if(rnd_num < p_max_ins) {
                    dN = ( connected(edge[0],edge[1]) ?0 : -1);
                    if(rnd_num < rcWeights[dB == -1? dN : 2-dN]) {
                        bonds[bidx]*=-1;
                        activeEdges++;
                    }
                }

            }
        }
    }
}
/******************************************************************************
 *****************************************************************************/
static inline void   sweep_sbfs(void)
{
    static __u32 bidx;
    static double rnd_num;
    __u32 i=0;

    for(;i<2*N ;++i) {
        bidx = gsl_rng_uniform_int(r,2*N);
        rnd_num  = gsl_rng_uniform(r);
        Adjacent(bidx);
        if(bonds[bidx] == 1) { //edge is active hence delete it
            dB = -1;
            if(rnd_num < p_min_del) { // accept bond deletion
                bonds[bidx]*=-1;
                activeEdges--;
            }
            else {
                if(rnd_num < p_max_del) {
                    bonds[bidx]*=-1;
                    dN = (connected_s(edge[0],edge[1])? 0 : 1);
                    if(rnd_num >= rcWeights[dB == -1? dN : 2-dN]) bonds[bidx]*=-1; // undo bond deletion
                    else activeEdges--; // accept bond deletion
                }

            }

        }
        else {
            // Insert cedge. If adjacent vertices are already connected, the new
            // edge will be a non-tree edge, i.e. dN = 0
            dB = 1;
            if(rnd_num < p_min_ins) { // accept bond insertion
                bonds[bidx] *=-1;
                activeEdges++;
            }
            else {
                if(rnd_num < p_max_ins) {
                    dN = ( connected_s(edge[0],edge[1]) ?0 : -1);
                    if(rnd_num < rcWeights[dB == -1? dN : 2-dN]) {
                        bonds[bidx]*=-1;
                        activeEdges++;
                    }
                }

            }
        }
    }
}
/******************************************************************************
 *****************************************************************************/
static void extract_observables(__u32 i) {
    static __u32 j=0;
    static __u32 clust_cnt=0;
    static __u64 sum=0,sum_2;
    static __u32 maxc=0,chksum=0;
    num_bonds[i] = activeEdges;
    offset_1=0;
    for(j=0;j<N;++j) visited[j] = offset_1;
    offset_1=1;
    for(j=0;j<N&&chksum<=N;++j) {
        if(!visited[j]){
            breadthFirstSearch_s(j,N);
            clust_cnt++;
            if(cs1 > maxc) maxc = cs1;
            sum+=cs1*cs1;
            sum_2+=pow(cs1,4);
            chksum+=cs1;
        }

    }
    num_cluster[i] = clust_cnt;
    size_giant[i] = maxc;
    sec_cs_moment[i] = sum;
    four_cs_moment[i] = sum_2;
    maxc=clust_cnt=sum=sum_2=chksum=0;
    offset_1=3;
    offset_2=2;
}
/******************************************************************************
 *****************************************************************************/
static void generateTimeSeries(void)
{

  __u32 i;
  if(setup==1) {
    for(i=0;i<cutoff;++i)sweep_ibfs();
    equilibration=0;
    if(verbose)
        printf("Equilibration done!\n");
    for(i=0;i<steps;++i) {
        sweep_ibfs();
        extract_observables(i);
    }
  }
  if(setup==2) {
    for(i=0;i<cutoff;++i)sweep_sbfs();
    equilibration=0;
    if(verbose)
        printf("Equilibration done!\n");
    for(i=0;i<steps;++i) {
        sweep_sbfs();
        extract_observables(i);
    }

  }

}
/******************************************************************************
 *****************************************************************************/
char init_sweeny_ibfs(double _q,unsigned int _l,double _beta,double _coupl,
        unsigned int _cutoff,unsigned _tslength,unsigned int rng_seed,
        void *ts_0,void *ts_1,void * ts_2,void *ts_3,void *ts_4) {
    setup=1;
    offset_1 =1;
    offset_2 = 2;
    activeEdges =0;
    q = _q;
    DX = _l;
    beta = _beta;
    coupling = _coupl;
    cutoff = _cutoff;
    steps = _tslength;
    seed = rng_seed;
    num_bonds = (__u32 *)ts_0;
    num_cluster = (__u32 *)ts_1;
    size_giant = (__u32 *)ts_2;
    sec_cs_moment = (__u64 *)ts_3;
    four_cs_moment = (__u64 *)ts_4;
    if(!init())
        setup=0;
    return setup;

}
/******************************************************************************
 *****************************************************************************/
char init_sweeny_sbfs(double _q,unsigned int _l,double _beta,double _coupl,
        unsigned int _cutoff,unsigned _tslength,unsigned int rng_seed,
        void *ts_0,void *ts_1,void * ts_2,void *ts_3, void *ts_4) {
    setup=init_sweeny_ibfs(_q,_l,_beta,_coupl,_cutoff,_tslength,rng_seed,
            ts_0,ts_1,ts_2,ts_3,ts_4);
    if(setup)
        return setup=2;
    else
        return setup;
}
/******************************************************************************
 *****************************************************************************/
void destroy_sweeny_ibfs(void) {
    if(setup) {
        destroy();
    }
    setup=0;
}
/******************************************************************************
 *****************************************************************************/
void destroy_sweeny_sbfs(void) {
    destroy_sweeny_ibfs();
}
/******************************************************************************
 *****************************************************************************/
char simulate_sweeny_ibfs(void) {
    if(setup!=1)
        return 0;
    else
        generateTimeSeries();
    return 1;
}
/******************************************************************************
 *****************************************************************************/
char simulate_sweeny_sbfs(void) {
    if(setup!=2)
        return 0;
    else
        generateTimeSeries();
    return 1;
}


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

/* Author: G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "gsl_sf_expint.h"

#include "error.h"

#include "chebyshev.h"
#include "cheb_eval.c"

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

/*
 Chebyshev expansions: based on SLATEC e1.f, W. Fullerton

 Series for AE11       on the interval -1.00000D-01 to  0.
					with weighted error   1.76E-17
					 log weighted error  16.75
			       significant figures required  15.70
				    decimal places required  17.55

 Series for AE12       on the interval -2.50000D-01 to -1.00000D-01
					with weighted error   5.83E-17
					 log weighted error  16.23
			       significant figures required  15.76
				    decimal places required  16.93

 Series for E11        on the interval -4.00000D+00 to -1.00000D+00
					with weighted error   1.08E-18
					 log weighted error  17.97
			       significant figures required  19.02
				    decimal places required  18.61

 Series for E12        on the interval -1.00000D+00 to  1.00000D+00
					with weighted error   3.15E-18
					 log weighted error  17.50
			approx significant figures required  15.8
				    decimal places required  18.10

 Series for AE13       on the interval  2.50000D-01 to  1.00000D+00
					with weighted error   2.34E-17
					 log weighted error  16.63
			       significant figures required  16.14
				    decimal places required  17.33

 Series for AE14       on the interval  0.	    to  2.50000D-01
					with weighted error   5.41E-17
					 log weighted error  16.27
			       significant figures required  15.38
				    decimal places required  16.97
*/

static double AE11_data[39] = {
   0.121503239716065790,
  -0.065088778513550150,
   0.004897651357459670,
  -0.000649237843027216,
   0.000093840434587471,
   0.000000420236380882,
  -0.000008113374735904,
   0.000002804247688663,
   0.000000056487164441,
  -0.000000344809174450,
   0.000000058209273578,
   0.000000038711426349,
  -0.000000012453235014,
  -0.000000005118504888,
   0.000000002148771527,
   0.000000000868459898,
  -0.000000000343650105,
  -0.000000000179796603,
   0.000000000047442060,
   0.000000000040423282,
  -0.000000000003543928,
  -0.000000000008853444,
  -0.000000000000960151,
   0.000000000001692921,
   0.000000000000607990,
  -0.000000000000224338,
  -0.000000000000200327,
  -0.000000000000006246,
   0.000000000000045571,
   0.000000000000016383,
  -0.000000000000005561,
  -0.000000000000006074,
  -0.000000000000000862,
   0.000000000000001223,
   0.000000000000000716,
  -0.000000000000000024,
  -0.000000000000000201,
  -0.000000000000000082,
   0.000000000000000017
};
static cheb_series AE11_cs = {
  AE11_data,
  38,
  -1, 1,
  20
};

static double AE12_data[25] = {
   0.582417495134726740,
  -0.158348850905782750,
  -0.006764275590323141,
   0.005125843950185725,
   0.000435232492169391,
  -0.000143613366305483,
  -0.000041801320556301,
  -0.000002713395758640,
   0.000001151381913647,
   0.000000420650022012,
   0.000000066581901391,
   0.000000000662143777,
  -0.000000002844104870,
  -0.000000000940724197,
  -0.000000000177476602,
  -0.000000000015830222,
   0.000000000002905732,
   0.000000000001769356,
   0.000000000000492735,
   0.000000000000093709,
   0.000000000000010707,
  -0.000000000000000537,
  -0.000000000000000716,
  -0.000000000000000244,
  -0.000000000000000058
};
static cheb_series AE12_cs = {
  AE12_data,
  24,
  -1, 1,
  15
};

static double E11_data[19] = {
  -16.11346165557149402600,
    7.79407277874268027690,
   -1.95540581886314195070,
    0.37337293866277945612,
   -0.05692503191092901938,
    0.00721107776966009185,
   -0.00078104901449841593,
    0.00007388093356262168,
   -0.00000620286187580820,
    0.00000046816002303176,
   -0.00000003209288853329,
    0.00000000201519974874,
   -0.00000000011673686816,
    0.00000000000627627066,
   -0.00000000000031481541,
    0.00000000000001479904,
   -0.00000000000000065457,
    0.00000000000000002733,
   -0.00000000000000000108
};
static cheb_series E11_cs = {
  E11_data,
  18,
  -1, 1,
  13
};

static double E12_data[16] = {
  -0.03739021479220279500,
   0.04272398606220957700,
  -0.13031820798497005440,
   0.01441912402469889073,
  -0.00134617078051068022,
   0.00010731029253063780,
  -0.00000742999951611943,
   0.00000045377325690753,
  -0.00000002476417211390,
   0.00000000122076581374,
  -0.00000000005485141480,
   0.00000000000226362142,
  -0.00000000000008635897,
   0.00000000000000306291,
  -0.00000000000000010148,
   0.00000000000000000315
};
static cheb_series E12_cs = {
  E12_data,
  15,
  -1, 1,
  10
};

static double AE13_data[25] = {
  -0.605773246640603460,
  -0.112535243483660900,
   0.013432266247902779,
  -0.001926845187381145,
   0.000309118337720603,
  -0.000053564132129618,
   0.000009827812880247,
  -0.000001885368984916,
   0.000000374943193568,
  -0.000000076823455870,
   0.000000016143270567,
  -0.000000003466802211,
   0.000000000758754209,
  -0.000000000168864333,
   0.000000000038145706,
  -0.000000000008733026,
   0.000000000002023672,
  -0.000000000000474132,
   0.000000000000112211,
  -0.000000000000026804,
   0.000000000000006457,
  -0.000000000000001568,
   0.000000000000000383,
  -0.000000000000000094,
   0.000000000000000023
};
static cheb_series AE13_cs = {
  AE13_data,
  24,
  -1, 1,
  15
};

static double AE14_data[26] = {
  -0.18929180007530170,
  -0.08648117855259871,
   0.00722410154374659,
  -0.00080975594575573,
   0.00010999134432661,
  -0.00001717332998937,
   0.00000298562751447,
  -0.00000056596491457,
   0.00000011526808397,
  -0.00000002495030440,
   0.00000000569232420,
  -0.00000000135995766,
   0.00000000033846628,
  -0.00000000008737853,
   0.00000000002331588,
  -0.00000000000641148,
   0.00000000000181224,
  -0.00000000000052538,
   0.00000000000015592,
  -0.00000000000004729,
   0.00000000000001463,
  -0.00000000000000461,
   0.00000000000000148,
  -0.00000000000000048,
   0.00000000000000016,
  -0.00000000000000005
};
static cheb_series AE14_cs = {
  AE14_data,
  25,
  -1, 1,
  13
};

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int gsl_sf_expint_E1_e(const double x, gsl_sf_result * result)
{
  const double xmaxt = -GSL_LOG_DBL_MIN;      /* XMAXT = -LOG (R1MACH(1)) */
  const double xmax  = xmaxt - log(xmaxt);    /* XMAX = XMAXT - LOG(XMAXT) */

  /* CHECK_POINTER(result) */

  if(x < -xmax) {
    OVERFLOW_ERROR(result);
  }
  else if(x <= -10.0) {
    const double s = exp(-x)/x;
    gsl_sf_result result_c;
    cheb_eval_e(&AE11_cs, 20.0/x+1.0, &result_c);
    result->val  = s * (1.0 + result_c.val);
    result->err  = s * result_c.err;
    result->err += 2.0 * GSL_DBL_EPSILON * (fabs(x) + 1.0) * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x <= -4.0) {
    const double s = exp(-x)/x;
    gsl_sf_result result_c;
    cheb_eval_e(&AE12_cs, (40.0/x+7.0)/3.0, &result_c);
    result->val  = s * (1.0 + result_c.val);
    result->err  = s * result_c.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x <= -1.0) {
    const double ln_term = -log(fabs(x));
    gsl_sf_result result_c;
    cheb_eval_e(&E11_cs, (2.0*x+5.0)/3.0, &result_c);
    result->val  = ln_term + result_c.val;
    result->err  = result_c.err + GSL_DBL_EPSILON * fabs(ln_term);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x == 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x <= 1.0) {
    const double ln_term = -log(fabs(x));
    gsl_sf_result result_c;
    cheb_eval_e(&E12_cs, x, &result_c);
    result->val  = ln_term - 0.6875 + x + result_c.val;
    result->err  = result_c.err + GSL_DBL_EPSILON * fabs(ln_term);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x <= 4.0) {
    const double s = exp(-x)/x;
    gsl_sf_result result_c;
    cheb_eval_e(&AE13_cs, (8.0/x-5.0)/3.0, &result_c);
    result->val  = s * (1.0 + result_c.val);
    result->err  = s * result_c.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x <= xmax) {
    const double s = exp(-x)/x;
    gsl_sf_result result_c;
    cheb_eval_e(&AE14_cs, 8.0/x-1.0, &result_c);
    result->val  = s * (1.0 +  result_c.val);
    result->err  = s * (GSL_DBL_EPSILON + result_c.err);
    result->err += 2.0 * (x + 1.0) * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    UNDERFLOW_ERROR(result);
  }
}

int gsl_sf_expint_E2_e(const double x, gsl_sf_result * result)
{
  const double xmaxt = -GSL_LOG_DBL_MIN;
  const double xmax  = xmaxt - log(xmaxt);

  /* CHECK_POINTER(result) */

  if(x < -xmax) {
    OVERFLOW_ERROR(result);
  }
  else if(x < 100.0) {
    const double ex = exp(-x);
    gsl_sf_result result_E1;
    int stat_E1 = gsl_sf_expint_E1_e(x, &result_E1);
    result->val  = ex - x*result_E1.val;
    result->err  = fabs(x) * (GSL_DBL_EPSILON*ex + result_E1.err);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat_E1;
  }
  else if(x < xmax) {
    const double c1  = -2.0;
    const double c2  =  6.0;
    const double c3  = -24.0;
    const double c4  =  120.0;
    const double c5  = -720.0;
    const double c6  =  5040.0;
    const double c7  = -40320.0;
    const double c8  =  362880.0;
    const double c9  = -3628800.0;
    const double c10 =  39916800.0;
    const double c11 = -479001600.0;
    const double c12 =  6227020800.0;
    const double c13 = -87178291200.0;
    const double y = 1.0/x;
    const double sum6 = c6+y*(c7+y*(c8+y*(c9+y*(c10+y*(c11+y*(c12+y*c13))))));
    const double sum  = y*(c1+y*(c2+y*(c3+y*(c4+y*(c5+y*sum6)))));
    result->val = exp(-x) * (1.0 + sum)/x;
    result->err = 2.0 * (x + 1.0) * GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else {
    UNDERFLOW_ERROR(result);
  }
}

int gsl_sf_expint_Ei_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  {
    int status = gsl_sf_expint_E1_e(-x, result);
    result->val = -result->val;
    return status;
  }
}

#if 0
static double recurse_En(int n, double x, double E1)
{
  int i;
  double En = E1;
  double ex = exp(-x);
  for(i=2; i<=n; i++) {
    En = 1./(i-1) * (ex - x * En);
  }
  return En;
}
#endif

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_expint_E1(const double x)
{
  EVAL_RESULT(gsl_sf_expint_E1_e(x, &result));
}

double gsl_sf_expint_E2(const double x)
{
  EVAL_RESULT(gsl_sf_expint_E2_e(x, &result));
}

double gsl_sf_expint_Ei(const double x)
{
  EVAL_RESULT(gsl_sf_expint_Ei_e(x, &result));
}


/****************************************************************************
 *                                                                          *
 * Copyright (C) 2001 ~ 2016 Neutrino International Inc.                    *
 *                                                                          *
 * Author : Brian Lin <lin.foxman@gmail.com>, Skype: wolfram_lin            *
 *                                                                          *
 * QtFFT acts as an interface between Qt and FFT libraries.                 *
 * Please keep QtFFT as simple as possible.                                 *
 *                                                                          *
 * Qt Version   : 5.4.1                                                     *
 * CIOS Version : 1.6.0                                                     *
 *                                                                          *
 ****************************************************************************/

#ifndef QT_FFT_H
#define QT_FFT_H

#include <QtCore>
#include <QtNetwork>
#include <QtSql>
#ifndef QT_STATIC
#include <QtScript>
#endif

#include <Essentials>

#ifdef QT_QTGMP_LIB
#include <QtGMP>
#endif

#ifdef QT_QTGSL_LIB
#include <QtGSL>
#endif

#ifdef QT_QTCUDA_LIB
#include <QtCUDA>
#endif

extern "C" {

#include <gsl/gsl_wavelet.h>
#include <gsl/gsl_wavelet2d.h>
#include <gsl/gsl_fft.h>
#include <gsl/gsl_fft_complex.h>
#include <gsl/gsl_fft_complex_float.h>
#include <gsl/gsl_fft_halfcomplex.h>
#include <gsl/gsl_fft_halfcomplex_float.h>
#include <gsl/gsl_fft_real.h>
#include <gsl/gsl_fft_real_float.h>
#include <QtFFT/fftw3.h>

}

QT_BEGIN_NAMESPACE

#ifndef QT_STATIC
#    if defined(QT_BUILD_QTFFT_LIB)
#      define Q_FFT_EXPORT Q_DECL_EXPORT
#    else
#      define Q_FFT_EXPORT Q_DECL_IMPORT
#    endif
#else
#    define Q_FFT_EXPORT
#endif

namespace N
{

class Q_FFT_EXPORT Wavelet        ;
class Q_FFT_EXPORT Hankel         ;
class Q_FFT_EXPORT AbstractFFT    ;
class Q_FFT_EXPORT CooleyFFT      ;
class Q_FFT_EXPORT KissFFT        ;
class Q_FFT_EXPORT GSLFFT         ;
class Q_FFT_EXPORT FFTW           ;
class Q_FFT_EXPORT AccelerateFFT  ;

class Q_FFT_EXPORT Wavelet
{
  public:

    gsl_wavelet           * wavelet   ;
    gsl_wavelet_workspace * work      ;
    gsl_wavelet_direction   direction ;

    explicit Wavelet             (void) ;
    virtual ~Wavelet             (void) ;

    bool    allocate             (const gsl_wavelet_type * T,size_t k) ;
    bool    workspace            (size_t n) ;
    QString Name                 (void) ;

    int Transform                (double * data,size_t stride,size_t n) ;
    int TransformForward         (double * data,size_t stride,size_t n) ;
    int TransformInverse         (double * data,size_t stride,size_t n) ;
    int Transform2d              (double * data,size_t tda,size_t size1,size_t size2) ;
    int Transform2dForward       (double * data,size_t tda,size_t size1,size_t size2) ;
    int Transform2dInverse       (double * data,size_t tda,size_t size1,size_t size2) ;
    int TransformMatrix          (gsl_matrix * m) ;
    int TransformMatrixForward   (gsl_matrix * m) ;
    int TransformMatrixInverse   (gsl_matrix * m) ;
    int nsTransform              (double * data,size_t tda,size_t size1,size_t size2) ;
    int nsTransformForward       (double * data,size_t tda,size_t size1,size_t size2) ;
    int nsTransformInverse       (double * data,size_t tda,size_t size1,size_t size2) ;
    int nsTransformMatrix        (gsl_matrix * m) ;
    int nsTransformMatrixForward (gsl_matrix * m) ;
    int nsTransformMatrixInverse (gsl_matrix * m) ;

  protected:

  private:

};

class Q_FFT_EXPORT Hankel
{
  public:

    gsl_dht * hankel ;

    explicit Hankel   (void) ;
    explicit Hankel   (int size) ;
    explicit Hankel   (int size,double nu, double xmax) ;
             Hankel   (const Hankel & hankel) ;
    virtual ~Hankel   (void) ;

    bool   Allocate   (int size) ;
    bool   New        (int size,double nu, double xmax) ;
    int    Initialize (double nu,double xmax) ;
    int    Apply      (double * In,double * Out) ;
    double x          (int n) ;
    double k          (int n) ;

  protected:

  private:

};

class Q_FFT_EXPORT AbstractFFT
{
  public:

    typedef enum         {
      fftForward  = 0    ,
      fftBackward = 1    }
      TransformDirection ;

    TransformDirection direction  ;
    Cpp::ValueTypes    vType      ;
    int                sourceType ; // 0 - Undecided , 1 - Real , 2 - Complex
    int                targetType ; // 0 - Undecided , 1 - Real , 2 - Complex
    int                dimension  ;

    explicit AbstractFFT        (void);
    virtual ~AbstractFFT        (void);

    virtual int  type           (void) const = 0 ;
    virtual bool isSupported    (Cpp::ValueTypes type,int dimension) const = 0 ;
    virtual int  AllocateSource (CUIDs elements,QByteArray & Source) = 0 ;
    virtual int  AllocateTarget (CUIDs elements,QByteArray & Target) = 0 ;
    virtual bool Prepare        (CUIDs        elements        ,
                                 VarArgs    & Paraments       ,
                                 QByteArray & Source          ,
                                 QByteArray & Target    ) = 0 ;
    virtual void Execution      (void) = 0 ;

  protected:

  private:

};

class Q_FFT_EXPORT CooleyFFT : public AbstractFFT
{
  public:

    explicit CooleyFFT          (void) ;
    virtual ~CooleyFFT          (void) ;

    virtual int  type           (void) const ;
    virtual bool isSupported    (Cpp::ValueTypes T,int dimension) const ;
    virtual int  AllocateSource (CUIDs elements,QByteArray & Source) ;
    virtual int  AllocateTarget (CUIDs elements,QByteArray & Target) ;
    virtual bool Prepare        (CUIDs        elements  ,
                                 VarArgs    & Paraments ,
                                 QByteArray & Source    ,
                                 QByteArray & Target  ) ;
    virtual void Execution      (void) ;

  protected:

  private:

};

class Q_FFT_EXPORT KissFFT : public AbstractFFT
{
  public:

    explicit KissFFT            (void) ;
    virtual ~KissFFT            (void) ;

    virtual int  type           (void) const ;
    virtual bool isSupported    (Cpp::ValueTypes T,int dimension) const ;
    virtual int  AllocateSource (CUIDs elements,QByteArray & Source) ;
    virtual int  AllocateTarget (CUIDs elements,QByteArray & Target) ;
    virtual bool Prepare        (CUIDs        elements  ,
                                 VarArgs    & Paraments ,
                                 QByteArray & Source    ,
                                 QByteArray & Target  ) ;
    virtual void Execution      (void) ;

  protected:

  private:

};

class Q_FFT_EXPORT GSLFFT : public AbstractFFT
{
  public:

    explicit GSLFFT             (void) ;
    virtual ~GSLFFT             (void) ;

    virtual int  type           (void) const ;
    virtual bool isSupported    (Cpp::ValueTypes T,int dimension) const ;
    virtual int  AllocateSource (CUIDs elements,QByteArray & Source) ;
    virtual int  AllocateTarget (CUIDs elements,QByteArray & Target) ;
    virtual bool Prepare        (CUIDs        elements  ,
                                 VarArgs    & Paraments ,
                                 QByteArray & Source    ,
                                 QByteArray & Target  ) ;
    virtual void Execution      (void) ;

  protected:

  private:

};

class Q_FFT_EXPORT FFTW : public AbstractFFT
{
  public:

    explicit       FFTW           (void) ;
    virtual       ~FFTW           (void) ;

    virtual int    type           (void) const ;
    virtual bool   isSupported    (Cpp::ValueTypes T,int dimension) const ;
    virtual int    AllocateSource (CUIDs elements,QByteArray & Source) ;
    virtual int    AllocateTarget (CUIDs elements,QByteArray & Target) ;
    virtual bool   Prepare        (CUIDs        elements  ,
                                   VarArgs    & Paraments ,
                                   QByteArray & Source    ,
                                   QByteArray & Target  ) ;
    virtual void   Execution      (void) ;

    void           Cleanup        (void);

    fftw_complex * array          (int size);
    fftw_complex * fromByteArray  (int & size,QByteArray & Array);
    QByteArray     toByteArray    (fftw_complex * complex,int size);
    void           Execute        (const fftw_plan plan);
    void           Destroy        (fftw_plan plan);

    fftw_plan      Frequency      (int N,double * data,fftw_complex * dout,unsigned int flags = FFTW_MEASURE) ;
    void           Retrieve       (int component,int index,int length,double * data,fftw_complex * dout) ;

    void           Execute        (const fftwf_plan plan);
    void           Destroy        (fftwf_plan plan);

    #ifndef DISABLE_FFTW_LDOUBLE
    void           Execute        (const fftwl_plan plan);
    void           Destroy        (fftwl_plan plan);
    #endif

  protected:

    fftw_plan  fftplan  ;
    fftwf_plan fftfplan ;
    #ifndef DISABLE_FFTW_LDOUBLE
    fftwl_plan fftlplan ;
    #endif

  private:

    int fftSize                   (int fft,int length) ;
    int fftSize                   (int fft,CUIDs elements) ;

};

class Q_FFT_EXPORT AccelerateFFT : public AbstractFFT
{ // Apple iPhone OS Accelerate Framework
  public:

    explicit     AccelerateFFT  (void) ;
    virtual     ~AccelerateFFT  (void) ;

    virtual int  type           (void) const ;
    virtual bool isSupported    (Cpp::ValueTypes T,int dimension) const ;
    virtual int  AllocateSource (CUIDs elements,QByteArray & Source) ;
    virtual int  AllocateTarget (CUIDs elements,QByteArray & Target) ;
    virtual bool Prepare        (CUIDs        elements  ,
                                 VarArgs    & Paraments ,
                                 QByteArray & Source    ,
                                 QByteArray & Target  ) ;
    virtual void Execution      (void) ;

  protected:

  private:

};

}

QT_END_NAMESPACE

Q_DECLARE_METATYPE(N::Wavelet)
Q_DECLARE_METATYPE(N::Hankel)
Q_DECLARE_METATYPE(N::CooleyFFT)
Q_DECLARE_METATYPE(N::KissFFT)
Q_DECLARE_METATYPE(N::GSLFFT)
Q_DECLARE_METATYPE(N::FFTW)
Q_DECLARE_METATYPE(N::AccelerateFFT)

Q_DECLARE_INTERFACE(N::AbstractFFT  , "com.neutrino.math.fft" )

#endif


/* linalg/trimult.c
 *
 * Copyright (C) 2019 Patrick Alken
 *
 * This is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation; either version 3, or (at your option) any
 * later version.
 *
 * This source 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.
 *
 * This module contains code to compute L^T L where L is a lower triangular matrix
 */

#include <config.h>

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

#include "recurse.h"

static int triangular_multsymm_L2(CBLAS_UPLO_t Uplo, gsl_matrix * T);
static int triangular_multsymm_L3(CBLAS_UPLO_t Uplo, gsl_matrix * T);
static int triangular_mult_L2(CBLAS_UPLO_t Uplo, gsl_matrix * A);
static int triangular_mult_L3(CBLAS_UPLO_t Uplo, gsl_matrix * A);

int
gsl_linalg_tri_LTL(gsl_matrix * L)
{
  return triangular_multsymm_L3(CblasLower, L);
}

int
gsl_linalg_tri_UL(gsl_matrix * LU)
{
  return triangular_mult_L3(CblasUpper, LU);
}

/*
triangular_multsymm_L2()
  Compute L^T L or U U^T

Inputs: Uplo - CblasUpper or CblasLower
        T    - on output the upper (or lower) part of T
               is replaced by L^T L or U U^T

Return: success/error

Notes:
1) Based on LAPACK routine DLAUU2 using Level 2 BLAS
*/

static int
triangular_multsymm_L2(CBLAS_UPLO_t Uplo, gsl_matrix * T)
{
  const size_t N = T->size1;

  if (N != T->size2)
    {
      GSL_ERROR ("matrix must be square", GSL_ENOTSQR);
    }
  else
    {
      gsl_vector_view v1, v2;
      size_t i;

      if (Uplo == CblasUpper)
        {
        }
      else
        {
          for (i = 0; i < N; ++i)
            {
              double Tii = gsl_matrix_get(T, i, i);

              if (i < N - 1)
                {
                  double tmp;

                  v1 = gsl_matrix_subcolumn(T, i, i, N - i);
                  gsl_blas_ddot(&v1.vector, &v1.vector, &tmp);
                  gsl_matrix_set(T, i, i, tmp);

                  if (i > 0)
                    {
                      gsl_matrix_view m = gsl_matrix_submatrix(T, i + 1, 0, N - i - 1, i);

                      v1 = gsl_matrix_subcolumn(T, i, i + 1, N - i - 1);
                      v2 = gsl_matrix_subrow(T, i, 0, i);

                      gsl_blas_dgemv(CblasTrans, 1.0, &m.matrix, &v1.vector, Tii, &v2.vector);
                    }
                }
              else
                {
                  v1 = gsl_matrix_row(T, N - 1);
                  gsl_blas_dscal(Tii, &v1.vector);
                }
            }
        }

      return GSL_SUCCESS;
    }
}

/*
triangular_multsymm_L3()
  Compute L^T L or U U^T

Inputs: Uplo - CblasUpper or CblasLower
        T    - on output the upper (or lower) part of T
               is replaced by L^T L or U U^T

Return: success/error

Notes:
1) Based on ReLAPACK routine DLAUUM using Level 3 BLAS
*/

static int
triangular_multsymm_L3(CBLAS_UPLO_t Uplo, gsl_matrix * T)
{
  const size_t N = T->size1;

  if (N != T->size2)
    {
      GSL_ERROR ("matrix must be square", GSL_ENOTSQR);
    }
  else if (N <= CROSSOVER_TRIMULT)
    {
      return triangular_multsymm_L2(Uplo, T);
    }
  else
    {
      /* partition matrix:
       *
       * T11 T12
       * T21 T22
       *
       * where T11 is N1-by-N1
       */
      int status;
      const size_t N1 = GSL_LINALG_SPLIT(N);
      const size_t N2 = N - N1;
      gsl_matrix_view T11 = gsl_matrix_submatrix(T, 0, 0, N1, N1);
      gsl_matrix_view T12 = gsl_matrix_submatrix(T, 0, N1, N1, N2);
      gsl_matrix_view T21 = gsl_matrix_submatrix(T, N1, 0, N2, N1);
      gsl_matrix_view T22 = gsl_matrix_submatrix(T, N1, N1, N2, N2);

      /* recursion on T11 */
      status = triangular_multsymm_L3(Uplo, &T11.matrix);
      if (status)
        return status;

      if (Uplo == CblasLower)
        {
          /* T11 += T21^T T21 */
          gsl_blas_dsyrk(Uplo, CblasTrans, 1.0, &T21.matrix, 1.0, &T11.matrix);

          /* T21 = T22^T * T21 */
          gsl_blas_dtrmm(CblasLeft, Uplo, CblasTrans, CblasNonUnit, 1.0, &T22.matrix, &T21.matrix);
        }
      else
        {
          /* T11 += T12 T12^T */
          gsl_blas_dsyrk(Uplo, CblasNoTrans, 1.0, &T12.matrix, 1.0, &T11.matrix);

          /* T12 = T12 * T22^T */
          gsl_blas_dtrmm(CblasRight, Uplo, CblasTrans, CblasNonUnit, 1.0, &T22.matrix, &T12.matrix);
        }

      /* recursion on T22 */
      status = triangular_multsymm_L3(Uplo, &T22.matrix);
      if (status)
        return status;

      return GSL_SUCCESS;
    }
}

/*
triangular_mult_L2()
  Compute U L or L U

Inputs: Uplo - CblasUpper or CblasLower (for the first triangular factor)
        A    - on input, matrix in LU format;
               on output, U L or L U

Return: success/error
*/

static int
triangular_mult_L2(CBLAS_UPLO_t Uplo, gsl_matrix * A)
{
  const size_t N = A->size1;

  if (N != A->size2)
    {
      GSL_ERROR ("matrix must be square", GSL_ENOTSQR);
    }
  else
    {
      size_t i;

      /* quick return */
      if (N == 1)
        return GSL_SUCCESS;

      if (Uplo == CblasUpper)
        {
          /* compute U * L and store in A */

          for (i = 0; i < N; ++i)
            {
              double * Aii = gsl_matrix_ptr(A, i, i);
              double Uii = *Aii;

              if (i < N - 1)
                {
                  gsl_vector_view lb = gsl_matrix_subcolumn(A, i, i + 1, N - i - 1);
                  gsl_vector_view ur = gsl_matrix_subrow(A, i, i + 1, N - i - 1);
                  double tmp;

                  gsl_blas_ddot(&lb.vector, &ur.vector, &tmp);
                  *Aii += tmp;

                  if (i > 0)
                    {
                      gsl_matrix_view U_TR = gsl_matrix_submatrix(A, 0, i + 1, i, N - i - 1);
                      gsl_matrix_view L_BL = gsl_matrix_submatrix(A, i + 1, 0, N - i - 1, i);
                      gsl_vector_view ut = gsl_matrix_subcolumn(A, i, 0, i);
                      gsl_vector_view ll = gsl_matrix_subrow(A, i, 0, i);

                      gsl_blas_dgemv(CblasTrans, 1.0, &L_BL.matrix, &ur.vector, Uii, &ll.vector);
                      gsl_blas_dgemv(CblasNoTrans, 1.0, &U_TR.matrix, &lb.vector, 1.0, &ut.vector);
                    }
                }
              else
                {
                  gsl_vector_view v = gsl_matrix_subrow(A, N - 1, 0, N - 1);
                  gsl_blas_dscal(Uii, &v.vector);
                }
            }
        }
      else
        {
        }

      return GSL_SUCCESS;
    }
}

/*
triangular_mult_L3()
  Compute U L or L U

Inputs: Uplo - CblasUpper or CblasLower (for the first triangular factor)
        A    - on input, matrix in LU format;
               on output, U L or L U

Return: success/error
*/

static int
triangular_mult_L3(CBLAS_UPLO_t Uplo, gsl_matrix * A)
{
  const size_t N = A->size1;

  if (N != A->size2)
    {
      GSL_ERROR ("matrix must be square", GSL_ENOTSQR);
    }
  else if (N <= CROSSOVER_TRIMULT)
    {
      return triangular_mult_L2(Uplo, A);
    }
  else
    {
      /* partition matrix:
       *
       * A11 A12
       * A21 A22
       *
       * where A11 is N1-by-N1
       */
      int status;
      const size_t N1 = GSL_LINALG_SPLIT(N);
      const size_t N2 = N - N1;
      gsl_matrix_view A11 = gsl_matrix_submatrix(A, 0, 0, N1, N1);
      gsl_matrix_view A12 = gsl_matrix_submatrix(A, 0, N1, N1, N2);
      gsl_matrix_view A21 = gsl_matrix_submatrix(A, N1, 0, N2, N1);
      gsl_matrix_view A22 = gsl_matrix_submatrix(A, N1, N1, N2, N2);

      /* recursion on A11 */
      status = triangular_mult_L3(Uplo, &A11.matrix);
      if (status)
        return status;

      if (Uplo == CblasLower)
        {
        }
      else
        {
          /* form U * L */

          /* A11 += A12 A21 */
          gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &A12.matrix, &A21.matrix, 1.0, &A11.matrix);

          /* A12 = A12 * L22 */
          gsl_blas_dtrmm(CblasRight, CblasLower, CblasNoTrans, CblasUnit, 1.0, &A22.matrix, &A12.matrix);

          /* A21 = U22 * A21 */
          gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, &A22.matrix, &A21.matrix);
        }

      /* recursion on A22 */
      status = triangular_mult_L3(Uplo, &A22.matrix);
      if (status)
        return status;

      return GSL_SUCCESS;
    }
}


/*
  This file is part of the Astrometry.net suite.
  Copyright 2006-2008 Dustin Lang, Keir Mierle and Sam Roweis.

  The Astrometry.net suite 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, version 2.

  The Astrometry.net suite 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 the Astrometry.net suite ; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
*/

#ifndef AN_GSL_UTILS_H
#define AN_GSL_UTILS_H

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

void gslutils_use_error_system();

/**
 Solves a least-squares matrix equation
 A X_i = B_i

 For NB pairs of X_i, B_i.

 NOTE: THIS DESTROYS A!

 A: MxN matrix
 B: array of NB x length-M vectors
 X: must be an array big enough to hold NB vectors.
   (they will be length-N).
 resids: if non-NULL, must be an array big enough to hold NB vectors
   (they will be length-M).

 The result vectors are freshly allocated and should be freed with gsl_vector_free().
 */
int gslutils_solve_leastsquares(gsl_matrix* A, gsl_vector** B,
                                gsl_vector** X, gsl_vector** resids,
                                int NB);

/**
 Same as above, but using varargs.  There must be exactly 3 * NB additional
 arguments, in the order:

 B0, &X0, &resid0, B1, &X1, &resid1, ...

 ie, the types must be repeating triples of:

 gsl_vector* b, gsl_vector** x, gsl_vector** resid

 */
int gslutils_solve_leastsquares_v(gsl_matrix* A, int NB, ...);

// C = A B
void gslutils_matrix_multiply(gsl_matrix* C, const gsl_matrix* A, const gsl_matrix* B);

int gslutils_invert_3x3(const double* A, double* B);

#endif


#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_histogram.h>
#include"mpi.h"

const double PI = 3.14159;

double rz(double red);

struct galaxy {
  double x, y, z, d, w;
};

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

  FILE * DD;
  FILE * PP;
  FILE * RR;

  const int Nr = 150;
  const int Nm = 100;
  const double RMIN = 0.0;
  const double RMAX = 150.0;
  const double RSTEP = 1.0;
  const double MSTEP = 0.01;

  const int maxNgal = 40000000;

  int dummy;

  char DDname[400];
  char PPname[400];
  char RRname[400];

  int i;
  int bit;
  int bittot;

  int *n;

  sprintf(DDname,argv[1]);
  sprintf(RRname,argv[2]);
  sprintf(PPname,argv[3]);

  struct galaxy * gald;
  struct galaxy * galr;

  double * DRcount;
  double * DRcount_in_place;
  gald = (struct galaxy*)malloc(maxNgal*sizeof(struct galaxy));
  galr = (struct galaxy*)malloc(maxNgal*sizeof(struct galaxy));
  DRcount = (double*)calloc(Nr*Nm,sizeof(double));
  DRcount_in_place = (double*)calloc(Nr*Nm,sizeof(double));
  struct galaxy * galpd, * galpr;
  galpd = gald;
  galpr = galr;

  DD = fopen(DDname,"r");
  RR = fopen(RRname,"r");

  double ra, dec, red, dist;
  double dd;
  int Nd = 0;
  double weight;
  double ddummy;
  int indexFKP;
  double weightFKP;
  double weight1, weight2, weight3, weight4;
  char line [2000];

  int N = 0;
  double sumW = 0;
  double sumW_in_place = 0;

  for (int i = 0; i < 0; i ++) {
  fgets (line, 2000, DD);
  printf("%s\n", line);
  }
  while (fscanf(DD,"%lf %lf %lf\n",&ra,&dec,&red) != EOF) {
    ra *= PI/180.0;
    dec *= PI/180.0;
    dist = rz(red);
    galpd->x = dist*cos(dec)*cos(ra);
    galpd->y = dist*cos(dec)*sin(ra);
    galpd->z = dist*sin(dec);
    galpd->d = dist;
//    galpd->w = weight1 * weight2;
    galpd++;
    Nd++;
  }
  fclose(DD);

  N = 0;
  for (int i = 0; i < 0; i ++) {
  fgets (line, 2000, RR);
  printf("%s\n", line);
  }
  while (fscanf(RR,"%le %le %le\n",&ra,&dec,&red) != EOF) {
    ra *= PI/180.0;
    dec *= PI/180.0;
    dist = rz(red);
    galpr->x = dist*cos(dec)*cos(ra);
    galpr->y = dist*cos(dec)*sin(ra);
    galpr->z = dist*sin(dec);
    galpr->d = dist;
//    galpr->w = weight1 * weight2;
    galpr++;
    N++;
  }
  fclose(RR);

  MPI_Init (&argc, &argv);
  int numproc;
  int totproc;
  MPI_Comm_rank (MPI_COMM_WORLD, &numproc);
  MPI_Comm_size (MPI_COMM_WORLD, &totproc);

  n = (int *) calloc(totproc + 1, sizeof(int));
  n[0] = 1;
  for (i = 1; i < totproc; i++) {
    n[i] = (int)floor(double(N)/totproc*i);
  }
  n[totproc] = N;
  bit = n[numproc];
  bittot = n[numproc + 1];

  sprintf(PPname, "%s.DR",PPname);
  double mu;
  double d1, d2, d3;
  double x1, y1, z1, x2, y2, z2, gd1, gd2, w1, w2;
  double r2, rat;
  double xb, yb, zb, db2;
  double rr;
  galpd = gald;
  int j;

  for (i = 0; i < Nd; i++) {
    x1 = galpd->x; y1 = galpd->y; z1 = galpd->z; gd1 = galpd->d; //w1 = galpd->w;
    galpr = galr + bit;
    for (j = bit; j < bittot; j++) {
       x2 = galpr->x; y2 = galpr->y; z2 = galpr->z; gd2 = galpr->d; //w2 = galpr->w;
       //sumW += w1*w2;
       sumW += 1.0;
       d1 = x1 - x2;
       d2 = y1 - y2;
       d3 = z1 - z2;
       r2 = d1*d1 + d2*d2 + d3*d3;
       if (r2 > RMAX*RMAX || r2 < RMIN*RMIN) {
	 galpr++;
         continue;
       }
       rat = gd1/gd2;
       xb = x1 + x2*rat;
       yb = y1 + y2*rat;
       zb = z1 + z2*rat;
       db2 = xb*xb + yb*yb + zb*zb;
       mu = fabs((xb*d1 + yb*d2 + zb*d3)/sqrt(r2)/sqrt(db2));
       rr = sqrt(r2);
       int binr = (int)((rr - RMIN)/RSTEP);
       int binm = (int)(mu/MSTEP);
       if (binr >= 0 && binm >= 0 && binr < Nr && binm < Nm){
         int ind = binr + Nr*binm;
//         DRcount[ind] += w1*w2;
         DRcount[ind] += 1.0;
       }
       galpr++;
    }
    galpd++;
  }
  free(gald);
  free(galr);
  free(n);

  MPI_Reduce(DRcount, DRcount_in_place, Nr*Nm, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
  MPI_Reduce(&sumW, &sumW_in_place, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
  printf("weighted number = %le %d\n", sumW, numproc);
  if (numproc == 0) {
    PP = fopen(PPname,"w");
    printf("%s\n", PPname);
    fprintf(PP,"# weighted number: %lf\n",sumW_in_place);
    fprintf(PP,"# RBINS: %d\n", Nr);
    fprintf(PP,"# MBINS: %d\n", Nm);
    int k, l;
    for (k = 0; k < Nm; k++) {
      for (l = 0; l < Nr; l++) {
      fprintf(PP,"%lf ",DRcount_in_place[k*Nr + l]);
      }
      fprintf(PP,"\n");
    }
    fclose(PP);
  }

  free(DRcount);
  free(DRcount_in_place);

  MPI_Finalize ();

  return 0;
}

double f (double x, void * p) {
  double Om = 0.310;
  double ff = 2997.92458/sqrt(Om*(1.0 + x)*(1.0 + x)*(1.0 + x) + 1.0 - Om);
  return ff;
}

double rz (double red) {
  gsl_integration_workspace * w = gsl_integration_workspace_alloc(1000);
  double result, error;
  gsl_function F;
  F.function = &f;
  gsl_integration_qags(&F, 0, red, 0, 1e-7, 1000, w, &result, &error);
  gsl_integration_workspace_free (w);
  return result;
}


#ifndef FIT_GSL_H
#define FIT_GSL_H

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

class Fit;

//! Structure for fitting data
struct FitData {
  size_t n;// number of points to be fitted (size of X, Y and sigma arrays)
  size_t p;// number of fit parameters
  double * X;// the data to be fitted (abscissae)
  double * Y; // the data to be fitted (ordinates)
  double * sigma; // the weighting data
  Fit *fitter; //pointer to the fitter object (used only for the NonLinearFit class)
};

int expd3_fdf (const gsl_vector * x, void *params, gsl_vector * f, gsl_matrix * J);
int expd3_df (const gsl_vector * x, void *params, gsl_matrix * J);
int expd3_f (const gsl_vector * x, void *params, gsl_vector * f);
double expd3_d (const gsl_vector * x, void *params);

int expd2_fdf (const gsl_vector * x, void *params, gsl_vector * f, gsl_matrix * J);
int expd2_df (const gsl_vector * x, void *params, gsl_matrix * J);
int expd2_f (const gsl_vector * x, void *params, gsl_vector * f);
double expd2_d (const gsl_vector * x, void *params);

int exp_fdf (const gsl_vector * x, void *params, gsl_vector * f, gsl_matrix * J);
int exp_df (const gsl_vector * x, void *params, gsl_matrix * J);
int exp_f (const gsl_vector * x, void *params, gsl_vector * f);
double exp_d (const gsl_vector * x, void *params);

int boltzmann_fdf (const gsl_vector * x, void *params, gsl_vector * f, gsl_matrix * J);
int boltzmann_df (const gsl_vector * x, void *params, gsl_matrix * J);
int boltzmann_f (const gsl_vector * x, void *params, gsl_vector * f);
double boltzmann_d (const gsl_vector * x, void *params);

int logistic_fdf (const gsl_vector * x, void *params, gsl_vector * f, gsl_matrix * J);
int logistic_df (const gsl_vector * x, void *params, gsl_matrix * J);
int logistic_f (const gsl_vector * x, void *params, gsl_vector * f);
double logistic_d (const gsl_vector * x, void *params);

int gauss_fdf (const gsl_vector * x, void *params, gsl_vector * f, gsl_matrix * J);
int gauss_df (const gsl_vector * x, void *params, gsl_matrix * J);
int gauss_f (const gsl_vector * x, void *params,gsl_vector * f);
double gauss_d (const gsl_vector * x, void *params);

int gauss_multi_peak_f (const gsl_vector * x, void *params, gsl_vector * f);
double gauss_multi_peak_d (const gsl_vector * x, void *params);
int gauss_multi_peak_df (const gsl_vector * x, void *params, gsl_matrix * J);
int gauss_multi_peak_fdf (const gsl_vector * x, void *params, gsl_vector * f, gsl_matrix * J);

int lorentz_multi_peak_f (const gsl_vector * x, void *params, gsl_vector * f);
double lorentz_multi_peak_d (const gsl_vector * x, void *params);
int lorentz_multi_peak_df (const gsl_vector * x, void *params, gsl_matrix * J);
int lorentz_multi_peak_fdf (const gsl_vector * x, void *params, gsl_vector * f, gsl_matrix * J);

int user_f(const gsl_vector * x, void *params, gsl_vector * f);
double user_d(const gsl_vector * x, void *params);
int user_df(const gsl_vector * x, void *params,gsl_matrix * J);
int user_fdf(const gsl_vector * x, void *params,gsl_vector * f, gsl_matrix * J);

#endif


#include <petsc.h>
#include <petscvec.h>
#include <petscmat.h>
#include <petscksp.h>
#include "del2mat.h"

#define DEL2MAT_MULT   ((void(*)(void))Del2Mat_mult)
#define DEL2MAT_DIAG   ((void(*)(void))Del2Mat_diag)

int main(int argc,char **argv)
{
  PetscInt n;
  PetscScalar h;
  Del2Mat shell;
  Mat A;
  Vec x,b;
  KSP ksp;
  PC  pc;
  /* PETSc initialization  */
  PetscInitialize(&argc, &argv, PETSC_NULL, PETSC_NULL);
  /* number of nodes in each direction
   * excluding those at the boundary */
  n = 32;
  h = 1.0/(n+1); /* grid spacing */
  /* setup linear system (shell) matrix */
  MatCreate(PETSC_COMM_SELF, &A);
  MatSetSizes(A, n*n*n, n*n*n, n*n*n, n*n*n);
  MatSetType(A, MATSHELL);
  shell.N = n;
  PetscMalloc((n+2)*(n+2)*(n+2)*sizeof(PetscScalar),&shell.F);
  PetscMemzero(shell.F, (n+2)*(n+2)*(n+2)*sizeof(PetscScalar));
  MatShellSetContext(A, (void**)&shell);
  MatShellSetOperation(A, MATOP_MULT,           DEL2MAT_MULT);
  MatShellSetOperation(A, MATOP_MULT_TRANSPOSE, DEL2MAT_MULT);
  MatShellSetOperation(A, MATOP_GET_DIAGONAL,   DEL2MAT_DIAG);
  MatSetUp(A);
  /* setup linear system vectors */
  MatCreateVecs(A, &x, &b);
  VecSet(x, 0);
  VecSet(b, 1);
  /* setup Krylov linear solver */
  KSPCreate(PETSC_COMM_SELF, &ksp);
  KSPGetPC(ksp, &pc);
  KSPSetType(ksp, KSPCG); /* use conjugate gradients */
  PCSetType(pc, PCNONE);  /* with no preconditioning */
  KSPSetFromOptions(ksp);
  /* iteratively solve linear system of equations A*x=b */
  KSPSetOperators(ksp,A,A);
  KSPSolve(ksp, b, x);
  /* scale solution vector to account for grid spacing */
  VecScale(x, h*h);
  /* free memory and destroy objects */
  PetscFree(shell.F);
  VecDestroy(&x);
  VecDestroy(&b);
  MatDestroy(&A);
  KSPDestroy(&ksp);
  /* finalize PETSc */
  PetscFinalize();
  return 0;
}


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

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

/*
gsl_spmatrix_cumsum()

Compute the cumulative sum:

p[j] = Sum_{k=0...j-1} c[k]

0 <= j < n + 1

Alternatively,
p[0] = 0
p[j] = p[j - 1] + c[j - 1]

Inputs: n - length of input array
        c - (input/output) array of size n + 1
            on input, contains the n values c[k]
            on output, contains the n + 1 values p[j]

Return: success or error
*/

void
gsl_spmatrix_cumsum(const size_t n, int * c)
{
  int sum = 0;
  size_t k;

  for (k = 0; k < n; ++k)
    {
      int ck = c[k];
      c[k] = sum;
      sum += ck;
    }

  c[n] = sum;
}


#include <geometry/basic_c.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <math.h>
#include <stdio.h>

// author:    Andrew Liew <liew@arch.ethz.ch>
// copyright: Copyright 2018, BLOCK Research Group - ETH Zurich
// license:   MIT License
// email:     liew@arch.ethz.ch

void drx_solver_c(
    double tol,    // Tolerance value.
    int steps,     // Maximum number of steps.
    int summary,   // Print summary at end (1:yes or 0:no).
    int m,         // Number of elements.
    int n,         // Number of nodes.
    int *u,        // Element start node.
    int *v,        // Element end node.
    double *X,     // Nodal co-ordinates.
    double *f0,    // Initial edge forces.
    double *l0,    // Initial edge lengths.
    double *k0,    // Initial edge axial stiffnesses.
    int *ind_c,    // Indices of compression only edges.
    int *ind_t,    // Indices of tension only edges.
    int ind_c_n,   // Length of ind_c.
    int ind_t_n,   // Length of ind_t.
    double *B,     // Constraint conditions Bx, By, Bz.
    double *P,     // Nodal loads Px, Py, Pz.
    double *S,     // Shear forces Sx, Sy, Sz.
    int *rows,     // Rows of Ct.
    int *cols,     // Columns of Ct.
    double *vals,  // Values of Ct.
    int nv,        // Length of rows/cols/vals.
    double *M,     // Mass matrix.
    double factor, // Convergence factor.
    double *V,     // Nodal velocities Vx, Vy, Vz.
    int *inds,     // Indices of beam element start nodes.
    int *indi,     // Indices of beam element intermediate nodes.
    int *indf,     // Indices of beam element finish nodes beams.
    double *EIx,   // Nodal EIx flexural stiffnesses.
    double *EIy,   // Nodal EIy flexural stiffnesses.
    int beams,     // Includes beams:1 or not:0.
    int nb)        // Length of inds/indi/indf.

    {

    int a;
    int b;
    int c;
    int i;
    int j;
    int k;
    int nans[nb];
    int ts;

    double alpha;
    double f[m];
    double fx[m];
    double fy[m];
    double fz[m];
    double frx[n];
    double fry[n];
    double frz[n];
    double kappa;
    double l;
    double La;
    double Lb;
    double Lc;
    double LQn;
    double Lmu;
    double Lc1;
    double Lc2;
    double Mi;
    double Ms;
    double q;
    double res;
    double Rx;
    double Ry;
    double Rz;
    double Rn;
    double Uo;
    double Un;
    double xd;
    double yd;
    double zd;

    gsl_vector *Xs = gsl_vector_alloc(3);
    gsl_vector *Xi = gsl_vector_alloc(3);
    gsl_vector *Xf = gsl_vector_alloc(3);
    gsl_vector *Qa = gsl_vector_alloc(3);
    gsl_vector *Qb = gsl_vector_alloc(3);
    gsl_vector *Qc = gsl_vector_alloc(3);
    gsl_vector *Qn = gsl_vector_alloc(3);
    gsl_vector *mu = gsl_vector_alloc(3);
    gsl_vector *ex = gsl_vector_alloc(3);
    gsl_vector *ey = gsl_vector_alloc(3);
    gsl_vector *ez = gsl_vector_alloc(3);
    gsl_vector *K  = gsl_vector_alloc(3);
    gsl_vector *Kx = gsl_vector_alloc(3);
    gsl_vector *Ky = gsl_vector_alloc(3);
    gsl_vector *Mc = gsl_vector_alloc(3);
    gsl_vector *ua = gsl_vector_alloc(3);
    gsl_vector *ub = gsl_vector_alloc(3);
    gsl_vector *c1 = gsl_vector_alloc(3);
    gsl_vector *c2 = gsl_vector_alloc(3);
    gsl_matrix *Sa = gsl_matrix_alloc(nb, 3);
    gsl_matrix *Sb = gsl_matrix_alloc(nb, 3);

    ts  = 0;
    Uo  = 0.;
    res = 1000. * tol;

    while (ts <= steps && res > tol)
    {
        #pragma omp parallel for private(i,j,k,xd,yd,zd,l,q)
        for (i = 0; i < m; i++)
        {
            j = 3 * v[i];
            k = 3 * u[i];
            xd = X[j + 0] - X[k + 0];
            yd = X[j + 1] - X[k + 1];
            zd = X[j + 2] - X[k + 2];
            l  = gsl_hypot3(xd, yd, zd);
            f[i] = f0[i] + k0[i] * (l - l0[i]);
            q = f[i] / l;
            fx[i] = xd * q;
            fy[i] = yd * q;
            fz[i] = zd * q;
        }

        if (ind_t_n > 0)
        {
            #pragma omp parallel for private(i)
            for (i = 0; i < ind_t_n; i++)
            {
                if (f[i] < 0)
                {
                    fx[i] = 0;
                    fy[i] = 0;
                    fz[i] = 0;
                }
            }
        }

        if (ind_c_n > 0)
        {
            #pragma omp parallel for private(i)
            for (i = 0; i < ind_c_n; i++)
            {
                if (f[i] > 0)
                {
                    fx[i] = 0;
                    fy[i] = 0;
                    fz[i] = 0;
                }
            }
        }

        if (beams)
        {
            #pragma omp parallel for private(i,j)
            for (i = 0; i < n; i++)
            {
                j = i * 3;
                S[j + 0] = 0.;
                S[j + 1] = 0.;
                S[j + 2] = 0.;
            }

            for (i = 0; i < nb; i++)
            {
                a = inds[i] * 3;
                b = indi[i] * 3;
                c = indf[i] * 3;

                vector_from_pointer(&X[a], Xs);
                vector_from_pointer(&X[b], Xi);
                vector_from_pointer(&X[c], Xf);
                subtract_vectors(Xi, Xs, Qa);
                subtract_vectors(Xf, Xi, Qb);
                subtract_vectors(Xf, Xs, Qc);
                cross_vectors(Qa, Qb, Qn);
                subtract_vectors(Xf, Xs, mu);
                scale_vector(mu, 0.5);

                La  = length_vector(Qa);
                Lb  = length_vector(Qb);
                Lc  = length_vector(Qc);
                LQn = length_vector(Qn);
                Lmu = length_vector(mu);
                alpha = acos((gsl_pow_2(La) + gsl_pow_2(Lb) - gsl_pow_2(Lc)) / (2. * La * Lb));
                kappa = 2. * sin(alpha) / Lc;

                gsl_vector_memcpy(ex, Qn);
                gsl_vector_memcpy(ez, mu);
                scale_vector(ex, 1./LQn);
                scale_vector(ez, 1./Lmu);
                cross_vectors(ez, ex, ey);
                gsl_vector_memcpy(K, Qn);
                scale_vector(K, kappa/LQn);
                gsl_vector_memcpy(Kx, ex);
                gsl_vector_memcpy(Ky, ey);
                scale_vector(Kx, dot_vectors(K, ex));
                scale_vector(Ky, dot_vectors(K, ey));
                scale_vector(Kx, EIx[i]);
                scale_vector(Ky, EIy[i]);
                add_vectors(Kx, Ky, Mc);
                cross_vectors(Mc, Qa, ua);
                cross_vectors(Mc, Qb, ub);
                normalize_vector(ua);
                normalize_vector(ub);
                cross_vectors(Qa, ua, c1);
                cross_vectors(Qb, ub, c2);

                Lc1 = length_vector(c1);
                Lc2 = length_vector(c2);
                Ms  = length_vector_squared(Mc);
                scale_vector(ua, Ms * Lc1 / (La * dot_vectors(Mc, c1)));
                scale_vector(ub, Ms * Lc2 / (Lb * dot_vectors(Mc, c2)));

                for (j = 0; j < 3; j++)
                {
                    gsl_matrix_set(Sa, i, j, gsl_vector_get(ua, j));
                    gsl_matrix_set(Sb, i, j, gsl_vector_get(ub, j));
                }
            }

            #pragma omp parallel for private(i,j)
            for (i = 0; i < nb; i++)
            {
                nans[i] = 0;
                for (j = 0; j < 3; j++)
                {
                    if (gsl_isnan(gsl_matrix_get(Sa, i, j)) || gsl_isnan(gsl_matrix_get(Sb, i, j)))
                    {
                        nans[i] = 1;
                        break;
                    }
                }
            }

            for (i = 0; i < nb; i++)
            {
                a = inds[i] * 3;
                b = indi[i] * 3;
                c = indf[i] * 3;

                if (nans[i] == 0)
                {
                    for (j = 0; j < 3; j++)
                    {
                        S[a + j] += gsl_matrix_get(Sa, i, j);
                        S[b + j] -= (gsl_matrix_get(Sa, i, j) + gsl_matrix_get(Sb, i, j));
                        S[c + j] += gsl_matrix_get(Sb, i, j);
                    }
                }
            }
        }

        #pragma omp parallel for private(i)
        for (i = 0; i < n; i++)
        {
            frx[i] = 0;
            fry[i] = 0;
            frz[i] = 0;
        }

        for (i = 0; i < nv; i++)
        {
            frx[rows[i]] += vals[i] * fx[cols[i]];
            fry[rows[i]] += vals[i] * fy[cols[i]];
            frz[rows[i]] += vals[i] * fz[cols[i]];
        }

        Un = 0.;
        Rn = 0.;

        #pragma omp parallel for private(i,j,Rx,Ry,Rz,Mi) reduction(+:Un,Rn)
        for (i = 0; i < n; i++)
        {
            j = 3 * i;
            Rx = (P[j + 0] - S[j + 0] - frx[i]) * B[j + 0];
            Ry = (P[j + 1] - S[j + 1] - fry[i]) * B[j + 1];
            Rz = (P[j + 2] - S[j + 2] - frz[i]) * B[j + 2];
            Rn += gsl_hypot3(Rx, Ry, Rz);
            Mi = M[i] * factor;
            V[j + 0] += Rx / Mi;
            V[j + 1] += Ry / Mi;
            V[j + 2] += Rz / Mi;
            Un += Mi * (gsl_pow_2(V[j + 0]) + gsl_pow_2(V[j + 1]) + gsl_pow_2(V[j + 2]));
        }

        if (Un < Uo)
        {
            #pragma omp parallel for private(i,j)
            for (i = 0; i < n; i++)
            {
                j = 3 * i;
                V[j + 0] = 0.;
                V[j + 1] = 0.;
                V[j + 2] = 0.;
            }
        }

        Uo = Un;

        #pragma omp parallel for private(i,j)
        for (i = 0; i < n; i++)
        {
            j = 3 * i;
            X[j + 0] += V[j + 0];
            X[j + 1] += V[j + 1];
            X[j + 2] += V[j + 2];
        }

        res = Rn / n;
        ts++;

    }

    if (summary == 1)
    {
        printf("Step: %i, Residual: %f\n", ts - 1, res);
    }

}


#include <math.h>
#include <gsl/gsl_linalg.h>
#include "getNextIterate.h"

void getNextIterate(const grid *grd, const bool eos_func,
		    const wksp *w
#if AA_M > 0
		    , const unsigned long itCount, const long nHist,
		    double *residMax
#endif
		    ) {

  long i;
#if AA_M > 0
  long j, mk, colptr;
  gsl_matrix *resid;
  gsl_vector *tau, *llsresid, *aa_wgts, *constraint;
  gsl_vector_view constraint_view;
#endif

#if AA_M == 0
/* No Anderson acceleration, so just move temporaries into next
   guess */
  for (i=0; i<grd->nr; i++) {
    w->colNew[i] = w->colTmp[i];
    w->presNew_g[i+1] = gsl_vector_get(w->presTmp_g, i+1);
    if (eos_func==1)
      w->eIntNew[i] = w->eIntTmp[i];
  }
#else

  /* Anderson acceleration code */

  /* Set up pointers */
  mk = itCount < AA_M+1 ? itCount : AA_M+1;
  colptr = (itCount - 1) % (AA_M+1);

  /* Store the current guesses in the history array */
  for (i=0; i<grd->nr; i++) {
    w->colHist[i + colptr*grd->nr] = w->colTmp[i];
    w->presHist[i + colptr*grd->nr] = gsl_vector_get(w->presTmp_g, i+1);
    if (eos_func)
      w->eIntHist[i + colptr*grd->nr] = w->eIntTmp[i];
  }

  /* Compute residuals and store them in the residual matrix */
  for (i=0; i<grd->nr; i++) {
    w->colResid[i + colptr*grd->nr] =
      (w->colHist[i+colptr*grd->nr]-w->colNew[i]) /
      w->colHist[i+colptr*grd->nr];
    if (fabs(w->colResid[i + colptr*grd->nr]) > *residMax) {
      *residMax = fabs(w->colResid[i + colptr*grd->nr]);
    }
    w->presResid[i + colptr*grd->nr] =
      (w->presHist[i+colptr*grd->nr]-w->presNew_g[i+1]) /
      w->presHist[i+colptr*grd->nr];
    if (fabs(w->presResid[i + colptr*grd->nr]) > *residMax) {
      *residMax = fabs(w->presResid[i + colptr*grd->nr]);
    }
    if (eos_func) {
      w->eIntResid[i + colptr*grd->nr] =
	(w->eIntHist[i+colptr*grd->nr]-w->eIntNew[i]) /
	w->eIntHist[i+colptr*grd->nr];
      if (fabs(w->eIntResid[i + colptr*grd->nr]) > *residMax)
	*residMax = fabs(w->eIntResid[i + colptr*grd->nr]);
    }
  }

  /* If we're in the first iteration, set move temporaries to next
     guess */
  if (itCount == 1) {
    for (i=0; i<grd->nr; i++) {
      w->colNew[i] = w->colHist[i+colptr*grd->nr];
      w->presNew_g[i+1] = w->presHist[i+colptr*grd->nr];
      if (eos_func)
	w->eIntNew[i] = w->eIntHist[i+colptr*grd->nr];
    }
  } else {
    /* On subsequent iterations, solve the linear least squares
       optimization problem to get a weighted sum of previous
       guesses for the new guess */

    /* Allocate workspace for this iteration */
    resid = gsl_matrix_alloc(nHist+1, mk);
    llsresid = gsl_vector_alloc(nHist+1);
    aa_wgts = gsl_vector_alloc(mk);
    if (nHist+1 < mk) {
      tau = gsl_vector_alloc(nHist+1);
    } else {
      tau = gsl_vector_alloc(mk);
    }

    /* Build matrix of residuals */
    for (i=0; i<grd->nr; i++) {
      for (j=0; j<mk; j++) {
	gsl_matrix_set(resid, i, j, w->colResid[i+j*grd->nr]);
	gsl_matrix_set(resid, i+grd->nr, j, w->presResid[i+j*grd->nr]);
	if (eos_func)
	  gsl_matrix_set(resid, i+2*grd->nr, j, w->eIntResid[i+j*grd->nr]);
      }
    }

    /* Set up constraints on coefficients */
    for (j=0; j<mk; j++) gsl_matrix_set(resid, nHist, j, 1.0e3*(*residMax));
    constraint_view = gsl_vector_subvector(w->constraint, 0, nHist+1);
    constraint = &(constraint_view.vector);
    gsl_vector_set(constraint, nHist, 1e3*(*residMax));

    /* Solve linear least squares system by QR decomposition */
    gsl_linalg_QR_decomp(resid, tau);
    gsl_linalg_QR_lssolve(resid, tau, constraint, aa_wgts, llsresid);

    /* Build the new guess from the computed weights */
    for (i=0; i<grd->nr; i++) {
      w->colNew[i] =
	gsl_vector_get(aa_wgts, 0)*w->colHist[i];
      w->presNew_g[i+1] =
	gsl_vector_get(aa_wgts, 0)*w->presHist[i];
      if (eos_func)
	w->eIntNew[i] =
	  gsl_vector_get(aa_wgts, 0)*w->eIntHist[i];
    }
    for (j=1; j<mk; j++) {
      for (i=0; i<grd->nr; i++) {
	w->colNew[i] += gsl_vector_get(aa_wgts, j) *
	  w->colHist[i+j*grd->nr];
	w->presNew_g[i+1] += gsl_vector_get(aa_wgts, j) *
	  w->presHist[i+j*grd->nr];
	if (eos_func)
	  w->eIntNew[i] += gsl_vector_get(aa_wgts, j) *
	    w->eIntHist[i+j*grd->nr];
      }
    }

    /* Free memory for this iteration */
    gsl_vector_free(llsresid);
    gsl_vector_free(tau);
    gsl_vector_free(aa_wgts);
    gsl_matrix_free(resid);
  }

#endif
  /* End Anderson acceleration code */
}


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

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

#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_filter.h>
#include <gsl/gsl_poly.h>

/* maximum derivative order allowed for Gaussian filter */
#define GSL_FILTER_GAUSSIAN_MAX_ORDER     10

typedef double gaussian_type_t;
typedef double ringbuf_type_t;
#include "ringbuf.c"

typedef struct
{
  size_t n;        /* window size */
  double * window; /* linear array with current window */
  ringbuf * rbuf;  /* ring buffer storing current window */
} gaussian_state_t;

static size_t gaussian_size(const size_t n);
static int gaussian_init(const size_t n, void * vstate);
static int gaussian_insert(const gaussian_type_t x, void * vstate);
static int gaussian_delete(void * vstate);
static int gaussian_get(void * params, gaussian_type_t * result, const void * vstate);

static const gsl_movstat_accum gaussian_accum_type;

/*
gsl_filter_gaussian_alloc()
  Allocate a workspace for Gaussian filtering.

Inputs: K - number of samples in window; if even, it is rounded up to
            the next odd, to have a symmetric window

Return: pointer to workspace
*/

gsl_filter_gaussian_workspace *
gsl_filter_gaussian_alloc(const size_t K)
{
  const size_t H = K / 2;
  gsl_filter_gaussian_workspace *w;
  size_t state_size;

  w = calloc(1, sizeof(gsl_filter_gaussian_workspace));
  if (w == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for workspace", GSL_ENOMEM);
    }

  w->K = 2 * H + 1;

  w->kernel = malloc(w->K * sizeof(double));
  if (w->kernel == 0)
    {
      gsl_filter_gaussian_free(w);
      GSL_ERROR_NULL ("failed to allocate space for kernel", GSL_ENOMEM);
      return NULL;
    }

  state_size = gaussian_size(w->K);

  w->movstat_workspace_p = gsl_movstat_alloc_with_size(state_size, H, H);
  if (!w->movstat_workspace_p)
    {
      gsl_filter_gaussian_free(w);
      GSL_ERROR_NULL ("failed to allocate space for movstat workspace", GSL_ENOMEM);
    }

  return w;
}

void
gsl_filter_gaussian_free(gsl_filter_gaussian_workspace * w)
{
  if (w->kernel)
    free(w->kernel);

  if (w->movstat_workspace_p)
    gsl_movstat_free(w->movstat_workspace_p);

  free(w);
}

/*
gsl_filter_gaussian()
  Apply a Gaussian filter to an input vector:

G_{sigma}(x) = exp [ -x^2 / (2 sigma^2) ]

Inputs: alpha - number of standard deviations to include in Gaussian kernel
        order - derivative order of Gaussian
        x     - input vector, size n
        y     - (output) filtered vector, size n
        w     - workspace

Notes:
1) If alpha = 3, then the Gaussian kernel will be a Gaussian of +/- 3 standard deviations
*/

int
gsl_filter_gaussian(const gsl_filter_end_t endtype, const double alpha, const size_t order, const gsl_vector * x,
                    gsl_vector * y, gsl_filter_gaussian_workspace * w)
{
  if (x->size != y->size)
    {
      GSL_ERROR("input and output vectors must have same length", GSL_EBADLEN);
    }
  else if (alpha <= 0.0)
    {
      GSL_ERROR("alpha must be positive", GSL_EDOM);
    }
  else
    {
      int status;
      gsl_vector_view kernel = gsl_vector_view_array(w->kernel, w->K);

      /* construct Gaussian kernel of length K */
      gsl_filter_gaussian_kernel(alpha, order, 1, &kernel.vector);

      status = gsl_movstat_apply_accum(endtype, x, &gaussian_accum_type, (void *) w->kernel, y,
                                       NULL, w->movstat_workspace_p);

      return status;
    }
}

/*
gsl_filter_gaussian_kernel()
  Construct Gaussian kernel with given sigma and order

Inputs: alpha     - number of standard deviations to include in window
        order     - kernel order (0 = gaussian, 1 = first derivative, ...)
        normalize - normalize so sum(G) = 1
        kernel    - (output) Gaussian kernel

Return: success/error

Notes:
1) If alpha = 3, then the output kernel will contain a Gaussian with +/- 3 standard deviations
*/

int
gsl_filter_gaussian_kernel(const double alpha, const size_t order, const int normalize, gsl_vector * kernel)
{
  const size_t N = kernel->size;

  if (alpha <= 0.0)
    {
      GSL_ERROR("alpha must be positive", GSL_EDOM);
    }
  else if (order > GSL_FILTER_GAUSSIAN_MAX_ORDER)
    {
      GSL_ERROR("derivative order is too large", GSL_EDOM);
    }
  else
    {
      const double half = 0.5 * (N - 1.0); /* (N - 1) / 2 */
      double sum = 0.0;
      size_t i;

      /* check for quick return */
      if (N == 1)
        {
          if (order == 0)
            gsl_vector_set(kernel, 0, 1.0);
          else
            gsl_vector_set(kernel, 0, 0.0);

          return GSL_SUCCESS;
        }

      for (i = 0; i < N; ++i)
        {
          double xi = ((double)i - half) / half;
          double yi = alpha * xi;
          double gi = exp(-0.5 * yi * yi);

          gsl_vector_set(kernel, i, gi);
          sum += gi;
        }

      /* normalize so sum(kernel) = 1 */
      if (normalize)
        gsl_vector_scale(kernel, 1.0 / sum);

      if (order > 0)
        {
          const double beta = -0.5 * alpha * alpha;
          double q[GSL_FILTER_GAUSSIAN_MAX_ORDER + 1];
          size_t k;

          /*
           * Need to calculate derivatives of the Gaussian window; define
           *
           * w(n) = C * exp [ p(n) ]
           *
           * p(n) = beta * n^2
           * beta = -1/2 * ( alpha / ((N-1)/2) )^2
           *
           * Then:
           *
           * d^k/dn^k w(n) = q_k(n) * w(n)
           *
           * where q_k(n) is a degree-k polynomial in n, which satisfies:
           *
           * q_k(n) = d/dn q_{k-1}(n) + q_{k-1}(n) * dp(n)/dn
           * q_0(n) = 1 / half^{order}
           */

          /* initialize q_0(n) = 1 / half^{order} */
          q[0] = 1.0 / gsl_pow_uint(half, order);
          for (i = 1; i <= GSL_FILTER_GAUSSIAN_MAX_ORDER; ++i)
            q[i] = 0.0;

          /* loop through derivative orders and calculate q_k(n) for k = 1,...,order */
          for (k = 1; k <= order; ++k)
            {
              double qm1 = q[0];

              q[0] = q[1];
              for (i = 1; i <= k; ++i)
                {
                  double tmp = q[i];
                  q[i] = (i + 1.0) * q[i + 1] + /* d/dn q_{k-1} */
                         2.0 * beta * qm1;      /* q_{k-1}(n) p'(n) */
                  qm1 = tmp;
                }
            }

          /* now set w(n) := q(n) * w(n) */
          for (i = 0; i < N; ++i)
            {
              double xi = ((double)i - half) / half;
              double qn = gsl_poly_eval(q, order + 1, xi);
              double *wn = gsl_vector_ptr(kernel, i);

              *wn *= qn;
            }
        }

      return GSL_SUCCESS;
    }
}

static size_t
gaussian_size(const size_t n)
{
  size_t size = 0;

  size += sizeof(gaussian_state_t);
  size += n * sizeof(gaussian_type_t);
  size += ringbuf_size(n);

  return size;
}

static int
gaussian_init(const size_t n, void * vstate)
{
  gaussian_state_t * state = (gaussian_state_t *) vstate;

  state->n = n;

  state->window = (gaussian_type_t *) ((unsigned char *) vstate + sizeof(gaussian_state_t));
  state->rbuf = (ringbuf *) ((unsigned char *) state->window + n * sizeof(gaussian_type_t));

  ringbuf_init(n, state->rbuf);

  return GSL_SUCCESS;
}

static int
gaussian_insert(const gaussian_type_t x, void * vstate)
{
  gaussian_state_t * state = (gaussian_state_t *) vstate;

  /* add new element to ring buffer */
  ringbuf_insert(x, state->rbuf);

  return GSL_SUCCESS;
}

static int
gaussian_delete(void * vstate)
{
  gaussian_state_t * state = (gaussian_state_t *) vstate;

  if (!ringbuf_is_empty(state->rbuf))
    ringbuf_pop_back(state->rbuf);

  return GSL_SUCCESS;
}

static int
gaussian_get(void * params, gaussian_type_t * result, const void * vstate)
{
  const gaussian_state_t * state = (const gaussian_state_t *) vstate;
  const double * kernel = (const double *) params;
  size_t n = ringbuf_copy(state->window, state->rbuf);
  double sum = 0.0;
  size_t i;

  for (i = 0; i < n; ++i)
    sum += state->window[i] * kernel[n - i - 1];

  *result = sum;

  return GSL_SUCCESS;
}

static const gsl_movstat_accum gaussian_accum_type =
{
  gaussian_size,
  gaussian_init,
  gaussian_insert,
  gaussian_delete,
  gaussian_get
};


/*
 * Copyright (c) 2018, Aleksi Kurkela, Aleksas Mazeliauskas, Jean-Francois
 * Paquet, Soeren Schlichting and Derek Teaney
 * All rights reserved.
 *
 * KoMPoST is distributed under MIT license;
 * see the LICENSE file that should be present in the root
 * of the source distribution, or alternately available at:
 * https://github.com/KMPST/KoMPoST/
 */

#include <gsl/gsl_math.h>
#include <iostream>
#ifndef M_HBARC
#define M_HBARC 0.197326979
#endif

namespace GreensFunctions {

class LowKArguments;

//////////////////////////////////////////////////

namespace EnergyPerturbations {

namespace FreeStreaming {
namespace CoordinateSpace {
double Gs(double dX, double dT);
double Gv(double dX, double dT);
double Gd(double dX, double dT);
double Gr(double dX, double dT);
}
} // FreeStreaming

namespace KineticTheory {
namespace CoordinateSpace {
double Gs(double dX, double dT, double ScalingVarOut);
double Gv(double dX, double dT, double ScalingVarOut);
double Gd(double dX, double dT, double ScalingVarOut);
double Gr(double dX, double dT, double ScalingVarOut);
}
} // KineticTheory

namespace LowKLimit {
namespace CoordinateSpace {
double Gs(double dX, double dT, const LowKArguments &in);
double Gv(double dX, double dT, const LowKArguments &in);
double Gd(double dX, double dT, const LowKArguments &in);
double Gr(double dX, double dT, const LowKArguments &in);
}
} // LowKLimit

} // EnergyPerturbations

//////////////////////////////////////////////////

namespace MomentumPerturbations {

namespace FreeStreaming {
namespace CoordinateSpace {
double Hv(double dX, double dT);
double Hd(double dX, double dT);
double Hr(double dX, double dT);
double Htd(double dX, double dT);
double Htm(double dX, double dT);
double Htr(double dX, double dT);
}
} // FreeStreaming

namespace KineticTheory {
namespace CoordinateSpace {
double Hv(double dX, double dT, double ScalingVarOut);
double Hd(double dX, double dT, double ScalingVarOut);
double Hr(double dX, double dT, double ScalingVarOut);
double Htd(double dX, double dT, double ScalingVarOut);
double Htm(double dX, double dT, double ScalingVarOut);
double Htr(double dX, double dT, double ScalingVarOut);
}
} // Kinetic Theory

} // MomentumPerturbations

//////////////////////////////////////////////////

void Setup(double Sigma, int NumberOfPoints, int ENERGY_PERTURBATIONS,
           int MOMENTUM_PERTURBATIONS);

void Output(int ENERGY_PERTURBATIONS, int MOMENTUM_PERTURBATIONS);

class LowKArguments {

  // private:
public:
  // Number of DOF
  const int NuG = 16;

  double tauF;  // Final time in fm
  double tauIn; // Initial time in fm

  double E0;
  double EF;

  double TXX0;
  double TXXF;

  double EtaByS;
  double SigmaBG; // Sigma in fm

public:
  LowKArguments(double tf, double tin, double e0, double ef, double txx0,
                double txxf, double etabys, double sigmabg)
      : tauF(tf), tauIn(tin), E0(e0), EF(ef), TXX0(txx0), TXXF(txxf),
        EtaByS(etabys), SigmaBG(sigmabg) {}

  double S(double dX, double dT) const {
    const double &s = SigmaBG;
    double rs = dX / s;
    return 1. / (M_PI * s * s) * exp(-rs * rs);
  }

  double DS(double dX, double dT) const {
    const double &s = SigmaBG;
    double rs = dX / s;
    return dT / (M_PI * s * s * s) * exp(-rs * rs) * (-2. * rs);
  }

  double DSOverR(double dX, double dT) const {
    const double &s = SigmaBG;
    double rs = dX / s;
    return dT * dT / (M_PI * s * s * s * s) * exp(-rs * rs) * (-2.);
  }

  double DDS(double dX, double dT) const {
    const double &s = SigmaBG;
    double rs = dX / s;
    return dT * dT / (M_PI * s * s * s * s) * exp(-rs * rs) *
           (4 * rs * rs - 2.);
  }

  double Ass() const {
    double dT = tauF - tauIn;
    double s2 = SigmaBG * SigmaBG / (dT * dT);
    return (-0.5 + s2 / 2.) * E0 / EF * (EF + TXXF) / (E0 + TXX0);
  }

  double Asv() const { return 0.5 * E0 / EF * (EF + TXXF) / (E0 + TXX0); }

  double ScalingVariableInverse() const {
    return 3. / 4. * EtaByS * Entropy(EF) * M_HBARC / (EF * tauF);
  }

  double Entropy(double e) const {
    double T = pow(30. / (M_PI * M_PI * NuG) * e, 0.25);
    return 4. / 3. * e / T;
  }
};

} // GreensFunctions


/**
 * @file localization.h
 * @brief source localization
 * @author Ulrich Klee
 */

#ifndef LOCALIZATION_H
#define LOCALIZATION_H

#include <stdio.h>
#include <assert.h>

#include <gsl/gsl_block.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_fit.h>

#include "common/jexception.h"

gsl_vector* getSrpPhat(double delta_f, gsl_matrix_complex *mFramePerChannel, gsl_vector *searchRangeX, gsl_vector *searchRangeY, gsl_matrix *arrgeom, int zPos);

void calcDelays(int x, int y, int z, const gsl_matrix* mpos, gsl_vector* delays);

void calcDelaysOfLinearMicrophoneArray(float azimuth, const gsl_matrix* mpos, gsl_vector* delays);

void calcDelaysOfCircularMicrophoneArray(float azimuth, float elevation, const gsl_matrix* mpos, gsl_vector* delays);

gsl_vector* getDelays(double delta_f, gsl_matrix_complex* mFramePerChannel, gsl_vector* searchRange);

double getAzimuth(double delta_f, gsl_matrix_complex *mFramePerChannel, gsl_matrix *arrgeom, gsl_vector *delays);

double getPlaneWaveSrp(double delta_f, gsl_matrix_complex *mFramePerChannel, gsl_vector *searchRangeY, gsl_matrix *arrgeom);

//void halfComplexPack(double* tgt, const gsl_vector_complex* src);

void getGCCRaw(const gsl_matrix_complex* spectralSample, double sampleRate, gsl_vector* gcc);

const gsl_vector* getGCC(gsl_matrix_complex *spectralSample, double sampleRate);
const gsl_vector* getWindowedGCC(gsl_matrix_complex *spectralSample, double sampleRate, double minDelay, double maxDelay);

const gsl_vector* getWindowedGCCratio(gsl_matrix_complex *spectralSample, double sampleRate, double minDelay, double maxDelay);

const gsl_vector* getWindowedGCCdirect(gsl_matrix_complex *spectralSample, double sampleRate, double minDelay, double maxDelay);

const gsl_vector* getWindowedGCCabs(gsl_matrix_complex *spectralSample, double sampleRate, double minDelay, double maxDelay);

const gsl_vector* getDynWindowedGCC(gsl_matrix_complex *spectralSample, double sampleRate, double minDelay, double maxDelay, double wMinDelay, double wMaxDelay, double threshold);

double getInterpolation(gsl_matrix *crossResult, int delayPos);

gsl_vector* get3DPosition(gsl_vector *yCoord, gsl_vector *azimuth1, gsl_vector *azimuth2, double xPos, double zPos);

gsl_vector* get3DPosition_T_shape(gsl_matrix *arrgeom1, int arrayNr1, gsl_matrix *arrgeom2, int arrayNr2,  gsl_matrix *arrgeom3, double azimuth1, double azimuth2, double azimuth3);

gsl_vector* getGCC_old(gsl_matrix_complex *spectralSample, double delta_f, gsl_vector *delays);

gsl_matrix* getLowerTriangMatrix(gsl_matrix* fullMatrix);

gsl_vector* getXi(gsl_matrix* D1_2);

class PhaseTransform {
public:
  PhaseTransform(unsigned sz);
  ~PhaseTransform();
private:
  gsl_vector_complex*					_crossSpectrum;
  double*						_crossCorrelation;
};

class NoisePowerSpectrum
{
 public:
  NoisePowerSpectrum(double alpha = 0.95);
  ~NoisePowerSpectrum() {
    if (powerSpectrum != NULL)
      gsl_vector_free(powerSpectrum);
  }
  void setAlpha(double alpha) {
    this->alpha = alpha;
    alpha1 = 1 - alpha;
  }
  double getAlpha() { return alpha; }
  void add(const gsl_vector_complex *noiseSpectrum, double timestamp);
  const gsl_vector *get() {
    return powerSpectrum;
  }
 private:
  gsl_vector *powerSpectrum;
  double alpha, alpha1;
  double timestamp;
};

class NoiseCrossSpectrum
{
 public:
  NoiseCrossSpectrum(double alpha = 0.95);
  ~NoiseCrossSpectrum() {
    if (crossSpectrum != NULL)
      gsl_vector_complex_free(crossSpectrum);
  }
  void setAlpha(double alpha) {
    this->alpha = alpha;
    alpha1 = 1 - alpha;
  }
  double getAlpha() { return alpha; }
  const gsl_vector_complex *get() {
    return crossSpectrum;
  }
  void add(const gsl_vector_complex *noiseSpectrum1, const gsl_vector_complex *noiseSpectrum2);
 private:
  gsl_vector_complex *crossSpectrum;
  double alpha, alpha1;
};

class GCC
{
 public:
  GCC(double sampleRate = 44100.0 , unsigned fftLen = 2048, unsigned nChan = 16, unsigned pairs = 6, double alpha = 0.95, double beta = 0.5, double q = 0.3, bool interpolate = true, bool noisereduction = true);
  ~GCC();
  void calculate(const gsl_vector_complex *spectralSample1, unsigned chan1, const gsl_vector_complex *spectralSample2, unsigned chan2, unsigned pair, double timestamp, bool sad = false, bool smooth = true);
  const gsl_vector* findMaximum(double minDelay = -HUGE_VAL, double maxDelay = HUGE_VAL);
  double getPeakDelay() { return delay; }
  double getPeakCorr() { return maxCorr; }
  double getRatio() { return ratio; }
  const gsl_vector* getNoisePowerSpectrum(unsigned chan) { return noisePowerSpectrum[chan].get(); }
  const gsl_vector_complex* getNoiseCrossSpectrum(unsigned pair) { return noiseCrossSpectrum[pair].get(); }
  const gsl_vector_complex* getCrossSpectrum() { return crossSpectrum; }
  const gsl_vector* getCrossCorrelation() { return crossCorrelation; }
  void setAlpha(double alpha) {
    for(unsigned i=0; i < pairs; i++)
      noiseCrossSpectrum[i].setAlpha(alpha);
    for(unsigned i=0; i < nChan; i++)
      noisePowerSpectrum[i].setAlpha(alpha);
  }
  double getAlpha() { return noiseCrossSpectrum[0].getAlpha(); }
 protected:
  virtual gsl_complex calcCrossSpectrumValue(gsl_complex x1, gsl_complex x2, const gsl_vector_complex* Gn1n2, const gsl_vector* N1, const gsl_vector* N2, unsigned i) { throw jdimension_error("Not implemented!!!\n"); };

  //auxiliary variables
  gsl_complex x1, x2, Gx1x2, Gs1s2, G;
  double W1, W2, X1, X12, X2, X22;

  double sampleRate;
  unsigned fftLen;
  unsigned fftLen2;
  unsigned len;
  unsigned nChan;
  unsigned pairs;
  unsigned delayPos;
  double q, q1, q2, beta, beta1;
  double ratio;
  double delay;
  double maxCorr;
  double maxCorr2;
  gsl_vector *crossCorrelation;
  gsl_matrix *interpolValues;
  gsl_vector* retValues;
  gsl_vector *powerSpectrum1, *powerSpectrum2;
  gsl_vector_complex *cSpectrum;
  gsl_vector_complex *crossSpectrum;
  NoisePowerSpectrum* noisePowerSpectrum;
  NoiseCrossSpectrum* noiseCrossSpectrum;
  bool noisereduction;
  bool sad;
  bool interpolate;
};

class GCCRaw : public GCC
{
 public:
  GCCRaw(double sampleRate = 44100.0 , unsigned fftLen = 2048, unsigned nChan = 16, unsigned pairs = 6, double alpha = 0.95, double beta = 0.5, double q = 0.3, bool interpolate = true, bool noisereduction = true) : GCC(sampleRate, fftLen, nChan, pairs, alpha, beta, q, interpolate, noisereduction) {};
 protected:
  gsl_complex calcCrossSpectrumValue(gsl_complex x1, gsl_complex x2, const gsl_vector_complex* Gn1n2, const gsl_vector* N1, const gsl_vector* N2, unsigned i);
};

class GCCGnnSub : public GCC
{
 public:
  GCCGnnSub(double sampleRate = 44100.0 , unsigned fftLen = 2048, unsigned nChan = 16, unsigned pairs = 6, double alpha = 0.95, double beta = 0.5, double q = 0.3, bool interpolate = true, bool noisereduction = true) : GCC(sampleRate, fftLen, nChan, pairs, alpha, beta, q, interpolate, noisereduction) {};
 protected:
  gsl_complex calcCrossSpectrumValue(gsl_complex x1, gsl_complex x2, const gsl_vector_complex* Gn1n2, const gsl_vector* N1, const gsl_vector* N2, unsigned i);
};

class GCCPhat : public GCC
{
 public:
  GCCPhat(double sampleRate = 44100.0 , unsigned fftLen = 2048, unsigned nChan = 16, unsigned pairs = 6, double alpha = 0.95, double beta = 0.5, double q = 0.3, bool interpolate = true, bool noisereduction = true) : GCC(sampleRate, fftLen, nChan, pairs, alpha, beta, q, interpolate, noisereduction) {};
 protected:
  gsl_complex calcCrossSpectrumValue(gsl_complex x1, gsl_complex x2, const gsl_vector_complex* Gn1n2, const gsl_vector* N1, const gsl_vector* N2, unsigned i);
};

class GCCGnnSubPhat : public GCC
{
 public:
  GCCGnnSubPhat(double sampleRate = 44100.0 , unsigned fftLen = 2048, unsigned nChan = 16, unsigned pairs = 6, double alpha = 0.95, double beta = 0.5, double q = 0.3, bool interpolate = true, bool noisereduction = true) : GCC(sampleRate, fftLen, nChan, pairs, alpha, beta, q, interpolate, noisereduction) {};
 protected:
  gsl_complex calcCrossSpectrumValue(gsl_complex x1, gsl_complex x2, const gsl_vector_complex* Gn1n2, const gsl_vector* N1, const gsl_vector* N2, unsigned i);
};

class GCCMLRRaw : public GCC
{
 public:
  GCCMLRRaw(double sampleRate = 44100.0 , unsigned fftLen = 2048, unsigned nChan = 16, unsigned pairs = 6, double alpha = 0.95, double beta = 0.5, double q = 0.3, bool interpolate = true, bool noisereduction = true) : GCC(sampleRate, fftLen, nChan, pairs, alpha, beta, q, interpolate, noisereduction) {};
 protected:
  gsl_complex calcCrossSpectrumValue(gsl_complex x1, gsl_complex x2, const gsl_vector_complex* Gn1n2, const gsl_vector* N1, const gsl_vector* N2, unsigned i);
};

class GCCMLRGnnSub : public GCC
{
 public:
  GCCMLRGnnSub(double sampleRate = 44100.0 , unsigned fftLen = 2048, unsigned nChan = 16, unsigned pairs = 6, double alpha = 0.95, double beta = 0.5, double q = 0.3, bool interpolate = true, bool noisereduction = true) : GCC(sampleRate, fftLen, nChan, pairs, alpha, beta, q, interpolate, noisereduction) {};
 protected:
  gsl_complex calcCrossSpectrumValue(gsl_complex x1, gsl_complex x2, const gsl_vector_complex* Gn1n2, const gsl_vector* N1, const gsl_vector* N2, unsigned i);
};

#endif