#' Extened Orthogonal Procrustes Rotation
#'
#' @param A The input matrix to be rotated
#' @param C The target matrix
#' @param estm = 1 to estimate column means
#' @param estd = 1 to estimate scaling constant
#' @param maxiter max # of iterations
#' @param eps convergence criterion for rmse
#' @param print = 0 to surpress the output
#'
#' @return A list of: \cr
#' B=d * A \%*\% T + matrix(1,nv,1)\%*\%t(m), \cr
#'  T, m, d, Cm the updated target matrix, rmse=sqrt(RSS)
#'
#' @examples
#'  set.seed(1701)
#'  nv <- 10; nr <- 2
#'  pmiss <- 0.3
#'
#'  A <- matrix(rnorm(nv*nr),nv,nr)
#'  dum <- matrix(rnorm(nv*nr),nv,nr)
#'  T <- eigen(t(dum)%*%dum)[[2]]
#'  m <- matrix(rnorm(nr),nr,1)
#'  d <- 10*runif(1)
#'  C <- d*A%*%T + matrix(rep(1,nv),nv,1)%*%t(m)
#'
#'  res <- procote( A, C, estm=0, estd=0, print=1 )
#'  res2 <- procote( A, C, estm=1, estd=1, print=1 )
#'
#'  # missing elements in the target
#'  dum <- matrix(runif(nv*nr),nv,nr)
#'  C[which(dum < pmiss)] <- NA
#'
#'  res3 <- procote( A, C, estm=0, estd=0, print=1 )
#'  res4 <- procote( A, C, estm=1, estd=1, print=1 )
#'
#'
#' @details
#' This program finds those A, m, d  and orthonormal T which minimize \cr
#'   \deqn{RSS = tr( ( C  - Chat )' ( C - Chat ) ) }\cr
#'   where \eqn{Chat = d * A \%*\% T + matrix(1,nv) \%*\% t(m)}, \cr
#'   \eqn{d} is a scalar, \eqn{T} is an orthonormal rotation matrix, \cr
#'   and \eqn{m} is the column mean vector. \cr
#'\cr
#' Note that when \code{estd=0} and \code{estm=0}
#' , and the target matrix does not
#' have missing elements, \cr
#' this is equivalent to the usual orthogonal Procrustes rotation.
#'
#' @export
#'

procote <- function( A, C, estm=0, estd=0, maxiter=200, eps=1e-6, print=2 ){
 # Extended Orthogonal Procrustes Rotation
 # Shin-ichi Mayekawa
 # translated from IML 880623-071222: 111109,10
 # ssq removed: 111204
 # bugfix for m: 111220
 # printm -> Print, print B: 20140816
 # lmax -> maxiter: 20170530dnc
 # remove ;  at the end: 20260516
 # Print -> printm: 20260516
 # tr, Diag replaced: 20260516
 #
 # Args:
 #  A       nv x nr matrix to be rotated
 #  C       nv x nr target matrix
 #  estm    = 1 to estimate m
 #  estd    = 1 to estimate d
 #  maxiter    max # of iterations when there are NAs in C
 #  eps     eps for the relative improvement of RMSE
 #  print   to control the print level
 #
 # Returns:
 #  list(B=B, T=T, m=m, d=d, Cm=Cm, rmse=rmse)
 #  where B is the result of the rotation,
 #  Cm is the nv x nr matrix with the missing elements estimated by LS.
 #
 # The program minimizes
 #   RSS = TR( ( C  - (d A T + 1*m`) )` ( C - (d A T + 1*m`) ) )
 #  w.r.t T, m and d subject to T'T = TT' = I  and d > 0.
 # The missing elements of C matrix, if any,
 # will be estimated so that they also
 # minimize RSS.
 #
 # rmse = sqrt(RSS/(nv*nr)) will be reported.
 #

 # const
 nv=nrow(A); nr=ncol(A)

 Aorg=A
 if (estm == 1){
  amean=as.matrix(apply(A,2,mean),ncol(A),1)
  A=A-matrix(1,nv,1)%*%t(amean)
 }
 traceAtA=sum(diag(t(A)%*%A))

 # missing target elements
 Cm=C
 locmiss=is.na(C)   # logical matrix of the same size as C
 nmiss=sum(locmiss)
 pmiss=nmiss/(nv*nr)
 if (nmiss > 0)
  Cm[locmiss]=A[locmiss]

 converged=0;

 if (print >= 1){
  cat("\n"); cat("\n"); cat("\n");
  cat("Extended Orthogonal Procrustes Rotation:\n")
  cat(" minimizing  TR( ( C  - (d A T + 1*m`) )` ( C - (d A T + 1*m`) ) )\n")
  cat("  w.r.t T, m and d subject to T'T = TT' = I  and d > 0 \n")
  cat(" The missing elements of the targetmatrix C, if any,",
      "will be updated.\n")
  cat("\n")
  cat("Input parameters:\n")
  cat(" Size of A and C matrices: ", nv, " x ", nr, "\n")
  cat(" estimation of m = ", estm, ", estimation of d = ", estd, "\n")
  cat(" max # of iterations = ", maxiter, ", eps for relative rmseimp = ", eps
  , "\n")
  cat(" print = ", print, "\n")
   cat(" # of missing elements in C = ", nmiss, "(", pmiss, ")\n")
 }


 # initial values
 d=1; m=matrix(0,nr,1)
 T=diag(nr); Tp=T
 rmsep=sqrt(sum((Cm-A)^2)/(nv*nr))
 rmse0=rmsep


 # main iteration
 for (llll in 1:maxiter){

  # update orthogonal T;
  CAAC=t(Cm)%*%A%*%t(A)%*%Cm
  ev=eigen(CAAC); eva=matrix(ev$values,length(ev$values),1); eve=ev$vectors
  rm(ev)
  loc=which(eva > 0); eva=eva[loc]; eve=eve[,loc,drop=FALSE]
  SQR=eve%*%diag(sqrt(1/eva),nrow=nr,ncol=nr)%*%t(eve)
  T=t(A)%*%Cm%*%SQR

  B=A%*%T

  # update m
  if (estm == 1) {
   m=as.matrix(apply(Cm-d*A%*%T,2,mean),nr,1)
  }

  # update d
  if (estd == 1 ) {
   d=sum(diag(t(B)%*%Cm))/traceAtA
   # make d positive
   if ( d < 0 ){
    d=-d; T=-T
   }
  }

#  Print(d, A, T, m)
  B=d*A%*%T + matrix(1,nv,1)%*%t(m)

  # check convergence
  rmse=sqrt( sum((Cm-B)^2) / (nv*nr) )
  rmseimp=rmsep-rmse
  if( rmsep > 0 ) rmseimpr=rmseimp/rmsep
  else rmseimpr=0
  maxadt=max(abs(T-Tp))

  if ( print >= 2 )
   printm(llll, rmsep, rmse, rmseimp, rmseimpr, maxadt, fmt="3 .4")

   if ( (rmseimpr < eps) || (nmiss == 0 && estd == 0 && estm == 0) ) {
    converged=1
    break
   }

   # update the target
   if ( nmiss > 0 ){
    Cm[locmiss]=B[locmiss]
   }

   # next iteration
   Tp=T; rmsep=rmse

  } # end of llll loop


  if ( !converged )
   cat("\n Warning(procote): Iteration did not converge.\n\n")

  # update m using the original A
  if (estm == 1) {
   m=as.matrix(apply(Cm-d*Aorg%*%T,2,mean),nr,1)
  }

  B=d*Aorg%*%T+matrix(1,nv,1)%*%t(m)
  rmse=sqrt( sum((Cm-B)^2) / (nv*nr) )

  if ( print > 0 ){
   if ( estd == 1 || estm == 1 || nmiss > 0 ){
    cat("Iteration Terminated.\n")
   printm(llll, rmsep, rmse, rmseimp, rmseimpr, maxadt, fmt=" 3 .4");
   }
   cat(" Final RMSE = ", rmse, "\n")
   cat("\n")
   printm(B, fmt=".4")
   printm(T, m, d, fmt=".4")
  };

  # returning the result
  res=list(B=B, T=T, m=m, d=d, Cm=Cm, rmse=rmse)
  return( res )


 } # end of procote


