# These functions by John Fox and Georges Monette
# Last modified 2016-05-24 by J. Fox

GaussianElimination <- function(A, B, tol=sqrt(.Machine$double.eps),
                                verbose=FALSE, fractions=FALSE){
    # A: coefficient matrix
    # B: right-hand side vector or matrix
    # tol: tolerance for checking for 0 pivot
    # verbose: if TRUE, print intermediate steps
    # fractions: try to express nonintegers as rational numbers
    # If B is absent returns the reduced row-echelon form of A.
    # If B is present, reduces A to RREF carrying B along.
    printMatrix <- function(A){
        if (fractions) print(fraction(as.matrix(A)))
        else print(round(as.matrix(A), round(abs(log(tol, 10)))))
    }
    rowadd <- function(x, from, to, mult) {
        x[to, ] <- x[to, ] + mult * x[from, ]
        x
    }
    rowswap <- function(x, from, to) {
        x[c(to, from), ] <- x[c(from, to), ]
        x
    }
    rowmult <- function(x, row, mult) {
        x[row, ] <- mult * x[row, ]
        x
    }
    if (fractions) {
        mass <- requireNamespace("MASS", quietly=TRUE)
        if (!mass) stop("fractions=TRUE needs MASS package")
        fraction <- MASS::fractions
        frac <- function(x) as.character(fraction(x))
    }
    if ((!is.matrix(A)) || (!is.numeric(A)))
        stop("argument must be a numeric matrix")
    n <- nrow(A)
    m <- ncol(A)
    if (!missing(B)){
        B <- as.matrix(B)
        if (!(nrow(B) == nrow(A)) || !is.numeric(B))
            stop("argument must be numeric and must match the number of row of A")
        A <- cbind(A, B)
    }
    i <- j <- 1
    if (verbose){
        cat("\nInitial matrix:\n")
        printMatrix(A)
    }
    while (i <= n && j <= m){
        if (verbose) cat("\nrow:", i, "\n")
        while (j <= m){
            currentColumn <- A[,j]
            currentColumn[1:n < i] <- 0
            # find maximum pivot in current column at or below current row
            which <- which.max(abs(currentColumn))
            pivot <- currentColumn[which]
            if (abs(pivot) <= tol) { # check for 0 pivot
                j <- j + 1
                next
            }
            if (which > i) {
                A <- rowswap(A, i, which) # exchange rows (E3)
                if (verbose) {
                    cat("\n exchange rows", i, "and", which, "\n")
                    printMatrix(A)
                }
            }
            A <- rowmult(A, i, 1/pivot) # pivot (E1)
            if (verbose && abs(pivot - 1) > tol){
                cat("\n multiply row", i, "by", 
                    if (fractions) frac(1/pivot) else 1/pivot, "\n")
                printMatrix(A)
            }
            for (k in 1:n){
                if (k == i) next
                factor <- A[k, j]
                if (abs(factor) < tol) next
                A <- rowadd(A, i, k, -factor) # sweep column j (E2)
                if (verbose){
                    if (abs(factor - 1) > tol){
                        cat("\n multiply row", i, "by",
                            if (fractions) frac(abs(factor)) else abs(factor),
                            if (factor > 0) "and subtract from row" else "and add to row", k, "\n")
                    }
                    else{
                        if (factor > 0) cat("\n subtract row", i, "from row", k, "\n")
                        else cat("\n add row", i, "from row", k, "\n")
                    }
                    printMatrix(A)
                }
            }
            j <- j + 1
            break
        }
        i <- i + 1
    }
    # 0 rows to bottom
    zeros <- which(apply(A[,1:m], 1, function(x) max(abs(x)) <= tol))
    if (length(zeros) > 0){
        zeroRows <- A[zeros,]
        A <- A[-zeros,]
        A <- rbind(A, zeroRows)
    }
    rownames(A) <- NULL
    ret <- if (fractions) fraction(A) else round(A, round(abs(log(tol, 10))))
    if (verbose) invisible(ret) else ret
}


matrixInverse <- function(X, tol=sqrt(.Machine$double.eps), ...){
  # returns the inverse of nonsingular X
  if ((!is.matrix(X)) || (nrow(X) != ncol(X)) || (!is.numeric(X))) 
    stop("X must be a square numeric matrix")
  n <- nrow(X)
  X <- GaussianElimination(X, diag(n), tol=tol, ...) # append identity matrix
  # check for 0 rows in the RREF of X:
  if (any(apply(abs(X[,1:n]) <= sqrt(.Machine$double.eps), 1, all)))
    stop ("X is numerically singular")
  X[,(n + 1):(2*n)]  # return inverse
}

RREF <- function(X, ...) GaussianElimination(X, ...)
# returns the reduced row-echelon form of X

Ginv <- function(A, tol=sqrt(.Machine$double.eps), verbose=FALSE, 
  fractions=FALSE){
  # return an arbitrary generalized inverse of the matrix A
  # A: a matrix
  # tol: tolerance for checking for 0 pivot
  # verbose: if TRUE, print intermediate steps
  # fractions: try to express nonintegers as rational numbers
  if (fractions) {
    mass <- requireNamespace("MASS", quietly=TRUE)
    if (!mass) stop("fractions=TRUE needs MASS package")
  }
  m <- nrow(A)
  n <- ncol(A)
  B <- GaussianElimination(A, diag(m), tol=tol, verbose=verbose, 
    fractions=fractions)
  L <- B[,-(1:n)]
  AR <- B[,1:n]
  C <- GaussianElimination(t(AR), diag(n), tol=tol, verbose=verbose, 
    fractions=fractions)
  R <- t(C[,-(1:m)])
  AC <- t(C[,1:m])
  ginv <- R %*% t(AC) %*% L
  if (fractions) MASS::fractions (ginv) else round(ginv, round(abs(log(tol, 10))))
}


cholesky <- function(X, tol=sqrt(.Machine$double.eps)){
  # returns the Cholesky square root of the nonsingular, symmetric matrix X
  # tol: tolerance for checking for 0 pivot
  # algorithm from Kennedy & Gentle (1980)
  if (!is.numeric(X)) stop("argument is not numeric")
  if (!is.matrix(X)) stop("argument is not a matrix")
  n <- nrow(X)
  if (ncol(X) != n) stop("matrix is not square")
  if (max(abs(X - t(X))) > tol) stop("matrix is not symmetric")
  D <- rep(0, n)
  L <- diag(n)
  i <- 2:n
  D[1] <- X[1, 1]
  if (abs(D[1]) < tol) stop("matrix is numerically singular")
  L[i, 1] <- X[i, 1]/D[1]
  for (j in 2:(n - 1)){
    k <- 1:(j - 1)
    D[j] <- X[j, j] - sum((L[j, k]^2) * D[k])
    if (abs(D[j]) < tol) stop("matrix is numerically singular")
    i <- (j + 1):n
    L[i, j] <- (X[i, j] -
      colSums(L[j, k] * t(L[i, k, drop=FALSE]) * D[k]))/D[j]
  }
  k <- 1:(n - 1)
  D[n] <- X[n, n] - sum((L[n, k]^2) * D[k])
  if (abs(D[n]) < tol) stop("matrix is numerically singular")
  L %*% diag(sqrt(D))
}

QR <- function(X, tol=sqrt(.Machine$double.eps)){
  # QR decomposition by Graham-Schmidt orthonormalization
  # X: a matrix
  # tol: 0 tolerance
  if (!is.numeric(X) || !is.matrix(X)) stop("X must be a numeric matrix")
  length <- function(u) sqrt(sum(u^2))
  U <- X
  E <- matrix(0, nrow(X), ncol(X))
  E[, 1] <- U[, 1]/length(U[, 1])
  for (j in 2:ncol(U)){
    for (k in 1:(j - 1)){
      U[, j] <- U[, j] - (X[, j] %*% E[, k]) * E[, k]
    }
    len.U.j <- length(U[, j])
    if (len.U.j > tol) E[, j] <- U[, j]/len.U.j
  }
  R <- t(E) %*% X
  R[abs(R) < tol] <- 0
  rank <- sum(rowSums(abs(R)) > 0)
  list(Q=-E, R=-R, rank=rank) # negated to match qr()
}

eig <- function(X, tol=sqrt(.Machine$double.eps), max.iter=100, retain.zeroes=TRUE){
  # returns the eigenvalues and eigenvectors of a square, symmetric matrix using the iterated QR decomposition
  # X: a square, symmetric matrix
  # tol: 0 tolerance
  # max.iter: iteration limit
  # retain.zeroes: retain 0 eigenvalues?
  if (!is.numeric(X) || !is.matrix(X) || nrow(X) != ncol(X) || any(abs(X - t(X)) > tol)) 
    stop("X must be a numeric, square, symmetric matrix")
  i <- 1
  Q <- diag(nrow(X))
  while (i <= max.iter){
    qr <- QR(X, tol=tol)
    Q <- Q %*% qr$Q 
    X <- qr$R %*% qr$Q
    if (max(abs(X[lower.tri(X)])) <= tol) break
    i <- i + 1
  }
  if (i > max.iter) warning("eigenvalues did not converge")
  values <- diag(X)
  if (!retain.zeroes){
    nonzero <- values != 0
    values <- values[nonzero]
    Q <- Q[, nonzero, drop=FALSE]
  }
  list(values=values, vectors=Q)
}

SVD <- function(X, tol=sqrt(.Machine$double.eps)){
  # compute the singular-value decomposition of a matrix X from the eigenstructure of X'X
  # X: a matrix
  # tol: 0 tolerance
  VV <- eig(t(X) %*% X, tol=tol, retain.zeroes=FALSE)
  V <- VV$vectors
  d <- sqrt(VV$values)
  U <- X %*% V %*% diag(1/d,nrow=length(d)) # magically orthogonal
  list(d=d, U=U, V=V)
}