#ifndef DBJ_VARIOUS_SIMPLE_MATMULS_INC
#define DBJ_VARIOUS_SIMPLE_MATMULS_INC

// not using any kind of "matrix framework"

#include "../dbj_matmul_common.h" // DBJ_MATRIX_DATA_TYPE

#include <stdlib.h>
#include <intrin.h> // #define __SSE__ 1
#ifdef HAVE_CBLAS
#include <cblas.h>
#endif

#undef DBJ_API
#define DBJ_API

//////////////////////////////////////////////////////////////////////
#ifndef DBJ_SANITY_MAX_ROW_COUNT
#define DBJ_SANITY_MAX_ROW_COUNT 0xFFFF
#endif

#ifndef DBJ_SANITY_MAX_COL_COUNT
#define DBJ_SANITY_MAX_COL_COUNT 0xFFFF
#endif
//////////////////////////////////////////////////////////////////////

typedef void* (*simple_mat_mul_function)(
	const unsigned, const unsigned, const unsigned,
	DBJ_MATRIX_DATA_TYPE[][*],
	DBJ_MATRIX_DATA_TYPE[][*],
	DBJ_MATRIX_DATA_TYPE[][*] /* the result */
	);

typedef struct
{
	const char* description;
	simple_mat_mul_function function;
} simple_description_function_pair;

#define DF_PAIR(D_, F_) \
	(simple_description_function_pair) { .description = D_, .function = F_ }

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

DBJ_API DBJ_MATRIX_DATA_TYPE* new_simple_matrix(const unsigned, const unsigned);

DBJ_API DBJ_MATRIX_DATA_TYPE* make_simple_matrix(
	const unsigned, const unsigned, DBJ_MATRIX_DATA_TYPE(*)(void));

DBJ_API void* simple_mat_transpose(
	const unsigned, const unsigned, DBJ_MATRIX_DATA_TYPE[][*], DBJ_MATRIX_DATA_TYPE[][*]);

DBJ_API void* simple_mat_mul_null(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
	// used for when algorithms are not implemented yet
	(void)a;
	(void)b;
	(void)m;
	return NULL;
}

DBJ_API void* simple_mat_mul_0_0(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE(*ax)[n_a_cols],
	DBJ_MATRIX_DATA_TYPE(*bx)[n_a_cols],
	DBJ_MATRIX_DATA_TYPE(*mx)[n_a_cols]);

DBJ_API void* simple_mat_mul_0(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/]);

DBJ_API void* simple_mat_mul_1(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/]);

DBJ_API void* simple_mat_mul_3(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/]);

DBJ_API void* simple_mat_mul_4(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE[/*n_a_rows*/][* /*n_b_cols*/]);

#if __SSE__

DBJ_API void* simple_mat_mul_2(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE a[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE b[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE m[/*n_a_rows*/][* /*n_b_cols*/]);

DBJ_API void* simple_mat_mul_7(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE a[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE b[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE m[/*n_a_rows*/][* /*n_b_cols*/]);

#endif // __SSE__

#ifdef HAVE_CBLAS
DBJ_API void* simple_mat_mul_5(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE a[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE b[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE m[/*n_a_rows*/][* /*n_b_cols*/]);

DBJ_API void* simple_mat_mul_6(const unsigned /*n_a_rows*/, const unsigned /*n_a_cols*/, const unsigned /*n_b_cols*/,
	DBJ_MATRIX_DATA_TYPE a[/*n_a_rows*/][* /*n_a_cols*/], DBJ_MATRIX_DATA_TYPE b[/*n_a_rows*/][* /*n_b_cols*/], DBJ_MATRIX_DATA_TYPE m[/*n_a_rows*/][* /*n_b_cols*/]);
#endif // HAVE_CBLAS

const simple_description_function_pair dbj_simple_matmuls_algo_table[] =
{
	DF_PAIR("0: MAX optimization", simple_mat_mul_0_0),
	DF_PAIR("1: simple naive - no optimization", simple_mat_mul_0),
	DF_PAIR("2: simple transposing the second matrix", simple_mat_mul_1),
	DF_PAIR("3: simple explicitly vectorized sdot() with SSE", simple_mat_mul_2), /* requires __SSE__ */
	DF_PAIR("4: simple explicitly SSE sdot() plus loop tiling", simple_mat_mul_7) , /* requires __SSE__ */
	DF_PAIR("5: simple implicitly vectorized sdot()", simple_mat_mul_3),
	DF_PAIR("6: simple no vectorization hints", simple_mat_mul_4),
#ifdef HAVE_CBLAS
	DF_PAIR("7: simple with sdot() from an external CBLAS library", simple_mat_mul_5),
	DF_PAIR("8: simple with sgemm() from an external CBLAS library", simple_mat_mul_6),
#endif																	// ! HAVE_CBLAS
};

static const unsigned dbj_simple_matmuls_algo_table_size = (sizeof(dbj_simple_matmuls_algo_table) / sizeof(dbj_simple_matmuls_algo_table[0]));

//////////////////////////////////////////////////////////////////////
#ifdef DBJ_VARIOUS_SIMPLE_MATMULS_IMPLEMENTATION
//////////////////////////////////////////////////////////////////////

DBJ_API DBJ_MATRIX_DATA_TYPE* new_simple_matrix(const unsigned n_rows, const unsigned n_cols)
{
	DBJ_MATRIX_DATA_TYPE* retval = calloc(n_rows * n_cols, sizeof(DBJ_MATRIX_DATA_TYPE));

	if (!retval)
	{
		fprintf(stderr, "\n%s(%d) new_simple_matrix() failed?\n", __FILE__, __LINE__);
		perror(" ");
		exit(1);
	}
	return retval;
}

DBJ_API DBJ_MATRIX_DATA_TYPE* make_simple_matrix(
	const unsigned n_rows, const unsigned n_cols, DBJ_MATRIX_DATA_TYPE(*value_provider)(void))
{
	DBJ_MATRIX_DATA_TYPE* rezult = new_simple_matrix(n_rows, n_cols);

	typedef DBJ_MATRIX_DATA_TYPE(*matrix)[n_cols];
	matrix mx = (matrix)rezult;

	for (unsigned i = 0; i < n_rows; ++i)
		for (unsigned j = 0; j < n_cols; ++j)
			mx[i][j] = value_provider(); //
	return rezult;
}

DBJ_API void* simple_mat_transpose(
	const unsigned n_rows, const unsigned n_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_rows][n_cols], DBJ_MATRIX_DATA_TYPE m[static n_cols][n_rows])
{
	for (unsigned i = 0; i < n_rows; ++i)
		for (unsigned j = 0; j < n_cols; ++j)
			m[j][i] = a[i][j];
	return m;
}

DBJ_API DBJ_MATRIX_DATA_TYPE simple_sdot_1(int n, const DBJ_MATRIX_DATA_TYPE x[static n], const DBJ_MATRIX_DATA_TYPE y[static n])
{
	DBJ_MATRIX_DATA_TYPE s = 0.0f;
	for (int i = 0; i < n; ++i)
		s += x[i] * y[i];
	return s;
}

DBJ_API DBJ_MATRIX_DATA_TYPE simple_sdot_8(int n, const DBJ_MATRIX_DATA_TYPE x[static n], const DBJ_MATRIX_DATA_TYPE y[static n])
{
	int i, n8 = n >> 3 << 3;
	DBJ_MATRIX_DATA_TYPE s = 0.0f, t[8] = { 0.0f };
	// t[0] = t[1] = t[2] = t[3] = t[4] = t[5] = t[6] = t[7] = 0.0f;
	for (i = 0; i < n8; i += 8)
	{
		t[0] += x[i + 0] * y[i + 0];
		t[1] += x[i + 1] * y[i + 1];
		t[2] += x[i + 2] * y[i + 2];
		t[3] += x[i + 3] * y[i + 3];
		t[4] += x[i + 4] * y[i + 4];
		t[5] += x[i + 5] * y[i + 5];
		t[6] += x[i + 6] * y[i + 6];
		t[7] += x[i + 7] * y[i + 7];
	}
	for (s = 0.0f; i < n; ++i)
		s += x[i] * y[i];
	s += t[0] + t[1] + t[2] + t[3] + t[4] + t[5] + t[6] + t[7];
	return s;
}

#ifdef __SSE__

DBJ_API DBJ_MATRIX_DATA_TYPE simple_sdot_sse(int n, const DBJ_MATRIX_DATA_TYPE x[static n], const DBJ_MATRIX_DATA_TYPE y[static n])
{
	int i, n8 = n >> 3 << 3;
	__m128 vs1, vs2;
	DBJ_MATRIX_DATA_TYPE s, t[4];
	vs1 = _mm_setzero_ps();
	vs2 = _mm_setzero_ps();
	for (i = 0; i < n8; i += 8)
	{
		__m128 vx1, vx2, vy1, vy2;
		vx1 = _mm_loadu_ps(&x[i]);
		vx2 = _mm_loadu_ps(&x[i + 4]);
		vy1 = _mm_loadu_ps(&y[i]);
		vy2 = _mm_loadu_ps(&y[i + 4]);
		vs1 = _mm_add_ps(vs1, _mm_mul_ps(vx1, vy1));
		vs2 = _mm_add_ps(vs2, _mm_mul_ps(vx2, vy2));
	}
	for (s = 0.0f; i < n; ++i)
		s += x[i] * y[i];
	_mm_storeu_ps(t, vs1);
	s += t[0] + t[1] + t[2] + t[3];
	_mm_storeu_ps(t, vs2);
	s += t[0] + t[1] + t[2] + t[3];
	return s;
}
#endif // __SSE__

/**************************************************

 Various Matrix multiplication algorithms

 NOTE: check and choose for your RT Env
	   there are differences but are *very* dependant on
	   compiler, OS and hardware

 */

DBJ_API void* simple_mat_mul_0_0(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE(*ax)[n_a_cols],
	DBJ_MATRIX_DATA_TYPE(*bx)[n_a_cols],
	DBJ_MATRIX_DATA_TYPE(*mx)[n_a_cols])
{
	// runtime casting takes time
	const unsigned n_b_rows = n_a_cols;
	// DBJ_MATRIX_DATA_TYPE(*ax)[n_a_cols] = a;
	// DBJ_MATRIX_DATA_TYPE(*bx)[n_b_cols] = b;
	// DBJ_MATRIX_DATA_TYPE(*mx)[n_b_rows] = m;

	for (unsigned i = 0; i < n_a_rows; ++i)
	{
		for (unsigned j = 0; j < n_b_cols; ++j)
		{
			DBJ_MATRIX_DATA_TYPE t = 0.0;
			for (unsigned k = 0; k < n_a_cols; ++k)
				t += ax[i][k] * bx[k][j];
			mx[i][j] = t;
		}
	}
	return mx;
}

DBJ_API void* simple_mat_mul_0(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
	for (unsigned i = 0; i < n_a_rows; ++i)
	{
		for (unsigned j = 0; j < n_b_cols; ++j)
		{
			DBJ_MATRIX_DATA_TYPE t = 0.0;
			for (unsigned k = 0; k < n_a_cols; ++k)
				t += a[i][k] * b[k][j];
			m[i][j] = t;
		}
	}
	return m;
}

DBJ_API void* simple_mat_mul_1(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
	const unsigned n_b_rows = n_a_cols;

	DBJ_MATRIX_DATA_TYPE* Temp = new_simple_matrix(n_b_cols, n_b_rows);
	// Temp rows and cols are inverted !
	typedef DBJ_MATRIX_DATA_TYPE(*matrix)[n_b_rows];
	matrix bT = (matrix)Temp;
	(void)simple_mat_transpose(n_b_rows, n_b_cols, b, bT);

	for (unsigned i = 0; i < n_a_rows; ++i)
	{
		const DBJ_MATRIX_DATA_TYPE* ai = a[i];
		DBJ_MATRIX_DATA_TYPE* mi = m[i];
		for (unsigned j = 0; j < n_b_cols; ++j)
		{
			DBJ_MATRIX_DATA_TYPE t = 0.0f, * bTj = bT[j];
			for (unsigned k = 0; k < n_a_cols; ++k)
				t += ai[k] * bTj[k];
			mi[j] = t;
		}
	}

	free(Temp);

	return m;
}

DBJ_API void* simple_mat_mul_2(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
#ifndef __SSE__
#error __SEE__ is required here
#endif

	const unsigned n_b_rows = n_a_cols;

	DBJ_MATRIX_DATA_TYPE* Temp = new_simple_matrix(n_b_cols, n_b_rows);
	// Temp rows and cols are inverted !
	typedef DBJ_MATRIX_DATA_TYPE(*matrix)[n_b_rows];
	matrix bT = (matrix)Temp;
	(void)simple_mat_transpose(n_b_rows, n_b_cols, b, bT);

	for (unsigned i = 0; i < n_a_rows; ++i)
		for (unsigned j = 0; j < n_b_cols; ++j)
			m[i][j] = simple_sdot_sse(n_a_cols, a[i], bT[j]);
	free(Temp);
	return m;
}

DBJ_API void* simple_mat_mul_7(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
#ifndef __SSE__
#error __SEE__ is required here
#endif

	const unsigned x = 16, n_b_rows = n_a_cols;

	DBJ_MATRIX_DATA_TYPE* Temp = new_simple_matrix(n_b_cols, n_b_rows);
	// Temp rows and cols are inverted !
	typedef DBJ_MATRIX_DATA_TYPE(*matrix)[n_b_rows];
	matrix bT = (matrix)Temp;
	(void)simple_mat_transpose(n_b_rows, n_b_cols, b, bT);

	for (unsigned i = 0; i < n_a_rows; i += x)
	{
		for (unsigned j = 0; j < n_b_cols; j += x)
		{
			unsigned je = n_b_cols < j + x ? n_b_cols : j + x;
			unsigned ie = n_a_rows < i + x ? n_a_rows : i + x;
			for (unsigned ii = i; ii < ie; ++ii)
				for (unsigned jj = j; jj < je; ++jj)
					m[ii][jj] += simple_sdot_sse(n_a_cols, a[ii], bT[jj]);
		}
	}
	free(Temp);
	return m;
}

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

DBJ_API void* simple_mat_mul_3(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
	int n_b_rows = n_a_cols;

	DBJ_MATRIX_DATA_TYPE* Temp = new_simple_matrix(n_b_cols, n_b_rows);
	// Temp rows and cols are inverted !
	typedef DBJ_MATRIX_DATA_TYPE(*matrix)[n_b_rows];
	matrix bT = (matrix)Temp;
	(void)simple_mat_transpose(n_b_rows, n_b_cols, b, bT);

	for (unsigned i = 0; i < n_a_rows; ++i)
		for (unsigned j = 0; j < n_b_cols; ++j)
			m[i][j] = simple_sdot_8(n_a_cols, a[i], bT[j]);
	free(Temp);
	return m;
}

DBJ_API void* simple_mat_mul_4(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
	int n_b_rows = n_a_cols;

	DBJ_MATRIX_DATA_TYPE* Temp = new_simple_matrix(n_b_cols, n_b_rows);
	// Temp rows and cols are inverted !
	typedef DBJ_MATRIX_DATA_TYPE(*matrix)[n_b_rows];
	matrix bT = (matrix)Temp;
	(void)simple_mat_transpose(n_b_rows, n_b_cols, b, bT);

	for (unsigned i = 0; i < n_a_rows; ++i)
		for (unsigned j = 0; j < n_b_cols; ++j)
			m[i][j] = simple_sdot_1(n_a_cols, a[i], bT[j]);
	free(Temp);
	return m;
}

#ifdef HAVE_CBLAS

DBJ_API void* simple_mat_mul_5(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
	const unsigned n_b_rows = n_a_cols;

	DBJ_MATRIX_DATA_TYPE* Temp = new_simple_matrix(n_rows, n_cols);
	// Temp rows and cols are inverted !
	typedef DBJ_MATRIX_DATA_TYPE(*matrix)[n_rows];
	matrix bT = (matrix)Temp;
	(void)simple_mat_transpose(n_b_rows, n_b_cols, b, bT);

	for (unsigned i = 0; i < n_a_rows; ++i)
		for (unsigned j = 0; j < n_b_cols; ++j)
			m[i][j] = cblas_sdot(n_a_cols, a[i], 1, bT[j], 1); // clas_sdot() ???
	free(Temp);
	return m;
}

DBJ_API void* simple_mat_mul_6(
	const unsigned n_a_rows, const unsigned n_a_cols, const unsigned n_b_cols,
	DBJ_MATRIX_DATA_TYPE a[static n_a_rows][n_a_cols], DBJ_MATRIX_DATA_TYPE b[static n_a_rows][n_b_cols], DBJ_MATRIX_DATA_TYPE m[static n_a_rows][n_b_cols])
{
	cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, n_a_rows, n_b_cols, n_a_cols, 1.0f, a[0], n_a_rows, b[0], n_b_rows, 0.0f, m[0], n_a_rows);
	return m;
}
#endif // HAVE_CBLAS

//////////////////////////////////////////////////////////////////////
#endif // DBJ_VARIOUS_SIMPLE_MATMULS_IMPLEMENTATION
//////////////////////////////////////////////////////////////////////

#undef DF_PAIR

#endif //  DBJ_VARIOUS_SIMPLE_MATMULS_INC


#ifdef HAVE_CONFIG_H
#include "../config.h"
#endif /* HAVE_CONFIG_H */

#ifndef DO_WITH_GSL

/* This test is not performed */
int main()
{
    return 0;
}

#else

#include <stdio.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_histogram.h>

int main(void)
{
  FILE* out_file; /* output stream*/
  int i;
  double cut_off;
  double sigma;
  const int sample_no=100000;   /* sample numbers */
  const int bin_no=100;      /* number of bins in histogram */
  gsl_histogram* my_hist;
  gsl_rng* my_rng;

  out_file=stdout;
  my_hist=gsl_histogram_calloc_uniform(bin_no,-10,10);
  my_rng=gsl_rng_alloc(gsl_rng_default);

  sigma=1;
  cut_off=-4;

  fprintf(out_file,"plot '-' using 1:3 title '1st',");
  fprintf(out_file,"'-' using 1:3 title '2nd',");
  fprintf(out_file,"'-' using 1:3 title '3rd'\n");
  while (cut_off<=4)
    {
      fprintf(out_file,"# cut_off %f, sigma %f\n",cut_off,sigma);
      gsl_histogram_reset(my_hist);
      for (i=0;i<sample_no;i++)
	gsl_histogram_increment(my_hist,gsl_ran_gaussian_tail(my_rng,cut_off,sigma));
      gsl_histogram_fprintf(out_file,my_hist,"%f","%f");
      fprintf(out_file,"e\n");
      cut_off+=4;
    }
  return 0;
}

#endif


/*****************************************************************************
*
* Copyright (c) 2003-2018 by The University of Queensland
* http://www.uq.edu.au
*
* Primary Business: Queensland, Australia
* Licensed under the Apache License, version 2.0
* http://www.apache.org/licenses/LICENSE-2.0
*
* Development until 2012 by Earth Systems Science Computational Center (ESSCC)
* Development 2012-2013 by School of Earth Sciences
* Development from 2014 by Centre for Geoscience Computing (GeoComp)
*
*****************************************************************************/

#ifndef __PASO_BLOCKOPS_H__
#define __PASO_BLOCKOPS_H__

#include "Paso.h"
#include "PasoException.h"

#include <cstring> // memcpy

#ifdef ESYS_HAVE_LAPACK
   #ifdef ESYS_MKL_LAPACK
      #include <mkl_lapack.h>
      #include <mkl_cblas.h>
   #else
      extern "C" {
      #include <clapack.h>
      #include <cblas.h>
      }
   #endif
#endif

namespace paso {

inline void BlockOps_Cpy_N(dim_t N, double* R, const double* V)
{
    memcpy((void*)R, (void*)V, N*sizeof(double));
}

/// performs operation R=R-mat*V (V and R are not overlapping) - 2x2
inline void BlockOps_SMV_2(double* R, const double* mat, const double* V)
{
    const double S1 = V[0];
    const double S2 = V[1];
    const double A11 = mat[0];
    const double A12 = mat[2];
    const double A21 = mat[1];
    const double A22 = mat[3];
    R[0] -= A11 * S1 + A12 * S2;
    R[1] -= A21 * S1 + A22 * S2;
}

/// performs operation R=R-mat*V (V and R are not overlapping) - 3x3
inline void BlockOps_SMV_3(double* R, const double* mat, const double* V)
{
    const double S1 = V[0];
    const double S2 = V[1];
    const double S3 = V[2];
    const double A11 = mat[0];
    const double A21 = mat[1];
    const double A31 = mat[2];
    const double A12 = mat[3];
    const double A22 = mat[4];
    const double A32 = mat[5];
    const double A13 = mat[6];
    const double A23 = mat[7];
    const double A33 = mat[8];
    R[0] -= A11 * S1 + A12 * S2 + A13 * S3;
    R[1] -= A21 * S1 + A22 * S2 + A23 * S3;
    R[2] -= A31 * S1 + A32 * S2 + A33 * S3;
}

#define PASO_MISSING_CLAPACK throw PasoException("You need to install a LAPACK version to enable operations on block sizes > 3.")

/// performs operation R=R-mat*V (V and R are not overlapping) - NxN
inline void BlockOps_SMV_N(dim_t N, double* R, const double* mat, const double* V)
{
#ifdef ESYS_HAVE_LAPACK
    cblas_dgemv(CblasColMajor,CblasNoTrans, N, N, -1., mat, N, V, 1, 1., R, 1);
#else
    PASO_MISSING_CLAPACK;
#endif
}

inline void BlockOps_MV_N(dim_t N, double* R, const double* mat, const double* V)
{
#ifdef ESYS_HAVE_LAPACK
    cblas_dgemv(CblasColMajor,CblasNoTrans, N, N, 1., mat, N, V, 1, 0., R, 1);
#else
    PASO_MISSING_CLAPACK;
#endif
}

inline void BlockOps_invM_2(double* invA, const double* A, int* failed)
{
    const double A11 = A[0];
    const double A12 = A[2];
    const double A21 = A[1];
    const double A22 = A[3];
    double D = A11*A22-A12*A21;
    if (std::abs(D) > 0) {
        D = 1./D;
        invA[0] =  A22*D;
        invA[1] = -A21*D;
        invA[2] = -A12*D;
        invA[3] =  A11*D;
    } else {
        *failed = 1;
    }
}

inline void BlockOps_invM_3(double* invA, const double* A, int* failed)
{
    const double A11 = A[0];
    const double A21 = A[1];
    const double A31 = A[2];
    const double A12 = A[3];
    const double A22 = A[4];
    const double A32 = A[5];
    const double A13 = A[6];
    const double A23 = A[7];
    const double A33 = A[8];
    double D = A11*(A22*A33-A23*A32) +
               A12*(A31*A23-A21*A33) +
               A13*(A21*A32-A31*A22);
    if (std::abs(D) > 0) {
        D = 1./D;
        invA[0] = (A22*A33-A23*A32)*D;
        invA[1] = (A31*A23-A21*A33)*D;
        invA[2] = (A21*A32-A31*A22)*D;
        invA[3] = (A13*A32-A12*A33)*D;
        invA[4] = (A11*A33-A31*A13)*D;
        invA[5] = (A12*A31-A11*A32)*D;
        invA[6] = (A12*A23-A13*A22)*D;
        invA[7] = (A13*A21-A11*A23)*D;
        invA[8] = (A11*A22-A12*A21)*D;
    } else {
        *failed = 1;
    }
}

/// LU factorization of NxN matrix mat with partial pivoting
inline void BlockOps_invM_N(dim_t N, double* mat, index_t* pivot, int* failed)
{
#ifdef ESYS_HAVE_LAPACK
#ifdef ESYS_MKL_LAPACK
    int res = 0;
    dgetrf(&N, &N, mat, &N, pivot, &res);
    if (res != 0)
        *failed = 1;
#else
    int res = clapack_dgetrf(CblasColMajor, N, N, mat, N, pivot);
    if (res != 0)
        *failed = 1;
#endif // ESYS_MKL_LAPACK
#else
    PASO_MISSING_CLAPACK;
#endif
}

/// solves system of linear equations A*X=B
inline void BlockOps_solve_N(dim_t N, double* X, double* mat, index_t* pivot, int* failed)
{
#ifdef ESYS_HAVE_LAPACK
#ifdef ESYS_MKL_LAPACK
    int res = 0;
    int ONE = 1;
    dgetrs("N", &N, &ONE, mat, &N, pivot, X, &N, &res);
    if (res != 0)
        *failed = 1;
#else
    int res = clapack_dgetrs(CblasColMajor, CblasNoTrans, N, 1, mat, N, pivot, X, N);
    if (res != 0)
        *failed = 1;
#endif // ESYS_MKL_LAPACK
#else
    PASO_MISSING_CLAPACK;
#endif
}

/// inplace matrix vector product - order 2
inline void BlockOps_MViP_2(const double* mat, double* V)
{
    const double S1 = V[0];
    const double S2 = V[1];
    const double A11 = mat[0];
    const double A12 = mat[2];
    const double A21 = mat[1];
    const double A22 = mat[3];
    V[0] = A11 * S1 + A12 * S2;
    V[1] = A21 * S1 + A22 * S2;
}

/// inplace matrix vector product - order 3
inline void BlockOps_MViP_3(const double* mat, double* V)
{
    const double S1 = V[0];
    const double S2 = V[1];
    const double S3 = V[2];
    const double A11 = mat[0];
    const double A21 = mat[1];
    const double A31 = mat[2];
    const double A12 = mat[3];
    const double A22 = mat[4];
    const double A32 = mat[5];
    const double A13 = mat[6];
    const double A23 = mat[7];
    const double A33 = mat[8];
    V[0] = A11 * S1 + A12 * S2 + A13 * S3;
    V[1] = A21 * S1 + A22 * S2 + A23 * S3;
    V[2] = A31 * S1 + A32 * S2 + A33 * S3;
}

inline void BlockOps_solveAll(dim_t n_block, dim_t n, double* D,
                              index_t* pivot, double* x)
{
    if (n_block == 1) {
#pragma omp parallel for
        for (dim_t i=0; i<n; ++i)
            x[i] *= D[i];
    } else if (n_block == 2) {
#pragma omp parallel for
        for (dim_t i=0; i<n; ++i)
            BlockOps_MViP_2(&D[4*i], &x[2*i]);
    } else if (n_block == 3) {
#pragma omp parallel for
        for (dim_t i=0; i<n; ++i)
            BlockOps_MViP_3(&D[9*i], &x[3*i]);
    } else {
        int failed = 0;
#pragma omp parallel for
        for (dim_t i=0; i<n; ++i) {
            const dim_t block_size = n_block*n_block;
            BlockOps_solve_N(n_block, &x[n_block*i], &D[block_size*i], &pivot[n_block*i], &failed);
        }
        if (failed > 0) {
            throw PasoException("BlockOps_solveAll: solution failed.");
        }
    }
}

} // namespace paso

#endif // __PASO_BLOCKOPS_H__


/*This class introduces a method to work with experimental fitness landscapes
given in a data file in the format
sequence          fitness
1   0   1   ...   1.987
and brings the adjacency matrix as well as other helpful properties

(cc) Johannes Neidhart, 2012*/

#ifndef LANDSCAPE_H
#define LANDSCAPE_H

#include <iostream>
#include <vector>
#include <fstream>
#include <sstream>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_multifit_nlin.h>
#include <vector>
#include <algorithm>
#include <cmath>
#include <map>
#include <numeric>

using namespace std;

class Clandscape {
    private:
        vector < vector < int> > linekey;
        int n, L;
        string name;
        int delta(int, int);
        int hamming( vector < int >, vector < int > );
    public:
        /*Clandscape gets a vector containing the fitness values, in the order of the
        linenumbers of the data file, a vector of vectors, containing the correspondance
        of the line number and the sequence. Sequence length and number of sequences are
        also given.
        */

        double binomial (int, int);
        Clandscape (string, int , int);
        int distance (int, int);
        vector < double > fitness;
        void fitit( vector < double >, vector < double >, vector < double > &);
        void fit(vector <double>, vector < double > &, vector < double > &, double);
        vector < vector < int > > adjacency;
        vector < double > spectrum();
        vector < vector < int > > hypercube;

};

#endif


#ifndef CIMPLE_POLYTOPE_LIBRARY_CIMPLE_H
#define CIMPLE_POLYTOPE_LIBRARY_CIMPLE_H

#include <math.h>
#include <gsl/gsl_vector_double.h>
#include <gsl/gsl_matrix.h>
#include "cimple_gsl_library_extension.h"
#include <gurobi_c.h>
#include "setoper.h"
#include <cdd.h>
#include "cimple_auxiliary_functions.h"
#include "cimple_minksum_wrapper.h"

/**
 * H left side of polytope (sometimes noted A or L)
 * G right side of polytope (sometimes noted b or M)
 *
 *      H.x <= G
 *
 * Chebyshev center is a possible definition  of the "center" of the polytope.
 */
typedef struct polytope{

    gsl_matrix * H;
    gsl_vector * G;
    double *chebyshev_center;

}polytope;

/**
 * @brief "Constructor" Dynamically allocates the space a polytope needs
 * @param k H.size1 == G.size
 * @param n H.size2
 * @return
 */
struct polytope *polytope_alloc(size_t k, size_t n);

/**
 * @brief "Destructor" Deallocates the dynamically allocated memory of the polytope
 * @param polytope
 */
void polytope_free(polytope *polytope);

/**
 * Subdivision of abstract state containing additionally to the polytope also safe mode instructions
 * The array of polytopes "polytope **safe_mode" contains N polytopes the system has to go through to reach the invariant set
 */
typedef struct cell{

    polytope **safe_mode;
    polytope *polytope_description;

}cell;

/**
 * @brief "Constructor" Dynamically allocates the space a cell needs
 * @param k cell.polytope.H.size1 == G.size
 * @param n cell.polytope.HH.size2
 * @return
 */
struct cell *cell_alloc(size_t k,
                        size_t n,
                        int time_horizon);

/**
 * @brief "Destructor" Deallocates the dynamically allocated memory of the cell
 * @param cell
 */
void cell_free(cell *cell);

/**
 * Convex region of several (cells_count) polytopes (array of polytopes**)
 * hull_over_polytopes is the convex hull of polytopes in that abstract state
 * hull.A = NULL if only one polytope exists in that region
 */
typedef struct abstract_state{

    polytope *convex_hull;

    int cells_count;
    cell **cells;

    int transitions_in_count;
    struct abstract_state** transitions_in;
    int transitions_out_count;
    struct abstract_state** transitions_out;

    int distance_invariant_set;
    struct abstract_state * next_state;
    cell *invariant_set;

}abstract_state;

/**
 * @brief "Constructor" Dynamically allocates the space a region of polytope needs
 *
 * Allocates memory space according to the cells_count and their respective sizes
 *
 * @param k Array with the number of rows of each polytope dim([cells_count])
 * @param k_hull number of rows of convex hull polytope of the region
 * @param n system_dynamics size (e.g. s_dyn.A.size1)
 * @param cells_count
 * @return
 */
struct abstract_state *abstract_state_alloc(size_t *k,
                                            size_t k_hull,
                                            size_t n,
                                            int transitions_in_count,
                                            int transitions_out_count,
                                            int cells_count,
                                            int time_horizon);

/**
 * @brief "Destructor" Deallocates the dynamically allocated memory of the region of polytopes
 * @param abstract_state
 */
void abstract_state_free(abstract_state * abstract_state);

/**
 * @brief Converts two C arrays to a polytope consistent of a left side matrix (i.e. H) and right side vector (i.e. G)
 * @param polytope empty polytope with allocated memory
 * @param k number of rows of polytope (H.size1 or G.size)
 * @param n dimension of polytope = system_dynamics size (e.g. s_dyn.A.size1)
 * @param left_side C array to be converted to left side matrix
 * @param right_side C array to be converted to right side vector
 * @param name name of polytope, will be displayed to user terminal to inform about successfull initialization
 */
void polytope_from_arrays(polytope *polytope,
                          double *left_side,
                          double *right_side,
                          double *cheby,
                          char*name);

/**
 * @brief Converts a polytope in gsl form to cdd constraint form
 * @param original
 * @param err
 * @return
 */
dd_PolyhedraPtr polytope_to_cdd(polytope *original,
                                dd_ErrorType *err);

/**
 * @brief Converts a polytope in cdd constraint form to gsl form
 * @param original
 * @return
 */
polytope* cdd_to_polytope(dd_PolyhedraPtr *original);

/**
 * @brief Generate a polytope representing a scaled unit cube
 * @param scale
 * @param dimensions
 * @return gsl polytope
 */
polytope * polytope_scaled_unit_cube(double scale,
                                     int dimensions);

/**
 * @brief Checks whether a state is in a certain polytope
 * @param polytope
 * @param x state to be checked
 * @return 0 if state is not in polytope or 1 if it is
 */
bool polytope_check_state(polytope *polytope, gsl_vector *x);

/**
 * @brief Check whether P1 \ subset P2
 * @param P1
 * @param P2
 * @return
 */
bool polytope_is_subset(polytope *P1,
                        polytope *P2);

/**
 * @brief Unite inequalities of P1 and P2 in new polytope and remove redundancies
 * @param P1
 * @param P2
 * @return
 */
polytope * polytope_unite_inequalities(polytope *P1,
                                       polytope *P2);

/**
 * @brief Project gsl polytope of n+ dimensions to the first n dimensions
 * @param original
 * @param n
 * @return gsl polytope
 */
polytope* polytope_projection(polytope * original,
                              size_t n);
/**
 * @brief Multiplication of a polytope with a matrix
 * @param original
 * @param scale
 * @return gsl polytope
 */
polytope * polytope_linear_transform(polytope *original,
                                     gsl_matrix *scale);

/**
 * @brief Remove redundancies from gsl polytope inequalities
 * @param original
 * @return
 */
polytope * polytope_minimize(polytope *original);

/**
 * @brief Compute Minkowski sum of two polytopes
 * @param P1
 * @param P2
 * @return
 */
polytope * polytope_minkowski(polytope *P1,
                              polytope *P2);

/**
 * @brief Compute Pontryagin difference of two polytopes C=A-B s.t.:
 * A-B = {c \in A-B| c+b \in A, \forall b \in B}
 * @param P1
 * @param P2
 * @return
 */
polytope * polytope_pontryagin(polytope* P1,
                               polytope* P2);

/**
 * @brief Set up constraints in quadratic problem for GUROBI
 * @param constraints
 * @param model
 * @param N
 * @return
 */
int polytope_to_constraints_gurobi(polytope *constraints,
                                   GRBmodel *model,
                                   size_t N);

/**
 * @brief Generate a polytope representing a scaled unit cube
 * @param scale
 * @param dimensions
 * @return cdd polytope
 */
dd_PolyhedraPtr cdd_scaled_unit_cube(double scale,
                                     int dimensions);

/**
 * @brief Project cdd polytope of n+ dimensions to the first n dimensions
 * @param original
 * @param n
 * @return cdd polytope
 */
void cdd_projection(dd_PolyhedraPtr *original,
                    dd_PolyhedraPtr *new,
                    size_t n,
                    dd_ErrorType *err);

/**
 * @brief Remove redundancies from cdd polytope inequalities
 * @param original
 * @return cdd polytope
 */
dd_PolyhedraPtr cdd_minimize(dd_PolyhedraPtr *original,
                             dd_ErrorType *err);

#endif //CIMPLE_POLYTOPE_LIBRARY_CIMPLE_H


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <fitsio.h>
#include <gsl/gsl_statistics.h>

#ifdef SPECTRIM
#include "FBSpectrim.h"
#else
#include "fbus.h"
#endif

typedef struct _Box {
  double  x;
  double  y;
  double  fwhm;
  double  cenx;
  double  ceny;
  double  counts;
  double  background;
  double  noise;
  double  sigmaxythresh;
  double  sigmaxy;
  double  sigmafwhmthresh;
  double  sigmafwhm;
  int     r;
} Box;

typedef struct _Back {
  double x;
  double y;
  double background;
  double sigma;
  int r;
  int width;
} Back;

Box box[4];
Back back;
long nelements, naxes[2], fpixel;
int boxsize = 13;

double stardist(int i, int j) {
  return( sqrt( (box[i].cenx-box[j].cenx)*(box[i].cenx-box[j].cenx) +
		(box[i].ceny-box[j].ceny)*(box[i].ceny-box[j].ceny) ) );
}

double seeing(double var, double d, double r) {
  double lambda;
  double b, K, seeing;

  lambda = 0.65e-6;
  b = r/d;

  /* pixel scale of 0.78 "/pixel, convert to rad */
  var = var*pow(0.78/206265.0,2);

  K = 0.364*(1.0 - 0.532*pow(b, -1/3) - 0.024*pow(b, -7/3));

  seeing = 206265.0*0.98*pow(d/lambda, 0.2)*pow(var/K, 0.6);

  return seeing;
}

double old_seeing(double var, double d, double r) {
  double lambda;
  double r0;

  lambda = 0.55e-6;

  /* pixel scale of 0.78 "/pixel, convert to rad */
  var = var*pow(0.78/206265.0,2);

  r0 = pow(2.0*(lambda*lambda)*(
				( 0.1790*pow(d, (-1.0/3.0)) -
				  0.0968*pow(r, (-1.0/3.0)) )/var
				), 0.6);

  return 206265.0*0.98*lambda/r0;
}

int background(char *image) {

  int i, j, backpix;
  int low_y, up_y, low_x, up_x;
  double dist, sum, sumsq;

  backpix = 0;
  sum = 0.0;
  sumsq = 0.0;

  low_y = back.y - back.r - back.width;
  up_y  = back.y + back.r + back.width;
  low_x = back.x - back.r - back.width;
  up_x  = back.x + back.r + back.width;
  if (low_y < 0) {
    low_y = 0;
  }
  if (up_y >= 480) {
    up_y = 479;
  }
  if (low_x < 0) {
    low_x = 0;
  }
  if (up_x >= 640) {
    up_x = 639;
  }

  for (i=low_y; i<up_y; i++) {
    for (j=low_x; j<up_x; j++) {
      dist = sqrt(pow(back.x-j, 2) + pow(back.y-i, 2));
      if (dist >= back.r && dist <= back.r+back.width) {
	sum += image[i*naxes[0]+j];
	backpix++;
      }
    }
  }

  back.background = sum/backpix;

  for (i=low_y; i<up_y; i++) {
    for (j=low_x; j<up_x; j++) {
      dist = sqrt(pow(back.x-j, 2) + pow(back.y-i, 2));
      if (dist >= back.r && dist <= back.r+back.width) {
	sumsq += (image[i*naxes[0]+j]-back.background)*
	  (image[i*naxes[0]+j]-back.background);
      }
    }
  }

  back.sigma = sqrt(sumsq/backpix);

  return 1;

}

int centroid(char *image, int num) {

  int i, j;
  double  sum    = 0.0;
  double  sumx   = 0.0;
  double  sumxx  = 0.0;
  double  sumy   = 0.0;
  double  sumyy  = 0.0;
  double  val = 0.0;
  double  rmom;
  double  dist;
  int low_y, up_y, low_x, up_x;
  int sourcepix = 0;

  low_y = box[num].y - box[num].r;
  up_y  = box[num].y + box[num].r;
  low_x = box[num].x - box[num].r;
  up_x  = box[num].x + box[num].r;
  if (low_y < 0) {
    low_y = 0;
  }
  if (up_y >= 480) {
    up_y = 479;
  }
  if (low_x < 0) {
    low_x = 0;
  }
  if (up_x >= 640) {
    up_x = 639;
  }

  for (i=low_y; i<up_y; i++) {
    for (j=low_x; j<up_x; j++) {

      dist = sqrt(pow(box[num].x-j, 2) + pow(box[num].y-i, 2));
      if (dist <= box[num].r) {
	val = image[i*naxes[0]+j] - back.background;
	sum   += val;
	sumx  += val*j;
	sumxx += val*j*j;
	sumy  += val*i;
	sumyy += val*i*i;
	sourcepix++;
      }

    }
  }

  if ( sum <= 0.0 ) {
    box[num].sigmaxy = -1.0;
    box[num].sigmafwhm = -1.0;
    box[num].cenx = 0.0;
    box[num].ceny = 0.0;
    box[num].fwhm = -1.0;
  } else {
    rmom = ( sumxx - sumx * sumx / sum + sumyy - sumy * sumy / sum ) / sum;

    if ( rmom <= 0 ) {
      box[num].fwhm = -1.0;
    } else {
      box[num].fwhm = sqrt(rmom)  * 2.354 / sqrt(2.0);
    }

    box[num].counts = sum;
    box[num].cenx   = sumx / sum;
    box[num].ceny   = sumy / sum;
    box[num].x = box[num].cenx;
    box[num].y = box[num].ceny;
    box[num].sigmaxy= box[num].noise * sourcepix / box[num].counts / sqrt(6.0);
    box[num].sigmafwhm = box[num].noise * pow(sourcepix,1.5) / 10.
      / box[num].fwhm / box[num].counts
      * 2.354 * 2.354 / 2.0;
  }

  return 1;

}

int main() {

  char * buffer;
  fitsfile *fptr;
  int i, j, f, status, nimages, anynul, nboxes;
  char fitsfile[256];
  char *froot;
  FILE *init, *out;
  float xx = 0.0, yy = 0.0, xsum = 0.0, ysum = 0.0;
  double dist01[900], dist02[900], dist03[900];
  double dist12[900], dist13[900], dist23[900];
  double sig01[900], sig02[900], sig03[900];
  double sig12[900], sig13[900], sig23[900];
  double mean01, mean02, mean03, mean12, mean13, mean23;
  double var01, var02, var03, var12, var13, var23, rad;
  double avesig01, avesig02, avesig03, avesig12, avesig13, avesig23;
  double d = 0.05;
  double r_01, r_02, r_03, r_12, r_13, r_23, seeing_ave;
  r_01 = 2.0*d;
  r_02 = 2.0*d;
  r_03 = 3.0*d;
  r_12 = 3.0*d;
  r_13 = 2.0*d;
  r_23 = 2.0*d;

  out = fopen("/dev/shm/video/seeing.dat", "a");
  init = fopen("/dev/shm/video/init_cen_all", "r");
  i = 0;
  while (fscanf(init, "%f %f\n", &xx, &yy) != EOF) {
    box[i].x = xx;
    box[i].cenx = xx;
    box[i].y = yy;
    box[i].ceny = yy;
    box[i].r = boxsize/2.0;
    i++;
  }
  fclose(init);

  back.r = 40;
  back.width = 5;

  status = 0;
  anynul = 0;
  naxes[0] = 640;
  naxes[1] = 480;
  fpixel = 1;

  nelements = naxes[0]*naxes[1];

  /* init card */
  if (FB_Init() == RET_ERROR) {
    printf("FlashBus failed init\n");
    return(-1);
  }

  /* configure video capture */
  FB_SetVideoConfig(TYPE_COMPOSITE, STANDARD_NTSC, 1, 0);

  /* set up offscreen video capture */
  if (FB_VideoOffscreen(naxes[0], naxes[1], 8, TRUE) != RET_SUCCESS) {
    printf("Couldn't set offscreen capture\n");
    return(-1);
  }

  /* allocate the buffer */
  if (!(buffer = malloc(nelements*sizeof(char)))) {
    printf("Couldn't Allocate Image Buffer\n");
    exit(-1);
  }

  FB_VideoLive(TRUE, ALIGN_ANY);
  FB_WaitVSync(12);

  nimages = 900;
  froot = "/dev/shm/video/seeing.fits";

  for (f=0; f<nimages; f++) {
    FB_WaitVSync(1);
    FB_CopyVGARect(0,
		   0,
		   naxes[0],
		   naxes[1],
		   buffer,
		   -1,
		   COPYDIR_FROMOFFSCREEN);
    xsum = 0.0;
    ysum = 0.0;
    for (i=0; i<4; i++) {
      xsum += box[i].cenx;
      ysum += box[i].ceny;
    }
    back.x = xsum/4.0;
    back.y = ysum/4.0;
    background(buffer);

    //printf("file: %s  \t  background = %f noise = %f\n",
    //	   fitsfile, back.background, back.sigma);

    for (i=0; i<4; i++) {
      box[i].noise = back.sigma;
      //box[i].r = boxsize/2.0;
      centroid(buffer, i);
      //box[i].r = boxsize/3.0;
      centroid(buffer, i);
      //box[i].r = boxsize/4.0;
      centroid(buffer, i);

      /* printf("\t star = %d, counts = %f, \n\t x = %f, y = %f, sigmaxy = %f\n",
	     i,
	     box[i].counts,
	     box[i].cenx+1,
	     box[i].ceny+1,
	     box[i].sigmaxy);
      */
    }

    dist01[f] = stardist(0, 1);
    sig01[f] = box[0].sigmaxy*box[0].sigmaxy + box[1].sigmaxy*box[1].sigmaxy;

    dist02[f] = stardist(0, 2);
    sig02[f] = box[0].sigmaxy*box[0].sigmaxy + box[2].sigmaxy*box[2].sigmaxy;

    dist03[f] = stardist(0, 3);
    sig03[f] = box[0].sigmaxy*box[0].sigmaxy + box[3].sigmaxy*box[3].sigmaxy;

    dist12[f] = stardist(1, 2);
    sig12[f] = box[1].sigmaxy*box[1].sigmaxy + box[2].sigmaxy*box[2].sigmaxy;

    dist13[f] = stardist(1, 3);
    sig13[f] = box[1].sigmaxy*box[1].sigmaxy + box[3].sigmaxy*box[3].sigmaxy;

    dist23[f] = stardist(2, 3);
    sig23[f] = box[3].sigmaxy*box[3].sigmaxy + box[2].sigmaxy*box[2].sigmaxy;

  }

  sprintf(fitsfile, "!%s", froot);
  fits_create_file(&fptr, fitsfile, &status);
  fits_create_img(fptr, BYTE_IMG, 2, naxes, &status);
  fits_write_img(fptr, TBYTE, fpixel, nelements, buffer, &status);
  fits_close_file(fptr, &status);
  fits_report_error(stderr, status);

  mean01 = gsl_stats_mean(dist01, 1, nimages);
  avesig01 = gsl_stats_mean(sig01, 1, nimages);
  printf("mean01 = %f, avesig01 = %f\n", mean01, avesig01);

  mean02 = gsl_stats_mean(dist02, 1, nimages);
  avesig02 = gsl_stats_mean(sig02, 1, nimages);
  printf("mean02 = %f, avesig02 = %f\n", mean02, avesig02);

  mean03 = gsl_stats_mean(dist03, 1, nimages);
  avesig03 = gsl_stats_mean(sig03, 1, nimages);
  printf("mean03 = %f, avesig03 = %f\n", mean03, avesig03);

  mean12 = gsl_stats_mean(dist12, 1, nimages);
  avesig12 = gsl_stats_mean(sig12, 1, nimages);
  printf("mean12 = %f, avesig12 = %f\n", mean12, avesig12);

  mean13 = gsl_stats_mean(dist13, 1, nimages);
  avesig13 = gsl_stats_mean(sig13, 1, nimages);
  printf("mean13 = %f, avesig13 = %f\n", mean13, avesig13);

  mean23 = gsl_stats_mean(dist23, 1, nimages);
  avesig23 = gsl_stats_mean(sig23, 1, nimages);
  printf("mean23 = %f, avesig23 = %f\n", mean23, avesig23);

  printf("\n");

  var01 = gsl_stats_variance_m(dist01, 1, nimages, mean01);
  var01 = var01 - avesig01;
  printf("sigma01 = %f, seeing01 = %f\n", sqrt(var01), seeing(var01, d, r_01));

  var02 = gsl_stats_variance_m(dist02, 1, nimages, mean02);
  var02 = var02 - avesig02;
  printf("sigma02 = %f, seeing02 = %f\n", sqrt(var02), seeing(var02, d, r_02));

  var03 = gsl_stats_variance_m(dist03, 1, nimages, mean03);
  var03 = var03 - avesig03;
  printf("sigma03 = %f, seeing03 = %f\n", sqrt(var03), seeing(var03, d, r_03));

  var12 = gsl_stats_variance_m(dist12, 1, nimages, mean12);
  var12 = var12 - avesig12;
  printf("sigma12 = %f, seeing12 = %f\n", sqrt(var12), seeing(var12, d, r_12));

  var13 = gsl_stats_variance_m(dist13, 1, nimages, mean13);
  var13 = var13 - avesig13;
  printf("sigma13 = %f, seeing13 = %f\n", sqrt(var13), seeing(var13, d, r_13));

  var23 = gsl_stats_variance_m(dist23, 1, nimages, mean23);
  var23 = var23 - avesig23;
  printf("sigma23 = %f, seeing23 = %f\n\n", sqrt(var23), seeing(var23, d, r_23));

  seeing_ave = (seeing(var01, d, r_01) +
		seeing(var02, d, r_02) +
		seeing(var03, d, r_03) +
		seeing(var12, d, r_12) +
		seeing(var13, d, r_13) +
		seeing(var23, d, r_23))/6.0;

  printf("Ave. seeing = %4.2f\"\n", seeing_ave);

  fprintf(out, "%f %f %f %f %f %f %f\n",
	  seeing(var01, d, r_01),
	  seeing(var02, d, r_02),
	  seeing(var03, d, r_03),
	  seeing(var12, d, r_12),
	  seeing(var13, d, r_13),
	  seeing(var23, d, r_23),
	  seeing_ave);

  init = fopen("/dev/shm/video/init_cen_all", "w");
  for (i=0; i<4; i++) {
    fprintf(init, "%f %f\n", box[i].cenx, box[i].ceny);
  }
  fclose(init);

  return (status);
}


#include <complex.h>
#include <math.h>
//#include <gsl/gsl_sf.h>
#include <stdio.h>
#include <stdlib.h>
#include "specfunc.h"

/*
 * to compile as a library (that can be used by specfunc.py):
 *
 *      cc -fPIC -c -O2 specfunc.c -o specfunc.o
 *      cc --shared specfunc.o -o libspecfunc.so
 *
 * if the gamma-function stuff an dthe parabolic cylinder function is commented
 * in, you need
 *
 *      cc --shared specfunc.o -o libspecfunc.so -lgslcblas -lgsl
 *
 * instead
 *
 * hyp2f1 and hyp1f1 seem to work well, the parabolic cylinder functions pcfd
 * are problematic for anything but small arguments
 *
 * felix, june 2014
 */

#define PREC_WARN_LIMIT 1e99 // warn if individual terms in the series are larger than this
// XXX 4. 12. 14: set to high value because contrary to what I thought I had
// understood, there seem to be many perfectly fine cases where individual
// terms in the sum are higher than 10^15

// hypergeometric function for |z| < 1
double complex hyp2f1(const double complex a, const double complex b,
        const double complex c, const double complex z, sf_prms_and_info_t* p)
{
    double frac = p->tol;
    double complex n = 0;
    double complex summand = a*b/c * z;
    double complex sum = 1. + summand;

    p->prec_warning = 0;
    for (n = 2; (creal(n) < p->max_iter) && (frac >= p->tol); n++)
    {
        summand *= (a+n-1)*(b+n-1)/(c+n-1) * z/n;
        p->prec_warning = p->prec_warning | (creal(summand) > PREC_WARN_LIMIT)
                                          | (cimag(summand) > PREC_WARN_LIMIT);
        sum += summand;
        frac = fabs(creal(summand/sum)) + fabs(cimag((summand/sum)));
    }
    p->iters_needed = (int)creal(n);
    p->tol_achieved = frac;
    return sum;
}

// confluent hypergeometric function
double complex hyp1f1(const double complex a, const double complex b,
        const double complex z, sf_prms_and_info_t* p)
{
    double frac = p->tol;
    double complex n = 0;
    double complex summand = a/b * z;
    double complex sum = 1. + summand;
    p->prec_warning = 0;
    for (n = 2; (creal(n) < p->max_iter) && (frac >= p->tol) ; n++)
    {
        summand = summand/(b+n-1) * (a+n-1) * (z/n);
        p->prec_warning = p->prec_warning | (creal(summand) > PREC_WARN_LIMIT)
                                          | (cimag(summand) > PREC_WARN_LIMIT);
        sum += summand;
        frac = fabs(creal(summand/sum)) + fabs(cimag((summand/sum)));
    }
    p->iters_needed = (int)creal(n);
    p->tol_achieved = frac;
    return sum;
}

void hyp1f1_a_arr(const double complex* a, const double complex b,
        const double complex z, double complex* out,
        size_t n_arr, sf_prms_and_info_t* p)
{
    double complex f = 0.;
    double frac = 0.;
    double maxfrac = p->tol;
    double complex n = 0;
    double complex* summand =
        (double complex*)malloc(sizeof(double complex) * n_arr);
    int i = 0;

    for (i = 0; i < n_arr; i++)
    {
        summand[i] = a[i]/b * z;
        out[i] = 1. + summand[i];
    }

    p->prec_warning = 0;
    for (n = 2; (creal(n) < p->max_iter) && (maxfrac >= p->tol) ; n++)
    {
        maxfrac = 0;
        f = 1./(b+n-1) * z / n; // same for all a
        for (i = 0; i < n_arr; i++)
        {
            summand[i] *= f * (a[i]+n-1);
            p->prec_warning = p->prec_warning | (creal(summand[i]) > PREC_WARN_LIMIT)
                                              | (cimag(summand[i]) > PREC_WARN_LIMIT);
            out[i] += summand[i];
            frac = fabs(creal(summand[i]/out[i]))
                + fabs(cimag((summand[i]/out[i])));
            maxfrac = frac > maxfrac ? frac : maxfrac;
        }
    }
    p->iters_needed = (int)creal(n);
    p->tol_achieved = maxfrac;
    free(summand);
}

void hyp1f1_b_arr(const double complex a, const double complex* b,
        const double complex z, double complex* out,
        size_t n_arr, sf_prms_and_info_t* p)
{
    double complex f = 0.;
    double frac = 0.;
    double maxfrac = p->tol;
    double complex n = 0;
    double complex* summand =
        (double complex*)malloc(sizeof(double complex) * n_arr);
    int i = 0;

    for (i = 0; i < n_arr; i++)
    {
        summand[i] = a/b[i] * z;
        out[i] = 1. + summand[i];
    }

    p->prec_warning = 0;
    for (n = 2; (creal(n) < p->max_iter) && (maxfrac >= p->tol) ; n++)
    {
        maxfrac = 0;
        f = (a+n-1) * z / n; // same for all b
        for (i = 0; i < n_arr; i++)
        {
            summand[i] *= f / (b[i]+n-1);
            p->prec_warning = p->prec_warning | (creal(summand[i]) > PREC_WARN_LIMIT)
                                              | (cimag(summand[i]) > PREC_WARN_LIMIT);
            out[i] += summand[i];
            frac = fabs(creal(summand[i]/out[i]))
                + fabs(cimag((summand[i]/out[i])));
            maxfrac = frac > maxfrac ? frac : maxfrac;
        }
    }
    p->iters_needed = (int)creal(n);
    p->tol_achieved = maxfrac;
    free(summand);
}

void hyp1f1_z_arr(const double complex a, const double complex b,
        const double complex* z, double complex* out,
        size_t n_arr, sf_prms_and_info_t* p)
{
    double complex f = 0.;
    double frac = 0.;
    double maxfrac = p->tol;
    double complex n = 0;
    double complex* summand =
        (double complex*)malloc(sizeof(double complex) * n_arr);
    int i = 0;

    for (i = 0; i < n_arr; i++)
    {
        summand[i] = a/b * z[i];
        out[i] = 1. + summand[i];
    }

    p->prec_warning = 0;
    for (n = 2; (creal(n) < p->max_iter) && (maxfrac >= p->tol) ; n++)
    {
        maxfrac = 0;
        f = (a+n-1)/(b+n-1) / n; // same for all z
        for (i = 0; i < n_arr; i++)
        {
            summand[i] *= f * z[i];
            p->prec_warning = p->prec_warning | (creal(summand[i]) > PREC_WARN_LIMIT)
                                              | (cimag(summand[i]) > PREC_WARN_LIMIT);
            out[i] += summand[i];
            frac = fabs(creal(summand[i]/out[i]))
                + fabs(cimag((summand[i]/out[i])));
            maxfrac = frac > maxfrac ? frac : maxfrac;
        }
    }
    p->iters_needed = (int)creal(n);
    p->tol_achieved = maxfrac;
    free(summand);
}

void hyp1f1_all_arr(const double complex* a, const double complex* b,
        const double complex* z, double complex* out,
        size_t n_arr, sf_prms_and_info_t* p)
{
    int i = 0;
    for (i = 0; i < n_arr; i++)
        out[i] = hyp1f1(a[i], b[i], z[i], p);
}

/*
double complex cgamma(const double complex z)
{
    gsl_sf_result lnabs = {0, 0};
    gsl_sf_result arg = {0, 0};
    gsl_sf_lngamma_complex_e(creal(z), cimag(z), &lnabs, &arg);
    return cexp(lnabs.val+1j*arg.val);
}

// parabolic cylinder function -- this is only ok for small parameters
double complex pcfd(const double complex nu, const double complex z,
        sf_prms_and_info_t* p)
{
    return cpow(2, nu/2)*cexp(-z*z/4) * csqrt(M_PI) *
        (1./cgamma((1-nu)/2) * hyp1f1(-nu/2, 1./2, z*z/2, p)
            - csqrt(2.)*z/cgamma(-nu/2)
            * hyp1f1((1-nu)/2, 3./2, z*z/2, p));
}

void pcfd_nu_arr(const double complex* nu, const double complex z,
        double complex* out, size_t n_arr, sf_prms_and_info_t* p)
{
    int i = 0;
    for (i = 0; i < n_arr; i++)
    {
        out[i] = pcfd(nu[i], z, p);
    }
}

void pcfd_z_arr(const double complex nu, const double complex* z,
        double complex* out, size_t n_arr, sf_prms_and_info_t* p)
{
    int i = 0;
    for (i = 0; i < n_arr; i++)
    {
        out[i] = pcfd(nu, z[i], p);
    }
}
*/

void hyp2f1_a_arr(const double complex* a, const double complex b,
        const double complex c, const double complex z, double complex* out,
        size_t n_arr, sf_prms_and_info_t* p)
{
    double complex f = 0.;
    double frac = 0.;
    double maxfrac = p->tol;
    double complex n = 0;
    double complex* summand =
        (double complex*)malloc(sizeof(double complex) * n_arr);
    int i = 0;

    for (i = 0; i < n_arr; i++)
    {
        summand[i] = a[i]*b/c * z;
        out[i] = 1. + summand[i];
    }

    p->prec_warning = 0;
    for (n = 2; (creal(n) < p->max_iter) && (maxfrac >= p->tol) ; n++)
    {
        maxfrac = 0;
        f = (b+n-1)/(c+n-1) * z / n; // same for all a
        for (i = 0; i < n_arr; i++)
        {
            summand[i] *= f * (a[i]+n-1);
            p->prec_warning = p->prec_warning | (creal(summand[i]) > PREC_WARN_LIMIT)
                                              | (cimag(summand[i]) > PREC_WARN_LIMIT);
            out[i] += summand[i];
            frac = fabs(creal(summand[i]/out[i]))
                + fabs(cimag((summand[i]/out[i])));
            maxfrac = frac > maxfrac ? frac : maxfrac;
        }
    }
    p->iters_needed = (int)creal(n);
    p->tol_achieved = maxfrac;
    free(summand);
}

void hyp2f1_b_arr(const double complex a, const double complex* b,
        const double complex c, const double complex z, double complex* out,
        size_t n_arr, sf_prms_and_info_t* p)
{
    hyp2f1_a_arr(b, a, c, z, out, n_arr, p);
}

void hyp2f1_c_arr(const double complex a, const double complex b,
        const double complex* c, const double complex z, double complex* out,
        size_t n_arr, sf_prms_and_info_t* p)
{
    double complex f = 0.;
    double frac = 0.;
    double maxfrac = p->tol;
    double complex n = 0;
    double complex* summand =
        (double complex*)malloc(sizeof(double complex) * n_arr);
    int i = 0;

    for (i = 0; i < n_arr; i++)
    {
        summand[i] = a*b/c[i] * z;
        out[i] = 1. + summand[i];
    }

    p->prec_warning = 0;
    for (n = 2; (creal(n) < p->max_iter) && (maxfrac >= p->tol) ; n++)
    {
        maxfrac = 0;
        f = (a+n-1)*(b+n-1) * z / n; // same for all a
        for (i = 0; i < n_arr; i++)
        {
            summand[i] *= f / (c[i]+n-1);
            p->prec_warning = p->prec_warning | (creal(summand[i]) > PREC_WARN_LIMIT)
                                              | (cimag(summand[i]) > PREC_WARN_LIMIT);
            out[i] += summand[i];
            frac = fabs(creal(summand[i]/out[i]))
                + fabs(cimag((summand[i]/out[i])));
            maxfrac = frac > maxfrac ? frac : maxfrac;
        }
    }
    p->iters_needed = (int)creal(n);
    p->tol_achieved = maxfrac;
    free(summand);
}

// hyp2d1 for an array of z values
void hyp2f1_z_arr(const double complex a, const double complex b,
        const double complex c, const double complex* z, double complex* out,
        size_t n_arr, sf_prms_and_info_t* p)
{
    double complex f = 0.;
    double frac = 0.;
    double maxfrac = p->tol;
    double complex n = 0;
    double complex* summand =
        (double complex*)malloc(sizeof(double complex) * n_arr);
    int i = 0;

    for (i = 0; i < n_arr; i++)
    {
        summand[i] = a*b/c * z[i];
        out[i] = 1. + summand[i];
    }

    p->prec_warning = 0;
    for (n = 2; (creal(n) < p->max_iter) && (maxfrac >= p->tol) ; n++)
    {
        maxfrac = 0;
        f = (a+n-1)*(b+n-1)/(c+n-1) / n; // same for all zs
        for (i = 0; i < n_arr; i++)
        {
            summand[i] *= f * z[i];
            p->prec_warning = p->prec_warning | (creal(summand[i]) > PREC_WARN_LIMIT)
                                              | (cimag(summand[i]) > PREC_WARN_LIMIT);
            out[i] += summand[i];
            frac = fabs(creal(summand[i]/out[i]))
                + fabs(cimag((summand[i]/out[i])));
            maxfrac = frac > maxfrac ? frac : maxfrac;
        }
    }
    p->iters_needed = (int)creal(n);
    p->tol_achieved = maxfrac;
    free(summand);
}

void hyp2f1_all_arr(const double complex* a, const double complex* b,
        const double complex* c, const double complex* z, double complex* out,
        size_t n_arr, sf_prms_and_info_t* p)
{
    int i = 0;
    for (i = 0; i < n_arr; i++)
        out[i] = hyp2f1(a[i], b[i], c[i], z[i], p);
}


/*
Performing approximate Bayesian computation sequential Monte Carlo (Toni et al.
2009) for linear regression.

Synthetic data is generated by `ground_truth_and_analysis.ipynb`.

This script writes particles.csv, where each row corresponds to a particle and
each column corresponds to a round of SMC.

Defining #DEBUG_MODE will silence all writing to stdout. One may then add
printf statements in the code, and perhaps write the output to file as:
`./run.sh > output.txt`

Parameter ordering convention:
0 - gradient
1 - intercept
2 - standard deviation

Author: Juvid Aryaman
*/

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

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sort_double.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_fit.h>

#define N_DATA 30
#define N_PARAMETERS 3

#define N_PARTICLES 20000

#define RND gsl_rng_uniform(r)
#define SEED 1

#define X_DATA_FILENAME "x.csv"
#define Y_DATA_FILENAME "y.csv"

// Global variables
/*Define the distance threshold for every round of SMC*/
double distance_threshold_schedule[] = {7.0, 6.375, 5.75, 5.125, 4.5, 3.875,
	3.25, 2.625, 2.0};
int N_ROUNDS_SMC = (int)(sizeof(distance_threshold_schedule) / sizeof(double));

#include "smc.h"
#include "lin_reg.h"

//#define DEBUG_MODE

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

#ifndef DEBUG_MODE
	printf("Threshold schedule:\n");
	print_double_array(distance_threshold_schedule, N_ROUNDS_SMC);
	printf("\n");
#endif

/* set up GSL RNG */
gsl_rng *r = gsl_rng_alloc(gsl_rng_mt19937);
/* end of GSL setup */

gsl_rng_set(r, SEED);

/////////////////////////
/*Read data*/
/////////////////////////

FILE *data_pointer_x, *data_pointer_y;

data_pointer_x = fopen(X_DATA_FILENAME, "r");
data_pointer_y = fopen(Y_DATA_FILENAME, "r");

double data_x[N_DATA];
double data_y[N_DATA];
int i, j, read_error_status_x, read_error_status_y;
for (i=0; i < N_DATA; i++){
	read_error_status_x = fscanf(data_pointer_x, "%lf\n", &data_x[i]);
	read_error_status_y = fscanf(data_pointer_y, "%lf\n", &data_y[i]);
}
if (read_error_status_x != 1){printf("Error reading X data\n"); return 0;}
if (read_error_status_y != 1){printf("Error reading Y data\n"); return 0;}

/////////////////////////
/*Initialise variables*/
/////////////////////////

/*Fit a linear model to the data, which will be used as summary statistics of
the data*/
double gradient_fit_data, intercept_fit_data, sigma_fit_data, cov00, cov01, cov11, sumsq;
int gsl_fit_return_value;
gsl_fit_return_value = gsl_fit_linear(data_x, 1, data_y, 1, N_DATA,
																			&intercept_fit_data, &gradient_fit_data,
																			&cov00, &cov01, &cov11, &sumsq);
if (gsl_fit_return_value != 0) {printf("Fit failed.\n"); return -1;}
sigma_fit_data = sqrt(sumsq/(N_DATA-2));

#ifndef DEBUG_MODE
	printf("gradient ML = %.8f\n", gradient_fit_data);
	printf("intercept ML = %.8f\n", intercept_fit_data);
	printf("sigma ML = %.8f\n", sigma_fit_data);
#endif

/*Make a (N_PARAMETERS X N_ROUNDS_SMC X N_PARTICLES) array to store all
particles at all rounds of SMC*/
double*** theta_particle;
theta_particle = (double***) malloc(N_PARAMETERS * sizeof(double**));
for (i = 0; i < N_PARAMETERS; i++){
	theta_particle[i] = (double**) malloc(N_ROUNDS_SMC * sizeof(double*));
}
for (i = 0; i < N_PARAMETERS; i++){
	for (j = 0; j < N_ROUNDS_SMC; j++){
		theta_particle[i][j] = (double*) malloc(N_PARTICLES * sizeof(double));
	}
}

double *simulated_data = (double*) malloc(N_DATA * sizeof(double));
double *distance = malloc(N_PARTICLES * sizeof(double));
double **weight;
weight = (double**) malloc(N_ROUNDS_SMC * sizeof(double*));
for (i = 0; i < N_ROUNDS_SMC; i++) {
	weight[i] = (double*) malloc(N_PARTICLES * sizeof(double));
}

double weight_normalizer = 0.0;

int time_smc=0; // an index of each round of SMC
int param_index_chosen, prior_violated;
int particle_index;
/////////////////////////
/*Perform ABC SMC*/
/////////////////////////

/*For every round of SMC*/
for (time_smc = 0; time_smc < N_ROUNDS_SMC; time_smc++) {
	#ifndef DEBUG_MODE
		printf("Round %d of SMC\n", time_smc);
	#endif

	/*Draw or perturb a particle and compute distance*/
	for (particle_index = 0; particle_index < N_PARTICLES; particle_index++) {
		// reset distance of particle
		distance[particle_index] = distance_threshold_schedule[time_smc] + 1.0;

		while (distance[particle_index] > distance_threshold_schedule[time_smc]) {
			if (time_smc == 0) {
				// Sample from the prior
				sample_prior(r, theta_particle, particle_index);
			}
			else{
				/*Sample from the old weights*/
				param_index_chosen = weighted_choice(r, weight[time_smc-1]);
				if ((param_index_chosen < 0)||(param_index_chosen >= N_PARTICLES)) {
					printf("Error in param_index_chosen\n");
					printf("time_smc = %d\n", time_smc);
					return -1;
				}

				perturb_particle(r, theta_particle, time_smc, param_index_chosen,
												 particle_index);

				// Check if prior support is 0
				prior_violated = check_prior_violated(theta_particle, time_smc,
																							particle_index);
				if(prior_violated == 1) continue;
			}

			simulate_dataset(r, theta_particle, data_x, simulated_data, time_smc,
											 particle_index);

			distance[particle_index] = distance_metric_sum_abs_res(simulated_data, data_y);
		}
	}

	#ifndef DEBUG_MODE
		printf("Particles sampled.\n");
	#endif

	/*Compute & Normalise weights. For uniform priors, and a uniform perturbation
	kernel, all surviving particles are weighted identically as 1/N_PARTICLES.
	Whilst seemingly inefficient, I keep this code here for clarity/generality.
	The bottleneck in computation time is the while loop above.*/
	if (time_smc==0){ for (i = 0; i < N_PARTICLES; i++) weight[0][i] = 1.0;}
	else{
		weight_normalizer = 0.0;
		for (particle_index = 0; particle_index < N_PARTICLES; particle_index++) {
			// kernel_pdf is uniform, so is independent of the parameters
			weight_normalizer +=  weight[time_smc-1][particle_index]*kernel_pdf();
		}
		for (particle_index = 0; particle_index < N_PARTICLES; particle_index++) {
			weight[time_smc][particle_index] = prior_pdf(theta_particle, time_smc,
																				 particle_index)/weight_normalizer;
		}
	}
	weight_normalizer = 0.0;
	for (i = 0; i < N_PARTICLES; i++)	weight_normalizer += weight[time_smc][i];
	for (i = 0; i < N_PARTICLES; i++){
		weight[time_smc][i] = weight[time_smc][i]/weight_normalizer;
	}

}

#ifndef DEBUG_MODE
	printf("Writing particles to file\n");
#endif
	write_particles_to_csv(theta_particle);

char weight_filename[] = "weights.csv";
write_2d_double_array_to_csv(weight, N_ROUNDS_SMC, N_PARTICLES, weight_filename);

#ifndef DEBUG_MODE
	printf("Done!\n");
#endif

return 0; //return from main
} //close main


/* -*- c++ -*- */
/*
 * Copyright 2015 Free Software Foundation, Inc.
 *
 * This is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published
 * by the Free Software Foundation; either version 3, or (at your
 * option) any later version.
 *
 * This software 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 software; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifndef INCLUDED_fec_mtrx_impl_H
#define INCLUDED_fec_mtrx_impl_H

#include <string>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gnuradio/fec/fec_mtrx.h>

namespace gr {
  namespace fec {
    namespace code {

      class fec_mtrx_impl : public fec_mtrx
      {
      protected:
        //! Constructor
        fec_mtrx_impl();

        //! Codeword length n
        unsigned int d_n;

        //! Information word length k
        unsigned int d_k;

        //! Number of rows in the matrix read in from alist file
        unsigned int d_num_rows;

        //! Number of columns in the matrix read in from alist file
        unsigned int d_num_cols;

        //! GSL matrix structure for the parity check matrix
        matrix_sptr d_H_sptr;

        //! Flag for whether or not the parity bits come first or last
        bool d_par_bits_last;

      public:
        //! Returns the parity check matrix H (needed by decoder)
        const gsl_matrix *H() const;

        //!Get the codeword length n
        unsigned int n() const;

        //! Get the information word length k
        unsigned int k() const;

        //! Subtract matrices using mod2 operations
        void add_matrices_mod2(gsl_matrix *result,
                               const gsl_matrix *,
                               const gsl_matrix *) const;

        //! Multiply matrices using mod2 operations
        void mult_matrices_mod2(gsl_matrix *result,
                                const gsl_matrix *,
                                const gsl_matrix *) const;

        //! Invert a square matrix using mod2 operations
        gsl_matrix *calc_inverse_mod2(const gsl_matrix *) const;

        /*!
         * \brief Get Boolean for whether or not parity bits come first or last
         * \details
         * The decoder will need to know if the parity bits are
         * coming first or last
         */
        bool parity_bits_come_last() const;

        virtual ~fec_mtrx_impl();
      };
    }
  }
}

#endif /* INCLUDED_fec_mtrx_impl_H */


/* min/bracketing.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Fabrice Rossi
 *
 * 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.
 */

/* bracketing.c -- find an initial bracketing interval for a function to minimize */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_machine.h>

#include "min.h"

int
gsl_min_find_bracket(gsl_function *f,double *minimum,double * f_minimum,
		     double * x_lower, double * f_lower,
                     double * x_upper, double * f_upper,
		     size_t eval_max)
{
  /* The three following variables must be declared volatile to avoid storage
     in extended precision registers available on some architecture. The code
     relies on the ability to compare double values. As the values will be
     store in regular memory, the extended precision will then be lost and
     values that are different in extended precision might have equal
     representation in double precision. This behavior might break the
     algorithm.
   */
  volatile double f_left = *f_lower;
  volatile double f_right = *f_upper;
  volatile double f_center;
  double x_left = *x_lower;
  double x_right= *x_upper;
  double x_center;
  const double golden = 0.3819660;	/* golden = (3 - sqrt(5))/2 */
  size_t nb_eval = 0;

  if (f_right >= f_left)
    {
      x_center = (x_right - x_left) * golden + x_left;
      nb_eval++;
      SAFE_FUNC_CALL (f, x_center, &f_center);
    }
  else
    {
      x_center = x_right ;
      f_center = f_right ;
      x_right = (x_center - x_left) / golden + x_left;
      nb_eval++;
      SAFE_FUNC_CALL (f, x_right, &f_right);
    }

  do
    {
      if (f_center < f_left )
	{
	  if (f_center < f_right)
	    {
	      *x_lower = x_left;
	      *x_upper = x_right;
	      *minimum = x_center;
	      *f_lower = f_left;
	      *f_upper = f_right;
	      *f_minimum = f_center;
/*	      gsl_ieee_printf_double (&f_left);
	      printf(" ");
	      gsl_ieee_printf_double (&f_center);
	      printf("\n");*/
	      return GSL_SUCCESS;
	    }
	  else if (f_center > f_right)
	    {
	      x_left = x_center;
	      f_left = f_center;
	      x_center = x_right;
	      f_center = f_right;
	      x_right = (x_center - x_left) / golden + x_left;
	      nb_eval++;
	      SAFE_FUNC_CALL (f, x_right, &f_right);
	    }
	  else /* f_center == f_right */
	    {
	      x_right = x_center;
	      f_right = f_center;
	      x_center = (x_right - x_left) * golden + x_left;
	      nb_eval++;
	      SAFE_FUNC_CALL (f, x_center, &f_center);
	    }
	}
      else /* f_center >= f_left */
	{
	  x_right = x_center;
	  f_right = f_center;
	  x_center = (x_right - x_left) * golden + x_left;
	  nb_eval++;
	  SAFE_FUNC_CALL (f, x_center, &f_center);
	}
    }
  while (nb_eval < eval_max
	 && (x_right - x_left) > GSL_SQRT_DBL_EPSILON * ( (x_right + x_left) * 0.5 ) + GSL_SQRT_DBL_EPSILON);
  *x_lower = x_left;
  *x_upper = x_right;
  *minimum = x_center;
  *f_lower = f_left;
  *f_upper = f_right;
  *f_minimum = f_center;
  return GSL_FAILURE;
}


#include "matrix.h"
#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <math.h>

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

/*************************** Complex Number *************************/

/*
typedef struct {
    double dat[2];
} Complex;
*/

Complex ComplexPolar(double r, double theta) {
    Complex z = {r * cos(theta), r * sin(theta)};
    return z;
}

void ComplexPrint(Complex z) {
    printf("%f + %fi\n", z.re, z.im);
}

Complex ComplexAdd(Complex z, Complex w) {
    Complex x = {z.re + w.re, z.im + w.im};
    return x;
}

Complex ComplexMul(Complex z, Complex w) {
    Complex x = {z.re*w.re - z.im*w.im, z.re*w.im + w.re*z.im};
    return x;
}

Complex ComplexMulReal(Complex z, double r) {
    Complex x = {z.re*r, z.im*r};
    return x;
}

double ComplexMag(Complex z) {
    return sqrt(ComplexMagSquare(z));
}

double ComplexMagSquare(Complex z) {
    return z.re*z.re + z.im*z.im;
}

/*************************** Bit Vector *************************/

/*
struct BitVector {
    unsigned int size;
    unsigned char* data;
};
*/

// alloc and init to 0
struct BitVector* newBitVector(unsigned int size) {
    assert(size > 0);

    struct BitVector* vec = (struct BitVector*)malloc(sizeof(struct BitVector));
    vec->size = size;

    vec->data = (unsigned char*)gsl_vector_alloc(vec->size);
    gsl_vector_set_zero((gsl_vector*)vec->data);

    return vec;
}

// alloc and init randomly
struct BitVector* newBitVectorRandom(unsigned int size) {
    assert(size > 0);

    struct BitVector* vec = (struct BitVector*)malloc(sizeof(struct BitVector));
    vec->size = size;
    vec->data = (unsigned char*)gsl_vector_alloc(vec->size);

    BitVectorSetRandom(vec);

    return vec;
}

void BitVectorSetRandom(struct BitVector* vec) {
    for (unsigned int i = 0; i < vec->size; i++) {
        BitVectorSet(vec, i, rand());
    }
}

void BitVectorSetZero(struct BitVector* vec) {
    gsl_vector_set_zero((gsl_vector*)vec->data);
}

void BitVectorFree(struct BitVector* vec) {
    gsl_vector_free((gsl_vector*)vec->data);
    free(vec);
}

// copy vec2 to vec1
void BitVectorCopy(struct BitVector* vec1, struct BitVector* vec2) {
    assert(vec1->size == vec2->size);

    gsl_vector_memcpy((gsl_vector*)vec1->data, (gsl_vector*)vec2->data);

}

unsigned int BitVectorGet(struct BitVector* vec, unsigned int loc) {
    assert(vec->size > loc);

    return (int)gsl_vector_get((gsl_vector*)vec->data, loc) % 2;
};

void BitVectorSet(struct BitVector* vec, unsigned int loc, unsigned int value) {
    assert(vec->size > loc);

    gsl_vector_set((gsl_vector*)vec->data, loc, (double) (value % 2));
}

void BitVectorFlip(struct BitVector* vec, unsigned int loc) {
    assert(vec->size > loc);

    BitVectorSet(vec, loc, 1-BitVectorGet(vec, loc));
}

int BitVectorSame(struct BitVector* vec1, struct BitVector* vec2) {
    assert(vec1->size == vec2->size);

    for (unsigned int i = 0; i < vec1->size; i++) {
        if (BitVectorGet(vec1, i) != BitVectorGet(vec2, i)) return 0;
    }
    return 1;
}

void BitVectorPrint(struct BitVector* vec) {
    printf("[");
    for (unsigned int i = 0; i < vec->size; i++) {
        printf("%d", BitVectorGet(vec, i));
    }
    printf("]\n");
}

unsigned int BitVectorInner(struct BitVector* vec1, struct BitVector* vec2) {
    assert(vec1->size == vec2->size);

    double inner;
    gsl_blas_ddot((gsl_vector*)vec1->data, (gsl_vector*)vec2->data, &inner);

    return (unsigned int)inner;
}

 // vec1 <- vec1 ^ vec2
void BitVectorXorSet(struct BitVector* vec1, struct BitVector* vec2) {
    assert(vec1->size == vec2->size);

    for (unsigned int i = 0; i < vec1->size; i++) {
        BitVectorSet(vec1, i, BitVectorGet(vec1,i) + BitVectorGet(vec2,i) % 2);
    }
}

// helper
void BitVectorMod2(struct BitVector* vec) {
    for (unsigned int i = 0; i < vec->size; i++) {
        BitVectorSet(vec, i, BitVectorGet(vec, i));
    }
}

/*************************** Bit Matrix *************************/

/*
struct BitMatrix {
    unsigned int rows;
    unsigned int cols;
    unsigned char* data;
};
*/

// alloc and init to 0
struct BitMatrix* newBitMatrixZero(unsigned int rows, unsigned int cols) {
    assert(rows > 0);
    assert(cols > 0);

    struct BitMatrix* mat = (struct BitMatrix*)malloc(sizeof(struct BitMatrix));
    mat->rows = rows;
    mat->cols = cols;

    mat->data = (unsigned char*)gsl_matrix_alloc(mat->rows, mat->cols);
    gsl_matrix_set_zero((gsl_matrix*)mat->data);

    return mat;
}

// alloc and init to identity
struct BitMatrix* newBitMatrixIdentity(unsigned int rows) {
    struct BitMatrix* mat = newBitMatrixZero(rows, rows);

    gsl_matrix_set_identity((gsl_matrix*)mat->data);

    return mat;
}

// alloc and init randomly
struct BitMatrix* newBitMatrixRandom(unsigned int rows, unsigned int cols) {
    assert(rows > 0);
    assert(cols > 0);

    struct BitMatrix* mat = newBitMatrixZero(rows, rows);
    BitMatrixSetRandom(mat);
    return mat;
}

void BitMatrixFree(struct BitMatrix* mat) {
    gsl_matrix_free((gsl_matrix*)mat->data);
    free(mat);
}

void BitMatrixCopy(struct BitMatrix* mat1, struct BitMatrix* mat2) {
    assert(mat1->rows == mat2->rows);
    assert(mat1->cols == mat2->cols);

    gsl_matrix_memcpy((gsl_matrix*)mat1->data, (gsl_matrix*)mat2->data);
}

void BitMatrixSetZero(struct BitMatrix* mat) {
    gsl_matrix_set_zero((gsl_matrix*)mat->data);
}

void BitMatrixSetRandom(struct BitMatrix* mat) {
    for (unsigned int i = 0; i < mat->rows; i++) {
        for (unsigned int j = 0; j < mat->cols; j++) {
            BitMatrixSet(mat, i, j, rand());
        }
    }
}

void BitMatrixSetIdentity(struct BitMatrix* mat) {
    gsl_matrix_set_identity((gsl_matrix*)mat->data);
}

void BitMatrixPrint(struct BitMatrix* mat) {
    printf("[");
    for (unsigned int i = 0; i < mat->rows; i++) {
        if (i != 0) printf("\n ");
        printf("[");
        for (unsigned int j = 0; j < mat->cols; j++) {
            printf("%d", BitMatrixGet(mat, i, j));
        }
        printf("]");
    }
    printf("]\n");

}

unsigned int BitMatrixGet(struct BitMatrix* mat, unsigned int row, unsigned int col) {
    assert(mat->cols > col);
    assert(mat->rows > row);

    return (int)gsl_matrix_get((gsl_matrix*)mat->data, row, col) % 2;
}

void BitMatrixSet(struct BitMatrix* mat, unsigned int row, unsigned int col, unsigned int value) {
    assert(mat->cols > col);
    assert(mat->rows > row);

    gsl_matrix_set((gsl_matrix*)mat->data, row, col, (double)(value % 2));
}

void BitMatrixFlip(struct BitMatrix* mat, unsigned int row, unsigned int col) {
    assert(mat->cols > col);
    assert(mat->rows > row);

    BitMatrixSet(mat, row, col, 1-BitMatrixGet(mat, row, col));
}

int BitMatrixSame(struct BitMatrix* mat1, struct BitMatrix* mat2) {
    assert(mat1->rows == mat2->rows);
    assert(mat1->cols == mat2->cols);

    for (unsigned int i = 0; i < mat1->rows; i++) {
        for (unsigned int j = 0; j < mat1->cols; j++) {
            if (BitMatrixGet(mat1, i, j) != BitMatrixGet(mat2, i, j)) return 0;
        }
    }
    return 1;
}

void BitMatrixXorSet(struct BitMatrix* mat1, struct BitMatrix* mat2) { // mat1 <- mat1 ^ mat2
    assert(mat1->cols == mat2->cols);
    assert(mat1->rows == mat2->cols);

    for (unsigned int i = 0; i < mat1->rows; i++) {
        for (unsigned int j = 0; j < mat1->cols; j++) {
            BitMatrixSet(mat1, i, j, BitMatrixGet(mat1,i,j) + BitMatrixGet(mat2,i,j) % 2);
        }
    }
}

void BitMatrixSetCol(struct BitMatrix* mat, struct BitVector* vec, unsigned int col) {
    assert(vec->size == mat->rows);
    assert(col < mat->cols);

    gsl_matrix_set_col((gsl_matrix*)mat->data, col, (gsl_vector*)vec->data);
}

void BitMatrixSetRow(struct BitMatrix* mat, struct BitVector* vec, unsigned int row) {
    assert(vec->size == mat->cols);
    assert(row < mat->rows);

    gsl_matrix_set_row((gsl_matrix*)mat->data, row, (gsl_vector*)vec->data);
}

struct BitVector* BitMatrixGetCol(struct BitMatrix* mat, unsigned int col) {
    assert(col < mat->cols);

    struct BitVector* vec = newBitVector(mat->rows);
    gsl_matrix_get_col((gsl_vector*)vec->data, (gsl_matrix*)mat->data, col);

    return vec;
}

struct BitVector* BitMatrixGetRow(struct BitMatrix* mat, unsigned int row) {
    assert(row < mat->rows);

    struct BitVector* vec = newBitVector(mat->cols);

    gsl_matrix_get_row((gsl_vector*)vec->data, (gsl_matrix*)mat->data, row);

    return vec;
}

void BitMatrixSwapCols(struct BitMatrix* mat, unsigned int col1, unsigned int col2) {
    gsl_matrix_swap_columns((gsl_matrix*)mat->data, col1, col2);
}

void BitMatrixSwapRows(struct BitMatrix* mat, unsigned int row1, unsigned int row2) {
    gsl_matrix_swap_rows((gsl_matrix*)mat->data, row1, row2);

}

// helper
void BitMatrixMod2(struct BitMatrix* mat) {
    for (unsigned int i = 0; i < mat->rows; i++) {
        for (unsigned int j = 0; j < mat->cols; j++) {
            BitMatrixSet(mat, i, j, BitMatrixGet(mat,i,j));
        }
    }
}

struct BitMatrix* BitMatrixTranspose(struct BitMatrix* mat) {
    struct BitMatrix* matT = newBitMatrixZero(mat->cols, mat->rows);

    gsl_matrix_transpose_memcpy((gsl_matrix*)matT->data, (gsl_matrix*)mat->data);

    return matT;
}

void BitMatrixTransposeSet(struct BitMatrix* mat) {
    struct BitMatrix* matT = BitMatrixTranspose(mat);

    mat->rows = matT->rows;
    mat->cols = matT->cols;
    BitMatrixCopy(matT, mat);

    BitMatrixFree(matT);
}

struct BitMatrix* BitMatrixMulMatrix(struct BitMatrix* mat1, struct BitMatrix* mat2) {
    assert(mat1->cols == mat2->rows);

    struct BitMatrix* matOut = newBitMatrixZero(mat1->rows, mat2->cols);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, (gsl_matrix*)mat1->data, (gsl_matrix*)mat2->data, 0, (gsl_matrix*)matOut->data);

    BitMatrixMod2(matOut);

    return matOut;
}

// mat2 <- mat1*mat2;
void BitMatrixMulMatrixLeft(struct BitMatrix* mat1, struct BitMatrix* mat2) {
    struct BitMatrix* matTmp = BitMatrixMulMatrix(mat1, mat2);
    BitMatrixCopy(mat2, matTmp);
    BitMatrixFree(matTmp);
}

// mat1 <- mat1*mat2;
void BitMatrixMulMatrixRight(struct BitMatrix* mat1, struct BitMatrix* mat2) {
    struct BitMatrix* matTmp = BitMatrixMulMatrix(mat1, mat2);
    BitMatrixCopy(mat1, matTmp);
    BitMatrixFree(matTmp);
}

struct BitVector* BitMatrixMulVector(struct BitMatrix* mat, struct BitVector* vec) {
    assert(mat->cols == vec->size);

    struct BitVector* vecOut = newBitVector(mat->rows);
    gsl_blas_dgemv(CblasNoTrans, 1, (gsl_matrix*)mat->data, (gsl_vector*)vec->data, 0, (gsl_vector*)vecOut->data);

    BitVectorMod2(vecOut);

    return vecOut;
}

// vec <- mat*vec
void BitMatrixMulVectorSet(struct BitMatrix* mat, struct BitVector* vec) {
    assert(mat->cols == vec->size);
    assert(mat->cols == mat->rows);

    gsl_blas_dgemv(CblasNoTrans, 1, (gsl_matrix*)mat->data, (gsl_vector*)vec->data, 0, (gsl_vector*)vec->data);
    BitVectorMod2(vec);
}

unsigned int BitMatrixRank(struct BitMatrix* mat) {
    struct BitMatrix* null = newBitMatrixIdentity(mat->cols);
    unsigned int rank = mat->cols; // rank of the null space

    struct BitMatrix* good = newBitMatrixZero(mat->cols, mat->cols);
    struct BitMatrix* bad = newBitMatrixZero(mat->cols, mat->cols);
    unsigned int numgood, numbad;

    for (unsigned int row = 0; row < mat->rows; row++) {
        struct BitVector* rowVec = BitMatrixGetRow(mat, row);
        numgood = 0;
        numbad = 0;

        for (unsigned int i = 0; i < rank; i++) {
            struct BitVector* nullVec = BitMatrixGetRow(null, i);

            if (BitVectorInner(nullVec, rowVec) % 2 == 0) {
                BitMatrixSetRow(good, nullVec, numgood);
                numgood += 1;
            } else {
                BitMatrixSetRow(bad, nullVec, numbad);
                numbad += 1;
            }
        }

        // copy good into null
        for (rank = 0; rank < numgood; rank++) {
            struct BitVector* goodVec = BitMatrixGetRow(good, rank);
            BitMatrixSetRow(null, goodVec, rank);
        }

        if (numbad > 1) {
            struct BitVector* flipVec = BitMatrixGetRow(bad, 0);

            for (; rank+1 < numbad+numgood; rank++) {
                struct BitVector* badVec = BitMatrixGetRow(bad, 1+rank-numgood);
                BitVectorXorSet(badVec, flipVec);
                BitMatrixSetRow(null, badVec, rank);
            }
        }
    }

    BitMatrixFree(null);
    BitMatrixFree(good);
    BitMatrixFree(bad);

    rank = mat->cols - rank; // rank of null space to rank of mat
    return rank;
}


#include <config.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>

int
main (void)
{
  int i;
  double xi, yi;
  double x[10], y[10];

  printf ("#m=0,S=2\n");

  for (i = 0; i < 10; i++)
    {
      x[i] = i + 0.5 * sin (i);
      y[i] = i + cos (i * i);
      printf ("%g %g\n", x[i], y[i]);
    }

  printf ("#m=1,S=0\n");

  {
    gsl_interp_accel *acc = gsl_interp_accel_alloc ();
    gsl_spline *spline = gsl_spline_alloc (gsl_interp_cspline, 10);
    gsl_spline_init (spline, x, y, 10);

    for (xi = x[0]; xi < x[9]; xi += 0.01)
      {
        double yi = gsl_spline_eval (spline, xi, acc);
        printf ("%g %g\n", xi, yi);
      }
    gsl_spline_free (spline);
    gsl_interp_accel_free(acc);
  }
}


#include <sstream>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_cblas.h>

// pretty print
std::string ppMatrix(const gsl_matrix *m) {
    std::ostringstream str;
    int r, c;

    str << "";
    for (r = 0; r < (int) m->size1; r++) {
        for (c = 0; c < (int) m->size2; c++) {
            str << gsl_matrix_get(m, r, c);
            if (c < m->size2 - 1) {
                str << " ";
            }
        }
        str << "\n";
    }
    return str.str();
}

void
matrixInverseCheck() {
	// Define the dimension n of the matrix
	// and the signum s (for LU decomposition)
	int n = 2;
	int s;

	// Define all the used matrices
	gsl_matrix * m = gsl_matrix_alloc (n, n);
	gsl_matrix * inverse = gsl_matrix_alloc (n, n);
	gsl_permutation * perm = gsl_permutation_alloc (n);

	// Fill the matrix m
        gsl_matrix_set (m, 0, 0, 0.378589);
        gsl_matrix_set (m, 0, 1, 0.971711);
        gsl_matrix_set (m, 0, 2, 0.016087);
        gsl_matrix_set (m, 0, 3, 0.037668);
        gsl_matrix_set (m, 0, 4, 0.312398);

        gsl_matrix_set (m, 1, 0, 0.756377);
        gsl_matrix_set (m, 1, 1, 0.345708);
        gsl_matrix_set (m, 1, 2, 0.922947);
        gsl_matrix_set (m, 1, 3, 0.846671);
        gsl_matrix_set (m, 1, 4, 0.856103);

        gsl_matrix_set (m, 2, 0, 0.732510);
        gsl_matrix_set (m, 2, 1, 0.108942);
        gsl_matrix_set (m, 2, 2, 0.476969);
        gsl_matrix_set (m, 2, 3, 0.398254);
        gsl_matrix_set (m, 2, 4, 0.507045);

        gsl_matrix_set (m, 3, 0, 0.162608);
        gsl_matrix_set (m, 3, 1, 0.227770);
        gsl_matrix_set (m, 3, 2, 0.533074);
        gsl_matrix_set (m, 3, 3, 0.807075);
        gsl_matrix_set (m, 3, 4, 0.180335);

        gsl_matrix_set (m, 4, 0, 0.517006);
        gsl_matrix_set (m, 4, 1, 0.315992);
        gsl_matrix_set (m, 4, 2, 0.914848);
        gsl_matrix_set (m, 4, 3, 0.460825);
        gsl_matrix_set (m, 4, 4, 0.731980);

	// Make LU decomposition of matrix m
	gsl_linalg_LU_decomp (m, perm, &s);

	// Invert the matrix m
	gsl_linalg_LU_invert (m, perm, inverse);
}

void
matrixOpsCheck() {
    fprintf(stdout, "\n Matrix Operations ..............................................\n");
    int L = 3, M = 3;
    int i, j;
    gsl_matrix *m = gsl_matrix_alloc(L, M);

    for (i = 0; i < L; i++)
        for (j = 0; j < M; j++)
            gsl_matrix_set(m, i, j, 0.23 + 100 * i + j);

    fprintf(stdout, "m = %s\n", ppMatrix(m).c_str());

    gsl_matrix_free(m);

    fprintf(stdout, "\n");
}

int
main(void) {
    matrixOpsCheck();

    return 0;
}


/* multiroots/fdjac.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 <gsl/gsl_multiroots.h>

int
gsl_multiroot_fdjacobian (gsl_multiroot_function * F,
                           const gsl_vector * x, const gsl_vector * f,
                           double epsrel, gsl_matrix * jacobian)
{
  const size_t n = x->size;
  const size_t m = f->size;
  const size_t n1 = jacobian->size1;
  const size_t n2 = jacobian->size2;

  if (m != n1 || n != n2)
    {
      GSL_ERROR ("function and jacobian are not conformant", GSL_EBADLEN);
    }

  {
    size_t i,j;
    gsl_vector *x1, *f1;

    x1 = gsl_vector_alloc (n);

    if (x1 == 0)
      {
	GSL_ERROR ("failed to allocate space for x1 workspace", GSL_ENOMEM);
      }

    f1 = gsl_vector_alloc (m);

    if (f1 == 0)
      {
	gsl_vector_free (x1);

	GSL_ERROR ("failed to allocate space for f1 workspace", GSL_ENOMEM);
      }

    gsl_vector_memcpy (x1, x);	/* copy x into x1 */

    for (j = 0; j < n; j++)
      {
	double xj = gsl_vector_get (x, j);
	double dx = epsrel * fabs (xj);

	if (dx == 0)
	  {
	    dx = epsrel;
	  }

	gsl_vector_set (x1, j, xj + dx);

        {
          int status = GSL_MULTIROOT_FN_EVAL (F, x1, f1);

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

	gsl_vector_set (x1, j, xj);

	for (i = 0; i < m; i++)
	  {
	    double g1 = gsl_vector_get (f1, i);
	    double g0 = gsl_vector_get (f, i);
	    gsl_matrix_set (jacobian, i, j, (g1 - g0) / dx);
	  }
      }

    gsl_vector_free (x1);
    gsl_vector_free (f1);
  }

  return GSL_SUCCESS;
}


/* linalg/test_lu.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 <gsl/gsl_test.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_permutation.h>

static int
test_LU_decomp_eps(const gsl_matrix * m, const double eps, const char * desc)
{
  int s = 0;
  const size_t M = m->size1;
  const size_t N = m->size2;

  gsl_matrix * A = gsl_matrix_alloc(M, N);
  gsl_matrix * PLU = gsl_matrix_calloc(M, N);
  gsl_permutation * p = gsl_permutation_alloc(M);
  size_t i, j;
  int signum;

  gsl_matrix_memcpy(A, m);
  gsl_linalg_LU_decomp(A, p, &signum);

  if (M >= N)
    {
      gsl_matrix_view U = gsl_matrix_submatrix(A, 0, 0, N, N);

      /* copy L factor into PLU */
      for (j = 0; j < GSL_MIN(M, N); ++j)
        {
          gsl_vector_view v1 = gsl_matrix_subcolumn(A, j, j, M - j);
          gsl_vector_view v2 = gsl_matrix_subcolumn(PLU, j, j, M - j);

          gsl_vector_memcpy(&v2.vector, &v1.vector);
          gsl_matrix_set(PLU, j, j, 1.0);
        }

      /* PLU := L * U */
      gsl_blas_dtrmm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, &U.matrix, PLU);
    }
  else
    {
      gsl_matrix_view L = gsl_matrix_submatrix(A, 0, 0, M, M);

      /* copy U factor into PLU */
      for (i = 0; i < GSL_MIN(M, N); ++i)
        {
          gsl_vector_view v1 = gsl_matrix_subrow(A, i, i, N - i);
          gsl_vector_view v2 = gsl_matrix_subrow(PLU, i, i, N - i);
          gsl_vector_memcpy(&v2.vector, &v1.vector);
        }

      /* PLU := L * U */
      gsl_blas_dtrmm(CblasLeft, CblasLower, CblasNoTrans, CblasUnit, 1.0, &L.matrix, PLU);
    }

  /* PLU := P * L * U */
  for (j = 0; j < N; ++j)
    {
      gsl_vector_view v = gsl_matrix_column(PLU, j);
      gsl_permute_vector_inverse(p, &v.vector);
    }

  /* now test m = PLU */
  for (i = 0; i < M; ++i)
    {
      for (j = 0; j < N; ++j)
        {
          double aij = gsl_matrix_get(PLU, i, j);
          double mij = gsl_matrix_get(m, i, j);

          gsl_test_rel(aij, mij, eps,
                       "%s: (%3lu,%3lu)[%lu,%lu]: %22.18g   %22.18g\n",
                       desc, M, N, i, j, aij, mij);
        }
    }

  gsl_matrix_free(A);
  gsl_matrix_free(PLU);
  gsl_permutation_free(p);

  return s;
}

static int
test_LU_decomp(gsl_rng * r)
{
  int s = 0;
  size_t n;

  /* test square matrices */
  for (n = 1; n <= 50; ++n)
    {
      gsl_matrix * m = gsl_matrix_alloc(n, n);

      create_random_matrix(m, r);
      test_LU_decomp_eps(m, 4096.0 * n * GSL_DBL_EPSILON, "LU_decomp random");

      create_hilbert_matrix2(m);
      test_LU_decomp_eps(m, 256.0 * n * GSL_DBL_EPSILON, "LU_decomp hilbert");

      gsl_matrix_free(m);
    }

  {
    gsl_matrix * m = gsl_matrix_alloc(100, 50);
    create_random_matrix(m, r);
    test_LU_decomp_eps(m, 256.0 * n * GSL_DBL_EPSILON, "LU_decomp rect1");
    gsl_matrix_free(m);
  }

  {
    gsl_matrix * m = gsl_matrix_alloc(50, 100);
    create_random_matrix(m, r);
    test_LU_decomp_eps(m, 256.0 * n * GSL_DBL_EPSILON, "LU_decomp rect2");
    gsl_matrix_free(m);
  }

  {
    gsl_matrix * m = gsl_matrix_alloc(80, 100);
    create_random_matrix(m, r);
    test_LU_decomp_eps(m, 256.0 * n * GSL_DBL_EPSILON, "LU_decomp rect3");
    gsl_matrix_free(m);
  }

  return s;
}

static int
test_LU_solve_eps(const gsl_matrix * m, const gsl_vector * rhs, const gsl_vector * sol, const double eps, const char * desc)
{
  int s = 0;
  const size_t N = m->size1;
  int signum;
  size_t i;

  gsl_permutation * perm = gsl_permutation_alloc(N);
  gsl_matrix * lu  = gsl_matrix_alloc(N, N);
  gsl_vector * x = gsl_vector_alloc(N);
  gsl_vector * residual = gsl_vector_alloc(N);

  gsl_matrix_memcpy(lu, m);

  s += gsl_linalg_LU_decomp(lu, perm, &signum);
  s += gsl_linalg_LU_solve(lu, perm, rhs, x);

  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(sol, i);

      gsl_test_rel(xi, yi, eps, "%s: %3lu[%lu]: %22.18g    %22.18g\n",
                   desc, N, i, xi, yi);
    }

  s += gsl_linalg_LU_refine(m, lu, perm, rhs, x, residual);

  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(sol, i);

      gsl_test_rel(xi, yi, eps, "%s: improved %3lu[%lu]: %22.18g    %22.18g\n",
                   desc, N, i, xi, yi);
    }

  gsl_vector_free(residual);
  gsl_vector_free(x);
  gsl_matrix_free(lu);
  gsl_permutation_free(perm);

  return s;
}

static int
test_LU_solve(gsl_rng * r)
{
  int s = 0;
  size_t n;

  for (n = 1; n <= 50; ++n)
    {
      gsl_matrix * m = gsl_matrix_alloc(n, n);
      gsl_vector * rhs = gsl_vector_alloc(n);
      gsl_vector * sol = gsl_vector_alloc(n);

      create_random_matrix(m, r);
      create_random_vector(sol, r);
      gsl_blas_dgemv(CblasNoTrans, 1.0, m, sol, 0.0, rhs);
      test_LU_solve_eps(m, rhs, sol, 1.0e5 * n * GSL_DBL_EPSILON, "LU_solve random");

      if (n <= 4)
        {
          create_hilbert_matrix2(m);
          gsl_blas_dsymv(CblasLower, 1.0, m, sol, 0.0, rhs);
          test_LU_solve_eps(m, rhs, sol, 4096.0 * n * GSL_DBL_EPSILON, "LU_solve hilbert");
        }

      gsl_matrix_free(m);
      gsl_vector_free(rhs);
      gsl_vector_free(sol);
    }

  return s;
}

int
test_LU_invert_eps(const gsl_matrix * m, const double eps, const char *desc)
{
  int s = 0;
  size_t i, j, N = m->size1;

  gsl_matrix * lu  = gsl_matrix_alloc(N, N);
  gsl_matrix * c  = gsl_matrix_alloc(N, N);
  gsl_permutation * perm = gsl_permutation_alloc(N);
  int signum;

  gsl_matrix_memcpy(lu, m);

  s += gsl_linalg_LU_decomp(lu, perm, &signum);
  s += gsl_linalg_LU_invx(lu, perm);

  /* c = m m^{-1} */
  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, m, lu, 0.0, c);

  /* c should be the identity matrix */

  for (i = 0; i < N; ++i)
    {
      for (j = 0; j < N; ++j)
        {
          double cij = gsl_matrix_get(c, i, j);
          double expected = (i == j) ? 1.0 : 0.0;

          gsl_test_rel(cij, expected, eps, "%s (%3lu,%3lu)[%lu,%lu]: %22.18g   %22.18g\n",
                       desc, N, N, i, j, cij, expected);
        }
    }

  gsl_matrix_free(lu);
  gsl_matrix_free(c);
  gsl_permutation_free(perm);

  return s;
}

static int
test_LU_invert(gsl_rng * r)
{
  int s = 0;
  size_t n;

  for (n = 1; n <= 50; ++n)
    {
      gsl_matrix * m = gsl_matrix_alloc(n, n);

      create_random_matrix(m, r);
      test_LU_invert_eps(m, 1.0e5 * n * GSL_DBL_EPSILON, "LU_invert random");

      gsl_matrix_free(m);
    }

  return s;
}


static char help[] =
"Load a matrix  A  and right-hand-side  b  from binary files (PETSc format).\n"
"Then solve the system  A x = b  using KSPSolve().\n"
"Example.  First save a system from tri.c:\n"
"  ./tri -ksp_view_mat binary:A.dat -ksp_view_rhs binary:b.dat\n"
"then load it and solve:\n"
"  ./loadsolve -fA A.dat -fb b.dat\n"
"To time the solution read the third printed number:\n"
"  ./loadsolve -fA A.dat -fb b.dat -log_view |grep KSPSolve\n"
"(This is a simpler code than src/ksp/ksp/examples/tutorials/ex10.c.)\n";

/*
small system example where RHS missing:
./tri -ksp_view_mat binary:A.dat
./loadsolve -fA A.dat -ksp_view_mat -ksp_view_rhs

large tridiagonal system (m=10^7) example:
./tri -tri_m 10000000 -ksp_view_mat binary:A.dat -ksp_view_rhs binary:b.dat
./loadsolve -fA A.dat -fb b.dat -log_view |grep KSPSolve
*/

#include <petsc.h>

int main(int argc,char **args) {
  PetscErrorCode ierr;
  Vec         x, b;
  Mat         A;
  KSP         ksp;
  int         m, n, mb;
  PetscBool   flg,
              verbose = PETSC_FALSE;
  char        nameA[PETSC_MAX_PATH_LEN] = "",
              nameb[PETSC_MAX_PATH_LEN] = "";
  PetscViewer fileA, fileb;

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

  ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"","options for loadsolve",""); CHKERRQ(ierr);
  ierr = PetscOptionsString("-fA","input file containing matrix A",
                            "loadsolve.c",nameA,nameA,PETSC_MAX_PATH_LEN,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsString("-fb","input file containing vector b",
                            "loadsolve.c",nameb,nameb,PETSC_MAX_PATH_LEN,&flg);CHKERRQ(ierr);
  ierr = PetscOptionsBool("-verbose","say what is going on",
                          "loadsolve.c",verbose,&verbose,NULL); CHKERRQ(ierr);
  ierr = PetscOptionsEnd(); CHKERRQ(ierr);
  if (strlen(nameA) == 0) {
      SETERRQ(PETSC_COMM_WORLD,1,
              "no input matrix provided ... ending  (usage: loadsolve -fA A.dat)\n");
  }

  if (verbose) {
      ierr = PetscPrintf(PETSC_COMM_WORLD,
         "reading matrix from %s ...\n",nameA); CHKERRQ(ierr);
  }
  ierr = MatCreate(PETSC_COMM_WORLD,&A);CHKERRQ(ierr);
  ierr = MatSetFromOptions(A);CHKERRQ(ierr);
  ierr = PetscViewerBinaryOpen(PETSC_COMM_WORLD,nameA,FILE_MODE_READ,&fileA);CHKERRQ(ierr);
  ierr = MatLoad(A,fileA);CHKERRQ(ierr);
  ierr = PetscViewerDestroy(&fileA);CHKERRQ(ierr);
  ierr = MatGetSize(A,&m,&n); CHKERRQ(ierr);
  if (verbose) {
      ierr = PetscPrintf(PETSC_COMM_WORLD,
         "matrix has size m x n = %d x %d ...\n",m,n); CHKERRQ(ierr);
  }
  if (m != n) {
      SETERRQ(PETSC_COMM_WORLD,2,"only works for square matrices\n");
  }

  ierr = VecCreate(PETSC_COMM_WORLD,&b);CHKERRQ(ierr);
  ierr = VecSetFromOptions(b);CHKERRQ(ierr);
  if (flg) {
      ierr = PetscViewerBinaryOpen(PETSC_COMM_WORLD,nameb,FILE_MODE_READ,&fileb);CHKERRQ(ierr);
      if (verbose) {
          ierr = PetscPrintf(PETSC_COMM_WORLD,
             "reading vector from %s ...\n",nameb); CHKERRQ(ierr);
      }
      ierr = VecLoad(b,fileb);CHKERRQ(ierr);
      ierr = PetscViewerDestroy(&fileb);CHKERRQ(ierr);
      ierr = VecGetSize(b,&mb); CHKERRQ(ierr);
      if (mb != m) {
          SETERRQ(PETSC_COMM_WORLD,3,"size of matrix and vector do not match\n");
      }
  } else {
      if (verbose) {
          ierr = PetscPrintf(PETSC_COMM_WORLD,
             "right-hand-side vector b not provided ... using zero vector of length %d\n",m); CHKERRQ(ierr);
      }
      ierr = VecSetSizes(b,PETSC_DECIDE,m); CHKERRQ(ierr);
      ierr = VecSet(b,0.0); CHKERRQ(ierr);
  }

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

  ierr = VecDuplicate(b,&x);CHKERRQ(ierr);
  ierr = KSPSolve(ksp,b,x); CHKERRQ(ierr);

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


#ifndef _MARKET_INTEGRATION_
#define _MARKET_INTEGRATION_
#include <gsl/gsl_integration.h>

typedef struct{
	double (*function)(double *x, size_t n, void * p);
	void *params;
} nquad_function;

typedef void (*multi_integration_type)(int n, nquad_function *f, double *lb, double *ub, double epsabs,
           double epsrel, size_t limit, double *result, double *abserr);

void nquad(int n, nquad_function *f,  double *lb, double *ub,
           double epsabs, double epsrel, size_t limit,
           double * result, double *abserr);

void mcint(int n, nquad_function *f, double *lb, double *ub, double epsabs,
           double epsrel, size_t limit, double *result, double *abserr);

#endif