#' Estimate measurement error covariance matrices
#'
#' Computes observation-specific estimates of the measurement error
#' covariance matrix using replicate measurements of contaminated covariates.
#'
#' @param W Numeric array of dimension \code{n x p x J} containing replicate
#'   measurements of contaminated covariates. Missing replicates should be
#'   coded as \code{NA}.
#'
#' @return Numeric array of dimension \code{n x p x p}. If an observation has
#'   fewer than two available replicates, the corresponding covariance matrix
#'   is returned as \code{NA}.
#'
#' @noRd

estimating_SigmaU <- function(W) {

  stopifnot(length(dim(W)) == 3)

  n <- dim(W)[1]
  p <- dim(W)[2]
  Jmax <- dim(W)[3]

  SigmaU_hat <- array(NA, dim = c(n, p, p))

  for (i in 1:n) {

    # find valid replicates for obs i
    valid <- which(!is.na(W[i, 1, ]))
    ni <- length(valid)

    if (ni < 2) {
      SigmaU_hat[i, , ] <- NA
      next
    }

    acc <- matrix(0, p, p)

    for (a in 1:(ni - 1)) {
      for (b in (a + 1):ni) {

        r1 <- valid[a]
        r2 <- valid[b]

        diff <- W[i, , r1] - W[i, , r2]
        acc <- acc + tcrossprod(diff)
      }
    }

    SigmaU_hat[i, , ] <- acc / (ni * (ni - 1))
  }

  ## Sigma_u_bar
  Wbar <- apply(W, c(1, 2), mean, na.rm = TRUE)
  nj <- apply(W[, 1, , drop = FALSE], 1, function(x) sum(!is.na(x)))

  ## Sigma_U after averaging
  SigmaU_bar <- array(0, dim = c(n, p, p))
  for (i in 1:n) {
    SigmaU_bar[i, , ] <- SigmaU_hat[i, , ] / nj[i]
  }

  mean_SigmaU_bar <- apply(SigmaU_bar, c(2, 3), mean, na.rm = TRUE)

  list(SigmaU_bar, mean_SigmaU_bar)
}


