#' Several internal functions to run the ROPES algorithm
#' 
#' @aliases dF dFWr Fn FnWr weightedNDiffMatrix splineMatrix bfgs cg
#'
#' @description 
#' This file contains eight extra functions. 
#' \code{dFWr} is a helper function to return the gradient for the 'BFGS' 
#' and 'CG' methods. It calls the \code{dF} function.
#' \code{FnWr} contains the function to be minimized. It calls 
#' the \code{Fn} function.
#' \code{weightedNDiffMatrix} is the linear operator DIFF_i(\alpha), see 
#' page 21 of the reference below.
#' \code{splineMatrix} is to compute the average of all curves
#' \code{bfgs} and \code{cg} execute the 'BFGS' and 'CG' methods, 
#' respectively.
#' 
#' These functions are called by \code{\link{ropes}}, so they are not 
#' solely used and therefore, there is no examples section here.
#' 
#' @keywords internal
#' 
#' @note 
#' These functions were kindly provided by Alexander Dokumentov 
#' and Rob Hyndman.
#'
#' @references 
#' Dokumentov, A., Hyndman, R. J., 2016. Low-dimensional decomposition, 
#' smoothing and forecasting of sparse functional data, 
#' \url{http://robjhyndman.com/papers/ROPES.pdf}. Working paper.
#'  
#' @author 
#' Alexander Dokumentov and Rob Hyndman
#' 
#' @seealso 
#' \code{\link{ropes}}

#' @importFrom MASS ginv
#' @importFrom compiler cmpfun

library(MASS)
library(compiler)

BAwR_internalROPES <- function(){} # This is to create an Rd file with 
# the same name as the function. I didn't find another alternative to do 
# this. Look for the word "internal" in r-pkgs.had.co.nz/man.html

dF <- cmpfun(function(W, twoW2, Y, U, V, A, B, twoAtA, twoBtB){
  com = twoW2 * (U %*% t(V) - Y)
  dFdU = com %*% V + twoAtA %*% U
  dFdV = t(com) %*% U + twoBtB %*% V
  return(list(dFdU = dFdU, dFdV = dFdV))
})

dFWr <- cmpfun(function(x, dimU, dimV, ...){
  U = array(x, dimU)
  V = array(x[(dimU[1] * dimU[2] + 1):(length(x))], dimV)
  result = dF(U = U, V = V, ...)
  return(c(as.vector(result$dFdU), as.vector(result$dFdV)))
})

Fn <- cmpfun(function(W, twoW2, Y, U, V, A, B, twoAtA, twoBtB){
  return(sum((W * (Y - U %*% t(V)))^2) + sum((A %*% U)^2) + sum((B %*% V)^2))
})

FnWr <- cmpfun(function(x, dimU, dimV, ...){
  U = array(x, dimU)
  V = array(x[(dimU[1]*dimU[2] + 1):(length(x))], dimV)
  return(Fn(U = U, V = V, ...))
})

weightedNDiffMatrix <- function(nCol, differences, weights){
  if (length(weights) == 1) weights = rep(weights, nCol - differences)
  if (nCol != length(weights) + differences) stop("nCol != length(weights) + differences")
  if (differences <= 0) return(diag(weights))
  else return(diag(weights) %*% diff(diag(rep(1, nCol)), differences = differences))
}

splineMatrix <- function(b){
  x = c()
  y = c()
  counter = 1
  for (j in 1:ncol(b)) {
    for (i in 1:nrow(b)) {
      if (!is.na(b[i,j])) {
        x[counter] = j
        y[counter] = b[i,j]
        counter = counter + 1
      }
    }
  }
  sm = smooth.spline(x,y)

  #if(length(unique(x)) == ncol(b)){
  #  return(matrix(sm$y, 1, ncol(b)))
  #}else{
  #  diff_cols <- ncol(b) - length(unique(x))
  #  return(matrix(sm$y, 1, ncol(b) - diff_cols))
  #}
  
  return(matrix(sm$y, 1, ncol(b)))
}

bfgs <- function(W, Y, A, B, UStart, VStart, maxIter = 100000L, 
                 factr = 1000, trace = 0, lmm = 5){
  twoW2 = 2 * W * W
  twoAtA = 2 * (t(A) %*% A)
  twoBtB = 2 * (t(B) %*% B)
  
  x = c(as.vector(UStart), as.vector(VStart))
  dimU = dim(UStart)
  dimV = dim(VStart)
  result = optim(par = x, fn = FnWr, gr = dFWr, dimU = dimU, dimV = dimV, 
                 W = W, twoW2 = twoW2, Y = Y, A = A, B = B, 
                 twoAtA = twoAtA, twoBtB = twoBtB, method = "L-BFGS-B", 
                 control = list(trace = trace, maxit = maxIter, factr = factr, 
                                lmm = lmm))
  
  if (result$convergence == 0){
    cat("\nMethod bfgs converged calculating function: "); cat(result$counts[1]); 
    cat(" times and gradient: "); cat(result$counts[2]); cat(" times")
  }else{
    cat("\nMethod bfgs did not converge.\nError code (optim(...)$convergence): "); cat(result$convergence)
    cat("\nError message (optim(...)$message): "); cat(result$message)
  }
  
  return(list(U = array(result$par, dimU), 
              V = array(result$par[(dimU[1]*dimU[2]+1):(length(result$par))], dimV), 
              isSuccess = result$convergence == 0, 
              F = result$value))
}

cg <- function(W, Y, A, B, UStart, VStart, maxIter = 100000L, 
               type = 3, reltol = 0.0001, trace = 0){
  twoW2 = 2 * W * W
  twoAtA = 2 * (t(A) %*% A)
  twoBtB = 2 * (t(B) %*% B)
  
  x = c(as.vector(UStart), as.vector(VStart))
  dimU = dim(UStart)
  dimV = dim(VStart)
  #?optim: 
  #fn : A function to be minimized (or maximized), with first argument 
  #     the vector of parameters over 
  #     which minimization is to take place. It should return a scalar result.
  #gr: A function to return the gradient for the "BFGS", "CG" methods.
  result = optim(par = x, fn = FnWr, gr = dFWr, dimU = dimU, dimV = dimV, 
                 W = W, twoW2 = twoW2, Y = Y, A = A, B = B, 
                 twoAtA = twoAtA, twoBtB = twoBtB, method = "CG", 
                 control = list(trace = trace, maxit = maxIter, type = type, 
                                reltol = reltol))

  if(result$convergence == 0) {
    if(trace > 0) {
      cat("\nMethod CG converged calculating function: "); 
      cat(result$counts[1]); cat(" times and gradient: "); 
      cat(result$counts[2]); cat(" times")}
  }
  else {
    cat("\nMethod CG did not converge.\nError code (optim(...)$convergence): "); 
    cat(result$convergence)
    cat("\nError message (optim(...)$message): "); cat(result$message)
  }
  
  # result$par is the best set of parameters found.
  # result$value is the value of fn corresponding to par.
  
  return(list(U = array(result$par, dimU), 
              V = array(result$par[(dimU[1]*dimU[2]+1):(length(result$par))], dimV), 
              isSuccess = result$convergence == 0, 
              F = result$value))
}