require(matlib)

#' Compute the variance, covariance matrix of K coordinate matrix
#'
#' @author Eric Coissac
#' @author Christelle Gonindard-Melodelima
#' @export
mvar = function(...) {

  Xs <- list(...)
  Xnames=names(Xs)
  nXs= sapply(Xs, nrow)

  if (any(nXs!=nXs[1])) {
    stop("Matrices have different number of rows: ", cat(nXs))
  }

  Xs <- lapply(Xs,scale,scale = FALSE)

  nX = length(Xs)

  Xx <- rep(1:nX,nX)
  Xy <- rep(1:nX,rep(nX,nX))

  XXs <- mapply(function(x,y) crossprod(Xs[[x]], Xs[[y]]),
                Xx,Xy,
                SIMPLIFY = FALSE)

  sol_xxs <- lapply(XXs,svd)

  CovXXs  = sapply(sol_xxs, function(sol) sum(sol$d))


  dim(CovXXs)=c(nX,nX)
  colnames(CovXXs)=Xnames
  rownames(CovXXs)=Xnames

  return(CovXXs)
}

#' Compute the correlation matrix of K coordinate matrix
#'
#' @author Eric Coissac
#' @author Christelle Gonindard-Melodelima
#' @export
mcor = function(...) {
  cov = mvar(...)
  s = sqrt(diag(cov))
  vv= outer(s,s)
  return(cov/vv)
}

#' @author Eric Coissac
#' @author Christelle Gonindard-Melodelima
#' @export
pm = function (model,data) {
  cl=match.call()
  terms = terms(model)

  if (missing(data))
    data=NULL

  vars = eval(attr(terms(model),"variables"),
            envir = data,
            enclos = globalenv())

  names(vars)=rownames(attr(terms,"factors"))

  cvars= sapply(vars, class)
  if (! all(cvars %in% c("matrix","data.frame"))) {
    stop("All the variables are not matrices", cat(cvars))
  }

  vars = lapply(vars, as.matrix)


  nrvars= sapply(vars, nrow)

  if (any(nrvars!=nrvars[1])) {
    stop("Matrices have different number of rows: ", cat(nrvars))
  }

  attr(vars,"terms")=terms

  nvar = length(vars) - 1

  data.cov = do.call(mvar,vars)

  xy.cov = data.cov[2:(nvar+1),1,drop=FALSE]
  xx.cov = data.cov[2:(nvar+1),2:(nvar+1)]

  Y = vars[[1]]
  Xs= vars[-1]

  Ymean <- colMeans(Y)
  Xmeans<- lapply(Xs,colMeans)

  YC  <- scale(Y,scale=FALSE)
  XCs <- lapply(Xs,scale,scale = FALSE)
  XYs <- lapply(XCs,function(x) crossprod(YC, x))
  sol_yxs <- lapply(XYs,svd)
  A_xys <- lapply(sol_yxs, function(x) x$v %*% t(x$u))

  Xrots <- mapply(function(x,a) x %*% a,
                  XCs,A_xys,
                  SIMPLIFY = FALSE)

  if (length(xx.cov)>1)
    ixx.cov=inv(xx.cov)
  else
    ixx.cov=1/xx.cov

  scales=ixx.cov %*% xy.cov

  dim(scales)=NULL

  b  =  Ymean - Reduce(function(a,b) a+b,
                       mapply(function(d,coef,rot) coef * (d %*% rot),
                              Xmeans, scales, A_xys,
                              SIMPLIFY = FALSE))

  yhat = mapply(function(coef,xrot) coef * xrot,
                scales, Xrots,
                SIMPLIFY = FALSE)

  init=matrix(rep(0,nrow(yhat[[1]])*ncol(yhat[[1]])),
              nrow=nrow(yhat[[1]]))
  yhat = Reduce(function(a,b) a+b,
                yhat,
                init)

  yhat = sweep(yhat,2,b,"+")

  names(scales)=attr(terms,"term.labels")

  results = list(coefficients = scales,
                 fitted.values=yhat,
                 residuals=Y - yhat,
                 call=cl,
                 model=vars,
           #      model=c(list(Y=YC),XCs),
                 terms = terms,
                 A = A_xys
                )

  class(results)="pm"

  return(results)
}

#' @author Eric Coissac
#' @author Christelle Gonindard-Melodelima
#' @export
print.pm = function(m,...) {
  cat(sprintf("\nCall:\n%s\n\nCoefficients:\n",
              deparse(m$call)))
  print(m$coefficients)
  invisible(m)
}

#' @author Eric Coissac
#' @author Christelle Gonindard-Melodelima
#' @export
plot.pm = function(m,
                   points.cex=0.6,
                   labels.cex=0.6,
                   arrow.length=0.05,
                   asp=1,
                   ...) {

  layout(matrix(c(1,1,1,1,2,3),nrow = 2))

  xlim = c(min(m$model[[1]][,1],m$fitted.values[,1]),
           max(m$model[[1]][,1],m$fitted.values[,1])
          )
  ylim = c(min(m$model[[1]][,2],m$fitted.values[,2]),
           max(m$model[[1]][,2],m$fitted.values[,2])
          )

  centers = (m$model[[1]] + m$fitted.values)/2

  plot(m$model[[1]][,1:2],
       xlim=xlim,ylim=ylim,
       cex=points.cex,
       col="blue",
       asp=asp)

  points(m$fitted.values[,1:2],
       xlim=xlim,ylim=ylim,
       cex=points.cex,
       col="red")

  arrows(m$fitted.values[,1],m$fitted.values[,2],
         m$model[[1]][,1],m$model[[1]][,2],
         length = arrow.length,
         col="black")

  text(centers[,1],centers[,2],
       rownames(m$fitted.values),
       xlim=xlim,ylim=ylim,
       cex=labels.cex,
       col="black",
       pos=3)

  anova = anova(m)
  v=c(anova$SCX,
      interaction=anova$SCI,
      error=anova$SCE)
  barplot(v/sum(v),las=2)
}

anova.pm = function(m,...) {

  ctrace <- function(MAT) sum(MAT^2)

  Xs <- lapply(m$model,scale,scale = FALSE)
  Y  <- Xs[[1]]
  Xs <- Xs[-1]

  Ymean <- colMeans(Y)

  Xrots <- mapply(function(x,a) x %*% a,
                  Xs,m$A,
                  SIMPLIFY = FALSE)

  Chat = mapply(function(coef,xrot) sweep(coef * xrot,
                                          2,
                                          Ymean,
                                          '-'),
                m$coefficients, Xrots,
                SIMPLIFY = FALSE)

  VarY  = ctrace(Y)

  VarYhats = sapply(Chat,ctrace)
  names(VarYhats) = names(m$coefficients)
  SChat = sum(VarYhats)

  SCE = sum(m$residuals^2)
  SCI = VarY - SCE - sum(SChat)

  return(list(SCI=SCI,
              SCX=VarYhats,
              SCE=SCE))
}

mprocuste = function (Y, ...)
{
  ctrace <- function(MAT) sum(MAT^2)

  Xs <- list(...)

  nY = nrow(Y)
  nXs= sapply(Xs, nrow)

  if (any(nXs!=nY)) {
    stop("Matrices have different number of rows: ", nY,
         " and ", cat(nXs))
  }

  Ymean <- colMeans(Y)
  Xmeans<- sapply(Xs,colMeans)

  Y  <- scale(Y,  scale = FALSE)

  Xs <- lapply(Xs,scale,scale = FALSE)

  XYs <- lapply(Xs,function(x) crossprod(Y, x))
  sol_yxs <- lapply(XYs,svd)

  A_xys <- lapply(sol_yxs, function(x) x$v %*% t(x$u))

  nX = length(Xs)

  Xx <- rep(1:nX,nX)
  Xy <- rep(1:nX,rep(nX,nX))

  XXs <- mapply(function(x,y) crossprod(Xs[x], Xs[y]),
                Xx,Xy,
                SIMPLIFY = FALSE)
  sol_xxs <- lapply(XXs,svd)
  A_xxs <- lapply(sol_xxs, function(x) x$v %*% t(x$u))

  Xrots <- mapply(function(x,a) x %*% a,
                  Xs,A_xys,
                  SIMPLIFY = FALSE)

  CovYXs  = lapply(sol_yxs, function(sol) sum(sol$d))

  CovX1X2 = sum(sol_x1x2$d)

  VarY  = ctrace(Y)
  VarX1 = ctrace(X1)
  VarX2 = ctrace(X2)

  CovEx = matrix(c(VarX1,CovX1X2,CovX1X2,VarX2),nrow=2)
  Cov2  = matrix(c(CovYX1,CovYX2),nrow=2)

  pentes = inv(CovEx) %*% Cov2

  print(pentes)

  SdX1  = sqrt(VarX1)
  SdX2  = sqrt(VarX2)

  a1 = (CovYX1 * VarX2 - CovYX2 * CovX1X2)/(VarX1*VarX2-CovX1X2^2)
  a2 = (CovYX2 - a1 * CovX1X2) / VarX2

  b  =  Ymean - a1 * X1mean %*% A_yx1 - a2 * X2mean %*% A_yx2


  SCX1 = sum((X1rot * a1)^2)
  SCX2 = sum((X2rot * a2)^2)

  Yhat = X1rot * a1 + X2rot * a2
  SCR  = sum((Y - Yhat)^2)

  SCT  = VarY

  SCI  = SCT - SCX1 - SCX2 - SCR

  ddl.t = (nrow(Y)-1)^2
  ddl.r = ddl.t - 3

  vt = SCT/ddl.t
  vx1= SCX1
  vx2= SCX2
  vi = SCI
  vr = SCR/ddl.r

  fx1=vx1/vr
  fx2=vx2/vr
  fi =vi/vr

  pf.x1=1-pf(fx1,1,ddl.r)
  pf.x2=1-pf(fx2,1,ddl.r)
  pf.i =1-pf(fi ,1,ddl.r)


  ddl = (nrow(Y)-1)^2-1
  sd.a1 = sqrt((VarY/VarX1 - a1^2)/ddl)
  sd.a2 = sqrt((VarY/VarX2 - a2^2)/ddl)

  t.a1  = a1/sd.a1
  t.a2  = a2/sd.a2

  p.a1  = 1 - pt(t.a1,ddl)
  p.a2  = 1 - pt(t.a2,ddl)



  res = list(coefficients = list(a1=a1,a2=a2,b=b),
             sd           = list(a1=sd.a1,a2=sd.a2),
             t            = list(a1=t.a1,a2=t.a2),
             p.value      = list(a1=p.a1,a2=p.a2),
             anova        = list(X1=SCX1/SCT, X2=SCX2/SCT, X1X2=SCI/SCT, Res=SCR/SCT),
             SunSq        = list(X1=SCX1, X2=SCX2, X1X2=SCI, Res=SCR),
             MeanSq       = list(X1=vx1,X2=vx2,X1X2=vi,Res=vr),
             Fvalue       = list(X1=fx1,X2=fx2,X1X2=fi),
             Fvalue       = list(X1=pf.x1,X2=pf.x2,X1X2=pf.i)
  )
  class(res) <- "mprocuste"

  return(res)
}

