/* eigen/gensymmv.c
 *
 * Copyright (C) 2007 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 <stdlib.h>

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

/*
 * This module computes the eigenvalues and eigenvectors of a real
 * generalized symmetric-definite eigensystem A x = \lambda B x, where
 * A and B are symmetric, and B is positive-definite.
 */

static void gensymmv_normalize_eigenvectors(gsl_matrix *evec);

/*
gsl_eigen_gensymmv_alloc()

Allocate a workspace for solving the generalized symmetric-definite
eigenvalue problem. The size of this workspace is O(4n).

Inputs: n - size of matrices

Return: pointer to workspace
*/

gsl_eigen_gensymmv_workspace *
gsl_eigen_gensymmv_alloc(const size_t n)
{
  gsl_eigen_gensymmv_workspace *w;

  if (n == 0)
    {
      GSL_ERROR_NULL ("matrix dimension must be positive integer",
                      GSL_EINVAL);
    }

  w = (gsl_eigen_gensymmv_workspace *) calloc (1, sizeof (gsl_eigen_gensymmv_workspace));

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

  w->size = n;

  w->symmv_workspace_p = gsl_eigen_symmv_alloc(n);
  if (!w->symmv_workspace_p)
    {
      gsl_eigen_gensymmv_free(w);
      GSL_ERROR_NULL("failed to allocate space for symmv workspace", GSL_ENOMEM);
    }

  return (w);
} /* gsl_eigen_gensymmv_alloc() */

/*
gsl_eigen_gensymmv_free()
  Free workspace w
*/

void
gsl_eigen_gensymmv_free (gsl_eigen_gensymmv_workspace * w)
{
  RETURN_IF_NULL (w);

  if (w->symmv_workspace_p)
    gsl_eigen_symmv_free(w->symmv_workspace_p);

  free(w);
} /* gsl_eigen_gensymmv_free() */

/*
gsl_eigen_gensymmv()

Solve the generalized symmetric-definite eigenvalue problem

A x = \lambda B x

for the eigenvalues \lambda and eigenvectors x.

Inputs: A    - real symmetric matrix
        B    - real symmetric and positive definite matrix
        eval - where to store eigenvalues
        evec - where to store eigenvectors
        w    - workspace

Return: success or error
*/

int
gsl_eigen_gensymmv (gsl_matrix * A, gsl_matrix * B, gsl_vector * eval,
                    gsl_matrix * evec, gsl_eigen_gensymmv_workspace * w)
{
  const size_t N = A->size1;

  /* check matrix and vector sizes */

  if (N != A->size2)
    {
      GSL_ERROR ("matrix must be square to compute eigenvalues", GSL_ENOTSQR);
    }
  else if ((N != B->size1) || (N != B->size2))
    {
      GSL_ERROR ("B matrix dimensions must match A", GSL_EBADLEN);
    }
  else if (eval->size != N)
    {
      GSL_ERROR ("eigenvalue vector must match matrix size", GSL_EBADLEN);
    }
  else if (evec->size1 != evec->size2)
    {
      GSL_ERROR ("eigenvector matrix must be square", GSL_ENOTSQR);
    }
  else if (evec->size1 != N)
    {
      GSL_ERROR ("eigenvector matrix has wrong size", GSL_EBADLEN);
    }
  else if (w->size != N)
    {
      GSL_ERROR ("matrix size does not match workspace", GSL_EBADLEN);
    }
  else
    {
      int s;

      /* compute Cholesky factorization of B */
      s = gsl_linalg_cholesky_decomp(B);
      if (s != GSL_SUCCESS)
        return s; /* B is not positive definite */

      /* transform to standard symmetric eigenvalue problem */
      gsl_eigen_gensymm_standardize(A, B);

      /* compute eigenvalues and eigenvectors */
      s = gsl_eigen_symmv(A, eval, evec, w->symmv_workspace_p);
      if (s != GSL_SUCCESS)
        return s;

      /* backtransform eigenvectors: evec -> L^{-T} evec */
      gsl_blas_dtrsm(CblasLeft,
                     CblasLower,
                     CblasTrans,
                     CblasNonUnit,
                     1.0,
                     B,
                     evec);

      /* the blas call destroyed the normalization - renormalize */
      gensymmv_normalize_eigenvectors(evec);

      return GSL_SUCCESS;
    }
} /* gsl_eigen_gensymmv() */

/********************************************
 *           INTERNAL ROUTINES              *
 ********************************************/

/*
gensymmv_normalize_eigenvectors()
  Normalize eigenvectors so that their Euclidean norm is 1

Inputs: evec - eigenvectors
*/

static void
gensymmv_normalize_eigenvectors(gsl_matrix *evec)
{
  const size_t N = evec->size1;
  size_t i;     /* looping */

  for (i = 0; i < N; ++i)
    {
      gsl_vector_view vi = gsl_matrix_column(evec, i);
      double scale = 1.0 / gsl_blas_dnrm2(&vi.vector);

      gsl_blas_dscal(scale, &vi.vector);
    }
} /* gensymmv_normalize_eigenvectors() */


#include "gemm.h"
#include "utils.h"
#include "cuda.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <time.h>

#include <omp.h>

#ifdef OPENBLAS
#include <cblas.h>
#endif

#ifdef OPENGEMM
#include "gemm_kernels.h"
#endif

void gemm_bin(int M, int N, int K, real_t ALPHA, char *A, int lda, real_t *B,
		int ldb, real_t *C, int ldc) {
	int i, j, k;
	for (i = 0; i < M; ++i) {
		for (k = 0; k < K; ++k) {
			char A_PART = A[i * lda + k];
			if (A_PART) {
				for (j = 0; j < N; ++j) {
					C[i * ldc + j] += B[k * ldb + j];
				}
			} else {
				for (j = 0; j < N; ++j) {
					C[i * ldc + j] -= B[k * ldb + j];
				}
			}
		}
	}
}

real_t *random_matrix(int rows, int cols) {
	int i;
	real_t *m = calloc(rows * cols, sizeof(real_t));
	for (i = 0; i < rows * cols; ++i) {
		m[i] = (real_t) rand() / RAND_MAX;
	}
	return m;
}

void time_random_matrix(int TA, int TB, int m, int k, int n) {
	real_t *a;
	if (!TA)
		a = random_matrix(m, k);
	else
		a = random_matrix(k, m);
	int lda = (!TA) ? k : m;
	real_t *b;
	if (!TB)
		b = random_matrix(k, n);
	else
		b = random_matrix(n, k);
	int ldb = (!TB) ? n : k;

	real_t *c = random_matrix(m, n);
	int i;
	clock_t start = clock(), end;
	for (i = 0; i < 10; ++i) {
		gemm_cpu(TA, TB, m, n, k, 1, a, lda, b, ldb, 1, c, n);
	}
	end = clock();
	printf("Matrix Multiplication %dx%d * %dx%d, TA=%d, TB=%d: %lf ms\n", m, k,
			k, n, TA, TB, (real_t)(end - start) / CLOCKS_PER_SEC);
	free(a);
	free(b);
	free(c);
}

void gemm(int TA, int TB, int M, int N, int K, real_t ALPHA, real_t *A, int lda,
		real_t *B, int ldb, real_t BETA, real_t *C, int ldc) {
	gemm_cpu(TA, TB, M, N, K, ALPHA, A, lda, B, ldb, BETA, C, ldc);
}

void gemm_nn(int M, int N, int K, real_t ALPHA, real_t *A, int lda, real_t *B,
		int ldb, real_t *C, int ldc) {
	int i, j, k;
#pragma omp parallel for
	for (i = 0; i < M; ++i) {
		for (k = 0; k < K; ++k) {
			register real_t A_PART = ALPHA * A[i * lda + k];
			for (j = 0; j < N; ++j) {
				C[i * ldc + j] += A_PART * B[k * ldb + j];
			}
		}
	}
}

void gemm_nt(int M, int N, int K, real_t ALPHA, real_t *A, int lda, real_t *B,
		int ldb, real_t *C, int ldc) {
	int i, j, k;
#pragma omp parallel for
	for (i = 0; i < M; ++i) {
		for (j = 0; j < N; ++j) {
			register real_t sum = 0;
			for (k = 0; k < K; ++k) {
				sum += ALPHA * A[i * lda + k] * B[j * ldb + k];
			}
			C[i * ldc + j] += sum;
		}
	}
}

void gemm_tn(int M, int N, int K, real_t ALPHA, real_t *A, int lda, real_t *B,
		int ldb, real_t *C, int ldc) {
	int i, j, k;
#pragma omp parallel for
	for (i = 0; i < M; ++i) {
		for (k = 0; k < K; ++k) {
			register real_t A_PART = ALPHA * A[k * lda + i];
			for (j = 0; j < N; ++j) {
				C[i * ldc + j] += A_PART * B[k * ldb + j];
			}
		}
	}
}

void gemm_tt(int M, int N, int K, real_t ALPHA, real_t *A, int lda, real_t *B,
		int ldb, real_t *C, int ldc) {
	int i, j, k;
#pragma omp parallel for
	for (i = 0; i < M; ++i) {
		for (j = 0; j < N; ++j) {
			register real_t sum = 0;
			for (k = 0; k < K; ++k) {
				sum += ALPHA * A[i + k * lda] * B[k + j * ldb];
			}
			C[i * ldc + j] += sum;
		}
	}
}

void gemm_cpu(int TA, int TB, int M, int N, int K, real_t ALPHA, real_t *A,
		int lda, real_t *B, int ldb, real_t BETA, real_t *C, int ldc) {
	//printf("cpu: %d %d %d %d %d %f %d %d %f %d\n",TA, TB, M, N, K, ALPHA, lda, ldb, BETA, ldc);

#ifdef OPENBLAS
	/*
	void cblas_sgemm 	( 	const CBLAS_LAYOUT  	layout,
		const CBLAS_TRANSPOSE  	TransA,
		const CBLAS_TRANSPOSE  	TransB,
		const int  	M,
		const int  	N,
		const int  	K,
		const float  	alpha,
		const float *  	A,
		const int  	lda,
		const float *  	B,
		const int  	ldb,
		const float  	beta,
		float *  	C,
		const int  	ldc
	)
	 */
	CBLAS_TRANSPOSE transa, transb;

	if (!TA && !TB){
		transa = CblasNoTrans;
		transb = CblasNoTrans;
	}else if (TA && !TB){
		transa = CblasTrans;
		transb = CblasNoTrans;
	}else if (!TA && TB){
		transa = CblasNoTrans;
		transb = CblasTrans;
	}else{
		transa = CblasTrans;
		transb = CblasTrans;
	}
	//	              0          1       2     3  4  5    6    7   8   9   10  11     12  13
	cblas_sgemm(CblasRowMajor, transa, transb, M, N, K, ALPHA, A, lda, B, ldb, BETA,  C, ldc);
#else

	int i, j;
	for (i = 0; i < M; ++i) {
		for (j = 0; j < N; ++j) {
			C[i * ldc + j] *= BETA;
		}
	}

	if (!TA && !TB)
		gemm_nn(M, N, K, ALPHA, A, lda, B, ldb, C, ldc);
	else if (TA && !TB)
		gemm_tn(M, N, K, ALPHA, A, lda, B, ldb, C, ldc);
	else if (!TA && TB)
		gemm_nt(M, N, K, ALPHA, A, lda, B, ldb, C, ldc);
	else
		gemm_tt(M, N, K, ALPHA, A, lda, B, ldb, C, ldc);
#endif
}

#ifdef GPU

void gemm_gpu(int TA, int TB, int M, int N, int K, real_t ALPHA, real_t *A_gpu,
		int lda, real_t *B_gpu, int ldb, real_t BETA, real_t *C_gpu, int ldc,
		unsigned char use_tensor_cores, cudaStream_t st) {
	cublasHandle_t handle = blas_handle(use_tensor_cores);
	cublasSetStream(handle, st);
//	if(TB || TA)
//	{
//		printf("Matrix need to be transposed %d %d\n", TB, TA);
//	}
#ifndef OPENGEMM

#if REAL_TYPE == HALF
	//run_cuda_gemm_half(int TA, int TB, int M, int N, int K, real_t ALPHA, real_t *A_gpu,
//	int lda, real_t *B_gpu, int ldb, real_t BETA, real_t *C_gpu, int ldc)
	run_cuda_gemm_half(handle, TA, TB, M, N, K, ALPHA, A_gpu, lda, B_gpu, ldb, BETA, C_gpu, ldc, st);
#elif REAL_TYPE == FLOAT
	cudaError_t status = (cudaError_t) cublasSgemm(handle, (TB ? CUBLAS_OP_T : CUBLAS_OP_N),
			(TA ? CUBLAS_OP_T : CUBLAS_OP_N), N, M, K, &ALPHA, B_gpu, ldb,
			A_gpu, lda, &BETA, C_gpu, ldc);
	check_error(status);
#elif REAL_TYPE == DOUBLE
	cudaError_t status = (cudaError_t) cublasDgemm(handle, (TB ? CUBLAS_OP_T : CUBLAS_OP_N),
			(TA ? CUBLAS_OP_T : CUBLAS_OP_N), N, M, K, &ALPHA, B_gpu, ldb,
			A_gpu, lda, &BETA, C_gpu, ldc);
	check_error(status);
#endif

#else

#if REAL_TYPE == HALF
	run_cuda_gemm_half(handle, TA, TB, M, N, K, ALPHA, A_gpu, lda, B_gpu, ldb, BETA, C_gpu, ldc, st);
#elif REAL_TYPE == FLOAT
	sgemm((TB ? CUBLAS_OP_T : CUBLAS_OP_N),
			(TA ? CUBLAS_OP_T : CUBLAS_OP_N), N, M, K, &ALPHA, B_gpu, ldb,
			A_gpu, lda, &BETA, C_gpu, ldc);

#elif REAL_TYPE == DOUBLE
	dgemm((TB ? CUBLAS_OP_T : CUBLAS_OP_N),
			(TA ? CUBLAS_OP_T : CUBLAS_OP_N), N, M, K, &ALPHA, B_gpu, ldb,
			A_gpu, lda, &BETA, C_gpu, ldc);

#endif

#endif

//	cublasHandle_t handle = blas_handle();
//	cudaError_t status = cublasSgemm(handle, (TB ? CUBLAS_OP_T : CUBLAS_OP_N),
//			(TA ? CUBLAS_OP_T : CUBLAS_OP_N), N, M, K, &ALPHA, B_gpu, ldb,
//			A_gpu, lda, &BETA, C_gpu, ldc);

}

void time_gpu_random_matrix(int TA, int TB, int m, int k, int n) {
	real_t *a;
	if (!TA)
		a = random_matrix(m, k);
	else
		a = random_matrix(k, m);
	int lda = (!TA) ? k : m;
	real_t *b;
	if (!TB)
		b = random_matrix(k, n);
	else
		b = random_matrix(n, k);
	int ldb = (!TB) ? n : k;

	real_t *c = random_matrix(m, n);
	int i;
	clock_t start = clock(), end;
	for (i = 0; i < 32; ++i) {
		gemm_gpu(TA, TB, m, n, k, 1, a, lda, b, ldb, 1, c, n, 0, 0x0);
	}
	end = clock();
	printf("Matrix Multiplication %dx%d * %dx%d, TA=%d, TB=%d: %lf s\n", m, k,
			k, n, TA, TB, (real_t)(end - start) / CLOCKS_PER_SEC);
	free(a);
	free(b);
	free(c);
}

void time_gpu(int TA, int TB, int m, int k, int n) {
	int iter = 10;
	real_t *a = random_matrix(m, k);
	real_t *b = random_matrix(k, n);

	int lda = (!TA) ? k : m;
	int ldb = (!TB) ? n : k;

	real_t *c = random_matrix(m, n);

	real_t *a_cl = cuda_make_array(a, m * k);
	real_t *b_cl = cuda_make_array(b, k * n);
	real_t *c_cl = cuda_make_array(c, m * n);

	int i;
	clock_t start = clock(), end;
	for (i = 0; i < iter; ++i) {
		gemm_gpu(TA, TB, m, n, k, 1, a_cl, lda, b_cl, ldb, 1, c_cl, n, 0, 0x0);
		cudaDeviceSynchronize();
	}
	double flop = ((double) m) * n * (2. * k + 2.) * iter;
	double gflop = flop / pow(10., 9);
	end = clock();
	double seconds = sec(end - start);
	printf(
			"Matrix Multiplication %dx%d * %dx%d, TA=%d, TB=%d: %lf s, %lf GFLOPS\n",
			m, k, k, n, TA, TB, seconds, gflop / seconds);
	cuda_free(a_cl);
	cuda_free(b_cl);
	cuda_free(c_cl);
	free(a);
	free(b);
	free(c);
}

void test_gpu_accuracy(int TA, int TB, int m, int k, int n) {
	srand(0);
	real_t *a;
	if (!TA)
		a = random_matrix(m, k);
	else
		a = random_matrix(k, m);
	int lda = (!TA) ? k : m;
	real_t *b;
	if (!TB)
		b = random_matrix(k, n);
	else
		b = random_matrix(n, k);
	int ldb = (!TB) ? n : k;

	real_t *c = random_matrix(m, n);
	real_t *c_gpu = random_matrix(m, n);
	memset(c, 0, m * n * sizeof(real_t));
	memset(c_gpu, 0, m * n * sizeof(real_t));
	int i;
	//pm(m,k,b);
	gemm_gpu(TA, TB, m, n, k, 1, a, lda, b, ldb, 1, c_gpu, n, 0, 0x0);
	//printf("GPU\n");
	//pm(m, n, c_gpu);

	gemm_cpu(TA, TB, m, n, k, 1, a, lda, b, ldb, 1, c, n);
	//printf("\n\nCPU\n");
	//pm(m, n, c);
	double sse = 0;
	for (i = 0; i < m * n; ++i) {
		//printf("%f %f\n", c[i], c_gpu[i]);
		sse += pow(c[i] - c_gpu[i], 2);
	}
	printf("Matrix Multiplication %dx%d * %dx%d, TA=%d, TB=%d: %g SSE\n", m, k,
			k, n, TA, TB, sse / (m * n));
	free(a);
	free(b);
	free(c);
	free(c_gpu);
}

int test_gpu_blas() {
	/*
	 test_gpu_accuracy(0,0,10,576,75);

	 test_gpu_accuracy(0,0,17,10,10);
	 test_gpu_accuracy(1,0,17,10,10);
	 test_gpu_accuracy(0,1,17,10,10);
	 test_gpu_accuracy(1,1,17,10,10);

	 test_gpu_accuracy(0,0,1000,10,100);
	 test_gpu_accuracy(1,0,1000,10,100);
	 test_gpu_accuracy(0,1,1000,10,100);
	 test_gpu_accuracy(1,1,1000,10,100);

	 test_gpu_accuracy(0,0,10,10,10);

	 time_gpu(0,0,64,2916,363);(cudaError_t) cublasDgemm
	 time_gpu(0,0,64,2916,363);
	 time_gpu(0,0,64,2916,363);
	 time_gpu(0,0,192,729,1600);
	 time_gpu(0,0,384,196,1728);
	 time_gpu(0,0,256,196,3456);
	 time_gpu(0,0,256,196,2304);
	 time_gpu(0,0,128,4096,12544);
	 time_gpu(0,0,128,4096,4096);
	 */
	time_gpu(0, 0, 64, 75, 12544);
	time_gpu(0, 0, 64, 75, 12544);
	time_gpu(0, 0, 64, 75, 12544);
	time_gpu(0, 0, 64, 576, 12544);
	time_gpu(0, 0, 256, 2304, 784);
	time_gpu(1, 1, 2304, 256, 784);
	time_gpu(0, 0, 512, 4608, 196);
	time_gpu(1, 1, 4608, 512, 196);

	return 0;
}
#endif


/****************************************************************
  gsl.h

  Defines wrappers for gsl functions.

  Language: C++

  Mark A. Caprio
  University of Notre Dame

  - 3/26/17 (mac): Created, based on code from spncci utilities (aem).

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

#ifndef MCUTILS_GSL_H_
#define MCUTILS_GSL_H_

#include <gsl/gsl_sf_gamma.h>

namespace mcutils {

  // GSL factorial wrappers
  //
  // Use more efficient int implementation for int arguments, else use
  // generic gamma function for double arguments.

  inline double Factorial(int x)
  {
    return gsl_sf_fact(x);
  }

  inline double Factorial(double x)
  {
    return gsl_sf_gamma(x+1);
  }

  // GSL combinatorial function wrappers

  inline int Choose(int x, int y)
  {
    int choose=0;
    if ((x>=y)&&(y>=0))
      {
        gsl_sf_result result;
        gsl_sf_choose_e(x,y,&result);
        choose=result.val;
      }
    return choose;
  }

} // namespace

#endif


static char help[] = "DAE system solver example using TS.  Models two\n"
"equal-mass balls (particles) moving in the plane with a rigid connecting\n"
"rod between.  (Alternatively, -rod_free removes the rod and the motion\n"
"is free and independent.)  A stabilized index-2 constrained Lagrangian\n"
"dynamics formulation is used.  We use cartesian coordinates (x,y) and\n" "velocities (v=dx/dt,w=dy/dt). The system has dimension 10.\n\n";

// DEBUG possibly a good solution using BDF3 and -snes_fd
// ./rod -ts_type bdf -ksp_type preonly -pc_type svd -ts_monitor binary:t.dat -ts_monitor_solution binary:u.dat -ts_dt 0.01 -ts_max_time 1.0 -snes_fd -ts_bdf_order 3
// ./trajectory.py -o figure.png t.dat u.dat

// DEBUG check Jacobian for rod problem using one backward-Euler step
// (2nd case with -snes_fd):
// ./rod -ts_type beuler -ts_max_time 0.1 -ts_dt 0.1 -ksp_type preonly -pc_type svd -snes_monitor -ksp_view_mat
// ./rod -ts_type beuler -ts_max_time 0.1 -ts_dt 0.1 -ksp_type preonly -pc_type svd -snes_monitor -ksp_view_mat -snes_fd

// DEBUG check BDF2 convergence in free problem using Lagrangian formulation:
//for T in 2 3 4 5 7 8 9 10 11 12; do ./rod -rod_free -ts_type bdf -ts_rtol 1.0e-$T -ts_atol 1.0e-$T; done

#include <petsc.h>

typedef struct {
    PetscBool    free;
    PetscReal    g,     // m s-2;  acceleration of gravity
                 m,     // kg;     ball mass
                 l;     // m;      spring or rod length
} RodCtx;

extern PetscErrorCode SetInitial(Vec, RodCtx*);
extern PetscErrorCode FreeExact(Vec, PetscReal, Vec, RodCtx*);
extern PetscErrorCode LagrangeIFcn(TS, PetscReal, Vec, Vec, Vec, void*);
extern PetscErrorCode LagrangeIJac(TS, PetscReal, Vec, Vec, PetscReal,
                                   Mat, Mat, void*);

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    PetscInt       steps;
    PetscReal      t0 = 0.0, tf = 2.0, dt = 0.1, errnorm;
    Vec            u, u0, uexact;
    Mat            A;
    TS             ts;
    RodCtx         user;
    char           probstr[20] = "rod";

    ierr = PetscInitialize(&argc, &argv, NULL, help); if (ierr) return ierr;

    user.free = PETSC_FALSE;
    user.g = 9.81;
    user.m = 58.0e-3; // 58 g for a tennis ball
    user.l = 0.5;     // rod length, and determines initial condition
    ierr = PetscOptionsBegin(PETSC_COMM_WORLD, "rod_", "options for rod",
                             ""); CHKERRQ(ierr);
    ierr = PetscOptionsBool("-free","remove rod for free motion",
                            "rod.c", user.free, &user.free,
                            NULL); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-g", "acceleration of gravity (m s-2)",
                            "rod.c", user.g, &user.g, NULL); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-l", "rod length (m)", "rod.c",
                            user.l, &user.l, NULL); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-m", "mass of each ball (kg)", "rod.c",
                            user.m, &user.m, NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);

    ierr = VecCreate(PETSC_COMM_WORLD,&u); CHKERRQ(ierr);
    ierr = TSCreate(PETSC_COMM_WORLD,&ts); CHKERRQ(ierr);
    ierr = TSSetApplicationContext(ts,&user); CHKERRQ(ierr);
    ierr = TSSetProblemType(ts,TS_NONLINEAR); CHKERRQ(ierr);

    // set time axis
    ierr = TSSetTime(ts,t0); CHKERRQ(ierr);
    ierr = TSSetMaxTime(ts,tf); CHKERRQ(ierr);
    ierr = TSSetTimeStep(ts,dt); CHKERRQ(ierr);
    ierr = TSSetExactFinalTime(ts,TS_EXACTFINALTIME_MATCHSTEP); CHKERRQ(ierr);

    // set up dimension, equation type, and solver type
    ierr = VecSetSizes(u,PETSC_DECIDE,10); CHKERRQ(ierr);
    ierr = MatCreate(PETSC_COMM_WORLD,&A); CHKERRQ(ierr);
    ierr = MatSetSizes(A,PETSC_DECIDE,PETSC_DECIDE,10,10); CHKERRQ(ierr);
    ierr = MatSetFromOptions(A); CHKERRQ(ierr);
    ierr = MatSetUp(A); CHKERRQ(ierr);
    if (user.free) {
        ierr = TSSetEquationType(ts, TS_EQ_DAE_IMPLICIT_INDEX1);
            CHKERRQ(ierr);
    } else {
        ierr = TSSetEquationType(ts, TS_EQ_DAE_IMPLICIT_INDEX2);
            CHKERRQ(ierr);
    }
    ierr = TSSetIFunction(ts,NULL,LagrangeIFcn,&user); CHKERRQ(ierr);
    ierr = TSSetIJacobian(ts,A,A,LagrangeIJac,&user);CHKERRQ(ierr);
    ierr = TSSetType(ts,TSBEULER); CHKERRQ(ierr); // FIXME BDF3 as default?

    // set-up of u, ts is complete
    ierr = VecSetFromOptions(u); CHKERRQ(ierr);
    ierr = TSSetFromOptions(ts); CHKERRQ(ierr);

    // set initial values and solve
    ierr = SetInitial(u, &user); CHKERRQ(ierr);
    ierr = TSSolve(ts, u); CHKERRQ(ierr);
    ierr = TSGetTime(ts, &tf); CHKERRQ(ierr);

    // numerical error in free case
    if (user.free) {
        ierr = VecDuplicate(u, &uexact); CHKERRQ(ierr);
        // get initial condition for evaluating exact solution
        ierr = VecDuplicate(u, &u0); CHKERRQ(ierr);
        ierr = SetInitial(u0, &user); CHKERRQ(ierr);
        ierr = FreeExact(u0, tf, uexact, &user); CHKERRQ(ierr);
        //ierr = VecView(uexact, PETSC_VIEWER_STDOUT_WORLD); CHKERRQ(ierr);
        ierr = VecAXPY(u, -1.0, uexact); CHKERRQ(ierr); // u <- u - uexact
        ierr = VecNorm(u, NORM_INFINITY, &errnorm); CHKERRQ(ierr);
        VecDestroy(&u0);  VecDestroy(&uexact);
        ierr = PetscPrintf(PETSC_COMM_WORLD,
               "numerical error at tf in free problem: |u-uexact|_inf = %.5e\n",
               errnorm); CHKERRQ(ierr);
    }

    // report
    ierr = TSGetStepNumber(ts,&steps); CHKERRQ(ierr);
    if (user.free)
        strcpy(probstr, "free");
    ierr = PetscPrintf(PETSC_COMM_WORLD,
               "%s problem solved to tf = %.3f (%d steps)\n",
               probstr, tf, steps); CHKERRQ(ierr);

    MatDestroy(&A);  VecDestroy(&u);  TSDestroy(&ts);
    return PetscFinalize();
}

/* Regarding the following functions:
In the rod case the solution is a 10-dimensional vector u.
Here is the correspondence with the notes (doc/rod.pdf):
u[0] = q_1 (= x_1)
u[1] = q_2 (= y_1)
u[2] = q_3 (= x_2)
u[3] = q_4 (= y_2)
u[4] = v_1 (= dx_1/dt)
u[5] = v_2 (= dy_1/dt)
u[6] = v_3 (= dx_2/dt)
u[7] = v_4 (= dy_2/dt)
u[8] = mu
u[9] = lambda
*/

PetscErrorCode SetInitial(Vec u, RodCtx *user) {
    /* Set initial conditions compatible with the rod constraint
        0 = g(q) = (1/2) ( (q1 - q3)^2 + (q2 - q4)^2 - l^2 )    (1)
    and the velocity constraint
        0 = G(q) v = (q1 - q3)(v1 - v3) + (q2 - q4)(v2 - v4)    (2)
    The initial conditions are based on the location of the first mass
    being at cartesian location (q1,q2)=(0,1) and the second at
    (q3,q4)=(0,1+l).  The initial velocity of the first mass is
    (v1,v2)=(10,10) and the second is (v3,v4)=(15,10).  In fact we
    adjust q4 and v4 from the other values so as to satisfy constraints
    (1) and (2).  The initial value mu=0 is always set.  We set lambda
    from the other values according to the acceleration constraint
        lambda = (m / (2 l^2)) ((v1-v3)^2 + (v2-v4)^2)          (3)
    Initial values of mu and lambda are ignored in BDF solutions. */
    PetscErrorCode   ierr;
    const PetscReal  c = user->m / (2.0 * user->l * user->l);
    PetscReal        *au;
    ierr = VecGetArray(u,&au); CHKERRQ(ierr);
    au[0] = 0.0;              // q1 = x1
    au[1] = 1.0;              // q2 = y1
    au[2] = 0.0;              // q3 = x2
    au[3] = au[1] + user->l;  // q4 = y2;  satisfies (1)
    au[4] = 10.0;             // v1
    au[5] = 10.0;             // v2
    au[6] = 15.0;             // v3
    au[7] = au[5];            // v4;  satisfies (2)
    au[8] = 0.0;          // mu
    if (user->free)
        au[9] = 0.0;
    else // set lambda to satisfy (3)
        au[9] = c * ( (au[4] - au[6]) * (au[4] - au[6])
                     + (au[5] - au[7]) * (au[5] - au[7]));
    ierr = VecRestoreArray(u,&au); CHKERRQ(ierr);
    return 0;
}

PetscErrorCode FreeExact(Vec u0, PetscReal tf, Vec uexact, RodCtx *user) {
    /* Exact solution based on parabolic motion.  Problem
        m x'' = 0,       x(0) = x0,  x'(0) = v0
        m y'' = - m g,   y(0) = y0,  y'(0) = w0
    has solution
        x(t) = x0 + v0 t,              x'(t) = v0,
        y(t) = y0 + w0 t - (g/2) t^2,  y'(t) = w0 - g t    */
    PetscErrorCode ierr;
    PetscReal *au0, *auex;
    if (!user->free) {
        SETERRQ(PETSC_COMM_SELF,7,"exact solution only implemented for free\n");
    }
    ierr = VecGetArray(u0, &au0); CHKERRQ(ierr);
    ierr = VecGetArray(uexact, &auex); CHKERRQ(ierr);
    auex[0] = au0[0] + au0[4] * tf;
    auex[1] = au0[1] + au0[5] * tf - 0.5 * user->g * tf * tf;
    auex[2] = au0[2] + au0[6] * tf;
    auex[3] = au0[3] + au0[7] * tf - 0.5 * user->g * tf * tf;
    auex[4] = au0[4];
    auex[5] = au0[5] - user->g * tf;
    auex[6] = au0[6];
    auex[7] = au0[7] - user->g * tf;
    auex[8] = 0.0;  // mu
    auex[9] = 0.0;  // lambda
    ierr = VecRestoreArray(u0, &au0); CHKERRQ(ierr);
    ierr = VecRestoreArray(uexact, &auex); CHKERRQ(ierr);
    return 0;
}

PetscErrorCode LagrangeIFcn(TS ts, PetscReal t, Vec u, Vec udot, Vec F,
                            void *ctx) {
    PetscErrorCode   ierr;
    RodCtx           *user = (RodCtx*)ctx;
    const PetscReal  *au, *audot, mg = user->m * user->g;
    PetscReal        *aF, dx, dy, dvx, dvy;

    PetscFunctionBeginUser;
    ierr = VecGetArrayRead(u,&au); CHKERRQ(ierr);
    ierr = VecGetArrayRead(udot,&audot); CHKERRQ(ierr);
    ierr = VecGetArray(F,&aF); CHKERRQ(ierr);
    dx = au[0] - au[2];
    dy = au[1] - au[3];
    dvx = au[4] - au[6];
    dvy = au[5] - au[7];
    aF[0] = audot[0] - au[4] + au[8] * dx;
    aF[1] = audot[1] - au[5] + au[8] * dy;
    aF[2] = audot[2] - au[6] - au[8] * dx;
    aF[3] = audot[3] - au[7] - au[8] * dy;
    aF[4] = user->m * audot[4] + au[9] * dx;
    aF[5] = user->m * audot[5] + mg + au[9] * dy;
    aF[6] = user->m * audot[6] - au[9] * dx;
    aF[7] = user->m * audot[7] + mg - au[9] * dy;
    if (user->free) { // trivial index 1 DAE
        aF[8] = au[8];             // equation:  mu = 0
        aF[9] = au[9];             // equation:  lambda = 0
    } else {
        aF[8] = 0.5 * ( dx * dx + dy * dy - user->l * user->l );  // constraint
        aF[9] = dx * dvx + dy * dvy;  // velocity constraint
    }
    ierr = VecRestoreArrayRead(u,&au); CHKERRQ(ierr);
    ierr = VecRestoreArrayRead(udot,&audot); CHKERRQ(ierr);
    ierr = VecRestoreArray(F,&aF); CHKERRQ(ierr);
    PetscFunctionReturn(0);
}

PetscErrorCode LagrangeIJac(TS ts, PetscReal t, Vec u, Vec udot,
                            PetscReal sigma, Mat J, Mat Jpre, void *ctx) {
    PetscErrorCode   ierr;
    RodCtx           *user = (RodCtx*)ctx;
    const PetscReal  *au;
    PetscInt         row, col[8], n;    // max nonzeros in a row of J is 4
    PetscReal        val[8];

    PetscFunctionBeginUser;
    ierr = VecGetArrayRead(u,&au); CHKERRQ(ierr);
    // construct Jacobian by rows, inserting nonzeros
    row = 0;  n = 4;   // row 0 has 4 nonzeros ...
    col[0] = 0;  col[1] = 2;  col[2] = 4;  col[3] = 8;
    val[0] = sigma + au[8];  val[1] = - au[8];
    val[2] = -1;             val[3] = au[0] - au[2];
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    row = 1;  n = 4;
    col[0] = 1;  col[1] = 3;  col[2] = 5;  col[3] = 8;
    val[0] = sigma + au[8];  val[1] = - au[8];
    val[2] = -1;             val[3] = au[1] - au[3];
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    row = 2;  n = 4;
    col[0] = 0;  col[1] = 2;  col[2] = 6;  col[3] = 8;
    val[0] = - au[8];        val[1] = sigma + au[8];
    val[2] = -1;             val[3] = -(au[0] - au[2]);
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    row = 3;  n = 4;
    col[0] = 1;  col[1] = 3;  col[2] = 7;  col[3] = 8;
    val[0] = - au[8];        val[1] = sigma + au[8];
    val[2] = -1;             val[3] = -(au[1] - au[3]);
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    row = 4;  n = 4;
    col[0] = 0;  col[1] = 2;  col[2] = 4;  col[3] = 9;
    val[0] = au[9];          val[1] = - au[9];
    val[2] = sigma*user->m;  val[3] = au[0] - au[2];
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    row = 5;  n = 4;
    col[0] = 1;  col[1] = 3;  col[2] = 5;  col[3] = 9;
    val[0] = au[9];          val[1] = - au[9];
    val[2] = sigma*user->m;  val[3] = au[1] - au[3];
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    row = 6;  n = 4;
    col[0] = 0;  col[1] = 2;  col[2] = 6;  col[3] = 9;
    val[0] = -au[9];         val[1] = au[9];
    val[2] = sigma*user->m;  val[3] = -(au[0] - au[2]);
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    row = 7;  n = 4;
    col[0] = 1;  col[1] = 3;  col[2] = 7;  col[3] = 9;
    val[0] = -au[9];         val[1] = au[9];
    val[2] = sigma*user->m;  val[3] = -(au[1] - au[3]);
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    if (user->free) {
        n = 1;
        row = 8;  col[0] = 8;  val[0] = 1.0; // equation mu = 0
        ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
        row = 9;  col[0] = 9;  val[0] = 1.0; // equation lambda = 0
        ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    } else {
        n = 4;  row = 8;  n = 4;
        col[0] = 0;  col[1] = 1;  col[2] = 2;  col[3] = 3;
        val[0] = au[0] - au[2];     val[1] = au[1] - au[3];
        val[2] = -(au[0] - au[2]);  val[3] = -(au[1] - au[3]);
        ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
        n = 4;  row = 9;  n = 8;
        col[0] = 0;  col[1] = 1;  col[2] = 2;  col[3] = 3;
        col[4] = 4;  col[5] = 5;  col[6] = 6;  col[7] = 7;
        val[0] = au[4] - au[6];     val[1] = au[5] - au[7];
        val[2] = -(au[4] - au[6]);  val[3] = -(au[5] - au[7]);
        val[4] = au[0] - au[2];     val[5] = au[1] - au[3];
        val[6] = -(au[0] - au[2]);  val[7] = -(au[1] - au[3]);
        ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    }
    ierr = VecRestoreArrayRead(u,&au); CHKERRQ(ierr);
    ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    if (J != Jpre) {
        ierr = MatAssemblyBegin(Jpre,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
        ierr = MatAssemblyEnd(Jpre,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    }
    PetscFunctionReturn(0);
}


/* Standalone c program to use viscdisk to run the Pringle (1981) ring
   problem, with a complex equation of state */

#include <math.h>
#include <stdio.h>
#include "init.h"
#include "vader.h"
#include "vader_common.h"
#include <gsl/gsl_sf_bessel.h>

/* Physical constants */
#define KB              1.3806488e-16
#define MH              1.6737236e-24
#define MU              0.61
#define A               7.5657314e-15

/* Problem parameters */

/* Set this to a non-empty value to restart from a checkpoint */
#define CHECKPOINT      "ringrad_00001.vader"

/* Grid parmeters */
#define NR              4096
#define RMIN            1.5e10
#define RMAX            1.5e12
#define LINEAR          1

/* EOS parameters */
#define EOS_FUNC        1
#define GAMMA           1.66666666667
#define DELTA           0.0

/* Viscosity parameters */
#define ALPHA_FUNC      1
#define ALPHA           0.0

/* Inner boundary condition */
#define IBC_PRES_TYPE   FIXED_TORQUE
#define IBC_ENTH_TYPE   FIXED_ENTHALPY_VALUE
#define IBC_FUNC        1
#define IBC_PRES_VAL    0.0
#define IBC_ENTH_VAL    0.0

/* Outer boundary condition */
#define OBC_PRES_TYPE   FIXED_TORQUE
#define OBC_ENTH_TYPE   FIXED_ENTHALPY_VALUE
#define OBC_FUNC        1
#define OBC_PRES_VAL    0.0
#define OBC_ENTH_VAL    0.0

/* Source functions */
#define MASS_SRC_FUNC   0
#define MASS_SRC_VAL    0.0
#define INT_EN_SRC_FUNC 0
#define INT_EN_SRC_VAL  0.0

/* Control parameters */
#define ERR_TOL         1.0e-6
#define DT_TOL          0.1
#define MAX_ITER        40
#define INTERP_ORDER    1
#define MAX_DT_INCREASE 1.5
#define DT_MIN          1.0e-20
#define MAX_STEP        100000
#define USE_BE          0
#define PRE_TIMESTEP    0
#define POST_TIMESTEP   0
#define VERBOSITY       1

/* Output parameters */
#define NSAVE            65
#define OUTFILE         "ringrad.out"
#define NUSEROUT        0
#define CHECKNAME       "ringrad"
#define USERREADCHK     false
#define USERWRITECHK    false

/* Time parameters */
#define DT_START        -1
#define END_TIME        0.128

/* Problem-specific parameters*/
#define MSTAR           (1.99e33*1.4)
#define RING_MASS       1.99e27
#define INIT_TEMP       1.0e4
#define COL_RATIO       1.0e10
#define RING_LOC        7.5e11
#define NU              5.93e8
#define FZ0             7.5e9
#define GAMMA_GAS       (5.0/3.0)

/* Main */
int main () {

  grid *grd;
  double col0, t0, colAnalyt, x, tau;
  double *col, *pres, *eInt;
  double *colOut, *presOut, *mBndOut, *eBndOut, *eIntOut, *tOut;
  double param[5];
  double tSave[NSAVE];
  bool writeCheckpoint[NSAVE];
  char *checkpoint = CHECKPOINT;
  int i, j, idx;
  FILE *fp;
  unsigned long nStep, nIter, nFail, nOut;

  /* Allocate grid and workspace */
  grd = gridInitKeplerian(NR, RMIN, RMAX, MSTAR, LINEAR);

  /* Allocate data */
  col = (double *) calloc(NR, sizeof(double));
  pres = (double *) calloc(NR, sizeof(double));
  eInt = (double *) calloc(NR, sizeof(double));

  /* Initialize column density and pressure arrays, and associated variables */
  col0 = RING_MASS / (M_PI*SQR(RING_LOC));
  t0 = SQR(RING_LOC)/(12.0*NU);
  for (i=0, idx=-1; i<NR; i++)
    if ((grd->r_h[i] < RING_LOC) && (RING_LOC <= grd->r_h[i+1])) idx = i;
  for (i=0; i<NR; i++)
    col[i] = RING_MASS / grd->area[idx] / COL_RATIO;
  col[idx] = RING_MASS / grd->area[idx];
  for (i=0; i<NR; i++) {
    pres[i] = col[i]*INIT_TEMP*KB/(MU*MH) + (1./3.)*FZ0*A*SQR(SQR(INIT_TEMP));
    eInt[i] = col[i]*INIT_TEMP*KB/(MU*MH*(GAMMA_GAS-1)) +
      FZ0*A*SQR(SQR(INIT_TEMP));
  }
  param[0] = NU;
  param[1] = RING_LOC;
  param[2] = RING_MASS;
  param[3] = RING_MASS / grd->area[idx] / COL_RATIO;
  param[4] = INIT_TEMP*KB/(MU*MH);

  /* Set output times */
  for (i=0; i<NSAVE; i++) {
    tSave[i] = t0 * i*END_TIME/((float) NSAVE-1);
    writeCheckpoint[i] = true;
  }

  /* Run the simulation */
  vader(
	/* Restart file name; if this is empty or NULL, calculation is
	   run from the start */
	checkpoint,
	/* Start and end time */
	0.0, END_TIME*t0,
	/* Equation of state parameters */
	EOS_FUNC, GAMMA, DELTA,
	/* Viscosity parameters */
	ALPHA_FUNC, ALPHA,
	/* Inner boundary condition */
	IBC_PRES_TYPE, IBC_ENTH_TYPE,
	IBC_FUNC, IBC_PRES_VAL, IBC_ENTH_VAL,
	/* Outer boundary condition */
	OBC_PRES_TYPE, OBC_ENTH_TYPE,
	OBC_FUNC, OBC_PRES_VAL, OBC_ENTH_VAL,
	/* Source functions */
	MASS_SRC_FUNC, MASS_SRC_VAL,
	INT_EN_SRC_FUNC, INT_EN_SRC_VAL,
	/* Control and method parameters */
	DT_START, DT_MIN, DT_TOL, ERR_TOL, MAX_DT_INCREASE,
	MAX_ITER, INTERP_ORDER, MAX_STEP, USE_BE,
	PRE_TIMESTEP, POST_TIMESTEP, VERBOSITY,
	/* Output control parameters */
	NSAVE, tSave, NUSEROUT, NULL,
	writeCheckpoint, CHECKNAME,
	USERREADCHK, USERWRITECHK,
	/* Computational grid */
	&grd,
	/* Initial conditions, holders for final conditions */
	&col, &pres, &eInt,
	/* User-defined extra parameters */
	&param,
	/* Diagnostic outputs */
	&nStep, &nIter, &nFail, &nOut,
	/* Simulation outputs */
	&tOut, &colOut, &presOut, &eIntOut, &mBndOut, &eBndOut,
	NULL, NULL, NULL
	);

  /* Print diagnostic output */
  printf("Total iterations = %ld, failed convergences = %ld\n",
	 nIter, nFail);

  /* Write output to file */
  if (!(fp=fopen(OUTFILE, "w"))) {
    fprintf(stderr, "Unable to open output file %s!\n", OUTFILE);
    exit(1);
  }
  fprintf(fp, "time/ts        x              col            colExact       pres           eInt\n");
  for (i=0; i<nOut; i++) {
    for (j=0; j<NR; j++) {
      x = grd->r_g[j+1]/RING_LOC;
      tau = tOut[i]/t0;
      colAnalyt = col0/ (pow(x, 0.25)*tau) * exp(-SQR(x-1.0)/tau) *
	gsl_sf_bessel_Inu_scaled(0.25, 2*x/tau);
      if (!isfinite(colAnalyt)) colAnalyt=0.0;
      if (colAnalyt < RING_MASS / grd->area[idx] / COL_RATIO)
	colAnalyt = RING_MASS / grd->area[idx] / COL_RATIO;
      fprintf(fp, "%e   %e   %e   %e   %e   %e\n",
	      tau, x,
	      colOut[grd->nr*i+j],
	      colAnalyt,
	      presOut[grd->nr*i+j],
	      eIntOut[grd->nr*i+j]);
    }
  }
  fclose(fp);

  /* Return successful exit */
  return(0);
}


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

struct _FINCHAIN {
  int length; // Length of state vector
  int currentIter;

  double *currentState, *avgState, *varState, *proposedState, *_M2;
  double logLHDCurrentState;
  double accProb, avgAccProb;

  gsl_rng *r;
};

typedef struct _FINCHAIN FINCHAIN;

/*
  Initialises the finite dimensional chain
*/
void finmcmc_initChain(FINCHAIN *C, int n);

/*
  Frees the finite dimensional chain
*/
void finmcmc_freeChain(FINCHAIN *C);

void finmcmc_printChain(FINCHAIN C);

void finmcmc_writeChain(const FINCHAIN *C, FILE *fp);

void finmcmc_gausRanVec(const FINCHAIN *C, double *x, double stdDev);

/*
  Proposes a standard RWMH move
*/
void finmcmc_proposeRWMH(const FINCHAIN *C, double stdDev, double beta);

/*
  Update chain
*/
void finmcmc_updateRWMH(FINCHAIN *C, double logLHDOfProposal);

//void fin_initChain(FINCHAIN *C, int n, int sizePhi);

//void sampleRMWH(CHAIN *C);
//void sampleIndependenceSampler(CHAIN *C);
//void updateMean(CHAIN *C);
//void updateVar(CHAIN *C);
//void randomPriorDraw(gsl_rng *r, double PRIOR_ALPHA, fftw_complex *randDrawCoeffs);


#include <gsl/gsl_sf_airy.h>
#include <gsl/gsl_errno.h>

/* function names generated by removing  "gsl_sf" from the beginning
   of the name.  Thus gsl_sf_airy_Ai goes to airy_Ai. */

static int sf_mode[] = { GSL_PREC_DOUBLE, GSL_PREC_SINGLE, GSL_PREC_APPROX } ;

void airy_Ai_e(double *x, int *len, int *mode, double *val, double *err, int *status)
{
	int i;
	gsl_sf_result result;
	gsl_set_error_handler_off();

	for(i = 0; i< *len ; i++){
		status[i] = gsl_sf_airy_Ai_e(x[i], sf_mode[*mode], &result) ;
		val[i] = result.val;
		err[i] = result.err;
	}
}

void airy_Bi_e(double *x, int *len, int *mode, double *val, double *err, int *status)
{
	int i;
	gsl_sf_result result;

	for(i = 0; i< *len ; i++){
		status[i] = gsl_sf_airy_Bi_e(x[i], sf_mode[*mode], &result) ;
		val[i] = result.val;
		err[i] = result.err;
	}
}

void airy_Ai_scaled_e(double *x, int *len, int *mode, double *val, double *err, int *status)
{
	int i;
	gsl_sf_result result;

	for(i = 0; i< *len ; i++){
		status[i] = gsl_sf_airy_Ai_scaled_e(x[i], sf_mode[*mode], &result) ;
		val[i] = result.val;
		err[i] = result.err;
	}
}

void airy_Bi_scaled_e(double *x, int *len, int *mode, double *val, double *err, int *status)
{
	int i;
	gsl_sf_result result;

	for(i = 0; i< *len ; i++){
		status[i] = gsl_sf_airy_Bi_scaled_e(x[i], sf_mode[*mode], &result) ;
		val[i] = result.val;
		err[i] = result.err;
	}
}

void airy_Ai_deriv_e(double *x, int *len, int *mode, double *val, double *err, int *status)
{
	int i;
	gsl_sf_result result;

	for(i = 0; i< *len ; i++){
		status[i] = gsl_sf_airy_Ai_deriv_e(x[i], sf_mode[*mode], &result) ;
		val[i] = result.val;
		err[i] = result.err;
	}
}

void airy_Bi_deriv_e(double *x, int *len, int *mode, double *val, double *err, int *status)
{
	int i;
	gsl_sf_result result;

	for(i = 0; i< *len ; i++){
		status[i] = gsl_sf_airy_Bi_deriv_e(x[i], sf_mode[*mode], &result) ;
		val[i] = result.val;
		err[i] = result.err;
	}
}

void airy_Ai_deriv_scaled_e(double *x, int *len, int *mode, double *val, double *err, int *status)
{
	int i;
	gsl_sf_result result;

	for(i = 0; i< *len ; i++){
		status[i] = gsl_sf_airy_Ai_deriv_scaled_e(x[i], sf_mode[*mode], &result) ;
		val[i] = result.val;
		err[i] = result.err;
	}
}

void airy_Bi_deriv_scaled_e(double *x, int *len, int *mode, double *val, double *err, int *status)
{
	int i;
	gsl_sf_result result;

	for(i = 0; i< *len ; i++){
		status[i] = gsl_sf_airy_Bi_deriv_scaled_e(x[i], sf_mode[*mode], &result) ;
		val[i] = result.val;
		err[i] = result.err;
	}
}

void airy_zero_Ai_e(int *n, int *len, double *val, double *err, int *status)
{
	int i;
	gsl_sf_result result;
	for(i = 0; i< *len ; i++){
		if(n[i]>0){
			status[i] = gsl_sf_airy_zero_Ai_e(n[i], &result) ;
		} else {
			result.val=0.0;
			result.err=GSL_EDOM;
		}
		val[i] = result.val;
		err[i] = result.err;
	}
}

void airy_zero_Bi_e(int *n, int *len, double *val, double *err, int *status)
{
	int i;
	gsl_sf_result result;
	for(i = 0; i< *len ; i++){
		if(n[i]>0){
			status[i] = gsl_sf_airy_zero_Bi_e(n[i], &result) ;
		} else {
			result.val=0.0;
			result.err=GSL_EDOM;
		}
		val[i] = result.val;
		err[i] = result.err;
	}
}

void airy_zero_Ai_deriv_e(int *n, int *len, double *val, double *err, int *status)
{
	int i;
	gsl_sf_result result;
	for(i = 0; i< *len ; i++){
		if(n[i]>0){
			status[i] = gsl_sf_airy_zero_Ai_deriv_e(n[i], &result) ;
		} else {
			result.val=0.0;
			result.err=GSL_EDOM;
		}
		val[i] = result.val;
		err[i] = result.err;
	}
}

void airy_zero_Bi_deriv_e(int *n, int *len, double *val, double *err, int *status)
{
	int i;
	gsl_sf_result result;
	for(i = 0; i< *len ; i++){
		if(n[i]>0){
			status[i] = gsl_sf_airy_zero_Bi_deriv_e(n[i], &result) ;
		} else {
			result.val=0.0;
			result.err=GSL_EDOM;
		}
		val[i] = result.val;
		err[i] = result.err;
	}
}


/* linalg/test_common.c
 *
 * Copyright (C) 2017 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_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_rng.h>

static int create_random_vector(gsl_vector * v, gsl_rng * r);
static int create_posdef_matrix(gsl_matrix * m, gsl_rng * r);
static int create_hilbert_matrix2(gsl_matrix * m);

static int
create_random_vector(gsl_vector * v, gsl_rng * r)
{
  const size_t N = v->size;
  size_t i;

  for (i = 0; i < N; ++i)
    {
      double vi = gsl_rng_uniform(r);
      gsl_vector_set(v, i, vi);
    }

  return GSL_SUCCESS;
}

static int
create_symm_matrix(gsl_matrix * m, gsl_rng * r)
{
  const size_t N = m->size1;
  size_t i, j;

  for (i = 0; i < N; ++i)
    {
      for (j = 0; j <= i; ++j)
        {
          double mij = gsl_rng_uniform(r);
          gsl_matrix_set(m, i, j, mij);
        }
    }

  /* copy lower triangle to upper */
  gsl_matrix_transpose_tricpy('L', 0, m, m);

  return GSL_SUCCESS;
}

static int
create_posdef_matrix(gsl_matrix * m, gsl_rng * r)
{
  const size_t N = m->size1;
  const double alpha = 10.0 * N;
  size_t i;

  /* The idea is to make a symmetric diagonally dominant
   * matrix. Make a symmetric matrix and add alpha*I to
   * its diagonal */

  create_symm_matrix(m, r);

  for (i = 0; i < N; ++i)
    {
      double mii = gsl_matrix_get(m, i, i);
      gsl_matrix_set(m, i, i, mii + alpha);
    }

  return GSL_SUCCESS;
}

static int
create_hilbert_matrix2(gsl_matrix * m)
{
  const size_t N = m->size1;
  size_t i, j;

  for (i = 0; i < N; i++)
    {
      for (j = 0; j < N; j++)
        {
          gsl_matrix_set(m, i, j, 1.0/(i+j+1.0));
        }
    }

  return GSL_SUCCESS;
}


#ifndef _MATH_UTIL_EULER_H_
#define	_MATH_UTIL_EULER_H_

/* space group numbers for structures known by this program */
/* these are from the International Tables, allowed values are [1-230] */
/*
 * #define FCC 225
 * #define BCC 229
 * #define DIA 227
 * #define SIMPLE_CUBIC 221
 * #define HEX 194
 * #define SAPPHIRE 167
 * #define TRICLINIC 1
 */

#ifndef CHECK_FREE
#define CHECK_FREE(A)   { if(A) free(A); (A)=NULL;}
#endif

#ifdef _MSC_VER						/* identifies this as a Microsoft compiler */
#include <gsl/gsl_math.h>			/* added RX2011 for NaN handling */
#define round(A) ceil(A-0.5)		/* added RX2011 for round() function */
#define NAN gsl_nan()				/* added RX2011 */
#define isNAN(A) gsl_isnan(A)		/* added RX2011 */
#endif

#ifndef NAN							/* probably only needed for pcs */
#define NAN nan("")					/* probably only needed for cygwin on pc */
#endif

#ifndef isNAN						/* a good test for NAN  */
#define isNAN(A) ( (A) != (A) )
#endif

#define VECTOR_COPY3(A,B) {A[0]=B[0]; A[1]=B[1]; A[2]=B[2];}

void DeletePoints(size_t len, void *ptr, size_t pntLen, size_t numDel);

void EulerMatrix(double alpha,double beta,double gamma,double M_Euler[3][3]);
void rot2EulerAngles(double A[3][3], double *alpha, double *beta, double *gamma);
void MatrixRz(double Rz[3][3],double angle);
void MatrixRy(double Ry[3][3],double angle);

/* void lowestOrderHKL(int hkl[3]); */
/* void lowestAllowedHKL(int hkl[3], int structure); */
/* long allowedHKL(long h, long k, long l, int structure); */
int gcf(int n1, int n2, int n3);

double normalize3(double a[3]);
void cross(double a[3], double b[3], double c[3]);
void vector3cons(double a[3], double x);
double dot3(double a[3],double b[3]);
double determinant33(double a[3][3]);
void MatrixMultiply31(double a[3][3], double v[3], double c[3]);
void MatrixMultiply33(double a[3][3], double b[3][3], double c[3][3]);
void MatrixTranspose33(double A[3][3]);					/* transpose the 3x3 matrix A */
void MatrixCopy33(double dest[3][3], double source[3][3]);
double diff3(double a[3], double b[3]);
double matsDelta(double a[3][3], double b[3][3]);

char *num2sexigesmal(char str[40], double seconds, long places);

#endif


#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <gsl/gsl_sf_trig.h>
#include <Accelerate/Accelerate.h>
#include "SE_trans.h"
#include "sigma.h"

#define ALPHA 0.99
#define DOMD 3.14

double* AllocVec(int dim)
{
  double* vec;
  vec = (double*)calloc(dim, sizeof(double));

  /* Check memory */
  if (vec == NULL) {
    free(vec);
    fprintf(stderr,"Vector was not allocated! \n");
    exit(1);
  }

  return vec; /* Return the first address of the allocated memory */
}

void FreeVec(double* v)
{
  free(v);
}

double f(double t)
{
  return 0.25 * ((1+t) * log1p(t) + (1-t) * log1p(-t) - 2 * M_LN2) / M_LN2;
}

double G(double x)
{
  return 0.25 * x * SE_trans_div(-1, 1, x) / M_LN2;
}

double Fapp(double* p, double t, int m)
{
  double h = sqrt(M_PI*DOMD / (ALPHA*m));

  int j;

  double sum = 0;

  for (j = -m; j <= m; j++) {
    sum += p[j+m] * gsl_sf_sinc((SE_trans_inv(-1, 1, t) - j*h) / h);
  }

  return sum;
}

double eta(double x)
{
  return 0.5*(1 + x);
}

int main()
{
  int i, j;
  int n, N;
  double t, h;
  double err, maxerr;
  double Iast;
  clock_t start, end;
  double time;
  double *J, *g, *c;

  int STEP = 1000;

  for (n = 3; n <= 180; n += 6) {
    start = clock();

    N = 2*n+1;
    J = AllocVec(N*N);
    g = AllocVec(N);
    c = AllocVec(N);

    h = sqrt(M_PI*DOMD / (ALPHA*n));

    Iast = 0;
    for (i = -n; i <= n; i++) {
      Iast += G(i*h);
    }
    Iast *= h;

    for (i = -n; i <= n; i++) {
      for (j = -n; j <= n; j++) {
        J[(i+n)*N + (j+n)] = 0.5 + (double)sigma_orig[1000 + (i - j)];
      }
      g[(i+n)] = G(i*h) - 0.5 * Iast * SE_trans_div(-1, 1, i*h);
    }

    /* c = h J g */
    cblas_dgemv(CblasRowMajor, CblasNoTrans, N, N, h, J, N, g, 1, 0.0, c, 1);

    maxerr = 0;
    for (i = - STEP + 1; i <= STEP - 1; i++) {
      t = (double)i / (double)(STEP);

      err = fabs(f(t) - Fapp(c, t, n) - Iast*eta(t));
      maxerr = fmax(err, maxerr);
    }
    end = clock();

    time = (double)(end - start) / CLOCKS_PER_SEC;
    printf("%d\t%e\t%e\n", n, err, time);

    FreeVec(J);
    FreeVec(g);
    FreeVec(c);
  }

  return EXIT_SUCCESS;
}


/*  These functions compute linear preprocessing for
    the UE using LAPACKE and CBLAS modules of
    LAPACK libraries.
    MMSE and MMSE whitening filters are available.
    Functions are using RowMajor storage of the
    matrices, like in conventional C. Traditional
    Fortran functions of LAPACK employ ColumnMajor
    data storage. */

#include<stdio.h>
#include<math.h>
#include<complex.h>
#include <stdlib.h>
#include <cblas.h>
#include <string.h>
#include <linux/version.h>
#if RHEL_RELEASE_CODE >= 1796
    #include <lapacke/lapacke_utils.h>
    #include <lapacke/lapacke.h>
#else
    #include <lapacke_utils.h>
    #include <lapacke.h>
#endif
//#define DEBUG_PREPROC

void transpose(int N, float complex *A, float complex *Result)
{
    // COnputes C := alpha*op(A)*op(B) + beta*C,
    enum CBLAS_TRANSPOSE transa = CblasTrans;
    enum CBLAS_TRANSPOSE transb = CblasNoTrans;
    int rows_opA = N; // number of rows in op(A) and in C
    int col_opB = N; //number of columns of op(B) and in C
    int col_opA = N; //number of columns in op(A) and rows in op(B)
    int col_B; //number of columns in B
    float complex alpha = 1.0 + I * 0;
    int lda  = rows_opA;
    float complex beta = 0.0 + I * 0;
    int ldc = rows_opA;
    int i;
    float complex *B;

    int ldb = col_opB;

    if(transb == CblasNoTrans)
    {
        B = (float complex *)calloc(ldb * col_opB, sizeof(float complex));
        col_B = col_opB;
    }
    else
    {
        B = (float complex *)calloc(ldb * col_opA, sizeof(float complex));
        col_B = col_opA;
    }
    float complex *C = (float complex *)malloc(ldc * col_opB * sizeof(float complex));

    for(i = 0; i < lda * col_B; i += N + 1)
    {
        B[i] = 1.0 + I * 0;
    }

    cblas_cgemm(CblasRowMajor, transa, transb, rows_opA, col_opB, col_opA, &alpha, A, lda, B, ldb, &beta, C, ldc);

    memcpy(Result, C, N * N * sizeof(float complex));

    free(B);
    free(C);
}

void conjugate_transpose(int N, float complex *A, float complex *Result)
{
    // Computes C := alpha*op(A)*op(B) + beta*C,
    enum CBLAS_TRANSPOSE transa = CblasConjTrans;
    enum CBLAS_TRANSPOSE transb = CblasNoTrans;
    int rows_opA = N; // number of rows in op(A) and in C
    int col_opB = N; //number of columns of op(B) and in C
    int col_opA = N; //number of columns in op(A) and rows in op(B)
    int col_B; //number of columns in B
    float complex alpha = 1.0 + I * 0;
    int lda  = rows_opA;
    float complex beta = 0.0 + I * 0;
    int ldc = rows_opA;
    int i;
    float complex *B;
    int ldb = col_opB;

    if(transb == CblasNoTrans)
    {
        B = (float complex *)calloc(ldb * col_opB, sizeof(float complex));
        col_B = col_opB;
    }
    else
    {
        B = (float complex *)calloc(ldb * col_opA, sizeof(float complex));
        col_B = col_opA;
    }
    float complex *C = (float complex *)malloc(ldc * col_opB * sizeof(float complex));

    for(i = 0; i < lda * col_B; i += N + 1)
    {
        B[i] = 1.0 + I * 0;
    }

    cblas_cgemm(CblasRowMajor, transa, transb, rows_opA, col_opB, col_opA, &alpha, A, lda, B, ldb, &beta, C, ldc);

    memcpy(Result, C, N * N * sizeof(float complex));

    free(B);
    free(C);
}

void H_hermH_plus_sigma2I(int N, int M, float complex *A, float sigma2, float complex *Result)
{
    //C := alpha*op(A)*op(B) + beta*C,
    enum CBLAS_TRANSPOSE transa = CblasConjTrans;
    enum CBLAS_TRANSPOSE transb = CblasNoTrans;
    int rows_opA = N; // number of rows in op(A) and in C
    int col_opB = N; //number of columns of op(B) and in C
    int col_opA = N; //number of columns in op(A) and rows in op(B)
    int col_C = N; //number of columns in B
    float complex alpha = 1.0 + I * 0;
    int lda  = col_opA;
    float complex beta = 1.0 + I * 0;
    int ldc = col_opA;
    int i;

    float complex *C = (float complex *)calloc(ldc * col_opB, sizeof(float complex));

    for(i = 0; i < lda * col_C; i += N + 1)
    {
        C[i] = sigma2 * (1.0 + I * 0);
    }

    cblas_cgemm(CblasRowMajor, transa, transb, rows_opA, col_opB, col_opA, &alpha, A, lda, A, lda, &beta, C, ldc);

    memcpy(Result, C, N * M * sizeof(float complex));
    free(C);

}

void HH_herm_plus_sigma2I(int M, int N, float complex *A, float sigma2, float complex *Result)
{
    //C := alpha*op(A)*op(B) + beta*C,
    enum CBLAS_TRANSPOSE transa = CblasNoTrans;
    enum CBLAS_TRANSPOSE transb = CblasConjTrans;
    int k = N; //number of columns in op(A) and rows in op(B),k
    float complex alpha = 1.0 + I * 0;
    int lda  = N;
    int ldb  = N;
    int ldc = M;
    int i;

    float complex *C = (float complex *)calloc(M * M, sizeof(float complex));

    for(i = 0; i < M * M; i += M + 1)
    {
        C[i] = 1.0 + I * 0;
    }

    cblas_cgemm(CblasRowMajor, transa, transb, M, M, k, &alpha, A, lda, A, ldb, &sigma2, C, ldc);

    memcpy(Result, C, M * M * sizeof(float complex));
    free(C);

}

void eigen_vectors_values(int N, float complex *A, float complex *Vectors, float *Values_Matrix)
{
    // This function computes ORTHONORMAL eigenvectors and eigenvalues of matrix A,
    // where Values_Matrix is a diagonal matrix of eigenvalues.
    // A=Vectors*Values_Matrix*Vectors'
    char jobz = 'V';
    char uplo = 'U';
    int order_A = N;
    int lda = N;
    int i;
    float *Values = (float *)malloc(sizeof(float) * 1 * N);

    LAPACKE_cheev(LAPACK_ROW_MAJOR, jobz, uplo, order_A, A, lda, Values);

    memcpy(Vectors, A, N * N * sizeof(float complex));

    for(i = 0; i < lda; i += 1)
    {
        Values_Matrix[i * (lda + 1)] = Values[i];
    }

    free(Values);
}

void lin_eq_solver(int N, float complex *A, float complex *B, float complex *Result)
{
    int n = N;
    int lda = N;
    int ldb = N;
    int nrhs = N;

    char transa = 'N';
    int *IPIV = malloc(N * N * sizeof(int));

    // Compute LU-factorization
    LAPACKE_cgetrf(LAPACK_ROW_MAJOR, n, nrhs, A, lda, IPIV);

    // Solve AX=B
    LAPACKE_cgetrs(LAPACK_ROW_MAJOR, transa, n, nrhs, A, lda, IPIV, B, ldb);

    // cgetrs( "N", N, 4, A, lda, IPIV, B, ldb, INFO )

    memcpy(Result, B, N * N * sizeof(float complex));

    free(IPIV);

}

void mutl_matrix_matrix_row_based(float complex *M0, float complex *M1, int rows_M0, int col_M0, int rows_M1, int col_M1, float complex *Result)
{
    enum CBLAS_TRANSPOSE transa = CblasNoTrans;
    enum CBLAS_TRANSPOSE transb = CblasNoTrans;
    int rows_opA = rows_M0; // number of rows in op(A) and in C
    int col_opB = col_M1; //number of columns of op(B) and in C
    int col_opA = col_M0; //number of columns in op(A) and rows in op(B)
    float complex alpha = 1.0;
    int lda  = col_M0;
    float complex beta = 0.0;
    int ldc = col_M1;
    int ldb = col_M1;

#ifdef DEBUG_PREPROC
    int i = 0;
    printf("rows_M0 %d, col_M0 %d, rows_M1 %d, col_M1 %d\n", rows_M0, col_M0, rows_M1, col_M1);

    for(i = 0; i < rows_M0 * col_M0; ++i)
    {
        printf(" rows_opA = %d, col_opB = %d, W_MMSE[%d] = (%f + i%f)\n", rows_opA, col_opB,  i, creal(M0[i]), cimag(M0[i]));
    }

    for(i = 0; i < rows_M1 * col_M1; ++i)
    {
        printf(" M1[%d] = (%f + i%f)\n", i, creal(M1[i]), cimag(M1[i]));
    }
#endif

    cblas_cgemm(CblasRowMajor, transa, transb, rows_opA, col_opB, col_opA, &alpha, M0, lda, M1, ldb, &beta, Result, ldc);

#ifdef DEBUG_PREPROC
    for(i = 0; i < rows_opA * col_opB; ++i)
    {
        printf(" result[%d] = (%f + i%f)\n", i, creal(Result[i]), cimag(Result[i]));
    }
#endif

}
void mutl_matrix_matrix_col_based(float complex *M0, float complex *M1, int rows_M0, int col_M0, int rows_M1, int col_M1, float complex *Result)
{
    enum CBLAS_TRANSPOSE transa = CblasNoTrans;
    enum CBLAS_TRANSPOSE transb = CblasNoTrans;
    int rows_opA = rows_M0; // number of rows in op(A) and in C
    int col_opB = col_M1; //number of columns of op(B) and in C
    int col_opA = col_M0; //number of columns in op(A) and rows in op(B)
    float complex alpha = 1.0;
    int lda  = col_M0;
    float complex beta = 0.0;
    int ldc = rows_M1;
    int ldb = rows_M1;

#ifdef DEBUG_PREPROC
    int i = 0;
    printf("rows_M0 %d, col_M0 %d, rows_M1 %d, col_M1 %d\n", rows_M0, col_M0, rows_M1, col_M1);

    for(i = 0; i < rows_M0 * col_M0; ++i)
    {
        printf(" rows_opA = %d, col_opB = %d, W_MMSE[%d] = (%f + i%f)\n", rows_opA, col_opB,  i, creal(M0[i]), cimag(M0[i]));
    }

    for(i = 0; i < rows_M1 * col_M1; ++i)
    {
        printf(" M1[%d] = (%f + i%f)\n", i, creal(M1[i]), cimag(M1[i]));
    }
#endif

    cblas_cgemm(CblasColMajor, transa, transb, rows_opA, col_opB, col_opA, &alpha, M0, lda, M1, ldb, &beta, Result, ldc);

#ifdef DEBUG_PREPROC
    for(i = 0; i < rows_opA * col_opB; ++i)
    {
        printf(" result[%d] = (%f + i%f)\n", i, creal(Result[i]), cimag(Result[i]));
    }
#endif
}

/*FILTERS */
void compute_MMSE(float complex *H, int order_H, float sigma2, float complex *W_MMSE)
{
    int N = order_H;
    float complex *H_hermH_sigmaI = malloc(N * N * sizeof(float complex));
    float complex *H_herm =  malloc(N * N * sizeof(float complex));

    H_hermH_plus_sigma2I(N, N, H, sigma2, H_hermH_sigmaI);

#ifdef DEBUG_PREPROC
    int i = 0;
    for(i = 0; i < N * N; i++)
    {
        printf(" H_hermH_sigmaI[%d] = (%f + i%f)\n", i, creal(H_hermH_sigmaI[i]), cimag(H_hermH_sigmaI[i]));
    }
#endif

    conjugate_transpose(N, H, H_herm);  //equals H_herm

#ifdef DEBUG_PREPROC
    for(i = 0; i < N * N; i++)
    {
        printf(" H_herm[%d] = (%f + i%f)\n", i, creal(H_herm[i]), cimag(H_herm[i]));
    }
#endif

    lin_eq_solver(N, H_hermH_sigmaI, H_herm, W_MMSE);

#ifdef DEBUG_PREPROC
    for(i = 0; i < N * N; i++)
    {
        printf(" W_MMSE[%d] = (%f + i%f)\n", i, creal(W_MMSE[i]), cimag(W_MMSE[i]));
    }
#endif

    free(H_hermH_sigmaI);
    free(H_herm);
}

float sqrt_float(float x, float sqrt_x)
{
    sqrt_x = (float)(sqrt((double)(x)));
    return sqrt_x;
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "parmt_mtsearch.h"
#ifdef PARMT_USE_INTEL
#include <mkl_cblas.h>
#else
#include <cblas.h>
#endif
#include "parmt_utils.h"
#include "iscl/memory/memory.h"

/*!
 * @brief Computes a synthetic seismogram corresponding for the
 *        given observations, location (depth), and moment tensor.
 *
 * @param[in] iobs      Observations number.
 * @param[in] ilocOpt   Location index.
 * @param[in] imtOpt    Moment tensor number.
 * @param[in] ldm       Leading dimension of mts.  This must be at leats 6.
 * @param[in] mts       Matrix of moement tensors with dimension [nmt x ldm].
 *
 * @param[in,out] data  On input contains the data.
 * @param[in,out] data  On exit contains the corresponding synthetic.
 *
 * @result 0 indicates success.
 *
 */
int parmt_computeSynthetic(const int iobs,
                           const int ilocOpt,
                           const int imtOpt,
                           const int ldm,
                           const double *__restrict__ mts,
                           struct parmtData_struct *data)
{
    double *G;
    int k, npts;
    k = iobs*data->nlocs + ilocOpt;
    fprintf(stdout, "%s: Processing observation=%d, location=%d, mt=%d, k=%d\n",
            __func__, iobs, ilocOpt, imtOpt, k);
    npts = data->sacGxx[k].header.npts;
    G = memory_calloc64f(6*npts);
    sacio_copyHeader(data->sacGxx[k].header, &data->est[iobs].header);
    cblas_dcopy(data->sacGxx[k].header.npts,
                data->sacGxx[k].data, 1, &G[0*npts], 1);
    cblas_dcopy(data->sacGyy[k].header.npts,
                data->sacGyy[k].data, 1, &G[1*npts], 1);
    cblas_dcopy(data->sacGzz[k].header.npts,
                data->sacGzz[k].data, 1, &G[2*npts], 1);
    cblas_dcopy(data->sacGxy[k].header.npts,
                data->sacGxy[k].data, 1, &G[3*npts], 1);
    cblas_dcopy(data->sacGxz[k].header.npts,
                data->sacGxz[k].data, 1, &G[4*npts], 1);
    cblas_dcopy(data->sacGyz[k].header.npts,
                data->sacGyz[k].data, 1, &G[5*npts], 1);
    data->est[iobs].data = memory_calloc64f(npts);
    data->est[iobs].npts = npts;
    strcpy(data->est[iobs].header.kcmpnm, data->data[iobs].header.kcmpnm);
    cblas_dgemv(CblasColMajor, CblasNoTrans,
                npts, 6, 1.0, G, npts,
                &mts[ldm*imtOpt], 1, 0.0, data->est[iobs].data, 1);
    memory_free64f(&G);
    return 0;
}


#include <lapacke.h>
#include <stdio.h>

/* Auxiliary routine: printing a matrix */
void print_matrix_rowmajor( char* desc, lapack_int m, lapack_int n, double* mat, lapack_int ldm ) {
        lapack_int i, j;
        printf( "\n %s\n", desc );

        for( i = 0; i < m; i++ ) {
                for( j = 0; j < n; j++ ) printf( " %6.2f", mat[i*ldm+j] );
                printf( "\n" );
        }
}

/* Auxiliary routine: printing a matrix */
void print_matrix_colmajor( char* desc, lapack_int m, lapack_int n, double* mat, lapack_int ldm ) {
        lapack_int i, j;
        printf( "\n %s\n", desc );

        for( i = 0; i < m; i++ ) {
                for( j = 0; j < n; j++ ) printf( " %6.2f", mat[i+j*ldm] );
                printf( "\n" );
        }
}

/* Auxiliary routine: printing a vector of integers */
void print_vector( char* desc, lapack_int n, lapack_int* vec ) {
        lapack_int j;
        printf( "\n %s\n", desc );
        for( j = 0; j < n; j++ ) printf( " %6i", vec[j] );
        printf( "\n" );
}


/**
* @license Apache-2.0
*
* Copyright (c) 2018 The Stdlib Authors.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*    http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/

#include <cblas.h>
#include <stdio.h>

int main() {
	double A[ 6 ] = { 1.0, 2.0, 1.0, -3.0, 4.0, -1.0 };
	double B[ 6 ] = { 1.0, 2.0, 1.0, -3.0, 4.0, -1.0 };
	double C[ 9 ] = { 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5 };
	int i = 0;

	cblas_dgemm( CblasColMajor, CblasNoTrans, CblasTrans, 3, 3, 2, 1, A, 3, B, 3, 2, C, 3 );

	for( i = 0; i < 9; i++ ) {
		printf( "%lf ", C[ i ] );
	}
	printf( "\n" );
}


/* multifit_nlinear/nielsen.c
 *
 * Copyright (C) 2016 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.
 */

/*
 * This module contains routines for updating the Levenberg-Marquardt
 * damping parameter on each iteration using Nielsen's method:
 *
 * [1] H. B. Nielsen, K. Madsen, Introduction to Optimization and
 *     Data Fitting, Informatics and Mathematical Modeling,
 *     Technical University of Denmark (DTU), 2010.
 *
 * 5 routines are needed to implement the update procedure:
 *
 * 1. accept - update parameter after a step has been accepted
 * 2. reject - update parameter after a step has been rejected
 * 3. free   - free workspace state
 */

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

#define LM_ONE_THIRD         (0.333333333333333)

static int nielsen_init(const gsl_matrix * J, const gsl_vector * diag,
                        double * mu, long * nu);
static int nielsen_accept(const double rho, double * mu, long * nu);
static int nielsen_reject(double * mu, long * nu);

static int
nielsen_init(const gsl_matrix * J, const gsl_vector * diag,
             double * mu, long * nu)
{
  const double mu0 = 1.0e-3;
  const size_t p = J->size2;
  size_t j;
  double max = -1.0;

  *nu = 2;

  /* set mu = mu0 * max(diag(J~^T J~)), with J~ = J D^{-1} */

  for (j = 0; j < p; ++j)
    {
      gsl_vector_const_view v = gsl_matrix_const_column(J, j);
      double dj = gsl_vector_get(diag, j);
      double norm = gsl_blas_dnrm2(&v.vector) / dj;
      max = GSL_MAX(max, norm);
    }

  *mu = mu0 * max * max;

  return GSL_SUCCESS;
}

static int
nielsen_accept(const double rho, double * mu, long * nu)
{
  double  b;

  /* reset nu */
  *nu = 2;

  b = 2.0 * rho - 1.0;
  b = 1.0 - b*b*b;
  *mu *= GSL_MAX(LM_ONE_THIRD, b);

  return GSL_SUCCESS;
}

static int
nielsen_reject(double * mu, long * nu)
{
  *mu *= (double) *nu;

  /* nu := 2*nu */
  *nu <<= 1;

  return GSL_SUCCESS;
}