/*-----------------------------------------------------------------------------

Copyright (C) 2004, 2006.

A. Ronald Gallant
Post Office Box 659
Chapel Hill NC 27514-0659
USA

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License along
with this program; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

-------------------------------------------------------------------------------

Class         snpden - Computes the log SNP density, computes its mean
                       and variance, and simulates from it.

Syntax        #include "libsnp.h"

              class snpden {
              public:
                snpden(INTEGER iKz, INTEGER iIz, INTEGER ily);
                snpden(const snpden& is, INTEGER iKz, INTEGER iIz, INTEGER ily);
                snpden();
                snpden(const snpden& is);
                ~snpden();
                snpden& operator=(const snpden& is);
                INTEGER get_Kz() const;
                INTEGER get_Iz() const;
                INTEGER get_ly() const;
                INTEGER get_la() const;
                INTEGER get_lR() const;
                const realmat& get_a() const;
                const realmat& get_u() const;
                const realmat& get_R() const;
                REAL get_e0() const;
                const std::vector<intvec>& get_alpha() const;
                void get_Rgen(realmat& Rgen) const;
                void set_a(const realmat& ia);
                void set_u(const realmat& iu);
                void set_R(const realmat& iR);
                void set_e0(REAL ie0);
                REAL log_f(const realmat& iy);
                REAL log_f(const realmat& iy, 
                  realmat& dlfwa, realmat& dlfwu, realmat& dlfwR);
                void reg_poly(realmat& a_reg) const;
                realmat herm_poly(realmat a_reg) const;
                realmat ezl(const std::vector<intvec>& lambda);
                void musig(realmat& mu, realmat& sig);
                realmat sampy(INT_32BIT& seed) const;
              };

Declared in   libsnp.h

Description   Kz is the degree of the main effects, Iz is the degree of
              the interactions, and ly is the length of y.  The second
              constructor copies the coefficients of the old snpden into
              the appropriate elements of the new.  The only restriction
              is that ly must be the same for the old and new.  The main
              use is to provide start values for the new from the old.
              Most methods are parameter input and output.  The main
              methods are log_f, which computes the log SNP density,
              musig, which computes its mean and variance, and sampy,
              which draws a random number from the density.

Remarks       The density is implemented as Hermite polynomials for
              numerical stability.  The literature and the algorithms for
              moment computation are implemented as regular polynomials.
              The methods reg_poly and herm_poly convert back and forth.
              They are public although really meant for internal use.
              The normalization constant is with respect to the Hermite
              expansion.  Thus, normalization constant for both reg_poly
              and herm_poly is the sum of squares of the Hermite
              coefficients plus e0.
              The methods lzl and musig are not const because they can
              change nmom.  The method log_f is not const because it
              changes the state of P.

Reference     Gallant, A. Ronald, and George Tauchen (1992), "A
              Nonparametric Approach to Nonlinear Time Series Analysis:
              Estimation and Simulation," in Brillinger, David, Peter
              Caines, John Geweke, Emanuel Parzen, Murray Rosenblatt, and
              Murad S. Taqqu eds. (1992),  New Directions in Time Series
              Analysis, Part II.  Springer--Verlag, New York, 71-92.

Sample        #include "libsnp.h"
program       using namespace scl;
              using namespace std;
              using namespace libsnp;
              //...
              INTEGER Kz=4; INTEGER Iz=2; INTEGER ly=3;
              snpden f(Kz, Iz, ly);
              INTEGER ltheta = f.get_la() + f.get_ly() + f.get_lR();
              realmat theta(ltheta,1);
              for (INTEGER i=1; i<=ltheta; ++i) theta[i] = start[i];
              theta[1]=1.0;  // Optimizer must hold theta[1] fixed.
              //... optimization loop
                f.set_a(theta(seg(1,f.get_la()),1));
                f.set_u(theta(seg(f.get_la()+1,f.get_la()+f.get_ly()),1));
                f.set_R(theta(seg(f.get_la()+f.get_ly()+1,ltheta,1));
                REAL log_likelihood = 0.0;
                realmat dlfwa, dlfwu, dlfwR;
                realmat dllwa(1,f.get_la,0.0);
                realmat dllwu(1,f.get_lu,0.0);
                realmat dllwR(1,f.get_lR,0.0);
                for (INTEGER t=1; t<=sample_size; ++t) {
                  log_likelihood += f.log_f(data("",t),dlfwa,dlfwu,dlfwR);
                  dllwa += dlfwa; dllwu += dlfwu; dllwR += dlfwR;
                }
                realmat dllwtheta;
                dllwtheta = cbind(dllwa,dllwu);
                dllwtheta = cbind(dllwtheta,dllwR);
              //... end optimization loop
              realmat mu,sig;
              f.musig(mu,sig);
              cout << mu << sig;
              INT_32BIT seed = 740726;
              ofstream simout("snpsim.dat");
              for (INTEGER t=1; t<=sim_size; ++t) {
                realmat y = f.sampy(seed);
                for (INTEGER i=1; i<=ly; ++i) simout << fmt('e',26,17,y[i]);
                simout << '\n'; 
              }
              //...

-----------------------------------------------------------------------------*/

#include "libsnp.h"

using namespace scl;
using namespace std;

using namespace libsnp;

void libsnp::snpden::make_private()
{
  la = P.get_len();
  a.resize(la,1,0.0);
  a[1]=1.0;
  u.resize(ly,1,0.0);
  lR = (ly*ly+ly)/2;
  R.resize(lR,1,0.0);
  for (INTEGER i=1; i<=ly; ++i) R[(i*i-i)/2+i]=1.0;
  e0 = REAL_EPSILON;
  P.get_multi(alpha);
  nmom.resize(16,1);
  nmom[1] = 1.0;
  nmom[2] = 1.0;
  for (INTEGER i=3; i<=16; ++i) nmom[i] = nmom[i-1]*REAL(2*i-3);
  intvec zero(ly,0);
  if (alpha[0] != zero) error("Error, snpden, snpden, alpha must be ordered");
}

libsnp::snpden::snpden(INTEGER iKz, INTEGER iIz, INTEGER ily) 
: Kz(iKz), Iz(iIz), ly(ily), y(ily,1,0.0), P('h',y,Kz,Iz)
{
  this->make_private();
}

libsnp::snpden::snpden(const snpden& is, INTEGER iKz, INTEGER iIz, INTEGER ily) 
: Kz(iKz), Iz(iIz), ly(ily), y(ily,1,0.0), P('h',y,Kz,Iz)
{
  if (ly != is.get_ly()) error("Error, snpden, snpden, y lengths differ");

  this->make_private();

  //Copy elements from is.a to a, matching by elements of alpha

  map<intvec,INTEGER,intvec_cmp> new_model;

  for (INTEGER i=1; i<=la; ++i) {
    intvec ivec = alpha[i-1];       // alpha indexes from 0 to la-1
    new_model[ivec] = i;      
  }

  #if defined SUN_CC_COMPILER
    map<intvec,INTEGER,intvec_cmp>::iterator itr = new_model.end();
  #else
    map<intvec,INTEGER,intvec_cmp>::const_iterator itr = new_model.end();
  #endif

  vector<intvec> old_alpha = is.get_alpha(); 
  realmat old_a = is.get_a();
  INTEGER old_la = is.get_la();

  bool nested = true;
  for (INTEGER i=1; i<=old_la; ++i) {
    intvec ivec = old_alpha[i-1];   // old_alpha indexes from 0 to old_la-1
    if (new_model.find(ivec) == itr) {
      nested = false;
    } 
    else {
      a[ new_model[ivec] ] = old_a[i]; 
    }
  }

  if (!nested) warn("Warning, snpden, snpden, models not nested");
}

libsnp::snpden::snpden()
: Kz(0), Iz(0), ly(0), y(realmat()), P(poly()), la(0), a(realmat()), 
  u(realmat()), lR(0), R(realmat()), e0(0.0), alpha(vector<intvec>()), 
  nmom(realmat())
{ }

libsnp::snpden::snpden(const snpden& is)
: Kz(is.Kz), Iz(is.Iz), ly(is.ly), y(is.y), P(is.P), la(is.la), a(is.a),
  u(is.u), lR(is.lR), R(is.R), e0(is.e0), alpha(is.alpha), nmom(is.nmom)
{ }

snpden& libsnp::snpden::operator=(const snpden& is)
{
  if (this != &is) {
    Kz=is.Kz; Iz=is.Iz; ly=is.ly; y=is.y; P=is.P; la=is.la; a=is.a; 
    u=is.u; lR=is.lR; R=is.R; e0=is.e0; alpha=is.alpha; nmom=is.nmom;
  }
  return *this;
}

void libsnp::snpden::get_Rgen(realmat& Rgen) const
{
  if ( (Rgen.nrow() != ly) || (Rgen.ncol() != ly) ) Rgen.resize(ly,ly);
  for (INTEGER j=1; j<=ly; ++j) {
    for (INTEGER i=1; i<=j; ++i) {
      Rgen(j,i) = 0.0;
      Rgen(i,j) = R[(j*j-j)/2+i];
    }
  }
}

REAL libsnp::snpden::log_f
  (const realmat& iy, realmat& dlfwa, realmat& dlfwu, realmat& dlfwR)
{
  /*
  compute 
    Rinv 
  the upper triangular inverse of R and store as a general matrix
  */

  realmat Rinv(ly,ly);
  for (INTEGER i=1; i<=lR; ++i) Rinv[i] = R[i];
  drinv(Rinv.get_x(),ly);
  upr2gen(Rinv.get_x(),ly);

  /*
  compute 
    z = Rinv*(y-u) 
  and derivatives 
   dzwu = (d/du)z = -Rinv
   dzwR = (d/dR)z = -Rinv*T(ij)*z = dzwu*T(ij)*z
  where T(ij) has a 1 in the ijth element and 0 elsewhere
  */

  realmat z = Rinv*(iy-u);
  realmat dzwu = -Rinv;
  realmat dzwR(ly,lR);
  for (INTEGER j=1; j<=ly; ++j) {
    for (INTEGER i=1; i<=j; ++i) {
      INTEGER ij = (j*j-j)/2+i;
      for (INTEGER ii=1; ii<=ly; ++ii) {
        dzwR[-ly+ly*ij+ii]=dzwu[-ly+ly*i+ii]*z[j];
      } 
    }
  }

  /*
  compute 
    S = (-1/2)*lz*ln(twopi) - ln(|det(R)|)
  and derivative
    dSwR = (d/dR)S
  */

  REAL S = (-0.91893853320467266954)*REAL(ly);
  realmat dSwR(1,lR,0.0);
  for (INTEGER i=1; i<=ly; ++i) {
    S -= log(fabs(R[(i*i+i)/2]));
    dSwR[(i*i+i)/2] = -1.0/R[(i*i+i)/2];
  }

  /*
  compute 
    Q = (-1/2)*T(z)*z
  and derivatives
    dQwz = (d/dz)Q = -T(z)
    dQwu = (d/dz)Q*(d/du)z = dQwz*dzwu
    dQwR = (d/dz)Q*(d/dR)z = dQwz*dzwR
  */

  realmat Qmat = (-0.5)*(T(z)*z);
  REAL Q = Qmat[1];
  realmat dQwz = -T(z);
  realmat dQwu = dQwz*dzwu;
  realmat dQwR = dQwz*dzwR;

  /*
  compute the log polynomial 
    lP(z|a) = log{[T(a)*basis(z)]**2 + e0}
  and derivatives 
    dlPwa = (d/da)lP(z|a)
    dlPwz = (d/dz)lP(z|a)
    dlPwu = (d/dz)lP(z|a)*(d/du)z = dlPwz*dzwu
    dlPwR = (d/dz)lP(z|a)*(d/dR)z = dlPwz*dzwR
  */

  P.set_x(z);

  realmat basis;
  P.get_basis(basis);

  realmat jacobian;
  P.get_jacobian(jacobian);

  realmat Pmat = T(a)*basis;

  REAL Pz = Pmat[1];
  REAL Pz2 = Pz*Pz;
  Pz2 += e0;

  REAL lP = log(Pz2);
  realmat dlPwa = 2.0*(Pz/Pz2)*T(basis);
  realmat dlPwz = 2.0*(Pz/Pz2)*T(a)*jacobian;
  realmat dlPwu = dlPwz*dzwu;
  realmat dlPwR = dlPwz*dzwR;

  /*
  compute the log normalization factor
    lE = log(E) = log(T(a)*a + e0)
  and derivatives
    dlEwa = (2.0/E)*T(a)
  */

  realmat Emat = T(a)*a;
  REAL E = Emat[1] + e0;
  REAL lE = log(E);
  realmat dlEwa = (2.0/E)*T(a);
  
  /*
  compute log density
    lf = log_f = S + Q + lP - lE
  and derivatives
    dlfwa = (d/da)lf = (d/da)P - (d/da)E = dlPwa - dlEwa
    dlfwu = (d/du)lf = (d/du)Q + (d/du)lP = dQwu + dlPwu
    dlfwR = (d/dR)lf = (d/dR)S + (d/dR)Q + (d/dR)lP = dSwR + dQwR + dlPwR
  */

  REAL lf = S + Q + lP - lE;

  dlfwa = dlPwa - dlEwa;
  dlfwu = dQwu + dlPwu;
  dlfwR = dSwR + dQwR + dlPwR;

  return lf;
}
  
void libsnp::snpden::reg_poly(realmat& a_reg) const
{
  if ( (a_reg.nrow() != la) || (a_reg.ncol() != 1) ) a_reg.resize(la,1);

  /*
  Construct a determining set.
  See Stroud, A. H., (1971) Approximate Calculation of Multiple
  Integrals, Academic, New York, p. 54-55.
  */

  realmat m(Kz+1,1);
  REAL inc = 8.0/REAL(Kz);
  REAL m0 = -4.0 - inc;
  for (INTEGER i=1; i<=Kz+1; ++i) {
    m[i] = m0 + REAL(i)*inc;
  }

  std::vector<realmat> Lm(la);
  for (INTEGER i=0; i<la; ++i) {
    realmat Mp(ly,1);
    for (INTEGER j=1; j<=ly; j++) {
      Mp[j] = m[alpha[i][j]+1];
    }
    Lm[i] = Mp;
  }

  realmat basis_herm;
  realmat basis_reg;

  scl::poly Hpoly('h',Lm[0],Kz,Iz);
  scl::poly Rpoly('r',Lm[0],Kz,Iz);

  /*
  std::vector<intvec> Ralpha;
  std::vector<intvec> Halpha;
  Rpoly.get_multi(Ralpha);
  Hpoly.get_multi(Halpha);
  for (INTEGER i=0; i<la; ++i) {
    if (Ralpha[i] != Halpha[i]) 
      error("Error, snpden, reg_poly, this should never happen");
  }
  */
  
  realmat W(la,la);

  for (INTEGER i=1; i<=la; ++i) {
    Hpoly.set_x(Lm[i-1]);
    Rpoly.set_x(Lm[i-1]);
    Hpoly.get_basis(basis_herm);
    Rpoly.get_basis(basis_reg);
    a_reg[i] = (T(basis_herm)*a)[1];
    for (INTEGER j=1; j<=la; ++j) W(i,j) = basis_reg[j]; 
  }

  if (solve(W,a_reg)) warn("Warning, snpden, reg_poly, inversion error");
}

realmat libsnp::snpden::herm_poly(realmat a_reg) const
{
  if ( (a_reg.nrow() != la) || (a_reg.ncol() != 1) ) { 
    error("Error, snpden, herm_poly, wrong dimensions for a_reg");
  }

  if (fabs(a_reg[1]) == 0.0) {
    error("Error, snpden, herm_poly, a_reg[1] == 0");
  }

  /*
  Construct a determining set.
  See Stroud, A. H., (1971) Approximate Calculation of Multiple
  Integrals, Academic, New York, p. 54-55.
  */

  realmat m(Kz+1,1);
  REAL inc = 8.0/REAL(Kz);
  REAL m0 = -4.0 - inc;
  for (INTEGER i=1; i<=Kz+1; ++i) {
    m[i] = m0 + REAL(i)*inc;             //p. 55, l. 3
  }

  std::vector<realmat> Lm(la);
  for (INTEGER i=0; i<la; ++i) {
    realmat Mp(ly,1);
    for (INTEGER j=1; j<=ly; j++) {
      Mp[j] = m[alpha[i][j]+1];          //p. 55, l. 5
    }
    Lm[i] = Mp;
  }

  realmat basis_herm;
  realmat basis_reg;

  scl::poly Hpoly('h',Lm[0],Kz,Iz);
  scl::poly Rpoly('r',Lm[0],Kz,Iz);
  
  realmat a_herm(la,1);
  realmat W(la,la);

  for (INTEGER i=1; i<=la; ++i) {
    Hpoly.set_x(Lm[i-1]);
    Rpoly.set_x(Lm[i-1]);
    Hpoly.get_basis(basis_herm);
    Rpoly.get_basis(basis_reg);
    a_herm[i] = (T(basis_reg)*a_reg)[1];
    for (INTEGER j=1; j<=la; ++j) W(i,j) = basis_herm[j]; 
  }

  if (solve(W,a_herm)) warn("Warning, snpden, herm_poly, inversion error");

  /*
  if (fabs(a_herm[1]) == 0.0) {
    error("Error, snpden, herm_poly, should never happen");
  }

  REAL a1 = a_herm[1];
  for (INTEGER i=1; i<=la; ++i) a_herm[i] /= a1;
  */

  return a_herm;
}

realmat libsnp::snpden::ezl(const std::vector<intvec>& lambda)
{
  INTEGER n = lambda.size();
  if ((n<=0)||(lambda[0].size()!=ly)) error("Error, snpden, ezl, bad input");

  realmat ezlam(n,1);

  //Note, lambda is indexed 0,...,n-1 whereas ezlam is 1,...,n
  //Similarly, alpha is 0,...,la-1 whereas a and a_reg are 1,...,la

  REAL ehz = 0.0;
  for (INTEGER i=1; i<=la; ++i) ehz += pow(a[i],2);
  ehz += e0;

  realmat a_reg;
  reg_poly(a_reg);
  
  intvec lam(ly);

  for (INTEGER i=0; i<n; ++i) {
    REAL sum = 0.0;
    for (INTEGER i0=0; i0<la; ++i0) {
      for (INTEGER j0=0; j0<la; ++j0) {
        lam = lambda[i] + alpha[i0] + alpha[j0];
        REAL prd = 1.0;
        for (INTEGER j=1; j<=ly; ++j) {
          INTEGER k = lam[j];
          INTEGER m = k/2 + 1;
          if (m > nmom.nrow()) {
            nmom.resize(2*m,1);
            nmom[1] = 1.0;
            nmom[2] = 1.0;
            for (INTEGER l=3; l<=2*m; ++l) nmom[l] = nmom[l-1]*REAL(2*l-3);
          }
          if (k%2) {
            prd = 0.0;
            break;
          } 
          else {
            prd *= nmom[m];        
          }
        }
        sum += a_reg[i0+1]*a_reg[j0+1]*prd;
      }
    }
    ezlam[i+1] = sum/ehz;
  }
  return ezlam;
}


void libsnp::snpden::musig(realmat& mu, realmat& sig)
{

  INTEGER n = ly + (ly*ly+ly)/2;
  std::vector<intvec> lambda(n);

  intvec lam(ly,0);
  INTEGER ij = 0;

  for (INTEGER i=1; i<=ly; ++i) {
    lam[i]=1;
    lambda[ij] = lam;
    lam[i]=0;
    ++ij;
  }

  for (INTEGER j=1; j<=ly; ++j) {
    for (INTEGER i=1; i<=j; ++i) {
      lam[i] += 1;
      lam[j] += 1;
      lambda[ij] = lam;
      lam[i] = 0;
      lam[j] = 0;
      ++ij;
    }
  }
      
  realmat ms = ezl(lambda);

  realmat mu_u(ly,1); 
  realmat sig_u(ly,ly);
  realmat Rgen;

  for (INTEGER i=1; i<=ly; ++i) {
    mu_u[i] = ms[i];
  }

  for (INTEGER j=1; j<=ly; ++j) {
    for (INTEGER i=1; i<=ly; ++i) {
      sig_u(i,j) = ms[ly + (j*j-j)/2 + i];
      sig_u(j,i) = ms[ly + (j*j-j)/2 + i];
    }
  }

  get_Rgen(Rgen);
  
  mu = u + Rgen*mu_u;
  sig = Rgen*(sig_u - mu_u*T(mu_u))*T(Rgen);
}

realmat libsnp::snpden::sampy(INT_32BIT& seed) const
{
  
  INT_32BIT local_seed;
  local_seed = seed;

  poly Rpoly('r',y,Kz,Iz);

  //Note, alpha is 0,...,la-1 whereas a and a_reg are 1,...,la

  realmat a_reg;
  reg_poly(a_reg);

  realmat abs_a_reg(la,1);
  for (INTEGER i=1; i<=la; ++i) abs_a_reg[i] = fabs(a_reg[i]);

  const REAL ln2 = log(2.0);
  
  /*
  Compute weights w(i,j) as defined in Gallant, A. Ronald, and George
  Tauchen (1992), "A Nonparametric Approach to Nonlinear Time Series
  Analysis: Estimation and Simulation," in Brillinger, David, Peter
  Caines, John Geweke, Emanuel Parzen, Murray Rosenblatt, and Murad
  S. Taqqu eds. (1992),  New Directions in Time Series Analysis, Part II.
  Springer--Verlag, New York, 71-92.  The formula there has a missing
  power of two which is corrected here
  */

  realmat w(la,la);
  REAL adj = -REAL_MAX;

  for (INTEGER j=1; j<=la; ++j) {
    intvec lj = alpha[j-1];
    for (INTEGER i=1; i<=la; ++i) {
      intvec li = alpha[i-1];
      REAL sum = 0.0;
      for (INTEGER k=1; k<=ly; ++k) {
        REAL v2 = 0.5*REAL(li[k]+lj[k]+1);
        #if defined MS_CL_COMPILER
          sum += gammln(v2) - (1.0 - v2)*ln2;
        #else
          sum += lgamma(v2) - (1.0 - v2)*ln2;
        #endif
      }
      w(i,j) = sum;
      adj = (adj < sum ? sum : adj); 
    }
  }

  REAL sum = 0.0;
  for (INTEGER j=1; j<=la; ++j) {
    for (INTEGER i=1; i<=la; ++i) {
      w(i,j) = abs_a_reg[i]*abs_a_reg[j] * exp(w(i,j) - adj);
      sum += w(i,j);
    }
  }
  
  adj = e0*w(1,1)/(abs_a_reg[1]*abs_a_reg[1]);
  w(1,1) += adj;
  sum += adj;

  for (INTEGER j=1; j<=la; ++j) {
    for (INTEGER i=1; i<=la; ++i) {
      w(i,j) /= sum;
    }
  }

  //Rejection algorithm

  realmat z(ly,1);
  REAL v, b, h;

  do {
    v = ran(&local_seed);
    REAL sum = 0.0;
    INTEGER ii,jj;
    ii = jj = 1;
    for (INTEGER j=1; j<=la; ++j) {
      jj=j;
      for (INTEGER i=1; i<=la; ++i) {
        ii=i;
        sum += w(i,j);
        if (sum > v) goto draw; 
      }
    }
 
    draw:
 
    intvec lam = alpha[ii-1] + alpha[jj-1];
 
    //Generate ly chi's and randomly change sign
  
    realmat abs_z(ly,1);
 
    for (INTEGER i=1; i<=ly; ++i) {
      REAL sa = REAL(lam[i]+1);
      z[i] = gchirv(sa,&local_seed);
      v = ran(&local_seed);
      if (v > 0.5) z[i] = -z[i];
      abs_z[i] = fabs(z[i]);
    } 
 
    //Accept or reject
 
    v = ran(&local_seed);

    realmat basis;

    Rpoly.set_x(abs_z);
    Rpoly.get_basis(basis);
    b = (T(abs_a_reg)*basis)[1];
    b *= b;

    Rpoly.set_x(z);
    Rpoly.get_basis(basis);
    h = (T(a_reg)*basis)[1];
    h *= h;

  } while (v*b > h); 
 
  realmat Rgen;
  get_Rgen(Rgen);

  seed = local_seed;

  return u + Rgen*z; 
}

REAL libsnp::snpden::log_f(const realmat& iy)
{
  /*
  compute 
    Rinv 
  the upper triangular inverse of R and store as a general matrix
  */

  realmat Rinv(ly,ly);
  for (INTEGER i=1; i<=lR; ++i) Rinv[i] = R[i];
  drinv(Rinv.get_x(),ly);
  upr2gen(Rinv.get_x(),ly);

  /*
  compute 
    z = Rinv*(y-u) 
  */

  realmat z = Rinv*(iy-u);

  /*
  compute 
    S = (-1/2)*lz*ln(twopi) - ln(|det(R)|)
  */

  REAL S = (-0.91893853320467266954)*REAL(ly);
  for (INTEGER i=1; i<=ly; ++i) {
    S -= log(fabs(R[(i*i+i)/2]));
  }

  /*
  compute 
    Q = (-1/2)*T(z)*z
  */

  realmat Qmat = (-0.5)*(T(z)*z);
  REAL Q = Qmat[1];

  /*
  compute the log polynomial 
    lP(z|a) = log{[T(a)*basis(z)]**2 + e0}
  */

  P.set_x(z);

  realmat basis;
  P.get_basis(basis);

  realmat Pmat = T(a)*basis;

  REAL Pz = Pmat[1];
  REAL Pz2 = Pz*Pz;
  Pz2 += e0;

  REAL lP = log(Pz2);

  /*
  compute the log normalization factor
    lE = log(E) = log(T(a)*a + e0)
  */

  realmat Emat = T(a)*a;
  REAL E = Emat[1] + e0;
  REAL lE = log(E);
  
  /*
  compute log density
    lf = log_f = S + Q + lP - lE
  */

  REAL lf = S + Q + lP - lE;

  return lf;
}

namespace {

  void multi(INTEGER n,INTEGER d,INTEGER k,intvec& jvec,vector<intvec>& midx)
  {
    if (k == d) { // external call must have k == d
      INTEGER m = 1;
      for (INTEGER i=1; i<=d; ++i) m *= n;
      midx.reserve(m);
      jvec.resize(d);
    }
  
    for (INTEGER j=1; j<=n; ++j) {
      jvec[k] = j;
      if (k == 1) {
        midx.push_back(jvec);
      }
      else {
        multi(n, d, k-1, jvec, midx);
      }
    }
  }

}

void libsnp::snpden::quad(INTEGER order, realmat& abcissae, realmat& weights)
{

  const REAL root2 = sqrt(2.0);
  const REAL lroot2 = log(root2);

  realmat x;
  realmat w;

  /*
  compute univariate quadrature rule
  */

  if( hquad(order,x,w) ) error("Error, libsnp::snpden::quad, hquad failed");

  /*
  compute multivariate quadrature rule
  */
  
  intvec jvec;
  vector<intvec> midx;
  multi(x.size(),ly,ly,jvec,midx);

  INTEGER n = midx.size();

  abcissae.resize(ly,n);
  weights.resize(n,1);

  realmat Rgen(ly,ly);
  get_Rgen(Rgen);

  #if defined SUN_CC_COMPILER
    vector<intvec>::iterator mitr;
  #else
    vector<intvec>::const_iterator mitr;
  #endif

  INTEGER j=0;
  for (mitr=midx.begin(); mitr!=midx.end(); ++mitr) {
  
    ++j;

    /*
    compute the pair
      y0 = root2*R*z + u
      z = Rinv*(y0-u)  
    compute the multivariate weight corresponding to z
      w0 = product for i = 1 to d of w[(*mitr)[i]]
    */
  
    realmat z(ly,1);
    REAL w0 = 1.0;

    for (INTEGER i=1; i<=ly; ++i) {
      z[i] = x[(*mitr)[i]];
      w0 *= w[(*mitr)[i]];
    }

    realmat y0 = root2*Rgen*z + u;

    /*
    compute 
      Smodified = (-1/2)*lz*ln(twopi) - ln(|det(R)|) + ln(|det(R)|) + ln(root2)
        = (-1/2)*lz*ln(twopi) + lz*ln(root2);
    */
  
    REAL Smodified = (lroot2 - 0.91893853320467266954)*REAL(ly);
  
    /*
    compute 
      Qmodified = 0;
    */
  
    REAL Qmodified = 0.0;
  
    /*
    compute the log polynomial 
      lP(z|a) = log{[T(a)*basis(root2*z)]**2 + e0}
    */
  
    P.set_x(root2*z);
  
    realmat basis;
    P.get_basis(basis);
  
    realmat Pmat = T(a)*basis;
  
    REAL Pz = Pmat[1];
    REAL Pz2 = Pz*Pz;
    Pz2 += e0;
  
    REAL lP = log(Pz2);
  
    /*
    compute the log normalization factor
      lE = log(E) = log(T(a)*a + e0)
    */
  
    realmat Emat = T(a)*a;
    REAL E = Emat[1] + e0;
    REAL lE = log(E);
    
    /*
    compute the modified log density + ln(|det(R)|) + (1/2)*T(z)*z
      lfmodified = log_f = Smodified + Qmodified + lP - lE
    */

    REAL lfmodified = Smodified + Qmodified + lP - lE;

    /*
    compute the weight
    */

    w0 *= exp(lfmodified);

    for (INTEGER i=1; i<=ly; ++i) abcissae(i,j) = y0[i];
    weights[j] = w0;
  }
}
