#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_multifit.h>

//DEFINES
#define SUCCESS 1
#define FAILURE -1
#define PROGRESS "===================="
#define PRG_WIDTH 20

//FUNCTIONS DECLARATION
int dcca_comp(double *, double *, double *, int, int, int, int, double *, int);
int rows_number(char *);
double mean(double *, int, int);
void cumsum(double *, double *, int);
void slice_vec(double *, double *, int, int);
void polynomialFit(int, int, double *, double *, double *);

//MAIN
int main(int argc, char **argv)
{
    //inputs
    int input_args = 7;
    if(argc < input_args){
        printf("Not enough input arguments!\n");
        return FAILURE;
    }else if(argc > input_args){
        printf("Too many input arguments!\n");
        return FAILURE;
    }
    char file_name1[255]; //path+name for the first file
    memset(file_name1, 0x00, sizeof(file_name1));
    sprintf(file_name1, "%s", argv[1]);
    char file_name2[255]; //path+name for the second file
    memset(file_name2, 0x00, sizeof(file_name2));
    sprintf(file_name2, "%s", argv[2]);
    int smallest_win_size = atoi(argv[3]); //minimum size for the sliding window
    int biggest_win_num = atoi(argv[4]); //number of windows in the last step
    int pol_ord = atoi(argv[5]); //polynomial order for fit
    char path_out[255]; //path+name for the output file
    memset(path_out, 0x00, sizeof(path_out));
    sprintf(path_out, "%s/rho_mw=%d_Mw=%d_pol=%d.txt", argv[6], smallest_win_size, biggest_win_num, pol_ord);
    //files length
    int N1 = rows_number(file_name1);
    if(N1 == FAILURE){
        printf("Cannot open input file (1).\n");
        return FAILURE;
    }
    int N2 = rows_number(file_name2);
    if(N2 == FAILURE){
        printf("Cannot open input file (2).\n");
        return FAILURE;
    }
    //time series
    double *ts1, *ts2;
    ts1 = calloc(N1, sizeof(double));
    if(!ts1){
        printf("MALLOC ERROR (ts1)\n");
        return FAILURE;
    }
    ts2 = calloc(N2, sizeof(double));
    if(!ts2){
        printf("MALLOC ERROR (ts2)\n");
        return FAILURE;
    }
    FILE *f1, *f2;
    f1 = fopen(file_name1, "r");
    if(!f1){
        printf("Cannot open input file (1).\n");
        return FAILURE;
    }
    for(int i = 0; i < N1; i++)
        fscanf(f1, "%lf", ts1+i);
    fclose(f1);
    f2 = fopen(file_name2, "r");
    if(!f2){
        printf("Cannot open input file (2).\n");
        return FAILURE;
    }
    for(int i = 0; i < N2; i++)
        fscanf(f2, "%lf", ts2+i);
    fclose(f2);
    //time series must have the same length
    int L = N1;
    if(N1 < N2){
        printf("Series 2 is longer than series 1. Data length reduced to: <%d>\n", L);
        slice_vec(ts2, ts2, 0, N1-1);
    }else if(N1 > N2){
        slice_vec(ts1, ts1, 0, N2-1);
        L = N2;
        printf("Series 1 is longer than series 2. Data length reduced to: <%d>\n", L);
    }else{
        printf("Data length: <%d>\n", L);
    }
    //time vector
    double *time;
    time = calloc(L, sizeof(double));
    if(!time){
        printf("MALLOC ERROR (time)\n");
        return FAILURE;
    }
    for(int i = 0; i < L; i++)
        time[i] = (double)(i+1);
    //time series minus its mean
    double ts1_ave = mean(ts1, L, L);
    double ts2_ave = mean(ts2, L, L);
    double *ts1_nomean, *ts2_nomean;
    ts1_nomean = calloc(L, sizeof(double));
    if(!ts1_nomean){
        printf("MALLOC ERROR (ts1_nomean)\n");
        return FAILURE;
    }
    ts2_nomean = calloc(L, sizeof(double));
    if(!ts2_nomean){
        printf("MALLOC ERROR (ts2_nomean)\n");
        return FAILURE;
    }
    for(int i = 0; i < L; i++){
        ts1_nomean[i] = ts1[i] - ts1_ave;
        ts2_nomean[i] = ts2[i] - ts2_ave;
    }
    //cumulative sum
    double *ts1_cum, *ts2_cum;
    ts1_cum = calloc(L, sizeof(double));
    if(!ts1_cum){
        printf("MALLOC ERROR (ts1_cum)\n");
        return FAILURE;
    }
    ts2_cum = calloc(L, sizeof(double));
    if(!ts2_cum){
        printf("MALLOC ERROR (ts2_cum)\n");
        return FAILURE;
    }
    printf("Data integration...");
    cumsum(ts1_nomean, ts1_cum, L);
    cumsum(ts2_nomean, ts2_cum, L);
    printf("done!\n");
    //dcca computation
    int biggest_win_size = L / biggest_win_num;
    int win_range = biggest_win_size - smallest_win_size + 1;
    double *F_DCCA, *F_DFA_x, *F_DFA_y, *rho;
    F_DCCA = calloc(win_range, sizeof(double));
    F_DFA_x = calloc(win_range, sizeof(double));
    F_DFA_y = calloc(win_range, sizeof(double));
    rho = calloc(win_range, sizeof(double));
    printf("Computing DCCA between series 1 and series 2...\n");
    int ret = dcca_comp(time, ts1_cum, ts2_cum, L, smallest_win_size, biggest_win_size, pol_ord, F_DCCA, win_range);
    if(ret == FAILURE)
        return FAILURE;
    printf("Computing DCCA between series 1 and series 1...\n");
    ret = dcca_comp(time, ts1_cum, ts1_cum, L, smallest_win_size, biggest_win_size, pol_ord, F_DFA_x, win_range);
    if(ret == FAILURE)
        return FAILURE;
    printf("Computing DCCA between series 2 and series 2...\n");
    ret = dcca_comp(time, ts2_cum, ts2_cum, L, smallest_win_size, biggest_win_size, pol_ord, F_DFA_y, win_range);
    if(ret == FAILURE)
        return FAILURE;
    FILE *fOut;
    fOut = fopen(path_out, "w");
    if(!fOut){
        printf("Cannot open output file.\n");
        return FAILURE;
    }
    printf("Computing rho...");
    for(int i = 0; i < win_range; i++){
        F_DFA_x[i] = sqrt(F_DFA_x[i]);
        F_DFA_y[i] = sqrt(F_DFA_y[i]);
        rho[i] = F_DCCA[i] / (double)(F_DFA_x[i] * F_DFA_y[i]);
        fprintf(fOut, "%lf %lf %lf %lf\n", F_DCCA[i], F_DFA_x[i], F_DFA_y[i], rho[i]);
    }
    printf("done\n");
    printf("Output file...");
    fclose(fOut);
    printf("done\n");
    //free memory
    free(ts1); free(ts2); free(time);
    free(ts1_nomean); free(ts2_nomean);
    free(ts1_cum); free(ts2_cum);
    free(F_DCCA); free(F_DFA_x); free(F_DFA_y); free(rho);
    return 0;
}

//FUNCTIONS
int dcca_comp(double *t, double *y1, double *y2, int N, int min_win_size, int last_win_len, int ord, double *F, int range_dcca)
{
    //fluctuations vector and other arrays
    int F_len = N - min_win_size;
    double *F_nu, *t_fit, *y_fit1, *y_fit2, *diff_vec, *fit_coeffs1, *fit_coeffs2;
    F_nu = calloc(F_len, sizeof(double));
    if(!F_nu){
        printf("MALLOC ERROR (F_nu)\n");
        return FAILURE;
    }
    t_fit = calloc((last_win_len+1), sizeof(double));
    if(!t_fit){
        printf("MALLOC ERROR (t_fit)\n");
        return FAILURE;
    }
    y_fit1 = calloc((last_win_len+1), sizeof(double));
    if(!y_fit1){
        printf("MALLOC ERROR (y_fit1)\n");
        return FAILURE;
    }
    y_fit2 = calloc((last_win_len+1), sizeof(double));
    if(!y_fit2){
        printf("MALLOC ERROR (y_fit2)\n");
        return FAILURE;
    }
    diff_vec = calloc((last_win_len+1), sizeof(double));
    if(!diff_vec){
        printf("MALLOC ERROR (diff_vec)\n");
        return FAILURE;
    }
    fit_coeffs1 = calloc(ord+1, sizeof(double));
    if(!fit_coeffs1){
        printf("MALLOC ERROR (fit_coeffs1)\n");
        return FAILURE;
    }
    fit_coeffs2 = calloc(ord+1, sizeof(double));
    if(!fit_coeffs2){
        printf("MALLOC ERROR (fit_coeffs2)\n");
        return FAILURE;
    }
    //computation
    int s, N_s, start_lim, end_lim;
    for(int i = 0; i < range_dcca; i++){
        double perc = i * 100 / (double)range_dcca;
        int prg = (i * PRG_WIDTH) / range_dcca;
        printf(" [%.*s%*s] %.2lf%%\r", prg, PROGRESS, PRG_WIDTH-prg, "", perc);
        fflush(stdout);
        s = i + min_win_size;
        N_s = N - s;
        for(int v = 0; v < N_s; v++){
            start_lim = v;
            end_lim = v + s;
            slice_vec(t, t_fit, start_lim, end_lim);
            slice_vec(y1, y_fit1, start_lim, end_lim);
            slice_vec(y2, y_fit2, start_lim, end_lim);
            polynomialFit(s+1, ord+1, t_fit, y_fit1, fit_coeffs1);
            polynomialFit(s+1, ord+1, t_fit, y_fit2, fit_coeffs2);
            for(int j = 0; j <= s; j++){
                for(int k = 0; k < ord+1; k++){
                    y_fit1[j] -= fit_coeffs1[k] * pow(t_fit[j], k);
                    y_fit2[j] -= fit_coeffs2[k] * pow(t_fit[j], k);
                }
                diff_vec[j] = y_fit1[j] * y_fit2[j];
            }
            F_nu[v] = mean(diff_vec, s+1, s-1);
        }
        F[i] = mean(F_nu, N_s, N_s);
    }
    printf(" [%s] 100.00%%\r", PROGRESS);
    fflush(stdout);
    printf("\n");
    free(F_nu); free(t_fit); free(y_fit1); free(y_fit2);
    free(diff_vec); free(fit_coeffs1); free(fit_coeffs2);
    return SUCCESS;
}

int rows_number(char *file_name)
{
    FILE *f;
    int stop;
    int lines = 0;
    f = fopen(file_name,"r");
    if(!f){
        printf("Cannot open file %s\n", file_name);
        return FAILURE;
    }
    while(!feof(f)){
        stop = fgetc(f);
        if(stop == '\n')
            lines++;
    }
    fclose(f);
    return lines;
}

double mean(double *vec, int vecL, int L)
{
    double avg = 0.0;
    for(int i = 0; i < vecL; i++)
        avg += vec[i];
    avg /= (double)L;
    return avg;
}

void cumsum(double *vec, double *sum_vec, int L)
{
    sum_vec[0] = vec[0];
    for(int i = 1; i < L; i++)
        sum_vec[i] = sum_vec[i-1] + vec[i];
}

void slice_vec(double *all_vec, double *sliced_vec, int start, int end)
{
    for(int i = 0; i <= (end-start); i++)
        sliced_vec[i] = all_vec[start+i];
}

void polynomialFit(int obs, int degree, double *dx, double *dy, double *store)
{
    gsl_multifit_linear_workspace *ws;
    gsl_matrix *cov, *X;
    gsl_vector *y, *c;
    double chisq;
    int i, j;
    //alloc
    X = gsl_matrix_alloc(obs, degree);
    y = gsl_vector_alloc(obs);
    c = gsl_vector_alloc(degree);
    cov = gsl_matrix_alloc(degree, degree);
    //computation
    for(i = 0; i < obs; i++){
        for(j = 0; j < degree; j++)
            gsl_matrix_set(X, i, j, pow(dx[i], j));
        gsl_vector_set(y, i, dy[i]);
    }
    ws = gsl_multifit_linear_alloc(obs, degree);
    gsl_multifit_linear(X, y, c, cov, &chisq, ws);
    for(i = 0; i < degree; i++)
        store[i] = gsl_vector_get(c, i);
    //free memory
    gsl_multifit_linear_free(ws);
    gsl_matrix_free(X);
    gsl_matrix_free(cov);
    gsl_vector_free(y);
    gsl_vector_free(c);
}


/*
 * Copyright (c) 2009-2010 Simon van Heeringen <s.vanheeringen@ncmls.ru.nl>
 *
 * This module is free software. You can redistribute it and/or modify it under
 * the terms of the MIT License, see the file COPYING included with this
 * distribution.
 *
 * This module contains all the code to compare motifs
 *
 */

#include <Python.h>
#include <math.h>
#include <string.h>
#include <stdio.h>
#include <gsl/gsl_cdf.h>

void fill_matrix(double matrix[][4], PyObject *matrix_o) {
	// Parse a PyObject matrix into a C array
	// Structure of the matrix is as follows: [[freqA, freqC, freqG, freqT], ...]
	int matrix_len = PyList_Size(matrix_o);
	int i,j;
	PyObject *item;
	for (i = 0; i < matrix_len; i++) {

		PyObject *row = PyList_GetItem(matrix_o, i);
		if (!PyList_Check(row)) {
			PyErr_SetString( PyExc_TypeError, "wrong matrix structure");
			return;
		}
		if (PyList_Size(row) != 4) {
			PyErr_SetString( PyExc_TypeError, "4 nucleotides!");
			return;
		}
		for (j = 0; j < 4; j++) {
			item = PyList_GetItem(row, j);
			matrix[i][j] = PyFloat_AsDouble(item);
		}
	}
}

double mean(double array[], double length) {
	double sum = 0;
	int i;
	for (i = 0; i < length; i++) {
		sum += array[i];
	}
	return sum / length;
}

double sum(double array[], double length) {
	double sum = 0;
	int i;
	for (i = 0; i < length; i++) {
		sum += array[i];
	}
	return sum;
}

double wic(double col1[], double col2[]) {
		int n;
		double x,y;
		double log2 = log(2.0);
		double factor = 2.5;
		double score = 0;;
		double score_a = 0.0;
		double score_b = 0.0;
		double pseudo = 0.0000001;

		for (n = 0; n < 4; n++) {
			x = col1[n] * log((col1[n] + pseudo) / 0.25) / log2;
			y = col2[n] * log((col2[n] + pseudo) / 0.25) / log2;
			score += fabs(x - y);
			score_a += x;
			score_b += y;
		}
		return sqrt(score_a * score_b) - factor * score;
}

double matrix_wic_mean(double matrix1[][4], double matrix2[][4], int length) {
	// Return the mean pcc of two matrices
	int i;
	double result[length];

	for (i = 0; i < length; i++ ) {
		result[i] = wic(matrix1[i], matrix2[i]);
	}
	return mean(result, length);
}

double matrix_wic_sum(double matrix1[][4], double matrix2[][4], int length) {
	// Return the mean pcc of two matrices
	int i;
	double result[length];

	for (i = 0; i < length; i++ ) {
		result[i] = wic(matrix1[i], matrix2[i]);
	}
	return sum(result, length);
}

double pcc(double col1[], double col2[]) {
	// Return the Pearson correlation coefficient of two motif columns
	int n;
	double sum1 = 0;
	double sum2 = 0;
	double mean1, mean2;

	for (n = 0; n < 4; n++) {
		sum1 += col1[n];
		sum2 += col2[n];
	}
	mean1 = sum1 / 4;
	mean2 = sum2 / 4;

	float a = 0;
	float x = 0;
	float y = 0;
	for (n = 0; n < 4; n++) {
		if (((col1[n] - mean1) != 0) && ((col2[n] - mean2) != 0)) {
			a += (col1[n] - mean1) * (col2[n] - mean2);
			x += pow((col1[n] - mean1), 2);
			y += pow((col2[n] - mean2), 2);
		}
		else {
			return 0;
		}
	}

	return  a / sqrt(x * y);
}

double matrix_pcc_mean(double matrix1[][4], double matrix2[][4], int length) {
	// Return the mean pcc of two matrices
	int i;
	double result[length];

	for (i = 0; i < length; i++ ) {
		result[i] = pcc(matrix1[i], matrix2[i]);
	}
	return mean(result, length);
}

double ed(double col1[], double col2[]) {
	// Return the euclidian distance of two motif columns
	int n;
	double score = 0;

	for (n = 0; n < 4; n++) {
		score = score + pow((col1[n] - col2[n]), 2);
	}

	return  -sqrt(score);
}

double matrix_ed_mean(double matrix1[][4], double matrix2[][4], int length) {
	// Return the mean euclidian distance of two matrices
	int i;
	double result[length];

	for (i = 0; i < length; i++ ) {
		result[i] = ed(matrix1[i], matrix2[i]);
	}
	return mean(result, length);
}

double distance(double col1[], double col2[]) {
	// Return the distance between two motifs (Harbison et al.)
	int n;
	double d = 0;

	for (n = 0; n < 4; n++) {
		d = d + pow(col1[n] - col2[n], 2);
	}
	d = d * 1 / sqrt(2);

	return  -d;
}

double matrix_distance_mean(double matrix1[][4], double matrix2[][4], int length) {
	// Return the mean distance (Harbison et al.) of two matrices
	int i;
	double result[length];

	for (i = 0; i < length; i++ ) {
		result[i] = distance(matrix1[i], matrix2[i]);
	}
	return mean(result, length);
}

double chisq(double col1[], double col2[]) {
	// Return the distance between two motifs (Harbison et al.)
	int n;
	double sum1 = 0;
	double sum2 = 0;
	double e1, e2;
	double c = 0;

	for (n = 0; n < 4; n++) {
		sum1 += col1[n];
		sum2 += col2[n];
	}

	for (n = 0; n < 4; n++) {
		e1 = (sum1 * (col1[n] + col2[n])) / (sum1 + sum2);
		e2 = (sum2 * (col1[n] + col2[n])) / (sum1 + sum2);
		c += pow((col1[n] - e1), 2) / e1;
		c += pow((col2[n] - e2), 2) / e2;

	}

	return 1 - gsl_cdf_chisq_P(c, 3.0);
}

double matrix_chisq_mean(double matrix1[][4], double matrix2[][4], int length) {
	// Return the mean distance (Harbison et al.) of two matrices
	int i;
	double result[length];

	for (i = 0; i < length; i++ ) {
		result[i] = chisq(matrix1[i], matrix2[i]);
	}
	return mean(result, length);
}

static PyObject * c_metrics_score(PyObject *self, PyObject * args)
{
	PyObject *matrix1_o;
	PyObject *matrix2_o;
	const char *metric;
	const char *combine;
	int matrix1_len;
	int matrix2_len;

	if (!PyArg_ParseTuple(args, "OOss", &matrix1_o, &matrix2_o, &metric, &combine)) {
		PyErr_SetString( PyExc_TypeError, "Error parsing arguments");
		return NULL;
	}

	// Retrieve frequency matrix
	if (!PyList_Check(matrix1_o)) {
		PyErr_SetString( PyExc_TypeError, "Error: missing required argument matrix1");
		return NULL;
	}

	if (!PyList_Check(matrix2_o)) {
		PyErr_SetString( PyExc_TypeError, "Error: missing required argument matrix2");
		return NULL;
	}
	// Fill matrix1
	matrix1_len = PyList_Size(matrix1_o);
	double matrix1[matrix1_len][4];
	fill_matrix(matrix1, matrix1_o);

	// Fill matrix2
	matrix2_len = PyList_Size(matrix2_o);
	double matrix2[matrix2_len][4];
	fill_matrix(matrix2, matrix2_o);

	if (matrix1_len != matrix2_len) {
		PyErr_SetString( PyExc_TypeError, "Error: matrices have different sizes");
		return NULL;
	}
	int i;
	double result[matrix1_len];
	if (!strcmp(metric, "chisq")) {
		for (i = 0; i < matrix1_len; i++ ) {
			result[i] = chisq(matrix1[i], matrix2[i]);
		}
	}
	else if (!strcmp(metric, "wic")) {
		for (i = 0; i < matrix1_len; i++ ) {
			result[i] = wic(matrix1[i], matrix2[i]);
		}
	}
	else if (!strcmp(metric, "ed")) {
		for (i = 0; i < matrix1_len; i++ ) {
			result[i] = ed(matrix1[i], matrix2[i]);
		}
	}
	else if (!strcmp(metric, "pcc")) {
		for (i = 0; i < matrix1_len; i++ ) {
			result[i] = pcc(matrix1[i], matrix2[i]);
		}
	}
	else if (!strcmp(metric, "distance")) {
		for (i = 0; i < matrix1_len; i++ ) {
			result[i] = distance(matrix1[i], matrix2[i]);
		}
	}
	else {
			PyErr_SetString( PyExc_TypeError, "Unknown metric");
			return NULL;
	}

	if (!strcmp(combine, "mean")) {
		return Py_BuildValue("f", mean(result, matrix1_len));
	}
	else if (!strcmp(combine, "sum")) {
		return Py_BuildValue("f", sum(result, matrix1_len));
	}
	else {
			PyErr_SetString( PyExc_TypeError, "Unknown combine");
			return NULL;
	}

}

int get_truncate_len(len1, len2, pos) {
	//

	if (pos < 0) {
		len2 += pos;
	}
	else if (pos > 0) {
		len1 -= pos;
	}

	if (len1 > len2) {
		return len2;
	}
	else {
		return len1;
	}

}

void fill_tmp_matrices(double matrix1[][4], double matrix2[][4], int pos, int l, double tmp_matrix1[][4], double tmp_matrix2[][4]) {
	int start1 = 0;
	int start2 = 0;
	int i, n;

	if (pos < 0) {
		start2 = -pos;
	}
	else {
		start1 = pos;
	}

	for (i = 0; i < l; i++) {
		for (n = 0; n < 4; n++) {
			tmp_matrix1[i][n] = matrix1[start1 + i][n];
			tmp_matrix2[i][n] = matrix2[start2 + i][n];
		}
	}

}

int index_at_max(double scores[], int len) {
	double max = scores[0];
	int i_at_max = 0;
	int i;

	if (len == 1) {
		return 0;
	}

	for (i = 1; i < len; i++) {
		if (scores[i] > max) {
			max = scores[i];
			i_at_max = i;
		}
	}
	return i_at_max;
}

void fill_rc_matrix(double matrix[][4], int len, double rc_matrix[][4]) {
	int i = 0;

	for (i = 0; i < len; i++) {
		rc_matrix[i][0] = matrix[len - i - 1][3];
		rc_matrix[i][1] = matrix[len - i - 1][2];
		rc_matrix[i][2] = matrix[len - i - 1][1];
		rc_matrix[i][3] = matrix[len - i - 1][0];
	}

}

static PyObject * c_metrics_max_subtotal(PyObject *self, PyObject * args)
{
	PyObject *matrix1_o;
	PyObject *matrix2_o;
	const char *metric;
	const char *combine;
	int matrix1_len;
	int matrix2_len;
	if (!PyArg_ParseTuple(args, "OOss", &matrix1_o, &matrix2_o, &metric, &combine)) {
		PyErr_SetString( PyExc_TypeError, "Error parsing arguments");
		return NULL;
	}

	// Retrieve frequency matrix
	if (!PyList_Check(matrix1_o)) {
		PyErr_SetString( PyExc_TypeError, "Error: missing required argument matrix1");
		return NULL;
	}

	if (!PyList_Check(matrix2_o)) {
		PyErr_SetString( PyExc_TypeError, "Error: missing required argument matrix2");
		return NULL;
	}
	// Fill matrix1
	matrix1_len = PyList_Size(matrix1_o);
	double matrix1[matrix1_len][4];
	fill_matrix(matrix1, matrix1_o);

	// Fill matrix2
	matrix2_len = PyList_Size(matrix2_o);
	double matrix2[matrix2_len][4];
	fill_matrix(matrix2, matrix2_o);

	// Fill rc matrix2
	double rc_matrix2[matrix2_len][4];
	fill_rc_matrix(matrix2, matrix2_len, rc_matrix2);

	// Assign correct function

	int pos = -2;
	int l;
	float max_score;
	int min_overlap = 6;
	int nr_matches;
	int start_pos = -(matrix2_len - min_overlap);
	nr_matches = matrix1_len + matrix2_len - 2 * min_overlap + 1;
	int i;
	int positions[nr_matches * 2];
	double scores[nr_matches * 2];
	int orients[nr_matches * 2];

	double (*ptr_metric_function)(double[][4], double[][4], int) = NULL;

	if ((!strcmp(metric, "chisq")) && (!strcmp(combine, "mean"))) {
		ptr_metric_function = &matrix_chisq_mean;
	}
	else if ((!strcmp(metric, "wic")) && (!strcmp(combine, "mean"))) {
		ptr_metric_function = &matrix_wic_mean;
	}
	else if ((!strcmp(metric, "wic")) && (!strcmp(combine, "sum"))) {
		ptr_metric_function = &matrix_wic_sum;
	}
	else if ((!strcmp(metric, "pcc")) && (!strcmp(combine, "mean"))) {
		ptr_metric_function = &matrix_pcc_mean;
	}
	else if ((!strcmp(metric, "ed")) && (!strcmp(combine, "mean"))) {
		ptr_metric_function = &matrix_ed_mean;
	}
	else if ((!strcmp(metric, "distance")) && (!strcmp(combine, "mean"))) {
		ptr_metric_function = &matrix_distance_mean;
	}
	else {
		PyErr_SetString( PyExc_TypeError, "Unknown metric or combination");
		return NULL;
	}

	for (i = 0; i < nr_matches; i++) {
		pos = i + start_pos;
		l = get_truncate_len(matrix1_len, matrix2_len, pos);

		// Normal matrix
		double tmp_matrix1[l][4];
		double tmp_matrix2[l][4];

		fill_tmp_matrices(matrix1, matrix2, pos, l, tmp_matrix1, tmp_matrix2);
		max_score = (*ptr_metric_function) (tmp_matrix1, tmp_matrix2, l);
		positions[i] = pos;
		scores[i] = max_score;
		orients[i] = 1;
		//printf("CC 1 Len %i score %f\n", l, max_score);

		// Reverse complement matrix
		fill_tmp_matrices(matrix1, rc_matrix2, pos, l, tmp_matrix1, tmp_matrix2);
		max_score = (*ptr_metric_function) (tmp_matrix1, tmp_matrix2, l);
		positions[i + nr_matches] = pos;
		scores[i + nr_matches] = max_score;
		orients[i+ nr_matches] = -1;

		//printf("CC -1 Len %i score %f\n", l, max_score);
	}

	i = index_at_max(scores, nr_matches * 2);
	return Py_BuildValue("fii", scores[i], positions[i], orients[i]);

}
static PyObject * c_metrics_pwmscan(PyObject *self, PyObject * args)
{
	PyObject *seq_o;
	PyObject *pwm_o;
	PyObject *cutoff_o;
	char *seq;
	int seq_len;
	int n_report;
	int pwm_len;
	int i, j;
	double zeta = 0.01;
	int scan_rc;

	if (!PyArg_ParseTuple(args, "OOOii", &seq_o, &pwm_o, &cutoff_o, &n_report, &scan_rc))
		return NULL;

	// Sequence and length
	if (!PyString_Check(seq_o))
		return NULL;
	seq = PyString_AsString(seq_o);
	seq_len = PyString_Size(seq_o);
	//Py_DECREF(seq_o);

	// Retrieve frequency matrix
	if (!PyList_Check(pwm_o))
		return NULL;

	// Weight matrices
	pwm_len = PyList_Size(pwm_o);
	double pwm[pwm_len][4];
	fill_matrix(pwm, pwm_o);
	//Py_DECREF(pwm_o);

	// Cutoff for every spacer length
	double cutoff;
	cutoff = PyFloat_AsDouble(cutoff_o);

	// Scan sequence
	int j_max = seq_len - pwm_len + 1;
	double score_matrix[j_max];
	double rc_score_matrix[j_max];
	double score, rc_score;

	if (j_max < 0) { j_max = 0;}
	int m;
	double g = 0.25;
	double z = 0.01;
	for (j = 0; j < j_max; j++) {
		score = 0;
		rc_score = 0;
		for (m = 0; m < pwm_len; m++) {
			switch(seq[j + m]) {
				case 'A':
					score += log(pwm[m][0] / g + z);
					rc_score += log(pwm[pwm_len - m - 1][3] / g + z);
					break;
				case 'C':
					score += log(pwm[m][1] / g + z);
					rc_score += log(pwm[pwm_len - m - 1][2] / g + z);
					break;
				case 'G':
					score += log(pwm[m][2] / g + z);
					rc_score += log(pwm[pwm_len - m - 1][1] / g + z);
					break;
				case 'T':
					score += log(pwm[m][3] / g + z);
					rc_score += log(pwm[pwm_len - m - 1][0] / g + z);
					break;
			}

		}
		score_matrix[j] = score;
		rc_score_matrix[j] = rc_score;
	}

	// Initialize matrices of n_report highest scores and corresponding positions + strands
	double maxScores[n_report];
	double maxPos[n_report];
	int maxStrand[n_report];
	for (j = 0; j < n_report; j++) {
		maxScores[j] = -100;
		maxPos[j] = -1;
		maxStrand[j] = 1;
	}
	PyObject*  return_list = PyList_New(0);

	int p,q;
	PyObject *x;
	for (j = 0; j < j_max; j++) {
		score = score_matrix[j];
		if (n_report > 0) {
			if (score >= cutoff) {
				p = n_report - 1;
				while ((p >= 0) && (score > maxScores[p])) {
					p--;
				}
				if (p < (n_report-1)) {
					for (q = n_report - 1; q > (p + 1); q--) {
						maxScores[q] = maxScores[q - 1];
						maxPos[q] = maxPos[q - 1];
						maxStrand[q] = maxStrand[q - 1];
					}
					maxScores[p + 1] = score;
					maxPos[p + 1] = j;
					maxStrand[p + 1] = 1;
				}
			}
		}
		else {
			if (score >= cutoff) {
				PyObject* row = PyList_New(0);
				x = PyFloat_FromDouble(score);
				PyList_Append(row, x); Py_DECREF(x);
				x = PyInt_FromLong((long) j);
				PyList_Append(row, x); Py_DECREF(x);
				x = PyInt_FromLong((long) 1);
				PyList_Append(row, x); Py_DECREF(x);
				PyList_Append(return_list, row);
				Py_DECREF(row);
			}
		}
	}

	if (scan_rc) {
		for (j = 0; j < j_max; j++) {
			score = rc_score_matrix[j];
			if (n_report > 0) {
				if (score >= cutoff) {
					p = n_report - 1;
					while ((p >= 0) && (score > maxScores[p])) {
						p--;
					}
					if (p < (n_report-1)) {
						for (q = n_report - 1; q > (p + 1); q--) {
							maxScores[q] = maxScores[q - 1];
							maxPos[q] = maxPos[q - 1];
							maxStrand[q] = maxStrand[q - 1];
						}
						maxScores[p + 1] = score;
						maxPos[p + 1] = j;
						maxStrand[p + 1] = -1;
					}
				}
			}
			else {
				if (score >= cutoff) {
					PyObject* row = PyList_New(0);
					x = PyFloat_FromDouble(score);
					PyList_Append(row, x); Py_DECREF(x);
					x = PyInt_FromLong((long) j);
					PyList_Append(row, x); Py_DECREF(x);
					x =  PyInt_FromLong((long) 1);
					PyList_Append(row, x); Py_DECREF(x);
					PyList_Append(return_list, row);
					Py_DECREF(row);
				}
			}
		}
	}

	for (i = 0; i < n_report; i++) {
		if (maxPos[i] > - 1) {
			PyObject* row = PyList_New(0);
			x = PyFloat_FromDouble(maxScores[i]);
			PyList_Append(row, x);  Py_DECREF(x);
			x = PyInt_FromLong((long)maxPos[i]);
			PyList_Append(row, x); Py_DECREF(x);
			x = PyInt_FromLong((long)maxStrand[i]);
			PyList_Append(row, x); Py_DECREF(x);
			PyList_Append(return_list, row);
			Py_DECREF(row);
		}
	}

	return return_list;
}

static PyMethodDef CoreMethods[] = {
	{"score", c_metrics_score, METH_VARARGS,"Test"},
	{"c_max_subtotal", c_metrics_max_subtotal, METH_VARARGS,"Test"},
	{"pwmscan", c_metrics_pwmscan, METH_VARARGS,"Test"},
	{NULL, NULL, NULL, 0, NULL}
};

PyMODINIT_FUNC
initc_metrics(void)
{
	(void) Py_InitModule("c_metrics", CoreMethods);
};


#include <stdio.h>
#include <stdlib.h>
#include <cblas.h>
#include <math.h>
#include "../inc/knnring.h"

// Application Entry Point
knnresult kNN(double * X, double * Y, int n, int m, int d, int k)
{

	// Calculate distances matrix D - D is row-major and nxm
	double* tempD = calculateD(X, Y, n, m, d, k);

	double* D = calloc(m*n, sizeof(double));

	// Transpose D to mxn
	//cblas_dimatcopy(CblasRowMajor, CblasTrans, n, m, 1.0, D, m, n);

	double* identityMat = calloc(n*n, sizeof(double));
	for(int f=0; f<n; f++)
		identityMat[f*n + f] = 1;

	cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, m, n, n, 1, tempD, m, identityMat, n, 0, D, n);

	// Free memory
	free(identityMat);
	free(tempD);

	// Create results struct
	knnresult results;
	results.k = k;
	results.m = m;
	results.ndist = calloc(m*k, sizeof(double));
	results.nidx = calloc(m*k, sizeof(int));

	// Create ids array for the X array
	int* ids = calloc(n, sizeof(int));

	// K-Select using quickselect to find k smallest distances for each point of Y
	for(int j = 0; j < m; j++){

		// Re-set ids vector before executing quickselect each time
		for(int i = 0; i < n; i++)
				ids[i] = i;

		// Call quickselect on the row of D to sort it up to its k-th element
		kselect(D + j * n, ids, n, k);

		// Quicksort the results
		quickSort(D + j * n, ids, 0, n-1);

		// Write results (point id and distance)
		for(int l = 0; l < k; l++){

			results.ndist[j * k + l] = D[j * n + l];
			results.nidx[j * k + l] = ids[l];
		}
	}

	// Free memory and return results
	free(ids);
	return results;

}

// Function used to calculate D = sqrt(sum(X.^2,2) -2* X*Y.' + sum(Y.^2,2).');
double* calculateD(double * X, double * Y, int n, int m, int d, int k){

		// Temporary variable
		double temp = 0;

		// Distance matrix for results
		double* D = calloc(n*m, sizeof(double));

		// Euclidean norm vectors
		double* normX = calloc(n, sizeof(double));
		double* normY = calloc(m, sizeof(double));

		// Matrice to store -2*X*Y'
		double* XY = calloc(n*m, sizeof(double));

		// Calculate -2*XY (https://software.intel.com/en-us/mkl-tutorial-c-multiplying-matrices-using-dgemm)
		cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, n, m, d, -2, X, d, Y, d, 0, XY, m);

		// Calculate sum(X.^2,2), sum(Y.^2,2)
		for(int i = 0; i < n; i++){
			temp = cblas_dnrm2(d, X+i*d, 1);
			normX[i] = temp * temp;
		}
		for(int i = 0; i < m; i++){
			temp = cblas_dnrm2(d, Y+i*d, 1);
			normY[i] = temp * temp;
		}

		// XY = sum(X.^2,2) -2* X*Y.'
	        for (int i=0; i<n; i++)
	    		for(int j=0; j<m; j++)
				XY[i*m+j] += normX[i] + normY[j];

		// D = sqrt(sum(X.^2,2) -2* X*Y.' + sum(Y.^2,2).');
		for(int i = 0; i < n*m; i++)
			D[i] = sqrt(fabs(XY[i]));

		// Free memory
		free(normX);
		free(normY);
		free(XY);

		return D;
}

// A utility function to swap two elements
void swap_double(double *a, double *b){
    double t = *a;
    *a = *b;
    *b = t;
}

// A utility function to swap two elements
void swap_int(int *a, int *b){
    int t = *a;
    *a = *b;
    *b = t;
}

// QuickSort Partition function. low and high are the range of indexes in arr where partition should work
int partition (double arr[], int *ids, int low, int high){

    // Select a pivot and initialize flag to position of smallest element before pivot
    double pivot = arr[high];
    int i = (low - 1);

    // Go through the array examining each element
    for (int j = low; j <= high - 1; j++)
    {
        // If current element is smaller than the pivot, increment i and swap it out with the one currently pointed by i
        if (arr[j] < pivot)
        {
            i++;
            // Swap distances and corresponding point ids
            swap_double(&arr[i], &arr[j]);
            swap_int(&ids[i], &ids[j]);
        }
    }

    // Finally place pivot in its correct position in the array and return the position as the middle point
    swap_double(&arr[i + 1], &arr[high]);
    swap_int(&ids[i + 1], &ids[high]);
    return (i + 1);
}

// Returns the median using the QuickSelect algorithm
double kselect(double arr[], int *ids, int length, int k){
	return quickSelect(arr, ids, length, k);
}

// Returns the idx-th element of arr when arr is sorted. idx is the index (starting from 1) of the point we want to find when the array is sorted.
double quickSelect(double arr[], int *ids, int length, int idx){

    // Check to end recursion
    if (length == 1){
        return arr[0];
    }

    // Select last array element as pivot
    double pivot = arr[length - 1];
    // Get index of pivot after we partition the array
    int pivotIndex = partition(arr, ids, 0, length - 1);

    // Create the higher and lower arrays that occur after partitioning in QuickSort fashion
    int lowerLength = pivotIndex;
    pivotIndex++;
    int higherLength = (length - (lowerLength + 1));
    // At this point pivotIndex, lowerLength and higherLength all start from 1 not 0

    // Variable to store result of following recursive calls
    double result = 0;

    // This means that the point we're looking (median in our case) is in the lower partition
    if (idx <= lowerLength){
        result = quickSelect(arr, ids, lowerLength, idx);
    }
    // This means that idx-th element is our pivot point
    else if(idx == pivotIndex){
        result = pivot;
    }
    // This means that the median is in the higher partition
    else{
        result = quickSelect(arr + pivotIndex, ids + pivotIndex, higherLength, idx - pivotIndex);
    }

    // Return result
    return result;
}

// Simple quicksort implementation that also sorts the ids array according to the way the main array was sorted
void quickSort(double arr[], int *ids, int low, int high)
{
    if (low < high)
    {
        // idx is the partition point
        int idx = partition(arr, ids, low, high);

        // Divide and conquer
        quickSort(arr, ids, low, idx - 1);
        quickSort(arr, ids, idx + 1, high);
    }
}


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

#include <gsl/gsl_math.h>
#include <gsl/gsl_movstat.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_statistics.h>

double
func(const size_t n, double x[], void * params)
{
  const double alpha = *(double *) params;

  gsl_sort(x, 1, n);

  return gsl_stats_trmean_from_sorted_data(alpha, x, 1, n);
}

int
main(void)
{
  const size_t N = 1000;                /* length of time series */
  const size_t K = 11;                  /* window size */
  double alpha = 0.1;                   /* trimmed mean parameter */
  gsl_vector *x = gsl_vector_alloc(N);  /* input vector */
  gsl_vector *y = gsl_vector_alloc(N);  /* filtered output vector for alpha1 */
  gsl_rng *r = gsl_rng_alloc(gsl_rng_default);
  gsl_movstat_workspace *w = gsl_movstat_alloc(K);
  gsl_movstat_function F;
  size_t i;
  double sum = 0.0;

  /* generate input signal */
  for (i = 0; i < N; ++i)
    {
      double ui = gsl_ran_gaussian(r, 1.0);
      double outlier = (gsl_rng_uniform(r) < 0.01) ? 10.0*GSL_SIGN(ui) : 0.0;
      sum += ui;
      gsl_vector_set(x, i, sum + outlier);
    }

  /* apply moving window function */
  F.function = func;
  F.params = α
  gsl_movstat_apply(GSL_MOVSTAT_END_PADVALUE, &F, x, y, w);

  /* print results */
  for (i = 0; i < N; ++i)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(y, i);

      printf("%f %f\n", xi, yi);
    }

  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_rng_free(r);
  gsl_movstat_free(w);

  return 0;
}


/* multiroots/test_funcs.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 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_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_multiroots.h>

#include "test_funcs.h"

/* For information on testing see the following paper,

   J.J More, B.S. Garbow, K.E. Hillstrom, "Testing Unconstrained
   Optimization Software", ACM Transactions on Mathematical Software,
   Vol 7, No 1, (1981) p 17-41

   */

/* Rosenbrock Function */

gsl_multiroot_function_fdf rosenbrock =
{&rosenbrock_f,
 &rosenbrock_df,
 &rosenbrock_fdf,
 2, 0};

void
rosenbrock_initpt (gsl_vector * x)
{
  gsl_vector_set (x, 0, -1.2);
  gsl_vector_set (x, 1, 1.0);
}

int
rosenbrock_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

  double y0 = 1 - x0;
  double y1 = 10 * (x1 - x0 * x0);

  gsl_vector_set (f, 0, y0);
  gsl_vector_set (f, 1, y1);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
rosenbrock_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double x0 = gsl_vector_get (x, 0);

  double df00 = -1;
  double df01 = 0;
  double df10 = -20 * x0;
  double df11 = 10;

  gsl_matrix_set (df, 0, 0, df00);
  gsl_matrix_set (df, 0, 1, df01);
  gsl_matrix_set (df, 1, 0, df10);
  gsl_matrix_set (df, 1, 1, df11);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
rosenbrock_fdf (const gsl_vector * x, void *params,
		gsl_vector * f, gsl_matrix * df)
{
  rosenbrock_f (x, params, f);
  rosenbrock_df (x, params, df);

  return GSL_SUCCESS;
}

/* Freudenstein and Roth function */

gsl_multiroot_function_fdf roth =
{&roth_f,
 &roth_df,
 &roth_fdf,
 2, 0};

void
roth_initpt (gsl_vector * x)
{
  gsl_vector_set (x, 0, 4.5);  /* changed from the value in the paper */
  gsl_vector_set (x, 1, 3.5);  /* otherwise the problem is too hard */
}

int
roth_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

  double y0 = -13.0 + x0 + ((5.0 - x1)*x1 - 2.0)*x1;
  double y1 = -29.0 + x0 + ((x1 + 1.0)*x1 - 14.0)*x1;

  gsl_vector_set (f, 0, y0);
  gsl_vector_set (f, 1, y1);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
roth_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double x1 = gsl_vector_get (x, 1);

  double df00 = 1;
  double df01 = -3 * x1 * x1 + 10 * x1 - 2;
  double df10 = 1;
  double df11 = 3 * x1 * x1 + 2 * x1 - 14;

  gsl_matrix_set (df, 0, 0, df00);
  gsl_matrix_set (df, 0, 1, df01);
  gsl_matrix_set (df, 1, 0, df10);
  gsl_matrix_set (df, 1, 1, df11);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
roth_fdf (const gsl_vector * x, void *params,
		gsl_vector * f, gsl_matrix * df)
{
  roth_f (x, params, f);
  roth_df (x, params, df);

  return GSL_SUCCESS;
}

/* Powell badly scaled function */

gsl_multiroot_function_fdf powellscal =
{&powellscal_f,
 &powellscal_df,
 &powellscal_fdf,
 2, 0};

void
powellscal_initpt (gsl_vector * x)
{
  gsl_vector_set (x, 0, 0.0);
  gsl_vector_set (x, 1, 1.0);
}

int
powellscal_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

  double y0 = 10000.0 * x0 * x1 - 1.0;
  double y1 = exp (-x0) + exp (-x1) - 1.0001;

  gsl_vector_set (f, 0, y0);
  gsl_vector_set (f, 1, y1);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
powellscal_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

  double df00 = 10000.0 * x1, df01 = 10000.0 * x0;
  double df10 = -exp (-x0), df11 = -exp (-x1);

  gsl_matrix_set (df, 0, 0, df00);
  gsl_matrix_set (df, 0, 1, df01);
  gsl_matrix_set (df, 1, 0, df10);
  gsl_matrix_set (df, 1, 1, df11);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
powellscal_fdf (const gsl_vector * x, void *params,
		  gsl_vector * f, gsl_matrix * df)
{
  powellscal_f (x, params, f);
  powellscal_df (x, params, df);

  return GSL_SUCCESS;
}

/* Brown badly scaled function */

gsl_multiroot_function_fdf brownscal =
{&brownscal_f,
 &brownscal_df,
 &brownscal_fdf,
 2, 0};

void
brownscal_initpt (gsl_vector * x)
{
  gsl_vector_set (x, 0, 1.0);
  gsl_vector_set (x, 1, 1.0);
}

int
brownscal_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

  double y0 = x0 - 1e6;
  double y1 = x0 * x1 - 2;

  gsl_vector_set (f, 0, y0);
  gsl_vector_set (f, 1, y1);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
brownscal_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

  double df00 = 1.0, df01 = 0.0;
  double df10 = x1, df11 = x0;

  gsl_matrix_set (df, 0, 0, df00);
  gsl_matrix_set (df, 0, 1, df01);
  gsl_matrix_set (df, 1, 0, df10);
  gsl_matrix_set (df, 1, 1, df11);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
brownscal_fdf (const gsl_vector * x, void *params,
		  gsl_vector * f, gsl_matrix * df)
{
  brownscal_f (x, params, f);
  brownscal_df (x, params, df);

  return GSL_SUCCESS;
}

/* Powell Singular Function */

gsl_multiroot_function_fdf powellsing =
{&powellsing_f,
 &powellsing_df,
 &powellsing_fdf,
 4, 0};

void
powellsing_initpt (gsl_vector * x)
{
  gsl_vector_set (x, 0, 3.0);
  gsl_vector_set (x, 1, -1.0);
  gsl_vector_set (x, 2, 0.0);
  gsl_vector_set (x, 3, 1.0);
}

int
powellsing_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);
  double x2 = gsl_vector_get (x, 2);
  double x3 = gsl_vector_get (x, 3);

  double y0 = x0 + 10 * x1;
  double y1 = sqrt (5.0) * (x2 - x3);
  double y2 = pow (x1 - 2 * x2, 2.0);
  double y3 = sqrt (10.0) * pow (x0 - x3, 2.0);

  gsl_vector_set (f, 0, y0);
  gsl_vector_set (f, 1, y1);
  gsl_vector_set (f, 2, y2);
  gsl_vector_set (f, 3, y3);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
powellsing_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);
  double x2 = gsl_vector_get (x, 2);
  double x3 = gsl_vector_get (x, 3);

  double df00 = 1, df01 = 10, df02 = 0, df03 = 0;
  double df10 = 0, df11 = 0, df12 = sqrt (5.0), df13 = -df12;
  double df20 = 0, df21 = 2 * (x1 - 2 * x2), df22 = -2 * df21, df23 = 0;
  double df30 = 2 * sqrt (10.0) * (x0 - x3), df31 = 0, df32 = 0, df33 = -df30;

  gsl_matrix_set (df, 0, 0, df00);
  gsl_matrix_set (df, 0, 1, df01);
  gsl_matrix_set (df, 0, 2, df02);
  gsl_matrix_set (df, 0, 3, df03);

  gsl_matrix_set (df, 1, 0, df10);
  gsl_matrix_set (df, 1, 1, df11);
  gsl_matrix_set (df, 1, 2, df12);
  gsl_matrix_set (df, 1, 3, df13);

  gsl_matrix_set (df, 2, 0, df20);
  gsl_matrix_set (df, 2, 1, df21);
  gsl_matrix_set (df, 2, 2, df22);
  gsl_matrix_set (df, 2, 3, df23);

  gsl_matrix_set (df, 3, 0, df30);
  gsl_matrix_set (df, 3, 1, df31);
  gsl_matrix_set (df, 3, 2, df32);
  gsl_matrix_set (df, 3, 3, df33);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
powellsing_fdf (const gsl_vector * x, void *params,
		    gsl_vector * f, gsl_matrix * df)
{
  powellsing_f (x, params, f);
  powellsing_df (x, params, df);

  return GSL_SUCCESS;
}

/* Wood function */

gsl_multiroot_function_fdf wood =
{&wood_f,
 &wood_df,
 &wood_fdf,
 4, 0};

void
wood_initpt (gsl_vector * x)
{
  gsl_vector_set (x, 0, -3.0);
  gsl_vector_set (x, 1, -1.0);
  gsl_vector_set (x, 2, -3.0);
  gsl_vector_set (x, 3, -1.0);
}

int
wood_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);
  double x2 = gsl_vector_get (x, 2);
  double x3 = gsl_vector_get (x, 3);

  double t1 = x1 - x0 * x0;
  double t2 = x3 - x2 * x2;

  double y0 = -200.0 * x0 * t1 - (1 - x0);
  double y1 = 200.0 * t1 + 20.2 * (x1 - 1) + 19.8 * (x3 - 1);
  double y2 = -180.0 * x2 * t2 - (1 - x2);
  double y3 = 180.0 * t2 + 20.2 * (x3 - 1) + 19.8 * (x1 - 1);

  gsl_vector_set (f, 0, y0);
  gsl_vector_set (f, 1, y1);
  gsl_vector_set (f, 2, y2);
  gsl_vector_set (f, 3, y3);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
wood_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);
  double x2 = gsl_vector_get (x, 2);
  double x3 = gsl_vector_get (x, 3);

  double t1 = x1 - 3 * x0 * x0;
  double t2 = x3 - 3 * x2 * x2;

  double df00 = -200.0 * t1 + 1, df01 = -200.0 * x0, df02 = 0, df03 = 0;
  double df10 = -400.0*x0, df11 = 200.0 + 20.2, df12 = 0, df13 = 19.8;
  double df20 = 0, df21 = 0, df22 = -180.0 * t2 + 1, df23 = -180.0 * x2;
  double df30 = 0, df31 = 19.8, df32 = -2 * 180 * x2, df33 = 180.0 + 20.2;

  gsl_matrix_set (df, 0, 0, df00);
  gsl_matrix_set (df, 0, 1, df01);
  gsl_matrix_set (df, 0, 2, df02);
  gsl_matrix_set (df, 0, 3, df03);

  gsl_matrix_set (df, 1, 0, df10);
  gsl_matrix_set (df, 1, 1, df11);
  gsl_matrix_set (df, 1, 2, df12);
  gsl_matrix_set (df, 1, 3, df13);

  gsl_matrix_set (df, 2, 0, df20);
  gsl_matrix_set (df, 2, 1, df21);
  gsl_matrix_set (df, 2, 2, df22);
  gsl_matrix_set (df, 2, 3, df23);

  gsl_matrix_set (df, 3, 0, df30);
  gsl_matrix_set (df, 3, 1, df31);
  gsl_matrix_set (df, 3, 2, df32);
  gsl_matrix_set (df, 3, 3, df33);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
wood_fdf (const gsl_vector * x, void *params,
		    gsl_vector * f, gsl_matrix * df)
{
  wood_f (x, params, f);
  wood_df (x, params, df);

  return GSL_SUCCESS;
}

/* Helical Valley Function */

gsl_multiroot_function_fdf helical =
{&helical_f,
 &helical_df,
 &helical_fdf,
 3, 0};

void
helical_initpt (gsl_vector * x)
{
  gsl_vector_set (x, 0, -1.0);
  gsl_vector_set (x, 1, 0.0);
  gsl_vector_set (x, 2, 0.0);
}

int
helical_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);
  double x2 = gsl_vector_get (x, 2);

  double t1, t2;
  double y0, y1, y2;

  if (x0 > 0)
    {
      t1 = atan(x1/x0) / (2.0 * M_PI);
    }
  else if (x0 < 0)
    {
      t1 = 0.5 + atan(x1/x0) / (2.0 * M_PI);
    }
  else
    {
      t1 = 0.25 * (x1 > 0 ? +1 : -1);
    }

  t2 = sqrt(x0*x0 + x1*x1) ;

  y0 = 10 * (x2 - 10 * t1);
  y1 = 10 * (t2 - 1);
  y2 = x2 ;

  gsl_vector_set (f, 0, y0);
  gsl_vector_set (f, 1, y1);
  gsl_vector_set (f, 2, y2);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
helical_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

  double t = x0 * x0 + x1 * x1 ;
  double t1 = 2 * M_PI * t ;
  double t2 = sqrt(t) ;

  double df00 = 100*x1/t1, df01 = -100.0 * x0/t1, df02 = 10.0;
  double df10 = 10*x0/t2, df11 = 10*x1/t2, df12 = 0;
  double df20 = 0, df21 = 0, df22 = 1.0;

  gsl_matrix_set (df, 0, 0, df00);
  gsl_matrix_set (df, 0, 1, df01);
  gsl_matrix_set (df, 0, 2, df02);

  gsl_matrix_set (df, 1, 0, df10);
  gsl_matrix_set (df, 1, 1, df11);
  gsl_matrix_set (df, 1, 2, df12);

  gsl_matrix_set (df, 2, 0, df20);
  gsl_matrix_set (df, 2, 1, df21);
  gsl_matrix_set (df, 2, 2, df22);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
helical_fdf (const gsl_vector * x, void *params,
		    gsl_vector * f, gsl_matrix * df)
{
  helical_f (x, params, f);
  helical_df (x, params, df);

  return GSL_SUCCESS;
}

/* Discrete Boundary Value Function */

#define N 10

gsl_multiroot_function_fdf dbv =
{&dbv_f,
 &dbv_df,
 &dbv_fdf,
 N, 0};

void
dbv_initpt (gsl_vector * x)
{
  size_t i;
  double h = 1.0 / (N + 1.0);

  for (i = 0; i < N; i++)
    {
      double t = (i + 1) * h;
      double z = t * (t - 1);
      gsl_vector_set (x, i, z);
    }
}

int
dbv_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  size_t i;

  double h = 1.0 / (N + 1.0);

  for (i = 0; i < N; i++)
    {
      double z, ti = (i + 1) * h;
      double xi = 0, xim1 = 0, xip1 = 0;

      xi = gsl_vector_get (x, i);

      if (i > 0)
        xim1 = gsl_vector_get (x, i - 1);

      if (i < N - 1)
        xip1 = gsl_vector_get (x, i + 1);

      z = 2 * xi - xim1 - xip1 + h * h * pow(xi + ti + 1, 3.0) / 2.0;

      gsl_vector_set (f, i, z);

    }

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
dbv_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  size_t i, j;

  double h = 1.0 / (N + 1.0);

  for (i = 0; i < N; i++)
    for (j = 0; j < N; j++)
      gsl_matrix_set (df, i, j, 0.0);

  for (i = 0; i < N; i++)
    {
      double dz_dxi, ti = (i + 1) * h;

      double xi = gsl_vector_get (x, i);

      dz_dxi = 2.0 + (3.0 / 2.0) * h * h * pow(xi + ti + 1, 2.0) ;

      gsl_matrix_set (df, i, i, dz_dxi);

      if (i > 0)
        gsl_matrix_set (df, i, i-1, -1.0);

      if (i < N - 1)
        gsl_matrix_set (df, i, i+1, -1.0);

    }

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
dbv_fdf (const gsl_vector * x, void *params,
		    gsl_vector * f, gsl_matrix * df)
{
  dbv_f (x, params, f);
  dbv_df (x, params, df);

  return GSL_SUCCESS;
}

/* Trigonometric Function */

gsl_multiroot_function_fdf trig =
{&trig_f,
 &trig_df,
 &trig_fdf,
 N, 0};

void
trig_initpt (gsl_vector * x)
{
  size_t i;

  for (i = 0; i < N; i++)       /* choose an initial point which converges */
    {
      gsl_vector_set (x, i, 0.05);
    }
}

int
trig_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  size_t i;
  double sum = 0;

  for (i = 0; i < N; i++)
    {
      sum += cos(gsl_vector_get(x,i));
    }

  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get (x,i);
      double z = N - sum + (i + 1) * (1 - cos(xi)) - sin(xi);

      gsl_vector_set (f, i, z);
    }

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
trig_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  size_t i, j;

  for (i = 0; i < N; i++)
    {
      for (j = 0; j < N; j++)
        {
          double dz;
          double xi = gsl_vector_get(x, i);
          double xj = gsl_vector_get(x, j);

          if (j == i)
            dz = sin(xi) + (i + 1) * sin(xi) - cos(xi);
          else
            dz = sin(xj);

          gsl_matrix_set(df, i, j, dz);
        }
    }

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
trig_fdf (const gsl_vector * x, void *params,
		    gsl_vector * f, gsl_matrix * df)
{
  trig_f (x, params, f);
  trig_df (x, params, df);

  return GSL_SUCCESS;
}


/*
 Copyright (C) 2002 M. Marques, A. Castro, A. Rubio, G. Bertsch, M. Oliveira

 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, 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.

 $Id$
*/

#include <config.h>

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

#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_sf_expint.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_combination.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_chebyshev.h>
#include <gsl/gsl_deriv.h>

#include "string_f.h"

#include <fortran_types.h>

/* Numerical threshold for oct_bessel_k0 and oct_bessel_k1 */
#define  BESSEL_K_THRES  1.0e2

/* ---------------------- Interface to GSL functions ------------------------ */

/* Mathematical Functions */
double FC_FUNC_(oct_asinh, OCT_ASINH)
     (const double *x)
{
  return gsl_asinh(*x);
}

/* Special Functions */
double FC_FUNC_(oct_gamma, OCT_GAMMA)
     (const double *x)
{
  return gsl_sf_gamma(*x);
}

double FC_FUNC_(oct_hypergeometric, OCT_HYPERGEOMETRIC)
     (const double *a, const double*b, const double *x)
{
  return gsl_sf_hyperg_U(*a, *b, *x);
}

double FC_FUNC_(oct_incomplete_gamma, OCT_INCOMPLETE_GAMMA)
     (const double *a, const double *x)
{
  return gsl_sf_gamma_inc_Q(*a, *x);
}

double FC_FUNC_(oct_sph_bessel, OCT_SPH_BESSEL)
     (const fint *l, const double*x)
{
  return gsl_sf_bessel_jl(*l, *x);
}

double FC_FUNC_(oct_bessel, OCT_BESSEL)
     (const fint *n, const double *x)
{
  return gsl_sf_bessel_Jn(*n, *x);
}

double FC_FUNC_(oct_bessel_in, OCT_BESSEL_IN)
     (const fint *n, const double *x)
{
  return gsl_sf_bessel_In(*n, *x);
}

double FC_FUNC_(oct_bessel_j0, OCT_BESSEL_J0)
     (const double *x)
{
  return gsl_sf_bessel_J0(*x);
}

double FC_FUNC_(oct_bessel_j1, OCT_BESSEL_J1)
     (const double *x)
{
  return gsl_sf_bessel_J1(*x);
}

double FC_FUNC_(oct_bessel_k0, OCT_BESSEL_K0)
     (const double *x)
{
  if( *x > BESSEL_K_THRES )
  {
    return 0.0e0;
  } else {
    return gsl_sf_bessel_K0(*x);
  }
}

double FC_FUNC_(oct_bessel_k1, OCT_BESSEL_K1)
     (const double *x)
{
  if( *x > BESSEL_K_THRES )
  {
    return 0.0e0;
  } else {
    return gsl_sf_bessel_K1(*x);
  }
}

/* the GSL headers specify double x, not const double x */
double FC_FUNC_(oct_erfc, OCT_ERFC)
     (const double *x)
{
  /* avoid floating invalids in the asymptotic limit */
  if(*x >  20.0) return  0.0;
  if(*x < -20.0) return  2.0;
  /* otherwise call gsl */
  return gsl_sf_erfc(*x);
}

/* the GSL headers specify double x, not const double x */
double FC_FUNC_(oct_erf, OCT_ERF)
     (const double *x)
{
  /* avoid floating invalids in the asymptotic limit */
  if(*x >  20.0) return  1.0;
  if(*x < -20.0) return -1.0;
  /* otherwise call gsl */
  return gsl_sf_erf(*x);
}

double FC_FUNC_(oct_legendre_sphplm, OCT_LEGENDRE_SPHPLM)
     (const fint *l, const int *m, const double *x)
{
  return gsl_sf_legendre_sphPlm(*l, *m, *x);
}

double FC_FUNC_(oct_sine_integral, OCT_SINE_INTEGRAL)
     (const double *x)
{
  return gsl_sf_Si(*x);
}

/* generalized Laguerre polynomials */
double FC_FUNC_(oct_sf_laguerre_n, OCT_SF_LAGUERRE_N)
     (const int *n, const double *a, const double *x)
{
  return gsl_sf_laguerre_n(*n, *a, *x);
}

/* Vectors and Matrices */

/* Permutations */

/* Combinations */

void FC_FUNC_(oct_combination_init, OCT_COMBINATION_INIT)
     (gsl_combination **c, const fint *n, const fint *k)
{
  *c = gsl_combination_calloc (*n, *k);
}

void FC_FUNC_(oct_combination_next, OCT_COMBINATION_NEXT)
     (gsl_combination **c, fint *next)
{
  *next = gsl_combination_next (((gsl_combination *)(*c)));
}

void FC_FUNC_(oct_get_combination, OCT_GET_COMBINATION)
     (gsl_combination **c, fint *comb)
{
  int i;
  for (i=0;i< ((gsl_combination *)(*c))->k; i++) {
    comb[i]=(fint)((gsl_combination *)(*c))->data[i];
  }
}

void FC_FUNC_(oct_combination_end, OCT_COMBINATION_END)
     (gsl_combination **c)
{
  gsl_combination_free (((gsl_combination *)(*c)));
}

/* Linear Algebra */

/* Random Number Generation */
void FC_FUNC_(oct_ran_init, OCT_RAN_INIT)
     (gsl_rng **r)
{
  gsl_rng_env_setup();
  *r = gsl_rng_alloc(gsl_rng_default);
}

void FC_FUNC_(oct_ran_end, OCT_RAN_END)
     (gsl_rng **r)
{
  gsl_rng_free(*r);
}

/* Random Number Distributions */
double FC_FUNC_(oct_ran_gaussian, OCT_RAN_GAUSSIAN)
     (const gsl_rng **r, const double *sigma)
{
  return gsl_ran_gaussian(*r, *sigma);
}

double FC_FUNC_(oct_ran_flat, OCT_RAN_FLAT)
     (const gsl_rng **r, const double *a, const double *b)
{
  return gsl_ran_flat(*r, *a, *b);
}

/* Interpolation */
void FC_FUNC_(oct_spline_end, OCT_SPLINE_END)
     (void **spl, void **acc)
{
  gsl_spline_free((gsl_spline *)(*spl));
  gsl_interp_accel_free((gsl_interp_accel *)(*acc));
}

void FC_FUNC_(oct_spline_fit, OCT_SPLINE_FIT)
		 (const fint *nrc, const double *x, const double *y, void **spl, void **acc)
{
  /* the GSL headers actually specify size_t instead of const int for nrc */
  *acc = (void *)gsl_interp_accel_alloc();
  *spl = (void *)gsl_spline_alloc(gsl_interp_cspline, *nrc);
  gsl_spline_init((gsl_spline *)(*spl), x, y, *nrc);
  fflush(stdout);
}

double FC_FUNC_(oct_spline_eval, OCT_SPLINE_EVAL)
     (const double *x, const void **spl, void **acc)
{
  /* the GSL headers specify double x instead of const double x */
  return gsl_spline_eval((gsl_spline *)(*spl), *x, (gsl_interp_accel *)(*acc));
}

void FC_FUNC_(oct_spline_eval_array, OCT_SPLINE_EVAL_ARRAY)
     (const fint * nn, double *xf, const void **spl, void **acc)
{
  int ii;
  for(ii = 0; ii < *nn; ii++){
    xf[ii] = gsl_spline_eval((gsl_spline *)(*spl), xf[ii], (gsl_interp_accel *)(*acc));
  }
}

void FC_FUNC_(oct_spline_eval_array4, OCT_SPLINE_EVAL_ARRAY4)
     (const fint * nn, float *xf, const void **spl, void **acc)
{
  int ii;
  for(ii = 0; ii < *nn; ii++){
    xf[ii] = (float) gsl_spline_eval((gsl_spline *)(*spl), (double) xf[ii], (gsl_interp_accel *)(*acc));
  }
}

/* use a stride of 2 to store into just the real part of a Fortran complex array */
void FC_FUNC_(oct_spline_eval_arrayz, OCT_SPLINE_EVAL_ARRAYZ)
     (const fint * nn, double *xf, const void **spl, void **acc)
{
  int ii;
  for(ii = 0; ii < *nn; ii++){
    xf[ii*2] = gsl_spline_eval((gsl_spline *)(*spl), xf[ii*2], (gsl_interp_accel *)(*acc));
  }
}

/* use a stride of 2 to store into just the real part of a Fortran complex array */
void FC_FUNC_(oct_spline_eval_arrayc, OCT_SPLINE_EVAL_ARRAYC)
     (const fint * nn, float *xf, const void **spl, void **acc)
{
  int ii;
  for(ii = 0; ii < *nn; ii++){
    xf[ii*2] = (float) gsl_spline_eval((gsl_spline *)(*spl), (double) xf[ii*2], (gsl_interp_accel *)(*acc));
  }
}

/* This function returns the number of points with which a spline
	 was constructed (the size component of the gsl_spline struct). */
fint FC_FUNC_(oct_spline_npoints, OCT_SPLINE_NPOINTS)
     (const void **spl)
{
  return (fint)((gsl_spline *)(*spl))->size;
}

/* This function places in the x array the x values of a given spline spl*/
void FC_FUNC_(oct_spline_x, OCT_SPLINE_X)
     (const void **spl, double *x)
{
  int size, i;

  size = (int)((gsl_spline *)(*spl))->size;
  for(i=0; i<size; i++)
    x[i] = ((gsl_spline *)(*spl))->x[i];
}

/* This function places in the y array the y values of a given spline spl*/
void FC_FUNC_(oct_spline_y, OCT_SPLINE_Y)
     (const void **spl, double *y)
{
  int size, i;

  size = (int)((gsl_spline *)(*spl))->size;
  for(i=0; i<size; i++)
    y[i] = ((gsl_spline *)(*spl))->y[i];
}

/* Returns the integral of the spline stored in spl, between a and b */
double FC_FUNC_(oct_spline_eval_integ, OCT_SPLINE_EVAL_INTEG)
     (const void **spl, const double *a, const double *b, void **acc)
{
  /* the GSL headers specify double a, double b */
  return gsl_spline_eval_integ((gsl_spline *)(*spl), *a, *b, (gsl_interp_accel *)(* acc));
}

/* Performs the derivative of a spline */
double FC_FUNC_(oct_spline_eval_der, OCT_SPLINE_EVAL_DER)
     (const double *x, const void **spl, void **acc)
{
  /* the GSL headers specify double x */
  return gsl_spline_eval_deriv((gsl_spline *)(*spl), *x, (gsl_interp_accel *)(*acc));
}

/* Performs the second derivative of a spline */
double FC_FUNC_(oct_spline_eval_der2, OCT_SPLINE_EVAL_DER2)
     (const double *x, const void **spl, void **acc)
{
  /* the GSL headers specify double x */
  return gsl_spline_eval_deriv2((gsl_spline *)(*spl), *x, (gsl_interp_accel *)(*acc));
}

void FC_FUNC_(oct_strerror, OCT_STRERROR)
     (const fint *err, STR_F_TYPE res STR_ARG1)
{
  const char *c;

  c = gsl_strerror(*err);
  TO_F_STR1(c, res);
}

/* Chebyshev Approximations */
/*
void FC_FUNC_(oct_chebyshev_coeffs, OCT_CHEBYSHEV_COEFFS)
     (gsl_complex *coeffs, int *order)
{
  int i;
  double f (double x, void *p){return cos(x);}
  double g (double x, void *p){return -sin(x);}
  gsl_cheb_series *cs = gsl_cheb_alloc (*order);
  gsl_function F;
  F.function = f;
  F.params = 0;
  gsl_cheb_init (cs, &F, -1.0, 1.0);
  for(i=0; i<=12; i++){GSL_SET_REAL(&coeffs[i], (*cs).c[i]);}
  F.function = g;
  F.params = 0;
  gsl_cheb_init (cs, &F, -1.0, 1.0);
  for(i=0; i<=12; i++){GSL_SET_IMAG(&coeffs[i], (*cs).c[i]);}
}
*/

 /* Numerical Derivatives.
    The following is an interface to the GSL support for numerical derivatives,
    in particular the gsl_deriv_central function. It computes a four points
    approximation to the derivative of a function, supplying an error estimation. */

 /* This is a type used to communicate with Fortran; func_nd is the type of the
    interface to a Fortran subroutine that calculates the value of the function.
    The first argument is the function argument, whereas the second is the function
    value. For convenience reasons, it is wrapped around the "param_nd_t" struct. */
typedef void (*func_nd)(double*, double*);
typedef struct{
  func_nd func;
} param_nd_t;

 /* This is the function that is called by the GSL function gsl_deriv_central. It
    receives as first argument the function argument, and as second argument
    a pointer to a params data type (a GSL data type), which in this case should
    be a pointer to a param_nd_t data type, where the address of the Fortran
    subroutine is. */
double function_oct_numerical_derivative (double x, void * params)
{
  double res;
  param_nd_t * p;

  p = (param_nd_t *) params;
  p->func(&x, &res);
  return res;
}
 /* This is the function that should be called by Fortran. The interface is defined
    in loct_math.F90 file. */
void FC_FUNC_(oct_numerical_derivative, OCT_NUMERICAL_DERIVATIVE)
     (const double *x, const double *h, double *result, double *abserr, const func_nd f)
{
  gsl_function F;
  param_nd_t p;

  p.func = f;
  F.function = &function_oct_numerical_derivative;
  F.params = (void *) &p;
  /* the GSL headers specify double x, double h */
  gsl_deriv_central (&F, *x, *h, result, abserr);
  return;
}


#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit_nlin.h>

struct data {
  size_t n;
  double * y;
  double * sigma;
};

int
expb_f (const gsl_vector * x, void *params,
        gsl_vector * f)
{
  size_t n = ((struct data *)params)->n;
  double *y = ((struct data *)params)->y;
  double *sigma = ((struct data *) params)->sigma;

  double A = gsl_vector_get (x, 0);
  double lambda = gsl_vector_get (x, 1);
  double b = gsl_vector_get (x, 2);

  size_t i;

  for (i = 0; i < n; i++)
    {
      /* Model Yi = A * exp(-lambda * i) + b */
      double t = i;
      double Yi = A * exp (-lambda * t) + b;
      gsl_vector_set (f, i, (Yi - y[i])/sigma[i]);
    }

  return GSL_SUCCESS;
}

int
expb_df (const gsl_vector * x, void *params,
         gsl_matrix * J)
{
  size_t n = ((struct data *)params)->n;
  double *sigma = ((struct data *) params)->sigma;

  double A = gsl_vector_get (x, 0);
  double lambda = gsl_vector_get (x, 1);

  size_t i;

  for (i = 0; i < n; i++)
    {
      /* Jacobian matrix J(i,j) = dfi / dxj, */
      /* where fi = (Yi - yi)/sigma[i],      */
      /*       Yi = A * exp(-lambda * i) + b  */
      /* and the xj are the parameters (A,lambda,b) */
      double t = i;
      double s = sigma[i];
      double e = exp(-lambda * t);
      gsl_matrix_set (J, i, 0, e/s);
      gsl_matrix_set (J, i, 1, -t * A * e/s);
      gsl_matrix_set (J, i, 2, 1/s);

    }
  return GSL_SUCCESS;
}

int
expb_fdf (const gsl_vector * x, void *params,
          gsl_vector * f, gsl_matrix * J)
{
  expb_f (x, params, f);
  expb_df (x, params, J);

  return GSL_SUCCESS;
}

#define N 40

int
main (void)
{
  const gsl_multifit_fdfsolver_type *T;
  gsl_multifit_fdfsolver *s;

  int status;
  size_t i, iter = 0;

  const size_t n = N;
  const size_t p = 3;

  gsl_matrix *covar = gsl_matrix_alloc (p, p);

  double y[N], sigma[N];

  struct data d = { n, y, sigma};

  gsl_multifit_function_fdf f;

  double x_init[3] = { 1.0, 0.0, 0.0 };

  gsl_vector_view x = gsl_vector_view_array (x_init, p);

  const gsl_rng_type * type;
  gsl_rng * r;

  gsl_rng_env_setup();

  type = gsl_rng_default;
  r = gsl_rng_alloc (type);

  f.f = &expb_f;
  f.df = &expb_df;
  f.fdf = &expb_fdf;
  f.n = n;
  f.p = p;
  f.params = &d;

  /* This is the data to be fitted */

  for (i = 0; i < n; i++)
    {
      double t = i;
      y[i] = 1.0 + 5 * exp (-0.1 * t)
                 + gsl_ran_gaussian (r, 0.1);
      sigma[i] = 0.1;
      printf ("data: %d %g %g\n", i, y[i], sigma[i]);
    };

  T = gsl_multifit_fdfsolver_lmsder;
  s = gsl_multifit_fdfsolver_alloc (T, n, p);
  gsl_multifit_fdfsolver_set (s, &f, &x.vector);

  print_state (iter, s);

  do
    {
      iter++;
      status = gsl_multifit_fdfsolver_iterate (s);

      printf ("status = %s\n", gsl_strerror (status));

      print_state (iter, s);

      if (status)
        break;

      status = gsl_multifit_test_delta (s->dx, s->x,
                                        1e-4, 1e-4);
    }
  while (status == GSL_CONTINUE && iter < 500);

  gsl_multifit_covar (s->J, 0.0, covar);

  gsl_matrix_fprintf (stdout, covar, "%g");

#define FIT(i) gsl_vector_get(s->x, i)
#define ERR(i) sqrt(gsl_matrix_get(covar,i,i))

  printf ("A      = %.5f +/- %.5f\n", FIT(0), ERR(0));
  printf ("lambda = %.5f +/- %.5f\n", FIT(1), ERR(1));
  printf ("b      = %.5f +/- %.5f\n", FIT(2), ERR(2));

  {
    double chi = gsl_blas_dnrm2(s->f);
    printf("chisq/dof = %g\n",  pow(chi, 2.0)/ (n - p));
  }

  printf ("status = %s\n", gsl_strerror (status));

  gsl_multifit_fdfsolver_free (s);
  return 0;
}

int
print_state (size_t iter, gsl_multifit_fdfsolver * s)
{
  printf ("iter: %3u x = % 15.8f % 15.8f % 15.8f "
          "|f(x)| = %g\n",
          iter,
          gsl_vector_get (s->x, 0),
          gsl_vector_get (s->x, 1),
          gsl_vector_get (s->x, 2),
          gsl_blas_dnrm2 (s->f));
}


/* randist/bernoulli.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>

/* The bernoulli distribution has the form,

   prob(0) = 1-p, prob(1) = p

   */

unsigned int
gsl_ran_bernoulli (const gsl_rng * r, double p)
{
  double u = gsl_rng_uniform (r) ;

  if (u < p)
    {
      return 1 ;
    }
  else
    {
      return 0 ;
    }
}

double
gsl_ran_bernoulli_pdf (const unsigned int k, double p)
{
  if (k == 0)
    {
      return 1 - p ;
    }
  else if (k == 1)
    {
      return p ;
    }
  else
    {
      return 0 ;
    }
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <stdarg.h>
#include <gbpLib.h>
#include <gbpMath.h>
#include <gbpCosmo_mass_functions.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>

// Scaled mass functions
double scaled_mass_function(double sigma, int mode, double *P) {
    double rval;
    if(SID_CHECK_BITFIELD_SWITCH(mode, MF_WATSON)) { // Watson et al. (2013) universal FoF mass function
        double A     = 0.282;
        double alpha = 2.163;
        double beta  = 1.406;
        double gamma = 1.210;
        if(SID_CHECK_BITFIELD_SWITCH(mode, MF_PASS_PARAMS)) {
            A     = P[0];
            alpha = P[1];
            beta  = P[2];
            gamma = P[3];
        }
        rval = A * (pow(beta / sigma, alpha) + 1.) * exp(-gamma / (sigma * sigma));
    } else if(SID_CHECK_BITFIELD_SWITCH(mode, MF_TIAMAT)) { // Poole et al. (2013) universal FoF mass function for Tiamat
        double A     = 0.03331;
        double alpha = 1.153;
        double beta  = 12.33;
        double gamma = 1.009;
        if(SID_CHECK_BITFIELD_SWITCH(mode, MF_PASS_PARAMS)) {
            A     = P[0];
            alpha = P[1];
            beta  = P[2];
            gamma = P[3];
        }
        rval = A * (pow(beta / sigma, alpha) + 1.) * exp(-gamma / (sigma * sigma));
    } else if(SID_CHECK_BITFIELD_SWITCH(mode, MF_JENKINS)) { // Jenkins et al. (2001)
        double A = 0.315;
        double B = 0.61;
        double C = 3.8;
        if(SID_CHECK_BITFIELD_SWITCH(mode, MF_PASS_PARAMS)) {
            A = P[0];
            B = P[1];
            C = P[2];
        }
        rval = A * exp(-pow((double)fabs((float)(take_ln(1. / sigma) + B)), C));
    } else if(SID_CHECK_BITFIELD_SWITCH(mode, MF_ST)) { // Sheth-Torman (1999)
        double delta_k = 1.686;
        double A       = 0.3222;
        double B       = 0.707;
        double C       = 0.3;
        if(SID_CHECK_BITFIELD_SWITCH(mode, MF_PASS_PARAMS)) {
            A = P[0];
            B = P[1];
            C = P[2];
        }
        rval = A * sqrt(2. * B / PI) * (delta_k / sigma) * exp(-B * delta_k * delta_k / (2. * sigma * sigma)) *
               (1. + pow(sigma * sigma / (B * delta_k * delta_k), C));
    } else if(SID_CHECK_BITFIELD_SWITCH(mode, MF_PS)) { // Press-Schechter (1974)
        double delta_k = 1.686;
        if(SID_CHECK_BITFIELD_SWITCH(mode, MF_PASS_PARAMS)) {
            delta_k = P[0];
        }
        rval = sqrt(2. / PI) * (delta_k / sigma) * exp(-delta_k * delta_k / (2. * sigma * sigma));
    } else
        SID_exit_error("A valid mass function was not specified with mode (%d) in scaled_mass_function().\n",
                       SID_ERROR_LOGIC, mode);
    return (rval);
}


static char help[] =
"Solves a 1D advection plus diffusion problem using FD discretization\n"
"and a structured-grid (DMDA).  Option prefix -b1_.  Equation is\n"
"  - eps u'' + (a(x) u)' = 0\n"
"with a(x)=1, on domain [-1,1], and with Dirichlet boundary conditions\n"
"u(-1) = 1, u(1) = 0.  Default eps=0.01.  The diffusion discretized by\n"
"centered, as usual, but advection is by first-order upwinding, centered,\n"
"or van Leer limiter scheme.  An analytic Jacobian is implemented, except\n"
"for the van Leer limiter.  The limiters in the residual and Jacobian\n"
"evaluations are separately controllable.\n\n";

#include <petsc.h>

typedef enum {NONE, CENTERED, VANLEER} LimiterType;
static const char *LimiterTypes[] = {"none","centered","vanleer",
                                     "LimiterType", "", NULL};

static PetscReal centered(PetscReal theta) {
    return 0.5;
}

static PetscReal vanleer(PetscReal theta) {
    const PetscReal abstheta = PetscAbsReal(theta);
    return 0.5 * (theta + abstheta) / (1.0 + abstheta);
}

static void* limiterptr[] = {NULL, &centered, &vanleer};

typedef struct {
    PetscReal   eps;          // amount of diffusion; require: eps > 0
    PetscReal   (*limiter_fcn)(PetscReal),
                (*jac_limiter_fcn)(PetscReal);
} AdCtx;

static PetscReal u_exact(PetscReal x, AdCtx *usr) {
    return (1.0 - exp((x-1) / usr->eps)) / (1.0 - exp(- 2.0 / usr->eps));
}

static PetscReal wind_a(PetscReal x) {
    return 1.0;
}

extern PetscErrorCode FormUExact(DMDALocalInfo*, AdCtx*, Vec);
extern PetscErrorCode FormFunctionLocal(DMDALocalInfo*, PetscReal*,PetscReal*, AdCtx*);
extern PetscErrorCode FormJacobianLocal(DMDALocalInfo*, PetscReal*, Mat, Mat, AdCtx*);

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    DM             da, da_after;
    SNES           snes;
    Vec            u_initial, u, u_exact;
    PetscReal      hx, err2, errinf;
    DMDALocalInfo  info;
    LimiterType    limiter = NONE, jac_limiter;
    PetscBool      snesfdset, snesfdcolorset;
    AdCtx          user;

    PetscInitialize(&argc,&argv,(char*)0,help);

    user.eps = 0.01;
    ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"b1_",
               "both1d (1D advection-diffusion solver) options",""); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-eps","positive diffusion coefficient",
               "both1d.c",user.eps,&(user.eps),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnum("-limiter","flux-limiter type",
               "both1d.c",LimiterTypes,
               (PetscEnum)limiter,(PetscEnum*)&limiter,NULL); CHKERRQ(ierr);
    jac_limiter = limiter;
    ierr = PetscOptionsEnum("-jac_limiter","flux-limiter type used in Jacobian evaluation",
               "both1d.c",LimiterTypes,
               (PetscEnum)jac_limiter,(PetscEnum*)&jac_limiter,NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);

    if (user.eps <= 0.0) {
        SETERRQ1(PETSC_COMM_SELF,2,"eps=%.3f invalid ... eps > 0 required",user.eps);
    }
    user.limiter_fcn = limiterptr[limiter];
    ierr = PetscOptionsHasName(NULL,NULL,"-snes_fd",&snesfdset); CHKERRQ(ierr);
    ierr = PetscOptionsHasName(NULL,NULL,"-snes_fd_color",&snesfdcolorset); CHKERRQ(ierr);
    if (snesfdset || snesfdcolorset) {
        user.jac_limiter_fcn = NULL;
        jac_limiter = 4;   // corresponds to empty string
    } else
        user.jac_limiter_fcn = limiterptr[jac_limiter];

    ierr = DMDACreate1d(PETSC_COMM_WORLD,DM_BOUNDARY_NONE,
                 3, // default to hx=1 grid
                 1,2, // d.o.f., stencil width
                 NULL,&da); CHKERRQ(ierr);
    ierr = DMSetFromOptions(da); CHKERRQ(ierr);
    ierr = DMSetUp(da); CHKERRQ(ierr);
    ierr = DMDASetUniformCoordinates(da,-1.0,1.0,-1.0,1.0,-1.0,1.0); CHKERRQ(ierr);
    ierr = DMSetApplicationContext(da,&user); CHKERRQ(ierr);

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

    ierr = DMGetGlobalVector(da,&u_initial); CHKERRQ(ierr);
    ierr = VecSet(u_initial,0.0); CHKERRQ(ierr);
    ierr = SNESSolve(snes,NULL,u_initial); CHKERRQ(ierr);
    ierr = DMRestoreGlobalVector(da,&u_initial); CHKERRQ(ierr);
    ierr = DMDestroy(&da); CHKERRQ(ierr);

    ierr = SNESGetSolution(snes,&u); CHKERRQ(ierr);
    ierr = SNESGetDM(snes,&da_after); CHKERRQ(ierr);
    ierr = DMDAGetLocalInfo(da_after,&info); CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD,
         "done on %d point grid (eps = %g, limiter = %s, jac_limiter = %s)\n",
         info.mx,user.eps,LimiterTypes[limiter],LimiterTypes[jac_limiter]); CHKERRQ(ierr);

    ierr = VecDuplicate(u,&u_exact); CHKERRQ(ierr);
    ierr = FormUExact(&info,&user,u_exact); CHKERRQ(ierr);
    ierr = VecAXPY(u,-1.0,u_exact); CHKERRQ(ierr);    // u <- u + (-1.0) u_exact
    ierr = VecNorm(u,NORM_INFINITY,&errinf); CHKERRQ(ierr);
    ierr = VecNorm(u,NORM_2,&err2); CHKERRQ(ierr);
    hx = 2.0 / (info.mx - 1);
    err2 *= PetscSqrtReal(hx);
    ierr = PetscPrintf(PETSC_COMM_WORLD,
         "numerical error:  |u-uexact|_inf = %.4e,  |u-uexact|_2 = %.4e\n",
         errinf,err2); CHKERRQ(ierr);

    VecDestroy(&u_exact);  SNESDestroy(&snes);
    return PetscFinalize();
}

PetscErrorCode FormUExact(DMDALocalInfo *info, AdCtx *usr, Vec uex) {
    PetscErrorCode  ierr;
    PetscInt   i;
    PetscReal  hx, x, *auex;

    hx = 2.0 / (info->mx - 1);
    ierr = DMDAVecGetArray(info->da, uex, &auex);CHKERRQ(ierr);
    for (i=info->xs; i<info->xs+info->xm; i++) {
        x = -1.0 + i * hx;
        auex[i] = u_exact(x,usr);
    }
    ierr = DMDAVecRestoreArray(info->da, uex, &auex);CHKERRQ(ierr);
    return 0;
}

/* compute residuals:
     F_i = (- eps u'' + (a(x) u)') * hx   at interior points
     F_i = c (u - (b.c.))          at boundary points         */
PetscErrorCode FormFunctionLocal(DMDALocalInfo *info, PetscReal *au,
                                 PetscReal *aF, AdCtx *usr) {
    const PetscReal  eps = usr->eps,
                     hx = 2.0 / (info->mx - 1),
                     halfx = hx / 2.0,
                     hx2 = hx * hx,
                     scdiag = (2.0 * eps) / hx + 1.0;
    PetscReal        x, uE, uW, uxx, a, u_up, flux, u_dn, u_far, theta;
    PetscInt         i;

    // for each owned cell, non-advective part of residual at cell center
    for (i=info->xs; i<info->xs+info->xm; i++) {
        if (i == 0) {
            aF[i] = scdiag * (au[i] - 1.0);
        } else if (i == info->mx-1) {
            aF[i] = scdiag * (au[i] - 0.0);
        } else {
            uW = (i == 1)          ? 1.0 : au[i-1];
            uE = (i == info->mx-2) ? 0.0 : au[i+1];
            uxx = (uW - 2.0 * au[i] + uE) / hx2;
            aF[i] = - eps * uxx * hx;
        }
    }
    // for each E face of an owned cell, compute flux at the face center
    //     and then add that to the correct residual
    // note -1 start to get W faces of owned cells living on ownership
    //     boundaries
    for (i=info->xs-1; i<info->xs+info->xm; i++) {
        // if cell center is outside [-1,1], or on x=1 boundary, then no need
        // to compute a flux
        if (i < 0 || i == info->mx-1)
            continue;
        // traverse E cell face center points x_{i+1/2} for flux contributions
        // get pth component of wind and first-order upwind flux
        x = -1.0 + i * hx;
        a = wind_a(x + halfx);
        if (a >= 0.0) {
            if (i == 0) {
                u_up = 1.0;
            } else {
                u_up = au[i];
            }
        } else {
            if (i+1 == info->mx-1) {
                u_up = 0.0;
            } else {
                u_up = au[i+1];
            }
        }
        flux = a * u_up;
        if (usr->limiter_fcn != NULL) {
            // flux correction from high-order formula with psi(theta)
            if (a >= 0)
                u_dn = (i+1 < info->mx-1) ? au[i+1] : 0.0;
            else
                u_dn = au[i];
            if (u_dn != u_up) {
                if (a >= 0)
                   u_far = (i-1 > 0) ? au[i-1] : 1.0;
                else
                   u_far = (i+2 < info->mx-1) ? au[i+2] : 0.0;
                theta = (u_up - u_far) / (u_dn - u_up);
                flux += a * (*(usr->limiter_fcn))(theta) * (u_dn - u_up);
            }
        }
        // update non-boundary and owned F_i on both sides of computed flux
        // note aF[] does not have stencil width
        if (i > 0 && i >= info->xs)
            aF[i] += flux;  // flux out of i at E
        if (i+1 < info->mx-1 && i+1 < info->xs + info->xm)
            aF[i+1] -= flux;  // flux into i+1 at W
    }
    return 0;
}

PetscErrorCode FormJacobianLocal(DMDALocalInfo *info, PetscReal *u,
                                 Mat J, Mat P, AdCtx *usr) {
    PetscErrorCode ierr;
    const PetscReal eps = usr->eps,
                    hx = 2.0 / (info->mx - 1),
                    halfx = hx / 2.0,
                    scdiag = (2.0 * eps) / hx + 1.0;
    PetscInt        i, col[3];
    PetscReal       x, aE, aW, v[3];

    if (usr->jac_limiter_fcn == &vanleer) {
        SETERRQ(PETSC_COMM_SELF,1,"Jacobian for vanleer limiter is not implemented");
    }

    ierr = MatZeroEntries(P); CHKERRQ(ierr);

    for (i=info->xs; i<info->xs+info->xm; i++) {
        if (i == 0 || i == info->mx-1) {
            v[0] = scdiag;
            ierr = MatSetValues(P,1,&i,1,&i,v,ADD_VALUES); CHKERRQ(ierr);
        } else {
            // diffusive part
            col[0] = i;
            v[0] = (2.0 * eps) / hx;
            col[1] = i-1;
            v[1] = (i-1 > 0) ? - eps / hx : 0.0;
            col[2] = i+1;
            v[2] = (i+1 < info->mx-1) ? - eps / hx : 0.0;
            ierr = MatSetValues(P,1,&i,3,col,v,ADD_VALUES); CHKERRQ(ierr);
            // advective part: from each adjacent face
            x = -1.0 + i * hx;
            aE = wind_a(x + halfx);
            aW = wind_a(x - halfx);
            if (aE >= 0.0) {
                col[0] = i;
                v[0] = aE;
            } else {
                col[0] = i+1;
                v[0] = (i+1 < info->mx-1) ? aE : 0.0;  // check if i+1 is boundary
            }
            if (aW >= 0.0) {
                col[1] = i-1;
                v[1] = (i-1 > 0) ? - aW : 0.0;  // check if i-1 is boundary
            } else {
                col[1] = i;
                v[1] = - aW;
            }
            ierr = MatSetValues(P,1,&i,2,col,v,ADD_VALUES); CHKERRQ(ierr);
            if (usr->jac_limiter_fcn == &centered) {
                col[0] = i+1;
                col[1] = i;
                if (aE >= 0.0) {
                    v[0] = (i+1 < info->mx-1) ? aE/2.0 : 0.0;  // check if i+1 is boundary
                    v[1] = - aE/2.0;
                } else {
                    v[0] = (i+1 < info->mx-1) ? - aE/2.0 : 0.0;  // check if i+1 is boundary
                    v[1] = aE/2.0;
                }
                ierr = MatSetValues(P,1,&i,2,col,v,ADD_VALUES); CHKERRQ(ierr);
                col[0] = i;
                col[1] = i-1;
                if (aW >= 0.0) {
                    v[0] = - aW/2.0;
                    v[1] = (i-1 > 0) ? aW/2.0 : 0.0;  // check if i-1 is boundary
                } else {
                    v[0] = aW/2.0;
                    v[1] = (i-1 > 0) ? - aW/2.0 : 0.0;  // check if i-1 is boundary
                }
                ierr = MatSetValues(P,1,&i,2,col,v,ADD_VALUES); CHKERRQ(ierr);
            }
        }
    }
    ierr = MatAssemblyBegin(P,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    ierr = MatAssemblyEnd(P,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    if (J != P) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}


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

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

/* Factorise a general M x N matrix A into
 *
 *   A = Q L
 *
 * where Q is orthogonal (M x M) and L is lower triangular (M x N).
 *
 * Q is stored as a packed set of Householder transformations in the
 * strict upper triangular part of the input matrix.
 *
 * L is stored in the diagonal and lower triangle of the input matrix.
 *
 * The full matrix for Q can be obtained as the product
 *
 *       Q = Q_k .. Q_2 Q_1
 *
 * where k = MIN(M,N) and
 *
 *       Q_i = (I - tau_i * v_i * v_i')
 *
 * and where v_i is a Householder vector
 *
 *       v_i = [A(1,N-k+i), A(2,N-k+i), ... , A(M-k+i,N-k+i), 1, 0, ..., 0]
 *
 * This storage scheme is the same as in LAPACK.  */

/*
gsl_linalg_QL_decomp()
  Perform QL decomposition of a matrix A

Inputs: A   - M-by-N matrix
        tau - (output) Householder coefficients, length N

Notes:
1) The K = MIN(M, N) Householder scalars are stored in tau(N-K+1:N)
on output; the rest of tau is used as temporary workspace
*/

int
gsl_linalg_QL_decomp (gsl_matrix * A, gsl_vector * tau)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

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

      for (i = 0; i < K; i++)
        {
          /* compute the Householder transformation to annihilate the (N-K+i)-th
             column of the matrix */

          gsl_vector_view c = gsl_matrix_subcolumn (A, N - i - 1, 0, M - i);
          double * alpha = gsl_matrix_ptr(A, M - i - 1, N - i - 1);
          double tau_j = gsl_linalg_householder_transform2 (alpha, &(c.vector));

          /* apply the transformation to A(1:M-i,1:N-i-2) from the left */
          if (i + 1 < N)
            {
              gsl_vector_view work = gsl_vector_subvector(tau, 0, N - i - 1);
              gsl_matrix_view m = gsl_matrix_submatrix (A, 0, 0, M - i, N - i - 1);
              double tmp = *alpha;

              *alpha = 1.0;
              gsl_linalg_householder_left (tau_j, &(c.vector), &(m.matrix), &work.vector);
              *alpha = tmp;
            }

          gsl_vector_set (tau, N - i - 1, tau_j);
        }

      return GSL_SUCCESS;
    }
}

/* form the orthogonal matrix Q from the packed QL matrix */
int
gsl_linalg_QL_unpack (const gsl_matrix * QL, const gsl_vector * tau, gsl_matrix * Q, gsl_matrix * L)
{
  const size_t M = QL->size1;
  const size_t N = QL->size2;

  if (Q->size1 != M || Q->size2 != M)
    {
      GSL_ERROR ("Q matrix must be M x M", GSL_ENOTSQR);
    }
  else if (L->size1 != M || L->size2 != N)
    {
      GSL_ERROR ("L matrix must be M x N", GSL_ENOTSQR);
    }
  else if (tau->size != N)
    {
      GSL_ERROR ("size of tau must be N", GSL_EBADLEN);
    }
  else
    {
      const size_t K = GSL_MIN(M, N);
      size_t i;

      /* initialize Q to the identity */
      gsl_matrix_set_identity (Q);

      for (i = 0; i < K; ++i)
        {
          gsl_vector_const_view h = gsl_matrix_const_subcolumn (QL, N - K + i, 0, M - K + i + 1);
          gsl_matrix_view m = gsl_matrix_submatrix (Q, 0, 0, M - K + i + 1, M - K + i + 1);
          gsl_vector_view work = gsl_matrix_subcolumn(L, 0, 0, M - K + i + 1);
          double ti = gsl_vector_get (tau, N - K + i);
          double * ptr = gsl_matrix_ptr((gsl_matrix *) QL, M - K + i, N - K + i);
          double tmp = *ptr;

          *ptr = 1.0;
          gsl_linalg_householder_left (ti, &h.vector, &m.matrix, &work.vector);
          *ptr = tmp;
        }

      /* form the left triangular matrix L from a packed QL matrix */
      gsl_matrix_set_zero(L);

      if (M >= N)
        {
          gsl_matrix_const_view src = gsl_matrix_const_submatrix(QL, M - N, 0, N, N);
          gsl_matrix_view dest = gsl_matrix_submatrix(L, M - N, 0, N, N);
          gsl_matrix_tricpy(CblasLower, CblasNonUnit, &dest.matrix, &src.matrix);
        }
      else
        {
          gsl_matrix_const_view src1 = gsl_matrix_const_submatrix(QL, 0, 0, M, N - M);
          gsl_matrix_view dest1 = gsl_matrix_submatrix(L, 0, 0, M, N - M);

          gsl_matrix_const_view src2 = gsl_matrix_const_submatrix(QL, 0, N - M, M, M);
          gsl_matrix_view dest2 = gsl_matrix_submatrix(L, 0, N - M, M, M);

          gsl_matrix_memcpy(&dest1.matrix, &src1.matrix);
          gsl_matrix_tricpy(CblasLower, CblasNonUnit, &dest2.matrix, &src2.matrix);
        }

      return GSL_SUCCESS;
    }
}