/* linalg/lu.c
 *
 * Copyright (C) 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 <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>

static int LU_band_decomp_L2 (const size_t M, const size_t lb, const size_t ub,
                              gsl_matrix * AB, gsl_vector_uint * ipiv);

/* Factorise a (p,q) banded M x N matrix A into,
 *
 *   P A = L U
 *
 * where P is a permutation matrix, L is unit lower triangular and U
 * is upper triangular.
 *
 * L is stored in the strict lower triangular part of the input
 * matrix. The diagonal elements of L are unity and are not stored.
 *
 * U is stored in the diagonal and upper triangular part of the
 * input matrix.
 *
 * P is stored in the permutation p. Column j of P is column k of the
 * identity matrix, where k = permutation->data[j]
 *
 * Inputs: M   - number of rows in matrix
 *         lb  - lower bandwidth
 *         ub  - upper bandwidth
 *         AB  - matrix in band storage format, N-by-(2*p + q + 1)
 *         piv - pivot vector, size MIN(M, N)
 */

int
gsl_linalg_LU_band_decomp (const size_t M, const size_t lb, const size_t ub, gsl_matrix * AB, gsl_vector_uint * piv)
{
  const size_t N = AB->size1;
  const size_t minMN = GSL_MIN(M, N);

  if (lb >= M)
    {
      GSL_ERROR ("lower bandwidth must be less than M", GSL_EDOM);
    }
  else if (ub >= N)
    {
      GSL_ERROR ("upper bandwidth must be less than N", GSL_EDOM);
    }
  else if (AB->size2 != 2*lb + ub + 1)
    {
      GSL_ERROR ("matrix size inconsistent with bandwidths", GSL_EBADLEN);
    }
  else if (piv->size != minMN)
    {
      GSL_ERROR ("pivot vector must have length MIN(M,N)", GSL_EBADLEN);
    }
  else
    {
      int status;

      status = LU_band_decomp_L2 (M, lb, ub, AB, piv);

      return status;
    }
}

int
gsl_linalg_LU_band_solve (const size_t lb, const size_t ub, const gsl_matrix * LUB,
                          const gsl_vector_uint * piv, const gsl_vector * b, gsl_vector * x)
{
  const size_t N = LUB->size1;

  if (N != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else if (N != b->size)
    {
      GSL_ERROR ("matrix size must match rhs size", GSL_EBADLEN);
    }
  else if (lb >= N)
    {
      GSL_ERROR ("lower bandwidth must be less than N", GSL_EDOM);
    }
  else if (ub >= N)
    {
      GSL_ERROR ("upper bandwidth must be less than N", GSL_EDOM);
    }
  else if (LUB->size2 != 2*lb + ub + 1)
    {
      GSL_ERROR ("matrix size inconsistent with bandwidths", GSL_EBADLEN);
    }
  else if (piv->size != N)
    {
      GSL_ERROR ("pivot vector must have length N", GSL_EBADLEN);
    }
  else
    {
      int status;

      gsl_vector_memcpy(x, b);

      status = gsl_linalg_LU_band_svx(lb, ub, LUB, piv, x);

      return status;
    }
}

int
gsl_linalg_LU_band_svx (const size_t lb, const size_t ub, const gsl_matrix * LUB,
                        const gsl_vector_uint * piv, gsl_vector * x)
{
  const size_t N = LUB->size1;

  if (N != x->size)
    {
      GSL_ERROR ("matrix size must match solution/rhs size", GSL_EBADLEN);
    }
  else if (lb >= N)
    {
      GSL_ERROR ("lower bandwidth must be less than N", GSL_EDOM);
    }
  else if (ub >= N)
    {
      GSL_ERROR ("upper bandwidth must be less than N", GSL_EDOM);
    }
  else if (LUB->size2 != 2*lb + ub + 1)
    {
      GSL_ERROR ("matrix size inconsistent with bandwidths", GSL_EBADLEN);
    }
  else if (piv->size != N)
    {
      GSL_ERROR ("pivot vector must have length N", GSL_EBADLEN);
    }
  else
    {
      if (lb > 0)
        {
          size_t j;

          for (j = 0; j < N - 1; ++j)
            {
              size_t pj = gsl_vector_uint_get(piv, j);
              double * xj = gsl_vector_ptr(x, j);
              size_t lm = GSL_MIN(lb, N - j - 1);
              gsl_vector_view xv = gsl_vector_subvector(x, j + 1, lm);
              gsl_vector_const_view yv = gsl_matrix_const_subrow(LUB, j, lb + ub + 1, lm);

              if (j != pj)
                {
                  double xl = gsl_vector_get(x, pj);
                  gsl_vector_set(x, pj, *xj);
                  *xj = xl;
                }

              gsl_blas_daxpy(-(*xj), &yv.vector, &xv.vector);
            }
        }

      /* solve U x = b */
      cblas_dtbsv(CblasColMajor, CblasUpper, CblasNoTrans, CblasNonUnit,
                  (int) N, (int) (lb + ub), LUB->data, LUB->tda,
                  x->data, x->stride);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_LU_band_unpack (const size_t M, const size_t lb, const size_t ub, const gsl_matrix * LUB,
                           const gsl_vector_uint * piv, gsl_matrix * L, gsl_matrix * U)
{
  const size_t N = LUB->size1;
  const size_t minMN = GSL_MIN(M, N);

  if (ub >= N)
    {
      GSL_ERROR ("upper bandwidth must be < N", GSL_EDOM);
    }
  else if (lb >= M)
    {
      GSL_ERROR ("lower bandwidth must be < M", GSL_EDOM);
    }
  else if (LUB->size2 != 2*lb + ub + 1)
    {
      GSL_ERROR ("matrix size inconsistent with bandwidths", GSL_EBADLEN);
    }
  else if (piv->size != minMN)
    {
      GSL_ERROR ("pivot vector must have length MIN(M,N)", GSL_EBADLEN);
    }
  else if (L->size1 != M || L->size2 != minMN)
    {
      GSL_ERROR ("L matrix has wrong dimensions", GSL_EBADLEN);
    }
  else if (U->size1 != minMN || U->size2 != N)
    {
      GSL_ERROR ("U matrix has wrong dimensions", GSL_EBADLEN);
    }
  else
    {
      const size_t ub_U = lb + ub;
      size_t j;

      gsl_matrix_set_identity(L);
      gsl_matrix_set_zero(U);

      /* compute L */
      if (lb > 0)
        {
          const size_t jstart = (M > N) ? minMN : minMN - 1;
          size_t j;

          for (j = jstart; j > 0 && j--; )
            {
              size_t pj = gsl_vector_uint_get(piv, j);
              size_t lm = GSL_MIN(lb, M - j - 1);
              gsl_vector_const_view xv = gsl_matrix_const_subrow(LUB, j, lb + ub + 1, lm);
              gsl_vector_const_view yv = gsl_matrix_const_subrow(L, j, 0, minMN);
              gsl_matrix_view m = gsl_matrix_submatrix(L, j + 1, 0, lm, minMN);

              gsl_blas_dger(1.0, &xv.vector, &yv.vector, &m.matrix);

              if (j != pj)
                {
                  gsl_vector_view Lj = gsl_matrix_row(L, j);
                  gsl_vector_view Lpj = gsl_matrix_row(L, pj);
                  gsl_blas_dswap(&Lj.vector, &Lpj.vector);
                }
            }
        }

      /* fill in U */
      for (j = 0; j <= GSL_MIN(ub_U, N - 1); ++j)
        {
          gsl_vector_const_view src = gsl_matrix_const_subcolumn(LUB, ub_U - j, j, GSL_MIN(M, N - j));
          gsl_vector_view dest = gsl_matrix_superdiagonal(U, j);
          gsl_vector_memcpy(&dest.vector, &src.vector);
        }

      return GSL_SUCCESS;
    }
}

/*
LU_band_decomp_L2
  LU decomposition with partial pivoting using Level 2 BLAS

Inputs: M    - number of rows in matrix
        lb   - lower bandwidth
        ub   - upper bandwidth
        AB   - on input, matrix to be factored; on output, L and U factors
               N-by-(2*p + q + 1)
        ipiv - (output) array containing row swaps

Notes:
1) Based on LAPACK DGBTF2
*/

static int
LU_band_decomp_L2 (const size_t M, const size_t lb, const size_t ub,
                   gsl_matrix * AB, gsl_vector_uint * ipiv)
{
  const size_t N = AB->size1;
  const size_t minMN = GSL_MIN(M, N);

  if (ipiv->size != minMN)
    {
      GSL_ERROR ("ipiv length must equal MIN(M,N)", GSL_EBADLEN);
    }
  else if (lb >= M)
    {
      GSL_ERROR ("lower bandwidth must be less than M", GSL_EDOM);
    }
  else if (ub >= N)
    {
      GSL_ERROR ("upper bandwidth must be less than N", GSL_EDOM);
    }
  else if (AB->size2 != 2*lb + ub + 1)
    {
      GSL_ERROR ("matrix size inconsistent with bandwidths", GSL_EBADLEN);
    }
  else
    {
      const size_t ub_U = lb + ub; /* upper bandwidth of U factor */
      const size_t ldab = AB->size2;
      size_t ju = 0;
      size_t j;

      if (lb > 0)
        {
          /* initialize fill-in elements to zero */
          gsl_matrix_view m = gsl_matrix_submatrix(AB, 0, 0, N, lb);
          gsl_matrix_set_zero(&m.matrix);
        }

      for (j = 0; j < minMN; ++j)
        {
          size_t lbj = GSL_MIN(lb, M - j - 1); /* subdiagonal elements in column j */
          gsl_vector_view x = gsl_matrix_subrow(AB, j, ub_U, lbj + 1);
          gsl_vector_view y;
          CBLAS_INDEX_t j_pivot = gsl_blas_idamax(&x.vector);
          double * ptr;

          gsl_vector_uint_set(ipiv, j, j + j_pivot);

          ptr = gsl_matrix_ptr(AB, j, ub_U + j_pivot);
          if (*ptr != 0.0)
            ju = GSL_MAX(ju, GSL_MIN(j + ub + j_pivot, N - 1));

          if (j_pivot != 0)
            {
              double *ptr2;

              /* swap columns */

              x = gsl_vector_view_array_with_stride(ptr, ldab - 1, ju - j + 1);

              ptr2 = gsl_matrix_ptr(AB, j, ub_U);
              y = gsl_vector_view_array_with_stride(ptr2, ldab - 1, ju - j + 1);

              gsl_blas_dswap(&x.vector, &y.vector);
            }

          if (lbj > 0)
            {
              double tmp = gsl_matrix_get(AB, j, ub_U);

              x = gsl_matrix_subrow(AB, j, ub_U + 1, lbj);
              gsl_blas_dscal(1.0 / tmp, &x.vector);

              if (ju > j)
                {
                  gsl_matrix_view m = gsl_matrix_submatrix(AB, j + 1, ub_U, ju - j, lbj);

                  ptr = gsl_matrix_ptr(AB, j + 1, ub_U - 1);
                  y = gsl_vector_view_array_with_stride(ptr, ldab - 1, ju - j);

                  m.matrix.tda = ldab - 1; /* unskew matrix */
                  gsl_blas_dger(-1.0, &y.vector, &x.vector, &m.matrix);
                }
            }
        }

      return GSL_SUCCESS;
    }
}


/* eigen/nonsymmv.c
 *
 * Copyright (C) 2006 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_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_vector_complex.h>
#include <gsl/gsl_matrix.h>

/*
 * This module computes the eigenvalues and eigenvectors of a real
 * nonsymmetric matrix.
 *
 * This file contains routines based on original code from LAPACK
 * which is distributed under the modified BSD license. The LAPACK
 * routines used are DTREVC and DLALN2.
 */

#define GSL_NONSYMMV_SMLNUM (2.0 * GSL_DBL_MIN)
#define GSL_NONSYMMV_BIGNUM ((1.0 - GSL_DBL_EPSILON) / GSL_NONSYMMV_SMLNUM)

static void nonsymmv_get_right_eigenvectors(gsl_matrix *T, gsl_matrix *Z,
                                            gsl_vector_complex *eval,
                                            gsl_matrix_complex *evec,
                                            gsl_eigen_nonsymmv_workspace *w);
static void nonsymmv_normalize_eigenvectors(gsl_vector_complex *eval,
                                            gsl_matrix_complex *evec);

/*
gsl_eigen_nonsymmv_alloc()

Allocate a workspace for solving the nonsymmetric eigenvalue problem.
The size of this workspace is O(5n).

Inputs: n - size of matrices

Return: pointer to workspace
*/

gsl_eigen_nonsymmv_workspace *
gsl_eigen_nonsymmv_alloc(const size_t n)
{
  gsl_eigen_nonsymmv_workspace *w;

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

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

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

  w->size = n;
  w->Z = NULL;
  w->nonsymm_workspace_p = gsl_eigen_nonsymm_alloc(n);

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

  /*
   * set parameters to compute the full Schur form T and balance
   * the matrices
   */
  gsl_eigen_nonsymm_params(1, 1, w->nonsymm_workspace_p);

  w->work = gsl_vector_alloc(n);
  w->work2 = gsl_vector_alloc(n);
  w->work3 = gsl_vector_alloc(n);
  if (w->work == 0 || w->work2 == 0 || w->work3 == 0)
    {
      gsl_eigen_nonsymmv_free(w);
      GSL_ERROR_NULL ("failed to allocate space for nonsymmv additional workspace", GSL_ENOMEM);
    }

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

/*
gsl_eigen_nonsymmv_free()
  Free workspace w
*/

void
gsl_eigen_nonsymmv_free (gsl_eigen_nonsymmv_workspace * w)
{
  RETURN_IF_NULL (w);

  if (w->nonsymm_workspace_p)
    gsl_eigen_nonsymm_free(w->nonsymm_workspace_p);

  if (w->work)
    gsl_vector_free(w->work);

  if (w->work2)
    gsl_vector_free(w->work2);

  if (w->work3)
    gsl_vector_free(w->work3);

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

/*
gsl_eigen_nonsymmv()

Solve the nonsymmetric eigensystem problem

A x = \lambda x

for the eigenvalues \lambda and right eigenvectors x

Inputs: A    - general real matrix
        eval - where to store eigenvalues
        evec - where to store eigenvectors
        w    - workspace

Return: success or error
*/

int
gsl_eigen_nonsymmv (gsl_matrix * A, gsl_vector_complex * eval,
                    gsl_matrix_complex * evec,
                    gsl_eigen_nonsymmv_workspace * w)
{
  const size_t N = A->size1;

  /* check matrix and vector sizes */

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

      /*
       * We need a place to store the Schur vectors, so we will
       * treat evec as a real matrix and store them in the left
       * half - the factor of 2 in the tda corresponds to the
       * complex multiplicity
       */
      Z.size1 = N;
      Z.size2 = N;
      Z.tda = 2 * N;
      Z.data = evec->data;
      Z.block = 0;
      Z.owner = 0;

      /* compute eigenvalues, Schur form, and Schur vectors */
      s = gsl_eigen_nonsymm_Z(A, eval, &Z, w->nonsymm_workspace_p);

      if (w->Z)
        {
          /*
           * save the Schur vectors in user supplied matrix, since
           * they will be destroyed when computing eigenvectors
           */
          gsl_matrix_memcpy(w->Z, &Z);
        }

      /* only compute eigenvectors if we found all eigenvalues */
      if (s == GSL_SUCCESS)
        {
          /* compute eigenvectors */
          nonsymmv_get_right_eigenvectors(A, &Z, eval, evec, w);

          /* normalize so that Euclidean norm is 1 */
          nonsymmv_normalize_eigenvectors(eval, evec);
        }

      return s;
    }
} /* gsl_eigen_nonsymmv() */

/*
gsl_eigen_nonsymmv_Z()
  Compute eigenvalues and eigenvectors of a real nonsymmetric matrix
and also save the Schur vectors. See comments in gsl_eigen_nonsymm_Z
for more information.

Inputs: A    - real nonsymmetric matrix
        eval - where to store eigenvalues
        evec - where to store eigenvectors
        Z    - where to store Schur vectors
        w    - nonsymmv workspace

Return: success or error
*/

int
gsl_eigen_nonsymmv_Z (gsl_matrix * A, gsl_vector_complex * eval,
                      gsl_matrix_complex * evec, gsl_matrix * Z,
                      gsl_eigen_nonsymmv_workspace * w)
{
  /* check matrix and vector sizes */

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

      w->Z = Z;

      s = gsl_eigen_nonsymmv(A, eval, evec, w);

      w->Z = NULL;

      return s;
    }
} /* gsl_eigen_nonsymmv_Z() */

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

/*
nonsymmv_get_right_eigenvectors()
  Compute the right eigenvectors of the Schur form T and then
backtransform them using the Schur vectors to get right eigenvectors of
the original matrix.

Inputs: T    - Schur form
        Z    - Schur vectors
        eval - where to store eigenvalues (to ensure that the
               correct eigenvalue is stored in the same position
               as the eigenvectors)
        evec - where to store eigenvectors
        w    - nonsymmv workspace

Return: none

Notes: 1) based on LAPACK routine DTREVC - the algorithm used is
          backsubstitution on the upper quasi triangular system T
          followed by backtransformation by Z to get vectors of the
          original matrix.

       2) The Schur vectors in Z are destroyed and replaced with
          eigenvectors stored with the same storage scheme as DTREVC.
          The eigenvectors are also stored in 'evec'

       3) The matrix T is unchanged on output

       4) Each eigenvector is normalized so that the element of
          largest magnitude has magnitude 1; here the magnitude of
          a complex number (x,y) is taken to be |x| + |y|
*/

static void
nonsymmv_get_right_eigenvectors(gsl_matrix *T, gsl_matrix *Z,
                                gsl_vector_complex *eval,
                                gsl_matrix_complex *evec,
                                gsl_eigen_nonsymmv_workspace *w)
{
  const size_t N = T->size1;
  const double smlnum = GSL_DBL_MIN * N / GSL_DBL_EPSILON;
  const double bignum = (1.0 - GSL_DBL_EPSILON) / smlnum;
  int i;              /* looping */
  size_t iu,          /* looping */
         ju,
         ii;
  gsl_complex lambda; /* current eigenvalue */
  double lambda_re,   /* Re(lambda) */
         lambda_im;   /* Im(lambda) */
  gsl_matrix_view Tv, /* temporary views */
                  Zv;
  gsl_vector_view y,  /* temporary views */
                  y2,
                  ev,
                  ev2;
  double dat[4],      /* scratch arrays */
         dat_X[4];
  double scale;       /* scale factor */
  double xnorm;       /* |X| */
  gsl_vector_complex_view ecol, /* column of evec */
                          ecol2;
  int complex_pair;   /* complex eigenvalue pair? */
  double smin;

  /*
   * Compute 1-norm of each column of upper triangular part of T
   * to control overflow in triangular solver
   */

  gsl_vector_set(w->work3, 0, 0.0);
  for (ju = 1; ju < N; ++ju)
    {
      gsl_vector_set(w->work3, ju, 0.0);
      for (iu = 0; iu < ju; ++iu)
        {
          gsl_vector_set(w->work3, ju,
                         gsl_vector_get(w->work3, ju) +
                         fabs(gsl_matrix_get(T, iu, ju)));
        }
    }

  for (i = (int) N - 1; i >= 0; --i)
    {
      iu = (size_t) i;

      /* get current eigenvalue and store it in lambda */
      lambda_re = gsl_matrix_get(T, iu, iu);

      if (iu != 0 && gsl_matrix_get(T, iu, iu - 1) != 0.0)
        {
          lambda_im = sqrt(fabs(gsl_matrix_get(T, iu, iu - 1))) *
                      sqrt(fabs(gsl_matrix_get(T, iu - 1, iu)));
        }
      else
        {
          lambda_im = 0.0;
        }

      GSL_SET_COMPLEX(&lambda, lambda_re, lambda_im);

      smin = GSL_MAX(GSL_DBL_EPSILON * (fabs(lambda_re) + fabs(lambda_im)),
                     smlnum);
      smin = GSL_MAX(smin, GSL_NONSYMMV_SMLNUM);

      if (lambda_im == 0.0)
        {
          int k, l;
          gsl_vector_view bv, xv;

          /* real eigenvector */

          /*
           * The ordering of eigenvalues in 'eval' is arbitrary and
           * does not necessarily follow the Schur form T, so store
           * lambda in the right slot in eval to ensure it corresponds
           * to the eigenvector we are about to compute
           */
          gsl_vector_complex_set(eval, iu, lambda);

          /*
           * We need to solve the system:
           *
           * (T(1:iu-1, 1:iu-1) - lambda*I)*X = -T(1:iu-1,iu)
           */

          /* construct right hand side */
          for (k = 0; k < i; ++k)
            {
              gsl_vector_set(w->work,
                             (size_t) k,
                             -gsl_matrix_get(T, (size_t) k, iu));
            }

          gsl_vector_set(w->work, iu, 1.0);

          for (l = i - 1; l >= 0; --l)
            {
              size_t lu = (size_t) l;

              if (lu == 0)
                complex_pair = 0;
              else
                complex_pair = gsl_matrix_get(T, lu, lu - 1) != 0.0;

              if (!complex_pair)
                {
                  double x;

                  /*
                   * 1-by-1 diagonal block - solve the system:
                   *
                   * (T_{ll} - lambda)*x = -T_{l(iu)}
                   */

                  Tv = gsl_matrix_submatrix(T, lu, lu, 1, 1);
                  bv = gsl_vector_view_array(dat, 1);
                  gsl_vector_set(&bv.vector, 0,
                                 gsl_vector_get(w->work, lu));
                  xv = gsl_vector_view_array(dat_X, 1);

                  gsl_schur_solve_equation(1.0,
                                           &Tv.matrix,
                                           lambda_re,
                                           1.0,
                                           1.0,
                                           &bv.vector,
                                           &xv.vector,
                                           &scale,
                                           &xnorm,
                                           smin);

                  /* scale x to avoid overflow */
                  x = gsl_vector_get(&xv.vector, 0);
                  if (xnorm > 1.0)
                    {
                      if (gsl_vector_get(w->work3, lu) > bignum / xnorm)
                        {
                          x /= xnorm;
                          scale /= xnorm;
                        }
                    }

                  if (scale != 1.0)
                    {
                      gsl_vector_view wv;

                      wv = gsl_vector_subvector(w->work, 0, iu + 1);
                      gsl_blas_dscal(scale, &wv.vector);
                    }

                  gsl_vector_set(w->work, lu, x);

                  if (lu > 0)
                    {
                      gsl_vector_view v1, v2;

                      /* update right hand side */

                      v1 = gsl_matrix_subcolumn(T, lu, 0, lu);
                      v2 = gsl_vector_subvector(w->work, 0, lu);
                      gsl_blas_daxpy(-x, &v1.vector, &v2.vector);
                    } /* if (l > 0) */
                } /* if (!complex_pair) */
              else
                {
                  double x11, x21;

                  /*
                   * 2-by-2 diagonal block
                   */

                  Tv = gsl_matrix_submatrix(T, lu - 1, lu - 1, 2, 2);
                  bv = gsl_vector_view_array(dat, 2);
                  gsl_vector_set(&bv.vector, 0,
                                 gsl_vector_get(w->work, lu - 1));
                  gsl_vector_set(&bv.vector, 1,
                                 gsl_vector_get(w->work, lu));
                  xv = gsl_vector_view_array(dat_X, 2);

                  gsl_schur_solve_equation(1.0,
                                           &Tv.matrix,
                                           lambda_re,
                                           1.0,
                                           1.0,
                                           &bv.vector,
                                           &xv.vector,
                                           &scale,
                                           &xnorm,
                                           smin);

                  /* scale X(1,1) and X(2,1) to avoid overflow */
                  x11 = gsl_vector_get(&xv.vector, 0);
                  x21 = gsl_vector_get(&xv.vector, 1);

                  if (xnorm > 1.0)
                    {
                      double beta;

                      beta = GSL_MAX(gsl_vector_get(w->work3, lu - 1),
                                     gsl_vector_get(w->work3, lu));
                      if (beta > bignum / xnorm)
                        {
                          x11 /= xnorm;
                          x21 /= xnorm;
                          scale /= xnorm;
                        }
                    }

                  /* scale if necessary */
                  if (scale != 1.0)
                    {
                      gsl_vector_view wv;

                      wv = gsl_vector_subvector(w->work, 0, iu + 1);
                      gsl_blas_dscal(scale, &wv.vector);
                    }

                  gsl_vector_set(w->work, lu - 1, x11);
                  gsl_vector_set(w->work, lu, x21);

                  /* update right hand side */
                  if (lu > 1)
                    {
                      gsl_vector_view v1, v2;

                      v1 = gsl_matrix_subcolumn(T, lu - 1, 0, lu - 1);
                      v2 = gsl_vector_subvector(w->work, 0, lu - 1);
                      gsl_blas_daxpy(-x11, &v1.vector, &v2.vector);

                      v1 = gsl_matrix_subcolumn(T, lu, 0, lu - 1);
                      gsl_blas_daxpy(-x21, &v1.vector, &v2.vector);
                    }

                  --l;
                } /* if (complex_pair) */
            } /* for (l = i - 1; l >= 0; --l) */

          /*
           * At this point, w->work is an eigenvector of the
           * Schur form T. To get an eigenvector of the original
           * matrix, we multiply on the left by Z, the matrix of
           * Schur vectors
           */

          ecol = gsl_matrix_complex_column(evec, iu);
          y = gsl_matrix_column(Z, iu);

          if (iu > 0)
            {
              gsl_vector_view x;

              Zv = gsl_matrix_submatrix(Z, 0, 0, N, iu);

              x = gsl_vector_subvector(w->work, 0, iu);

              /* compute Z * w->work and store it in Z(:,iu) */
              gsl_blas_dgemv(CblasNoTrans,
                             1.0,
                             &Zv.matrix,
                             &x.vector,
                             gsl_vector_get(w->work, iu),
                             &y.vector);
            } /* if (iu > 0) */

          /* store eigenvector into evec */

          ev = gsl_vector_complex_real(&ecol.vector);
          ev2 = gsl_vector_complex_imag(&ecol.vector);

          scale = 0.0;
          for (ii = 0; ii < N; ++ii)
            {
              double a = gsl_vector_get(&y.vector, ii);

              /* store real part of eigenvector */
              gsl_vector_set(&ev.vector, ii, a);

              /* set imaginary part to 0 */
              gsl_vector_set(&ev2.vector, ii, 0.0);

              if (fabs(a) > scale)
                scale = fabs(a);
            }

          if (scale != 0.0)
            scale = 1.0 / scale;

          /* scale by magnitude of largest element */
          gsl_blas_dscal(scale, &ev.vector);
        } /* if (GSL_IMAG(lambda) == 0.0) */
      else
        {
          gsl_vector_complex_view bv, xv;
          size_t k;
          int l;
          gsl_complex lambda2;

          /* complex eigenvector */

          /*
           * Store the complex conjugate eigenvalues in the right
           * slots in eval
           */
          GSL_SET_REAL(&lambda2, GSL_REAL(lambda));
          GSL_SET_IMAG(&lambda2, -GSL_IMAG(lambda));
          gsl_vector_complex_set(eval, iu - 1, lambda);
          gsl_vector_complex_set(eval, iu, lambda2);

          /*
           * First solve:
           *
           * [ T(i:i+1,i:i+1) - lambda*I ] * X = 0
           */

          if (fabs(gsl_matrix_get(T, iu - 1, iu)) >=
              fabs(gsl_matrix_get(T, iu, iu - 1)))
            {
              gsl_vector_set(w->work, iu - 1, 1.0);
              gsl_vector_set(w->work2, iu,
                             lambda_im / gsl_matrix_get(T, iu - 1, iu));
            }
          else
            {
              gsl_vector_set(w->work, iu - 1,
                             -lambda_im / gsl_matrix_get(T, iu, iu - 1));
              gsl_vector_set(w->work2, iu, 1.0);
            }
          gsl_vector_set(w->work, iu, 0.0);
          gsl_vector_set(w->work2, iu - 1, 0.0);

          /* construct right hand side */
          for (k = 0; k < iu - 1; ++k)
            {
              gsl_vector_set(w->work, k,
                             -gsl_vector_get(w->work, iu - 1) *
                             gsl_matrix_get(T, k, iu - 1));
              gsl_vector_set(w->work2, k,
                             -gsl_vector_get(w->work2, iu) *
                             gsl_matrix_get(T, k, iu));
            }

          /*
           * We must solve the upper quasi-triangular system:
           *
           * [ T(1:i-2,1:i-2) - lambda*I ] * X = s*(work + i*work2)
           */

          for (l = i - 2; l >= 0; --l)
            {
              size_t lu = (size_t) l;

              if (lu == 0)
                complex_pair = 0;
              else
                complex_pair = gsl_matrix_get(T, lu, lu - 1) != 0.0;

              if (!complex_pair)
                {
                  gsl_complex bval;
                  gsl_complex x;

                  /*
                   * 1-by-1 diagonal block - solve the system:
                   *
                   * (T_{ll} - lambda)*x = work + i*work2
                   */

                  Tv = gsl_matrix_submatrix(T, lu, lu, 1, 1);
                  bv = gsl_vector_complex_view_array(dat, 1);
                  xv = gsl_vector_complex_view_array(dat_X, 1);

                  GSL_SET_COMPLEX(&bval,
                                  gsl_vector_get(w->work, lu),
                                  gsl_vector_get(w->work2, lu));
                  gsl_vector_complex_set(&bv.vector, 0, bval);

                  gsl_schur_solve_equation_z(1.0,
                                             &Tv.matrix,
                                             &lambda,
                                             1.0,
                                             1.0,
                                             &bv.vector,
                                             &xv.vector,
                                             &scale,
                                             &xnorm,
                                             smin);

                  if (xnorm > 1.0)
                    {
                      if (gsl_vector_get(w->work3, lu) > bignum / xnorm)
                        {
                          gsl_blas_zdscal(1.0/xnorm, &xv.vector);
                          scale /= xnorm;
                        }
                    }

                  /* scale if necessary */
                  if (scale != 1.0)
                    {
                      gsl_vector_view wv;

                      wv = gsl_vector_subvector(w->work, 0, iu + 1);
                      gsl_blas_dscal(scale, &wv.vector);
                      wv = gsl_vector_subvector(w->work2, 0, iu + 1);
                      gsl_blas_dscal(scale, &wv.vector);
                    }

                  x = gsl_vector_complex_get(&xv.vector, 0);
                  gsl_vector_set(w->work, lu, GSL_REAL(x));
                  gsl_vector_set(w->work2, lu, GSL_IMAG(x));

                  /* update the right hand side */
                  if (lu > 0)
                    {
                      gsl_vector_view v1, v2;

                      v1 = gsl_matrix_subcolumn(T, lu, 0, lu);
                      v2 = gsl_vector_subvector(w->work, 0, lu);
                      gsl_blas_daxpy(-GSL_REAL(x), &v1.vector, &v2.vector);

                      v2 = gsl_vector_subvector(w->work2, 0, lu);
                      gsl_blas_daxpy(-GSL_IMAG(x), &v1.vector, &v2.vector);
                    } /* if (lu > 0) */
                } /* if (!complex_pair) */
              else
                {
                  gsl_complex b1, b2, x1, x2;

                  /*
                   * 2-by-2 diagonal block - solve the system
                   */

                  Tv = gsl_matrix_submatrix(T, lu - 1, lu - 1, 2, 2);
                  bv = gsl_vector_complex_view_array(dat, 2);
                  xv = gsl_vector_complex_view_array(dat_X, 2);

                  GSL_SET_COMPLEX(&b1,
                                  gsl_vector_get(w->work, lu - 1),
                                  gsl_vector_get(w->work2, lu - 1));
                  GSL_SET_COMPLEX(&b2,
                                  gsl_vector_get(w->work, lu),
                                  gsl_vector_get(w->work2, lu));
                  gsl_vector_complex_set(&bv.vector, 0, b1);
                  gsl_vector_complex_set(&bv.vector, 1, b2);

                  gsl_schur_solve_equation_z(1.0,
                                             &Tv.matrix,
                                             &lambda,
                                             1.0,
                                             1.0,
                                             &bv.vector,
                                             &xv.vector,
                                             &scale,
                                             &xnorm,
                                             smin);

                  x1 = gsl_vector_complex_get(&xv.vector, 0);
                  x2 = gsl_vector_complex_get(&xv.vector, 1);

                  if (xnorm > 1.0)
                    {
                      double beta;

                      beta = GSL_MAX(gsl_vector_get(w->work3, lu - 1),
                                     gsl_vector_get(w->work3, lu));
                      if (beta > bignum / xnorm)
                        {
                          gsl_blas_zdscal(1.0/xnorm, &xv.vector);
                          scale /= xnorm;
                        }
                    }

                  /* scale if necessary */
                  if (scale != 1.0)
                    {
                      gsl_vector_view wv;

                      wv = gsl_vector_subvector(w->work, 0, iu + 1);
                      gsl_blas_dscal(scale, &wv.vector);
                      wv = gsl_vector_subvector(w->work2, 0, iu + 1);
                      gsl_blas_dscal(scale, &wv.vector);
                    }
                  gsl_vector_set(w->work, lu - 1, GSL_REAL(x1));
                  gsl_vector_set(w->work, lu, GSL_REAL(x2));
                  gsl_vector_set(w->work2, lu - 1, GSL_IMAG(x1));
                  gsl_vector_set(w->work2, lu, GSL_IMAG(x2));

                  /* update right hand side */
                  if (lu > 1)
                    {
                      gsl_vector_view v1, v2, v3, v4;

                      v1 = gsl_matrix_subcolumn(T, lu - 1, 0, lu - 1);
                      v4 = gsl_matrix_subcolumn(T, lu, 0, lu - 1);
                      v2 = gsl_vector_subvector(w->work, 0, lu - 1);
                      v3 = gsl_vector_subvector(w->work2, 0, lu - 1);

                      gsl_blas_daxpy(-GSL_REAL(x1), &v1.vector, &v2.vector);
                      gsl_blas_daxpy(-GSL_REAL(x2), &v4.vector, &v2.vector);
                      gsl_blas_daxpy(-GSL_IMAG(x1), &v1.vector, &v3.vector);
                      gsl_blas_daxpy(-GSL_IMAG(x2), &v4.vector, &v3.vector);
                    } /* if (lu > 1) */

                  --l;
                } /* if (complex_pair) */
            } /* for (l = i - 2; l >= 0; --l) */

          /*
           * At this point, work + i*work2 is an eigenvector
           * of T - backtransform to get an eigenvector of the
           * original matrix
           */

          y = gsl_matrix_column(Z, iu - 1);
          y2 = gsl_matrix_column(Z, iu);

          if (iu > 1)
            {
              gsl_vector_view x;

              /* compute real part of eigenvectors */

              Zv = gsl_matrix_submatrix(Z, 0, 0, N, iu - 1);
              x = gsl_vector_subvector(w->work, 0, iu - 1);

              gsl_blas_dgemv(CblasNoTrans,
                             1.0,
                             &Zv.matrix,
                             &x.vector,
                             gsl_vector_get(w->work, iu - 1),
                             &y.vector);

              /* now compute the imaginary part */
              x = gsl_vector_subvector(w->work2, 0, iu - 1);

              gsl_blas_dgemv(CblasNoTrans,
                             1.0,
                             &Zv.matrix,
                             &x.vector,
                             gsl_vector_get(w->work2, iu),
                             &y2.vector);
            }
          else
            {
              gsl_blas_dscal(gsl_vector_get(w->work, iu - 1), &y.vector);
              gsl_blas_dscal(gsl_vector_get(w->work2, iu), &y2.vector);
            }

          /*
           * Now store the eigenvectors into evec - the real parts
           * are Z(:,iu - 1) and the imaginary parts are
           * +/- Z(:,iu)
           */

          /* get views of the two eigenvector slots */
          ecol = gsl_matrix_complex_column(evec, iu - 1);
          ecol2 = gsl_matrix_complex_column(evec, iu);

          /*
           * save imaginary part first as it may get overwritten
           * when copying the real part due to our storage scheme
           * in Z/evec
           */
          ev = gsl_vector_complex_imag(&ecol.vector);
          ev2 = gsl_vector_complex_imag(&ecol2.vector);
          scale = 0.0;
          for (ii = 0; ii < N; ++ii)
            {
              double a = gsl_vector_get(&y2.vector, ii);

              scale = GSL_MAX(scale,
                              fabs(a) + fabs(gsl_vector_get(&y.vector, ii)));

              gsl_vector_set(&ev.vector, ii, a);
              gsl_vector_set(&ev2.vector, ii, -a);
            }

          /* now save the real part */
          ev = gsl_vector_complex_real(&ecol.vector);
          ev2 = gsl_vector_complex_real(&ecol2.vector);
          for (ii = 0; ii < N; ++ii)
            {
              double a = gsl_vector_get(&y.vector, ii);

              gsl_vector_set(&ev.vector, ii, a);
              gsl_vector_set(&ev2.vector, ii, a);
            }

          if (scale != 0.0)
            scale = 1.0 / scale;

          /* scale by largest element magnitude */

          gsl_blas_zdscal(scale, &ecol.vector);
          gsl_blas_zdscal(scale, &ecol2.vector);

          /*
           * decrement i since we took care of two eigenvalues at
           * the same time
           */
          --i;
        } /* if (GSL_IMAG(lambda) != 0.0) */
    } /* for (i = (int) N - 1; i >= 0; --i) */
} /* nonsymmv_get_right_eigenvectors() */

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

Inputs: eval - eigenvalues
        evec - eigenvectors
*/

static void
nonsymmv_normalize_eigenvectors(gsl_vector_complex *eval,
                                gsl_matrix_complex *evec)
{
  const size_t N = evec->size1;
  size_t i;     /* looping */
  gsl_complex ei;
  gsl_vector_complex_view vi;
  gsl_vector_view re, im;
  double scale; /* scaling factor */

  for (i = 0; i < N; ++i)
    {
      ei = gsl_vector_complex_get(eval, i);
      vi = gsl_matrix_complex_column(evec, i);

      re = gsl_vector_complex_real(&vi.vector);

      if (GSL_IMAG(ei) == 0.0)
        {
          scale = 1.0 / gsl_blas_dnrm2(&re.vector);
          gsl_blas_dscal(scale, &re.vector);
        }
      else if (GSL_IMAG(ei) > 0.0)
        {
          im = gsl_vector_complex_imag(&vi.vector);

          scale = 1.0 / gsl_hypot(gsl_blas_dnrm2(&re.vector),
                                  gsl_blas_dnrm2(&im.vector));
          gsl_blas_zdscal(scale, &vi.vector);

          vi = gsl_matrix_complex_column(evec, i + 1);
          gsl_blas_zdscal(scale, &vi.vector);
        }
    }
} /* nonsymmv_normalize_eigenvectors() */


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

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_histogram.h>
#include <gsl/gsl_histogram2d.h>

#include "find.c"

int
gsl_histogram2d_pdf_sample (const gsl_histogram2d_pdf * p,
                            double r1, double r2,
                            double *x, double *y)
{
  size_t k;
  int status;

/* Wrap the exclusive top of the bin down to the inclusive bottom of
   the bin. Since this is a single point it should not affect the
   distribution. */

  if (r2 == 1.0)
    {
      r2 = 0.0;
    }
  if (r1 == 1.0)
    {
      r1 = 0.0;
    }

  status = find (p->nx * p->ny, p->sum, r1, &k);

  if (status)
    {
      GSL_ERROR ("cannot find r1 in cumulative pdf", GSL_EDOM);
    }
  else
    {
      size_t i = k / p->ny;
      size_t j = k - (i * p->ny);
      double delta = (r1 - p->sum[k]) / (p->sum[k + 1] - p->sum[k]);
      *x = p->xrange[i] + delta * (p->xrange[i + 1] - p->xrange[i]);
      *y = p->yrange[j] + r2 * (p->yrange[j + 1] - p->yrange[j]);
      return GSL_SUCCESS;
    }
}

gsl_histogram2d_pdf *
gsl_histogram2d_pdf_alloc (const size_t nx, const size_t ny)
{
  const size_t n = nx * ny;

  gsl_histogram2d_pdf *p;

  if (n == 0)
    {
      GSL_ERROR_VAL ("histogram2d pdf length n must be positive integer",
                        GSL_EDOM, 0);
    }

  p = (gsl_histogram2d_pdf *) malloc (sizeof (gsl_histogram2d_pdf));

  if (p == 0)
    {
      GSL_ERROR_VAL ("failed to allocate space for histogram2d pdf struct",
                        GSL_ENOMEM, 0);
    }

  p->xrange = (double *) malloc ((nx + 1) * sizeof (double));

  if (p->xrange == 0)
    {
      free (p);         /* exception in constructor, avoid memory leak */

      GSL_ERROR_VAL ("failed to allocate space for histogram2d pdf xranges",
                        GSL_ENOMEM, 0);
    }

  p->yrange = (double *) malloc ((ny + 1) * sizeof (double));

  if (p->yrange == 0)
    {
      free (p->xrange);
      free (p);         /* exception in constructor, avoid memory leak */

      GSL_ERROR_VAL ("failed to allocate space for histogram2d pdf yranges",
                        GSL_ENOMEM, 0);
    }

  p->sum = (double *) malloc ((n + 1) * sizeof (double));

  if (p->sum == 0)
    {
      free (p->yrange);
      free (p->xrange);
      free (p);         /* exception in constructor, avoid memory leak */

      GSL_ERROR_VAL ("failed to allocate space for histogram2d pdf sums",
                        GSL_ENOMEM, 0);
    }

  p->nx = nx;
  p->ny = ny;

  return p;
}

int
gsl_histogram2d_pdf_init (gsl_histogram2d_pdf * p, const gsl_histogram2d * h)
{
  size_t i;
  const size_t nx = p->nx;
  const size_t ny = p->ny;
  const size_t n = nx * ny;

  if (nx != h->nx || ny != h->ny)
    {
      GSL_ERROR ("histogram2d size must match pdf size", GSL_EDOM);
    }

  for (i = 0; i < n; i++)
    {
      if (h->bin[i] < 0)
        {
          GSL_ERROR ("histogram bins must be non-negative to compute"
                     "a probability distribution", GSL_EDOM);
        }
    }

  for (i = 0; i < nx + 1; i++)
    {
      p->xrange[i] = h->xrange[i];
    }

  for (i = 0; i < ny + 1; i++)
    {
      p->yrange[i] = h->yrange[i];
    }

  {
    double mean = 0, sum = 0;

    for (i = 0; i < n; i++)
      {
        mean += (h->bin[i] - mean) / ((double) (i + 1));
      }

    p->sum[0] = 0;

    for (i = 0; i < n; i++)
      {
        sum += (h->bin[i] / mean) / n;
        p->sum[i + 1] = sum;
      }
  }

  return GSL_SUCCESS;
}

void
gsl_histogram2d_pdf_free (gsl_histogram2d_pdf * p)
{
  RETURN_IF_NULL (p);
  free (p->xrange);
  free (p->yrange);
  free (p->sum);
  free (p);
}


/// \file energy.c
/// \brief Sort and fill energy levels to obtain occupancy of each level.
//
//	Copyright (c) 2014, Christian B. Mendl
//	All rights reserved.
//	http://christian.mendl.net
//
//	This program is free software; you can redistribute it and/or
//	modify it under the terms of the Simplified BSD License
//	http://www.opensource.org/licenses/bsd-license.php
//
//	Reference:
//	  Christian B. Mendl, Francesc Malet, Paola Gori-Giorgi
//	  Wigner localization in quantum dots from Kohn-Sham density functional theory without symmetry breaking
//	  Physical Review B 89, 125106 (2014)
//	  (preprint http://arxiv.org/abs/1311.6011)
//_______________________________________________________________________________________________________________________
//

#include "energy.h"
#include <stdlib.h>
#include <assert.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_errno.h>

/// \brief Allocate memory for energy levels structure
void AllocateEnergyLevels(const int m_max, const int num, en_levels_t *levels)
{
	levels->m_max = m_max;
	levels->num   = num;

	levels->en = malloc((m_max+1)*num * sizeof(double));
}

/// \brief Free memory of energy levels structure
void FreeEnergyLevels(en_levels_t *levels)
{
	free(levels->en);

	levels->num   = 0;
	levels->m_max = 0;
}

//_______________________________________________________________________________________________________________________
//

/// \brief Comparison function used for sorting
static int en_occupancy_cmp(const void *e1, const void *e2)
{
	const en_occupancy_t *a = (en_occupancy_t *)e1;
	const en_occupancy_t *b = (en_occupancy_t *)e2;

	// sort according to energy
	if (a->en < b->en)
	{
		return -1;
	}
	else if (a->en > b->en)
	{
		return 1;
	}

	// should actually never reach this point

	// compare 'm' quantum numbers
	if (a->m < b->m)
	{
		return -1;
	}
	else if (a->m > b->m)
	{
		return 1;
	}

	// compare energy quantum numbers
	if (a->n < b->n)
	{
		return -1;
	}
	else if (a->n > b->n)
	{
		return 1;
	}

	return 0;
}

/// \brief Get the maximum occupancy of an energy level, depending on 'm' quantum number and whether state is spin-polarized
static inline int GetMaxOccupancy(const int m, const bool spinpol)
{
	int occ = 1;

	if (m != 0)
	{
		occ *= 2;
	}
	if (!spinpol)
	{
		occ *= 2;
	}

	return occ;
}

//_______________________________________________________________________________________________________________________
//

/// \brief Fermi-Dirac distribution function
static double FermiDirac(const double kBT, const double mu, const double en)
{
	return 1 / (exp((en - mu)/kBT) + 1);
}

/// \brief GSL parameters for NumParticleDifference()
typedef struct
{
	int nelec;			//!< physical number of electrons
	int nlevels;		//!< number of energy levels
	double *en;			//!< energies
	int *occ;			//!< maximum occupancy of each energy level
	double kBT;			//!< k_B T (Boltzmann constant times temperature)
}
gsl_FD_params_t;

/// \brief Calculate difference in number of particles given a Fermi-Dirac distribution with chemical potential 'mu'
static double NumParticleDifference(double mu, void *p)
{
	int i;

	const gsl_FD_params_t *params = (gsl_FD_params_t *)p;

	double N = 0;
	for (i = 0; i < params->nlevels; i++)
	{
		N += params->occ[i] * FermiDirac(params->kBT, mu, params->en[i]);
	}

	return N - params->nelec;
}

//_______________________________________________________________________________________________________________________
///
/// \brief Calculate chemical potential 'mu' such that FermiDirac distribution gives exactly 'nelec' electrons
///
static double CalculateChemicalPotential(const en_levels_t *levels, const int nelec, const bool spinpol, const double kBT)
{
	int m, n, i;

	const int maxiter = 128;

	gsl_FD_params_t params;
	params.nelec = nelec;
	params.nlevels = (levels->m_max+1)*levels->num;
	params.en  = malloc(params.nlevels * sizeof(double));
	params.occ = malloc(params.nlevels * sizeof(int));
	params.kBT = kBT;
	// fill energy levels and occupancies
	for (m = 0; m <= levels->m_max; m++)
	{
		for (n = 0; n < levels->num; n++)
		{
			params.en [n + m*levels->num] = levels->en[n + m*levels->num];
			params.occ[n + m*levels->num] = GetMaxOccupancy(m, spinpol);
		}
	}

	// first interval used in interval bisection method
	double mu_lo = 0.0;
	double mu_hi = levels->en[(levels->m_max+1)*levels->num-1];		// use "last" energy level as upper bound

	gsl_function F;
	F.function = &NumParticleDifference;
	F.params = &params;

	const gsl_root_fsolver_type *T = gsl_root_fsolver_brent;
	gsl_root_fsolver *s = gsl_root_fsolver_alloc(T);
	gsl_root_fsolver_set(s, &F, mu_lo, mu_hi);

	double mu = 0;
	int status = GSL_CONTINUE;
	for (i = 0; i < maxiter && status == GSL_CONTINUE; i++)
	{
		status = gsl_root_fsolver_iterate(s);
		if (status != GSL_SUCCESS) {
			fprintf(stderr, "CalculateOccupancy() warning: gsl_root_fsolver_iterate() returned with status code %i\n", status);
		}

		mu = gsl_root_fsolver_root(s);
		mu_lo  = gsl_root_fsolver_x_lower(s);
		mu_hi  = gsl_root_fsolver_x_upper(s);
		status = gsl_root_test_interval(mu_lo, mu_hi, 0, 1e-8);
	}

	if (status != GSL_SUCCESS) {
		fprintf(stderr, "CalculateOccupancy() warning: not converged after %i iterations, status: %i\n", maxiter, status);
	}

	// clean up
	free(params.occ);
	free(params.en);
	gsl_root_fsolver_free(s);

	return mu;
}

//_______________________________________________________________________________________________________________________
///
/// \brief Calculate occupancy of energy levels
///
void CalculateOccupancy(const en_levels_t *levels, const int nelec, const bool spinpol, const double kBT, en_occlist_t *occlist)
{
	int i, m, n;

	// accumulate all energies into one list
	occlist->occupancy = malloc((levels->m_max+1)*levels->num * sizeof(en_occupancy_t));

	if (kBT == 0)	// zero temperature
	{
		for (m = 0; m <= levels->m_max; m++)
		{
			for (n = 0; n < levels->num; n++)
			{
				en_occupancy_t *encur = &occlist->occupancy[n + m*levels->num];
				encur->en  = levels->en[n + m*levels->num];
				encur->m   = m;
				encur->n   = n;
				encur->occ = 0;		// initially set to zero
			}
		}

		// sort energies
		qsort(occlist->occupancy, (levels->m_max+1)*levels->num, sizeof(en_occupancy_t), en_occupancy_cmp);

		// fill up energy levels
		en_occupancy_t *encur = occlist->occupancy;
		occlist->length = 1;
		for (i = 0; i < nelec; i++)
		{
			if (encur->occ >= GetMaxOccupancy(encur->m, spinpol)) {
				encur++;	// use next level
				occlist->length++;
			}

			encur->occ++;
		}
	}
	else	// k_B T > 0
	{
		assert(kBT > 0);

		const double mu = CalculateChemicalPotential(levels, nelec, spinpol, kBT);

		// all energy levels have nonzero occupancy since Fermi-Dirac function is strictly positive
		occlist->length = (levels->m_max+1)*levels->num;
		for (m = 0; m <= levels->m_max; m++)
		{
			for (n = 0; n < levels->num; n++)
			{
				en_occupancy_t *encur = &occlist->occupancy[n + m*levels->num];
				encur->en  = levels->en[n + m*levels->num];
				encur->m   = m;
				encur->n   = n;
				encur->occ = GetMaxOccupancy(m, spinpol) * FermiDirac(kBT, mu, encur->en);
			}
		}

		// sort energies
		qsort(occlist->occupancy, (levels->m_max+1)*levels->num, sizeof(en_occupancy_t), en_occupancy_cmp);
	}
}

/// \brief Free memory of energy level occupancy list
void FreeOccupancy(en_occlist_t *occlist)
{
	free(occlist->occupancy);
	occlist->length = 0;
}


/* linalg/ptlq.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Gerard Jungman, Brian Gough
 * Copyright (C) 2004 Joerg Wensch, modifications for LQ.
 *
 * 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 <string.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_linalg.h>

#include "apply_givens.c"

/* The purpose of this package is to speed up QR-decomposition for
   large matrices.  Because QR-decomposition is column oriented, but
   GSL uses a row-oriented matrix format, there can considerable
   speedup obtained by computing the LQ-decomposition of the
   transposed matrix instead.  This package provides LQ-decomposition
   and related algorithms.  */

/* Factorise a general N x M matrix A into
 *
 *   P A = L Q
 *
 * where Q is orthogonal (M x M) and L is lower triangular (N x M).
 * When A is rank deficient, r = rank(A) < n, then the permutation is
 * used to ensure that the lower n - r columns of L are zero and the first
 * l rows of Q form an orthonormal basis for the rows of A.
 *
 * Q is stored as a packed set of Householder transformations in the
 * strict upper triangular part of the input matrix.
 *
 * L is stored in the diagonal and lower triangle of the input matrix.
 *
 * P: column j of P is column k of the identity matrix, where k =
 * permutation->data[j]
 *
 * The full matrix for Q can be obtained as the product
 *
 *       Q = Q_k .. Q_2 Q_1
 *
 * where k = MIN(M,N) and
 *
 *       Q_i = (I - tau_i * v_i * v_i')
 *
 * and where v_i is a Householder vector
 *
 *       v_i = [1, m(i,i+1), m(i,i+2), ... , m(i,M)]
 *
 * This storage scheme is the same as in LAPACK.  See LAPACK's
 * dgeqpf.f for details.
 *
 */

int
gsl_linalg_PTLQ_decomp (gsl_matrix * A, gsl_vector * tau, gsl_permutation * p, int *signum, gsl_vector * norm)
{
  const size_t N = A->size1;
  const size_t M = A->size2;

  if (tau->size != GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (p->size != N)
    {
      GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
    }
  else if (norm->size != N)
    {
      GSL_ERROR ("norm size must be N", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      *signum = 1;

      gsl_permutation_init (p); /* set to identity */

      /* Compute column norms and store in workspace */

      for (i = 0; i < N; i++)
        {
          gsl_vector_view c = gsl_matrix_row (A, i);
          double x = gsl_blas_dnrm2 (&c.vector);
          gsl_vector_set (norm, i, x);
        }

      for (i = 0; i < GSL_MIN (M, N); i++)
        {
          /* Bring the column of largest norm into the pivot position */

          double max_norm = gsl_vector_get(norm, i);
          size_t j, kmax = i;

          for (j = i + 1; j < N; j++)
            {
              double x = gsl_vector_get (norm, j);

              if (x > max_norm)
                {
                  max_norm = x;
                  kmax = j;
                }
            }

          if (kmax != i)
            {
              gsl_matrix_swap_rows (A, i, kmax);
              gsl_permutation_swap (p, i, kmax);
              gsl_vector_swap_elements(norm,i,kmax);

              (*signum) = -(*signum);
            }

          /* Compute the Householder transformation to reduce the j-th
             column of the matrix to a multiple of the j-th unit vector */

          {
            gsl_vector_view c = gsl_matrix_subrow (A, i, i, M - i);
            double tau_i = gsl_linalg_householder_transform (&c.vector);

            gsl_vector_set (tau, i, tau_i);

            /* Apply the transformation to the remaining columns */

            if (i + 1 < N)
              {
                gsl_matrix_view m = gsl_matrix_submatrix (A, i +1, i, N - (i+1), M - i);
                gsl_linalg_householder_mh (tau_i, &c.vector, &m.matrix);
              }
          }

          /* Update the norms of the remaining columns too */

          if (i + 1 < M)
            {
              for (j = i + 1; j < N; j++)
                {
                  double x = gsl_vector_get (norm, j);

                  if (x > 0.0)
                    {
                      double y = 0;
                      double temp= gsl_matrix_get (A, j, i) / x;

                      if (fabs (temp) >= 1)
                        y = 0.0;
                      else
                        y = x * sqrt (1 - temp * temp);

                      /* recompute norm to prevent loss of accuracy */

                      if (fabs (y / x) < sqrt (20.0) * GSL_SQRT_DBL_EPSILON)
                        {
                          gsl_vector_view c = gsl_matrix_subrow (A, j, i + 1, M - (i + 1));
                          y = gsl_blas_dnrm2 (&c.vector);
                        }

                      gsl_vector_set (norm, j, y);
                    }
                }
            }
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_PTLQ_decomp2 (const gsl_matrix * A, gsl_matrix * q, gsl_matrix * r, gsl_vector * tau, gsl_permutation * p, int *signum, gsl_vector * norm)
{
  const size_t N = A->size1;
  const size_t M = A->size2;

  if (q->size1 != M || q->size2 !=M)
    {
      GSL_ERROR ("q must be M x M", GSL_EBADLEN);
    }
  else if (r->size1 != N || r->size2 !=M)
    {
      GSL_ERROR ("r must be N x M", GSL_EBADLEN);
    }
  else if (tau->size != GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (p->size != N)
    {
      GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
    }
  else if (norm->size != N)
    {
      GSL_ERROR ("norm size must be N", GSL_EBADLEN);
    }

  gsl_matrix_memcpy (r, A);

  gsl_linalg_PTLQ_decomp (r, tau, p, signum, norm);

  /* FIXME:  aliased arguments depends on behavior of unpack routine! */

  gsl_linalg_LQ_unpack (r, tau, q, r);

  return GSL_SUCCESS;
}

/* Solves the system x^T A = b^T using the P^T L Q  factorisation,

   z^T L = b^T Q^T

   x = P z;

   to obtain x. Based on SLATEC code. */

int
gsl_linalg_PTLQ_solve_T (const gsl_matrix * QR,
                         const gsl_vector * tau,
                         const gsl_permutation * p,
                         const gsl_vector * b,
                         gsl_vector * x)
{
  if (QR->size1 != QR->size2)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (QR->size2 != p->size)
    {
      GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
    }
  else if (QR->size2 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (QR->size1 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      gsl_vector_memcpy (x, b);

      gsl_linalg_PTLQ_svx_T (QR, tau, p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_PTLQ_svx_T (const gsl_matrix * LQ,
                       const gsl_vector * tau,
                       const gsl_permutation * p,
                       gsl_vector * x)
{
  if (LQ->size1 != LQ->size2)
    {
      GSL_ERROR ("LQ matrix must be square", GSL_ENOTSQR);
    }
  else if (LQ->size2 != p->size)
    {
      GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
    }
  else if (LQ->size1 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* compute sol = b^T Q^T */

      gsl_linalg_LQ_vecQT (LQ, tau, x);

      /* Solve  L^T x = sol, storing x inplace in sol */

      gsl_blas_dtrsv (CblasLower, CblasTrans, CblasNonUnit, LQ, x);

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_PTLQ_LQsolve_T (const gsl_matrix * Q, const gsl_matrix * L,
                           const gsl_permutation * p,
                           const gsl_vector * b,
                           gsl_vector * x)
{
  if (Q->size1 != Q->size2 || L->size1 != L->size2)
    {
      return GSL_ENOTSQR;
    }
  else if (Q->size1 != p->size || Q->size1 != L->size1
           || Q->size1 != b->size)
    {
      return GSL_EBADLEN;
    }
  else
    {
      /* compute b' = Q b */

      gsl_blas_dgemv (CblasNoTrans, 1.0, Q, b, 0.0, x);

      /* Solve L^T x = b', storing x inplace */

      gsl_blas_dtrsv (CblasLower, CblasTrans, CblasNonUnit, L, x);

      /* Apply permutation to solution in place */

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_PTLQ_Lsolve_T (const gsl_matrix * LQ,
                        const gsl_permutation * p,
                        const gsl_vector * b,
                        gsl_vector * x)
{
  if (LQ->size1 != LQ->size2)
    {
      GSL_ERROR ("LQ matrix must be square", GSL_ENOTSQR);
    }
  else if (LQ->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (LQ->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match x size", GSL_EBADLEN);
    }
  else if (p->size != x->size)
    {
      GSL_ERROR ("permutation size must match x size", GSL_EBADLEN);
    }
  else
    {
      /* Copy x <- b */

      gsl_vector_memcpy (x, b);

      /* Solve L^T x = b, storing x inplace */

      gsl_blas_dtrsv (CblasLower, CblasTrans, CblasNonUnit, LQ, x);

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_PTLQ_Lsvx_T (const gsl_matrix * LQ,
                        const gsl_permutation * p,
                        gsl_vector * x)
{
  if (LQ->size1 != LQ->size2)
    {
      GSL_ERROR ("LQ matrix must be square", GSL_ENOTSQR);
    }
  else if (LQ->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match x size", GSL_EBADLEN);
    }
  else if (p->size != x->size)
    {
      GSL_ERROR ("permutation size must match x size", GSL_EBADLEN);
    }
  else
    {
      /* Solve L^T x = b, storing x inplace */

      gsl_blas_dtrsv (CblasLower, CblasTrans, CblasNonUnit, LQ, x);

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

/* Update a P^T L Q factorisation for P A= L Q ,  A' =  A +  v u^T,
                                                 PA' = PA + Pv u^T

 * P^T L' Q' = P^T LQ + v u^T
 *       = P^T (L + (P v) u^T Q^T) Q
 *       = P^T (L + (P v) w^T) Q
 *
 * where w = Q^T u.
 *
 * Algorithm from Golub and Van Loan, "Matrix Computations", Section
 * 12.5 (Updating Matrix Factorizations, Rank-One Changes)
 */

int
gsl_linalg_PTLQ_update (gsl_matrix * Q, gsl_matrix * L,
                        const gsl_permutation * p,
                        const gsl_vector * v, gsl_vector * w)
{
  if (Q->size1 != Q->size2 || L->size1 != L->size2)
    {
      return GSL_ENOTSQR;
    }
  else if (L->size1 != Q->size2 || v->size != Q->size2 || w->size != Q->size2)
    {
      return GSL_EBADLEN;
    }
  else
    {
      size_t j, k;
      const size_t N = Q->size1;
      const size_t M = Q->size2;
      double w0;

      /* Apply Given's rotations to reduce w to (|w|, 0, 0, ... , 0)

         J_1^T .... J_(n-1)^T w = +/- |w| e_1

         simultaneously applied to L,  H = J_1^T ... J^T_(n-1) L
         so that H is upper Hessenberg.  (12.5.2) */

      for (k = M - 1; k > 0; k--)
        {
          double c, s;
          double wk = gsl_vector_get (w, k);
          double wkm1 = gsl_vector_get (w, k - 1);

          gsl_linalg_givens (wkm1, wk, &c, &s);
          gsl_linalg_givens_gv (w, k - 1, k, c, s);
          apply_givens_lq (M, N, Q, L, k - 1, k, c, s);
        }

      w0 = gsl_vector_get (w, 0);

      /* Add in v w^T  (Equation 12.5.3) */

      for (j = 0; j < N; j++)
        {
          double lj0 = gsl_matrix_get (L, j, 0);
          size_t p_j = gsl_permutation_get (p, j);
          double vj = gsl_vector_get (v, p_j);
          gsl_matrix_set (L, j, 0, lj0 + w0 * vj);
        }

      /* Apply Givens transformations L' = G_(n-1)^T ... G_1^T H
         Equation 12.5.4 */

      for (k = 1; k < N; k++)
        {
          double c, s;
          double diag = gsl_matrix_get (L, k - 1, k - 1);
          double offdiag = gsl_matrix_get (L, k - 1, k );

          gsl_linalg_givens (diag, offdiag, &c, &s);
          apply_givens_lq (M, N, Q, L, k - 1, k, c, s);
        }

      return GSL_SUCCESS;
    }
}


#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <float.h>
#include <string.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_randist.h>

#include "allvars.h"
#include "proto.h"

void mcmc_conline_run()
{
  double aicc, chi2, aicc_best;
  double *theta_best_this, *theta_best_var_this;

  char fname_mcmc[100];

  aicc_best = DBL_MAX;
  theta_best_this = malloc(ntheta_max*sizeof(double));
  theta_best_var_this = malloc(ntheta_max*2*sizeof(double));

  printf("*******con-line mcmc\n");
  if(flag_mcmc==1)
  {
    fprintf(fp_results, "************AICC************\n");
    for(nc = nc_lim_low; nc<=nc_lim_up; nc++)
    {
      printf("nc = %d\n", nc);
      sprintf(fname_mcmc, "data/mcmc_%02d.txt", nc);
      mcmc_conline_init();
      mcmc_sampling(fname_mcmc, &probability_conline);
      mcmc_stats(fname_mcmc);
      aicc = cal_aicc();
      if(aicc < aicc_best)
      {
        aicc_best = aicc;
        nc_best = nc;
        memcpy(theta_best_this, theta_best, ntheta * sizeof(double));
        memcpy(theta_best_var_this, theta_best_var, ntheta * 2 * sizeof(double));
      }
      printf("aicc: %d %f\n", nc, aicc);
      fprintf(fp_results, "aicc: %d %f\n", nc, aicc);
    }
  }
  else
  {
    printf("reading par.txt\n");
    nc = nc_best = nc_lim_low;
    sprintf(fname_mcmc, "data/mcmc_%02d.txt", nc);
    mcmc_conline_init();
    read_input();
    memcpy(theta_best_this, theta_best, ntheta * sizeof(double));
    memcpy(theta_best_var_this, theta_best_var, ntheta * 2 * sizeof(double));
  }
  printf("*******finish mcmc\n");

  fprintf(fp_results, "************Best Estimate************\n");
  fprintf(fp_results, "best nc: %d\n", nc_best);
  printf("best nc: %d\n", nc_best);

  nc = nc_best;
  memcpy(theta_best, theta_best_this, ntheta * sizeof(double));
  memcpy(theta_best_var, theta_best_var_this, ntheta * 2 * sizeof(double));
  mcmc_conline_init(); // reset the grid of tau

  reconstruct_conline();
  transfer_function(theta_best);
  aicc = cal_aicc();
  chi2 = chi_square();
  line_convolution();
  fprintf(fp_results, "aicc: %f\n", aicc);
  fprintf(fp_results, "chi2: %f\n", chi2);

  free(theta_best_this);
  free(theta_best_var_this);
}

void mcmc_conline_init()
{
  int i, j;

#ifdef JAVELIN  // only one-tophat is used in JAVELIN
  ntheta = 2 + 3 + 1;

  theta_range[0][0] = log(1.0e-6);  // sigma in DRW
  theta_range[0][1] = log(10.0);    //

  theta_range[1][0] = log(1.0e-2);  // tau in DRW
  theta_range[1][1] = log(1.0e5);

  theta_range[2][0] = (tau_lim_up - tau_lim_low)/1000.0;  // width of tophat
  theta_range[2][1] = (tau_lim_up - tau_lim_low)*10.0;

  theta_range[3][0] = 0.0;       // height of tophat
  theta_range[3][1] = 1.0e3;

  theta_range[4][0] = tau_lim_low;  //central value of tophat
  theta_range[4][1] = tau_lim_up;

  theta_range[5][0] = log(1.0e-5);  // systematic error
  theta_range[5][1] = log(1.0e6);

  i = 0;
  sigma_input[i++] = 0.01; // sigma
  sigma_input[i++] = 0.01; // taud
  sigma_input[i++] = 0.01; // width
  sigma_input[i++] = 0.1;  // fk
  sigma_input[i++] = 0.1;  // tauc
  sigma_input[i++] = 0.1;

  theta_input[0] = theta_best[0];
  theta_input[1] = theta_best[1];
  theta_input[2] = (tau_lim_up-tau_lim_low)/5.0;
  theta_input[3] = 1.0;
  theta_input[4] = (tau_lim_up-tau_lim_low)/2.0;
  theta_input[5] = log(10.0);

#else

  ntheta = nc + 3 + 1;
// grid of time lag
  for(i=0; i<nc; i++)
  {
    grid_tau[i] = (tau_lim_up-tau_lim_low)/(nc-1.0) * i + tau_lim_low;
  }

// limit range for parameters
  i=0;
  theta_range[i][0] = log(1.0e-6);
  theta_range[i++][1] = log(10.0);

  theta_range[i][0] = log(1.0);
  theta_range[i++][1] = log(1.0e4);

  theta_range[i][0] = (tau_lim_up - tau_lim_low)/(nc-1.0)/2.0;
  theta_range[i++][1] = (tau_lim_up - tau_lim_low)/(nc-1.0);
  for(j=0; j<nc; j++)
  {
    theta_range[i][0] = log(1.0e-10);
    theta_range[i++][1] = log(100.0);
  }

  theta_range[i][0] = log(1.0e-5);
  theta_range[i++][1] = log(1.0e6);

/* input step sizes for mcmc sampling */
  i = 0;
  sigma_input[i++] = 0.01; // sigma
  sigma_input[i++] = 0.01; // taud
  sigma_input[i++] = 0.01; // width
  for(j=0; j<nc; j++)
  {
    sigma_input[i++] = 0.1;  // fk
  }
  sigma_input[i++] = 0.1;    // systematic error

/* input values for parameters */
  theta_input[0] = theta_best[0];
  theta_input[1] = theta_best[1];
  theta_input[2] = (tau_lim_up-tau_lim_low)/(nc-1.0)/2.0;
  for(i=0; i<nc; i++)
  {
    theta_input[3+i] = log(1.0/nc);
  }
  theta_input[3 + nc] = log(10.0);

#endif

/* set if the parameters are fixed */
  for(i=0; i<ntheta; i++)
  {
    theta_fixed[i] = 0;
  }

/* for TOPHAT case, the width is fixed */
#ifdef TOPHAT
  theta_fixed[2] = 1;
#endif

}

/* theta represets sigma, tuad, and the parameters for transfer function */
double probability_conline(double *theta)
{
  double prob, prior, lndet_C, lndet_ICq, sigma, taud;
  double *Larr, *ybuf, *Cq, *ICq, *ave, *ysub, *yrec, *yrec_err, *yave;
  int i, nq, info, sign_C, sign_Cq;

  taud = exp(theta[1]);
  sigma = exp(theta[0]) * sqrt(taud/2.0);

  nq = (flag_detrend + 1)*2;
  Larr = workspace;
  ybuf = Larr + nq*nall_data;
  Cq = ybuf + nall_data;
  ICq = Cq + nq*nq;
  ave = ICq + nq*nq;

  ysub = ave + nq;
  yrec = ysub + nall_data;
  yrec_err = yrec + nall_data;
  yave = yrec_err + nall_data;

  set_covar_mat(theta);

  for(i=0; i<nall_data*nall_data; i++)
  {
    Cmat[i] = Smat[i] + Nmat[i];
  }

  for(i=0; i<ncon_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[i*nq+0] = 1.0;
      Larr[i*nq+1] = 0.0;
    }
    else
    {
      Larr[i*nq + 0] = 1.0;
      Larr[i*nq + 1] = Tcon_data[i];
      Larr[i*nq + 2] = 0.0;
      Larr[i*nq + 3] = 0.0;
    }
  }
  for(i=0; i<nline_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[(i+ncon_data)*nq+0] = 0.0;
      Larr[(i+ncon_data)*nq+1] = 1.0;
    }
    else
    {
      Larr[(i+ncon_data)*nq + 0] = 0.0;
      Larr[(i+ncon_data)*nq + 1] = 0.0;
      Larr[(i+ncon_data)*nq + 2] = 1.0;
      Larr[(i+ncon_data)*nq + 3] = Tline_data[i];
    }
  }

// cal q
  memcpy(Tmat1, Cmat, nall_data*nall_data*sizeof(double));
  memcpy(Tmat2, Larr, nall_data*nq*sizeof(double));
  //printf("FFFF\n");
  //display_mat(Tmat1, 1, nall_data);
  multiply_mat_MN_inverseA(Tmat1, Tmat2, nall_data, nq); // Tmat2 = C^-1 * L

  multiply_mat_MN_transposeA(Larr, Tmat2, ICq, nq, nq, nall_data); // ICq = L^T*C^-1*L
  multiply_mat_MN_transposeA(Tmat2, Fall_data, ave, nq, 1, nall_data); // ave = L^T*C^-1*y
  memcpy(Tmat1, ICq, nq*nq*sizeof(double));
  multiply_mat_MN_inverseA(Tmat1, ave, nq, 1); // (L^T*C^-1*L)^-1 * L^T*C^-1*y

  multiply_mat_MN(Larr, ave, yave, nall_data, 1, nq);
  for(i=0; i<nall_data; i++)ysub[i] = Fall_data[i] - yave[i];
  memcpy(Tmat1, Cmat, nall_data*nall_data*sizeof(double));
  memcpy(ybuf, ysub, nall_data*sizeof(double));
  multiply_mat_MN_inverseA(Tmat1, ybuf, nall_data, 1);

  prob = -0.5*cblas_ddot(nall_data, ysub, 1, ybuf, 1) / (sigma*sigma);
  //printf("%f\n", prob);
  if(prob > 0.0 )  // check if prob is positive
  {
    prob = -1.0e10;
    printf("prob >0!\n");
    return prob;
  }

  //memcpy(Tmat1, Cmat, n_data*nall_data*sizeof(double));
  lndet_C = lndet_mat3(Cmat, nall_data, &info, &sign_C) + 2.0*nall_data * log(sigma);
  if(info!=0|| sign_C==-1)
  {
    prob = -1.0e10;
    printf("lndet_C %f %d!\n", lndet_C, sign_C);
    return prob;
  }

  //memcpy(Tmat1, ICq, nq*nq*sizeof(double));
  lndet_ICq = lndet_mat3(ICq, nq, &info, &sign_Cq) - 2.0*nq*log(sigma);
  if(info!=0 || sign_Cq==-1 )
  {
    prob = -1.0e10;
    printf("lndet_ICq!\n");
    return prob;
  }

  prob -= 0.5*(lndet_C + lndet_ICq);

  prior = 0.0;
// for sigmad
  if( theta[0] < theta_best_con[0])
  {
    prior += -0.5*pow(theta[0] - theta_best_con[0], 2.0)/pow(theta_best_var_con[0*2], 2.0);
  }
  else
  {
    prior += -0.5*pow(theta[0] - theta_best_con[0], 2.0)/pow(theta_best_var_con[0*2+1], 2.0);
  }
// for taud
  if( theta[1] < theta_best_con[1])
  {
    prior += -0.5*pow(theta[1] - theta_best_con[1], 2.0)/pow(theta_best_var_con[1*2], 2.0);
  }
  else
  {
    prior += -0.5*pow(theta[1] - theta_best_con[1], 2.0)/pow(theta_best_var_con[1*2+1], 2.0);
  }
//penalize very large tau or very small tau
/*  if(theta[1] > log(len_con) )
  {
    prior += (log(len_con) - theta[1])/fabs(log(cad_con));
  }
  if(theta[1] < log(cad_con))
  {
    prior += (theta[1] - log(cad_con) ) / fabs(log(cad_con));
  }*/

  prob += prior;
  return prob;
}

double cal_aicc()
{
  double prob, aic, aicc;
  int k, n;

  k =  nc + 4;
  n = nall_data;

  prob = probability_conline_aicc(theta_best);

  aic = 2.0*k - 2.0*prob;

  aicc = aic + 2.0*k*(k+1.0)/(n - k - 1.0);

  //printf("aicc: %d %f %f\n", nc, aicc, prob);
  return aicc;
}

double probability_conline_aicc(double *theta)
{
  double prob,  lndet_C, lndet_ICq, sigma, taud;
  double *Larr, *ybuf, *Cq, *ICq, *ave, *ysub, *yrec, *yrec_err, *yave;
  int i, nq, info, sign_C, sign_Cq;

  taud = exp(theta[1]);
  sigma = exp(theta[0]) * sqrt(taud/2.0);

  nq = (flag_detrend + 1)*2;
  Larr = workspace;
  ybuf = Larr + nq*nall_data;
  Cq = ybuf + nall_data;
  ICq = Cq + nq*nq;
  ave = ICq + nq*nq;

  ysub = ave + nq;
  yrec = ysub + nall_data;
  yrec_err = yrec + nall_data;
  yave = yrec_err + nall_data;

  set_covar_mat(theta);

  for(i=0; i<nall_data*nall_data; i++)
  {
    Cmat[i] = Smat[i] + Nmat[i];
  }

  for(i=0; i<ncon_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[i*nq+0] = 1.0;
      Larr[i*nq+1] = 0.0;
    }
    else
    {
      Larr[i*nq + 0] = 1.0;
      Larr[i*nq + 1] = Tcon_data[i];
      Larr[i*nq + 2] = 0.0;
      Larr[i*nq + 3] = 0.0;
    }
  }
  for(i=0; i<nline_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[(i+ncon_data)*nq+0] = 0.0;
      Larr[(i+ncon_data)*nq+1] = 1.0;
    }
    else
    {
      Larr[(i+ncon_data)*nq + 0] = 0.0;
      Larr[(i+ncon_data)*nq + 1] = 0.0;
      Larr[(i+ncon_data)*nq + 2] = 1.0;
      Larr[(i+ncon_data)*nq + 3] = Tline_data[i];
    }
  }

// cal q
  memcpy(Tmat1, Cmat, nall_data*nall_data*sizeof(double));
  memcpy(Tmat2, Larr, nall_data*nq*sizeof(double));
  //printf("FFFF\n");
  //display_mat(Tmat1, 1, nall_data);
  multiply_mat_MN_inverseA(Tmat1, Tmat2, nall_data, nq); // Tmat2 = C^-1 * L

  multiply_mat_MN_transposeA(Larr, Tmat2, ICq, nq, nq, nall_data); // ICq = L^T*C^-1*L
  multiply_mat_MN_transposeA(Tmat2, Fall_data, ave, nq, 1, nall_data); // ave = L^T*C^-1*y
  memcpy(Tmat1, ICq, nq*nq*sizeof(double));
  multiply_mat_MN_inverseA(Tmat1, ave, nq, 1); // (L^T*C^-1*L)^-1 * L^T*C^-1*y

  multiply_mat_MN(Larr, ave, yave, nall_data, 1, nq);
  for(i=0; i<nall_data; i++)ysub[i] = Fall_data[i] - yave[i];
  memcpy(Tmat1, Cmat, nall_data*nall_data*sizeof(double));
  memcpy(ybuf, ysub, nall_data*sizeof(double));
  multiply_mat_MN_inverseA(Tmat1, ybuf, nall_data, 1);

  prob = -0.5*cblas_ddot(nall_data, ysub, 1, ybuf, 1) / (sigma*sigma);
  //printf("%f\n", prob);
  if(prob > 0.0 )  // check if prob is positive
  {
    prob = -1.0e10;
    printf("prob >0!\n");
    return prob;
  }

  //memcpy(Tmat1, Cmat, n_data*nall_data*sizeof(double));
  lndet_C = lndet_mat3(Cmat, nall_data, &info, &sign_C) + 2.0*nall_data * log(sigma);
  if(info!=0|| sign_C==-1)
  {
    prob = -1.0e10;
    printf("lndet_C %f %d!\n", lndet_C, sign_C);
    return prob;
  }

  //memcpy(Tmat1, ICq, nq*nq*sizeof(double));
  lndet_ICq = lndet_mat3(ICq, nq, &info, &sign_Cq) - 2.0*nq*log(sigma);
  if(info!=0 || sign_Cq==-1 )
  {
    prob = -1.0e10;
    printf("lndet_ICq!\n");
    return prob;
  }

  prob -= 0.5*(lndet_C + lndet_ICq);

  return prob;
}

double chi_square()
{
  double chi2, sigma, taud;
  double *Larr, *ybuf, *Cq, *ICq, *ave, *ysub, *yrec, *yrec_err, *yave;
  int i, nq, info;

  taud = exp(theta_best[1]);
  sigma = exp(theta_best[0]) * sqrt(taud/2.0);

  nq = 2*(flag_detrend + 1);

  Larr = workspace;
  ybuf = Larr + nq*nall_data;
  Cq = ybuf + nall_data;
  ICq = Cq + nq*nq;
  ave = ICq + nq*nq;

  ysub = ave + nq;
  yrec = ysub + nall_data;
  yrec_err = yrec + nall_data;
  yave = yrec_err + nall_data;

  set_covar_mat(theta_best);

  for(i=0; i<nall_data*nall_data; i++)
  {
    Cmat[i] = Smat[i] + Nmat[i];
  }

  for(i=0; i<ncon_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[i*nq+0] = 1.0;
      Larr[i*nq+1] = 0.0;
    }
    else
    {
      Larr[i*nq + 0] = 1.0;
      Larr[i*nq + 1] = Tcon_data[i];
      Larr[i*nq + 2] = 0.0;
      Larr[i*nq + 3] = 0.0;
    }
  }
  for(i=0; i<nline_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[(i+ncon_data)*nq+0] = 0.0;
      Larr[(i+ncon_data)*nq+1] = 1.0;
    }
    else
    {
      Larr[(i+ncon_data)*nq + 0] = 0.0;
      Larr[(i+ncon_data)*nq + 1] = 0.0;
      Larr[(i+ncon_data)*nq + 2] = 1.0;
      Larr[(i+ncon_data)*nq + 3] = Tline_data[i];
    }
  }

  memcpy(ICmat, Cmat, nall_data*nall_data*sizeof(double));
  inverse_mat(ICmat, nall_data, &info);

  multiply_mat_MN(ICmat, Larr, Tmat1, nall_data, nq, nall_data);
  multiply_mat_MN_transposeA(Larr, Tmat1, ICq, nq, nq, nall_data);

  memcpy(Cq, ICq, nq*nq*sizeof(double));
  inverse_mat(Cq, nq, &info);

  multiply_mat_MN_transposeA(Larr, ICmat, Tmat1, nq, nall_data, nall_data);
  multiply_mat_MN(Cq, Tmat1, Tmat2, nq, nall_data, nq);
  multiply_mat_MN(Tmat2, Fall_data, ave, nq, 1, nall_data);

  multiply_mat_MN(Larr, ave, yave, nall_data, 1, nq);
  for(i=0; i<nall_data; i++)ysub[i] = Fall_data[i] - yave[i];
  multiply_matvec(ICmat, ysub, nall_data, ybuf);

  multiply_matvec_MN(Smat, nall_data, nall_data, ybuf, yrec);
  for(i=0; i<nall_data; i++)yrec[i] += yave[i];

  multiply_mat_MN(Smat, ICmat, Tmat1, nall_data, nall_data, nall_data);
  multiply_mat_MN_transposeB(Tmat1, Smat, Tmat2, nall_data, nall_data, nall_data);

  multiply_mat_MN(Tmat1, Larr, Tmat3, nall_data, nq, nall_data);
  for(i=0; i<nall_data*nq; i++)Tmat3[i] -= Larr[i];
  multiply_mat_MN(Tmat3, Cq, Tmat1, nall_data, nq, nq);
  multiply_mat_MN_transposeB(Tmat1, Tmat3, Tmat4, nall_data, nall_data, nq);

  for(i=0; i<nall_data; i++)
  {
    yrec_err[i] = sigma*sqrt((Smat[i*nall_data+i] - Tmat2[i*nall_data+i] + Tmat4[i*nall_data+i]));
  }

  FILE *fp;
  fp = fopen("data/tmp.txt", "w");
  for(i=0; i<ncon_data; i++)
  {
    fprintf(fp, "%f %f %f\n", Tcon_data[i], yrec[i]*scale_con, yrec_err[i]*scale_con);
  }
  fprintf(fp, "\n");
  for(i=ncon_data; i<nall_data; i++)
  {
    fprintf(fp, "%f %f %f\n", Tline_data[i-ncon_data], yrec[i]*scale_line, yrec_err[i]*scale_line);
  }
  fclose(fp);

  chi2 = 0.0;
  for(i=0; i<ncon_data; i++)chi2 += pow(Fcon_data[i] - yrec[i], 2)
                                /( pow(Fcerrs_data[i], 2) );
  for(i=ncon_data; i<nall_data; i++) chi2 += pow(Fline_data[i-ncon_data] - yrec[i], 2)
                                /( pow(Flerrs_data[i-ncon_data], 2) );
  printf("chi2: %f\n", chi2/(nall_data-ntheta));
  return chi2/(nall_data - ntheta);
}

void set_covar_mat(double *theta)
{
  double t1, t2, nerr, syserr;
  double taud, sigma;
  int i, j;

  //alpha = 1.0;
  taud = exp(theta[1]);
  sigma = exp(theta[0]) * sqrt(taud/2.0);
  syserr = exp(theta[ntheta-1]);

// first con-con
  for(i=0; i<ncon_data; i++)
  {
    t1 = Tcon_data[i];
    for(j=0; j<i; j++)
    {
      t2 = Tcon_data[j];
      Smat[i*nall_data+j] = Smat[j*nall_data+i] = exp( - fabs(t2-t1)/taud );
      Nmat[i*nall_data+j] = Nmat[j*nall_data+i] = 0.0;
    }
    nerr = Fcerrs_data[i];
    Nmat[i*nall_data+i] = (nerr*nerr + syserr*syserr)/(sigma*sigma);
    Smat[i*nall_data+i] = 1.0;
  }

// then con-line and line-con
  for(i=0; i<ncon_data; i++)
  {
    t1 = Tcon_data[i];
    for(j=0; j<nline_data; j++)
    {
      t2 = Tline_data[j];
      Smat[ i*nall_data + (ncon_data+j)] = Smat[(j+ncon_data)*nall_data + i] =  Slc(t1, t2, theta);
      Nmat[ i*nall_data + (ncon_data+j)] = Nmat[(j+ncon_data)*nall_data + i] = 0.0;
    }
  }

/*// then line-con
  for(i=0; i<nline_data; i++)
  {
    t1 = Tline_data[i];
    for(j=0; j<ncon_data; j++)
    {
      t2 = Tcon_data[j];
      Smat[ (i+ncon_data)*n_data + j] = Slc(t2, t1, theta);
      Nmat[ (i+ncon_data)*n_data + j] = 0.0;
    }
  }  */

// then line-line
  for(i=0; i<nline_data; i++)
  {
    t1 = Tline_data[i];
    for(j=0; j<i; j++)
    {
      t2 = Tline_data[j];
      Smat[ (i+ncon_data)*nall_data + (ncon_data+j)] = Smat[ (j+ncon_data)*nall_data + (ncon_data+i)] = Sll(t1, t2, theta);
      Nmat[ (i+ncon_data)*nall_data + (ncon_data+j)] = Nmat[ (j+ncon_data)*nall_data + (ncon_data+i)] = 0.0;
    }
    nerr = Flerrs_data[i];
    Nmat[ (i+ncon_data)*nall_data + (ncon_data+i) ] = (nerr * nerr + syserr*syserr)/(sigma*sigma);
    Smat[ (i+ncon_data)*nall_data + (ncon_data+i) ] = Sll(t1, t1, theta);
  }

}

void set_covar_Umat(double *theta)
{
  double t1, t2, taud;
  int i, j;

  taud = exp(theta[1]);
  //sigma = exp(theta[0]) * sqrt(taud/2.0);

  for(i=0; i<ncon; i++)
  {
    t1 = Tcon[i];
    for(j=0; j<ncon_data; j++)
    {
      t2 = Tcon_data[j];
      USmat[i*nall_data+j] = exp (- fabs(t1-t2) / taud );
    }

    for(j=0; j<nline_data; j++)
    {
      t2 = Tline_data[j];
      USmat[i*nall_data + (ncon_data+j)] = Slc(t1, t2, theta);
    }
  }

  for(i=0; i<nline; i++)
  {
    t1 = Tline[i];
    for(j=0; j<ncon_data; j++)
    {
      t2 = Tcon_data[j];
      USmat[ (i+ncon)*nall_data + j] = Slc(t2, t1, theta);
    }

    for(j=0; j<nline_data; j++)
    {
      t2 = Tline_data[j];
      USmat[ (i+ncon)*nall_data + (ncon_data+j)] = Sll(t1, t2, theta);
    }
  }
  return;
}

void set_covar_Amat(double *theta)
{
  double t1, t2, taud;
  int i, j;

  taud = exp(theta[1]);
  //sigma = exp(theta[0]) * sqrt(taud/2.0);

  for(i=0; i<ncon; i++)
  {
    t1 = Tcon[i];
    for(j=0; j<ncon; j++)
    {
      t2 = Tcon[j];
      ASmat[i*nall+j] = exp (- fabs(t1-t2) / taud );
    }

    for(j=0; j<nline; j++)
    {
      t2 = Tline[j];
      ASmat[i*nall + (ncon+j)] = Slc(t1, t2, theta);
    }
  }

  for(i=0; i<nline; i++)
  {
    t1 = Tline[i];
    for(j=0; j<ncon; j++)
    {
      t2 = Tcon[j];
      ASmat[ (i+ncon)*nall + j] = Slc(t2, t1, theta);
    }

    for(j=0; j<nline; j++)
    {
      t2 = Tline[j];
      ASmat[ (i+ncon)*nall + (ncon+j)] = Sll(t1, t2, theta);
    }
  }
  return;
}

double Slc(double tcon, double tline, double *theta)
{
  double Dt, DT, w, tauk, fk, Sk, Stot, taud;
  int i;

  Dt = tline - tcon;
  taud = exp(theta[1]);
  w = theta[2];

  Stot = 0.0;
#ifdef TOPHAT
  for(i=0; i<nc; i++)
  {
    tauk = grid_tau[i]; //theta[3+nc+i];
    fk = exp(theta[3+i]);
    DT = Dt - tauk;
    if(DT < -w)
    {
      Sk = exp( (DT + w) / taud ) - exp( (DT - w)/taud);
    }
    else if(DT < w)
    {
      Sk = 2.0 - exp(- (DT + w) / taud) - exp( (DT - w)/taud);
    }
    else
    {
      Sk = exp( - (DT - w)/taud) - exp ( - (DT + w)/taud);
    }
    Stot += fk * Sk;
  }
  Stot *= (taud/2.0/w);
#elif defined JAVELIN
  tauk = theta[4]; //theta[3+nc+i];
  fk = theta[3];
  DT = Dt - tauk;
  if(DT < -w)
  {
    Sk = exp( (DT + w) / taud ) - exp( (DT - w)/taud);
  }
  else if(DT < w)
  {
    Sk = 2.0 - exp(- (DT + w) / taud) - exp( (DT - w)/taud);
  }
  else
  {
    Sk = exp( - (DT - w)/taud) - exp ( - (DT + w)/taud);
  }
  Stot = fk * Sk;
  Stot *= (taud/2.0/w);
#else
  for(i=0; i<nc; i++)
  {
    tauk = grid_tau[i]; //theta[3+nc+i];
    fk = exp(theta[3+i]);
    DT = Dt - tauk;
    Sk = exp(-DT/taud) * erfc( -(DT/w - w/taud)/sqrt(2.0) )
        +exp( DT/taud) * erfc(  (DT/w + w/taud)/sqrt(2.0) );

    Stot += Sk * fk;
  }
  Stot *= 1.0/2.0 * exp(w*w/2.0/taud/taud);
#endif
  return Stot;
}

double Sll(double ti, double tj, double *theta)
{
  double Dt, DT, w, tauk, fk, taum, fm, Skm, Stot, taud;
  int k, m;

  Dt = ti - tj;
  taud = exp(theta[1]);
  w = theta[2];

  Stot = 0.0;
#ifdef TOPHAT
  for(k=0; k<nc; k++)
  {
    tauk = grid_tau[k]; //theta[3+nc+k];
    fk = exp(theta[3+k]);
    for(m=0; m<nc; m++)
    {
      taum = grid_tau[m]; //theta[3+nc+m];
      fm = exp(theta[3+m]);

      DT = Dt - (tauk-taum);

      if(DT < -2.0*w)
      {
        Skm = exp((DT + 2.0*w)/taud) + exp((DT - 2.0*w)/taud) - 2.0*exp(DT/taud);
      }
      else if(DT < 0.0)
      {
        Skm = exp(-(DT + 2.0*w)/taud)  + exp((DT - 2.0*w)/taud)  - 2.0*exp(DT/taud) + 2.0*(DT+2.0*w)/taud;
      }
      else if(DT < 2.0*w)
      {
        Skm = exp(-(DT + 2.0*w)/taud) + exp((DT - 2.0*w)/taud)  - 2.0*exp(-DT/taud) - 2.0*(DT-2.0*w)/taud;
      }
      else
      {
        Skm = exp( -(DT + 2.0*w)/taud) + exp(-(DT - 2.0*w)/taud) - 2.0*exp(-DT/taud);
      }
      Stot += fk*fm*Skm;
    }
  }
  Stot *= (taud*taud/4.0/w/w);

#elif defined JAVELIN
  for(k=0; k<1; k++)
  {
    tauk = theta[3+k+1]; //theta[3+nc+k];
    fk = theta[3+k];
    for(m=0; m<1; m++)
    {
      taum = theta[3+m+1]; //theta[3+nc+m];
      fm = theta[3+m];

      DT = Dt - (tauk-taum);

      if(DT < -2.0*w)
      {
        Skm = exp((DT + 2.0*w)/taud) + exp((DT - 2.0*w)/taud) - 2.0*exp(DT/taud);
      }
      else if(DT < 0.0)
      {
        Skm = exp(-(DT + 2.0*w)/taud)  + exp((DT - 2.0*w)/taud)  - 2.0*exp(DT/taud) + 2.0*(DT+2.0*w)/taud;
      }
      else if(DT < 2.0*w)
      {
        Skm = exp(-(DT + 2.0*w)/taud) + exp((DT - 2.0*w)/taud)  - 2.0*exp(-DT/taud) - 2.0*(DT-2.0*w)/taud;
      }
      else
      {
        Skm = exp( -(DT + 2.0*w)/taud) + exp(-(DT - 2.0*w)/taud) - 2.0*exp(-DT/taud);
      }
      Stot += fk*fm*Skm;
    }
  }
  Stot *= (taud*taud/4.0/w/w);
#else
  for(k=0; k<nc; k++)
  {
    tauk = grid_tau[k]; //theta[3+nc+k];
    fk = exp(theta[3+k]);
    for(m=0; m<nc; m++)
    {
      taum = grid_tau[m]; //theta[3+nc+m];
      fm = exp(theta[3+m]);
      DT = Dt - (tauk-taum);
      Skm = ( exp(-DT/taud) * erfc( -DT/2.0/w + w/taud )
            +  exp( DT/taud) * erfc(  DT/2.0/w + w/taud ) );
      Stot += Skm * fm*fk;
    }
  }
  Stot *= 1.0/2.0 * exp( w*w/taud/taud);
#endif
  return Stot;
}

int reconstruct_conline()
{
  FILE *frec;
  double *Larr, *Larr_rec, *ave, *yave, *yave_rec, *ysub, *ybuf, *yrec, *yrec_err, *Cq, *ICq;
  double *Tmp1, *Tmp2, *Tmp3, *Tmp4;
  double sigma, taud;
  int i, nq, info;

  nq = 2*(flag_detrend + 1);

  Larr = array_malloc(nall_data * nq);
  Larr_rec = array_malloc(nall * nq);
  ybuf = array_malloc(nall_data);
  ysub = array_malloc(nall_data);
  yave = array_malloc(nall_data);
  yave_rec = array_malloc(nall);
  yrec = array_malloc(nall);
  yrec_err = array_malloc(nall);
  ave = array_malloc(nq);
  Cq = array_malloc(nq*nq);
  ICq = array_malloc(nq*nq);

  Tmp1 = array_malloc(max(nall,nall_data)*max(nall,nall_data));
  Tmp2 = array_malloc(max(nall,nall_data)*max(nall,nall_data));
  Tmp3 = array_malloc(max(nall,nall_data)*max(nall,nall_data));
  Tmp4 = array_malloc(max(nall,nall_data)*max(nall,nall_data));

  taud = exp(theta_best[1]);
  sigma = exp(theta_best[0]) * sqrt(taud/2.0);
  set_covar_mat(theta_best);
  set_covar_Umat(theta_best);
  set_covar_Amat(theta_best);

  for(i=0;i<nall_data*nall_data; i++)
  {
    Cmat[i] = Smat[i] + Nmat[i];
  }
  memcpy(ICmat, Cmat, nall_data*nall_data*sizeof(double));
  inverse_mat(ICmat, nall_data, &info);

  for(i=0; i<ncon_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[i*nq+0] = 1.0;
      Larr[i*nq+1] = 0.0;
    }
    else
    {
      Larr[i*nq + 0] = 1.0;
      Larr[i*nq + 1] = Tcon_data[i];
      Larr[i*nq + 2] = 0.0;
      Larr[i*nq + 3] = 0.0;
    }
  }
  for(i=0; i<nline_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[(i+ncon_data)*nq+0] = 0.0;
      Larr[(i+ncon_data)*nq+1] = 1.0;
    }
    else
    {
      Larr[(i+ncon_data)*nq + 0] = 0.0;
      Larr[(i+ncon_data)*nq + 1] = 0.0;
      Larr[(i+ncon_data)*nq + 2] = 1.0;
      Larr[(i+ncon_data)*nq + 3] = Tline_data[i];
    }
  }

  for(i=0; i<ncon; i++)
  {
    if(flag_detrend==0)
    {
      Larr_rec[i*nq+0] = 1.0;
      Larr_rec[i*nq+1] = 0.0;
    }
    else
    {
      Larr_rec[i*nq + 0] = 1.0;
      Larr_rec[i*nq + 1] = Tcon[i];
      Larr_rec[i*nq + 2] = 0.0;
      Larr_rec[i*nq + 3] = 0.0;
    }
  }
  for(i=0; i<nline; i++)
  {
    if(flag_detrend==0)
    {
      Larr_rec[(i+ncon)*nq+0] = 0.0;
      Larr_rec[(i+ncon)*nq+1] = 1.0;
    }
    else
    {
      Larr_rec[(i+ncon)*nq + 0] = 0.0;
      Larr_rec[(i+ncon)*nq + 1] = 0.0;
      Larr_rec[(i+ncon)*nq + 2] = 1.0;
      Larr_rec[(i+ncon)*nq + 3] = Tline[i];
    }
  }

  multiply_mat_MN(ICmat, Larr, Tmat1, nall_data, nq, nall_data);
  multiply_mat_MN_transposeA(Larr, Tmat1, ICq, nq, nq, nall_data);
  memcpy(Cq, ICq, nq*nq*sizeof(double));
  inverse_mat(Cq, nq, &info);

  multiply_mat_MN_transposeA(Larr, ICmat, Tmat1, nq, nall_data, nall_data);
  multiply_mat_MN(Cq, Tmat1, Tmat2, nq, nall_data, nq);
  multiply_mat_MN(Tmat2, Fall_data, ave, nq, 1, nall_data);

  if(flag_detrend==0)
  {
    printf("ave: %e %e\n", ave[0]*scale_con, ave[1]*scale_line);
    fprintf(fp_results, "ave: %e %e\n", ave[0]*scale_con, ave[1]*scale_line);
  }
  else
  {
    printf("ave_line: %e %e\n", ave[2]*scale_line, ave[3]*scale_line);
    printf("ave_con: %e %e\n", ave[0]*scale_con, ave[1]*scale_con);
    fprintf(fp_results, "ave_con: %e %e\n", ave[0]*scale_con, ave[1]*scale_con);
    fprintf(fp_results, "ave_line: %e %e\n", ave[2]*scale_line, ave[3]*scale_line);
  }

  multiply_mat_MN(Larr, ave, yave, nall_data, 1, nq);
  for(i=0; i<nall_data; i++)ysub[i] = Fall_data[i] - yave[i];
  multiply_matvec(ICmat, ysub, nall_data, ybuf);

  multiply_matvec_MN(USmat, nall, nall_data, ybuf, yrec);
  multiply_mat_MN(Larr_rec, ave, yave_rec, nall, 1, nq);

/* store the reconstructed, mean-substracted contionum */
  memcpy(Fcon, yrec, ncon*sizeof(double));

  for(i=0; i<nall; i++)yrec[i] += yave_rec[i];

  // get errors

  multiply_mat_MN(USmat, ICmat, Tmp1, nall, nall_data, nall_data);
  multiply_mat_MN_transposeB(Tmp1, USmat, Tmp2, nall, nall, nall_data);

  multiply_mat_MN(Tmp1, Larr, Tmp3, nall, nq, nall_data);
  for(i=0; i<nall*nq; i++)Tmp3[i] -= Larr_rec[i];
  multiply_mat_MN(Tmp3, Cq, Tmp1, nall, nq, nq);
  multiply_mat_MN_transposeB(Tmp1, Tmp3, Tmp4, nall, nall, nq);

  for(i=0; i<nall; i++)
  {
    yrec_err[i] = sigma * sqrt(ASmat[i*nall+i] - Tmp2[i*nall+i] + Tmp4[i*nall+i]);
  }

/* store the error */
  for(i=0; i<ncon; i++)
  {
    Fcerrs[i] = sigma * sqrt(ASmat[i*nall+i] - Tmp2[i*nall+i]);
  }

  frec = fopen("data/sall_con.txt", "w");
  for(i=0; i<ncon; i++)
  {
    fprintf(frec, "%f %e %e\n", Tcon[i], yrec[i]*scale_con, yrec_err[i]*scale_con);
  }
  fclose(frec);
  frec = fopen("data/sall_line.txt", "w");
  for(i=ncon; i<nall; i++)
  {
    fprintf(frec, "%f %e %e\n", Tline[i-ncon], yrec[i]*scale_line, yrec_err[i]*scale_line);
  }
  fclose(frec);
  free(Tmp1);
  free(Tmp2);
  free(Tmp3);
  free(Tmp4);
  return 0;
}


static char help[] = "Assemble a Mat sparsely.\n";

#include <petsc.h>

int main(int argc,char **args) {
  PetscErrorCode ierr;
  Mat    A;
  int    i1[3] = {0, 1, 2},
         j1[3] = {0, 1, 2},
         i2 = 3,
         j2[3] = {1, 2, 3},
         i3 = 1,
         j3 = 3;
  double aA1[9] = { 1.0,  2.0,  3.0,
                    2.0,  1.0, -2.0,
                   -1.0,  1.0,  1.0},
         aA2[3] = { 1.0,  1.0, -1.0},
         aA3 = -3.0;

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

  ierr = MatCreate(PETSC_COMM_WORLD,&A); CHKERRQ(ierr);
  ierr = MatSetSizes(A,PETSC_DECIDE,PETSC_DECIDE,4,4); CHKERRQ(ierr);
  ierr = MatSetFromOptions(A); CHKERRQ(ierr);
  ierr = MatSetUp(A); CHKERRQ(ierr);
  ierr = MatSetValues(A,3,i1,3,j1,aA1,INSERT_VALUES); CHKERRQ(ierr);
  ierr = MatSetValues(A,1,&i2,3,j2,aA2,INSERT_VALUES); CHKERRQ(ierr);
  ierr = MatSetValue(A,i3,j3,aA3,INSERT_VALUES); CHKERRQ(ierr);
  ierr = MatAssemblyBegin(A,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
  ierr = MatAssemblyEnd(A,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);

  MatDestroy(&A);
  return PetscFinalize();
}