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

Copyright (C) 1997, 2002, 2006.

A. Ronald Gallant
P.O. 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      snpscore - compute the SNP score, univariate case.

Syntax        #include "snpden.h"

              REAL snpscore(REAL x, realmat& theta, REAL xmean, 
                              REAL xsigma, realmat& score);

Prototype in  snpden.h

Description   snpscore returns the log of a univariate SNP density f(x|theta)
              and computes its score = (d/d_theta) log f(x|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]).  Theta is of length 
              p; the degree of the hermite polynomial is K = p - 3; and 
              c will be normalized so that c[0]=1 on return.

Remark        Regardless of the dimensions of score on entry, snpscore
              resizes it to score(p,1).  

Reference     Gallant, A. Ronald and George Tauchen (1997), "The Relative 
              Efficiency of EMM Estimators."  Manuscript, Department of
              Economics, University of North Carolina, Chapel Hill NC.

Return value  The value of l = log f(x|theta).

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

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

REAL snpscore(REAL x, realmat& theta, REAL xmean, 
                REAL xsigma, realmat& score)
{
  REAL const zero = 0.0;
  REAL const one = 1.0;
  REAL const two = 2.0;

  if (theta[2] <= zero) {
    error("Error, snpscore, theta[2] must be positive");
  }

  INTEGER p = theta.get_rows();  

  INTEGER i;  

  REAL tmp, sum;
 
  if ((tmp=theta[3]) != one) {
    if (tmp <= zero) {
      error("Error, snpscore, theta[3] must be positive");
    }
    else {
      for(i=3; i<=p; i++) { theta[i] /= tmp; }
    }
  }
                            
  INTEGER K = p-3;

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

  REAL l;

  REAL* c = theta.get_x()+2;

  REAL  s[3];

  score.resize(p,1);

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

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

  REAL A, C;   //for common subexpression elimination

  u = (x - xmean)/xsigma;
  
  z = (u-mu)/S;

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

  h = g*g;

  dhwz = two*g*s[1];

  f = h/S;

  f /= xsigma;   
         
  if (f == 0.0)
    l = DBL_MIN;
  else
    l = log(f);

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

  score[1] = -A/S; 

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

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

  for (i=3, sum=zero; i<=p; i++) { tmp=theta[i]; sum += tmp*tmp; }

  l -= log(sum);

  tmp = two/sum;

  for (i=3; i<=p; i++) { score[i] -= tmp*theta[i]; }

  score[3]=zero;

  delete [] w; 

  return l;
}
