#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <mpfr.h>
#include <petsc.h>

#include "../ellipsoid/ellipsoid.h"
#include "../ellipsoid/ellSolv.h"
#include "../sphere/sphere.h"
#include "../sphere/sphSolv.h"
#include "../constants.h"
#include "testFunctions.h"

//compares ellipsoidal and spherical
//has ellipsoidal distribution of charges just inside ellipsoidal boundary
//computes the solution at two point, one inside smallest enclosing sphere where spherical diverges
//other solution point is outside of sphere and ellipse to compare convergence
#undef __FUNCT__
#define __FUNCT__ "main"
PetscErrorCode main(int argc, char **argv)
{
  PetscErrorCode ierr;
  //semi-axes of ellipsoid
  PetscReal a = 3.0;
  PetscReal b = 2.5;
  PetscReal c = 1.2;
  //sphere radius
  PetscReal sphRad = 3;
  //dielecric permitivities both set to 1
  const PetscReal eps1 = 1.0;
  const PetscReal eps2 = 1.0;
  //charges placed on ellipsoid with sem-axes smaller by epsilon
  PetscReal epsilon = .5;
  //interior charge XYZ and magnitude
  const PetscInt SQRT_NUM_CHARGES = 3;
  Vec chargeXYZ;
  Vec chargeMag;
  //maximum expansion order for test
  const PetscInt MAX_N = 25;
  //solutions vector
  const PetscInt NUM_SOL_PTS = 2;
  Vec ellSolutions[MAX_N], sphSolutions[MAX_N];
  Vec ellError[MAX_N], sphError[MAX_N];
  Vec exactSolution;
  Vec solXYZ;
  //solution point inside of sphere
  PetscReal sol1X = .1;
  PetscReal sol1Y = .1;
  PetscReal sol1Z = 1.5;
  //solution point outside of sphere
  PetscReal sol2X = 2.1;
  PetscReal sol2Y = 2.6;
  PetscReal sol2Z = 1;
  //pointer for accessing petsc vectors
  PetscScalar *vecPt, *vecPt2;
  PetscFunctionBeginUser;

  //initialize petsc
  ierr = PetscInitialize(&argc, &argv, NULL, NULL);CHKERRQ(ierr);
  ierr = PetscLogDefaultBegin();CHKERRQ(ierr);

  //create, set charge magnitudes to 1
  ierr = VecCreateSeq(PETSC_COMM_SELF, SQRT_NUM_CHARGES*SQRT_NUM_CHARGES, &chargeMag);CHKERRQ(ierr);
  ierr = VecSet(chargeMag, 1.0);CHKERRQ(ierr);

  //create, set charge XYZ values to be on a-eps,b-eps,c-eps ellipsoid
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*SQRT_NUM_CHARGES*SQRT_NUM_CHARGES, &chargeXYZ);CHKERRQ(ierr);
  ierr = VecGetArray(chargeXYZ, &vecPt);CHKERRQ(ierr);
  PetscReal pu, pv;
  for(PetscInt i=0; i<SQRT_NUM_CHARGES; ++i) {
    for(PetscInt j=0; j<SQRT_NUM_CHARGES; ++j) {
      pu = i*2.0*PETSC_PI/(SQRT_NUM_CHARGES-1);
      pv = j*PETSC_PI/(SQRT_NUM_CHARGES-1);
      PetscInt ind = i*SQRT_NUM_CHARGES + j;
      //there is a slight issue with on-axes coordinate transforms which is what the +.01 is for
      vecPt[3*ind + 0] = (a-epsilon)*PetscCosReal(pu)*PetscSinReal(pv) +.01;
      vecPt[3*ind + 1] = (b-epsilon)*PetscSinReal(pu)*PetscSinReal(pv) + .01;
      vecPt[3*ind + 2] = (c-epsilon)*PetscCosReal(pv) + .01;
      printf("pu: %3.3f pv: %3.3f\n", pu, pv);
    }
  }
  ierr = VecRestoreArray(chargeXYZ, &vecPt);CHKERRQ(ierr);

  //set solution points vector
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*NUM_SOL_PTS, &solXYZ);CHKERRQ(ierr);
  ierr = VecGetArray(solXYZ, &vecPt);CHKERRQ(ierr);
  vecPt[0] = sol1X; vecPt[1] = sol1Y; vecPt[2] = sol1Z;
  vecPt[3] = sol2X; vecPt[4] = sol2Y; vecPt[5] = sol2Z;
  ierr = VecRestoreArray(solXYZ, &vecPt);CHKERRQ(ierr);

  //caulculate exact solution
  ierr = VecCreateSeq(PETSC_COMM_SELF, NUM_SOL_PTS, &exactSolution);CHKERRQ(ierr);
  ierr = CoulombExact(eps1, chargeXYZ, chargeMag, solXYZ, exactSolution);CHKERRQ(ierr);

  //initialize solutions and error vectors
  for(PetscInt k=0; k<MAX_N; ++k) {
    ierr = VecCreateSeq(PETSC_COMM_SELF, NUM_SOL_PTS, ellSolutions+k);CHKERRQ(ierr);
    ierr = VecCreateSeq(PETSC_COMM_SELF, NUM_SOL_PTS, sphSolutions+k);CHKERRQ(ierr);
    ierr = VecCreateSeq(PETSC_COMM_SELF, NUM_SOL_PTS, ellError+k);CHKERRQ(ierr);
    ierr = VecCreateSeq(PETSC_COMM_SELF, NUM_SOL_PTS, sphError+k);CHKERRQ(ierr);
  }

  //compute solutions for expansions up to order MAX_N
  for(PetscInt k=1; k<=MAX_N; ++k) {
    printf("N = %d\n", k);
    printf("calculating ellipsoidal solution...\n");
    ierr = CalcEllipsoidCoulombPotential(a, b, c, eps1, eps2, SQRT_NUM_CHARGES*SQRT_NUM_CHARGES, chargeXYZ, chargeMag, NUM_SOL_PTS, solXYZ, k, ellSolutions[k-1]);CHKERRQ(ierr);
    printf("calculating spherical solution...\n");
    ierr = CalcSphericalCoulombPotential(1.0, eps1, eps2, SQRT_NUM_CHARGES*SQRT_NUM_CHARGES, chargeXYZ, chargeMag, NUM_SOL_PTS, solXYZ, k, sphSolutions[k-1]);CHKERRQ(ierr);
  }

  //compute errors
  printf("Computing errors...\n");
  for(PetscInt k=0; k<MAX_N; ++k) {
    ierr = VecCopy(exactSolution, ellError[k]);CHKERRQ(ierr);
    ierr = VecCopy(exactSolution, sphError[k]);CHKERRQ(ierr);
    ierr = VecAXPY(ellError[k], -1.0, ellSolutions[k]);CHKERRQ(ierr);
    ierr = VecAXPY(sphError[k], -1.0, sphSolutions[k]);CHKERRQ(ierr);
    ierr = VecAbs(ellError[k]);CHKERRQ(ierr);
    ierr = VecAbs(sphError[k]);CHKERRQ(ierr);
  }

  FILE *fp = fopen("out/ex1.txt", "w");

  for(PetscInt k=0; k<MAX_N; ++k) {
    ierr = VecGetArrayRead(ellError[k], &vecPt);
    ierr = VecGetArrayRead(sphError[k], &vecPt2);
    printf("Error[%d] = %4.4e\n", k, vecPt[0]);
    fprintf(fp, "%4.4e %4.4e %4.4e %4.4e\n", vecPt[0], vecPt2[0], vecPt[1], vecPt2[1]);
    ierr = VecRestoreArrayRead(ellError[k], &vecPt);
    ierr = VecRestoreArrayRead(sphError[k], &vecPt2);
  }
  printf("Exact: \n");
  ierr = VecView(exactSolution, PETSC_VIEWER_STDOUT_SELF);
  printf("Ell: \n");
  ierr = VecView(ellSolutions[MAX_N-1], PETSC_VIEWER_STDOUT_SELF);

  ierr = PetscFinalize();CHKERRQ(ierr);
  PetscFunctionReturn(0);
}


/*
 #  File            : condat_l1ballproj.c
 #
 #  Version History : 1.0, Aug. 15, 2014
 #
 #  Author          : Laurent Condat, PhD, CNRS research fellow in France.
 #
 #  Description     : This file contains an implementation in the C language
 #                    of algorithms described in the research paper:
 #
 #                    L. Condat, "Fast Projection onto the Simplex and the
 #                    l1 Ball", preprint Hal-01056171, 2014.
 #
 #                    This implementation comes with no warranty: due to the
 #                    limited number of tests performed, there may remain
 #                    bugs. In case the functions would not do what they are
 #                    supposed to do, please email the author (contact info
 #                    to be found on the web).
 #
 #                    If you use this code or parts of it for any purpose,
 #                    the author asks you to cite the paper above or, in
 #                    that event, its published version. Please email him if
 #                    the proposed algorithms were useful for one of your
 #                    projects, or for any comment or suggestion.
 #
 #  Usage rights    : Copyright Laurent Condat.
 #                    This file is distributed under the terms of the CeCILL
 #                    licence (compatible with the GNU GPL), which can be
 #                    found at the URL "http://www.cecill.info".
 #
 #  This software is governed by the CeCILL license under French law and
 #  abiding by the rules of distribution of free software. You can  use,
 #  modify and or redistribute the software under the terms of the CeCILL
 #  license as circulated by CEA, CNRS and INRIA at the following URL :
 #  "http://www.cecill.info".
 #
 #  As a counterpart to the access to the source code and rights to copy,
 #  modify and redistribute granted by the license, users are provided only
 #  with a limited warranty  and the software's author,  the holder of the
 #  economic rights,  and the successive licensors  have only  limited
 #  liability.
 #
 #  In this respect, the user's attention is drawn to the risks associated
 #  with loading,  using,  modifying and/or developing or reproducing the
 #  software by the user in light of its specific status of free software,
 #  that may mean  that it is complicated to manipulate,  and  that  also
 #  therefore means  that it is reserved for developers  and  experienced
 #  professionals having in-depth computer knowledge. Users are therefore
 #  encouraged to load and test the software's suitability as regards their
 #  requirements in conditions enabling the security of their systems and/or
 #  data to be ensured and,  more generally, to use and operate it in the
 #  same conditions as regards security.
 #
 #  The fact that you are presently reading this means that you have had
 #  knowledge of the CeCILL license and that you accept its terms.
*/

/* This code was compiled using
gcc -march=native -O2 condat_l1ballproj.c -o main  -I/usr/local/include/
	-lm -lgsl  -L/usr/local/lib/
On my machine, gcc is actually a link to the compiler Apple LLVM version 5.1
(clang-503.0.40) */

/* The following functions are implemented:
l1ballproj_IBIS
l1ballproj_Condat (proposed algorithm)
These functions take the same parameters. They project the vector y onto
the closest vector x of same length (parameter N in the paper) with
sum_{n=0}^{N-1}|x[n]|<=a.
We must have length>=1.
For l1ballproj_Condat only:
we can have x==y (projection done in place). If x!=y, the arrays x and y must
not overlap, as x is used for temporary calculations before y is accessed.
*/

#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#define datatype double /* type of the elements in y */

/* Proposed algorithm */
static void l1ballproj_Condat(datatype* y, datatype* x,
const unsigned int length, const double a) {
	if (a<=0.0) {
		if (a==0.0) memset(x,0,length*sizeof(datatype));
		return;
	}
	datatype*	aux = (x==y ? (datatype*)malloc(length*sizeof(datatype)) : x);
	int		auxlength=1;
	int		auxlengthold=-1;
	double	tau=(*aux=(*y>=0.0 ? *y : -*y))-a;
	int 	i=1;
	for (; i<length; i++)
		if (y[i]>0.0) {
			if (y[i]>tau) {
				if ((tau+=((aux[auxlength]=y[i])-tau)/(auxlength-auxlengthold))
				<=y[i]-a) {
					tau=y[i]-a;
					auxlengthold=auxlength-1;
				}
				auxlength++;
			}
		} else if (y[i]!=0.0) {
			if (-y[i]>tau) {
				if ((tau+=((aux[auxlength]=-y[i])-tau)/(auxlength-auxlengthold))
				<=aux[auxlength]-a) {
					tau=aux[auxlength]-a;
					auxlengthold=auxlength-1;
				}
				auxlength++;
			}
		}
	if (tau<=0) {	/* y is in the l1 ball => x=y */
		if (x!=y) memcpy(x,y,length*sizeof(datatype));
		else free(aux);
	} else {
		datatype*  aux0=aux;
		if (auxlengthold>=0) {
			auxlength-=++auxlengthold;
			aux+=auxlengthold;
			while (--auxlengthold>=0)
				if (aux0[auxlengthold]>tau)
					tau+=((*(--aux)=aux0[auxlengthold])-tau)/(++auxlength);
		}
		do {
			auxlengthold=auxlength-1;
			for (i=auxlength=0; i<=auxlengthold; i++)
				if (aux[i]>tau)
					aux[auxlength++]=aux[i];
				else
					tau+=(tau-aux[i])/(auxlengthold-i+auxlength);
		} while (auxlength<=auxlengthold);
		for (i=0; i<length; i++)
			x[i]=(y[i]-tau>0.0 ? y[i]-tau : (y[i]+tau<0.0 ? y[i]+tau : 0.0));
		if (x==y) free(aux0);
	}
}

/* function eplb of the package SLEP, implementing the algorithm IBIS of the paper
J. Liu and J. Ye. Efficient Euclidean Projections in Linear Time, ICML 2009.
Version without the initial guess lambda0 of lambda (tau in our notations) */
static void l1ballproj_IBIS(datatype* y, datatype* x,
const unsigned int length, const double a) {
    #define delta 1e-13
    int i, j, flag=0;
    int rho_1, rho_2, rho, rho_T, rho_S;
    int V_i_b, V_i_e, V_i;
    double lambda_1, lambda_2, lambda_T, lambda_S, lambda;
    double s_1, s_2, s, s_T, s_S, y_max, temp;
    double f_lambda_1, f_lambda_2, f_lambda, f_lambda_T, f_lambda_S;
    int iter_step=0;

    /* find the maximal absolute value in y
     * and copy the (absolute) values from y to x
     */

	/*if (a< 0){
		printf("\n a should be nonnegative!");
		return;
	}*/

    V_i=0;
    if (y[0] !=0){
        rho_1=1;
        s_1=x[V_i]=y_max=fabs(y[0]);
        V_i++;
    }
    else{
        rho_1=0;
        s_1=y_max=0;
    }

    for (i=1;i<length; i++){
        if (y[i]!=0){
            x[V_i]=fabs(y[i]); s_1+= x[V_i]; rho_1++;

            if (x[V_i] > y_max)
                y_max=x[V_i];
            V_i++;
        }
    }

    /* If ||y||_1 <= a, then y is the solution  */
    if (s_1 <= a){
        flag=1;        lambda=0;
        for(i=0;i<length;i++){
            x[i]=y[i];
        }
        /**root=lambda;
        *steps=iter_step;*/
        return;
    }

    lambda_1=0; lambda_2=y_max;
    f_lambda_1=s_1 -a;
    /*f_lambda_1=s_1-rho_1* lambda_1 -a;*/
    rho_2=0; s_2=0; f_lambda_2=-a;
    V_i_b=0; V_i_e=V_i-1;

    /*lambda=lambda0;
    if ( (lambda<lambda_2) && (lambda> lambda_1) ){
         i=V_i_b; j=V_i_e; rho=0; s=0;
        while (i <= j){
            while( (i <= V_i_e) && (x[i] <= lambda) ){
                i++;
            }
            while( (j>=V_i_b) && (x[j] > lambda) ){
                s+=x[j];
                j--;
            }
            if (i<j){
                s+=x[i];

                temp=x[i];  x[i]=x[j];  x[j]=temp;
                i++;  j--;
            }
		}

        rho=V_i_e-j;  rho+=rho_2;  s+=s_2;
		f_lambda=s-rho*lambda-a;

        if ( fabs(f_lambda)< delta ){
            flag=1;
		}

		if (f_lambda <0){
			lambda_2=lambda; s_2=s;	rho_2=rho; f_lambda_2=f_lambda;

			V_i_e=j;  V_i=V_i_e-V_i_b+1;
		}
		else{
			lambda_1=lambda; rho_1=rho;	s_1=s; f_lambda_1=f_lambda;

			V_i_b=i; V_i=V_i_e-V_i_b+1;
		}

		if (V_i==0){
			lambda=(s - a)/ rho;
			flag=1;
		}
    }*/

    while (!flag){
        iter_step++;

        /* compute lambda_T  */
        lambda_T=lambda_1 + f_lambda_1 /rho_1;
        if(rho_2 !=0){
            if (lambda_2 + f_lambda_2 /rho_2 >	lambda_T)
                lambda_T=lambda_2 + f_lambda_2 /rho_2;
        }

        /* compute lambda_S */
        lambda_S=lambda_2 - f_lambda_2 *(lambda_2-lambda_1)/(f_lambda_2-f_lambda_1);

        if (fabs(lambda_T-lambda_S) <= delta){
            lambda=lambda_T; flag=1;
            break;
        }

        /* set lambda as the middle point of lambda_T and lambda_S */
        lambda=(lambda_T+lambda_S)/2;

        s_T=s_S=s=0;
        rho_T=rho_S=rho=0;
        i=V_i_b; j=V_i_e;
        while (i <= j){
            while( (i <= V_i_e) && (x[i] <= lambda) ){
                if (x[i]> lambda_T){
                    s_T+=x[i]; rho_T++;
                }
                i++;
            }
            while( (j>=V_i_b) && (x[j] > lambda) ){
                if (x[j] > lambda_S){
                    s_S+=x[j]; rho_S++;
                }
                else{
                    s+=x[j];  rho++;
                }
                j--;
            }
            if (i<j){
                if (x[i] > lambda_S){
                    s_S+=x[i]; rho_S++;
                }
                else{
                    s+=x[i]; rho++;
                }

                if (x[j]> lambda_T){
                    s_T+=x[j]; rho_T++;
                }

                temp=x[i]; x[i]=x[j];  x[j]=temp;
                i++; j--;
            }
		}

        s_S+=s_2; rho_S+=rho_2;
        s+=s_S; rho+=rho_S;
        s_T+=s; rho_T+=rho;
        f_lambda_S=s_S-rho_S*lambda_S-a;
        f_lambda=s-rho*lambda-a;
        f_lambda_T=s_T-rho_T*lambda_T-a;

        /*printf("\n %d & %d  & %5.6f & %5.6f & %5.6f & %5.6f & %5.6f \\\\ \n \\hline ", iter_step, V_i, lambda_1, lambda_T, lambda, lambda_S, lambda_2);*/

        if ( fabs(f_lambda)< delta ){
            /*printf("\n lambda");*/
            flag=1;
            break;
        }
        if ( fabs(f_lambda_S)< delta ){
           /* printf("\n lambda_S");*/
            lambda=lambda_S; flag=1;
            break;
        }
        if ( fabs(f_lambda_T)< delta ){
           /* printf("\n lambda_T");*/
            lambda=lambda_T; flag=1;
            break;
        }

        /*
        printf("\n\n f_lambda_1=%5.6f, f_lambda_2=%5.6f, f_lambda=%5.6f",f_lambda_1,f_lambda_2, f_lambda);
        printf("\n lambda_1=%5.6f, lambda_2=%5.6f, lambda=%5.6f",lambda_1, lambda_2, lambda);
        printf("\n rho_1=%d, rho_2=%d, rho=%d ",rho_1, rho_2, rho);
         */

        if (f_lambda <0){
            lambda_2=lambda;  s_2=s;  rho_2=rho;
            f_lambda_2=f_lambda;

            lambda_1=lambda_T; s_1=s_T; rho_1=rho_T;
            f_lambda_1=f_lambda_T;

            V_i_e=j;  i=V_i_b;
            while (i <= j){
                while( (i <= V_i_e) && (x[i] <= lambda_T) ){
                    i++;
                }
                while( (j>=V_i_b) && (x[j] > lambda_T) ){
                    j--;
                }
                if (i<j){
                    x[j]=x[i];
                    i++;   j--;
                }
            }
            V_i_b=i; V_i=V_i_e-V_i_b+1;
        }
        else{
            lambda_1=lambda;  s_1=s; rho_1=rho;
            f_lambda_1=f_lambda;

            lambda_2=lambda_S; s_2=s_S; rho_2=rho_S;
            f_lambda_2=f_lambda_S;

            V_i_b=i;  j=V_i_e;
            while (i <= j){
                while( (i <= V_i_e) && (x[i] <= lambda_S) ){
                    i++;
                }
                while( (j>=V_i_b) && (x[j] > lambda_S) ){
                    j--;
                }
                if (i<j){
                    x[i]=x[j];
                    i++;   j--;
                }
            }
            V_i_e=j; V_i=V_i_e-V_i_b+1;
        }

        if (V_i==0){
            lambda=(s - a)/ rho; flag=1;
            /*printf("\n V_i=0, lambda=%5.6f",lambda);*/
            break;
        }
    }/* end of while */

    for(i=0;i<length;i++){
        if (y[i] > lambda)
            x[i]=y[i]-lambda;
        else
            if (y[i]< -lambda)
                x[i]=y[i]+lambda;
            else
                x[i]=0;
    }
    /**root=lambda;
    *steps=iter_step;*/
}

/* Example of code to compare the computation times
Result on my machine: my algorithm is 2.5x faster than IBIS */
int main() {
	datatype*	y;
	datatype*	x;
	double	val=0.0;
	double 	aux;
	double 	epsi;
    int 	i,j;
    int 	Nbrea;
    unsigned int length;
    clock_t	start, end;
    const gsl_rng_type *T;
  	gsl_rng *r;
  	gsl_rng_env_setup();
	gsl_rng_default_seed=32067;
	T = gsl_rng_default;
	r = gsl_rng_alloc(T);
	double timemean=0.0, timevar=0.0, timedelta;
	const double a = 1.0;
	srand((unsigned int)pow(time(NULL)%100,3));
    double*	 timetable;

    length = 1000000;
	Nbrea = 100;
	y=(datatype*)malloc(length*sizeof(datatype));
	x=(datatype*)malloc(length*sizeof(datatype));
	timetable=(double*)malloc((Nbrea+1)*sizeof(double));
	for (j=0; j<=Nbrea; j++) {
		for (i=0; i<length; i++) {
	    	y[i]=gsl_ran_gaussian(r, 0.01);
	    }
	    y[(int)(rand() / (((double)RAND_MAX+1.0)/length))]+=a;
	    start=clock();
	    l1ballproj_Condat(y,x,length,a);
	    /*l1ballproj_IBIS(y,x,length,a); */
	    end=clock();
	    timetable[j]=(double)(end-start)/CLOCKS_PER_SEC;
	}
	/* we discard the first value, because the computation time is higher,
	probably because of cache operations */
	for (j=1; j<=Nbrea; j++) {
		timedelta=timetable[j]-timemean;
		timemean+=timedelta/j;
		timevar+=timedelta*(timetable[j]-timemean);
	}
	timevar=sqrt(timevar/Nbrea);
	printf("av. time: %e std dev: %e\n",timemean,timevar);
	free(x);
    free(y);
    return 0;
}


// https://stackoverflow.com/questions/66536134/gsl-gsl-linalg-sv-decomp-returning-u-and-v-reversed
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

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

void pretty_print(const gsl_matrix * M)
{
  // Get the dimension of the matrix.
  int rows = M->size1;
  int cols = M->size2;
  // Now print out the data in a square format.
  for(int i = 0; i < rows; i++){
    for(int j = 0; j < cols; j++){
      printf("%f ", gsl_matrix_get(M, i, j));
    }
    printf("\n");
  }
  printf("\n");
}

void pretty_print_vector(const gsl_vector * M)
{
  int cols = M->size;
    for(int j = 0; j < cols; j++){
      printf("%f ", gsl_vector_get(M, j));
    }
  printf("\n");
}

int
main()
{
  const size_t M = 4;
  const size_t N = 5;
  double A_data[] = {
    1.0, 0.0, 0.0, 0.0, 2.0,
    0.0, 0.0, 3.0, 0.0, 0.0,
    0.0, 0.0, 0.0, 0.0, 0.0,
    0.0, 2.0, 0.0, 0.0, 0.0 };
  gsl_matrix_view A = gsl_matrix_view_array(A_data, 4, 5);
  gsl_matrix * B = gsl_matrix_alloc(N, M);
  gsl_matrix * V = gsl_matrix_alloc(M, M);
  gsl_vector * S = gsl_vector_alloc(M);
  gsl_vector * work = gsl_vector_alloc(M);

  gsl_matrix_transpose_memcpy(B, &A.matrix);

  gsl_linalg_SV_decomp(B, V, S, work);

  printf("U:\n");
  pretty_print(V);

  printf("S:\n");
  pretty_print_vector(S);

  printf("V:\n");
  pretty_print(B);

  gsl_matrix_free(B);
  gsl_matrix_free(V);
  gsl_vector_free(S);
  gsl_vector_free(work);

  return 0;
}


/* gsl_histogram_stat.c
 * Copyright (C) 2000  Simone Piccardi
 *
 * This library 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 library; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */
/***************************************************************
 *
 * File gsl_histogram_stat.c:
 * Routines for statisticalcomputations on histograms.
 * Need GSL library and header.
 * Contains the routines:
 * gsl_histogram_mean    compute histogram mean
 * gsl_histogram_sigma   compute histogram sigma
 *
 * Author: S. Piccardi
 * Jan. 2000
 *
 ***************************************************************/
#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_histogram.h>

/* FIXME: We skip negative values in the histogram h->bin[i] < 0,
   since those correspond to negative weights (BJG) */

double
gsl_histogram_mean (const gsl_histogram * h)
{
  const size_t n = h->n;
  size_t i;

  /* Compute the bin-weighted arithmetic mean M of a histogram using the
     recurrence relation

     M(n) = M(n-1) + (x[n] - M(n-1)) (w(n)/(W(n-1) + w(n)))
     W(n) = W(n-1) + w(n)

   */

  long double wmean = 0;
  long double W = 0;

  for (i = 0; i < n; i++)
    {
      double xi = (h->range[i + 1] + h->range[i]) / 2;
      double wi = h->bin[i];

      if (wi > 0)
        {
          W += wi;
          wmean += (xi - wmean) * (wi / W);
        }
    }

  return wmean;
}

double
gsl_histogram_sigma (const gsl_histogram * h)
{
  const size_t n = h->n;
  size_t i;

  long double wvariance = 0 ;
  long double wmean = 0;
  long double W = 0;

  /* Use a two-pass algorithm for stability.  Could also use a single
     pass formula, as given in N.J.Higham 'Accuracy and Stability of
     Numerical Methods', p.12 */

  /* Compute the mean */

  for (i = 0; i < n; i++)
    {
      double xi = (h->range[i + 1] + h->range[i]) / 2;
      double wi = h->bin[i];

      if (wi > 0)
        {
          W += wi;
          wmean += (xi - wmean) * (wi / W);
        }
    }

  /* Compute the variance */

  W = 0.0;

  for (i = 0; i < n; i++)
    {
      double xi = ((h->range[i + 1]) + (h->range[i])) / 2;
      double wi = h->bin[i];

      if (wi > 0) {
        const long double delta = (xi - wmean);
        W += wi ;
        wvariance += (delta * delta - wvariance) * (wi / W);
      }
    }

  {
    double sigma = sqrt (wvariance) ;
    return sigma;
  }
}

/*
  sum up all bins of histogram
 */

double
gsl_histogram_sum(const gsl_histogram * h)
{
  double sum=0;
  size_t i=0;
  size_t n;
  n=h->n;

  while(i < n)
    sum += h->bin[i++];

  return sum;
}


//
// Created by Harold on 2020/10/7.
//

#ifndef M_MATH_M_HIST_H
#define M_MATH_M_HIST_H

#include <gsl/gsl_histogram.h>
#include <gsl/gsl_histogram2d.h>
#include <vector>
#include <utility>

namespace M_MATH {
    template<typename T>
    class Histogram1D {
    public:
        // [min, max), it: <x>
        template<typename IT>
        Histogram1D(IT begin, IT end, T min, T max, size_t N);
        ~Histogram1D() { gsl_histogram_free(h); gsl_histogram_pdf_free(p); };
        // counts in every bin
        std::vector<size_t> counts();
        // bins with [lower, upper] boundaries
        std::vector<std::pair<T, T>> bins();
        // probability distribution, r in [0, 1]
        double pdf_sample(double r);
        // standard deviation of the histogrammed variable
        double sigma() { return gsl_histogram_sigma(h); }
        // mean of the histogrammed variable
        double mean() { return gsl_histogram_mean(h); }
        // sum of all bin values
        double sum() { return gsl_histogram_sum(h); }
        // find x in which bin
        size_t find(double x) { size_t idx; gsl_histogram_find(h, x, &idx); return idx; }

    private:
        gsl_histogram *h;
        gsl_histogram_pdf *p;
    };

    template<typename T>
    class Histogram2D {
    public:
        // [min, max), it: <x, y>
        template<typename IT>
        Histogram2D(IT begin, IT end, T xmin, T xmax, T ymin, T ymax, size_t xdim, size_t ydim);
        ~Histogram2D() { gsl_histogram2d_free(h); gsl_histogram2d_pdf_free(p); };
        // counts in every bin
        std::vector<size_t> counts();
        // bins with [lower, upper] boundaries, <xrange, yrange>
        std::pair<std::vector<std::pair<T, T>>, std::vector<std::pair<T, T>>> bins();
        // probability distribution <x, y>, r1, r2 in [0, 1]
        std::pair<double, double> pdf_sample(double r1, double r2);
        // standard deviation of the histogrammed x and y variables
        std::pair<double, double> sigma() { return std::make_pair(gsl_histogram2d_xsigma(h), gsl_histogram2d_ysigma(h)); }
        // mean of the histogrammed x and y variables
        std::pair<double, double> mean() { return std::make_pair(gsl_histogram2d_xmean(h), gsl_histogram2d_ymean(h)); }
        // sum of all bin values
        double sum() { return gsl_histogram2d_sum(h); }
        // covariance of the histogrammed x and y variables
        double cov() { return gsl_histogram2d_cov(h); }
        // find x in which bin, <x, y>
        std::pair<size_t, size_t> find(double x, double y) { size_t xidx, yidx; gsl_histogram2d_find(h, x, y, &xidx, &yidx); return std::make_pair(xidx, yidx); }
    private:
        gsl_histogram2d *h;
        gsl_histogram2d_pdf *p;
    };

    template<typename T>
    template<typename IT>
    Histogram1D<T>::Histogram1D(IT const begin, IT const end, T min, T max, size_t N) : h(nullptr), p(nullptr) {
        h = gsl_histogram_alloc(N);
        gsl_histogram_set_ranges_uniform(h, min, max);
        for (auto it = begin; it != end; ++it)
            gsl_histogram_increment(h, *it);
    }

    template<typename T>
    std::vector<size_t> Histogram1D<T>::counts() {
        std::vector<size_t> res(h->n);
        for (auto i = 0; i < h->n; ++i)
            res[i] = gsl_histogram_get(h, i);
        return res;
    }

    template<typename T>
    std::vector<std::pair<T, T>> Histogram1D<T>::bins() {
        std::vector<std::pair<T, T>> res(h->n);
        double lower, upper;
        for (auto i = 0; i < h->n; ++i) {
            gsl_histogram_get_range(h, i, &lower, &upper);
            res[i] = std::make_pair(lower, upper);
        }
        return res;
    }

    template<typename T>
    double Histogram1D<T>::pdf_sample(double r) {
        if (p == nullptr) {
            p = gsl_histogram_pdf_alloc(h->n);
            gsl_histogram_pdf_init(p, h);
        }
        return gsl_histogram_pdf_sample(p, r);
    }

    template<typename T>
    template<typename IT>
    Histogram2D<T>::Histogram2D(IT begin, IT end,
                                T xmin, T xmax,
                                T ymin, T ymax,
                                size_t xdim, size_t ydim) : h(nullptr), p(nullptr) {
        h = gsl_histogram2d_alloc(xdim, ydim);
        gsl_histogram2d_set_ranges_uniform(h, xmin, xmax, ymin, ymax);
        for (auto it = begin; it != end; ++it)
            gsl_histogram2d_increment(h, (*it).x, (*it).y);
    }

    template<typename T>
    std::vector<size_t> Histogram2D<T>::counts() {
        std::vector<size_t> res((h->nx) * (h->ny));
        for (auto i = 0; i < h->nx; ++i)
            for (auto j = 0; j < h->ny; ++j)
                res[j * (h->nx) + i] = gsl_histogram2d_get(h, i, j);
        return res;
    }

    template<typename T>
    std::pair<std::vector<std::pair<T, T>>, std::vector<std::pair<T, T>>> Histogram2D<T>::bins() {
        std::vector<std::pair<T, T>> xrange(h->nx), yrange(h->ny);
        double lower, upper;
        for (auto i = 0; i < h->nx; ++i) {
            gsl_histogram2d_get_xrange(h, i, &lower, &upper);
            xrange[i] = std::make_pair(lower, upper);
        }
        for (auto i = 0; i < h->ny; ++i) {
            gsl_histogram2d_get_yrange(h, i, &lower, &upper);
            yrange[i] = std::make_pair(lower, upper);
        }
        return std::make_pair(xrange, yrange);
    }

    template<typename T>
    std::pair<double, double> Histogram2D<T>::pdf_sample(double r1, double r2) {
        if (p == nullptr) {
            p = gsl_histogram2d_pdf_alloc(h->nx, h->ny);
            gsl_histogram2d_pdf_init(p, h);
        }
        double x, y;
        gsl_histogram2d_pdf_sample(p, r1, r2, &x, &y);
        return std::make_pair(x, y);
    }
}

#endif //M_MATH_M_HIST_H


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

double A[] =
{
  -1.0, 0.0, 2.0, 1.0,
  0.0, -1.0, 3.0, 2.0,
  1.0, 2.0, 3.0, 0.0,
  -3.0, 1.0, 4.0, 2.0
};

double y[] = { 0.0, 1.0, 2.0, 1.0 };

int main() {
  gsl_matrix_view m = gsl_matrix_view_array(A, 4, 4);
  gsl_vector_view b = gsl_vector_view_array(y, 4);
  gsl_vector* x = gsl_vector_alloc(4);

  int s;
  gsl_permutation* p = gsl_permutation_alloc(4);
  gsl_linalg_LU_decomp(&m.matrix, p, &s);
  gsl_linalg_LU_solve(&m.matrix, p, &b.vector, x);

  printf("x = \n");
  gsl_vector_fprintf(stdout, x, "%5.1f");

  gsl_permutation_free(p);
  gsl_vector_free(x);
  return 0;
}


/**
* Subroutines for fitting functions to data.
 * Most of the functions are used for the
 * background determination.
 */

#include <gsl/gsl_sort.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_multifit.h>

#include "spce_fitting.h"

/**
 * Function: det_vector_polyN
 * A function using GSL fitting functions to fit a set x,y,w of n elements
 * such that an Nth degree polynomial is fitted to the x and y vectors
 * using the weights w. This function avoids NaN values in ys AND elements
 * with an associated weight that is zero.
 *
 * Parameters:
 * @param m   - order of the fit
 * @param xs  - double vector containing the x values
 * @param ys  - double vector containing the y values
 * @param ws  - double vector containing the weights associated with ys
 * @param n   - number of points in xs,ys, and ws (must be greater than m!)
 * @param c   - the vector with the fitted coefficients
 * @param cov - the covariance matrix
 *
 * Returns:
 * @return interp - vector with additional fitting information
 */
gsl_vector *
det_vector_polyN (int m, const double *const xs, double *const ys,
		   double *const ws, const int n, gsl_vector *c,
		   gsl_matrix *cov)
{
  int i, j, nn, ii;
  double chisq;
  gsl_matrix *X;
  gsl_vector *y, *w;

  double *tmp, *xmp;
  double median, xean;

  gsl_vector *interp;

  // allocate the return vector
  interp = gsl_vector_alloc(5);

  // allocate temporary vectors
  tmp = (double *) malloc (n * sizeof (double));
  xmp = (double *) malloc (n * sizeof (double));

  // fill weights and independent values
  // for the background pixels into the arrays
  nn = 0;
  for (i = 0; i < n; i++)
    {
      if (ws[i] > 0.0)
	{
	  //	  tmp[nn] = ws[i];
	  tmp[nn] = 1.0/(ws[i]*ws[i]);
	  xmp[nn] = xs[i];
	  nn++;
	}
    }

  // compute the median weight
  // to be used for all pixels
  gsl_sort( tmp, 1, m);
  median = gsl_stats_median_from_sorted_data(tmp, 1, nn);

  // determine the anchor point in the
  // independent variable
  xean = gsl_stats_mean (xmp, 1, nn);

  // check whether the desired
  // intepolation degree is feasible
  if (nn < m)
    {
      aXe_message (aXe_M_WARN4, __FILE__, __LINE__,
		   "Not enough points (%d)  found to perform the %d order fit. Doing %d order instead.\n",
		   nn, m, nn);
      m = nn;
    }

  // allocate more temporary space
  X = gsl_matrix_alloc (nn, m);
  y = gsl_vector_alloc (nn);
  w = gsl_vector_alloc (nn);
  //  c = gsl_vector_alloc (m);
  //  cov = gsl_matrix_alloc (m, m);

  // transfer independent/dependent and weight
  // values from the background pixels to the
  // temporary arrays
  ii = 0;
  for (i = 0; i < n; i++)
    {
      if (ws[i] > 0.0)
	{
	  // shift the x-values
	  // and store them in the matrix
	  for (j = 0; j < m; j++)
	    {
	      gsl_matrix_set (X, ii, j, pow (xs[i] - xean, j));
	    }
	  gsl_vector_set (y, ii, ys[i]);
	  gsl_vector_set (w, ii, median);
	  ii++;
	}

    }

  // allocate space and do the fit; release the space
  gsl_multifit_linear_workspace *work = gsl_multifit_linear_alloc (nn, m);
  gsl_multifit_wlinear (X, w, y, c, cov, &chisq, work);
  gsl_multifit_linear_free (work);

  // release memory
  gsl_matrix_free (X);
  gsl_vector_free (y);
  gsl_vector_free (w);

  // free the tmp-arrays
  free(tmp);
  free(xmp);

  // fill the mean x-value
  // and the order into the return vector
  gsl_vector_set(interp, 0, xean);
  gsl_vector_set(interp, 1, (double)m);

  return interp;
}

/**
 * Function: fill_polyN_interp
 * Evaluates a polynomial of any order and fills the
 * result plus the error, compute using the covariance
 * matrix, in vectors.
 *
 * Parameters:
 * @param xs     - double vector containing the x values
 * @param ys     - double vector containing the y values
 * @param yi     - double vector containing the intermediate y-values
 * @param ws     - double vector containing the weights associated with ys
 * @param n      - number of points in xs,ys, and ws (must be greater than m!)
 * @param c      - the vector with the fitted coefficients
 * @param cov    - the covariance matrix
 * @param interp - additional fitting ifnormation
 * @param final  - flagg to indicate final fit and
 *                 therefore a replacement
 */
void
fill_polyN_interp(const double *const xs, double *const ys,
		  double *const ws, double *yi, const int n,
		  gsl_vector *coeffs, gsl_matrix *cov,
		  gsl_vector *interp, const int final)
{
  int i, j, m;
  double xf, yf, sq_yf_err, yf_err;
  double xean;

  // get the mean x-value and
  // polynomial order from the vector
  xean = gsl_vector_get(interp, 0);
  m    = (int)gsl_vector_get(interp, 1);

  // put the interpolated values
  // for object pixels back into the
  // inut arrays
  for (i = 0; i < n; i++)
    {

      // compute and store interpolated
      // values an corresponding error
      xf = xs[i]-xean;
      yf = 0.0;
      yf_err = 0.0;
      sq_yf_err = 0.0;

      // compute the polynimials
      for (j = 0; j < m; j++)
	{
	  // first compute the y-value
	  yf += gsl_vector_get(coeffs,j)*pow (xf, j);

	  // compute the associated error
	  sq_yf_err += gsl_matrix_get(cov,j,j) * pow (xf, j) * pow (xf, j);
	}

      // put the intepolated value in the intermediate vector
      yi[i] = yf;

      // if requested, fill the interpolated values
      // into the origina; arrays
      if (final && ws[i] == 0.0)
	{
	  ys[i] = yf;
	  ws[i] = sqrt(sq_yf_err);
	}
    }
}

/**
 * Function: comp_vector_average
 * The function provides the computation of the average of
 * the y-values and store that average in temporary
 * or the original y-vector.
 *
 * @param xs    - absissa
 * @param ys    - value at xs[]
 * @param ws    - weight in ys[]
 * @param yi    - temprary y-values
 * @param n     - number of points in xs, ys, ws
 * @param final - flagg to store to temp (0) or final (1) vectors
 */
void
comp_vector_average (const double *xs, double *ys,
		     double *ws, double *yi, const int n, const int final)
{
  double ave, std;
  //int i, m;

  det_vector_average (xs, ys, ws, n, &ave, &std);
  fill_const_value(ys, ws, yi, n, ave, std, final);
}

/**
 * Function: comp_vector_median
 * The function provides the computation of the median of
 * the y-values and store that median in temporary
 * or the original y-vector
 *
 * @param xs - absissa
 * @param ys - value at xs[]
 * @param ws - weight in ys[]
 * @param yi    - temprary y-values
 * @param n     - number of points in xs, ys, ws
 * @param final - flagg to store to temp (0) or final (1) vectors
 */
void
comp_vector_median (const double *xs, double *ys,
		    double *ws, double *yi, const int n, const int final)
{
  double med, std;

  det_vector_median (xs, ys, ws, n, &med, &std);
  fill_const_value(ys, ws, yi, n, med, std, final);

}

/**
 * Function: comp_vector_linear
 * The function fits a linear function to the y-values
 * and fills the linear inteprolated values in a temporary
 * vector and, if requested, in the original vector as well.
 *
 * @param xs    - absissa
 * @param ys    - value at xs[]
 * @param ws    - weight in ys[]
 * @param yi    - temprary y-values
 * @param n     - number of points in xs, ys, ws
 * @param final - flagg to store to temp (0) or final (1) vectors
 */
void
comp_vector_linear (const double *xs, double *ys,
		    double *ws, double *yi, const int n, const int final)
{
  gsl_vector *interp;

  // make a weighted linear fit
  interp = det_vector_linear(xs, ys, ws, n, 1);

  // make the interpolations,
  // using the linear fit
  fill_linear_interp(xs, ys, ws, yi, n, interp, final);

  gsl_vector_free(interp);
}

/**
 * Function: comp_vector_polyN
 * The function fits a polynomial function to the y-values
 * and fills the evaluated polynomial values in a temporary
 * vector and, if requested, in the original vector as well.
 *
 * @param xs    - absissa
 * @param ys    - value at xs[]
 * @param ws    - weight in ys[]
 * @param yi    - temprary y-values
 * @param n     - number of points in xs, ys, ws
 * @param final - flagg to store to temp (0) or final (1) vectors
 */
void
comp_vector_polyN (const int m, const double *xs, double *ys,
		    double *ws, double *yi, const int n, const int final)
{
  gsl_vector *interp, *coeffs;
  gsl_matrix *cov;

  coeffs = gsl_vector_alloc(m);
  cov = gsl_matrix_alloc(m,m);

  interp = det_vector_polyN (m, xs, ys, ws, n, coeffs, cov);
  fill_polyN_interp(xs, ys, ws, yi, n, coeffs, cov, interp, final);

  gsl_vector_free(interp);
  gsl_vector_free(coeffs);
  gsl_matrix_free(cov);
}

/**
 * Function: det_vector_linear
 * Performs a linear fit of the xs, ys, and ws arrays.
 * Ignores NaN values. Fitted values are returned in
 * ys and errors in ws.
 *
 * Parameters:
 * @param xs     - absissa
 * @param ys     - value at xs[]
 * @param ws     - error in ys[]
 * @param n      - number of points in xs, ys, ws
 * @param weight - make weighted fit (=1) or not (=0)
 *
 * Returns:
 * @return ret - the covariance values
 */
gsl_vector *
det_vector_linear(const double *xs, double *ys, double *ws,
		  const int n, const int weight)
{
  int i, m;
  double c0, c1, cov00, cov01, cov11, chisq;
  double *xx, *yy, *ww, *tmp, *xmp;
  double median, xean;
  //double xf, yf, yf_err;

  gsl_vector *ret;

  ret = gsl_vector_alloc(10);

  // allocate space for temporary vectors
  tmp = (double *) malloc (n * sizeof (double));
  xmp = (double *) malloc (n * sizeof (double));

  // fill the temporary vectors
  // with background values and weights
  m = 0;
  for (i = 0; i < n; i++)
    {
      if (ws[i] > 0.0)
	{
	  //	  tmp[m] = ws[i];
	  tmp[m] = 1.0/(ws[i]*ws[i]);
	  xmp[m] = xs[i];
	  m++;
	}
    }

  // determine the median weight which
  // is used for all data
  gsl_sort( tmp, 1, m);
  median = gsl_stats_median_from_sorted_data(tmp, 1, m);

  // determine the mean in the independent variable
  xean = gsl_stats_mean (xmp, 1, m);

  // allocate more temporary vectors
  xx = (double *) malloc (m * sizeof (double));
  yy = (double *) malloc (m * sizeof (double));
  ww = (double *) malloc (m * sizeof (double));

  // fill the temporary vectors with
  // background independent/dependent variables
  // plus the weight
  m = 0;
  for (i = 0; i < n; i++)
    {
      if (ws[i] > 0.0)
	{
	 xx[m] = xs[i]-xean;
	 yy[m] = ys[i];
	 ww[m] = median;
	 m++;
	}
    }

  // check for weighted fit
  if (weight)
    // doe the linear fit
    gsl_fit_wlinear (xx, 1, ww, 1, yy, 1, m, &c0, &c1, &cov00,
		     &cov01, &cov11, &chisq);
  else
    // does the linear fit
    gsl_fit_linear (xx, 1, yy, 1, m, &c0, &c1, &cov00,
		    &cov01, &cov11, &chisq);

  gsl_vector_set(ret, 0, xean);
  gsl_vector_set(ret, 1, c0);
  gsl_vector_set(ret, 2, c1);
  gsl_vector_set(ret, 3, cov00);
  gsl_vector_set(ret, 4, cov01);
  gsl_vector_set(ret, 5, cov11);
  gsl_vector_set(ret, 6, chisq);

  // free the arrays
  free(tmp);
  free(xmp);
  free(xx);
  free(yy);
  free(ww);

  return ret;
}

/**
 * Function: fill_linear_interp
 * The function computes and stores linear interpolated values
 * in one or several output vectors.
 *
 * Parameters:
 * @param xs     - double vector containing the x values
 * @param ys     - double vector containing the y values
 * @param ws     - double vector containing the weights associated with ys
 * @param yi     - double vector containing the intermediate y-values
 * @param n      - number of points in xs,ys, and ws (must be greater than m!)
 * @param interp - vector with the fitted values
 * @param final  - flagg to indicate final fit and
 *                 therefore a replacement
 */
void
fill_linear_interp(const double *const xs, double *const ys, double *const ws,
		   double *yi, const int n, gsl_vector *interp,
		   const int final)
{
  double c0, c1, cov00, cov01, cov11, xean;
  double xf, yf, yf_err;
  int i;

  xean  = gsl_vector_get(interp, 0);
  c0    = gsl_vector_get(interp, 1);
  c1    = gsl_vector_get(interp, 2);
  cov00 = gsl_vector_get(interp, 3);
  cov01 = gsl_vector_get(interp, 4);
  cov11 = gsl_vector_get(interp, 5);

  // determine the interpolated values
  // and store them in the input arrays
  for (i = 0; i < n; i++)
    {

      xf = xs[i]-xean;
      gsl_fit_linear_est (xf, c0, c1, cov00, cov01, cov11, &yf, &yf_err);
      yi[i] = yf;

      // for object pixels,
      // fill in the interpolated values
      if (final && ws[i] == 0.0)
	{
	  ys[i] = yf;
	  ws[i] = yf_err;
	}
    }
}

/**
 * Function: det_vector_average
 * The function computes the average and the standard
 * deviation from the values in a vector
 *
 * @param xs  - absissa
 * @param ys  - value at xs[]
 * @param ws  - weight in ys[]
 * @param n   - number of points in xs, ys, ws
 * @param avg - the average
 * @param std - the standard deviation
 */
void
det_vector_average (const double *xs, double *ys,
		    double *ws, const int n, double *avg, double *std)
{
  double *tmp, sum = 0.0;
  int nn = 0;
  int j;

  // count the number of
  // interpolation points
  nn = 0;
  for (j = 0; j < n; j++)
    {
      if (ws[j] != 0.0)
	nn++;
    }

  // allocate a tmp-array
  tmp = malloc (nn * sizeof (double));

  // sum up all values in the
  // interpolation points,
  // and transfer the values to the
  // tmp-array
  nn = 0;
  for (j = 0; j < n; j++)
    {
      if (ws[j] != 0.0)
	{
	  sum += ys[j];
	  tmp[nn] = ys[j];
	  nn++;
	}
    }

  // compute the average
  if (nn > 0)
    *avg = sum / (double)nn;

  // compute the standard deviation
  *std = gsl_stats_sd (tmp, 1, nn);
}

/**
 * Function: det_vector_median
 * The function computes the median and the standard
 * deviation from the values in a vector
 *
 * @param xs  - absissa
 * @param ys  - value at xs[]
 * @param ws  - weight in ys[]
 * @param n   - number of points in xs, ys, ws
 * @param med - the median
 * @param std - the standard deviation
 */
void
det_vector_median (const double *xs, double *ys,
		   double *ws, const int n, double *med, double *std)
{
  double *tmp;
  int nn = 0;
  int j;

  // count the number of
  // interpolation points
  for (j = 0; j < n; j++)
    {
      if (ws[j] != 0.0)
	  nn++;
    }

  // allocate a tmp-array
  tmp = malloc (nn * sizeof (double));

  // transfer the values of the interpolation
  // points to the tmp-array
  nn = 0;
  for (j = 0; j < n; j++)
    {
      if (ws[j] != 0.0)
	{
	  tmp[nn] = ys[j];
	  nn++;
	}
    }

  // sort the tmp array
  gsl_sort (tmp, 1, nn);

  // compute the median
  *med = gsl_stats_median_from_sorted_data (tmp, 1, nn);

  // compute the standard deviation
  *std = gsl_stats_sd (tmp, 1, nn);
}

/**
 * Function:fill_const_value
 * The function fills two constant values into
 * up to three vectors.
 *
 * @param ys    - value at xs[]
 * @param ws    - weight in ys[]
 * @param yi    - value at xs[]
 * @param n     - number of points in xs, ys, ws
 * @param cval  - number of points in xs, ys, ws
 * @param stdev - number of points in xs, ys, ws
 * @param final - flagg which indicates which vectors to fill
 */
void
fill_const_value(double *ys, double *ws, double *yi, const int n,
		 double cval, double stdev, const int final)
{

  int j;
  int m=0;

  // fill the constant value and the stdev
  // in the whole value and error
  // arrays, respectively
  for (j = 0; j < n; j++)
    {
      yi[j] = cval;

      if (final && ws[j] == 0.0)
	{
	  ys[j] = cval;
	  ws[j] = stdev;
	  m++;
	}
    }
}


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

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

#include "test.h"

gsl_function create_function (double (*f)(double, void *))
{
  gsl_function F ;
  F.function = f;
  F.params = 0;
  return F ;
}

gsl_function_fdf create_fdf (double (*f)(double, void *),
                             double (*df)(double, void *),
                             void (*fdf)(double, void *, double *, double *))
{
  gsl_function_fdf FDF ;
  FDF.f = f ;
  FDF.df = df ;
  FDF.fdf = fdf ;
  FDF.params = 0 ;
  return FDF ;
}

/* f(x) = x^{20} - 1 */
/* f'(x) = 20x^{19} */
/* zero at x = 1 or -1 */

double
func1 (double x, void *p)
{
  return pow (x, 20.0) - 1;
}

double
func1_df (double x, void * p)
{
  return 20.0 * pow (x, 19.0);
}

void
func1_fdf (double x, void * p, double *y, double *yprime)
{
  *y = func1 (x, p);
  *yprime = 20.0 * pow (x, 19.0);
}

/* f(x) = sqrt(abs(x))*sgn(x) */
/* f'(x) = 1 / sqrt(abs(x) */
/* zero at x = 0 */
double
func2 (double x, void * p)
{
  double delta;

  if (x > 0)
    delta = 1.0;
  else if (x < 0)
    delta = -1.0;
  else
    delta = 0.0;

  return sqrt (fabs (x)) * delta;
}

double
func2_df (double x, void * p)
{
  return 1 / sqrt (fabs (x));
}

void
func2_fdf (double x, void * p, double *y, double *yprime)
{
  *y = func2 (x, p);
  *yprime = 1 / sqrt (fabs (x));
}

/* f(x) = x^2 - 1e-8 */
/* f'(x) = 2x */
/* zero at x = sqrt(1e-8) or -sqrt(1e-8) */
double
func3 (double x, void * p)
{
  return pow (x, 2.0) - 1e-8;
}

double
func3_df (double x, void * p)
{
  return 2 * x;
}

void
func3_fdf (double x, void * p, double *y, double *yprime)
{
  *y = func3 (x, p);
  *yprime = 2 * x;
}

/* f(x) = x exp(-x) */
/* f'(x) = exp(-x) - x exp(-x) */
/* zero at x = 0 */
double
func4 (double x, void * p)
{
  return x * exp (-x);
}

double
func4_df (double x, void * p)
{
  return exp (-x) - x * exp (-x);
}

void
func4_fdf (double x, void * p, double *y, double *yprime)
{
  *y = func4 (x, p);
  *yprime = exp (-x) - x * exp (-x);
}

/* f(x) = 1/(1+exp(x)) */
/* f'(x) = -exp(x) / (1 + exp(x))^2 */
/* no roots! */
double
func5 (double x, void * p)
{
  return 1 / (1 + exp (x));
}

double
func5_df (double x, void * p)
{
  return -exp (x) / pow (1 + exp (x), 2.0);
}

void
func5_fdf (double x, void * p, double *y, double *yprime)
{
  *y = func5 (x, p);
  *yprime = -exp (x) / pow (1 + exp (x), 2.0);
}

/* f(x) = (x - 1)^7 */
/* f'(x) = 7 * (x - 1)^6 */
/* zero at x = 1 */
double
func6 (double x, void * p)
{
  return pow (x - 1, 7.0);
}

double
func6_df (double x, void * p)
{
  return 7.0 * pow (x - 1, 6.0);
}

void
func6_fdf (double x, void * p, double *y, double *yprime)
{
  *y = func6 (x, p);
  *yprime = 7.0 * pow (x - 1, 6.0);
}

/* sin(x) packaged up nicely. */
double
sin_f (double x, void * p)
{
  return sin (x);
}

double
sin_df (double x, void * p)
{
  return cos (x);
}

void
sin_fdf (double x, void * p, double *y, double *yprime)
{
  *y = sin (x);
  *yprime = cos (x);
}

/* cos(x) packaged up nicely. */
double
cos_f (double x, void * p)
{
  return cos (x);
}

double
cos_df (double x, void * p)
{
  return -sin (x);
}

void
cos_fdf (double x, void * p, double *y, double *yprime)
{
  *y = cos (x);
  *yprime = -sin (x);
}


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

double f1(double x, void* params)
{
    return x*x;
}

double f2(double x, void* params)
{
    return 1 / sqrt(x);
}

void calculateIntegralVoid(double (*f)(double x), double a, double b, int maxInterval, double expected)
{
    FILE* output = fopen("integral.txt", "w");
    int n = 1;
    while (n < maxInterval)
    {
        double dx = (b - a) / (double)n;
        double sum = 0;

        for (int i = 1; i < n; ++i)
        {
            sum += f(a + i * dx);
        }
        sum *= dx;
        fprintf (output,"%d %f\n", n, expected - sum);

        n++;
    }
}

int calculateIntegral(double (*f)(double x), double a, double b, int n, double expected, double error)
{
    double dx = (b - a) / (double)n;
    double sum = 0;

    for (int i = 1; i < n; ++i)
    {
        sum += f(a + i * dx);
    }
    sum *= dx;
    if (fabs(sum - expected) < error)
    {
        printf("Sum with error %.1e error for %d samples: %.18f\n", error, n, sum);
        return 1;
    }
    printf("DIFFERENCE with error %.1e error for %d samples: %.18f\n", error, n, fabs(sum - expected));
    return 1;
}

void tryFindIntergral(double (*f)(double x), double a, double b, double expected, double error, int step)
{
    int interval = step;
    while (!calculateIntegral(f, a, b, interval, expected, error))
    {
        interval += step;
    }
}

void gslIntegral(double (*f)(double x, void* params), double expected)
{
    gsl_integration_workspace * w = gsl_integration_workspace_alloc (1000);

    double result, error;
    double alpha = 1.0;

    gsl_function F;
    F.function = f;
    F.params = α

    gsl_integration_qags (&F, 0, 1, 0, 1e-7, 1000, w, &result, &error);

    printf ("result          = % .18f\n", result);
    printf ("exact result    = % .18f\n", expected);
    printf ("estimated error = % .18f\n", error);
    printf ("actual error    = % .18f\n", result - expected);
    printf ("intervals       = %zu\n", w->size);

    gsl_integration_workspace_free (w);
}

int main (int argc, char* argv[])
{
    // tryFindIntergral(f2, 0, 1.0, 2.0, 1e-3, 1000);
    // tryFindIntergral(f2, 0, 1.0, 2.0, 1e-4, 215000000);
    // tryFindIntergral(f2, 0, 1.0, 2.0, 1e-5, 100000);
    // tryFindIntergral(f2, 0, 1.0, 2.0, 1e-6, 1000000);

    // calculateIntegralVoid(f2, 0.0, 1.0, 100000, 2.0);

    gslIntegral(f2, 2.0);
    return 0;
}


//
// Created by chris on 07.05.21.
//
#pragma once

#include <gsl/gsl_errno.h>
#include <gsl/gsl_fft_real.h>
#include <gsl/gsl_fft_halfcomplex.h>
#include <numeric>
#include <vector>

class test {
public:
    int n;
};

/**
 * Calculates autocorrelation with FFT
 * @param vec
 * @return
 *//*
std::vector<double> ac(const std::vector<double> &vec){
    std::vector<double> fftReturn=vec;

    gsl_fft_real_wavetable *real;
    gsl_fft_halfcomplex_wavetable *hc;
    gsl_fft_real_workspace *work;

    //FFT
    work = gsl_fft_real_workspace_alloc(vec.size());
    real = gsl_fft_real_wavetable_alloc(vec.size());

    gsl_fft_real_transform(&fftReturn[0], 1, vec.size(), real, work);
    gsl_fft_real_wavetable_free(real);

    hc = gsl_fft_halfcomplex_wavetable_alloc(vec.size());

    gsl_fft_halfcomplex_inverse(&fftReturn[0], 1, vec.size(), hc, work);

    gsl_fft_halfcomplex_wavetable_free(hc);
    return fftReturn;
}*/

/**
 * @tparam T
 * @param vec
 * @return maximum element of given vector
 */
template<typename T>
T max(const std::vector<T> &vec) {
    T maxVal = vec[0];
    for (int i = 1; i < vec.size(); ++i) {
        maxVal = std::max(maxVal, vec[i]);
    }
    return maxVal;
}

/**
 * @tparam T
 * @param vec
 * @return minimum element of given vector
 */
template<typename T>
T min(const std::vector<T> &vec) {
    T minVal = vec[0];
    for (int i = 1; i < vec.size(); ++i) {
        minVal = std::min(minVal, vec[i]);
    }
    return minVal;
}

/**
 * Calculates the arithmetic mean of given vector-elements
 * @tparam T
 * @param vec
 * @return the arithmetic mean
 */
template<typename T>
T mean(const std::vector<T> &vec) {
    return std::accumulate(vec.begin(), vec.end(), 0.0)/vec.size();
}

/**
 * Calculates the arithmetic mean of given vector-of-vector-of-elements
 * @tparam T
 * @param vec vector of vector of elements (tensor of rank 2)
 * @return the arithmetic mean of all values
 */
template<typename T>
T mean(const std::vector<std::vector<T>> &vec) {
    std::vector<T> firstMeans;
    firstMeans.reserve(vec.size());
    for(auto &i:vec){
        firstMeans.push_back(mean(i));
    }
    return mean(firstMeans);
}

/**
 * Calculates the standard deviation of given vector-elements
 * @tparam T
 * @param vec
 * @return standard deviation
 */
template<typename T>
T stdev(const std::vector<T> &vec) {
    T summation=0;
    const T vec_mean=mean(vec);
    for (size_t i = 0; i < vec.size(); ++i) {
        summation+=pow(vec[i]-vec_mean,2);
    }
    return sqrt(summation*1.0/(vec.size()-1));
}

/**
 * Calculates the autocorrelation-function of given data-sample
 * @tparam T
 * @param vec of data-sample with length n
 * @return autocorrelation-function in vector with length n/2
 */
template<typename T>
std::vector<T> autoCorr(const std::vector<T> &vec) {
    float mean = std::accumulate(vec.begin(), vec.end(), 0.0f) / vec.size();

    std::vector<T> autocorrelation(vec.size() / 2);
    for (size_t t = 0; t < autocorrelation.size(); t++) {
        float n = 0; // Numerator
        float d = 0; // Denominator
        for (size_t i = 0; i < vec.size() - t; i++) {
            float xim = vec[i] - mean;
            n += xim * (vec[(i + t) % vec.size()] - mean);
            d += xim * xim;
        }
        autocorrelation[t] = n / d;
    }
    return autocorrelation;
}

/**
 * normalizes given vector call by reference
 * @tparam T
 * @param vec vector which should be normalized
 */
template<typename T>
void normalize(std::vector<T> &vec) {
    const T vec_max = max(vec);
    for (T &i : vec) {
        i /= 1.0 * vec_max;
    }
}


#include <sys/time.h>
#include <cblas.h>
#include "kadbg.h"
#include "matrix.h"
#include "kalas.h"

#define MAX_BUF_SIZE 128*1024*1024
#define NUM_OF_STEPS 1024

void test_sgemm( KalasState *state)
{
	int rowA, colA, rowB, colB, rowC, colC;
	Matrix *matA = NULL, *matB = NULL, *matC1 = NULL, *matC2 = NULL;
	float alpha = 1.0, beta = 0.0, e;
  struct timeval t;
  double ts, te;

	for ( rowA = NUM_OF_STEPS; rowA <= MAX_BUF_SIZE / NUM_OF_STEPS
					/ sizeof(float);
				rowA += NUM_OF_STEPS) {
		for ( colA = NUM_OF_STEPS; rowA * colA <= MAX_BUF_SIZE / sizeof(float);
					colA += NUM_OF_STEPS) {

			rowB = colA;
			rowC = rowA;
			for ( colB = NUM_OF_STEPS; rowB * colB <= MAX_BUF_SIZE / sizeof(float);
						colB += NUM_OF_STEPS) {
				colC = colB;

				if ( rowC * colC > MAX_BUF_SIZE / sizeof(float)) break;

				if ( IS_FAILED( ( matA = matrixNew( rowA, colA, colA,
																						sizeof(float), true)) != NULL))
					break;
				if ( IS_FAILED( ( matB = matrixNew( rowB, colB, colB,
																						sizeof(float), true)) != NULL))
					break;
				if ( IS_FAILED( ( matC1 = matrixNew( rowC, colC, colC,
																						 sizeof(float), true)) != NULL))
					break;
				if ( IS_FAILED( ( matC2 = matrixNew( rowC, colC, colC,
																						 sizeof(float), true)) != NULL))
					break;

				printf( "(%d,%d)(%d,%d)=(%d,%d) ",
								rowA, colA, rowB, colB, rowC, colC);

				gettimeofday( &t, NULL);
				ts = t.tv_sec + (double)(t.tv_usec) / 1000000.0;

				cblas_sgemm( CblasRowMajor, CblasNoTrans, CblasNoTrans,
										 matC1->row, matC1->col, matA->col,
										 alpha, matA->elm, matA->col,
										 matB->elm, matB->col, beta,
										 matC1->elm, matC1->col);

				gettimeofday( &t, NULL);
				te = t.tv_sec + (double)(t.tv_usec) / 1000000.0;
				printf( "%f s %f GFlops ", (te - ts),
								(double)rowA * (double)colA * (double)colB * 2.0
								/ ( te - ts) / 1000000000.0);

				gettimeofday( &t, NULL);
				ts = t.tv_sec + (double)(t.tv_usec) / 1000000.0;

				kalasSgemm( state, clblasRowMajor, clblasNoTrans, clblasNoTrans,
										matC2->row, matC2->col, matA->col,
										alpha, matA->elm, matA->ld,
										matB->elm, matB->ld, beta,
										matC2->elm, matC2->ld);

				gettimeofday( &t, NULL);
				te = t.tv_sec + (double)(t.tv_usec) / 1000000.0;
				printf( "%f s %f GFlops ", (te - ts),
								(double)rowA * (double)colA * (double)colB * 2.0
								/ ( te - ts) / 1000000000.0);

				printf( "%e err", matrixCalcDiff( matC1, matC2));
				putchar( '\n');
				matrixDelete( matA); matA = NULL;
				matrixDelete( matB); matB = NULL;
				matrixDelete( matC1); matC1 = NULL;
				matrixDelete( matC2); matC2 = NULL;
			}
		}
  }
}

void test_dgemm( KalasState *state)
{
	int rowA, colA, rowB, colB, rowC, colC;
	Matrix *matA = NULL, *matB = NULL, *matC1 = NULL, *matC2 = NULL;
	double alpha = 1.0, beta = 0.0, e;
  struct timeval t;
  double ts, te;

	for ( rowA = NUM_OF_STEPS; rowA <= MAX_BUF_SIZE / NUM_OF_STEPS
					/ sizeof(double);
				rowA += NUM_OF_STEPS) {
		for ( colA = NUM_OF_STEPS; rowA * colA <= MAX_BUF_SIZE / sizeof(double);
					colA += NUM_OF_STEPS) {

			rowB = colA;
			rowC = rowA;
			for ( colB = NUM_OF_STEPS; rowB * colB <= MAX_BUF_SIZE / sizeof(double);
						colB += NUM_OF_STEPS) {
				colC = colB;

				if ( rowC * colC > MAX_BUF_SIZE / sizeof(double)) break;

				if ( IS_FAILED( ( matA = matrixNew( rowA, colA, colA,
																						sizeof(double), true)) != NULL))
					break;
				if ( IS_FAILED( ( matB = matrixNew( rowB, colB, colB,
																						sizeof(double), true)) != NULL))
					break;
				if ( IS_FAILED( ( matC1 = matrixNew( rowC, colC, colC,
																						 sizeof(double), true)) != NULL))
					break;
				if ( IS_FAILED( ( matC2 = matrixNew( rowC, colC, colC,
																						 sizeof(double), true)) != NULL))
					break;

				printf( "(%d,%d)(%d,%d)=(%d,%d) ",
								rowA, colA, rowB, colB, rowC, colC);

				gettimeofday( &t, NULL);
				ts = t.tv_sec + (double)(t.tv_usec) / 1000000.0;

				cblas_dgemm( CblasRowMajor, CblasNoTrans, CblasNoTrans,
										 matC1->row, matC1->col, matA->col,
										 alpha, matA->elm, matA->col,
										 matB->elm, matB->col, beta,
										 matC1->elm, matC1->col);

				gettimeofday( &t, NULL);
				te = t.tv_sec + (double)(t.tv_usec) / 1000000.0;
				printf( "%f s %f GFlops ", (te - ts),
								(double)rowA * (double)colA * (double)colB * 2.0
								/ ( te - ts) / 1000000000.0);

				gettimeofday( &t, NULL);
				ts = t.tv_sec + (double)(t.tv_usec) / 1000000.0;

				kalasDgemm( state, clblasRowMajor, clblasNoTrans, clblasNoTrans,
										matC2->row, matC2->col, matA->col,
										alpha, matA->elm, matA->ld,
										matB->elm, matB->ld, beta,
										matC2->elm, matC2->ld);

				gettimeofday( &t, NULL);
				te = t.tv_sec + (double)(t.tv_usec) / 1000000.0;
				printf( "%f s %f GFlops ", (te - ts),
								(double)rowA * (double)colA * (double)colB * 2.0
								/ ( te - ts) / 1000000000.0);

				printf( "%e err", matrixCalcDiff( matC1, matC2));
				putchar( '\n');
				matrixDelete( matA); matA = NULL;
				matrixDelete( matB); matB = NULL;
				matrixDelete( matC1); matC1 = NULL;
				matrixDelete( matC2); matC2 = NULL;
			}
		}
  }
}

int main( void)
{
	KalasState *state;

	state = kalasStateNew();

	test_sgemm( state);
	test_dgemm( state);

	kalasStateDelete( state);
	return 0;
}


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

int main(int argc, char *argv[]) {
    unsigned long seed = 1234;
    int n = 10;
    if (argc > 1)
        n = atoi(argv[1]);
    gsl_rng_env_setup();
    gsl_rng *rng = gsl_rng_alloc(gsl_rng_mt19937);
    gsl_rng_set(rng, seed);
    for (int i = 0; i < n; i++) {
        double x = gsl_rng_uniform(rng);
        printf("%.12f\n", x);
    }
    gsl_rng_free(rng);
    return EXIT_SUCCESS;
}


/*
* Copyright (c) 2014, Newcastle University, UK.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright notice,
*    this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
*    this list of conditions and the following disclaimer in the documentation
*    and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/

// Linear Regression
// Dan Jackson, 2014 (but see below)

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

#include "linearregression.h"

// Linear regression with one independent variable
double *LinearModelFitOneIndependent(int n, double *y, double *x)
{
	static double coef[3];		// offset(intersect), scale(gradient), spare(to be compatible with two-variable version)
	int i;

	// sum(Xi * Yi)
	// sum(Xi^2)
	double sumx = 0, sumy = 0;
	double sumxy = 0, sumxsq = 0;
	for (i = 0; i < n; i++)
	{
		sumx += x[i];
		sumy += y[i];
		sumxy += x[i] * y[i];
		sumxsq += x[i] * x[i];
	}

	// Calculate mean of x and y
	double xmean = n > 0 ? sumx / n : 0;
	double ymean = n > 0 ? sumy / n : 0;

	//                 sum(x*y) - (n * xm * ym)
	// Slope (b):  b = ------------------------
	//                  sum(x^2) - (n * xm^2)
	double bNumerator = sumxy - (n * xmean * ymean);
	double bDenominator = sumxsq - (n * xmean * xmean);
	double b = bDenominator != 0.0 ? bNumerator / bDenominator : 0.0;

	// Intercept (a):  a = ym - b * xm
	double a = ymean - (b * xmean);

	// Line of best fit: y = a + b * X
	coef[0] = a;
	coef[1] = b;
	coef[2] = 0.0;		// just to be compatible with two-variable version

	return &coef[0];
}

// Linear regression with two independent variables
double *LinearModelFitTwoIndependent(int n, double *y, double *x1, double *x2)
{
	// Implemented from information from: http://faculty.cas.usf.edu/mbrannick/regression/Reg2IV.html
	static double coef[3];		// offset (intersect), scale 1 (gradient 1), scale 2 (gradient 2)
	int i;

	double sumx1 = 0, sumx2 = 0, sumy = 0;
	double sumx1y = 0, sumx2y = 0;
	double sumx1x2 = 0;
	double sumx1sq = 0, sumx2sq = 0;
	for (i = 0; i < n; i++)
	{
		sumx1 += x1[i];
		sumx2 += x2[i];
		sumy += y[i];
		sumx1y += x1[i] * y[i];
		sumx2y += x2[i] * y[i];
		sumx1x2 += x1[i] * x2[i];
		sumx1sq += x1[i] * x1[i];
		sumx2sq += x2[i] * x2[i];
	}

	// Calculate mean of x1, x2, and y
	double x1mean = n > 0 ? sumx1 / n : 0;
	double x2mean = n > 0 ? sumx2 / n : 0;
	double ymean = n > 0 ? sumy / n : 0;

	//                   sum(x2^2).sum(x1*y) - sum(x1*x2).sum(x2*y)
	// Slope (b1):  b1 = ------------------------------------------
	//                       sum(x1^2).sum(x2^2) - sum(x1*x2)^2
	double b1Numerator = (sumx2sq * sumx1y) - (sumx1x2 * sumx2y);
	double b1Denominator = (sumx1sq * sumx2sq) - (sumx1x2 * sumx1x2);
	double b1 = b1Numerator != 0.0 ? b1Numerator / b1Denominator : 0.0;

	//                   sum(x1^2).sum(x2*y) - sum(x1*x2).sum(x1*y)
	// Slope (b2):  b2 = ------------------------------------------
	//                       sum(x1^2).sum(x2^2) - sum(x1*x2)^2
	double b2Numerator = (sumx1sq * sumx2y) - (sumx1x2 * sumx1y);
	double b2Denominator = b1Denominator;		// Same as denominator of b1
	double b2 = b2Denominator != 0.0 ? b2Numerator / b2Denominator : 0.0;

	// Intercept (a):  a = ym - b1 * x1m - b2 * x2m
	double a = ymean - (b1 * x1mean) - (b2 * x2mean);

	// Line of best fit: y = a + b1 * x1 + b2 * x2
	coef[0] = a;
	coef[1] = b1;
	coef[2] = b2;

	return &coef[0];
}

#ifdef ENABLE_APPROXIMATE
// Linear regression with two independent variables, weighted
// NOTE: This is not really correct...
double *LinearModelFitTwoIndependentWeightedApproximately(int n, double *y, double *x1, double *x2, double *weights)
{
	double *weightedY = (double *)malloc(sizeof(double) * n);
	double *weightedX1 = (double *)malloc(sizeof(double) * n);
	double *weightedX2 = (double *)malloc(sizeof(double) * n);
	int i;

	// HACK: To reduce influence on intercept
	double influenceSum = 0;
	for (i = 0; i < n; i++)
	{
		double sw = sqrt(weights[i]);
		weightedY[i] = y[i] * sw;
		weightedX1[i] = x1[i] * sw;
		weightedX2[i] = x2[i] * sw;
		influenceSum += sw;
	}

	double *coef = LinearModelFitTwoIndependent(n, weightedY, weightedX1, weightedX2);
	double influence = n != 0 ? influenceSum / n : 0;
	if (influence != 0.0) { coef[0] /= influence; }

	free(weightedY);
	free(weightedX1);
	free(weightedX2);
	return coef;
}
#endif

#ifdef ENABLE_GSL

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

/* General weighted case in gsl/multifit/multilinear.c */

double *LinearModelFitTwoIndependentWeighted(int n, double *y, double *x1, double *x2, double *weights)
{
	#define NPARAMS 2

	// Allocate
	gsl_matrix *matrixX = gsl_matrix_alloc(n, NPARAMS);
	gsl_vector *vectorW = gsl_vector_alloc(n);
	gsl_vector *vectorY = gsl_vector_alloc(n);
	gsl_vector *vectorC = gsl_vector_alloc(1 + NPARAMS);

	// Copy data
	int i;
	for (i = 0; i < n; i++)
	{
		// NPARAMS
		gsl_matrix_set(matrixX, i, 0, x1[i]);
		gsl_matrix_set(matrixX, i, 1, x2[i]);
		gsl_vector_set(vectorW, i, weights[i]);
		gsl_vector_set(vectorY, i, y[i]);
	}

	// Compute
	gsl_multifit_linear_workspace *work = gsl_multifit_linear_alloc(n, NPARAMS);
	int ret = gsl_multifit_wlinear(matrixX, vectorW, vectorY, vectorC, /*gsl_matrix * cov*/ NULL, /*double * chisq*/ NULL, work);
	gsl_multifit_linear_free(work);

	static double coef[NPARAMS + 1];
	// NPARAMS+1
	coef[0] = gsl_vector_get(vectorC, 0);
	coef[1] = gsl_vector_get(vectorC, 1);
	coef[2] = gsl_vector_get(vectorC, 2);

	// Free
	gsl_matrix_free(matrixX);
	gsl_vector_free(vectorW);
	gsl_vector_free(vectorY);
	gsl_vector_free(vectorC);

	return coef;
}
#endif


/*
** False disovery rate (FDR)
**
** G.Lohmann, Jan 2017
*/

#include <viaio/VImage.h>
#include <viaio/Vlib.h>
#include <viaio/mu.h>
#include <viaio/option.h>

#include <stdio.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_histogram.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_math.h>

/* get array of FDR values */
double *GetFdr(gsl_histogram *nullhist,gsl_histogram *realhist)
{
  size_t i=0;
  size_t nbins = gsl_histogram_bins (nullhist);

  /* cumulative distribution functions (CDF) */
  gsl_histogram_pdf *cdfz = gsl_histogram_pdf_alloc(nbins);
  gsl_histogram_pdf *cdf0 = gsl_histogram_pdf_alloc(nbins);
  gsl_histogram_pdf_init (cdfz,realhist);
  gsl_histogram_pdf_init (cdf0,nullhist);

  /* fdr array */
  double *fdr = (double *)VCalloc(nbins,sizeof(double));
  for (i=0; i<nbins; i++) {
    double Fz = cdfz->sum[i];
    double F0 = cdf0->sum[i];
    double xFdr = 1.0;
    if (Fz  < 1.0) xFdr = (1.0-F0)/(1.0-Fz);
    if (xFdr > 1.0) xFdr = 1.0;
    if (xFdr < 0.0) xFdr = 0.0;
    fdr[i] = xFdr;
  }

  /* enforce monotonicity */
  fdr[0] = 1.0;
  for (i=1; i<nbins; i++) {
    if (fdr[i] > fdr[i-1]) fdr[i] = fdr[i-1];
  }
  return fdr;
}

/* thresholding */
void ApplyFdrThreshold(VImage dest,double alpha)
{
  double u=0,beta=1.0-alpha;
  size_t i,n=0;
  VFloat *pp = VImageData(dest);
  for (i=0; i<VImageNPixels(dest); i++) {
    u = (*pp);
    if (u < beta) *pp = 0;
    else n++;
    pp++;
  }
  fprintf(stderr," Number of voxels at Fdr < %.4f:  %lu\n",alpha,n);
}

/* estimage false discovery rates */
void FDR(VImage src,VImage dest,gsl_histogram *nullhist,gsl_histogram *realhist,double alpha)
{
  size_t j;
  double u=0,tiny=0.002;
  double hmin = gsl_histogram_min (realhist);
  double hmax = gsl_histogram_max (realhist);

  /* compute Fdr */
  double *Fdr = GetFdr(nullhist,realhist);

  /* apply FDR */
  VFillImage(dest,VAllBands,0);
  int b,r,c;
  for (b=0; b<VImageNBands(dest); b++) {
    for (r=0; r<VImageNRows(dest); r++) {
      for (c=0; c<VImageNColumns(dest); c++) {

	u = VGetPixel(src,b,r,c);
	if (u > hmax) u = hmax-tiny;
	if (u < hmin) u = hmin+tiny;
	if (fabs(u) > 0) {
	  if (gsl_histogram_find (realhist,(double)u,&j) == GSL_SUCCESS) {
	    VPixel(dest,b,r,c,VFloat) = 1.0 - Fdr[j];
	  }
	}
      }
    }
  }

  /* apply threshold if needed */
  if (alpha < 0.99) ApplyFdrThreshold(dest,alpha);
  return;
}


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

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

/* This is the uniform distribution in the range [a, b)

   p(x) dx = 1/(b-a) dx   if  a <= x < b
   .....   = 0            otherwise

 */

double
gsl_ran_flat (const gsl_rng * r, const double a, const double b)
{
  double u = gsl_rng_uniform (r);

  /* A uniform distribution over [a,b) */

  return a * (1 - u) + b * u;
}

double
gsl_ran_flat_pdf (double x, const double a, const double b)
{
  if (x < b && x >= a)
    {
      return 1 / (b - a);
    }
  else
    {
      return 0;
    }
}


/* ode-initval2/modnewton1.c
 *
 * Copyright (C) 2008, 2009, 2010 Tuomo Keskitalo
 *
 * 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.
 */

/* A modified Newton iteration method for solving non-linear
   equations in implicit Runge-Kutta methods.
*/

/* References:

   Ascher, U.M., Petzold, L.R., Computer methods for ordinary
   differential and differential-algebraic equations, SIAM,
   Philadelphia, 1998. ISBN 0898714125, 9780898714128

   Hairer, E., Wanner, G., Solving Ordinary Differential
   Equations II: Stiff and Differential-Algebraic Problems,
   Springer, 1996. ISBN 3540604529, 9783540604525
*/

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>

#include "odeiv_util.h"

typedef struct
{
  /* iteration matrix I - h A (*) J */
  gsl_matrix *IhAJ;

  /* permutation for LU-decomposition */
  gsl_permutation *p;

  /* difference vector for kth Newton iteration */
  gsl_vector *dYk;

  /* scaled dYk with desired error level */
  gsl_vector *dScal;

  /* current Runge-Kutta points (absolute values) */
  double *Yk;

  /* function values at points Yk */
  double *fYk;

  /* right hand side of Newton iteration formula */
  gsl_vector *rhs;

  /* stopping criterion value from previous step */
  double eeta_prev;
}
modnewton1_state_t;

static void *
modnewton1_alloc (size_t dim, size_t stage)
{
  modnewton1_state_t *state =
    (modnewton1_state_t *) malloc (sizeof (modnewton1_state_t));

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

  state->IhAJ = gsl_matrix_alloc (dim * stage, dim * stage);

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

  state->p = gsl_permutation_alloc (dim * stage);

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

  state->dYk = gsl_vector_alloc (dim * stage);

  if (state->dYk == 0)
    {
      gsl_permutation_free (state->p);
      gsl_matrix_free (state->IhAJ);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for dYk", GSL_ENOMEM);
    }

  state->dScal = gsl_vector_alloc (dim * stage);

  if (state->dScal == 0)
    {
      gsl_vector_free (state->dYk);
      gsl_permutation_free (state->p);
      gsl_matrix_free (state->IhAJ);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for dScal", GSL_ENOMEM);
    }

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

  if (state->Yk == 0)
    {
      gsl_vector_free (state->dScal);
      gsl_vector_free (state->dYk);
      gsl_permutation_free (state->p);
      gsl_matrix_free (state->IhAJ);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for Yk", GSL_ENOMEM);
    }

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

  if (state->fYk == 0)
    {
      free (state->Yk);
      gsl_vector_free (state->dScal);
      gsl_vector_free (state->dYk);
      gsl_permutation_free (state->p);
      gsl_matrix_free (state->IhAJ);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for Yk", GSL_ENOMEM);
    }

  state->rhs = gsl_vector_alloc (dim * stage);

  if (state->rhs == 0)
    {
      free (state->fYk);
      free (state->Yk);
      gsl_vector_free (state->dScal);
      gsl_vector_free (state->dYk);
      gsl_permutation_free (state->p);
      gsl_matrix_free (state->IhAJ);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for rhs", GSL_ENOMEM);
    }

  state->eeta_prev = GSL_DBL_MAX;

  return state;
}

static int
modnewton1_init (void *vstate, const gsl_matrix * A,
                 const double h, const gsl_matrix * dfdy,
                 const gsl_odeiv2_system * sys)
{
  /* Initializes the method by forming the iteration matrix IhAJ
     and generating its LU-decomposition
   */

  modnewton1_state_t *state = (modnewton1_state_t *) vstate;

  gsl_matrix *const IhAJ = state->IhAJ;
  gsl_permutation *const p = state->p;

  const size_t dim = sys->dimension;
  const size_t stage = A->size1;

  state->eeta_prev = GSL_DBL_MAX;

  /* Generate IhAJ */

  {
    size_t i, j, k, m;

    for (i = 0; i < dim; i++)
      for (j = 0; j < dim; j++)
        for (k = 0; k < stage; k++)
          for (m = 0; m < stage; m++)
            {
              size_t x = dim * k + i;
              size_t y = dim * m + j;

              if (x != y)
                gsl_matrix_set (IhAJ, x, y,
                                -h * gsl_matrix_get (A, k, m) *
                                gsl_matrix_get (dfdy, i, j));
              else
                gsl_matrix_set (IhAJ, x, y,
                                1.0 - h * gsl_matrix_get (A, k, m) *
                                gsl_matrix_get (dfdy, i, j));
            }
  }

  /* decompose */

  {
    int signum;
    int s = gsl_linalg_LU_decomp (IhAJ, p, &signum);

    if (s != GSL_SUCCESS)
      return s;
  }

  return GSL_SUCCESS;
}

static int
modnewton1_solve (void *vstate, const gsl_matrix * A,
                  const double c[], const double t, const double h,
                  const double y0[], const gsl_odeiv2_system * sys,
                  double YZ[], const double errlev[])
{
  /* Solves the non-linear equation system resulting from implicit
     Runge-Kutta methods by a modified Newton iteration. The
     modified Newton iteration with this problem is stated in the
     form

     IhAJ * dYk = rhs

     in which IhAJ is the iteration matrix: IhAJ = (I - hA (*) J) in
     which (*) is the Kronecker matrix product (size of IhAJ =
     dim*stage, dim*stage), dYk is the Runge-Kutta point (Y)
     difference vector for kth Newton iteration: dYk = Y(k+1) - Y(k),
     and rhs = Y(k) - y0 - h * sum j=1..stage (a_j * f(Y(k)))

     This function solves dYk by LU-decomposition of IhAJ with partial
     pivoting.
   */

  modnewton1_state_t *state = (modnewton1_state_t *) vstate;

  gsl_matrix *const IhAJ = state->IhAJ;
  gsl_permutation *const p = state->p;
  gsl_vector *const dYk = state->dYk;
  double *const Yk = state->Yk;
  double *const fYk = state->fYk;
  gsl_vector *const rhs = state->rhs;
  double *const eeta_prev = &(state->eeta_prev);
  gsl_vector *const dScal = state->dScal;

  const size_t dim = sys->dimension;
  const size_t stage = A->size1;

  int status = GSL_CONTINUE;    /* Convergence status for Newton iteration */

  /* Set starting values for iteration. Use starting values of Y(k) =
     y0. FIXME: Implement better initial guess described in Hairer and
     Wanner.
   */

  {
    size_t j, m;

    gsl_vector_set_zero (dYk);

    for (j = 0; j < stage; j++)
      for (m = 0; m < dim; m++)
        Yk[j * dim + m] = y0[m];
  }

  /* Loop modified Newton iterations */

  {
    size_t iter = 0;
    size_t j, m, n;

    /* Maximum number of Newton iterations. */
    const size_t max_iter = 7;

    double dScal_norm = 0.0;    /* Newton iteration step length */
    double dScal_norm_prev = 0.0;       /* Previous dScal_norm */

    while (status == GSL_CONTINUE && iter < max_iter)
      {
        iter++;

        /* Update Y(k) and evaluate function */

        for (j = 0; j < stage; j++)
          {
            for (m = 0; m < dim; m++)
              {
                Yk[j * dim + m] += gsl_vector_get (dYk, j * dim + m);
              }

            {
              int s = GSL_ODEIV_FN_EVAL (sys, t + c[j] * h, &Yk[j * dim],
                                         &fYk[j * dim]);
              if (s != GSL_SUCCESS)
                {
                  return s;
                }
            }
          }

        /* Calculate rhs  */

        for (j = 0; j < stage; j++)
          for (m = 0; m < dim; m++)
            {
              double sum = 0;

              for (n = 0; n < stage; n++)
                sum += gsl_matrix_get (A, j, n) * fYk[n * dim + m];

              gsl_vector_set (rhs, j * dim + m,
                              -1.0 * Yk[j * dim + m] + y0[m] + h * sum);
            }

        /* Solve dYk */

        {
          int s = gsl_linalg_LU_solve (IhAJ, p, rhs, dYk);

          if (s != GSL_SUCCESS)
            {
              return s;
            }
        }

        /* Evaluate convergence according to method by Hairer and
           Wanner, section IV.8. The iteration step is normalized by
           desired error level before calculation of the norm, to take
           the tolerance on each component of y into account.
         */

        {
          /* relative change in two Newton iteration steps */
          double theta_k;

          /* transformation of theta_k */
          double eeta_k = 0.0;

          for (j = 0; j < stage; j++)
            for (m = 0; m < dim; m++)
              {
                gsl_vector_set (dScal, j * dim + m,
                                gsl_vector_get (dYk, j * dim + m)
                                / errlev[m]);
              }

          dScal_norm_prev = dScal_norm;
          dScal_norm = gsl_blas_dnrm2 (dScal);

          theta_k = dScal_norm / dScal_norm_prev;

          if (iter > 1)
            {
              /* check for increase in step size, which indicates divergence */

              if (theta_k >= 1.0)
                {
                  return GSL_FAILURE;
                }

              eeta_k = theta_k / (1.0 - theta_k);
            }

          else
            {
              eeta_k = pow (GSL_MAX_DBL (*eeta_prev, GSL_DBL_EPSILON), 0.8);
            }

          *eeta_prev = eeta_k;

          if (eeta_k * dScal_norm < 1.0)
            {
              status = GSL_SUCCESS;
            }
        }
      }
  }

  /* No convergence reached within allowed iterations */

  if (status != GSL_SUCCESS)
    {
      return GSL_FAILURE;
    }

  /* Give solution in YZ */

  else
    {
      size_t j, m;

      for (j = 0; j < stage; j++)
        for (m = 0; m < dim; m++)
          YZ[j * dim + m] =
            Yk[j * dim + m] + gsl_vector_get (dYk, j * dim + m);

      return status;
    }
}

static void
modnewton1_free (void *vstate)
{
  modnewton1_state_t *state = (modnewton1_state_t *) vstate;

  gsl_vector_free (state->rhs);
  free (state->fYk);
  free (state->Yk);
  gsl_vector_free (state->dScal);
  gsl_vector_free (state->dYk);
  gsl_permutation_free (state->p);
  gsl_matrix_free (state->IhAJ);
  free (state);
}


#include <string.h>
#include <math.h>

#include <cblas.h>

#define SWAP(x, y, T) do { T SWAP = x; x = y; y = SWAP; } while (0)

//The Struct that contains the k nearest neighbors of m queries
typedef struct knnresult{
  int    * nidx;    //!< Indices (0-based) of nearest neighbors [m-by-k]
  double * ndist;   //!< Distance of nearest neighbors          [m-by-k]
  int      m;       //!< Number of query points                 [scalar]
  int      k;       //!< Number of nearest neighbors            [scalar]
} knnresult;

//A sorting alogorithm for the k first items of a list
void k_select(int * nidx,
            double * ndist,
            int k,
            int n)
{
    for(int i=0; i<k; i++){
        int minidx = i;
        double min = ndist[i];
        for(int j=i+1; j<n; j++){
            if(min > ndist[j]){
                min = ndist[j];
                minidx = nidx[j];
                SWAP(ndist[i],ndist[j], double);
                SWAP(nidx[i], nidx[j], int);
            }
        }
    }
}

int partition(int * nidx,
            double * ndist,
            int left,
            int right,
            int pivot)
{

    double val = ndist[pivot];

    SWAP(ndist[right], ndist[pivot], double);
    SWAP(nidx[right], nidx[pivot], int);

    int storeIdx = left;

    for(int i=left; i<right; i++){
        if(ndist[i] < val){
            SWAP(ndist[storeIdx], ndist[i], double);
            SWAP(nidx[storeIdx], nidx[i], int);
            storeIdx++;

        }
    }
    SWAP(ndist[storeIdx], ndist[right], double);
    SWAP(nidx[storeIdx], nidx[right], int);
    return storeIdx;
}

void quickselect(int * nidx,
            double * ndist,
            int left,
            int right,
            int k)
{
    if(left >= right) return;

    int pivot = left + rand() % (right - left + 1);
    pivot = partition(nidx, ndist, left, right, pivot);

    if(k == pivot){
        return;
    }else if(k< pivot){
        quickselect(nidx, ndist, left, pivot - 1, k);
    }else{
        quickselect(nidx, ndist, pivot + 1, right, k);
    }

}

//Calculates a column of the Euclidean distance matrix D
double *calc_Dcol(  double *X,
                    double *Y,
                    int n,
                    int d,
                    int row)
{
    double *Dcol= (double *)malloc(n * sizeof(double));

    for(int i=0; i<n; i++) Dcol[i] = 0;

    for(int i=0; i<n; i++){
        for(int k=0; k<d; k++){
            Dcol[i] += X[k + i*d]*X[k + i*d] + Y[k + row*d]*Y[k + row*d] - 2*X[k + i*d]*Y[k + row*d];
        }
    }

    return Dcol;
}

//Calculates the Euclidean distance matrix D
double *calc_D( double *X,
                double *Y,
                int n,
                int d,
                int m)
{
    double *D= (double *)malloc(n * m * sizeof(double));

    double xsum[n],
           ysum[m];
    for(int i=0; i<n; i++) xsum[i] = cblas_ddot(d, X + i*d, 1, X + i*d, 1);
    for(int i=0; i<m; i++) ysum[i] = cblas_ddot(d, Y + i*d, 1, Y + i*d, 1);

    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, n, m, d, -2, X, d, Y, d, 0, D, m);

    for(int i=0; i<n; i++){
        for(int j=0; j<m; j++){
            D[j + m*i] += xsum[i] + ysum[j];
        }
    }

    return D;
}

void calc_part_knn( double *D,
                    int n,
                    int m,
                    int k,
                    int col,
                    int *final_nidx,
                    double *final_ndist)
{

    int * nidx = (int *)malloc(n * sizeof(int));
    double * ndist = (double *)malloc(n * sizeof(double));

    for(int i=0; i<n; i++){
        nidx[i] = i;
        ndist[i] = D[i * m + col];
    }

    quickselect(nidx, ndist, 0, n-1, k);

    memcpy(final_nidx, nidx, k * sizeof(int));
    memcpy(final_ndist, ndist, k * sizeof(double));

    free(nidx);
    free(ndist);
}

//Finds for each point in a query set Y the k nearest neighbors
//in the corpus set X
knnresult kNN(  double *X,
                double *Y,
                int n,
                int m,
                int d,
                int k)
{

    knnresult knn;
    knn.m = m;
    knn.k = k;
    knn.nidx = (int *)malloc(m * k * sizeof(int));
    knn.ndist = (double *)malloc(m * k * sizeof(double));

    int max_m = 1e3;
    if(m > max_m){

        int iteration_m[m/max_m];
        for(int i=0; i<m/max_m; i++) iteration_m[i] = max_m;
        iteration_m[m/max_m - 1] = max_m + m%max_m;

        int displ = 0;
        for(int i=0; i<m/max_m; i++){
            double *D = calc_D(X, Y + displ * d, n, d, iteration_m[i]);

            for(int j=0; j<iteration_m[i]; j++) calc_part_knn(D, n, iteration_m[i], k, j, knn.nidx + (j + displ) * k, knn.ndist + (j + displ) * k);

            free(D);
            displ += iteration_m[i];
        }

    }else{

        double *D = calc_D(X, Y, n, d, m);

        for(int j=0; j<m; j++) calc_part_knn(D, n, m, k, j, knn.nidx + j * k, knn.ndist + j * k);

        free(D);

    }

    return knn;
}

knnresult alt_kNN(  double *X,
                    double *Y,
                    int n,
                    int m,
                    int d,
                    int k)
{

    knnresult knn;
    knn.m = m;
    knn.k = k;
    knn.nidx = (int *)malloc(m * k * sizeof(int));
    knn.ndist = (double *)malloc(m * k * sizeof(double));

    for(int j=0; j<m; j++){

        double * ndist = calc_Dcol(X, Y, n, d, j);

        int * nidx = (int *)malloc(n * sizeof(int));
        for(int i=0; i<n; i++) nidx[i] = i;

        quickselect(nidx, ndist, 0, n-1, k);

        memcpy(knn.nidx + j * k, nidx, k * sizeof(int));
        memcpy(knn.ndist + j * k, ndist, k * sizeof(double));

        free(nidx);
        free(ndist);
    }

    return knn;
}


#include <stdio.h>
#include <time.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <cblas.h>

int max(int x, int y){
   if(x > y) return x;
   else return y;
}
void PrintMatrix(float* pMatrix, const size_t nR, const size_t nC, const CBLAS_ORDER Order) {
    unsigned int i, j;
    if (Order == CblasRowMajor) {
        for (i = 0; i < nR; i++) {
            for (j = 0; j < nC; j++) {
                printf("%f \t ", pMatrix[i * nC + j]); // !!!
            }
            printf("\n"); // !!!
        }
    } else {
        for (i = 0; i < nR; i++) {
            for (j = 0; j < nC; j++) {
                printf("%f \t ", pMatrix[i + j* nR ]); // !!!
            }
            printf("\n"); // !!!
        }
    }
    printf("\n"); // !!!
}

int main(void)
{
    const int m = 4;
    const int n = 5;
    const int k = 1;

    float A[] = { 8, 4, 7, 3, 5, 1, 1, 3, 2, 1, 2, 3, 2, 0, 1, 1, 2, 3, 4, 1};
    float B[5] = { -1, 2, -1, 1, 2 };

    float alpha = 1.0, beta = 0.0;
    int lda, ldb, ldc;

    float * C = (float*) malloc(m * k * sizeof(float));
    for (int i = 0; i < m*k; i++) C[i] = 0.0;

    //working cblas_sgemv(CblasRowMajor, CblasNoTrans, m, n, alpha, A, n, B, k, beta, C, k);
    //lda = max(1,n); ldb = max(1,k); ldc = max(1,k);  // refer manual and switch n and k
    //cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, m,k,n, alpha, A, lda, B, ldb, beta, C, ldc);

    // working cblas_sgemv(CblasColMajor, CblasNoTrans, m, n, alpha, A, m, B, k, beta, C, k);
    // working cblas_sgemv(CblasColMajor, CblasTrans, m, n, alpha, A, m, B, k, beta, C, k);
    lda = max(1,n); ldb = max(1,n); ldc = max(1,m);  // refer manual and switch n and k
    cblas_sgemm(CblasColMajor, CblasTrans, CblasNoTrans, m,k,n, alpha, A, lda, B, ldb, beta, C, ldc);

    PrintMatrix(C, m, k, CblasRowMajor);

    free(C);

    return 0;
}


/* randist/exppow.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 James Theiler, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/* The exponential power probability distribution is

   p(x) dx = (1/(2 a Gamma(1+1/b))) * exp(-|x/a|^b) dx

   for -infty < x < infty. For b = 1 it reduces to the Laplace
   distribution.

   The exponential power distribution is related to the gamma
   distribution by E = a * pow(G(1/b),1/b), where E is an exponential
   power variate and G is a gamma variate.

   We use this relation for b < 1. For b >=1 we use rejection methods
   based on the laplace and gaussian distributions which should be
   faster.

   See P. R. Tadikamalla, "Random Sampling from the Exponential Power
   Distribution", Journal of the American Statistical Association,
   September 1980, Volume 75, Number 371, pages 683-686.

*/

double
gsl_ran_exppow (const gsl_rng * r, const double a, const double b)
{
  if (b < 1)
    {
      double u = gsl_rng_uniform (r) ;
      double v = gsl_ran_gamma (r, 1/b, 1.0) ;
      double z = a * pow(v, 1/b) ;

      if (u > 0.5)
	{
	  return z ;
	}
      else
	{
	  return -z ;
	}
    }
  else if (b == 1)
    {
      /* Laplace distribution */
      return gsl_ran_laplace (r, a) ;
    }
  else if (b < 2)
    {
      /* Use laplace distribution for rejection method */

      double x, y, h, ratio, u ;

      /* Scale factor chosen by upper bound on ratio at b = 2 */

      double s = 1.4489 ;
      do
	{
	  x = gsl_ran_laplace (r, a) ;
	  y = gsl_ran_laplace_pdf (x,a) ;
	  h = gsl_ran_exppow_pdf (x,a,b) ;
	  ratio = h/(s * y) ;
	  u = gsl_rng_uniform (r) ;
	}
      while (u > ratio) ;

      return x ;
    }
  else if (b == 2)
    {
      /* Gaussian distribution */
      return gsl_ran_gaussian (r, a/sqrt(2.0)) ;
    }
  else
    {
      /* Use gaussian for rejection method */

      double x, y, h, ratio, u ;
      const double sigma = a / sqrt(2.0) ;

      /* Scale factor chosen by upper bound on ratio at b = infinity.
	 This could be improved by using a rational function
	 approximation to the bounding curve. */

      double s = 2.4091 ;  /* this is sqrt(pi) e / 2 */

      do
	{
	  x = gsl_ran_gaussian (r, sigma) ;
	  y = gsl_ran_gaussian_pdf (x, sigma) ;
	  h = gsl_ran_exppow_pdf (x, a, b) ;
	  ratio = h/(s * y) ;
	  u = gsl_rng_uniform (r) ;
	}
      while (u > ratio) ;

      return x;
    }
}

double
gsl_ran_exppow_pdf (const double x, const double a, const double b)
{
  double p ;
  double lngamma = gsl_sf_lngamma (1+1/b) ;
  p = (1/(2*a)) * exp(-pow(fabs(x/a),b) - lngamma);
  return p;
}


/* cdf/binomial.c
 *
 * Copyright (C) 2004 Jason H. Stover.
 * Copyright (C) 2004 Giulio Bottazzi
 *
 * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_sf_gamma.h>

#include "error.h"

/* Computes the cumulative distribution function for a binomial
   random variable. For a binomial random variable X with n trials
   and success probability p,

           Pr( X <= k ) = Pr( Y >= p )

   where Y is a beta random variable with parameters k+1 and n-k.

   The binomial distribution has the form,

   prob(k) =  n!/(k!(n-k)!) *  p^k (1-p)^(n-k) for k = 0, 1, ..., n

   The cumulated distributions can be expressed in terms of normalized
   incomplete beta functions (see Abramowitz & Stegun eq. 26.5.26 and
   eq. 6.6.3).

   Reference:

   W. Feller, "An Introduction to Probability and Its
   Applications," volume 1. Wiley, 1968. Exercise 45, page 173,
   chapter 6.
 */

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

double
gsl_cdf_binomial_P (const unsigned int k, const double p, const unsigned int n)
{
  double P;
  double a;
  double b;

  if (p > 1.0 || p < 0.0)
    {
      CDF_ERROR ("p < 0 or p > 1", GSL_EDOM);
    }

  if (k >= n)
    {
      P = 1.0;
    }
  else
    {
      a = (double) k + 1.0;
      b = (double) n - k;
      P = gsl_cdf_beta_Q (p, a, b);
    }

  return P;
}

double
gsl_cdf_binomial_Q (const unsigned int k, const double p, const unsigned int n)
{
  double Q;
  double a;
  double b;

  if (p > 1.0 || p < 0.0)
    {
      CDF_ERROR ("p < 0 or p > 1", GSL_EDOM);
    }

  if (k >= n)
    {
      Q = 0.0;
    }
  else
    {
      a = (double) k + 1.0;
      b = (double) n - k;
      Q = gsl_cdf_beta_P (p, a, b);
    }

  return Q;
}