#' Subspace distance
#'
#' @param A,B Basis matrices as representations of elements of the Grassmann
#'  manifold.
#' @param is.ortho Boolean to specify if \eqn{A} and \eqn{B} are semi-orthogonal.
#'   If false, the projection matrices are computed as
#' \deqn{P_A = A (A' A)^{-1} A'}
#'  otherwise just \eqn{P_A = A A'} since \eqn{A' A} is the identity.
#' @param normalize Boolean to specify if the distance shall be normalized.
#'  Meaning, the maximal distance scaled to be \eqn{1} independent of dimensions.
#' 
#' @seealso
#'  K. Ye and L.-H. Lim (2016) "Schubert varieties and distances between
#'  subspaces of different dimensions" <arXiv:1407.0900>
#' 
#' @export
dist.subspace <- function (A, B, is.ortho = FALSE, normalize = FALSE,
    tol = sqrt(.Machine$double.eps)
) {
    if (!is.matrix(A)) A <- as.matrix(A)
    if (!is.matrix(B)) B <- as.matrix(B)

    if (!is.ortho) {
        qrA <- qr(A, tol)
        if (qrA$rank < ncol(A)) {
            A <- qr.Q(qrA)[, abs(diag(qr.R(qrA))) > tol, drop = FALSE]
        } else {
            A <- qr.Q(qrA)
        }
        qrB <- qr(B, tol)
        if (qrB$rank < ncol(B)) {
            B <- qr.Q(qrB)[, abs(diag(qr.R(qrB))) > tol, drop = FALSE]
        } else {
            B <- qr.Q(qrB)
        }
    }

    PA <- tcrossprod(A, A)
    PB <- tcrossprod(B, B)

    if (normalize) {
        rankSum <- ncol(A) + ncol(B)
        c <- 1 / sqrt(max(1, min(rankSum, 2 * nrow(A) - rankSum)))
    } else {
        c <- sqrt(2)
    }

    c * norm(PA - PB, type = "F")
}