############################################################
### el.test.3(), modeled after Owen's code el.test ####
### Modified by Mai Zhou  9/2008                   #######
############################################################
### simplified version 3, use SVD to find Newton step ###
### no gradian step.                                  ###

el.test.3 <- function( x, mu, lam, maxit=25, gradtol=1e-7, 
                        itertrace=FALSE ){
x <- as.matrix(x)
n <- nrow(x)
p <- ncol(x)
mu <- as.vector(mu)
if( length(mu) !=p )
  stop("mu must have same dimension as observation vectors.")
if( n <= p )
  stop("Need more observations than length(mu) in el.test().")
z <- t( t(x) -mu )

#    Scale the problem, by a measure of the size of a 
# typical observation.  Add a tiny quantity to protect
# against dividing by zero in scaling.  Since z*lam is
# dimensionless, lam must be scaled inversely to z.

TINY <- sqrt( .Machine$double.xmin )
scale <- mean( abs(z) ) + TINY
z <- z/scale
if( !missing(lam) ){
  lam <- as.vector(lam)
  lam <- lam*scale
  if( logelr(z,rep(0,p),lam)>0 ) lam <- rep(0,p)
}
if( missing(lam) ) lam <- rep(0,p)
#
#     Take some precaution against users specifying
# tolerances too small.
#

if( gradtol < TINY) gradtol <- TINY

#
# Preset the weights for Newton steps at each of 4 inner iterations
#

nwts <- c( 3^-c(0:3) )

#
#  Iterate, finding the Newton step, and choosing 
#  a step that reduces the objective if possible.
#

nits <- 0
gsize <- gradtol + 1
while( nits < maxit && gsize > gradtol ){
  arg  <- 1 + z %*% lam
  wts1 <- as.vector( llogp(arg, 1/n) )    #### this approx  1/arg
  grad <- as.matrix( -z*wts1 )
  grad <- as.vector(colSums(grad))
  gsize <- mean( abs(grad) )       ### or max( abs(grad)) ??

#                              -1
#    The Newton step is -(Hess)    grad
#

  Hess <- t(as.matrix( z*wts1 )) %*% ( as.matrix(z*wts1) )
  SVDH <- svd( Hess )
  svdtol <- max( SVDH$d ) * 1e-9    #### svd tol 1e-9
  if( min(SVDH$d) < svdtol ) SVDH$d[ SVDH$d < svdtol ] <- svdtol
  nstep <- SVDH$v %*% (t(SVDH$u)/SVDH$d)
  nstep <- - as.vector( nstep %*% as.matrix(grad) )


  oGsize <- Gsize(z=z, lam=lam)
  ninner <- 0
  for(  i in 1:length(nwts) ){
    nGsize <- Gsize( z, lam+nwts[i]*nstep )
    if( nGsize < oGsize ){
      lam <- lam+nwts[i]*nstep
      ninner <- i
      break
    }
  }
  nits <- nits+1
  if( itertrace ) print( c(lam, nGsize, ninner) )
  if( ninner==0 ) break
}

lambda <- lam/scale
Nlogelr <- logelr(x=x, mu=mu, lam=lambda)

list( "-2LLR" = -2*Nlogelr, Pval = 1-pchisq(-2*Nlogelr, df=p),
     lambda = lambda, grad=grad*scale, wts=wts1, nits=nits )
}


logelr <- function( x, mu, lam ){ 
x <- as.matrix(x)
n <- nrow(x)
p <- ncol(x)
if( n <= p )
  stop("Need more observations than variables in logelr.")
mu <- as.vector(mu)
if( length(mu) != p )
  stop("Length of mean doesn't match number of variables in logelr.")

z <- t( t(x) -mu )
arg <- 1 + z %*% lam
return( - sum( llog(arg,1/n) ) ) 
}

#
#    The function llog() is equal to the natural
#  logarithm on the interval from eps >0 to infinity.
#  Between -infinity and eps, llog() is quadratic.
#  llogp() is the first derivative of llog(). Both functions 
#  are continuous across the "knot" at eps.
#
#    The cutoff point, eps, is usually 1/n, where n is the 
#    number of observations.  Unless n is extraordinarily large, 
#    dividing by eps is not expected to cause numerical difficulty.
#

llog <- function( z, eps ){
ans <- z
lo <- (z<eps)
ans[ lo  ] <- log(eps) - 1.5 + 2*z[lo]/eps - 0.5*(z[lo]/eps)^2
ans[ !lo ] <- log( z[!lo] )
ans
}

llogp <- function( z, eps ){
ans <- z
lo <- (z<eps)
ans[ lo  ] <- 2.0/eps - z[lo]/eps^2
ans[ !lo ] <- 1/z[!lo]
ans
}

Gsize <- function(z, lam) {
         n <- nrow(z)
         arg  <- 1 + z %*% lam
         wts1 <- as.vector( llogp(arg, 1/n) )  ### this approx 1/arg
         grad <- as.matrix( -z*wts1 )
         grad <- as.vector(colSums(grad))
         gsize <- mean( abs(grad) )       ### or max( abs(grad)) ??
return(gsize)
}