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

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_permutation.h>

static int
test_QR_decomp_dim(const gsl_matrix * m, double eps)
{
  int s = 0;
  const size_t M = m->size1;
  const size_t N = m->size2;
  size_t i, j;

  gsl_matrix * qr = gsl_matrix_alloc(M, N);
  gsl_matrix * a  = gsl_matrix_alloc(M, N);
  gsl_matrix * q  = gsl_matrix_alloc(M, M);
  gsl_matrix * r  = gsl_matrix_alloc(M, N);
  gsl_vector * d = gsl_vector_alloc(N);

  gsl_matrix_memcpy(qr, m);

  s += gsl_linalg_QR_decomp(qr, d);
  s += gsl_linalg_QR_unpack(qr, d, q, r);

  /* compute a = q r */
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, q, r, 0.0, a);

  for(i=0; i<M; i++) {
    for(j=0; j<N; j++) {
      double aij = gsl_matrix_get(a, i, j);
      double mij = gsl_matrix_get(m, i, j);
      int foo = check(aij, mij, eps);
      if(foo) {
        printf("(%3lu,%3lu)[%lu,%lu]: %22.18g   %22.18g\n", M, N, i,j, aij, mij);
      }
      s += foo;
    }
  }

  gsl_vector_free(d);
  gsl_matrix_free(qr);
  gsl_matrix_free(a);
  gsl_matrix_free(q);
  gsl_matrix_free(r);

  return s;
}

static int
test_QR_decomp(void)
{
  int f;
  int s = 0;

  f = test_QR_decomp_dim(m35, 2 * 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  QR_decomp m(3,5)");
  s += f;

  f = test_QR_decomp_dim(m53, 2 * 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  QR_decomp m(5,3)");
  s += f;

  f = test_QR_decomp_dim(hilb2, 2 * 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  QR_decomp hilbert(2)");
  s += f;

  f = test_QR_decomp_dim(hilb3, 2 * 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  QR_decomp hilbert(3)");
  s += f;

  f = test_QR_decomp_dim(hilb4, 2 * 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  QR_decomp hilbert(4)");
  s += f;

  f = test_QR_decomp_dim(hilb12, 2 * 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  QR_decomp hilbert(12)");
  s += f;

  f = test_QR_decomp_dim(vander2, 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  QR_decomp vander(2)");
  s += f;

  f = test_QR_decomp_dim(vander3, 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  QR_decomp vander(3)");
  s += f;

  f = test_QR_decomp_dim(vander4, 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  QR_decomp vander(4)");
  s += f;

  f = test_QR_decomp_dim(vander12, 0.0005); /* FIXME: bad accuracy */
  gsl_test(f, "  QR_decomp vander(12)");
  s += f;

  return s;
}

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

  gsl_matrix * QR = gsl_matrix_alloc(M, N);
  gsl_matrix * T = gsl_matrix_alloc(N, N);
  gsl_matrix * A  = gsl_matrix_alloc(M, N);
  gsl_matrix * R  = gsl_matrix_alloc(N, N);
  gsl_matrix * Q  = gsl_matrix_alloc(M, M);
  gsl_matrix_view Q1 = gsl_matrix_submatrix(Q, 0, 0, M, N);

  gsl_matrix_memcpy(QR, m);

  s += gsl_linalg_QR_decomp_r(QR, T);
  s += gsl_linalg_QR_unpack_r(QR, T, Q, R);

  /* compute A = Q R */
  gsl_matrix_memcpy(A, &Q1.matrix);
  gsl_blas_dtrmm (CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, R, A);

  for (i = 0; i < M; 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, M, N, i,j, aij, mij);
        }
    }

  if (M > N)
    {
      gsl_matrix * R_alt  = gsl_matrix_alloc(M, N);
      gsl_matrix * Q_alt  = gsl_matrix_alloc(M, M);
      gsl_vector_view tau = gsl_matrix_diagonal(T);

      /* test that Q2 was computed correctly by comparing with Level 2 algorithm */
      gsl_linalg_QR_unpack(QR, &tau.vector, Q_alt, R_alt);

      for (i = 0; i < M; i++)
        {
          for (j = 0; j < M; j++)
            {
              double aij = gsl_matrix_get(Q, i, j);
              double bij = gsl_matrix_get(Q_alt, i, j);

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

      gsl_matrix_free(R_alt);
      gsl_matrix_free(Q_alt);
    }

  gsl_matrix_free(QR);
  gsl_matrix_free(T);
  gsl_matrix_free(A);
  gsl_matrix_free(Q);
  gsl_matrix_free(R);

  return s;
}

static int
test_QR_decomp_r(gsl_rng * r)
{
  int s = 0;
  size_t M, N;

  for (M = 1; M <= 50; ++M)
    {
      for (N = 1; N <= M; ++N)
        {
          gsl_matrix * A = gsl_matrix_alloc(M, N);

          create_random_matrix(A, r);
          s += test_QR_decomp_r_eps(A, 1.0e6 * M * GSL_DBL_EPSILON, "QR_decomp_r random");

          gsl_matrix_free(A);
        }
    }

  s += test_QR_decomp_r_eps(m53,   1.0e2 * GSL_DBL_EPSILON, "QR_decomp_r m(5,3)");
  s += test_QR_decomp_r_eps(hilb2, 1.0e2 * GSL_DBL_EPSILON, "QR_decomp_r hilbert(2)");
  s += test_QR_decomp_r_eps(hilb3, 1.0e2 * GSL_DBL_EPSILON, "QR_decomp_r hilbert(3)");
  s += test_QR_decomp_r_eps(hilb4, 1.0e2 * GSL_DBL_EPSILON, "QR_decomp_r hilbert(4)");
  s += test_QR_decomp_r_eps(hilb12, 1.0e2 * GSL_DBL_EPSILON, "QR_decomp_r hilbert(12)");
  s += test_QR_decomp_r_eps(vander2, 1.0e1 * GSL_DBL_EPSILON, "QR_decomp_r vander(2)");
  s += test_QR_decomp_r_eps(vander3, 1.0e1 * GSL_DBL_EPSILON, "QR_decomp_r vander(3)");
  s += test_QR_decomp_r_eps(vander4, 1.0e1 * GSL_DBL_EPSILON, "QR_decomp_r vander(4)");

  return s;
}

static int
test_QR_QTmat_r_eps(const gsl_matrix * A, const gsl_matrix * B, const double eps, const char * desc)
{
  int s = 0;
  const size_t M = A->size1;
  const size_t N = A->size2;
  const size_t K = B->size2;
  size_t i, j;

  gsl_matrix * QR = gsl_matrix_alloc(M, N);
  gsl_matrix * T = gsl_matrix_alloc(N, N);
  gsl_matrix * work  = gsl_matrix_alloc(N, K);
  gsl_matrix * B1 = gsl_matrix_alloc(M, K);
  gsl_matrix * B2 = gsl_matrix_alloc(M, K);
  gsl_vector_view tau = gsl_matrix_diagonal(T);

  gsl_matrix_memcpy(QR, A);
  gsl_matrix_memcpy(B1, B);
  gsl_matrix_memcpy(B2, B);

  s += gsl_linalg_QR_decomp_r(QR, T);

  /* compute Q^T B with both recursive and non-recursive methods and compare */
  s += gsl_linalg_QR_QTmat_r(QR, T, B1, work);
  s += gsl_linalg_QR_QTmat(QR, &tau.vector, B2);

  for (i = 0; i < M; i++)
    {
      for (j = 0; j < K; j++)
        {
          double aij = gsl_matrix_get(B1, i, j);
          double bij = gsl_matrix_get(B2, i, j);

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

  gsl_matrix_free(QR);
  gsl_matrix_free(T);
  gsl_matrix_free(B1);
  gsl_matrix_free(B2);
  gsl_matrix_free(work);

  return s;
}

static int
test_QR_QTmat_r(gsl_rng * r)
{
  int s = 0;
  size_t M, N;

  for (M = 1; M <= 50; ++M)
    {
      for (N = 1; N <= M; ++N)
        {
          const size_t K = GSL_MAX(N / 2, 1);
          gsl_matrix * A = gsl_matrix_alloc(M, N);
          gsl_matrix * B = gsl_matrix_alloc(M, K);

          create_random_matrix(A, r);
          create_random_matrix(B, r);
          s += test_QR_QTmat_r_eps(A, B, 1.0e6 * M * GSL_DBL_EPSILON, "QR_QTmat_r random");

          gsl_matrix_free(A);
          gsl_matrix_free(B);
        }
    }

  return s;
}

static int
test_QR_solve_r_eps(const gsl_matrix * A, const gsl_vector * rhs, const gsl_vector * sol,
                    const double eps, const char * desc)
{
  int s = 0;
  const size_t M = A->size1;
  const size_t N = A->size2;
  size_t i;

  gsl_matrix * QR = gsl_matrix_alloc(M, N);
  gsl_matrix * T = gsl_matrix_alloc(N, N);
  gsl_vector * x  = gsl_vector_alloc(N);

  gsl_matrix_memcpy(QR, A);

  s += gsl_linalg_QR_decomp_r(QR, T);
  s += gsl_linalg_QR_solve_r(QR, T, rhs, x);

  for (i = 0; i < M; 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_matrix_free(QR);
  gsl_matrix_free(T);
  gsl_vector_free(x);

  return s;
}

static int
test_QR_solve_r(gsl_rng * r)
{
  int s = 0;
  size_t N;

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

      create_random_matrix(A, r);
      create_random_vector(sol, r);
      gsl_blas_dgemv(CblasNoTrans, 1.0, A, sol, 0.0, rhs);

      s += test_QR_solve_r_eps(A, rhs, sol, 1.0e5 * N * GSL_DBL_EPSILON, "QR_solve_r random");

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

  return s;
}

static int
test_QR_lssolve_r_eps(const gsl_matrix * A, const gsl_vector * b, const double eps, const char * desc)
{
  int s = 0;
  const size_t M = A->size1;
  const size_t N = A->size2;
  size_t i;

  gsl_matrix * QR = gsl_matrix_alloc(M, N);
  gsl_matrix * T = gsl_matrix_alloc(N, N);
  gsl_vector * x = gsl_vector_alloc(M);
  gsl_vector * work  = gsl_vector_alloc(N);

  gsl_matrix * U = gsl_matrix_alloc(M, N);
  gsl_matrix * V = gsl_matrix_alloc(N, N);
  gsl_vector * S = gsl_vector_alloc(N);
  gsl_vector * x_svd = gsl_vector_alloc(N);

  gsl_vector * residual = gsl_vector_alloc(M);

  gsl_matrix_memcpy(QR, A);
  s += gsl_linalg_QR_decomp_r(QR, T);
  s += gsl_linalg_QR_lssolve_r(QR, T, b, x, work);

  gsl_matrix_memcpy(U, A);
  gsl_linalg_SV_decomp(U, V, S, work);
  gsl_linalg_SV_solve(U, V, S, b, x_svd);

  /* compare QR with SVD solution */
  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(x_svd, i);

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

  if (M > N)
    {
      gsl_vector_view x1 = gsl_vector_subvector(x, 0, N);
      gsl_vector_view x2 = gsl_vector_subvector(x, N, M - N);
      double norm = gsl_blas_dnrm2(&x2.vector);
      double norm_expected;

      /* compute residual and check || x(N+1:end) || = || b - A x || */
      gsl_vector_memcpy(residual, b);
      gsl_blas_dgemv(CblasNoTrans, -1.0, A, &x1.vector, 1.0, residual);
      norm_expected = gsl_blas_dnrm2(residual);

      gsl_test_rel(norm, norm_expected, eps, "%s rnorm (%3lu,%3lu): %22.18g   %22.18g\n",
                   desc, M, N, norm, norm_expected);
    }

  gsl_matrix_free(QR);
  gsl_matrix_free(T);
  gsl_vector_free(x);
  gsl_matrix_free(U);
  gsl_matrix_free(V);
  gsl_vector_free(x_svd);
  gsl_vector_free(work);
  gsl_vector_free(S);
  gsl_vector_free(residual);

  return s;
}

static int
test_QR_lssolve_r(gsl_rng * r)
{
  int s = 0;
  size_t M, N;

  for (M = 1; M <= 30; ++M)
    {
      for (N = 1; N <= M; ++N)
        {
          gsl_matrix * A = gsl_matrix_alloc(M, N);
          gsl_vector * b = gsl_vector_alloc(M);

          create_random_matrix(A, r);
          create_random_vector(b, r);

          s += test_QR_lssolve_r_eps(A, b, 1.0e5 * M * GSL_DBL_EPSILON, "QR_lssolve_r random");

          gsl_matrix_free(A);
          gsl_vector_free(b);
        }
    }

  return s;
}

static int
test_QR_UR_decomp_eps(const gsl_matrix * S, const gsl_matrix * A, const double eps, const char * desc)
{
  int s = 0;
  const size_t M = A->size1;
  const size_t N = A->size2;
  size_t i, j;

  gsl_matrix * R = gsl_matrix_calloc(N, N);
  gsl_matrix * V = gsl_matrix_alloc(M, N);
  gsl_matrix * T = gsl_matrix_alloc(N, N);
  gsl_matrix * B1  = gsl_matrix_calloc(N, N);
  gsl_matrix * B2  = gsl_matrix_alloc(M, N);

  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, R, S);
  gsl_matrix_memcpy(V, A);

  s += gsl_linalg_QR_UR_decomp(R, V, T);

  /*
   * compute B = Q R = [ R - T R ]
   *                   [ -V~ T R ]
   */

  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, B1, T);
  gsl_matrix_memcpy(B2, V);

  gsl_blas_dtrmm (CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, R, B1);   /* B1 = T R */
  gsl_blas_dtrmm (CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, -1.0, B1, B2); /* B2 = -V~ T R */
  gsl_matrix_sub (B1, R);                                                            /* B1 = T R - R */
  gsl_matrix_scale (B1, -1.0);                                                       /* B1 = R - T R */

  /* test S = B1 */
  for (i = 0; i < N; i++)
    {
      for (j = i; j < N; j++)
        {
          double aij = gsl_matrix_get(B1, i, j);
          double mij = gsl_matrix_get(S, i, j);

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

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

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

  gsl_matrix_free(R);
  gsl_matrix_free(V);
  gsl_matrix_free(T);
  gsl_matrix_free(B1);
  gsl_matrix_free(B2);

  return s;
}

static int
test_QR_UR_decomp(gsl_rng * r)
{
  int s = 0;
  size_t M, N;

  for (M = 1; M <= 50; ++M)
    {
      for (N = 1; N <= M; ++N)
        {
          gsl_matrix * S = gsl_matrix_alloc(N, N);
          gsl_matrix * A = gsl_matrix_alloc(M, N);

          create_random_matrix(A, r);
          create_random_matrix(S, r);
          s += test_QR_UR_decomp_eps(S, A, 1.0e6 * M * GSL_DBL_EPSILON, "QR_UR_decomp random");

          gsl_matrix_free(S);
          gsl_matrix_free(A);
        }
    }

  return s;
}

static int
test_QR_UZ_decomp_eps(const gsl_matrix * S, const gsl_matrix * A, const double eps, const char * desc)
{
  int s = 0;
  const size_t M = A->size1;
  const size_t N = A->size2;
  size_t i, j;

  gsl_matrix * R = gsl_matrix_calloc(N, N);
  gsl_matrix * V = gsl_matrix_alloc(M, N);
  gsl_matrix * T = gsl_matrix_alloc(N, N);
  gsl_matrix * B1  = gsl_matrix_calloc(N, N);
  gsl_matrix * B2  = gsl_matrix_alloc(M, N);

  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, R, S);
  gsl_matrix_memcpy(V, A);

  s += gsl_linalg_QR_UZ_decomp(R, V, T);

  /*
   * compute B = Q R = [ R - T R ]
   *                   [ -V~ T R ]
   */

  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, B1, T);

  gsl_blas_dtrmm (CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, R, B1);   /* B1 = T R */
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, -1.0, V, B1, 0.0, B2);                 /* B2 = -V~ T R */
  gsl_matrix_sub (B1, R);                                                            /* B1 = T R - R */
  gsl_matrix_scale (B1, -1.0);                                                       /* B1 = R - T R */

  /* test S = B1 */
  for (i = 0; i < N; i++)
    {
      for (j = i; j < N; j++)
        {
          double aij = gsl_matrix_get(B1, i, j);
          double mij = gsl_matrix_get(S, i, j);

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

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

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

  gsl_matrix_free(R);
  gsl_matrix_free(V);
  gsl_matrix_free(T);
  gsl_matrix_free(B1);
  gsl_matrix_free(B2);

  return s;
}

static int
test_QR_UZ_decomp(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 20;
  const size_t M_max = 2*N_max;
  gsl_matrix * U1 = gsl_matrix_alloc(N_max, N_max);
  gsl_matrix * U2 = gsl_matrix_alloc(N_max, N_max);
  gsl_matrix * A = gsl_matrix_alloc(M_max, N_max);
  size_t M, N;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix_view S = gsl_matrix_submatrix(U1, 0, 0, N, N);
      gsl_matrix_view T = gsl_matrix_submatrix(U2, 0, 0, N, N);

      for (M = N; M <= 2*N_max; ++M)
        {
          gsl_matrix_view B = gsl_matrix_submatrix(A, 0, 0, M, N);
          gsl_matrix_view Bu = gsl_matrix_submatrix(&B.matrix, M - N, 0, N, N);

          gsl_matrix_set_zero(&B.matrix);

          if (M > N)
            {
              gsl_matrix_view Bd = gsl_matrix_submatrix(&B.matrix, 0, 0, M - N, N);
              create_random_matrix(&Bd.matrix, r);
            }

          create_random_matrix(&S.matrix, r);
          create_random_matrix(&T.matrix, r);
          gsl_matrix_tricpy(CblasUpper, CblasNonUnit, &Bu.matrix, &T.matrix);

          s += test_QR_UZ_decomp_eps(&S.matrix, &B.matrix, 1.0e6 * M * GSL_DBL_EPSILON,
                                     "QR_UZ_decomp random");
        }
    }

  gsl_matrix_free(U1);
  gsl_matrix_free(U2);
  gsl_matrix_free(A);

  return s;
}

static int
test_QR_UU_decomp_eps(const gsl_matrix * U, const gsl_matrix * S, const double eps, const char * desc)
{
  int s = 0;
  const size_t N = U->size2;
  size_t i, j;

  gsl_matrix * R = gsl_matrix_calloc(N, N);
  gsl_matrix * V = gsl_matrix_calloc(N, N);
  gsl_matrix * T = gsl_matrix_alloc(N, N);
  gsl_matrix * B1  = gsl_matrix_calloc(N, N);
  gsl_matrix * B2  = gsl_matrix_calloc(N, N);

  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, R, U);
  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, V, S);

  s += gsl_linalg_QR_UU_decomp(R, V, T);

  /*
   * compute B = Q R = [ R - T R ]
   *                   [ -V~ T R ]
   */

  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, B1, T);

  gsl_blas_dtrmm (CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, R, B1);   /* B1 = T R */
  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, B2, B1);                               /* B2 := T R */
  gsl_blas_dtrmm (CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, -1.0, V, B2);   /* B2 = -V~ T R */
  gsl_matrix_sub (B1, R);                                                            /* B1 = T R - R */
  gsl_matrix_scale (B1, -1.0);                                                       /* B1 = R - T R */

  /* test U = B1 */
  for (i = 0; i < N; i++)
    {
      for (j = i; j < N; j++)
        {
          double aij = gsl_matrix_get(B1, i, j);
          double mij = gsl_matrix_get(U, i, j);

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

  /* test S = B2 */
  for (i = 0; i < N; i++)
    {
      for (j = 0; j < N; j++)
        {
          double aij = gsl_matrix_get(B2, i, j);
          double mij = gsl_matrix_get(S, i, j);

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

  gsl_matrix_free(R);
  gsl_matrix_free(V);
  gsl_matrix_free(T);
  gsl_matrix_free(B1);
  gsl_matrix_free(B2);

  return s;
}

static int
test_QR_UU_decomp(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 50;
  gsl_matrix * U = gsl_matrix_alloc(N_max, N_max);
  gsl_matrix * S = gsl_matrix_alloc(N_max, N_max);
  gsl_matrix * T = gsl_matrix_alloc(N_max, N_max);
  size_t N;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix_view a = gsl_matrix_submatrix(U, 0, 0, N, N);
      gsl_matrix_view b = gsl_matrix_submatrix(S, 0, 0, N, N);
      gsl_matrix_view c = gsl_matrix_submatrix(T, 0, 0, N, N);

      create_random_matrix(&c.matrix, r);
      gsl_matrix_set_zero(&a.matrix);
      gsl_matrix_tricpy(CblasUpper, CblasNonUnit, &a.matrix, &c.matrix);

      create_random_matrix(&c.matrix, r);
      gsl_matrix_set_zero(&b.matrix);
      gsl_matrix_tricpy(CblasUpper, CblasNonUnit, &b.matrix, &c.matrix);

      s += test_QR_UU_decomp_eps(&a.matrix, &b.matrix, 1.0e6 * N * GSL_DBL_EPSILON,
                                 "QR_UU_decomp random");
    }

  gsl_matrix_free(U);
  gsl_matrix_free(S);
  gsl_matrix_free(T);

  return s;
}

static int
test_QR_UU_lssolve_eps(const gsl_matrix * U, const gsl_matrix * S, const gsl_vector * b,
                       const double eps, const char * desc)
{
  int s = 0;
  const size_t N = U->size1;
  const size_t M = 2 * N;
  size_t i;

  gsl_matrix * R = gsl_matrix_calloc(N, N);
  gsl_matrix * Y = gsl_matrix_calloc(N, N);
  gsl_matrix * T = gsl_matrix_alloc(N, N);
  gsl_vector * x = gsl_vector_alloc(M);
  gsl_vector * work  = gsl_vector_alloc(N);

  gsl_matrix * A = gsl_matrix_calloc(M, N);
  gsl_matrix * U_svd = gsl_matrix_alloc(M, N);
  gsl_matrix * V_svd = gsl_matrix_alloc(N, N);
  gsl_vector * S_svd = gsl_vector_alloc(N);
  gsl_vector * x_svd = gsl_vector_alloc(N);

  gsl_vector * residual = gsl_vector_alloc(M);

  gsl_matrix_view tmp;

  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, R, U);
  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, Y, S);

  s += gsl_linalg_QR_UU_decomp(R, Y, T);
  s += gsl_linalg_QR_UU_lssolve(R, Y, T, b, x, work);

  tmp = gsl_matrix_submatrix(A, 0, 0, N, N);
  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, &tmp.matrix, U);

  tmp = gsl_matrix_submatrix(A, N, 0, N, N);
  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, &tmp.matrix, S);

  gsl_matrix_memcpy(U_svd, A);
  gsl_linalg_SV_decomp(U_svd, V_svd, S_svd, work);
  gsl_linalg_SV_solve(U_svd, V_svd, S_svd, b, x_svd);

  /* compare QR with SVD solution */
  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(x_svd, i);

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

  if (M > N)
    {
      gsl_vector_view x1 = gsl_vector_subvector(x, 0, N);
      gsl_vector_view x2 = gsl_vector_subvector(x, N, M - N);
      double norm = gsl_blas_dnrm2(&x2.vector);
      double norm_expected;

      /* compute residual and check || x(N+1:end) || = || b - A x || */
      gsl_vector_memcpy(residual, b);
      gsl_blas_dgemv(CblasNoTrans, -1.0, A, &x1.vector, 1.0, residual);
      norm_expected = gsl_blas_dnrm2(residual);

      gsl_test_rel(norm, norm_expected, eps, "%s rnorm (%3lu,%3lu): %22.18g   %22.18g\n",
                   desc, M, N, norm, norm_expected);
    }

  gsl_matrix_free(A);
  gsl_matrix_free(R);
  gsl_matrix_free(Y);
  gsl_matrix_free(T);
  gsl_vector_free(x);
  gsl_matrix_free(U_svd);
  gsl_matrix_free(V_svd);
  gsl_vector_free(x_svd);
  gsl_vector_free(work);
  gsl_vector_free(S_svd);
  gsl_vector_free(residual);

  return s;
}

static int
test_QR_UU_lssolve(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 30;
  gsl_matrix * U = gsl_matrix_alloc(N_max, N_max);
  gsl_matrix * S = gsl_matrix_alloc(N_max, N_max);
  gsl_matrix * T = gsl_matrix_alloc(N_max, N_max);
  gsl_vector * rhs = gsl_vector_alloc(2*N_max);
  size_t N;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix_view a = gsl_matrix_submatrix(U, 0, 0, N, N);
      gsl_matrix_view b = gsl_matrix_submatrix(S, 0, 0, N, N);
      gsl_matrix_view c = gsl_matrix_submatrix(T, 0, 0, N, N);
      gsl_vector_view rhsv = gsl_vector_subvector(rhs, 0, 2*N);

      create_random_vector(&rhsv.vector, r);

      create_random_matrix(&c.matrix, r);
      gsl_matrix_set_zero(&a.matrix);
      gsl_matrix_tricpy(CblasUpper, CblasNonUnit, &a.matrix, &c.matrix);

      create_random_matrix(&c.matrix, r);
      gsl_matrix_set_zero(&b.matrix);
      gsl_matrix_tricpy(CblasUpper, CblasNonUnit, &b.matrix, &c.matrix);

      s += test_QR_UU_lssolve_eps(&a.matrix, &b.matrix, &rhsv.vector, 1.0e4 * N * GSL_DBL_EPSILON,
                                  "QR_UU_lssolve random");
    }

  gsl_matrix_free(U);
  gsl_matrix_free(S);
  gsl_matrix_free(T);
  gsl_vector_free(rhs);

  return s;
}

static int
test_QR_UD_decomp_eps(const gsl_matrix * U, const gsl_vector * D, const double eps, const char * desc)
{
  int s = 0;
  const size_t N = U->size2;
  size_t i, j;

  gsl_matrix * R = gsl_matrix_calloc(N, N);
  gsl_matrix * V = gsl_matrix_calloc(N, N);
  gsl_matrix * T = gsl_matrix_alloc(N, N);
  gsl_matrix * B1  = gsl_matrix_calloc(N, N);
  gsl_matrix * B2  = gsl_matrix_calloc(N, N);

  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, R, U);

  s += gsl_linalg_QR_UD_decomp(R, D, V, T);

  /*
   * compute B = Q R = [ R - T R ]
   *                   [ -V~ T R ]
   */

  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, B1, T);

  gsl_blas_dtrmm (CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, R, B1);   /* B1 = T R */
  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, B2, B1);                               /* B2 := T R */
  gsl_blas_dtrmm (CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, -1.0, V, B2);   /* B2 = -V~ T R */
  gsl_matrix_sub (B1, R);                                                            /* B1 = T R - R */
  gsl_matrix_scale (B1, -1.0);                                                       /* B1 = R - T R */

  /* test U = B1 */
  for (i = 0; i < N; i++)
    {
      for (j = i; j < N; j++)
        {
          double aij = gsl_matrix_get(B1, i, j);
          double mij = gsl_matrix_get(U, i, j);

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

  /* test S = B2 */
  for (i = 0; i < N; i++)
    {
      for (j = 0; j < N; j++)
        {
          double aij = gsl_matrix_get(B2, i, j);
          double mij = (i == j) ? gsl_vector_get(D, i) : 0.0;

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

  gsl_matrix_free(R);
  gsl_matrix_free(V);
  gsl_matrix_free(T);
  gsl_matrix_free(B1);
  gsl_matrix_free(B2);

  return s;
}

static int
test_QR_UD_decomp(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 30;
  gsl_matrix * U = gsl_matrix_alloc(N_max, N_max);
  gsl_matrix * S = gsl_matrix_alloc(N_max, N_max);
  gsl_vector * D = gsl_vector_alloc(N_max);
  size_t N;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix_view a = gsl_matrix_submatrix(U, 0, 0, N, N);
      gsl_matrix_view b = gsl_matrix_submatrix(S, 0, 0, N, N);
      gsl_vector_view diag = gsl_vector_subvector(D, 0, N);

      create_random_matrix(&b.matrix, r);
      gsl_matrix_set_zero(&a.matrix);
      gsl_matrix_tricpy(CblasUpper, CblasNonUnit, &a.matrix, &b.matrix);

      create_random_vector(&diag.vector, r);

      s += test_QR_UD_decomp_eps(&a.matrix, &diag.vector, 1.0e4 * N * GSL_DBL_EPSILON,
                                 "QR_UD_decomp random");
    }

  gsl_matrix_free(U);
  gsl_matrix_free(S);
  gsl_vector_free(D);

  return s;
}

static int
test_QR_UD_lssolve_eps(const gsl_matrix * U, const gsl_vector * D, const gsl_vector * b,
                       const double eps, const char * desc)
{
  int s = 0;
  const size_t N = U->size1;
  const size_t M = 2 * N;
  size_t i;

  gsl_matrix * R = gsl_matrix_calloc(N, N);
  gsl_matrix * Y = gsl_matrix_calloc(N, N);
  gsl_matrix * T = gsl_matrix_alloc(N, N);
  gsl_vector * x = gsl_vector_alloc(M);
  gsl_vector * work  = gsl_vector_alloc(N);

  gsl_matrix * A = gsl_matrix_calloc(M, N);
  gsl_matrix * U_svd = gsl_matrix_alloc(M, N);
  gsl_matrix * V_svd = gsl_matrix_alloc(N, N);
  gsl_vector * S_svd = gsl_vector_alloc(N);
  gsl_vector * x_svd = gsl_vector_alloc(N);

  gsl_vector * residual = gsl_vector_alloc(M);

  gsl_matrix_view tmp;
  gsl_vector_view diag;

  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, R, U);

  s += gsl_linalg_QR_UD_decomp(R, D, Y, T);
  s += gsl_linalg_QR_UD_lssolve(R, Y, T, b, x, work);

  tmp = gsl_matrix_submatrix(A, 0, 0, N, N);
  gsl_matrix_tricpy(CblasUpper, CblasNonUnit, &tmp.matrix, U);

  tmp = gsl_matrix_submatrix(A, N, 0, N, N);
  diag = gsl_matrix_diagonal(&tmp.matrix);
  gsl_vector_memcpy(&diag.vector, D);

  gsl_matrix_memcpy(U_svd, A);
  gsl_linalg_SV_decomp(U_svd, V_svd, S_svd, work);
  gsl_linalg_SV_solve(U_svd, V_svd, S_svd, b, x_svd);

  /* compare QR with SVD solution */
  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(x_svd, i);

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

  if (M > N)
    {
      gsl_vector_view x1 = gsl_vector_subvector(x, 0, N);
      gsl_vector_view x2 = gsl_vector_subvector(x, N, M - N);
      double norm = gsl_blas_dnrm2(&x2.vector);
      double norm_expected;

      /* compute residual and check || x(N+1:end) || = || b - A x || */
      gsl_vector_memcpy(residual, b);
      gsl_blas_dgemv(CblasNoTrans, -1.0, A, &x1.vector, 1.0, residual);
      norm_expected = gsl_blas_dnrm2(residual);

      gsl_test_rel(norm, norm_expected, eps, "%s rnorm (%3lu,%3lu): %22.18g   %22.18g\n",
                   desc, M, N, norm, norm_expected);
    }

  gsl_matrix_free(A);
  gsl_matrix_free(R);
  gsl_matrix_free(Y);
  gsl_matrix_free(T);
  gsl_vector_free(x);
  gsl_matrix_free(U_svd);
  gsl_matrix_free(V_svd);
  gsl_vector_free(x_svd);
  gsl_vector_free(work);
  gsl_vector_free(S_svd);
  gsl_vector_free(residual);

  return s;
}

static int
test_QR_UD_lssolve(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 30;
  gsl_matrix * U = gsl_matrix_alloc(N_max, N_max);
  gsl_matrix * S = gsl_matrix_alloc(N_max, N_max);
  gsl_vector * D = gsl_vector_alloc(N_max);
  gsl_vector * rhs = gsl_vector_alloc(2*N_max);
  size_t N;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix_view a = gsl_matrix_submatrix(U, 0, 0, N, N);
      gsl_matrix_view b = gsl_matrix_submatrix(S, 0, 0, N, N);
      gsl_vector_view rhsv = gsl_vector_subvector(rhs, 0, 2*N);
      gsl_vector_view diag = gsl_vector_subvector(D, 0, N);

      create_random_vector(&rhsv.vector, r);
      create_random_vector(&diag.vector, r);

      create_random_matrix(&b.matrix, r);
      gsl_matrix_set_zero(&a.matrix);
      gsl_matrix_tricpy(CblasUpper, CblasNonUnit, &a.matrix, &b.matrix);

      s += test_QR_UD_lssolve_eps(&a.matrix, &diag.vector, &rhsv.vector, 1.0e4 * N * GSL_DBL_EPSILON,
                                  "QR_UD_lssolve random");
    }

  gsl_matrix_free(U);
  gsl_matrix_free(S);
  gsl_vector_free(D);
  gsl_vector_free(rhs);

  return s;
}


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

double f1(double x, void *p) {
    (void) (p); /* avoid unused parameter warning */
    return exp(pow(x, 2));
}

double f2(double x, void *p) {
    (void) (p); /* avoid unused parameter warning */
    return fabs(x + pow(x, 3));
}

double f3(double x, void *p) {
    (void) (p); /* avoid unused parameter warning */
    return x <= 0 ? -1 : 1;
}

double (*f[])(double, void *) = {f1, f2, f3};

char *a[] = {"pierwsze1.dat", "pierwsze2.dat", "pierwsze3.dat"};

int main(void) {
    int i, n = 10000;
    gsl_cheb_series *cs = gsl_cheb_alloc(40);
    gsl_function F;
    FILE *file;
    for (int j = 0; j < 3; j++) {
        file = fopen(a[j], "w");
        F.function = f[j];
        F.params = 0;
        gsl_cheb_init(cs, &F, -1.0, 1.0);
        for (i = -n; i < n; i++) {
            double x = i / (double) n;
            double r10 = gsl_cheb_eval_n(cs, 10, x);
            double r40 = gsl_cheb_eval(cs, x);
            fprintf(file, "%g %g %g %g\n",
                    x, GSL_FN_EVAL (&F, x), r10, r40);
        }
        fclose(file);
    }
    gsl_cheb_free(cs);
    return EXIT_SUCCESS;
}


/* specfunc/bessel_k.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_pow_int.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_bessel.h>

#include "error.h"
#include "check.h"

#include "bessel.h"

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

/* [Abramowitz+Stegun, 10.2.4 + 10.2.6]
 * with lmax=15, precision ~ 15D for x < 3
 *
 * assumes l >= 1
 */
static int bessel_kl_scaled_small_x(int l, const double x, gsl_sf_result * result)
{
  gsl_sf_result num_fact;
  double den  = gsl_sf_pow_int(x, l+1);
  int stat_df = gsl_sf_doublefact_e((unsigned int) (2*l-1), &num_fact);

  if(stat_df != GSL_SUCCESS || den == 0.0) {
    OVERFLOW_ERROR(result);
  }
  else {
    const int lmax = 50;
    gsl_sf_result ipos_term;
    double ineg_term;
    double sgn = (GSL_IS_ODD(l) ? -1.0 : 1.0);
    double ex  = exp(x);
    double t = 0.5*x*x;
    double sum = 1.0;
    double t_coeff = 1.0;
    double t_power = 1.0;
    double delta;
    int stat_il;
    int i;

    for(i=1; i<lmax; i++) {
      t_coeff /= i*(2*(i-l) - 1);
      t_power *= t;
      delta = t_power*t_coeff;
      sum += delta;
      if(fabs(delta/sum) < GSL_DBL_EPSILON) break;
    }

    stat_il = gsl_sf_bessel_il_scaled_e(l, x, &ipos_term);
    ineg_term =  sgn * num_fact.val/den * sum;
    result->val = -sgn * 0.5*M_PI * (ex*ipos_term.val - ineg_term);
    result->val *= ex;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat_il;
  }
}

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int gsl_sf_bessel_k0_scaled_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else {
    result->val = M_PI/(2.0*x);
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    CHECK_UNDERFLOW(result);
    return GSL_SUCCESS;
  }
}

int gsl_sf_bessel_k1_scaled_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < (M_SQRTPI+1.0)/(M_SQRT2*GSL_SQRT_DBL_MAX)) {
    OVERFLOW_ERROR(result);
  }
  else {
    result->val = M_PI/(2.0*x) * (1.0 + 1.0/x);
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    CHECK_UNDERFLOW(result);
    return GSL_SUCCESS;
  }
}

int gsl_sf_bessel_k2_scaled_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 2.0/GSL_ROOT3_DBL_MAX) {
    OVERFLOW_ERROR(result);
  }
  else {
    result->val = M_PI/(2.0*x) * (1.0 + 3.0/x * (1.0 + 1.0/x));
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    CHECK_UNDERFLOW(result);
    return GSL_SUCCESS;
  }
}

int gsl_sf_bessel_kl_scaled_e(int l, const double x, gsl_sf_result * result)
{
  if(l < 0 || x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(l == 0) {
    return gsl_sf_bessel_k0_scaled_e(x, result);
  }
  else if(l == 1) {
    return gsl_sf_bessel_k1_scaled_e(x, result);
  }
  else if(l == 2) {
    return gsl_sf_bessel_k2_scaled_e(x, result);
  }
  else if(x < 3.0) {
    return bessel_kl_scaled_small_x(l, x, result);
  }
  else if(GSL_ROOT3_DBL_EPSILON * x > (l*l + l + 1)) {
    int status = gsl_sf_bessel_Knu_scaled_asympx_e(l + 0.5, x, result);
    double pre = sqrt((0.5*M_PI)/x);
    result->val *= pre;
    result->err *= pre;
    return status;
  }
  else if(GSL_MIN(0.29/(l*l+1.0), 0.5/(l*l+1.0+x*x)) < GSL_ROOT3_DBL_EPSILON) {
    int status = gsl_sf_bessel_Knu_scaled_asymp_unif_e(l + 0.5, x, result);
    double pre = sqrt((0.5*M_PI)/x);
    result->val *= pre;
    result->err *= pre;
    return status;
  }
  else {
    /* recurse upward */
    gsl_sf_result r_bk;
    gsl_sf_result r_bkm;
    int stat_1 = gsl_sf_bessel_k1_scaled_e(x, &r_bk);
    int stat_0 = gsl_sf_bessel_k0_scaled_e(x, &r_bkm);
    double bkp;
    double bk  = r_bk.val;
    double bkm = r_bkm.val;
    int j;
    for(j=1; j<l; j++) {
      bkp = (2*j+1)/x*bk + bkm;
      bkm = bk;
      bk  = bkp;
    }
    result->val  = bk;
    result->err  = fabs(bk) * (fabs(r_bk.err/r_bk.val) + fabs(r_bkm.err/r_bkm.val));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);

    return GSL_ERROR_SELECT_2(stat_1, stat_0);
  }
}

int
gsl_sf_bessel_kl_scaled_array(const int lmax, const double x, double * result_array)
{
  if(lmax < 0 || x <= 0.0) {
    GSL_ERROR("domain error", GSL_EDOM);
  } else if (lmax == 0) {
    gsl_sf_result result;
    int stat = gsl_sf_bessel_k0_scaled_e(x, &result);
    result_array[0] = result.val;
    return stat;
  } else {
    int ell;
    double kellp1, kell, kellm1;
    gsl_sf_result r_kell;
    gsl_sf_result r_kellm1;
    gsl_sf_bessel_k1_scaled_e(x, &r_kell);
    gsl_sf_bessel_k0_scaled_e(x, &r_kellm1);
    kell   = r_kell.val;
    kellm1 = r_kellm1.val;
    result_array[0] = kellm1;
    result_array[1] = kell;
    for(ell = 1; ell < lmax; ell++) {
      kellp1 = (2*ell+1)/x * kell + kellm1;
      result_array[ell+1] = kellp1;
      kellm1 = kell;
      kell   = kellp1;
    }
    return GSL_SUCCESS;
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_bessel_k0_scaled(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_k0_scaled_e(x, &result));
}

double gsl_sf_bessel_k1_scaled(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_k1_scaled_e(x, &result));
}

double gsl_sf_bessel_k2_scaled(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_k2_scaled_e(x, &result));
}

double gsl_sf_bessel_kl_scaled(const int l, const double x)
{
  EVAL_RESULT(gsl_sf_bessel_kl_scaled_e(l, x, &result));
}


#include "GeneralFunctions.h"
#include <math.h>
#include <stdio.h>
#include <iostream>
#include <time.h>
//
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_log.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include "gsl/gsl_cdf.h"
#include "gsl/gsl_randist.h"
#include "gsl/gsl_rng.h"

int AcceleratedGibbs (int maxK,int bias, int N, int D, int K, char *C,  int *R, double alpha, double s2B, double s2Y, gsl_matrix **Y, gsl_matrix *Z, int *nest, gsl_matrix *P, gsl_matrix *Pnon, gsl_matrix **lambda, gsl_matrix **lambdanon);
void SampleY (double missing, int N, int D, int K, char Cd,  int Rd, double fd, double mud, double wd, double s2Y,double s2u, double s2theta, gsl_matrix *X, gsl_matrix *Z, gsl_matrix *Yd,  gsl_matrix *Bd, gsl_vector *thetad, const gsl_rng *seed);
double Samples2Y (double missing, int N, int d, int K, char Cd,  int Rd, double fd, double mud, double wd, double s2u, double s2theta, gsl_matrix *X, gsl_matrix *Z, gsl_matrix *Yd,  gsl_matrix *Bd, gsl_vector *thetad, const gsl_rng *seed);
int IBPsampler_func (double missing, gsl_matrix *X, char *C, gsl_matrix *Z, gsl_matrix **B, gsl_vector **theta, int *R, double *f, double *mu,  double *w, int maxR, int bias, int N, int D, int K, double alpha, double s2B, double *s2Y,double s2u, int maxK,int Nsim);
int initialize_func (int N, int D, int maxK, double missing, gsl_matrix *X, char *C, gsl_matrix **B, gsl_vector **theta, int *R, double *f, double *mu,  double *w, double *s2Y);


#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>
#include "growthfactor.h"

//Code to calculate the linear growth factor D, and linear growth rate, f. Where a linear perturbation delta grows as
//delta(a') = delta(a)*(D(a')/D(a))^2 and f = dlnD/dlna
//Note anyone using Komatsu's CRL library should note: growth_factor_crl = D *(1+z) and growth_rate_crl = f/(1+z)

double D;
double linear_f;
double w0,wa;
double omega_m,omega_lambda;

#define LIMIT_SIZE 1000

int get_growthfactor(double a,double om, double w, double w2, double *gf)
{
	w0 = w;
	wa = w2;
	omega_m = om;
	omega_lambda = 1.0 - omega_m;
        growth_de(a,gf);
	return 0;
}

// 2 parameter dark energy equation of state

double w (double a)
{
return w0 + (1.0-a)*wa;
}

double w_int(double z, void *param)
{
	return (1. + w(1./(z+1)) )/( 1. + z);
}

double Xde_int (double a,void *params )
{
    if (a == 0.) a = 1e-3;
    double Xde_int = w(a)/a;
    return Xde_int;
}

double Xde (double a)
{
    gsl_integration_workspace * worksp  = gsl_integration_workspace_alloc (LIMIT_SIZE);
    gsl_function F;
    F.function = &Xde_int;
    F.params =0;
    double integral,error;
    if (a == 0.)
		a = 1e-3;
    gsl_integration_qags (&F, a, 1,  1.0e-20, 1.0e-10, LIMIT_SIZE,worksp, &integral, &error);
    gsl_integration_workspace_free (worksp);

    return omega_m/(1.-omega_m)*exp(-3.*integral);
}

int func (double t, const double y[], double f[], void *params)
{
	double mu = *(double *)params;
	f[0] = y[1];
	f[1] = -( 3.5 - 1.5 * w(t)/( 1 + Xde(t) ) )*y[1]/t - 1.5 * ( 1 - w(t) )/( 1 + Xde(t))*(y[0]/t/t);
	return GSL_SUCCESS;
}

int jac (double t, const double y[], double *dfdy, double dfdt[], void *params)
{
	double mu = *(double *)params;
	gsl_matrix_view dfdy_mat = gsl_matrix_view_array (dfdy, 2, 2);
	gsl_matrix * m = &dfdy_mat.matrix;
	gsl_matrix_set (m, 0, 0, 0.0); //dy1/dx1
	gsl_matrix_set (m, 0, 1, 1.0);
	gsl_matrix_set (m, 1, 0, - 1.5 * ( 1 - w(t) )/( 1 + Xde(t))*(1./t/t));
	gsl_matrix_set (m, 1, 1, -( 3.5 - 1.5 * w(t)/( 1 + Xde(t) ) )/t);
	dfdt[0] = 0.0;
	dfdt[1] = 0.0;
	return GSL_SUCCESS;
}

double growth_de (double a, double *gf)
{
	const gsl_odeiv_step_type * T
		= gsl_odeiv_step_rk4;

	gsl_odeiv_step * s
         = gsl_odeiv_step_alloc (T, 2);
	gsl_odeiv_control * c
         = gsl_odeiv_control_y_new (1e-6, 0.0);
	gsl_odeiv_evolve * e
         = gsl_odeiv_evolve_alloc (2);

	double mu = 10;
	gsl_odeiv_system sys = {func, jac, 2, &mu};

	double t =1.e-3, t1 = a;
	double h = 1e-6;
	double y[2] = {1.,0.};

	while (t < t1)
	{
		int status = gsl_odeiv_evolve_apply (e, c, s, &sys, &t, t1, &h, y);
		if (status != GSL_SUCCESS)
			break;
	}
	gsl_odeiv_evolve_free (e);
	gsl_odeiv_control_free (c);
	gsl_odeiv_step_free (s);

//	return y[0]*a;  //D growth factor
//	return y[1]*a*a/(y[0]*a) +1.; // f = d ln D/ d ln a
	gf[0] = y[0]*a;
	gf[1] = y[1]*a*a/(y[0]*a) +1.;
	return y[0]*a;

}


#pragma once

#include <bitset>
#include <type_traits>

#include <cuda/define_specifiers.hpp>

#include <gsl-lite/gsl-lite.hpp>

namespace thrustshift {

namespace detail {

// to be compiler independent std::bitset is used here
template <typename I>
int count_leading_zeros_cpu(I i) {

	std::bitset<sizeof(I) * 8> bs(i);
	static_assert(bs.size() > 0);
	int lz = 0;
	for (int bi = gsl_lite::narrow<int>(sizeof(I) * 8) - 1; bi >= 0; --bi) {
		if (bs[bi]) {
			break;
		}
		++lz;
	}
	return lz;
}

template <typename I>
int count_leading_zeros_gpu(I i);

template<> CUDA_FD int count_leading_zeros_gpu<int>(int i) {
	return __clz(i);
}

template<> CUDA_FD int count_leading_zeros_gpu<long long int>(long long int i) {
	return __clzll(i);
}

} // namespace detail

template <typename I>
CUDA_FHD int count_leading_zeros(I i) {

#ifdef __CUDA_ARCH__
	return detail::count_leading_zeros_gpu(i);
#else
	return detail::count_leading_zeros_cpu(i);
#endif

}

} // namespace thrustshift


/* eigen/jacobi.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  G. Jungman
 */
/* Simple linear algebra operations, operating directly
 * on the gsl_vector and gsl_matrix objects. These are
 * meant for "generic" and "small" systems. Anyone
 * interested in large systems will want to use more
 * sophisticated methods, presumably involving native
 * BLAS operations, specialized data representations,
 * or other optimizations.
 */
#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include "gsl_eigen.h"

#define REAL double

inline static void
jac_rotate(gsl_matrix * a,
           unsigned int i, unsigned int j, unsigned int k, unsigned int l,
           double * g, double * h,
           double s, double tau)
{
  *g = gsl_matrix_get(a, i, j);
  *h = gsl_matrix_get(a, k, l);
  gsl_matrix_set(a, i, j, (*g) - s*((*h) + (*g)*tau));
  gsl_matrix_set(a, k, l, (*h) + s*((*g) - (*h)*tau));
}

int
gsl_eigen_jacobi(gsl_matrix * a,
                         gsl_vector * eval,
                         gsl_matrix * evec,
                         unsigned int max_rot,
                         unsigned int * nrot)
{
  if(a->size1 != a->size2) {
     GSL_ERROR ("eigenproblem requires square matrix", GSL_ENOTSQR);
  }
  else if(a->size1 != evec->size1 || a->size1 != evec->size2) {
     GSL_ERROR ("eigenvector matrix must match input matrix", GSL_EBADLEN);
  }
  else if(a->size1 != eval->size) {
    GSL_ERROR ("eigenvalue vector must match input matrix", GSL_EBADLEN);
  }
  else {
    const unsigned int n = a->size1;
    unsigned int i, j, iq, ip;
    double t, s;

    REAL * b = (REAL *) malloc(n * sizeof(REAL));
    REAL * z = (REAL *) malloc(n * sizeof(REAL));
    if(b == 0 || z == 0) {
      if(b != 0) free(b);
      if(z != 0) free(z);
      GSL_ERROR ("could not allocate memory for workspace", GSL_ENOMEM);
    }

    /* Set eigenvectors to coordinate basis. */
    for(ip=0; ip<n; ip++) {
      for(iq=0; iq<n; iq++) {
        gsl_matrix_set(evec, ip, iq, 0.0);
      }
      gsl_matrix_set(evec, ip, ip, 1.0);
    }

    /* Initialize eigenvalues and workspace. */
    for(ip=0; ip<n; ip++) {
      REAL a_ipip = gsl_matrix_get(a, ip, ip);
      z[ip] = 0.0;
      b[ip] = a_ipip;
      gsl_vector_set(eval, ip, a_ipip);
    }

    *nrot = 0;

    for(i=1; i<=max_rot; i++) {
      REAL thresh;
      REAL tau;
      REAL g, h, c;
      REAL sm = 0.0;
      for(ip=0; ip<n-1; ip++) {
        for(iq=ip+1; iq<n; iq++) {
          sm += fabs(gsl_matrix_get(a, ip, iq));
        }
      }
      if(sm == 0.0) {
        free(z);
        free(b);
        return GSL_SUCCESS;
      }

      if(i < 4)
        thresh = 0.2*sm/(n*n);
      else
        thresh = 0.0;

      for(ip=0; ip<n-1; ip++) {
        for(iq=ip+1; iq<n; iq++) {
          const REAL d_ip = gsl_vector_get(eval, ip);
          const REAL d_iq = gsl_vector_get(eval, iq);
	  const REAL a_ipiq = gsl_matrix_get(a, ip, iq);
          g = 100.0 * fabs(a_ipiq);
          if(   i > 4
             && fabs(d_ip)+g == fabs(d_ip)
             && fabs(d_iq)+g == fabs(d_iq)
	     ) {
            gsl_matrix_set(a, ip, iq, 0.0);
          }
          else if(fabs(a_ipiq) > thresh) {
            h = d_iq - d_ip;
            if(fabs(h) + g == fabs(h)) {
              t = a_ipiq/h;
            }
            else {
              REAL theta = 0.5*h/a_ipiq;
              t = 1.0/(fabs(theta) + sqrt(1.0 + theta*theta));
              if(theta < 0.0) t = -t;
            }

            c   = 1.0/sqrt(1.0+t*t);
            s   = t*c;
            tau = s/(1.0+c);
            h   = t * a_ipiq;
            z[ip] -= h;
            z[iq] += h;
	    gsl_vector_set(eval, ip, d_ip - h);
	    gsl_vector_set(eval, iq, d_iq + h);
	    gsl_matrix_set(a, ip, iq, 0.0);

            for(j=0; j<ip; j++){
              jac_rotate(a, j, ip, j, iq, &g, &h, s, tau);
            }
            for(j=ip+1; j<iq; j++){
              jac_rotate(a, ip, j, j, iq, &g, &h, s, tau);
            }
            for(j=iq+1; j<n; j++){
              jac_rotate(a, ip, j, iq, j, &g, &h, s, tau);
            }
            for (j=0; j<n; j++){
              jac_rotate(evec, j, ip, j, iq, &g, &h, s, tau);
            }
            ++(*nrot);
          }
        }
      }
      for (ip=0; ip<n; ip++) {
        b[ip] += z[ip];
        z[ip]  = 0.0;
	gsl_vector_set(eval, ip, b[ip]);
      }

      /* continue iteration */
    }

    return GSL_EMAXITER;
  }
}

int
gsl_eigen_invert_jacobi(const gsl_matrix * a,
                             gsl_matrix * ainv,
                             unsigned int max_rot)
{
  if(a->size1 != a->size2 || ainv->size1 != ainv->size2) {
    return GSL_ENOTSQR;
  }
  else if(a->size1 != ainv->size2) {
    return GSL_EBADLEN;
  }
  else {
    const unsigned int n = a->size1;
    unsigned int nrot;
    unsigned int i,j,k,l;

    /* This is annoying because I do not want
     * the error handling in these functions.
     * But there are no "impl"-like versions
     * of these allocators... sigh.
     */
    gsl_vector * eval = gsl_vector_alloc(n);
    gsl_matrix * evec = gsl_matrix_alloc(n, n);
    gsl_matrix * inv_diag = gsl_matrix_alloc(n, n);

    if(eval == 0 || evec == 0 || inv_diag == 0) {
      if(eval != 0) gsl_vector_free(eval);
      if(evec != 0) gsl_matrix_free(evec);
      if(inv_diag != 0) gsl_matrix_free(inv_diag);
      return GSL_ENOMEM;
    }

    memcpy(ainv->data, a->data, n*n*sizeof(REAL));

    gsl_eigen_jacobi(ainv, eval, evec, max_rot, &nrot);

    for(i=0; i<n; i++) {
      if(fabs(gsl_vector_get(eval, i)) < 100.0 * GSL_DBL_EPSILON) {
        /* apparent singularity */
        gsl_vector_free(eval);
        gsl_matrix_free(evec);
        gsl_matrix_free(inv_diag);
        return GSL_ESING;
      }
    }

    /* Invert the diagonalized matrix. */
    for(i=0; i<n; i++) {
      for(j=0; j<n; j++) {
        gsl_matrix_set(inv_diag, i, j, 0.0);
      }
      gsl_matrix_set(inv_diag, i, i, 1.0/gsl_vector_get(eval, i));
    }

    for(i=0; i<n; i++) {
      for(j=0; j<n; j++) {
        gsl_matrix_set(ainv, i, j, 0.0);
        for(k=0; k<n; k++) {
          for(l=0; l<n; l++) {
	    REAL ainv_ij = gsl_matrix_get(ainv, i, j);
	    REAL evec_il = gsl_matrix_get(evec, i, l);
	    REAL evec_jk = gsl_matrix_get(evec, j, k);
	    REAL inv_diag_lk = gsl_matrix_get(inv_diag, l, k);
	    REAL delta = evec_il * inv_diag_lk * evec_jk;
	    gsl_matrix_set(ainv, i, j, ainv_ij + delta);
	  }
	}
      }
    }

    gsl_vector_free(eval);
    gsl_matrix_free(evec);
    gsl_matrix_free(inv_diag);
    return GSL_SUCCESS;
  }
}


/*
 * Copyright 2020 Makani Technologies LLC
 *
 * 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.
 */

#ifndef SIM_MATH_INTERP_H_
#define SIM_MATH_INTERP_H_

#include <gsl/gsl_interp.h>
#include <math.h>
#include <vector>

#include "common/c_math/vec3.h"
#include "common/macros.h"

namespace sim {

// A TimeseriesInterpolator performs linear interpolation at timepoints within
// the bounds of t, and 0th order extrapolation past the endpoints.
class TimeseriesInterpolator {
 public:
  explicit TimeseriesInterpolator(const std::vector<double> &t);
  ~TimeseriesInterpolator();

  double Interp(double t, const std::vector<double> &data);
  Vec3 InterpVec3(double t, const std::vector<Vec3> &data);

  double WrapInterp(double t, const std::vector<double> &data, double lower,
                    double upper);
  Vec3 WrapInterpVec3(double t, const std::vector<Vec3> &data, double lower,
                      double upper);

 private:
  double WrapInterpInternal(const std::vector<double> &values,
                            const std::vector<double> &weights, double lower,
                            double upper);

  struct InterpParams {
    InterpParams(const std::vector<int32_t> &indices__,
                 const std::vector<double> &weights__)
        : indices(indices__), weights(weights__) {}
    ~InterpParams() {}

    std::vector<int32_t> indices;
    std::vector<double> weights;
  };

  const std::vector<double> &t_;
  gsl_interp_accel *interp_accel_;

  InterpParams GetInterpParams(double t);

  DISALLOW_COPY_AND_ASSIGN(TimeseriesInterpolator);
};

}  // namespace sim

#endif  // SIM_MATH_INTERP_H_


/* rstat/rstat.c
 *
 * Copyright (C) 2015 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 <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_rstat.h>

gsl_rstat_workspace *
gsl_rstat_alloc(void)
{
  gsl_rstat_workspace *w;

  w = calloc(1, sizeof(gsl_rstat_workspace));

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

  w->median_workspace_p = gsl_rstat_quantile_alloc(0.5);

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

  gsl_rstat_reset(w);

  return w;
} /* gsl_rstat_alloc() */

void
gsl_rstat_free(gsl_rstat_workspace *w)
{
  if (w->median_workspace_p)
    gsl_rstat_quantile_free(w->median_workspace_p);

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

size_t
gsl_rstat_n(const gsl_rstat_workspace *w)
{
  return w->n;
} /* gsl_rstat_n() */

/* add a data point to the running totals */
int
gsl_rstat_add(const double x, gsl_rstat_workspace *w)
{
  double delta = x - w->mean;
  double delta_n, delta_nsq, term1, n;

  /* update min and max */
  if (w->n == 0)
    {
      w->min = x;
      w->max = x;
    }
  else
    {
      if (x < w->min)
        w->min = x;
      if (x > w->max)
        w->max = x;
    }

  /* update mean and variance */
  n = (double) ++(w->n);
  delta_n = delta / n;
  delta_nsq = delta_n * delta_n;
  term1 = delta * delta_n * (n - 1.0);
  w->mean += delta_n;
  w->M4 += term1 * delta_nsq * (n * n - 3.0 * n + 3.0) +
           6.0 * delta_nsq * w->M2 - 4.0 * delta_n * w->M3;
  w->M3 += term1 * delta_n * (n - 2.0) - 3.0 * delta_n * w->M2;
  w->M2 += term1;

  /* update median */
  gsl_rstat_quantile_add(x, w->median_workspace_p);

  return GSL_SUCCESS;
} /* gsl_rstat_add() */

double
gsl_rstat_min(const gsl_rstat_workspace *w)
{
  return w->min;
} /* gsl_rstat_min() */

double
gsl_rstat_max(const gsl_rstat_workspace *w)
{
  return w->max;
} /* gsl_rstat_max() */

double
gsl_rstat_mean(const gsl_rstat_workspace *w)
{
  return w->mean;
} /* gsl_rstat_mean() */

double
gsl_rstat_variance(const gsl_rstat_workspace *w)
{
  if (w->n > 1)
    {
      double n = (double) w->n;
      return (w->M2 / (n - 1.0));
    }
  else
    return 0.0;
} /* gsl_rstat_variance() */

double
gsl_rstat_sd(const gsl_rstat_workspace *w)
{
  double var = gsl_rstat_variance(w);

  return (sqrt(var));
} /* gsl_rstat_sd() */

double
gsl_rstat_rms(const gsl_rstat_workspace *w)
{
  double rms = 0.0;

  if (w->n > 0)
    {
      double mean = gsl_rstat_mean(w);
      double sigma = gsl_rstat_sd(w);
      double n = (double) w->n;
      double a = sqrt((n - 1.0) / n);
      rms = gsl_hypot(mean, a * sigma);
    }

  return rms;
}

/* standard deviation of the mean: sigma / sqrt(n) */
double
gsl_rstat_sd_mean(const gsl_rstat_workspace *w)
{
  if (w->n > 0)
    {
      double sd = gsl_rstat_sd(w);
      return (sd / sqrt((double) w->n));
    }
  else
    return 0.0;
} /* gsl_rstat_sd_mean() */

double
gsl_rstat_median(gsl_rstat_workspace *w)
{
  return gsl_rstat_quantile_get(w->median_workspace_p);
}

double
gsl_rstat_skew(const gsl_rstat_workspace *w)
{
  if (w->n > 0)
    {
      double n = (double) w->n;
      double fac = pow(n - 1.0, 1.5) / n;
      return ((fac * w->M3) / pow(w->M2, 1.5));
    }
  else
    return 0.0;
} /* gsl_rstat_skew() */

double
gsl_rstat_kurtosis(const gsl_rstat_workspace *w)
{
  if (w->n > 0)
    {
      double n = (double) w->n;
      double fac = ((n - 1.0) / n) * (n - 1.0);
      return ((fac * w->M4) / (w->M2 * w->M2) - 3.0);
    }
  else
    return 0.0;
} /* gsl_rstat_kurtosis() */

int
gsl_rstat_reset(gsl_rstat_workspace *w)
{
  int status;

  w->min = 0.0;
  w->max = 0.0;
  w->mean = 0.0;
  w->M2 = 0.0;
  w->M3 = 0.0;
  w->M4 = 0.0;
  w->n = 0;

  status = gsl_rstat_quantile_reset(w->median_workspace_p);

  return status;
} /* gsl_rstat_reset() */


/*
 * Copyright (c) 2008-2011 Zhang Ming (M. Zhang), zmjerry@163.com
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 2 or any later version.
 *
 * 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 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. A copy of the GNU General Public License is available at:
 * http://www.fsf.org/licensing/licenses
 */

/*****************************************************************************
 *                                  dgt_usefftw.h
 *
 * Discrete Gabor Transform by Using FFTW.
 *
 * These routines are designed for calculating discrete Gabor transform and
 * its inversion of 1D signals. In order to eliminate the border effect, the
 * input signal("signal") is extended by three forms: zeros padded("zpd"),
 * periodized extension("ppd") and symetric extension("sym").
 *
 * The analysis/synthesis function is given by users, and it's daul
 * (synthesis/analysis) function can be computed by "daul" routine. The over
 * sampling rate is equal to N/dM, where N denotes frequency sampling numbers
 * and dM denotes the time sampling interval.
 *
 * N and dM should can be devided evenly by the window length "Lw". The
 * recovered signal just has the elements from 1 to dM*floor(Ls/dM) of the
 * original signal. So you'd better let dM can be deviede evenly by  the
 * original signal length "Ls".
 *
 * Zhang Ming, 2010-03, Xi'an Jiaotong University.
 *****************************************************************************/

#ifndef DGT_USEFFTW_H
#define DGT_USEFFTW_H

#include <string>
#include <complex>
#include <fftw.h>
#include <matrix.h>
#include <linequs1.h>
//#include <linequs3.h>
#include <utilities.h>

namespace splab
{

    template<typename Type> Vector<Type>
    daulFFTW( const Vector<Type>&, int, int );

    template<typename Type>
    Matrix< complex<Type> > dgtFFTW( const Vector<Type>&,
                                     const Vector<Type>&,
                                     int, int,
                                     const string &mode = "zpd" );
    template<typename Type>
    Vector<Type> idgtFFTW( const Matrix< complex<Type> >&,
                           const Vector<Type>&,
                           int, int );

    #include <dgt_usefftw-impl.h>

}
// namespace splab

#endif
// DGT_USEFFTW_H


#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_linalg.h>
#include "userFunc.h"

#define G        6.67384e-8    /* 2010 CODATA value in CGS units */
#define QCRIT    1.0           /* Q value at which GI turns on */
#define QDAMP    10.0          /* Rate of damping of alpha above Qcrit */
#define ALPHAMIN 1.0e-6        /* Minimum allowed alpha value */

/**************************************************************************/
/* This defines userFunc routines for the Krumholz & Burkert (2010) GI    */
/* disk problem.                                                          */
/**************************************************************************/

void
userEOS(const double t, const double dt, const grid *grd,
	const double *col, const double *pres, const double *eInt,
	void *params,
	double *gamma, double *delta) {
  fprintf(stderr,
	  "Warning: userEOS function called but not implemented!\n");
  return;
}

void
userAlpha(const double t, const double dt, const grid *grd,
	  const double *col, const double *pres, const double *eInt,
	  const double *gamma, const double *delta,
	  void *params,
	  double *alpha) {

  static int nr = 0;
  static double rmin = -1.0, rmax = -1.0;
  static double *x = NULL;
  static double *h1 = NULL;
  static double *h0 = NULL;
  static double *H = NULL;
  static double *Q = NULL;
  static gsl_vector *ud_g = NULL;
  static gsl_vector *ld_g = NULL;
  static gsl_vector *diag_g = NULL;
  static gsl_vector *rhs_g = NULL;
  static gsl_vector *tau_g = NULL;
  double eta = ((double *) params)[0];
  double chi = ((double *) params)[1];
  double tQ = ((double *) params)[2];
  double u, dlnx, sL, s, sR, dsdx, beta, dbetadx;
  double mdot;
  double dlnQdlnt;
  int i;

  /* Allocate memory if necessary */
  if (grd->nr != nr) {
    nr = grd->nr;
    rmin = -1.0;
    rmax = -1.0;
    /* If previously allocated, free */
    if (x != NULL) free(x);
    if (h1 != NULL) free(h1);
    if (h0 != NULL) free(h0);
    if (H != NULL) free(H);
    if (Q != NULL) free(Q);
    if (ud_g != NULL) gsl_vector_free(ud_g);
    if (ld_g != NULL) gsl_vector_free(ld_g);
    if (diag_g != NULL) gsl_vector_free(diag_g);
    if (rhs_g != NULL) gsl_vector_free(rhs_g);
    if (tau_g != NULL) gsl_vector_free(tau_g);
    /* Allocate new memory */
    if (!(x = calloc(grd->nr, sizeof(double)))) {
      fprintf(stderr, "Error: unable to allocate memory in userAlpha!\n");
      exit(1);
    }
    if (!(h1 = calloc(grd->nr, sizeof(double)))) {
      fprintf(stderr, "Error: unable to allocate memory in userAlpha!\n");
      exit(1);
    }
    if (!(h0 = calloc(grd->nr, sizeof(double)))) {
      fprintf(stderr, "Error: unable to allocate memory in userAlpha!\n");
      exit(1);
    }
    if (!(H = calloc(grd->nr, sizeof(double)))) {
      fprintf(stderr, "Error: unable to allocate memory in userAlpha!\n");
      exit(1);
    }
    if (!(Q = calloc(grd->nr, sizeof(double)))) {
      fprintf(stderr, "Error: unable to allocate memory in userAlpha!\n");
      exit(1);
    }
    if (!(ud_g = gsl_vector_alloc(grd->nr+1))) {
      fprintf(stderr, "Error: unable to allocate memory in userAlpha!\n");
      exit(1);
    }
    if (!(ld_g = gsl_vector_alloc(grd->nr+1))) {
      fprintf(stderr, "Error: unable to allocate memory in userAlpha!\n");
      exit(1);
    }
    if (!(diag_g = gsl_vector_alloc(grd->nr+2))) {
      fprintf(stderr, "Error: unable to allocate memory in userAlpha!\n");
      exit(1);
    }
    if (!(rhs_g = gsl_vector_alloc(grd->nr+2))) {
      fprintf(stderr, "Error: unable to allocate memory in userAlpha!\n");
      exit(1);
    }
    if (!(tau_g = gsl_vector_alloc(grd->nr+2))) {
      fprintf(stderr, "Error: unable to allocate memory in userAlpha!\n");
      exit(1);
    }
  }
  /* Initailize x if necessary */
  if ((rmin != grd->r_h[0]) || (rmax != grd->r_h[grd->nr+1])) {
    for (i=0; i<grd->nr; i++) x[i] = grd->r_g[i+1]/grd->r_h[grd->nr];
    rmin = grd->r_h[0];
    rmax = grd->r_h[grd->nr+1];
  }

  /* Compute coefficients of torque equation */
  dlnx = log(grd->r_g[1]/grd->r_g[0]);
  for (i=0; i<grd->nr; i++) {

    /* Define dimensionless velocity */
    u = grd->vphi_g[i+1] / grd->vphi_h[grd->nr];

    /* Note: compute derivatives of s using one-sided differences at
       edge cells, centered-limited differences elsewhere. */
    if (i==0) {
      s = sL = sqrt(pres[0]/col[0])/grd->vphi_h[grd->nr];
      sR = sqrt(pres[1]/col[1])/grd->vphi_h[grd->nr];
      dsdx = (sR - sL) / (x[i]*dlnx);
    } else if (i==grd->nr-1) {
      sL = sqrt(pres[grd->nr-2]/col[grd->nr-2])/grd->vphi_h[grd->nr];
      s = sR = sqrt(pres[grd->nr-1]/col[grd->nr-1])/grd->vphi_h[grd->nr];
      dsdx = (sR - sL) / (x[i]*dlnx);
    } else {
      sL = sqrt(pres[i+1]/col[i+1])/grd->vphi_h[grd->nr];
      s = sqrt(pres[i]/col[i])/grd->vphi_h[grd->nr];
      sR = sqrt(pres[i-1]/col[i-1])/grd->vphi_h[grd->nr];
      dsdx = ((sL-s)*(s-sR)) > 0 ? (sL-sR)/(2.0*x[i]*dlnx) : 0;
    }

    /* Compute derivative of beta using centered difference */
    beta = grd->beta_g[i+1];
    dbetadx = (grd->beta_h[i+1] - grd->beta_h[i]) / (x[i]*dlnx);

    /* Compute torque equation coefficients */
    h1[i] = -(5.0*(beta+1)*x[i]*dsdx + 2.0*s*(beta+SQR(beta)+x[i]*dbetadx)) /
      (2.0*(beta+1)*s*x[i]);
    h0[i] = (SQR(beta)-1) * SQR(u/(x[i]*s)) / 2.0;
    Q[i] = sqrt(2.0*(1+beta))*grd->vphi_g[i+1]/grd->r_g[i+1] *
      s*grd->vphi_h[grd->nr]/(M_PI*G*col[i]);
    dlnQdlnt = Q[i] < QCRIT ? -(exp(QCRIT/Q[i]) - exp(1.0))*u/x[i] : 0.0;
    H[i] = G*grd->r_h[grd->nr]*u*(1+beta)*col[i] *
      (2*M_PI*u*eta - 3*dlnQdlnt/tQ) / (2.0*SQR(grd->vphi_h[grd->nr])*chi);
  }

  /* Load matrix elements for tridiagonal solve */
  for (i=0; i<grd->nr; i++) {
    gsl_vector_set(ld_g, i, 1.0/SQR(x[i]*dlnx) +
		   1.0/(2.0*SQR(x[i])*dlnx) - h1[i]/(2.0*x[i]*dlnx));
    gsl_vector_set(diag_g, i+1, -2.0/SQR(x[i]*dlnx) + h0[i]);
    gsl_vector_set(ud_g, i+1, 1.0/SQR(x[i]*dlnx) -
		   1.0/(2.0*SQR(x[i])*dlnx) + h1[i]/(2.0*x[i]*dlnx));
    gsl_vector_set(rhs_g, i+1, H[i]);
  }

  /* Load boundary conditions into matrix */
  /* Inner BC: tau = -x in ghost cell */
  gsl_vector_set(diag_g, 0, 1);
  gsl_vector_set(ud_g, 0, 0);
  gsl_vector_set(rhs_g, 0, -grd->r_g[0]/grd->r_h[grd->nr]);
  /* Outer BC: tau' = -beta-1 */
  gsl_vector_set(ld_g, grd->nr, -1.0/dlnx);
  gsl_vector_set(diag_g, grd->nr+1, 1.0/dlnx);
  gsl_vector_set(rhs_g, grd->nr+1, -grd->beta_g[grd->nr+1]-1);

  /* Solve matrix equation to get dimensionless torque */
  gsl_linalg_solve_tridiag(diag_g, ud_g, ld_g, rhs_g, tau_g);

  /* Use torque to compute alpha */
  mdot = chi*SQR(grd->vphi_h[grd->nr])*grd->vphi_h[grd->nr] / G;
  for (i=0; i<grd->nr; i++) {
    alpha[i] = -gsl_vector_get(tau_g, i+1)*mdot*grd->vphi_h[grd->nr] *
      grd->r_h[grd->nr] /
      (2.0*M_PI*SQR(grd->r_g[i+1])*pres[i]);
    if (Q[i] > QCRIT) alpha[i] *= exp(-QDAMP*(Q[i]-QCRIT));
    if (alpha[i] < 0.0) alpha[i] = ALPHAMIN;
  }
}

void
userMassSrc(const double t, const double dt, const grid *grd,
	    const double *col, const double *pres, const double *eInt,
	    const double *gamma, const double *delta,
	    void *params,
	    double *massSrc) {
  fprintf(stderr,
	  "Warning: userMassSrc function called but not implemented!\n");
  return;
}

void
userIntEnSrc(const double t, const double dt, const grid *grd,
	     const double *col, const double *pres, const double *eInt,
	     const double *gamma, const double *delta,
	     void *params,
	     double *intEnSrc) {
  /* Cooling rate = eta Sigma sigma^2 Omega = eta P vphi/r */
  int i;
  double eta = ((double *) params)[0];

  for (i=0; i<grd->nr; i++)
    intEnSrc[i] = -eta * pres[i] * grd->vphi_g[i+1] / grd->r_g[i+1];
}

void
userIBC(const double t, const double dt, const grid *grd,
	const double *col, const double *pres, const double *eInt,
	const double *gamma, const double *delta,
	const pres_bc_type ibc_pres, const enth_bc_type ibc_enth,
	void *params,
	double *ibc_pres_val, double *ibc_enth_val) {

  /* Set torque. If Q <= QCRIT in innermost cell, so that disk is
     gravitationally unstable at the center, dimensionless torque =
     -x, dimensional torque = -(r_in/R) Mdot vphi/R. If Q > QCRIT in
     innermost cell, scale down torque in the same way as in
     userAlpha. */
  double chi = ((double *) params)[1];
  double x, mdot, s, Q;

  s = sqrt(pres[0]/col[0])/grd->vphi_h[grd->nr];
  Q = sqrt(2.0*(1+grd->beta_g[1]))*grd->vphi_g[1]/grd->r_g[1] *
    s*grd->vphi_h[grd->nr]/(M_PI*G*col[0]);
  mdot = chi*SQR(grd->vphi_h[grd->nr])*grd->vphi_h[grd->nr] / G;
  x = grd->r_g[0] / grd->r_h[grd->nr];
  *ibc_pres_val = -x * mdot * grd->vphi_g[0] * grd->r_h[grd->nr];
  if (Q > QCRIT) *ibc_pres_val *= exp(-QDAMP*(Q-QCRIT));
  *ibc_enth_val = gamma[0]/(gamma[0]-1.0)*pres[0]/col[0];
}

void
userOBC(const double t, const double dt, const grid *grd,
	const double *col, const double *pres, const double *eInt,
	const double *gamma, const double *delta,
	const pres_bc_type obc_pres, const enth_bc_type obc_enth,
	void *params,
	double *obc_pres_val, double *obc_enth_val) {
  fprintf(stderr,
	  "Warning: userOBC function called but not implemented!\n");
  return;
}

void
userPreTimestep(const double t, const double dt,
		const grid *grd, double *col, double *pres,
		double *eInt, double *mBnd, double *eBnd,
		double *mSrc, double *eSrc,
		void *params, const unsigned long nUserOut,
		double *userOut) {
  fprintf(stderr,
	  "Warning: userPreTimestep function called but not implemented!\n");
  return;
}

void
userCheckRead(
	      FILE *fp, grid *grd, const unsigned long nOut,
	      double *tOut, double *colOut,
	      double *presOut, double *eIntOut, double *mBndOut,
	      double *eBndOut, double *mSrcOut, double *eSrcOut,
	      const unsigned long nUserOut, double *userOut,
	      void *params
	      ) {
  fprintf(stderr,
	  "Warning: userCheckRead function called but not implemented!\n");
  return;
}

void
userCheckWrite(
	      FILE *fp,
	      const grid *grd, const unsigned long nOut,
	      const double *tOut, const double *colOut,
	      const double *presOut, const double *eIntOut,
	      const double *mBndOut, const double *eBndOut,
	      const double *mSrcOut, const double *eSrcOut,
	      const unsigned long nUserOut, const double *userOut,
	      const void *params
	      ) {
  fprintf(stderr,
	  "Warning: userCheckWrite function called but not implemented!\n");
  return;
}


#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 compute_MCMC_chain_stats(double **x,
                              int      n_x,
                              int      n_avg,
                              double * x_min,
                              double * x_bar_in,
                              double * x_max,
                              double * x_sigma,
                              double **auto_cor,
                              double * slopes,
                              double * dP_sub,
                              double * ln_Pr_in,
                              double * ln_Pr_min,
                              double * ln_Pr_avg,
                              double * ln_Pr_max) {
    int     i_P;
    int     i_avg;
    double *x_bar;
    double *s_temp;
    double  c0, c1, cov00, cov01, cov11, sumsq;
    double  covar;
    double  var;

    // Allocate a temporary array for the mean if it hasn't been given/isn't requested but is needed
    if((x_sigma != NULL || auto_cor != NULL || slopes != NULL) && x_bar_in == NULL)
        x_bar = (double *)SID_malloc(sizeof(double) * n_x);
    else
        x_bar = x_bar_in;

    // Compute the mean (if it's requested/needed)
    if(x_bar != NULL) {
        for(i_P = 0; i_P < n_x; i_P++) {
            x_bar[i_P] = 0.;
            for(i_avg = 0; i_avg < n_avg; i_avg++)
                x_bar[i_P] += x[i_P][i_avg];
            x_bar[i_P] /= (double)n_avg;
        }
    }

    // Compute the std deviation (if it's requested/needed)
    if(x_sigma != NULL) {
        for(i_P = 0; i_P < n_x; i_P++) {
            x_sigma[i_P] = 0.;
            for(i_avg = 0; i_avg < n_avg; i_avg++)
                x_sigma[i_P] += pow(x[i_P][i_avg] - x_bar[i_P], 2.);
            x_sigma[i_P] = sqrt(x_sigma[i_P] / (double)n_avg);
        }
    }

    // Compute the slope across the interval (if it's requested/needed)
    if(slopes != NULL) {
        s_temp = (double *)SID_malloc(sizeof(double) * n_avg);
        for(i_avg = 0; i_avg < n_avg; i_avg++)
            s_temp[i_avg] = (double)i_avg;
        for(i_P = 0; i_P < n_x; i_P++) {
            gsl_fit_linear(s_temp, 1, x[i_P], 1, n_avg, &c0, &c1, &cov00, &cov01, &cov11, &sumsq);
            slopes[i_P] = c1;
            if(dP_sub != NULL)
                dP_sub[i_P] = sqrt(sumsq / (double)n_avg);
        }
        SID_free(SID_FARG s_temp);
    }

    // Compute the auto correlation function (if it's requested/needed)
    if(auto_cor != NULL) {
        for(i_P = 0; i_P < n_x; i_P++) {
            for(i_P = 1; i_P < n_avg; i_P++) {
                for(i_avg = 0, var = 0., covar = 0.; i_avg < (n_avg - i_P); i_avg++) {
                    covar += (x[i_P][i_avg] - x_bar[i_P]) * (x[i_P][i_avg + i_P] - x_bar[i_P]);
                    var += (x[i_P][i_avg] - x_bar[i_P]) * (x[i_P][i_avg] - x_bar[i_P]);
                }
                auto_cor[i_P][i_P - 1] = covar / var;
            }
        }
    }

    // Compute ln_Pr statistics
    if(ln_Pr_in != NULL) {
        (*ln_Pr_min) = ln_Pr_in[0];
        (*ln_Pr_avg) = ln_Pr_in[0];
        (*ln_Pr_max) = ln_Pr_in[0];
        for(i_avg = 1; i_avg < n_avg; i_avg++) {
            (*ln_Pr_min) = GBP_MIN((*ln_Pr_min), ln_Pr_in[i_avg]);
            (*ln_Pr_avg) += ln_Pr_in[i_avg];
            (*ln_Pr_max) = GBP_MAX((*ln_Pr_max), ln_Pr_in[i_avg]);
        }
        (*ln_Pr_avg) /= (double)n_avg;
    }

    if(x_bar != x_bar_in)
        SID_free(SID_FARG x_bar);
}


#ifndef _H_MAIN
#define _H_MAIN

#define CPP 0

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include <gsl/gsl_linalg.h>
#include <GL/glew.h>
#include <GLFW/glfw3.h>

#if CPP
#include <glm/glm.hpp>
#include <glm/gtx/transform.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>
#endif

#define WW 640
#define WH 480
#define RAD(d) (d) * (M_PI/180)
#define ZEROCHK(i) ((i) == 0)?0:i

extern GLFWwindow *window;
extern GLuint tr_vb;
extern GLuint tr_c_vb;
extern GLuint va;
extern GLuint vs;
extern GLuint fs;
extern GLuint shader_bin;
uint64_t timer[10];

extern void cookTriangle(void);
extern void cookCube(void);
extern uint64_t getCycles(void);
extern char * readFile(char *);
extern void debugSC(GLuint);
extern uint64_t getCycles(void);

extern gsl_matrix * m_new_diag(uint32_t);
extern gsl_matrix * m_new(uint32_t, uint32_t);
extern void m_setT(gsl_matrix *, double, double, double, uint8_t);
extern void m_setRy(gsl_matrix *, double, uint8_t);
extern void m_mul(gsl_matrix *, gsl_matrix *, gsl_matrix *);
extern void m_print(gsl_matrix *, uint8_t, uint8_t);
extern void m_array(gsl_matrix *, uint8_t, uint8_t, float *);
extern void glmPerspective(double, double, double, double, gsl_matrix *);
extern void normalize(double *, uint8_t, double *);
extern double getVectorLength(double *, uint8_t);
extern void getCrossV3(double *, double *, double *);
extern double scalar(double *, double *, uint8_t);
extern void subVec(double *, double *, uint8_t, double *);
extern void glmLookAt(double *, double *, double *, gsl_matrix *);
extern void m_set_all(gsl_matrix *, double);

#include "lib.c"
#include "mat.c"

#endif