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

Copyright (C) 1993, 1994, 1999, 2002, 2006.

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

Permission to use, copy, modify, and distribute this software and its docu-
mentation for any purpose and without fee is hereby granted, provided that 
the above copyright notice appears in all copies and that both that copyright 
notice and this permission notice appear in supporting documentation.  This 
software is provided "as is" without any expressed or implied warranty.

-----------------------------------------------------------------------------*/
/*
Function      snpl - compute the SNP objective function, univariate case.

Syntax        #include "snpden.h"

              REAL snpl(realmat& X, realmat& theta);
              REAL snplg(realmat& X, realmat& theta, realmat& gradient);
              REAL snplgh(realmat& X, realmat& theta,
                            realmat& gradient, realmat& hessian);
              REAL snplgi(realmat& X, realmat& theta,
                            realmat& gradient, realmat& info);
              REAL snplgirmt(realmat& X, realmat& theta,
                        REALl, realmat& gradient, realmat& info, INTEGER t);

              REAL snpl(realmat& X, realmat& theta, REAL e0);
              REAL snplg(realmat& X, realmat& theta, REAL e0,
                            realmat& gradient);
              REAL snplgh(realmat& X, realmat& theta, REAL e0,
                            realmat& gradient, realmat& hessian);
              REAL snplgi(realmat& X, realmat& theta, REAL e0,
                            realmat& gradient, realmat& info);
              REAL snplgirmt(realmat& X, realmat& theta, REAL e0,
                        REAL l, realmat& gradient, realmat& info, INTEGER t);

Prototype in  snpden.h

Description   snpl returns the log likelihood of a univariate SNP density
              l = (-1/n) {log f(X[1],theta) + ... + log f(X[n],theta)} where
              f(x|theta) = (1/theta[2])*h{(x-theta[1])/theta[2]|c}, h(z|c)
              is the SNP density function as described in library function 
              hermite, and c = (theta[3], ..., theta[p]).  X has length n and 
              contains the univariate data; theta is of length p.  The degree 
              of the hermite polynomial is K = p - 3; c must be normalized so 
              that (c[0]*c[0] + ... + c[K]*c[K]) = 1-e0 prior to calling snpl
              for the versions that have argument e0.  For those that do not,  
              normalize so that (c[0]*c[0] + ... + c[K]*c[K]) = 1.  snplgh 
              computes the gradient and hessian with respect to theta as well
              and returns them as arguments.  snplgi computes the gradient and 
              information matrix.  snplg computes the gradient.  snplgirmt 
              removes the effect of the t'th observation from l, gradient, and
              info as returned by snplgi.  It has the same effect as a call to
              snplgi with the t'th observation removed from X, but is more 
              efficient and does not the reduce sample size to n-1.

Remark        If the dimensions of gradient and hessian are too small upon 
              entry then snplgh resizes them to gradient(p,1) and hessian(p,p).
              If they are large enough then the dimensions are left the same
              and results are in the northwest portion.  Simlarly for snplgi
              and snplg.

Reference     Fenton, Victor M., and A. Ronald Gallant (1996), "Qualitative 
              and Asymptotic Performance of SNP Density Estimators," Journal 
              of Econometrics 74, 77--118.  

Return value  The value of l.

Functions     Library: log, sqrt
called        libscl: hermite, realmat
*/

#include "snpden.h"
using namespace std;
using namespace scl;


REAL snpl(realmat& X, realmat& theta, REAL e0)
{
//REAL const zero = 0.0;
//REAL const one = 1.0;
//REAL const two = 2.0;
//REAL const four = 4.0;

  INTEGER p = theta.get_rows();  

  INTEGER K = p-3;
  INTEGER n = X.get_rows();

  REAL mu = theta[1];
  REAL S = theta[2];

  REAL   l = 0.0;

  REAL*  c = theta.get_x()+2;
  REAL   s[3];
  REAL*  w = new(nothrow) REAL[K+1];
  if (w == 0) {error("Error, snpl, operator new failed");}

  REAL h, g, z;  

  REAL f;

  INTEGER t;  
 

  for (t=1; t<=n; t++) {

    z = (X[t]-mu)/S;

    g = hermite(z,K,c,w,s);

    h = g*g + e0*w[0]*w[0];

    f = h/S;
         
    if (f == 0.0)
      l -= REAL_MAX;
    else
      l += log(f);

  }

  REAL nr = REAL(-n);

  l /= nr;

  delete [] w; 

  return l;

}


REAL snplg(realmat& X, realmat& theta, REAL e0, realmat& gradient)
{
  REAL const one = 1.0;
  REAL const two = 2.0;

  INTEGER p = theta.get_rows();  

  if( p > gradient.get_rows() ) gradient.resize(p,1);
                              
  INTEGER K = p-3;
  INTEGER n = X.get_rows();

  REAL mu = theta[1];
  REAL S = theta[2];

  REAL   l = 0.0;  fill(gradient);

  REAL*  c = theta.get_x()+2;
  REAL   s[3];

  REAL*  w = new(nothrow) REAL[K+1];
  if (w == 0) {
    error("Error, snpl, operator new failed");
  }
  REAL* wi;

  REAL*  grad = new(nothrow) REAL[p];
  if (grad == 0) {
    delete [] w;
    error("Error, snpl, operator new failed");
  }
  REAL* gradi;  REAL* gradj;

  REAL h, g, z, dhwz;  
  REAL f;

  INTEGER i,j,t;  

  REAL A, C;   //for common subexpression elimination
  

  for (t=1; t<=n; t++) {

    z = (X[t]-mu)/S;

    g = hermite(z,K,c,w,s);

    h = g*g + e0*w[0]*w[0];

    dhwz = two*g*s[1] - e0*z*w[0]*w[0];

    f = h/S;
         
    if (f == 0.0)
      l -= REAL_MAX;
    else
      l += log(f);

    A = dhwz/h; 
    C = g/h;

    gradi = grad;

    *gradi++ = -A/S; 

    *gradi++ = (-z*A - one)/S;

    for (i=3, wi=w; i<=p; i++, wi++) {
      *gradi++  = two*C*(*wi);
    }

    for (j=1, gradj=grad; j<=p; j++, gradj++) {
      gradient[j] += *gradj;
    }
  }

  REAL nr = REAL(-n);

  l /= nr;

  for (j=1; j<=p; j++) {
    gradient[j] /= nr;
  }

  delete [] w; 
  delete [] grad; 

  return l;

}


REAL snplgh(realmat& X, realmat& theta, REAL e0, 
              realmat& gradient, realmat& hessian)
{
  REAL const zero = 0.0;
  REAL const one = 1.0;
  REAL const two = 2.0;
  REAL const four = 4.0;

  INTEGER p = theta.get_rows();  

  if( p > gradient.get_rows() ) gradient.resize(p,1);
  if( p > hessian.get_rows() || p > hessian.get_cols() ) hessian.resize(p,p);
                              
  INTEGER K = p-3;
  INTEGER n = X.get_rows();

  REAL mu = theta[1];
  REAL S = theta[2];

  REAL   l = 0.0;  fill(gradient);  fill(hessian);

  REAL*  c = theta.get_x()+2;
  REAL   s[3];
  REAL*  w = new(nothrow) REAL[K+1];
  if (w == 0) {error("Error, snpl, operator new failed");}

  REAL h, g, z, dhwz, d2hwz2;  
  REAL* wi;  REAL* wj;  REAL ri, wlag;
  REAL f;

  INTEGER i,j,t;  

  REAL A, A2, B, C, C2, D, S2;   //for common subexpression elimination
  

  for (t=1; t<=n; t++) {

    z = (X[t]-mu)/S;

    g = hermite(z,K,c,w,s);

    h = g*g + e0*w[0]*w[0];

    dhwz = two*g*s[1] - e0*z*w[0]*w[0];

    d2hwz2 = two*g*s[2] + 2*s[1]*s[1] - e0*w[0]*w[0] + e0*z*z*w[0]*w[0];

    f = h/S;
         
    if (f == 0.0)
      l -= REAL_MAX;
    else
      l += log(f);

    A = dhwz/h; 
    A2 = A*A;
    B = d2hwz2/h;
    C = g/h;
    C2 = C*C;
    S2 = S*S;

    gradient[1] += -A/S; 

    gradient[2] += (-z*A - one)/S;

    for (i=3, wi=w; i<=p; i++, wi++) {
      gradient[i] += two*C*(*wi);
    }

    hessian(1,1) += (-A2 + B)/S2;

    hessian(2,1) += (-z*A2 + z*B + A)/S2;

    hessian(2,2) += (-A2*z*z + z*z*B + two*z*A + one)/S2;

    for (i=3, wi=w, wlag=zero; i<=p; i++, wi++) {
      ri = sqrt(REAL(i-3));
      D = (two*C*(*wi)*A - two*s[1]*(*wi)/h - two*C*ri*wlag + z*C*(*wi))/S;
      wlag = *wi;
      hessian(i,1) += D;
      hessian(i,2) += z*D;
    }

    for (j=3, wj=w; j<=p; j++, wj++) {
      for (i=j, wi=w+j-3; i<=p; i++, wi++) {
        hessian(i,j) += (-four*C2 + two/h)*(*wi)*(*wj);
      }
    }
  }

  REAL nr = REAL(-n);

  l /= nr;

  for (j=1; j<=p; j++) {
    gradient[j] /= nr;
    for (i=j; i<=p; i++) {
      hessian(j,i) = hessian(i,j) /= nr;
    }
  }

  delete [] w; 

  return l;

}


REAL snplgi(realmat& X, realmat& theta, REAL e0, 
              realmat& gradient, realmat& info)
{
  REAL const one = 1.0;
  REAL const two = 2.0;

  INTEGER p = theta.get_rows();  

  if( p > gradient.get_rows() ) gradient.resize(p,1);
  if( p > info.get_rows() || p > info.get_cols() ) info.resize(p,p);
                              
  INTEGER K = p-3;
  INTEGER n = X.get_rows();

  REAL mu = theta[1];
  REAL S = theta[2];

  REAL   l = 0.0;  fill(gradient);  fill(info);

  REAL*  c = theta.get_x()+2;
  REAL   s[3];

  REAL*  w = new(nothrow) REAL[K+1];
  if (w == 0) {
    error("Error, snpl, operator new failed");
  }
  REAL* wi;

  REAL*  grad = new(nothrow) REAL[p];
  if (grad == 0) {
    delete [] w;
    error("Error, snpl, operator new failed");
  }
  REAL* gradi;  REAL* gradj;

  REAL h, g, z, dhwz;  
  REAL f;

  INTEGER i,j,t;  

  REAL A, C;   //for common subexpression elimination
  

  for (t=1; t<=n; t++) {

    z = (X[t]-mu)/S;

    g = hermite(z,K,c,w,s);

    h = g*g + e0*w[0]*w[0];

    dhwz = two*g*s[1] - e0*z*w[0]*w[0];

    f = h/S;
         
    if (f == 0.0)
      l -= REAL_MAX;
    else
      l += log(f);

    A = dhwz/h; 
    C = g/h;

    gradi = grad;

    *gradi++ = -A/S; 

    *gradi++ = (-z*A - one)/S;

    for (i=3, wi=w; i<=p; i++, wi++) {
      *gradi++  = two*C*(*wi);
    }

    for (j=1, gradj=grad; j<=p; j++, gradj++) {
      gradient[j] += *gradj;
      for (i=j, gradi=gradj; i<=p; i++, gradi++) {
        info(i,j) += (*gradi)*(*gradj);
      }
    }
  }

  REAL nr = REAL(-n);
  REAL nn = REAL(n);

  l /= nr;

  for (j=1; j<=p; j++) {
    gradient[j] /= nr;
    for (i=j; i<=p; i++) {
      info(j,i) = info(i,j) /= nn;
    }
  }

  delete [] w; 
  delete [] grad; 

  return l;

}

REAL snplgirmt(realmat& X, realmat& theta, REAL e0, 
          REAL l, realmat& gradient, realmat& info, INTEGER t)
{
  REAL const one = 1.0;
  REAL const two = 2.0;

  INTEGER p = theta.get_rows();  

  if( p > gradient.get_rows() ) 
    {error("Error, snpl, wrong size gradient vector");}
  if( p > info.get_rows() || p > info.get_cols() )
    {error("Error, snpl, wrong size info matrix");}
                              
  INTEGER K = p-3;
  INTEGER n = X.get_rows();

  REAL mu = theta[1];
  REAL S = theta[2];

  REAL*  c = theta.get_x()+2;
  REAL   s[3];

  REAL*  w = new(nothrow) REAL[K+1];
  if (w == 0) {
    error("Error, snpl, operator new failed");}
  REAL* wi;

  REAL*  grad = new(nothrow) REAL[p];
  if (grad == 0) {
    delete [] w;
    error("Error, snpl, operator new failed");
  }
  REAL* gradi;  REAL* gradj;

  REAL h, g, z, dhwz;  
  REAL f;

  INTEGER i,j;  

  REAL A, C;   //for common subexpression elimination
  
  REAL nr = REAL(-n);
  REAL nn = REAL(n);

  z = (X[t]-mu)/S;

  g = hermite(z,K,c,w,s);

  h = g*g + e0*w[0]*w[0];

  dhwz = two*g*s[1] - e0*z*w[0]*w[0];

  f = h/S;
         
  if (f == 0.0) {
    l *= nr;
    l += REAL_MAX;
    l /= nr;
  }
  else {
    l -= log(f)/nr;
  }

  A = dhwz/h; 
  C = g/h;

  gradi = grad;

  *gradi++ = -A/S; 

  *gradi++ = (-z*A - one)/S;

  for (i=3, wi=w; i<=p; i++, wi++) {
    *gradi++  = two*C*(*wi);
  }

  for (j=1, gradj=grad; j<=p; j++, gradj++) {
    gradient[j] -= *gradj/nr;
    for (i=j, gradi=gradj; i<=p; i++, gradi++) {
      info(j,i) = info(i,j) -= (*gradi)*(*gradj)/nn;
    }
  }

  delete [] w; 
  delete [] grad; 

  return l;

}

REAL snpl(realmat& X, realmat& theta)
{
//REAL const zero = 0.0;
//REAL const one = 1.0;
//REAL const two = 2.0;
//REAL const four = 4.0;

  INTEGER p = theta.get_rows();  

  INTEGER K = p-3;
  INTEGER n = X.get_rows();

  REAL mu = theta[1];
  REAL S = theta[2];

  REAL   l = 0.0;

  REAL*  c = theta.get_x()+2;
  REAL   s[3];
  REAL*  w = new(nothrow) REAL[K+1];
  if (w == 0) {error("Error, snpl, operator new failed");}

  REAL h, g, z;  

  REAL f;

  INTEGER t;  
 

  for (t=1; t<=n; t++) {

    z = (X[t]-mu)/S;

    g = hermite(z,K,c,w,s);

    h = g*g;

    f = h/S;
         
    if (f == 0.0)
      l -= REAL_MAX;
    else
      l += log(f);

  }

  REAL nr = REAL(-n);

  l /= nr;

  delete [] w; 

  return l;

}


REAL snplg(realmat& X, realmat& theta, realmat& gradient)
{
  REAL const one = 1.0;
  REAL const two = 2.0;

  INTEGER p = theta.get_rows();  

  if( p > gradient.get_rows() ) gradient.resize(p,1);
                              
  INTEGER K = p-3;
  INTEGER n = X.get_rows();

  REAL mu = theta[1];
  REAL S = theta[2];

  REAL   l = 0.0;  fill(gradient);

  REAL*  c = theta.get_x()+2;
  REAL   s[3];

  REAL*  w = new(nothrow) REAL[K+1];
  if (w == 0) {
    error("Error, snpl, operator new failed");
  }
  REAL* wi;

  REAL*  grad = new(nothrow) REAL[p];
  if (grad == 0) {
    delete [] w;
    error("Error, snpl, operator new failed");
  }
  REAL* gradi;  REAL* gradj;

  REAL h, g, z, dhwz;  
  REAL f;

  INTEGER i,j,t;  

  REAL A, C;   //for common subexpression elimination
  

  for (t=1; t<=n; t++) {

    z = (X[t]-mu)/S;

    g = hermite(z,K,c,w,s);

    h = g*g;

    dhwz = two*g*s[1];

    f = h/S;
         
    if (f == 0.0)
      l -= REAL_MAX;
    else
      l += log(f);

    A = dhwz/h; 
    C = g/h;

    gradi = grad;

    *gradi++ = -A/S; 

    *gradi++ = (-z*A - one)/S;

    for (i=3, wi=w; i<=p; i++, wi++) {
      *gradi++  = two*C*(*wi);
    }

    for (j=1, gradj=grad; j<=p; j++, gradj++) {
      gradient[j] += *gradj;
    }
  }

  REAL nr = REAL(-n);

  l /= nr;

  for (j=1; j<=p; j++) {
    gradient[j] /= nr;
  }

  delete [] w; 
  delete [] grad; 

  return l;

}


REAL snplgh(realmat& X, realmat& theta,
              realmat& gradient, realmat& hessian)
{
  REAL const zero = 0.0;
  REAL const one = 1.0;
  REAL const two = 2.0;
  REAL const four = 4.0;

  INTEGER p = theta.get_rows();  

  if( p > gradient.get_rows() ) gradient.resize(p,1);
  if( p > hessian.get_rows() || p > hessian.get_cols() ) hessian.resize(p,p);
                              
  INTEGER K = p-3;
  INTEGER n = X.get_rows();

  REAL mu = theta[1];
  REAL S = theta[2];

  REAL   l = 0.0;  fill(gradient);  fill(hessian);

  REAL*  c = theta.get_x()+2;
  REAL   s[3];
  REAL*  w = new(nothrow) REAL[K+1];
  if (w == 0) {error("Error, snpl, operator new failed");}

  REAL h, g, z, dhwz, d2hwz2;  
  REAL* wi;  REAL* wj;  REAL ri, wlag;
  REAL f;

  INTEGER i,j,t;  

  REAL A, A2, B, C, C2, D, S2;   //for common subexpression elimination
  

  for (t=1; t<=n; t++) {

    z = (X[t]-mu)/S;

    g = hermite(z,K,c,w,s);

    h = g*g;

    dhwz = two*g*s[1];

    d2hwz2 = two*g*s[2] + 2*s[1]*s[1];

    f = h/S;
         
    if (f == 0.0)
      l -= REAL_MAX;
    else
      l += log(f);

    A = dhwz/h; 
    A2 = A*A;
    B = d2hwz2/h;
    C = g/h;
    C2 = C*C;
    S2 = S*S;

    gradient[1] += -A/S; 

    gradient[2] += (-z*A - one)/S;

    for (i=3, wi=w; i<=p; i++, wi++) {
      gradient[i] += two*C*(*wi);
    }

    hessian(1,1) += (-A2 + B)/S2;

    hessian(2,1) += (-z*A2 + z*B + A)/S2;

    hessian(2,2) += (-A2*z*z + z*z*B + two*z*A + one)/S2;

    for (i=3, wi=w, wlag=zero; i<=p; i++, wi++) {
      ri = sqrt(REAL(i-3));
      D = (two*C*(*wi)*A - two*s[1]*(*wi)/h - two*C*ri*wlag + z*C*(*wi))/S;
      wlag = *wi;
      hessian(i,1) += D;
      hessian(i,2) += z*D;
    }

    for (j=3, wj=w; j<=p; j++, wj++) {
      for (i=j, wi=w+j-3; i<=p; i++, wi++) {
        hessian(i,j) += (-four*C2 + two/h)*(*wi)*(*wj);
      }
    }
  }

  REAL nr = REAL(-n);

  l /= nr;

  for (j=1; j<=p; j++) {
    gradient[j] /= nr;
    for (i=j; i<=p; i++) {
      hessian(j,i) = hessian(i,j) /= nr;
    }
  }

  delete [] w; 

  return l;

}


REAL snplgi(realmat& X, realmat& theta,
              realmat& gradient, realmat& info)
{
  REAL const one = 1.0;
  REAL const two = 2.0;

  INTEGER p = theta.get_rows();  

  if( p > gradient.get_rows() ) gradient.resize(p,1);
  if( p > info.get_rows() || p > info.get_cols() ) info.resize(p,p);
                              
  INTEGER K = p-3;
  INTEGER n = X.get_rows();

  REAL mu = theta[1];
  REAL S = theta[2];

  REAL   l = 0.0;  fill(gradient);  fill(info);

  REAL*  c = theta.get_x()+2;
  REAL   s[3];

  REAL*  w = new(nothrow) REAL[K+1];
  if (w == 0) {
    error("Error, snpl, operator new failed");
  }
  REAL* wi;

  REAL*  grad = new(nothrow) REAL[p];
  if (grad == 0) {
    delete [] w;
    error("Error, snpl, operator new failed");
  }
  REAL* gradi;  REAL* gradj;

  REAL h, g, z, dhwz;  
  REAL f;

  INTEGER i,j,t;  

  REAL A, C;   //for common subexpression elimination
  

  for (t=1; t<=n; t++) {

    z = (X[t]-mu)/S;

    g = hermite(z,K,c,w,s);

    h = g*g;

    dhwz = two*g*s[1];

    f = h/S;
         
    if (f == 0.0)
      l -= REAL_MAX;
    else
      l += log(f);

    A = dhwz/h; 
    C = g/h;

    gradi = grad;

    *gradi++ = -A/S; 

    *gradi++ = (-z*A - one)/S;

    for (i=3, wi=w; i<=p; i++, wi++) {
      *gradi++  = two*C*(*wi);
    }

    for (j=1, gradj=grad; j<=p; j++, gradj++) {
      gradient[j] += *gradj;
      for (i=j, gradi=gradj; i<=p; i++, gradi++) {
        info(i,j) += (*gradi)*(*gradj);
      }
    }
  }

  REAL nr = REAL(-n);
  REAL nn = REAL(n);

  l /= nr;

  for (j=1; j<=p; j++) {
    gradient[j] /= nr;
    for (i=j; i<=p; i++) {
      info(j,i) = info(i,j) /= nn;
    }
  }

  delete [] w; 
  delete [] grad; 

  return l;

}

REAL snplgirmt(realmat& X, realmat& theta,
          REAL l, realmat& gradient, realmat& info, INTEGER t)
{
  REAL const one = 1.0;
  REAL const two = 2.0;

  INTEGER p = theta.get_rows();  

  if( p > gradient.get_rows() ) 
    {error("Error, snpl, wrong size gradient vector");}
  if( p > info.get_rows() || p > info.get_cols() )
    {error("Error, snpl, wrong size info matrix");}
                              
  INTEGER K = p-3;
  INTEGER n = X.get_rows();

  REAL mu = theta[1];
  REAL S = theta[2];

  REAL*  c = theta.get_x()+2;
  REAL   s[3];

  REAL*  w = new(nothrow) REAL[K+1];
  if (w == 0) {
    error("Error, snpl, operator new failed");
  }
  REAL* wi;

  REAL*  grad = new(nothrow) REAL[p];
  if (grad == 0) {
    delete [] w;
    error("Error, snpl, operator new failed");
  }
  REAL* gradi;  REAL* gradj;

  REAL h, g, z, dhwz;  
  REAL f;

  INTEGER i,j;  

  REAL A, C;   //for common subexpression elimination
  
  REAL nr = REAL(-n);
  REAL nn = REAL(n);

  z = (X[t]-mu)/S;

  g = hermite(z,K,c,w,s);

  h = g*g;

  dhwz = two*g*s[1];

  f = h/S;
         
  if (f == 0.0) {
    l *= nr;
    l += REAL_MAX;
    l /= nr;
  }
  else {
    l -= log(f)/nr;
  }

  A = dhwz/h; 
  C = g/h;

  gradi = grad;

  *gradi++ = -A/S; 

  *gradi++ = (-z*A - one)/S;

  for (i=3, wi=w; i<=p; i++, wi++) {
    *gradi++  = two*C*(*wi);
  }

  for (j=1, gradj=grad; j<=p; j++, gradj++) {
    gradient[j] -= *gradj/nr;
    for (i=j, gradi=gradj; i<=p; i++, gradi++) {
      info(j,i) = info(i,j) -= (*gradi)*(*gradj)/nn;
    }
  }

  delete [] w; 
  delete [] grad; 

  return l;

}
