#' Draws a sample from the invariant measure on the Stiefel manifold \eqn{S(p, q)}.
#'
#' @param p row dimension
#' @param q col dimension
#' @return \code{p} times \code{q} semi-orthogonal matrix.
#' @examples
#'  V <- rStiefel(6, 4)
#' @export
rStiefel <- function(p, q) {
    return(qr.Q(qr(matrix(rnorm(p * q, 0, 1), p, q))))
}

#' Retraction to the manifold.
#'
#' @param A matrix.
#' @return `(p, q)` semi-orthogonal matrix, aka element of the Stiefel manifold.
#' @keywords internal
#' @export
retractStiefel <- function(A) {
    return(qr.Q(qr(A)))
}

#' Skew-Symmetric matrix computed from `A` as
#' \eqn{1/2 (A - A^T)}.
#' @param A Matrix of dim `(p, q)`
#' @return Skew-Symmetric matrix of dim `(p, p)`.
#' @keywords internal
#' @export
skew <- function(A) {
    0.5 * (A - t(A))
}

#' Symmetric matrix computed from `A` as
#' \eqn{1/2 (A + A^T)}.
#' @param A Matrix of dim `(p, q)`
#' @return Symmetric matrix of dim `(p, p)`.
#' @keywords internal
#' @export
sym <- function(A) {
    0.5 * (A + t(A))
}

#' Orthogonal Projection onto the tangent space of the stiefel manifold.
#'
#' @param V Point on the stiefel manifold.
#' @param G matrix to be projected onto the tangent space at `V`.
#' @return `(p, q)` matrix as element of the tangent space at `V`.
#' @keywords internal
#' @export
projTangentStiefel <- function(V, G) {
    Q <- diag(1, nrow(V)) - V %*% t(V)
    return(Q %*% G + V %*% skew(t(V) %*% G))
}

#' Null space basis of given matrix `V`
#'
#' @param V `(p, q)` matrix
#' @return Semi-orthogonal `(p, p - q)` matrix spaning the null space of `V`.
#' @keywords internal
#' @export
null <- function(V) {
    tmp <- qr(V)
    set <- if(tmp$rank == 0L) seq_len(ncol(V)) else -seq_len(tmp$rank)
    return(qr.Q(tmp, complete = TRUE)[, set, drop = FALSE])
}

#' Creates a (numeric) matrix where each column contains
#' an element to element matching.
#' @param elements numeric vector of elements to match
#' @return matrix of size `(2, n * (n - 1) / 2)` for a argument of lenght `n`.
#' @keywords internal
#' @examples
#'  elem.pairs(seq.int(2, 5))
#' @export
elem.pairs <- function(elements) {
    # Number of elements to match.
    n <- length(elements)
    # Create all combinations.
    pairs <- rbind(rep(elements, each=n), rep(elements, n))
    # Select unique combinations without self interaction.
    return(pairs[, pairs[1, ] < pairs[2, ]])
}