#' Duplication Matrix
#'
#' Matrix `D` such that `vec(A) = D vech(A)` for `A` symmetric
#' @examples
#' p <- 8
#' A <- matrix(rnorm(p^2), p, p)
#' A <- A + t(A)
#' stopifnot(all.equal(c(Dup(nrow(A)) %*% vech(A)), c(A)))
#'
#' @export
Dup <- function(p) {
    # setup `vec` and `vech` element indices (zero indexed)
    vec <- matrix(NA_integer_, p, p)
    vec[lower.tri(vec, diag = TRUE)] <- seq_len(p * (p + 1) / 2)
    vec[upper.tri(vec)] <- t(vec)[upper.tri(vec)]
    vech <- matrix(vec)[lower.tri(vec, diag = TRUE)]

    # construct duplication matrix
    Dp <- outer(c(vec), c(vech), `==`)
    storage.mode(Dp) <- "double"
    Dp
}

#' Pseudo Inverse of the Duplication Matrix
#'
#' Matrix such that `vech(A) = D^+ vec(A)` for `A` symmetric
#'
#' @examples
#' p <- 5
#' stopifnot(all.equal(Dup(p) %*% Dup.pinv(p), N(p)))
#'
#' @export
Dup.pinv <- function(p) {
    Dp <- Dup(p)
    solve(crossprod(Dp), t(Dp))
}

#' Generalized Commutation Permutation
#'
#' Equivalent permutation to the Commutation matrix.
#'
#' @examples
#' A <- array(rnorm(105), c(28, 7, 11))
#' stopifnot(expr = {
#'     all.equal(c(A)[K.perm(dim(A), 1)], c(A))
#'     all.equal(c(A)[K.perm(dim(A), 2)], c(K(dim(A), 2) %*% c(A)))
#'     all.equal(c(A)[K.perm(dim(A), 3)], c(K(dim(A), 3) %*% c(A)))
#' })
#'
#' @export
K.perm <- function(dim, mode) {
    # special case of classic commutation matrix `K(p, q) == K(c(p, q), 2)`
    if (length(dim) == 1) {
        dim <- c(dim, mode)
        mode <- 2
    }

    # construct permutation
    c(aperm(array(seq_len(prod(dim)), dim), c(mode, seq_len(length(dim))[-mode])))
}

#' Generalized Commutation Matrix
#'
#'      K_p,(m) vec(A) = vec(A_(m))
#'
#' for an `p` dimensional array `A`. The special case of the commutation matrix
#' is given by `K(p, q)` for the matrix dimensions `p by q`.
#'
#' @examples
#' # Special case of matrices
#' stopifnot(all.equal(K(5, 7), K(c(5, 7), 2)))
#' A <- matrix(rnorm(35), 5, 7)
#' stopifnot(all.equal(c(K(5, 7) %*% c(A)), c(t(A))))
#'
#' # Kronecker commutation identity
#' A <- matrix(rnorm(3 * 7), 3, 7)
#' B <- matrix(rnorm(5 * 2), 5, 2)
#' stopifnot(all.equal(
#'     B %x% A,
#'     K(nrow(B), nrow(A)) %*% (A %x% B) %*% K(ncol(A), ncol(B))
#' ))
#'
#' # General case for tensors
#' A <- array(rnorm(105), c(28, 7, 11))
#' stopifnot(all.equal(K(dim(A), 1), diag(prod(dim(A)))))
#' stopifnot(all.equal(c(K(dim(A), 2) %*% c(A)), c(mat(A, 2))))
#' stopifnot(all.equal(c(K(dim(A), 3) %*% c(A)), c(mat(A, 3))))
#'
#' # Generalized Kronecker Product Commutation Identity
#' p <- c(4, 2, 3, 4)
#' q <- c(3, 4, 2, 5)
#' A <- mapply(function(p_i, q_i) {
#'     matrix(rnorm(p_i * q_i), p_i, q_i)
#' }, p, q)
#' for (mode in seq_along(A)) {
#'     stopifnot(all.equal(
#'         Reduce(`%x%`, A[c(rev(seq_along(A)[-mode]), mode)]),
#'         K(p, mode) %*% Reduce(`%x%`, rev(A)) %*% t(K(q, mode))
#'     ))
#' }
#'
#' @export
K <- function(dim, mode) {
    # special case of classic commutation matrix `K(p, q) == K(c(p, q), 2)`
    if (length(dim) == 1) {
        dim <- c(dim, mode)
        mode <- 2
    }

    # construct permutation
    perm <- aperm(array(seq_len(prod(dim)), dim), c(mode, seq_len(length(dim))[-mode]))
    # commutation matrix
    diag(prod(dim))[perm, ]
}

#' Symmetrizer Matrix
#'
#'      N_p vec(A) = 1/2 (vec(A) + vec(A'))
#'
#' @examples
#' p <- 7
#' stopifnot(all.equal(N(p), Dup(p) %*% Dup.pinv(p)))
#'
#' @export
N <- function(p) {
    0.5 * (diag(p^2) + K(p, p))
}

#' Selection Matrix
#'
#' Selects the diagonal elements of a matrix vectorization
#'
#' @export
S <- function(p) {
    index <- matrix(seq_len(p^2), p, p)
    s <- outer(diag(index), c(index), `==`)
    storage.mode(s) <- "integer"
    s
}
