#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_sf_gamma.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

/* very simple approximation */
double st_gamma(double x)
{
  return sqrt(2.0*M_PI/x)*pow(x/M_E, x);
}

#define A 12
double sp_gamma(double z)
{
  const int a = A;
  static double c_space[A];
  static double *c = NULL;
  int k;
  double accm;

  if ( c == NULL ) {
    double k1_factrl = 1.0; /* (k - 1)!*(-1)^k with 0!==1*/
    c = c_space;
    c[0] = sqrt(2.0*M_PI);
    for(k=1; k < a; k++) {
      c[k] = exp(a-k) * pow(a-k, k-0.5) / k1_factrl;
	  k1_factrl *= -k;
    }
  }
  accm = c[0];
  for(k=1; k < a; k++) {
    accm += c[k] / ( z + k );
  }
  accm *= exp(-(z+a)) * pow(z+a, z+0.5); /* Gamma(z+1) */
  return accm/z;
}

int main()
{
  double x;

  printf("%15s%15s%15s%15s\n", "Stirling", "Spouge", "GSL", "libm");
  for(x=1.0; x <= 10.0; x+=1.0) {
    printf("%15.8lf%15.8lf%15.8lf%15.8lf\n", st_gamma(x/3.0), sp_gamma(x/3.0),
	   gsl_sf_gamma(x/3.0), tgamma(x/3.0));
  }
  return 0;
}


#include <math.h>
#include <cblas.h>

void matmult_mnk(int M, int N, int K, double **A, double **B, double **C) {
	for (int l = 0; l < M*N; l++) {
		C[0][l] = 0;
	}
    	for (int m = 0; m < M; m++) {
    		for (int n = 0; n < N; n++) {
			for (int k = 0; k < K; k++) {
				C[m][n] += A[m][k] * B[k][n];
		}
    	}
    }
}

void matmult_mkn(int M, int N, int K, double **A, double **B, double **C) {
	for (int l = 0; l < M*N; l++) {
		C[0][l] = 0;
	}
    for (int m = 0; m < M; m++) {
    	for (int k = 0; k < K; k++) {
			for (int n = 0; n < N; n++) {
				C[m][n] += A[m][k] * B[k][n];
			}
    	}
    }
}

void matmult_nmk(int M, int N, int K, double **A, double **B, double **C) {
	for (int l = 0; l < M*N; l++) {
		C[0][l] = 0;
	}
    for (int n = 0; n < N; n++) {
    	for (int m = 0; m < M; m++) {
			for (int k = 0; k < K; k++) {
				C[m][n] += A[m][k] * B[k][n];
			}
    	}
    }
}

void matmult_nkm(int M, int N, int K, double **A, double **B, double **C) {
	for (int l = 0; l < M*N; l++) {
		C[0][l] = 0;
	}
    for (int n = 0; n < N; n++) {
    	for (int k = 0; k < K; k++) {
			for (int m = 0; m < M; m++) {
				C[m][n] += A[m][k] * B[k][n];
			}
    	}
    }
}

void matmult_kmn(int M, int N, int K, double **A, double **B, double **C) {
	for (int l = 0; l < M*N; l++) {
		C[0][l] = 0;
	}
    for (int k = 0; k < K; k++) {
    	for (int m = 0; m < M; m++) {
			for (int n = 0; n < N; n++) {
				C[m][n] += A[m][k] * B[k][n];
			}
    	}
    }
}

void matmult_knm(int M, int N, int K, double **A, double **B, double **C) {
	for (int l = 0; l < M*N; l++) {
		C[0][l] = 0;
	}
    for (int k = 0; k < K; k++) {
    	for (int n = 0; n < N; n++) {
			for (int m = 0; m < M; m++) {
				C[m][n] += A[m][k] * B[k][n];
			}
    	}
    }
}

void matmult_nat(int M, int N, int K, double **A, double **B, double **C) {
	for (int m = 0; m < M; m++) {
		for (int n = 0; n < N; n++) {
			C[m][n] = 0;
			for (int k = 0; k < K; k++) {
				C[m][n] += A[m][k] * B[k][n];
			}
    	}
    }
}

void matmult_lib(int M, int N, int K, double **A, double **B, double **C) {
	int layout, TRANSA, TRANSB, LDA, LDB, LDC;
	double alpha, beta;

    layout = 101; // rowmajor
	TRANSA = 111; // A is not transposed
	TRANSB = 111; // B is not transposed

	LDA = fmax(1,K); // leading dimension of A
	LDB = fmax(1,N); // leading dimension of B
	LDC = fmax(1,N); // leading dimension of C

	alpha = 1.0; // no scaling
	beta = 0.0; //

	cblas_dgemm(layout, TRANSA, TRANSB, M, N, K, alpha, *A, LDA, *B, LDB, beta, *C, LDC);
}

void matmult_blk(int M, int N, int K, double **A, double **B, double **C, int bs)
{
	// MKN

	bs = fmax(1, fmin(bs, K));

	for (int i = 0; i < M*N; i++)
		C[0][i] = 0.0;

	for (int m0 = 0; m0 < M; m0 += bs)
	{
		for (int k0 = 0; k0 < K; k0 += bs)
		{
			for (int n0 = 0; n0 < N; n0 += bs)
			{
				for (int m = m0; m < fmin(m0 + bs, M); m++)
				{
					for (int k = k0; k < fmin(k0 + bs, K); k++)
					{
						for (int n = n0; n < fmin(n0 + bs, N); n++)
							C[m][n] += A[m][k] * B[k][n];
					}
				}
			}
		}
	}
}


//STARTWHOLE
static char help[] = "Solve a tridiagonal system of arbitrary size.\n"
"Option prefix = tri_.\n";

#include <petsc.h>

int main(int argc,char **args) {
    PetscErrorCode ierr;
    Vec    x, b, xexact;
    Mat    A;
    KSP    ksp;
    int    m = 4, i, Istart, Iend, j[3];
    double v[3], xval, errnorm;

    PetscInitialize(&argc,&args,NULL,help);

    ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"tri_","options for tri",""); CHKERRQ(ierr);
    ierr = PetscOptionsInt("-m","dimension of linear system","tri.c",m,&m,NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);

    ierr = VecCreate(PETSC_COMM_WORLD,&x); CHKERRQ(ierr);
    ierr = VecSetSizes(x,PETSC_DECIDE,m); CHKERRQ(ierr);
    ierr = VecSetFromOptions(x); CHKERRQ(ierr);
    ierr = VecDuplicate(x,&b); CHKERRQ(ierr);
    ierr = VecDuplicate(x,&xexact); CHKERRQ(ierr);

    ierr = MatCreate(PETSC_COMM_WORLD,&A); CHKERRQ(ierr);
    ierr = MatSetSizes(A,PETSC_DECIDE,PETSC_DECIDE,m,m); CHKERRQ(ierr);
    ierr = MatSetOptionsPrefix(A,"a_"); CHKERRQ(ierr);
    ierr = MatSetFromOptions(A); CHKERRQ(ierr);
    ierr = MatSetUp(A); CHKERRQ(ierr);
    ierr = MatGetOwnershipRange(A,&Istart,&Iend); CHKERRQ(ierr);
    for (i=Istart; i<Iend; i++) {
        if (i == 0) {
            v[0] = 3.0;  v[1] = -1.0;
            j[0] = 0;    j[1] = 1;
            ierr = MatSetValues(A,1,&i,2,j,v,INSERT_VALUES); CHKERRQ(ierr);
        } else {
            v[0] = -1.0;  v[1] = 3.0;  v[2] = -1.0;
            j[0] = i-1;   j[1] = i;    j[2] = i+1;
            if (i == m-1) {
                ierr = MatSetValues(A,1,&i,2,j,v,INSERT_VALUES); CHKERRQ(ierr);
            } else {
                ierr = MatSetValues(A,1,&i,3,j,v,INSERT_VALUES); CHKERRQ(ierr);
            }
        }
        xval = exp(cos(i));
        ierr = VecSetValues(xexact,1,&i,&xval,INSERT_VALUES); CHKERRQ(ierr);
    }
    ierr = MatAssemblyBegin(A,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(A,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = VecAssemblyBegin(xexact); CHKERRQ(ierr);
    ierr = VecAssemblyEnd(xexact); CHKERRQ(ierr);
    ierr = MatMult(A,xexact,b); CHKERRQ(ierr);

    ierr = KSPCreate(PETSC_COMM_WORLD,&ksp); CHKERRQ(ierr);
    ierr = KSPSetOperators(ksp,A,A); CHKERRQ(ierr);
    ierr = KSPSetFromOptions(ksp); CHKERRQ(ierr);
    ierr = KSPSolve(ksp,b,x); CHKERRQ(ierr);

    ierr = VecAXPY(x,-1.0,xexact); CHKERRQ(ierr);
    ierr = VecNorm(x,NORM_2,&errnorm); CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD,
    "error for m = %d system is |x-xexact|_2 = %.1e\n",m,errnorm); CHKERRQ(ierr);

    KSPDestroy(&ksp);  MatDestroy(&A);
    VecDestroy(&x);  VecDestroy(&b);  VecDestroy(&xexact);
    return PetscFinalize();
}
//ENDWHOLE


//============================================================================
// Name        : main.c
// Author      : Cesare De Cal
// Version     :
// Copyright   : Cesare De Cal
// Description : Exercise 7 - System of Linear Equations
//============================================================================

#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

// Useful: https://gist.github.com/bjd2385/7f4685e703f7437e513608f41c65bbd7 (LU decomposition)
// Useful: https://github.com/rwinlib/gsl/blob/master/include/gsl/gsl_linalg.h#L173 (GEPP)

gsl_matrix *createMatrix(int size) {
  gsl_matrix *matrix = gsl_matrix_alloc(size, size);
  gsl_matrix_set_zero(matrix);

  for (int i = 1; i <= size; i++) {
    for (int j = 1; j <= size; j++) {
      // Fill diagonal
      if (i == j) {
        if ((i+1) % 3 == 0) {
          double value = (2.0*(i+1))/3;
          gsl_matrix_set(matrix, i-1, j-1, value);
        } else {
          gsl_matrix_set(matrix, i-1, j-1, 1);
        }
        // Fill adjacent lower diagonal
      } else if (i == j+1) {
        gsl_matrix_set(matrix, i-1, j-1, 1);
        // Fill adjacent upper diagonal
      } else if (i+1 == j) {
        gsl_matrix_set(matrix, i-1, j-1, -1);
      }
    }
  }
  return matrix;
}

// Helper methods

void printVectorContents(gsl_vector *vector) {
  for (int i = 0; i < vector->size; i++) {
    printf("%.9e\\\\\n", gsl_vector_get(vector, i));
  }
  printf("\n");
}

void printMatrixContents(gsl_matrix *matrix) {
  for (int i = 0; i < matrix->size1; ++i) {
    for (int j = 0; j < matrix->size2; ++j) {
      printf("%.9e ", gsl_matrix_get(matrix, i, j));

      if (j == matrix->size2-1) {
        printf("\\\\");
      } else {
        printf("& ");
      }
    }
    printf("\n");
  }
  printf("\n");
}

void computeForOrder(int size, int shouldPrintVerboseOutput) {
  gsl_matrix *matrixA = createMatrix(size);
  if (shouldPrintVerboseOutput) {
    printf("Creating matrix A:\n");
    printMatrixContents(matrixA);
  }

  // Right-hand side vector y
  gsl_vector *bVector = gsl_vector_alloc(matrixA->size2);
  gsl_vector_set_zero(bVector);
  // Fill element at index 0 with 1
  gsl_vector_set(bVector, 0, 1);

  if (shouldPrintVerboseOutput) {
    printf("Creating vector y (b):\n");
    printVectorContents(bVector);
  }

  // Solve system using LU decomposition
  gsl_matrix *luMatrix = gsl_matrix_alloc(matrixA->size1, matrixA->size2);
  gsl_permutation *permutation = gsl_permutation_alloc(matrixA->size1);
  int signum; // Sign of the permutation
  // Copy A over newly created matrixA
  gsl_matrix_memcpy(luMatrix, matrixA);
  // Compute LU decomposition
  gsl_linalg_LU_decomp(luMatrix, permutation, &signum);

  if (shouldPrintVerboseOutput) {
    printf("LU decomposition of A:\n");
    printMatrixContents(luMatrix);
  }

  // Result vector v
  gsl_vector *xVector = gsl_vector_alloc(matrixA->size2);
  gsl_vector_set_zero(xVector);

  gsl_linalg_LU_solve(luMatrix, permutation, bVector, xVector);

  if (shouldPrintVerboseOutput) {
    printf("Solutions x vector:\n");
    printVectorContents(xVector);
  }

  // Calculate condition number
  // There is no function in GSL to directly compute this number
  // the condition number is given by taking the absolute value of the ratio of the largest singular value and the smallest singular value
  // cond(A) = abs( largest_sing_val / smallest_sing_val )
  gsl_matrix *matrixV = gsl_matrix_alloc(matrixA->size1, matrixA->size2);
  gsl_vector *vectorS = gsl_vector_alloc(matrixA->size1);
  gsl_vector *vectorWorkspace = gsl_vector_alloc(matrixA->size1);
  gsl_linalg_SV_decomp(matrixA, matrixV, vectorS, vectorWorkspace);

  if (shouldPrintVerboseOutput) {
    printf("Singular diagonal vector:\n");
    printVectorContents(vectorS);
  }

  double minSingularValue, maxSingularValue;
  gsl_vector_minmax(vectorS, &minSingularValue, &maxSingularValue);
  if (shouldPrintVerboseOutput) {
    printf("Max: %.9e, min: %.9e\n", maxSingularValue, minSingularValue);
  }

  double conditionNumber = fabs(maxSingularValue/minSingularValue);
  double x1 = gsl_vector_get(xVector, 0);
  double error = (M_E - 2) - x1;

  printf("%d & %.9e & %.9e & %.9e \\\\", size, x1, error, conditionNumber);

  // Free memory space
  gsl_matrix_free(matrixA);
  gsl_vector_free(bVector);
  gsl_matrix_free(luMatrix);
  gsl_permutation_free(permutation);
  gsl_vector_free(xVector);
  gsl_matrix_free(matrixV);
  gsl_vector_free(vectorS);
  gsl_vector_free(vectorWorkspace);
}

int main() {
  printf("This program is going to generate a LaTeX friendly output to be used in the report table.\n");
  printf("------------------------------\n");
  printf("n, x1, error, condition number\n");
  for (int n = 1; n <= 50; n++) {
    computeForOrder(n, 0);
    printf("\n");
  }
  printf("------------------------------\n");
  return 0;
}


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

#define NUMBER_OF_ELEMENTS 1
#define MEAN 0
#define STANDARD_DEVIATION 1

int main (int argc, char *argv[])
{
        int number_of_elements;

        if (argc == 1) {
                number_of_elements = NUMBER_OF_ELEMENTS;
        } else if (argc == 2) {
                number_of_elements = atoi(argv[1]);
        } else {
                fprintf(stderr, "%s\n", "Wrong number of arguments.");
                fprintf(stderr, "%s\n", "Use: ./normal_numbers number_of_elements");
                exit(EXIT_FAILURE);
        }

        double *numbers = malloc (number_of_elements * sizeof(double));

        double mean = MEAN;
        double standard_deviation = STANDARD_DEVIATION;

        // declare the necessary random number generator variables
        const gsl_rng_type *T;
        gsl_rng *r;

        // set the default values for the random number generator variables
        gsl_rng_env_setup();

        // create a seed based on time
        struct timeval tv;
        gettimeofday(&tv,0);
        unsigned long seed = tv.tv_sec + tv.tv_usec;

        // setup the generator using the seed just created
        T = gsl_rng_default;
        r = gsl_rng_alloc (T);
        gsl_rng_set(r, seed);

        // generate number_of_elements normal random numbers
        for (int i = 0; i < number_of_elements; i++) {
                numbers[i] = mean + gsl_ran_gaussian (r, standard_deviation);
        }

        // print numbers
        if(number_of_elements < 10) {
                for (int i = 0; i < number_of_elements; i++) {
                        printf (" %f", numbers[i]);
                }
                printf("\n");
        } else {
                printf("Done.\n");
        }

        // free memory
        gsl_rng_free (r);
        free(numbers);

        return 0;
}


#include <stdio.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_multimin.h>
/* Paraboloid with a minimum at f(dp[0],dp[1]) = 30.0 */

double
my_f (const gsl_vector * v, void *params)
{
  double x, y;
  double *dp = (double *) params;

  x = gsl_vector_get (v, 0);
  y = gsl_vector_get (v, 1);

  return 10.0 * (x - dp[0]) * (x - dp[0]) +
    20.0 * (y - dp[1]) * (y - dp[1]) + 30.0;
}

/* The gradient of f, df = (df/dx, df/dy). */
void
my_df (const gsl_vector * v, void *params, gsl_vector * df)
{
  double x, y;
  double *dp = (double *) params;

  x = gsl_vector_get (v, 0);
  y = gsl_vector_get (v, 1);

  gsl_vector_set (df, 0, 20.0 * (x - dp[0]));
  gsl_vector_set (df, 1, 40.0 * (y - dp[1]));
}

/* Now both f and df together. */
void
my_fdf (const gsl_vector * x, void *params, double *f, gsl_vector * df)
{
  *f = my_f (x, params);
  my_df (x, params, df);
}

int
main (void)
{
  size_t iter = 0;
  int status;

  const gsl_multimin_fdfminimizer_type *T;
  gsl_multimin_fdfminimizer *s;

  /* Position of the minimum (1,2). */
  double par[2] = { 1.0, 2.0 };

  gsl_vector *x;
  gsl_multimin_function_fdf my_func;

  my_func.f = &my_f;
  my_func.df = &my_df;
  my_func.fdf = &my_fdf;
  my_func.n = 2;
  my_func.params = ∥

  /* Starting point, x = (5,7) */

  x = gsl_vector_alloc (2);
  gsl_vector_set (x, 0, 5.0);
  gsl_vector_set (x, 1, 7.0);

  T = gsl_multimin_fdfminimizer_conjugate_fr;
  s = gsl_multimin_fdfminimizer_alloc (T, 2);

  gsl_multimin_fdfminimizer_set (s, &my_func, x, 0.01, 1e-4);

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

      if (status)
        break;

      status = gsl_multimin_test_gradient (s->gradient, 1e-3);

      if (status == GSL_SUCCESS)
        printf ("Minimum found at:\n");

      printf ("%5d %.5f %.5f %10.5f\n", iter,
              gsl_vector_get (s->x, 0), gsl_vector_get (s->x, 1), s->f);

    }
  while (status == GSL_CONTINUE && iter < 100);

  gsl_multimin_fdfminimizer_free (s);
  gsl_vector_free (x);

  return 0;
}


//
// Created by robin on 2018/9/1.
// Copyright (c) 2018 Robin. All rights reserved.
//

#pragma once

#include <gsl/gsl>

namespace satz {

template <typename P>
const P zero = P::ZERO;

template <typename P>
const P one = P::ONE;

template <typename P>
const P X = P::X;

/**
 * @brief Compute $b^e$.
 * @param b base
 * @param e exponent
 * @param acc
 * @pre $e \ge 0, acc = 1$.
 */
template <typename P, typename N>
P pow (P b, N e, P acc = one<P>) {
  Expects(e >= 0);

  while (e) {
    if (e % 2) acc = acc * b;
    b = b * b;
    e = e / 2;
  }

  return acc;
}

/**
 * @brief Compute $(b^e) mod m$.
 * @param b base
 * @param e exponent
 * @param m modulus
 * @param acc
 * @pre $e >= 0, acc = 1$
 */
template <typename P, typename N>
P mod_pow (P b, N e, const P& m, P acc = one<P>) {
  Expects(e >= 0);

  while (e) {
    if (e % 2) acc = (acc * b) % m;
    b = (b * b) % m;
    e = e / 2;
  }

  return acc;
}

template <typename P>
P gcd (P a, P b) {
  while (b) {
    std::tie(a, b) = std::make_pair(b, a % b);
  }
  return a;
}

/**
 * @brief Compute $(x^{2^p} - x) mod m$.
 * @param p
 * @param m modulus
 */
template <typename P, typename N>
P reduce_exponent (N p, const P& m) {
  auto r = X<P>;
  for (; p > 0; --p) r = (r * r) % m;
  return (r - X<P>) % m;
}

/**
 * @brief Given a polynomial p over GF(2) return whether it is irreducible
 *    with Ben-Or's algorithm.
 * @param p polynomial over GF(2)
 * @return true if p is irreducible.
 * @pre p is non-constant.
 */
template <typename P>
bool ben_or_test (const P& p) {
  Expects(p.degree() > 0);

  auto             d = p.degree();
  for (decltype(d) i = 1; i <= d / 2; ++i) {
    auto b = reduce_exponent(i, p);
    auto g = gcd(p, b);
    if (g != one<P>) return false;
  }
  return true;
}

/**
 * @brief Given a polynomial p over GF(2), return whether it is irreducible.
 * @param p polynomial over GF(2)
 * @return true if p is irreducible.
 */
template <typename P>
bool is_irreducible (const P& p) {
  if (p.degree() > 0) return ben_or_test(p);
  else return false;
}

}


/* File: set_cosmo_dist.c */
/*
  This file is a part of the Corrfunc package
  Copyright (C) 2015-- Manodeep Sinha (manodeep@gmail.com)
  License: MIT LICENSE. See LICENSE file under the top-level
  directory at https://github.com/manodeep/Corrfunc/
*/

/* function set_cosmo_dist (modelled after cosmodist.c)

   --- Computes the comoving distance as a function of redshift for
   a given cosmological model.
*/

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

#include "set_cosmo_dist.h"
#include "cosmology_params.h"

#define SQR(x) ((x)*(x))
#define CUBE(x) ((x)*(x)*(x))
#define epsilon 1e-10

int set_cosmo_dist(const double zmax,const int max_size,double *zc,double *dc,const int lasdamas_cosmology)
{
    /* First, initialize cosmology*/
    int status = init_cosmology(lasdamas_cosmology);
    if(status != EXIT_SUCCESS) {
        return -1;
    }

    int i = 0;
    double Omegak;
    double smallh = 1.0;//Andreas pointed out that I don't need the real value of LITTLE_H
    Omegak = 1.0 - OMEGA_M - OMEGA_L;
    const double Dh = SPEED_OF_LIGHT*0.01/smallh ;// c/(100) -> in units of little h^-1 Mpc

    const double Deltaz = 1.0/max_size;
    const double dz = 1e-2*Deltaz;

    double Eint = 0.0;
    double E2 = 1.0 ;
    double z2 = Deltaz ;
    for(double z=2.0*dz;z<zmax;z+=2.0*dz) {
        double E0 = E2 ;

        double E1 = 1.0/sqrt(OMEGA_M * CUBE(1+z-dz) + Omegak *(1+z-dz) + OMEGA_L);
        E2 = 1.0/sqrt(OMEGA_M * CUBE(1+z) + Omegak *(1+z) + OMEGA_L);
        Eint += dz*(E0 + 4.*E1 + E2)/3. ;

        if(z>(z2-epsilon) && z<(z2+epsilon)) {
            if( i >= max_size )
                break ;

            const double Dc = Eint*Dh ;
            zc[i] = z ;
            dc[i] = Dc ;
            z2 += Deltaz ;
            i++ ;
        }
    }

#ifdef DEBUG
    fprintf(stderr,"%s> (Omega_m, Omega_L, Omega_k, h, zmax) = (%4.2f, %4.2f, %4.2f, %4.2f,%4.2lf)\n", __FUNCTION__, OMEGA_M,OMEGA_L,Omegak,LITTLE_H,zmax) ;
    fprintf(stderr,"%s> tabulated redshift: %g to %g  (distance: %g to %g Mpc)\n", __FUNCTION__, zc[0], zc[i-1], dc[0], dc[i-1]) ;
#endif
    return i ;
}


static char help[] = "Newton's method for a three-variable system.\n"
"The system is from fitting logistic population model\n"
"P(t) = N / (1 + A exp(-Nbt)) to 3 points (t,P) from census data in\n"
"D. Zill, 11th ed., exercise 4 in section 3.2.  The points are\n"
"(1790,3.929), (1850,23.192), (1910,91.972); populations in millions.\n";

/*
example:
$ make census
$ ./census -snes_fd -snes_monitor
  0 SNES Function norm 2.233793904584e+01
...
 17 SNES Function norm 6.521724317021e-07
 18 SNES Function norm 4.358557709045e-09
Vec Object: 1 MPI processes
  type: seq
197.274
49.2096
0.000158863
*/

#include <petsc.h>

extern PetscErrorCode FormFunction(SNES, Vec, Vec, void*);

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    SNES  snes;          // nonlinear solver context
    Vec   x, r;          // solution, residual vectors
    double *ax;

    PetscInitialize(&argc,&argv,NULL,help);
    ierr = VecCreate(PETSC_COMM_WORLD,&x); CHKERRQ(ierr);
    ierr = VecSetSizes(x,PETSC_DECIDE,3); CHKERRQ(ierr);
    ierr = VecSetFromOptions(x); CHKERRQ(ierr);
    ierr = VecSetUp(x); CHKERRQ(ierr);
    ierr = VecGetArray(x,&ax);CHKERRQ(ierr);
    // ax = [N, A, b]; see equations in FormFunction() below
    // these initial values are based on *guessing* N=300
    // as the limiting population, then computing A, then b
    ax[0] = 300.0;
    ax[1] = 75.0;
    ax[2] = 1.0e-4;
    ierr = VecRestoreArray(x,&ax);CHKERRQ(ierr);
    ierr = VecAssemblyBegin(x);CHKERRQ(ierr);
    ierr = VecAssemblyEnd(x);CHKERRQ(ierr);
    ierr = VecDuplicate(x,&r); CHKERRQ(ierr);

    ierr = SNESCreate(PETSC_COMM_WORLD,&snes); CHKERRQ(ierr);
    ierr = SNESSetFunction(snes,r,FormFunction,NULL); CHKERRQ(ierr);
    ierr = SNESSetFromOptions(snes); CHKERRQ(ierr);
    ierr = SNESSolve(snes,NULL,x); CHKERRQ(ierr);
    ierr = VecView(x,PETSC_VIEWER_STDOUT_WORLD); CHKERRQ(ierr);

    VecDestroy(&x);  VecDestroy(&r);  SNESDestroy(&snes);
    return PetscFinalize();
}

PetscErrorCode FormFunction(SNES snes, Vec x, Vec F, void *ctx) {
    PetscErrorCode ierr;
    const double *ax;
    double       *aF;

    ierr = VecGetArrayRead(x,&ax);CHKERRQ(ierr);
    ierr = VecGetArray(F,&aF);CHKERRQ(ierr);
    // ax = [N, A, b]
    //  3.929 (1 + A)               - N = 0
    // 23.192 (1 + A exp(-60*N*b))  - N = 0
    // 91.972 (1 + A exp(-120*N*b)) - N = 0
    aF[0] =  3.929 * (1.0 + ax[1]) - ax[0];
    aF[1] = 23.192 * (1.0 + ax[1] * PetscExpReal(-60.0*ax[0]*ax[2])) - ax[0];
    aF[2] = 91.972 * (1.0 + ax[1] * PetscExpReal(-120.0*ax[0]*ax[2])) - ax[0];
    ierr = VecRestoreArray(F,&aF);CHKERRQ(ierr);
    ierr = VecRestoreArrayRead(x,&ax);CHKERRQ(ierr);
    return 0;
}


#include <stdio.h>
#include <math.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_randist.h>
#include "stats.h"

double ttest_equalvar(const double *data1, int n1, const double *data2, int n2)
{
    double mean1, mean2, var1, var2, num, sp, denom, t, dof, p;
    mean1 = gsl_stats_mean(data1, 1, n1);
    mean2 = gsl_stats_mean(data2, 1, n2);
    var1 = gsl_stats_variance(data1, 1, n1);
    var2 = gsl_stats_variance(data2, 1, n2);
    num = mean1 - mean2;
    sp = sqrt((((n1 - 1) * var1) + ((n2 - 1) * var2)) / (n1 + n2 - 2));
    denom = sp * sqrt((1 / (double) n1) + (1 / (double) n2));
    t = fabs(num / denom);
    dof = (double) n1 + n2 - 2;
    p = (1 - gsl_cdf_tdist_P(t, dof)) * 2;
    return p;
}

double ttest_unequalvar(const double *data1, int n1, const double *data2, int n2)
{
    double mean1, mean2, var1, var2, r1, r2, t, dof, p;
    mean1 = gsl_stats_mean(data1, 1, n1);
    mean2 = gsl_stats_mean(data2, 1, n2);
    var1 = gsl_stats_variance(data1, 1, n1);
    var2 = gsl_stats_variance(data2, 1, n2);
    r1 = var1 / n1;
    r2 = var2 / n2;
    // Compute t-statistic
    t = fabs((mean1 - mean2) / sqrt(r1 + r2));
    dof = pow(r1 + r2, 2) / ((pow(r1, 2) / (n1 - 1)) + (pow(r2, 2) / (n2 - 1)));
    p = (1 - gsl_cdf_tdist_P(t, dof)) * 2;
    return p;
}

double ttest_paired(const double *data1, const double *data2, int n)
{
    double data[n];
    double mean, sd, t, dof, p;
    for (int i = 0; i < n; i++)
    {
        data[i] = data1[i] - data2[i];
    }
    p = ttest(data, n);
    return p;
}

double ttest(const double *data, int n)
{
    double mean, sd, t, dof, p;
    mean = gsl_stats_mean(data, 1, n);
    sd = gsl_stats_sd(data, 1, n);
    t = fabs((mean - 0) / (sd / sqrt((double) n)));
    dof = (double) n - 1;
    p = (1 - gsl_cdf_tdist_P(t, dof)) * 2;
    return p;
}


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

#include <stdio.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sum.h>

#define N 20

int
main (void)
{
  double t[N];
  double sum_accel, err;
  double sum = 0;
  int n;

  gsl_sum_levin_u_workspace * w = gsl_sum_levin_u_alloc (N);

  const double zeta_2 = M_PI * M_PI / 6.0;

  /* terms for zeta(2) = \sum_{n=1}^{\infty} 1/n^2 */

  for (n = 0; n < N; n++)
    {
      double np1 = n + 1.0;
      t[n] = 1.0 / (np1 * np1);
      sum += t[n] ;
    }

  gsl_sum_levin_u_accel (t, N, w, &sum_accel, &err);

  printf("term-by-term sum = % .16f using %d terms\n", sum, N) ;

  printf("term-by-term sum = % .16f using %d terms\n",
         w->sum_plain, w->terms_used) ;

  printf("exact value      = % .16f\n", zeta_2) ;
  printf("accelerated sum  = % .16f using %d terms\n",
         sum_accel, w->terms_used) ;

  printf("estimated error  = % .16f\n", err) ;
  printf("actual error     = % .16f\n", sum_accel - zeta_2) ;

  gsl_sum_levin_u_free (w);

  return 0;
}


/*
 * Take the input stream and tally the distribution of 8-bit bytes.
 * Calculate the entropy.
 */

#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_sf_log.h>
#include <gsl/gsl_math.h>

int main(int argc, char **argv) {
	int tally[256];
	int ch;
	int n = 0;
	double h = 0;

	for (int i=0; i<256; i++) tally[i] = 0;
	FILE *in;

	printf ("argc = %d\n", argc);

	if (argc < 2) {
		printf ("Input from stdin.\n");
		in = stdin;
	} else {
		printf ("Input file: %s\n", argv[1]);
		in = fopen(argv[1], "r");
		if (in == NULL) {
			printf("BAD file: Usage: %s <random_file>\n", argv[0]);
			return 1;
		}
	}

	while ((ch = fgetc(in)) != EOF) {
		tally[ch] ++;
		n ++;
	}
	fclose(in);

	printf("Frequency by byte:");
	for (int i=0; i<256; i++) {
		printf(" %d", tally[i]);
	}
	printf("\n");
	printf("Input length = %d\n", n);

	for (int i=0; i<256; i++) {
		double p = (double)(tally[i])/n;
		if (p>0) {
			h = h - p*gsl_sf_log(p);
		}
	}
	// Divide by ln(2) to produce base 2 logarithms to calculate bits of entropy.
	h = h/M_LN2;
	printf("Entropy (h) = %1.10f\n", h);

	// Maximum entropy on bytes is 8 bits. Efficiency is calculated by dividing by the max.
	double eta = h/8;
	printf("Efficiency (eta) = %1.10f\n", eta);
	printf("ln(1-eta) = %1.10f\n", gsl_sf_log(1-eta));

	// This prediction is based on the regression from test5.
	printf("Predicted value = %1.10f\n", -1.035*gsl_sf_log(n) + 3.562);

	return 0;
}


#ifndef __MATH_SO3_H__
#define __MATH_SO3_H__

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

#include "fasttrig.h"

#ifdef __cplusplus
extern "C" {
#endif

/*
% ROTX  Compute a rotation matrix about the X-axis.
%   R = ROTX(PHI) returns [3x3] rotation matrix R.  Note: PHI
%   is measured in radians.  PHI measures orientation of coordinate
%   frame 2 relative to coordinate frame 1.  Multiplication by rotation
%   matrix R rotates a vector in coordinate frame 1 into coordinate
%   frame 2.
*/
static inline void
so3_rotx (double Rx[9], const double phi)
{
    double s, c;
    fsincos (phi, &s, &c);
    const double data[] = { 1,  0, 0,
                            0,  c, s,
                            0, -s, c };
    memcpy (Rx, data, 9 * sizeof(double));
}

static inline void
so3_rotx_gsl (gsl_matrix *Rx, const double phi)
{
    assert (Rx->size1==3 && Rx->size2==3 && Rx->tda==3);
    so3_rotx (Rx->data, phi);
}

/*
% ROTY  Compute a rotation matrix about the Y-axis.
%   R = ROTY(THETA) returns [3x3] rotation matrix R.  Note: THETA
%   is measured in radians.  THETA measures orientation of coordinate
%   frame 2 relative to coordinate frame 1.  Multiplication by rotation
%   matrix R rotates a vector in coordinate frame 1 into coordinate
%   frame 2.
*/
static inline void
so3_roty (double Ry[9], const double theta)
{
    double s, c;
    fsincos (theta, &s, &c);
    const double data[] = { c, 0, -s,
                            0, 1,  0,
                            s, 0,  c };
    memcpy (Ry, data, 9 * sizeof(double));
}

static inline void
so3_roty_gsl (gsl_matrix *Ry, const double theta)
{
    assert (Ry->size1==3 && Ry->size2==3 && Ry->tda==3);
    so3_roty (Ry->data, theta);
}

/*
% ROTZ  Compute a rotation matrix about the Z-axis.
%   R = ROTZ(PSI) returns [3x3] rotation matrix R.  Note: PSI
%   is measured in radians.  PSI measures orientation of coordinate
%   frame 2 relative to coordinate frame 1.  Multiplication by rotation
%   matrix R rotates a vector in coordinate frame 1 into coordinate
%   frame 2.
*/
static inline void
so3_rotz (double Rz[9], const double psi)
{
    double s, c;
    fsincos (psi, &s, &c);
    const double data[] = { c, s, 0,
                           -s, c, 0,
                            0, 0, 1 };
    memcpy (Rz, data, 9 * sizeof(double));
}

static inline void
so3_rotz_gsl (gsl_matrix *Rz, const double psi)
{
    assert (Rz->size1==3 && Rz->size2==3 && Rz->tda==3);
    so3_rotz (Rz->data, psi);
}

/*
% ROTXYZ  Compute a rotation matrix about the XYZ-axes.
%   R = ROTXYZ(RPH) returns [3x3] rotation matrix R where RPH
%   is a 3-vector of Euler angles [roll,pitch,heading] measured in
%   radians.  RPH measures orientation of coordinate frame 2
%   relative to coordinate frame 1.  Multiplication by rotation
%   matrix R rotates a vector in coordinate frame 2 into coordinate
%   frame 1.
*/
static inline void
so3_rotxyz (double R[9], const double rph[3])
{
    double sr, sp, sh, cr, cp, ch;
    fsincos (rph[0], &sr, &cr);
    fsincos (rph[1], &sp, &cp);
    fsincos (rph[2], &sh, &ch);
    const double data[] = {  ch*cp, -sh*cr + ch*sp*sr,  sh*sr + ch*sp*cr,
                             sh*cp,  ch*cr + sh*sp*sr, -ch*sr + sh*sp*cr,
                            -sp,     cp*sr,             cp*cr             };
    memcpy (R, data, 9 * sizeof(double));
}

static inline void
so3_rotxyz_gsl (gsl_matrix *R, const gsl_vector *rph)
{
    assert (R->size1==3 && R->size2==3 && rph->size==3);

    // when rph is vector view, stride might not be 1
    double rph_data[3] = {rph->data[0], rph->data[rph->stride], rph->data[2*rph->stride]};

    if (R->tda==3) {
        so3_rotxyz (R->data, rph_data);
    }
    else {
        double rot[9];
        so3_rotxyz (rot, rph_data);
        for (int i=0; i < 3; ++i)
            for (int j=0; j < 3; ++j)
                gsl_matrix_set (R, i, j, rot[i*3 + j]);
    }
}

static inline void
so3_rotxyz_cmath (double R[9], const double rph[3])
{
    double sr, sp, sh, cr, cp, ch;
    sr = sin(rph[0]); cr = cos(rph[0]);
    sp = sin(rph[1]); cp = cos(rph[1]);
    sh = sin(rph[2]); ch = cos(rph[2]);
    const double data[] = {  ch*cp, -sh*cr + ch*sp*sr,  sh*sr + ch*sp*cr,
                             sh*cp,  ch*cr + sh*sp*sr, -ch*sr + sh*sp*cr,
                            -sp,     cp*sr,             cp*cr             };
    memcpy (R, data, 9 * sizeof(double));
}

/*
%ROT2RPH  Convert rotation matrix into Euler roll,pitch,heading.
%   RPH = ROT2RPH(R) computes 3-vector of Euler angles
%   [roll,pitch,heading] from [3x3] rotation matrix R.  Angles are
%   measured in radians.
%
*/
static inline void
so3_rot2rph (const double R[9], double rph[3])
{
#define R(i,j) (R[i*3+j])
    // heading
    rph[2] = fatan2 (R(1,0), R(0,0));
    double sh, ch;
    fsincos (rph[2], &sh, &ch);

    // pitch
    rph[1] = fatan2 (-R(2,0), R(0,0)*ch + R(1,0)*sh);

    // roll
    rph[0] = fatan2 (R(0,2)*sh - R(1,2)*ch, -R(0,1)*sh + R(1,1)*ch);
#undef R
}

static inline void
so3_rot2rph_gsl (const gsl_matrix *R, gsl_vector *rph)
{
    assert (R->size1==3 && R->size2==3 && R->tda==3 && rph->size==3);
    so3_rot2rph (R->data, rph->data);
}

/*
% DROTX  Compute the derivative of a rotation matrix about the X-axis.
%   dRx = DROTZ(PHI) returns [3x3] rotation matrix dRx.  Note: PHI
%   is measured in radians.  PHI measures orientation of coordinate
%   frame 2 relative to coordinate frame 1.
*/
static inline void
so3_drotx (double dRx[9], const double phi)
{
    double s, c;
    fsincos (phi, &s, &c);
    const double data[9] = { 0,  0,  0,
                             0, -s,  c,
                             0, -c, -s };
    memcpy (dRx, data, 9 * sizeof(double));
}

static inline void
so3_drotx_gsl (gsl_matrix *dRx, const double phi)
{
    assert (dRx->size1==3 && dRx->size2==3 && dRx->tda==3);
    so3_drotx (dRx->data, phi);
}

/*
% DROTY  Compute the derivative of a rotation matrix about the Y-axis.
%   dRy = DROTY(THETA) returns [3x3] rotation matrix dRy.  Note: THETA
%   is measured in radians.  THETA measures orientation of coordinate
%   frame 2 relative to coordinate frame 1.
*/
static inline void
so3_droty (double dRy[9], const double theta)
{
    double s, c;
    fsincos (theta, &s, &c);
    const double data[9] = { -s,  0, -c,
                              0,  0,  0,
                              c,  0, -s };
    memcpy (dRy, data, 9 * sizeof(double));
}

static inline void
so3_droty_gsl (gsl_matrix *dRy, const double theta)
{
    assert (dRy->size1==3 && dRy->size2==3 && dRy->tda==3);
    so3_droty (dRy->data, theta);
}

/*
% DROTZ  Compute the derivative of a rotation matrix about the Z-axis.
%   dRz = DROTZ(PSI) returns [3x3] rotation matrix dRz.  Note: PSI
%   is measured in radians.  PSI measures orientation of coordinate
%   frame 2 relative to coordinate frame 1.
*/
static inline void
so3_drotz (double dRz[9], const double psi)
{
    double s, c;
    fsincos (psi, &s, &c);
    const double data[9] = { -s,  c,  0,
                             -c, -s,  0,
                              0,  0,  0 };
    memcpy (dRz, data, 9 * sizeof(double));
}

static inline void
so3_drotz_gsl (gsl_matrix *dRz, const double psi)
{
    assert (dRz->size1==3 && dRz->size2==3 && dRz->tda==3);
    so3_drotz (dRz->data, psi);
}

/*
% QUAT2ROT quaternion to rotation matrix.
%   R = QUAT2ROT(q) takes a 4-vector unit quaternion reprsented by q,
%   (i.e. q = [q0;qx;qy;qz]) and returns the corresponding [3 x 3]
%   orthonormal rotation matrix R.
*/
static inline void
so3_quat2rot (const double q[4], double R[9])
{
    const double q0 = q[0], qx = q[1], qy = q[2], qz = q[3];

    const double q0q0 = q0*q0;
    const double qxqx = qx*qx;
    const double qyqy = qy*qy;
    const double qzqz = qz*qz;

    const double q0qx = q0*qx;
    const double q0qz = q0*qz;
    const double q0qy = q0*qy;
    const double qxqy = qx*qy;
    const double qxqz = qx*qz;
    const double qyqz = qy*qz;

    const double data[9] =
        { q0q0 + qxqx - qyqy - qzqz,  2*(qxqy - q0qz),            2*(qxqz + q0qy),
          2*(qxqy + q0qz),            q0q0 - qxqx + qyqy - qzqz,  2*(qyqz - q0qx),
          2*(qxqz - q0qy),            2*(qyqz + q0qx),            q0q0 - qxqx - qyqy + qzqz };
    memcpy (R, data, 9 * sizeof(double));
}

static inline void
so3_quat2rot_gsl (const gsl_vector *q, gsl_matrix *R)
{
    assert (q->size==4 && R->size1==3 && R->size2==3 && R->tda==3);
    so3_quat2rot (q->data, R->data);
}

/*
% ROT2QUAT rotation matrix to quaternion.
%   q = ROT2QUAT(R) returns a 4-vector unit quaternion reprsented by q,
%   (i.e. q = [q0;qx;qy;qz]) corresponding to the [3 x 3]
%   orthonormal rotation matrix R.
*/
static inline void
so3_rot2quat (const double R[9], double q[4])
{
    // read
    const double r00 = R[0], r01 = R[1], r02 = R[2];
    const double r10 = R[3], r11 = R[4], r12 = R[5];
    const double r20 = R[6], r21 = R[7], r22 = R[8];

    const double tr = r00 + r11 + r22;

    double qw, qx, qy, qz;
    if (tr > 0) {
        const double S = sqrt (tr+1.0) * 2; // S=4*qw
        qw = 0.25 * S;
        qx = (r21 - r12) / S;
        qy = (r02 - r20) / S;
        qz = (r10 - r01) / S;
    } else if ((r00 > r11) && (r00 > r22)) {
        const double S = sqrt (1.0 + r00 - r11 - r22) * 2; // S=4*qx
        qw = (r21 - r12) / S;
        qx = 0.25 * S;
        qy = (r01 + r10) / S;
        qz = (r02 + r20) / S;
    } else if (r11 > r22) {
        const double S = sqrt (1.0 + r11 - r00 - r22) * 2; // S=4*qy
        qw = (r02 - r20) / S;
        qx = (r01 + r10) / S;
        qy = 0.25 * S;
        qz = (r12 + r21) / S;
    } else {
        const double S = sqrt (1.0 + r22 - r00 - r11) * 2; // S=4*qz
        qw = (r10 - r01) / S;
        qx = (r02 + r20) / S;
        qy = (r12 + r21) / S;
        qz = 0.25 * S;
    }

    // write
    q[0] = qw; q[1] = qx; q[2] = qy; q[3] = qz;
}

static inline void
so3_rot2quat_gsl (const gsl_matrix *R, gsl_vector *q)
{
    assert (q->size==4 && R->size1==3 && R->size2==3 && R->tda==3);
    so3_rot2quat (R->data, q->data);
}

/*
% QUAT2RPH converts unit quaternion to Euler RPH.
%   rph = QUAT2RPH(q) returns a [3 x 1] Euler xyz representation
%   equivalent to the [4 x 1] unit quaternion (provided q is
%   not near an Euler singularity).
*/
static inline void
so3_quat2rph (const double q[4], double rph[3])
{
    double R[9];
    so3_quat2rot (q, R);
    so3_rot2rph (R, rph);
}

static inline void
so3_quat2rph_gsl (const gsl_vector *q, gsl_vector *rph)
{
    assert (q->size==4 && rph->size==4);
    so3_quat2rph (q->data, rph->data);
}

/*
% RPH2QUAT converts Euler RPH to unit quaternion.
%   q = RPH2QUAT(R) converts [3 x 1] Euler xyz representation
%   to an equivalent [4 x 1] unit quaternion (provided R is
%   not near an Euler singularity).
*/
static inline void
so3_rph2quat (const double rph[3], double q[4])
{
    double R[9];
    so3_rotxyz (R, rph);
    so3_rot2quat (R, q);
}

static inline void
so3_rph2quat_gsl (const gsl_vector *rph, gsl_vector *q)
{
    assert (q->size==4 && rph->size==4);
    so3_rph2quat (rph->data, q->data);
}

/*
% function [rph_dot,J] = body2euler(pqr,rph)
% BODY2EULER converts body frame angular rates to Euler rates.
%   RPH_DOT = BODY2EULER(PQR,RPH) converts body frame angular rates PQR to
%   Euler angular rates RPH_DOT given Euler angles RPH.
%
%   [RPH_DOT,J] = BODY2EULER(PQR,RPH) also returns the [3 x 6] Jacobian J
%   associated with the transformation where J = [J_pqr, J_rph].
%   see body2euler.m for cleaner notation
*/
static inline void
so3_body2euler (const double pqr[3], const double rph[3],
                double rph_dot[3], double J[18]) {

    double p = pqr[0];
    double q = pqr[1];
    double r = pqr[2];

    double sr, cr;
    fsincos (rph[0], &sr, &cr);
    double sp, cp;
    fsincos (rph[1], &sp, &cp);
    double tp = ftan (rph[1]);
    double kp = 1.0/cp;

    //Euler angular rates
    rph_dot[0] = p+sr*tp*q+cr*tp*r;
    rph_dot[1] = cr*q-sr*r;
    rph_dot[2] = sr*kp*q+cr*kp*r;

    if (J != NULL) {
        // Jacobian wrt
        // first row
        J[0] = 1;                   J[1] = sr*tp;                   J[2] = cr*tp;
        J[3] = cr*tp*q-sr*tp*r;     J[4] = sr*kp*kp*q+cr*kp*kp*r;    J[5] = 0;
        // second row
        J[6] = 0;                   J[7] = cr;                      J[8] = -sr;
        J[9] = -sr*q-cr*r;          J[10] = 0;                      J[11] = 0;
        // third row
        J[12] = 0;                  J[13] = sr*kp;                  J[14] = cr*kp;
        J[15] = cr*kp*q-sr*kp*r;    J[16] = sr*kp*tp*q+cr*kp*tp*r;  J[17] = 0;
    }
}

/*
% function [pqr,J] = euler2body(rph_dot,rph)
%   converts euler rates to body frame angular rates
%
%   if not null also returns the [3 x 6] Jacobian J
%   associated with the transformation where J = [rph_dot, J_rph].
%   see body2euler.m for cleaner notation
*/
static inline void
so3_euler2body (const double rph_dot[3], const double rph[3],
                double pqr[3], double J[18]) {

    double rd = rph_dot[0];
    double pd = rph_dot[1];
    double hd = rph_dot[2];

    double sr, cr;
    fsincos (rph[0], &sr, &cr);
    double sp, cp;
    fsincos (rph[1], &sp, &cp);

    //body rates
    pqr[0] = -sp*hd + rd;
    pqr[1] = sr*cp*hd + cr*pd;
    pqr[2] = cr*cp*hd - sr*pd;

    if (J != NULL) {
        // Jacobian wrt
        // first row
        J[0] = 1;                   J[1] = 0;                   J[2] = -sp;
        J[3] = 0;                   J[4] = -cp*hd;              J[5] = 0;
        // second row
        J[6] = 0;                   J[7] = cr;                  J[8] = sr*cp;
        J[9] = cr*cp*hd-sr*pd;      J[10] = sr*-sp*hd;          J[11] = 0;
        // third row
        J[12] = 0;                  J[13] = sr;                 J[14] = cr*cp;
        J[15] = -sr*cp*hd-cr*pd;    J[16] = cr*+sp*hd;          J[17] = 0;
    }
}

static inline void
so3_body2euler_gsl (const gsl_vector *pqr, const gsl_vector *rph,
                    gsl_vector *rph_dot, gsl_matrix *J) {

    assert (pqr->size==3 && rph->size==3 && rph_dot->size==3
            && J->size1==3 && J->size2==6 && J->tda==6);

    so3_body2euler (pqr->data, rph->data, rph_dot->data, J->data);

}

#ifdef __cplusplus
}
#endif

#endif // __MATH_SO3_H__


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

double
f(double x, void * params)
{
  int m = *(int *) params;
  double f = gsl_pow_int(x, m) + 1.0;
  return f;
}

int
main (int argc, char *argv[])
{
  gsl_integration_fixed_workspace * w;
  const gsl_integration_fixed_type * T = gsl_integration_fixed_hermite;
  int m = 10;
  int n = 6;
  double expected, result;
  gsl_function F;

  if (argc > 1)
    m = atoi(argv[1]);

  if (argc > 2)
    n = atoi(argv[2]);

  w = gsl_integration_fixed_alloc(T, n, 0.0, 1.0, 0.0, 0.0);

  F.function = &f;
  F.params = &m;

  gsl_integration_fixed(&F, &result, w);

  if (m % 2 == 0)
    expected = M_SQRTPI + gsl_sf_gamma(0.5*(1.0 + m));
  else
    expected = M_SQRTPI;

  printf ("m               = %d\n", m);
  printf ("intervals       = %zu\n", gsl_integration_fixed_n(w));
  printf ("result          = % .18f\n", result);
  printf ("exact result    = % .18f\n", expected);
  printf ("actual error    = % .18f\n", result - expected);

  gsl_integration_fixed_free (w);

  return 0;
}


#include <math.h>
#include <stdio.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_cblas.h>

#include "motionControl.h"

void printMatrix(gsl_matrix* m, int row, int col) {
    for(int i = 0; i < row; i++) {
        for(int j = 0; j < col; j++) {
            printf("%g ", gsl_matrix_get(m, i, j));
        }
        printf("\n");
    }
}

gsl_matrix* measurementMatrix() {
    gsl_matrix *m = gsl_matrix_alloc (3, 3);

    gsl_matrix_set (m, 0, 0, -.5);
    gsl_matrix_set (m, 0, 1, .866);
    gsl_matrix_set (m, 0, 2, (.866*6.9282 - -.5*4));
    gsl_matrix_set (m, 1, 0, -.5);
    gsl_matrix_set (m, 1, 1, -.866);
    gsl_matrix_set (m, 1, 2, (.866*6.9282 - -.5*4));
    gsl_matrix_set (m, 2, 0, 1);
    gsl_matrix_set (m, 2, 1, 0);
    gsl_matrix_set (m, 2, 2, 8);

    return m;
}

gsl_matrix* rotationMatrix(float omegaWorld, float omegaBody) {
    gsl_matrix *m = gsl_matrix_alloc (3, 3);

    float theta = omegaWorld - omegaBody; // TODO check if radians or degrees

    gsl_matrix_set (m, 0, 0, cos(theta));
    gsl_matrix_set (m, 0, 1, sin(theta));
    gsl_matrix_set (m, 0, 2, 0);
    gsl_matrix_set (m, 1, 0, -sin(theta));
    gsl_matrix_set (m, 1, 1, cos(theta));
    gsl_matrix_set (m, 1, 2, 0);
    gsl_matrix_set (m, 2, 0, 0);
    gsl_matrix_set (m, 2, 1, 0);
    gsl_matrix_set (m, 2, 2, 1);

    return m;
}

gsl_matrix* pointToVelocity(float cX, float cY, float cOmega, float dX, float dY, float dOmega, float time) { //d stands for desired
    gsl_matrix *m = gsl_matrix_alloc (3, 1);

    float vX = (dX - cX) / time;
    float vY = (dY - cY) / time;
    float omega = (dOmega - cOmega) / time;

    gsl_matrix_set (m, 0, 0, vX);
    gsl_matrix_set (m, 1, 0, vY);
    gsl_matrix_set (m, 2, 0, omega);

    return m;
}

gsl_matrix* linearToAngular(gsl_matrix* R, gsl_matrix* M, gsl_matrix* V) {
    gsl_matrix_transpose(R);

    int signum;

    gsl_matrix * inverseR = gsl_matrix_alloc (3, 3);
    gsl_permutation * perm = gsl_permutation_alloc (3);

    gsl_linalg_LU_decomp (M, perm, &signum);
    gsl_linalg_LU_invert (M, perm, inverseR);

    gsl_matrix_mul_elements (inverseR, M);
    gsl_matrix* result = inverseR;

    float omega1 = rowSum(result, V, 0);
    float omega2 = rowSum(result, V, 1);
    float omega3 = rowSum(result, V, 2);

    gsl_matrix * final = gsl_matrix_alloc (3, 1);
    gsl_matrix_set (final, 0, 0, omega1);
    gsl_matrix_set (final, 1, 0, omega2);
    gsl_matrix_set (final, 2, 0, omega3);

    gsl_matrix_free(inverseR);
    gsl_permutation_free(perm);
    return final;
}

float rowSum(gsl_matrix* result, gsl_matrix* V, int row) {
    float sum = 0;
    for(int i = 0; i < 3; i++) {
        sum += gsl_matrix_get(result, row, i) * gsl_matrix_get(V, i, 0);
    }
    return sum;
}

int main (void) {
    gsl_matrix* M = measurementMatrix();
    gsl_matrix* R = rotationMatrix(1, 2);
    gsl_matrix* V = pointToVelocity(0, 0, 0, 1, 1, 0, 1);

    gsl_matrix* final = linearToAngular(R, M, V);

    printMatrix(final, 3, 1);
    gsl_matrix_free (M);
    gsl_matrix_free (R);
    gsl_matrix_free (V);
    gsl_matrix_free (final);

    return 0;
}


#ifndef _INC_REGRESSION_
#define _INC_REGRESSION_
/**
 * contains the functions needed to handle the linear regression
 * this entails, creating hvectors (for given x vectors in the parameter space)
 * estimatingBeta, this needs the inverse covariance matrix and the training data
 * creating hmatrix, a done once kind of job, slap together hvectors for each of the design points
 */

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

/**
 * \brief creates a vector of regression coefficients at x_location
 *
 * create a h_vector evaluated at given x_location.,
 * this currently only creates a set of linear basis fns, you could add another loop which
 * would add squares etc
 *
 * on calling h_vector should be initliased to a vector of length nregression_fns which
 * in the default case nregression_fns = nparams + 1
 *
 * the value of options->nregression_fns is going to be very important for this
 *
 * this pointer is set to one of the fns in this file by optstruct.c:setup_regression
 */

void (*makeHVector)(gsl_vector *h_vector, gsl_vector *x_location, int nparams);

//void makeHVector(gsl_vector *h_vector, gsl_vector *x_location, int nparams);

void makeHMatrix(gsl_matrix *h_matrix, gsl_matrix *xmodel, int nmodel_points, int nparams, int nregresion_fns);

void makeHMatrix_fnptr(gsl_matrix *h_matrix, gsl_matrix *xmodel, int nmodel_points, int nparams, int nregression_fns,
											 void (*makeHVector_ptr)(gsl_vector *h_vector, gsl_vector *x_location, int nparams));

void estimateBeta(gsl_vector *beta_vector, gsl_matrix *h_matrix, gsl_matrix* cinverse, gsl_vector *trainingvector, int nmodel_points, int nregression_fns);

void makeHVector_linear(	gsl_vector *h_vector, gsl_vector *x_location, int nparams);
void makeHVector_quadratic( gsl_vector *h_vector, gsl_vector *x_location, int nparams);
void makeHVector_trivial(gsl_vector *h_vector, gsl_vector *x_location, int nparams);
void makeHVector_cubic( gsl_vector *h_vector, gsl_vector *x_location, int nparams);
#endif


#pragma once

#include <list>

#include <gsl/gsl_math.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>

#include "../../../Utils/Random.h"
#include "../../Expression.h"
#include "../RandomDistributionDirectory.h"

double rand_Gauss(const std::list<expression>& args, const Variables& vars){
    if (args.size() == 1){
        double sigma = args.front()->value(vars);
        return gsl_ran_gaussian(rng.get(), sigma);
    }
    return GSL_NAN;
}

double pdf_Gauss(double x, const std::list<expression>& args, const Variables& vars){
    if (args.size() == 1){
        double sigma = args.front()->value(vars);
        return gsl_ran_gaussian_pdf(x, sigma);
    }
    return GSL_NAN;
}

double cdf_P_Gauss(double x, const std::list<expression>& args, const Variables& vars){
    if (args.size() == 1){
        double sigma = args.front()->value(vars);
        return gsl_cdf_gaussian_P(x, sigma);
    }
    return GSL_NAN;
}

double cdf_Q_Gauss(double x, const std::list<expression>& args, const Variables& vars){
    if (args.size() == 1){
        double sigma = args.front()->value(vars);
        return gsl_cdf_gaussian_Q(x, sigma);
    }
    return GSL_NAN;
}

double cdf_Pinv_Gauss(double x, const std::list<expression>& args, const Variables& vars){
    if (args.size() == 1){
        double sigma = args.front()->value(vars);
        return gsl_cdf_gaussian_Pinv(x, sigma);
    }
    return GSL_NAN;
}

double cdf_Qinv_Gauss(double x, const std::list<expression>& args, const Variables& vars){
    if (args.size() == 1){
        double sigma = args.front()->value(vars);
        return gsl_cdf_gaussian_Qinv(x, sigma);
    }
    return GSL_NAN;
}


#ifndef __EMULATOR_INC_
#define __EMULATOR_INC_
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_sf.h>

/**
 * the fn ptr to the covariance function, this is the most called function in libEmu
 * you can change this when you setup the optstruct.
 */
double (*covariance_fn)(gsl_vector*, gsl_vector*, gsl_vector*, int, int) ;

void print_matrix(gsl_matrix* m, int nx, int ny);

double covariance_fn_gaussian(gsl_vector *xm, gsl_vector* xn, gsl_vector* thetas, int nthetas, int nparams);

void derivative_l_gauss(gsl_matrix *dCdTheta, gsl_matrix* xmodel,
												double thetaLength, int index, int nmodel_points, int nparams);

// same as above but without clamping on small values
double covariance_fn_gaussian_exact(gsl_vector *xm, gsl_vector* xn, gsl_vector* thetas, int nthetas, int nparams);

double covariance_fn_matern_three(gsl_vector *xm, gsl_vector* xn, gsl_vector* thetas, int nthetas, int nparams);

void derivative_l_matern_three(gsl_matrix *dCdTheta, gsl_matrix* xmodel, double thetaLength,
															  int index, int nmodel_points, int nparams);

double covariance_fn_matern_five(gsl_vector *xm, gsl_vector* xn, gsl_vector* thetas, int nthetas, int nparams);

void derivative_l_matern_five(gsl_matrix *dCdTheta, gsl_matrix* xmodel, double thetaLength,
															int index, int nmodel_points, int nparams);

void makeKVector(gsl_vector* kvector, gsl_matrix *xmodel, gsl_vector *xnew, gsl_vector* thetas, int nmodel_points, int nthetas, int nparams);

void makeKVector_fnptr(gsl_vector* kvector, gsl_matrix *xmodel, gsl_vector *xnew, gsl_vector* thetas, int nmodel_points, int nthetas, int nparams,
											 double (*covariance_fn_ptr)(gsl_vector*, gsl_vector*, gsl_vector*, int, int));

void makeCovMatrix(gsl_matrix *cov_matrix, gsl_matrix *xmodel, gsl_vector* thetas, int nmodel_points, int nthetas, int nparams);

void makeCovMatrix_fnptr(gsl_matrix *cov_matrix, gsl_matrix *xmodel, gsl_vector* thetas, int nmodel_points, int nthetas, int nparams,
												 double (*covariance_fn_ptr)(gsl_vector*, gsl_vector*, gsl_vector*, int, int));

double makeEmulatedMean(gsl_matrix *inverse_cov_matrix, gsl_vector *training_vector, gsl_vector *kplus_vector, gsl_vector* h_vector, gsl_matrix* h_matrix, gsl_vector* beta_vector,  int nmodel_points);

double makeEmulatedVariance(gsl_matrix *inverse_cov_matrix, gsl_vector *kplus_vector, gsl_vector *h_vector, gsl_matrix *h_matrix, double kappa, int nmodel_points, int nregression_fns);

void initialise_new_x(gsl_matrix* new_x, int nparams, int nemulate_points, double emulate_min, double emulate_max);

/* deprecated */
/* double covariance_fn_gaussian_nondiag(gsl_vector* xm, gsl_vector*xn, gsl_vector*thetas, int nthetas, int nparams); */
/* double covariance_fn_matern(gsl_vector *xm, gsl_vector* xn, gsl_vector* thetas, int nthetas, int nparams); */

#endif


#ifndef LOGSUMEXP_INCLUDED
#define LOGSUMEXP_INCLUDED

#include <limits>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <cmath>
#include "vguard.h"

/* uncomment to disable lookup table */
/*
#define LOG_SUM_EXP_SLOW
*/

/* uncomment to catch NaN errors */
/*
#define NAN_DEBUG
*/

#define LOG_SUM_EXP_LOOKUP_MAX 10
#define LOG_SUM_EXP_LOOKUP_PRECISION .0001

#define LOG_SUM_EXP_LOOKUP_ENTRIES (((int) (LOG_SUM_EXP_LOOKUP_MAX / LOG_SUM_EXP_LOOKUP_PRECISION)) + 1)

/* comment to disable interpolation */
#define LOG_SUM_EXP_INTERPOLATE

namespace MachineBoss {

using namespace std;

typedef double LogProb;

double log_sum_exp_unary_slow (double x);  /* does not use lookup table */

struct LogSumExpLookupTable {
  double *lookup;
  LogSumExpLookupTable();
  ~LogSumExpLookupTable();
};

extern LogSumExpLookupTable logSumExpLookupTable;

inline double log_sum_exp_unary (double x) {
  /* returns log(1 + exp(-x)) for nonnegative x */
#ifdef LOG_SUM_EXP_SLOW
  return log_sum_exp_unary_slow(x);
#else /* LOG_SUM_EXP_SLOW */
  if (x >= LOG_SUM_EXP_LOOKUP_MAX || std::isnan(x) || std::isinf(x))
    return 0;
  if (x < 0) {  /* really dumb approximation for x < 0. Should never be encountered, so issue a warning */
    cerr << "Called log_sum_exp_unary(x) for negative x = " << x << endl;
    return -x;
  }
  const int n = (int) (x / LOG_SUM_EXP_LOOKUP_PRECISION);
  const double f0 = logSumExpLookupTable.lookup[n];
#ifdef LOG_SUM_EXP_INTERPOLATE
  const double dx = x - (n * LOG_SUM_EXP_LOOKUP_PRECISION);
  const double f1 = logSumExpLookupTable.lookup[n+1];
  const double df = f1 - f0;
  return f0 + df * (dx / LOG_SUM_EXP_LOOKUP_PRECISION);
#else /* LOG_SUM_EXP_INTERPOLATE */
  return f0;
#endif /* LOG_SUM_EXP_INTERPOLATE */
#endif /* LOG_SUM_EXP_SLOW */
}

inline double log_sum_exp (double a, double b) {
  /* returns log(exp(a) + exp(b)) */
  double max, diff, ret;
  // Note: Infinity plus or minus a finite quantity is still Infinity,
  // but Infinity - Infinity = NaN.
  // Thus, we are susceptible to NaN errors when trying to add 0+0 in log-space.
  // To work around this, we explicitly test for a==b.
  if (a == b) { max = a; diff = 0; }
  else if (a < b) { max = b; diff = b - a; }
  else { max = a; diff = a - b; }
  ret = max + log_sum_exp_unary (diff);
#if defined(NAN_DEBUG)
  if (std::isnan(ret)) {
    cerr << "NaN error in log_sum_exp" << endl;
    throw;
  }
#endif
  return ret;
}

inline double log_sum_exp (double a, double b, double c) {
    return log_sum_exp (log_sum_exp (a, b), c);
}

inline double log_sum_exp (double a, double b, double c, double d) {
    return log_sum_exp (log_sum_exp (log_sum_exp (a, b), c), d);
}

inline double log_accum_exp (double& a, double b) {
  a = log_sum_exp (a, b);
  return a;
}

inline double log_sum_exp (double a, double b, double c, double d, double e) {
    return log_sum_exp (log_sum_exp (log_sum_exp (log_sum_exp (a, b), c), d), e);
}

inline double log_sum_exp (const vguard<double>& v) {
  double lpTot = -numeric_limits<double>::infinity();
  for (auto lp : v)
    (void) log_accum_exp (lpTot, lp);
  return lpTot;
}

inline double log_sum_exp (const vguard<vguard<double> >& v) {
  double lpTot = -numeric_limits<double>::infinity();
  for (auto lp : v)
    (void) log_accum_exp (lpTot, log_sum_exp (lp));
  return lpTot;
}

double log_sum_exp_slow (double a, double b);  /* does not use lookup table */
double log_sum_exp_slow (double a, double b, double c);
double log_sum_exp_slow (double a, double b, double c, double d);

double log_accum_exp_slow (double& a, double b);

inline double log_subtract_exp (double a, double b) {
  if (a < b) {
    cerr << "Sign error in log_subtract_exp" << endl;
    throw;
  }
  return a + log (1. - exp(b-a));
}

vguard<LogProb> log_vector (const vguard<double>& v);
vguard<vguard<LogProb> > log_matrix (const vguard<vguard<double> >& m);

vguard<LogProb> log_gsl_vector (const gsl_vector* v);
vguard<double> gsl_vector_to_stl (const gsl_vector* v);

vguard<vguard<LogProb> > log_vector_gsl_vector (const vguard<const gsl_vector*>& v);

vguard<vguard<double> > gsl_matrix_to_stl (const gsl_matrix* m);
gsl_matrix* stl_to_gsl_matrix (const vguard<vguard<double> >& m);

inline LogProb logInnerProduct (const vguard<LogProb>& v1, const vguard<LogProb>& v2) {
  LogProb lip = -numeric_limits<double>::infinity();
  for (vguard<LogProb>::const_iterator iter1 = v1.begin(), iter2 = v2.begin(); iter1 != v1.end(); ++iter1, ++iter2)
    lip = log_sum_exp (lip, *iter1 + *iter2);
  return lip;
}

inline LogProb logInnerProduct (const vguard<LogProb>& v1, const vguard<LogProb>& v2, const vguard<LogProb>& v3) {
  LogProb lip = -numeric_limits<double>::infinity();
  for (vguard<LogProb>::const_iterator iter1 = v1.begin(), iter2 = v2.begin(), iter3 = v3.begin(); iter1 != v1.end(); ++iter1, ++iter2, ++iter3)
    lip = log_sum_exp (lip, *iter1 + *iter2 + *iter3);
  return lip;
}

inline LogProb logInnerProduct (const vguard<vguard<LogProb> >& v1, const vguard<vguard<LogProb> >& v2) {
  LogProb lip = -numeric_limits<double>::infinity();
  for (vguard<vguard<LogProb> >::const_iterator iter1 = v1.begin(), iter2 = v2.begin(); iter1 != v1.end(); ++iter1, ++iter2)
    lip = log_sum_exp (lip, logInnerProduct (*iter1, *iter2));
  return lip;
}

double logBetaPdf (double prob, double yesCount, double noCount);  // alpha = yesCount+1, beta = noCount+1
double logGammaPdf (double rate, double eventCount, double waitTime);  // alpha = shape = eventCount+1, beta = rate = 1/scale = 1/theta = waitTime
double logDirichletPdf (const vguard<double>& prob, const vguard<double>& count);  // alpha[n] = count[n] + 1
double logGaussianPdf (double x, double mu, double sigma);

}  // end namespace

#endif /* LOGSUMEXP_INCLUDED */


/*
 *  Created on: Oct. 19, 2019
 *      Author: Govind
 */

/*
 * standard includes */
	#include <stdio.h>
	#include <stdlib.h>

/*
 *  includes for GSL components
 *  	- use double precision
 */
	#include <gsl/gsl_vector_double.h>
	#include <gsl/gsl_matrix_double.h>
	#include <gsl/gsl_rng.h>
	#include <gsl/gsl_randist.h>

/*
 * FUNCTIONS
 */

/*
 * simple Fibonacci sequence generator function, using recursion
 * */
size_t fib(size_t k)
{
	if (k==0)
	{
		return 0;
	}
	else if (k==1)
	{
		return 1;
	}
	else /* k >= 2 */
	{
		return fib(k-1) + fib(k-2);
	}
}

gsl_matrix *embt_mm(const gsl_matrix *U, const gsl_matrix *V, size_t N)
{
	gsl_matrix *W = gsl_matrix_alloc(N,N);
	double dp;
	double uk,vk;

	for (size_t i=0; i != N; ++i)
	{
		for (size_t j=0; j != N; ++j)
		{
			/* compute element (i,j) of W */
			dp = 0;
			for (size_t k=0; k != N; ++k)
			{
				uk = gsl_matrix_get(U,i,k);
				vk = gsl_matrix_get(V,k,j);
				dp += uk*vk;
			}
			gsl_matrix_set(W,i,j,dp);
		}
	}
	return W;
}

gsl_vector gsl_print_vector(const gsl_vector V, size_t N)
{
	  /* Print the vector*/
	   for (size_t i = 0; i < N; i++)
	   {
	       printf ("v_%d = %g\n", i, gsl_vector_get (V, i));
	   }
}
gsl_matrix embt_print_matrix(const gsl_matrix V, size_t N)
{
	printf("Matrix m\n");
	  for (size_t i=0;i!=N;i++)
	    {
	      for (size_t j=0;j!=N;j++)
		{
		  printf("%f ",gsl_matrix_get(V,i,j));
		}
	      printf("\n");
	    }
	  printf("\n");
}
int main()
{
	/*
	 * INITIALIZE PARAMETERS
	 */
		/* vectors parameters */
		size_t		N=10; /* index type, vector sizes */
		gsl_vector *a = gsl_vector_alloc(N); /* allocate vector from heap of size N */
		gsl_vector *b = gsl_vector_alloc(N); /* allocate vector from heap of size N */
		gsl_vector *c = gsl_vector_calloc(N); /* allocate vector of size N but initialize entries to zero */

		/* random number generator parameters */
		const gsl_rng_type *T;
		gsl_rng *r; /* handle for our random number generator */

		/* matrix parameters */
		gsl_matrix *A = gsl_matrix_alloc(N,N);
		gsl_matrix *B = gsl_matrix_alloc(N,N);
		gsl_matrix *C = gsl_matrix_calloc(N,N);

	/*
	 * SET UP RANDOM NUMBER GENERATION
	 */
		gsl_rng_env_setup();
		T = gsl_rng_default;
		r = gsl_rng_alloc(T);

	/*
	 *  VECTOR OPERATIONS
	 */
		/* set the vector elements */
		for (size_t i = 0; i != N; ++i)
		{
			gsl_vector_set(a, i, fib(i)); /* set element i of vector a to Fibonacci number i */
			gsl_vector_set(b, i, gsl_ran_flat(r,-1.0,+1.0)); /* set element of vector b to random number */
		}

		/* c = a + b */
		gsl_vector_add(c, a); /* c += a */
		gsl_vector_add(c, b); /* c += b */

		/* print results */
		for (size_t i = 0; i != N; ++i)
		{
			printf("i=%d, a(i)=%f, b(i)=%f, c(i)=%f\n", i,
			gsl_vector_get(a, i),
			gsl_vector_get(b, i),
			gsl_vector_get(c, i));
		}

	/*
	 *	MATRIX OPERATIONS - your homework!! :)
	 */
		/* fill A with first N*N Fibonacci numbers, starting with row 1 (cols 1-10), then row 2, etc. */
		for (size_t i=0; i != N; ++i)
		{
			for (size_t j = 0; j != N; ++j)
			{
				gsl_matrix_set(A, i, j, (double) fib(j+i*N));
			}
		}

		/* fill B with N*N random numbers, uniformly distributed over the interval (-100, 100) */
		for (size_t i=0; i != N; ++i)
		{
			for (size_t j = 0; j != N; ++j)
			{
				gsl_matrix_set(B, i, j, gsl_ran_flat(r,-100.0,+100.0));
			}
		}

		/* make C the product of A and B */
		C = embt_mm(A,B,N);

		/* print the results */
		embt_print_vector(a,N);
		embt_print_vector(b,N);
		embt_print_vector(c,N);
		embt_print_matrix(A,N);
		embt_print_matrix(B,N);
		embt_print_matrix(C,N);

	/* de-allocate memory */
	gsl_vector_free(a);
	gsl_vector_free(b);
	gsl_vector_free(c);
	gsl_matrix_free(A);
	gsl_matrix_free(B);
	gsl_matrix_free(C);
	return EXIT_SUCCESS;
}