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

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

#include "linear_common.c"

/* General weighted case */

static int
multifit_wlinear_svd (const gsl_matrix * X,
                      const gsl_vector * w,
                      const gsl_vector * y,
                      double tol,
                      int balance,
                      size_t * rank,
                      gsl_vector * c,
                      gsl_matrix * cov,
                      double *chisq, gsl_multifit_linear_workspace * work)
{
  if (X->size1 != y->size)
    {
      GSL_ERROR
        ("number of observations in y does not match rows of matrix X",
         GSL_EBADLEN);
    }
  else if (X->size2 != c->size)
    {
      GSL_ERROR ("number of parameters c does not match columns of matrix X",
                 GSL_EBADLEN);
    }
  else if (w->size != y->size)
    {
      GSL_ERROR ("number of weights does not match number of observations",
                 GSL_EBADLEN);
    }
  else if (cov->size1 != cov->size2)
    {
      GSL_ERROR ("covariance matrix is not square", GSL_ENOTSQR);
    }
  else if (c->size != cov->size1)
    {
      GSL_ERROR
        ("number of parameters does not match size of covariance matrix",
         GSL_EBADLEN);
    }
  else if (X->size1 != work->n || X->size2 != work->p)
    {
      GSL_ERROR
        ("size of workspace does not match size of observation matrix",
         GSL_EBADLEN);
    }
  else
    {
      const size_t n = X->size1;
      const size_t p = X->size2;

      size_t i, j, p_eff;

      gsl_matrix *A = work->A;
      gsl_matrix *Q = work->Q;
      gsl_matrix *QSI = work->QSI;
      gsl_vector *S = work->S;
      gsl_vector *t = work->t;
      gsl_vector *xt = work->xt;
      gsl_vector *D = work->D;

      /* Scale X,  A = sqrt(w) X */

      gsl_matrix_memcpy (A, X);

      for (i = 0; i < n; i++)
        {
          double wi = gsl_vector_get (w, i);

          if (wi < 0)
            wi = 0;

          {
            gsl_vector_view row = gsl_matrix_row (A, i);
            gsl_vector_scale (&row.vector, sqrt (wi));
          }
        }

      /* Balance the columns of the matrix A if requested */

      if (balance)
        {
          gsl_linalg_balance_columns (A, D);
        }
      else
        {
          gsl_vector_set_all (D, 1.0);
        }

      /* Decompose A into U S Q^T */

      gsl_linalg_SV_decomp_mod (A, QSI, Q, S, xt);

      /* Solve sqrt(w) y = A c for c, by first computing t = sqrt(w) y */

      for (i = 0; i < n; i++)
        {
          double wi = gsl_vector_get (w, i);
          double yi = gsl_vector_get (y, i);
          if (wi < 0)
            wi = 0;
          gsl_vector_set (t, i, sqrt (wi) * yi);
        }

      gsl_blas_dgemv (CblasTrans, 1.0, A, t, 0.0, xt);

      /* Scale the matrix Q,  Q' = Q S^-1 */

      gsl_matrix_memcpy (QSI, Q);

      {
        double s0 = gsl_vector_get (S, 0);
        p_eff = 0;

        for (j = 0; j < p; j++)
          {
            gsl_vector_view column = gsl_matrix_column (QSI, j);
            double sj = gsl_vector_get (S, j);
            double alpha;

            if (sj <= tol * s0)
              {
                alpha = 0.0;
              }
            else
              {
                alpha = 1.0 / sj;
                p_eff++;
              }

            gsl_vector_scale (&column.vector, alpha);
          }

        *rank = p_eff;
      }

      gsl_vector_set_zero (c);

      /* solution */
      gsl_blas_dgemv (CblasNoTrans, 1.0, QSI, xt, 0.0, c);

      /* unscale the balancing factors */
      gsl_vector_div (c, D);

      /* compute chisq, from residual r = y - X c */
      {
        double r2 = 0;

        for (i = 0; i < n; i++)
          {
            double yi = gsl_vector_get (y, i);
            double wi = gsl_vector_get (w, i);
            gsl_vector_const_view row = gsl_matrix_const_row (X, i);
            double y_est, ri;
            gsl_blas_ddot (&row.vector, c, &y_est);
            ri = yi - y_est;
            r2 += wi * ri * ri;
          }

        *chisq = r2;

        /* Form covariance matrix cov = (X^T W X)^-1 = (Q S^-1) (Q S^-1)^T */

        for (i = 0; i < p; i++)
          {
            gsl_vector_view row_i = gsl_matrix_row (QSI, i);
            double d_i = gsl_vector_get (D, i);

            for (j = i; j < p; j++)
              {
                gsl_vector_view row_j = gsl_matrix_row (QSI, j);
                double d_j = gsl_vector_get (D, j);
                double s;

                gsl_blas_ddot (&row_i.vector, &row_j.vector, &s);

                gsl_matrix_set (cov, i, j, s / (d_i * d_j));
                gsl_matrix_set (cov, j, i, s / (d_i * d_j));
              }
          }
      }

      return GSL_SUCCESS;
    }
}

int
gsl_multifit_wlinear (const gsl_matrix * X,
                      const gsl_vector * w,
                      const gsl_vector * y,
                      gsl_vector * c,
                      gsl_matrix * cov,
                      double *chisq, gsl_multifit_linear_workspace * work)
{
  size_t rank;
  int status = gsl_multifit_wlinear_tsvd(X, w, y, GSL_DBL_EPSILON, c, cov, chisq, &rank, work);

  return status;
}

int
gsl_multifit_wlinear_tsvd (const gsl_matrix * X,
                           const gsl_vector * w,
                           const gsl_vector * y,
                           const double tol,
                           gsl_vector * c,
                           gsl_matrix * cov,
                           double * chisq,
                           size_t * rank,
                           gsl_multifit_linear_workspace * work)
{
  const size_t n = X->size1;
  const size_t p = X->size2;

  if (y->size != n)
    {
      GSL_ERROR("number of observations in y does not match matrix",
                GSL_EBADLEN);
    }
  else if (w->size != n)
    {
      GSL_ERROR("number of weights in w does not match matrix",
                GSL_EBADLEN);
    }
  else if (p != c->size)
    {
      GSL_ERROR ("number of parameters c does not match matrix",
                 GSL_EBADLEN);
    }
  else if (tol <= 0)
    {
      GSL_ERROR ("tolerance must be positive", GSL_EINVAL);
    }
  else
    {
      int status;
      double rnorm, snorm;
      gsl_matrix_view A = gsl_matrix_submatrix(work->A, 0, 0, n, p);
      gsl_vector_view b = gsl_vector_subvector(work->t, 0, n);

      /* compute A = sqrt(W) X, b = sqrt(W) y */
      status = gsl_multifit_linear_applyW(X, w, y, &A.matrix, &b.vector);
      if (status)
        return status;

      /* compute SVD of A */
      status = gsl_multifit_linear_bsvd(&A.matrix, work);
      if (status)
        return status;

      status = multifit_linear_solve(X, &b.vector, tol, 0.0, rank,
                                     c, &rnorm, &snorm, work);
      if (status)
        return status;

      *chisq = rnorm * rnorm;

      /* variance-covariance matrix cov = s2 * (Q S^-1) (Q S^-1)^T */
      {
        const size_t p = X->size2;
        size_t i, j;
        gsl_matrix_view QSI = gsl_matrix_submatrix(work->QSI, 0, 0, p, p);
        gsl_vector_view D = gsl_vector_subvector(work->D, 0, p);

        for (i = 0; i < p; i++)
          {
            gsl_vector_view row_i = gsl_matrix_row (&QSI.matrix, i);
            double d_i = gsl_vector_get (&D.vector, i);

            for (j = i; j < p; j++)
              {
                gsl_vector_view row_j = gsl_matrix_row (&QSI.matrix, j);
                double d_j = gsl_vector_get (&D.vector, j);
                double s;

                gsl_blas_ddot (&row_i.vector, &row_j.vector, &s);

                gsl_matrix_set (cov, i, j, s / (d_i * d_j));
                gsl_matrix_set (cov, j, i, s / (d_i * d_j));
              }
          }
      }
    }

  return GSL_SUCCESS;
}

int
gsl_multifit_wlinear_svd (const gsl_matrix * X,
                          const gsl_vector * w,
                          const gsl_vector * y,
                          double tol,
                          size_t * rank,
                          gsl_vector * c,
                          gsl_matrix * cov,
                          double *chisq, gsl_multifit_linear_workspace * work)
{
  int status  = multifit_wlinear_svd (X, w, y, tol, 1, rank, c,
                                      cov, chisq, work);
  return status;

}

int
gsl_multifit_wlinear_usvd (const gsl_matrix * X,
                           const gsl_vector * w,
                           const gsl_vector * y,
                           double tol,
                           size_t * rank,
                           gsl_vector * c,
                           gsl_matrix * cov,
                           double *chisq, gsl_multifit_linear_workspace * work)
{
  int status  = multifit_wlinear_svd (X, w, y, tol, 0, rank, c,
                                      cov, chisq, work);
  return status;

}


#if !defined(SM_H)
#define SM_H

#include <gsl/gsl_matrix.h>

/**
* @brief Class describing the Standard Model
*
* This class holds all the parameters specifying the Standard Model, and
* methods to access them in convenient ways.
*
*/
class SM {

 public:

  /// Default value for EM coupling \f$ \alpha_{\rm{EM}}(M_Z) \f$
  const static double alpha    = 1./127.934;

  /// Default value for EM coupling at low energy
  const static double alpha0   = 1./137.0359997;

  /// Default value for the Fermi constant \f$ G_F \f$ (\f$ \rm{GeV}^{-2} \f$)
  const static double GF      = 1.16637E-5;

  /// Default value for \f$ M_Z \f$ (GeV)
  const static double MZ      = 91.15349;

  /// Default value for \f$ M_W \f$ (GeV)
  const static double MW      = 80.36951;

  /// Default value for the strong coupling \f$ \alpha_s(M_Z) \f$
  const static double alpha_s = 0.119;

  /// Default value for total width \f$ \Gamma_Z \f$ (GeV)
  const static double GammaZ  = 2.49581;

  /// Default value for total width \f$ \Gamma_W \f$ (GeV)
  const static double GammaW  = 2.08856;

  /// Default value for quark pole mass \f$ m_d \f$ (GeV)
  const static double md_p    = 0.0;

  /// Default value for quark pole mass \f$ m_u \f$ (GeV)
  const static double mu_p    = 0.0;

  /// Default value for quark pole mass \f$ m_s \f$ (GeV)
  const static double ms_p    = 0.1;

  // Scale for strange quark mass (GeV)
  const static double Q_ms    = 2.0;

  /// Default value for quark pole mass \f$ m_c \f$ (GeV)
  const static double mc_p    = 1.42;

  /// Default value for quark pole mass \f$ m_b \f$ (GeV)
  const static double mb_p    = 4.75;

  /// Default value for quark pole mass \f$ m_t \f$ (GeV)
  const static double mt_p    = 172.5;

  /// Default value for electron pole mass \f$ m_e \f$ (GeV)
  const static double me_p    = 0.510998918E-3;

  /// Default value for muon pole mass \f$ m_\mu \f$ (GeV)
  const static double mmu_p   = 0.105658367;

  /// Default value for tau pole mass \f$ m_\tau \f$ (GeV)
  const static double mtau_p  = 1.77684;

  // PDG booklet 2012
  /// Default value for CKM matrix element \f$ V_{ud} \f$
  const static double Vud     = 0.97427;
  /// Default value for CKM matrix element \f$ V_{us} \f$
  const static double Vus     = 0.22534;
  /// Default value for CKM matrix element \f$ V_{ub} \f$
  const static double Vub     = 0.00351;
  /// Default value for CKM matrix element \f$ V_{cd} \f$
  const static double Vcd     = 0.22520;
  /// Default value for CKM matrix element \f$ V_{cs} \f$
  const static double Vcs     = 0.97334;
  /// Default value for CKM matrix element \f$ V_{cb} \f$
  const static double Vcb     = 0.0412;
  /// Default value for CKM matrix element \f$ V_{td} \f$
  const static double Vtd     = 0.00867;
  /// Default value for CKM matrix element \f$ V_{ts} \f$
  const static double Vts     = 0.0404;
  /// Default value for CKM matrix element \f$ V_{tb} \f$
  const static double Vtb     = 0.999146;

  /**
  * @brief Default constructor initializing the Standard Model with default parameter
  *        values
  *
  * This constructor initializes a new SM object with default parameters. The
  * values used as defaults are specified by public constants in this class.
  */
  SM();

  /**
  * @brief Sets the EM coupling \f$ \alpha \f$
  *
  * @param alpha_in New value for \f$ \alpha(M_Z) \f$
  */
  void set_alpha(double alpha_in);

  /**
  * @brief Sets the EM coupling \f$ \alpha_0 \f at low energy$
  *
  * @param alpha_in New value for \f$ \alpha(Q^2=0) \f$
  */
  void set_alpha0(double alpha_in);

  /**
  * @brief Sets the Fermi constant \f$ G_F \f$
  *
  * @param GF_in New value for \f$ G_F \f$ (\f$ GeV^{-2} \f$)
  */
  void set_GF(double GF_in);

  /**
  * @brief Sets \f$ M_Z \f$
  *
  * @param MZ_in New value for \f$ M_Z \f$
  */
  void set_MZ(double MZ_in);

  /**
  * @brief Sets \f$ M_W \f$
  *
  * @param MW_in New value for \f$ M_W \f$
  */
  void set_MW(double MW_in);

  /**
  * @brief Sets \f$ \gamma_Z \f$
  *
  * @param GammaZ_in New value for \f$ \Gamma_Z \f$
  */
  void set_gamma_Z(double GammaZ_in);

  /**
  * @brief Sets \f$ \gamma_W \f$
  *
  * @param GammaW_in New value for \f$ \Gamma_W \f$
  */
  void set_gamma_W(double GammaW_in);

  /**
  * @brief Sets the strong coupling \f$ \alpha_s \f$
  *
  * @param alpha_s_in New value for \f$ \alpha_s(M_Z) \f$
  */
  void set_alpha_s(double alpha_s_in);

  /**
  * @brief Current value of \f$ \alpha \f$
  *
  * @returns Value of \f$ \alpha \f$
  */
  double get_alpha();
  double get_alpha0();

  /**
  * @brief Current value of \f$ G_F \f$
  *
  * @returns Value of \f$ G_F \f$ (\f$ \rm{GeV}^{-2} \f$)
  */
  double get_GF();

  /**
  * @brief Current value of \f$ \sin\theta_W \f$
  *
  * @returns Value of \f$ \sin\theta_W \f$
  */
  double get_sintw();

  /**
  * @brief Current value of \f$ \cos\theta_W \f$
  *
  * @returns Value of \f$ \cos\theta_W \f$
  */
  double get_costw();

  /**
  * @brief Current value of weak SU(2) coupling \f$ g \f$
  *
  * @returns Value of \f$ g \f$
  */
  double get_g();

  /**
  * @brief Current value of hypercharge U(1) coupling \f$ g' \f$
  *
  * @returns Value of \f$ g' \f$
  */
  double get_gprime();

  /**
  * @brief Current value of EM coupling \f$ e \f$
  *
  * @returns Value of \f$ g' \f$
  */
  double get_e();

  /**
  * @brief Current value of \f$ \alpha_s \f$
  *
  * @returns Value of \f$ \alpha_s \f$
  */
  double get_alpha_s();

  /**
  * @brief Current value of \f$ M_W \f$
  *
  * @returns Value of \f$ M_W \f$ (GeV)
  */
  double get_MW();

  /**
  * @brief Current value of \f$ \Gamma_W \f$
  *
  * @returns Value of \f$ \Gamma_W \f$ (GeV)
  */
  double get_gamma_W();

  /**
  * @brief Current value of \f$ M_Z \f$
  *
  * @returns Value of \f$ M_Z \f$ (GeV)
  */
  double get_MZ();

  /**
  * @brief Current value of \f$ \Gamma_Z \f$
  *
  * @returns Value of \f$ \Gamma_Z \f$ (GeV)
  */
  double get_gamma_Z();

  /**
  * @brief Current value of the vev \f$ v=1/\sqrt{\sqrt{2}G_F} \f$
  *
  * @returns Value of \f$ v \f$ (GeV)
  */
  double get_v();

  /**
  * @brief Current value of the vev squared \f$ v^2=1/(\sqrt{2}G_F) \f$
  *
  * @returns Value of \f$ v^2 \f$ (\f$ \rm{GeV}^2 \f$)
  */
  double get_v2();

  /**
  * @brief Vector boson mass
  *
  * This method returns the mass of a given vector boson. The numbering
  * convention used throughout 2HDMC is such that (1 = photon, 2 = Z0, 3 = W+).
  *
  * @param v Index (1-3) for the vector boson
  *
  * @returns Value of \f$ m_V \f$ (GeV)
  */
  double get_vmass(int v);

  /**
  * @brief Vector boson width
  *
  * This method returns the decay width of a given vector boson. The numbering
  * convention is such that (1 = photon, 2 = Z0, 3 = W+).
  *
  * @param v Index (1-3) for the vector boson
  *
  * @returns Value of \f$ \Gamma_V \f$ (GeV)
  */
  double get_gamma_V(int v);

 /**
  * @brief Sets the CKM matrix to a 3x3 unit matrix
  *
  * This method sets the CKM matrix to a 3x3 unit matrix, i.e. eliminating
  * all charged current interactions between different generations.
  */
  void set_diagonal_CKM();

  /**
  * @brief Full CKM matrix
  *
  * This method can be used to retrieve the full CKM matrix as a gsl_matrix.
  *
  * @returns CKM matrix
  *
  * @see get_MD, get_MU, get_ML, get_CKM_element
  */
  gsl_matrix* get_CKM_matrix();

  /**
  * @brief Element of the CKM matrix
  *
  * This method can be used to retrieve a certain element of the CKM matrix.
  *
  * @param i Row index (1,2,3 = u,c,t) of CKM matrix element
  * @param j Column index (1,2,3 = d,s,b) of CKM matrix element
  * @returns The requested CKM element
  *
  * @see get_CKM
  */
  double get_CKM_element(int i, int j);

  /**
  * @brief Sets the pole mass of a lepton
  *
  * This method sets the pole mass of lepton \a l to the supplied value.
  *
  * @param l Index of lepton (1,2,3 = \f$ e,\mu,\tau \f$)
  * @param lmass_in New mass of lepton
  */
  void set_lmass_pole(int l, double lmass_in);

  /**
  * @brief Sets the pole mass of a quark
  *
  * This method sets the pole mass of quark \a q to the supplied value.
  *
  * @param q Index of quark (1,2,3,4,5,6 = \f$ d,u,s,c,b,t \f$)
  * @param qmass_in New mass of quark
  */
  void set_qmass_pole(int q, double qmass_in);

  void set_qmass_msbar(int flav, double qmass_in);

  /**
  * @brief Sets the pole mass of a down-type quark
  *
  * This method sets the pole mass of quark \a d to the supplied value.
  *
  * @param d Index of quark (1,2,3 = \f$ d,s,b \f$)
  * @param dmass_in New mass of quark
  */
  void set_dmass_pole(int d, double dmass_in);

  /**
  * @brief Sets the pole mass of an up-type quark
  *
  * This method sets the pole mass of quark \a u to the supplied value.
  *
  * @param u Index of quark (1,2,3 = \f$ u,c,t \f$)
  * @param umass_in New mass of quark
  */
  void set_umass_pole(int u, double umass_in);

  /**
  * @brief Quark pole mass
  *
  * @param q Index (1,2,3,4,5,6 = \f$ d,u,s,c,b,t \f$) of quark
  *
  * @returns The pole mass \f$ m_q \f$
  */
  double get_qmass_pole(int q);

  /**
  * @brief Down-type quark pole mass
  *
  * @param d Index (1,2,3 = \f$ d,s,b \f$) of down-type quark
  *
  * @returns Quark pole mass \f$ m_d \f$
  */
  double get_dmass_pole(int d);

  /**
  * @brief Up-type quark pole mass
  *
  * @param u Index (1,2,3 = \f$ u,c,t \f$) of up-type quark
  *
  * @returns Quark pole mass \f$ m_u \f$
  */
  double get_umass_pole(int u);

  /**
  * @brief Lepton pole mass
  *
  * @param l Index (1,2,3 = \f$ e,\mu,\tau \f$) of lepton
  *
  * @returns The pole mass \f$ m_l \f$
  */
  double get_lmass_pole(int l);

  /**
  * @brief Mass matrix for down-type quarks
  *
  * This method can be used to retrieve, in matrix form, the current pole masses
  * used for the down-type quarks. The masses are returned as the diagonal
  * elements of a 3x3 gsl_matrix that must be allocated by the user.
  *
  * @returns Diagonal mass matrix for the down-type quarks
  *
  * @see get_MU, get_ML, get_CKM
  */
  gsl_matrix* get_MD();

  /**
  * @brief Mass matrix for up-type quarks
  *
  * This method can be used to retrieve, in matrix form, the current pole masses
  * used for the up-type quarks. The masses are returned as the diagonal
  * elements of a 3x3 gsl_matrix that must be allocated by the user.
  *
  * @returns Diagonal mass matrix for the up-type quarks
  *
  * @see get_MD, get_ML, get_CKM
  */
  gsl_matrix* get_MU();

  /**
  * @brief Mass matrix for leptons
  *
  * This method can be used to retrieve, in matrix form, the current pole masses
  * used for the leptons. The masses are returned as the diagonal
  * elements of a 3x3 gsl_matrix that must be allocated by the user.
  *
  * @returns Diagonal mass matrix for the leptons
  *
  * @see get_MD, get_MU, get_CKM
  */
  gsl_matrix* get_ML();

  /**
  * @brief Quark \f$ \overline{\rm{MS}}\f$ mass
  *
  * This method determines the \f$ \overline{\rm{MS}}\f$ mass \f$ \overline{m}_q(\overline{m}_q) \f$
  *
  * @param q Index (1,2,3,4,5,6 = \f$ d,u,s,c,b,t \f$) of quark
  *
  * @returns Quark \f$ \overline{\rm{MS}}\f$ mass \f$ \overline{m}_q(\overline{m}_q) \f$
  *
  * @see run_qmass_MSbar
  */
  double get_qmass_MSbar(int q);

  /**
  * @brief Down-type quark \f$ \overline{\rm{MS}}\f$ mass
  *
  * This method determines the \f$ \overline{\rm{MS}}\f$ mass \f$ \overline{m}_d(\overline{m}_d) \f$
  *
  * @param d Index (1,2,3 = \f$ d,s,b \f$) of down-type quark
  *
  * @returns Quark \f$ \overline{\rm{MS}}\f$ mass \f$ \overline{m}_d(\overline{m}_d) \f$
  *
  * @see get_umass_MSbar, run_qmass_MSbar
  */
  double get_dmass_MSbar(int d);

  /**
  * @brief Up-type quark \f$ \overline{\rm{MS}}\f$ mass
  *
  * This method determines the \f$ \overline{\rm{MS}}\f$ mass \f$ \overline{m}_u(\overline{m}_u) \f$
  *
  * @param u Index (1,2,3 = \f$ u,c,t \f$) of up-type quark
  *
  * @returns Quark \f$ \overline{\rm{MS}}\f$ mass \f$ \overline{m}_u(\overline{m}_u) \f$
  *
  * @see get_dmass_MSbar, run_qmass_MSbar
  */
  double get_umass_MSbar(int u);

  /**
  * @brief Evaluates running quark masses
  *
  * This method evaluates the running quark_mass \a quark_mass, initially specified
  * at the input scale \a Qinit, at the new scale \a Qfin. The calculation is performed
  * in the \f$ \overline{\rm{MS}} \f$ scheme with variable number of active
  * flavours and threshold matching.
  *
  * @param quark_mass The input value for the running quark mass
  * @param Qinit      Starting scale
  * @param Qfin       Final scale at which the mass is to be evaluated
  * @param mtop       \f$ \overline{\rm{MS}} \f$ top mass (for thresholds)
  * @param mbot       \f$ \overline{\rm{MS}} \f$ bottom mass (for thresholds)
  *
  * @returns The running mass at the scale \a Qfin
  */
  double run_qmass_MSbar(double quark_mass, double Qinit, double Qfin, double mtop, double mbot);

  /**
  * @brief Evaluates the running strong coupling
  *
  * This method evaluates the running strong coupling \f$ \alpha_s \f$ in the
  * \f$ \overline{\rm{MS}} \f$ scheme at the scale specified by \a Q.
  *
  * @param Q          Scale at which to evaluate the running coupling
  * @param mtop       \f$ \overline{\rm{MS}} \f$ top mass (for thresholds)
  * @param mbot       \f$ \overline{\rm{MS}} \f$ bottom mass (for thresholds)
  *
  * @returns The strong coupling \f$ \alpha_s \f$ at the scale \a Q
  */
  double run_alphas_MSbar(double Q, double mtop, double mbot);

  /**
  * @brief Number of active flavours
  *
  * This method returns the number of active quark flavours to be used at
  * a certain mass scale when decoupling the heavier quarks. The thresholds
  * are determined from the quark \f$ \overline{\rm{MS}} \f$ masses.
  *
  * @param M The scale for which the number of flavours is desired
  *
  * @returns The number of quark flavours active at this scale
  */
  int get_Nactivef(double M);

  /**
  * @brief %SM width of the top quark
  *
  * Calculates the width of the top quark in the %SM, i.e. no contributions from
  * \f$ t\to H^+ b \f$ decays etc. are included.
  *
  * @returns The top width \f$ \Gamma_t \f$
  */
  double get_gamma_top();
  double get_gamma_tWd(int d);

  // --- These masses are for compatibility test with HDECAY ---
  // Scale for HD quark masses (HD)
  const static double Q_HD    = 5.0;

  // switch for HD quark masses (HD)
  const static bool b_HD    = false;

  /// Default value for running quark mass at 5 GeV \f$ m_s(5) \f$ (GeV)
  const static double ms_5    = 0.08160951656;

  /// Default value for running quark mass at 5 GeV \f$ m_c(5) \f$ (GeV)
  const static double mc_5    = 0.8960809463;

  /// Default value for running quark mass at 5 GeV \f$ m_b(5) \f$ (GeV)
  const static double mb_5    = 4.310679848;

  /// Default value for running quark mass at 5 GeV \f$ m_t(5) \f$ (GeV)
  const static double mt_5    = 246.5552058;
  // -------------------------------------------------------------

 private:

  // Internal variables which store the current values of the SM parameters
  double m_alpha;
  double m_alpha0;
  double m_GF;
  double m_s2tW;
  double m_alpha_s;
  double m_md_p[3];
  double m_mu_p[3];
  double m_ml_p[3];
  double m_MW;
  double m_MZ;
  double m_GammaW;
  double m_GammaZ;
  double m_CKM[3][3];

  double m_qmass_ms[7];

	// Support functions for the running mass calculation
  double m_threshold(double as);
  double RQ(double as, int nf, int loops);
  double QCD_beta(int c, int nf);

  void clear_lookup();

};

#endif


#ifndef _params_h_
#define _params_h_

#include <stdio.h>
#include <time.h>
#include <unistd.h> //For getpid() function
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fftw3.h>

#include <gsl/gsl_spline.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_matrix.h>

//****Simulation Parameters:
#define N_ps (int) 64 //Number of points in PS file
#define N_gen (int) 4 //To generate the 21cm field
#define LEN (double) 8.0 //Length of simulation box (in Mpc h^{-1})
#define freq_width (double) 2.0 //Width of frequency channel (in MHz)

//****Foreground Removal Parameters:
double *expo;
int expo_array;
int count(int);

//****Function declarations:
double interp_ps_lin(double);
double read_21cm_cube(char *);
double sim_21cm_cube();
double generate_full_signal();
double count_freq_channels();
double noise_removal();

//****Multifrequency fitting declarations:
double *y, *x, *N, *Ni, *xtNi, *xtNix, *xtNix_i, *big_matrix, *a, *xa;

//****GSL rng declarations:
const gsl_rng_type *T;
gsl_rng *rand1, *rand2;
long seed;
double a_q, b_q;

//****Others:
int SimON, XY_pix, Z_pix, Npol; //XY_pix and Z_pix are the box dimensions in pixels
double frequency, original_21cm_mean, total_21cm_mean;
double *original_21cm_field, *total_21cm_field, *detector_noise_field, *total_noise_field , *recovered_21cm_field, *parameter_a_cube;
double k[N_ps], Pk[N_ps];
double *freq_array;
double sigma, nu, tau, offset;
double XY_LEN, Z_LEN; //Length of the box

FILE *file;
char cmnd[1000], header[100], polystr[100];
//****polystr is to store the exponents of variables of the fitting polynomial

#endif


/*==========================================================
 * pca.c - Principal component analysis of diffraction data
 *
 * Calculates the two main components of the data
 * by Principal Component Analysis, as detailed in
 * Bernhardt et.al (2016), Biophys. J.
 *
 * The calling syntax is:
 *
 *		pca(data, mask, qx, qy, w, phi, m, n)
 *
 * Copyright 2017 Institute for X-ray Physics (University of Göttingen)
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE
 * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 *========================================================*/

#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>

#define PI 3.14159265

/* The computational routine */
void pca(double *x, double *mask, double *qx, double *qy, double *w, double *phi, int m, int n)
{
    int col;
    int row;
    double tr = 0;

    /* total sum of x */
    for (row=0; row<m; row++) {
        for (col=0; col<n; col++) {
            x[row*m + col] = x[row*m + col] * mask[row*m + col];
            tr += x[row*m + col];
        }
    }

    /* covariance matrix */
    double cov_qx = 0;
    double cov_mixed = 0;
    double cov_qy = 0;

    /* total sum of x */
    for (row=0; row<m; row++) {
        for (col=0; col<n; col++) {
            cov_qx += x[row*m + col] * pow(qx[row*m + col],2);
            cov_qy += x[row*m + col] * pow(qy[row*m + col],2);
            cov_mixed += x[row*m + col] * qx[row*m + col] * qy[row*m + col];
        }
    }

    double data[] = {cov_qx/tr   , cov_mixed/tr,
                     cov_mixed/tr, cov_qy/tr   };

    gsl_matrix_view mv
        = gsl_matrix_view_array (data, 2, 2);

    gsl_vector *eval = gsl_vector_alloc (2);
    gsl_matrix *evec = gsl_matrix_alloc (2, 2);

    gsl_eigen_symmv_workspace * ws
        = gsl_eigen_symmv_alloc (2);

    gsl_eigen_symmv (&mv.matrix, eval, evec, ws);

    gsl_eigen_symmv_free (ws);

    gsl_eigen_symmv_sort (eval, evec,
                        GSL_EIGEN_SORT_ABS_DESC);

    /* get eigenvalues and eigenvectors (assuming they are sorted) */
    double eval_i = gsl_vector_get (eval, 0);
    double eval_j = gsl_vector_get (eval, 1);

    gsl_vector_view evec_i
       = gsl_matrix_column (evec, 0);

    double xcomp = gsl_vector_get (&evec_i.vector, 0);
    double ycomp = gsl_vector_get (&evec_i.vector, 1);

    /* anisotropy */
    *w = (eval_i - eval_j) / (eval_i + eval_j);

    /* orientation */
    *phi = atan2(ycomp,xcomp) * 180.0 / PI;
    if (*phi < 0){
        *phi += 180;
    }

    gsl_vector_free (eval);
    gsl_matrix_free (evec);

    /* debugging information */
    /*
    printf ("%g\t%g\t%g\n", cov_qx/tr, cov_mixed/tr, cov_qy/tr );

    printf ("eigenvalue 1 = %g\n", eval_i);
    printf ("eigenvalue 2 = %g\n", eval_j);

    printf ("eigenvector = \n");
    gsl_vector_fprintf (stdout,
                        &evec_i.vector, "%g");

    printf ("x = %g\n", xcomp);
    printf ("y = %g\n", ycomp);
    printf ("w = %g\n", *w);
    printf ("phi = %g\n", *phi);
    */
}


#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_blas.h>

#include "su3_matrix.c"

#ifndef gauge_config
#include "config.h"
#define gauge_config 1

#define sq(x) ((x)*(x))
#define cb(x) ((x)*(x)*(x))
#endif

int acc, tot; // acceptance rate

#define FORWARD 1 //true
#define BACKWARD 0 //false

// Access pattern methods --------------------------------------------------

// index of link site=(t,x,y,z) in direction dir
int index(const int* site, int dir){
  return dir+site[0]*4+site[1]*N*4+site[2]*N*N*4+site[3]*N*N*N*4;
}

// shift site array by one to direction dir
void shift(int* site, int dir, int forward){
  if(forward == 1){
    site[dir] = (site[dir]+1)%N;
  }else{
    site[dir] = (site[dir]-1+N)%N;
  }
}

// spatial rotation by 90deg of shape array
void rotate(int n, int* shape){
  for(int i=0;i<n;i++){
    if(shape[i]==1) shape[i]=2;
    if(shape[i]==2) shape[i]=3;
    if(shape[i]==3) shape[i]=1;
  }
}

// end of access pattern methods -------------------------------------------

// Plaquette methods -------------------------------------------------------

// plaquette operator at site (t,x,y,z) in plane (mu,nu)
// normally mu>nu
double plaquette(const su3_matrix* links, int mu, int nu, int* site){
  su3_matrix A,B,C,D;
  A = links[index(site,mu)];
  shift(site,mu,FORWARD);
  B = links[index(site,nu)];
  shift(site,mu,BACKWARD);
  shift(site,nu,FORWARD);
  C = su3_inv(links[index(site,mu)]);
  shift(site,nu,BACKWARD); // restore site position
  D = su3_inv(links[index(site,nu)]);

  A = su3_mul(A,B);
  A = su3_mul(A,C);
  A = su3_mul(A,D);

  return su3_trace(A)/3.0;
}

// plaquette spatial mean
double plaq_mean_s(int t, const su3_matrix* links){
  double mean = 0;
  int site[4];
  for(int z=0;z<N;z++){
    for(int y=0;y<N;y++){
      for(int x=0;x<N;x++){
          site[0]=t; site[1]=x; site[2]=y; site[3]=z;
          for(int mu=1;mu<4;mu++){
            for(int nu=mu+1;nu<4;nu++){
              mean += plaquette(links,mu,nu,site);
            }
          }
      }
    }
  }
  return mean/ (3*N*N*N); // 3 directions & N^3 sites
}

// end of plaquette methods ------------------------------------------------

// Metropolis update --------------------------------------------------------

void do_hb_update(su3_matrix* links, const gsl_rng* r, const su3_matrix* rands, int* site, int dir);
double action(su3_matrix* links, int* site, int dir);

// the big loop
void hb_update(su3_matrix* links, const gsl_rng* r, const su3_matrix* rands){
  int site[4];
  for(int z=0;z<N;z++){
    for(int y=0;y<N;y++){
      for(int x=0;x<N;x++){
        for(int t=0;t<N;t++){
          for(int dir=0;dir<4;dir++){
            site[0]=t; site[1]=x; site[2]=y; site[3]=z;
            do_hb_update(links,r,rands,site,dir);
          }
        }
      }
    }
  }
}

// update
void do_hb_update(su3_matrix* links, const gsl_rng* r, const su3_matrix* rands, int* site, int dir){
  if(verbose) {printf("Updating {%d, %d, %d, %d} direction %d",site[0],site[1],site[2],site[3],dir);getchar();}
  double dS,action_old;
  su3_matrix link_old;
  for(int times=0; times<Nk; times++){
    action_old = action(links,site,dir);
    link_old = links[index(site,dir)];
    links[index(site,dir)]=su3_mul(links[index(site,dir)],rands[gsl_rng_uniform_int(r, 50)]);

    dS = action(links,site,dir)-action_old;

    if(verbose) {printf("Old action = %f, ",action_old); su3_print(link_old); getchar();}
    if(verbose) {printf("New action = %f, ",action_old+dS); su3_print(links[index(site,dir)]); getchar();}
    acc++;tot++;
    if(dS>0){
      if(exp(-dS)<gsl_rng_uniform(r)){
        acc--;
        links[index(site,dir)]=link_old; // revert update
        if(verbose) {printf("Reverted"); getchar();}
      }
    }
  }

}

// calculating the action
// for each direction we need to find plaquette at 2 sites in each of the 3 planes
// mu > nu
double action(su3_matrix* links, int* site, int dir){
  int mu,nu;
  double result=0;
  for(int k=0; k<4; k++){
    if(k==dir) continue; // loop over planes
    if(k>dir){mu=k; nu=dir;} // mu=larger number
    if(k<dir){mu=dir; nu=k;}

    result += plaquette(links, mu,nu,site);
    shift(site,k,BACKWARD); // shift to the perpendicular direction backwards
    result += plaquette(links, mu,nu,site);
    shift(site,k,FORWARD);  // shift it back
  }

  return -beta*result;
}

// end of heat bath update -------------------------------------------------

// Loops -------------------------------------------------------------------
// Generic loop ------------------------------------------------------------

// TODO: not working
double gloop(int n, int* shape, const su3_matrix* links, int* site){
  su3_matrix A[2*n];

  // forward
  for(int i=0;i<n;i++){
    A[i]=links[index(site,shape[i])];
    shift(site,shape[i],FORWARD);
  }

  // backward
  for(int i=0;i<n;i++){
    shift(site,shape[i],BACKWARD);
    A[n+i]=su3_inv(links[index(site,shape[i])]);
  }

  // multiply
  for(int i=1;i<2*n;i++){
    A[0]=su3_mul(A[0],A[i]);
  }

  return su3_trace(A[0])/3.0;
}

double loop(int r, int t, int mu, int nu, const su3_matrix* links, int* site){
  int shape[r+t];
  for(int i=0;i<t;i++)shape[i]=nu;
  for(int i=0;i<r;i++)shape[t+i]=mu;

  return gloop(r+t,shape,links,site);
  /*
  su3_matrix A[(r+t)*2];

  // bottom side
  for(int i=0;i<t;i++){
    A[i]=links[index(site,nu)];
    shift(site,nu,FORWARD);
  }

  // right side
  for(int i=0;i<r;i++){
    A[t+i]=links[index(site,mu)];
    shift(site,mu,FORWARD);
  }

  // top side
  for(int i=0;i<t;i++){
    shift(site,nu,BACKWARD);
    A[t+r+i]=su3_inv(links[index(site,nu)]);
  }

  // left side
  for(int i=0;i<r;i++){
    shift(site,mu,BACKWARD);
    A[t+r+t+i]=su3_inv(links[index(site,mu)]);
  }

  // multiply
  for(int i=1;i<2*(r+t);i++){
    A[0]=su3_mul(A[0],A[i]);
  }

  return su3_trace(A[0])/3.0;*/
}

// end of Loop -------------------------------------------------------------
// Wilson loop -------------------------------------------------------------

double wloop(int r, int t, int mu,const su3_matrix* links, int* site){
  return loop(r, t, mu, 0, links, site);
}

double wloop_mean(int wR, int wT, const su3_matrix* links){
  double mean = 0;
  int site[4];
  for(int z=0;z<N;z++){
    for(int y=0;y<N;y++){
      for(int x=0;x<N;x++){
        for(int t=0;t<N;t++){
            site[0]=t; site[1]=x; site[2]=y; site[3]=z;
            for(int mu=1;mu<4;mu++){
              mean += wloop(wR,wT,mu,links,site);
            }
        }
      }
    }
  }
  return mean/ (3*N*N*N*N); // 3 directions & N^3 sites
}
// end of Wilson loop ------------------------------------------------------
// Polyakov loop -----------------------------------------------------------
double polyakov(const su3_matrix* links, int* site){
  su3_matrix A[N];

  for(int i=0;i<N;i++){
    A[i]=links[index(site,0)];
    shift(site,0,FORWARD);
  }

  // multiply
  for(int i=1;i<N;i++){
    A[0]=su3_mul(A[0],A[i]);
  }

  return su3_trace(A[0])/3.0;
}

double polyakov_mean(const su3_matrix* links){
  double mean = 0;
  int site[4];
  site[0]=0;
  for(int z=0;z<N;z++){
    for(int y=0;y<N;y++){
      for(int x=0;x<N;x++){
        site[1]=x; site[2]=y; site[3]=z;
        mean += polyakov(links,site);
      }
    }
  }
  return mean/ (N*N*N);
}
// end of Polyakov loop ----------------------------------------------------

// Twisted Wilson loop -----------------------------------------------------
// Ref: Lepage p.33

// r=sqrt(2)
double twloop2(int t, int mu, int nu ,const su3_matrix* links, int* site){
  su3_matrix A[(2+t)*2];

  // bottom side
  for(int i=0;i<t;i++){
    A[i]=links[index(site,0)];
    shift(site,0,FORWARD);
  }

  // right side, twisted
  A[t+0]=links[index(site,mu)];
  shift(site,mu,FORWARD);
  A[t+1]=links[index(site,nu)];
  shift(site,nu,FORWARD);

  // top side
  for(int i=0;i<t;i++){
    shift(site,0,BACKWARD);
    A[t+2+i]=su3_inv(links[index(site,0)]);
  }

  // left side, twisted
  shift(site,mu,BACKWARD);
  A[t+2+t+0]=su3_inv(links[index(site,mu)]);
  shift(site,nu,BACKWARD);
  A[t+2+t+1]=su3_inv(links[index(site,nu)]);

  // multiply
  for(int i=1;i<2*(2+t);i++){
    A[0]=su3_mul(A[0],A[i]);
  }

  return su3_trace(A[0])/3.0;
}

double twloop2_mean(int wT, const su3_matrix* links){
  double mean = 0;
  int site[4];
  for(int z=0;z<N;z++){
    for(int y=0;y<N;y++){
      for(int x=0;x<N;x++){
        for(int t=0;t<N;t++){
            site[0]=t; site[1]=x; site[2]=y; site[3]=z;
            for(int mu=1;mu<4;mu++){
              for(int nu=1;nu<4;nu++){
                if(mu!=nu){
                  mean += twloop2(wT,mu,nu,links,site);
                }
              }
            }
        }
      }
    }
  }
  return mean/ (6*N*N*N*N); // 3 x 2 directions & N^3 sites
}

// r=sqrt(3)
double twloop3(int t, const su3_matrix* links, int* site){
  su3_matrix A[(3+t)*2];

  // bottom side
  for(int i=0;i<t;i++){
    A[i]=links[index(site,0)];
    shift(site,0,FORWARD);
  }

  // right side, twisted
  A[t+0]=links[index(site,1)];
  shift(site,1,FORWARD);
  A[t+1]=links[index(site,2)];
  shift(site,2,FORWARD);
  A[t+2]=links[index(site,3)];
  shift(site,3,FORWARD);

  // top side
  for(int i=0;i<t;i++){
    shift(site,0,BACKWARD);
    A[t+3+i]=su3_inv(links[index(site,0)]);
  }

  // left side, twisted
  shift(site,1,BACKWARD);
  A[t+3+t+0]=su3_inv(links[index(site,1)]);
  shift(site,2,BACKWARD);
  A[t+3+t+1]=su3_inv(links[index(site,2)]);
  shift(site,3,BACKWARD);
  A[t+3+t+2]=su3_inv(links[index(site,3)]);

  // multiply
  for(int i=1;i<2*(3+t);i++){
    A[0]=su3_mul(A[0],A[i]);
  }

  return su3_trace(A[0])/3.0;
}

double twloop3_mean(int wT, const su3_matrix* links){
  double mean = 0;
  int site[4];
  for(int z=0;z<N;z++){
    for(int y=0;y<N;y++){
      for(int x=0;x<N;x++){
        for(int t=0;t<N;t++){
            site[0]=t; site[1]=x; site[2]=y; site[3]=z;
            mean += twloop3(wT,links,site);
        }
      }
    }
  }
  return mean/ (N*N*N*N); // N^3 sites
}
// end of twisted Wilson loop --------------------------------------------

// Plaquette correlator
double plaq_corr(int T, const su3_matrix* links){
  double mean=0;
  for(int i=0;i<N;i++){
    mean+=plaq_mean_s(i,links)*plaq_mean_s(i+T,links);
  }
  return mean/N;
}
// end of plaquette correlator

// TODO: vvvvvvvv

// Smeared Wilson loop ---------------------------------------------------

// end of smeared Wilson loop --------------------------------------------


#include <float.h>

#include <gsl/gsl_blas.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_spmatrix.h>
#include <gsl/gsl_vector.h>
#include <osqp/osqp.h>

#include "qdm.h"

#define QDM_THETA_MIN_THRESHOLD 0.0001

int
qdm_theta_optimize(
    gsl_vector *result,
    gsl_vector *emperical_quantiles,
    gsl_matrix *ix
)
{
  int status = 0;

  gsl_matrix *p = gsl_matrix_alloc(ix->size2, ix->size2);
  gsl_matrix *a = gsl_matrix_alloc(ix->size2, ix->size2);
  gsl_vector *q = gsl_vector_alloc(ix->size2);
  gsl_vector *l = gsl_vector_alloc(ix->size2);
  gsl_vector *u = gsl_vector_alloc(ix->size2);

  // FIXME check c_malloc...
  OSQPData *data         = c_malloc(sizeof(OSQPData));
  OSQPSettings *settings = c_malloc(sizeof(OSQPSettings));
  OSQPWorkspace *work    = NULL;

  /* Ensure all data fields are initialized before use. */
  *data = (OSQPData) {
    .n = 0,
    .m = 0,
    .P = NULL,
    .A = NULL,
    .q = NULL,
    .l = NULL,
    .u = NULL,
  };

  status = qdm_matrix_tmm(ix, p);
  if (status != 0) {
    goto cleanup;
  }

  qdm_matrix_select_upper_triangle(p);

  gsl_matrix_set_identity(a);

  gsl_vector_set_all(l, QDM_THETA_MIN_THRESHOLD);
  gsl_vector_set_all(u, INFINITY);

  status = gsl_blas_dgemv(CblasTrans, -1.0, ix, emperical_quantiles, 0, q);
  if (status != 0) {
    goto cleanup;
  }

  /* Solve... */
  data->n = ix->size2;
  data->m = ix->size2;

  status = qdm_matrix_to_csc_matrix(&data->P, p);
  if (status != 0) {
    goto cleanup;
  }

  status = qdm_matrix_to_csc_matrix(&data->A, a);
  if (status != 0) {
    goto cleanup;
  }

  data->q = q->data;
  data->l = l->data;
  data->u = u->data;

  osqp_set_default_settings(settings);
  settings->verbose = 0;

  status = osqp_setup(&work, data, settings);
  if (status != 0) {
    goto cleanup;
  }

  status = osqp_solve(work);
  if (status != 0) {
    goto cleanup;
  }

  gsl_vector_view beta0 = gsl_vector_view_array(work->solution->x, ix->size2);
  status = gsl_vector_memcpy(result, &beta0.vector);
  if (status != 0) {
    goto cleanup;
  }

cleanup:
  if (data != NULL) {
    if (data->A != NULL) {
      free(data->A->p);
      free(data->A->i);
      free(data->A->x);
    }

    c_free(data->A);

    if (data->P != NULL) {
      free(data->P->p);
      free(data->P->i);
      free(data->P->x);
    }

    c_free(data->P);

    c_free(data);
  }

  c_free(settings);
  osqp_cleanup(work);

  gsl_vector_free(u);
  gsl_vector_free(l);
  gsl_vector_free(q);
  gsl_matrix_free(a);
  gsl_matrix_free(p);

  return status;
}

void
qdm_theta_matrix_constrain(
    gsl_matrix *theta,
    double min
)
{
  /* First row is constrained to be greater than zero. */
  for (size_t j = 0; j < theta->size2; j++) {
    if (gsl_matrix_get(theta, 0, j) < 0) {
      gsl_matrix_set(theta, 0, j, min);
    }
  }

  /* Remaining cells are set to zero if the sum of the column is negative.
   *
   * NOTE: The first column is not modified.
   */
  for (size_t i = 1; i < theta->size1; i++) {
    for (size_t j = 1; j < theta->size2; j++) {
      gsl_vector_view column = gsl_matrix_column(theta, j);
      double sum = qdm_vector_sum(&column.vector);

      if (sum < 0) {
        gsl_matrix_set(theta, i, j, 0);
      }
    }
  }
}


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2013.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// --------------------------------------------------------------------------
// $Maintainer: Clemens Groepl $
// $Authors: $
// --------------------------------------------------------------------------

#ifndef OPENMS_MATH_STATISTICS_LINEARREGRESSION_H
#define OPENMS_MATH_STATISTICS_LINEARREGRESSION_H

#include <OpenMS/CONCEPT/Types.h>
#include <OpenMS/CONCEPT/Exception.h>

#include <vector>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_cdf.h>

namespace OpenMS
{
  namespace Math
  {
    /**
      @brief This class offers functions to perform least-squares fits to a straight line model, \f$ Y(c,x) = c_0 + c_1 x \f$.

      It encapsulates the GSL methods for a weighted and an unweighted linear regression.

      Next to the intercept with the y-axis and the slope of the fitted line, this class computes the:
        - squared pearson coefficient
        - value of the t-distribution
        - standard deviation of the residuals
        - standard error of the slope
        - intercept with the x-axis (useful for additive series experiments)
        - lower border of confidence interval
        - higher border of confidence interval
        - chi squared value
        - x mean

      @ingroup Math
    */
    class OPENMS_DLLAPI LinearRegression
    {
public:

      /// Constructor
      LinearRegression() :
        intercept_(0),
        slope_(0),
        x_intercept_(0),
        lower_(0),
        upper_(0),
        t_star_(0),
        r_squared_(0),
        stand_dev_residuals_(0),
        mean_residuals_(0),
        stand_error_slope_(0),
        chi_squared_(0),
        rsd_(0)
      {
      }

      /// Destructor
      virtual ~LinearRegression()
      {
      }

      /**
          @brief This function computes the best-fit linear regression coefficients \f$ (c_0,c_1) \f$
          of the model \f$ Y = c_0 + c_1 X \f$ for the dataset \f$ (x, y) \f$.

          The values in x-dimension of the dataset \f$ (x,y) \f$ are given by the iterator range [x_begin,x_end)
          and the corresponding y-values start at position y_begin.

          For a  "x %" Confidence Interval use confidence_interval_P = x/100.
          For example the 95% Confidence Interval is supposed to be an interval that has a 95% chance of
          containing the true value of the parameter.

          @return If an error occurred during the fit.

          @exception Exception::UnableToFit is thrown if fitting cannot be performed
      */
      template <typename Iterator>
      void computeRegression(double confidence_interval_P, Iterator x_begin, Iterator x_end, Iterator y_begin);

      /**
          @brief This function computes the best-fit linear regression coefficient \f$ (c_0) \f$
          of the model \f$ Y = c_1 X \f$ for the dataset \f$ (x, y) \f$.

          The values in x-dimension of the dataset \f$ (x,y) \f$ are given by the iterator range [x_begin,x_end)
          and the corresponding y-values start at position y_begin.

          For a  "x %" Confidence Interval use confidence_interval_P = x/100.
          For example the 95% Confidence Interval is supposed to be an interval that has a 95% chance of
          containing the true value of the parameter.

          @return If an error occurred during the fit.

          @exception Exception::UnableToFit is thrown if fitting cannot be performed
      */
      template <typename Iterator>
      void computeRegressionNoIntercept(double confidence_interval_P, Iterator x_begin, Iterator x_end, Iterator y_begin);

      /**
          @brief This function computes the best-fit linear regression coefficients \f$ (c_0,c_1) \f$
          of the model \f$ Y = c_0 + c_1 X \f$ for the weighted dataset \f$ (x, y) \f$.

          The values in x-dimension of the dataset \f$ (x, y) \f$ are given by the iterator range [x_begin,x_end)
          and the corresponding y-values start at position y_begin. They will be weighted by the
          values starting at w_begin.

          For a  "x %" Confidence Interval use confidence_interval_P = x/100.
          For example the 95% Confidence Interval is supposed to be an interval that has a 95% chance of
          containing the true value of the parameter.

          @return If an error occurred during the fit.

          @exception Exception::UnableToFit is thrown if fitting cannot be performed
      */
      template <typename Iterator>
      void computeRegressionWeighted(double confidence_interval_P, Iterator x_begin, Iterator x_end, Iterator y_begin, Iterator w_begin);

      /// Non-mutable access to the y-intercept of the straight line
      DoubleReal getIntercept() const;
      /// Non-mutable access to the slope of the straight line
      DoubleReal getSlope() const;
      /// Non-mutable access to the x-intercept of the straight line
      DoubleReal getXIntercept() const;
      /// Non-mutable access to the lower border of confidence interval
      DoubleReal getLower() const;
      /// Non-mutable access to the upper border of confidence interval
      DoubleReal getUpper() const;
      /// Non-mutable access to the value of the t-distribution
      DoubleReal getTValue() const;
      /// Non-mutable access to the squared pearson coefficient
      DoubleReal getRSquared() const;
      /// Non-mutable access to the standard deviation of the residuals
      DoubleReal getStandDevRes() const;
      /// Non-mutable access to the residual mean
      DoubleReal getMeanRes() const;
      /// Non-mutable access to the standard error of the slope
      DoubleReal getStandErrSlope() const;
      /// Non-mutable access to the chi squared value
      DoubleReal getChiSquared() const;
      /// Non-mutable access to relative standard deviation
      DoubleReal getRSD() const;

protected:

      /// The intercept of the fitted line with the y-axis
      double intercept_;
      /// The slope of the fitted line
      double slope_;
      /// The intercept of the fitted line with the x-axis
      double x_intercept_;
      /// The lower bound of the confidence interval
      double lower_;
      /// The upper bound of the confidence interval
      double upper_;
      /// The value of the t-statistic
      double t_star_;
      /// The squared correlation coefficient (Pearson)
      double r_squared_;
      /// The standard deviation of the residuals
      double stand_dev_residuals_;
      /// Mean of residuals
      double mean_residuals_;
      /// The standard error of the slope
      double stand_error_slope_;
      /// The value of the Chi Squared statistic
      double chi_squared_;
      /// the relative standard deviation
      double rsd_;

      /// Computes the goodness of the fitted regression line
      void computeGoodness_(double * X, double * Y, int N, double confidence_interval_P);

      /// Copies the distance(x_begin,x_end) elements starting at x_begin and y_begin into the arrays x_array and y_array
      template <typename Iterator>
      void iteratorRange2Arrays_(Iterator x_begin, Iterator x_end, Iterator y_begin, double * x_array, double * y_array);

      /// Copy the distance(x_begin,x_end) elements starting at  x_begin, y_begin and w_begin into the arrays x_array, y_array and w_array
      template <typename Iterator>
      void iteratorRange3Arrays_(Iterator x_begin, Iterator x_end, Iterator y_begin, Iterator w_begin, double * x_array, double * y_array, double * w_array);

private:

      /// Not implemented
      LinearRegression(const LinearRegression & arg);
      /// Not implemented
      LinearRegression & operator=(const LinearRegression & arg);
    };

    template <typename Iterator>
    void LinearRegression::computeRegression(double confidence_interval_P, Iterator x_begin, Iterator x_end, Iterator y_begin)
    {
      int N = int(distance(x_begin, x_end));

      double * X = new double[N];
      double * Y = new double[N];
      iteratorRange2Arrays_(x_begin, x_end, y_begin, X, Y);

      double cov00, cov01, cov11;

      // Compute the unweighted linear fit.
      // Get the intercept and the slope of the regression Y_hat=intercept_+slope_*X
      // and the value of Chi squared, the covariances of the intercept and the slope
      int error = gsl_fit_linear(X, 1, Y, 1, N, &intercept_, &slope_, &cov00, &cov01, &cov11, &chi_squared_);

      if (!error)
      {
        computeGoodness_(X, Y, N, confidence_interval_P);
      }

      delete[] X;
      delete[] Y;

      if (error)
      {
        throw Exception::UnableToFit(__FILE__, __LINE__, __PRETTY_FUNCTION__, "UnableToFit-LinearRegression", "Could not fit a linear model to the data");
      }
    }

    template <typename Iterator>
    void LinearRegression::computeRegressionNoIntercept(double confidence_interval_P, Iterator x_begin, Iterator x_end, Iterator y_begin)
    {
      int N = int(distance(x_begin, x_end));

      double * X = new double[N];
      double * Y = new double[N];
      iteratorRange2Arrays_(x_begin, x_end, y_begin, X, Y);

      double cov;

      // Compute the linear fit.
      // Get the intercept and the slope of the regression Y_hat=intercept_+slope_*X
      // and the value of Chi squared, the covariances of the intercept and the slope
      int error = gsl_fit_mul(X, 1, Y, 1, N, &slope_, &cov, &chi_squared_);
      intercept_ = 0.0;

      if (!error)
      {
        computeGoodness_(X, Y, N, confidence_interval_P);
      }

      delete[] X;
      delete[] Y;

      if (error)
      {
        throw Exception::UnableToFit(__FILE__, __LINE__, __PRETTY_FUNCTION__, "UnableToFit-LinearRegression", "Could not fit a linear model to the data");
      }
    }

    template <typename Iterator>
    void LinearRegression::computeRegressionWeighted(double confidence_interval_P, Iterator x_begin, Iterator x_end, Iterator y_begin, Iterator w_begin)
    {
      int N = int(distance(x_begin, x_end));

      double * X = new double[N];
      double * Y = new double[N];
      double * W = new double[N];
      iteratorRange3Arrays_(x_begin, x_end, y_begin, w_begin, X, Y, W);

      double cov00, cov01, cov11;

      // Compute the weighted linear fit.
      // Get the intercept and the slope of the regression Y_hat=intercept_+slope_*X
      // and the value of Chi squared, the covariances of the intercept and the slope
      int error = gsl_fit_wlinear(X, 1, W, 1, Y, 1, N, &intercept_, &slope_, &cov00, &cov01, &cov11, &chi_squared_);

      if (!error)
      {
        computeGoodness_(X, Y, N, confidence_interval_P);
      }

      delete[] X;
      delete[] Y;
      delete[] W;

      if (error)
      {
        throw Exception::UnableToFit(__FILE__, __LINE__, __PRETTY_FUNCTION__, "UnableToFit-LinearRegression", "Could not fit a linear model to the data");
      }
    }

    template <typename Iterator>
    void LinearRegression::iteratorRange2Arrays_(Iterator x_begin, Iterator x_end, Iterator y_begin, double * x_array, double * y_array)
    {
      int i = 0;
      while (x_begin < x_end)
      {
        x_array[i] = *x_begin;
        y_array[i] = *y_begin;
        ++x_begin;
        ++y_begin;
        ++i;
      }
    }

    template <typename Iterator>
    void LinearRegression::iteratorRange3Arrays_(Iterator x_begin, Iterator x_end, Iterator y_begin, Iterator w_begin, double * x_array, double * y_array, double * w_array)
    {
      int i = 0;
      while (x_begin < x_end)
      {
        x_array[i] = *x_begin;
        y_array[i] = *y_begin;
        w_array[i] = *w_begin;
        ++x_begin;
        ++y_begin;
        ++w_begin;
        ++i;
      }
    }

  } // namespace Math
} // namespace OpenMS

#endif


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <assert.h>
#include <math.h>
#include <sys/time.h>
#include <mm_malloc.h>
#include <cblas.h>
#include <lapacke.h>

#include "random_matrix.h"
#include "driver.h"
#include "../../runtime/pcp.h"

static double compare_lower_triangular_matrices(int n, double *L1, int ldL1, double *L2, int ldL2)
{
#define L1(i,j) L1[(i) + (j) * ldL1]
#define L2(i,j) L2[(i) + (j) * ldL2]

    // Clear out the upper triangular part.
    for (int j = 0; j < n; ++j) {
        for (int i = 0; i < j; i++) {
            L1(i,j) = 0.0;
            L2(i,j) = 0.0;
        }
    }

    // Compute L1 := L1 - L2.
    for (int j = 0; j < n; j++) {
        for (int i = 0; i < n; i++) {
            L1(i,j) = L1(i,j) - L2(i,j);
        }
    }

    // Compute || L1 ||_F.
    double error = LAPACKE_dlange(LAPACK_COL_MAJOR, 'F', n, n, L1, ldL1);
    error /= n;
    return error;

#undef L1
#undef L2
}

static void usage(char *prog)
{
    printf("Usage: %s n b p q\n", prog);
    printf("\n");
    printf("n: The size of the matrix\n");
    printf("b: The tile size\n");
    printf("p: The number of threads (or 0 for one per core)\n");
    printf("q: The size of the reserved set (or 0 to use the regular mode)\n");
    exit(EXIT_FAILURE);
}

int main(int argc, char** argv)
{
    // Verify the number of command line options.
    if (argc != 5 && argc != 6) {
        usage(argv[0]);
    }

    // Parse command line.
    int n = atoi(argv[1]);
    int blksz = atoi(argv[2]);
    int num_threads = atoi(argv[3]);
    int reserved_set_size = atoi(argv[4]);
    int verify = 1;
    if (argc == 6) {
        verify = 0;
    }

    // Verify options.
    if (n < 1 ||
        blksz < 1 ||
        num_threads < 0 ||
        reserved_set_size < 0 ||
        (reserved_set_size >= num_threads && num_threads > 0)) {
        usage(argv[0]);
    }

    // Initialise random number generator.
    srand(time(NULL));

    // Generate random SPD matrix A.
    printf("Generating SPD matrix A...\n");
    int ldA = n;
    double *A = (double *) malloc(n * ldA * sizeof(double));
    generate_dense_spd_matrix(n, A, ldA);

    // Copy A to Ain.
    printf("Copying A to Ain...\n");
    double *Ain = (double *) malloc(n * ldA * sizeof(double));
    memcpy(Ain, A, n * ldA * sizeof(double));

    // Start the runtime system.
    pcp_start(num_threads);

    // Set the execution mode.
    if (reserved_set_size == 0) {
        pcp_set_mode(PCP_REGULAR);
    } else {
        pcp_set_mode(PCP_FIXED);
        pcp_set_reserved_set_size(reserved_set_size);
    }

    // Call the driver.
    printf("Calling the driver routine...\n");
    parallel_block_chol(n, blksz, A, ldA);

    if (verify) {
        // Verify the solution.
        printf("Verifying the solution... ");
        fflush(stdout);
        double tm = pcp_get_time();
        LAPACKE_dpotrf(LAPACK_COL_MAJOR, 'L', n, Ain, ldA);
        tm = pcp_get_time() - tm;
        double error = compare_lower_triangular_matrices(n, Ain, ldA, A, ldA);
        if (error < 1e-14) {
            printf("PASSED\n");
        } else {
            printf("FAILED\n");
        }
        printf("LAPACK_dpotrf took %.6lf seconds\n", tm);
    } else {
        printf("Verification disabled\n");
    }

    // Save the trace.
    printf("Saving the trace...\n");
    pcp_view_trace_tikz();

    // View statistics.
    printf("Printing statistics...\n");
    pcp_view_statistics_stdout();

    // Stop the runtime system.
    pcp_stop();

    // Clean up.
    free(A);
    free(Ain);

    return EXIT_SUCCESS;
}


#ifndef TOY_VN_GENERATOR_H
#define TOY_VN_GENERATOR_H

#include <iostream>
#include <fstream>
#include <string>
#include <cmath>
#include <complex>
#include <vector>
#include <iomanip>
#include <stdio.h>
#include <algorithm>
#include <chrono>
#include <random>
#include <omp.h>
#include "sampled_distribution.h"
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>

using namespace std;

namespace flow_generator
{
	// ------------------------------------
	// Initialize default parameters here.
	// ------------------------------------

	int n_pT                               = 6;
	double max_pT                          = 3000000.0;
	double min_pT				           = 0.0;
	int order                              = 2;
	//double eta_low                         = 2.0;
	//double eta_high                        = 10.0;

	int N_events_to_generate               = 1000;
	int N_particles_per_event              = 100;
	long N_total_events                    = 0;
	long N_running_total_events            = 0;

	bool use_seed                          = true;
	int seed_index                         = -1;

	bool print_randomly_generated_data     = false;

	bool include_psi2_PTslope_fluctuations = false;
	double v2_magnitude                    = 0.1;

	string resultsDirectory                = "./results/";
	string output_name                     = resultsDirectory + "/data.dat";

	// ------------------------------------
	// Define structs and vectors.
	// ------------------------------------

	// struct for event information
	typedef struct
	{
		int eventID;
		int Nparticles;

		vector<vector<double> > eta_vec;
		vector<vector<double> > pphi_vec;
		vector<vector<double> > pT_vec;
	} event_data;

	// declare needed vectors
	vector<event_data>                         all_events;

	vector<vector<double> >                    pT_vec;
	vector<vector<double> >                    eta_vec;
	vector<vector<double> >                    phi_vec;
	vector<vector<int> >                       event_vec;

	//---------------------------------------------------------
	// Generate data randomly to mimic (non-factorizable) flow.
	void generate_random_data(int dataset=-1)
	{
		double delta_pT = (max_pT-min_pT)/(n_pT);

		pT_vec.clear();
		eta_vec.clear();
		phi_vec.clear();
		event_vec.clear();
		pT_vec = vector<vector<double> >(n_pT, vector<double> (0));
		eta_vec = vector<vector<double> >(n_pT, vector<double> (0));
		phi_vec = vector<vector<double> >(n_pT, vector<double> (0));
		event_vec = vector<vector<int> >(n_pT, vector<int> (0));

		// Set up distributions
		unsigned seed = chrono::system_clock::now().time_since_epoch().count();
		default_random_engine generator;
		if ( use_seed )
			generator = default_random_engine(seed);
		else
			generator = default_random_engine(seed_index);

		uniform_real_distribution<double> mean_pT_distribution(0.90, 1.10);
		//const double mean_pT = mean_pT_distribution(generator);
		const double mean_pT = 1.0;
		exponential_distribution<double> pT_distribution( mean_pT );
		normal_distribution<double> eta_distribution(0.0, 2.0);
		uniform_real_distribution<double> v2_distribution(0.05, 0.45);
		uniform_real_distribution<double> psi2_distribution(0.0, 2.0*M_PI);
		uniform_real_distribution<double> pphi_distribution(0.0, 2.0*M_PI);
		normal_distribution<double> psi2_pTslope_distribution(0.0,0.1);

		auto phiFunc = [](double x, double v2) { return x + v2*sin(2.0*x); };
	    std::mt19937 gen;
		if ( use_seed )
			gen = std::mt19937(seed);
		else
			gen = std::mt19937(seed_index);

		double fluctuation_switch_factor
				= double(include_psi2_PTslope_fluctuations);

		for (int iEvent = 0; iEvent < N_events_to_generate; iEvent++)
		{

			//double v2 = v2_distribution(generator);
			double v2 = v2_magnitude;
			double psi2 = 0.0*psi2_distribution(generator);
			double psi2_pTslope = fluctuation_switch_factor
									* psi2_pTslope_distribution(generator);

			for (int iParticle = 0; iParticle < N_particles_per_event; iParticle++)
			{
				// ---------------------------------------------
				// Try to mimic different types of flow effects
				// ---------------------------------------------
				const double pT_value = pT_distribution(generator);
			    //Sampled_distribution<> dist(phiFunc, 0.0, 2.0*M_PI, v2*pT_value / (mean_pT+pT_value));
			    Sampled_distribution<> dist(phiFunc, 0.0, 2.0*M_PI, v2);
				const double pphi_value = dist(gen) + psi2 + psi2_pTslope*pT_value;
				//const double pphi_value = dist(gen) + psi2 + 0.5*M_PI*pT_value / (mean_pT+pT_value);

				// Sort into bins.
				double pT = pT_value;
				double eta = 0.0*eta_distribution(generator);
				double phi = pphi_value;
				int event = iEvent;

				if(pT > max_pT)
				{
					//cout << "Warning: pT = " << pT << " larger than max_pT = " << max_pT << " in event = " << event << endl;
					continue;
				}
				if(pT < min_pT)
				{
					//cout << "Warning: pT = " << pT << " smaller than min_pT = " << min_pT << " in event = " << event << endl;
					continue;
				}
				else
				{
					int bin = int((pT-min_pT)/delta_pT);
					eta_vec[bin].push_back(eta);
					phi_vec[bin].push_back(phi);
					pT_vec[bin].push_back(pT);
					event_vec[bin].push_back(event);
					//cout << "Check: " << pT << "   " << eta << "   " << phi << "   " << event << " " << bin << endl;
				}
			}
		}

		//----------------------------------------------
		all_events.clear();
		all_events = vector<event_data>(N_events_to_generate);
		for ( auto & this_event : all_events )
		{
			this_event.eta_vec = vector<vector<double> >( n_pT, vector<double>(0) );
			this_event.pphi_vec = vector<vector<double> >( n_pT, vector<double>(0) );
			this_event.pT_vec = vector<vector<double> >( n_pT, vector<double>(0) );
		}
		for (int bin = 0; bin < n_pT; bin++)
		{
			const int n_particles_in_this_bin = event_vec[bin].size();
			for (int iParticle = 0; iParticle < n_particles_in_this_bin; iParticle++)
			{
				int event_ID_of_this_particle = event_vec[bin][iParticle];

				all_events[event_ID_of_this_particle].eta_vec[bin].push_back( eta_vec[bin][iParticle] );
				all_events[event_ID_of_this_particle].pphi_vec[bin].push_back( phi_vec[bin][iParticle] );
				all_events[event_ID_of_this_particle].pT_vec[bin].push_back( pT_vec[bin][iParticle] );
				all_events[event_ID_of_this_particle].eventID = event_ID_of_this_particle;
			}
		}
		//----------------------------------------------

		//----------------------------------------------
		// Tally number of completed events
		N_running_total_events += N_events_to_generate;
		cout << "Generated " << N_particles_per_event * N_events_to_generate
				<< " particles belonging to "
				<< N_events_to_generate << " events."
				<< endl;
		cout << "Currently analyzed " << N_running_total_events
				<< " / " << N_total_events << " events." << endl;

		//----------------------------------------------
		// Dump simulated data to file if desired
		if ( print_randomly_generated_data )
		{
			int n_zero = static_cast<int>( log10( N_particles_per_event*N_total_events ) )-1;
			string dataset_stem = ( dataset >= 0 ) ?
									std::string(n_zero - std::to_string(dataset).length(), '0')
									+ std::to_string( dataset ) : "";
			string RNG_filename = resultsDirectory + "/event_" + dataset_stem + ".dat";
			ofstream RNG_output( RNG_filename.c_str() );
			for ( auto & this_event : all_events )
			for (int bin = 0; bin < n_pT; bin++)
			{
				const int n_particles_in_this_bin = this_event.eta_vec[bin].size();
				const auto & this_pT_bin = this_event.pT_vec[bin];
				const auto & this_pphi_bin = this_event.pphi_vec[bin];
				const auto & this_eta_bin = this_event.eta_vec[bin];
				for (int iParticle = 0; iParticle < n_particles_in_this_bin; iParticle++)
					RNG_output << setprecision(15) << setw(18)
							   /*<< this_pT_bin[iParticle] << "   "
                               << this_eta_bin[iParticle] << "   "*/
                               << this_pphi_bin[iParticle] /*<< "   "
                               << this_event.eventID*/ << endl;
			}
			RNG_output.close();
		}

		return;
	}

}

#endif


/*
 * SPDX-License-Identifier:  BSD-3-Clause
 *
 * example_02-StructOfArrays-Naive-Omp.c :
 *      Example of SPH Density Calculation using a
 *      naive implementation of the main density loop,
 *      no neighbours earch, and Struct of Arrays (SoA)
 *      data layout and OpenMP parallelization.
 *
 * (C) Copyright 2021 José Hugo Elsas
 * Author: José Hugo Elsas <jhelsas@gmail.com>
 *
 * Command Line Options:
 *   -runs  <int>   : Set the number of repetitions (runs) for
 *                      calculating the density. The value of
 *                      the density is based on the last
 *                      iteration.
 *                    Default value: 1
 *   -run_seed <int>: Flag to set an alternative seed use for
 *                      for the PRNG. Instead of feeding seed
 *                      to the PRNG directly, it feeds
 *                      seed + iteration, as to generate different
 *                      configurations for each iteration.
 *                    Default value: 0 - (possible 0/1)
 *   -seed     <int>: Set the seed to use for the SPH particles
 *                      uniform position generation in the box
 *                    Default value: 123123123
 *
 *   -N        <int>: Set the number of SPH particles to be used
 *                    Default value: 1e5 = 100,000
 *   -h      <float>: Set the value of the smoothing kernel
 *                      parameter h, which corresponds to half
 *                      of the support of the kernel.
 *                    Default value: 0.05
 *
 *   -Nx       <int>: Set the number of Cells in the X direction
 *                    Default value: 10
 *   -Ny       <int>: Set the number of Cells in the Y direction
 *                    Default value: 10
 *   -Nz       <int>: Set the number of Cells in the Z direction
 *                    Default value: 10
 *
 *   -Xmin   <float>: Set the lower bound in the X direction for
 *                      the Cell Linked List box
 *                    Default value: 0.0
 *   -Ymin   <float>: Set the lower bound in the Y direction for
 *                    the Cell Linked List box
 *                      Default value: 0.0
 *   -Ymin   <float>: Set the lower bound in the Z direction for
 *                      the Cell Linked List box
 *                    Default value: 0.0
 *
 *   -Xmax   <float>: Set the lower bound in the X direction for
 *                      the Cell Linked List box
 *                    Default value: 1.0
 *   -Ymax   <float>: Set the lower bound in the Y direction for
 *                      the Cell Linked List box
 *                    Default value: 1.0
 *   -Zmax   <float>: Set the lower bound in the Z direction for
 *                      the Cell Linked List box
 *                    Default value: 1.0
 */

#include <math.h>
#include <ctype.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <limits.h>
#include <unistd.h>
#include <stdbool.h>
#include <sys/time.h>
#include <inttypes.h>

#include <omp.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_heapsort.h>

#include "sph_data_types.h"
#include "sph_linked_list.h"
#include "sph_utils.h"

#ifndef M_PI
#define M_PI (3.14159265358979323846)
#endif

#define COMPUTE_BLOCKS 1

int main_loop(int run, bool run_seed, int64_t N, double h, long int seed,
              void *swap_arr, linkedListBox *box, SPHparticle *lsph, double *times);

int compute_density_3d_naive_omp(int N,double h,
                                 double* restrict x, double* restrict y,
                                 double* restrict z, double* restrict nu,
                                 double* restrict rho);

double w_bspline_3d(double r,double h);

int main(int argc, char **argv){
  bool run_seed = false;       // By default the behavior is is to use the same seed
  int runs = 1,err;            // it only runs once
  long int seed = 123123123;   // The default seed is 123123123
  int64_t N = 100000;          // The default number of particles is N = 1e5 = 100,000
  double h=0.05;               // The default kernel smoothing length is h = 0.05
  linkedListBox *box;          // Uninitialized Box containing the cells for the cell linked list method
  SPHparticle *lsph;           // Uninitialized array of SPH particles

  box = (linkedListBox*)malloc(1*sizeof(linkedListBox)); // Create a box

  // allow for command line customization of the run
  arg_parse(argc,argv,&N,&h,&seed,&runs,&run_seed,box);  // Parse the command
                                                         // line arguments and override default values

  err = SPHparticle_SoA_malloc(N,&lsph);                 // Create an array of N particles
  if(err)
    fprintf(stderr,"error in SPHparticle_SoA_malloc\n");

  void *swap_arr = malloc(N*sizeof(double));
  double times[runs*COMPUTE_BLOCKS];

  for(int run=0;run<runs;run+=1)
    main_loop(run,run_seed,N,h,seed,swap_arr,box,lsph,times);

  bool is_cll = false;
  const char *prefix = "ex02,naive,SoA,omp";
  print_time_stats(prefix,is_cll,N,h,seed,runs,lsph,box,times);
  print_sph_particles_density(prefix,is_cll,N,h,seed,runs,lsph,box);

  SPHparticleSOA_safe_free(N,&lsph);
  safe_free_box(box);
  free(swap_arr);

  return 0;
}

/*
 *  Function main_loop:
 *    Runs the main loop of the program, including the particle array generation,
 *    density calculation and the timings annotations.
 *
 *    Arguments:
 *       run <int>            : index (or value) or the present iteration
 *       run_seed <bool>      : boolean defining whether to use run index for seed or not
 *       N <int>              : Number of SPH particles to be used in the run
 *       h <double>           : Smoothing Length for the Smoothing Kernel w_bspline
 *       seed <long int>      : seed for GSL PRNG generator to generate particle positions
 *       box  <linkedListBox> : Box of linked list cells, encapsulating the 3d domain
 *       lsph <SPHparticle>   : Array (pointer) of SPH particles to be updated
 *       times <double>       : Array to store the computation timings to be updated
 *    Returns:
 *       0                    : error code returned
 *       lsph <SPHparticle>   : SPH particle array is updated in the rho field by reference
 *       times <double>       : Times is updated by reference
 */
int main_loop(int run, bool run_seed, int64_t N, double h, long int seed,
              void *swap_arr, linkedListBox *box, SPHparticle *lsph, double *times)
{
  int err;

  // Initialize the particles' positions and other values
  if(run_seed)
    err = gen_unif_rdn_pos_box(N,seed+run,box,lsph);
  else
    err = gen_unif_rdn_pos_box(N,seed,box,lsph);

  if(err)
    fprintf(stderr,"error in gen_unif_rdn_pos\n");

  // ------------------------------------------------------ //

  double t0,t1;

  t0 = omp_get_wtime();

  compute_density_3d_naive_omp(N,h,lsph->x,lsph->y,lsph->z,
                               lsph->nu,lsph->rho);         // Compute the density for all particles

  t1 = omp_get_wtime();

  // ------------------------------------------------------ //

  times[COMPUTE_BLOCKS*run+0] = t1-t0;                      // Only one component to measure time

  return 0;
}

/*
 *  Function compute_density_3d_naive_omp:
 *    Computes the SPH density from the particles naively (i.e. direct loop).
 *    The outer-most loop is parallelized using openMP.
 *
 *    Arguments:
 *       N <int>              : Number of SPH particles to be used in the run
 *       h <double>           : Smoothing Length for the Smoothing Kernel w_bspline
 *       x       <double*>    : Array of particles' X positions
 *       y       <double*>    : Array of particles' Y positions
 *       z       <double*>    : Array of particles' Z positions
 *       nu      <double*>    : Array of particles' density weights (i.e. masses)
 *    Returns:
 *       0                    : error code returned
 *       rho       <double*>  : Array of particles' densities
 */
int compute_density_3d_naive_omp(int N,double h,
                                 double* restrict x, double* restrict y,
                                 double* restrict z,double* restrict nu,
                                 double* restrict rho){
  memset(rho,(int)0,N*sizeof(double));        // Pre-initialize the density to zero

  #pragma omp parallel for                    // Iterate in parallel
  for(int64_t ii=0;ii<N;ii+=1){               // For every particle
    for(int64_t jj=0;jj<N;jj+=1){             // Run over every other particle
      double dist = 0.;                       // the contributions from each direction by

      dist += (x[ii]-x[jj])*(x[ii]-x[jj]);    // adding the contribution in the X direction
      dist += (y[ii]-y[jj])*(y[ii]-y[jj]);    // adding the contribution in the Y direction
      dist += (z[ii]-z[jj])*(z[ii]-z[jj]);    // adding the contribution in the Z direction

      dist = sqrt(dist);                      // take the sqrt to have the distance

      rho[ii] += nu[jj]*w_bspline_3d(dist,h); // and add the contribution to the density
    }
  }

  return 0;
}

/*
 *  Function w_bspline_3d:
 *    Returns the normalized value of the cubic b-spline SPH smoothing kernel
 *
 *    Arguments:
 *       q <double>           : Distance between particles
 *       h <double>           : Smoothing Length for the Smoothing Kernel w_bspline
 *    Returns:
 *       wq <double>          : Normalized value of the kernel
 */
double w_bspline_3d(double r,double h){
  const double A_d = 3./(2.*M_PI*h*h*h);      // The 3d normalization constant
  double q=0.;                                // normalized distance, initialized to zero

  if(r<0||h<=0.)                              // If either distance or smoothing length
    exit(10);                                 // are negative, declare an emergency

  q = r/h;                                    // Compute the normalized distance
  if(q<=1)                                    // If the distance is small
    return A_d*(2./3.-q*q + q*q*q/2.0);       // Compute this first polynomal
  else if((1.<=q)&&(q<2.))                    // If the distance is a bit larger
    return A_d*(1./6.)*(2.-q)*(2.-q)*(2.-q);  // Compute this other polynomial
  else                                        // Otherwise, if the distance is large
    return 0.;                                // The value of the kernel is 0
}


/*
 *  hubble.c
 *
 *  written by christian wagner
 *
 */

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_odeiv.h>
#include <gsl/gsl_errno.h>

#define VC 299792.458
#define TWOPI 6.283185307179586
#define PI    3.141592653589793
#define Tcmb  2.725
#define pow2(x) ((x)*(x))
#define pow3(x) ((x)*(x)*(x))
#define pow4(x) ((x)*(x)*(x)*(x))

#include "hubble.h"

double hubble(struct cosmo mycosmo, double a){
  double H0=mycosmo.hub*100.;
  double Omega_m = mycosmo.wm/pow2(mycosmo.hub);
  double Omega_r  = 4.15e-5/pow2(mycosmo.hub);
  double Omega_X  = 1.-Omega_m;
  double Omega_k = 0.;

  return  H0*sqrt(Omega_r/pow4(a)+Omega_m/pow3(a)+Omega_k/pow2(a)+Omega_X*pow(a,-3*(1+mycosmo.w0+mycosmo.wa))*exp(-3.*mycosmo.wa*(1.-a)));

}

double co_distance_int(double a,void * params){
    double t;
    struct cosmo mycosmo = *(struct cosmo *) params;

    return  1./(a*a*hubble(mycosmo,a));

}

double co_distance(struct cosmo mycosmo, double a){

  gsl_function  F;
   double result;
   double epsabs=0.001;
   double epsrel=0.001;
   double abserr;
   size_t neval;

   F.function = &co_distance_int;
   F.params = &mycosmo;

   gsl_integration_qng (&F, a, 1., epsabs, epsrel, &result, &abserr, &neval);

   return result*VC*mycosmo.hub;

}

double ang_dist(struct cosmo mycosmo, double a){
  return co_distance(mycosmo,a); // assuming flatness
}

int
ode_growth (double a, const double y[], double f[], void *params)
{

  struct cosmo mycosmo = * (struct cosmo *) params;

  double w_a=mycosmo.w0+(1-a)*mycosmo.wa;
  double Omega_m=mycosmo.wm/pow2(mycosmo.hub);
  double Omega_X=1-Omega_m;
  double Xvar=(1-Omega_m)*pow(a,-3.*(1+mycosmo.w0+mycosmo.wa))*exp(-3.*mycosmo.wa*(1.-a))/(Omega_m/(a*a*a)+(1-Omega_m)*pow(a,-3.*(1+mycosmo.w0+mycosmo.wa))*exp(-3.*mycosmo.wa*(1.-a)));

    f[0] = y[1];
    f[1] = -1.5*(1.-w_a)*Xvar*y[0]/(a*a) - (3.5-1.5*w_a*Xvar)*y[1]/a ;

    return GSL_SUCCESS;
}

int
jac_growth (double a, const double y[], double *dfdy,
     double dfdt[], void *params)
{
    return GSL_SUCCESS;
}

double growth(struct cosmo mycosmo, double a){

    const gsl_odeiv_step_type * T
	= gsl_odeiv_step_rkf45;

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

    double mu = 10;
    gsl_odeiv_system sys = {ode_growth, jac_growth, 2, &mycosmo};

    double  t1=a;
    double  t = 1.E-12;

    double d1;

    double h = 1e-12;
    double y[2] = { 1.0, 0.0 };

    int status;

    while (t < t1){
      status = gsl_odeiv_evolve_apply (e, c, s,
					   &sys,
					   &t, t1,
					   &h, y);
      if  (h>0.001)
	h=0.001;

      if (status != GSL_SUCCESS)
	break;

    }

    d1=y[0]*(t);

    gsl_odeiv_evolve_free (e);
    gsl_odeiv_control_free (c);
    gsl_odeiv_step_free (s);

    return d1;
}

double z_lastscattering(struct cosmo mycosmo){
  /*
  z_lastscattering(cosmo):
  Returns z_LS from Hu & White, DampingTail paper.
  */
  double wm = mycosmo.wm;
  double wb = mycosmo.wb;
  double b1 = 0.0783*pow(wb,(-0.238))/(1+39.5*pow(wb,0.763));
  double b2 = 0.560/(1+21.1*pow(wb,1.81));
  double zls= 1048.*(1+0.00124*pow(wb,(-0.738)))*(1+b1*pow(wm,b2));
  return zls;
}

double soundhorizon(struct cosmo mycosmo){
  /*
  soundhorizon(cosmo):
  A fit to the sound horizon, in Mpc, from Hu & Sugiyama (1995), Eq. B6
  */
  double wm = mycosmo.wm;
  double wb = mycosmo.wb;
  double wg = 2.4888e-5*pow4(Tcmb/2.73);
  double r0 = 0.75*wb/wg;
  double zeq= 5464.0*(wm/0.135)/pow4(Tcmb/2.725)/(1+0.6851)-1.0;
  double req= r0/(1.+zeq);
  double zLS= z_lastscattering(mycosmo);
  double rls= r0/(1+zLS);
  double tmp= (sqrt(1.+rls)+sqrt(rls+req))/(1.+sqrt(req));
  tmp= 3997.0*sqrt(wg/wm/wb)*log(tmp);
  return(tmp);
}

double distls(struct cosmo mycosmo){
  /*
  distls(cosmo):
  Returns the distance to LS, in Mpc.
  */
  double zLS = z_lastscattering(mycosmo);
  double dLS = ang_dist(mycosmo,1./(1.+zLS))/mycosmo.hub;
  return(dLS);
}

double solvehh(double dLS,struct cosmo mycosmo){
  /*
  solvehh(dLS,cosmo):
  Solves for h given the other cosmological parameters and the distance
  to last scattering (in Mpc).
  */
  struct cosmo hmin = mycosmo;
  hmin.hub = 0.3;
  // printf("min_cosmo: %g %g %g %g %g\n",hmin.hub,hmin.w0,hmin.wa,hmin.wm,hmin.wb);
  double ymin = distls(hmin)-dLS;
  //  printf("ymin: %g\n",ymin);
  struct cosmo hmax = mycosmo;
  hmax.hub = 1.0;
  double ymax = distls(hmax)-dLS;
  //  printf("ymax: %g\n",ymax);
  double ymid;
  struct cosmo hmid = mycosmo;
  while (fabs(hmax.hub-hmin.hub)>1e-3){
    hmid.hub = (hmax.hub+hmin.hub)/2.0;
    ymid = distls(hmid)-dLS;
    if (ymin*ymid<0){
	hmax = hmid;
	ymax = ymid;
      } else {
      hmin = hmid;
      ymin = ymid;
    }
  }
  //  printf("hubble from CMB: %g\n",hmid.hub);
  return hmid.hub;
}

//void getH0fromCMB(double *xstar, double myh, double *rs, double *z_lss, double *d_lss, double *h_CMB, int *writeout, char *fname){
void getH0fromCMB(double *xstar, double *stuff){

  FILE *fp;

  struct cosmo mycosmo={ 0.72, xstar[3], 0, xstar[1], xstar[0]};

  double rs=soundhorizon(mycosmo);
  stuff[0] = rs;

  double z_lss=z_lastscattering(mycosmo);
  stuff[1] = z_lss;

  double d_lss=302.4*rs/PI;
  stuff[2] = d_lss;

  double hubble_cmb= solvehh(d_lss,mycosmo);
  stuff[3] = hubble_cmb;

}

// Linker function for use with Fortran

void geth0fromcmb_(double *xstar, double *stuff){
  void getH0fromCMB();
  getH0fromCMB(xstar,stuff);
}


/*******************************************************************
FUNCTION NAME:   interpolate - performs the interpolation to calculate
                 the pixel value e.g. during resampling

INPUT: interpolation  - interpolation method (nearest neighbor, bilinear, sinc)
       inbuf          - input image buffer
       nLines         - number of lines (image buffer)
       nSamples       - number of samples per line (image buffer)
       xLine          - line of interest
       xSample        - sample position within line
       weighting      - weighting function to be applied (Kaiser, Hamming)
       nKernel        - kernel size for sinc function
*******************************************************************/
#include <assert.h>
#include <math.h>

#include <glib.h>
#include <gsl/gsl_sf_bessel.h>
#include "asf.h"
#include "asf_raster.h"
#include "float_image.h"

#define SQR(X) ((X)*(X))

// Some parameters and weights for bicubic interpolation
#define CUBE(X) ((X)*(X)*(X))
#define P(X) ( X > 0  ? X : 0 )

// For sinc interpolation, the goal is to weight source values using a
// sinc function.  We want to position the peak of the sinc at the
// point of interest.  The sinc filter itself will interpolate a small
// number of point, perhaps 8 or 16, for example.  In order to achieve
// the desired location of the peak, we want to introduce a shift in
// the sinc function computed.  However, we don't want to recompute
// the sinc function for every point to be interpolated!  The solution
// is to precompute an array of slightly shifted sinc functions, and
// then choose the appropriate one for the point we are interpolating.
// For example, if we have known values for 1 and 2, and we want to
// interpolate a value for 1.2, we choose the (1.2 - 1.0) * (2 - 1) *
// NUM_SINCS sinc function to evaluate the result.
#define NUM_SINCS 512 // Number of sinc functions to compute.

// For convenience using buffered image functions
#define GET_PIXEL(x, y) float_image_get_pixel (inbuf, x, y)
#define SET_PIXEL(x, y, value) float_image_set_pixel (inbuf, x, y, value)

// Fetch pixel ii, jj from inbuf, where inbuf is taken to be an image
// nSamples wide and nLines high.  If ii, jj is outside the image, a
// "reflected" value is returned.  For example, these two calls will
// return the same pixel:
//
//     get_pixel_with_reflection (inbuf, sample_count, line_count, -1, -2)
//     get_pixel_with_reflection (inbuf, sample_count, line_count, 1, 2);
//
// If the reflected indicies are still outside the image, an exception
// is triggered.

//static float
//get_pixel_with_reflection (float *inbuf, int nSamples, int nLines, int sample,
//			   int line)
//{
//  int ii = sample;		// Convenience alias.
//  int jj = line;		// Convenience alias.
//  // Handle reflection at image edges.
//  if ( G_UNLIKELY (ii < 0) ) {
//    ii = -ii;
//  }
//  else if ( G_UNLIKELY (ii >= nSamples) ) {
//    ii = ii - (ii - nSamples + 1) - 1;
//  }
//  if ( G_UNLIKELY (jj < 0) ) {
//    jj = -jj;
//  }
//  else if ( G_UNLIKELY (jj >= nLines) ) {
//    jj = jj - (jj - nLines + 1) - 1;
//  }
//  // Now we better be in the image.
//  assert (ii >= 0 && ii < nSamples);
//  assert (jj >= 0 && jj < nLines);
//
//  return inbuf[nSamples * jj + ii];
//}

void samples2coefficients(FloatImage *inbuf, char dimension)
{
  int ii, kk;
  float z = sqrt(3.0) - 2.0;
  float lambda = (1.0 - z) * (1.0 - 1.0/z);
  float value;

  // Apply overall gain
  for (ii=0; ii<inbuf->size_x; ii++)
    for (kk=0; kk<inbuf->size_y; kk++) {
      value = GET_PIXEL(ii,kk) * lambda;
      SET_PIXEL(ii,kk,value);
    }

  for (kk=0; kk<inbuf->size_y; kk++) {
    // Causal initialization (first pixel of line)

  }

  // Causal recursion

  // Anticausal initialization

  // Anticausal recursion

}

float interpolate(interpolate_type_t interpolation, FloatImage *inbuf, float yLine,
		  float xSample, weighting_type_t weighting, int sinc_points)
{
  int ix, iy, ii, kk, ll, xI[4], yI[4];
  float a00, a10, a01, a11;
  float value, dx, dy, wx[4], wy[4], w;

  /* Get closed pixel position to start from */

  switch ( interpolation )
    {
    case NEAREST:
      ix = (int) (xSample + 0.5);
      iy = (int) (yLine + 0.5);
      value = GET_PIXEL(ix,iy);
      break;

    case BILINEAR:
      assert (xSample >= 0.0);
      assert (yLine >= 0.0);
      assert (xSample <= inbuf->size_x - 1);
      assert (yLine <= inbuf->size_y - 1);
      ix = floor(xSample);
      iy = floor(yLine);

      a00 = GET_PIXEL(ix,iy);
      a10 = GET_PIXEL(ix+1,iy) - GET_PIXEL(ix,iy);
      a01 = GET_PIXEL(ix,iy+1) - GET_PIXEL(ix,iy);
      a11 = GET_PIXEL(ix,iy) - GET_PIXEL(ix+1,iy) - GET_PIXEL(ix,iy+1)
	+ GET_PIXEL(ix+1,iy+1);
      value = (a00 + a10 * (xSample - ix) + a01 * (yLine - iy)
	       + a11 * (xSample - ix) * (yLine - iy));
      break;

    case BICUBIC:
      assert (xSample >= 0.0);
      assert (yLine >= 0.0);
      assert (xSample <= inbuf->size_x - 1);
      assert (yLine <= inbuf->size_y - 1);
      ix = floor(xSample);
      iy = floor(yLine);
      dx = xSample - ix;
      dy = yLine - iy;

      // Calculating weights
      for (ii=-1; ii<=2; ii++) {
	wx[ii+1] = 1.0/6.0 * (CUBE(P(ii-dx+2)) - 4*CUBE(P(ii-dx+1))
			      + 6*CUBE(P(ii-dx)) - 4*CUBE(P(ii-dx-1)));
	wy[ii+1] = 1.0/6.0 * (CUBE(P(dy-ii+2)) - 4*CUBE(P(dy-ii+1))
			      + 6*CUBE(P(dy-ii)) - 4*CUBE(P(dy-ii-1)));
      }

      // Get the interpolated pixel value
      value = 0.0;
      for (ii=-1; ii<=2; ii++)
	for (kk=-1; kk<=2; kk++)
	  value += GET_PIXEL(ix+ii,iy+kk) * wx[ii+1] * wy[kk+1];

      break;

    case SPLINES:
      // Determine B-Spline coefficients.
      // This is a essentially a pre-filtering applied to the entire image
      // that converts the image from a sample representation into a
      // representation based on B-spline coefficients.
      // Without this step the result would look blurry at the end.

      // Convert samples to coefficients in x direction
      samples2coefficients(inbuf, 'x');

      // Convert samples to coefficients in y direction
      samples2coefficients(inbuf, 'y');

      // Calculate interpolation indices
      ii = (int)floor(xSample) - 1;
      kk = (int)floor(yLine) - 1;
      for (ll=0; ll<=3; ll++) {
	xI[ll] = ii++;
	yI[ll] = kk++;
      }

      // Calculate interpolation weights
      w = xSample - (float)xI[1];
      wx[3] = (1.0/6.0)*CUBE(xSample);
      wx[0] = (1.0/6.0) + (1.0/2.0)*w*(w-1.0) - wx[3];
      wx[2] = w + wx[0] - 2.0*wx[3];
      wx[1] = 1.0 - wx[0] - wx[2] - wx[3];
      w = yLine - (float)yI[1];
      wy[3] = (1.0/6.0)*CUBE(xSample);
      wy[0] = (1.0/6.0) + (1.0/2.0)*w*(w-1.0) - wy[3];
      wy[2] = w + wy[0] - 2.0*wy[3];
      wy[1] = 1.0 - wy[0] - wy[2] - wy[3];

      // Get interpolation value
      value = 0.0;
      for (kk=0; kk<=3; kk++) {
	w = 0.0;
	for (ii=0; ii<=3; ii++)
	  w += wx[ii] * 1;
	value += wy[kk] * w;
      }
      break;
    case SINC:
      break;
    default:
      assert (FALSE);
      break;
  }

  return value;
}