/* linalg/test_cholesky.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.
 */

#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>

#include "test_common.c"

static int test_cholesky_decomp_eps(const int scale, const gsl_matrix * m,
                                    const double expected_rcond, const double eps,
                                    const char * desc);
static int test_cholesky_decomp(gsl_rng * r);
int test_cholesky_invert_eps(const gsl_matrix * m, const double eps, const char *desc);
int test_cholesky_invert(gsl_rng * r);
static int test_pcholesky_decomp_eps(const int scale, const gsl_matrix * m,
                                     const double expected_rcond, const double eps,
                                     const char * desc);
static int test_pcholesky_decomp(gsl_rng * r);
int test_pcholesky_solve_eps(const int scale, const gsl_matrix * m, const gsl_vector * rhs,
                             const gsl_vector * sol, const double eps,
                             const char * desc);
static int test_pcholesky_solve(gsl_rng * r);
int test_pcholesky_invert_eps(const gsl_matrix * m, const double eps, const char *desc);
int test_pcholesky_invert(gsl_rng * r);

static int test_mcholesky_decomp_eps(const int posdef, const int scale, const gsl_matrix * m,
                                     const double expected_rcond, const double eps, const char * desc);

/* Hilbert matrix condition numbers, as calculated by LAPACK DPOSVX */
double hilb_rcond[] = { 1.000000000000e+00, 3.703703703704e-02, 1.336898395722e-03,
                        3.524229074890e-05, 1.059708198754e-06, 3.439939465186e-08,
                        1.015027593823e-09, 2.952221630602e-11, 9.093751565191e-13,
                        2.828277420229e-14, 8.110242564869e-16, 2.409320075800e-17 };

static int
test_cholesky_decomp_eps(const int scale, const gsl_matrix * m,
                         const double expected_rcond, const double eps,
                         const char * desc)
{
  int s = 0;
  size_t i, j, N = m->size2;

  gsl_matrix * V  = gsl_matrix_alloc(N, N);
  gsl_matrix * A  = gsl_matrix_alloc(N, N);
  gsl_matrix * L  = gsl_matrix_calloc(N, N);
  gsl_matrix * LT = gsl_matrix_calloc(N, N);
  gsl_vector * S = gsl_vector_alloc(N);

  gsl_matrix_memcpy(V, m);

  if (scale)
    s += gsl_linalg_cholesky_decomp2(V, S);
  else
    s += gsl_linalg_cholesky_decomp1(V);

  /* compute L and LT */
  gsl_matrix_tricpy('L', 1, L, V);
  gsl_matrix_transpose_tricpy('L', 1, LT, L);

  if (scale)
    {
      /* L <- S^{-1} L, LT <- LT S^{-1} */
      for (i = 0; i < N; ++i)
        {
          double Si = gsl_vector_get(S, i);
          gsl_vector_view v = gsl_matrix_row(L, i);
          gsl_vector_view w = gsl_matrix_column(LT, i);

          gsl_vector_scale(&v.vector, 1.0 / Si);
          gsl_vector_scale(&w.vector, 1.0 / Si);
        }
    }

  /* compute A = L LT */
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, L, LT, 0.0, A);

  for (i = 0; i < N; i++)
    {
      for (j = 0; j < N; j++)
        {
          double Aij = gsl_matrix_get(A, 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, N, N, i, j, Aij, mij);
        }
    }

  if (expected_rcond > 0 && !scale)
    {
      gsl_vector *work = gsl_vector_alloc(3 * N);
      double rcond;

      gsl_linalg_cholesky_rcond(V, &rcond, work);

      gsl_test_rel(rcond, expected_rcond, 1.0e-6,
                   "%s rcond: (%3lu,%3lu): %22.18g   %22.18g\n",
                   desc, N, N, rcond, expected_rcond);

      gsl_vector_free(work);
    }

  gsl_matrix_free(V);
  gsl_matrix_free(A);
  gsl_matrix_free(L);
  gsl_matrix_free(LT);
  gsl_vector_free(S);

  return s;
}

static int
test_cholesky_decomp(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 50;
  size_t N;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix * m = gsl_matrix_alloc(N, N);

      create_posdef_matrix(m, r);
      test_cholesky_decomp_eps(0, m, -1.0, 1.0e2 * N * GSL_DBL_EPSILON, "cholesky_decomp unscaled random");
      test_cholesky_decomp_eps(1, m, -1.0, 1.0e2 * N * GSL_DBL_EPSILON, "cholesky_decomp scaled random");

      if (N <= 12)
        {
          double expected_rcond = -1.0;

          if (hilb_rcond[N - 1] > 1.0e-12)
            expected_rcond = hilb_rcond[N - 1];

          create_hilbert_matrix2(m);

          test_cholesky_decomp_eps(0, m, expected_rcond, N * GSL_DBL_EPSILON, "cholesky_decomp unscaled hilbert");
          test_cholesky_decomp_eps(1, m, expected_rcond, N * GSL_DBL_EPSILON, "cholesky_decomp scaled hilbert");
        }

      gsl_matrix_free(m);
    }

  return s;
}

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

  gsl_matrix * v  = gsl_matrix_alloc(N, N);
  gsl_matrix * c  = gsl_matrix_alloc(N, N);

  gsl_matrix_memcpy(v, m);

  s += gsl_linalg_cholesky_decomp1(v);
  s += gsl_linalg_cholesky_invert(v);

  /* c = m m^{-1} */
  gsl_blas_dsymm(CblasLeft, CblasUpper, 1.0, m, v, 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(v);
  gsl_matrix_free(c);

  return s;
}

int
test_cholesky_invert(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 50;
  size_t N;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix * m = gsl_matrix_alloc(N, N);

      create_posdef_matrix(m, r);

      test_cholesky_invert_eps(m, N * GSL_DBL_EPSILON, "cholesky_invert unscaled random");

      if (N <= 4)
        {
          create_hilbert_matrix2(m);
          test_cholesky_invert_eps(m, N * 256.0 * GSL_DBL_EPSILON, "cholesky_invert unscaled hilbert");
        }

      gsl_matrix_free(m);
    }

  return s;
}

static int
test_mcholesky_decomp_eps(const int posdef, const int scale, const gsl_matrix * m,
                          const double expected_rcond, const double eps, const char * desc)
{
  int s = 0;
  size_t i, j, N = m->size2;

  gsl_matrix * LDLT = gsl_matrix_alloc(N, N);
  gsl_matrix * V  = gsl_matrix_alloc(N, N);
  gsl_matrix * A  = gsl_matrix_alloc(N, N);
  gsl_matrix * L  = gsl_matrix_alloc(N, N);
  gsl_matrix * LT = gsl_matrix_alloc(N, N);
  gsl_vector * S = gsl_vector_alloc(N);
  gsl_vector * E = gsl_vector_alloc(N);
  gsl_permutation * perm = gsl_permutation_alloc(N);

  gsl_vector_view D = gsl_matrix_diagonal(LDLT);

  gsl_matrix_memcpy(LDLT, m);
  s += gsl_linalg_mcholesky_decomp(LDLT, perm, E);

  /* check that the upper triangle of LDLT equals original matrix */
  for (i = 0; i < N; ++i)
    {
      for (j = i + 1; j < N; ++j)
        {
          double mij = gsl_matrix_get(m, i, j);
          double aij = gsl_matrix_get(LDLT, i, j);

          gsl_test_rel(aij, mij, 1.0e-12,
                       "%s upper triangle: (%3lu,%3lu)[%lu,%lu]: %22.18g   %22.18g\n",
                       desc, N, N, i, j, aij, mij);
        }
    }

  if (posdef)
    {
      /* ||E|| should be 0 */
      double norm = gsl_blas_dnrm2(E);
      s = norm != 0.0;
      gsl_test(s, "%s: (%zu,%zu): ||E|| = %.12e",
               desc, N, N, norm);

      /* check that D is decreasing */
      s = 0;
      for (i = 1; i < N; ++i)
        {
          double dprev = gsl_vector_get(&D.vector, i - 1);
          double di = gsl_vector_get(&D.vector, i);

          if (di > dprev)
            s = 1;
        }

      gsl_test(s, "%s: (%zu,%zu): D is not decreasing",
               desc, N, N);
    }

  /* compute L and LT */
  gsl_matrix_set_identity(L);
  gsl_matrix_set_identity(LT);

  gsl_matrix_tricpy('L', 0, L, LDLT);
  gsl_matrix_transpose_tricpy('L', 0, LT, L);

  /* compute (L sqrt(D)) and (sqrt(D) LT) */
  for (i = 0; i < N; ++i)
    {
      gsl_vector_view v = gsl_matrix_column(L, i);
      gsl_vector_view w = gsl_matrix_row(LT, i);
      double di = gsl_vector_get(&D.vector, i);

      gsl_vector_scale(&v.vector, sqrt(di));
      gsl_vector_scale(&w.vector, sqrt(di));
    }

  /* compute A = L D LT */
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, L, LT, 0.0, A);

  /* compute V = P (S M S + E) P^T */

  gsl_matrix_memcpy(V, m);
  D = gsl_matrix_diagonal(V);

  /* compute S M S */
  if (scale)
    {
      gsl_linalg_cholesky_scale_apply(V, S);
      gsl_matrix_transpose_tricpy('L', 0, V, V);
    }

  /* compute S M S + E */
  gsl_vector_add(&D.vector, E);

  /* compute M P^T */
  for (i = 0; i < N; ++i)
    {
      gsl_vector_view v = gsl_matrix_row(V, i);
      gsl_permute_vector(perm, &v.vector);
    }

  /* compute P M P^T */
  for (i = 0; i < N; ++i)
    {
      gsl_vector_view v = gsl_matrix_column(V, i);
      gsl_permute_vector(perm, &v.vector);
    }

  for (i = 0; i < N; i++)
    {
      double Ei = gsl_vector_get(E, i);

      for (j = 0; j < N; j++)
        {
          double Aij = gsl_matrix_get(A, i, j); /* L D L^T */
          double Bij = gsl_matrix_get(V, i, j); /* P M P^T */
          double Cij;                           /* P M P^T + E */

          if (i == j)
            Cij = Bij + Ei*0;
          else
            Cij = Bij;

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

  if (expected_rcond > 0 && !scale)
    {
      gsl_vector *work = gsl_vector_alloc(3 * N);
      double rcond;

      gsl_linalg_mcholesky_rcond(LDLT, perm, &rcond, work);

      gsl_test_rel(rcond, expected_rcond, 1.0e-6,
                   "%s rcond: (%3lu,%3lu): %22.18g   %22.18g\n",
                   desc, N, N, rcond, expected_rcond);

      gsl_vector_free(work);
    }

  gsl_matrix_free(LDLT);
  gsl_matrix_free(V);
  gsl_matrix_free(A);
  gsl_matrix_free(L);
  gsl_matrix_free(LT);
  gsl_vector_free(S);
  gsl_vector_free(E);
  gsl_permutation_free(perm);

  return s;
}

static int
test_mcholesky_decomp(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 50;
  size_t N;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix * m = gsl_matrix_alloc(N, N);

      create_posdef_matrix(m, r);
      test_mcholesky_decomp_eps(1, 0, m, -1.0, 128.0 * N * GSL_DBL_EPSILON, "mcholesky_decomp unscaled random posdef");

      create_symm_matrix(m, r);
      test_mcholesky_decomp_eps(0, 0, m, -1.0, 8192.0 * N * GSL_DBL_EPSILON, "mcholesky_decomp unscaled random symm");

      if (N <= 8)
        {
          double expected_rcond = -1.0;

          if (hilb_rcond[N - 1] > 1.0e-12)
            expected_rcond = hilb_rcond[N - 1];

          create_hilbert_matrix2(m);

          test_mcholesky_decomp_eps(1, 0, m, expected_rcond, 128.0 * N * GSL_DBL_EPSILON, "mcholesky_decomp unscaled hilbert");
        }

      gsl_matrix_free(m);
    }

  return s;
}

int
test_mcholesky_solve_eps(const gsl_matrix * m, const gsl_vector * rhs,
                         const gsl_vector * sol, const double eps,
                         const char * desc)
{
  int s = 0;
  size_t i, N = m->size1;
  gsl_matrix * u  = gsl_matrix_alloc(N, N);
  gsl_vector * x = gsl_vector_calloc(N);
  gsl_vector * S = gsl_vector_alloc(N);
  gsl_permutation * perm = gsl_permutation_alloc(N);

  gsl_matrix_memcpy(u, m);

  s += gsl_linalg_mcholesky_decomp(u, perm, NULL);
  s += gsl_linalg_mcholesky_solve(u, 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);
    }

  gsl_vector_free(x);
  gsl_vector_free(S);
  gsl_matrix_free(u);
  gsl_permutation_free(perm);

  return s;
}

static int
test_mcholesky_solve(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 50;
  size_t N;

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

      create_posdef_matrix(m, r);
      create_random_vector(sol, r);
      gsl_blas_dsymv(CblasLower, 1.0, m, sol, 0.0, rhs);

      test_mcholesky_solve_eps(m, rhs, sol, 64.0 * N * GSL_DBL_EPSILON, "mcholesky_solve random");

      if (N <= 3)
        {
          create_hilbert_matrix2(m);
          gsl_blas_dsymv(CblasLower, 1.0, m, sol, 0.0, rhs);
          test_mcholesky_solve_eps(m, rhs, sol, 1.0e3 * N * GSL_DBL_EPSILON, "mcholesky_solve hilbert");
        }

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

  return s;
}

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

  gsl_matrix * v  = gsl_matrix_alloc(N, N);
  gsl_matrix * c  = gsl_matrix_alloc(N, N);
  gsl_matrix * minv = gsl_matrix_alloc(N, N);
  gsl_vector * E = gsl_vector_alloc(N);
  gsl_permutation * p = gsl_permutation_alloc(N);

  gsl_matrix_memcpy(v, m);

  s += gsl_linalg_mcholesky_decomp(v, p, E);
  s += gsl_linalg_mcholesky_invert(v, p, minv);

  /* c = m m^{-1} */
  gsl_blas_dsymm(CblasLeft, CblasUpper, 1.0, m, minv, 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(v);
  gsl_matrix_free(c);
  gsl_matrix_free(minv);
  gsl_vector_free(E);
  gsl_permutation_free(p);

  return s;
}

int
test_mcholesky_invert(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 30;
  size_t N;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix * m = gsl_matrix_alloc(N, N);

      create_posdef_matrix(m, r);
      test_mcholesky_invert_eps(m, N * GSL_DBL_EPSILON, "mcholesky_invert unscaled random");

      if (N <= 4)
        {
          create_hilbert_matrix2(m);
          test_mcholesky_invert_eps(m, 256.0 * N * GSL_DBL_EPSILON, "mcholesky_invert unscaled hilbert");
        }

      gsl_matrix_free(m);
    }

  return s;
}

static int
test_pcholesky_decomp_eps(const int scale, const gsl_matrix * m,
                          const double expected_rcond, const double eps,
                          const char * desc)
{
  int s = 0;
  size_t i, j, N = m->size2;

  gsl_matrix * LDLT = gsl_matrix_alloc(N, N);
  gsl_matrix * V  = gsl_matrix_alloc(N, N);
  gsl_matrix * A  = gsl_matrix_alloc(N, N);
  gsl_matrix * L  = gsl_matrix_alloc(N, N);
  gsl_matrix * LT = gsl_matrix_alloc(N, N);
  gsl_vector * S = gsl_vector_alloc(N);
  gsl_permutation * perm = gsl_permutation_alloc(N);

  gsl_vector_view D = gsl_matrix_diagonal(LDLT);

  gsl_matrix_memcpy(LDLT, m);

  if (scale)
    s += gsl_linalg_pcholesky_decomp2(LDLT, perm, S);
  else
    s += gsl_linalg_pcholesky_decomp(LDLT, perm);

  /* check that the upper triangle of LDLT equals original matrix */
  for (i = 0; i < N; ++i)
    {
      for (j = i + 1; j < N; ++j)
        {
          double mij = gsl_matrix_get(m, i, j);
          double aij = gsl_matrix_get(LDLT, i, j);

          gsl_test_rel(aij, mij, 1.0e-12,
                       "%s upper triangle: (%3lu,%3lu)[%lu,%lu]: %22.18g   %22.18g\n",
                       desc, N, N, i, j, aij, mij);
        }
    }

  /* check that D is decreasing */
  s = 0;
  for (i = 1; i < N; ++i)
    {
      double dprev = gsl_vector_get(&D.vector, i - 1);
      double di = gsl_vector_get(&D.vector, i);

      if (di > dprev)
        s = 1;
    }

  gsl_test(s, "%s: (%zu,%zu): D is not decreasing",
           desc, N, N);

  /* compute L and LT */
  gsl_matrix_set_identity(L);
  gsl_matrix_set_identity(LT);

  gsl_matrix_tricpy('L', 0, L, LDLT);
  gsl_matrix_transpose_tricpy('L', 0, LT, L);

  /* compute (L sqrt(D)) and (sqrt(D) LT) */
  for (i = 0; i < N; ++i)
    {
      gsl_vector_view v = gsl_matrix_column(L, i);
      gsl_vector_view w = gsl_matrix_row(LT, i);
      double di = gsl_vector_get(&D.vector, i);

      gsl_vector_scale(&v.vector, sqrt(di));
      gsl_vector_scale(&w.vector, sqrt(di));
    }

  /* compute A = L D LT */
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, L, LT, 0.0, A);

  /* compute V = P S M S P^T */

  gsl_matrix_memcpy(V, m);

  /* compute S M S */
  if (scale)
    {
      gsl_linalg_cholesky_scale_apply(V, S);
      gsl_matrix_transpose_tricpy('L', 0, V, V);
    }

  /* compute M P^T */
  for (i = 0; i < N; ++i)
    {
      gsl_vector_view v = gsl_matrix_row(V, i);
      gsl_permute_vector(perm, &v.vector);
    }

  /* compute P M P^T */
  for (i = 0; i < N; ++i)
    {
      gsl_vector_view v = gsl_matrix_column(V, i);
      gsl_permute_vector(perm, &v.vector);
    }

  for (i = 0; i < N; i++)
    {
      for (j = 0; j < N; j++)
        {
          double Aij = gsl_matrix_get(A, i, j); /* L D L^T */
          double Bij = gsl_matrix_get(V, i, j); /* P M P^T */

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

  if (expected_rcond > 0 && !scale)
    {
      gsl_vector *work = gsl_vector_alloc(3 * N);
      double rcond;

      gsl_linalg_pcholesky_rcond(LDLT, perm, &rcond, work);

      gsl_test_rel(rcond, expected_rcond, 1.0e-6,
                   "%s rcond: (%3lu,%3lu): %22.18g   %22.18g\n",
                   desc, N, N, rcond, expected_rcond);

      gsl_vector_free(work);
    }

  gsl_matrix_free(LDLT);
  gsl_matrix_free(V);
  gsl_matrix_free(A);
  gsl_matrix_free(L);
  gsl_matrix_free(LT);
  gsl_vector_free(S);
  gsl_permutation_free(perm);

  return s;
}

static int
test_pcholesky_decomp(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 50;
  size_t N;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix * m = gsl_matrix_alloc(N, N);

      create_posdef_matrix(m, r);
      test_pcholesky_decomp_eps(0, m, -1.0, 1024.0 * N * GSL_DBL_EPSILON, "pcholesky_decomp unscaled random");
      test_pcholesky_decomp_eps(1, m, -1.0, 1024.0 * N * GSL_DBL_EPSILON, "pcholesky_decomp scaled random");

      if (N <= 12)
        {
          double expected_rcond = -1.0;

          if (hilb_rcond[N - 1] > 1.0e-12)
            expected_rcond = hilb_rcond[N - 1];

          create_hilbert_matrix2(m);

          test_pcholesky_decomp_eps(0, m, expected_rcond, 1024.0 * N * GSL_DBL_EPSILON, "pcholesky_decomp unscaled hilbert");
          test_pcholesky_decomp_eps(1, m, expected_rcond, 1024.0 * N * GSL_DBL_EPSILON, "pcholesky_decomp scaled hilbert");
        }

      gsl_matrix_free(m);
    }

  return s;
}

int
test_pcholesky_solve_eps(const int scale, const gsl_matrix * m, const gsl_vector * rhs,
                         const gsl_vector * sol, const double eps,
                         const char * desc)
{
  int s = 0;
  size_t i, N = m->size1;
  gsl_matrix * u  = gsl_matrix_alloc(N, N);
  gsl_vector * x = gsl_vector_calloc(N);
  gsl_vector * S = gsl_vector_alloc(N);
  gsl_permutation * perm = gsl_permutation_alloc(N);

  gsl_matrix_memcpy(u, m);

  if (scale)
    {
      s += gsl_linalg_pcholesky_decomp2(u, perm, S);
      s += gsl_linalg_pcholesky_solve2(u, perm, S, rhs, x);
    }
  else
    {
      s += gsl_linalg_pcholesky_decomp(u, perm);
      s += gsl_linalg_pcholesky_solve(u, 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);
    }

  gsl_vector_free(x);
  gsl_vector_free(S);
  gsl_matrix_free(u);
  gsl_permutation_free(perm);

  return s;
}

static int
test_pcholesky_solve(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 50;
  size_t N;

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

      create_posdef_matrix(m, r);
      create_random_vector(sol, r);
      gsl_blas_dsymv(CblasLower, 1.0, m, sol, 0.0, rhs);

      test_pcholesky_solve_eps(0, m, rhs, sol, 64.0 * N * GSL_DBL_EPSILON, "pcholesky_solve unscaled random");
      test_pcholesky_solve_eps(1, m, rhs, sol, 64.0 * N * GSL_DBL_EPSILON, "pcholesky_solve scaled random");

      if (N <= 3)
        {
          create_hilbert_matrix2(m);
          gsl_blas_dsymv(CblasLower, 1.0, m, sol, 0.0, rhs);
          test_pcholesky_solve_eps(0, m, rhs, sol, 1024.0 * N * GSL_DBL_EPSILON, "pcholesky_solve unscaled hilbert");
          test_pcholesky_solve_eps(1, m, rhs, sol, 2048.0 * N * GSL_DBL_EPSILON, "pcholesky_solve scaled hilbert");
        }

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

  return s;
}

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

  gsl_matrix * v  = gsl_matrix_alloc(N, N);
  gsl_matrix * c  = gsl_matrix_alloc(N, N);
  gsl_matrix * minv = gsl_matrix_alloc(N, N);
  gsl_permutation * p = gsl_permutation_alloc(N);

  gsl_matrix_memcpy(v, m);

  s += gsl_linalg_pcholesky_decomp(v, p);
  s += gsl_linalg_pcholesky_invert(v, p, minv);

  /* c = m m^{-1} */
  gsl_blas_dsymm(CblasLeft, CblasUpper, 1.0, m, minv, 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(v);
  gsl_matrix_free(c);
  gsl_matrix_free(minv);
  gsl_permutation_free(p);

  return s;
}

int
test_pcholesky_invert(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 30;
  size_t N;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix * m = gsl_matrix_alloc(N, N);

      create_posdef_matrix(m, r);
      test_pcholesky_invert_eps(m, N * GSL_DBL_EPSILON, "pcholesky_invert unscaled random");

      if (N <= 4)
        {
          create_hilbert_matrix2(m);
          test_pcholesky_invert_eps(m, 1024.0 * N * GSL_DBL_EPSILON, "pcholesky_invert unscaled hilbert");
        }

      gsl_matrix_free(m);
    }

  return s;
}


#include <gsl/gsl_math.h>
#include <gsl/gsl_cblas.h>
#include "cblas.h"

void
cblas_zgeru (const enum CBLAS_ORDER order, const int M, const int N,
             const void *alpha, const void *X, const int incX, const void *Y,
             const int incY, void *A, const int lda)
{
#define BASE double
#include "source_geru.h"
#undef BASE
}


/*
ODE: a program to get optime Runge-Kutta and multi-steps methods.

Copyright 2011-2019, Javier Burguete Tolosa.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

	1. Redistributions of source code must retain the above copyright notice,
		this list of conditions and the following disclaimer.

	2. Redistributions in binary form must reproduce the above copyright notice,
		this list of conditions and the following disclaimer in the
		documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY Javier Burguete Tolosa ``AS IS'' AND ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL Javier Burguete Tolosa OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

/**
 * \file rk_2_2.c
 * \brief Source file to optimize Runge-Kutta 3 steps 2nd order methods.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2011-2019.
 */
#define _GNU_SOURCE
#include <string.h>
#include <math.h>
#include <libxml/parser.h>
#include <glib.h>
#include <libintl.h>
#include <gsl/gsl_rng.h>
#include "config.h"
#include "utils.h"
#include "optimize.h"
#include "rk.h"
#include "rk_2_2.h"

#define DEBUG_RK_2_2 0          ///< macro to debug.

/**
 * Function to obtain the coefficients of a 2 steps 2nd order Runge-Kutta
 * method.
 */
int
rk_tb_2_2 (Optimize * optimize) ///< Optimize struct.
{
  long double *tb, *r;
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_tb_2_2: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t2 (tb) = 1.L;
  t1 (tb) = r[0];
  b21 (tb) = 0.5L / t1 (tb);
  rk_b_2 (tb);
#if DEBUG_RK_2_2
  rk_print_tb (optimize, "rk_tb_2_2", stderr);
  fprintf (stderr, "rk_tb_2_2: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 2 steps 2nd order, 3rd order in
 * equations depending only in time, Runge-Kutta method.
 */
int
rk_tb_2_2t (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_tb_2_2t: start\n");
#endif
  tb = optimize->coefficient;
  t2 (tb) = 1.L;
  t1 (tb) = 2.L / 3.L;
  b21 (tb) = 0.5L / t1 (tb);
  rk_b_2 (tb);
#if DEBUG_RK_2_2
  rk_print_tb (optimize, "rk_tb_2_2t", stderr);
  fprintf (stderr, "rk_tb_2_2t: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 2 steps 1st-2nd order Runge-Kutta
 * pair.
 */
int
rk_tb_2_2p (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_tb_2_2p: start\n");
#endif
  if (!rk_tb_2_2 (optimize))
    return 0;
  tb = optimize->coefficient;
  e20 (tb) = 1.L;
#if DEBUG_RK_2_2
  rk_print_tb (optimize, "rk_tb_2_2p", stderr);
  fprintf (stderr, "rk_tb_2_2p: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 2 steps 1st-2nd order, 1st-3rd order
 * in equations depending only on time, Runge-Kutta pair.
 */
int
rk_tb_2_2tp (Optimize * optimize)       ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_tb_2_2tp: start\n");
#endif
  if (!rk_tb_2_2t (optimize))
    return 0;
  tb = optimize->coefficient;
  e20 (tb) = 1.L;
#if DEBUG_RK_2_2
  rk_print_tb (optimize, "rk_tb_2_2tp", stderr);
  fprintf (stderr, "rk_tb_2_2tp: end\n");
#endif
  return 1;
}

/**
 * Function to calculate the objective function of a 2 steps 2nd order
 * Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_2_2 (RK * rk)   ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_objective_tb_2_2: start\n");
#endif
  tb = rk->tb->coefficient;
  if (b20 (tb) < 0.L)
    {
      o = 40.L - b20 (tb);
      goto end;
    }
  o = 30.L + fmaxl (1.L, t1 (tb));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_objective_tb_2_2: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_2_2: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 2 steps 2nd order, 3rd
 * order in equations depending only in time, Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_2_2t (RK * rk)  ///< RK struct.
{
  long double o;
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_objective_tb_2_2t: start\n");
#endif
  o = 31.L;
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_objective_tb_2_2t: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_2_2t: end\n");
#endif
  return o;
}


#ifndef libCV_ellipse
#define libCV_ellipse

#include <objc/Object.h>
#include "cvtypes.h"
#include "util.h"
#include "Array.h"
#include "math.h"
#include <gsl/gsl_math.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_eigen.h>

@interface Ellipse : Object {

	f64 x_axis;
	f64 y_axis;
	f64 x_center;
	f64 y_center;
	f64 rotation;

	f64 general[6];

	f64 bounds[4];

}

+(id) newAtX:(f64)xc andY:(f64)yc withXAxis:(f64)xa andYAxis:(f64)ya rotatedBy:(f64)phi;
+(id) newWithA:(f64)a b:(f64)b c:(f64)c d:(f64)d e:(f64)e f:(f64)f;

+(id) newFromPoints:(ArrayP)points;

-(void) drawInArray:(ArrayP)array;

-(void) toGeneralConic;
-(void) toGeneralEllipse;
-(void) findBounds;

// Get and set general ellipse parameters

-(f64) x_axis;
-(f64) y_axis;
-(f64) major_axis;
-(f64) minor_axis;
-(f64) rotation;
-(f64) majorRotation;
-(f64) minorRotation;
-(f64) x_center;
-(f64) y_center;

-(void) setX_axis:(f64)newx;
-(void) setY_axis:(f64)newy;
-(void) setX_center:(f64)newx;
-(void) setY_center:(f64)newy;
-(void) setRotation:(f64)newr;
-(void) setX_axis:(f64)xa y_axis:(f64)ya x_center:(f64)xc y_center:(f64)yc rotation:(f64)r;

// Get and set general conic parameters

-(f64) A;
-(f64) B;
-(f64) C;
-(f64) D;
-(f64) E;
-(f64) F;

-(f64 *) general;

-(void) setA:(f64)A;
-(void) setB:(f64)B;
-(void) setC:(f64)C;
-(void) setD:(f64)D;
-(void) setE:(f64)E;
-(void) setF:(f64)F;
-(void) setGeneralA:(f64)A B:(f64)B C:(f64)C D:(f64)D E:(f64)E F:(f64)F;

-(void) printInfoTo:(FILE *)fp;

-(u08) isValid;
-(id) free;

@end

enum ellipseindices {
	AX,
	BX,
	CX,
	DX,
	EX,
	FX,
};

enum ellipsebounds {
	XHI,
	XLO,
	YHI,
	YLO,
};

typedef Ellipse * EllipseP;

void originCenteredLSQEllipse( f64 x_values[], f64 y_values[], u64 number_of_points, f64 ellipse[] );
void generateBoundEllipses( f64 e[], f64 b1[], f64 b2[], f64 treshold );
f64 ellipseCircumference( f64 e[] );
EllipseP ellipseRANSAC( ArrayP edges, f64 fit_treshold, f64 min_percent_inliers, f64 certainty_probability, u64 max_iterations );

#endif


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <float.h>
#define COMPEARTH_PRIVATE_CROSS3 1
#define COMPEARTH_PRIVATE_NORM3 1
#define COMPEARTH_PRIVATE_DOT3 1
#define COMPEARTH_PRIVATE_WRAP360 1
#include "compearth.h"
#ifdef COMPEARTH_USE_MKL
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wreserved-id-macro"
#pragma clang diagnostic ignored "-Wstrict-prototypes"
#endif
#include <mkl_cblas.h>
#ifdef __clang__
#pragma clang diagnostic pop
#endif
#else
#include <cblas.h>
#endif

//#define MAXMT 64

static void setZero(const int nmt, const double tol, double *__restrict__ X);
static void faultVec2Ang(const double *__restrict__ S,
                         const double *__restrict__ N,
                         double *theta, double *sigma,
                         double *kappa, double *__restrict__ K, int *ierr);
static int pickP1(const double thetaA, const double sigmaA, const double kappaA,
                  const double thetaB, const double sigmaB, const double kappaB,
                  const double tol, const int p1, const int p2);

/*!
 * @brief Converts a moment tensor to six parameters of Tape and Tape 2012.
 *        The reverse program is TT2CMT.c
 *
 * @param[in] nmt           Number of moment tensors.
 * @param[in] Min           [6 x nmt] array of moment tensors in CMT
 *                          convention (i.e. UP-SOUTH-EAST).  M is packed:
 *                          \f$
 *                           M = \{ M_{rr}, M_{\theta \theta}, M_{\phi \phi}
 *                                  M_{r \theta}, M_{r \phi}, M_{\theta \phi} \}
 *                          \f$.
 *                          The leading dimension is 6.
 * @param[in] ldisplay      If true then display the results. \n
 *                          Otherwise, this routine will be quiet unless
 *                          an error is encountered.
 *
 * @param[out] gamma        Angle from DC meridian to MT point.
 *                          Note that \f$ \gamma \in [-30, 30] \f$.
 *                          This is an array of dimension [nmt].
 * @param[out] delta        Angle from deviatoric plane to MT point.
 *                          Note that \f$ \delta \in [-90, 90] \f$.
 *                          This is an array of dimension [nmt].
 * @param[out] M0           Seismic moment in N-m.  This is an array of
 *                          dimension [nmt].
 * @param[out] kappa        Strike angle \f$ \kappa \in [0,360] \f$.
 *                          This is an array of dimension [nmt].
 * @param[out] theta        Dip angle \f$ \theta \in [0,90] \f$.
 *                          This is an array of dimension [nmt].
 * @param[out] sigma        Slip (or rake) angle \f$ \sigma \in [-90,90] \f$.
 *                          This is an array of dimension [nmt].
 * @param[out] K            If K is not NULL then it is the strike vector
 *                          (SOUTH-EAST-UP).  In this case K is an array
 *                          of dimension [3 x nmt] with leading dimension 3.
 * @param[out] N            If N is not NULL then it is the normal vector
 *                          (SOUTH-EAST-UP).  In this case N is an array
 *                          of dimension [3 x nmt] with leading dimension 3.
 * @param[out] S            If S Is not NULL then it is the slip vector
 *                          (SOUTH-EAST-UP).  In this case S is an array
 *                          of dimension [3 x nmt] with leading dimension 3.
 * @param[out] thetadc      If thetadc is not NULL then it is angle between
 *                          the moment tensor and the double couple where
 *                          \f$ \theta_{DC} \in [0,90] \f$.
 *                          In this case thetadc is an array of dimension [nmt].
 * @param[out] lam          If lam is not NULL then these are the eigenvalues
 *                          corresponding to the moment tensor.  In this
 *                          case lam is an array of dimension [3 x nmt]
 *                          with leading dimension 3.
 * @param[out] U            If U is not NULL then this is the basis in
 *                          SOUTH-EAST-UP for the moment tensor.  In this
 *                          case U an array of dimension [3 x 3 x nmt] with
 *                          leading dimension 9 and the understanding that
 *                          each 3 x 3 matrix is in column major format.
 *
 * @result 0 indicates success.
 *
 * @author Carl Tape and translated to C by Ben Baker
 *
 * @date July 2017
 *
 * @copyright MIT
 *
 */
int compearth_CMT2TT(const int nmt, const double *__restrict__ Min,
                     const bool ldisplay,
                     double *__restrict__ gamma,
                     double *__restrict__ delta,
                     double *__restrict__ M0,
                     double *__restrict__ kappa,
                     double *__restrict__ theta,
                     double *__restrict__ sigma,
                     double *__restrict__ K, double *__restrict__ N,
                     double *__restrict__ S, double *__restrict__ thetadc,
                     double *__restrict__ lam, double *__restrict__ U)
{
    double *lamdev, *lamiso, Vwork[9], Yrot[9], M[6],
           Sloc[12], Kloc[12], Nloc[12], kappaL[4], sigmaL[4], thetaL[4];
    double *thetadcWork;
    double Uwork[9*CE_CHUNKSIZE] __attribute__((aligned(64)));
    double lamWork[3*CE_CHUNKSIZE] __attribute__((aligned(64)));
    double Nwork[3*CE_CHUNKSIZE] __attribute__((aligned(64)));
    double Swork[3*CE_CHUNKSIZE] __attribute__((aligned(64)));
    //double lamSpace[3*MAXMT];
    //double uSpace[9*MAXMT];
    bool lwantLam, lwantK, lwantN, lwantS, lwantU;
    int itemp[4], ierr, ierrAll, match, imt, j, jmt, nmtLoc, nMatch;
    const int isort = 1;
    const double rotAngle = 45.0;
    const double tol = 1.e-6;
    ierr = 0;
    if (nmt < 1 || Min == NULL || gamma == NULL || delta == NULL ||
        M0 == NULL || kappa == NULL || theta == NULL || sigma == NULL)
    {
        if (nmt < 1){fprintf(stderr, "%s: No moment tensors\n", __func__);}
        if (Min == NULL){fprintf(stderr, "%s: Min is NULL\n", __func__);}
        if (gamma == NULL){fprintf(stderr, "%s: gamma is NULL\n", __func__);}
        if (delta == NULL){fprintf(stderr, "%s: delta is NULL\n", __func__);}
        if (M0 == NULL){fprintf(stderr, "%s: M0 is NULL\n", __func__);}
        if (kappa == NULL){fprintf(stderr, "%s: kappa is NULL\n", __func__);}
        if (theta == NULL){fprintf(stderr, "%s: theta is NULL\n", __func__);}
        if (sigma == NULL){fprintf(stderr, "%s: sigma is NULL\n", __func__);}
        return -1;
    }
    // Determine if eigenvalue/eigenvector decomposition is requested
    lwantLam = false;
    if (lam != NULL){lwantLam = true;}
    lwantU = false;
    if (U != NULL){lwantU = true;}
    // Determine the desired fault vectors
    lwantK = false;
    lwantN = false;
    lwantS = false;
    if (K != NULL){lwantK = true;}
    if (N != NULL){lwantN = true;}
    if (S != NULL){lwantS = true;}
    // Loop on moment tensor chunks
    for (jmt=0; jmt<nmt; jmt=jmt+CE_CHUNKSIZE)
    {
        nmtLoc = MIN(CE_CHUNKSIZE, nmt - jmt);
        // This is the numerically expensive part b/c of eigendecomposition
        ierrAll = 0;
        for (imt=0; imt<nmtLoc; imt++)
        {
            // KEY: Convert M into another basis.
            // YOU MUST ALSO CHANGE north AND zenith IN fault2vecang BELOW
            // --> U will be with respect to this basis (from CMTdecom.m)
            // ierr = compearth_convertMT(1, CE_USE, CE_NWU, &Min[6*imt], M);
            ierr = compearth_convertMT(1, CE_USE, CE_SEU,
                                       &Min[6*(jmt+imt)], M);
            if (ierr != 0)
            {
                fprintf(stderr, "%s: Error switching basis\n", __func__);
                ierrAll = ierrAll + 1; //break;
            }
            // PART 1: moment tensor source type (or pattern)
            // Decompose moment tensor into eigenvalues + basis (M = U*lam*U')
            // NOTE: ordering of eigenvalues is important.
            ierr = compearth_CMTdecom(1, M, isort, &lamWork[3*imt],
                                      &Uwork[9*imt]);
            if (ierr != 0)
            {
                fprintf(stderr, "%s: Error decomposing CMT\n", __func__);
                ierrAll = ierrAll + 1; //break;
            }
        }
        if (ierrAll != 0)
        {
            fprintf(stderr, "%s: Error during eigendecomposition\n", __func__);
            ierr = 1;
            goto ERROR;
        }
        // Compute the lune coordinates and magnitude from eigenvalues
        lamdev = NULL;
        lamiso = NULL;
        thetadcWork = NULL;
        if (thetadc != NULL){thetadcWork = &thetadc[jmt];}
        ierr = compearth_lam2lune(nmtLoc, lamWork, &gamma[jmt], &delta[jmt],
                                  &M0[jmt], thetadcWork,
                                  lamdev, lamiso);
        // Part 2: moment tensor orientation; TT2012, Section 6.3
        ierr = compearth_eulerUtil_rotmat(1, &rotAngle, 2, Yrot);
        if (ierr != 0)
        {
            fprintf(stderr, "%s: Error computing rotation matrix\n", __func__);
            return -1;
        }
        // Compute candidate fault vectors
        for (imt=0; imt<nmtLoc; imt++)
        {
            cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                        3, 3, 3, 1.0, &Uwork[9*imt], 3, Yrot, 3,
                        0.0, Vwork, 3); // V = U*Yrot (TT2012, p. 487)
            Swork[3*imt+0] = Vwork[0];
            Swork[3*imt+1] = Vwork[1];
            Swork[3*imt+2] = Vwork[2];
            Nwork[3*imt+0] = Vwork[6];
            Nwork[3*imt+1] = Vwork[7];
            Nwork[3*imt+2] = Vwork[8];
        }
        // Save lambda and U
        if (lwantLam){cblas_dcopy(3*nmtLoc, lamWork, 1, &lam[3*jmt], 1);}
        if (lwantU){cblas_dcopy(9*nmtLoc, Uwork, 1, &U[9*jmt], 1);}
        // Reassign ~0 elements to 0; ~1 elements to 1, and ~-1 elements to -1.
        setZero(nmtLoc, tol, Swork);
        setZero(nmtLoc, tol, Nwork);
        // Compute fault angles for four possible combinations (TT2012 Fig 15)
        for (imt=0; imt<nmtLoc; imt++)
        {
            for (j=0; j<3; j++)
            {
                Sloc[3*0+j] = Swork[3*imt+j]; Nloc[3*0+j] = Nwork[3*imt+j];
                Sloc[3*1+j] =-Swork[3*imt+j]; Nloc[3*1+j] =-Nwork[3*imt+j];
                Sloc[3*2+j] = Nwork[3*imt+j]; Nloc[3*2+j] = Swork[3*imt+j];
                Sloc[3*3+j] =-Nwork[3*imt+j]; Nloc[3*3+j] =-Swork[3*imt+j];
            }
            faultVec2Ang(&Sloc[0], &Nloc[0], &thetaL[0], &sigmaL[0],
                         &kappaL[0], &Kloc[0], &ierr);
            faultVec2Ang(&Sloc[3], &Nloc[3], &thetaL[1], &sigmaL[1],
                         &kappaL[1], &Kloc[3], &ierr);
            faultVec2Ang(&Sloc[6], &Nloc[6], &thetaL[2], &sigmaL[2],
                         &kappaL[2], &Kloc[6], &ierr);
            faultVec2Ang(&Sloc[9], &Nloc[9], &thetaL[3], &sigmaL[3],
                         &kappaL[3], &Kloc[9], &ierr);
            // There are four combinations of N and S that represent a double
            // copule moment tensor, as shown in Figure 15 of TT2012.
            // From these four combinations, there are two possible fault
            // planes.  We want to isoate the combination that is within the
            // boundingregion shown in Figures 16 and B1.
            memset(itemp, 0, 4*sizeof(int));
            nMatch = 0;
            for (j=0; j<4; j++)
            {
                if (thetaL[j] <= 90.0 + tol && fabs(sigmaL[j]) <= 90.0 + tol)
                {
                    itemp[nMatch] = j; //bmatch[j] = true;
                    nMatch = nMatch + 1;
                }
            }
            if (nMatch == 1)
            {
                match = itemp[0];
            }
            else if (nMatch == 2)
            {
                match = pickP1(thetaL[itemp[0]], sigmaL[itemp[0]], kappaL[itemp[0]],
                               thetaL[itemp[1]], sigmaL[itemp[1]], kappaL[itemp[1]],
                               tol, itemp[0], itemp[1]);
                if (match < 0)
                {
                    fprintf(stderr, "%s: Failed to pick a fault plane\n",
                             __func__);
                    ierr = 1;
                    goto ERROR;
                }
            }
            else if (nMatch == 3)
            {
                fprintf(stdout,
                  "%s: Warning mt on bdry of orientation domain 3 candidates\n",
                  __func__);
                fprintf(stdout, "%s: thetas: %e %e %e\n", __func__,
                        thetaL[0], thetaL[1], thetaL[2]);
                fprintf(stdout, "%s: sigmas: %e %e %e\n", __func__,
                        sigmaL[0], sigmaL[1], sigmaL[2]);
                fprintf(stdout, "%s: kappas: %e %e %e\n", __func__,
                        kappaL[0], kappaL[1], kappaL[2]);
                // Just take the first one
                match = itemp[0];
            }
            else if (nMatch == 4)
            {
                fprintf(stderr, "%s: Error not yet programmed\n", __func__);
                ierr = 1;
                goto ERROR;
            }
            else
            {
                fprintf(stderr, "%s: Error no match\n", __func__);
                ierr = 1;
                goto ERROR;
            }
            // Select the angle
            kappa[jmt+imt] = kappaL[match];
            sigma[jmt+imt] = sigmaL[match];
            theta[jmt+imt] = thetaL[match];
            // Fault vectors
            if (lwantK)
            {
                for (j=0; j<3; j++){K[3*(jmt+imt)+j] = Kloc[3*match+j];}
            }
            if (lwantN)
            {
                for (j=0; j<3; j++){N[3*(jmt+imt)+j] = Nloc[3*match+j];}
            }
            if (lwantS)
            {
                for (j=0; j<3; j++){S[3*(jmt+imt)+j] = Sloc[3*match+j];}
            }
        }
    } // Loop on moment tensor chunks
    if (ldisplay)
    {
        fprintf(stderr, "%s: ldisply not yet supported\n", __func__);
    }
ERROR:;
/*
    Uwork = NULL;
    lamWork = NULL;
*/
    return ierr;
}
/*!
 * @brief Returns fault angles in degrees.  Assumes input vectors are in
 *        the South-East-Up basis.
 *
 * @author Carl Tape and converted to C by Ben Baker
 *
 * @copyright MIT
 *
 */
static void faultVec2Ang(const double *__restrict__ S,
                         const double *__restrict__ N,
                         double *theta, double *sigma,
                         double *kappa, double *__restrict__ K, int *ierr)
{
    double v[3], costh, vnorm;
    int ierr1;
    const double deg = 180.0/M_PI;
    // South-East-Up (as In TT2012)
    const double zenith[3] = {0, 0, 1};
    const double negZenith[3] = {0, 0, -1};
    const double north[3] = {-1, 0, 0};
    *ierr = 0;
    *kappa = (double) NAN;
    *theta = (double) NAN;
    *sigma = (double) NAN;
    // Strike vector from TT2012, Eqn 29
    cross3(zenith, N, v);
    vnorm = norm3(v);
    if (vnorm < DBL_EPSILON) //== 0.0)
    {
        fprintf(stderr,
              "%s: Horizontal fault -- strike vector is same as slip vector\n",
                __func__);
        K[0] = S[0];
        K[1] = S[1];
        K[2] = S[2];
    }
    else
    {
        K[0] = v[0]/vnorm;
        K[1] = v[1]/vnorm;
        K[2] = v[2]/vnorm;
    }
    // Figure 14
    *kappa = compearth_eulerUtil_fangleSigned(3, north, K, negZenith, &ierr1);
    if (ierr1 != 0){*ierr = *ierr + 1;}
    *kappa = wrap360(*kappa);
    // Figure 14
    costh = dot3(N, zenith);
    *theta = acos(costh)*deg;
    // Figure 14
    *sigma = compearth_eulerUtil_fangleSigned(3, K, S, N, &ierr1);
    if (ierr1 != 0){*ierr = *ierr + 1;}
    return;
}

static void setZero(const int nmt, const double tol, double *__restrict__ X)
{
   double dmax;
   int i, idmax;
   // Compute the largest element of abs(X)
   idmax = (int) cblas_idamax(3*nmt, X, 1);
   dmax = X[idmax];
   // Elements near zero whilst trying to eliminate round-off errors
   #pragma omp simd
   for (i=0; i<3*nmt; i++)
   {
       if (fabs(X[i]/dmax) < tol){X[i] = 0.0;}
   }
   #pragma omp simd
   for (i=0; i<3*nmt; i++)
   {
       if (fabs(X[i] - 1.0) < tol){X[i] =-1.0;}
   }
   #pragma omp simd
   for (i=0; i<3*nmt; i++)
   {
       if (fabs(X[i] + 1.0) < tol){X[i] = 1.0;}
   }
   return;
}

static int pickP1(const double thetaA, const double sigmaA, const double kappaA,
                  const double thetaB, const double sigmaB, const double kappaB,
                  const double tol, const int p1, const int p2)
{
    int ipick;
    ipick =-1;
    if (fabs(thetaA - 90.0) < tol)
    {
        if (kappaA < 180.0){ipick = p1;}
        if (kappaB < 180.0){ipick = p2;}
        return ipick;
    }
    if (fabs(sigmaA - 90.0) < tol)
    {
        if (kappaA < 180.0){ipick = p1;}
        if (kappaB < 180.0){ipick = p2;}
        return ipick;
    }
    if (fabs(sigmaA + 90.0) < tol)
    {
        if (kappaA < 180.0){ipick = p1;}
        if (kappaB < 180.0){ipick = p2;}
        return ipick;
    }
    fprintf(stderr, "%s: Error no selection criterion was met\n", __func__);
    fprintf(stderr, "thetaA,sigmaA,kappaA=%f,%f,%f\n", thetaA, sigmaA, kappaA);
    fprintf(stderr, "thetaB,sigmaB,kappaB=%f,%f,%f\n", thetaB, sigmaB, kappaB);
    return ipick;
}


/*
   (c) 2016 Greg Lee
*/

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

#include <gsl/gsl_sf_bessel.h>

// union cast macro from Matt Gallagher
#define UNION_CAST(x, destType) \
 (((union {__typeof__(x) a; destType b;})x).b)

long int gsl_sf_bessel_J0_c( long int i)
{
   double result = gsl_sf_bessel_J0( UNION_CAST( i, double ) );
   return UNION_CAST( result, long int );
}

/* End of file */


/*
 findcluster_ternary_par.c
 written by Eunseok Lee

 function: find the most representative cluster function

 v1: Feb 2, 2018
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <sys/stat.h>
//#include <cem.h>
#include <mpi.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

void randperm(int*, int);
void load_double_mat(double*, int, int, int, char*);
void load_int_mat(int*, int, int, int, char*);
void load_double_array(double*, int, char*);
void load_int_array(int*, int, char*);
void load_double_value(int,char);
void load_int_value(int,char);
int mat_size_row(int*);
int find_value_in_array(int*,int,int,int);
void mat2d_prod(double*, int, int, double*, int, int, double*, int, int);
double obtainerr2_par(int,int,double*, int, int, int*, int, double*);
void least_square_solver(double*, int, int, double*, double*);

#define MASTER 0
#define FROM_MASTER 1
#define FROM_WORKER 2

int main(int argc, char **argv)
{
    int n1, n2, n3;
    int np, nfu;
    double kv[3][3], pk[3][3], pp[8][3];
    double *rp, *rpL, *rpN, *rpO;
    int *map_to_cluster1, *map_to_cluster2, *map_to_cluster3;
    int *nlist;
    int neighbor_num;
    int ncluster, ncluster2, ncluster3, ncorr_col;
    int c2start, c3start;
    int data;
    double *E, *Ef;
    int ndata;
    int i, j, k, nj, nk, ctn;
    int iter, max_iter;
    double kT=0.0256;   // Default value of kT
    int howmanycluster;
    int dispfreq;
    char buff_line[200], dummy[200], param_name[100];
    int param_name_len;
    double cvs_tol = 0.01;  // Default value of cvs_tol
    int construct_corr_mat = 0; //Default value of construct_corr_mat

    // mpi parameters and initialization
    int numprocs, rank, mtype;
    int row_dist_size;
    int row_ini, row_end, row_offset;
    MPI_Status status;
    MPI_Init(&argc, &argv);
    MPI_Comm_size(MPI_COMM_WORLD, &numprocs);  // Get # processors
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);      // Get my rank (id)

    if (numprocs>1)
        printf("Parallel computing: the input files are read by all nodes. Multiple messages are not due to error!\n",numprocs);

    // Construct conrrelation matrix. It can also be loaded.

    int n_corr_mat_ug_row;
    int n_non_singular_col;
    FILE *fp;
    char dirname[100]="dir_inputs";
    char paramfilename[100];
    sprintf(paramfilename,"%s/param.dat",dirname);
    char corr_mat_ugs_filename[200];
    fp = fopen(paramfilename, "r");
    while(fgets(buff_line,sizeof(buff_line),fp) != NULL) {
        if (buff_line[0] == '#') {
            //            printf("Comment line: %s",buff_line);
            continue;
        }
        strcpy(param_name,"corr_mat_ugs_filename");
        param_name_len = strlen(param_name);
        strncpy(dummy,buff_line,param_name_len);
        dummy[param_name_len] = '\0';
        if (strcmp(dummy,param_name)==0) {
            sscanf(buff_line,"%s %s",dummy,corr_mat_ugs_filename);
            if (rank == MASTER)
                printf("corr_mat_ugs_filename = %s\n",corr_mat_ugs_filename);
        }
        strcpy(param_name,"n_corr_mat_ug_row");
        param_name_len = strlen(param_name);
        strncpy(dummy,buff_line,param_name_len);
        dummy[param_name_len] = '\0';
        if (strcmp(dummy,param_name)==0) {
            sscanf(buff_line,"%s %d",dummy,&n_corr_mat_ug_row);
            if (rank == MASTER)
                printf("n_corr_mat_ug_row = %d\n",n_corr_mat_ug_row);
        }
        strcpy(param_name,"n_non_singular_col");
        param_name_len = strlen(param_name);
        strncpy(dummy,buff_line,param_name_len);
        dummy[param_name_len] = '\0';
        if (strcmp(dummy,param_name)==0) {
            sscanf(buff_line,"%s %d",dummy,&n_non_singular_col);
            if (rank == MASTER)
                printf("n_non_singular_col = %d\n",n_non_singular_col);
        }
        strcpy(param_name,"howmanycluster");
        param_name_len = strlen(param_name);
        strncpy(dummy,buff_line,param_name_len);
        dummy[param_name_len] = '\0';
        if (strcmp(dummy,param_name)==0) {
            sscanf(buff_line,"%s %d",dummy,&howmanycluster);
            if (rank == MASTER)
                printf("howmanycluster = %d\n",howmanycluster);
        }
        strcpy(param_name,"max_iter");
        param_name_len = strlen(param_name);
        strncpy(dummy,buff_line,param_name_len);
        dummy[param_name_len] = '\0';
        if (strcmp(dummy,param_name)==0) {
            sscanf(buff_line,"%s %d",dummy,&max_iter);
            if (rank == MASTER)
                printf("max_iter = %d\n",max_iter);
        }
        strcpy(param_name,"kT");
        param_name_len = strlen(param_name);
        strncpy(dummy,buff_line,param_name_len);
        dummy[param_name_len] = '\0';
        if (strcmp(dummy,param_name)==0) {
            sscanf(buff_line,"%s %lf",dummy,&kT);
            if (rank == MASTER)
                printf("kT = %f\n",kT);
        }
        strcpy(param_name,"cvs_tol");
        param_name_len = strlen(param_name);
        strncpy(dummy,buff_line,param_name_len);
        dummy[param_name_len] = '\0';
        if (strcmp(dummy,param_name)==0) {
            sscanf(buff_line,"%s %lf",dummy,&cvs_tol);
            if (rank == MASTER)
                printf("cvs_tol = %f\n",cvs_tol);
        }
        strcpy(param_name,"dispfreq");
        param_name_len = strlen(param_name);
        strncpy(dummy,buff_line,param_name_len);
        dummy[param_name_len] = '\0';
        if (strcmp(dummy,param_name)==0) {
            sscanf(buff_line,"%s %d",dummy,&dispfreq);
            if (rank == MASTER)
                printf("dispfreq = %d\n",dispfreq);
        }
    }
    fclose(fp);

    char loadfilename[100];
    double *corr_mat_ugs;
    corr_mat_ugs = (double*) malloc(n_corr_mat_ug_row*n_non_singular_col*sizeof(double));
    sprintf(loadfilename,"%s/%s",dirname,corr_mat_ugs_filename);
    load_double_mat(corr_mat_ugs,n_corr_mat_ug_row,n_non_singular_col,1,loadfilename);

    int usefulcorr_col[n_non_singular_col];
    sprintf(loadfilename,"%s/usefulcorr_col.dat",dirname);
    load_int_array(usefulcorr_col,n_non_singular_col,loadfilename);

    double Ef_ug[n_corr_mat_ug_row];
    sprintf(loadfilename,"%s/Ef_ug.dat",dirname);
    load_double_array(Ef_ug,n_corr_mat_ug_row,loadfilename);

/*    int nC_ug[n_corr_mat_ug_row];
    sprintf(loadfilename,"%s/nC_ug.dat",dirname);
    load_int_array(nC_ug,n_corr_mat_ug_row,loadfilename);
*/
    row_offset = (int) ceil(n_corr_mat_ug_row*1.0/numprocs);
    if (numprocs>1 && rank==MASTER)
        printf("Correlation Matrix is calculated over %d processors: row_offset = %d\n",numprocs,row_offset);
    double err2_sum_from_master;
    double err2_sum_from_worker;
    int cluster_set1[howmanycluster];

    /**** Start of MC simulation ****/
    if (rank == MASTER) {
        int cluster_set1_old[howmanycluster], cluster_set1_min[howmanycluster], cluster_set2[n_non_singular_col-howmanycluster];
        double cvs, cvs_old, cvs_min;
        int select1, select2, target, candidate;

        for (i=0;i<howmanycluster;i++)
            cluster_set1[i] = i;
        for (i=0;i<n_non_singular_col-howmanycluster;i++)
            cluster_set2[i] = i+howmanycluster;

        mtype = FROM_MASTER;
        for (i=1;i<numprocs;i++) {
            MPI_Send(&cluster_set1, howmanycluster, MPI_INT, i, mtype, MPI_COMM_WORLD);
        }
        row_ini = rank * row_offset;
        row_end = row_ini +row_offset;
        err2_sum_from_master = obtainerr2_par(row_ini,row_end,corr_mat_ugs,n_corr_mat_ug_row,n_non_singular_col,cluster_set1,howmanycluster,Ef_ug);
//        printf("initial err2 = %f\n",err2_sum_from_master);
        for (i=1;i<numprocs;i++) {
            mtype = FROM_WORKER;
            MPI_Recv(&err2_sum_from_worker, 1, MPI_DOUBLE, i, mtype, MPI_COMM_WORLD, &status);
            err2_sum_from_master += err2_sum_from_worker;
        }
        printf("initial err2 = %f\n",err2_sum_from_master);
        cvs = sqrt(err2_sum_from_master/n_corr_mat_ug_row);
        cvs_min = cvs;
        for (i=0;i<howmanycluster;i++)
            cluster_set1_min[i] = cluster_set1[i];
        printf("initial cvs = %f\n",cvs);
        for (iter=0;iter<max_iter;iter++) {
            select1 = (int) floor(drand48()*howmanycluster);
            select2 = (int) floor(drand48()*(n_non_singular_col-howmanycluster));
            target = cluster_set1[select1];
            candidate = cluster_set2[select2];
            cvs_old = cvs;
            cluster_set1[select1] = candidate;
            mtype = FROM_MASTER;
            for (i=1;i<numprocs;i++) {
                MPI_Send(&cluster_set1, howmanycluster, MPI_INT, i, mtype, MPI_COMM_WORLD);
            }
            row_ini = rank * row_offset;
            row_end = row_ini +row_offset;
            err2_sum_from_master = obtainerr2_par(row_ini,row_end,corr_mat_ugs,n_corr_mat_ug_row,n_non_singular_col,cluster_set1,howmanycluster,Ef_ug);
            for (i=1;i<numprocs;i++) {
                mtype = FROM_WORKER;
                MPI_Recv(&err2_sum_from_worker, 1, MPI_DOUBLE, i, mtype, MPI_COMM_WORLD, &status);
                err2_sum_from_master += err2_sum_from_worker;
            }
            cvs = sqrt(err2_sum_from_master/n_corr_mat_ug_row);
            if (cvs < cvs_old)
                cluster_set2[select2] = target;
            else if (exp(-(cvs-cvs_old)/kT) > drand48())
                cluster_set2[select2] = target;
            else {
                cluster_set1[select1] = target;
                cvs = cvs_old;
            }
            if (cvs < cvs_min) {
                for (i=0;i<howmanycluster;i++)
                    cluster_set1_min[i] = cluster_set1[i];
                cvs_min = cvs;
            }
            if (cvs < cvs_tol)
                break;
            if (iter%dispfreq==0)
                printf("iter=%8d, cvs=%f, cvs_min=%f, cvs_min/fu=%f\n",iter,cvs,cvs_min,cvs_min/72);
        }

        // print the result
        printf("End of MC iteration. The selected clusters are...\n");
        printf("ID in non-singular | ID in entire\n");
        for (i=0;i<howmanycluster;i++)
            printf("    %4d                %4d\n",cluster_set1_min[i],usefulcorr_col[cluster_set1_min[i]]);

        double err_pred, rms;
        double Ef_pred[n_corr_mat_ug_row];
        double *corr_mat_ugs_reduced;
        corr_mat_ugs_reduced = (double*) malloc(n_corr_mat_ug_row*howmanycluster*sizeof(double));
        double eci[howmanycluster];
        for (i=0;i<n_corr_mat_ug_row;i++) {
            for (j=0;j<howmanycluster;j++)
                *(corr_mat_ugs_reduced+howmanycluster*i+j) = *(corr_mat_ugs+n_non_singular_col*i+cluster_set1_min[j]);
        }
        least_square_solver(corr_mat_ugs_reduced,n_corr_mat_ug_row,howmanycluster,Ef_ug,eci);
        mat2d_prod(Ef_pred,n_corr_mat_ug_row,1,corr_mat_ugs_reduced,n_corr_mat_ug_row,howmanycluster,eci,howmanycluster,1);
        err_pred = 0.0;
        for (i=0;i<n_corr_mat_ug_row;i++)
            err_pred += (Ef_ug[i] - Ef_pred[i])*(Ef_ug[i] - Ef_pred[i]);
        rms = sqrt(err_pred/n_corr_mat_ug_row);

        // Write result into files
        FILE *fp2, *fp3;
        char cluster_set_filename[200];
        char eci_filename[100];
        strcpy(dirname,"dir_result");
        struct stat st = {0};
        if (stat(dirname, &st) == -1) {
            mkdir(dirname,0777);
            printf("Created a new directory for storing result.\n");
        }
        sprintf(cluster_set_filename, "%s/cluster_set_N%d.dat",dirname,howmanycluster);
        sprintf(eci_filename,"%s/x.dat",dirname);
        fp2 = fopen(cluster_set_filename,"w");
        fp3 = fopen(eci_filename,"w");
        for (i=0;i<howmanycluster;i++) {
            fprintf(fp2,"%4d\t%4d\n",cluster_set1_min[i],usefulcorr_col[cluster_set1_min[i]]);
            fprintf(fp3,"%.4f\n",eci[i]);
        }
    }
    else {
        for (iter=0;iter<(max_iter+1);iter++) {
            mtype = FROM_MASTER;
            MPI_Recv(&cluster_set1, howmanycluster, MPI_INT, MASTER, mtype, MPI_COMM_WORLD, &status);
            row_ini = rank * row_offset;
            row_end = row_ini + row_offset;
            if (rank == numprocs-1)
                row_end = n_corr_mat_ug_row;  // last processor till the last row, -1 is taken inside obtainerr2_par
            err2_sum_from_worker = obtainerr2_par(row_ini,row_end,corr_mat_ugs,n_corr_mat_ug_row,n_non_singular_col,cluster_set1,howmanycluster,Ef_ug);
            mtype = FROM_WORKER;
            MPI_Send(&err2_sum_from_worker, 1, MPI_DOUBLE, MASTER, mtype, MPI_COMM_WORLD);
        }
    }
    MPI_Finalize();

}

void load_double_mat(double *A, int Arow, int Acol, int Apgs, char *datfilename) {

    FILE *fp;
    int i, j, k;
    double tmp;

    printf("filename: %s\n", datfilename);
    fp = fopen(datfilename, "r");
    for (k=0;k<Apgs;k++)
        for (i=0;i<Arow;i++)
            for (j=0;j<Acol;j++) {
                fscanf(fp, "%lf", &tmp);
                *(A+Arow*Acol*k+Acol*i+j) = tmp;
            }

    fclose(fp);

}

void load_int_mat(int *A, int Arow, int Acol, int Apgs, char *datfilename) {

    FILE *fp;
    int i, j, k, tmp;

    printf("filename: %s\n", datfilename);
    fp = fopen(datfilename, "r");
    for (k=0;k<Apgs;k++)
        for (i=0;i<Arow;i++)
            for (j=0;j<Acol;j++) {
                fscanf(fp, "%d", &tmp);
                *(A+Arow*Acol*k+Acol*i+j) = tmp;
            }

    fclose(fp);

}

void load_int_array(int *array, int arraysize, char *datfilename) {

    FILE *fp;
    int i;

    printf("filename: %s\n", datfilename);
    fp = fopen(datfilename, "r");
    //    printf("step2\n");
    for (i=0;i<arraysize;i++) {
        fscanf(fp, "%d", &array[i]);
        //        printf("%d\n",array[i]);
    }

    fclose(fp);
    //    printf("step3\n");
}

void load_double_array(double *array, int arraysize, char *datfilename) {

    FILE *fp;
    int i;

    printf("filename: %s\n", datfilename);
    fp = fopen(datfilename, "r");
    //    printf("step4\n");
    for (i=0;i<arraysize;i++) {
        fscanf(fp, "%lf", &array[i]);
        //        printf("%f\n",array[i]);
    }

    fclose(fp);
    //    printf("step5\n");
}

void randperm(int *a, int np) {

    int i, id1, id2, id3, tmpr, num_perm;

    num_perm = 100*np;

    for (i=0;i<np;i++)
        *(a+i) = i;

    for (i=0;i<num_perm;i++) {
        id1 = rand() % np;
        id3 = rand() % np;
        id2 = (id1 + id3) % np;
        if (id1 != id2) {
            tmpr = *(a+id2);
            *(a+id2) = *(a+id1);
            *(a+id1) = tmpr;
        }
    }
}

int find_value_in_array(int *A, int target_rowA, int colA, int target_value) {
    int i;

    for (i=0;i<colA;i++)
        if (*(A+colA*target_rowA+i)==target_value)
            return(i);
    if (i==colA)
        return(-1);
}


#include <stdio.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <gbpLib.h>
#include <gbpRNG.h>
#include <gbpMCMC.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_interp.h>

void generate_MCMC_parameters(MCMC_info *MCMC) {
    int                i_P;
    int                j_P;
    double             factor_i;
    int                flag_continue;
    static int         reflect_priors;
    static int         n_P;
    static double      factor;
    static double *    P_new;
    static double *    P_chain;
    static double *    P_init;
    static double *    P_limit_min;
    static double *    P_limit_max;
    static gsl_vector *b;
    static gsl_matrix *m;
    static RNG_info *  RNG;

    // Initialize a few things on the first call
    switch(MCMC->first_parameter_call) {
        case GBP_TRUE:
            n_P                        = MCMC->n_P;
            P_new                      = MCMC->P_new;
            P_chain                    = MCMC->P_chain;
            P_init                     = MCMC->P_init;
            P_limit_min                = MCMC->P_limit_min;
            P_limit_max                = MCMC->P_limit_max;
            b                          = MCMC->b;
            m                          = MCMC->m;
            RNG                        = MCMC->RNG;
            factor                     = 2.4 / sqrt((double)MCMC->n_M_total);
            MCMC->first_parameter_call = GBP_FALSE;
            reflect_priors             = SID_CHECK_BITFIELD_SWITCH(MCMC->mode, MCMC_MODE_REFLECT_PRIORS);
            break;
    }

    // Loop until a satisfactory set of parameters has been selected
    flag_continue = GBP_TRUE;
    while(flag_continue) {
        // Generate a random displacement vector
        factor_i = MCMC->temperature * factor;
        for(i_P = 0; i_P < n_P; i_P++)
            gsl_vector_set(b, i_P, factor_i * random_gaussian(RNG));

        // Use the rotated covariance matrix (if available)
        if(m != NULL) {
            memcpy(P_new, P_chain, n_P * sizeof(double));
            for(i_P = 0; i_P < n_P; i_P++) {
                for(j_P = 0; j_P < n_P; j_P++)
                    P_new[i_P] += gsl_matrix_get(m, i_P, j_P) * gsl_vector_get(b, j_P);
            }
        } else {
            for(j_P = 0; j_P < n_P; j_P++)
                P_new[j_P] = P_init[j_P] * (1. + gsl_vector_get(b, j_P));
        }

        // Enforce parameter limits
        if(reflect_priors) {
            for(i_P = 0; i_P < n_P; i_P++) {
                if(P_new[i_P] < P_limit_min[i_P])
                    P_new[i_P] = 2 * (P_limit_min[i_P]) - P_new[i_P];
                if(P_new[i_P] > P_limit_max[i_P])
                    P_new[i_P] = 2 * (P_limit_max[i_P]) - P_new[i_P];
            }
        } else {
            for(i_P = 0, flag_continue = GBP_FALSE; i_P < n_P; i_P++)
                if(P_new[i_P] < P_limit_min[i_P] || P_new[i_P] > P_limit_max[i_P])
                    flag_continue = GBP_TRUE;
        }
    }

    if(!SID_CHECK_BITFIELD_SWITCH(MCMC->mode, MCMC_MODE_PARALLEL))
        SID_Bcast(P_new, n_P, SID_DOUBLE, SID_MASTER_RANK, MCMC->comm);
}