# runs simulations in confornation space
simallo <- function (n, mshape, msize, deltavar, phi, alpha, epsvar, eigfac=1, lognormal=FALSE) {
  p <- length(mshape)
  if (lognormal) {
    delvec <- msize * rlnorm(n, 0, sqrt(deltavar)) - msize
  } else {
    delvec <- rnorm(n, 0, sqrt(deltavar))
  }
  if (any(delvec < -msize)) {
    delvec[which(delvec < -msize)] <- -msize + 0.000001
    message("Negative values for size encountered in simulation")
  }
  if (eigfac >= 1) {
    eps <- matrix(rnorm(n*p, 0, sqrt(epsvar / p)), n, p)
  } else {
    sum <- (eigfac^p - 1) / (eigfac - 1)
    evs <- (epsvar / sum) * eigfac ^ (0:(p-1))
    rot <- ranrot(p)
    eps <- matrix(rnorm(n*p), n, p) %*% diag(sqrt(evs)) %*% rot
  }
  xmat <- (msize + (cos(phi) * delvec)) %o% mshape + (sin(phi) * delvec) %o% alpha + eps
  return(xmat)
}

#runs simulations in shape tangent space
shsimallo <- function (n, mshape, msize, deltavar, phi, alpha, epsvar, eigfac=1, lognormal=FALSE) {
  p <- length(mshape)
  if (lognormal) {
    delvec <- msize * rlnorm(n, 0, sqrt(deltavar)) - msize
  } else {
    delvec <- rnorm(n, 0, sqrt(deltavar))
  }
  if (any(delvec < -msize)) {
    delvec[which(delvec < -msize)] <- -msize + 0.000001
    message("Negative values for size encountered in simulation")
  }
  if (eigfac >= 1) {
    eps <- matrix(rnorm(n*p, 0, sqrt(epsvar / p)), n, p)
  } else {
    sum <- (eigfac^p - 1) / (eigfac - 1)
    evs <- (epsvar / sum) * eigfac ^ (0:(p-1))
    rot <- ranrot(p)
    eps <- matrix(rnorm(n*p), n, p) %*% diag(sqrt(evs)) %*% rot
  }
  eps <- eps / msize
  regcoef <- sin(phi) / msize
  ones <- rep(1,n)
  allotang <- ones %o% mshape + (regcoef*delvec) %o% alpha + eps
  scale <- sqrt(rowSums(allotang^2))
  xmat <- allotang * (1 / scale) * (ones * msize + delvec)
  return(xmat)
}

# generates a random shape (in tangent space if refvec is provided)
ranshape <- function(k, m, refvec=NULL) {
  ranvec <- rnorm(k * m)
  mat <- matrix(0, k * m, m)
  mat[(rep(1:k-1,times=m)*m) + (rep(1:m-1,each=k)*k*m) + rep(1:m,each=k)] <- sqrt(1/k)
  centmat <- diag(k * m) - mat %*% t(mat)
  ranvec <- centmat %*% ranvec
  if (! is.null(refvec)) {
    refarr <- matrix(refvec, m, k)
    ranarr <- matrix(ranvec, k, m, byrow=TRUE)
    sv <- svd(refarr %*% ranarr)
    ranarr <- ranarr %*% (sv$v %*% t(sv$u))
    ranvec <- (diag(k*m) - (refvec %o% refvec)) %*% as.vector(t(ranarr))
  }
  ranvec <- as.vector(ranvec) / (sqrt(sum(ranvec^2)))
  return(ranvec)
}

# minimal Procrustes fit routine
procfit <- function(X, k, m, shape=TRUE, iniavg=NULL) {
  n <- nrow(X)
  mat <- matrix(0, k * m, m)
  mat[(rep(1:k-1,times=m)*m) + (rep(1:m-1,each=k)*k*m) + rep(1:m,each=k)] <- sqrt(1/k)
  centmat <- diag(k * m) - mat %*% t(mat)
  cX <- X %*% centmat
  csize <- sqrt(rowSums(cX^2))
  if (shape) {
    cX <- cX / csize
  }
  xvec <- as.vector(t(cX))
  xarr <- aperm(array(xvec,c(m,k,n)),c(2,1,3))
  if (is.null(iniavg)) {
    avg <- xarr[,,1]
  } else {
    avg <- matrix(iniavg,k,m,byrow=TRUE)
  }
  diff <- 1
  while (diff > 1e-10) {
    tavg <- t(avg)
    tot <- matrix(0,k,m)
    for (i in 1:n) {
      sv <- svd(tavg %*% xarr[,,i])
      tot <- tot + xarr[,,i] %*% (sv$v %*% t(sv$u))
    }
    newc <- tot / n
    if (shape) {
      newc <- newc * 1/sqrt(sum(newc^2))
    }
    diff <- sqrt(sum((newc - avg)^2))
    avg <- newc
  }
  tavg <- t(avg)
  xproc <- matrix(0, n, k*m)
  for (i in 1:n) {
    sv <- svd(tavg %*% xarr[,,i])
    mat <- xarr[,,i] %*% (sv$v %*% t(sv$u))
    xproc[i,] <- as.vector(t(mat))
  }
  avgvec <- as.vector(tavg)
  avgmat <- matrix(avgvec, n, k*m, byrow=TRUE)
  if (shape) {
    tanproc <- (xproc - avgmat) %*% (diag(k*m) - avgvec %o% avgvec) + avgmat
  } else {
    tanproc <- xproc
  }
  out <- list("mean"=avgvec, "xproc"=xproc, "tanproc"=tanproc, "csize"=csize)
  return(out)
}

# generates distance matrix from a set of observations
pwprocdist <- function (X, k, m, shape=TRUE) {
  n <- nrow(X)
  mat <- matrix(0, k * m, m)
  mat[(rep(1:k-1,times=m)*m) + (rep(1:m-1,each=k)*k*m) + rep(1:m,each=k)] <- sqrt(1/k)
  centmat <- diag(k * m) - mat %*% t(mat)
  cX <- X %*% centmat
  csize <- sqrt(rowSums(cX^2))
  xvec <- as.vector(t(cX))
  xarr <- aperm(array(xvec,c(m,k,n)),c(2,1,3))
  if (shape) {
    for (i in 1:n) {
      xarr[,,i] <- xarr[,,i] / csize[i]
    }
  }
  diff <- matrix(0,n,n)
  for (i in 1:(n-1)) {
    trtar <- t(xarr[,,i])
    for (j in (i+1):n) {
       sv <- svd(trtar %*% xarr[,,j])
       mat <- xarr[,,j] %*% (sv$v %*% t(sv$u))
       diff[i,j] <- sqrt(sum((xarr[,,i] - mat)^2))
    }
  }
  return(diff)
}

# generates a random rotation matrix
ranrot <- function (k) {
  mat <- matrix(rnorm(k*k), k, k)
  qrd <- qr(mat)
  qmat <- qr.Q(qrd, complete=TRUE)
  if (det(qmat) < 0) {
    qmat[c(1,2),] <- qmat[c(2,1),]
  }
  return(qmat)
}

# generates matrix of conformation and Boas distances from a set of observations
distcomp <- function (X, k, m) {
  prconf <- procfit(X, k, m, shape=FALSE)
  xconf <- prconf$xproc
  xsh <- procfit(X, k, m, shape=TRUE)
  xboas <- xsh$xproc * xsh$csize 
  n <- nrow(X)
  dist <- matrix(0, n, n)
  for (i in 1:(n-1)) {
    for (j in (i+1):n) {
      dist[i,j] <- sqrt(sum((xconf[i,] - xconf[j,])^2))
      dist[j,i] <- sqrt(sum((xboas[j,] - xboas[i,])^2))
    }
  }
  return(dist)
}