.packageName <- "rggm"
"duplication" <-
function(n)
{
  m <- n * (n+1) / 2
  a <- diag(n)
  a[lower.tri(a, TRUE)] <- 1:m
  a[upper.tri(a)] <- t(a)[upper.tri(a)]
  
  a.vec <- as.vector(a)

  D <- matrix(rep(0, (n*n) * m), (n*n), m)
  for(r in 1:(n*n)) {
    D[r, a.vec[r]] <- 1
  }

  return(D)
}

"duplication.inverse" <-
function(n)
{
  d <- duplication(n)
  return(solve(t(d) %*% d) %*% t(d))
}

"elimination" <-
function(n)
{
  d <- duplication(n)
  return(solve(t(d) %*% d) %*% t(d))
}

"rob.asymv.sigma" <-
function(estcov, amat, w.par) {

  ################################################################
  bindex <- function(vec)
  {
    list.1 <-
      as.vector(sapply(vec,
                       function(item) rep(item, length(vec)) ))
    list.2 <- rep(vec, length(vec))
    return(lapply((1:length(list.1)),
           function(i) c(list.1[i], list.2[i])))
  }
  
  ################################################################
  asymv.v <- function(w.par, p) {

    bicond <- function( bi ) {
      # return the list-value matrix indexed by TRUE/FALSE
      tf <- 
        sapply( bi, function( item.1 ) {
          lapply( bi, function( item.2 ) {
            tf.vec <- rep( FALSE, 3 )
            item.1 <- sort( item.1 ); item.2 <- sort( item.2 )
            if( item.1[1] == item.1[2] ) {
              if( (item.1[1] == item.2[1]) && (item.1[2] == item.2[2]) ) {
                tf.vec[1] <- TRUE
                return( tf.vec )
              }
              if( item.2[1] == item.2[2] ) {
                tf.vec[2] <- TRUE
                return( tf.vec )
              }
            }
            if((item.1[1] == item.2[1]) && (item.1[2] == item.2[2])) {
              tf.vec[3] <- TRUE
            }
            return( tf.vec )
          } )
        } )
      return( tf )
    }
    
    coef.a <- (4 * w.par^2 + 2)/( (2 * w.par + 1)^(0.5 * (p + 4)) ) -
      w.par^2 / (w.par + 1)^(p + 2)
    coef.b <- (4 * w.par^2)/( (2 * w.par + 1)^(0.5 * (p + 4)) ) -
      w.par^2 / (w.par + 1)^(p + 2)
    coef.c <- 1 / (2 * w.par + 1)^(0.5 * (p + 4))

    # Main routine
    idx.mtx <- as.matrix(bicond(bindex(1:p)))
    rslt <- 
      apply(idx.mtx, 1, function(vc) {
        sapply(vc, function(lst) {
          c(coef.a, coef.b, coef.c) %*% unlist(lst) }) })

    return(rslt)
  }
  
  ################################################################
  asymv.ve <- function(w.par, estcov) {

    ################################################################
    bicond <- function(bi) {
      tf <-
        sapply(bi, function(item.1) {
          lapply(bi, function(item.2) {
            tf.vec <- rep(FALSE, 2)
            item.1 <- sort(item.1); item.2 <- sort(item.2)
            if(item.1[1] == item.1[2] && item.2[1] == item.2[2]) {
              if(item.1[1] == item.2[1]) {
                tf.vec[1] <- TRUE
                return(tf.vec)
              } else {
                tf.vec[2] <- TRUE
                return(tf.vec)
              }
            }
            if((item.1[1] == item.2[1]) && (item.1[2] == item.2[2])) {
              tf.vec[2] <- TRUE
              return(tf.vec)
            }
            return(tf.vec) }) })
      return(tf)
    }
    
    ################################################################
    ve.core <- function(w.par, p) {
      coef.a1 <- -1.5 * w.par/(w.par + 1)^(0.5 * (p + 4))
      coef.a2 <- -0.5 * w.par/(w.par + 1)^(0.5 * (p + 4))
      coef.b <- 0.5 * w.par/(w.par + 1)^(0.5 * (p + 2))

      rslt <- 
        apply( as.matrix( bicond( bindex( 1:p ) ) ),
              1, function( vc ) {
                sapply( vc, function( lst ) {
                  c( coef.a1, coef.a2 ) %*% unlist( lst ) } ) } )

      vecp <- as.matrix(as.vector(diag(p)))
      rslt <- rslt + coef.b * (vecp %*% t(vecp))

      return( rslt )
    }

    ################################################################
    # Main routine
    p <- ncol(estcov)

    sqm.inv <- sqrtm(m = estcov)
    sqm <- solve(sqm.inv)
    
    first <- (sqm.inv %x% sqm.inv) %*%
      ve.core(w.par, p) %*% (sqm %x% sqm)

    coef.c <- 1/(w.par + 1)^(0.5 * (p + 2))
    second <- coef.c * diag(p * p)

    return(first + second)
  }
  
  ################################################################
  # Main routine
  p <- nrow(amat)
 
  sqm.inv <- sqrtm(m = estcov)

  dlt <- rob.delta(amat)
  kn <- sqm.inv %x% sqm.inv

  dp <- duplication(n = p)
  di <- duplication.inverse(n = p)

  coef.m <- t(dlt) %*% di %*% kn
  v <- coef.m %*% asymv.v(w.par = w.par, p = p) %*% t(coef.m)

  av.slv <-
    solve(t(dlt) %*% di %*% 
          asymv.ve(w.par = w.par, estcov) %*%
          dp %*% dlt)

  return(av.slv %*% v %*% t(av.slv))
}

"rob.browne.test" <-
function(estcov, amat, w.par, smpl.frm)
{

  complete.ug <- function(amat) {
    amat[] <- 1
    sat <- amat - diag(nrow(amat))
    vnames <- vertices(amat)
    dimnames(sat) <- list(vnames, vnames)

    return(sat)
  }

  sat <- complete.ug(amat)
  
  # coerce smpl.frm to be as data frame
  smpl.frm <- as.data.frame(smpl.frm)

  nost.inv   <-
    solve(rob.estimConGraph(amat = sat, w.par = w.par, smpl.frm = smpl.frm)$Sigma)
  struct.inv <- solve(estcov)

  diff <-vech(nost.inv - struct.inv)

  gamma <-
    rob.asymv.sigma(estcov = nost.inv, amat = sat, w.par = w.par)
  gamma.inv <- solve(gamma)

  dlt <- rob.delta(amat)

  V <- gamma.inv -
    gamma.inv %*% dlt %*%
      solve(t(dlt) %*% gamma.inv %*% dlt) %*%
        t(dlt) %*% gamma.inv

  chi.val <-  nrow(smpl.frm) * t(diff) %*% V %*% diff
  df      <- sum(diag(gamma %*% V))
  p.val   <- 1 - pchisq(chi.val, df)

  return(list(chi.val = chi.val,
              df = df,
              p.val = p.val))
}

"rob.delta" <-
function(amat)
{
  # Return a matrix some columns are partialed out based on a given adajcent matrix
  # amat: an adjacent matrix
  # Value: a matrix of indices partilaed out

  p <- nrow(amat)

  amat <- diag(p) + amat
  del <- as.logical(amat[lower.tri(amat, TRUE)]) # 0: non-adjacent; 1: adjacent

  delta <- diag(0.5 * p * (p+1))
  return(delta[, del])
}

"rob.estimConGraph" <-
function(amat, w.par, smpl.frm, it.limit = 200, tol = 1e-06)
{
  ################################################################
  mu <- function(smpl.frm, w.par, w.vec) {
    dnm <- t(exp(w.par * w.vec)) %*% as.matrix(smpl.frm)
    nmr <- sum(exp(w.par * w.vec))
    if (is.infinite(nmr)) {
      print(paste("Denominator: ", dnm))
      print(paste("Numerator: ", nmr))
      stop("Convergence failed.\n")
    }

    return(dnm / nmr)
  }
  
  ################################################################
  sigma <- function(smpl.frm, mu, w.par, w.vec) {
    swped <- as.matrix(sweep(smpl.frm, 2, mu))
    exp.vec <- as.vector(exp(w.par * w.vec))

    p <- ncol(smpl.frm)
    n <- nrow(smpl.frm)
    sigma <- matrix(0, p, p)
    for (i in 1:n) {
      sigma <- sigma + exp.vec[i] * swped[i, ] %o% swped[i, ]
    }

    dnm <- mean(exp.vec) - (w.par / (w.par + 1)^(1 + 0.5 * p))
    sigma <- sigma / (n * dnm)

    return(sigma)
  }
  
  ################################################################
  weight <- function(smpl.frm, mu, Sigma) {
    S.inv <- solve(Sigma)
    swped <- sweep(smpl.frm, 2, mu)
    
    w.vec <- apply(swped, 1, function(swprow) {
      -0.5 * swprow %*% S.inv %*% swprow
    })
    
    return(w.vec)
  }
  
  ################################################################
  # Main routine
  p <- ncol(smpl.frm)
  n <- nrow(smpl.frm)

  tf <- TRUE

  # Initialze
  rM <- mean(smpl.frm)
  rS <- cov(smpl.frm)
  w.vec <- rep(1, n) # Conventional ML setting

  it <- 0
  repeat {
    it <- it + 1

    rS.old <- rS
    rM <-
      mu(smpl.frm = smpl.frm, w.par = w.par, w.vec = w.vec)
    rS <-
      sigma(smpl.frm = smpl.frm, mu = rM, w.par = w.par, w.vec = w.vec)
    rS <- fitConGraph(amat = amat, S = rS, n = n, pri = FALSE, alg = 2, tol = tol)$Shat

    w.vec <- weight(smpl.frm = smpl.frm, mu = rM, Sigma = rS)

    if (sum(abs(rS.old - rS)) < tol) {
      rslt <-
        list(mu = rM, Sigma = rS, weight = w.vec, w.par = w.par, it = it, success = tf)
      break
    }

    if (it > it.limit) {
      tf <- FALSE
      rslt <-
        list(mu = NULL, Sigma = NULL, weight = NULL, w.par = w.par, it = it, success = tf)
      break
    }
  }

  return(rslt)
}

"rob.fitConGraph" <-
function(amat, w.par, smpl.frm, it.limit = 200, tol = 1e-06)
{
  cgm <- rob.estimConGraph(amat = amat, w.par = w.par, smpl.frm = smpl.frm,
                            it.limit = it.limit, tol = tol)

  rbt <- rob.browne.test(estcov = cgm$Sigma, amat = amat,
                         w.par = w.par, smpl.frm = smpl.frm)

  return(list(mhat = cgm$mu,
              Shat = cgm$Sigma,
              w.vec = cgm$weight,
              w.par = w.par,
              it = cgm$it,
              tstat = rbt$chi.val,
              df = rbt$df,
              p.val = rbt$p.val))
}

"rob.pcov.test" <-
function(estcov, amat, w.par, smpl.frm)
{
  ################################################################
  nC2 <- function(vec) {
    c1 <- function(vec) {
      rslt <- rep(vec[1], length(vec))
      if (length(vec) > 1)
        rslt <- append(rslt, c1(vec[-1]))
      return(rslt)
    }
    
    c2 <- function(vec) {
      rslt <- vec
      if (length(vec) > 1)
        rslt <- append(rslt, c2(vec[-1]))
      
      return(rslt)
    }

    list.1 <- c1(vec)
    list.2 <- c2(vec)

    return(lapply((1:length(list.1)),
                  function(i) c(list.1[i], list.2[i])))
  }

  pvalue <- function(value, avalue, sample.size) {
    tstat <- value/sqrt(avalue/n)
    pval <- 2*(1 - pnorm(abs(tstat)))
    
    return(list(tstat = tstat,
                pvalue = pval))
  }

  ################################################################
  # Main routine
  p <- nrow(amat)

  sigma.inv.trad <-
    solve(rob.estimConGraph(amat = amat, w.par = 0.0, smpl.frm = smpl.frm)$Sigma)
  pcor.trad <- diag(2, p) - cov2cor(sigma.inv.trad)

  sigma.inv <- solve(estcov)
  pcor <- diag(2, p) - cov2cor(sigma.inv)

  asym.sgm <- rob.asymv.sigma(sigma.inv, amat = amat, w.par = w.par)

  n <- nrow(smpl.frm)
  vals <- as.vector(t(rob.delta(amat)) %*% vech(sigma.inv))
  avals <- diag(asym.sgm)
  vs <- nC2(vertices(amat))[as.logical(vech(amat + diag(p)))]
  trslts <- lapply(1:length(vals), function(i) {
    pval <- pvalue(vals[i], avals[i], n)
    lst <- list(vs = vs[[i]],
                val = vals[i],
                aval = avals[i],
                tstat = pval$tstat,
                pval = pval$pvalue)

    return(lst)
  })
  
  return(trslts)
}

"sqrtm" <-
function(m)
{
  # Get a square root matrix of a square matrix
  # m: a square matrix

  x1 <- eigen(m)
  if(any(x1$values < 0)) {
    stop("A matrix should be positive definite.")
  }
  x1e <- sqrt(diag(x1$values))
  u1 <- cbind(x1$vectors)
  u1i <- solve(u1)
  return(u1 %*% x1e %*% u1i)
}

"vech" <-
function(m, diag = TRUE)
{
  # Return the half vectororized elements of a given matrix.
  # m: a matrix
  
  return(as.matrix(m[lower.tri(m, diag)]))
}

