seal = function(X){
  return (cbind(rbind(X, X[1,])))
}

cplot = function(...){
  # clean plot
  plot(..., asp=1, axes=FALSE, xlab='', ylab='')
}

rotate = function(X, a){
  # rotation matrix
  T = matrix(c(
    cos(a), sin(a),
    -sin(a), cos(a)),
    byrow=TRUE, nrow=2)
  return(X %*% T)
}

translate = function(X, t){
  # translation by given vector
  X = cbind(X, rep(1, dim(X)[1]))
  T = matrix(c(
    1,0,t[1],
    0,1,t[2],
    0,0,1
  ), byrow=TRUE, nrow=3
  )
  T = t(T)
  return((X %*% T)[,c(1,2)])
}

translate_3d = function(X, t){
  # translation by given vector
  X = cbind(X, rep(1, dim(X)[1]))
  T = matrix(c(
    1,0,0,t[1],
    0,1,0,t[2],
    0,0,1,t[3],
    0,0,0,1
  ), byrow=TRUE, nrow=4
  )
  T = t(T)
  return((X %*% T)[,c(1,2,3)])
}

scale = function(X, k){
  # scale by constant or vector
  if(typeof(k) == "double"){
    k = rep(k, dim(X)[1])
  }
  
  if(dim(X)[2] == 2){
    T = matrix(c(
      k[1],0,
      0,k[2]
    ), byrow=TRUE, nrow=2
    )
  } else {
    T = matrix(c(
      k[1],0,0,
      0,k[2],0,
      0,0,k[3]
    ), byrow=TRUE, nrow=3
    )
  }
  
  return(X %*% T)
}


cs = function(X){
  # centroid size
  X = opt_translation(X)
  return(sqrt(sum(diag(X %*% t(X)))))
}


centroid = function(X){
  # centroids
  return(colMeans(X))
}

opt_translation = function(X){
  # optimal translation
  if(dim(X)[2] == 2){
    return(translate(X, -centroid(X)))   
  }else{
    return(translate_3d(X, -centroid(X)))   
  }
}

opt_scaling = function(X, Y){
  # optimal scaling from X to Y
  k = rep(cs(Y) / cs(X), 2)
  return(scale(X, k))
}

opt_rotation = function(X, Y){
  # optimal rotation from X to Y
  s = svd(t(X) %*% Y)
  G = s$u %*% t(s$v)
  return(X %*% G)
}

library(MASS)

apply3d = function(X, fun){
  # apply function on matrices in third dimension
  for(i in 1:dim(X)[3]){
    X[,,i] = fun(X[,,i])
  }
  return(X)
}

procrust = function(M, threshold=0.00001){
  # center and scale all matrices
  M = apply3d(M, function(x) opt_translation(x))
  M = apply3d(M, function(x) scale(x, 1./cs(x)))
  
  # Procrust cycle
  Xp = M[,,1]
  
  while(TRUE){
    last_Xp = Xp
    
    # rotate to reference matrix
    M = apply3d(M, function(x) opt_rotation(x, Xp))
    
    # new reference matrix as a mean
    Xp = apply(M, c(1,2), mean)
    
    # break
    cat('Procrust error:', sum(abs(last_Xp - Xp)), '\n')
    if(sum(abs(last_Xp - Xp)) < threshold){
      break
    }
    if(runif(1) < 0.1){
      break
    }
  }
  return(M)
}

gen_similar = function(Y, m=10){
  # generate similar configuration matrices
  M = array(0, dim=c(dim(Y)[1], dim(Y)[2], m))
  
  for(k in 1:dim(Y)[1]){
    M[k,,] = t(mvrnorm(n=m, mu=Y[k,], Sigma=diag(1,2) * 0.1^2))
  }
  return(M)
}

flatten_matrix = function(X){
  return (cbind(as.vector(X[,1,]), as.vector(X[,2,])))
}